CN110285838A - 基于重力矢量时间差分的惯性导航设备对准方法 - Google Patents

Abstract

本发明属于导航技术领域，涉及一种基于重力矢量时间差分的惯性导航设备对准方法，包括：1)建立任意惯性坐标系，根据陀螺的实时输出，对惯性坐标系到机体系之间的变换矩阵进行实时更新，保证惯性坐标系相对于惯性空间静止，惯性坐标系自动隔离了包括地球自转角速度在内的外界任何角运动；2)算出重力在惯性坐标系中的重力矢量方向；在惯性坐标系中确定任意时刻天向和东向的实时值；3)得到东北天地理坐标系与惯性坐标系的转换关系，确定东北天地理坐标系与机体系的变换矩阵，即姿态矩阵，完成初始粗对准。本发明提供了一种能很好地解决初始对准精度不够及耗时过长等难题的基于重力矢量时间差分的惯性导航设备对准方法。

Description

基于重力矢量时间差分的惯性导航设备对准方法

技术领域

本发明属于导航技术领域，涉及一种导航设备的对准方法，尤其涉及一种基于重力矢量时间差分的惯性导航设备对准方法。

背景技术

舰船在系泊状态或航行过程中会因风浪的干扰而处于摇摆状态以及现代战争要求陆地军用车辆具有快速启动，但在风吹和地面松软等情况下，捷联惯性导航系统(一种不依赖于任何外部信息，也不向外部辐射能量的自主式导航系统)的陀螺仪和加速度计输出包含了因舰船、车辆摇摆而产生的干扰信息，特别是导弹发射车重心较高，摆动幅度明显增大，此时初始对准就不能采用静基座对准方法。

初始对准(惯性导航系统开始导航之前的工作状态，一般进行了坐标系对准、初始参数的测定，将平台调整在给定的导航坐标系(导航时根据导航系统工作的需要而选取的作为导航基准的坐标系)上)是惯性导航系统能够正常解算运行的关键阶段，指在系统进入工作状态之前建立导航所必需的初始条件，对准的精度直接影响了惯导导航系统的性能。惯导导航系统的初始对准通常分为粗对准和精对准阶段。粗对准为精对准提供一定精度的初始条件，保证精对准的快速性。普通的粗对准不能满足摇摆状态下的粗对准需求。主要原因是在于载体的干扰角速度，另一个次要原因是在于载体的干扰加速度。其中初始对准原理是：根据相应的敏感器件数据，通过算法实现姿态与方位角的解算。传统方法在粗对准结束后采用滤波方式进行初始精对准，即惯导系统安装在载体上后，不考虑载体特性，只考虑自身，进行自主性对准，受初始失准角的限制，对准精度差，可观测性差。为了提高导航精度实现更高精度的初始对准，各类研究人员和工程人员始终关注动基座，环境相对较复杂情况下的初始对准技术，现阶段相关技术包括了适用于动基座的基于惯性凝固系的粗对准，基于方差分量估计的自适应容积卡尔曼滤波技术，无迹卡尔曼滤波(UKF)和线性卡尔曼滤波(KF)初始对准方法等。

其中：

基于方差分量估计(Variance Component Estimation，VCE)的自适应容积卡尔曼滤波，抑制系统非线性和不确定性问题对舰船SINS初始对准精度及导航精度的影响，完成舰船的高精度初始对准，但消耗时间过长，解算麻烦，干扰角速度误差未能完全消除。

为提高捷联惯导系统在晃动基座(初始对准时物体处于运动状态)下初始对准的快速性和精度，提出了一种基于四元数的捷联惯导惯性系晃动基座自对准算法。该算法利用惯性坐标系下的姿态更新来实时地反映载体在晃动干扰下的姿态变化，通过四元数(能够解决空间中角度转动的一种简单超复数)推导将初始姿态的最优估计转化为姿态确定问题，以消除角晃动干扰的影响；并根据惯性系下重力矢量和晃动干扰加速度不同频的特点，引入小波阈值消噪以消除线振动干扰的影响，从而提高算法在晃动基座下的对准精度。但此方法计算麻烦，很难计算机较好实现。

由于动基座下载体运动状态复杂，初始失准角的三个分量往往都很大，在这种情况下，捷联惯性导航系统的失准角误差微分方程、速度误差微分方程的非线性程度均很高，采用扩展卡尔曼滤波法(EKF)对姿态失准角进行估计效果有限。而无迹卡尔曼滤波法(UKF)能够对非线性问题有很好的处理效果并且状态更新迭代过程与线性卡尔曼滤波算法类似。但是，相比于线性KF，UKF需要耗费更多的计算量，并且UKF收敛速度较慢。

晃动基座捷联惯导系统初始对准迭代方法由惯性导航计算出水平速度误差，利用最小二乘法估算出水平角速度误差、姿态误差和航向误差，然后进行迭代计算，从而算出导航初始时刻的姿态和航向，操作简单，但精度不够，对干扰角速度影响不能有效避免。

初始对准的精度直接影响导航精度，初始对准时间直接关系到系统快速反应能力，这两项结果是衡量初始对准效果优劣的主要指标，而现有的技术往往在这两项能力上不能兼顾，能满足晃动干扰基座初始对准的要求但不能满足在快速机动情况下的对准需要。如传统的解析粗对准方法因为晃动干扰角速率远大于地球自转角速率而失效，基于凝固惯性系的粗对准方案在系统存在线运动干扰时精度会严重下降，基于重力加速度积分的解析粗对准算法在解决线运动干扰影响的能力有限，以及利用数字滤波器滤除载体在惯性系下的干扰加速度信息，从而实现摇摆基座下的抗干扰粗对准，但针对不同情况下的外界干扰需要设置不同的截止频率，导致人为的因素过大，缺乏自适应性。

基于重力矢量的QUESR姿态最优估计算法，在传统基于重力矢量的初始对准方法基础上，将方向余弦矩阵的求解过程转换为Wahba问题，实现对重力矢量信息的充分利用，并通过QUEST算法实现了晃动基座下行进间粗对准的最优算法，改善了原有算法鲁棒性以及实时性等方面的问题，但改善后的算法也是批处理算法，无法很好地处理干扰角速度。

发明内容

为了解决背景技术中存在的上述技术问题，本发明提供了一种能很好地解决初始对准精度不够及耗时过长等难题的基于重力矢量时间差分的惯性导航设备对准方法。

为了实现上述目的，本发明采用如下技术方案：

一种基于重力矢量时间差分的惯性导航设备对准方法，所述基于重力矢量时间差分的惯性导航设备对准方法包括以下步骤：

1)建立一个任意的惯性坐标系q，根据陀螺的实时输出，对惯性坐标系q到机体系b之间的变换矩阵进行实时更新，保证惯性坐标系q相对于惯性空间静止，惯性坐标系q自动隔离了包括地球自转角速度在内的外界任何角运动；

2)根据加速度计的输出值，算出重力在惯性坐标系q中的重力矢量方向；所述重力矢量方向旋转轨迹在时刻t的切线方向就是此时的东向e在惯性坐标系q中的方向；在惯性坐标系q中确定任意时刻天向r和东向e的实时值；

3)根据任意时刻天向r和东向e这两个方向，得到东北天地理坐标系t与惯性坐标系q的转换关系，最终确定东北天地理坐标系t与机体系b的变换矩阵，即姿态矩阵，完成初始粗对准。

作为优选，本发明所采用的步骤1)的具体实现方式是：

初始时刻，直接建立一个惯性坐标系q，零时刻机体系b到惯性坐标系q的变换矩阵直接写为：

从零时刻开始，矩阵根据陀螺的输出值直接进行更新，也就是说惯性坐标系q相对惯性空间稳定，根据陀螺的输出，采用四元数法得到任意时刻的矩阵

作为优选，本发明所采用的任意时刻的矩阵的具体计算方式是：

设定初始四元数则

根据陀螺输出的角增量由旋转矢量的子样算法计算得到，然后依次类推到任意时刻的四元数根据任意时刻的四元数计算得到任意时刻的矩阵

作为优选，本发明所采用的步骤2)的具体实现方式是：

将加速度计的输出值向惯性坐标系q投影为：

先不考虑杆臂效应引起的干扰力，则f^q(t)就是-g在惯性坐标系q的投影：

因此根据f^q(t)就确定天向在惯性坐标系q中的投影为：

由于在惯性系看来，当地天向随着地球向东方转动，因此的微分即是东向在惯性坐标系q中的方向，即得到：

在实际的算法中，由的差分形式而得到所述与相互垂直：

共同决定了东北天地理坐标系的方位。

作为优选，本发明所采用的步骤3)的具体实现方式是：

根据步骤2)计算得到的以及计算从惯性坐标系q到东北天地理坐标系t的转换矩阵

假设3个向量在东北天地理坐标系t和惯性坐标系q中的表示分别为：

由于变换矩阵关系则得到转换矩阵为：

由于是正交的，满足因此上式变换为

采用上述原理，根据构造出三个向量：

则这三个向量在东北天地理坐标系中的表示为：

这三个向量在惯性坐标系q中的表示为：

将(11)式以及(12)式代入(9)式，得到最终的变换矩阵为：

根据在惯性坐标系q中得到的矢量分量由上式直接得出转换矩阵得到IMU的姿态矩阵为：

其中由系统根据和陀螺输出实时计算得到；

由于理论上正交，为了减小误差，需要按照施密特正交化方法对进行正交化处理，处理的方法如下：

β₁＝α₁ (17)

正交化处理后，在进行归一化，即得到了合理的由此完成了系统的初始粗对准。

本发明的优点是：

本发明提供了一种基于重力矢量时间差分的惯性导航设备对准方法，该方法是基于重力矢量时间差分的惯性导航设备对准技术是通过建立一个任意的惯性坐标系，保证惯性坐标系相对于惯性空间静止，从而惯性坐标系能自动隔离外界任何角运动，再根据陀螺的实时输出，能够对惯性坐标系到机体系之间的变换矩阵进行实时更新，然后根据加速度计的输出值，可以算出重力在惯性坐标系中的矢量方向。由于重力矢量方向在惯性坐标系中以地球自转角速度旋转，因此重力矢量方向在惯性坐标系中不是一个固定值，而是在微小的旋转，其旋转轨迹的在时刻的切线方向就是此时的东向在惯性坐标系中的方向。可见在惯性坐标系中可以确定任意时刻天向和东向的实时值。最后根据天向和东向这两个方向，得到东北天地理坐标系与惯性坐标系的转换关系，从而最终确定东北天坐标系与机体系的变换矩阵，即姿态矩阵。此方法具有摇摆状态下的可行性与有效性，能很好地解决初始对准精度不够及耗时过长的难题。本发明中，由于惯性坐标系自动隔离了载体的各种干扰角速度或者摇摆角速度，也就是说角速度干扰对对准没有任何影响。面对干扰加速度时，在存在晃动的时候对加速度可以用两级低通滤波器或者卡尔曼滤波来保证准确性。基于重力矢量时间差分的惯性导航设备对准方法能够完善比如二阶调平、方位对准采用四元数补偿的初始对准方案，可以更有效地实现晃动基座上的对准。这种方法的主要优点是对干扰角速度完全不敏感，从而克服了以前粗对准方法中的主要误差因素：干扰角速度。在采用本发明之后，外界的干扰加速度就成为了主要矛盾。但是在一般情况下，普通的晃动，或者对准时间很长，这种粗对准方法的效果是非常好的。在特使的情况下，例如晃动幅度非常大，而且对准时间有要求很短，则需要低通滤波，从而保证重力切线方向的准确度，即保证了方位的准确性。但两级低通滤波器的设计增加了麻烦性，而且设计不当会造成东向的计算偏差很大，可以考虑用卡尔曼滤波(一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法)来实现。本发明的核心思想是通过重力矢量时间差分来降低干扰角速度的影响，对干扰角速度完全不敏感，从而可以克服一直以来粗对准方法中的主要误差因素干扰角速度，同时如若对准时间要求很短而且晃动幅度也相对较大时，对于外界的干扰加速度则可考虑采用卡尔曼来实现。

具体实施方式

本发明的实现原理是：总体思想是首先建立一个任意的惯性坐标系q(符合牛顿力学定律的坐标系，即是绝对静止或只做匀速直线运动的坐标系)，根据陀螺的实时输出，可以对惯性坐标系q到机体系b之间的变换矩阵进行实时更新，保证惯性坐标系q相对于惯性空间静止，从而惯性坐标系q自动隔离了外界的任何角运动，包括地球自转角速度。然后根据加速度计的输出值，可以算出重力在惯性坐标系q中的矢量方向。由于重力矢量方向在惯性坐标系中以地球自转角速度旋转，因此重力矢量方向在惯性坐标系q中不是一个固定值，而是在微小的旋转，其旋转轨迹的在时刻t的切线方向就是此时的东向e惯性坐标系在q中的方向。可见在惯性坐标系q中可以确定任意时刻天向r和东向e的实时值。最后根据天向r和东向e这两个方向，可以得到东北天地理坐标系t与惯性坐标系q的转换关系，从而最终确定东北天地理坐标系t与机体系b的变换矩阵，即姿态矩阵。

初始时刻，直接建立一个惯性坐标系q，零时刻机体系b到惯性坐标系q的变换矩阵直接写为：

从零时刻开始，矩阵根据陀螺的输出值直接进行更新，也就是说惯性坐标系q相对惯性空间稳定，根据陀螺的输出，采用四元数法可以得到任意时刻的矩阵具体过程如下：

设定初始四元数则

可以根据陀螺输出的角增量由旋转矢量的子样算法计算得到，然后依次类推可以到任意时刻的四元数根据现有技术(高伟，郝燕玲，蔡同英.摇摆状态下捷联惯导系统初始对准技术的研究[J].中国惯性技术学报，2004(03)：16-20.公式(3-36))即可以得到

那么同时，将加速度计的输出值向q系投影为：

先不考虑杆臂效应引起的干扰力，则f^q(t)就是-g在惯性坐标系q的投影：

因此根据f^q(t)就可以确定天向在惯性坐标系q中的投影为：

由于在惯性系看来，当地天向随着地球向东方转动，因此的微分即是东向在惯性坐标系q中的方向，即可以得到：

在实际的算法中，可以由的差分形式而得到注意相互垂直：

它们共同决定了东北天地理坐标系的方位。

下面讨论如何根据计算出从q系到t系的转换矩阵

假设3个向量在t系和q系中的表示分别为：

由于变换矩阵关系则可以得到转换矩阵为：

由于是正交的，满足因此上式可以变换为

采用上述原理，可以根据构造出三个向量：

则这三个向量在东北天地理坐标系中的表示为：

这三个向量在惯性坐标系q中的表示为：

将(11)(12)式代入(9)式，很容易得到最终的变换矩阵为：

根据在q系中得到的矢量分量由上式直接得出转换矩阵然后就可以得到IMU的姿态矩阵为：

其中由系统根据和陀螺输出实时计算得到。

由于理论上正交，为了减小误差，需要按照施密特正交化方法对进行正交化处理，处理的方法如下：

β₁＝α₁ (17)

正交化处理后，在进行归一化，即得到了合理的由此完成了系统的初始粗对准。

干扰加速度滤波：设整个粗对准时间600秒。由于船舶摇摆及上面机械的振动，f^q(t)的输出值存在一定的波动，这种情况下，的值就会计算非常不准确，甚至错误。因此必须对f^q(t)的输出值进行低通滤波。初步考虑采用两级滤波法，第一级滤波，对f^q(t)的进行10Hz采样，滤去频率高于0.5Hz的所有分量，然后累加得到1Hz的f^q(t)滤波输出值。第二级滤波，对前一级滤波输出的1Hz的f^q(t)值，滤去频率高于0.05Hz的所有分量，最后输出1Hz滤波后的f^q(t)值。滤波最后一刻第600秒的值计算得到值为：

上面A为归一化因子。

最后150秒滤波值之和，减去300s—450s间滤波值之和，得到：

上面B为归一化因子。

估计这种方法能够保证任何环境下方位误差不超过1度，水平误差不超过0.5度。

在导航程序中，考虑到低通滤波器的使用即增加了编程的麻烦，也增加了粗对准时间。因此只用这种方法确立重力方向，方位估算则放在水平对准时完成。若只用这种方法确立水平面，只需要120秒钟的时间，方法如下。

累加120秒钟中q系中的加表输出值，并进行归一化，最后得到：

将后60秒钟和前60秒钟的加表输出值相减得到，并与进行施米特正交化，然后再进行归一化，最后得到与垂直的单位矢量：

根据公式(15)(16)即可得到粗对准的IMU姿态矩阵。

上述过程在静态情况下能够得到正确的水平和方位。但是在动态情况下，由于没有进行低通滤波，并且对准时间很短，得到的很不准确的，导致最后方位误差很大甚至错误。需要进一步完成方位粗对准。

Claims (5)

1.一种基于重力矢量时间差分的惯性导航设备对准方法，其特征在于：所述基于重力矢量时间差分的惯性导航设备对准方法包括以下步骤：

1)建立一个任意的惯性坐标系q，根据陀螺的实时输出，对惯性坐标系q到机体系b之间的变换矩阵进行实时更新，保证惯性坐标系q相对于惯性空间静止，惯性坐标系q自动隔离了包括地球自转角速度在内的外界任何角运动；

2)根据加速度计的输出值，算出重力在惯性坐标系q中的重力矢量方向；所述重力矢量方向旋转轨迹在时刻t的切线方向就是此时的东向e在惯性坐标系q中的方向；在惯性坐标系q中确定任意时刻天向r和东向e的实时值；

3)根据任意时刻天向r和东向e这两个方向，得到东北天地理坐标系t与惯性坐标系q的转换关系，最终确定东北天地理坐标系t与机体系b的变换矩阵，即姿态矩阵，完成初始粗对准。

2.根据权利要求1所述的基于重力矢量时间差分的惯性导航设备对准方法，其特征在于：所述步骤1)的具体实现方式是：

初始时刻，直接建立一个惯性坐标系q，零时刻机体系b到惯性坐标系q的变换矩阵直接写为：

从零时刻开始，矩阵根据陀螺的输出值直接进行更新，也就是说惯性坐标系q相对惯性空间稳定，根据陀螺的输出，采用四元数法得到任意时刻的矩阵

3.根据权利要求2所述的基于重力矢量时间差分的惯性导航设备对准方法，其特征在于：所述任意时刻的矩阵的具体计算方式是：

设定初始四元数则

根据陀螺输出的角增量由旋转矢量的子样算法计算得到，然后依次类推到任意时刻的四元数根据任意时刻的四元数计算得到任意时刻的矩阵

4.根据权利要求3所述的基于重力矢量时间差分的惯性导航设备对准方法，其特征在于：所述步骤2)的具体实现方式是：

将加速度计的输出值向惯性坐标系q投影为：

先不考虑杆臂效应引起的干扰力，则f^q(t)就是-g在惯性坐标系q的投影：

因此根据f^q(t)就确定天向在惯性坐标系q中的投影为：

由于在惯性系看来，当地天向随着地球向东方转动，因此的微分即是东向在惯性坐标系q中的方向，即得到：

在实际的算法中，由的差分形式而得到所述与相互垂直：

共同决定了东北天地理坐标系的方位。

5.根据权利要求4所述的基于重力矢量时间差分的惯性导航设备对准方法，其特征在于：所述步骤3)的具体实现方式是：

根据步骤2)计算得到的以及计算从惯性坐标系q到东北天地理坐标系t的转换矩阵

假设3个向量在东北天地理坐标系t和惯性坐标系q中的表示分别为：

由于变换矩阵关系则得到转换矩阵为：

由于是正交的，满足因此上式变换为

采用上述原理，根据构造出三个向量：

则这三个向量在东北天地理坐标系中的表示为：

这三个向量在惯性坐标系q中的表示为：

将(11)式以及(12)式代入(9)式，得到最终的变换矩阵为：

根据在惯性坐标系q中得到的矢量分量由上式直接得出转换矩阵得到IMU的姿态矩阵为：

其中由系统根据和陀螺输出实时计算得到；

由于理论上正交，为了减小误差，需要按照施密特正交化方法对进行正交化处理，处理的方法如下：

β₁＝α₁ (17)

正交化处理后，在进行归一化，即得到了合理的由此完成了系统的初始粗对准。