CN104180821B - 一种基于同步测量与定位计算的里程计标定方法 - Google Patents

Abstract

一种基于同步测量与定位计算的里程计标定方法，它有七大步骤：一、将惯组、激光测速仪和里程计安装到载体上，系统上电启动；二、装订初始参数至导航计算机；三、采集和处理陀螺和加速度计的输出数据；并完成系统的粗对准和精对准；四、导航系统由对准模式切换到航位推算模式，同时分别进行惯导/激光测速仪航位推算与惯导/里程计航位推算；五、分别采集惯导/激光测速仪航位推算输出经纬度值与惯导/里程计航位推算输出经纬度值；六、将采集的经纬度值转换成高斯坐标系下坐标值；七、将惯导/里程计航位推算得到的位置值与惯导/激光测速仪航位推算得到的位置值进行比较，利用轨迹相似原理对里程计的航向安装角及标度因数进行标定。

Description

一种基于同步测量与定位计算的里程计标定方法

技术领域：

本发明涉及一种基于同步测量与定位计算的里程计标定方法，属于惯性技术领域。

背景技术：

里程计是测量车辆行驶速度和路程的一种传感器，具有完全自主、精度高、测速范围宽、动态性能好、测量误差不随时间发散的优点。单独的里程计不具备导航定位功能，但是与惯导系统组合能优势互补，能实现全自主、高精度导航定位。

组合导航系统实际使用中捷联惯导系统和里程计分别装在载体的不同位置，需要标定里程计安装角，主要是针对航向安装角的标定。车轮周长受轮胎温度、充气压力以及表面磨损等因素的影响，需要同时对里程计标度因数进行标定。目前公开文献中用于导航定位领域的里程计并没有统一的标定方法，本文提出了一种里程计航向安装角和标度因数的标定方法。

发明内容

1、目的：本发明的目的是提供了一种基于同步测量与定位计算的里程计标定方法，它克服了现有技术的不足，解决了里程计装到载体上时需要标定航向安装角和标度因数的问题。

2、技术方案

本发明一种基于同步测量与定位计算的里程计标定方法，该方法具体步骤如下：

步骤1、将惯组、激光测速仪和里程计安装到载体上，系统上电启动。

步骤2、装订初始参数(包括初始的经度、纬度、高度、激光测速仪的标度、激光测速仪的安装角、里程计标度初值)至导航计算机。

步骤3、采集陀螺和加速度计的输出数据，对采集到的陀螺和加速度计数据进行处理，根据捷联惯导系统误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成系统的粗对准，初步确定载体姿态角。粗对准时间为2分钟。粗对准后利用卡尔曼滤波技术精对准3分钟。

步骤4、导航系统由对准模式切换到航位推算模式，同时分别进行惯导/激光测速仪航位推算与惯导/里程计航位推算，切换完成后载体开始运动，运动过程中保持航位推算模式。

步骤5、分别采集惯导/激光测速仪航位推算输出经纬度值与惯导/里程计航位推算输出经纬度值。

步骤6、将采集的经纬度值转换成高斯坐标系下坐标值。

步骤7、将惯导/里程计航位推算得到的位置值与惯导/激光测速仪航位推算得到的位置值进行比较，利用轨迹相似原理对里程计的航向安装角及标度因数进行标定。

其中，步骤6中所述“将采集的经纬度值转换成高斯坐标系下坐标值”，具体实现过程说明如下：

定义L、λ分别为纬度和经度，λ₀为中央子午线经度，x、_x为经纬度转化后的高斯坐标值，n为带区编号，INT(*)为取整运算。

n＝INT(λ/6)+1

λ₀＝6n-3

以上计算经纬度单位为度，将经纬度转高斯坐标运算时需要进行单位转换，将度转换成弧度。经纬度转高斯坐标公式如下：

X＝C₀L-(C₁sinL+C₂sin³L)cosL

m＝(λ-λ₀)cosL

C＝6399698.9018,C₀＝6367558.49686,C₁＝32005.79642,C₂＝133.86115,E₂＝0.006738525,Y₀＝500000；

其中，步骤7中所述“利用轨迹相似原理对里程计的航向安装角及标度因数进行标定”，具体实现过程说明如下：

定义初始对准位置的经度为λ₀、纬度为L₀，其高斯坐标为(x₀,y₀)；t时刻采集惯导/激光测速仪航位推算得到的经度为λ₁、纬度为L₁，其高斯坐标为(x₁,y₁)，惯导/里程计航位推算得到的经度为λ₂、纬度为L₂，其高斯坐标为(x₂,y₂)；S₁为t时刻激光测速仪航位推算得到的位置与初始对准位置之间的距离，称作参考轨迹；S₂为t时刻里程计航位推算得到的位置与初始对准位置之间的距离，称作航位轨迹；K₀为里程计标度初始装订值，K_D为里程计真实标度，P_D为里程计输出脉冲数，α_ψ为航向安装角。由轨迹相似原理计算得：

3、优点及功效：本发明一种基于同步测量与定位计算的里程计标定方法，该方法的优点是：相对目前公开的标定方法，该标定方法的算法简单，且用激光测速仪同步测量标定里程计不需要标准参考点以及停车标定，可进行在线标定。

附图说明

图1为测速仪航位推算轨迹与里程计航位推算轨迹图；

图2为里程计标定方法框图；

图3为本发明里程计标定方法的流程图。

图中符号说明如下：

λ₀：初始对准位置的经度

L₀：初始对准位置的纬度

λ₁：惯导/激光测速仪航位推算得到的经度

L₁：惯导/激光测速仪航位推算得到的纬度

λ₂：惯导/里程计航位推算得到的经度

L₂：惯导/里程计航位推算得到的纬度

O(x₀,y₀)：初始对准位置的高斯坐标

B(x₁,y₁)：惯导/激光测速仪航位推算得到的经纬度转换的高斯坐标

B'(x₂,y₂)：惯导/里程计航位推算得到的经纬度转换的高斯坐标

α_ψ：里程计航向安装角

K_D：里程计标度

S₁：t时刻惯导/激光测速仪航位推算得到的位置与初始对准位置之间的距离，称作参考轨迹

S₂：t时刻惯导/里程计航位推算得到的位置与初始对准位置之间的距离，称作航位轨迹。

具体实施方式

见图1—图3，本发明一种基于同步测量与定位计算的里程计标定方法，该方法具体步骤如下：

步骤1、将惯组、激光测速仪和里程计安装到载体上，系统上电启动。

步骤2、装订初始参数(包括初始的经度、纬度、高度、激光测速仪的标度、激光测速仪的安装角、里程计标度初值)至导航计算机。

步骤3、采集陀螺和加速度计的输出数据，对采集到的陀螺和加速度计数据进行处理，根据捷联惯导系统误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成系统的粗对准，初步确定载体姿态角。粗对准时间为2分钟。粗对准后利用卡尔曼滤波技术精对准3分钟。

步骤4、导航系统由对准模式切换到航位推算模式，同时分别进行惯导/激光测速仪航位推算与惯导/里程计航位推算，切换完成后载体开始运动，运动过程中保持航位推算模式。

步骤5、分别采集惯导/激光测速仪航位推算输出经纬度值与惯导/里程计航位推算输出经纬度值。

步骤6、将采集的经纬度值转换成高斯坐标系下坐标值。

步骤7、将惯导/里程计航位推算得到的位置值与惯导/激光测速仪航位推算得到的位置值进行比较，利用轨迹相似原理对里程计的航向安装角及标度因数进行标定。

其中，步骤6中所述“将采集的经纬度值转换成高斯坐标系下坐标值”，具体实现过程说明如下：

定义L、λ分别为纬度和经度，λ₀为中央子午线经度，x、y为经纬度转化后的高斯坐标值，n为带区编号，INT(*)为取整运算。

n＝INT(λ/6)+1

λ₀＝6n-3

以上计算经纬度单位为度，将经纬度转高斯坐标运算时需要进行单位转换，将度转换成弧度。经纬度转高斯坐标公式如下：

X＝C₀L-(C₁sinL+C₂sin³L)cosL

m＝(λ-λ₀)cosL

C＝6399698.9018,C₀＝6367558.49686,C₁＝32005.79642,C₂＝133.86115,E₂＝0.006738525,Y₀＝500000；

其中，步骤7中所述“利用轨迹相似原理对里程计的航向安装角及标度因数进行标定”，具体实现过程说明如下：

定义初始对准位置的经度为λ₀、纬度为L₀，其高斯坐标为(x₀,y₀)；t时刻采集惯导/激光测速仪航位推算得到的经度为λ₁、纬度为L₁，其高斯坐标为(x₁,y₁)，惯导/里程计航位推算得到的经度为λ₂、纬度为L₂，其高斯坐标为(x₂,y₂)；S₁为t时刻激光测速仪航位推算得到的位置与初始对准位置之间的距离，称作参考轨迹；S₂为t时刻里程计航位推算得到的位置与初始对准位置之间的距离，称作航位轨迹；K₀为里程计标度初始装订值，K_D为里程计真实标度，P_D为里程计输出脉冲数，α_ψ为航向安装角。由轨迹相似原理计算得：

Claims (2)

1.一种基于同步测量与定位计算的里程计标定方法，其特征在于：该方法具体步骤如下：

步骤1、将惯组、激光测速仪和里程计安装到载体上，系统上电启动；

步骤2、装订初始参数，包括初始的经度、纬度、高度、激光测速仪的标度、激光测速仪的安装角、里程计标度初值至导航计算机；

步骤3、采集陀螺和加速度计的输出数据，对采集到的陀螺和加速度计数据进行处理，根据捷联惯导系统误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成系统的粗对准，初步确定载体姿态角；粗对准时间为2分钟，粗对准后利用卡尔曼滤波技术精对准3分钟；

步骤4、导航系统由对准模式切换到航位推算模式，同时分别进行惯导/激光测速仪航位推算与惯导/里程计航位推算，切换完成后载体开始运动，运动过程中保持航位推算模式；

步骤5、分别采集惯导/激光测速仪航位推算输出经纬度值与惯导/里程计航位推算输出经纬度值；

步骤6、将采集的经纬度值转换成高斯坐标系下坐标值；

步骤7、将惯导/里程计航位推算得到的位置值与惯导/激光测速仪航位推算得到的位置值进行比较，利用轨迹相似原理对里程计的航向安装角及标度因数进行标定；

步骤6中所述“将采集的经纬度值转换成高斯坐标系下坐标值”，具体实现过程说明如下：

定义L、λ分别为纬度和经度，λ₀为中央子午线经度，x、y为经纬度转化后的高斯坐标值，n为带区编号，INT(*)为取整运算；

n＝INT(λ/6)+1

λ₀＝6n-3

以上计算经纬度单位为度，将经纬度转高斯坐标运算时需要进行单位转换，将度转换成弧度，经纬度转高斯坐标公式如下：

X＝C₀L-(C₁sinL+C₂sin³L)cosL

$N=\frac{C}{\sqrt{1+E_2{\cos }^{2}L}}$

m＝(λ-λ₀)cosL

$x=X+N\tan L\[\frac{1}{2}{m}^2+\frac{1}{24}{m}^4(5-{\tan }^{2}L+9E_2\cos L)\]$

$y=N\[m+\frac{1}{6}{m}^3(1-{\tan }^{2}L+E_2\cos L)\]+Y_0$

C＝6399698.9018，C₀＝6367558.49686，C₁＝32005.79642，C₂＝133.86115，E₂＝0.006738525，Y₀＝500000。

2.根据权利要求1所述的一种基于同步测量与定位计算的里程计标定方法，其特征在于：步骤7中所述“利用轨迹相似原理对里程计的航向安装角及标度因数进行标定”，具体实现过程说明如下：

定义初始对准位置的经度为λ₀、纬度为L₀，其高斯坐标为(x₀,y₀)；t时刻采集惯导/激光测速仪航位推算得到的经度为λ₁、纬度为L₁，其高斯坐标为(x₁,y₁)，惯导/里程计航位推算得到的经度为λ₂、纬度为L₂，其高斯坐标为(x₂,y₂)；S₁为t时刻激光测速仪航位推算得到的位置与初始对准位置之间的距离，称作参考轨迹；S₂为t时刻里程计航位推算得到的位置与初始对准位置之间的距离，称作航位轨迹；K₀为里程计标度初始装订值，K_D为里程计真实标度，P_D为里程计输出脉冲数，α_ψ为航向安装角，由轨迹相似原理计算得：

$S_1=\sqrt{{(x_1-x_0)}^2+{(y_1-y_0)}^2}=K_D{P}_D$

$S_2=\sqrt{{(x_2-x_0)}^2+{(y_2-y_0)}^2}=K_0{P}_D$

$K_D=\frac{S_1}{S_2}{K}_0=\frac{\sqrt{{(x_1-x_0)}^2+{(y_1-y_0)}^2}}{\sqrt{{(x_2-x_0)}^2+{(y_2-y_0)}^2}}{K}_0$

${\mathrm{\α}}_{\mathrm{\ψ}}=\arctan \left(\frac{y_1-y_0}{x_1-x_0}\right)-\arctan \left(\frac{y_2-y_0}{x_2-x_0}\right).$