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

Abstract

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

Description

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

技术领域

本发明属于组合导航设计技术领域，具体涉及到一种基于近距离地标测距的组合导航方法。

背景技术

组合导航系统能有效地利用各种传感器信息，提高导航精度，降低了对惯性传感器的要求，可以有效降低成本。而且，组合导航系统具有较好的容错性和可靠性，因此应用较为广泛。

目前广泛使用的组合导航方式为惯性/卫星组合和惯性/里程计组合，由于卫星导航系统容易受到欺骗干扰，从而无法满足系统对导航参数的高可靠、高精度的测量要求。而里程计辅助手段存在受打滑、轮胎气压变化等因素影响的问题。此外，里程计仍是一种相对位置测量工具，存在无法消除的累积误差。因此有必要寻求一种新的完全自主的、不受任何外部因素影响的组合导航系统。

发明内容

针对现有技术的缺陷和技术需求，本发明提供了一种基于近距离地标测距的组合导航方法，在载体通过具有已知位置信息的地标点时，利用测距设备测量载体与地标点之间的距离，并建立相应误差模型，以修正惯性导航的误差，实现组合导航。

为实现上述目的，本发明提供一种基于近距离地标测距的组合导航方法，所述方法包括步骤：

S1、将捷联惯导系统和两个测距装置安装在载体上，采集捷联惯导系统惯性测量单元正常工作状态下输出的载体运动信息并进行惯导解算；

S2、载体行驶至预设地标区域内，采集两个测距装置到地标点的距离以及地标点的位置信息；

S3、建立状态方程其中，F为状态转移矩阵，w为系统噪声；状态向量所述x_INSSys为惯性导航系统参数误差状态向量，所述 x_INSSens为加速度计和陀螺参数误差状态向量；

S4、建立量测方程z＝Hx+v，z_Obs1对应的量测矩阵z_Obs2对应的量测矩阵H₂ ＝ [0_{1 × 2} 1 0_{1 × 34} ] ；所述为惯导解算的姿态矩阵，所述分别为两个测距装置在体坐标系下的投影，R为地球半径；观测量为惯导计算的两个测距装置到地标点距离的平方差，所述为所述两个测距装置到地标点测量距离的平方差；观测量所述为惯导计算的载体高度，所述为对应地标点的实际高度；v为量测噪声；

S5、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。

作为进一步优选地，所述步骤S3中，状态向量惯性导航系统参数误差状态向量其中 分别为导航系下载体位置误差角在X、Y、Z轴三个方向的分量，分别为导航系下载体速度误差在X、Y、Z轴三个方向的分量，分别为导航系下载体姿态误差角在X、Y、Z轴三个方向的分量，δh为载体高度误差；x_INSSens为加速度计和陀螺参数误差状态向量，依次包含加速度计零偏误差、加速度计标度因数误差、加速度计失准角误差、加速度计二次项系数误差和陀螺零偏误差、陀螺标度因数误差、陀螺失准角误差。

作为进一步优选地，所述步骤S3中，

其中，为导航系下位移角速度在X、Y、Z三方向的分量， 为导航系下地球自转角速度在Y、Z方向分量，R_M为子午圈曲率半径， R_N为卯酉圈曲率半径，L为纬度，h为高度，为导航系下的速度在X、Y、Z三方向的分量，分别为三个加速度计测得的比力在导航系下的值；

i'_x、i'_y、i'_z分别为惯组坐标系下三个加速度计输出的比力；

i″_x、i″_y、i″_z分别为惯组坐标系下三个陀螺仪输出的角速度。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：考虑到地标点离用户的距离同用户位置的计算误差量级相当，在进行组合导航时不能忽略，本发明利用两个测距仪测量距离的平方差作为量测信息，建立相应的数学模型，可获得高精度的定位定向结果。本发明方法通过利用测距设备测量载体与地标点之间的距离，相比于现有的SINS/GPS等组合方式，避免了信号干扰，具备较强的隐蔽性和自主性；且通过利用具有绝对位置信息的地标点，相比于相对位置测量的里程计辅助手段，不存在误差累积的问题。

附图说明

图1为本发明方法组合导航方法流程示意图；

图2为基于本发明方法的组合导航系统各设备布置示意图；

图3为基于本发明方法的组合导航系统各设备矢量关系示意图；

图4为本发明方法组合导航方法原理示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明提供一种基于近距离地标测距组合导航方法，方法包括以下步骤：

S1、将捷联惯导系统和两个测距装置安装在载体上，载体通常为车辆。所述捷联惯导系统位于车辆内部中间位置，两个测距装置安装在车辆靠近地标一侧，采集所述捷联惯导系统惯性测量单元与两个测距仪之间的位置关系

S2、采集捷联惯导系统惯性测量单元正常工作状态下输出的载体运动信息并进行导航解算；

S3、载体行驶至预设地标区域内，采集两个测距装置到地标点的距离以及地标点的位置信息；

S4、建立导航系统的状态方程和量测方程；

S5、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，实现组合导航。

上述步骤S1中，测距装置通常为测距仪。捷联惯导系统惯性测量装置 (IMU)与测距仪之间的位置关系为理想位置关系l^B加测距仪测量误差。

作为一种优选的方式，将捷联惯导系统惯性测量单元安装在车辆内部靠中间的位置，以该位置为圆心建立体坐标系B系，定义X轴沿载体纵轴方向，指向车载头部，Z轴在车载纵对称面内，垂直X轴指向下，Y轴沿车载横向垂直X轴且指向右，如图2所示。在车载的前方靠右边安装一个测距设备，即测距仪1，并测量测距仪1在体坐标系B系中的位置即测距仪1在体坐标系B系下的投影；在车载的后方靠右边安装另一个测距设备，即测距仪2，并测量测距仪2在体系坐标系B系中的位置由此得到捷联惯导系统惯性测量单元与两个测距装置之间的位置关系

上述步骤S2中，对捷联惯导系统预热后采集其惯性测量单元正常工作状态下输出的载体运动信息，所述载体运动信息包括载体的加速度和角速度。

上述步骤S3中，预设地标区域是指每隔一定距离(如30公里)建立一个地标区，该地标区内每行驶一定距离(如2公里)后在车载行驶道路的右侧有一个包含高精度位置信息的地标点，上述位置信息包括地标的经度、纬度和高度信息。

上述步骤S4中，建立组合导航系统的状态方程和量测方程，具体实现方式如下：

组合导航系统的状态方程为：

式中，作为本发明的一种实施方式，状态向量x为37维向量，包含10 维惯性导航系统参数误差状态向量x_INSSys以及27维器件参数误差状态向量 x_INSSens。w为组合导航系统状态噪声。

其中x_INSSys具体表示为：

分别表示导航坐标系下载体位置误差角在X、Y、Z轴三个方向的分量，分别为导航系下载体速度误差在 X、Y、Z轴三个方向的分量，分别为导航系下载体姿态误差角在X、Y、Z轴三个方向的分量，δh为载体高度误差。

x_INSSens依次包含3个加速度计零偏误差B'、3个加速度计标度因数误差δ'_SF、6个加速度计失准角误差MA'、3个加速度计二次项系数误差SO和3 个陀螺零偏误差B”、3个陀螺标度因数误差δ″_SF、6个陀螺失准角误差MA”。

F为状态转移矩阵，其中F₁₁为10×10维的捷联惯性导航系统状态误差方程线性化表示后的系数矩阵，具体表达式为：

其中，为导航系下位移角速度在X、Y、Z三方向的分量，为导航系下地球自转角速度在Y、Z方向分量，R_M为子午圈曲率半径，R_N为卯酉圈曲率半径，L为纬度，h为高度，为导航系下的速度在X、Y、Z三方向的分量，分别为三个加速度计测得的比力在导航系下的值；

F′₁₂、F″₁₂分别为惯性器件加速度计和陀螺的误差模型线性化后的系数矩阵。

对于加速度计，有：

其中，x'_INSSens为加速度计的误差。i'_x、i'_y、i'_z分别为惯组坐标系下三个加速度计输出的比力。

din表示dt采样时间间隔内器件测量量增量的真实值(正交的惯组坐标系下的分量，对于加速度计来说即视速度增量，对于陀螺来说即角度增量)。i²表示i各元素的平方值组成的向量。

陀螺的误差模型线性化后的系数矩阵求解方式与加速度计相同，陀螺无二次项系数误差，即i″_x、i″_y、 i″_z分别为惯组坐标系下三个陀螺仪输出的角速度。

组合导航系统量测方程为：

z＝Hx+v

其中，H为量测矩阵，z为2×1的矩阵，即：

z_Obs1对应的量测矩阵H₁为：

其中，表示位置误差角及高度误差到位置矢量误差的转换矩阵，有：

R为地球半径。

为惯导解算的姿态矩阵。分别为捷联惯导系统惯性测量装置与两个测距仪之间的理想位置关系，实际计算中用测量值代替。

z_Obs2仅用来表示高度误差，对应的量测矩阵H₂为：

H₂＝[0_1×2 1 0_1×34]

因此，有H：

v表示量测噪声。

由步骤S1可知，载体上安装有两个测距仪，因此，观测量z_Obs1的计算方法定义为如下形式：

其中，表示惯导计算的两个测距仪到地标点距离的平方差，表示两个测距仪实际测量距离的平方差。

其中，表示惯导计算的高度，表示通过测量地标点给出的高度。

在获得上述导航系统的状态方程和量测方程之后，采用经典的卡尔曼滤波的反馈校正方式，具体执行方式为：

外推计算周期：

更新计算周期：

反馈校正：

(+E)为上标，表示经过离散型卡尔曼滤波基本方程中的状态更新之后，指定参数在指定时刻(此处为t_n时刻)的值；(+C)为上标，表示经过离散型卡尔曼滤波基本方程中的控制式之后，指定参数在指定时刻的值；(-)为上标，表示在状态更新式以及控制式发挥作用以前，指定参数在指定时刻的值。

经过卡尔曼滤波，可获得各个误差状态的估计值，利用估计值修正导航参数，以修正后导航参数作为下一次导航初值继续进行导航解算，直至导航过程结束。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于近距离地标测距的组合导航方法，其特征在于，所述方法包括步骤：

S1、将捷联惯导系统和两个测距装置安装在载体上，采集捷联惯导系统惯性测量单元正常工作状态下输出的载体运动信息并进行惯导解算；

S2、载体行驶至预设地标区域内，采集两个测距装置到地标点的距离以及地标点的位置信息；

S3、建立状态方程其中，F为状态转移矩阵，w为系统噪声；状态向量所述x_INSSys为惯性导航系统参数误差状态向量，所述x_INSSens为加速度计和陀螺参数误差状态向量；

S4、建立量测方程z＝Hx+v，z_Obs1对应的量测矩阵z_Obs2对应的量测矩阵H₂＝[0_1×210_1×34]；所述为惯导解算的姿态矩阵，所述分别为两个测距装置在体坐标系下的投影，R为地球半径；观测量 为惯导计算的两个测距装置到地标点距离的平方差，所述为所述两个测距装置到地标点测量距离的平方差；观测量所述为惯导计算的载体高度，所述为对应地标点的实际高度；v为量测噪声；

S5、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。

2.如权利要求1所述的基于近距离地标测距的组合导航方法，其特征在于，所述步骤S3中，状态向量惯性导航系统参数误差状态向量其中分别为导航系下载体位置误差角在X、Y、Z轴三个方向的分量，分别为导航系下载体速度误差在X、Y、Z轴三个方向的分量，分别为导航系下载体姿态误差角在X、Y、Z轴三个方向的分量，δh为载体高度误差；x_INSSens为加速度计和陀螺参数误差状态向量，依次包含加速度计零偏误差、加速度计标度因数误差、加速度计失准角误差、加速度计二次项系数误差和陀螺零偏误差、陀螺标度因数误差、陀螺失准角误差。

3.如权利要求2所述的基于近距离地标测距的组合导航方法，其特征在于，所述步骤S3中，

其中，为导航系下位移角速度在X、Y、Z三方向的分量， 为导航系下地球自转角速度在Y、Z方向分量，R_M为子午圈曲率半径，R_N为卯酉圈曲率半径，L为纬度，h为高度，为导航系下的速度在X、Y、Z三方向的分量，分别为三个加速度计测得的比力在导航系下的值；

i'_x、i'_y、i'_z分别为惯组坐标系下三个加速度计输出的比力；

i'_x'、i'_y'、i'_z'分别为惯组坐标系下三个陀螺仪输出的角速度。