一种基于近距离地标测距的组合导航方法

摘要

本发明公开了一种基于近距离地标测距的组合导航方法，方法包括以下步骤：S1、将捷联惯导系统和两个测距装置安装在载体上，采集捷联惯导系统惯性测量单元正常工作状态下输出的载体运动信息以进行惯导解算；S2、载体行驶至预设地标区域内，采集两个测距装置到地标点的距离以及地标点的位置信息；S3、建立状态方程；S4、建立量测方程，将两个测距装置测量距离的平方差作为量测信息；S5、利用状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，实现组合导航。实施本发明方法可有效避免信号干扰，具备较强的隐蔽性和自主性，且消除了误差累积的问题。

主权项

一种基于近距离地标测距的组合导航方法，其特征在于，所述方法包括步骤：S1、将捷联惯导系统和两个测距装置安装在载体上，采集捷联惯导系统惯性测量单元正常工作状态下输出的载体运动信息并进行惯导解算；S2、载体行驶至预设地标区域内，采集两个测距装置到地标点的距离以及地标点的位置信息；S3、建立状态方程其中，F为状态转移矩阵，w为系统噪声；状态向量$x=\left[\begin{array}{c}{x}_{\mathrm{INSSys}}\\ x_{\mathrm{INSSens}}\end{array}\right],$所述x_INSSys为惯性导航系统参数误差状态向量，所述x_INSSens为加速度计和陀螺参数误差状态向量；S4、建立量测方程z＝Hx+v，$z=\left[\begin{array}{c}{z}_{\mathrm{Obs}1}\\ z_{\mathrm{Obs}2}\end{array}\right],H=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right],$z_Obs1对应的量测矩阵$H_1=\left[\begin{array}{cc}2{\left(C_B^N(l_1^B-l_2^B)\right)}^T{D}_{\mathrm{\delta \theta h}}^{\mathrm{\delta R}}& 0\end{array}\right],$z_Obs2对应的量测矩阵H₂＝[0 1 0]；所述为惯导解算的姿态矩阵，所述分别为两个测距装置在体坐标系下的投影$D_{\mathrm{\delta \theta h}}^{\mathrm{\delta R}}=\left[\begin{array}{ccc}0& R& 0\\ -R& 0& 0\\ 0& 0& 1\end{array}\right],$R为地球半径；观测量$z_{\mathrm{Obs}1}=\Delta {\hat{\rho }}_{\mathrm{INS}}^2-\Delta {\tilde{\rho }}_{\mathrm{Distmt}}^2,$为惯导计算的两个测距装置到地标点距离的平方差，所述为所述两个测距装置到地标点测量距离的平方差；观测量所述为惯导计算的载体高度，所述为对应地标点的实际高度；v为量测噪声；S5、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。