CN109974707A - 一种基于改进点云匹配算法的室内移动机器人视觉导航方法 - Google Patents

Abstract

本发明涉及基于改进点云匹配算法的室内移动机器人视觉导航方法，属于机器人视觉导航领域。该方法包括：S1：校准Kinect相机，通过外部参数对齐多个点云；S2：得到离线校准的外部参数形成的变换矩阵，计算出多视点变换矩阵；S3：获取周围环境的ORB特征点进行特征提取和匹配；S4：估算相邻图像间相机运动构建局部地图，使用RANSAC算法消除误匹配，对获取的特征点进行PNP求解；S5：引入闭环检测机制确保机器人得到全局一致的轨迹和地图；S6：结合闭环检测机制，采用基于局部与全局特征点的相机位姿闭环约束点云优化算法，抑制系统的累积误差。本发明能够降低计算量，实现机器人对实时性的要求。

Description

一种基于改进点云匹配算法的室内移动机器人视觉导航方法

技术领域

本发明属于室内移动机器人视觉导航领域，涉及一种基于改进点云匹配算法的室内移动机器人视觉导航方法。

背景技术

随着机器人技术的不断发展，人类对移动机器人要求也在不断提高。同时定位和映射 (Simultaneous Localization and Mapping，SLAM)是机器人能否真正实现智能化的关键技术之一。视觉传感器可以获得更加丰富的环境信息，其中，三维场景重建是机器人视觉应用中的一个重要问题，比如地图构建、环境识别、增强现实、以及SLAM等。在点云匹配中，局部匹配已被证明能够生成准确的重建模型，已经提出了几种精度更高的局部匹配方法。有三种类型的局部配准方法：迭代最近点(ICP)，陈氏方法和遗传算法。

现在常用的局部匹配方法是ICP方法，ICP方法由Besl等人提出有效匹配三维形状，包括自由曲线和曲面。随后的时间内研究人员对ICP方法的进行改进。Rusinkiewicz等人基于法线空间的均匀采样得出ICP变体，这可以为相对较小的场景和稀疏特征提供相对较好的收敛结果。Low提出线性最小二乘问题逼近非线性最小二乘优化问题来解决点到面的ICP算法，使用近似方法来提高确定最优解的效率。Serafin和Grisetti提出了一种新的优化成本函数，不仅包括点到点距离，还包括表面法线或切线；不仅增加了收敛速度，而且具有更好得鲁棒性。李恒宇等人提出了一种基于图像的RGB-D图像拼接方法，首先进行校准找出深度图和彩色图像之间的变换矩阵，然后对齐深度图和彩色图像。但是上述方法计算量大并且难以满足机器人实时性。

发明内容

有鉴于此，本发明的目的在于提供一种基于改进点云匹配算法的室内移动机器人视觉导航方法，用于降低计算量，从而实现机器人对实时性的要求。

为达到上述目的，本发明提供如下技术方案：

一种基于改进点云匹配算法的室内移动机器人视觉导航方法，具体包括以下步骤：

S1：使用平面棋盘格校准Kinect相机，通过Kinect相机的外部参数对齐多个点云；

S2：得到离线校准的外部参数形成的变换矩阵，计算出多视点变换矩阵，用于全局匹配；

S3：使用Kinect相机获取周围环境的ORB特征点进行特征提取和匹配；

S4：估算相邻图像间相机运动构建局部地图，使用消除误匹配，对获取的特征点进行PNP 求解，用于机器人相邻位姿的增量估计；

S5：引入闭环检测机制确保机器人得到全局一致的轨迹和地图；

S6：结合闭环检测机制，采用基于局部与全局特征点的相机位姿闭环约束点云优化算法，用于抑制系统的累积误差。

进一步，所述步骤S1具体包括：通过旋转RGB-D相机或平移机器人位置得到棋盘不同角度的对应参数，变换矩阵从正向运动学中分析得到：

其中，T_i ⁰为相机正向运动方程，为相机的第k-1和k个关键帧之间的变换矩阵，为相机转向角，为相机倾斜角，平台与棋盘转向角，平台与棋盘倾斜角度，α₂为方程偏导数，θ_p和θ_t分别表示相机转向角度和倾斜角度。

进一步，所述步骤S2具体包括：

S21：得到离线校准的外部参数形成的变换矩阵为：

其中，K是固定值，旋转矩阵，是机器人坐标到相机坐标的平移矢量，[u v 1]^T是二维平面上2D像素坐标，[x y z 1]^T是全局坐标系中三维点坐标，s是与深度图片尺寸比，从Kinect相机深度图像中获得；

S21：计算多视点变换矩阵：

基于多视点摄像机标定生成多视角变换矩阵的离线标定方法：

其中，P_r是棋盘上坐标系的关键点坐标，P_i为第i帧点云图，为第i帧处棋盘特征点矩阵，是将P_r映射到P_i，是将P_r映射到P_i的变换矩阵，通过使用Kinect标定获得不同视角的变换矩阵；假设定义F₀全局坐标为W，并把所有的P_i都转换为F₀，得到：

其中，F₀为初始坐标，为平移矢量，为初始平移矢量，是旋转矩阵；

定义旋转矩阵用于将P_i变换到F₀坐标，平移矢量计算出相机视图的所有变换矩阵为：

进一步，所述步骤S5中，引入的闭环检测机制具体包括：引用机器人位姿优化的目标函数：

其中，x_k表示第k时刻机器人位姿即pose节点，y_j表示第k时刻观测到的路标即point 节点，z_k,j表示x_k和y_j之间的约束；在闭环检测过程中，e(x_k,y_j,z_k,j)为两点之间所产生的向量误差，即z_k,j满足约束的程度，当其值为0时，表示完全满足约束条件；g(x_k,y_j)表示第k 时刻的位姿节点和路标之间的函数，C表示二维矩阵，Ω_kj表示所有位恣节点和路标节点；

其次，在机器人运动过程中，误差函数符合高斯分布函数，将视觉SLAM等效为一个最小二乘问题，利于李群代数模型求解出目标函数与各节点的导数F_X和F_y，求解方程为：

e(x+δ_x,y+δ_y)＝e(x,y)+F_xδ_x+F_yδ_y

其中，δ_x为机器人位姿误差，δ_y为机器人路标误差，为李群代数，e_i为代价函数，矩阵H为稀疏特征矩阵，F_X和F_y分别表示相机运动和机器人运动后像素变化；通过使用变换矩阵对稀疏特征点进行匹配，包含所有SLAM求解变量与边缘值的问题；通过多次迭代得到适合目标函数的x值；通过上述转化把SLAM问题转换成节点和边，节点包含pose节点和point节点，表示优化变量；边为不同节点的约束，表示为误差约束条件，point-pose约束取决于系统中闭环产生的约束，pose-point取决于相机观测产生的约束。

进一步，所述步骤S5中，在闭环检测中为保证机器人闭环检测实时性设定相应性阈值，其中相似性计算函数为：

其中，V_l表示上一帧图像加权量，V_c表示当前帧加权向量，X越大说明图像相似性越高。因此，当X大于一定阈值则舍弃当前帧。

进一步，所述步骤S6中，对于稠密点云地图，基于局部与全局特征点的相机位姿闭环约束点云优化算法，具体步骤为：

S61：根据帧间关联度选出关键帧序列，机器人局部状态与关键帧进行匹配；

S62：根据求得的相机多视角变换矩阵，得到优化后的相机位姿，对获取的点云进行坐标变换，初步实现点云融合；

S63：闭环检测采用K均值分类方法构建视觉词典树，提高闭环识别率；

S64：闭环检测完成后，遍历关键帧与当前帧进行相似变换矩阵Sim3求解与优化，如果闭环成功则跳出Sim3，得到对应相似性变换矩阵为：

其中，SO(3)为特殊正交群，R为点云对准时旋转矩阵，t为平移向量，s是与深度图片尺寸比，从Kinect相机深度图像中获得；

S65：点云优化：使用因子图优化算法对点云进行优化，进行点云拼接融合，实现点云优化，构建精确的三维点云地图。

本发明的有益效果在于：本发明提出的是一种改进的校准方法，通过使用深度相机标定技术计算出离线状态下不同角度控制点之间的变换矩阵，然后在匹配过程中直接使用这些变换矩阵。通过离线校准，可以获得相机在固定位置及固定角度的相对变换矩阵并用于全局匹配，由于仅需要对每个输入点云数据进行简单的坐标变换计算，具有较低的计算量，可以满足机器人实时性要求。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为本发明所述室内机器人视觉导航方法示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

请参阅图1，图1为一种基于改进点云匹配的室内移动机器人视觉导航方法，具有包括：首先使用平面棋盘格校准Kinect相机，通过Kinect相机的外部参数对齐多个点云，得到由外部参数离线校准后形成的多视点变换矩阵，用于全局点云匹配。其次，在构建地图的过程采用计算量小、实时性高的ORB特征进行特征提取和匹配。利用RANSAC算法对可能存在的误匹配点进行剔除完成初始匹配，对获取的特征点进行PNP求解，用于机器人相邻位姿的增量估计。最后，在后端优化中，结合闭环检测机制提出了一种点云优化算法，用于抑制系统的累积误差，确保机器人得到全局一致的轨迹和地图。

实施例：根据如下表1、2取值进行不同算法直接的效果比较。

表1不同算法之间的RMS比较结果

	ICP	Super4PCS	FRG	本发明算法
					fr1/xyz	0.225	0.215	0.197	0.195
fr2/rpy	0.354	0.352	0.307	0.289
					fr2/pioneer_slam	0.741	0.844	0.862	0.704
fr2/coke	0.243	0.356	0.235	0.209
					Fr3/teddy	0.394	0.416	0.445	0.359

表2不同算法之间每帧处理视觉比较结果

	FRG	ORB-SLAM	本发明算法
				fr1/xyz	0.009	0.015	0.005
fr2/pioneer_slam	0.461	0.556	0.453
				fr2/coke	0.089	0.106	0.096
Fr3/teddy	0.049	0.056	0.035

表1表示本发明采用算法和Super4PCS方法和FRG算法以及传统ICP方法实时匹配对比结果。从表1可以得到由于模型不确定性和相机校准误差等原因的存在，本发明提出的算法相对于Super4PCS算法和传统ICP方法分别具有较低的RMS误差。在数据集fr2/pioneer_slam 中，由于场景比较复杂，存在大量的无效数据，降低了所提出的方法和所比较的方法的匹配性能，但是本文算法依然高于所对比的算法，算法在实时性也优于所对比算法。

表2表示不同算之间每帧处理时间对比结果，本发明所述方法由于事先对相机进行标定，平均处理每帧图片仅用0.201s，FRG方法平均每帧处理时间为0.649s，而Super4PCS算法平均每帧时间消耗约是本文算法的3倍。在没有其他外部加速的情况下，不能保证v-SLAM实时性和精准性。本发明采用算法在后端不需要使用RANSAC算法剔除误匹配，在使用相机离线校准的参数后点云地图大约以9.6帧/秒速度进行更新，可以满足v-SLAM实时性。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，该方法具体包括以下步骤：

S1：使用平面棋盘格校准Kinect相机，通过Kinect相机的外部参数对齐多个点云；

S2：得到离线校准的外部参数形成的变换矩阵，计算出多视点变换矩阵，用于全局匹配；

S3：使用Kinect相机获取周围环境的ORB特征点进行特征提取和匹配；

S4：估算相邻图像间相机运动构建局部地图，使用随机抽样一致算法(Random sampleconsensus，RANSAC)消除误匹配，对获取的特征点进行PNP(Perspective-N-Point)求解，用于机器人相邻位姿的增量估计；

S5：引入闭环检测机制确保机器人得到全局一致的轨迹和地图；

S6：结合闭环检测机制，采用基于局部与全局特征点的相机位姿闭环约束点云优化算法，用于抑制系统的累积误差。

2.根据权利要求1所述的一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，所述步骤S1具体包括：通过旋转RGB-D相机或平移机器人位置得到棋盘不同角度的对应参数，变换矩阵从正向运动学中分析得到：

其中，T_i ⁰为相机正向运动方程，为相机的第k-1和k个关键帧之间的变换矩阵，为相机转向角，为相机倾斜角，平台与棋盘转向角，平台与棋盘倾斜角度，α₂为方程偏导数，θ_p和θ_t分别表示相机转向角度和倾斜角度。

3.根据权利要求2所述的一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，所述步骤S2具体包括：

S21：得到离线校准的外部参数形成的变换矩阵为：

其中，K为固定值，为旋转矩阵，为机器人坐标到相机坐标的平移矢量，[u v 1]^T是二维平面上2D像素坐标，[x y z 1]^T是全局坐标系中三维点坐标，s是与深度图片尺寸比，从Kinect相机深度图像中获得；

S21：计算多视点变换矩阵：

基于多视点摄像机标定生成多视角变换矩阵的离线标定方法：

其中，P_r是棋盘上坐标系的关键点坐标，P_i为第i帧点云图，为第i帧处棋盘特征点矩阵、是将P_r映射到P_i，T_r ⁱ是将P_r映射到P_i的变换矩阵，通过使用Kinect标定获得不同视角的变换矩阵；假设定义F₀全局坐标为W，并把所有的P_i都转换为F₀，得到：

其中，F₀为初始坐标，为平移矢量，为初始平移矢量，是旋转矩阵；

定义旋转矩阵用于将P_i变换到F₀坐标，平移矢量计算出相机视图的所有变换矩阵为：

4.根据权利要求3所述的一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，所述步骤S5中，引入的闭环检测机制具体包括：引用机器人位姿优化的目标函数：

其中，x_k表示第k时刻机器人位姿即pose节点，y_j表示第k时刻观测到的路标即point节点，z_k,j表示x_k和y_j之间的约束；在闭环检测过程中，e(x_k,y_j,z_k,j)为两点之间所产生的向量误差，即z_k,j满足约束的程度，当其值为0时，表示完全满足约束条件；g(x_k,y_j)表示第k时刻的位姿节点和路标之间的函数，C表示二维矩阵，Ω_kj表示所有位恣节点和路标节点；

其次，在机器人运动过程中，误差函数符合高斯分布函数，将视觉同步定位与映射(simultaneous localization and mapping,SLAM)等效为一个最小二乘问题，利于李群代数模型求解出目标函数与各节点的导数F_X和F_y，求解方程为：

e(x+δ_x,y+δ_y)＝e(x,y)+F_xδ_x+F_yδ_y

其中，δ_x为机器人位姿误差，δ_y为机器人路标误差，为李群代数，e_i为代价函数，矩阵H为稀疏特征矩阵，F_X和F_y分别表示相机运动和机器人运动后像素变化；通过使用变换矩阵对稀疏特征点进行匹配，包含所有SLAM求解变量与边缘值的问题；通过多次迭代得到适合目标函数的x值；通过上述转化把SLAM问题转换成节点和边，节点包含pose节点和point节点，表示优化变量；边为不同节点的约束，表示为误差约束条件，point-pose约束取决于系统中闭环产生的约束，pose-point取决于相机观测产生的约束。

5.根据权利要求4所述的一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，所述步骤S5中，在闭环检测中为保证机器人闭环检测实时性设定相应性阈值，其中相似性计算函数为：

其中，V_l表示上一帧图像加权量，V_c表示当前帧加权向量，X越大说明图像相似性越高。因此，当X大于一定阈值则舍弃当前帧。

6.根据权利要求5所述的一种基于改进点云匹配算法的室内移动机器人视觉导航方法，其特征在于，所述步骤S6中，基于局部与全局特征点的相机位姿闭环约束点云优化算法，具体步骤为：

S61：根据帧间关联度选出关键帧序列，机器人局部状态与关键帧进行匹配；

S62：根据求得的相机多视角变换矩阵，得到优化后的相机位姿，对获取的点云进行坐标变换，初步实现点云融合；

S63：闭环检测采用K均值分类方法构建视觉词典树，提高闭环识别率；

S64：闭环检测完成后，遍历关键帧与当前帧进行相似变换矩阵Sim3求解与优化，如果闭环成功则跳出Sim3，得到对应相似性变换矩阵为：

其中，SO(3)为特殊正交群，R为点云对准时旋转矩阵，t为平移向量，s是与深度图片尺寸比，从Kinect相机深度图像中获得；

S65：点云优化：使用因子图优化算法对点云进行优化，进行点云拼接融合，实现点云优化，构建精确的三维点云地图。