CN106500727B - 一种基于路径匹配的惯导系统误差修正方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，涉及一种基于路径匹配的惯导系统误差修正方法；首先建立基准路径地图数据库，然后在载体行驶过程中选取惯导系统一定路程内的行驶轨迹数据与惯导系统误差范围内的所有基准路径数据进行比对匹配计算，获得当前定位定向系统所在位置的参考路径，实现载体在基准路径数据库中的定位，完成对惯导系统误差的修正；但这种路径匹配获得的基准位置信息是不连续的，载体行驶一段距离才能获得一个基准点，虽然可以直接用基准位置信息对惯导位置进行置换，但这样不能对惯导的速度、姿态以及惯性器件误差进行修正，进一步是采用卡尔曼滤波的方法，利用这种不连续的、稀疏条件下的基准位置信息实现对惯导系统误差的估计与修正。

Description

一种基于路径匹配的惯导系统误差修正方法

技术领域

本发明属于惯性导航技术领域，涉及一种基于路径匹配的惯导系统误差修正方法。

背景技术

目前定位定向系统，通常采用旋转调制技术和里程计组合技术来提高系统精度，但不论是采取旋转调制技术降低器件误差对系统精度的影响，还是结合里程计进行组合误差修正，系统的定位误差仍然会随着车辆的行驶距离增大以及系统的工作时间加长而逐渐变大。

对于上述问题，常用的解决方法是经过一段时间后，利用卫星信息对系统的位置误差进行修正以消除累计误差。但卫星信息不属于自主导航信息来源，容易受到外界干扰而出现无法使用的情况。

发明内容

针对上述现有技术，本发明的目的在于提供一种基于路径匹配的惯导系统误差修正方法，实现车辆在基准路径数据库中的定位，从而用基准路径中的位置信息完成对惯导系统误差的修正。

为了达到上述目的，本发明采用以下技术方案。

本发明的目的在于提供一种基于路径匹配的惯导系统误差修正方法，包括以下步骤：

步骤一、建立基准路径地图数据库；

步骤二、利用惯导系统的误差大小，从步骤一所建立的基准路径地图数据库中选取路径匹配的基准路径范围；

步骤三、在步骤二中选取的基准路径范围中，利用惯导系统运动路径的变化特点，在所选取的基准路径范围内中寻找与惯导系统路径最为匹配的一段路径。

进一步，步骤一所述的基准路径地图数据库的建立，参照车载导航领域常用的地理信息系统标准数据结构建立，或者针对定位定向系统应用特点建立基于标准定位点的路径数据库，以获取路径的经纬度值；

进一步，当惯导系统在两次匹配间隔内的误差发散大小在δS(δx,δy)以内，则选取基准路径范围为当前惯导系统位置点相距误差δS(δx,δy)内的基准路径地图数据库中所有基准路径。

进一步，首先，在惯导系统载体行驶过程中，每隔一定距离记录惯导系统一位置点如D(x,y)，用所记录的位置坐标D_i(x_i,y_i)与行驶的这段路径坐标均值的差表示这段路径的变化：

式中：x，y分别为惯导系统坐标的经纬度，单位rad；

所记录的每一个位置点相对于行驶的这段路径的变化(dx_i,dy_i)为：

其次，相应的，在基准路径中取相同数量的位置点R_i(p_i,q_i)，位置点之间的间隔与上述惯导系统记录的位置点间隔相同，基准路径中这段路径坐标均值为：

式中：p、q分别为基准路径坐标的经纬度，单位rad；

基准路径中每个位置点相对于此段路径的变化(dp_i,dq_i)为：

最后，用r表示载体行驶路径与基准路径变化规律的相近度：

则当r最小时的基准路径为与载体路径最为匹配的路径。

进一步，在路径匹配过程中，对惯导系统路径的变化幅度进行判断，计算各个位置点到路径首尾连线的距离，当距离之和小于一定的门限时，认为这段路径较直，变化较小，此时路径匹配的错误率可能变大，不进行位置校正。

进一步，所述惯导系统路径的变化幅度判断的方法为：

式中：d_i为第i个点距离路径首尾连线的距离；

当D小于门限T_D时，认为路径较直，此时不进行匹配；

当匹配成功时，以基准路径中的路径端点为此时惯导系统的基准位置信息，惯导系统位置对应的基准路径的位置。

进一步，该方法还包括以下步骤：

采用卡尔曼滤波的方法完成对惯导系统误差的估计与补偿，在惯导系统转导航后，重启卡尔曼滤波器，持续进行时间更新；在路径匹配成功时，开始量测更新，以基准位置为参考信息进行位置匹配，在路径匹配未成功时仍然进行时间更新。

进一步，所述卡尔曼滤波的方法包括以下步骤：

(1)滤波状态量：

采用15维的状态量：

式中：ΔL、Δλ分别为惯导系统的纬度、经度误差，单位：rad；Δh为惯导系统的高度，单位：m；ΔV_n、ΔV_u、ΔV_e分别为惯导系统的北向、天向、东向速度误差，单位m/s；Φ_n、Φ_u、Φ_e分别为惯导系统北向、天向、东向失准角误差，单位：rad；分别为惯导系统x、y、z轴加速度计零偏，单位：m/s²；ε_x、ε_y、ε_z分别为惯导系统x、y、z轴陀螺漂移，单位：rad/s；

(2)状态方程：

式中：X为状态量；F为系统矩阵；W是系统噪声矩阵，依据系统的误差特性确定；

系统矩阵F为：

式中，R_M为子午圈地球半径，单位：m；R_N为卯酉圈地球半径，单位：m；V_n，V_u，V_e分别为惯导系统北向、天向、东速向速度，单位：m/s；ω_ie为地球自转角速率，单位rad/s；L为当地纬度，单位：rad；h为当地高度，单位：m，f_n，f_u，f_e分别为惯导系统北向、天向、东向加速度，单位：m/s²；为系统计算姿态矩阵；

(3)量测方程：

Z＝HX+V (9)

式中：Z为量测量；H为量测阵；X为状态量；V为量测噪声矩阵；

a)Z为量测量：

式中：l_ins、λ_ins分别为惯导系统的纬度、经度，单位：rad；h_ins为惯导系统的高度，单位：m；l_b、λ_b分别为基准的纬度、经度，单位：rad；h_b为基准的高度，单位：m；

b)H为量测阵：

当进行量测更新时，

当进行时间更新时，

H＝[0_3×15] (12)

(4)卡尔曼滤波方程：

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差阵：

估计误差方差阵：

P_k＝[I-K_kH_k]P_k,k-1 (17)

式中：为一步状态预测值，为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，为的转置矩阵，Φ_k,k-1，H_k为量测矩阵，为H_k的转置矩阵，Z_k为量测量，K_k为滤波增益矩阵，R_k为观测噪声阵，P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，为Γ_k,k-1的转置矩阵，Q_k-1为系统噪声阵。

本发明实施例提供的技术方案带来的有益效果是：

本发明一种基于路径匹配的惯导系统误差修正方法，具有很高自主性的；首先建立路径地图数据库，可参照目前车载导航领域常用的地理信息系统(GIS)标准数据结构，也可针对定位定向系统应用特点建立基于定位点的路径数据库；然后在载体行驶过程中选取惯导系统一定路程内的行驶轨迹数据与惯导系统误差范围内的所有基准路径数据进行比对计算，基于路径的变化特性完成路径匹配，获得当前惯导系统所在位置的参考路径，实现载体在基准路径数据库中的定位，从而用基准路径中的位置信息完成对惯导系统误差的修正。

本发明一种基于路径匹配的惯导系统误差修正方法，路径匹配获得的基准位置信息是不连续的，载体行驶一段距离才能获得一个基准点，虽然可以直接用基准位置信息对惯导位置进行置换，但这样不能对惯导的速度、姿态以及惯性器件误差进行修正；本发明进一步在路径匹配的基础上，采用卡尔曼滤波的方法，利用这种不连续的、稀疏条件下的基准位置信息实现对惯导系统误差的估计与修正。

附图说明

图1是本发明路径匹配的基本原理示意图；

图2是本发明路径变化不明显时的路径匹配示意图；

图3是本发明路径变化大小判断示意图；

具体实施方式

下面结合附图和具体实施方式对本发明一种基于路径匹配的惯导系统误差修正方法作详细说明。

本发明一种基于路径匹配的惯导系统误差修正方法，包括以下步骤：

步骤一、建立路径地图数据库；

路径地图数据库的建立，可参照目前车载导航领域常用的地理信息系统(GIS)标准数据结构，也可针对定位定向系统应用特点建立基于标准定位点的路径数据库；其主要目的是从上述现有数据库中获取路径的经纬度值；

步骤二、利用惯导系统的误差大小，选取路径匹配的基准路径范围；目的是减少计算量，缩短路径匹配时间；

例如，惯导系统在两次匹配间隔内的误差发散大小在δS(δx,δy)以内，则选取基准路径范围为当前惯导系统位置点相距误差δS(δx,δy)内的所有基准路径；

步骤三、在步骤二中选取的基准路径范围中，寻找与惯导系统运动路径最为匹配的路径，其基本原理如图1所示，利用惯导系统运动路径的变化特点，在所选取的基准路径范围内中寻找与惯导系统路径最为相近的一段路径。

实施例1：

在载体行驶过程中，每隔一定距离记录惯导系统一位置点如D(x,y)，用所记录的位置坐标D_i(x_i,y_i)与行驶的这段路径坐标均值的差表示路径的变化，其中：

式中：x，y分别为惯导系统坐标的经纬度，单位rad；

所记录的每一个位置点相对于行驶的这段路径的变化(dx_i,dy_i)为：

相应的，在基准路径中取相同数量的位置点R_i(p_i,q_i)，位置点之间的间隔与上述惯导系统记录的位置点间隔相同，基准路径中这段路径坐标均值为：

式中：p、q分别为基准路径坐标的经纬度，单位rad；

基准路径中每个位置点相对于此段路径的变化(dp_i,dq_i)为：

用r表示载体行驶路径与基准路径变化规律的相近度：

则当r最小时的基准路径为与载体路径最为匹配的路径；

实施例2：

但上述方法也存在一定的局限性，当路径变化越明显时，路径匹配的正确率越高。若较长的一段路径比较直，变化不明显时，很可能出现匹配错误的情况，如图2所示的路径。

为了避免上述路径匹配错误的情况出现，对惯导路径的变化大小加以判断，计算各个位置点到路径首尾连线的距离，当距离之和小于一定的门限时，认为这段路径较直，变化较小，此时路径匹配的错误率可能变大，不进行位置校正。

例如图3所示：

式中：d_i为第i个点距离路径首尾连线的距离；

当D小于门限T_D时，认为路径较直，此时不进行匹配；

当匹配成功时，以基准路径中的路径端点为此时惯导系统的基准位置信息，惯导位置对应的基准路径的位置。

实施例3：

由于上述路径匹配获得的基准位置信息是不连续的，车辆行驶一段距离才能获得一个基准点，虽然可以直接用基准位置信息对惯导位置进行置换，但这样不能对惯导系统的速度、姿态以及惯性器件误差进行修正；

为解决上述技术问题，进一步的采用卡尔曼滤波的方法，利用这种不连续的、稀疏条件下的基准位置信息实现对惯导系统误差的估计与修正。

具体方法为：在惯导系统转导航后，重启卡尔曼滤波器，持续进行时间更新；在路径匹配成功时，开始量测更新，以基准位置为参考信息进行位置匹配，在路径匹配未成功时仍然进行时间更新。

例如，一种滤波模型如下：

(1)滤波状态量：

采用15维的状态量：

式中：ΔL、Δλ分别为惯导系统的纬度、经度误差，单位：rad；Δh为惯导系统的高度，单位：m；ΔV_n、ΔV_u、ΔV_e分别为惯导系统的北向、天向、东向速度误差，单位m/s；Φ_n、Φ_u、Φ_e分别为惯导系统北向、天向、东向失准角误差，单位：rad；分别为惯导系统x、y、z轴加速度计零偏，单位：m/s²；ε_x、ε_y、ε_z分别为惯导系统x、y、z轴陀螺漂移，单位：rad/s；

(2)状态方程：

式中：X为状态量；F为系统矩阵；W是系统噪声矩阵，依据系统的误差特性确定；

系统矩阵F为：

式中，R_M为子午圈地球半径，单位：m；R_N为卯酉圈地球半径，单位：m；V_n，V_u，V_e分别为惯导系统北向、天向、东速向速度，单位：m/s；ω_ie为地球自转角速率，单位rad/s；L为当地纬度，单位：rad；h为当地高度，单位：m，f_n，f_u，f_e分别为惯导系统北向、天向、东向加速度，单位：m/s²；为系统计算姿态矩阵；

(3)量测方程：

Z＝HX+V (9)

式中：Z为量测量；H为量测阵；X为状态量；V为量测噪声矩阵；

a)Z为量测量：

式中：l_ins、λ_ins分别为惯导系统的纬度、经度，单位：rad；h_ins为惯导系统的高度，单位：m；l_b、λ_b分别为基准的纬度、经度，单位：rad；h_b为基准的高度，单位：m；

b)H为量测阵：

当进行量测更新时，

当进行时间更新时，

H＝[0_3×15] (12)

(4)卡尔曼滤波方程：

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差阵：

估计误差方差阵：

P_k＝[I-K_kH_k]P_k,k-1 (17)

式中：为一步状态预测值，为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，为的转置矩阵，Φ_k,k-1，H_k为量测矩阵，为H_k的转置矩阵，Z_k为量测量，K_k为滤波增益矩阵，R_k为观测噪声阵，P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，为Γ_k,k-1的转置矩阵，Q_k-1为系统噪声阵。

Claims (5)

1.一种基于路径匹配的惯导系统误差修正方法，其特征在于：该方法包括以下步骤：

步骤一、建立基准路径地图数据库；

步骤二、利用惯导系统的误差大小，从步骤一所建立的基准路径地图数据库中选取路径匹配的基准路径范围；

步骤三、在步骤二中选取的基准路径范围中，利用惯导系统运动路径的变化特点，在所选取的基准路径范围内中寻找与惯导系统路径最为匹配的一段路径；

步骤一所述的基准路径地图数据库的建立，参照车载导航领域常用的地理信息系统标准数据结构建立，或者针对定位定向系统应用特点建立基于标准定位点的路径数据库，以获取路径的经纬度值；

当惯导系统在两次匹配间隔内的误差发散大小在δS(δx,δy)以内，则选取基准路径范围为当前惯导系统位置点相距误差δS(δx,δy)内的基准路径地图数据库中所有基准路径；

其特征在于：

首先，在惯导系统载体行驶过程中，每隔一定距离记录惯导系统一位置点如D(x,y)，用所记录的位置坐标D_i(x_i,y_i)与行驶的这段路径坐标均值的差表示这段路径的变化：

所记录的每一个位置点相对于行驶的这段路径的变化(dx_i,dy_i)为：

其次，相应的，在基准路径中取相同数量的位置点R_i(p_i,q_i)，位置点之间的间隔与上述惯导系统记录的位置点间隔相同，基准路径中这段路径坐标均值为：

基准路径中每个位置点相对于此段路径的变化(dp_i,dq_i)为：

最后，用r表示载体行驶路径与基准路径变化规律的相近度：

则当r最小时的基准路径为与载体路径最为匹配的路径。

2.根据权利要求1所述的一种基于路径匹配的惯导系统误差修正方法，其特征在于：

在路径匹配过程中，对惯导系统路径的变化幅度进行判断，计算各个位置点到路径首尾连线的距离，当距离之和小于一定的门限时，认为这段路径较直，变化较小，此时路径匹配的错误率可能变大，不进行位置校正。

3.根据权利要求2所述的一种基于路径匹配的惯导系统误差修正方法，其特征在于：

所述惯导系统路径的变化幅度判断的方法为：

式中：d_i为第i个点距离路径首尾连线的距离；

当D小于门限T_D时，认为路径较直，此时不进行匹配；

当匹配成功时，以基准路径中的路径端点为此时惯导系统的基准位置信息，惯导系统位置对应的基准路径的位置。

4.根据权利要求1或3所述的一种基于路径匹配的惯导系统误差修正方法，其特征在于：该方法还包括以下步骤：

采用卡尔曼滤波的方法完成对惯导系统误差的估计与补偿，在惯导系统转导航后，重启卡尔曼滤波器，持续进行时间更新；在路径匹配成功时，开始量测更新，以基准位置为参考信息进行位置匹配，在路径匹配未成功时仍然进行时间更新。

5.根据权利要求4所述的一种基于路径匹配的惯导系统误差修正方法，其特征在于：所述卡尔曼滤波的方法包括以下步骤：

(1)滤波状态量：

采用15维的状态量：

式中：ΔL、Δλ分别为惯导系统的纬度、经度误差，单位：rad；Δh为惯导系统的高度，单位：m；ΔV_n、ΔV_u、ΔV_e分别为惯导系统的北向、天向、东向速度误差，单位m/s；Φ_n、Φ_u、Φ_e分别为惯导系统北向、天向、东向失准角误差，单位：rad；分别为惯导系统x、y、z轴加速度计零偏，单位：m/s²；ε_x、ε_y、ε_z分别为惯导系统x、y、z轴陀螺漂移，单位：rad/s；

(2)状态方程：

式中：X为状态量；F为系统矩阵；W是系统噪声矩阵，依据系统的误差特性确定；

系统矩阵F为：

式中，R_M为子午圈地球半径，单位：m；R_N为卯酉圈地球半径，单位：m；V_n，V_u，V_e分别为惯导系统北向、天向、东速向速度，单位：m/s；ω_ie为地球自转角速率，单位rad/s；L为当地纬度，单位：rad；h为当地高度，单位：m，f_n，f_u，f_e分别为惯导系统北向、天向、东向加速度，单位：m/s²；为系统计算姿态矩阵；

(3)量测方程：

Z＝HX+V (9)

式中：Z为量测量；H为量测阵；X为状态量；V为量测噪声矩阵；

a)Z为量测量：

式中：l_ins，λ_ins分别为惯导系统的纬度、经度，单位：rad；h_ins为惯导系统的高度，单位：m；l_b，λ_b分别为基准的纬度、经度，单位：rad；h_b为基准的高度，单位：m；

b)H为量测阵：

当进行量测更新时，

当进行时间更新时，

H＝[0_3×15] (12)

(4)卡尔曼滤波方程：

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差阵：

估计误差方差阵：

P_k＝[I-K_kH_k]P_k,k-1 (17)

式中：为一步状态预测值，为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，为Φ_k,k-1的转置矩阵，H_k为量测矩阵，为H_k的转置矩阵，Z_k为量测量，K_k为滤波增益矩阵，R_k为观测噪声阵，P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，为Γ_k,k-1的转置矩阵，Q_k-1为系统噪声阵。