CN104165641A - 一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法 - Google Patents

Abstract

一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，它有五大步骤：一、将惯组、激光测速仪和里程计安装到载体上，系统上电启动；二、装订初始参数至导航计算机；三、采集和处理陀螺和加速度计的输出数据；并完成系统的粗对准和精对准；四、导航系统由对准模式切换到惯导/激光测速仪/里程计组合导航模式，切换完成后载体开始运动，运动过程中保持组合导航模式；五、在组合导航模式下利用卡尔曼滤波器融合捷联惯导、激光测速仪输出信息与里程计输出信息，完成里程计安装角误差和标度因数误差的估计与补偿。

Description

一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法

技术领域：

本发明涉及一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，属于惯性技术领域。 

背景技术：

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

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

发明内容：

1、发明目的： 

本发明的目的是提供了一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，它克服了现有技术的不足，解决了里程计装到载体上时需要标定安装角和标度因数的问题。 

2、技术方案： 

本发明一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，该方法具体步骤如下： 

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

步骤2、装订初始参数(包括初始的经度、纬度、高度、激光测速仪的标度和零偏、激光测速仪的安装角、里程计标度初值K_D0、里程计航向安装角初值α₀，里程计俯仰安装角初值γ₀)至导航计算机。 

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

步骤4、导航系统由对准模式切换到惯导/激光测速仪/里程计组合导航模式，切换完成后载体开始运动，运动过程中保持组合导航模式。 

步骤5、在组合导航模式下利用卡尔曼滤波器融合捷联惯导、激光测速仪输出信息与里程计输出信息，完成里程计安装角误差和标度因数误差的估计与补偿。 

其中，步骤5中所述“在组合导航模式下利用卡尔曼滤波器融合捷联惯导、激光测速仪输出信息与里程计输出信息，完成里程计安装角误差和标度因数误差的估计与补偿”的具体实现过程说明如下： 

1、建立组合系统误差模型 

建立包含惯导位置误差、速度误差、姿态角误差、陀螺输出误差、加速度计输出误差、激光测速仪标度误差、激光测速仪速度零偏误差、激光测速仪安装角余弦值误差、里程计标度因数误差、里程计安装角误差23维的一体化误差模型。 

定义L为纬度，λ为经度，h为高度，R_M、R_N分别表示子午圈曲率半径和卯酉圈曲率半径，V_E、V_N、V_U分别为导航系下东向、北向、天向速度，ω_ie为地球自转角速率，f_E、f_N、f_U分别为导航系下东向、北向、天向比力，φ_E、φ_N、φ_U分别为东向、北向、天向的姿态误差角，分别载体系下x、y、z轴陀螺零偏，分别载体系下x、y、z轴加速度计零偏，为姿态矩阵，为姿态矩阵的第i行第j列元素，δL为纬度误差，δλ为经度误差，δh为高度误差，δV_E、δV_N、δV_U分别东向、北向、天向速度误差，δK_A激光测速仪标度误差，δB_l激光测速仪零偏误差，δ(cosα_b)、δ(cosβ_b)、δ(cosγ_b)为激光测速仪安装角余弦值误差，δK_D为里程计标度因数误差，δα为里程计航向安装角误差，δ_γ为里程计俯仰安装角误差。一体化误差模型如下： 

$M_{3\×6}=\left[\begin{array}{cccccc}0& 0& -\frac{V_N}{{(R_M+h)}^2}& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L\sec L}{R_N+h}& 0& -\frac{V_E\sec L}{{(R_N+h)}^2}& \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$N_{3\×6}=\left[\begin{array}{cccccc}0& 0& \frac{V_N}{{(R_M+h)}^2}& 0& -\frac{1}{R_M+h}& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -\frac{V_E}{{(R_N+h)}^2}& \frac{1}{R_N+h}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L& 0& -\frac{V_E\tan L}{{(R_N+h)}^2}& \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]$

$R_{3\×3}=\left[\begin{array}{ccc}0& ({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& -\frac{V_N}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\end{array}\right]$

$C_b^n=\left[\begin{array}{ccc}{C}_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)& C_b^n\left(\mathrm{1,3}\right)\\ C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& C_b^n\left(\mathrm{2,3}\right)\\ C_b^n\left(\mathrm{3,1}\right)& C_b^n\left(\mathrm{3,2}\right)& C_b^n\left(\mathrm{3,3}\right)\end{array}\right]$

$A_{3\×3}=\left[\begin{array}{ccc}{2\mathrm{\ω}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{R_N+h}& 0& \frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}\\ -({2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E^2}{R_N+h}{\sec }^{2}L)& 0& \frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}\\ {-2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -(\frac{V_N^2}{{(R_M+h)}^2}+\frac{V_E^2}{{(R_N+h)}^2})\end{array}\right]$

$B_{3\×3}=\left[\begin{array}{ccc}\frac{V_N\tan L-V_U}{R_N+h}& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& -\frac{V_U}{R_M+h}& -\frac{V_N}{R_M+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})& \frac{{2V}_N}{R_M+h}& 0\end{array}\right]$

2、建立组合系统量测模型 

将惯性导航的输出速度与转换到导航坐标系下的激光测速仪速度输出和里程计速度输出作比较作为观测量，建立6维的一体化量测量，量测模型可以表示为： 

$I_{3\×3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

$D_{3\×1}=\left[\begin{array}{c}-({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{1,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{1,2}\right)\cos {\mathrm{\β}}_b+V_b^n\left(\mathrm{1,3}\right)\cos {\mathrm{\γ}}_b)\\ -({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{2,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{2,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{2,3}\right)\cos {\mathrm{\γ}}_b)\\ -({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{3,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{3,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{3,3}\right)\cos {\mathrm{\γ}}_b)\end{array}\right]$

$E_{3\×1}=\left[\begin{array}{c}{K}_A(C_b^n\left(\mathrm{1,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{1,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{1,3}\right)\cos {\mathrm{\γ}}_b)\\ K_A(C_b^n\left(\mathrm{2,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{2,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{2,3}\right)\cos {\mathrm{\γ}}_b)\\ K_A(C_b^n\left(\mathrm{3,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{3,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{3,3}\right)\cos {\mathrm{\γ}}_b)\end{array}\right]$

$P_{3\×3}=\left[\begin{array}{ccc}{-C}_b^n\left(\mathrm{1,1}\right)V_D& {-C}_b^n\left(\mathrm{1,2}\right)V_D& {-C}_b^n\left(\mathrm{1,3}\right)V_D\\ {-V}_b^n\left(\mathrm{2,1}\right)V_D& {-C}_b^n\left(\mathrm{2,2}\right)V_D& {-C}_b^n\left(\mathrm{2,3}\right)V_D\\ {-C}_b^n\left(\mathrm{3,1}\right)V_D& {-C}_b^n\left(\mathrm{3,2}\right)V_D& {-C}_b^n\left(\mathrm{3,3}\right)V_D\end{array}\right]$

$Q_{3\×3}=\left[\begin{array}{ccc}{-C}_b^n\left(\mathrm{1,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{1,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{1,3}\right)v_{\mathrm{ly}}^m\\ {-V}_b^n\left(\mathrm{2,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{2,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{2,3}\right)v_{\mathrm{ly}}^m\\ {-C}_b^n\left(\mathrm{3,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{3,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{3,3}\right)v_{\mathrm{ly}}^m\end{array}\right]$

ΔV_INS/LDV表示惯导与激光测速仪在导航系下的速度之差，ΔV_INS/OD表示惯导与里程计在导 航系下的速度之差，α_b、β_b、γ_b为激光测速仪的安装角，K_A为激光测速仪标度，B_l为激光测速仪速度零偏，为激光测速仪的速度输出，为激光测速仪在载体系下的速度输出，V_D为里程计速度输出。 

3、基于卡尔曼滤波的误差估计与补偿 

基于上述建立的组合系统误差模型与量测模型，建立误差状态方程与量测方程，并基于此采用卡尔曼滤波器进行最优估计计算，得到里程计的标度误差δK_D、航向安装角误差δα、俯仰安装角误差δγ。 

根据卡尔曼滤波估计得到的标度误差δK_D、航向安装角误差δα、俯仰安装角误差δα，完成误差补偿，得到里程计标度K_Dt、航向安装角α_t、俯仰安装角γ_t准确值如下： 

K_Dt＝K_D0-δK_D

α_t＝α₀-δα 

γ_t＝γ₀-δγ 

3、优点及功效：本发明一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，该方法的优点是：相对目前公开的标定方法，该标定方法不需要外部参考信息，无需停车标定，可进行实时标定。 

附图说明

图1为里程计标定框图 

图2为里程计标定流程图 

图中符号说明如下： 

δK_D：里程计标度误差 

δα：里程计航向安装角误差 

δ_γ：里程计俯仰安装角误差 

K_Dt：里程计标度 

α_t：里程计航向安装角 

γ_t：里程计俯仰安装角 

具体实施方式：

见图1—图2，本发明一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，该方法具体步骤如下： 

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

步骤2、装订初始参数(包括初始的经度、纬度、高度、激光测速仪的标度和零偏、激光测速仪的安装角、里程计标度初值K_D0、里程计航向安装角初值α₀，里程计俯仰安装角初值γ₀)至导航计算机。 

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

步骤4、导航系统由对准模式切换到惯导/激光测速仪/里程计组合导航模式，切换完成后载体开始运动，运动过程中保持组合导航模式。 

步骤5、在组合导航模式下利用卡尔曼滤波器融合捷联惯导、激光测速仪输出信息与里程计输出信息，完成里程计安装角误差和标度因数误差的估计与补偿。 

其中，步骤5中在组合导航模式下利用卡尔曼滤波器标定里程计的具体实现过程说明如下： 

1、建立组合系统误差模型 

建立包含惯导位置误差、速度误差、姿态角误差、陀螺输出误差、加速度计输出误差、激光测速仪标度误差、激光测速仪速度零偏误差、激光测速仪安装角余弦值误差、里程计标度因数误差、里程计安装角误差23维的一体化误差模型。 

定义L为纬度，λ为经度，h为高度，R_M、R_N分别表示子午圈曲率半径和卯酉圈曲率半径，V_E、V_N、V_U分别为导航系下东向、北向、天向速度，ω_ie为地球自转角速率，f_E、f_N、f_U分别为导航系下东向、北向、天向比力，φ_E、φ_N、φ_U分别为东向、北向、天向的姿态误差角，分别载体系下x、y、z轴陀螺零偏，分别载体系下x、y、z轴加速度计零偏，为姿态矩阵，为姿态矩阵的第i行第j列元素，δL为纬度误差，δλ为经度误差，δh为高度误差，δV_E、δV_N、δV_U分别东向、北向、天向速度误差，δK_A激光测速仪标度误差，δB_l激光测速仪零偏误差，δ(cosα_b)、δ(cosβ_b)、δ(cosγ_b)为激光测速仪安装角余弦值误差，δK_D为里程计标度因数误差，δα为里程计航向安装角误差，δγ为 里程计俯仰安装角误差。一体化误差模型如下： 

$M_{3\×6}=\left[\begin{array}{cccccc}0& 0& -\frac{V_N}{{(R_M+h)}^2}& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L\sec L}{R_N+h}& 0& -\frac{V_E\sec L}{{(R_N+h)}^2}& \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$N_{3\×6}=\left[\begin{array}{cccccc}0& 0& \frac{V_N}{{(R_M+h)}^2}& 0& -\frac{1}{R_M+h}& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -\frac{V_E}{{(R_N+h)}^2}& \frac{1}{R_N+h}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L& 0& -\frac{V_E\tan L}{{(R_N+h)}^2}& \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]$

$R_{3\×3}=\left[\begin{array}{ccc}0& ({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& -\frac{V_N}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\end{array}\right]$

$C_b^n=\left[\begin{array}{ccc}{C}_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)& C_b^n\left(\mathrm{1,3}\right)\\ C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& C_b^n\left(\mathrm{2,3}\right)\\ C_b^n\left(\mathrm{3,1}\right)& C_b^n\left(\mathrm{3,2}\right)& C_b^n\left(\mathrm{3,3}\right)\end{array}\right]$

$A_{3\×3}=\left[\begin{array}{ccc}{2\mathrm{\ω}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{R_N+h}& 0& \frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}\\ -({2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E^2}{R_N+h}{\sec }^{2}L)& 0& \frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}\\ {-2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -(\frac{V_N^2}{{(R_M+h)}^2}+\frac{V_E^2}{{(R_N+h)}^2})\end{array}\right]$

$B_{3\×3}=\left[\begin{array}{ccc}\frac{V_N\tan L-V_U}{R_N+h}& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& -\frac{V_U}{R_M+h}& -\frac{V_N}{R_M+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})& \frac{{2V}_N}{R_M+h}& 0\end{array}\right]$

2、建立组合系统量测模型 

将惯性导航的输出速度与转换到导航坐标系下的激光测速仪速度输出和里程计速度输出作比较作为观测量，建立6维的一体化量测量，量测模型可以表示为： 

$I_{3\×3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

$D_{3\×1}=\left[\begin{array}{c}-({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{1,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{1,2}\right)\cos {\mathrm{\β}}_b+V_b^n\left(\mathrm{1,3}\right)\cos {\mathrm{\γ}}_b)\\ -({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{2,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{2,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{2,3}\right)\cos {\mathrm{\γ}}_b)\\ -({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{3,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{3,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{3,3}\right)\cos {\mathrm{\γ}}_b)\end{array}\right]$

$E_{3\×1}=\left[\begin{array}{c}{K}_A(C_b^n\left(\mathrm{1,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{1,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{1,3}\right)\cos {\mathrm{\γ}}_b)\\ K_A(C_b^n\left(\mathrm{2,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{2,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{2,3}\right)\cos {\mathrm{\γ}}_b)\\ K_A(C_b^n\left(\mathrm{3,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{3,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{3,3}\right)\cos {\mathrm{\γ}}_b)\end{array}\right]$

$P_{3\×3}=\left[\begin{array}{ccc}{-C}_b^n\left(\mathrm{1,1}\right)V_D& {-C}_b^n\left(\mathrm{1,2}\right)V_D& {-C}_b^n\left(\mathrm{1,3}\right)V_D\\ {-V}_b^n\left(\mathrm{2,1}\right)V_D& {-C}_b^n\left(\mathrm{2,2}\right)V_D& {-C}_b^n\left(\mathrm{2,3}\right)V_D\\ {-C}_b^n\left(\mathrm{3,1}\right)V_D& {-C}_b^n\left(\mathrm{3,2}\right)V_D& {-C}_b^n\left(\mathrm{3,3}\right)V_D\end{array}\right]$

$Q_{3\×3}=\left[\begin{array}{ccc}{-C}_b^n\left(\mathrm{1,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{1,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{1,3}\right)v_{\mathrm{ly}}^m\\ {-V}_b^n\left(\mathrm{2,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{2,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{2,3}\right)v_{\mathrm{ly}}^m\\ {-C}_b^n\left(\mathrm{3,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{3,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{3,3}\right)v_{\mathrm{ly}}^m\end{array}\right]$

ΔV_INS/LDV表示惯导与激光测速仪在导航系下的速度之差，ΔV_INS/OD表示惯导与里程计在导 航系下的速度之差，α_b、β_b、γ_b为激光测速仪的安装角，K_A为激光测速仪标度，B_l为激光测速仪速度零偏，为激光测速仪的速度输出，为激光测速仪在载体系下的速度输出，V_D为里程计速度输出。 

3、基于卡尔曼滤波的误差估计与补偿 

基于上述建立的组合系统误差模型与量测模型，建立误差状态方程与量测方程，并基于此采用卡尔曼滤波器进行最优估计计算，得到里程计的标度误差δK_D、航向安装角误差δα、俯仰安装角误差δγ。 

根据卡尔曼滤波估计得到的标度误差δK_D、航向安装角误差δα、俯仰安装角误差δα，完成误差补偿，得到里程计标度K_Dt、航向安装角α_t、俯仰安装角γ_t准确值如下： 

K_Dt＝K_0D-δK_D

α_t＝α₀-δα 

γ_t＝γ₀-δγ。 

Claims (2)

1.一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，其特征在于：该方法具体步骤如下：

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

步骤2、装订初始参数，包括初始的经度、纬度、高度、激光测速仪的标度和零偏、激光测速仪的安装角、里程计标度初值K_D0、里程计航向安装角初值α₀，里程计俯仰安装角初值γ₀至导航计算机；

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

步骤4、导航系统由对准模式切换到惯导/激光测速仪/里程计组合导航模式，切换完成后载体开始运动，运动过程中保持组合导航模式；

步骤5、在组合导航模式下利用卡尔曼滤波器融合捷联惯导、激光测速仪输出信息与里程计输出信息，完成里程计安装角误差和标度因数误差的估计与补偿。

2.根据权利要求1所述的一种基于捷联惯导/激光测速仪组合导航系统的里程计标定方法，其特征在于：步骤5中所述“在组合导航模式下利用卡尔曼滤波器融合捷联惯导、激光测速仪输出信息与里程计输出信息，完成里程计安装角误差和标度因数误差的估计与补偿”的具体实现过程说明如下：

(1)建立组合系统误差模型

建立包含惯导位置误差、速度误差、姿态角误差、陀螺输出误差、加速度计输出误差、激光测速仪标度误差、激光测速仪速度零偏误差、激光测速仪安装角余弦值误差、里程计标度因数误差、里程计安装角误差23维的一体化误差模型；

定义L为纬度，λ为经度，h为高度，R_M、R_N分别表示子午圈曲率半径和卯酉圈曲率半径，V_E、V_N、V_U分别为导航系下东向、北向、天向速度，ω_ie为地球自转角速率，f_E、f_N、f_U分别为导航系下东向、北向、天向比力，φ_E、φ_N、φ_U分别为东向、北向、天向的姿态误差角，分别载体系下x、y、z轴陀螺零偏，分别载体系下x、y、z轴加速度计零偏，为姿态矩阵，为姿态矩阵的第i行第j列元素，δL为纬度误差，δλ为经度误差，δh为高度误差，δV_E、δV_N、δV_U分别东向、北向、天向速度误差，δK_A激光测速仪标度误差，δB_l激光测速仪零偏误差，δ(cosα_b)、δ(cosβ_b)、δ(cosγ_b)为激光测速仪安装角余弦值误差，δK_D为里程计标度因数误差，δα为里程计航向安装角误差，δγ为里程计俯仰安装角误差，一体化误差模型如下：

$M_{3\×6}=\left[\begin{array}{cccccc}0& 0& -\frac{V_N}{{(R_M+h)}^2}& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L\sec L}{R_N+h}& 0& -\frac{V_E\sec L}{{(R_N+h)}^2}& \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$N_{3\×6}=\left[\begin{array}{cccccc}0& 0& \frac{V_N}{{(R_M+h)}^2}& 0& -\frac{1}{R_M+h}& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -\frac{V_E}{{(R_N+h)}^2}& \frac{1}{R_N+h}& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L& 0& -\frac{V_E\tan L}{{(R_N+h)}^2}& \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]$

$R_{3\×3}=\left[\begin{array}{ccc}0& ({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& 0& -\frac{V_N}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}& \frac{V_N}{R_M+h}& 0\end{array}\right]$

$C_b^n=\left[\begin{array}{ccc}{C}_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)& C_b^n\left(\mathrm{1,3}\right)\\ C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& C_b^n\left(\mathrm{2,3}\right)\\ C_b^n\left(\mathrm{3,1}\right)& C_b^n\left(\mathrm{3,2}\right)& C_b^n\left(\mathrm{3,3}\right)\end{array}\right]$

$A_{3\×3}=\left[\begin{array}{ccc}{2\mathrm{\ω}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{R_N+h}& 0& \frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}\\ -({2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E^2}{R_N+h}{\sec }^{2}L)& 0& \frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}\\ {-2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -(\frac{V_N^2}{{(R_M+h)}^2}+\frac{V_E^2}{{(R_N+h)}^2})\end{array}\right]$

$B_{3\×3}=\left[\begin{array}{ccc}\frac{V_N\tan L-V_U}{R_N+h}& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L& -({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)& -\frac{V_U}{R_M+h}& -\frac{V_N}{R_M+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})& \frac{{2V}_N}{R_M+h}& 0\end{array}\right]$

(2)建立组合系统量测模型

将惯性导航的输出速度与转换到导航坐标系下的激光测速仪速度输出和里程计速度输出作比较作为观测量，建立6维的一体化量测量，量测模型表示为：

$I_{3\×3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

$D_{3\×1}=\left[\begin{array}{c}-({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{1,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{1,2}\right)\cos {\mathrm{\β}}_b+V_b^n\left(\mathrm{1,3}\right)\cos {\mathrm{\γ}}_b)\\ -({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{2,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{2,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{2,3}\right)\cos {\mathrm{\γ}}_b)\\ -({\hat{v}}_l-B_l)(C_b^n\left(\mathrm{3,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{3,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{3,3}\right)\cos {\mathrm{\γ}}_b)\end{array}\right]$

$E_{3\×1}=\left[\begin{array}{c}{K}_A(C_b^n\left(\mathrm{1,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{1,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{1,3}\right)\cos {\mathrm{\γ}}_b)\\ K_A(C_b^n\left(\mathrm{2,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{2,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{2,3}\right)\cos {\mathrm{\γ}}_b)\\ K_A(C_b^n\left(\mathrm{3,1}\right)\cos {\mathrm{\α}}_b+C_b^n\left(\mathrm{3,2}\right)\cos {\mathrm{\β}}_b+C_b^n\left(\mathrm{3,3}\right)\cos {\mathrm{\γ}}_b)\end{array}\right]$

$P_{3\×3}=\left[\begin{array}{ccc}{-C}_b^n\left(\mathrm{1,1}\right)V_D& {-C}_b^n\left(\mathrm{1,2}\right)V_D& {-C}_b^n\left(\mathrm{1,3}\right)V_D\\ {-V}_b^n\left(\mathrm{2,1}\right)V_D& {-C}_b^n\left(\mathrm{2,2}\right)V_D& {-C}_b^n\left(\mathrm{2,3}\right)V_D\\ {-C}_b^n\left(\mathrm{3,1}\right)V_D& {-C}_b^n\left(\mathrm{3,2}\right)V_D& {-C}_b^n\left(\mathrm{3,3}\right)V_D\end{array}\right]$

$Q_{3\×3}=\left[\begin{array}{ccc}{-C}_b^n\left(\mathrm{1,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{1,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{1,3}\right)v_{\mathrm{ly}}^m\\ {-V}_b^n\left(\mathrm{2,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{2,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{2,3}\right)v_{\mathrm{ly}}^m\\ {-C}_b^n\left(\mathrm{3,1}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{3,2}\right)v_{\mathrm{ly}}^m& {-C}_b^n\left(\mathrm{3,3}\right)v_{\mathrm{ly}}^m\end{array}\right]$

ΔV_INS/LDV表示惯导与激光测速仪在导航系下的速度之差，ΔV_INS/OD表示惯导与里程计在导航系下的速度之差，α_b、β_b、γ_b为激光测速仪的安装角，K_A为激光测速仪标度，B_l为激光测速仪速度零偏，为激光测速仪的速度输出，为激光测速仪在载体系下的速度输出，V_D为里程计速度输出；

(3)基于卡尔曼滤波的误差估计与补偿

基于上述建立的组合系统误差模型与量测模型，建立误差状态方程与量测方程，并基于此采用卡尔曼滤波器进行最优估计计算，得到里程计的标度误差δK_D、航向安装角误差δα、俯仰安装角误差δγ；

根据卡尔曼滤波估计得到的标度误差δK_D、航向安装角误差δα、俯仰安装角误差δα，完成误差补偿，得到里程计标度K_Dt、航向安装角α_t、俯仰安装角γ_t准确值如下：

K_Dt＝K_D0-δK_D

α_t＝α₀-δα             。

γ_t＝γ₀-δγ