一种基于立体视觉惯性里程计的车辆定位导航方法及系统技术方案

本发明专利技术公开了一种基于立体视觉惯性里程计的车辆定位导航方法及系统，该方法包括如下步骤：捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息；将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。据此输出定位结果。据此输出定位结果。

【技术实现步骤摘要】
一种基于立体视觉惯性里程计的车辆定位导航方法及系统


[0001]本专利技术涉及一种基于立体视觉惯性里程计的车辆定位导航方法及系统，涉及卫星导航、人工智能、无人驾驶以及时间频率应用等领域。

技术介绍

[0002]车辆的准确和连续定位是自动驾驶汽车和移动地图系统的主要要求，全球导航卫星系统(GNSS)是满足户外环境中这一需求的关键技术；然而，在建筑密集的地区、城市峡谷和隧道中，GNSS无法很好利用，准确和可靠的车辆定位成为一个挑战。
[0003]惯性传感器通常与GNSS集成，可以在没有GNSS观测的情况下提供位置估计，但惯性导航系统(INS,Inertial Navigation System)定位会随着时间的推移定位误差会大量地积累和增长，因此INS不适合在GNSS信号丢失时长时间的定位。
[0004]惯性传感器和视觉传感器具有互补的特性，INS可以在每次图像采集时提供相机姿态的初始估计，视觉特征可以限制INS漂移；此外，无法保证纯粹基于视觉的导航系统可以提供连续定位，因为它们高度依赖视觉特征，因此在无特征的环境中表现不佳。
[0005]基于相机与惯性传感器集成的一般导航方法被称为视觉惯性里程计(VIO,Visual
‑
Inertial Odometry)。在机器人和计算机视觉方面对VIO进行了大量研究；用于将惯性测量与图像观测相结合的算法主要分为两类：基于优化的方法和滤波方法；优化算法通过对最小二乘误差函数的迭代最小化来估计未知参数，尽管这些算法提供了更好的估计，但它们必须限制可用的测量值，以实现实时姿态估计；与优化方法相反，滤波算法一有图像观测结果就以递归方式估计参数；在过去的几十年里，人们提出了许多滤波方法，用来解决实时问题；这些方法要么计算成本高，因此不适合实时定位，要么没有充分利用跟踪特性为提供的约束。
[0006]多态约束卡尔曼滤波(MSCKF,Multi
‑
State Constraint Kalman Filter)使用了一个特征从多个相机姿势提供的最大几何约束，很好地解决了实时性和跟踪特征的约束问题，但该方法的一个挑战是单图像序列跟踪特征可靠性低，影响姿态估计的准确性。

技术实现思路

[0007]本专利技术的目的是提供一种基于立体视觉惯性里程计的车辆定位导航方法及系统，解决现有技术中单图像序列跟踪特征可靠性低，姿态估计的准确性低等问题。
[0008]本专利技术是通过下述技术方案实现的：
[0009]第一方面，提供了一种基于立体视觉惯性里程计的车辆定位导航方法，包括如下步骤：
[0010]捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息，将所述位姿信息、特征数据、位姿偏差信息和状态信息进行传播；
[0011]将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；
[0012]通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；
[0013]根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。
[0014]结合第一方面，进一步的，通过INS捕获待观测点的位姿信息。
[0015]结合第一方面，进一步的，通过左侧相机捕获待观测点的特征数据。
[0016]结合第一方面，进一步的，预校准包括比对和分析。
[0017]结合第一方面，进一步的，INS状态向量通过如下公式预设：
[0018][0019]其中，是4
×
1单位四元数，表示在框架坐标系{B}中解析的INS的旋转量；
ε
V
B
和
ε
P
B
分别表示INS相对于地心地固坐标系{ε}的线性速度和位置；b
g
和b
a
分别表示3
×
1的陀螺仪和加速度计的偏差。
[0020]结合第一方面，进一步的，滤波处理是通过扩展卡尔曼滤波进行的。
[0021]第二方面，本专利技术还提供了一种基于立体视觉惯性里程计的车辆定位导航系统，包括MSCKF传播部分、MSCKF增强部分、相机部分和MSCKF更新部分；
[0022]MSCKF传播部分：用于捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息；
[0023]MSCKF增强部分：用于将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；
[0024]相机部分：包括左侧相机和右侧相机，通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；
[0025]MSCKF更新部分：用于根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。
[0026]与现有技术相比，本专利技术所达到的有益效果是：
[0027]本专利技术提供的一种基于立体视觉惯性里程计的车辆定位导航方法及系统，利用MSCKF集成待观测点的位姿信息和特征数据，然后进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵，增加了相机位姿估计的约束，提高了精确度；在相机部分，本专利技术采用左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息，增加对目标特征的估计；在MSCKF更新部分，根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，增加了对目标特征的滤波约束，提高了特征跟踪的有效性和准确性。
附图说明
[0028]为了更清楚地说明本方实施例的技术方案，下面将对本专利技术实施例中所需要使用的附图作简单地介绍，显而易见地，下面所描述的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的
附图。
[0029]图1是本专利技术实施例提供的一种基于立体视觉惯性里程计的车辆定位导航方法的流程图；
[0030]图2是本专利技术实施例提供的单声道MSCKF、立体声MSCKF、INS/GNSS组合与地面真实数据的比较图；
[0031]图3是本专利技术实施例提供的单声道MSCKF、立体声MSCKF、INS/GNSS组合的误差累计分布图。
具体实施方式
[0032]下面结合附图对本专利技术作进一步描述。以下实施例仅用于更加清楚地说明本专利技术的技术方案，而不能以此来限制本专利技术的保护范围。
[0033]实施例一
[0034]本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，包括如下步骤：捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息，将所述位姿信息、特征数据、位姿偏差信息和状态信息进行传播；将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。2.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，通过INS捕获待观测点的位姿信息。3.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，通过左侧相机捕获待观测点的特征数据。4.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，所述预校准包括比对和分析。5.根据权利要求1所述的一种基于立体视觉惯性里程计的车辆定位导航方法，其特征在于，所述INS状态向量通过如下公式预设：其中，是4
×
1单位四元数，表示在框架坐标系{B}中解析的INS的旋转量；...

【专利技术属性】
技术研发人员：徐劲松，
申请(专利权)人：江苏师范大学，
类型：发明
国别省市：