[发明专利]一种基于二阶量测更新的低成本姿态估计方法

摘要

本发明提供的是一种基于二阶量测更新的低成本姿态估计方法。本发明通过对三轴微机械陀螺、三轴微机械加速度计和三轴磁强计的输出数据，进行滤波处理得到载体的姿态信息。针对在室内或磁干扰较强场所，磁强计输出会使横摇和纵摇误差变大，传统方法难以解决的问题。本方法在滤波的量测更新阶段，创新性地采用二阶量测更新，即先进行加速度计量测更新再进行磁强计量测更新。以此修正标准量测更新算法从而使磁强计更新只影响方位角。利用本方法可以使用低成本的微惯性测量单元和磁强计进行姿态估计，并且估计精度高、实时性好、适应强磁干扰环境。本方法适用于车辆、无人机和船舰等载体的姿态估计。

主权项

1.一种基于二阶量测更新的低成本姿态估计方法，其特征是：(1)在k=0时刻，即初始时刻由初始对准得到初始姿态φ₀，所述初始姿态包括横摇、纵摇和航向；(2)在k≥1时刻，利用微机械陀螺输出的载体三轴的角速率数据，计算出姿态角的变化值Δφ，由式计算出滤波开始前的粗略姿态角；(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据；(4)设滤波的状态变量为x=[q_e b_g b_a]^T∈R^9×1；式中：q_e表示四元数误差矢量部分，b_g表示陀螺漂移，b_a表示加速度计零偏；假设四元数误差q~e=1qeT,]]>根据四元数相关性质建立状态方程：q·eb·gb·a=-[yg×]-0.5I0000000qebgba+-0.5vgvbgvba]]>式中：[yg×]=0-ygzygyygz0-ygx-ygyygx0,]]>下标g代表陀螺、a代表加速度计、m代表磁力计，y_g表示陀螺实际输出。传感器噪声v_a、v_g以及v_m为零均值高斯白噪声；取观测量为z=ya-C(q^)g~ym-C(q^)m~,]]>由于q_e为小量，再忽略二阶小项，建立量测方程：ya-C(q^)g~=2[C(q^)g~×]qe+va+ba]]>ym-C(q^)m~=2[C(q^)m~×]qe+vm]]>式中：表示由估计四元数组成的姿态矩阵，y_a和y_m分别为加速度计和磁强计输出，定义g~=00gT]]>和m~=cosα0-sinαT,]]>g为重力加速度，α为磁倾角；(5)利用步骤(4)中建立的状态方程进行滤波的时间更新，由下式x^k-=Φkx^k-1]]>Pk-=ΦkPk-1ΦkT+Qk]]>分别计算出一步预测量和状态误差协方差阵式中：Φ_k为系统状态转移矩阵，Q_k为系统噪声协方差阵；(6)在滤波的量测更新阶段采用二阶量测更新，先进行加速度计量测更新；取此阶段观测量根据建立的量测方程有其更新过程如下：先计算滤波增益：Ka,k=Pk-Ha,k′(Ha,kPk-Ha,k′+Ra)-1;]]>然后计算状态估计量：x^a,k=x^k-+Ka,k(za,k-Ha,kx^k-);]]>再计算状态误差协方差阵：Pa,k=(I-Ka,kHa,k)Pk-(I-Ka,kHa,k)′+Ka,kRaKa,k′;]]>式中：下标a表示加速度计量测更新阶段，R_a为加速度计噪声协方差阵此时的9个状态都会更新，利用更新用代表已更新状态，取即前三个变量值，利用式进行校正，然后进行四元数规范化：q^⇐q^||q^||]]>加速度计更新结束后，将的q_e部分设置为零；(7)利用步骤(6)中得到的进行磁力计量测更新；取此阶段观测量为zm,k≡ym,k-C(q^k)m~,]]>则得Hm,k≡2[C(q^)m^×]00,]]>其更新过程如下：取此时状态误差协方差阵为Pm,k-=Pa,k(1:3,1:3)03×606×306×6]]>由此计算滤波增益：Km,k=r3r3′03×606×306×6Pm,k-Hm,k′(Hm,kPm,k-Hm,k′+Rm)-1;]]>然后计算状态估计量：xk′=x^a,k+Km,k(zm,k-Hm,kx^a,k);]]>再计算状态误差协方差阵：P_k=(I-K_m,kH_m,k)P_a,k(I-K_m,kH_m,k)′+K_m,kR_mK′_m,k；式中：下标m表示磁力计量测更新阶段，P_a,k(1:3,1:3)表示由P_a,k前三行前三列所组成的矩阵，0_mn为m行n列的零矩阵并且r3=C(q^)001T;]]>(8)根据滤波计算出的四元数误差，校正步骤(2)中的粗略姿态角，得到精确的姿态角。