CN113959462A - 一种基于四元数的惯性导航***自对准方法 - Google Patents

Abstract

本发明提出一种基于四元数的惯性导航***自对准的方法，包括以下步骤：基于凝固惯性系的粗对准技术执行惯性导航***粗对准，基于卡尔曼滤波的精对准技术执行惯性导航***精对准，基于优化迭代的回溯对准技术执行回溯对准；在回溯对准过程中，首先利用保存的惯性陀螺和加速度计的测量数据进行姿态反推，反推到起始时刻的初始角度信息，然后执行精对准滤波步骤，反复循环运算直到两次对准结果航向角的差值小于预设的精度偏差值时退出回溯对准；或者当回溯次数大于10次时退出回溯对准。

Description

一种基于四元数的惯性导航***自对准方法

技术领域

本发明属惯性导航技术领域，涉及一种基于四元数的惯性导航***自对准方法。

背景技术

惯性导航***在进入导航之前，必须进行初始对准，初始对准误差是惯性导航***的主要误差源之一，它直接影响惯性导航***的性能。初始对准时间是反映***快速反应能力的重要技术指标，尤其对于军事应用领域，要求初始对准精度高、对准时间短，即精而快。然而，较高的对准精度往往需要较长的对准时间，因此如何解决对准精度和快速性之间的矛盾一直是初始对准的一项关键技术。

目前，通常是将初始对准划分为粗对准和精对准两个过程。在众多粗对准算法中，解析式粗对准是一种重要的方法，但在初始对准过程中，信号采集容易受到车辆振动、阵风、人员走动等外界环境干扰，产生较大的测量噪声，传统的解析粗对准方法在基座晃动条件下抗干扰能力差，甚至失效。其根本原因在于晃动干扰角速率远大于地球自转角速率，无法从陀螺测量中分离出有效的地球自转信息。为解决晃动下的初始对准问题，提出了基于凝固惯性系的粗对准方案，该方法可以有效抑制角运动干扰对对准精度的影响。

精对准的目的，是在粗对准的基础上精确估计失准角，以便得到更加准确的初始姿态矩阵。近年来，现代控制理论的一些方法在惯性导航***中有了成功的应用，其中之一就是运用现代控制理论中的卡尔曼滤波进行惯性导航***的初始对准。运用卡尔曼滤波的初始对准方法是在平台粗对准的基础上进行的，运用卡尔曼滤波将惯性导航***失准角估计出来，然后根据估计结果将失准角消除掉，提高对准精度。

发明内容

为解决上述技术问题，本发明提出了一种基于四元数的惯性导航***自对准的方法，所述方法包括以下步骤：

步骤1，基于凝固惯性系的粗对准技术执行惯性导航***粗对准，包括：根据惯性陀螺和加速度计测得的角速度信息和比力信息，求解两个不同时刻的不共线矢量g(t1)和g(t2)在不同坐标系下的积分结果，进而求解对应时刻的姿态矩阵；并根据当前姿态矩阵与欧拉角的关系式：解算出当前的三个姿态角；

所述姿态角包括：横滚角、航向角和俯仰角；

所述不同坐标系包含：导航惯性坐标系和载体惯性坐标系；

步骤2，基于卡尔曼滤波的精对准技术执行惯性导航***精对准，建立***状态方程以及量测方程，并利用速度误差与失准角之间的关系完成三个失准角的误差估计，使用估计的失准角修正姿态矩阵；

步骤3，基于优化迭代的回溯对准技术执行回溯对准；首先利用保存的惯性陀螺和加速度计的测量数据进行姿态反推，反推到起始时刻的初始角度信息，然后按照步骤2执行精对准滤波，反复循环运算直到两次对准结果航向角的差值小于预设的精度偏差值时退出回溯对准；或者当回溯次数大于10次时退出回溯对准。

进一步的，步骤1包括子步骤：

步骤1.1，定义惯性凝固坐标系，在对准起始时刻，n0是将北天东导航系n相对惯性空间凝固所得的导航惯性坐标系；b0是将前上右载体系b相对惯性空间凝固所得的载体惯性坐标系；

步骤1.2，确定载体实时姿态矩阵与惯性系常值矩阵的关系式，其中惯性系常值矩阵作为待求矩阵；

步骤1.3，将当前时刻姿态矩阵代入姿态矩阵与欧拉角的关系式，求出横滚角、航向角和俯仰角。

进一步的，计算两个不同时刻的不共线矢量时，该两个时刻的时间差值大于等于30秒。

进一步的，步骤2包括子步骤：

步骤2.1，依据速度误差方程、失准角误差方程、陀螺误差模型及加速度计误差模型建立***状态方程；

步骤2.2，建立精确对准***量测方程；

步骤2.3，基于优化迭代方式执行回溯对准计算。

进一步的，步骤3包括子步骤：

步骤3.1，保存对准过程中陀螺的测量数据，以及加速度计的测量数据；

步骤3.2，执行姿态反推计算，包括依据地球自转角速度以及当前时刻的四元数反推中间时刻四元数；依据存储的陀螺角速度信息以及中间时刻四元数反推上一时刻的四元数；

步骤3.3，反推到对准起始时刻后，按照步骤2进行精对准滤波。

进一步的，所述预设的精度偏差值为0.005°。

采用本发明的方法，创新地提出一种基于四元数的惯性导航***自对准技术，在完成粗对准以及精对准后，通过增加回溯对准环节，实现初始对准精度高、对准时间短的需求，提高惯性导航精度。

基于四元数的惯性导航***自对准技术能够在短时间内实现高精度初始对准，解决对准精度和对准时间之间的矛盾难题。

附图说明

图1为重力加速度在惯性空间的旋转示意图。

图2为精对准水平姿态角和航向角的收敛情况。

图3为回溯对准横滚角、航向角以及俯仰角的收敛情况。

具体实施方式

本发明涉及一种基于四元数的惯性导航***自对准技术，特别涉及一种抗晃动基座的惯性导航***自对准技术，主要用途是实现惯性导航***高精度自对准。

与现有技术相比，本发明在完成粗对准以及精对准后，通过增加回溯对准环节，实现对准时间不变的情况下提高初始对准精度的目的，解决对准精度和快速性之间的矛盾难题。

本发明的技术解决方案：一种基于四元数的惯性导航***自对准技术，在完成粗对准以及精对准后，通过增加回溯对准环节，实现初始对准精度高、对准时间短的需求，具体方法通过以下步骤实现：

第一步，基于凝固惯性系的粗对准技术。

首先利用惯性凝固假设定义如下坐标系：

n₀―导航惯性坐标系。在对准起始时刻，将北天东导航系n相对惯性空间凝固所得的惯性坐标系。

b₀―载体惯性坐标系。在对准起始时刻，将前上右载体系b相对惯性空间凝固所得的惯性坐标系。

根据方向余弦矩阵的链乘原理，载体实时姿态矩阵

可表示为：

式中

为惯性系常值矩阵，作为待求矩阵；

可通过陀螺输出求解；而

其中，e代表地球坐标系，在载***置无移动时为已知矩阵

式中t为对准经历时间，ω_ie代表地球自转角速度，L₀代表起始纬度。

矩阵

可根据陀螺测量数据

进行姿态更新得到：

其中积分初值为

将

作为待求矩阵，物理意义非常明确：重力矢量在b₀系投影

可由加速度计输出

测量，即

重力矢量在n₀系的投影

可根据对准位置按地球模型求解。在不同对准时刻t₁和t₂，从惯性系来看，重力矢量g(t)受地球自转影响，缓慢漂移。即在惯性坐标系n₀中，

与

不共线，且存在关系

根据双矢量定姿原理，只要获得两个不同时刻的不共线矢量g(t₁)和g(t₂)在n₀和b₀系的观测值，就可以确定姿态矩阵

考虑矩阵

和

已知，则

和

可以求解

为了不丢失信息并滤除量测噪声，可对上式进行积分运算，得到两个不同时刻的积分结果，

利用TRIAD算法可求解姿态矩阵

将上式得到的矩阵

带入到式(1-1)中即可求得当前时刻的姿态矩阵

也即完成了粗对准。

第二步，基于卡尔曼滤波的精对准技术。

精对准技术即利用Kalman滤波器融合惯性速度误差信息完成失准角φ的估计，***状态方程以及量测方程如下所示。

1)***状态方程

考虑速度误差方程、失准角误差方程、陀螺及加速度计误差模型，***状态方程为：

其中，

为状态矢量，δv_x,δv_y,δv_z分别代表北天东速度误差，φ_x,φ_y,φ_z分别代表XYZ三个方向的失准角大小，ε_x,ε_y,ε_z分别代表陀螺常值零偏，

分别代表加速度计常值零偏，W(t)＝[w_ax w_ayw_az w_gx w_gy w_gz]为噪声矢量，w_ax,w_ay,w_az分别代表加速度计随机噪声，w_gx,w_gy,w_gz分别代表陀螺仪随机噪声，F(t)为状态矩阵，G(t)为噪声驱动矩阵。

F(t)和G(t)如下所示：

F_N是速度误差方程和失准角误差方程对应的***矩阵，其非零元素为：

F_N(1,6)＝g

F_N(3,4)＝-g

其中，

分别代表北天东三个方向的速度信息，L,H分别代表纬度信息和高度信息，R_M,R_N分别代表子午圈曲率半径信息和卯酉圈曲率半径信息。G(t)为对应的噪声驱动矩阵：

2)***量测方程

精对准量测方程为：

Z_k+1＝HX_k+1,k+V_k+1 (1-11)

其中，Z_k+1代表速度误差量测量，H为量测矩阵，V_k+1为量测噪声。

Z_k+1的表达式如下所示：

H的表达式如下所示：

第三步，基于优化迭代的回溯对准技术。

1)数据保存

为进行优化迭代，需保存对准过程中陀螺的测量数据，以及加速度计的测量数据。

2)姿态反推

a)依据地球自转角速度以及当前时刻的四元数

反推四元数

其中，

为旋转矢量Φ对应的四元数，且

Update_t为更新周期。

b)依据存储的陀螺角速度信息以及四元数

反推上一时刻的四元数

其中，

为旋转矢量Φ对应的对偶四元数，且

为陀螺测量的三个轴向角速度数据。

3)正向对准

反推到对准起始时刻后，按照第二步进行精对准滤波，反复循环直到两次对准结果航向角的差值小于0.005°时则退出回溯对准，或者当回溯大于10次时退出回溯对准。

以下结合附图对本发明的具体实施方式作出详细说明。

第一步，基于凝固惯性系的粗对准技术。

首先利用惯性凝固假设定义如下坐标系：

n₀―导航惯性坐标系。在对准起始时刻，将北天东导航系n相对惯性空间凝固所得的惯性坐标系。

b₀―载体惯性坐标系。在对准起始时刻，将前上右载体系b相对惯性空间凝固所得的惯性坐标系。

根据方向余弦矩阵的链乘原理，载体实时姿态矩阵

可表示为：

式中

为惯性系常值矩阵，作为待求矩阵；

可通过陀螺输出求解；而

在载***置无移动时为已知矩阵

式中t为对准经历时间，ω_ie代表地球自转角速度，L₀代表起始纬度。

矩阵

可根据陀螺数据

进行姿态更新得到

其中积分初值为

将

作为待求矩阵，物理意义非常明确：重力矢量在b₀系投影

可由加速度计输出

测量，即

重力矢量在n₀系的投影

可根据对准位置按地球模型求解。如图1所示，在不同对准时刻t₁和t₂，从惯性系来看，重力矢量g(t)受地球自转影响，沿图1所示的锥面缓慢漂移。即在惯性坐标系n₀中，

与

不共线，且存在关系

根据双矢量定姿原理，只要获得两个不同时刻的不共线矢量g(t₁)和g(t₂)在n₀和b₀系的观测值，就可以确定姿态矩阵

考虑矩阵

和

已知，则

和

可以求解

为了不丢失信息并滤除量测噪声，可对上式进行积分运算，得到两个不同时刻的积分结果，

利用TRIAD算法可求解姿态矩阵

将上式得到的矩阵

带入到式(1-1)中即可求得当前时刻的姿态矩阵

也即完成了粗对准。

第二步，基于卡尔曼滤波的精对准技术。

精对准技术即利用Kalman滤波器融合惯性速度误差信息完成失准角φ的估计，***状态方程以及量测方程如下所示。

1)***状态方程

考虑速度误差方程、失准角误差方程、陀螺及加速度计误差模型，***状态方程为：

其中，

为状态矢量，δv_x,δv_y,δv_z分别代表北天东速度误差，φ_x,φ_y,φ_z分别代表XYZ三个方向的失准角大小，ε_x,ε_y,ε_z分别代表陀螺常值零偏，

分别代表加速度计常值零偏，W(t)＝[w_ax w_ayw_az w_gx w_gy w_gz]为噪声矢量，w_ax,w_ay,w_az分别代表加速度计随机噪声，w_gx,w_gy,w_gz分别代表陀螺仪随机噪声，F(t)为状态矩阵，G(t)为噪声驱动矩阵。

F(t)和G(t)如下所示：

F_N是速度误差方程和失准角误差方程对应的***矩阵，其非零元素为：

F_N(1,6)＝g

F_N(3,4)＝-g

其中，

分别代表北天东三个方向的速度信息，L,H分别代表纬度信息和高度信息，R_M,R_N分别代表子午圈曲率半径信息和卯酉圈曲率半径信息。G(t)为对应的噪声驱动矩阵：

2)***量测方程

精对准量测方程为：

Z_k+1＝HX_k+1,k+V_k+1 (1-11)

其中，Z_k+1代表速度误差量测量，H为量测矩阵，V_k+1为量测噪声。

Z_k+1的表达式如下所示：

H的表达式如下所示：

第三步，基于优化迭代的回溯对准技术。

1)数据保存

为进行优化迭代，需保存对准过程中陀螺的测量数据，以及加速度计的测量数据。

2)姿态反推

a)依据地球自转角速度以及当前时刻的四元数

反推四元数

其中，

为旋转矢量Φ对应的四元数，且

Update_t为更新周期。

b)依据存储的陀螺角速度信息以及四元数

反推上一时刻的四元数

其中，

为旋转矢量Φ对应的对偶四元数，且

为陀螺测量的三个轴向角速度数据。

3)正向对准

反推到对准起始时刻后，按照第二步进行精对准滤波，反复循环直到两次对准结果航向角的差值小于0.005°时则退出回溯对准，或者当回溯大于10次时退出回溯对准。、

实施例一，

为了验证算法的准确性，利用试验数据进行了离线仿真验证，对准时间在275秒以内，粗对准时间为60秒。

第一步，利用基于凝固惯性系的粗对准技术完成惯性导航***粗对准。

根据惯性陀螺和加速度计测得的角速度信息和比力信息按照公式(1-6)求解两个不同时刻的积分结果，其中，t1设定为30秒，t2设定为60秒。然后按照公式(1-7)求解姿态矩阵

最后，按照公式(1-1)求解当前时刻的姿态矩阵

并根据姿态矩阵

与欧拉角的关系：

求解三个姿态角，横滚角(γ)、航向角(ψ)、俯仰角(θ)分别为：-0.2571°，-119.82097°，0.40511°。

第二步，利用基于卡尔曼滤波的精对准技术完成惯性导航***精对准。

首先建立***状态方程以及量测方程，并利用速度误差与失准角之间的关系完成三个失准角的误差估计，利用估计的失准角修正姿态矩阵C_b ⁿ，提高对准精度，精对准结果如图2所示。

第三步，利用基于优化迭代的回溯对准技术完成回溯对准。

首先利用保存的陀螺和加速度计的测量数据完成姿态反推至起始时刻，然后按照第二步进行精对准滤波，反复循环直到两次对准结果航向角的差值小于0.005°时则退出回溯对准，或者当回溯大于10次时退出回溯对准。回溯对准结果如图3所示，由试验结果可以看出，经过三次回溯迭代后，两次航向角对准结果的差值小于0.005°退出回溯对准，而三次回溯对准中横滚角和俯仰角差别不大。

最后应说明的是，以上实施方式仅用以说明本发明实施例的技术方案而非限制，尽管参照以上较佳实施方式对本发明实施例进行了详细说明，本领域的普通技术人员应当理解，可以对本发明实施例的技术方案进行修改或等同替换都不应脱离本发明实施例的技术方案的精神和范围。

Claims (6)

1.一种基于四元数的惯性导航***自对准的方法，其特征在于，所述方法包括以下步骤：

步骤1，基于凝固惯性系的粗对准技术执行惯性导航***粗对准，包括：根据惯性陀螺和加速度计测得的角速度信息和比力信息，求解两个不同时刻的不共线矢量g(t₁)和g(t₂)在不同坐标系下的积分结果，进而求解对应时刻的姿态矩阵；并根据当前姿态矩阵与欧拉角的关系式，解算出当前的三个姿态角；

所述姿态角包括：横滚角、航向角和俯仰角；

所述不同坐标系包含：导航惯性坐标系和载体惯性坐标系；

步骤2，基于卡尔曼滤波的精对准技术执行惯性导航***精对准，建立***状态方程以及量测方程，并利用速度误差与失准角之间的关系完成三个失准角的误差估计，使用估计的失准角修正姿态矩阵；

步骤3，基于优化迭代的回溯对准技术执行回溯对准；首先利用保存的惯性陀螺和加速度计的测量数据进行姿态反推，反推到起始时刻的初始角度信息，然后按照步骤2执行精对准滤波，反复循环运算直到两次对准结果航向角的差值小于预设的精度偏差值时退出回溯对准；或者当回溯次数大于10次时退出回溯对准。

2.如权利要求1所述的方法，其特征在于，步骤1包括子步骤：

步骤1.1，定义惯性凝固坐标系，在对准起始时刻，n₀是将北天东导航系n相对惯性空间凝固所得的导航惯性坐标系；b₀是将前上右载体系b相对惯性空间凝固所得的载体惯性坐标系；

步骤1.2，确定载体实时姿态矩阵与惯性系常值矩阵的关系式，其中惯性系常值矩阵作为待求矩阵；

步骤1.3，将当前时刻姿态矩阵代入姿态矩阵与欧拉角的关系式，求出横滚角、航向角和俯仰角。

3.如权利要求2所述的方法，其特征在于，计算两个不同时刻的不共线矢量时，该两个时刻的时间差值大于等于30秒。

4.如权利要求1所述的方法，其特征在于，步骤2包括子步骤：

步骤2.1，依据速度误差方程、失准角误差方程、陀螺误差模型及加速度计误差模型建立***状态方程；

步骤2.2，建立精确对准***量测方程；

步骤2.3，基于优化迭代方式执行回溯对准计算。

5.如权利要求4所述的方法，其特征在于，步骤3包括子步骤：

步骤3.1，保存对准过程中陀螺的测量数据，以及加速度计的测量数据；

步骤3.2，执行姿态反推计算，包括依据地球自转角速度以及当前时刻的四元数反推中间时刻四元数；依据存储的陀螺角速度信息以及中间时刻四元数反推上一时刻的四元数；

步骤3.3，反推到对准起始时刻后，按照步骤2进行精对准滤波。

6.如权利要求5所述的方法，其特征在于，所述预设的精度偏差值为0.005°。

Images