CN112066983B - 惯性/里程计组合导航滤波方法、电子设备及存储介质 - Google Patents

Abstract

针对复杂环境下无人驾驶车辆的导航定位精度问题，本发明提供了一种惯性/里程计组合导航滤波方法、电子设备及存储介质，包括：基于捷联惯导和轮式里程计各自接收的数据信息，建立基于ST‑EKF的惯性/里程计组合导航模型；基于建立的基于ST‑EKF的惯性/里程计组合导航模型完成前向滤波，并将输出的滤波信息实时存入滑动窗口空间；利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航***输出的姿态、速度、位置信息。本发明是一种具备实时航迹校正能力和更好的滤波鲁棒性的在线平滑滤波方法，是改善复杂环境下无人驾驶车辆导航定位精度的可行方法。

Description

惯性/里程计组合导航滤波方法、电子设备及存储介质

技术领域

本发明涉及组合导航非线性滤波技术领域，特别是涉及一种基于状态变换卡尔曼滤波(ST-EKF)的惯性/里程计组合导航的在线平滑方法。

背景技术

近年来，随着智能化水平的不断提高，无人驾驶技术取得了飞速发展，Google、特斯拉、丰田等厂商纷纷将高新的科研成果投放入市场，给人们的生活带来了许多便利。但是，现阶段的无人驾驶技术远没有达到十分完善的地步，在诸如乡村道路，牧场草原，海滩沙漠等复杂场景下，没有固定不变的特征结构，缺乏地面基站的外源信息，再加上变化多样的野外气候，无人驾驶车辆的导航定位精度大打折扣。因此，如何构建在复杂地形和恶劣环境条件下具有更高精度的自主导航定位***是推动无人驾驶技术进一步发展的关键问题。

轮式里程计(OD)由于不需要对外发射信号，可以通过测量车轮转过的角度独立的提供车辆的行驶速度和路程，对外部因素的抗干扰能力较强，与捷联惯导***(SINS)结合使用有助于提高复杂野外环境下无人驾驶车辆的导航定位精度。但是，惯性/里程计组合导航***在缺乏卫星信号等外源信息的校正作用时，***的位置定位误差会随着时间的推移或路程的增长逐渐累积，使最终解算得到的行驶轨迹偏离实际的路线。同时，当导航平台处于复杂外部环境时，传统的基于扩展卡尔曼滤波(EKF)的捷联惯性组合导航***容易出现方差估计不一致的问题,这也将影响车辆的导航定位精度。

RTS平滑算法是Rauth等人基于极大似然估计准则推导出的，对一个区间内的状态估计和误差协方差进行最优平滑的固定区间平滑器，是一种有效的事后处理方法。相比于单向滤波只能基于当前量测信息来估计当前状态，RTS平滑算法则可以利用区间内所有的量测信息对任一时刻状态进行估计，常用于惯性卫星组合导航的高精度数据处理与卫星的事后基准确定。

但是传统的RTS平滑器对于平滑区间的选取不够灵活，缺乏对航迹校正的实时性，在意外性较大的复杂环境下有时不能发挥最佳的滤波效果。

发明内容

针对复杂环境下无人驾驶车辆的导航定位精度问题，本发明提供了一种惯性/里程计组合导航滤波方法、电子设备及存储介质。本发明是一种具备实时航迹校正能力和更好的滤波鲁棒性的在线平滑滤波方法，是改善复杂环境下无人驾驶车辆导航定位精度的可行方法。

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

惯性/里程计组合导航滤波方法，包括：

基于捷联惯导和轮式里程计各自接收的数据信息，建立基于ST-EKF的惯性/里程计组合导航模型；

基于建立的基于ST-EKF的惯性/里程计组合导航模型完成前向滤波，并将输出的滤波信息实时存入滑动窗口空间；

利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航***输出的姿态、速度、位置信息。

本发明中，轮式里程计接收的数据信息包括：无人车辆行驶的瞬时速度信息。捷联惯导接收的数据信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息。

本发明中，基于ST-EKF的惯性/里程计组合导航模型包括基于ST-EKF的惯性/里程计组合导航***的误差状态方程和基于ST-EKF的惯性/里程计组合导航***观测模型，根据基于ST-EKF的惯性/里程计组合导航模型即可完成前向滤波，输出滤波信息，同时将滤波信息实时存储入预先设置好的滑动窗口空间并标注好时刻。

本发明中，基于ST-EKF的惯性/里程计组合导航模型的建立方法，包括：

(1)建立ST-EKF模型；

其中：gⁿ为当地重力矢量；∧代表将向量转化为斜对称矩阵；

为载体坐标系相对地球运动的速度在导航坐标系下的投影；

为地球相对地心惯性坐标系的自转角速率在导航系的投影；φⁿ和

分别为姿态失准角和速度误差向量；

为当地北东地(NED)导航坐标系相对地球运动产生的角速率在当地导航系的投影；

为姿态矩阵；

为加速度计测量零偏；w_g和w_a分别为陀螺和加速度计误差的白噪声向量；ε^b为陀螺测量零偏；

为

的误差。

(2)建立基于ST-EKF的惯性/里程计组合导航***的误差状态方程；

其中：x为误差状态向量，

为误差状态向量的微分，F为***矩阵，G为噪声转移矩阵，w为过程噪声向量。

表示速度误差矢量；

δrⁿ表示位置误差矢量；δk_D表示里程计刻度因子误差；δα表示捷联惯导***与车体之间的安装误差，包括俯仰角安装误差δα_y和偏航角安装误差δα_z；L^b表示捷联惯导***与里程计之间的杆臂向量。

τ_δk表示里程计刻度因子误差相关时间；τ_δαy表示俯仰安装误差角相关时间；w中的元素w_bg、w_ba、w_δk、

分别表示陀螺零偏噪声、加速度计零偏噪声、刻度因子误差马尔科夫过程的白噪声、俯仰安装偏角误差马尔科夫过程的白噪声。F_{STEKF-INS-NED}表示ST-EKF滤波框架下的15状态***矩阵，具体定义如下：

v_N、v_E分别为北向、东向速度；R_N为子午圈主曲率半径；R_E为卯酉圈主曲率半径；L、h分别为纬度、高度；Ω为地球自转速度。

(3)建立基于ST-EKF的惯性/里程计组合导航***观测模型；

δz＝δz_v＝Hx+v_4×1

其中，v_4×1是测量噪声向量；H＝H_v是观测矩阵：

v_D代表里程计测量的前向速度。

本发明中所述前向滤波的方法，包括：

惯性/里程计组合导航***开始运动后，首先进行捷联惯导解算，当运动时间达到惯性/里程计组合导航***设定的的滤波周期时，开始前向滤波，步骤如下：

(1)时间更新；

t时刻的***矩阵F(t)离散化为***矩阵Φ_k,k-1，t时刻噪声转移矩阵G(t)离散化为噪声转移矩阵Γ_k,k-1，下标k-1,k为滤波更新时刻。

x_f,k/k-1＝Φ_k,k-1x_f,k-1

x_f,k-1,x_f,k表示前向滤波过程中不同时刻的误差状态向量，P_f,k-1,P_f,k表示前向滤波过程中不同时刻的均方误差矩阵，Q_k-1表示k-1时刻的过程噪声方差矩阵。

(2)量测更新；

x_f,k＝x_f,k/k-1+K_f,k(z_f,k-H_f,kx_f,k/k-1)

K_f,k表示前向滤波过程中k时刻的增益矩阵，H_f,k表示前向滤波过程中k时刻的观测矩阵，R_f表示前向滤波过程中的观测噪声矩阵，z_f,k表示前向滤波过程中k时刻的观测矩阵，I表示单位矩阵。

每一次前向滤波过程中，记录下时间更新后输出的滤波信息(即误差状态向量x_f,k/k-1、均方误差矩阵P_f,k/k-1和状态转移矩阵Φ_k,k-1)和量测更新后输出的滤波信息(即误差状态向量x_f,k和均方误差矩阵P_f,k)，并将所有的滤波信息实时存储入预先设置好的滑动窗口空间并标注好时刻。滑动窗口空间的信息储存上限长度L可以根据行车的里程、实时行驶环境的复杂性以及导航计算机的解算能力灵活调整。

本发明中利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航***输出的姿态、速度、位置信息，方法如下：

当滑动窗口空间中存储的滤波信息长度达到预设长度L时，停止前向滤波，以滑动窗口空间中储存的第一组滤波信息对应的时刻为起始位置，以当前时刻为终点，利用滑动窗口空间中储存的所有滤波信息对滑动窗口空间内每一时刻的误差状态向量和均方误差矩阵进行RTS反向平滑，平滑过程结束后，利用反向平滑后得到的误差状态向量x_s,k对惯性/里程计组合导航***输出的姿态、速度、位置信息进行修正。其中RTS反向平滑公式为：

x_s,k＝x_f,k+K_s,k(x_s,k+1-x_f,k+1/k)

其中，下标k与上文中定义相同，数值变化规律可以表示为k＝L-1,L-2,...2,1,0。K_s,k表示反向滤波过程中k时刻的增益矩阵，x_s,k,x_s,k+1表示后向滤波过程中不同时刻的误差状态向量，P_f,k+1/k与前文定义相似，P_s,k,P_s,k+1表示后向滤波过程中不同时刻的均方误差矩阵，P_s,L＝P_f,L，x_s,L＝x_f,L，其中，L为滑动窗口的长度，即表示前向滤波的终端时刻。

利用反向平滑后得到的误差状态向量x_s,k对惯性/里程计组合导航***输出的姿态、速度、位置信息进行修正，其具体的修正过程采用现有的传统方法，即与＝与传统的惯性里程计组合导航***的信息更新过程相同，在此不再赘述。

本发明提供一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述惯性/里程计组合导航滤波方法的步骤。

本发明提供一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述惯性/里程计组合导航滤波方法的步骤。

本发明所涉及的滑动窗口滤波技术除惯性/里程计组合导航***外，还适用于惯性与其他任何传感器的组合导航***，如惯性/卫星、惯性/视觉组合导航***等等。

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

1)本发明中的组合导航***模型中不再含有比力项，而是被引力相关项替代，使得组合导航***可以更好的适用于复杂的动态环境，克服比力剧烈变化对滤波的不利影响，具有更好的滤波鲁棒性。

2)本发明能对惯性/里程计组合导航***进行航迹修正，改善因为复杂环境引发的导航定位精度下降问题。

3)本发明改善了传统的平滑滤波技术实时性不足的问题，通过引入滑动窗口空间实时的对前向滤波信息进行存储，然后利用这些信息对***的输出信息平滑优化，并可以根据车辆的行驶环境调整滑动窗口的长度，改进平滑效果。

综上所述，本发明一方面引入滤波鲁棒性更好的状态变换卡尔曼滤波替代传统的扩展卡尔曼滤波，另一方面利用滑动窗口中储存的状态误差向量、误差协方差和状态转移矩阵等数据，实时对车辆行驶的航迹进行修正，以达到提高导航定位精度的目的。

附图说明

图1为本发明的流程图。

图2为某次长航时激光陀螺IMU车载导航实验轨迹图。

图3为不加平滑处理的EKF和ST-EKF的惯性/里程计组合导航方法的水平定位结果图。

图4为L＝6000时平滑处理后的EKF和ST-EKF的惯性/里程计组合导航方法的水平定位结果图。

具体实施方式

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

实施例1：

本实施例提供一种惯性/里程计组合导航滤波方法，包括：

S1.基于捷联惯导和轮式里程计各自接收的数据信息，建立基于ST-EKF的惯性/里程计组合导航模型。

(1)建立ST-EKF模型；

其中：gⁿ为当地重力矢量；∧代表将向量转化为斜对称矩阵；

为载体坐标系相对地球运动的速度在导航坐标系下的投影；

为地球相对地心惯性坐标系的自转角速率在导航系的投影；φⁿ和

分别为姿态失准角和速度误差向量；

为当地北东地(NED)导航坐标系相对地球运动产生的角速率在当地导航系的投影；

为姿态矩阵；

为加速度计测量零偏；w_g和w_a分别为陀螺和加速度计误差的白噪声向量；ε^b为陀螺测量零偏；

为

的误差。

由于常用的线性速度误差微分方程中含有比力项，比力项的计算精度易受比力量化精度和姿态解算精度的影响，从而导致传统的基于EKF的组合导航***的***矩阵的计算容易产生方差估计不一致的问题，进而损害组合导航精度本发明定义了一种新的速度误差，是一种非线性误差形式，比传统的线性误差定义更为严格。本发明定义了一种新的速度误差，是一种非线性误差形式，比传统的线性误差定义更为严格。从构建的EKF模型可以看出，其中不再含有比力项，而是由重力相关项替代。而对于一般的车载导航来说，gⁿ的变化很小，因此，基于ST-EKF的组合导航***可以有效避免由于比力量化噪声和姿态的剧烈变化带来的计算不精确的问题，避免误差的累计，确保滤波的鲁棒性。

(2)建立基于ST-EKF的惯性/里程计组合导航***的误差状态方程；

其中：x为误差状态向量，

为误差状态向量的微分，F为***矩阵，G为噪声转移矩阵，W为过程噪声向量。

表示速度误差矢量；

δrⁿ表示位置误差矢量；δk_D表示里程计刻度因子误差；δα表示捷联惯导***与车体之间的安装误差，包括俯仰角安装误差δα_y和偏航角安装误差δα_z；L^b表示捷联惯导***与里程计之间的杆臂向量。由于路况的颠簸，导致俯仰角安装误差δα_y一般不为常值，因此可以建模为一阶马尔科夫过程。对于长航时的车载导航来说，里程计刻度因子误差也会有不稳定性，因而也可以建模为一阶马尔科夫过程。

τ_δk表示里程计刻度因子误差相关时间；τ_δαy表示俯仰安装误差角相关时间；w中的元素w_bg、w_ba、w_δk、

分别表示陀螺零偏噪声、加速度计零偏噪声、刻度因子误差马尔科夫过程的白噪声、俯仰安装偏角误差马尔科夫过程的白噪声。F_{STEKF-INS-NED}表示ST-EKF滤波框架下的15状态***矩阵，具体定义如下：

v_N、v_E分别为北向、东向速度；R_N为子午圈主曲率半径；R_E为卯酉圈主曲率半径；L、h分别为纬度、高度；Ω为地球自转速度。

(3)建立基于ST-EKF的惯性/里程计组合导航***观测模型。

假设IMU系与车体系之间的安装角是小角度，则方向余弦矩阵

可以写为：

其中,δα是捷联惯导***与车体之间的安装误差。

由捷联惯导***解算的速度在车体坐标系m系下的投影可以表达为：

其中,

为载体相对地球的角速度在载体系下的投影,L^b为捷联惯导***与车体之间杆臂向量。

将捷联惯导***解算出的速度信息投影到里程计坐标系下与里程计的速度相减作为惯性/里程计组合导航***观测量δz_v，即：

其中,

是m系下里程计输出速度向量。

则，可以定义基于ST-EKF的惯性/里程计组合导航***的观测方程为：

δz＝δz_v＝Hx+v_4×1

其中，v_4×1是测量噪声向量；H＝H_v是观测矩阵：

v_D代表里程计测量的前向速度。

S2.基于建立的基于ST-EKF的惯性/里程计组合导航模型完成前向滤波，并将输出的滤波信息实时存入滑动窗口空间。

惯性/里程计组合导航***开始运动后，首先进行纯惯导解算，当运动时间达到惯性/里程计组合导航***设定的的滤波周期时，开始前向滤波，步骤如下：

(1)时间更新；

t时刻的***矩阵F(t)离散化为***矩阵Φ_k,k-1，t时刻噪声转移矩阵G(t)离散化为噪声转移矩阵Γ_k,k-1，下标k-1,k为滤波更新时刻。

x_f,k/k-1＝Φ_k,k-1x_f,k-1

(2)量测更新；

X_f,k＝X_f,k/k-1+K_f,k(z_f,k-H_kx_f,k/k-1)

K_f,k表示前向滤波过程中k时刻的增益矩阵，H_f,k表示前向滤波过程中k时刻的观测矩阵，R_f表示前向滤波过程中的观测噪声矩阵，z_f,k表示前向滤波过程中k时刻的观测矩阵，I表示单位矩阵。

每一次前向滤波过程中，记录下时间更新后输出的滤波信息(即误差状态向量x_f,k/k-1、均方误差矩阵P_f,k/k-1和状态转移矩阵Φ_k,k-1)和量测更新后输出的滤波信息(即误差状态向量x_f,k和均方误差矩阵P_f,k)，并将所有的滤波信息实时存储入预先设置好的滑动窗口空间并标注好时刻。滑动窗口空间的信息储存上限长度L可以根据行车的里程、实时行驶环境的复杂性以及导航计算机的解算能力灵活调整。

S3.利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航***输出的姿态、速度、位置信息。

当滑动窗口空间中存储的滤波信息长度达到预设长度L时，停止前向滤波，以滑动窗口空间中储存的第一组滤波信息对应的时刻为起始位置，以当前时刻为终点，利用滑动窗口空间中储存的所有滤波信息对滑动窗口空间内每一时刻的误差状态向量和均方误差矩阵进行RTS反向平滑，平滑过程结束后，利用反向平滑后得到的误差状态向量x_s,k对惯性/里程计组合导航***输出的姿态、速度、位置信息进行修正。其中RTS反向平滑公式为：

x_s,k＝x_f,k+K_s,k(x_s,k+1-x_f,k+1/k)

其中，K_s,k表示反向滤波过程中k时刻的增益矩阵，x_s,k,x_s,k+1表示后向滤波过程中k时刻、k+1时刻的误差状态向量，P_f,k+1/k、x_f,k+1/k采用前向滤波过程中时间更新的对应公式计算得到；P_s,k,P_s,k+1表示后向滤波过程中k时刻、k+1时刻的均方误差矩阵，P_s,L＝P_f,L，x_s,L＝x_f,L，其中，L为滑动窗口的长度，下标中表示前向滤波的终端时刻。

利用反向平滑后得到的误差状态向量x_s,k对惯性/里程计组合导航***输出的姿态、速度、位置信息进行修正，其具体的修正过程采用现有的传统方法，即与＝与传统的惯性里程计组合导航***的信息更新过程相同，在此不再赘述。

当前反向平滑结束后，清空滑动窗口空间中的储存数据，将当前时刻的状态误差向量等滤波信息作为滑动窗口空间的初始值存入滑动窗口空间，之后继续进行前向滤波和滤波信息存储，直到储存的滤波数据长度再次达到滑动窗口区间的设定长度或者车辆到达行驶终点。

实施例2：

参照图1是本实施例的流程图，本实施例提供一种惯性/里程计组合导航滤波方法，具体包括如下步骤：

步骤一，惯性/里程计组合导航***开始输出捷联惯导和轮式里程计的数据。捷联惯导***输出载体系(前-右-下)的陀螺和加表数据，以北-东-地(N-E-D)当地地理坐标系为导航系，轮式里程计输出载车的瞬时速度。

步骤二，基于实施例1中所构建的基于ST-EKF的惯性/里程计组合导航模型，完成基于ST-EKF的惯性/里程计组合导航的前向滤波以及滑动窗口的数据存储。

***前向滤波过程中，每次时间更新与量测更新结束后，将得到的滤波信息x_f,k、x_f,k/k-1、P_f,k、P_f,k/k-1、Φ_k,k-1存入滑动窗口空间并标注好时刻，滑动窗口空间的长度L可以根据行车里程、复杂环境的分布时段等实际情况灵活调整。需要注意的是，由于只有受噪声驱动的状态分量才具有可平滑性，不受噪声影响的随机常值状态分量的平滑精度不会优于单向滤波的精度，为了减少不必要的计算，在滑动窗口空间只需要存储误差状态向量中前九个状态的相关信息，即：

步骤三，利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航***输出的姿态、速度、位置信息。

当滑动窗口空间中实时存储的滤波信息长度l与设定的滑动窗口空间长度L相等时，停止前向滤波，以当前的状态误差向量x_f,N作为初始量x_s,N,以***的初始误差协方差矩阵P_f,0作为P_s,N，利用滑动窗口空间中的滤波信息进行反向RTS平滑。

为验证本发明所提供方法的有效性，以某次长航时激光陀螺IMU车载导航实验数据的事后处理为例，对比本发明提供的新的组合导航滤波方案与传统的组合导航滤波方案性能。陀螺和加表数据来自于高精度激光陀螺IMU，IMU输出频率为200Hz，陀螺零偏稳定性为0.01(°)/h，角度随机游走为

加速度计零偏稳定性为50ug，速度随机游走为

跑车轨迹如图2所示，行车时间总计7h，在大约3.5h时到达半程(去程)终点，总行程为489.88km。卡尔曼滤波的预测频率和更新频率均设置为1Hz。

图3为不进行反向平滑处理的EKF和ST-EKF组合导航方案解算得到的水平位置分别相对于基准位置的水平定位误差和精度随时间以及行驶里程的变化而产生的变化趋势，图4为滑动窗口空间长度L＝6000时，采用反向平滑处理后的EKF和ST-EKF组合导航方案的水平定位误差和精度变化趋势。基准位置由惯性/卫星组合导航事后平滑算法解算得到。

由图3、图4可以对比发现，平滑处理后的组合导航算法相对于未处理时，水平定位误差的抑制效果得到了有效提升，误差曲线更加平滑，跳点大幅度减少。同时，由图4可以发现，在同等条件下本发明提供的方法有着更好的航迹修正效果，半程(去程)的水平定位精度提高45.95％，全程的水平定位精度提高46.24％。

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

Claims (6)

1.惯性/里程计组合导航滤波方法，其特征在于，包括：

基于捷联惯导和轮式里程计各自接收的数据信息，建立基于ST-EKF的惯性/里程计组合导航模型；

基于ST-EKF的惯性/里程计组合导航模型完成前向滤波，并将输出的滤波信息实时存入滑动窗口空间；

利用滑动窗口空间中储存的滤波信息进行在线平滑，修正组合导航***输出的姿态、速度、位置信息；

所述利用滑动窗口空间中储存的滤波信息进行在线平滑的方法如下：

当滑动窗口空间中存储的滤波信息长度达到预设长度L时，停止前向滤波，以滑动窗口空间中储存的第一组滤波信息对应的时刻为起始位置，以当前时刻为终点，利用滑动窗口空间中储存的所有滤波信息对滑动窗口空间内每一时刻的误差状态向量和均方误差矩阵进行RTS反向平滑，利用反向平滑后得到的误差状态向量x_s,k对惯性/里程计组合导航***输出的姿态、速度、位置信息进行修正；

所述RTS反向平滑的公式为：

x_s,k＝x_f,k+K_s,k(x_s,k+1-x_f,k+1/k)

其中，K_s,k表示反向滤波过程中k时刻的增益矩阵，x_s,k,x_s,k+1表示后向滤波过程中k时刻、k+1时刻的误差状态向量，P_f,k+1/k、x_f,k+1/k采用前向滤波过程中时间更新的对应公式计算得到；P_s,k,P_s,k+1表示后向滤波过程中k时刻、k+1时刻的均方误差矩阵，P_s,L＝P_f,L，x_s,L＝x_f,L，其中，L为滑动窗口的长度，下标中表示前向滤波的终端时刻；

所述基于ST-EKF的惯性/里程计组合导航模型，包括：

(1)建立ST-EKF模型；

其中：gⁿ为当地重力矢量；∧代表将向量转化为斜对称矩阵；

为载体坐标系相对地球运动的速度在导航坐标系下的投影；

为地球相对地心惯性坐标系的自转角速率在导航系的投影；φⁿ和

分别为姿态失准角和速度误差向量；

为当地北东地导航坐标系相对地球运动产生的角速率在当地导航系的投影；

为姿态矩阵；

为加速度计测量零偏；w_g和w_a分别为陀螺和加速度计误差的白噪声向量；ε^b为陀螺测量零偏；

为

的误差；

(2)建立基于ST-EKF的惯性/里程计组合导航***的误差状态方程；

其中：x为误差状态向量，

为误差状态向量的微分，F为***矩阵，G为噪声转移矩阵，w为过程噪声向量；

表示速度误差矢量；

δrⁿ表示位置误差矢量；δk_D表示里程计刻度因子误差；δα表示捷联惯导***与车体之间的安装误差，包括俯仰角安装误差δα_y和偏航角安装误差δα_z；L^b表示捷联惯导***与里程计之间的杆臂向量；

τ_δk表示里程计刻度因子误差相关时间；τ_δαy表示俯仰安装误差角相关时间；w中的元素w_bg、w_ba、w_δk、

分别表示陀螺零偏噪声、加速度计零偏噪声、刻度因子误差马尔科夫过程的白噪声、俯仰安装偏角误差马尔科夫过程的白噪声；F_{STEKF-INS-NED}表示ST-EKF滤波框架下的15状态***矩阵，定义如下：

v_N、v_E分别为北向、东向速度；R_N为子午圈主曲率半径；R_E为卯酉圈主曲率半径；L、h分别为纬度、高度；Ω为地球自转速度；

(3)建立基于ST-EKF的惯性/里程计组合导航***观测模型；

δz＝δz_v＝Hx+v_4×1

其中，v_4×1是测量噪声向量；H＝H_v是观测矩阵：

v_D代表里程计测量的前向速度；

所述基于ST-EKF的惯性/里程计组合导航模型完成前向滤波的方法如下：

惯性/里程计组合导航***开始运动后，首先进行捷联惯导解算，当运动时间达到惯性/里程计组合导航***设定的滤波周期时，开始前向滤波，步骤如下：

(1)时间更新；

t时刻的***矩阵F(t)离散化为***矩阵Φ_k,k-1，t时刻噪声转移矩阵G(t)离散化为噪声转移矩阵Γ_k,k-1，下标k-1,k为滤波更新时刻；

x_f,k/k-1＝Φ_k,k-1x_f,k-1

x_f,k-1表示前向滤波过程中k-1时刻的误差状态向量，P_f,k-1,表示前向滤波过程中k-1时刻的均方误差矩阵，Q_k-1表示k-1时刻的过程噪声方差矩阵；

(2)量测更新；

x_f,k＝x_f,k/k-1+K_f,k(z_f,k-H_f,kx_f,k/k-1)

K_f,k表示前向滤波过程中k时刻的增益矩阵，H_f,k表示前向滤波过程中k时刻的观测矩阵，R_f表示前向滤波过程中的观测噪声矩阵，z_f,k表示前向滤波过程中k时刻的观测矩阵，I表示单位矩阵；

每一次前向滤波过程中，记录下时间更新后输出的滤波信息和量测更新后输出的滤波信息，其中时间更新后输出的滤波信息包括误差状态向量x_f,k/k-1、均方误差矩阵P_f,k/k-1和状态转移矩阵Φ_k,k-1，量测更新后输出的滤波信息包括误差状态向量x_f,k和均方误差矩阵P_f,k，并将时间更新后输出的滤波信息和量测更新后输出的滤波信息实时存储入预先设置好的滑动窗口空间并标注好时刻。

2.根据权利要求1所述的惯性/里程计组合导航滤波方法，其特征在于：轮式里程计接收的数据信息包括：无人车辆行驶的瞬时速度信息。

3.根据权利要求1所述的惯性/里程计组合导航滤波方法，其特征在于：捷联惯导接收的数据信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息。

4.根据权利要求1所述的惯性/里程计组合导航滤波方法，其特征在于：基于ST-EKF的惯性/里程计组合导航模型包括基于ST-EKF的惯性/里程计组合导航***的误差状态方程和基于ST-EKF的惯性/里程计组合导航***观测模型，根据基于ST-EKF的惯性/里程计组合导航模型完成前向滤波。

5.一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于：所述处理器执行所述计算机程序时实现权利要求1至4中任一项所述惯性/里程计组合导航滤波方法的步骤。

6.一种存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现权利要求1至4中任一项所述惯性/里程计组合导航滤波方法的步骤。

Images