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

技术特征：
1.一种基于先验轨迹辅助的微惯性/轮速计组合导航方法，其特征在于，具体包括以下步骤：步骤s1：建立区域先验地图库，地图库包含道路边界和先验轨迹点；步骤s2：进行先验轨迹辅助导航初始化；步骤s3：利用组合导航输出的绝对位置从先验地图库中获取先验轨迹，经最近点匹配获取最近匹配点辅助微惯性/轮速计组合导航，重复该步骤直至导航结束。2.根据权利要求1所述的基于先验轨迹辅助的微惯性/轮速计组合导航方法，其特征在于，所述步骤s2具体包括以下过程：s2.1载体在已建好地图库的区域行驶一段路程，同时进行相对地图原点的微惯性/轮速计组合导航，得到该段路程相对地图原点的位置轨迹和相对地图坐标轴的姿态序列，其中，组合导航采用经典卡尔曼滤波算法，组合系统状态空间模型为导航采用经典卡尔曼滤波算法，组合系统状态空间模型为导航采用经典卡尔曼滤波算法，组合系统状态空间模型为系统状态向量x选择惯导系统的失准角φ、速度误差δv
n
、定位误差δp
n
、陀螺仪相关漂移加速度计相关偏值作为状态，f为系统矩阵，g为系统噪声输入矩阵，w为系统噪声向量，为微惯性导航解算的载体系速度，为轮速计测量的载体系速度，与的差值作为卡尔曼滤波的观测向量z，h为量测矩阵，v为观测噪声向量，为导航坐标系n与载体坐标系b的转换矩阵，组合导航过程充分利用车辆的非完整约束；s2.2利用微惯性/轮速计组合导航轨迹与载体真实运动轨迹的相似性原理，将导航轨迹经旋转和平移后进行地图匹配来获取对应路段的先验轨迹点；s2.3利用步骤s2.2中的旋转和平移将先验轨迹反旋转和平移至地图原点：其中，为全局先验轨迹点，r和t分别为步骤s2.2中的旋转矩阵和平移向量，为经反旋转和平移后得到的相对地图原点的先验轨迹点；s2.4进行先验轨迹点与微惯性/轮速计组合导航轨迹点的最近点匹配：其中，为第j个相对地图原点的微惯性/轮速计组合导航轨迹点，为第i个相对地图原点的先验轨迹点，选择组合导航轨迹点中与先验轨迹点距离最近的点作为最近匹配点；s2.5回溯进行基于最近先验轨迹点位置辅助的微惯性/轮速计组合导航，得到修正后的相对位置和姿态，其中，组合导航仍采用经典卡尔曼滤波算法，状态向量及状态方程与步骤s2.1所述相同，量测向量及量测方程变为
其中，为与相匹配的组合导航位置；s2.6对修正后的相对位置和姿态进行全局化处理，即经旋转和平移后得到绝对位置和姿态：姿态：其中，和分别为组合导航绝对位置和姿态，和分别为修正后的相对地图原点的组合导航位置和姿态。

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


技术研发人员：徐晓苏 仲灵通 姚逸卿 周帅
受保护的技术使用者：东南大学
技术研发日：2022.11.22
技术公布日：2023/3/3