CN112762961A - 一种车载惯性里程计组合导航在线标定方法 - Google Patents

Abstract

本发明涉及一种车载惯性/里程计组合导航在线标定方法，利用惯导系统和里程计实现里程刻度系数误差和航向安装误差的在线标定，采集和处理陀螺和加速度计数据，并完成载车怠速状态下初始粗对准和精对准；再进行纯惯导解算和航位推算，计算状态转移矩阵和量测矩阵，将得到的纯惯性位置和航位推算位置作差得到滤波观测量；在滤波时刻进行卡尔曼滤波并实时修正；经过一段时间的机动后可以完成里程刻度系数误差和航向安装误差的在线标定。本发明的方法解决了在复杂环境下，惯性/里程计组合导航系统由于里程刻度系数和航向安装误差的变化造成的定位精度降低的问题，显著提高了惯性/里程计组合导航系统的可靠性。

Description

一种车载惯性里程计组合导航在线标定方法

技术领域

本发明属于导航技术领域，尤其是涉及一种车载惯性里程计组合导航在线标 定方法。

背景技术

惯性导航系统能够提供全方位的导航信息，并且具备完全的自主性，因此得 到了广泛的应用，但惯性导航系统误差随时间不断积累。里程计是测量车辆行驶 速度和路程的测量装置，具备完全自主，精度高，动态性能好，测量误差不随时 间发散的优点，能够与惯性导航系统构成组合导航系统以实现优势互补，达到全 自主、高精度定位导航的目的。

惯性/里程计组合导航系统在实际使用过程中，需要标定里程刻度系数和里 程计安装误差角，现有的标定方法通常为基于轨迹相似原理的标定方法：装订粗 略的里程计的标定参数，利用惯性和里程计进行航位推算得到计算导航值，利用 轨迹相似原理将该导航值和真实位置直接计算里程计的标定参数变化值，以得到 准确的里程计的标定参数。

然而里程刻度系数容易受到轮胎温度、充气压力以及表面磨损程度的影响， 同时由于初始对准误差和惯性器件误差的存在，若里程计安装误差角为常值，则 会使载体坐标系和里程计坐标系存在偏差，同样影响惯性/里程计组合导航的定 位精度。

发明内容

为解决上述技术问题，本发明提供一种车载惯性里程计组合导航在线标定方 法。

本发明采用的技术方案是：一种车载惯性里程计组合导航在线标定方法，利 用惯导系统和里程计实现里程刻度系数误差和航向安装误差的在线标定，包括以 下步骤：

步骤一装订初始导航参数；

步骤二采集陀螺和加速度计的输出数据，在载车怠速条件下，采用惯性系对 准的方法完成系统的粗对准，随后采用卡尔曼滤波技术完成精对准；

步骤三进入导航模式，对卡尔曼滤波器参数进行初始化；

步骤四进行纯惯性解算，将采集里程计脉冲信号转化为位移信息，结合纯惯 导解算的姿态信息进行航位推算；

步骤五根据陀螺加表数据和导航信息计算状态转移矩阵和量测矩阵，将计算 得到的纯惯性位置和航位推算位置作差得到滤波观测量；

步骤六在滤波时刻进行卡尔曼滤波，以速度误差的状态估计结果实时修正纯 惯导的速度信息；

步骤七经过机动后，实现里程刻度系数误差和航向安装误差的在线标定

优选地，坐标系定义如下：

n：导航坐标系ox_ny_nz_n，东北天地理坐标系，x轴指东，y轴指北，z轴指天；

b：载体坐标系ox_by_bz_b，x轴指载体的右方，y轴指载体的前方，z轴指载体 的上方；

m：里程计理想测量坐标系ox_my_mz_m，x轴指载车的右方，y轴指载车的前方， z轴指载车的上方。

优选地，步骤四中，里程计的速度输出在m系上可以表示为：

其中，v_D为里程仪测得的前向速度大小，右向和天向速度均为零；

假设b系与m系的同名坐标轴重合，通过惯性测量装置中的三陀螺组合可以 实时计算载车的姿态矩阵，记为

利用

对

转换可得在导航坐标系下里程 仪速度输出，即：

航位推算位置更新算法如下：

其中，p_D＝[L_D λ_D h_D]^T，L_D、λ_D和h_D分别为航位推算的地理纬度、经度和 高度；R_MhD＝R_MD+h_D，R_NhD＝R_ND+h_D，R_MD和R_ND分别为使用航位推算地理位置 计算的子午圈和卯酉圈主曲率半径。

优选地，步骤五中，假设从m系至b系存在小量的安装偏差角，即绕车体横 轴ox_m、纵轴oy_m及竖轴oz_m分别存在俯仰δα_θ、滚动δα_γ和方位δα_ψ偏角，记偏 差角矢量α＝[δα_θ δα_γ δα_ψ]^T。其中δα_γ不影响里程计的速度测量值，δα_θ对 里程计的速度测量值影响远远小于δα_ψ，里程计刻度系数误差可表示为δK_D， 地球自转角速率记为

载体绕地球运动产生的角速率记为

加速度计测量 的比力记为

方向余弦姿态阵记为

纯惯导的位置误差和航位推算的位置 分别记为p和p_D，载车真实位置记为p_real，纯惯导的姿态误差记为φ，纯惯导 的速度误差记为δvⁿ，纯惯导的纬度误差和经度误差分别记为δL和δλ，航位推 算的纬度误差和经度误差分别记为δL_D和δλ_D，设计如下16维惯导/里程计组合 导航误差模型：

其中，

纯惯导的位置误差和航位推算的位置误差分别记为δp和δp_D，设计量测方 程矩阵为：

z＝p-p_D＝(p_real+δp)-(p_real+δp_D)

＝δp-δp_D，

H_SINS/DR＝[0_2×3 0_2×2 I_2×2 0_2×3 0_2×3 -I_2×2 0_2×1 0_2×1]。

优选地，步骤六中，卡尔曼滤波周期为1s，运用卡尔曼滤波修正纯惯性计 算速度的计算方法如下：

vⁿ＝vⁿ-δvⁿ

修正速度后需要将卡尔曼滤波速度误差估计量δvⁿ置零。

优选地，步骤七中，

基于组合导航误差模型与量测模型，建立误差状态方程和量测方程，运用卡 尔曼滤波器进行最优估计计算，经过机动后得到里程刻度系数误差δK_D和航向 安装误差δα_ψ，则里程刻度系数和航向安装误差补偿方法如下：

K_D＝K_D-δK_D，α_ψ＝α_ψ-δα_ψ。

本发明具有的优点和积极效果是：相比于传统的基于轨迹相似原理的标定方 法，除载车起始点外该方法不需要其他外部的标定点位置信息，提高了标定效率； 该标定方法不需要停车，只需要进行适当的机动便可进行实时标定；相比于现有 的基于卡尔曼滤波的的标定方法，该方法针对惯性/里程计组合导航系统的主要 误差源，建立了16维滤波模型，更加稳定可靠，并且大大降低了导航计算机的 运算负荷。

附图说明

图1是本发明一个实施例惯性/里程计组合导航在线标定的原理框图；

图2是本发明一个实施例惯性/里程计组合导航在线标定的总体流程图；

图3是本发明一个实施例载体运动轨迹仿真图；

图4是本发明一个实施例里程系数误差估计曲线；

图5是本发明一个实施例航向安装误差估计曲线；

图6是本发明一个实施例航位推算水平定位误差曲线；

图7是本发明一个实施例在线自标定水平定位误差曲线。

具体实施方式

下面结合附图对本发明的一个实施例做出说明。

本发明提出一种工程适用的车载惯性/里程计组合导航在线标定方法，实现 里程刻度系数和里程计安装误差的在线标定；解决目前在使用过程中里程刻度系 数和里程计安装误差发生变化而影响定位精度的问题。

如图1图2所示，本发明是一种车载惯性/里程计组合导航在线标定方法， 利用惯导系统和里程计实现里程刻度系数误差和航向安装误差的在线标定，该在 线标定方法包括以下步骤：

步骤0：将惯组和里程计通过过渡板安装到载车上，系统上电启动；

步骤1：装订初始参数，包括初始的经度、纬度、高度、里程计刻度系数、 里程计安装误差角初值至导航计算机；

步骤2：采集陀螺和加速度计的输出数据，对采集到的陀螺和加速度计数据 进行处理，在载车怠速条件下，采用惯性系对准的方法完成系统的粗对准，以确 定载体的姿态角的粗略值，粗对准时间为2分钟，随后采用卡尔曼滤波技术完成 精对准，精对准时间为3分钟；

步骤3：对准模式完成后进入惯性/里程组合导航模式，对卡尔曼滤波器参数 进行初始化；

步骤4：进行纯惯性解算，将采集里程计脉冲信号转化为位移信息，结合纯 惯导解算的姿态信息进行航位推算；

步骤5：根据陀螺加表数据和导航信息计算状态转移矩阵和量测矩阵，将计 算得到的纯惯性位置和航位推算位置作差得到滤波观测量；

步骤6：在滤波时刻进行卡尔曼滤波，将速度误差的状态估计结果实时修正 纯惯导的速度信息；

步骤7：经过一段时间的机动后，可以实现里程刻度系数误差和航向安装误 差的在线标定。

其中，坐标系定义如下：

n：导航坐标系ox_ny_nz_n，东北天地理坐标系，x轴指东，y轴指北，z轴指天； b：载体坐标系ox_by_bz_b，x轴指载体的右方，y轴指载体的前方，z轴指载体的上 方；m：里程计理想测量坐标系ox_my_mz_m，x轴指载车的右方，y轴指载车的前方， z轴指载车的上方；

所述步骤4中，航位推算具体实现方法如下：

里程计的速度输出在m系上可以表示为：

其中，v_D为里程仪测得的前向速度大小，右向和天向速度均为零，可视为 载车正常行驶时的速度约束条件。

假设b系与m系的同名坐标轴重合，通过惯性测量装置中的陀螺组合可以实 时计算载车的姿态矩阵

利用

对

转换可得在导航坐标系下里程仪速度输 出，即：

航位推算位置更新算法如下：

其中，p_D＝[L_D λ_D h_D]^T，L_D、λ_D和h_D分别为航位推算的地理纬度、经度和 高度；R_MhD＝R_MD+h_D，R_NhD＝R_ND+h_D，R_MD和R_ND分别为使用航位推算地理位置 计算的子午圈和卯酉圈主曲率半径；

在航位推算姿态更新算法与捷联惯导更新算法一致，只需将里程计计算速度 代替捷联惯导速度，并以航位推算纬度代替捷联惯导解算纬度。

所述步骤5中，根据陀螺加表数据和导航信息计算状态转移矩阵和量测矩阵 的具体实现方法如下：

假设从m系至b系存在小量的安装偏差角，即绕车体横轴ox_m、纵轴oy_m及 竖轴oz_m分别存在俯仰δα_θ、滚动δα_γ和方位δα_ψ偏角，记偏差角矢量 α＝[δα_θ δα_γ δα_ψ]^T。其中δα_γ不影响里程计的速度测量值，δα_θ对里程计的 速度测量值影响远远小于δα_ψ，里程计刻度系数误差可表示为δK_D，地球自转 角速率记为

载体绕地球运动产生的角速率记为

加速度计测量的比力记 为

方向余弦姿态阵记为

纯惯导的位置误差和航位推算的位置分别记为 p和p_D，载车真实位置记为p_real，纯惯导的姿态误差记为φ，纯惯导的速度误 差记为δvⁿ，纯惯导的纬度误差和经度误差分别记为δL和δλ，航位推算的纬度 误差和经度误差分别记为δL_D和δλ_D。为了减小系统内导航计算机的计算负担， 忽略天向相关的状态量，设计如下16维惯导/里程计组合导航误差模型：

其中

纯惯导的位置误差和航位推算的位置误差分别记为δp和δp_D，设计量测方 程矩阵为：

z＝p-p_D＝(p_real+δp)-(p_real+δp_D)

＝δp-δp_D

H_SINS/DR＝[0_2×3 0_2×2 I_2×2 0_2×3 0_2×3 -I_2×2 0_2×1 0_2×1]

所述步骤6中，运用卡尔曼滤波修正纯惯性计算速度的具体实现方法如下：

vⁿ＝vⁿ-δvⁿ

修正速度后需要将卡尔曼滤波速度误差估计量δvⁿ置零。

所述步骤7中，里程刻度系数和航向安装误差的具体补偿方法如下：

基于上述建立的组合导航误差模型与量测模型，建立误差状态方程和量测方 程，运用卡尔曼滤波器进行最优估计计算，经过一段时间机动后得到里程刻度系 数误差δK_D和航向安装误差δα_ψ，则里程刻度系数和航向安装误差补偿方法为：

K_D＝K_D-δK_D，α_ψ＝α_ψ-δα_ψ。

相比于传统的基于轨迹相似原理的标定方法，除载车起始点外该方法不需要其他外部的标定点位置信息，提高了标定效率；

相比于传统的基于轨迹相似原理的标定方法，该标定方法不需要停车，只需要进行适当的机动便可进行实时标定；

相比于现有的基于卡尔曼滤波的的标定方法，该方法针对惯性/里程计组合导航系统的主要误差源，建立了16维滤波模型，更加稳定可靠，并且大大降低了导 航计算机的运算负荷。

本发明的方法解决了在复杂环境下，惯性/里程计组合导航系统由于里程刻 度系数和航向安装误差的变化造成的定位精度降低的问题，显著提高了惯性/里 程计组合导航系统的可靠性。

下面通过实施例对本发明进一步说明。

实施例：

按照上述步骤进行仿真，仿真条件设置如下：

(1)传感器相关参数设置如表1所示；

(2)载体运动轨迹参数设置如表2所示

(3)载体运动轨迹三维图如图3所示；

表1传感器相关参数设置

表2仿真轨迹参数设置

里程刻度系数和航向安装误差估计曲线分别如图4和图5所示，估计精度如 表3所示。可以看出本专利提出的在线自标定方案能准确估计出里程计的相关误 差系数；航位推算定位结果和在线自标定定位误差曲线如图6图7所示，其中δX 和δY分别表示2000坐标系下X坐标和Y坐标的误差，由表4所示，可以看出相 比于航位推算而言，在线自标定方案能有效地抑制由于里程计相关误差所引起的 定位误差。

表3里程计误差参数估计结果

表4水平定位结果

定位方法	δX	δY	终点定位误差(m)
				航位推算	36.7	114.3	151
在线自标定	-1.2	-0.8	2

以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施 例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变 化与改进等，均应仍归属于本发明的专利涵盖范围之内。

Claims (6)

1.一种车载惯性里程计组合导航在线标定方法，其特征在于：利用惯导系统和里程计实现里程刻度系数误差和航向安装误差的在线标定，包括以下步骤：

步骤一装订初始导航参数；

步骤二采集陀螺和加速度计的输出数据，在载车怠速条件下，采用惯性系对准的方法完成系统的粗对准，随后采用卡尔曼滤波技术完成精对准；

步骤三进入导航模式，对卡尔曼滤波器参数进行初始化；

步骤四进行纯惯性解算，将采集里程计脉冲信号转化为位移信息，结合纯惯导解算的姿态信息进行航位推算；

步骤五根据陀螺加表数据和导航信息计算状态转移矩阵和量测矩阵，将计算得到的纯惯性位置和航位推算位置作差得到滤波观测量；

步骤六在滤波时刻进行卡尔曼滤波，以速度误差的状态估计结果实时修正纯惯导的速度信息；

步骤七经过机动后，实现里程刻度系数误差和航向安装误差的在线标定。

2.根据权利要求1所述的车载惯性里程计组合导航在线标定方法，其特征在于：坐标系定义如下：

n：导航坐标系ox_ny_nz_n，东北天地理坐标系，x轴指东，y轴指北，z轴指天；

b：载体坐标系ox_by_bz_b，x轴指载体的右方，y轴指载体的前方，z轴指载体的上方；

m：里程计理想测量坐标系ox_my_mz_m，x轴指载车的右方，y轴指载车的前方，z轴指载车的上方。

3.根据权利要求2所述的车载惯性里程计组合导航在线标定方法，其特征在于：所述步骤四中，里程计的速度输出在m系上可以表示为：

其中，v_D为里程仪测得的前向速度大小，右向和天向速度均为零；

假设b系与m系的同名坐标轴重合，通过惯性测量装置中的三陀螺组合可以实时计算载车的姿态矩阵，记为

利用

对

转换可得在导航坐标系下里程仪速度输出，即：

航位推算位置更新算法如下：

其中，p_D＝[L_D λ_D h_D]^T，L_D、λ_D和h_D分别为航位推算的地理纬度、经度和高度；

R_MhD＝R_MD+h_D，R_NhD＝R_ND+h_D，R_MD和R_ND分别为使用航位推算地理位置计算的子午圈和卯酉圈主曲率半径。

4.根据权利要求3所述的车载惯性里程计组合导航在线标定方法，其特征在于：所述步骤五中，假设从m系至b系存在小量的安装偏差角，即绕车体横轴ox_m、纵轴oy_m及竖轴oz_m分别存在俯仰δα_θ、滚动δα_γ和方位δα_ψ偏角，记偏差角矢量α＝[δα_θ δα_γ δα_ψ]^T。其中δα_γ不影响里程计的速度测量值，δα_θ对里程计的速度测量值影响远远小于δα_ψ，里程计刻度系数误差可表示为δK_D，地球自转角速率记为

载体绕地球运动产生的角速率记为

加速度计测量的比力记为

方向余弦姿态阵记为

纯惯导的位置误差和航位推算的位置分别记为p和p_D，载车真实位置记为p_real，纯惯导的姿态误差记为φ，纯惯导的速度误差记为δvⁿ，纯惯导的纬度误差和经度误差分别记为δL和δλ，航位推算的纬度误差和经度误差分别记为δL_D和δλ_D，设计如下16维惯导/里程计组合导航误差模型：

其中，

纯惯导的位置误差和航位推算的位置误差分别记为δp和δp_D，设计量测方程矩阵为：

H_SINS/DR＝[0_2×3 0_2×2 I_2×2 0_2×3 0_2×3 -I_2×2 0_2×1 0_2×1]。

5.根据权利要求4所述的车载惯性里程计组合导航在线标定方法，其特征在于：所述步骤六中，卡尔曼滤波周期为1s，运用卡尔曼滤波修正纯惯性计算速度的计算方法如下：

vⁿ＝vⁿ-δvⁿ

修正速度后需要将卡尔曼滤波速度误差估计量δvⁿ置零。

6.根据权利要求5所述的车载惯性里程计组合导航在线标定方法，其特征在于：所述步骤七中，

基于组合导航误差模型与量测模型，建立误差状态方程和量测方程，运用卡尔曼滤波器进行最优估计计算，经过机动后得到里程刻度系数误差δK_D和航向安装误差δα_ψ，则里程刻度系数和航向安装误差补偿方法如下：

K_D＝K_D-δK_D，α_ψ＝α_ψ-δα_ψ。

Images