[发明专利]一种行人惯性导航全时全程逆向平滑滤波方法

说明书

一种行人惯性导航全时全程逆向平滑滤波方法：在系统进行初始对准之后，在运动状态时，利用加速度和角速度信息进行捷联解算，当进入下一个零速区间时，即脚部着地时，利用RTS平滑算法，对导航系统状态误差进行最优估计，并修正系统状态；然后利用逆向捷联解算，反向计算系统的导航状态变量，计算到上一个零速静止区间，然后再利用卡尔曼滤波器将前向捷联解算的系统状态信息与逆向捷联解算的系统状态信息进行融合，获得精度更高的状态信息；最后，利用卡尔曼滤波得到的运动状态信息，对零速区间再次进行RTS平滑滤波，得到系统状态信息的最优估计。

技术领域

本发明属于行人惯性导航领域，特别是涉及一种利用行人运动的零速周期，全时刻、全程地对行人惯性导航系统状态进行逆向平滑滤波的方法。

背景技术

随着惯性器件的飞速发展，由于自主性好，抗干扰能力强，惯性导航系统已经被广泛应用于全球卫星导航系统(GNSS)信号较弱或完全中断的环境中，例如密集的城市建筑群、室内场景和地下环境等。作为一种应用在纯惯导情况下的高精度定位算法，零速更新算法越来越多的出现在隧道、地下环境的车载导航和室内环境的人员定位中。通过利用载体周期性的静止状态和使用滤波器(如卡尔曼滤波器)估计速度误差，零速更新算法可以校正载体的移动速度，限制位置和姿态误差，甚至估计传感器输出的误差。在车载惯性导航和水下机器人领域，各种改进的ZUPT误差估计算法被提出，而在室内人员定位领域，ZUPT算法也是提升定位精度的关键，为了提升ZUPT算法的误差估计精度，各地的研究者提出了各种方法来提高零速时刻的检测精度和优化误差估计算法。

传统的零速更新算法，只能在载体周期性的零速状态下，利用3维速度误差的观测量，通过卡尔曼滤波器对系统的9维、12维，甚至15维状态误差进行估计和修正。现在有的零速更新算法，主要存在两个问题：(1)只能在周期性的零速状态下对系统状态误差进行估计，从而对系统状态进行修正，无法实现全时刻、全轨迹的系统状态的最优估计；(2)在误差估计部分，仅仅利用当前时刻的3维速度观测信息，估计精度难以满足高精度的应用需求。针对以上问题，本发明提供了一种行人惯性导航全时刻、全程的逆向平滑滤波方法。

发明内容

本发明的目的是提供一种可以全时刻全程进行平滑滤波的方法，提高行人惯性导航系统的状态误差估计精度，同时也为其他惯性导航应用提供一种状态估计算法的优化思路。

本发明采用的技术方案是：一种行人惯性导航全时全程逆向平滑滤波方法，包括以下步骤：

步骤1，以当地地理坐标系为导航坐标系，建立系统的误差传递模型，模型的15维状态变量包括：3维位置误差、3维速度误差、3维姿态误差、3维加速度计输出误差、3维陀螺仪输出误差。

步骤2，初始的静止区间，进行系统初始对准，利用加速度计和陀螺仪传感器信息，计算载体的初始姿态角，包括俯仰角、滚转角和偏航角，确定初始四元数。

步骤3，进入运动区间，利用加速度和角速度信息进行捷联解算，计算系统的3维位置、3维速度和3维姿态信息。

步骤4，进入下一个静止区间，利用RTS平滑算法，在系统的零速状态下进行平滑滤波，对系统的15维状态误差进行最佳估计，然后利用估计的误差信息对系统的位置、速度、姿态、加速度零偏误差和陀螺仪漂移误差进行校正。

步骤5，从第二个静止区间的开始时刻，记作Tj，利用逆向导航算法，以Tj时刻的系统状态作为初始状态，并利用步骤4中估计的传感器误差对运动区间的传感器输出进行修正，然后逆向解算系统的状态信息。

步骤6，逆向解算到达上一个静止区间的最后时刻，记作Ti，此时已获得系统运动区间的正向捷联解算状态和逆向捷联解算状态，利用卡尔曼滤波器，将两个方向解算的系统状态信息进行融合，得到更加准确的系统状态信息。

步骤7，从Tj时刻开始，再次进行RTS平滑滤波，此时利用的传感器输出为步骤4中的校正后的传感器输出，Tj时刻的系统状态为步骤6中双向融合后的系统状态信息。