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

权利要求书

1.一种相机、imu和激光雷达联合的机器人定位方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述一种相机、imu和激光雷达联合的机器人定位方法，其特征在于，所述S7运动估计模型约束处理具体包括：

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

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

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

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

3.根据权利要求1所述一种相机、imu和激光雷达联合的机器人定位方法，其特征在于，所述S10闭环优化具体包括：先通过视觉和imu数据得到当前帧与回环帧相对位姿，再将当前帧及其局部点云图投射到回环帧中，根据点云图得到相对位姿，根据帧间转换，修正闭环中各帧位姿和地图数据。