[发明专利]一种多传感器的高精度即时定位与建图方法

摘要

本发明提供了一种多传感器的高精度即时定位与建图方法，即使用彩色相机与激光雷达对周围环境进行高精度的实时三维构建，并实时测算出相机的位置与姿态。在快速的视觉SLAM的基础上，引入激光雷达的信息，并有选择的使用激光雷达信息，及时进行结果修正，再依靠修正后的结果构建三维地图，使定位与建图具有正确的尺度和较高的精度，同时使算法具有较低的复杂度。

主权项

一种多传感器的高精度即时定位与建图方法，其特征在于步骤如下：步骤1：输入彩色图像并预处理：输入相机拍摄的彩色图像序列，将每一帧彩色图像转换为灰度图像；步骤2：特征提取：利用ORB特征提取算法对每一帧灰度图像进行特征提取，得到每一帧图像的ORB特征点；步骤3：特征匹配和运动估计：利用近似最近邻库对相邻两帧图像的特征点进行匹配，对匹配好的点对利用基于随机抽样一致法的PnP求解进行运动估计，得到相机的运动估计序列其中，为相机在拍摄第i帧和第i‑1帧图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵，tCam表示相机位移矩阵；步骤4：输入对应激光雷达数据并预处理：输入激光雷达数据，将距离大于激光雷达量程的0.8倍的点去掉，由剩余点构成激光雷达点云数据；这里，假定所述的激光雷达与步骤1中拍摄彩色图像的相机的位置关系固定、已完成联合标定，且二者视野相同、内部参数已知；步骤5：激光雷达点云数据的特征匹配和运动估计：对激光雷达点云数据每隔x帧进行一次特征匹配和运动估计，即将每一帧激光雷达点云数据在水平方向上划分为四个等大区域，每个区域根据光滑度提取4个边缘点和4个平面点构成特征点，根据光滑度对第j帧和第j‑x帧数据的特征点进行匹配，如果对匹配好的点对利用ICP算法进行运动估计，得到激光雷达的运动估计序列其中，为激光雷达在扫描第j帧和第j‑x帧数据之间的运动估计，j＝x+1,2x+1,…,M，M为激光雷达数据总帧数，x取值范围为[5,20]，RLidar表示激光雷达旋转矩阵，tLidar表示激光雷达位移矩阵；所述的边缘点为区域内光滑度最小的点，所述的平面点为区域内光滑度最大的点，光滑度为其中，S代表第k次扫描中所有的点构成的集合，Lk,p与Lk,q代表p与q点的空间坐标，||·||表示欧式距离；所述的根据光滑度进行匹配是指如果光滑度的差值小于当前点的光滑度的5％，则匹配，否则，不匹配；步骤6：误差修正并记录轨迹：用步骤5得到的激光雷达的运动估计替换对应时间间隔内相机的运动估计之和，得到误差修正后的相机运动估计序列S′，即相机的运动轨迹；步骤7：建立三维点云地图：按照计算得到三维点云地图pclmap，其中，为第nj帧图像变换到第一帧图像视角坐标系下的三维点云地图，设第m帧激光雷达点云扫描的同一时刻相机拍摄的图像帧序号为nj，由nj构成的序列为N，m＝x+1,2x+1,…,M；为第nj帧图像的点云地图，L为步骤2中提取的第nj帧图像的特征点的个数，(xl,yl,zl)表示点的坐标，按以下公式计算：xl＝(ul‑cx)·dl/fxyl＝(vl‑cy)·dl/fyzl＝dl其中，l＝1,…,L，(ul,vl)为第l个特征点的图像坐标，(fx,fy,cx,cy)为给定的相机参数，dl为第l个特征点在图像拍摄时刻对应的激光雷达点云中检索到的深度值；如果点云密度过大，则对点云地图数据进行降采样，即得到最终的三维点云地图；所述的点云密度过大是指一立方米空间中点的个数超过10000。