CN112254723A - 基于自适应ekf算法的小型无人机marg航姿估计方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于组合导航技术领域，具体涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法。

背景技术

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

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何设计一种小型无人机MARG航姿估计方法，提高姿态估计精度。

(二)技术方案

为了解决上述技术问题，本发明提供了一种基于自适应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]时间段内的角增量向量，

步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；

由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：

其中，q₀,q₁,q₂,q₃为姿态四元数；

步骤四、选取所述姿态四元数的扩展卡尔曼滤波系统状态向量为:

X＝[q₀ q₁ q₂ q₃ 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为角速度漂移系统噪声。

优选地，还包括步骤七、自适应调节加速度量测噪声；

当飞机进行加减速或机动飞行时，加速度的残差y_acc的值会发生较大的变化，利用三轴的残差值来分析外部加速度的影响，动态调节三轴加速度的量测噪声，设计量测噪声自适应调节方程如下：

式中，n＝x,y,z代表机体坐标系的三个方向，R_acc,n为自适应调节后的加速度量测噪声，R_acc,ori是滤波器初始化设置的加速度量测噪声，y_max是残差的阈值，当y_acc,n超过阈值时，说明当前方向的外部加速度较大，则需要对量测噪声进行调整，ρ_n为y_acc,n的影响因子。

优选地，还包括步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；

量测方程分为两部分，如下：

式中，Z_Acc,Z_Mag为观测向量，V_k,acc,V_k,mag为观测噪声；

加速度量测方程为：

式中，

为导航坐标系至机体坐标系的转换矩阵，

gⁿ＝[0 0 g]^T为重力在导航坐标系下的投影，对系统量测方程h_acc进行泰勒展开并取一阶近似项得到加速度量测矩阵为：

则加速度量测更新方程为：

X_k＝X_k/k-1+K_acc,kY_acc,k

P_k＝(I-K_acc,kH_acc,k)P_k/k-1

式中，K_acc为加速度滤波增益矩阵，Y_acc,k为加速度残差，R_acc,k为加速度计量测噪声，Y_acc,k＝Z_acc,k-h_acc,k(X_k/k-1)，其中Z_acc,k为当前时刻加速度计的测量值。

优选地，还包括步骤九、基于步骤一判断磁力计精度。

优选地，步骤九具体为：

当满足以下任一条件时，不进行地磁量测更新：

式中，||m^b||为当前时刻地磁强度的模值，

为起飞前地磁强度的模值，

为前Δt时间内仅使用四元数更新得到的航向值，

为前Δt时间内使用磁力计解算得到的航向变化值，

磁航向计算方法如下：

为地磁强度在x轴的分量的计算值，

为地磁强度在y轴的分量的计算值。

优选地，步骤十、基于步骤三、步骤六进行地磁量测更新。

优选地，步骤十具体为：

设

为机体坐标系下的地磁测量值在导航坐标系下的投影，则

设mⁿ为理想情况下导航坐标系的地磁分量，则：

为地磁强度在z轴的分量的计算值；

根据上述关系，地磁量测方程为：

对系统量测方程h_mag进行泰勒展开并取一阶近似项得到地磁量测矩阵为：

则地磁量测更新方程为：

X_k＝X_k/k-1+K_mag,kY_mag,k

P_k＝(I-K_mag,kH_mag,k)P_k/k-1

式中，K_mag为磁力计滤波增益矩阵，Y_mag,k为磁力计残差R_mag为磁力计量测噪声，Y_mag,k＝Z_mag,k-h_mag,k(X_k/k-1)，Z_mag,k为当前时刻磁力计的测量值。

优选地，Δt＝1s。

优选地，所述方法基于MARG的传感器测量模型进行姿态估计。

本发明还提供了一种所述的方法在组合导航技术领域中的应用。

(三)有益效果

本发明中，首先，将磁力计和加速度计量测更新分开进行，增加了磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明还提出了针对外部加速度的自适应滤波算法，当飞行时外部加速度较大时，会导致姿态估计精度下降。通常的方法是将三轴加速度的模值与标准重力值g进行比较，如果差值较大，增大加速度的量测噪声，这样相当于舍弃三轴的加速度信息，无法对加速度测量值进行充分利用。本发明为了提高估计精度，不直接判断三轴加速度模值，而是对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。

附图说明

图1是本发明的提供的卡尔曼滤波流程图；

图2是本发明提出改进Q-EKF和普通Q-EKF的姿态误差对比图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

本发明提供了一种基于自适应EKF算法的小型无人机MARG航姿估计方法，基于MARG的传感器测量模型进行姿态估计。采用四元数的扩展卡尔曼滤波算法(QuaternionExtended Kalman Filter，Q-EKF)，并对Q-EKF滤波器进行了改进。首先，将磁力计和加速度计量测更新分开进行，增加磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明提出了针对外部加速度的自适应滤波算法，针对飞机加减速以及振动对水平姿态估计的影响，利用三轴的残差信息，设计了量测噪声自适应调节方程,提高姿态估计精度。具体包括如下步骤：

步骤一、采集陀螺仪测量值

加速度计测量值

和磁力计的测量值

步骤二、更新载体姿态四元数，四元数递推计算公式为：

式中，

表示四元数乘法，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]时间段内的角增量向量，

步骤三、导航坐标系(n系)选用采用北-东-地坐标系，计算姿态矩阵，由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：

其中，q₀,q₁,q₂,q₃为姿态四元数；

步骤四、选取所述姿态四元数的扩展卡尔曼滤波系统状态向量为:

X＝[q₀ q₁ q₂ q₃ 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为角速度漂移系统噪声；

步骤七、自适应调节加速度量测噪声；

当飞机进行加减速或机动飞行时，加速度的残差y_acc的值会发生较大的变化，利用三轴的残差值来分析外部加速度的影响，动态调节三轴加速度的量测噪声，设计量测噪声自适应调节方程如下：

式中，n＝x,y,z代表机体坐标系的三个方向，R_acc,n为自适应调节后的加速度量测噪声，R_acc,ori是滤波器初始化设置的加速度量测噪声，y_max是残差的阈值，当y_acc,n超过阈值时，说明当前方向的外部加速度较大，需要对量测噪声进行调整，ρ_n为y_acc,n的影响因子，可加快量测噪声的调节速度；

步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；

量测方程分为两部分，如下：

式中，Z_Acc,Z_Mag为观测向量，V_k,acc,V_k,mag为观测噪声；

加速度量测方程为：

式中，

为导航坐标系至机体坐标系的转换矩阵，

gⁿ＝[0 0 g]^T为重力在导航坐标系下的投影，对系统量测方程h_acc进行泰勒展开并取一阶近似项得到加速度量测矩阵为：

则加速度量测更新方程为：

X_k＝X_k/k-1+K_acc,kY_acc,k

P_k＝(I-K_acc,kH_acc,k)P_k/k-1

式中，K_acc为加速度滤波增益矩阵，Y_acc,k为加速度残差，R_acc,k为加速度计量测噪声，Y_acc,k＝Z_acc,k-h_acc,k(X_k/k-1)，其中Z_acc,k为当前时刻加速度计的测量值；

步骤九、基于步骤一判断磁力计精度；

无人机在一定范围内飞行时，地磁强度可视为固定值，当无人机所处的外部环境存在干扰时，磁力计测量的地磁强度会产生波动，对航向造成影响；当满足以下任一条件时，说明磁力计受到了较大的干扰，不进行地磁量测更新：

式中，||m^b||为当前时刻地磁强度的模值，

为起飞前地磁强度的模值，

为前Δt时间内仅使用四元数更新得到的航向值，

为前Δt时间内使用磁力计解算得到的航向变化值，

本实施例中Δt＝1s，磁航向计算方法如下：

为地磁强度在x轴的分量的计算值，

为地磁强度在y轴的分量的计算值；

步骤十、基于步骤三、步骤六进行地磁量测更新；

设

为机体坐标系下的地磁测量值在导航坐标系下的投影，则

设mⁿ为理想情况下导航坐标系的地磁分量，则：

为地磁强度在z轴的分量的计算值；

，根据上述关系，地磁量测方程为：

对系统量测方程h_mag进行泰勒展开并取一阶近似项可得地磁量测矩阵为：

则地磁量测更新方程为：

X_k＝X_k/k-1+K_mag,kY_mag,k

P_k＝(I-K_mag,kH_mag,k)P_k/k-1

式中，K_mag为磁力计滤波增益矩阵，Y_mag,k为磁力计残差R_mag为磁力计量测噪声，Y_mag,k＝Z_mag,k-h_mag,k(X_k/k-1)，Z_mag,k为当前时刻磁力计的测量值。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

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]时间段内的角增量向量，

步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；

由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：

其中，q₀,q₁,q₂,q₃为姿态四元数；

步骤四、选取所述姿态四元数的扩展卡尔曼滤波系统状态向量为:

X＝[q₀ q₁ q₂ q₃ 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所述的方法，其特征在于，还包括步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；

量测方程分为两部分，如下：

式中，Z_Acc,Z_Mag为观测向量，V_k,acc,V_k,mag为观测噪声；

加速度量测方程为：

式中，

为导航坐标系至机体坐标系的转换矩阵，

gⁿ＝[0 0 g]^T为重力在导航坐标系下的投影，对系统量测方程h_acc进行泰勒展开并取一阶近似项得到加速度量测矩阵为：

则加速度量测更新方程为：

X_k＝X_k/k-1+K_acc,kY_acc,k

P_k＝(I-K_acc,kH_acc,k)P_k/k-1

式中，K_acc为加速度滤波增益矩阵，Y_acc,k为加速度残差，R_acc,k为加速度计量测噪声，Y_acc,k＝Z_acc,k-h_acc,k(X_k/k-1)，其中Z_acc,k为当前时刻加速度计的测量值。

4.如权利要求3所述的方法，其特征在于，还包括步骤九、基于步骤一判断磁力计精度。

5.如权利要求4所述的方法，其特征在于，步骤九具体为：

当满足以下任一条件时，不进行地磁量测更新：

式中，||m^b||为当前时刻地磁强度的模值，

为起飞前地磁强度的模值，

为前Δt时间内仅使用四元数更新得到的航向值，

为前Δt时间内使用磁力计解算得到的航向变化值，

磁航向计算方法如下：

为地磁强度在x轴的分量的计算值，

为地磁强度在y轴的分量的计算值。

6.如权利要求5所述的方法，其特征在于，步骤十、基于步骤三、步骤六进行地磁量测更新。

7.如权利要求6所述的方法，其特征在于，步骤十具体为：

设

为机体坐标系下的地磁测量值在导航坐标系下的投影，则

设mⁿ为理想情况下导航坐标系的地磁分量，则：

为地磁强度在z轴的分量的计算值；

根据上述关系，地磁量测方程为：

对系统量测方程h_mag进行泰勒展开并取一阶近似项得到地磁量测矩阵为：

则地磁量测更新方程为：

X_k＝X_k/k-1+K_mag,kY_mag,k

P_k＝(I-K_mag,kH_mag,k)P_k/k-1

式中，K_mag为磁力计滤波增益矩阵，Y_mag,k为磁力计残差R_mag为磁力计量测噪声，Y_mag,k＝Z_mag,k-h_mag,k(X_k/k-1)，Z_mag,k为当前时刻磁力计的测量值。

8.如权利要求5所述的方法，其特征在于，Δt＝1s。

9.如权利要求1至8中任一项所述的方法，其特征在于，所述方法基于MARG的传感器测量模型进行姿态估计。

10.一种如权利要求1至9中任一项所述的方法在组合导航技术领域中的应用。

Images