CN106595701A - 一种基于加性四元数的大方位失准角线性对准方法 - Google Patents

Abstract

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

Description

一种基于加性四元数的大方位失准角线性对准方法

技术领域

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

背景技术

初始对准是惯性导航的关键技术，也是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[J].1997.和Kong X.INS algorithm using quaternion modelfor low cost IMU[J].Robotics and Autonomous Systems,2004,46(4):221-246.

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

发明内容

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

本发明的技术要点：

1.采用加性四元数表示姿态角，分别用线性化的状态方程(线性化主要指速度误差方程)实现粗对准和精对准过程；

2.提出一种粗对准到精对准的模型切换准则；

假设失准角在导航坐标系中的分量分别为φ_E、φ_N、φ_U，则总误差水平可表示为：

则加性四元数与导航坐标系n系和计算坐标系p系的旋转矢量之间存在如下关系：

在大方位失准角条件下可认为φ_E、φ_N为小量，φ≈φ_U。当粗对准收敛后，有：

假定δq_i和φ_U都是无偏的，即其期望满足E(δq_i)＝0和E(φ_U)＝0，则航向角误差的方差可以表示为：

若设定由粗到精的切换阈值为φ_th，则切换条件可表示为：

3.平稳过渡方案；

采用滑动窗口法，设定长度为Δt的时间窗口，只有当滑动窗口内的失准角均满足阈值条件时，才判定粗对准完成。

本发明技术解决方案：一种基于加性四元数的大方位失准角线性对准方法，步骤如下：

(1)选择经度误差，纬度误差，高程误差，东向、北向和天向速度误差，加性四元数误差作为系统状态变量，根据粗对准的状态和观测方程，利用GPS观测信息，通过卡尔曼滤波，进行粗对准，其中，状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成；

(2)在粗对准期间，采用滑动窗口法来进行切换准则判定，即设定固定长度的时间窗口，在窗口内，直接由加性四元数计算当前航向角收敛水平，并判断航向角误差是否全部满足阈值条件；若满足，则保存对准系统的协方差阵，转入精对准；若不满足，则继续步骤(1)的粗对准；

(3)延续粗对准的系统状态变量及其位置误差方程、姿态误差方程和观测方程，使用小角度下的线性化速度误差方程，并将保存的协方差阵作为卡尔曼滤波的初始条件，进行精对准，使精对准能快速进入收敛状态，至对准达到预期水平。

所述步骤(1)中，所述卡尔曼滤波为线性卡尔曼滤波。

所述步骤(2)中的阈值条件为： 表示计算得到的加性四元数，表示加性四元数误差，φ_th为设定的航向角阈值。

所述步骤(3)中的小角度为小于等于5°。

本发明与现有技术相比的优点在于：

(1)本发明分为粗对准和精对准过程，均采用线性化误差模型，避免非线性滤波；这样采用线性滤波方案，易于工程实现。

(2)采用加性四元数描述系统姿态，计算简便，无奇异性，适用于全状态。

(3)本发明的粗对准和精对准过程采用相同的系统状态变量，粗对准完成时的协方差阵可以直接作为精对准的初始条件，精对准过程收敛快。

(4)采用加性四元数构造模型切换准则，可以由四元数直接导出航向角误差水平，实现阈值判别，有效实现对粗对准收敛状态的度量，避免由四元数求解欧拉角的复杂过程。

(5)采用滑动窗口方案判断阈值条件，保证线性化假设成立，避免提前切换造成系统发散。

附图说明

图1为本发明方法实现流程图；

图2为本发明的切换准则检验流程图。

具体实施方式

如图1所示，本发明具体实现如下：

(1)粗对准过程

定义为载体坐标系b系到导航坐标系n系的转动四元数(各元素记为q₀,q₁,q₂,q₃)，为计算四元数(各元素记为)，表示任意变量x的计算值，‘δx’表示任意变量x的误差。定义加性四元数误差(AQE)为计算四元数与真实四元数的差，记为：

系统状态变量可以表示为：

x＝[δq₀ δq₁ δq₂ δq₃ δv_e δv_n δv_u δL δλ δh]^T (7)

姿态误差方程：

其中，为陀螺的理论测量值在载体系的投影，表示导航坐标系的角速度的真实值。

式中：

线性化后的速度误差方程：

其中，vⁿ＝[v_e v_n v_u]^T是速度在载体系上的投影，是载体系到导航系的姿态转换矩阵，f^b是比力测量值，是地球自转角速率在导航系的投影，是导航系相对地球坐标系的角速度在导航系的投影，gⁿ是重力加速度在导航系的投影。

位置误差方程：

其中L、λ和h分别为纬度、经度和高度，R_M,R_N分别是子午圈半径和卯酉圈半径。

动基座对准中采用GPS输出的速度和位置作为观测量，观测方程如下：

式中

p_GPS和v_GPS分别为GPS提供的位置和速度；δp_GPS和δv_GPS为GPS的位置和速度误差，构成观测噪声n；p_IMU和v_IMU分别为INS提供的位置和速度；0_m×n和I_k分别表示大小为m×n的零矩阵和k×k的单位矩阵。

最后，通过系统状态方程和观测方程构成线性卡尔曼滤波器，进行粗对准过程。

(2)切换准则检验

在粗对准过程开始后，采用滑动窗口法判别阈值条件，具体执行过程如下：

首先，记录当前时刻t1，作为窗口时间起点；

其次，由四元数，计算每一时刻的航向角误差标准差，并与设定阈值对比。若在设定的Δt时间长度内，每一时刻的航向角误差标准差均满足阈值条件，则判定符合切换准则，记录当前时刻粗对准的协方差阵，转入精对准过程；若Δt时间长度内，出现任意时刻的航向角误差标准差不满足阈值条件，则更新t1，滑动窗口，重新判定。

(3)精对准过程

当粗对准角度收敛到小角度时，切换到精对准方程。精对准方程只需将粗对准的速度误差方程调整为如下即可：

将记录的粗对准的协方差阵作为精度准系统的滤波初值。

提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (4)

1.一种基于加性四元数的大方位失准角线性对准方法，其特征在于步骤如下：

(1)选择经度误差，纬度误差，高程误差，东向、北向和天向速度误差，加性四元数误差作为状态变量，根据状态方程和观测方程，利用GPS观测信息，通过卡尔曼滤波，进行粗对准，其中状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成；

(2)在粗对准期间，采用滑动窗口法来进行切换准则判定，即设定固定长度的时间窗口，在窗口内，直接由加性四元数计算当前航向角收敛水平，并判断航向角误差是否全部满足阈值条件；若满足，则保存协方差阵，转入精对准；若不满足，则继续步骤(1)的粗对准；

(3)延续步骤(1)的状态变量及其位置误差方程、姿态误差方程和观测方程，使用小角度下的线性化速度误差方程，并将保存的协方差阵作为卡尔曼滤波的初始条件，进行精对准，使精对准能快速进入收敛状态，至对准达到预期水平。

2.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法，其特征在于：所述步骤(1)中，所述卡尔曼滤波为线性卡尔曼滤波。

3.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法，其特征在于：所述步骤(2)中的阈值条件为： 表示计算得到的加性四元数，表示加性四元数误差，φ_th为设定的航向角阈值。

4.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法，其特征在于：所述步骤(3)中的小角度为小于等于5°。