CN112396656A - 一种视觉与激光雷达融合的室外移动机器人位姿估计方法 - Google Patents

一种视觉与激光雷达融合的室外移动机器人位姿估计方法 Download PDF

Info

Publication number
CN112396656A
CN112396656A CN202011329017.3A CN202011329017A CN112396656A CN 112396656 A CN112396656 A CN 112396656A CN 202011329017 A CN202011329017 A CN 202011329017A CN 112396656 A CN112396656 A CN 112396656A
Authority
CN
China
Prior art keywords
point
points
ground
frame
pose
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011329017.3A
Other languages
English (en)
Other versions
CN112396656B (zh
Inventor
何炳蔚
刘宸希
朱富伟
张立伟
林立雄
陈彦杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fuzhou University
Original Assignee
Fuzhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fuzhou University filed Critical Fuzhou University
Priority to CN202011329017.3A priority Critical patent/CN112396656B/zh
Publication of CN112396656A publication Critical patent/CN112396656A/zh
Application granted granted Critical
Publication of CN112396656B publication Critical patent/CN112396656B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical methods
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明涉及一种视觉与激光雷达融合的室外移动机器人位姿估计方法，包括以下步骤：步骤S1:获取点云数据及视觉图像数据；步骤S2:采用迭代拟合的算法对地面模型进行准确估计并提取地面点；步骤S3:对视觉图像下半区域提取ORB特征点，并根据地面点为视觉特征点估计深度；步骤S4:获取由点云的深度信息构成的深度图像；步骤S5:提取边缘特征、平面特征及地面特征；步骤S6:利用汉明距离及RANSAC算法对视觉特征进行匹配并采用迭代最近点法初步计算移动机器人的相对位姿；步骤S7:根据视觉得到的相对位姿、地面点云提供的点面约束、法向量约束和非地面点云提供的点线、点面约束，得到机器人的最终位姿。本发明实现移动机器人在室外环境中精度更高、鲁棒性更高的位姿估计。

Description

一种视觉与激光雷达融合的室外移动机器人位姿估计方法
技术领域
本发明涉及移动机器人的自主导航领域，，具体涉及一种视觉与激光雷达融合的室外移动机器人位姿估计方法。
背景技术
近年来，以自主导航技术为核心的移动机器人在许多领域均展现出巨大的发展前景，并已经在生活中的各个场景下开始广泛应用，例如在室内环境下工作的家用扫地机器人，服务机器人，仓储UGV等；在室外场景下的农业调查勘测，无人物流运输，电力、安全巡检等作业。
定位模块作为整个机器人导航系统的首要模块，也是保证机器人实现导航任务的基础模块。定位模块提供了移动机器人的实时位置信息，解决了移动机器人“在哪”的问题，高精度的定位技术为后续其它的技术模块提供有力的支撑。在室外场景中常使用GPS来进行定位，但是GPS信号在遇到各种遮挡时无法提供准确的信息，且问题常有发生。因此，基于激光雷达、视觉、惯性等传感器的同时定位与建图(SLAM)技术成为研究热点。SLAM技术使机器人在没有GPS的情况下仍实现高精度的位姿估计。但是由于室外复杂环境以及传感器自身缺陷带来的挑战，基于视觉的方法受到光照影响较大，且特征深度难以得知；基于激光雷达的方案在一些雷达退化场景下难以提取足够的特征。以上问题都对定位方法的精度产生影响，严重时导致方法失效。因此，为了解决上述存在的问题，有必要设计一种通过融合视觉与激光雷达两种传感信息，有效应对室外复杂环境的高精度，高鲁棒性的移动机器人位姿估计方法。
发明内容
有鉴于此，本发明的目的在于提供一种视觉与激光雷达融合的室外移动机器人位姿估计方法，克服仅基于单一传感器方法的不足，实现移动机器人在室外环境中精度更高、鲁棒性更高的位姿估计。
为实现上述目的，本发明采用如下技术方案：
一种视觉与激光雷达融合的室外移动机器人位姿估计方法，包括以下步骤：
步骤S1:获取点云数据及视觉图像数据；
步骤S2:采用迭代拟合的算法对每一帧的点云的地面模型进行准确估计并提取地面点；
步骤S3:对视觉图像下半区域提取ORB特征点，并根据提取得到的地面点为相应视觉特征点估计深度；
步骤S4:根据激光雷达的线数及角分辨率，获取由点云的深度信息构成的深度图像；
步骤S5:根据得到的深度图像，通过提取每个点相邻位置多个点的深度信息，计算其粗糙度，并根据粗糙度阈值从整帧点云提取边缘特征、平面特征及地面特征；
步骤S6：利用汉明距离及RANSAC算法对视觉特征进行匹配并采用迭代最近点法初步计算移动机器人的相对位姿；
步骤S7：将相对位姿作为初始值，将地面点云提供的点面约束、法向量约束和非地面点云提供的点线、点面约束加入到优化方程中，得到机器人的最终位姿。
进一步的，所述步骤S1具体为：根据移动机器人搭载一台集激光雷达与多个摄像头于一身的Pandora套件，获取点云数据及视觉图像数据。
进一步的，所述步骤S2具体为：
步骤S21：设传感器的坐标系为X轴向前，Y轴向左，Z轴向上，移动机器人所处位置身边小范围内必存在地面点，这些点处于激光雷达较低的扫描线上；选取最低两条线上的地面点，记为
Figure BDA0002795198110000031
步骤S22：对
Figure BDA0002795198110000032
中所有点按照Z轴坐标值从小到大排序，记排序后的
Figure BDA0002795198110000033
为
Figure BDA0002795198110000034
对
Figure BDA0002795198110000035
按照下式求取均值：
Figure BDA0002795198110000036
其中，Pf为均值点；Nf为计算均值需要从Psort选取的点的数量；
步骤S23：遍历整帧点云
Figure BDA0002795198110000037
通过下式提取拟合地平面模型的初始点集
Figure BDA0002795198110000038
Pf.Z-Thz＜Pi.Z＜Pf.Z+Thz (2)
其中，
Figure BDA0002795198110000039
Thz为Z轴阈值；Pf.Z、Pi.Z代表该点在激光雷达坐标系下Z轴的坐标值；
步骤S24：对初始点集Ps做主成分分析，得到地平面模型M如下式：
Ax+By+Cz+D＝0 (3)
其中，(A，B，C)T为地面模型的法向量，D为截距，A，B，C，D为地平面模型M的四个参数；
步骤S25：遍历整帧点云
Figure BDA0002795198110000041
将点带入地平面模型M，并与预先设定的阈值作比较，筛选出地面点
Figure BDA0002795198110000042
筛选的条件为：
|Axi+Byi+Czi+D|＜Thd (4)
其中，
Figure BDA0002795198110000043
Thd为距离阈值；
步骤S26：利用筛选出的地面点
Figure BDA0002795198110000044
重复步骤S24与S25，重新估计地平面的模型并筛选新的地面点；经过N次迭代，得到最终的地面点。
进一步的，所述步骤S3具体为：
步骤S31：对图像下半区域提取ORB特征，记为Fv；
步骤S32：将点云中的地面点
Figure BDA0002795198110000045
投影至图像上；
步骤S33：对于图像上的每一个提取到的特征点Fvi∈Fv，选取其周围矩形小范围内的
Figure BDA0002795198110000046
投影点来估计其周围的平面参数；若周围没有足够数量的投影点，将其视为无效点并放弃；对于每个有效特征点按照下式计算其深度：
Figure BDA0002795198110000047
其中，(ui，vi)T∈Fv，为有效特征点的像素坐标；K为相机内参数；si为对应特征点的深度；
Figure BDA0002795198110000051
和d为点云估计的局部平面参数，
Figure BDA0002795198110000052
为法向量，d为截距。
进一步的，所述步骤S4具体为：根据所使用的激光雷达的线数M及角分辨率a，将整帧点云投影至一个M×(360/a)的二维图像中，每个像素存储该点对应的深度信息，计算公式如下：
Figure BDA0002795198110000053
其中，Depi为第i个点的深度，
Figure BDA0002795198110000054
为第i个点在激光雷达坐标系下的坐标值。
进一步的，所述步骤S5具体为：
步骤S51：对点云中的每个点，选取其在深度图像所在位置同一行的前后各若干个连续的点组成点集S，计算该点的粗糙度：
Ci＝[∑j∈S(Depj-Depi)]2 (7)
其中，Ci为第i个点的粗糙度，Depj为点集S中第j个点的深度信息；
步骤S52：对于非地面点，设定阈值Cthp和Cthe区分不同类型的特征点；将粗糙度小于Cthp的点归为平面特征FP，将粗糙度大于Cthe的点归为边缘特征FE；设FP与FE的数量分别为NP和NE；从平面特征FP中选择nP个粗糙度最小的特征F′P，从边缘特征FE中选择nE个粗糙度最大的特征F′E，作为待匹配特征点；
其中，F′P∈FP，F′E∈FE，且nP＜NP，nE＜NE；
步骤S53：对于地面点，直接选出nv个粗糙度最小的地面特征点F′G。
进一步的，所述步骤S6具体为：
步骤S61：对由步骤S3获取的带有深度信息的有效特征记为F′v，采用汉明距离对待求解相对位姿的第j帧和第k帧的特征点进行匹配，并采用RANSAC去除误匹配，得到一组配对好的3D点，表示为下式：
Figure BDA0002795198110000061
其中，Pj与Pk即为两帧之间一一匹配好的特征点集；
步骤S62：通过构建最小二乘问题，求使得匹配点投影误差的平方和达到最小的相对位姿T(R，t)，公式如下：
Figure BDA0002795198110000062
其中，
Figure BDA0002795198110000063
R和t表示第j帧和第k帧之间的旋转与平移变换；
步骤S63：计算两组对应点的质心及去质心坐标：
Figure BDA0002795198110000064
Figure BDA0002795198110000065
其中，qj与qk分别为Pj和Pk的质心，
Figure BDA0002795198110000066
与
Figure BDA0002795198110000067
分别为Pj和Pk的去质心坐标；
步骤S64：定义一个3×3的矩阵
Figure BDA0002795198110000068
对W进行SVD分解，得到：W＝U∑VT；
其中U和V为3×3的单位正交矩阵，∑为对角矩阵，求解出相对旋转变换R＝UVT；
步骤S65：根据所求解的R，平移变换t的求解公式为t＝qj-R·qk。
进一步的，所述步骤S7具体为：
步骤S71：对于第k帧中的地面特征
Figure BDA0002795198110000071
与平面特征
Figure BDA0002795198110000072
通过kd-tree在第j帧中的地面点和平面特征
Figure BDA0002795198110000073
寻找最近点，同时提取最近点同线和相邻线的两个平面点，由三个点组成平面；通过相对位姿变换，计算特征点到平面的距离作为点面约束ConsPP如下式：
Figure BDA0002795198110000074
其中，
Figure BDA0002795198110000075
为第j帧中寻找到的最近点，
Figure BDA0002795198110000076
为与
Figure BDA0002795198110000077
同线上最近的平面点，
Figure BDA0002795198110000078
为与
Figure BDA0002795198110000079
相邻线上最近的平面点；
Figure BDA00027951981100000710
为第k帧中特征点
Figure BDA00027951981100000711
在变换到第j帧的坐标；
步骤S72：对于第k帧中的边缘特征
Figure BDA00027951981100000712
通过kd-tree在第j帧中的边缘特征
Figure BDA00027951981100000713
寻找最近点，同时提取最近点相邻线的边缘点，由两个点确定一条直线；通过相对位姿变换，计算特征点到直线的距离作为点线约束ConsPL如下式：
Figure BDA00027951981100000714
其中，
Figure BDA0002795198110000081
为第j帧中寻找到的最近点，
Figure BDA0002795198110000082
为与
Figure BDA0002795198110000083
相邻线上最近的边缘点。
Figure BDA0002795198110000084
为第k帧中特征点
Figure BDA0002795198110000085
在变换到第j帧的坐标，即
Figure BDA0002795198110000086
Figure BDA0002795198110000087
步骤S73：由地面点云提供的法向量约束Consnor，方程如下式：
Figure BDA0002795198110000088
其中，
Figure BDA0002795198110000089
与
Figure BDA00027951981100000810
分别为第j帧和第k帧的地面点云的法向量，由提取到的地面特征F′G计算得到；
步骤S74：用欧拉角表示旋转变换R，则整个相对位姿变换T(R，t)可以表示为一个6×1的向量，即(θroll，θpitch，θyaw，tx，ty，tZ)T；
地面特征对于θroll，θpitch，tz有更好的约束，对整个优化采用分步求解的策略，以视觉特征提供的相对位姿作为初值，加入地面特征提供的点面约束ConsPP和法向量约束Consnor，采用Levenberg-Marquardt(L-M)算法迭代求解位姿，记为T1；
步骤S75：以T1作为初值，加入非地面点提供的点面约束ConsPP和点线约束ConsPL，此外，取T1中的θroll，θpitch，tz值同时作为约束，再次采用L-M算法迭代求解得到最终位姿Tfinal。
本发明与现有技术相比具有以下有益效果：
1.本发明采用了视觉传感器与激光雷达多源信息融合的方式，利用点云信息为视觉特征估计深度，利用视觉特征与点云特征共同对机器人位姿进行估计，克服了仅基于单一传感器方法的不足，能够在某个传感器退化的极端条件下仍能为机器人提供鲁棒的位姿估计；
2.本发明提取了来自激光点云中的地面特征、非地面点中的平面特征与边缘特征以及补充的视觉ORB特征；多种类型的特征保证对三个坐标轴方向均能够产生足够的约束，从而实现更精确的位姿估计；
3.本发明在帧间匹配阶段还考虑了来自地平面的法向量约束，更充分地利用了室外场景的特性，实现对角度的更准确地估计。
附图说明
图1是本发明的方法流程图；
图2是本发明采用的激光雷达(Pandora)设备实物图；
图3是本发明的一实施例中提取的地面点云示意图；
图4是本发明的一实施例中Pandora的点云投影效果图；
图5是本发明的一实施例中提取的ORB特征示意图；
图6是本发明的一实施例中提取的平面与边缘特征示意图；
图7是本发明的一实施例中在“P”型环境的位姿对比图；
图8是本发明的一实施例中在“Z”型环境的位姿对比图；
图9是本发明的一实施例中在KITTI数据集00场景的测试结果图。
具体实施方式
下面结合附图及实施例对本发明做进一步说明。
请参照图1，本发明提供一种视觉与激光雷达融合的室外移动机器人位姿估计方法，包括以下步骤：
步骤S1:获取点云数据及视觉图像数据；
步骤S2:采用迭代拟合的算法对每一帧的点云的地面模型进行准确估计并提取地面点；
步骤S3:对视觉图像下半区域提取ORB特征点，并根据提取得到的地面点为相应视觉特征点估计深度；
步骤S4:根据激光雷达的线数及角分辨率，获取由点云的深度信息构成的深度图像；
步骤S5:根据得到的深度图像，通过提取每个点相邻位置多个点的深度信息，计算其粗糙度，并根据粗糙度阈值从整帧点云提取边缘特征、平面特征及地面特征；
步骤S6:利用汉明距离及RANSAC算法对视觉特征进行匹配并采用迭代最近点法初步计算移动机器人的相对位姿；
步骤S7:将相对位姿作为初始值，将地面点云提供的点面约束、法向量约束和非地面点云提供的点线、点面约束加入到优化方程中，得到机器人的最终位姿。
在本实施例中，选用一辆Robotnik的Guardian作为移动机器人底盘，移动速度为3m/s；并搭载一台集激光雷达与多个摄像头于一身的Pandora套件，用于以10hz的频率采集点云与图像数据，如图2所示。其中图像分辨率为1280×720，激光雷达为40线，角分辨率为0.2°；此外，使用一台具有i7 CPU以及16Gb内存的笔记本电脑来运行整个系统，运行环境为Linux系统的Ubuntu 18.04版本且基于ROS(Robot Operating System)系统。
在本实施例中，地面提取是点云分割的第一步，良好的地面点提取对后续的点云处理有促进作用，具体如下：
步骤S21：设传感器的坐标系为X轴向前，Y轴向左，Z轴向上，移动机器人所处位置身边小范围内必存在地面点，这些点处于激光雷达较低的扫描线上；选取最低两条线上的地面点，记为
Figure BDA0002795198110000111
步骤S22：对
Figure BDA0002795198110000112
中所有点按照Z轴坐标值从小到大排序，记排序后的
Figure BDA0002795198110000113
为
Figure BDA0002795198110000114
对
Figure BDA0002795198110000115
按照下式求取均值：
Figure BDA0002795198110000116
其中，Pf为均值点；Nf为计算均值需要从Psort选取的点的数量；优选的Nf以约
Figure BDA0002795198110000117
数量的四分之一为宜，本实施例中Nf取800。
步骤S23：遍历整帧点云
Figure BDA0002795198110000118
通过下式提取拟合地平面模型的初始点集
Figure BDA0002795198110000119
Pf.Z-Thz＜Pi.Z＜Pf.Z+Thz (2)
其中，
Figure BDA00027951981100001110
Thz为Z轴阈值；Pf.Z、Pi.Z代表该点在激光雷达坐标系下Z轴的坐标值；
步骤S24：对初始点集Ps做主成分分析，得到地平面模型M如下式：
Ax+By+Cz+D＝0 (3)
其中，(A，B，C)T为地面模型的法向量，D为截距，A，B，C，D为地平面模型M的四个参数；
步骤S25：遍历整帧点云
Figure BDA00027951981100001111
将点带入地平面模型M，并与预先设定的阈值作比较，筛选出地面点
Figure BDA00027951981100001112
筛选的条件为：
|Axi+Byi+Czi+D|＜Thd (4)
其中，
Figure BDA0002795198110000121
Thd为距离阈值；
步骤S26：利用筛选出的地面点
Figure BDA0002795198110000122
重复步骤S24与S25，重新估计地平面的模型并筛选新的地面点；经过N次迭代，得到最终的地面点。本实施例中N取2，如图3所示，蓝色点为一帧扫描到的全部点云，其中红色点即为本实施例中最终得到的地面点。
在本实施例中，如图4所示，Pandora组合套件已完成图像与激光雷达的外参标定，图为点云投影至图像的效果验证外参的效果。所述步骤S3对ORB特征的深度估计具体为：
图5所示为两个不同场景下提取ORB特征点的图像。通过对图像下半区域提取ORB特征，记为Fv。将点云中的地面点
Figure BDA0002795198110000123
投影至图像上。对于图像上的每一个提取到的特征点Fvi∈Fv，选取其周围矩形小范围内的
Figure BDA0002795198110000124
投影点来估计其周围的平面参数。若周围没有足够数量的投影点，将其视为无效点并放弃。对于每个有效特征点按照下式计算其深度：
Figure BDA0002795198110000125
其中，(ui，vi)T∈Fv，为有效特征点的像素坐标；K为相机内参数；si为对应特征点的深度；
Figure BDA0002795198110000126
和d为点云估计的局部平面参数，
Figure BDA0002795198110000127
为法向量，d为截距。
在本实施例中，所述步骤S4具体为：根据所使用的激光雷达的线数M及角分辨率a，两者的取值分别为M＝40，a＝0.2°。将整帧点云投影至一个40×1800的二维图像中，每个像素存储该点对应的深度信息，计算公式如下：
Figure BDA0002795198110000131
其中，Depi为第i个点的深度，
Figure BDA0002795198110000132
为第i个点在激光雷达坐标系下的坐标值。
在本实施例中，所述步骤S5具体为：
步骤S51：对点云中的每个点，选取其在深度图像所在位置同一行的前后各5个连续的点组成点集S，计算该点的粗糙度：
Ci＝[∑j∈S(Depj-Depi)]2 (7)
其中，Ci为第i个点的粗糙度，Depj为点集S中第j个点的深度信息；
步骤S52：对于非地面点，设定阈值Cthp和Cthe区分不同类型的特征点；将粗糙度小于Cthp的点归为平面特征FP，将粗糙度大于Cthe的点归为边缘特征FE；设FP与FE的数量分别为NP和NE；从平面特征FP中选择nP个粗糙度最小的特征F′P，从边缘特征FE中选择nE个粗糙度最大的特征F′E，作为待匹配特征点；
其中，F′P∈FP，F′E∈FE，且nP＜NP，nE＜NE；
步骤S53：对于地面点，直接选出nv个粗糙度最小的地面特征点F′G。
在本实施例中，对视觉特征进行匹配来估计位姿初值，所述步骤S6具体为：
步骤S61：对由步骤S3获取的带有深度信息的有效特征记为F′v，采用汉明距离对待求解相对位姿的第j帧和第k帧的特征点进行匹配，并采用RANSAC去除误匹配，得到一组配对好的3D点，表示为下式：
Figure BDA0002795198110000141
其中，Pj与Pk即为两帧之间一一匹配好的特征点集；如图6所示，粉红色点为点云中提取到的平面特征F′P，绿色点为点云中提取到的边缘特征F′E。
步骤S62：通过构建最小二乘问题，求使得匹配点投影误差的平方和达到最小的相对位姿T(R，t)，公式如下：
Figure BDA0002795198110000142
其中，
Figure BDA0002795198110000143
R和t表示第j帧和第k帧之间的旋转与平移变换；
步骤S63：计算两组对应点的质心及去质心坐标：
Figure BDA0002795198110000144
Figure BDA0002795198110000145
其中，qj与qk分别为Pj和Pk的质心，
Figure BDA0002795198110000146
与
Figure BDA0002795198110000147
分别为Pj和Pk的去质心坐标；
步骤S64：定义一个3×3的矩阵
Figure BDA0002795198110000148
对W进行SVD分解，得到：W＝U∑VT；
其中U和V为3×3的单位正交矩阵，∑为对角矩阵，求解出相对旋转变换R＝UVT；
步骤S65：根据所求解的R，平移变换t的求解公式为t＝qj-R·qk。
在本实施例中，采用点面约束、点线约束、法向量约束以及分步的位姿求解方法，所述步骤S7具体为：
步骤S71：对于第k帧中的地面特征
Figure BDA0002795198110000151
与平面特征
Figure BDA0002795198110000152
通过kd-tree在第j帧中的地面点和平面特征
Figure BDA0002795198110000153
寻找最近点，同时提取最近点同线和相邻线的两个平面点，由三个点组成平面；通过相对位姿变换，计算特征点到平面的距离作为点面约束ConsPP如下式：
Figure BDA0002795198110000154
其中，
Figure BDA0002795198110000155
为第j帧中寻找到的最近点，
Figure BDA0002795198110000156
为与
Figure BDA0002795198110000157
同线上最近的平面点，
Figure BDA0002795198110000158
为与
Figure BDA0002795198110000159
相邻线上最近的平面点；
Figure BDA00027951981100001510
为第k帧中特征点
Figure BDA00027951981100001511
在变换到第j帧的坐标；
步骤S72：对于第k帧中的边缘特征
Figure BDA00027951981100001512
通过kd-tree在第j帧中的边缘特征
Figure BDA00027951981100001513
寻找最近点，同时提取最近点相邻线的边缘点，由两个点确定一条直线；通过相对位姿变换，计算特征点到直线的距离作为点线约束ConsPL如下式：
Figure BDA00027951981100001514
其中，
Figure BDA00027951981100001515
为第j帧中寻找到的最近点，
Figure BDA00027951981100001516
为与
Figure BDA00027951981100001517
相邻线上最近的边缘点。
Figure BDA00027951981100001518
为第k帧中特征点
Figure BDA00027951981100001519
在变换到第j帧的坐标，即
Figure BDA00027951981100001520
Figure BDA00027951981100001521
步骤S73：由地面点云提供的法向量约束Consnor，方程如下式：
Figure BDA0002795198110000161
其中，
Figure BDA0002795198110000162
与
Figure BDA0002795198110000163
分别为第j帧和第k帧的地面点云的法向量，由提取到的地面特征F′G计算得到；
步骤S74：用欧拉角表示旋转变换R，则整个相对位姿变换T(R，t)可以表示为一个6×1的向量，即(θroll，θpitch，θyaw，tx，ty，tz)T；
地面特征对于θroll，θpitch，tz有更好的约束，对整个优化采用分步求解的策略，以视觉特征提供的相对位姿作为初值，加入地面特征提供的点面约束ConsPP和法向量约束Consnor，采用Levenberg-Marquardt(L-M)算法迭代求解位姿，记为T1；
步骤S75：以T1作为初值，加入非地面点提供的点面约束ConsPP和点线约束ConsPL，此外，取T1中的θroll，θpitch，tz值同时作为约束，再次采用L-M算法迭代求解得到最终位姿Tfinal。
在本实施例中，如表1所示，为各个模块的平均运行时间。
表1：各个模块平均运行时间
Figure BDA0002795198110000164
如图7所示，为本方法与仅基于激光雷达方法在学校实际环境中的“P”型路段分别对机器人进行位姿估计的轨迹图对比，在没有回环检测的情况下，本方法实现了更高的精度。
如图8所示，在学校“Z”型路段中，由于环境更加复杂，仅基于雷达的机器人位姿估计方法(绿色轨迹)在第二个转弯处出现了严重漂移；本方法仍能准确进行位姿估计(蓝色轨迹)。
除了在学校的实际环境中进行实验，本方法在KITTI数据集中同样进行了测试，并与目前著名的雷达里程计方法LOAM进行对比。如图9所示，本方法(蓝色)在数据集上表现依旧远远优于仅基于雷达的方法LOAM(绿色)。
如表2所示，为本方法与仅基于激光雷达方法在实际“P”型与“Z”型场景中的相对位移误差。
表2：相对位置误差分析
Figure BDA0002795198110000171
以上轨迹估计对比及误差分析均能说明本方法相较于仅基于单一传感器的方法在应对室外复杂环境的时候对移动机器人的位姿估计有更好的效果和更高的精度，且充分说明本方法的合理性和有效性。
以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (8)

1.一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，包括以下步骤：
步骤S1：获取点云数据及视觉图像数据；
步骤S2：采用迭代拟合的算法对每一帧的点云的地面模型进行准确估计并提取地面点；
步骤S3：对视觉图像下半区域提取ORB特征点，并根据提取得到的地面点为相应视觉特征点估计深度；
步骤S4：根据激光雷达的线数及角分辨率，获取由点云的深度信息构成的深度图像；
步骤S5：根据得到的深度图像，通过提取每个点相邻位置多个点的深度信息，计算其粗糙度，并根据粗糙度阈值从整帧点云提取边缘特征、平面特征及地面特征；
步骤S6：利用汉明距离及RANSAC算法对视觉特征进行匹配并采用迭代最近点法初步计算移动机器人的相对位姿；
步骤S7：将相对位姿作为初始值，将地面点云提供的点面约束、法向量约束和非地面点云提供的点线、点面约束加入到优化方程中，得到机器人的最终位姿。
2.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S1具体为：根据移动机器人搭载一台集激光雷达与多个摄像头于一身的Pandora套件，获取点云数据及视觉图像数据。
3.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S2具体为：
步骤S21：设传感器的坐标系为X轴向前，Y轴向左，Z轴向上，移动机器人所处位置身边小范围内必存在地面点，这些点处于激光雷达较低的扫描线上；选取最低两条线上的地面点，记为
Figure FDA0002795198100000021
步骤S22：对
Figure FDA0002795198100000022
中所有点按照Z轴坐标值从小到大排序，记排序后的
Figure FDA0002795198100000023
为
Figure FDA0002795198100000024
对
Figure FDA0002795198100000025
按照下式求取均值：
Figure FDA0002795198100000026
其中，Pf为均值点；Nf为计算均值需要从Psort选取的点的数量；
步骤S23：遍历整帧点云
Figure FDA0002795198100000027
通过下式提取拟合地平面模型的初始点集
Figure FDA0002795198100000028
Pf.Z-Thz＜Pi.Z＜Pf.Z+Thz (2)
其中，
Figure FDA0002795198100000029
Thz为Z轴阈值；Pf.Z、Pi.Z代表该点在激光雷达坐标系下Z轴的坐标值；
步骤S24：对初始点集Ps做主成分分析，得到地平面模型M如下式：
Ax+By+Cz+D＝0 (3)
其中，(A，B，C)T为地面模型的法向量，D为截距，A，B，C，D为地平面模型M的四个参数；
步骤S25：遍历整帧点云
Figure FDA00027951981000000210
将点带入地平面模型M，并与预先设定的阈值作比较，筛选出地面点
Figure FDA00027951981000000211
筛选的条件为：
|Axi+Byi+Czi+D|＜Thd (4)
其中，
Figure FDA0002795198100000031
Thd为距离阈值；
步骤S26：利用筛选出的地面点
Figure FDA0002795198100000032
重复步骤S24与S25，重新估计地平面的模型并筛选新的地面点；经过N次迭代，得到最终的地面点。
4.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S3具体为：
步骤S31：对图像下半区域提取ORB特征，记为Fv；
步骤S32：将点云中的地面点
Figure FDA0002795198100000033
投影至图像上；
步骤S33：对于图像上的每一个提取到的特征点Fvi∈Fv，选取其周围矩形小范围内的
Figure FDA0002795198100000034
投影点来估计其周围的平面参数；若周围没有足够数量的投影点，将其视为无效点并放弃；对于每个有效特征点按照下式计算其深度：
Figure FDA0002795198100000035
其中，(ui，vi)T∈Fv，为有效特征点的像素坐标；K为相机内参数；si为对应特征点的深度；
Figure FDA0002795198100000036
和d为点云估计的局部平面参数，
Figure FDA0002795198100000038
为法向量，d为截距。
5.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S4具体为：根据所使用的激光雷达的线数M及角分辨率a，将整帧点云投影至一个M×(360/a)的二维图像中，每个像素存储该点对应的深度信息，计算公式如下：
Figure FDA0002795198100000037
其中，Depi为第i个点的深度，
Figure FDA0002795198100000041
为第i个点在激光雷达坐标系下的坐标值。
6.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S5具体为：
步骤S51：对点云中的每个点，选取其在深度图像所在位置同一行的前后各若干个连续的点组成点集S，计算该点的粗糙度：
Ci＝[∑j∈S(Depj-Depi)]2 (7)
其中，Ci为第i个点的粗糙度，Depj为点集S中第j个点的深度信息；
步骤S52：对于非地面点，设定阈值Cthp和Cthe区分不同类型的特征点；将粗糙度小于Cthp的点归为平面特征FP，将粗糙度大于Cthe的点归为边缘特征FE；设FP与FE的数量分别为NP和NE；从平面特征FP中选择nP个粗糙度最小的特征F′P，从边缘特征FE中选择nE个粗糙度最大的特征F′E，作为待匹配特征点；
其中，F′P∈FP，F′E∈FE，且nP＜NP，nE＜NE；
步骤S53：对于地面点，直接选出nv个粗糙度最小的地面特征点F′G。
7.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S6具体为：
步骤S61：对由步骤S3获取的带有深度信息的有效特征记为F′v，采用汉明距离对待求解相对位姿的第j帧和第k帧的特征点进行匹配，并采用RANSAC去除误匹配，得到一组配对好的3D点，表示为下式：
Figure FDA0002795198100000051
其中，Pj与Pk即为两帧之间一一匹配好的特征点集；
步骤S62：通过构建最小二乘问题，求使得匹配点投影误差的平方和达到最小的相对位姿T(R，t)，公式如下：
Figure FDA0002795198100000052
其中，
Figure FDA0002795198100000053
R和t表示第j帧和第k帧之间的旋转与平移变换；
步骤S63：计算两组对应点的质心及去质心坐标：
Figure FDA0002795198100000054
Figure FDA0002795198100000055
其中，qj与qk分别为Pj和Pk的质心，
Figure FDA0002795198100000056
与
Figure FDA0002795198100000057
分别为Pj和Pk的去质心坐标；
步骤S64：定义一个3×3的矩阵
Figure FDA0002795198100000058
对W进行SVD分解，得到：W＝U∑VT；
其中U和V为3×3的单位正交矩阵，∑为对角矩阵，求解出相对旋转变换R＝UVT；
步骤S65：根据所求解的R，平移变换t的求解公式为t＝qj-R·qk。
8.根据权利要求1所述的一种视觉与激光雷达融合的室外移动机器人位姿估计方法，其特征在于，所述步骤S7具体为：
步骤S71：对于第k帧中的地面特征
Figure FDA0002795198100000061
与平面特征
Figure FDA0002795198100000062
通过kd-tree在第j帧中的地面点和平面特征
Figure FDA0002795198100000063
寻找最近点，同时提取最近点同线和相邻线的两个平面点，由三个点组成平面；通过相对位姿变换，计算特征点到平面的距离作为点面约束ConsPP如下式：
Figure FDA0002795198100000064
其中，
Figure FDA0002795198100000065
为第j帧中寻找到的最近点，
Figure FDA0002795198100000066
为与
Figure FDA0002795198100000067
同线上最近的平面点，
Figure FDA0002795198100000068
为与
Figure FDA0002795198100000069
相邻线上最近的平面点；
Figure FDA00027951981000000610
为第k帧中特征点
Figure FDA00027951981000000611
在变换到第j帧的坐标；
步骤S72：对于第k帧中的边缘特征
Figure FDA00027951981000000612
通过kd-tree在第j帧中的边缘特征
Figure FDA00027951981000000613
寻找最近点，同时提取最近点相邻线的边缘点，由两个点确定一条直线；通过相对位姿变换，计算特征点到直线的距离作为点线约束ConsPL如下式：
Figure FDA00027951981000000614
其中，
Figure FDA00027951981000000615
为第j帧中寻找到的最近点，
Figure FDA00027951981000000616
为与
Figure FDA00027951981000000617
相邻线上最近的边缘点。
Figure FDA00027951981000000618
为第k帧中特征点
Figure FDA00027951981000000619
在变换到第j帧的坐标，即
Figure FDA00027951981000000620
Figure FDA00027951981000000621
步骤S73：由地面点云提供的法向量约束Consnor，方程如下式：
Figure FDA00027951981000000622
其中，
Figure FDA00027951981000000623
与
Figure FDA00027951981000000624
分别为第j帧和第k帧的地面点云的法向量，由提取到的地面特征F′G计算得到；
步骤S74：用欧拉角表示旋转变换R，则整个相对位姿变换T(R，t)可以表示为一个6×1的向量，即(θroll，θpitch，θyaw，tx，ty，tz)T；
地面特征对于θroll，θpitch，tz有更好的约束，对整个优化采用分步求解的策略，以视觉特征提供的相对位姿作为初值，加入地面特征提供的点面约束ConsPP和法向量约束Consnor，采用Levenberg-Marquardt(L-M)算法迭代求解位姿，记为T1；
步骤S75：以T1作为初值，加入非地面点提供的点面约束ConsPP和点线约束ConsPL，此外，取T1中的θroll，θpitch，tz值同时作为约束，再次采用L-M算法迭代求解得到最终位姿Tfinal。
CN202011329017.3A 2020-11-24 2020-11-24 一种视觉与激光雷达融合的室外移动机器人位姿估计方法 Active CN112396656B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011329017.3A CN112396656B (zh) 2020-11-24 2020-11-24 一种视觉与激光雷达融合的室外移动机器人位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011329017.3A CN112396656B (zh) 2020-11-24 2020-11-24 一种视觉与激光雷达融合的室外移动机器人位姿估计方法

Publications (2)

Publication Number Publication Date
CN112396656A true CN112396656A (zh) 2021-02-23
CN112396656B CN112396656B (zh) 2023-04-07

Family

ID=74607661

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011329017.3A Active CN112396656B (zh) 2020-11-24 2020-11-24 一种视觉与激光雷达融合的室外移动机器人位姿估计方法

Country Status (1)

Country Link
CN (1) CN112396656B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112907610A (zh) * 2021-03-25 2021-06-04 东南大学 一种基于LeGO-LOAM的分步式帧间位姿估计算法
CN113256718A (zh) * 2021-05-27 2021-08-13 浙江商汤科技开发有限公司 定位方法和装置、设备及存储介质
CN113607166A (zh) * 2021-10-08 2021-11-05 广东省科学院智能制造研究所 基于多传感融合的自主移动机器人室内外定位方法及装置
CN113671511A (zh) * 2021-07-30 2021-11-19 南京航空航天大学 一种面向区域场景的激光雷达高精度定位方法
CN113947636A (zh) * 2021-10-19 2022-01-18 中南大学 一种基于深度学习的激光slam定位系统及方法
CN115712111A (zh) * 2022-11-07 2023-02-24 北京斯年智驾科技有限公司 一种相机雷达联合标定方法、系统、电子装置、计算机设备及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107590827A (zh) * 2017-09-15 2018-01-16 重庆邮电大学 一种基于Kinect的室内移动机器人视觉SLAM方法
CN110223348A (zh) * 2019-02-25 2019-09-10 湖南大学 基于rgb-d相机的机器人场景自适应位姿估计方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
US20190333231A1 (en) * 2017-08-11 2019-10-31 Zhejiang University Stereo visual odometry method based on image gradient joint optimization
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190333231A1 (en) * 2017-08-11 2019-10-31 Zhejiang University Stereo visual odometry method based on image gradient joint optimization
CN107590827A (zh) * 2017-09-15 2018-01-16 重庆邮电大学 一种基于Kinect的室内移动机器人视觉SLAM方法
CN110223348A (zh) * 2019-02-25 2019-09-10 湖南大学 基于rgb-d相机的机器人场景自适应位姿估计方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
JACOPO SERAFIN等: "Fast and robust 3D feature extraction from sparse point clouds", 《2016 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 *
何炳蔚: "线激光－立体视觉三维数据获取系统研究", 《中国工程机械学报》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112907610A (zh) * 2021-03-25 2021-06-04 东南大学 一种基于LeGO-LOAM的分步式帧间位姿估计算法
CN112907610B (zh) * 2021-03-25 2022-04-29 东南大学 一种基于LeGO-LOAM的分步式帧间位姿估计算法
CN113256718A (zh) * 2021-05-27 2021-08-13 浙江商汤科技开发有限公司 定位方法和装置、设备及存储介质
CN113256718B (zh) * 2021-05-27 2023-04-07 浙江商汤科技开发有限公司 定位方法和装置、设备及存储介质
CN113671511A (zh) * 2021-07-30 2021-11-19 南京航空航天大学 一种面向区域场景的激光雷达高精度定位方法
CN113607166A (zh) * 2021-10-08 2021-11-05 广东省科学院智能制造研究所 基于多传感融合的自主移动机器人室内外定位方法及装置
CN113607166B (zh) * 2021-10-08 2022-01-07 广东省科学院智能制造研究所 基于多传感融合的自主移动机器人室内外定位方法及装置
CN113947636A (zh) * 2021-10-19 2022-01-18 中南大学 一种基于深度学习的激光slam定位系统及方法
CN113947636B (zh) * 2021-10-19 2024-04-26 中南大学 一种基于深度学习的激光slam定位系统及方法
CN115712111A (zh) * 2022-11-07 2023-02-24 北京斯年智驾科技有限公司 一种相机雷达联合标定方法、系统、电子装置、计算机设备及存储介质

Also Published As

Publication number Publication date
CN112396656B (zh) 2023-04-07

Similar Documents

Publication Publication Date Title
CN112396656B (zh) 一种视觉与激光雷达融合的室外移动机器人位姿估计方法
CN111462135B (zh) 基于视觉slam与二维语义分割的语义建图方法
Ji et al. Panoramic SLAM from a multiple fisheye camera rig
Geiger et al. Automatic camera and range sensor calibration using a single shot
US8600192B2 (en) System and method for finding correspondence between cameras in a three-dimensional vision system
US9124873B2 (en) System and method for finding correspondence between cameras in a three-dimensional vision system
CN111462200A (zh) 一种跨视频行人定位追踪方法、系统及设备
US11488322B2 (en) System and method for training a model in a plurality of non-perspective cameras and determining 3D pose of an object at runtime with the same
CN112184812B (zh) 提升无人机相机对AprilTag识别定位精度方法及定位方法和系统
Munoz-Banon et al. Targetless camera-lidar calibration in unstructured environments
CN112233177A (zh) 一种无人机位姿估计方法及系统
Li et al. Automatic targetless LiDAR–camera calibration: a survey
Yan et al. Joint camera intrinsic and lidar-camera extrinsic calibration
Su et al. A novel camera calibration method based on multilevel-edge-fitting ellipse-shaped analytical model
Ma et al. Crlf: Automatic calibration and refinement based on line feature for lidar and camera in road scenes
Tang et al. Fmd stereo slam: Fusing mvg and direct formulation towards accurate and fast stereo slam
Zhang LILO: A Novel Lidar–IMU SLAM System With Loop Optimization
Kleinschmidt et al. Visual multimodal odometry: Robust visual odometry in harsh environments
Tsaregorodtsev et al. Extrinsic camera calibration with semantic segmentation
Wang et al. A survey of extrinsic calibration of lidar and camera
CN114419259B (zh) 一种基于物理模型成像仿真的视觉定位方法及系统
Zhao et al. A review of visual SLAM for dynamic objects
CN115456870A (zh) 基于外参估计的多图像拼接方法
CN114608522A (zh) 一种基于视觉的障碍物识别与测距方法
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant