CN109470276B - 基于零速修正的里程计标定方法与装置 - Google Patents

Abstract

本发明涉及基于零速修正的里程计标定方法与装置，属于测量技术领域，其中标定方法通过选取一段航程大于设定距离的路段作为标定航线，在标定航线中每隔设定时间停车一次，获取标定航线行驶过程和停车时的惯性导航系统的输出值，利用零速修正算法得到惯性导航系统的精确位置，并将其作为里程计标定的位置基准，利用航迹推算算法得到标定航线终点的位置信息；最后根据两个标定航线终点的位置信息和标定航线上起点的位置信息计算确定标定参数。本发明不需要额外的传感器或者额外的观测信息，只利用惯性导航系统和里程计的观测信息就可以完成标定，为军事应用、尤其是卫星信号缺失环境下，提供了一种有效的参数标定手段。

Description

基于零速修正的里程计标定方法与装置

技术领域

本发明属于测量技术领域，具体涉及基于零速修正的里程计标定方法与装置。

背景技术

惯性/里程计组合导航系统作为一种低成本、高精度、自主的导航手段，在陆地导航尤其是军事行动中应用广泛，里程计作为惯性/里程计组合导航系统的主要传感器之一，其标定参数(包括刻度系数与安装偏差角)的准确程度将影响整个组合导航系统的定位定向精度。因此，为保证组合导航系统的精度，需要对里程计的刻度系数与安装偏差角进行精确标定。

惯性导航系统(INS，简称惯导)是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过陀螺仪和加速度计测量载体的加速度，进而得到载体在导航坐标系中的速度、姿态和位置等信息。

现有的标定方法主要有两种：一是基于轨迹相似原理，如作者时海涛等在2015年公开在期刊《导航与控制》的论文《里程计初始标定及误差分析》里提到，在两参考基准点之间，选取一段平直的路段作为标定线路，根据航迹推算轨迹和真实轨迹相似的原理对里程计的刻度系数和航向安装偏角进行标定。该方法的缺点在于，除了惯性导航系统和里程计之外还需要额外的参考点信息。

二是在线估计算法，如作者张金余等在2011年公开在期刊《导航与控制》的论文《捷联惯导与里程计组合导航系统标定方法研究》里提到，把里程计的安装偏差角及刻度系数作为组合导航的误差参数进行在线估计。该方法的缺点在于，刻度系数误差是经过初标定的小量，且里程计误差参数估计效果受载体机动的影响，且需要一定的时间收敛。

综上，目前的里程计标定方法需要额外的传感器或者额外的观测信息导致标定过程复杂的问题。

发明内容

本发明的目的是提供基于零速修正的里程计标定方法与装置，用于解决目前里程计标定方法需要额外的传感器或者额外的观测信息导致标定过程复杂的问题。

为解决上述技术问题，本发明提出一种基于零速修正的里程计标定方法，包括以下步骤：

1)选取一段航程大于设定距离的路段作为标定航线；

2)将惯性导航系统和里程计安装到载体上，通过惯性导航系统记录载体在标定航线上起点的位置信息；

3)控制载体在标定航线上行驶，根据里程计测量的速度值和惯性导航系统测量航位信息通过航迹推算确定标定航线终点的位置信息；

4)控制载体在标定航线上每隔设定时间停止一次，且至少停止三次，采用零速修正算法得到标定航线终点的位置信息；

5)根据步骤3)中标定航线终点的位置信息、步骤4)中标定航线终点的位置信息以及标定航线上起点的位置信息计算里程计航向安装偏差角和刻度系数。

为解决上述技术问题，本发明还提出一种基于零速修正的里程计标定装置，包括处理器，该处理器用于连接安装到载体上的惯性导航系统和里程计，处理器用于获取标定航线后实现以上步骤2)-步骤5)。

本发明通过在标定航线中每隔设定时间停车一次，获取标定航线行驶过程和停车时的惯性导航系统的输出值，利用零速修正算法得到惯性导航系统的精确位置，并将其作为里程计标定的位置基准，利用航迹推算算法得到标定航线终点的位置信息；最后，根据上述两个标定航线终点的位置信息和标定航线上起点的位置信息计算确定标定参数。本发明不需要额外的传感器或者额外的观测信息，只利用惯性导航系统和里程计的观测信息就可以完成标定，为军事应用、尤其是卫星信号缺失环境下，提供了一种有效的参数标定手段。

里程计航向安装偏差角和刻度系数的计算式如下：

式中，ε_y为安装偏差角，k为刻度系数，L₀为步骤2)中标定航线上起点的位置信息中的初始纬度，λ₀为步骤2)中标定航线上起点的位置信息中的初始经度，L_Z(t_s)为步骤4)中采用零速修正算法得到标定航线终点的位置信息中的纬度，λ_Z(t_s)为步骤4)中采用零速修正算法得到标定航线终点的位置信息中的经度，L_D(t_s)为步骤3)中通过航迹推算确定标定航线终点的位置信息中的纬度，λ_D(t_s)为步骤3)中通过航迹推算确定标定航线终点的位置信息中的经度，r_N为地球椭球的子午圈曲率半径，r_E为地球椭球的卯酉圈曲率半径，R为地球的半径，e为地球椭球的扁率，T表示求矩阵的转置。

作为地球椭球的子午圈曲率半径、卯酉圈曲率半径的进一步限定，该子午圈曲率半径、卯酉圈曲率半径的计算公式分别如下：

附图说明

图1是本发明的惯性导航系统速度误差示意图；

图2是本发明的里程计标定原理框图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的说明。

标定方法实施例：

惯性导航系统能够实时自主地提供载体的姿态、速度和位置信息，更新频率快，短时精度高，在陆地导航中得到广泛应用。但是惯性导航的误差会随时间累积。而零速修正(Zero Velocity Update，ZUPT，即零速修正算法)是解决车载惯性导航系统误差累积的一种廉价而有效的手段。它利用车辆停止时惯性导航系统的输出作为系统速度误差的观测量，进而对其位置误差进行修正。

由于惯性器件误差等的影响，惯性导航系统的速度误差近似为舒勒周期振荡过程，如图1所示。

基于零速修正的里程计标定方法包括如下步骤：

步骤1：选取一段较为平直的公路作为标定航线，标定航程大于20公里。

步骤2：将惯性导航系统、里程计安装到载体(即车辆)上，惯性导航系统上电启动，进行惯性导航系统的初始对准，初始对准完成之后记录初始纬度、经度坐标值(L₀、λ₀)。

步骤3：车辆启动，惯性导航系统工作在纯惯性导航状态(纯惯性导航状态即只利用惯性导航系统本身的陀螺仪和加速度计进行解算，不利用外界传感器)，惯性导航系统的经纬度信息通过航位推算(简称DR)进行更新，航位推算的计算式如下：

其中，

为惯性导航系统计算得到的方向余弦矩阵，

为里程计的速度测量值。由航迹推算得到的标定航程终点坐标值记为(L_D(t_s)，λ_D(t_s))。

车辆在行驶过程中每5分钟停车一次(停车间隔取值为5-10分钟)，至少停车三次，利用零速修正算法计算停车点的位置，由零速修正算法得到的标定航程终点的位置信息的坐标值记为(L_Z(t_s)，λ_Z(t_s))。

步骤4：利用初始纬度、经度坐标值(L₀、λ₀)，以及步骤3得到的2个终点坐标(L_D(t_s)，λ_D(t_s))、(L_Z(t_s)，λ_Z(t_s))，可以用以下公式(2)、(3)计算航向安装偏差角ε_y和刻度系数k，原理框图如图2所示。

其中，ε_y为安装偏差角，k为刻度系数，L₀为标定航线上起点的位置信息中的初始纬度，λ₀为标定航线上起点的位置信息中的初始经度，L_Z(t_s)为采用零速修正算法得到标定航线终点的位置信息中的纬度，λ_Z(t_s)为采用零速修正算法得到标定航线终点的位置信息中的经度，L_D(t_s)为通过航迹推算确定标定航线终点的位置信息中的纬度，λ_D(t_s)为通过航迹推算确定标定航线终点的位置信息中的经度，r_N为地球椭球的子午圈曲率半径，r_E为地球椭球的卯酉圈曲率半径，R为地球的半径，e为地球椭球的扁率，T表示求矩阵的转置。

上述航向安装偏差角ε_y和刻度系数k的推导过程如下：

在导航坐标系(NUE地理坐标系)下，里程计(坐标系为前-上-右)的测量值可以表示为：

其中，

为里程计的速度测量值，ε为安装偏差角，ε×为安装偏差角ε的斜对称矩阵，

为b系到d系转换矩阵的计算值，

为b系到d系转换矩阵的真值，

为n系到b系转换矩阵的真值，vⁿ为n系下的速度真值，δk为刻度系数误差。

根据上式整理可以得到：

忽略二阶小量有：

整理得：

目前，通过安装工艺可以保证惯性导航系统和里程计的安装偏差角为小量。当载体航行时，向上速度

和侧向速度值

也是小量，因此，滚动安装偏差角ε_x的影响可以忽略。本发明只考虑主要的误差项，标定时仅考虑里程计的刻度系数误差δk、方位安装偏角ε_y。

考虑到载体行驶时一般采用水平的航行方式，因此，水平姿态角θ、γ为小角度，近似有：

其中，

为方位角，θ为俯仰角、γ为滚动角。按照公式(9)，则有：

其中，

为

为里程计测量的前向速度，在匀速直航的条件下，向上速度

和侧向速度

为小量，且忽略二阶小量，有：

展开得：

其中，

v_N为北向速度，v_E为东向速度，由于车在地面上行驶，认为天向没有速度，即速度为零，因此vⁿ的中间元素为零。

考虑到ε_x、ε_y、ε_z均为小量，进一步忽略二阶小量，可得：

根据公式(14)可以得到：

对上式两边积分：

上式两边同时点积

得到：

考虑到：

因而方位安装偏差角可以表示为：

公式(16)两边同时点积

得到：

考虑到：

因而刻度系数误差δk的表达式为：

若在[t₀,t_s]时间内，载体做近似匀速直航运动，则有：

把公式(23)-(25)代入公式(19)、(22)可以得到计算航向安装偏差角ε_y和刻度系数k的公式(2)、(3)。

上述步骤3中利用零速修正算法计算停车点的位置的过程如下：

采用二次曲线拟合法来估计惯性导航系统的误差，如图1所示，假设t_i到t_i+2时刻惯性导航系统的速度误差可以由二次曲线表示为：

其中，dv_i表示惯性导航系统在t_i时刻的速度误差，a_i,0、a_i,1、a_i,2表示二次曲线的系数。由于车辆停止时惯性导航系统的速度输出就是惯性导航系统的速度误差值，因此，在3个停车时刻t_i、t_i+1、t_i+2，就可以得到以下3个观测方程：

由以上3个方程就可以解出二次曲线的系数a_i,0、a_i,1、a_i,2，从而可以求得t_i到t_i+2的位置误差δp_i：

把惯性导航系统的输出的位置减去位置误差就可以得到较为准确的停车点的位置。

利用零速修正算法得到的停车点的位置精度较高，因此，以零速修正得到位置信息作为参考基准，对里程计的刻度系数和航向安装偏角进行标定。本发明的里程计标定方法不需要里程计参数(刻度系数、安装偏差角)的初值，只利用惯性导航系统和里程计的观测信息就可以完成标定，即同时完成里程计刻度系数和航向安装偏差角的标定，不需要额外的传感器，为军事应用，尤其是卫星信号缺失环境下，提供了一种有效的里程计标定手段。

标定装置实施例：

一种基于零速修正的里程计标定装置，包括处理器，该处理器用于连接安装到载体上的惯性导航系统和里程计，处理器用于在获取标定航线后，实现标定方法实施例中的步骤，即该标定装置不用于选定标定航线，仅在标定航线选定后，获取标定航线。

上述实施例中所指的标定装置，实际上是基于本发明方法流程的一种计算机解决方案，即一种软件构架，可以应用到计算机中，上述装置即为与方法流程相对应的处理进程。由于对上述方法的介绍已经足够清楚完整，故不再详细进行描述。

Claims (4)

1.一种基于零速修正的里程计标定方法，其特征在于，该标定方法包括以下步骤：

1)选取一段航程大于设定距离的路段作为标定航线；

2)将惯性导航系统和里程计安装到载体上，通过惯性导航系统记录载体在标定航线上起点的位置信息；

3)控制载体在标定航线上行驶，根据里程计测量的速度值和惯性导航系统测量航位信息通过航迹推算确定标定航线终点的位置信息；

4)控制载体在标定航线上每隔设定时间停止一次，且至少停止三次，采用零速修正算法得到标定航线终点的位置信息；

5)根据步骤3)中标定航线终点的位置信息、步骤4)中标定航线终点的位置信息以及标定航线上起点的位置信息计算里程计航向安装偏差角和刻度系数；

步骤4)中采用零速修正算法得到标定航线终点的位置的过程如下：

采用二次曲线拟合法估计惯性导航系统的误差，设t_i到t_i+2时刻惯性导航系统的速度误差由二次曲线表示为：

其中，dv_i表示惯性导航系统在t_i时刻的速度误差，a_i,0、a_i,1、a_i,2表示二次曲线的系数；

在3个停车时刻t_i、t_i+1、t_i+2，得到以下3个观测方程：

由以上3个方程解出二次曲线的系数a_i,0、a_i,1、a_i,2，从而可以求得t_i到t_i+2的位置误差δp_i：

把惯性导航系统输出的位置减去位置误差得到停车点的位置；

将最后一次停车点的位置作为标定航线终点的位置；

所述里程计航向安装偏差角和刻度系数的计算式如下：

式中，ε_y为所述安装偏差角，k为所述刻度系数，L₀为步骤2)中所述标定航线上起点的位置信息中的初始纬度，λ₀为步骤2)中所述标定航线上起点的位置信息中的初始经度，L_Z(t_s)为步骤4)中采用零速修正算法得到标定航线终点的位置信息中的纬度，λ_Z(t_s)为步骤4)中采用零速修正算法得到标定航线终点的位置信息中的经度，L_D(t_s)为步骤3)中通过航迹推算确定标定航线终点的位置信息中的纬度，λ_D(t_s)为步骤3)中通过航迹推算确定标定航线终点的位置信息中的经度，r_N为地球椭球的子午圈曲率半径，r_E为地球椭球的卯酉圈曲率半径，T表示求矩阵的转置。

2.根据权利要求1所述的基于零速修正的里程计标定方法，其特征在于，地球椭球的子午圈曲率半径、卯酉圈曲率半径的计算公式分别如下：

式中，R为地球的半径，e为地球椭球的扁率。

3.一种基于零速修正的里程计标定装置，其特征在于，包括处理器，该处理器用于连接安装到载体上的惯性导航系统和里程计，所述处理器用于执行计算机程序实现以下步骤：

1)获取标定航线；

2)通过惯性导航系统记录载体在标定航线上起点的位置信息；

3)控制载体在标定航线上行驶，根据里程计测量的速度值和惯性导航系统测量航位信息通过航迹推算确定标定航线终点的位置信息；

4)控制载体在标定航线上每隔设定时间停止一次，且至少停止三次，采用零速修正算法得到标定航线终点的位置信息；

5)根据步骤3)中标定航线终点的位置信息、步骤4)中标定航线终点的位置信息以及标定航线上起点的位置信息计算里程计航向安装偏差角和刻度系数；

步骤4)中采用零速修正算法得到标定航线终点的位置的过程如下：

采用二次曲线拟合法估计惯性导航系统的误差，设t_i到t_i+2时刻惯性导航系统的速度误差由二次曲线表示为：

其中，dv_i表示惯性导航系统在t_i时刻的速度误差，a_i,0、a_i,1、a_i,2表示二次曲线的系数；

在3个停车时刻t_i、t_i+1、t_i+2，得到以下3个观测方程：

由以上3个方程解出二次曲线的系数a_i,0、a_i,1、a_i,2，从而可以求得t_i到t_i+2的位置误差δp_i：

把惯性导航系统输出的位置减去位置误差得到停车点的位置；

将最后一次停车点的位置作为标定航线终点的位置；

所述里程计航向安装偏差角和刻度系数的计算式如下：

式中，ε_y为所述安装偏差角，k为所述刻度系数，L₀为步骤2)中所述标定航线上起点的位置信息中的初始纬度，λ₀为步骤2)中所述标定航线上起点的位置信息中的初始经度，L_Z(t_s)为步骤4)中采用零速修正算法得到标定航线终点的位置信息中的纬度，λ_Z(t_s)为步骤4)中采用零速修正算法得到标定航线终点的位置信息中的经度，L_D(t_s)为步骤3)中通过航迹推算确定标定航线终点的位置信息中的纬度，λ_D(t_s)为步骤3)中通过航迹推算确定标定航线终点的位置信息中的经度，r_N为地球椭球的子午圈曲率半径，r_E为地球椭球的卯酉圈曲率半径，T表示求矩阵的转置。

4.根据权利要求3所述的基于零速修正的里程计标定装置，其特征在于，地球椭球的子午圈曲率半径、卯酉圈曲率半径的计算公式分别如下：

式中，R为地球的半径，e为地球椭球的扁率。

Images