一种多传感器的高精度即时定位与建图方法
阅读：80发布：2020-05-12
专利汇可以提供一种多传感器的高精度即时定位与建图方法专利检索，专利查询，专利分析的服务。并且本 发明 提供了一种多 传感器 的高 精度 即时 定位 与建图方法，即使用彩色相机与 激光雷达 对周围环境进行高精度的实时三维构建，并实时测算出相机的 位置 与 姿态 。在快速的视觉SLAM的 基础 上，引入激光雷达的信息，并有选择的使用激光雷达信息，及时进行结果修正，再依靠修正后的结果构建三维地图，使定位与建图具有正确的尺度和较高的精度，同时使 算法 具有较低的复杂度。，下面是一种多传感器的高精度即时定位与建图方法专利的具体信息内容。
1.一种多传感器的高精度即时定位与建图方法，其特征在于步骤如下：
步骤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/fx
yl＝(vl -cy)·dl/fy
zl＝dl
其中，l＝1,…,L，(ul,vl)为第l个特征点的图像坐标，(fx,fy,cx,cy)为给定的相机参数，dl为第l个特征点在图像拍摄时刻对应的激光雷达点云中检索到的深度值；
如果点云密度过大，则对点云地图数据进行降采样，即得到最终的三维点云地图；所述的点云密度过大是指一立方米空间中点的个数超过10000。
一种多传感器的高精度即时定位与建图方法
技术领域
背景技术
[0002] 现阶段机器人的移动大多依靠人工路径规划，机器人的自主导航能力依赖于即时定位与建图技术(Simultaneous Localization and Mapping，以下简称为SLAM)。其核心任务是当机器人进入未知工作环境时，利用传感器信息，对周围环境进行高效率且准确地构建(Mapping)，同时得到设备在空间中的位置与姿态(Localization)。除了可以应用在机器人领域外，虚拟现实与增强现实设备的空间追踪以及自动驾驶也同样可以使用SLAM技术。
[0003] SLAM问题自提出以来已经过去了30年，这期间SLAM问题所用的传感器与计算方法都发生了巨大的变化。主流的SLAM技术多采用视觉传感器，包括单目传感器、多目传感器及彩色图像及深度信息(RGB-D)的传感器等。近年来，随着激光雷达技术的发展，激光雷达设备凭借其对环境度量的精准估计能力，以及对于光照变化不敏感等特性让激光雷达相关的SLAM方案在实际应用中有着广阔的发展空间。
[0004] 现阶段主流的基于视觉的SLAM方案可根据优化方法分为两类，一类为使用滤波器的SLAM方法，另一类的是使用图优化的SLAM方法。
[0005] 使用滤波器的SLAM方案模型构建比较简单，但误差会逐渐累计不可修复。 Davison等人在文献“A.Davison,I.Reid,and N.Molton.MonoSLAM:Real-Time Single Camera SLAM.IEEE Transactions on Pattern Analysis and Machine Intelligence,pp. 1052-1067,2007.”中提出了MonoSLAM方案，即是基于扩展卡尔曼滤波器(简称EKF) 的单目传感器SLAM方案，通过在图像中构建路标点并使用EKF对SLAM进行模型构建而求解。
[0006] 使用图优化方法的SLAM方案由于要构建姿态图往往运算量比较大。Artal等人在文献“R.Artal,J.Montiel,and J.Tardos.ORB-SLAM:A Versatile and Accurate Monocular SLAM System.IEEE Transactions on Robotics,vol.31,no.5,pp 1147-1163, 2015.”中提出的ORB-SLAM是目前基于图优化方法的单目视觉SLAM方案，ORB-SLAM通过对图像提取ORB特征描述符(一种提取速度极快的特征描述符)，可以达到很高的速度，并且通过完善的图优化操作，得到的地图也具有很高的精确度。
[0007] 单目视觉传感器的SLAM算法在实际应用中存在的问题有：单目视觉传感器难以估计环境的规模，即尺度；环境的深度是由三角化等数学计算求得，往往带有误差；视觉传感器难以处理光照剧烈，快速移动以及缺乏纹理等场景，会导致姿态估计精度下降；为了消除上述误差，需要引入大规模优化，求解困难且费时。
[0008] 而单独使用其他的传感器如激光雷达传感器会有计算缓慢、信息不够丰富等问题，同样无法很好的解决SLAM任务。
发明内容
[0009] 为了克服现有技术的不足，本发明提供一种多传感器的高精度即时定位与建图方法，即融合视觉传感器与激光雷达数据完成高精度即时定位与建图的方法。在快速的视觉SLAM基础上，通过引入激光雷达的信息，具有更高的定位与建图精度，且通过对激光雷达的数据进行有选择的使用，在保证精确度的情况下，让算法的复杂度较低。
[0010] 一种多传感器的高精度即时定位与建图方法，其特征在于步骤如下：
[0012] 步骤2：特征提取：利用ORB特征提取算法对每一帧灰度图像进行特征提取，得到每一帧图像的ORB特征点；
[0013] 步骤3：特征匹配和运动估计：利用近似最近邻库对相邻两帧图像的特征点进行匹配，对匹配好的点对利用基于随机抽样一致法的PnP求解进行运动估计，得到相机的运动估计序列 其中， 为相机在拍摄第 i帧和第i-1帧图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵， tCam表示相机位移矩阵；
[0014] 步骤4：输入对应激光雷达数据并预处理：输入激光雷达数据，将距离大于激光雷达量程的0.8倍的点去掉，由剩余点构成激光雷达点云数据；这里，假定所述的激光雷达与步骤1中拍摄彩色图像的相机的位置关系固定、已完成联合标定，且二者视野相同、内部参数已知；
[0015] 步骤5：激光雷达点云数据的特征匹配和运动估计：对激光雷达点云数据每隔x帧进行一次特征匹配和运动估计，即将每一帧激光雷达点云数据在水平方向上划分为四个等大区域，每个区域根据光滑度提取4个边缘点和4个平面点构成特征点，根据光滑度对第j帧和第j-x帧数据的特征点进行匹配，对匹配好的点对利用ICP算法进行运动估计，得到激光雷达的运动估计序列
[0016] 其中， 为激光雷达在扫描第j帧和第j-x帧数据之间的运动估计， j＝x+1,2x+1,…,M，M为激光雷达数据总帧数，x取值范围为[5,20]，RLidar表示激光雷达旋转矩阵，tLidar表示激光雷达位移矩阵；
[0017] 所述的边缘点为区域内光滑度最小的点，所述的平面点为区域内光滑度最大的点，光滑度为 其中，S代表第k次扫描中所有的点构成的集合，Lk,p与Lk,q代表p与q点的空间坐标，||·||表示欧式距离；所述的根据光滑度进行匹配是指如果光滑度的差值小于当前点的光滑度的5％，则匹配，否则，不匹配；
[0018] 步骤6：误差修正并记录轨迹：用步骤5得到的激光雷达的运动估计替换对应时间间隔内相机的运动估计之和，得到误差修正后的相机运动估计序列S′，即相机的运动轨迹；
[0019] 步骤7：建立三维点云地图：按照 计算得到三维点云地图pclmap，其中， 为第nj帧图像变换到第一帧图像视角坐标系下的三维点云地图，设第m帧激光雷达点云扫描的同一时刻相机拍摄的图像帧序号为nj，由nj构成的序列为N，m＝x+1,2x+1,…,M； 为第nj帧图像的点云
地图，L为步骤2中提取的第nj帧图像的特征点的个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
地图，L为步骤2中提取的第nj帧图像的特征点的个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
[0020] xl＝(ul-cx)·dl/fx
[0021] yl＝(vl-cy)·dl/fy
[0022] zl＝dl
[0025] 本发明的有益效果是：在快速的视觉SLAM的基础上，结合激光雷达的信息，使 SLAM任务中的定位与建图都有了较高的精度与正确的尺度，同时克服了单激光雷达 SLAM速度较慢、没有颜色信息等缺点；在保证精确度的前提下，算法有选择的使用激光雷达信息，保证了算法的实时性与低复杂度；本算法可以实时输出运动估计结果，并及时修正该结果，同时依靠修正后的结果构建三维地图，这样既保证了算法实用性，又保证了算法在较长时间内有着较高的精度。附图说明
[0026] 图1是本发明的一种多传感器的高精度即时定位与建图方法流程图
具体实施方式
[0027] 下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。
[0028] 本发明提供了一种多传感器的高精度即时定位与建图方法，使用彩色相机与激光雷达对周围环境进行高精度的实时三维构建，并实时测算出拍摄图像的相机的位置与姿态。本发明方法实现的前提是：用于拍摄的相机与激光雷达的位置关系是固定的，即二者在拍摄过程中有着相同的位移与偏转。并且，激光雷达已经完成与相机的联合标定，即图像中的每个像素可以在激光雷达三维点云中获取其深度值，且二者视野相同，内部参数已知。
[0029] 如图1所示，本发明的一种多传感器的高精度即时定位与建图方法，其实现过程如下：
[0030] 步骤1：输入彩色图像并预处理
[0031] 本实施例使用PointGray Flea2彩色相机拍摄彩色图像。图像原始分辨率为 1392×1040，帧率为17fps(帧每秒)，即一秒钟输入17帧的图像序列。首先，对图像进行预处理，即将彩色图像转化为灰度图像以进行后续运算。
[0032] 步骤2：特征提取
[0033] 对于上一步骤输入的灰度图像，首先逐帧提取ORB(Oriented FAST and Rotated BRIEF)特征点。此处的特征点也可以根据需求更换为其他类型的特征点，如SIFT、 SURF等，但由于这些特征点的提取较为耗时，算法整体的运行速度可能会受到影响。
[0034] ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点提取和描述的算法。这个算法是由Rublee等学者在2011年提出，“Rublee Ethan,et al."ORB:An efficient alternative to SIFT or SURF."IEEE International Conference on Computer Vision IEEE, 2012:2564-2571.”。ORB算法分为两部分，分别是特征点提取和特征点描述。特征提取是由FAST(Features from Accelerated Segment Test)算法发展来的，特征点描述是根据BRIEF(Binary Robust Independent Elementary Features)特征描述算法改进的。 ORB特征是将FAST特征点的检测方法与BRIEF特征描述子结合起来，并在它们原来的基础上做了改进与优化。经实验表明ORB特征的提取速度约为SIFT的100倍，是SURF的10倍。
[0035] 本实施例直接使用计算机视觉库OPENCV中的ORB特征描述符提取函数对每一帧图像进行特征提取。
[0036] 步骤3：特征匹配和运动估计
[0037] 对提取特征点后相邻的两帧图像，使用近似最近邻库(Fast library for approximate nearest neighbors，FLANN)进行特征匹配。FLANN库是目前最完整的近似最近邻匹配开源库，其不但实现了一系列查找算法，还包含了一种自动选取最快算法的机制。 FLANN的基础算法由Muja等人在2009年提出，“M.Muja and D.Lowe,"Fast Approximate Nearest Neighbors with Automatic Algorithm Configuration",in International Conference on Computer Vision Theory and Applications(VISAPP'09),2009”。
[0038] 对新输入的每一帧图像提取完特征点后，均使用计算机视觉库OPENCV中的 FLANNbasedMatcher函数与上一帧图像进行特征匹配，得到两帧图像中若干对对应的特征点；然后对匹配结果基于随机抽样一致法(RANdom SAmple Consensus， RANSAC)的PnP求解对相机拍摄两帧图像之间的姿态变化进行估计。RANSAC是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。由Fischler和Bolles于1981年最先提出，“Fischler,M.A.and Bolles,R.C.Random Sample Consensus:A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartography.Communications of the ACM,24(6):381–395,1981.”。PnP求解的目的是利用已知匹配好的平面点对确定摄像头相对世界坐标系的位移和旋转，由 Moreno-Noguer在2007年提出，“F.Moreno-Noguer,V.Lepetit and P.Fua,Accurate Non-Iterative O(n)Solution to the PnP Problem,IEEE International Conference on Computer Vision,Rio de Janeiro,Brazil,October 2007.”。本实施例直接使用计算机视觉库OPENCV中的solve PnPRansac函数来进行运算。
[0039] 通过此步骤可以得到任意相邻两帧图像之间相机关于世界坐标系(第一帧图像的视角为主方向的坐标系)的运动估计(RCam,tCam)和相机的运动估计序列其中， 为相机在拍摄第i帧和第i-1帧
图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵，tCam表示相机位移矩阵。
图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵，tCam表示相机位移矩阵。
[0040] 由于特征点提取与匹配过程中有大量噪声存在，此步骤得到的姿态变化仍有一定误差，将利用步骤6进行修正。
[0041] 步骤4：输入对应激光雷达数据并预处理
[0042] 本实施例使用美国Velodyne公司生产的HDL-64E激光雷达，是一种小型多线全景激光雷达，拥有纵向26.9°，横向360°的视野。每秒钟会得到均匀分布在64条环境扫描线上的220000个点，来表示周围空间点距离激光雷达中心点的距离与位置关系。有效测量范围为120m。
[0043] 实际使用中，为减小误差，将超出有效测量范围的0.8倍的点去掉，即去除测量距离大于80m的空间点，随后导入余下的所有点构成点云数据。
[0044] 步骤5：激光雷达点云数据的特征匹配和运动估计
[0045] 在本步骤中，本发明将根据输入的激光雷达点云数据，每隔x帧，提取特征并进行一次运动估计，以降低算法复杂度、提高计算效率，这样，可以得到激光雷达的运动估计序列 其中， 为激光雷达在扫描第j帧和第j-x帧数据之间的运动估计，j＝x+1,2x+1,…,M，M为激光雷达数据帧数， x取值范围为[5,20]，本实施例中x取值为5，RLidar表示激光雷达旋转矩阵，tLidar表示激光雷达位移矩阵。具体为：
[0046] 由于点云是空间中离散的三维点，除了坐标之外没有其他信息，而同一个空间点坐标在移动的数据采集过程中是不断变化的，所以需在点云中定义可追踪的特征，为此定义一种基于光滑度的特征提取方式，即定义光滑度c为：
[0047]
[0048] 其中，S代表第k次扫描中所有的点构成的集合，Lk,p与Lk,q代表p与q点的空间坐标， ||·||表示欧氏距离。
[0049] 对于S中所有的点进行光滑度的计算，光滑度最大的点定义为平面点，光滑度最小的点则定义为边缘点。为了避免产生的特征点过多，首先将每次扫描的点云划分为四个等大区域，如取0°、90°、180°与270°四个主方向，每个方向左右各取±45°划分为一个区域。每个区域最多有4个边缘点和4个平面点构成特征点。
[0050] 得到特征点后，根据光滑度对第j帧和第j-x帧数据的特征点进行匹配，光滑度的差值小于当前点的光滑度的5％的特征点点对认为是同一个空间点在两次扫描中的表达，即为匹配的点对。得到匹配的特征点后利用ICP算法(Iterative Closest Point)进行运动估计，即通过求解两组三维空间点得到激光雷达的运动估计(RLidar,tLidar)。ICP算法由 Zhang等人提出，“Zhang,Zhengyou(1994)."Iterative point matching for registration of free-form curves and surfaces".International Journal of Computer Vision.Springer.13(12): 119–152.”
[0051] 步骤6：误差修正并记录轨迹
[0052] 由于激光雷达点云数据精度高，数据可靠，在定位的结果上的误差极小，但是运算速度较慢；而基于彩色图像的算法在运算速度上较快，但精度较差。本发明针对两种传感器的姿态追踪的不同特性，采用激光雷达运动估计替换相机运动估计之和的方式来进行误差修正，提升整体算法的精确度，降低算法的整体时间复杂度。
[0053] 即用步骤5得到的激光雷达的运动估计(扫描第j帧和第j-x帧数据之间的一次运动估计)替换对应时间间隔(扫描第j帧和第j-x帧数据的时间间隔)内相机的运动估计之和，得到误差修正后的相机运动估计序列，也就是相机的运动轨迹。
[0054] 步骤7：建立三维点云地图
[0055] 在最终建立高精度三维点云地图时，只使用误差较小的激光雷达的运动估计 (RLidar,tLidar)。具体为：
[0056] 按照 计算得到三维点云地图pclmap，其中为第m帧激光雷达点云扫描的同一时刻相机拍摄的第nj帧图像(即设激光雷达扫描第m 帧点云同一时刻相机拍摄的图像帧序号为nj，由nj构成的序列为N)变换到第一帧图像视角坐标系下的三维点云地图， 为第nj帧图像的点云
地图，L为步骤2中第nj帧图像提取得到的特征点个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
地图，L为步骤2中第nj帧图像提取得到的特征点个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
[0057] xl＝(ul-cx)·dl/fx
[0058] yl＝(vl-cy)·dl/fy
[0059] zl＝dl
[0060] 其中，l＝1,…,L，(ul, vl)为第l个特征点的图像坐标，(fx,fy,cx,cy)为给定的相机参数，dl为第l个特征点在图像拍摄时刻对应的激光雷达点云中检索到的深度值。
[0061] 可以看出，这样构建的点云地图由多次扫描叠加而来，整体过于稠密。为了提高显示速度，降低点云密集程度，在点云的存储上设置空间点密度阈值，即当空间一立方米中的空间点个数超过10000个时，对点云数据进行降采样以保证空间点的数目在合理的范围内，得到最终的点云地图。当然，在扫描狭窄的空间时，空间点密度阈值可以相应的提高。
[0062] 本实施例是在中央处理器为 i5-4590 3.4GHz CPU、内存16G、Ubuntu14.04 操作系统的电脑上进行的实验，实验使用第三方数据集在城市环境下相机运动轨迹以证明精确程度。
[0063] 实验使用的序列13来自于KITTI数据集，该数据库由A.Geiger等人在文献“A. Geiger and Philip Lenz,Vision meets Robotics:The KITTI Dataset,International Journal of Robotics Research,2013”中提出，由PointGray Flea2 color cameras相机拍摄彩色图像，由Velodyne HDL-64E激光雷达采集点云数据。
[0064] 采用相机运动轨迹与真实值的偏移距离占地图大小的比例(％)及算法耗时为性能参数，将本发明方法与单目视觉中主流的ORB-SLAM算法进行对比，结果如表1所列。可以看出，本发明方法在引入激光雷达信息后，不但提高了整体的运算速度，而且在精度上超越了主流视觉SLAM方法。本发明还可以应用到自动驾驶、机器人自主导航等方面。
[0065] 表1算法性能比较
[0066]方法 耗时(s) 偏移距离(％)
本发明方法 120.3 1.8
ORB-SLAM 126.0 3.1
本发明方法 120.3 1.8
ORB-SLAM 126.0 3.1
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种行人导航定位中的自适应零速修正方法 | 2020-05-08 | 121 |
| 一种测量位置的方法及装置 | 2020-05-12 | 290 |
| 一种激光熔覆离焦量在线监测与负反馈状态识别方法 | 2020-05-16 | 474 |
| 一种三驱动机械臂的视觉跟随系统 | 2020-05-11 | 540 |
| 一种基于姿态的考试作弊检测方法 | 2020-05-14 | 717 |
| MRI中的运动的实时监测和预测 | 2020-05-15 | 666 |
| 一种基于深度神经网络的运动预测方法和智能终端 | 2020-05-08 | 1012 |
| 一种基于双流卷积神经网络和SVM的抽烟动作识别方法 | 2020-05-12 | 767 |
| 基于计算机视觉的电梯轿厢内异常行为的检测方法 | 2020-05-15 | 503 |
| 基于视觉SLAM和网络地图结合的机器人城市导航系统 | 2020-05-16 | 226 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
分析报告
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
QQ群二维码
意见反馈
意见反馈