一种用于航姿系统的自适应姿态估计方法及其平滑切换算法技术方案

本发明专利技术公布了一种用于航姿系统的自适应姿态估计方法及其平滑切换算法，由基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法构成自适应姿态平滑切换。该算法根据GPS状态实现低阶与高阶的切换，包括以下步骤：在GPS状态良好的情况下，采用高阶组合导航融合算法估计姿态信息；在GPS信号受到干扰时，停止高阶组合导航算法并切换为低阶姿态估计算法；当GPS信号恢复后，将低阶姿态估计状态作为高阶滤波算法的初值，实现姿态融合算法的平滑切换。针对该算法设计基于ARM架构的航姿系统，并通过仿真与试验验证设计算法的可靠性，在切换过程中姿态角误差小于0.5

【技术实现步骤摘要】
一种用于航姿系统的自适应姿态估计方法及其平滑切换算法


[0001]本专利技术涉及一种用于航姿系统的自适应姿态估计方法及其平滑切换算法，适用于航空导航

技术背景
[0002]随着微机电技术的发展，三轴MEMS陀螺仪、加速度计、磁力计成为最主要的惯性测量元件。但现有的MEMS器件均存在某些缺陷：MEMS陀螺仪存在漂移，单独使用陀螺仪估计姿态存在累积误差；MEMS加速度计受线性加速度和震动影响较大，单独利用加速度计估计姿态角，会出现较大误差；MEMS磁力计易受周围环境所产生的磁场干扰，从而影响航向角的输出。由于此类误差是随机的，无法预先消除，因此不能单独通过上述方法获取准确的姿态角，必须通过合理的数据融合方法来补偿各类误差带来的影响，才能获得稳定可靠的姿态信息。

技术实现思路

[0003]专利技术目的
[0004]本专利技术的目的在于为了提高系统在GPS信号丢失情况下姿态估计的准确性，确保无人机在丢失GPS信号器件仍能够稳定飞行。
[0005]技术方案
[0006]为了实现上述目的，本专利技术采用以下技术方案：一种用于航姿系统的自适应姿态估计方法，所述方法的实现步骤包括：
[0007]步骤1，在GPS状态良好的情况下，基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法同时运行，实际采用高阶组合导航算法估计姿态信息；
[0008]步骤2，在GPS信号受到干扰时，停止高阶组合导航算法并切换为采用低阶姿态估计算法；
[0009]步骤3，当GPS信号恢复后，将低阶姿态估计算法得到的四元数值作为高阶滤波算法初值，实现姿态融合算法的平滑切换。
[0010]进一步的，所述基于四元数的低阶EKF姿态估计算法，具体为采用三轴加速度计，三轴陀螺仪、磁力计，应用低阶卡尔曼滤波器实现各传感器数据融合，解算无人机姿态与航向信息。
[0011]更进一步的，所述的基于四元数的低阶EKF姿态估计算法包括以下步骤：在基于四元数的低阶卡尔曼滤波器中，设系统状态量X为姿态四元数：
[0012]X＝[q
0 q
1 q
2 q3]T
ꢀꢀꢀ
(1)
[0013]其中，q0、q1、q2、q3为姿态四元数；
[0014]根据卡尔曼滤波器原理，定义离散系统的状态方程如下：
[0015]X
k
＝A
k
X
k
‑1+ω
k
‑1ꢀꢀꢀ
(2)
[0016]其中，X
k
、X
k
‑1分别表示第k、k
‑
时刻的系统状态量，A
k
为从k
‑
1时刻到k时刻的状态转
移矩阵，ω
k
‑1表示k
‑
1时刻的系统过程噪声序列，则对应于卡尔曼滤波器的状态方程为：
[0017][0018]表示系统k时刻先验估计状态，表示系统k
‑
1时刻后验估计状态，其中：
[0019][0020]其中，表示角速率矢量矩阵：
[0021][0022]式中：ω
x
表示绕横滚轴角速率，ω
y
表示绕俯仰轴角速率，ω
z
表示绕航向轴角速率。
[0023]更进一步的，对所述卡尔曼滤波器状态的先验估计过程，仅使用陀螺仪进行角度更新；然后开展后验估计，所述后验估计包括两个阶段：
[0024]第一阶段基于加速度计数据，第二阶段基于磁力计数据，根据卡尔曼滤波器原理，校正方程的公式如下所示：
[0025][0026]式中，Z
k
表示k时刻实际量测值，具体为加速度计与磁力计所检测的数据；是根据系统先验估计状态计算的k时刻期望量测值；K
k
表示k时刻卡尔曼增益，由公式(7)得出：
[0027][0028]式中，表示先验估计协方差矩阵，Q
k
‑1为过程噪声矩阵，P
k
‑1和P
k
分别表示上一次和本次后验估计协方差矩阵，R
k
表示量测噪声矩阵，H
k
有两种形式，分别表示期望的重力矢量或磁场矢量对四元数求偏导的雅可比矩阵H
k1
和H
k2
，I表示单位矩阵；
[0029]h1表示体轴系下期望的重力矢量量测值，由方向余弦矩阵与NED坐标系下重力矢量计算得出，设重力加速度的模值|g|为常值9.807m/s2，则：
[0030][0031]由上可得：
[0032][0033]H
k1
表示重力矢量量测值h1对状态X求偏导的雅可比矩阵；
[0034]同理，h2表示体轴系下期望的磁场矢量量测值，查询当地磁偏角ψ
mag
和磁倾角θ
mag
，并将磁场归一化得出：
[0035][0036]则：
[0037][0038]H
k2
表示磁场矢量量测值h2对状态X求偏导的雅可比矩阵。
[0039]进一步的，所述姿态估计算法具体为根据GPS接收机测量的位置、速度，应用卡尔曼滤波器不断修正惯性导航系统，以达到无人机的姿态、速度、位置估计的目的。
[0040]进一步的，基于GPS量测的高阶组合导航算法的过程具体包括以下步骤：
[0041]在基于GPS量测的高阶卡尔曼滤波器中，设系统状态量为：
[0042][0043]式中，q0、q1、q2、q3为姿态四元数，v
N
、v
E
、v
d
分别为NED坐标系下北向速度，东向速度，地向速度，P
N
、P
E
、P
D
分别为NED坐标系下北向位置、东向位置和地向位置，bias
x
、bias
y
、bias
z
分别为绕横滚轴、俯仰轴和航向轴的陀螺仪随机漂移估计值；
[0044]利用公式(4)更新状态量中的前四个状态的先验估计x
k(1∶4)
，x
k(1:4)
为由q0、q1、q2、q3构成的状态；利用运动学方程，机体加速度和方向余弦矩阵得到先验估计x
k(5:10)
，更新过程如下所示：
[0045]x
k(1
:
4)
为由q0、q1、q2、q3构成的状态
[0046][0047]式中，a
x
、a
y
、a
z
分别为机体坐标系下的加速度量测值，即进行量纲变换以后的加速度计量测值，dt表示系统先验估计的递推时间；
[0048]此时，系统状态转移矩阵为：
[0049][0050]量测信号为NED坐标系下的三轴速度V
GNSS本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种用于航姿系统的自适应姿态估计方法，其特征在于，所述方法的实现步骤包括：步骤1，在GPS状态良好的情况下，基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法同时运行，实际采用高阶组合导航算法估计姿态信息；步骤2，在GPS信号受到干扰时，停止高阶组合导航算法并切换为采用低阶姿态估计算法；步骤3，当GPS信号恢复后，将低阶姿态估计算法得到的四元数值作为高阶滤波算法初值，实现姿态融合算法的平滑切换。2.根据权利要求1所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，所述基于四元数的低阶EKF姿态估计算法，具体为采用三轴加速度计，三轴陀螺仪、磁力计，应用低阶卡尔曼滤波器实现各传感器数据融合，解算无人机姿态与航向信息。3.根据权利要求2所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，所述的基于四元数的低阶EKF姿态估计算法包括以下步骤：在基于四元数的低阶卡尔曼滤波器中，设系统状态量X为姿态四元数：X＝[q
0 q
1 q
2 q3]
T
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)其中，q0、q1、q2、q3为姿态四元数；根据卡尔曼滤波器原理，定义离散系统的状态方程如下：X
k
＝A
k
X
k
‑1+ω
k
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)其中，X
k
、X
k
‑1分别表示第k、k
‑
时刻的系统状态量，A
k
为从k
‑
1时刻到k时刻的状态转移矩阵，ω
k
‑1表示k
‑
1时刻的系统过程噪声序列，则对应于卡尔曼滤波器的状态方程为：1时刻的系统过程噪声序列，则对应于卡尔曼滤波器的状态方程为：表示系统k时刻先验估计状态，表示系统k
‑
1时刻后验估计状态，其中：其中，表示角速率矢量矩阵：式中：ω
x
表示绕横滚轴角速率，ω
y
表示绕俯仰轴角速率，ω
z
表示绕航向轴角速率。4.根据权利要求3所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，对所述卡尔曼滤波器状态的先验估计过程，仅使用陀螺仪进行角度更新；然后开展后验估计，所述后验估计包括两个阶段：第一阶段基于加速度计数据，第二阶段基于磁力计数据，根据卡尔曼滤波器原理，校正方程的公式如下所示：式中，Z
k
表示k时刻实际量测值，具体为加速度计与磁力计所检测的数据；是根据系统先验估计状态计算的k时刻期望量测值；K
k
表示k时刻卡尔曼增益，由公式(7)得出：
式中，表示先验估计协方差矩阵，Q
k
‑1为过程噪声矩阵，P
k
‑1和P
k
分别表示上一次和本次后验估计协方差矩阵，R
k
表示量测噪声矩阵，H
k
有两种形式，分别表示期望的重力矢量或磁场矢量对四元数求偏导的雅可比矩阵H
k1
和H
k2
，I表示单位矩阵；h1表示体轴系下期望的重力矢量量测值，由方向余弦矩阵与NED坐标系下重力矢量计算得出，重力加速度的模值|g|为常值9.807m/s2，则：由上得：H
k1
表示重力矢量量测值h1对状态X求偏导的雅可比矩阵；同理，h2表示体轴系下期望的磁场矢量量测值，查询当地磁偏角ψ
mag
和磁倾角θ
mag
，并将磁场归一化得出：则：H
k2
表示磁场矢量量测值h2对状态X求偏导的雅可比矩阵。5.根据权利要求1所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，所述姿态估计算法具体为根据GPS接收机测量的位置、速度，应用卡尔曼滤波器不断修正惯性导航系统，以达到无人机的姿态、速度、位置估计的目的。6.根据权利要求5所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，基于GPS量测的高阶组合导航算法的过程具体包括以下步骤：在基于GPS量测的高阶卡尔曼滤波器中，设系统状态量为：式中，q0、q1、q2、q3为...

【专利技术属性】
技术研发人员：郭剑东，刘基，桑标，王辉，李浩，孙哲，孙伟，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：