CN109959374A - 一种行人惯性导航全时全程逆向平滑滤波方法 - Google Patents

Abstract

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

Description

一种行人惯性导航全时全程逆向平滑滤波方法

技术领域

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

背景技术

随着惯性器件的飞速发展，由于自主性好，抗干扰能力强，惯性导航系统已经被广泛应用于全球卫星导航系统(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中双向融合后的系统状态信息。

上述步骤就是本发明从一个零速区间到另一个零速区间的逆向平滑滤波方法的一个完整周期，后续滤波重复上述周期即可。

与现有技术相比，本发明的有益效果是：

(1)本发明除了在零速条件下可以对系统状态误差进行估计外，还可以在非零速条件下，对系统的状态进行最优估计，提升系统的全程导航精度。

(2)本发明在零速区间采用RTS平滑算法来对系统状态误差进行估计，与传统的利用当前时刻的观测信息进行状态估计不同，本发明利用零速区间内所有的状态观测量，来对当前时刻的状态误差进行估计，极大地提升了估计精度。

附图说明

图1为本发明的整体时序图；

图2为本发明中捷联解算算法流程图；

图3为本发明中零速区间平滑滤波图；

图4为本发明中双向状态融合图；

图5为行人惯性导航行走轨迹图。

具体实施方式

下面结合附图对本发明的一种行人惯性导航全时全程逆向平滑滤波方法做出详细说明。

由于人体运动时，脚部的运动是周期性动静交替的，所以本发明的算法也呈周期性，第一个周期包括(1)初始对准；(2)正向捷联解算；(3)RTS平滑滤波；(4)逆向捷联解算；(5)运动区间状态最优估计；(6)二次RTS平滑滤波，共6个步骤。第二个周期，则省去初始对准的步骤。本发明的算法时序图如图1所示。

(一)系统初始对准。行人惯性导航系统，一般是将惯性测量单元固定在脚部，即捷联载体是脚，则载体坐标系是以脚的左-前-上三个方向构成，同时选择当地的地理坐标系为导航坐标系，即东-北-天。在导航开始之前，需要先进行初始对准，也就是计算出载体坐标系相对于导航坐标系的方向余弦矩阵。

本发明中的载体初始姿态角的计算是通过加速度的值计算得到的。其中，偏航角设为0，横滚角和俯仰角按以下公式计算：

其中，f_x,f_y,f_z分别为载体坐标系中三个轴向的加速度。利用上式确定初始姿态之后，就可以得到初始四元数和初始方向余弦矩阵。

(二)正向捷联解算。利用加速度和陀螺仪信息，计算载体的姿态、速度和位置信息。正向捷联解算算法如图2所示。首先利用陀螺仪信息更新四元数，然后利用四元数计算姿态矩阵，利用姿态矩阵，将载体坐标系下的加速度转化到导航坐标系下，然后逐次积分，得到载体在导航坐标系下的速度和位置。

捷联解算的关键是求姿态矩阵，姿态矩阵是通过四元数更新得到的。描述载体系到导航系的姿态变化四元数为：

对上式两边同时求导数，写成矩阵的形式即得四元数微分方程，如下所示：

公式(4)中，[q₀ q₁ q₂ q₃]为四元数，[w_x w_y w_z]为三轴角速率。通过求解四元数微分方程，便可以对四元数进行更新，从而实现目标姿态的更新。

(三)RTS平滑滤波。在检测到载体为零速状态时，在零速区间内，便可以进行RTS平滑滤波。现有的技术一般是进行各种形式的卡尔曼滤波，仅仅利用当前时刻的3维速度信息作为观测量，来估计整个系统的状态误差，估计精度较差，本发明则是采用RTS平滑滤波算法，对于每个零速时刻，利用整个零速区间内的3维速度误差观测量，来对该时刻的系统状态误差进行估计，估计精度要明显高于仅仅采用卡尔曼滤波的方法。

RTS平滑算法主要分为两个过程，前向的卡尔曼滤波过程和逆向的平滑过程，如图3所示。若要进行卡尔曼滤波，则需要先建立系统的状态模型，本发明选择的是捷联惯性导航系统的误差模型，其中，状态变量选择3维的位置误差、3维的速度误差、3维的姿态误差、3维的加速度计零偏误差和3维的陀螺仪随机漂移误差。位置误差传递模型、速度误差传递模型和姿态误差传递模型如下所示：

公式(5)中，p为位置矢量，v为速度矢量，ψ为姿态，f^b为载体系下的加速度，[fⁿ×]表示导航系下加速度的斜对称矩阵，表示从载体系到导航系的姿态转换矩阵。根据上述的位置、速度和姿态误差传递模型，建立系统动态模型为：

其中，分别为东北天方向上的位置误差、速度误差、姿态误差、加速度计零偏误差和陀螺仪随机漂移误差；Φ_k,k-1是系统从k-1时刻到k时刻的状态转移矩阵；W_k-1是k-1时刻的系统噪声矩阵；Γ_k-1是k-1时刻的系统噪声驱动矩阵；Z_k是k时刻的状态观测向量；H_k是观测矩阵；V_k是观测噪声矩阵。另外，设系统噪声矩阵W_k-1的协方差阵为Q_k-1，观测噪声矩阵V_k的协方差阵为R_k-1。

按公式(6)建立系统动态模型，然后执行前向卡尔曼滤波过程，具体流程如下式所示：

时间更新：

量测更新：

上述公式中，K为卡尔曼增益，P为协方差矩阵，I为单位矩阵。

前向卡尔曼滤波过程中，为了进行逆向平滑过程，需要存储前向过程中一步预测状态最优估计状态一步预测协方差矩阵P_f,k/k-1、预测协方差矩阵P_f,k和状态转移矩阵Φ_k,k-1。

逆向平滑过程，以前向卡尔曼滤波过程的最后一个时刻作为起点，按照以下公式对历史状态进行逆向平滑。设N为前向卡尔曼过程的最后时刻，

其中，k＝N-1,N-2,…2,1,0,P_s,N＝P_f,N，

通过RTS平滑过程，将会对零速区间内的每一时刻状态误差进行最优估计，然后利用估计的误差对系统状态进行修正，得到零速区间内每一时刻的最优系统状态。

(四)逆向捷联解算。应用RTS平滑滤波对零速区间进行最优估计后，静止区间内的状态估计较为精确，然后利用逆向捷联解算算法，逆向递推回去，以零速区间的起始时刻的状态为递推的初始状态，得到一组新的运动状态数据。

逆向捷联解算在形式上与正向的捷联解算相同，只需要在正向捷联解算的基础上，将陀螺输出、地球自转角速度和速度取反，然后按照正向捷联的形式计算即可。如果对于同一组惯性数据，在同一时刻下，不考虑其他的影响，逆向捷联解算的结果和正向捷联解算的结果基本相同，其中，位置和姿态相同，速度方向相反。

本发明中，在进行逆向捷联解算之前，先利用步骤(三)RTS平滑滤波中估计的传感器误差，包括加速度零偏误差和陀螺仪随机漂移误差，对正向捷联解算过程中记录的惯性数据进行修正，然后利用修正后的数据进行逆向解算，由此提高解算精度。

(五)运动区间状态最优估计。进行完上述步骤之后，可以得到正向捷联解算的系统状态和逆向捷联解算的系统状态，其中，正向捷联解算是在初始对准的基础上，进行解算的；逆向捷联解算是在利用RTS平滑算法对零速区间进行最优估计的基础上，进行解算的。下面，利用卡尔曼滤波算法，将两个方向解算的系统状态进行融合，得到运动区间状态的最优估计。

首先建立9维的状态模型，以捷联惯性导航算法的误差传递模型为基础，选取的状态变量为X＝[δP_E δP_N δP_U δv_E δv_N δv_U δα δβ δγ]，分别为位置误差、速度误差和姿态误差。建立的误差模型如下所示：

其中，X＝[δP_E δP_N δP_U δv_E δv_N δv_U δα δβ δγ]，分别为东北天方向上的位置误差、速度误差和姿态误差；Φ_k,k-1是系统从k-1时刻到k时刻的状态转移矩阵；W_k-1是k-1时刻的系统噪声矩阵；Γ_k-1是k-1时刻的系统噪声驱动矩阵；Z_k是k时刻的状态观测向量；H_k是观测矩阵；V_k是观测噪声矩阵。另外，设系统噪声矩阵W_k-1的协方差阵为Q_k-1，观测噪声矩阵V_k的协方差阵为R_k-1

其中，状态转移矩阵的离散形式为：

观测矩阵为：

噪声驱动矩阵的离散形式为：

由于逆向捷联解算的速度与正向捷联解算的速度方向相反，所以将逆向捷联解算的速度取反，然后9个状态变量分别取差分，以9个状态的差分量为观测量，进行卡尔曼滤波，估计状态误差，然后利用状态误差的估计量修正当前时刻的状态，实现运动区间状态的最优估计，算法框图如图4所示。

(六)二次RTS平滑滤波。在步骤五进行完毕后，会对运动状态的最后一个时刻的状态信息重新估计，因此，可以利用最新的运动状态信息，对后面的零速区间进行二次的RTS平滑滤波，此次滤波的初始时刻状态，是由运动区间的最后时刻的状态解算得到。平滑滤波的具体算法步骤可参考步骤(三)。

上述的六个步骤，就是本发明算法的单周期操作，后续周期，重复上述步骤即可，唯一区别就是不用执行步骤(一)的初始对准。按照本发明上述的算法进行行人惯性导航定位实验，将惯性测量单元固定在脚部，在室内环境进行行走，解算的结果如图5所示，运动起始点与终点几乎重合，水平误差为0.19米，约为总行程的0.2％，定位精度要明显优于现有的行人惯性定位算法。

Claims (1)

1.一种行人惯性导航全时全程逆向平滑滤波方法，其特征在于：包括以下步骤：

步骤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中双向融合后的系统状态信息。

上述步骤就是本发明从一个零速区间到另一个零速区间的逆向平滑滤波方法的一个完整周期，后续滤波重复上述周期即可。