一种基于先验轨迹辅助的微惯性/轮速计组合导航方法

1.本发明涉及一种基于先验轨迹辅助的微惯性/轮速计组合导航方法，属于信息融合技术，特别适用于组合导航领域。


背景技术：

2.微惯性/轮速计组合导航是目前无人车组合导航常用的技术手段，具有自主性好、抗干扰性强的优点。但仅依靠微惯性和轮速计无法实现绝对定位定向，常选择与全球导航卫星系统(gnss)进行组合。而在无卫星信号条件下，为抑制微惯性/轮速计组合导航位置误差累积，激光雷达、相机等成为常用的辅助传感器。但激光雷达、相机易受环境等外界因影响，存在工作中断的问题。由于微惯性/轮速计组合导航轨迹与载体真实轨迹具有相似性，同时载体真实轨迹又与规划轨迹和路网轨迹等先验轨迹具有相似性，则这为先验轨迹辅助微惯性/轮速计组合导航提供了可能。地图匹配为实现组合导航轨迹与先验轨迹的匹配对应提供了技术手段。另外，基于地图先验轨迹的辅助将弥补微惯性/轮速计组合导航无法进行全局定位定向的缺陷。


技术实现要素：

3.为了应对在无卫星信号条件下，激光雷达、相机等传感器无法提供辅助导航信息时，微惯性/轮速计组合导航存在姿态不准确、位置误差累积以及无法进行全局定位定向的问题，本发明提出了一种基于先验轨迹辅助的微惯性/轮速计组合导航方法。
4.本发明的上述目的通过以下技术方案来实现：
5.一种基于先验轨迹辅助的微惯性/轮速计组合导航方法，具体包括以下步骤：
6.步骤s1：建立区域先验地图库，地图库包含道路边界和先验轨迹点；
7.步骤s2：进行先验轨迹辅助导航初始化；
8.步骤s3：利用组合导航输出的绝对位置从先验地图库中获取先验轨迹，经最近点匹配获取最近匹配点辅助微惯性/轮速计组合导航，重复该步骤直至导航结束。
9.进一步，所述步骤s2具体包括以下过程：
10.s2.1载体在已建好地图库的区域行驶一段路程，同时进行相对地图原点的微惯性/轮速计组合导航，得到该段路程相对地图原点的位置轨迹和相对地图坐标轴的姿态序列，其中，组合导航采用经典卡尔曼滤波算法，组合系统状态空间模型为
[0011][0012][0013][0014]
系统状态向量x选择惯导系统的失准角φ、速度误差δvn、定位误差δpn、陀螺仪相关
漂移加速度计相关偏值作为状态，f为系统矩阵，g为系统噪声输入矩阵，w为系统噪声向量，为微惯性导航解算的载体系速度，为轮速计测量的载体系速度，与的差值作为卡尔曼滤波的观测向量z，h为量测矩阵，v为观测噪声向量，为导航坐标系n与载体坐标系b的转换矩阵，组合导航过程充分利用车辆的非完整约束；
[0015]
s2.2利用微惯性/轮速计组合导航轨迹与载体真实运动轨迹的相似性原理，将导航轨迹经旋转和平移后进行地图匹配来获取对应路段的先验轨迹点；
[0016]
s2.3利用步骤s2.2中的旋转和平移将先验轨迹反旋转和平移至地图原点：
[0017][0018]
其中，为全局先验轨迹点，r和t分别为步骤s2.2中的旋转矩阵和平移向量，为经反旋转和平移后得到的相对地图原点的先验轨迹点；
[0019]
s2.4进行先验轨迹点与微惯性/轮速计组合导航轨迹点的最近点匹配：
[0020][0021]
其中，为第j个相对地图原点的微惯性/轮速计组合导航轨迹点，为第i个相对地图原点的先验轨迹点，选择组合导航轨迹点中与先验轨迹点距离最近的点作为最近匹配点；
[0022]
s2.5回溯进行基于最近先验轨迹点位置辅助的微惯性/轮速计组合导航，得到修正后的相对位置和姿态，其中，组合导航仍采用经典卡尔曼滤波算法，状态向量及状态方程与步骤s2.1所述相同，量测向量及量测方程变为
[0023][0024][0025]
其中，为与相匹配的组合导航位置；
[0026]
s2.6对修正后的相对位置和姿态进行全局化处理，即经旋转和平移后得到绝对位置和姿态：
[0027][0028][0029]
其中，和分别为组合导航绝对位置和姿态，和分别为修正后的相对地图原点的组合导航位置和姿态。
[0030]
进一步，所述步骤s3具体包括以下过程：
[0031]
利用组合导航输出的绝对位置从先验地图库中获取先验轨迹，经最近点匹配获取最近匹配点辅助微惯性/轮速计组合导航，其中，最近点匹配和最近匹配点辅助微惯性/轮速计组合导航均与步骤s2.4和步骤s2.5相同。
[0032]
本发明具有如下优点和有益效果：
[0033]
(1)本发明为在无卫星信号条件下，激光雷达、相机等传感器无法提供辅助导航信息时，依靠先验轨迹、微惯性和轮速计进行组合定位定向提供了解决方案；
[0034]
(2)本发明利用先验轨迹辅助有效修正了微惯性/轮速计组合导航结果，弥补了依靠微惯性和轮速计无法进行全局定位定向的缺陷。
[0035]
(3)本发明在完成先验轨迹辅助导航初始化后，便可在线实时辅助微惯性/轮速计组合导航。
附图说明
[0036]
图1为本发明所述的基于先验轨迹辅助的微惯性/轮速计组合导航方法原理框图；
[0037]
图2为本发明所述的先验轨迹辅助的微惯性/轮速计组合导航初始化示意图；
[0038]
图3为实施例中先验轨迹点与最近匹配点示意图；
[0039]
图4为实施例中先验轨迹、真实轨迹、sins/odo和先验轨迹辅助后的轨迹对比图；
[0040]
图5为实施例中先验轨迹辅助修正前后水平定位误差对比图；
[0041]
图6为实施例中先验轨迹辅助修正前后姿态误差对比图。
具体实施方式
[0042]
下面将结合附图及具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
[0043]
实施例1：参照图1和图2，在一个实施例中，基于先验轨迹辅助的微惯性/轮速计组合导航方法主要包括以下步骤：
[0044]
步骤s1：建立区域先验地图库，地图库包含道路边界和先验轨迹点等。
[0045]
步骤s2：进行先验轨迹辅助导航初始化：
[0046]
s2.1载体在已建好地图库的区域行驶一段路程，同时进行相对地图原点的微惯性/轮速计组合导航，得到该段路程相对地图原点的位置轨迹和相对地图坐标轴的姿态序列。其中，组合导航采用经典卡尔曼滤波算法，组合系统状态空间模型为
[0047][0048][0049][0050]
系统状态向量x选择惯导系统的失准角φ、速度误差δvn、定位误差δpn、陀螺仪相关漂移加速度计相关偏值作为状态，f为系统矩阵，g为系统噪声输入矩阵，w为系统噪声向量，为微惯性导航解算的载体系速度，为轮速计测量的载体系速度，与的差值作为卡尔曼滤波的观测向量z，h为量测矩阵，v为观测噪声向量，为导航坐标系n与载体坐标系b的转换矩阵，组合导航过程充分利用车辆的非完整约束；
[0051]
s2.2利用微惯性/轮速计组合导航轨迹与载体真实运动轨迹的相似性原理，将导航轨迹经旋转和平移后进行地图匹配来获取对应路段的先验轨迹点；
[0052]
s2.3利用步骤s2.2中的旋转和平移将先验轨迹反旋转和平移至地图原点：
[0053][0054]
其中，为全局先验轨迹点，r和t分别为步骤s2.2中的旋转矩阵和平移向量，为经反旋转和平移后得到的相对地图原点的先验轨迹点；
[0055]
s2.4进行先验轨迹点与微惯性/轮速计组合导航轨迹点的最近点匹配：
[0056][0057]
其中，为第j个相对地图原点的微惯性/轮速计组合导航轨迹点，为第i个相对地图原点的先验轨迹点，选择组合导航轨迹点中与先验轨迹点距离最近的点作为最近匹配点；
[0058]
s2.5回溯进行基于最近先验轨迹点位置辅助的微惯性/轮速计组合导航，得到修正后的相对位置和姿态。其中，组合导航仍采用经典卡尔曼滤波算法，状态向量及状态方程与步骤s2.1所述相同，量测向量及量测方程变为
[0059][0060][0061]
其中，为与相匹配的组合导航位置；
[0062]
s2.6对修正后的相对位置和姿态进行全局化处理，即经旋转和平移后得到绝对位置和姿态：
[0063][0064][0065]
其中，和分别为组合导航绝对位置和姿态，和分别为修正后的相对地图原点的组合导航位置和姿态。
[0066]
步骤s3：利用组合导航输出的绝对位置从先验地图库中获取先验轨迹，经最近点匹配获取最近匹配点辅助微惯性/轮速计组合导航，重复该步骤直至导航结束。
[0067]
下面是实施例的仿真实验结果。图4为先验轨迹、载体真实轨迹、sins/odo轨迹和先验轨迹辅助后的导航轨迹对比图。图5为先验轨迹辅助修正前后水平定位误差对比图。图6为先验轨迹辅助修正前后航向误差对比图。仿真结果表明，本发明能够有效修正微惯性/轮速计组合导航结果，为依靠先验轨迹、微惯性和轮速计进行组合定位定向提供了解决方案。
[0068]
需要说明的是上述实施例仅仅是本发明的较佳实施例，并没有用来限定本发明的保护范围，在上述技术方案基础上做出的等同替换或者替代，均属于本发明的保护范围。