[发明专利]一种面向封闭园区自动驾驶的单目视觉惯性定位方法

说明书

本发明公开了一种面向封闭园区自动驾驶的单目视觉惯性定位方法，方法包括：S0，读入多帧连续单目图像和IMU测量信息，初始化处理；S1，跟踪上一帧的特征点，在当前帧提取特征点，处理当前帧与上一帧的IMU测量信息；S2，求取当前帧的位姿；S3，将平移向量和特征点深度与旋转向量解耦，确定当前帧的特征点中的异常点，剔除异常点；S4，在滑动窗口中选择视觉重投影误差和IMU残差为代价函数，构建最小二乘问题，求解车辆位姿；S5，根据IMU测量信息的不确定性设置优化模式；S6，在滑动窗口中，若当前帧的上一帧为非关键帧，则去掉上一帧；否则去掉滑动窗口的初始帧；去帧后转S1。本发明能够保证车辆在园区的各种工况下实现实时稳定准确的定位。

技术领域

本发明涉及自动驾驶技术领域，特别为涉及一种面向封闭园区自动驾驶的单目视觉惯性定位方法。

背景技术

近年来，L3级别以上的自动驾驶受到了广大企业和高校的关注，而封闭园区因车速基本在30km/h以下、线路固定和动态物体少等特点被认为为当前自动驾驶技术应用的优先场景。为了保证在园区中安全可靠地到达目的地，解决无人车“我在哪”、“怎么去”的问题相关重要，因此需要一种精确且鲁棒的定位技术。

自动驾驶的常用定位方法为由全球卫星导航定位单元(GNSS)和惯性单元(INS)组成的组合惯导系统。车辆大多采用安装双天线的卫星定位方式获取车辆的绝对三维坐标，在开阔地带定位精度可达厘米级，但在树木和建筑遮挡、天气不良的情况下GNSS信号不稳定，定位误差大。而IMU(英文全称为“Inertial Measuring Unit”，中文全称为“惯性测量单元”)不受外部测量干扰，通过自身高频率积分得到的位姿与低频率的GNSS位姿共同协作，可以完成高频率和高精度的定位。如果GNSS信号长时间较弱，单纯地依靠惯导系统，会因为IMU的噪声和偏差产生漂移。近年来不少专利采用同步定位与建图(SLAM)方法，通过搭载激光雷达或摄像头，能够在未知的环境中进行自主定位和导航。激光SLAM定位较为准确和稳定，但激光雷达成本高；视觉SLAM定位具有丰富的信息且成本低，但在纯旋转运动和急速转弯等情况下定位不准确。

当前自动驾驶定位系统的相关专利提出将上述定位方法结合以解决彼此弊端的技术。江铃汽车股份有限公司提出的商用车自动驾驶定位方法通过摄像头判断车辆前方为否为隧道、廊桥场景，如果为，则使用激光SLAM定位，否则使用组合惯导定位。安徽酷哇机器人有限公司提出的自动驾驶定位方法能够根据需求自动在组合惯导、激光SLAM和视觉SLAM定位方法间转换。杭州电子科技大学提出的基于激光SLAM和视觉SLAM地图融合的方法可以克服单种SLAM的缺陷，使融合后的地图更加适用于导航。

通过上述专利可以看出多传感器融合定位已经为一种趋势，相比单传感器定位，其定位的精度更高、稳定性更好。但除了定位的效果，同时也要考虑系统的易用性、传感器的价格等。

发明内容

针对上述问题，本发明的目的为提供一种面向封闭园区自动驾驶的单目视觉惯性定位系统，该系统不需要环境的先验信息，通过融合IMU的绝对尺度、快速响应的特点以及单目视觉标定简单、位姿准确的特点，保证车辆在园区的各种工况下实现实时稳定准确的定位，且其成本低、体积小，经济性好。

为实现上述目的，本发明提供一种面向封闭园区自动驾驶的单目视觉惯性定位方法，所述方法包括以下步骤：

S0，读入多帧连续图像和相应的IMU测量信息，并进行初始化处理，确定IMU参数，并将优化模式初始设置为mode A模式，其中，在mode A模式下，以IMU旋转积分和平移积分得到的旋转向量和平移向量作为图像帧的位姿；

S1，读取当前帧对应的IMU测量信息，并跟踪上一帧的特征点，并在当前帧提取预设数量的特征点，同时采取预积分的方法处理当前帧与上一帧间的IMU测量信息；

S2，基于优化模式，采用当前帧的特征点或IMU测量值积分求取当前帧的位姿；