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

Abstract

本发明涉及一种基于加性四元数的大方位失准角两阶段线性对准方法，选择“东‑北‑天”坐标系作为导航坐标系，先静止一段时间，通过测量当地的重力加速度垂线方向，估计初始俯仰角和滚转角，完成初始调平；构建粗对准的状态和观测方程，利用GNSS接收机的速度和位置观测信息，通过卡尔曼滤波，完成粗对准；在精对准阶段，仍然采用粗对准的系统状态变量及其位置误差方程、姿态误差方程和观测方程，但三个失准角的初值均设为小角度，并将保存的协方差阵作为卡尔曼滤波的初始条件，进行精对准。本发明既能实现粗对准的快速收敛，又有利于精对准精度的提升，且计算简便。

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.在进行粗对准之前，先进行静态调平，使得粗对准时俯仰角和滚转角的误差(失准角)为小角度，只有方位角的误差是大角度；

3.针对速度误差方程中的非线性项，进行线性化处理，在粗对准阶段，俯仰角和滚转角的误差初值按小角度设置，而方位角的误差初值按大角度设置；在精对准阶段，三个失准角的初值均按小角度设置。

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

(1)选择“东-北-天”坐标系作为导航坐标系，先静止一段时间，通过测量当地的重力加速度垂线方向，估计初始俯仰角和滚转角，完成初始调平即静态调平；

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

其中：vⁿ＝[v_e v_n v_u]^T是速度在导航系n系上的投影，δvⁿ为速度误差，

失准角在导航系上的投影为

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

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

为计算的姿态转换矩阵，

是比力在b系上的投影，δf^b为加速度计误差，为b系到n系的转动四元数，

为计算四元数，

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

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

和

分别为

和

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

(3)在粗对准阶段，设Δφ_e和Δφ_n的初值为小角度，而Δφ_u的初值为大角度；粗对准完成后，转入步骤(4)，进行精对准；

(4)在精对准阶段，仍然采用粗对准的系统状态变量及其位置误差方程、姿态误差方程和观测方程，但三个失准角Δφ_e、Δφ_n和Δφ_u的初值均设为小角度，并将保存的协方差阵作为卡尔曼滤波的初始条件，进行精对准。

所述步骤(1)中的静止一段时间通常为10s～20s。

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

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

所述步骤(3)中的大角度为120°～150°。

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

(1)本发明将整个对准过程分为静态调平、粗对准和精对准三个阶段过程，经过静态调平，后续的粗对准阶段俯仰角和滚转角的误差初值可认为是小角度，大大降低了线性化难度。

(2)针对粗对准和精对准的方位失准角初值的不同，分别采用不同的线性化参数，以提高速度误差方程的线性化精度，有利于加快对准收敛速度和提高对准精度。

(3)本发明既能实现粗对准的快速收敛，又有利于精对准精度的提升，且计算简便。

附图说明

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

图2为本发明的静态调平过程流程图。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。

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

(1)静态调平过程，如图2所示。

将INS静止放置，其三个加速度计在载体坐标系b系中的比力输出为

设其在一段时间内的均值为

应用中该时间段通常在10s～20s之间，那么，估计的初始俯仰角

和滚转角

可分别计算如下：

(2)粗对准过程

定义

为b系到导航坐标系n系的转动四元数，

为计算四元数，

“x”表示任意变量x的计算值，“δx”表示任意变量x的误差。定义加性四元数误差(Additive Quaternion Error，AQE)为计算四元数与真实四元数的差，记为：

姿态误差方程：

其中：

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

表示n系的理论角速度，

为n系的真实角速度。

式中：

速度误差方程：

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

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

为计算的姿态转换矩阵，

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

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

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

分别为

和

的误差，gⁿ是重力加速度在n系的投影。在式(9)中，

为非线性项，其可等效为：

其中：失准角在n系的投影为

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

在现有的专利技术中，将c_n近似为0，且将R_RR_R非主对角线上的元素均设为0，从而将式(10)线性化为：

在现有的专利技术中，在粗对准阶段，a₁₁和a₂₂设为2，a₃₃设为0；在精对准阶段，a₁₁、a₂₂和a₃₃均设为0。但是，实际上只有在Δφ₀趋于180°时，c_n才近似为0，而大方位失准角并不意味着是趋于180°，而R_RR_R非主对角线上的元素并不为0，特别是在精对准阶段，这种近似将导致较大的线性化误差。针对这些问题，本发明提出如下的线性化方程：

且有：

相比较于式(15)，在式(16)中未将c_n近似为0，并保留了R_RR_R非主对角线上的元素，因而，线性化误差更小，从而有利于提高对准的收敛速度和精度。这些参数将由Δφ_e、Δφ_n和Δφ_u确定，在本发明中，在粗对准阶段，Δφ_e和Δφ_n的初值设为小角度，而Δφ_u设为大角度；在精对准阶段，Δφ_e、Δφ_n和Δφ_u的初值均设为小角度。

位置误差方程：

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

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

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

动基座对准中采用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的单位矩阵。

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

在粗对准开始时，由于完成了静态调平，因此，设Δφ_e和Δφ_n为小角度，比如2°～5°，但是方位失准角Δφ_u是大角度，在粗对准开始时，其设为120°～150°。

(3)精对准过程

当粗对准角度收敛到小角度时，切换到精对准方程。精对准方程在形式上与粗对准过程完全一样，只是在精对准开始时，将三个失准角均设为2°～5°。

车载实验表明，在同样的条件下，采用现有的专利技术，在同样的对准精度下，对准收敛时间需要200s左右，而采用本发明技术，对准收敛时间只需要50s左右。

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

Claims (5)

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

(1)选择“东-北-天”坐标系作为导航坐标系，先静止一段时间，通过测量当地的重力加速度垂线方向，估计初始俯仰角和滚转角，完成初始调平，即静态调平；

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

其中：vⁿ＝[v_e v_n v_u]^T是速度在导航系n系上的投影，δvⁿ为速度误差，

失准角在导航系上的投影为

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

是载体坐标系b系到n系的姿态转换矩阵，

为计算的姿态转换矩阵，

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

为b系到n系的转动四元数，

为计算四元数，

是地球自转角速率在n系的投影，是n系相对地球坐标系的角速度在n系的投影，

和

分别为

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

(3)在粗对准阶段，设Δφ_e和Δφ_n的初值为小角度，而Δφ_u的初值为大角度；粗对准完成后，转入步骤(4)，进行精对准；

(4)在精对准阶段，仍然采用粗对准的系统状态变量及其位置误差方程、姿态误差方程和观测方程，但三个失准角Δφ_e、Δφ_n和Δφ_u的初值均设为小角度，并将保存的协方差阵作为卡尔曼滤波的初始条件，进行精对准。

2.根据权利要求1所述的基于加性四元数的大方位失准角两阶段线性对准方法，其特征在于：所述步骤(1)中的静止一段时间通常为10s～20s。

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

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

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

Images