CN110779551A

CN110779551A - 一种基于加性四元数的两阶段线性对准在线切换方法

Info

Publication number: CN110779551A
Application number: CN201911095442.8A
Authority: CN
Inventors: 王可东
Original assignee: Nanjing Hello Tour Communications Technology Co Ltd
Current assignee: Nanjing Hello Tour Communications Technology Co Ltd
Priority date: 2019-11-11
Filing date: 2019-11-11
Publication date: 2020-02-11

Abstract

本发明涉及一种基于加性四元数的两阶段线性对准在线切换方法，步骤：(1)选择经度误差、纬度误差、高程误差、东向速度误差、北向速度误差、天向速度误差和姿态角的加性四元数误差作为状态变量，构建状态方程和观测方程，利用GNSS接收机观测信息，通过线性卡尔曼滤波，进行粗对准过程；(2)在滑动窗口内，由加性四元数直接估计失准角的收敛情况，如果满足收敛阈值条件，则保存协方差阵，转入精对准；若不满足，则继续步骤(1)的粗对准。本发明可以实现由粗对准向精对准的平稳切换，既适用于导航坐标系为地理坐标系的情况，又适用于导航坐标系为地球固联坐标系的情况，应用方便。

Description

一种基于加性四元数的两阶段线性对准在线切换方法

技术领域

本发明涉及一种基于加性四元数的两阶段线性对准在线切换方法，属于惯性导航技术领域。

背景技术

惯性导航系统(Inertial Navigation System，INS)由于能同时提供姿态、速度和位置等全导航信息，且完全自主，抗干扰性能好，得到了广泛应用。特别是随着微电子加工技术的快速发展，基于MEMS(Micro-Electro-Mechanical System)加工的陀螺仪和加速度计精度的提升和价格的降低，为基于MEMS器件的INS的更广泛应用奠定了坚实的基础。

由于INS基于的是积分式工作原理，因此，在进行正式导航解算之前，需要进行初始姿态、速度和位置的确定，即初始对准。由于初始速度和位置可以通过卫星导航(GlobalNavigation Satellite System，GNSS)接收机或其它方式确定，初始对准的主要任务是完成初始姿态的确定。传统高精度的INS可以通过对地球自转角速度和重力加速度的静态测量，实现对初始姿态的高精度确定。但是，对于精度较低的MEMS INS，通过静基座对准只能完成初始俯仰角和滚转角的较高精度确定，初始方位角则无法通过静基座对准进行精确确定，从而造成了初始大方位失准角的问题，其中一种解决方法是利用GNSS接收机输出的速度和位置，进行动基座的初始对准。

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

常用的姿态角描述方法有欧拉角法和四元数法，对准方案也可据此分为两类。欧拉角法用于描述姿态角的优点在于直观性，各参数物理意义明确，但其存在奇异性问题。现有的技术方案中，除直接进行非线性滤波外，主要是将对准过程分为粗对准和精对准两个过程，并在粗对准中用航向角误差的正余弦项作为系统状态变量，实现系统方程的线性化；在精对准中则采用小角度近似，实现线性化，从而进行线性滤波。

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

因此，现有的技术方案中，非线性滤波模型计算量大，不适宜工程应用；欧拉角法在计算复杂度上要高于四元数，且存在奇异性问题；虽然加性四元数法计算简单，且不存在奇异性问题，但速度误差方程仍然是非线性的，不利于对准过程的快速收敛和对准精度的提升。针对上述问题，专利“一种基于加性四元数的大方位失准角线性对准方法.中国发明专利，专利号:ZL201610835249.3”提出了一种基于加性四元数的线性化对准方法，其中基于加性四元数的姿态误差和位置误差方程是线性的，对非线性的速度误差方程进行了线性化处理，然后采用线性卡尔曼滤波算法，进行粗对准和精对准，并提出了一种直接由加性四元数计算当前方位角(也称为航向角)收敛水平的粗对准向精对准在线切换准则。在该切换准则中，在完成当前方位角误差估计之后，要求在滑动窗口内所有估计的方位角误差小于设定的阈值，才判定粗对准收敛，转入精对准阶段。但是，在实际应用中发现，单次方位角误差的估计结果波动范围非常大，从而导致该切换准则受单次方位角误差的估计结果影响严重，切换结果趋于保守，导致整个粗对准收敛时间加长。另外，该切换准则只适用于地理坐标系为导航坐标系的情况，而如果采用地球固联坐标系作为导航坐标系，将不能利用该切换准则。因此，在确认粗对准收敛后，如何尽快切换到精对准阶段，以有效缩短粗对准时间，是加快对准快速性的关键；如果切换准则也适用于导航坐标系为地球固联坐标系，则有利于降低实时计算量。

发明内容

本发明技术解决问题：克服了现有技术中由粗对准向精对准切换准则过于保守的问题，提供了一种基于加性四元数的两阶段线性对准在线切换方法，基于该切换方法可以实现由粗对准向精对准的平稳切换，既适用于导航坐标系为地理坐标系的情况，又适用于导航坐标系为地球固联坐标系的情况，应用方便。

本发明的技术要点：

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

2.提出一种粗对准到精对准的模型在线切换准则，该准则既适用于导航坐标系为地理坐标系的情况，也适用于导航坐标系为地球固联坐标系的情况。

本发明技术解决方案：一种基于加性四元数的两阶段线性对准在线切换方法，步骤如下：

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

(2)在粗对准期间，采用滑动窗口法来进行切换准则判定，当选择地理坐标系n系为导航坐标系时，采用如下切换准则：

其中：

其中：

和分别表示由载体坐标系b系转到n系的理论的加性四元数和计算得到的加性四元数，q_i和

分别为

和

的元素，

i＝0,1,2,3，

表示从T时刻开始的第i个失准角估计值，Δφ_th为设定的失准角阈值，N为滑动窗口的宽度；

当选择地球固联坐标系e系为导航坐标系时，采用如下切换准则：

其中：

其中：

和

分别表示由b系转到e系的理论的加性四元数和计算得到的加性四元数，q_i和

分别为

和

的元素，

i＝0,1,2,3；

在设定的固定长度时间窗口内，如果满足切换准则，则保存协方差阵，转入精对准；若不满足，则继续步骤(1)的粗对准。

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

所述步骤(1)中，所述GNSS接收机为GPS接收机、北斗接收机、Galileo接收机、GLONASS接收机或兼容接收机。

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

(1)本发明采用滑动窗口内失准角估计值的平均值作为切换判定依据，大大降低了单次失准角估计值的波动对切换准则的影响，既保证了切换判定的准确性，又保证了切换判定的实时性。

(2)本发明提出的切换准则是基于失准角的估计构建的，因而可以适用于各种导航坐标系，比如既适用于导航坐标系为地理坐标系的情况，又适合于导航坐标系为地球固联坐标系的情况。

附图说明

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

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

具体实施方式

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

(1)地理坐标系中的状态方程和观测方程构建

在不同的导航坐标系中，状态方程是不一样的，在本发明中针对最常见的地理坐标系和地球固联坐标系，分别构建状态方程、观测方程和相应的两阶段线性对准在线切换准则。

当选择“东-北-天”地理坐标系为导航坐标系n系时，姿态误差方程为：

其中：

和

分别表示由载体坐标系b系转到n系的理论的加性四元数和计算得到的加性四元数，q_i和

分别为

和的元素，

i＝0,1,2,3，

为陀螺的理论测量值在b系的投影，

表示n系的理论角速度，

为n系的真实角速度，且有：

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

其中：vⁿ＝[v_e v_n v_u]^T是速度在n系的投影，v_e、v_n和v_u分别为东向、北向和天向速度，δv_n为速度误差，

是b系到n系的姿态转换矩阵，

为计算的姿态转换矩阵，

是比力在b系的投影，δf^b为加速度计误差，失准角在n系的投影为

Δφ_e、Δφ_n和Δφ_u分别为东向、北向和天向失准角，

是地球自转角速率在n系的投影，

是n系相对地球固联坐标系e系的角速度在n系的投影，和

分别为

和

的误差，δgⁿ是重力加速度模型误差在n系的投影，且有：

位置误差方程：

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

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

Xⁿ＝[δq₀ δq₁ δq₂ δq₃ δv_e δv_n δv_u δL δλ δh]^T (14)

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

式中：

其中：p_GNSS和v_GNSS分别为GNSS接收机提供的位置和速度；δp_GNSS和δv_GNSS为GNSS接收机提供的位置和速度的误差，构成观测噪声nⁿ；p_IMU和v_IMU分别为INS提供的位置和速度；0_m×n和I_k分别表示大小为m×n的零矩阵和k×k的单位矩阵。

(2)地球固联坐标系中的状态方程和观测方程构建

当选择地球固联坐标系e系为导航坐标系时，姿态误差方程为：

其中：

和

分别为

和

的元素，

i＝0,1,2,3，

地球自转角速度ω_ie＝15.041°/h，

表示e系的角速度的真实值，且有：

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

其中：v^e＝[v_x v_y v_z]^T是速度在e系上的投影，δv^e为速度误差在e系上的投影，

是b系到e系的姿态转换矩阵，δg^e是重力加速度模型误差在e系的投影，且有：

其中：

表示四元数乘法，

为的共轭复数。

位置误差方程：

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

X^e＝[δq₀ δq₁ δq₂ δq₃ δv_x δv_y δv_z δx δy δz]^T (25)

对准中采用GNSS接收机输出的速度和位置作为观测量，观测方程如下：

其中：r_GNSS和r_IMU分别为GNSS接收机和INS提供的位置，δr_GNSS为GNSS接收机位置误差，且有：

(3)粗对准阶段

在两种导航坐标系下，由于采用线性化处理，系统状态方程和观测方程都是线性的，因此，在粗对准阶段，采用线性卡尔曼滤波器，进行粗对准。

(4)切换准则检验

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

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

其次，当每个滤波周期结束后，在地理坐标系中，失准角的估计如下：

其中：表示从T时刻开始的第i个失准角估计值。在地球固联坐标系中，失准角的估计如下：

第三，当第N个失准角估计值计算完成，计算失准角估计值的平均值如下：

最后，若计算的失准角估计值的平均值小于设定的阈值Δφ_th，即

则判定符合切换准则，记录当前时刻粗对准的协方差阵，转入精对准阶段；若计算的失准角估计值的平均值大于等于设定的阈值，则判定不满足切换准则，则更新T，滑动窗口，重新判定。

(5)精对准阶段

当粗对准角度收敛到小角度时，切换到精对准阶段，由于在两种导航坐标系中，精对准状态方程和观测方程均为线性，因此，仍然利用线性卡尔曼滤波器进行对准。

车载实验表明，采用现有的专利技术，粗对准过程通常需要100s～150s，而采用本发明的在线切换准则，粗对准通常只需要40s～60s即可完成，准确判定了粗对准的收敛结果。

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

Claims

1.一种基于加性四元数的两阶段线性对准在线切换方法，其特征在于，步骤如下：

其中：

其中：

和

分别为

和

的元素，

其中：

其中：

和分别表示由b系转到e系的理论的加性四元数和计算得到的加性四元数，q_i和

分别为和

的元素，

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

3.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法，其特征在于：所述步骤(1)中，所述GNSS接收机为GPS接收机、北斗接收机、Galileo接收机、GLONASS接收机或兼容接收机。