[发明专利]一种相机、imu和激光雷达联合的机器人定位方法

说明书

本发明公开了一种相机、imu和激光雷达联合的机器人定位方法，通过相机和激光雷达维护较准确周围环境的深度，并通过特征点的连续跟踪构造里程计。本方法能结合相机、imu和激光雷达的优点，建立精确稠密的点云地图，供机器人做路径规划用，通过相机与激光雷达的紧耦合，提高帧间里程计的精度，并达到较高鲁棒性；通过激光与相机实时维护的点云地图，再激光扫描中做里程计的局部优化；通过相机提供的单词信息及imu数据判断回环，在视觉和激光共同提供的点云地图实现进行闭环优化。

技术领域

本发明涉及机器人定位技术领域，尤其涉及一种相机、imu和激光雷达联合的机器人定位方法。

背景技术

随着计算能力和传感器等技术的不断增强，室外巡逻型和安防型机器人的功能也变得日益增大，与此同时，多传感器数据的混合使用也变得日益多样。机器人不再局限于单一的传感器的使用，而可以通过多传感器数据的相互配合从而获得更加准确的环境以及位姿信息。多传感器数据的相互配合使用作为机器人定位的重要手段，需要较强的计算能力，现在计算机的发展正好为此条件提供了良好的基础。

现有机器人方案大多基于摄像机或低线束激光雷达，基于摄像机的方案会带来精度和稳定性不高的问题，单纯基于激光雷达也会带来不能很好进行点云匹配的问题。

发明内容

本发明的目的就在于为了解决上述问题而提供一种相机、imu和激光雷达联合的机器人定位方法，包括如下步骤：

S1:标定激光雷达和相机，使得激光点与像素点达到像素级精度；

S2:设置激光雷达扫描频率、扫描角度、相机帧率及imu频率并进行数据采集；

S3:对imu数据预积分，并通过视觉特征点抑制其误差，实时获取帧间位姿变换信息与特征点信息；

S4:配准特征点与激光点云，选取特征点射线位于激光点云曲率较小区域的特征点对，插值出特征点距离，并对特征点对赋予首要权重；

S5:判断环境光照是否稳定以及点云纹理信息是否丰富，若是则转S6，否则通过imu获得帧间运动数据，并通过局部点云配准进行优化,转S6；

S6:计算位姿变换获得连续跟踪的特征点的深度信息并赋予次要权重；

S7:将点云图加入运动估计模型约束处理；

S8:根据特征点数量及imu激励判断并根据需要插入关键帧进行局部优化；

S9:通过相机获得特征点所维护的单词库，通过比较图像帧编码的单词信息和图像关键帧库的单词信息是否匹配并判断是否连续多帧为回环帧，若是则开启闭环检测进行闭环优化，转S10；否则转S3；

S10:优化全部帧及点云图。

进一步的，所述S7运动估计模型约束处理具体包括：

设：表示本帧特征点位置，表示上一帧特征点位置；为特征点的归一化方向；i表示特征点编号，k表示用于插值特征点的图像帧编号，R表示姿态变换值，T表示位移变换值；

(1)对于能插值出深度的特征点，直接使用位姿转换的估计作为约束，约束公式：

(2)对于只能前一帧能插值出深度的特征点，约束公式：

(3)对于两帧都不能插值出深度的特征点，通过imu的运动，三角化出特征点的位置，使用imu得到位姿变换初值，并在当前帧的基础上进行局部地图优化。