[发明专利]一种基于加性四元数的大方位失准角线性对准方法

说明书

本发明涉及一种基于加性四元数的大方位失准角线性对准方法，步骤：(1)根据粗对准的状态方程和观测方程，利用GPS观测信息，通过线性卡尔曼滤波，进行粗对准过程；(2)在滑动窗口内，利用本发明提出的阈值条件，直接由加性四元数计算当前航向角收敛水平，并判断其是否全部满足阈值条件；若满足，则保存系统协方差信息，转入精对准；若不满足，则继续步骤(1)的粗对准；(3)延续粗对准的系统状态变量及其位置、姿态误差方程和观测方程，使用新的速度误差方程，并将保存的协方差阵作为初始条件，进行精对准，使精对准能快速进入收敛状态，至对准达到预期水平。本发明既能实现平稳的模型切换，又满足全状态应用条件，且计算简便。

技术领域

本发明涉及一种基于加性四元数的大方位失准角线性对准方法，属于惯性导航技术领域。

背景技术

初始对准是惯性导航的关键技术，也是INS/GNSS组合导航关键技术之一。在基于MEMS-INS/GNSS组合导航系统中，由于MEMS器件特别是陀螺仪的限制，导致无法通过自对准实现方位失准角的初始化，从而造成了大方位失准角问题，其中一种解决方法是直接进行动基座的对准。

建立准确的INS误差传播方程和采用适当的滤波技术是进行初始对准的主要问题。在大方位失准角的情况下的动基座对准模型本质上是非线性的，而非线性滤波方法计算量大，不适宜工程应用。

常用的姿态角描述方法有欧拉角法和四元数法。相应的，对准方案也可据此分为两类。

欧拉角法用于描述姿态角的优点在于直观性，各参数物理意义明确，但其存在奇异性问题。现有的技术方案中，除直接进行非线性滤波外，主要是将对准过程分为粗对准和精对准两个过程，并在粗对准中用航向角误差的正余弦项作为系统状态变量，实现系统方程的线性化；在精对准中则采用小角度近似，即sinα≈α，cosα≈1实现线性化，从而进行线性滤波。一般在粗对准精度达到某一阈值条件时实现对准模型的切换。

四元数由于计算简单，无奇异性等优点，常被用于导航解算中。现有的技术方案中推导了加性四元数描述的姿态误差方程，其优点在于姿态误差方程并没有经过任何小角度假设，并且是误差四元数的线性函数。不足之处在于其速度误差方程仍然保留非线性。现有的大失准角对准方案中，对加性四元数的利用主要停留在非线性滤波的程度上。

现有的技术方案中，大失准角条件下线性化的速度误差方程已经被推导出来，并且证明了小角度近似下，加性四元数方法和等倾角模型的等价性，这为线性滤波模型的建立提供了条件。参见Yu M J,Park H W,Jeon C B.Equivalent Nonlinear Error Modelsof Strapdown Inertial[C].Proceedings of 1997Guidance,Navigation,and ControlConference,August 11-13,1997,p.581-587.和Kong X.INS algorithm usingquaternion model for low cost IMU[J].Robotics and Autonomous Systems,2004,46(4):221-246.

但现有方案中的非线性滤波模型，计算量大，不适宜工程应用。欧拉角在计算复杂度上要高于四元数，且存在奇异性问题，无法应用于全状态。欧拉角法在粗对准和精对准需要采用不同的状态变量和误差模型，系统状态变量的协方差阵无法直接传递，使精对准需要重新对状态的协方差阵初始化，从而造成模型切换过渡不平稳，影响收敛速度。虽然加性四元数的粗对准和精对准模型都被推导出来，但由于四元数与姿态角之间的关系不直观，对准过程的收敛情况判断难度大，由粗到精的模型切换时刻难以有效确定，所以加性四元数应用于线性滤波模型的大失准角初始对准方案仍存在困难。

发明内容

本发明技术解决问题：克服了现有技术中欧拉角法粗对准和精对准模型切换不平稳，而四元数法滤波模型非线性的问题，提供了一种基于加性四元数的大方位失准角线性对准方法，既能实现平稳的模型切换，又满足全状态应用条件，且计算简便。