CN115880364A - 基于激光点云和视觉slam的机器人位姿估计方法 - Google Patents

基于激光点云和视觉slam的机器人位姿估计方法 Download PDF

Info

Publication number
CN115880364A
CN115880364A CN202310084251.1A CN202310084251A CN115880364A CN 115880364 A CN115880364 A CN 115880364A CN 202310084251 A CN202310084251 A CN 202310084251A CN 115880364 A CN115880364 A CN 115880364A
Authority
CN
China
Prior art keywords
robot
pose
frame
laser
point
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
CN202310084251.1A
Other languages
English (en)
Other versions
CN115880364B (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.)
Guangdong Polytechnic Normal University
Original Assignee
Guangdong Polytechnic Normal 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 Guangdong Polytechnic Normal University filed Critical Guangdong Polytechnic Normal University
Priority to CN202310084251.1A priority Critical patent/CN115880364B/zh
Publication of CN115880364A publication Critical patent/CN115880364A/zh
Application granted granted Critical
Publication of CN115880364B publication Critical patent/CN115880364B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Image Processing (AREA)
  • Manipulator (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了基于激光点云和视觉SLAM的机器人位姿估计方法，使用摄像机采集图像，再使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，进行位姿估计获得摄像机下的机器人位姿；使用激光雷达采集激光点云数据，使用激光雷达里程计提取激光点云数据的特征，与关键帧进行特征匹配并进行位姿估计求得激光雷达下的机器人位姿；根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理对齐时间戳；进行双线程回环检测得到回环检测结果；将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中融合得到机器人位姿。本发明补全了与视觉特征点对应的点云缺失，提高了视觉SLAM建图及回环检测精度。

Description

基于激光点云和视觉SLAM的机器人位姿估计方法
技术领域
本发明属于机器人定位的技术领域，具体涉及一种基于激光点云和视觉SLAM的机器人位姿估计方法。
背景技术
随着无人驾驶、5G通信被人们提出和高度关注，SLAM（即时定位与建图）迎来了很大的挑战。SLAM技术是机器人领域的关键技术，其依靠传感器完成自主导航、建图、以及路径规划等任务。传统SLAM系统一般是依靠单一的传感器进行工作，但单一传感器的缺陷也较为明显，其对于环境因素的要求高导致其工作性能不稳定；而多传感器SLAM系统存在着不同传感器的测量原理不同以及其运行频率不同的问题，导致SLAM系统建图的误差较大；针对多传感器SLAM系统的问题，传统解决方法是将频率较快的视觉传感器的位姿估计作为激光传感器位姿估计的先验数据放入卡尔曼滤波器中，然后进行SLAM系统的位姿估计输出，但由于多传感器的数据并没有对齐在同一个时间戳内，可能存在着无法满足多传感器SLAM系统的建图要求。
近几年，深度学习十分热门，图像处理技术的进步也带动了视觉SLAM的发展，一方面由于摄像机的价格相对便宜，可以在立体空间中简单快速地获取重要的角或边信息；另一方面在于摄像机具有较高的采样率，从而被广泛青睐。但是摄像机也有自己的缺点，在大范围旋转时，很容易失去正常功能，造成较大的漂移误差；同时摄像机还需要运行在稳定的光源环境才能提取到较多且质量高的特征点。激光雷达可以测量的范围和距离非常广，并且是主动传感器，所以激光雷达在黑暗中仍然具有极好的鲁棒性；但激光雷达的缺点在于价格昂贵，同时由于二维激光传感器只有深度信息，并且只扫描一个平面，当机器人在走廊中运动时，传感器返回的深度信息不随时间变化。因此通过不同传感器的相互协同降低位姿估计的误差，成为提高SLAM系统定位建图的鲁棒性的重要手段。
发明内容
本发明的主要目的在于克服现有技术的缺点与不足，提供一种基于激光点云和视觉SLAM的机器人位姿估计方法，利用三次样条函数对激光雷达的位姿估计进行样条插值处理，同时引入视觉-激光雷达双线程回环检测模块，提高了SLAM系统建图精度，提高了回环检测的精度，防止出现“假阳性”的回环现象。
为了达到上述目的，本发明采用以下技术方案：
基于激光点云和视觉SLAM的机器人位姿估计方法，所述方法包括下述步骤：
使用摄像机采集机器人行进过程中的图像，再使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，对摄像机进行位姿估计获得摄像机下的机器人位姿；
使用激光雷达采集图像的激光点云数据，再使用激光雷达里程计提取激光点云数据的特征，与关键帧进行特征匹配，并对激光雷达进行位姿估计求得激光雷达下的机器人位姿；
根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳；
对采集图像和激光点云数据进行双线程回环检测，得到回环检测结果；
根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿。
作为优选的技术方案，所述ORB特征点包括FAST角点和BRIEF描述子；
所述使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，具体为：
将摄像机采集的图像输入视觉里程计中，通过对图像上的像素点进行阈值比较，提取FAST角点，同时引入图像金字塔对每一层金字塔图像进行FAST角点提取；
比对FAST角点附近的两个随机像素，构成描述子向量得到BRIEF描述子；
根据摄像机的移动距离是否超过阈值距离来确定图像是否为关键帧；
对关键帧的ORB特征点进行距离计算，通过暴力匹配法进行特征匹配得到匹配点对。
作为优选的技术方案，所述视觉里程计通过在图像上选取像素点p，计算该像素点的亮度I p ，以像素点p为中心点，选取半径为3的圆上的像素点，若该圆上12个连续的像素点亮度与中心点p亮度的差值大于阈值，则认定像素点p为FAST角点；
引入图像金字塔对图像进行不同层次的下采样，得到多层不同分辨率的图像，并对每一层图像进行FAST角点提取；
对于每一FAST角点，随机选取FAST角点附近128组像素点对p、q，比较每一组像素点p和像素点q的大小并编码为二进制描述向量，若p大于q则取1，否则取0，得到的128个描述向量构成该FAST角点的BRIEF描述子；
所述确定图像是否为关键帧，具体为：
获取图像的摄像机移动距离，并与阈值距离进行比较；
若图像的摄像机移动距离大于阈值距离，则判定该图像为关键帧。
作为优选的技术方案，所述通过暴力匹配法进行特征匹配得到匹配点对，具体为：
设图像中第t帧提取到的第m个ORB特征点表示为x t,m ，m=1,2,…,M，M表示第t帧中提取到的ORB特征点的个数，第t+1帧提取到的第n个ORB特征点表示为x t+1,n ，n= 1,2,…,N，N表示第t+1帧中提取到的ORB特征点的个数；
采用欧氏距离测量第t帧中ORB特征点的BRIEF描述子与第t+1帧中ORB特征点的BRIEF描述子之间的距离，对齐排序，选取距离最近的一个作为匹配点对，计算公式为：
Figure SMS_1
，
其中，n表示ORB特征点的BRIEF描述子的长度，x t,i,k 表示图像中第t帧提取到的第i个ORB特征点的BRIEF描述子中的第k个元素，x t+1,j,k 表示图像中第t+1帧提取到的第j个ORB特征点的BRIEF描述子中的第k个元素。
作为优选的技术方案，根据获得的匹配点对，使用PnP法对摄像机进行位姿估计，并使用最小化投影求解，获得摄像机下的机器人位姿，具体为：
设图像在投影空间中某点的坐标为P i = [X i ,Y i ,Z i ] T ，该点投影的像素坐标为U i =[u i ,v i ] T ，将投影关系转换为齐次坐标系下表示，转换式为：
Figure SMS_2
，
其中，Z i 表示图像中的特征点在世界坐标系下的深度值，K表示摄像机的内参矩阵，T C,W 表示世界坐标系与相机坐标系之间的转换矩阵；
通过转换式求得变换矩阵T C,W ，构建位姿估计中第i个点的误差项，表示为：
Figure SMS_3
，
把位姿估计的误差项进行求和，构建最小二乘问题，寻找误差最小时摄像机下的机器人位姿变换矩阵
Figure SMS_4
，公式为：
Figure SMS_5
，
其中，n表示图像中提取出的ORB特征点数量，P i 为第i个ORB特征点在世界坐标系下的坐标，
Figure SMS_6
表示通过变换矩阵得到的第i个ORB特征点在世界坐标系下的估计坐标；
根据机器人位姿变换矩阵
Figure SMS_7
求得第k帧时摄像机下的机器人位姿估计值，公式为：
Figure SMS_8
，
其中，
Figure SMS_9
表示视觉里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。
作为优选的技术方案，所述对激光雷达进行位姿估计求得激光雷达下的机器人位姿，具体为：
将激光雷达采集的图像的激光点云数据输入激光雷达里程计中，计算每一帧激光点云数据中每个点的曲率，并对每个点的曲率进行排序，选择曲率最大的2个点作为边缘点，曲率最小的4个点作为平面点；
对激光点云数据进行异常值排除；
使用基于特征的Scan-to-Scan匹配方法将异常值排除后的激光点云数据进行特征点匹配，获取边缘线和平面；
计算边缘点到边缘线的点线残差和边缘点到平面的点面残差；
计算残差总和，通过对残差总和构建最小二乘法求得极值，使用迭代法求解使得残差总和最小，并根据激光点云数据进行位姿估计，求得激光雷达下的机器人位姿。
作为优选的技术方案，所述曲率的计算公式为：
Figure SMS_10
，
其中，C表示曲率，
Figure SMS_11
表示激光雷达坐标系下第k帧激光点云数据中第i个点的坐标，S表示激光点云数据的集合；
所述异常值包括激光点云数据中平行于激光扫描平面的点和遮挡区域边界的点；
所述获取边缘线和平面，具体为：
设第k-1帧的激光点云数据为
Figure SMS_12
，第k帧的激光点云数据为
Figure SMS_13
；
在
Figure SMS_14
中找到与
Figure SMS_15
中边缘点i最近的边缘点j，再在另外的激光线束上找到距离边缘点j最近的边缘点l，连接边缘点j和边缘点l得到直线jl构成边缘线，并在
Figure SMS_16
中找到三个与
Figure SMS_17
中面点i最近的面点j，在同一激光线束上找到距离面点j最近的面点l，再在另外的激光线束上找到距离面点j最近的面点m，构成平面jlm；
记
Figure SMS_18
中边缘点i到
Figure SMS_19
中直线jl的距离为点线残差d ε ，
Figure SMS_20
中面点i到
Figure SMS_21
中jlm平面的距离为点面残差d ω ，则点线残差d ε 的计算公式为：
Figure SMS_22
，
点面残差d ω 的计算公式为：
Figure SMS_23
，
其中，
Figure SMS_24
表示激光雷达坐标系下第k-1帧激光点云数据中第j个点的坐标；
Figure SMS_25
表示激光雷达坐标系下第k-1帧激光点云数据中l个点的坐标；
Figure SMS_26
表示激光雷达坐标系下第k-1帧激光点云数据中m个点的坐标；
所述残差总和d=d ε +d ω ；
使用点线残差和点面残差构建与激光雷达坐标系下两帧之间位姿变换矩阵T L 的方程，表示为：
Figure SMS_27
，
其中，f ε ()表示点线残差的计算式；f ω ()表示点面残差的计算式；
Figure SMS_28
表示激光雷达坐标系下第k-1帧与第k帧之间机器人的位姿变换矩阵T L ；
使用残差总和构建最小二乘问题，表示为：
Figure SMS_29
，
其中，F(x)表示最小二乘问题的目标函数；
使用迭代法对最小二乘问题求极值：给定初始值x 0的坐标，对于第k-1次迭代，寻找增量
Figure SMS_30
，使得
Figure SMS_31
达到极小值；若
Figure SMS_32
足够小则停止，得到极小值F(x)下的变换矩阵
Figure SMS_33
；否则令
Figure SMS_34
，继续下一次迭代；其中，
Figure SMS_35
表示激光雷达坐标系下第k-1帧激光点云数据中第i个点的坐标，
Figure SMS_36
表示在取得极小值F(x)下的变换矩阵；
通过极小值下的变换矩阵
Figure SMS_37
求得第k帧时激光雷达下的机器人位姿估计值，公式为：
Figure SMS_38
，
其中，
Figure SMS_39
表示激光雷达里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。
作为优选的技术方案，所述根据摄像机的频率，对雷达位姿进行三次样条插值处理，利用n-1个三次函数线段拟合出n个时间帧的激光雷达输出，通过插值对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：
根据样条插值假设条件，利用已知的历史激光雷达的机器人位姿数据构建出n-1段样条函数S i (x)，表示为：
S i (x) =a i +b i (x-x i ) +c i (x-x i )2+d i (x-x i )3 ,i= 1,2,…,n-1，
根据样条函数性质，列出四个等式约束，令
Figure SMS_40
，
Figure SMS_41
，求得样条函数S i (x)的曲线参数a i 、b i 、c i 、d i ，等式约束表示为：
Figure SMS_42
，
其中，x表示样条函数自变量为时间，x i 表示第i帧激光点云数据对应的时间，n表示激光点云数据的个数或者长度，h i =x i+1-x i ，y i 表示第i帧时激光雷达下的机器人位姿估计值；
假设摄像机的采样频率为f c ，激光雷达的采样频率为f L ，根据已知的历史激光雷达的机器人位姿数据，利用三次样条函数来拟合机器人运动的轨迹关系，再使用摄像机的采样频率对该拟合函数进行插值处理，即利用样本函数S i (x)分别插值得到时间点为
Figure SMS_43
、
Figure SMS_44
、…、
Figure SMS_45
下的函数值
Figure SMS_46
，i=1,2,…,l，其中l为同一段时间内摄像机获得的帧数；插值后得到对应时刻的激光雷达的机器人位姿
Figure SMS_47
，即表示相机第i帧时对应的激光雷达的机器人位姿，实现摄像机和激光雷达数据的同步生成。
作为优选的技术方案，所述双线程回环检测包括视觉回环检测、激光雷达回环检测和过滤器；
所述视觉回环检测采用词袋模型进行检测，具体为：
采用机器学习的K-means++算法对词袋模型中的根节点进行k次聚类，形成第一层节点，对第一层节点进行聚类处理，得到第二层节点，依次聚类d次，形成一个深度为d，枝叶数为K d 的KD-树；
利用KD-树构造字典的单词，通过输入的图像，提取图像的特征矩阵，利用公式计算图像相似度，其中A K 表示第k帧图像的特征矩阵，A K-1表示第k-1帧图像的特征矩阵，则图像相似度计算公式为：
Figure SMS_48
，
其中，M表示特征矩阵的长度，m1表示特征矩阵的m1范数；
当两帧图像的特征矩阵完全相同时，则输出1；完全不同时则输出0；
将当前帧的特征矩阵与每层中间节点的聚类中心比较d次，根据当前帧与历史帧的相似度和设定阈值进行比较，判断当前帧图像是否发生回环现象；
所述激光雷达回环检测采用Scan-Context描述子以及相似距离来进行检测，具体为：
计算激光点云数据的描述子，将每一帧的激光点云数据按照径向与环向进行划分，划分为不同的扇形栅格，其中径向与环向数量分别为Nr与Ns，将激光点云数据转换为一个(Nr，Ns)的Scan-Context描述子矩阵，该描述子矩阵的元素值为所在扇形栅格中高度的最大值；
Scan-Context描述子矩阵中每一列向量表示一个描述子，通过计算相似距离描述两个描述子的相似程度，公式为：
Figure SMS_49
，
其中，I q 为当前帧激光点云数据，I c 为历史帧激光点云数据，
Figure SMS_50
为当前帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值，
Figure SMS_51
为历史帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值；
对历史中的关键帧进行相似距离比对，找到与当前帧最相似的描述子，最终找到对应的回环帧，判断当前帧是否发生回环现象；
将视觉回环检测与激光雷达回环检测的结果输入过滤器中，当检测结果均为当前帧的回环帧时，认定当前帧发生回环现象。
作为优选的技术方案，所述根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿，具体为：
已知SLAM中的机器人状态方程为：
Figure SMS_52
，
其中A为状态转移矩阵，B为控制矩阵，H为观测矩阵，W∼N(0,Q)是过程噪声，V∼N(0,R)是观测噪声，X k 为k时刻的机器人位姿，Z k 为k时刻的机器人位姿估计值，U k-1为k-1时刻的控制输入，W k 为k时刻的过程噪声，V k 为k时刻的观测噪声值；
假设机器人初始状态以及每一时刻的噪声是互相独立的；
根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，对第一帧进行判断，若第一帧是第一次进行检测，则使用第一帧时摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值对卡尔曼滤波器进行初始化，初始值表示为
Figure SMS_53
且
Figure SMS_54
；
Figure SMS_55
为摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值；
根据上一时刻的机器人位姿估计值
Figure SMS_56
，预测当前时刻机器人位姿的先验估计值
Figure SMS_57
，公式为：
Figure SMS_58
，
计算先验估计值的协方差
Figure SMS_59
：
Figure SMS_60
，
由于先验估计与实际值存在误差，通过更新卡尔曼增益系数K k ，使得误差最小：
Figure SMS_61
，
根据卡尔曼增益系数计算出当前时刻机器人位姿的后验估计值
Figure SMS_62
以及协方差矩阵P k ，公式为：
Figure SMS_63
，
其中，P k-1为k-1帧的协方差矩阵，A T 为状态转移矩阵的转置，Q为过程噪声协方差，R为测量噪声协方差，H T 为观测矩阵的转置，
Figure SMS_64
表示当前时刻机器人位姿的后验估计值，即当前时刻误差最小的机器人位姿；
将计算结果的后验估计值与协方差矩阵保存，以便于下次的迭代计算。
本发明与现有技术相比，具有如下优点和有益效果：
1、多传感器SLAM系统存在着不同传感器的测量原理不同以及其运行频率不同导致整个SLAM系统建图的误差较大，通过补全了与视觉特征点对应的点云缺失，来解决建图时多传感器数据不一致的问题，利用视觉里程计的位姿估计和经过插值得到的激光雷达里程计的位姿估计这两者的均值，通过输入两者的位姿估计均值即同一时间戳的机器人位姿估计到卡尔曼滤波器中，通过不断更新卡尔曼增益，从而输出机器人的位姿估计，得到更好的SLAM系统的建图效果。
2、引入视觉-雷达双线程回环检测，将视觉回环检测与激光雷达的回环检测的结果输入过滤器，只有都找到了当前帧的回环帧，才认定当前帧发生了回环现象，有效避免不是回环帧的“过阳性”情况发生，对于未检测到的回环现象，通过多次检测也可以判断出该回环现象，提高回环检测的鲁棒性以及准确率。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例基于激光点云和视觉SLAM的机器人位姿定位方法的流程图；
图2为本发明实施例获得相机位姿的流程图；
图3为本发明实施例求得雷达位姿的流程图；
图4为本发明实施例激光点云数据中异常值排除的示意图；
图5为本发明实施例进行双线程回环检测的流程图；
图6为本发明实施例词袋模型的结构示意图；
图7为本发明实施例卡尔曼滤波器的流程步骤图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本申请所描述的实施例可以与其它实施例相结合。
如图1所示，本实施例提供一种基于激光点云和视觉SLAM的机器人位姿估计方法，包括下述步骤：
S1、使用摄像机采集机器人行进过程中的图像，再使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，对摄像机进行位姿估计获得摄像机下的机器人位姿；
具体的，如图2所示，视觉里程计提取ORB特征点的步骤分为FAST角点提取及BRIEF描述子计算，判断关键帧后进行特征匹配，再对摄像机进行位姿估计获得摄像机下的机器人位姿，具体步骤为：
S101、将摄像机采集的图像输入视觉里程计中，通过对图像上的像素点进行阈值比较，提取FAST角点，同时引入图像金字塔对每一层金字塔图像进行FAST角点提取；
本实施例中，视觉里程计通过在图像上选取像素点p，计算该像素点的亮度I p ，以像素点p为中心点，选取半径为3的圆上的像素点，若该圆上12个连续的像素点亮度与中心点p亮度的差值大于阈值，则认定像素点p为FAST角点；同时引入图像金字塔对图像进行不同层次的下采样，得到多层不同分辨率的图像，并对每一层图像进行FAST角点提取，实现不同层金字塔之间的尺度不变性。
S102、比对FAST角点附近的两个随机像素，构成描述子向量得到BRIEF描述子；
本实施例中，对于每一FAST角点，随机选取FAST角点附近128组像素点对p、q，比较每一组像素点p和像素点q的大小并编码为二进制描述向量，若p大于q则取1，否则取0，得到的128个描述向量构成该FAST角点的BRIEF描述子。
S103、根据摄像机的移动距离是否超过阈值距离来确定图像是否为关键帧；
本实施例中，通过获取图像的摄像机移动距离与阈值距离进行比较；若图像的摄像机移动距离大于阈值距离，则判定该图像为关键帧；否则该图像不为关键帧且不进行特征匹配。
S104、对关键帧的ORB特征点进行距离计算，通过暴力匹配法进行特征匹配得到匹配点对；
本实施例中，通过暴力匹配法进行特征匹配得到匹配点对，具体为：
设图像中第t帧提取到的第m个ORB特征点表示为x t,m ，m= 1,2,…,M，M表示第t帧中提取到的ORB特征点的个数，第t+1帧提取到的第n个ORB特征点表示为x t+1,n ，n= 1,2,…,N，N表示第t+1帧中提取到的ORB特征点的个数；
采用欧氏距离测量第t帧中ORB特征点的BRIEF描述子与第t+1帧中ORB特征点的BRIEF描述子之间的距离，对齐排序，选取距离最近的一个作为匹配点对，计算公式为：
Figure SMS_65
，
其中，n表示ORB特征点的BRIEF描述子的长度，x t,i,k 表示图像中第t帧提取到的第i个ORB特征点的BRIEF描述子中的第k个元素，x t+1,j,k 表示图像中第t+1帧提取到的第j个ORB特征点的BRIEF描述子中的第k个元素。
S105、根据获得的匹配点对，使用PnP法对摄像机进行位姿估计，并使用最小化投影求解，获得摄像机下的机器人位姿；
PnP是一类问题，通过n个3D空间点及其投影，估计摄像机的机器人位姿；PnP问题求解方法有很多种，本实施例使用非线性优化的方式，构建最小二乘问题并进行迭代求解，最终得到的结果是误差最小下的相机变换矩阵，根据这个变换矩阵及像素坐标，估算出机器人的世界坐标值，即摄像机下的机器人位姿，具体为：
设图像在投影空间中某点的坐标为P i = [X i ,Y i ,Z i ] T ，该点投影的像素坐标为U i =[u i ,v i ] T ，将投影关系转换为齐次坐标系下表示，转换式为：
Figure SMS_66
，
其中，Z i 表示图像中的特征点在世界坐标系下的深度值，K表示摄像机的内参矩阵，T C,W 表示世界坐标系与相机坐标系之间的转换矩阵；
通过转换式求得变换矩阵T C,W ，构建位姿估计中第i个点的误差项，表示为：
Figure SMS_67
，
把位姿估计的误差项进行求和，构建最小二乘问题，寻找误差最小时摄像机下的机器人位姿变换矩阵
Figure SMS_68
，公式为：
Figure SMS_69
，
其中，n表示图像中提取出的ORB特征点数量，P i 为第i个ORB特征点在世界坐标系下的坐标，
Figure SMS_70
表示通过变换矩阵得到的第i个ORB特征点在世界坐标系下的估计坐标；
根据机器人位姿变换矩阵
Figure SMS_71
求得第k帧时摄像机下的机器人位姿估计值，公式为：
Figure SMS_72
，
其中，
Figure SMS_73
表示视觉里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。
S2、使用激光雷达采集图像的激光点云数据，再使用激光雷达里程计提取激光点云数据的特征，与关键帧进行特征匹配，并对激光雷达进行位姿估计求得激光雷达下的机器人位姿；
具体的，如图3所示，激光雷达里程计的操作步骤具体为：
S201、将激光雷达采集的图像的激光点云数据输入激光雷达里程计中，计算每一帧激光点云数据中每个点的曲率，并对每个点的曲率进行排序，选择曲率最大的2个点作为边缘点，曲率最小的4个点作为平面点，曲率的计算公式为：
Figure SMS_74
，
其中，C表示曲率，
Figure SMS_75
表示激光雷达坐标系下第k帧激光点云数据中第i个点的坐标，S表示激光点云数据的集合，取模是求点云的数量；
S202、对激光点云数据进行异常值排除；
本实施例中，如图4所示，异常值包括平行于激光扫描平面的点和遮挡区域边界的点，即排除平行于激光扫描平面的点时，当前帧激光点云数据中的点及其附近点集合拟合的平面与激光线束的夹角不能过小；排除遮挡区域边界的点时，当前帧激光点云数据中点附近的点集中没有在激光束方向上间距过大的点。
如在图4（a）中点A位于与激光线束(虚线段)有一定角度的线上；点B位于与激光线束(虚线段)大致平行的线上；由于激光点云数据中的点及其附近点集合拟合的平面与激光线束的夹角不能过小，故认为B是一个不可靠的激光返回点，不选择它作为特征点因此将其排除。
又如在图4（b）中，实线段是激光可观测的物体，点A在被遮挡区域的边界上(虚线段)可以被检测到的边缘点；然而，如果从不同的角度观察，被遮挡的区域可以改变并变得可观察到，因此不把A作为显著边点，也不选择A作为特征点，将其排除。
S203、使用基于特征的Scan-to-Scan匹配方法将异常值排除后的激光点云数据进行特征点匹配，获取边缘线和平面；
本实施例中，设第k-1帧的激光点云数据为
Figure SMS_76
，第k帧的激光点云数据为
Figure SMS_77
；基于这两个激光点云数据来寻找它们之间的对应关系
Figure SMS_78
；
在
Figure SMS_79
中找到与
Figure SMS_80
中边缘点i最近的边缘点j，再在另外的激光线束上找到距离边缘点j最近的边缘点l，连接边缘点j和边缘点l得到直线jl构成边缘线，并在
Figure SMS_81
中找到三个与
Figure SMS_82
中面点i最近的面点j，在同一激光线束上找到距离面点j最近的面点l，再在另外的激光线束上找到距离面点j最近的面点m，构成平面jlm。
S204、计算边缘点到边缘线的点线残差和边缘点到平面的点面残差；
本实施例中，记
Figure SMS_83
中边缘点i到
Figure SMS_84
中直线jl的距离为点线残差d ε ，
Figure SMS_85
中面点i到
Figure SMS_86
中jlm平面的距离为点面残差d ω ，则点线残差d ε 的计算公式为：
Figure SMS_87
，
点面残差d ω 的计算公式为：
Figure SMS_88
，
其中，
Figure SMS_89
表示激光雷达坐标系下第k-1帧激光点云数据中第j个点的坐标；
Figure SMS_90
表示激光雷达坐标系下第k-1帧激光点云数据中l个点的坐标；
Figure SMS_91
表示激光雷达坐标系下第k-1帧激光点云数据中m个点的坐标；
S205、计算残差总和，通过对残差总和构建最小二乘法求得极值，使用迭代法求解使得残差总和最小，并根据激光点云数据进行位姿估计，求得激光雷达下的机器人位姿。
本实施例中，残差总和d=d ε +d ω ；使用点线残差和点面残差构建与激光雷达坐标系下两帧之间位姿变换矩阵T L 的方程，表示为：
Figure SMS_92
，
其中，f ε ()表示点线残差的计算式；f ω ()表示点面残差的计算式；
Figure SMS_93
表示激光雷达坐标系下第k-1帧与第k帧之间机器人的位姿变换矩阵T L ；
使用残差总和构建最小二乘问题，表示为：
Figure SMS_94
，
其中，F(x)表示最小二乘问题的目标函数；
使用迭代法对最小二乘问题求极值：给定初始值x 0的坐标，对于第k-1次迭代，寻找增量
Figure SMS_95
，使得
Figure SMS_96
达到极小值；若
Figure SMS_97
足够小则停止，得到极小值F(x)下的变换矩阵
Figure SMS_98
；否则令
Figure SMS_99
，继续下一次迭代；其中，
Figure SMS_100
表示激光雷达坐标系下第k-1帧激光点云数据中第i个点的坐标，
Figure SMS_101
表示在取得极小值F(x)下的变换矩阵；
通过极小值下的变换矩阵
Figure SMS_102
求得第k帧时激光雷达下的机器人位姿估计值，公式为：
Figure SMS_103
，
其中，
Figure SMS_104
表示激光雷达里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。
S3、根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，利用n-1个三次函数线段拟合出n个时间帧的激光雷达输出，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳；
本实施例中，假设获得的激光雷达下的机器人位姿有n个时间点的状态，共n-1个段，插入3次多项式，以光滑的线段连接，并使其二阶导数连续的方法称为三次样条插值法；则对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：
S301、根据样条插值假设条件，构建样条函数方程组；
样条函数是用于拟合出机器人的位姿曲线，通过得到位姿估计曲线，然后运用插值方法得到与摄像机同一时间戳下的机器人位姿估计值，样条函数有以下四个假设条件：
条件1：样条函数穿过所有已知节点；
条件2：在所有节点(除了第一节点和最后一个节点)处0阶连续(保证数据不间断，无跳变)，即前一段方程在节点处的函数值和后一段方程在相同节点处的函数值相等；
条件3：在所有节点（除了第一节点和最后一个节点）处1阶连续（保证节点处有相同的斜率，原函数曲线上没有剧烈的跳变）；
条件4：在所有节点（除了第一节点和最后一个节点）处2阶连续（保证节点处有相同的曲率，即相同的弯曲程度）。
根据上述四个假设条件，利用已知的历史激光雷达的机器人位姿数据可以构建出n-1段样条函数S i (x)，表示为：
S i (x) =a i +b i (x-x i ) +c i (x-x i )2+d i (x-x i )3 ,i= 1,2,…,n-1，
根据样条函数性质，列出四个等式约束，令
Figure SMS_105
，
Figure SMS_106
，求得样条函数S i (x)的曲线参数a i 、b i 、c i 、d i ，等式约束表示为：
Figure SMS_107
，
其中，x表示样条函数自变量为时间，x i 表示第i帧激光点云数据对应的时间，n表示激光点云数据的个数或者长度，h i =x i+1-x i ，y i 表示第i帧时激光雷达下的机器人位姿估计值；
S302、根据已知的历史激光雷达的机器人位姿数据，对等式约束进行求解得到所有参数，利用三次样条函数来拟合机器人运动的轨迹关系；
假设摄像机的采样频率为f c ，激光雷达的采样频率为f L 由于激光雷达的采样第k帧与摄像机采样的第k帧对应的时间不同，所以不能直接合并利用，因此
S303、使用摄像机的采样频率对该拟合函数进行插值处理，插值出对应时刻的激光雷达的机器人位姿，实现摄像机和激光雷达数据的同步生成，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：
利用样本函数S i (x)分别插值得到时间点为
Figure SMS_108
、
Figure SMS_109
、…、
Figure SMS_110
下的函数值
Figure SMS_111
，i=1,2,…,l，其中l为同一段时间内摄像机获得的帧数；
插值后得到对应时刻的激光雷达的机器人位姿
Figure SMS_112
，即表示相机第i帧时对应的激光雷达的机器人位姿。
S4、对采集图像和激光点云数据进行双线程回环检测，得到回环检测结果；
本实施例中的双线程回环检测包括视觉回环检测和激光雷达回环检测，其中视觉回环检测采用词袋模型（已训练好）进行检测；激光雷达回环检测采用Scan-Context描述子以及相似距离来进行检测；如图5所示，双线回环检测的具体步骤为：
S401、如图6所示，采用机器学习的K-means++算法对词袋模型中的根节点进行k次聚类，形成第一层节点，对第一层节点进行聚类处理，得到第二层节点，依次聚类d次，形成一个深度为d，枝叶数为K d 的KD-树；
S402、利用KD-树构造字典的单词，通过输入的图像，提取图像的特征矩阵，利用公式计算图像相似度，其中A K 表示第k帧图像的特征矩阵，A K-1表示第k-1帧图像的特征矩阵，则图像相似度计算公式为：
Figure SMS_113
，
其中，M表示特征矩阵的长度，m1表示特征矩阵的m1范数；
S403、当两帧图像的特征矩阵完全相同时，则输出1；完全不同时则输出0；将当前帧的特征矩阵与每层中间节点的聚类中心比较d次，即可找到最后对应的帧数，保证了对数级别的查找效率；在根据当前帧与历史帧的相似度和设定阈值进行比较，判断当前帧图像是否发生回环现象；本实施例中当前帧与历史帧的相似度大于0.7这个阈值时，则认为当前帧发生了回环现象。
S404、计算激光点云数据的描述子，将每一帧的激光点云数据按照径向与环向进行划分，划分为不同的扇形栅格，其中径向与环向数量分别为Nr与Ns，将激光点云数据转换为一个(Nr,Ns)的Scan-Context描述子矩阵，该描述子矩阵的元素值为所在扇形栅格中高度的最大值；
S405、Scan-Context描述子矩阵中每一列向量表示一个描述子，通过计算相似距离描述两个描述子的相似程度，公式为：
Figure SMS_114
，
其中，I q 为当前帧激光点云数据，I c 为历史帧激光点云数据，
Figure SMS_115
为当前帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值，
Figure SMS_116
为历史帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值；本实施例中的历史帧是指执行本方法后得到的所有关键帧，会存储下来用于进行回环检测。
S406、对历史中的关键帧进行相似距离比对，找到与当前帧最相似的描述子，最终找到对应的回环帧，判断当前帧是否发生回环现象；
S407、将视觉回环检测与激光雷达回环检测的结果输入过滤器中，当检测结果均为当前帧的回环帧时，认定当前帧发生回环现象，有效避免不是回环帧判定情况的发生，克服SLAM系统建图不准确的问题；同时对于未检测到的回环现象，可通过多次检测判断出该回环现象。
S5、根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿。
具体的，本实施例中已知SLAM中的机器人状态方程为：
Figure SMS_117
，
其中A为状态转移矩阵，B为控制矩阵，H为观测矩阵，W∼N(0,Q)是过程噪声，V∼N(0,R)是观测噪声，X k 为k时刻的机器人位姿，Z k 为k时刻的机器人位姿估计值，U k-1为k-1时刻的控制输入，W k 为k时刻的过程噪声，V k 为k时刻的观测噪声值；同时假设机器人初始状态以及每一时刻的噪声是互相独立的，且帧数以摄像机采样频率取得的帧数为准；则如图7所示卡尔曼滤波器的流程具体为：
S501、根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，对第一帧进行判断，若第一帧是第一次进行检测，则使用时摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值对卡尔曼滤波器进行初始化，初始值表示为
Figure SMS_118
且
Figure SMS_119
；
Figure SMS_120
为摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值；
S502、根据上一时刻的机器人位姿估计值
Figure SMS_121
，预测当前时刻机器人位姿的先验估计值
Figure SMS_122
，公式为：
Figure SMS_123
，
S503、计算先验估计值的协方差
Figure SMS_124
：
Figure SMS_125
，
S504、由于先验估计与实际值存在误差，通过更新卡尔曼增益系数K k ，使得误差最小：
Figure SMS_126
，
S505、根据卡尔曼增益系数计算出当前时刻机器人位姿的后验估计值
Figure SMS_127
以及协方差矩阵P k ，公式为：
Figure SMS_128
，
其中，P k-1为k-1帧的协方差矩阵，A T 为状态转移矩阵的转置，Q为过程噪声协方差，R为测量噪声协方差，H T 为观测矩阵的转置；
本发明中将视觉里程计输出的摄像机下的位姿结果与激光雷达里程计输出的激光雷达下的机器人后结果进行融合使用，将上一帧的位姿估计平均值作为上一帧机器人的位姿估计值
Figure SMS_129
，入到卡尔曼滤波器中，同时利用噪声相互独立的条件以及噪声的协方差Q、R，通过机器人的状态方程粗略计算当前时刻的先验估计值
Figure SMS_130
，然后更新卡尔曼增益系数K k ，计算出当前时刻机器人位姿的后验估计值
Figure SMS_131
以及协方差矩阵P k ，得到当前时刻误差最小的机器人位姿
Figure SMS_132
；最后将计算结果的后验估计值与协方差矩阵保存，用于下次的迭代计算，降低了不同传感器的相互协同位姿估计的误差，提高了SLAM系统定位建图的鲁棒性。
需要说明的是，对于前述的各方法实施例，为了简便描述，将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述方法包括：
使用摄像机采集机器人行进过程中的图像，再使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，对摄像机进行位姿估计获得摄像机下的机器人位姿；
使用激光雷达采集图像的激光点云数据，再使用激光雷达里程计提取激光点云数据的特征，与关键帧进行特征匹配，并对激光雷达进行位姿估计求得激光雷达下的机器人位姿；
根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳；
对采集图像和激光点云数据进行双线程回环检测，得到回环检测结果；
根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿。
2.根据权利要求1所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述ORB特征点包括FAST角点和BRIEF描述子；
所述使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，具体为：
将摄像机采集的图像输入视觉里程计中，通过对图像上的像素点进行阈值比较，提取FAST角点，同时引入图像金字塔对每一层金字塔图像进行FAST角点提取；
比对FAST角点附近的两个随机像素，构成描述子向量得到BRIEF描述子；
根据摄像机的移动距离是否超过阈值距离来确定图像是否为关键帧；
对关键帧的ORB特征点进行距离计算，通过暴力匹配法进行特征匹配得到匹配点对。
3.根据权利要求2所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述视觉里程计通过在图像上选取像素点p，计算该像素点的亮度I p ，以像素点p为中心点，选取半径为3的圆上的像素点，若该圆上12个连续的像素点亮度与中心点p亮度的差值大于阈值，则认定像素点p为FAST角点；
引入图像金字塔对图像进行不同层次的下采样，得到多层不同分辨率的图像，并对每一层图像进行FAST角点提取；
对于每一FAST角点，随机选取FAST角点附近128组像素点对p、q，比较每一组像素点p和像素点q的大小并编码为二进制描述向量，若p大于q则取1，否则取0，得到的128个描述向量构成该FAST角点的BRIEF描述子；
所述确定图像是否为关键帧，具体为：
获取图像的摄像机移动距离，并与阈值距离进行比较；
若图像的摄像机移动距离大于阈值距离，则判定该图像为关键帧。
4.根据权利要求3所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述通过暴力匹配法进行特征匹配得到匹配点对，具体为：
设图像中第t帧提取到的第m个ORB特征点表示为x t,m ，m=1,2,…,M，M表示第t帧中提取到的ORB特征点的个数，第t+1帧提取到的第n个ORB特征点表示为x t+1,n ，n= 1,2,…,N，N表示第t+1帧中提取到的ORB特征点的个数；
采用欧氏距离测量第t帧中ORB特征点的BRIEF描述子与第t+1帧中ORB特征点的BRIEF描述子之间的距离，对齐排序，选取距离最近的一个作为匹配点对，计算公式为：
Figure QLYQS_1
，
其中，n表示ORB特征点的BRIEF描述子的长度，x t,i,k 表示图像中第t帧提取到的第i个ORB特征点的BRIEF描述子中的第k个元素，x t+1,j,k 表示图像中第t+1帧提取到的第j个ORB特征点的BRIEF描述子中的第k个元素。
5.根据权利要求4所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，根据获得的匹配点对，使用PnP法对摄像机进行位姿估计，并使用最小化投影求解，获得摄像机下的机器人位姿，具体为：
设图像在投影空间中某点的坐标为P i = [X i ,Y i ,Z i ] T ，该点投影的像素坐标为U i = [u i , v i ] T ，将投影关系转换为齐次坐标系下表示，转换式为：
Figure QLYQS_2
，
其中，Z i 表示图像中的特征点在世界坐标系下的深度值，K表示摄像机的内参矩阵，T C,W 表示世界坐标系与相机坐标系之间的转换矩阵；
通过转换式求得变换矩阵T C,W ，构建位姿估计中第i个点的误差项，表示为：
Figure QLYQS_3
，
把位姿估计的误差项进行求和，构建最小二乘问题，寻找误差最小时摄像机下的机器人位姿变换矩阵
Figure QLYQS_4
，公式为：
Figure QLYQS_5
，
其中，n表示图像中提取出的ORB特征点数量，P i 为第i个ORB特征点在世界坐标系下的坐标，
Figure QLYQS_6
表示通过变换矩阵得到的第i个ORB特征点在世界坐标系下的估计坐标；
根据机器人位姿变换矩阵
Figure QLYQS_7
求得第k帧时摄像机下的机器人位姿估计值，公式为：
Figure QLYQS_8
，
其中，
Figure QLYQS_9
表示视觉里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。
6.根据权利要求5所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述对激光雷达进行位姿估计求得激光雷达下的机器人位姿，具体为：
将激光雷达采集的图像的激光点云数据输入激光雷达里程计中，计算每一帧激光点云数据中每个点的曲率，并对每个点的曲率进行排序，选择曲率最大的2个点作为边缘点，曲率最小的4个点作为平面点；
对激光点云数据进行异常值排除；
使用基于特征的Scan-to-Scan匹配方法将异常值排除后的激光点云数据进行特征点匹配，获取边缘线和平面；
计算边缘点到边缘线的点线残差和边缘点到平面的点面残差；
计算残差总和，通过对残差总和构建最小二乘法求得极值，使用迭代法求解使得残差总和最小，并根据激光点云数据进行位姿估计，求得激光雷达下的机器人位姿。
7.根据权利要求6所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述曲率的计算公式为：
Figure QLYQS_10
，
其中，C表示曲率，
Figure QLYQS_11
表示激光雷达坐标系下第k帧激光点云数据中第i个点的坐标，S表示激光点云数据的集合；
所述异常值包括激光点云数据中平行于激光扫描平面的点和遮挡区域边界的点；
所述获取边缘线和平面，具体为：
设第k-1帧的激光点云数据为
Figure QLYQS_12
，第k帧的激光点云数据为
Figure QLYQS_13
；
在
Figure QLYQS_14
中找到与
Figure QLYQS_15
中边缘点i最近的边缘点j，再在另外的激光线束上找到距离边缘点j最近的边缘点l，连接边缘点j和边缘点l得到直线jl构成边缘线，并在
Figure QLYQS_16
中找到三个与
Figure QLYQS_17
中面点i最近的面点j，在同一激光线束上找到距离面点j最近的面点l，再在另外的激光线束上找到距离面点j最近的面点m，构成平面jlm；
记
Figure QLYQS_18
中边缘点i到
Figure QLYQS_19
中直线jl的距离为点线残差d ε ，
Figure QLYQS_20
中面点i到
Figure QLYQS_21
中jlm平面的距离为点面残差d ω ，则点线残差d ε 的计算公式为：
Figure QLYQS_22
，
点面残差d ω 的计算公式为：
Figure QLYQS_23
，
其中，
Figure QLYQS_24
表示激光雷达坐标系下第k-1帧激光点云数据中第j个点的坐标；
Figure QLYQS_25
表示激光雷达坐标系下第k-1帧激光点云数据中l个点的坐标；
Figure QLYQS_26
表示激光雷达坐标系下第k-1帧激光点云数据中m个点的坐标；
所述残差总和d = d ε + d ω ；
使用点线残差和点面残差构建与激光雷达坐标系下两帧之间位姿变换矩阵T L 的方程，表示为：
Figure QLYQS_27
，
其中，f ε ()表示点线残差的计算式；f ω ()表示点面残差的计算式；
Figure QLYQS_28
表示激光雷达坐标系下第k-1帧与第k帧之间机器人的位姿变换矩阵T L ；
使用残差总和构建最小二乘问题，表示为：
Figure QLYQS_29
，
其中，F(x)表示最小二乘问题的目标函数；
使用迭代法对最小二乘问题求极值：给定初始值x 0的坐标，对于第k-1次迭代，寻找增量
Figure QLYQS_30
，使得
Figure QLYQS_31
达到极小值；若
Figure QLYQS_32
足够小则停止，得到极小值F(x)下的变换矩阵
Figure QLYQS_33
；否则令
Figure QLYQS_34
，继续下一次迭代；其中，
Figure QLYQS_35
表示激光雷达坐标系下第k-1帧激光点云数据中第i个点的坐标，
Figure QLYQS_36
表示在取得极小值F(x)下的变换矩阵；
通过极小值下的变换矩阵
Figure QLYQS_37
求得第k帧时激光雷达下的机器人位姿估计值，公式为：
Figure QLYQS_38
，
其中，
Figure QLYQS_39
表示激光雷达里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。
8.根据权利要求7所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，利用n-1个三次函数线段拟合出n个时间帧的激光雷达输出，通过插值对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：
根据样条插值假设条件，利用已知的历史激光雷达的机器人位姿数据构建出n-1段样条函数S i (x)，表示为：
S i (x) = a i + b i (x - x i ) + c i (x - x i )2 + d i (x - x i )3 ,i = 1,2,…,n-1，
根据样条函数性质，列出四个等式约束，令
Figure QLYQS_40
，
Figure QLYQS_41
，求得样条函数S i (x)的曲线参数a i 、b i 、c i 、d i ，等式约束表示为：
Figure QLYQS_42
，
其中，x表示样条函数自变量为时间，x i 表示第i帧激光点云数据对应的时间，n表示激光点云数据的个数或者长度，h i = x i+1 - x i ，y i 表示第i帧时激光雷达下的机器人位姿估计值；
假设摄像机的采样频率为f c ，激光雷达的采样频率为f L ，根据已知的历史激光雷达的机器人位姿数据，利用三次样条函数来拟合机器人运动的轨迹关系，再使用摄像机的采样频率对该拟合函数进行插值处理，即利用样本函数S i (x)分别插值得到时间点为
Figure QLYQS_43
、
Figure QLYQS_44
、…、
Figure QLYQS_45
下的函数值
Figure QLYQS_46
，i=1,2,…,l，其中l为同一段时间内摄像机获得的帧数；插值后得到对应时刻的激光雷达的机器人位姿
Figure QLYQS_47
，即表示相机第i帧时对应的激光雷达的机器人位姿，实现摄像机和激光雷达数据的同步生成。
9.根据权利要求8所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述双线程回环检测包括视觉回环检测、激光雷达回环检测和过滤器；
所述视觉回环检测采用词袋模型进行检测，具体为：
采用机器学习的K-means++算法对词袋模型中的根节点进行k次聚类，形成第一层节点，对第一层节点进行聚类处理，得到第二层节点，依次聚类d次，形成一个深度为d，枝叶数为K d 的KD-树；
利用KD-树构造字典的单词，通过输入的图像，提取图像的特征矩阵，利用公式计算图像相似度，其中A K 表示第k帧图像的特征矩阵，A K-1表示第k-1帧图像的特征矩阵，则图像相似度计算公式为：
Figure QLYQS_48
，
其中，M表示特征矩阵的长度，m1表示特征矩阵的m1范数；
当两帧图像的特征矩阵完全相同时，则输出1；完全不同时则输出0；
将当前帧的特征矩阵与每层中间节点的聚类中心比较d次，根据当前帧与历史帧的相似度和设定阈值进行比较，判断当前帧图像是否发生回环现象；
所述激光雷达回环检测采用Scan-Context描述子以及相似距离来进行检测，具体为：
计算激光点云数据的描述子，将每一帧的激光点云数据按照径向与环向进行划分，划分为不同的扇形栅格，其中径向与环向数量分别为Nr与Ns，将激光点云数据转换为一个(Nr,Ns)的Scan-Context描述子矩阵，该描述子矩阵的元素值为所在扇形栅格中高度的最大值；
Scan-Context描述子矩阵中每一列向量表示一个描述子，通过计算相似距离描述两个描述子的相似程度，公式为：
Figure QLYQS_49
，
其中，I q 为当前帧激光点云数据，I c 为历史帧激光点云数据，
Figure QLYQS_50
为当前帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值，
Figure QLYQS_51
为历史帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值；
对历史中的关键帧进行相似距离比对，找到与当前帧最相似的描述子，最终找到对应的回环帧，判断当前帧是否发生回环现象；
将视觉回环检测与激光雷达回环检测的结果输入过滤器中，当检测结果均为当前帧的回环帧时，认定当前帧发生回环现象。
10.根据权利要求9所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿，具体为：
已知SLAM中的机器人状态方程为：
Figure QLYQS_52
，
其中A为状态转移矩阵，B为控制矩阵，H为观测矩阵，W∼N(0,Q)是过程噪声，V∼N(0,R)是观测噪声，X k 为k时刻的机器人位姿，Z k 为k时刻的机器人位姿估计值，U k-1为k-1时刻的控制输入，W k 为k时刻的过程噪声，V k 为k时刻的观测噪声值；
假设机器人初始状态以及每一时刻的噪声是互相独立的；
根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，对第一帧进行判断，若第一帧是第一次进行检测，则使用第一帧时摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值对卡尔曼滤波器进行初始化，初始值表示为
Figure QLYQS_53
且
Figure QLYQS_54
；
Figure QLYQS_55
为摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值；
根据上一时刻的机器人位姿估计值
Figure QLYQS_56
，预测当前时刻机器人位姿的先验估计值
Figure QLYQS_57
，公式为：
Figure QLYQS_58
，
计算先验估计值的协方差
Figure QLYQS_59
：
Figure QLYQS_60
，
由于先验估计与实际值存在误差，通过更新卡尔曼增益系数K k ，使得误差最小：
Figure QLYQS_61
，
根据卡尔曼增益系数计算出当前时刻机器人位姿的后验估计值
Figure QLYQS_62
以及协方差矩阵P k ，公式为：
Figure QLYQS_63
，
其中，P k-1为k-1帧的协方差矩阵，A T 为状态转移矩阵的转置，Q为过程噪声协方差，R为测量噪声协方差，H T 为观测矩阵的转置，
Figure QLYQS_64
表示当前时刻机器人位姿的后验估计值，即当前时刻误差最小的机器人位姿；
将计算结果的后验估计值与协方差矩阵保存，以便于下次的迭代计算。
CN202310084251.1A 2023-02-09 2023-02-09 基于激光点云和视觉slam的机器人位姿估计方法 Active CN115880364B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310084251.1A CN115880364B (zh) 2023-02-09 2023-02-09 基于激光点云和视觉slam的机器人位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310084251.1A CN115880364B (zh) 2023-02-09 2023-02-09 基于激光点云和视觉slam的机器人位姿估计方法

Publications (2)

Publication Number Publication Date
CN115880364A true CN115880364A (zh) 2023-03-31
CN115880364B CN115880364B (zh) 2023-05-16

Family

ID=85760914

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310084251.1A Active CN115880364B (zh) 2023-02-09 2023-02-09 基于激光点云和视觉slam的机器人位姿估计方法

Country Status (1)

Country Link
CN (1) CN115880364B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116452422A (zh) * 2023-04-24 2023-07-18 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116698046A (zh) * 2023-08-04 2023-09-05 苏州观瑞汽车技术有限公司 一种物业室内服务机器人建图定位和回环检测方法及系统
CN117173247A (zh) * 2023-11-02 2023-12-05 中国海洋大学 基于2D激光雷达与LightGBM的室外定位与构图方法及系统
CN117433511A (zh) * 2023-12-20 2024-01-23 绘见科技(深圳)有限公司 一种多传感器融合定位方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111076733A (zh) * 2019-12-10 2020-04-28 亿嘉和科技股份有限公司 一种基于视觉与激光slam的机器人室内建图方法及系统
CN112595322A (zh) * 2020-11-27 2021-04-02 浙江同善人工智能技术有限公司 一种融合orb闭环检测的激光slam方法
CN113103232A (zh) * 2021-04-12 2021-07-13 电子科技大学 一种基于特征分布匹配的智能设备自适应运动控制方法
CN114332233A (zh) * 2022-03-17 2022-04-12 北京理工大学 一种激光slam回环检测方法和系统
CN114419147A (zh) * 2021-11-16 2022-04-29 新兴际华集团有限公司 一种救援机器人智能化远程人机交互控制方法及系统
CN115240047A (zh) * 2022-08-04 2022-10-25 中国矿业大学(北京) 一种融合视觉回环检测的激光slam方法及系统
CN115359115A (zh) * 2022-08-29 2022-11-18 浙江工业大学 一种动态环境下基于多传感器的同时定位与建图方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111076733A (zh) * 2019-12-10 2020-04-28 亿嘉和科技股份有限公司 一种基于视觉与激光slam的机器人室内建图方法及系统
CN112595322A (zh) * 2020-11-27 2021-04-02 浙江同善人工智能技术有限公司 一种融合orb闭环检测的激光slam方法
CN113103232A (zh) * 2021-04-12 2021-07-13 电子科技大学 一种基于特征分布匹配的智能设备自适应运动控制方法
CN114419147A (zh) * 2021-11-16 2022-04-29 新兴际华集团有限公司 一种救援机器人智能化远程人机交互控制方法及系统
CN114332233A (zh) * 2022-03-17 2022-04-12 北京理工大学 一种激光slam回环检测方法和系统
CN115240047A (zh) * 2022-08-04 2022-10-25 中国矿业大学(北京) 一种融合视觉回环检测的激光slam方法及系统
CN115359115A (zh) * 2022-08-29 2022-11-18 浙江工业大学 一种动态环境下基于多传感器的同时定位与建图方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
曾庆化等: "视觉及其融合惯性的SLAM 技术发展综述", 《南京航空航天大学学报》 *
郭辰等: "基于多传感器融合的机器人室内建图方法研究", 《仪表技术》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116452422A (zh) * 2023-04-24 2023-07-18 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116452422B (zh) * 2023-04-24 2024-02-20 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116358532B (zh) * 2023-05-31 2023-09-26 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116698046A (zh) * 2023-08-04 2023-09-05 苏州观瑞汽车技术有限公司 一种物业室内服务机器人建图定位和回环检测方法及系统
CN116698046B (zh) * 2023-08-04 2023-12-01 苏州观瑞汽车技术有限公司 一种物业室内服务机器人建图定位和回环检测方法及系统
CN117173247A (zh) * 2023-11-02 2023-12-05 中国海洋大学 基于2D激光雷达与LightGBM的室外定位与构图方法及系统
CN117173247B (zh) * 2023-11-02 2024-02-02 中国海洋大学 基于2D激光雷达与LightGBM的室外定位与构图方法及系统
CN117433511A (zh) * 2023-12-20 2024-01-23 绘见科技(深圳)有限公司 一种多传感器融合定位方法
CN117433511B (zh) * 2023-12-20 2024-03-12 绘见科技(深圳)有限公司 一种多传感器融合定位方法

Also Published As

Publication number Publication date
CN115880364B (zh) 2023-05-16

Similar Documents

Publication Publication Date Title
WO2021233029A1 (en) Simultaneous localization and mapping method, device, system and storage medium
CN115880364B (zh) 基于激光点云和视觉slam的机器人位姿估计方法
CN111258313B (zh) 多传感器融合slam系统及机器人
CN113345018B (zh) 一种动态场景下的激光单目视觉融合定位建图方法
CN107301654B (zh) 一种多传感器的高精度即时定位与建图方法
CN102411778B (zh) 一种机载激光点云与航空影像的自动配准方法
US8199977B2 (en) System and method for extraction of features from a 3-D point cloud
CN112258600A (zh) 一种基于视觉与激光雷达的同时定位与地图构建方法
CN110223348A (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
JP6456141B2 (ja) 地図データの生成
CN113837277B (zh) 一种基于视觉点线特征优化的多源融合slam系统
CN112784873B (zh) 一种语义地图的构建方法及设备
CN114419147A (zh) 一种救援机器人智能化远程人机交互控制方法及系统
CN111123242B (zh) 一种基于激光雷达和相机的联合标定方法及计算机可读存储介质
CN113658337B (zh) 一种基于车辙线的多模态里程计方法
CN117197333A (zh) 基于多目视觉的空间目标重构与位姿估计方法及系统
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN115824190A (zh) 基于视觉与gps的目标船融合定位方法
CN115965712A (zh) 一种建筑物二维矢量图构建方法、系统、设备及存储介质
CN116045965A (zh) 一种融合多传感器的环境地图构建方法
CN115655291B (zh) 激光slam闭环建图的方法、装置、移动机器人、设备及介质
CN107194334B (zh) 基于光流模型的视频卫星影像密集匹配方法及系统
CN106228593A (zh) 一种图像密集匹配方法
Dong et al. PVE-LIOM: Pseudo-Visual Enhanced LiDAR-Inertial Odometry and Mapping
CN118521653B (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