[发明专利]基于自适应EKF算法的小型无人机MARG航姿估计方法

权利要求书

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为角速度漂移系统噪声；

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

量测噪声自适应调节方程如下：

式中，n＝x,y,z代表机体坐标系的三个方向，R_acc,n为自适应调节后的加速度量测噪声，R_acc,ori是滤波器初始化设置的加速度量测噪声，y_max是残差的阈值，ρ_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轴的分量的计算值。