[发明专利]一种基于四元数CKF的低精度惯导初始对准方法

摘要

本发明提供的是一种基于四元数CKF的低精度惯导初始对准方法。首先利用加速度计和磁强计的测量值进行粗对准，得到粗略的姿态矩阵。然后利用GPS提供的准确外界信息，取位置和速度误差作为量测量，通过四元数CKF进行非线性滤波，得到姿态误差矩阵。最后利用姿态误差矩阵进行校正，得到精确的姿态矩阵。本发明利用加速度计和磁强计分别对重力加速度和磁力的量测值，进行粗对准。解决了在低精度惯性器件的条件下，由于陀螺性能低，无法正确敏感地球自转角速率，不能应用传统粗对准方法的问题；另一方面，本方法中提出的四元数CKF能适用于大角度误差的非线性模型，可以在大方位失准角的条件下依然很好的完成对准，且对准精度高。

主权项

1.一种基于四元数CKF的低精度惯导初始对准方法，其特征在于包括以下步骤：(1)利用GPS确定载体的初始精度、纬度参数；(2)采集加速度计和磁强计输出的载体三轴加速度和磁力信息；(3)利用采集的载体三轴加速度信息，根据下式确定初始纵摇角α₀和初始横摇角β₀，α0=arcsin(fxbg)β0=arcsin(-fybgcosα0)]]>式中：和分别是载体的x、y轴的加速度，g表示地球重力加速度；(4)利用载体的三轴磁力信息以及步骤(3)中确定的载体初始纵摇角α₀和初始横摇角β₀，根据下式确定初始方位角γ₀；γ0=arctanmycosβ0+mzsinβ0mxcosα0+mysinα0sinβ0-mzsinα0cosβ0]]>式中：m_x、m_y和m_z分别为载体x、y和z轴的磁力量；(5)利用步骤(3)、(4)中确定的载体初始纵摇角α₀和初始横摇角β₀和初始方位角γ₀，采用解析法来进行系统的粗对准，粗略的得到载体坐标系到计算地理坐标系的姿态矩阵Cbp(0)=cosβ0cosγ0-sinβ0sinα0sinγ0-cosα0sinγ0sinβ0cosγ0+cosβ0sinα0sinγ0cosβ0sinγ0+sinβ0sinα0cosγ0cosα0cosγ0sinβ0sinγ0-cosβ0sinα0cosγ0-sinβ0cosα0sinα0cosβ0cosα0;]]>(6)建立捷联惯导系统初始对准非线性状态空间模型，取系统的状态变量为取速度误差和位置误差作为观测量当地地理坐标选取东北天坐标系，q₀、q₁、q₂和q₃分别是误差四元数δQ的四个元素，其中q₀为标量部分，δv_x、δv_y分别为东向和北向速度误差，δλ分别为纬度和经度误差，和分别是三个方向上的陀螺仪常值偏移，和分别是加速度计常值零偏；(7)根据捷联惯导系统静基座下的姿态四元数误差方程、速度误差方程和位置误差方程，建立捷联惯导系统在大方位失准角下的状态空间模型：X·=f(X)+G(X)WZ=HX+V]]>式中：f(X)为系统矩阵，G(X)为噪声驱动阵，W为系统噪声阵，H为观测矩阵，V为观测噪声阵，其中：H=[0_4×4 I_4×4 0_4×5]；(8)滤波初始化，在初始时刻即k=0时，对步骤(6)中的变量初始化式中：为初始状态估计值，P₀为初始状态误差协方差阵，E表示求取期望；(9)当k≥1时，将GPS提供的载体速度、位置与惯导解算的速度、位置的差值作为滤波的观测量，通过四元数CKF对状态量进行估计，直到估计出精确的误差四元数δQ；(10)利用步骤(9)估计出的误差四元数δQ，根据下式确定姿态误差矩阵Cnp=Cnp(δQ)=2q02+2q12-12(q1q2+q0q3)2(q1q3-q0q2)2(q1q2-q0q3)2q02+2q22-12(q2q3+q0q1)2(q1q3+q0q2)2(q2q3-q0q1)2q02+2q32-1]]>(11)利用步骤(10)中确定的姿态误差矩阵校正粗略姿态矩阵得到精确的姿态矩阵完成初始对准。