CN113503892B - 一种基于里程计和回溯导航的惯导系统动基座初始对准方法 - Google Patents

Abstract

本发明公开了一种基于里程计和回溯导航的惯导系统动基座初始对准方法，包括以下步骤：采集惯导系统的惯导信息和里程计的速度信息；对惯导信息和速度信息进行补偿；进行第一次抗干扰初始对准，得到辅助姿态矩阵；根据辅助姿态矩阵进行第二次抗干扰初始对准，获得对准结束时刻的姿态矩阵；对所述对准结束时刻的姿态矩阵进行回溯导航解算，得到对准初始时刻的姿态矩阵；根据所述对准初始时刻的姿态矩阵，利用卡尔曼滤波算法，获得初始对准结束时刻的惯导姿态信息，完成惯导系统的动基座初始对准。本发明可以在短时间实现较高精度的动基座初始对准，对增强各种平台的机动性能，提高反应能力和自身生存能力具有重要意义。

Description

一种基于里程计和回溯导航的惯导系统动基座初始对准方法

技术领域

本发明涉及惯导系统动基座初始对准技术领域，尤其涉及一种基于里程计和回溯导航的惯导系统动基座初始对准方法，适用于使用里程计辅助量测和支持数据存储的惯导系统动基座初始对准，特别适合要求在短时间实现较高初始对准精度的应用场合。

背景技术

惯性导航系统是一种航位推算系统，初始对准是惯导进行导航的前提，初始对准的好坏将直接影响到系统的导航精度。传统的初始对准一般在静基座约束下进行，而快速动基座初始对准能有效增强各种平台的机动性能，提高反应能力和自身生存能力，在军事应用中具有重要意义。

现有较成熟的动基座对准方法通常借助卫星导航设备，利用其提供载体的运动速度和位置为参考，通过卡尔曼滤波等估计方法实现平台偏角和惯性器件漂移、零偏等的估计，实现动机座对准。流程上，先粗对准，通过加速度计量测信息确定水平姿态角，通过陀螺仪测量信息或者航迹确定方位角。在粗对准的基础上，以卫导的速度、位置为参考，利用卡尔曼滤波方法进行精对准。

考虑到在战时卫星导航系统不能完全依赖，因此需要研究基于里程计量测的动基座对准算法。里程计的量测信息不同于卫星导航系统，只能提供载体坐标系下的速度参考信息，现有的文献中使用里程计的初始对准算法中一般使用抗干扰对准算法，该算法需要限制对准时的载体机动(如尽量匀速直行)才能达到一定的对准精度，且后续还需要较长时间的精对准过程才能基本满足使用要求。因此为了进一步提高基于里程计的动基座初始对准精度，需要研究一种不限制载体机动，且能够在短时间内实现较高精度的对准方法，从而提高各种作战平台的机动性，赢得战机，提升军事装备的武器效能。

发明内容

有鉴于此，本发明提供了一种基于里程计和回溯导航的惯导系统动基座初始对准方法，该方法可以在短时间实现较高精度的动基座初始对准，对增强各种平台的机动性能，提高反应能力和自身生存能力具有重要意义。本发明使用里程计量测和回溯导航的基本思路，首先采集惯导系统和里程计的输出信息，在对陀螺和加速度计补偿后连同里程计数据保存下来。在此基础上通过两次抗干扰对准获得结束时刻的姿态矩阵，进而通过回溯导航获得较高精度的姿态矩阵初值，最终通过卡尔曼滤波实现动基座初始对准。

一种基于里程计和回溯导航的惯导系统动基座初始对准方法，具体包括以下步骤：

S1，采集惯导系统的惯导信息和里程计的速度信息；

S2，对惯导信息和速度信息进行补偿得到补偿数据；

S3，根据补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵；

S4，根据辅助姿态矩阵进行第二次抗干扰初始对准，获得对准结束时刻的姿态矩阵；

S5,对所述对准结束时刻的姿态矩阵进行回溯导航解算，得到对准初始时刻的姿态矩阵；

S6，根据所述对准初始时刻的姿态矩阵，利用卡尔曼滤波算法，获得初始对准结束时刻的惯导姿态信息，完成惯导系统的动基座初始对准。

优选地，步骤S3中根据补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵的具体步骤为：

建立初始时刻的载体惯性系和导航惯性系；

建立抗干扰对准模型；

根据所述抗干扰对准模型和补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵。

优选地，步骤S4中根据辅助姿态矩阵进行第二次抗干扰初始对准的具体步骤为：

调整抗干扰对准模型；

根据辅助姿态矩阵和调整后的抗干扰对准模型进行第二次抗干扰初始对准，获得对准结束时刻的姿态矩阵。

优选地，第2次抗干扰对准的误差模型可表示为如下公式：

其中，是b系到b₀系的姿态转换矩阵，是陀螺输出补偿后的角速度，是加速度计输出补偿后的加速度，v^b是载体速度，是n到n₀系的姿态转换矩阵，g是重力加速度；

姿态转换矩阵的粗略值在步骤(4)中已经获取，因此可以进行姿态更新和纬度更新，如下公式所示：

其中ω_ie是地球自转角速度，L是纬度，ψ是航向角。进而可获得

至此，公式(1)中除了剩余变量均为已知，将式(1)两边同时积分可得：

其中，

对上式进行分部积分：

由上式可见通过分步积分后，不再不需要进行速度差分。选择两个时刻t_k1和t_k2作为对准的时间点，其中t_k1为对准中间时刻，t_k2为对准结束时刻，根据速度矢量和的关系可以构建如下关系式：

然后根据正交矢量的构造方法，通过双矢量定姿原理可以确定姿态矩阵

最后将带入即可计算出姿态矩阵即可完成惯导系统的抗干扰初始对准。

本发明的有益效果是：

(1)本发明提出的基于里程计和回溯导航的动基座初始对准方法，不依赖卫星导航的辅助信息，提高了对准的自主性；而基于数据存储和回溯导航的算法提高了数据的利用率，有效降低了初始对准时间，提高惯导系统动基座对准的快速性。

(2)本发明提出的基于里程计和回溯导航的动基座初始对准方法，设计了2次抗干扰的对准流程，通过辅助姿态矩阵修正了原有对准算法中的误差，为后续的卡尔曼滤波提供了较好的初始信息，进而提高了惯导系统动基座初始对准的收敛速度和精度。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明方法的流程图。

图2为本发明具体实施例的车载试验的运动轨迹曲线；

图3为本发明具体实施例的车载试验的速度曲线；

图4为本发明具体实施例的车载试验的姿态曲线；

图5为本发明具体实施例的卡尔曼滤波对准的东向平台偏角收敛曲线；

图6为本发明具体实施例的卡尔曼滤波对准的北向平台偏角收敛曲线；

图7为本发明具体实施例的卡尔曼滤波对准的天向平台偏角收敛曲线；

图8为本发明具体实施例的结束对准后导航过程中的东向速度误差曲线；

图9为本发明具体实施例的结束对准后导航过程中的北向速度误差曲线。

具体实施方式

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

下面通过具体的实施例并结合附图对本申请做进一步的详细描述。

本发明给出一种基于里程计和回溯导航的惯导系统动基座初始对准方法，该方法的主要步骤是：采集惯导系统和里程计的输出信息；保存各种关键数据，包括惯导补偿后的陀螺和加速度计信息，以及里程计的速度信息；进行第1次抗干扰初始对准；计算辅助姿态矩阵；利用获取的辅助姿态矩阵进行第2次抗干扰初始对准，获得对准结束时刻的姿态矩阵；结合获取的姿态矩阵进行回溯导航，获得对准初始时刻的姿态矩阵；通过卡尔曼滤波进行对准，获得初始对准结束时刻的高精度姿态信息，完成动基座对准算法。

本发明给出的基于里程计和回溯导航的惯导系统动基座初始对准方法，具体包括以下步骤：

S1，采集惯导系统的惯导信息和里程计的速度信息。

所述惯导系统的惯导信息包括陀螺仪的角速度和加速度计输出的加速度。

S2，对惯导信息和速度信息进行补偿得到补偿数据。

针对惯导系统的惯导数据，首先通过温度模型获得当前温度下的漂移、零偏、刻度系数误差、安装偏角误差的参数值，进而通过补偿模型补偿陀螺和加速度计的输出，获取补偿后的角速度和加速度。

针对里程计的速度信息，按照预先设定的标定值补偿刻度系数误差、俯仰安装误差和偏航安装误差，将补偿后的角速度、加速度和里程计速度信息保存下来。

S3，根据补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵。

具体地，首先，定义两个惯性坐标系：

初始时刻载体惯性系(b₀)：与初始对准开始瞬时的载体坐标系(b 系)重合，随后相对于惯性空间无转动。

初始时刻导航惯性系(n₀)：与初始对准开始瞬时的导航坐标系(n 系，即地理坐标系)重合，随后相对于惯性空间无转动。

建立抗干扰对准模型，如公式(1)所示。

其中是b系到b₀系的姿态转换矩阵，是陀螺输出补偿后的角速度，是加速度计输出补偿后的加速度，v^b是载体速度，是 n到n₀系的姿态转换矩阵，g是重力加速度。

将式(1)两边同时积分可得：

其中，

对上式进行分部积分：

由上式可见通过分步积分后，不再不需要进行速度差分。选择两个时刻t_k1和t_k2作为对准的时间点，其中t_k1为对准中间时刻，t_k2为对准结束时刻，根据速度矢量和的关系可以构建如下关系式：

然后根据正交矢量的构造方法，通过双矢量定姿原理可以确定姿态矩阵

最后将带入即可计算出姿态矩阵即可完成惯导系统行进间的对准。

姿态矩阵即为辅助姿态矩阵。

S4，根据辅助姿态矩阵进行第二次抗干扰初始对准，获得对准结束时刻的姿态矩阵。

具体地，将抗干扰对准模型调整为如下公式(9)：

其中，是b系到b₀系的姿态转换矩阵，是陀螺输出补偿后的角速度，是加速度计输出补偿后的加速度，v^b是载体速度，是n到n₀系的姿态转换矩阵，g是重力加速度。

姿态转换矩阵的粗略值在步骤(4)中已经获取，因此可以进行姿态更新和纬度更新，如下公式所示：

其中ω_ie是地球自转角速度，L是纬度，ψ是航向角。进而可获得

至此，公式(9)中除了剩余变量均为已知，可再次进行抗干扰对准，其过程似于步骤S3，这里不再赘述。

S5,对所述对准结束时刻的姿态矩阵进行回溯导航解算，得到对准初始时刻的姿态矩阵。

具体地，将对准结束时刻的姿态矩阵反解得到姿态角作为初始姿态，将结束时刻的速度向量取反作为初始速度，将结束时刻的位置向量作为初值位置，进行回溯导航解算。完成回溯导航后即获得对准初始时刻的姿态矩阵。

此外，在解算过程中需要将陀螺输出的角速度和地球自转角速度取反。

S6，根据所述对准初始时刻的姿态矩阵，利用卡尔曼滤波算法，获得初始对准结束时刻的惯导姿态信息，完成惯导系统的动基座初始对准。

具体地，利用步骤S5中对准结束时刻的姿态矩阵反解得到姿态角作为初始姿态，初始位置速度信息由外界给出，在此基础上进行基于卡尔曼滤波的初始对准，过程如下：

选取状态变量：

其中φ_E,φ_N,φ_U为东向、北向、天向平台偏角，ε_x,ε_y,ε_z为陀螺漂移，为加速度计零偏。

选取量测变量：

其中v_INS为惯导速度，为里程计速度在载体坐标系的投影。

在此基础上建立基于卡尔曼滤波的初始对准模型：

其中W与V分别为系统噪声与量测噪声，二者均为白噪声。F为系统矩阵，H为量测矩阵，二者的表达式分别为：

H＝[0_3×3 I_3×3 0_3×9]_3×15 (17)

(15)式和(16)式中0表示零元素，I_6×6表示6维单位阵，F₀,F₁的表达式分别为：

在此基础上对式(15)所示的模型离散化：

其中Φ_k/k-1为系统状态转移矩阵，t为状态转移步长，Q_k为系统噪声矩阵。进而通过卡尔曼滤波算法即可标定正交陀螺和加速度计各项误差：

其中R_k为量测噪声矩阵，P_k为状态变量的协方差矩阵。

在结束卡尔曼滤波后即可获取结束时刻高精度的惯导姿态信息，从而完成惯导系统的动基座初始对准。

本发明的原理如下：

初始对准的关键是求解b₀系与n₀系的方位关系，即以下给出具体实现过程：

首先，重力矢量在n₀系的投影为：

其中，gⁿ为常矢量，即gⁿ＝[0 0 -g]^T，而又由于为常值，即n系相对于n₀系为定轴转动，由上式可解得：

所以有：

其次，加速度计的比力输出在b₀系投影为：

其中：

通过建立重力与加速度计比力量测之间的关系。对于加速度量测有如下公式：

通过姿态矩阵将以上比力方程转换到地理坐标系b(t)下：

由于：

因此可求得：

由此可得：

进而得到：

公式(32)即为完整的抗干扰对准误差模型。由于在粗对准结束前，姿态矩阵都是未知的，因此无法获得，先忽略项基于公式(1)进行第1次抗干扰对准，在获得粗略的姿态矩阵后即可实施更新姿态矩阵和纬度，即通过式(32)进行第2次抗干扰对准，从而提高对准精度，为后续的卡尔曼滤波提供较好的初始信息。

SINS的姿态、速度和位置导航算法可用如下微分方程表示：

其中，gⁿ＝[0 0 -g]，矩阵L、λ和h分别表示惯导姿态矩阵、速度、纬度经度和高度，和分别表示陀螺角速度测量和加速度计比力测量，ω_ie和g分别为地球自转角速率和当地重力加速度，R_M和R_N分别为当地地球的子午圈和卯酉圈半径，运算符表示由向量构成的反对称矩阵。假设SINS中陀螺和加速度计的采样周期均为T_s，将惯导解算微分方程离散化，得到基本的惯性导航解算递推算法：

其中：

假设从离散化的t₀时刻到t_m时刻，惯导系统从坐标A点导航至B 点，则为了使软件算法从B点能逆向导航解算至A点，对上式移项并变化，整理出逆向捷联惯导的解算算法为：

其中，

若记： 并且，令p＝m-k+1，则通过记号和下标变换可得和等，由此，可得捷联惯导系统的对准导航回溯递推算法如下：

其中：

对比式(34)和式(36)可见，回溯算法与顺序解算算法在形式上完全一致，只需将顺序解算中的陀螺采样和地球自转角速率符号取反，并设置回溯导航算法的初值为： 即可实现从t_m时刻(B点)至t₀时刻(A点)的回溯解算。

在完成回溯导航解算后即可获取初始时刻的姿态信息，此姿态信息精度较高，从而可以提高的卡尔曼滤波的收敛速度和精度，完成高精度的动基座初始对准。

本实施例中，惯导系统使用的惯性传感器为光纤陀螺和石英挠性加速度计，光纤陀螺的零偏稳定性约为0.02°/h，石英加速度计的零偏稳定性约为70ug。

图2给出了本发明具体应用的车载试验的运动轨迹，车载试验总时长约3小时，从A地附近出发，行至B地，然后沿原路返回，最终回到A地附近。通过车载的GPS天线和接收机提供速度合成后加入误差，作为里程计量测信息。

图3给出了本发明具体应用的车载试验的速度曲线，其中最大速度超过20m/s，图4给出了本发明具体应用的车载试验的姿态曲线，其中俯仰角和横滚角的波动超过了5°，航向运动基本覆盖了 0-360°全部范围。

图5、图6、图7给出了卡尔曼滤波对准过程中的东向、北向、天向平台偏角收敛曲线，算法对准时间为6分钟，随机选取一段车载数据作为对准数据。从这三幅图中可以看出，经过两次抗干扰对准和回溯导航，东向和北向平台偏角的收敛值在10″左右，天向平台偏角的收敛值在600″左右，且收敛较快，这间接说明了初始姿态的精度较高。而传统的抗干扰对准的方位精度误差较大，甚至能达到1°左右，因此在后续精对准过程中收敛较慢，精度较低。

考虑到车载试验数据没有高精度的姿态/航向参考信息，因此针对实际数据在评估对准算法的精度时，可以在对准结束后进行一段时间的纯惯导航，结合GPS提供的速度参考，通过速度误差的舒拉振荡峰值判断该次对准的精度。

根据惯导系统静基座误差传播规律可知，水平姿态对准误差会引起形如sinω_st规律的东向/北向速度误差，其中10″的水平对准误差造成的振荡峰值约为0.4m/s；方位对准误差会引起形如1-cosω_st规律的北向速度误差，其中180″的方位对准误差造成的振荡峰值约为 0.6m/s。

图8、图9给出了结束对准后导航过程中的东向、北向速度误差曲线。为了便于比对，图中还给出了使用传统算法结束对准后的速度误差曲线。使用传感算法在完成对准后北向速度误差的峰值约为 1.8m/s，且主要为1-cosω_st形式，对应的航向对准精度约为0.15°，而使用本文提出的对准算法，东向和北向速度误差最大值约为 0.5m/s，表明该算法的水平对准精度优于10″，方位对准精度优于 180″，明显优于传统对准算法，从而证明了本发明提出的惯导系统动基座对准算法的有效性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (3)

1.一种基于里程计和回溯导航的惯导系统动基座初始对准方法，其特征在于，具体包括以下步骤：

S1，采集惯导系统的惯导信息和里程计的速度信息；

S2，对惯导信息和速度信息进行补偿得到补偿数据；

S3，根据补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵；

S4，根据辅助姿态矩阵进行第二次抗干扰初始对准，获得对准结束时刻的姿态矩阵；

第二次抗干扰对准的误差模型可表示为如下公式：

其中，b代表载体惯性坐标系，b₀代表初始时刻的载体惯性系，与初始对准开始瞬时的b系重合，随后相对于惯性空间无转动；n代表导航坐标系，n₀代表初始时刻的导航惯性系，与初始对准开始瞬时的n系重合，随后相对于惯性空间无转动；是b系到b₀系的姿态转换矩阵，是陀螺输出补偿后的角速度，是加速度计输出补偿后的加速度，v^b是载体速度，是n到n₀系的姿态转换矩阵，g是重力加速度；

姿态转换矩阵的粗略值在步骤(4)中已经获取，因此可以进行姿态更新和纬度更新，如下公式所示：

其中ω_ie是地球自转角速度，L是纬度，ψ是航向角；进而可获得

至此，公式(1)中除了剩余变量均为已知，将式(1)两边同时积分可得：

其中，

对上式进行分部积分：

由上式可见通过分步积分后，不再进行速度差分；选择两个时刻t_k1和t_k2作为对准的时间点，其中t_k1为对准中间时刻，t_k2为对准结束时刻，根据速度矢量和的关系可以构建如下关系式：

然后根据正交矢量的构造方法，通过双矢量定姿原理可以确定姿态矩阵

最后将带入即可计算出姿态矩阵即可完成惯导系统的抗干扰初始对准；

S5,对所述对准结束时刻的姿态矩阵进行回溯导航解算，得到对准初始时刻的姿态矩阵；

S6，根据所述对准初始时刻的姿态矩阵，利用卡尔曼滤波算法，获得初始对准结束时刻的惯导姿态信息，完成惯导系统的动基座初始对准。

2.根据权利要求1所述的基于里程计和回溯导航的惯导系统动基座初始对准方法，其特征在于，步骤S3中根据补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵的具体步骤为：

建立初始时刻的载体惯性系和导航惯性系；

建立抗干扰对准模型；

根据所述抗干扰对准模型和补偿数据进行第一次抗干扰初始对准，得到辅助姿态矩阵。

3.根据权利要求1所述的基于里程计和回溯导航的惯导系统动基座初始对准方法，其特征在于，步骤S4中根据辅助姿态矩阵进行第二次抗干扰初始对准的具体步骤为：

调整抗干扰对准模型；

根据辅助姿态矩阵和调整后的抗干扰对准模型进行第二次抗干扰初始对准，获得对准结束时刻的姿态矩阵。