基于自适应EKF算法的小型无人机MARG航姿估计方法技术

本发明专利技术涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法，属于组合导航技术领域。本发明专利技术中，首先，将磁力计和加速度计量测更新分开进行，增加了磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明专利技术还提出了针对外部加速度的自适应滤波算法，为了提高估计精度，不直接判断三轴加速度模值，而是对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。计精度。计精度。

【技术实现步骤摘要】
基于自适应EKF算法的小型无人机MARG航姿估计方法


[0001]本专利技术属于组合导航
，具体涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法。

技术介绍

[0002]小型无人机由于其体积和成本限制，通常使用低成本的MEMS惯性器件作为其导航系统的主要传感器，结合磁力计和GPS卫星定位系统进行组合导航。在飞行中遇到卫星数不足或在室内飞行的情况下，需要切换到航姿模式(AHRS)提供姿态信息。常用的AHRS姿态算法有互补滤波法、梯度下降法和卡尔曼滤波等，都是基于MARG(magnetic,angular rate,and gravity)传感器测量模型进行姿态估计。前两种算法实现起来比较容易，无需针对系统进行专门的建模，在大部分的情况下可以满足飞行要求，但遇到大的机动飞行或磁干扰等情况，会导致姿态角精度下降、误差收敛速度慢等情况，使飞机发生振荡、自旋等现象，影响飞行安全。卡尔曼滤波相对前两种方法更为复杂，需要对系统进行建模，但可以对不同传感器的量测噪声进行调节，同时还能对传感器的误差进行估计。飞机的导航系统方程通常都是非线性的，扩展卡尔曼滤波(Extended Kalman Filter，EKF)可将系统方程和量测方程作泰勒级数展开，略去高阶项，近似为线性系统，再做卡尔曼滤波估计。

技术实现思路

[0003](一)要解决的技术问题
[0004]本专利技术要解决的技术问题是：如何设计一种小型无人机MARG航姿估计方法，提高姿态估计精度。
[0005](二)技术方案
>[0006]为了解决上述技术问题，本专利技术提供了一种基于自适应EKF算法的小型无人机MARG航姿估计方法，包括以下步骤：
[0007]步骤一、采集陀螺仪测量值加速度计测量值和磁力计的测量值
[0008]步骤二、更新载体姿态四元数；
[0009]姿态四元数的递推计算公式为：
[0010][0011]式中，表示四元数乘法，q
k
为当前时刻k的姿态四元数，k＝0,1,2，q
new
为[t,t
k+1
]时间段内载体姿态变化四元数；
[0012][0013]式中，Δθ
x
,Δθ
y
,Δθ
z
是[t,t
k+1
]时间段内x,y,z陀螺的角增量输出，Δθ是[t,t
k+1
]时间段内的角增量向量，
[0014]步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；
[0015]由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：
[0016][0017]其中，q0,q1,q2,q3为姿态四元数；
[0018]步骤四、选取所述姿态四元数的扩展卡尔曼滤波系统状态向量为:
[0019]X＝[q
0 q
1 q
2 q
3 bw
x bw
y bw
z
][0020]式中，bw
x
,bw
y
,bw
z
为三轴的陀螺漂移；
[0021]步骤五、基于步骤四建立系统状态方程；
[0022]系统离散时间下的状态空间方程为：
[0023][0024]式中，X
k
是k时刻的状态向量；f(X
k
)为状态更新方程，是7维非线性向量函数，h(X
k-1
)是量测矩阵，为6维非线性向量函数；W
k
是协方差矩阵为Q的零均值白噪声；V
k
协方差矩阵为R的观测噪声；Z
k
是k时刻的观测向量；
[0025]步骤六、基于步骤五进行Q-EKF滤波状态更新；
[0026]系统状态更新方程为：
[0027][0028]bw＝[bw
x
,bw
y
,bw
z
]，对系统状态方程f(
·
)在参考状态X
k-1
邻域附近进行泰勒展开并取一阶近似项可得系统的状态转移矩阵为：
[0029][0030]四元数的EKF滤波状态更新方程如下：
[0031][0032]式中，P
k
为均方误差矩阵，Q
k
为系统噪声矩阵：
[0033][0034]Q
quat
为姿态四元数系统噪声，Q
wb
为角速度漂移系统噪声。
[0035]优选地，还包括步骤七、自适应调节加速度量测噪声；
[0036]当飞机进行加减速或机动飞行时，加速度的残差y
acc
的值会发生较大的变化，利用三轴的残差值来分析外部加速度的影响，动态调节三轴加速度的量测噪声，设计量测噪声自适应调节方程如下：
[0037][0038]式中，n＝x,y,z代表机体坐标系的三个方向，R
acc,n
为自适应调节后的加速度量测噪声，R
acc,ori
是滤波器初始化设置的加速度量测噪声，y
max
是残差的阈值，当y
acc,n
超过阈值时，说明当前方向的外部加速度较大，则需要对量测噪声进行调整，ρ
n
为y
acc,n
的影响因子。
[0039]优选地，还包括步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；
[0040]量测方程分为两部分，如下：
[0041][0042]式中，Z
Acc
,Z
Mag
为观测向量，V
k,acc
,V
k,mag
为观测噪声；
[0043]加速度量测方程为：
[0044][0045]式中，为导航坐标系至机体坐标系的转换矩阵，g
n
＝[0 0 g]T
为重力在导航坐标系下的投影，对系统量测方程h
acc
进行泰勒展开并取一阶近似项得到加速度量测矩阵为：
[0046][0047]则加速度量测更新方程为：
[0048][0049]X
k
＝X
k/k-1
+K
acc,k
Y
acc,k
[0050]P
k
＝(I-K
acc,k
H
acc,k
)P
k/k-1
[0051]式中，K
acc
为加速度滤波增益矩阵，Y
acc,k
为加速度残差，R
acc,k
为加速度计量测噪声，Y
acc,k
＝Z
acc,k-h
acc,k
(X
k/k-1
)，其中Z
acc,k
为当前时刻加速度计的测量值。
[0052]优选地，还包括步骤九本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于自适应EKF算法的小型无人机MARG航姿估计方法，其特征在于，包括以下步骤：步骤一、采集陀螺仪测量值加速度计测量值和磁力计的测量值步骤二、更新载体姿态四元数；姿态四元数的递推计算公式为：式中，表示四元数乘法，q
k
为当前时刻k的姿态四元数，k＝0,1,2，q
new
为[t,t
k+1
]时间段内载体姿态变化四元数；式中，Δθ
x
,Δθ
y
,Δθ
z
是[t,t
k+1
]时间段内x,y,z陀螺的角增量输出，Δθ是[t,t
k+1
]时间段内的角增量向量，步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：其中，q0,q1,q2,q3为姿态四元数；步骤四、选取所述姿态四元数的扩展卡尔曼滤波系统状态向量为:X＝[q
0 q
1 q
2 q
3 bw
x bw
y bw
z
]式中，bw
x
,bw
y
,bw
z
为三轴的陀螺漂移；步骤五、基于步骤四建立系统状态方程；系统离散时间下的状态空间方程为：式中，X
k
是k时刻的状态向量；f(X
k
)为状态更新方程，是7维非线性向量函数，h(X
k-1
)是量测矩阵，为6维非线性向量函数；W
k
是协方差矩阵为Q的零均值白噪声；V
k
协方差矩阵为R的观测噪声；Z
k
是k时刻的观测向量；步骤六、基于步骤五进行Q-EKF滤波状态更新；
系统状态更新方程为：bw＝[bw
x
,bw
y
,bw
z
]，对系统状态方程f(
·
)在参考状态X
k-1
邻域附近进行泰勒展开并取一阶近似项可得系统的状态转移矩阵为：四元数的EKF滤波状态更新方程如下：式中，P
k
为均方误差矩阵，Q
k
为系统噪声矩阵：Q
quat
为姿态四元数系统噪声，Q
wb
为角速度漂移系统噪声。2.如权利要求1所述的方法，其特征在于，还包括步骤七、自适应调节加速度量测噪声；当飞机进行加减速或机动飞行时，加速度的残差y
acc
的值会发生较大的变化，利用三轴的残差值来分析外部加速度的影响，动态调节三轴加速度的量测噪声，设计量测噪声自适应调节方程如下：式中，n＝x,y,z代表机体坐标系的三个方向，R
acc,n
为自适应调节后的加速度量测噪声，R
acc,ori
是滤波器初始化设置的加速度量测噪声，y
max
是残差的阈值，当y
acc,n
超过阈值时，说明当前方向的外部加速度较大，则需要对量测噪声进行调整，ρ
n
为y
acc,n
的影响因子。3.如权利要求2所述的方法，其特征在于，还包括步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；量测方程分为两部分，如下：式中...

【专利技术属性】
技术研发人员：姜鑫，
申请(专利权)人：天津津航计算技术研究所，
类型：发明
国别省市：