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

说明书

本发明公开了一种基于先验轨迹辅助的微惯性/轮速计组合导航方法，具体包括以下步骤：首先建立先验地图库；接着进行先验轨迹辅助导航初始化，包括相对地图原点的微惯性/轮速计组合导航；旋转和平移导航轨迹进行地图匹配获取对应路段的先验轨迹；将先验轨迹反旋转和平移至地图原点，先验轨迹点与组合导航轨迹点进行最近点匹配；回溯进行基于最近先验轨迹点位置辅助的组合导航；对修正后的相对位置和姿态绝对化处理；最后重复利用绝对位置从先验地图库中获取先验轨迹，进行基于最近先验轨迹点位置辅助的组合导航，直至导航结束。本发明有效修正了微惯性/轮速计组合导航结果，弥补了依靠微惯性和轮速计无法进行全局定位定向的缺陷。

技术领域

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

背景技术

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

发明内容

为了应对在无卫星信号条件下，激光雷达、相机等传感器无法提供辅助导航信息时，微惯性/轮速计组合导航存在姿态不准确、位置误差累积以及无法进行全局定位定向的问题，本发明提出了一种基于先验轨迹辅助的微惯性/轮速计组合导航方法。

本发明的上述目的通过以下技术方案来实现：

一种基于先验轨迹辅助的微惯性/轮速计组合导航方法，具体包括以下步骤：

步骤S1：建立区域先验地图库，地图库包含道路边界和先验轨迹点；

步骤S2：进行先验轨迹辅助导航初始化；

步骤S3：利用组合导航输出的绝对位置从先验地图库中获取先验轨迹，经最近点匹配获取最近匹配点辅助微惯性/轮速计组合导航，重复该步骤直至导航结束。

进一步，所述步骤S2具体包括以下过程：

S2.1载体在已建好地图库的区域行驶一段路程，同时进行相对地图原点的微惯性/轮速计组合导航，得到该段路程相对地图原点的位置轨迹和相对地图坐标轴的姿态序列，其中，组合导航采用经典卡尔曼滤波算法，组合系统状态空间模型为

系统状态向量X选择惯导系统的失准角φ、速度误差δvⁿ、定位误差δpⁿ、陀螺仪相关漂移加速度计相关偏值作为状态，F为系统矩阵，G为系统噪声输入矩阵，W为系统噪声向量，为微惯性导航解算的载体系速度，为轮速计测量的载体系速度，与的差值作为卡尔曼滤波的观测向量Z，H为量测矩阵，V为观测噪声向量，为导航坐标系n与载体坐标系b的转换矩阵，组合导航过程充分利用车辆的非完整约束；

S2.2利用微惯性/轮速计组合导航轨迹与载体真实运动轨迹的相似性原理，将导航轨迹经旋转和平移后进行地图匹配来获取对应路段的先验轨迹点；

S2.3利用步骤S2.2中的旋转和平移将先验轨迹反旋转和平移至地图原点：

其中，为全局先验轨迹点，R和t分别为步骤S2.2中的旋转矩阵和平移向量，为经反旋转和平移后得到的相对地图原点的先验轨迹点；