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

权利要求书

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为里程计标度因数误差，δα为里程计航向安装角误差，δγ为里程计俯仰安装角误差，一体化误差模型如下：

M3×6=00-VN(RM+h)201RM+h0VEtanLsecLRN+h0-VEsecL(RN+h)2secLRN+h00000001]]>

N3×6=00VN(RM+h)20-1RM+h0-ωiesinL0-VE(RN+h)21RN+h00ωiecosL+VERN+hsec2L0-VEtanL(RN+h)2tanLRN+h00]]>

R3×3=0(ωiesinL+VERN+htanL)-(ωiecosL+VERN+h)-(ωiesinL+VERN+htanL)0-VNRM+hωiecosL+VERN+hVNRM+h0]]>

Cbn=Cbn(1,1)Cbn(1,2)Cbn(1,3)Cbn(2,1)Cbn(2,2)Cbn(2,3)Cbn(3,1)Cbn(3,2)Cbn(3,3)]]>

A3×3=2ωie(VUsinL+VNcosL)+VEVNRN+h0VEVU-VEVNtanL(RN+h)2-(2VEωiecosL+VE2RN+hsec2L)0VNVU(RM+h)2+VE2tanL(RN+h)2-2VEωiesinL0-(VN2(RM+h)2+VE2(RN+h)2)]]>

B3×3=VNtanL-VURN+h2ωiesinL+VERN+htanL-(2ωiecosL+VERN+h)-2(ωiesinL+VERN+htanL)-VURM+h-VNRM+h2(ωiecosL+VERN+h)2VNRM+h0]]>

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

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

I3×3=100010001]]>

D3×1=-(v^l-Bl)(Cbn(1,1)cosαb+Cbn(1,2)cosβb+Vbn(1,3)cosγb)-(v^l-Bl)(Cbn(2,1)cosαb+Cbn(2,2)cosβb+Cbn(2,3)cosγb)-(v^l-Bl)(Cbn(3,1)cosαb+Cbn(3,2)cosβb+Cbn(3,3)cosγb)]]>

E3×1=KA(Cbn(1,1)cosαb+Cbn(1,2)cosβb+Cbn(1,3)cosγb)KA(Cbn(2,1)cosαb+Cbn(2,2)cosβb+Cbn(2,3)cosγb)KA(Cbn(3,1)cosαb+Cbn(3,2)cosβb+Cbn(3,3)cosγb)]]>

P3×3=-Cbn(1,1)VD-Cbn(1,2)VD-Cbn(1,3)VD-Vbn(2,1)VD-Cbn(2,2)VD-Cbn(2,3)VD-Cbn(3,1)VD-Cbn(3,2)VD-Cbn(3,3)VD]]>

Q3×3=-Cbn(1,1)vlym-Cbn(1,2)vlym-Cbn(1,3)vlym-Vbn(2,1)vlym-Cbn(2,2)vlym-Cbn(2,3)vlym-Cbn(3,1)vlym-Cbn(3,2)vlym-Cbn(3,3)vlym]]>

Δ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＝γ₀-δγ