基于状态变换的惯性/卫星组合导航动态滤波方法与流程

本申请涉及组合导航滤波技术领域，特别是涉及一种可以适用于复杂环境的基于状态变换的惯性/卫星组合导航动态滤波方法。

背景技术：

随着无人车、低空无人机等载体的应用环境越来越复杂，应用范围越来越广泛，实现无人车、低空无人机等载体在复杂环境下的高精度自主导航迫在眉睫。然而,传统卡尔曼滤波框架下的惯性/卫星等组合导航滤波方式很容易出现方差估计不一致的问题，即在传感器均能正常工作的情况下，组合导航系统应有的精度也不能完全发挥。

如何解决组合导航系统方差估计不一致的问题属于导航领域的最新发展方向，国外学者对于此问题作了大量的相关研究和跟踪研究，但是国内学者对于此问题的研究甚少。解决组合导航方差估计不一致问题的最新方法是2017年法国学者提出的不变卡尔曼滤波理论，但是，由于保密的原因，其真正在组合导航系统中应用的模型并未公开。

技术实现要素：

针对现有技术中惯性基组合导航系统方差估计不一致的问题，本发明提供一种基于状态变换的惯性/卫星组合导航动态滤波方法，以提高捷联惯导初始对准及组合导航的精度。与现有的绝大部分解决方差估计不一致问题的方法所不同的是，本发明所提出的状态变换卡尔曼滤波方式不需要预先知道组合导航系统的可观测性，所以，具有很强的工程应用灵活性和适用性。

本发明通过分析方差估计不一致问题产生的原因，以原因为导向，设计基于状态变换的组合导航卡尔曼滤波方式，防止方差估计不一致问题的产生，进而为国内组合导航系统中的应用提供理论依据。

具体地，为实现上述技术目的，本发明采用的技术方案是：

基于状态变换的惯性/卫星组合导航动态滤波方法，包括：

第一步：卫星接收机和捷联惯导分别输出各自的数据信息；

第二步：根据卫星接收机和捷联惯导输出的数据信息，建立惯性/卫星初始对准模型，完成基于状态变换卡尔曼滤波的初始对准。

第三步：建立惯性/卫星组合导航模型，完成基于状态变换卡尔曼滤波的惯性/卫星组合导航，将组合导航的姿态、速度、位置信息输出。

本发明第一步中，卫星接收机输出的数据信息包括：卫星观测的经度、纬度、高度、北向速度、东向速度和地向速度信息。

捷联惯导输出的数据信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力增量信息。

本发明第二步中，惯性/卫星初始对准模型包括状态变换卡尔曼滤波系统的惯性/卫星组合导航系统模型和惯性/卫星组合导航系统观测模型。

(2.1)定义速度误差

传统的速度误差定义是直接将速度误差定义为其中是由捷联惯导解算的载体相对于地球的速度在当地导航系(以北-东-地(n-e-d)当地地理坐标系为当地导航系)下的投影，是真实的载体相对于地球的速度在当地导航系下的投影。但是，这种速度误差定义仅仅考虑了向量之间的幅度差异，并不完全符合坐标系的一致性。

本发明将速度误差在同一个坐标系中进行定义，即将捷联惯导解算的速度通过坐标变换投影到真实的当地导航系下，与真实的速度进行相减：

其中，exp(·)代表矩阵的指数，^为将向量转换成斜对称矩阵，为真实的姿态矩阵，为捷联惯导解算的姿态矩阵的转置，i为三维的单位矩阵，φⁿ为姿态误差向量。可以发现，公式(1)中新的速度误差定义相比传统的速度误差定义多出了项从物理上可以解释为，两个向量的差异不仅有幅度上的差异，亦存在方向上的差异。传统速度误差简单的定义为导航坐标系下和计算坐标系下的速度向量的幅度差异，这对于在小失准角的情况下是合理的。而新的速度误差定义中同时兼顾了真实的当地导航坐标系(这里既可以是北-东-地也可以是东-北-天)和计算的当地导航坐标系下的速度向量的幅度和方向差异，即是考虑了速度向量的方向差异的附加项。新的速度误差定义对于大失准角情况下的初始对准滤波更合理并且更有效，并能有益于提高不利环境下的组合导航滤波精度。

本发明定义的速度误差的微分方程的详细推导如下：

其中，ε^b和▽^b分别为陀螺仪和加速度计的零偏；wg和wa分别为陀螺仪和加速度计误差的白噪声向量；为本发明新定义的速度误差的微分；为传统定义下的速度误差的微分；代表计算的速度的微分；gⁿ为重力向量；为计算的地球自转角速度在导航系的投影；为计算的导航系相对地球的变化率在导航系的投影；为真实的地球自转角速度在导航系的投影，为其对应的误差；为真实的导航系相对地球的变化率在导航系的投影，为其对应的误差；为其对应的误差。

显然，从公式(2)可以看出，新的速度误差微分方程中不再含有比力项，而是由重力向量替代。由于imu在一般情况下的输出都是比力增量而不是比力本身，并且比力增量的值由于器件等原因会掺杂着量化噪声，所以传统基于扩展卡尔曼滤波的组合导航系统的系统矩阵的计算会被高动态环境下的比力fⁿ高度影响，进而导致方差不一致性。但是，新的系统矩阵中不再有比力项fⁿ，而是被重力项gⁿ所替代。新的基于状态变换的卡尔曼滤波方式可以避免由于比力量化噪声较大带来的系统矩阵计算不精确的问题，因为对一般载体的当地局部导航来说，gⁿ的变化很小，几乎为常值。

(2.2)构建状态变换卡尔曼滤波系统的惯性/卫星组合导航系统模型；

是姿态误差的微分:

其中为陀螺的误差，其它符号定义见前面公式所述。

陀螺仪和加速度计误差可以建模为随机游走过程:

其中δf^b代表加速度计的误差，其它符号定义见前面公式所述。

根据本发明定义的速度误差可以推出位置误差微分方程为：

其中δrⁿ为位置误差向量，对应纬度、经度和高度的误差。

其中，ve为东向速度、vn为北向速度、h高程值、l为当地地理纬度、rn为子午圈半径、re为卯酉圈半径。

根据公式(2),(3),(4),(5)，构建状态变换卡尔曼滤波系统的惯性/卫星组合导航系统模型为：

其中惯性/卫星组合导航系统矩阵为f，惯导误差状态向量为x，噪声转移矩阵为g，过程噪声向量为w，其对应的协方差矩阵为q。它们的定义如下：

w＝[wgxwgywgzwaxwaywaz]^t(9)

其中wgx，wgy，wgz对应三轴陀螺仪其三个陀螺误差的白噪声，wax，way，waz对应三轴加速度计其三个加速度计误差的白噪声。

其中，ω为地球自转角速度的模值。

f22＝[09×6](14)

(2.3)构建状态变换卡尔曼滤波系统的惯性/卫星组合导航系统观测模型：

状态变换卡尔曼滤波系统的惯性/卫星组合导航系统观测模型设计为：δz＝hx+υ，其中：

其中，δzv和δzr分别为捷联惯导与卫星之间的速度误差和位置误差，υv,3×1和υr,3×1分别为速度误差和位置误差对应的观测白噪声。

由于卡尔曼滤波估计的是新的速度误差状态，所以，滤波之后速度状态的更新应该根据：

姿态和位置的校正则与传统的卡尔曼滤波相同。

根据以上模型即可完成惯性/卫星组合导航系统的初始对准。

在初始对准完成之后，进行本发明的第三步，建立惯性/卫星组合导航模型，完成基于状态变换卡尔曼滤波的惯性/卫星组合导航，将组合导航的姿态、速度、位置信息输出。本发明中，惯性/卫星组合导航模型与惯性/卫星初始对准模型完全相同，同样包括状态变换卡尔曼滤波系统的惯性/卫星组合导航系统模型和观测模型。初始对准过程和组合导航过程无需进行状态的切换，惯性/卫星组合导航系统一直保持15状态不变，这使得从初始对准到组合导航无需进行协方差的过渡，提高组合导航的收敛速度。通过以上方法，实现了复杂环境下捷联惯导的初始对准和惯性/卫星组合导航。

传统的惯性基组合导航卡尔曼滤波方法存在速度误差定义的坐标系不一致问题，进而导致方差估计不一致问题的产生。本发明通过对速度误差定义进行严格的状态变换，使其满足坐标系的一致性，进而克服复杂环境下的方差估计不一致的问题，进而提高捷联惯导在复杂环境下的初始对准精度和组合导航精度。

与现有技术相比，本发明具有以下优点：

1)速度误差定义相比传统速度误差定义更为严格，可以提高大失准角环境下的捷联惯导初始对准精度；

2)组合导航系统模型中不再还有比力项，使得组合导航系统可以更好的适用于复杂的动态环境，克服比力对滤波的不利影响。

3)本发明方法实现容易，并且计算量小。传统的卡尔曼滤波系统矩阵需要以高频率进行预测使得系统可以跟踪比力的动态变化，而状态变换卡尔曼滤波下的滤波预测和更新过程可以同时进行，提高运算效率。

本发明所提出的方法不需要预先知道组合导航系统的可观测性，具有很强的工程应用灵活性和适用性，亦可以灵活的扩展于惯性与其它传感器组合导航中，如惯性/视觉、惯性/激光测速仪组合导航等。

附图说明

图1为本发明的流程图。

图2为激光陀螺imu/卫星组合导航实验轨迹图。

图3为激光陀螺imu/卫星组合导航传统卡尔曼滤波和状态变化卡尔曼滤波的水平位置误差对比图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述清楚说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。

图1是本实施例的流程图，图2和图3为某次激光陀螺imu/卫星组合导航跑车实验结果。

具体包括如下步骤：

步骤一，初始化数据。

惯性/卫星组合导航系统开始输出捷联惯导和卫星接收机的数据。捷联惯导系统输出载体系(前-右-下)的陀螺和加表数据，以北-东-地(n-e-d)当地地理坐标系为当地导航系，卫星接收机输出经度、纬度、高度、北向、东向、地向的速度信息。

步骤二，基于状态变换卡尔曼滤波的初始对准。

根据捷联惯导和卫星接收机的数据完成初始对准。以卫星观测的经度、纬度、高度、北向速度、东向速度、地向速度信息作为初始对准的位置和速度值。初始对准采用先3分钟解析粗对准，再进行10分钟(具体对准时间根据需要选择)15状态的状态变换卡尔曼滤波精对准。

状态变换卡尔曼滤波的15状态选择为：

分别对应姿态误差向量，对应的速度误差向量，位置误差向量，陀螺零偏向量，加表零偏向量。

状态变换卡尔曼滤波系统的惯性/卫星组合导航系统模型为：其中各分量已经在公式(8)-(17)进行了定义。

状态变换卡尔曼滤波系统的惯性/卫星组合导航系统观测模型为：δz＝hx+υ

观测状态为惯导与卫星的速度误差和位置误差，共6个状态，观测矩阵为：

卡尔曼滤波具体实施即为标准的五公式的卡尔曼滤波，这里不再赘述。

每一次卡尔曼滤波完成之后，速度更新需根据：姿态和位置的更新与传统卡尔曼滤波更新完全一致。

步骤三，基于状态变换卡尔曼滤波的组合导航。

对准过程完成之后，直接进行15状态的惯性/卫星组合导航，不需要重新切换卡尔曼滤波，这样可以提高组合导航滤波的收敛速度。组合导航实施的模型与初步对准过程中所用的模型完全一致，即惯性/卫星组合导航模型与惯性/卫星初始对准模型完全相同。

最后，将组合导航的姿态、速度、位置信息输出。

以某次激光陀螺捷联惯导与卫星松组合导航实验为例，对比新的组合导航滤波方案与传统的组合导航滤波方案性能。激光陀螺零偏优于0.01°/h，imu输出频率为100hz,gps输出位置速度频率为1hz。跑车轨迹的谷歌地球显示如图1所示。在组合导航过程中，将卫星信号人为取多段进行断开90s，观察水平位置误差的精度。传统卡尔曼滤波和本发明提供的状态变换卡尔曼滤波的预测频率和更新频率均设置为1s，4个小时组合导航的水平位置误差如图2所示。

图2可以看出，在卫星信号断开的6个时间段内，传统卡尔曼滤波具有比本发明所提供的状态变换卡尔曼滤波更大的水平位置误差，因为状态变换卡尔曼滤波的总体水平位置误差曲线都在传统卡尔曼滤波的水平位置误差曲线下面。所以，本发明提出的状态变换卡尔曼滤波具有比传统卡尔曼滤波更优的组合导航水平位置精度。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。