基于kinect的地面移动机器人即时定位与地图构建（SLAM）研究

康玉溪,潘海彬,毛新宇,欧阳盟

江苏大学机械工程学院，江苏 镇江 212000

摘 要针对地面移动机器人即时定位与地图构建问题（SLAM，Simultaneous Localization and Mapping），设计一种基于Kinect摄像头的ROS机器人，建立合理的数学模型，并改进经典点云配准（ICP）算法，通过深度图像检测点云地面，经过平面拟合获得高精度的空间信息，最后在简单点云地图模型的基础之上，完成机器人导航与环境认知的地图构建方法。实验表明，改进后的点云配准算法精度高，能有效抑制噪声和数据缺失对配准精度的影响，更好地实现地图的即时构建。

关键词：Kinect摄像头；SLAM；ROS；ICP算法；点云地图；

0引言

即时定位与地图构建（SLAM）技术，赋予了机器如人一样的的能力，是移动载体(如地面移动机器人等)在未知环境情况的条件下，进行自主定位、创建地图，并最终完成导航的重要技术,但由于该技术相关应用领域不断拓展，社会对SLAM系统实时性和精度也提出了越来越高的要求。而视觉SLAM[1]则是SLAM中的一个重要研究领域，由微软开发的Kinect摄像头，就具备了采集图像色彩（RGB）信息以及深度（Depth）信息的能力，且在价格、性能、便携程度上都有不小的优势。本文即是研究以Kinect摄像头为传感器，建立在机器人操作系统（ROS）上的地面移动机器人为主，并通过选用改进后的经典点云配准算法（迭代最近法，也称ICP算法），从而提升整个机器人系统在地图构建上的效率和精准度。

1 视觉SLAM概述

SLAM技术，简单来说，就是希望机器人与人一样，能在未知的环境中通过传感器了解周遭环境，在本文中，摄像头即是机器人感知外界的“眼睛”，可通过摄像头对已经观测的环境进行实时地图的创建，从而完成机器人实时导航、路径规划等复杂问题。在摄像头的选择上，一般有单目、双目、深度三种类型，本文中选用的Kinect摄像头，则可以采集图像色彩信息（RGB）和深度信息（Depth），合称RGB-D图像[2]信息，能很好地满足所需达到的要求。

1.1 视觉SLAM原理

在经典视觉SLAM系统中，除去用于获取深度图像信息，并对信息进行初步处理的传感器外（即本文中的Kinect摄像头），一般如图1所示，分为前端的视觉里程计[3]，后端的非线性优化[4]，回环检测[5]以及地图创建四个部分。

图1 经典视觉SLAM框架的基本组成

其中，视觉里程计的任务是估计两幅相邻图像之间摄像头的运动，同时对该过程的局部地图进行创建。后端则是负责接收不同时刻前端测量得到的摄像头的位置和姿态，以及回环检测反馈的信息，并对它们进行调整、优化，从而得到地面移动机器人运动的轨迹和全局地图。回环检测主要解决位置估计随时间发生漂移的问题，实际上是一种计算图像数据相似性的算法。至于建图，导航时常选用稠密地图，即按照某种分辨率，由许多小块组成，在二维度量地图中体现为许多个正方形小格子，在三维度量地图中则是体现为许多小立方体[6]。

1.2 视觉SLAM的数学表述

SLAM问题的本质是“定位”以及“建图”，即指在环境未知的情况下，要使机器人确定自身所在的位置，同时要通过运动轨迹，建立实时地图。而对于视觉SLAM问题，我们需要用数学公式描述的同样是有两个方面，一是相机在未知环境中的运动是如何变化的，二是相机在运动过程中观测到的情况（如障碍物、路标等等）。

针对以上两个方面的问题，我们可以假设相机在各个时刻所处的位置为,因相机是在空间中运动，所以我们对其位置及姿态的估计，可以通过三个位置坐标和一个角度进行表示，即：

(1)

其中，a,b,c分别表示相机在三个轴上的位置，而则是代表相机的角度。同时，输入两个相邻时刻，相机位置和角度的变化量，即：

(2)

最终，就能得到相机在时刻时的具体运动方程：

(3)

对于第二个方面，可以描述为相机观测到空间中某点P之后，得到图像中像素的问题，这牵涉到对相机模型的描述，我们通过对图2所示相机观测模型进行几何建模，从而利用数学方法描述这一问题。

图2 简单相机观测模型

首先，假设空间中某点P的坐标为,设P点在某一物理成像平面的像点的坐标为，同时假设在物理成像面上存在着一个像素平面，通过成像原理进行计算，最终就可以得到P点的像素坐标。

将这一原理运用到相机的观测上，就可以将一个真实世界的坐标点转化到相机坐标系中，再进行归一化处理，就能得到点P在归一化平面上的投影[7]：

(4)

其中，R是相机的旋转矩阵（Rotation matrix）[8]，t是相机的平移向量（Translation vector）[9]，它们共同描述了相机的位置和姿态（也就是相机的位姿），同时也被称为相机的外参，而相机的内参，一般来说是固定的，某些相机在购买时，厂商就会直接告知内参数，但在不知晓内参的情况下，我们就需要对相机进行标定（张正友标定法）

[10]。

2 系统设计

2.1 系统整体框架

图3 系统整体框架

如图3所示，本次设计中，主要框架可分为主控、移动、传感（Kinect V1.0摄像头）三个模块，主控模块负责提供电源驱动底盘以及Kinect摄像头，同时也负责接收并处理前端视觉里程计以及摄像头观测到的RGB-D图像数据，完成定位以及建图，从而进一步完成导航功能。

移动模块选用麦克纳姆轮，可使机器人达到全向移动的目的，驱动麦克纳姆轮的电机选用编码电机。同时将主控模块的设备驱动以及建图处理两个功能分开，从而减小主板使用压力，空出更多接口，提高了系统的可扩展性，在主板上，选用了树莓派（4B/4G）作为主板，同时选用可进行分布式控制，且以嵌入式单片机（STM32）为主控器的设备驱动板，供电则是使用4000mAh的大容量锂电池组，相机则是本文选用的Kinect v1.0摄像机。

2.2 ICP算法改进

迭代最近点（ICP，Iterative Closest Point）算法，是当前最常使用的点云图像配准算法[11]。但由于算法本身计算效率不高，面对实际测量中大量的点云图像数据时，会花费大量的时间，很大程度上影响了地图的创建，所以无法直接运用于实际检测环境中，需要对其进行些许改进。

在实际应用中，假设有两组配对好的3D点：

(5)

然后，定义两组匹配好的点的质心为：

(6)

同时，需要一组相机的旋转矩阵R以及平移向量t，使得：

(7)

确定质心位置之后，可以计算出每个点的去质心坐标，即可得到：

(8)

随后可计算旋转矩阵R：

(9)

在得到相机的旋转矩阵R之后，就可以很轻易地得到相机的平移向量：(10)

3实验结果与分析

通过图4可以看出，根据本次设计方案，以Kinect V1.0作为传感器，树莓派(4B/4G)作为主控板的地面移动机器人，而在将点云数据转化为二维数据之后，就可以满足实时定位与建图的功能，且能通过Rviz查看相机观测环境以及实际建图情况。

a.初始时建图                      b.移动一段距离后的建图

图4 Rivz下Kinect机器人的建图情况

同时，我们可以编写在ROS系统中使用键盘控制机器人移动的程序，在终端运行之后可实时操控机器人进行移动，从而完成观测建图的目的。但由于树莓派带宽的影响，我们在使用PC机远程操控、观测、创建地图时，需要降低机器人移动的速度，否则就会出现特征点缺失、创建的地图存在断点等问题。当然，如果在树莓派上外接一块屏幕，运行时还是较为流畅的。

4结语

本文以Kinect摄像头作为机器人感知外界的传感器，同时选用改进后的ICP算法对点云进行匹配。实验表明，通过该方法设计的地面移动机器人系统可以实现室内环境的建图以及导航。但Kinect摄像头作为一种RGB-D摄像头，其用于检测深度信息的红外受外界环境因素的影响很大，在室外应用时，效果并不佳。同时，摄像头在采集视野上的局限，也使得构建完整地图的速度并不算很快。因此，本系统在算法以及结构搭建上，依旧还有进一步优化的空间。

参考文献：

[1] 毕浩博,谭丹丹,兰崇嘉,高丽.视觉SLAM与构建地图方法概述[J].电脑知识与技术.2020(12).

[2]孟特,李富才,刘邦彦,张凤生.基于RGB-D图像的视觉SLAM算法研究[J].青岛大学学报(自然科学版),2022,35(01):79-84+92.

[3] 魏麟,陈华杰,王彪,陈玉.SLAM技术中视觉里程计的特征匹配方法研究[J].电子技术与软件工程. 2020(18)．

[4]季晨,宋燕燕,秦军,敖甜甜.非线性优化同时定位与地图创建问题[J].软件工程,2020,23(09):39-42.DOI:10.19644/j.cnki.issn2096-1472.2020.09.011.

[5]姚亚健. 基于改进位姿估计与回环检测算法的移动机器人SLAM研究[D].湖北工业大学,2021.DOI:10.27131/d.cnki.ghugc.2021.000363.

[6]高翔,张涛．视觉SLAM十四讲:从理论到实践[M]．北京:电子工业出版社,2017.

[7]牛家旭,孟真.基于卷积神经网络的的室内机器人视觉定位算法[J].信息技术与信息化,2019(03):65-67.

[8] Lichti D D. Self-calibration of a 3D range camera[J]. The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, 2008, 37: 927-932.

[9] Papaioannou S, Wen H, Markham A, et al. Fusion of Radio and Camera Sensor Data for Accurate Indoor Positioning[C]. IEEE, International Conference on Mobile Ad Hoc and Sensor Systems. IEEE, 2015: 109-117.

[10]卢英,王慧琴,佟威,李俊杰.基于Harris-张正友平面标定法的摄像机标定算法[J].西安建筑科技大学学报(自然科学版),2014,46(06):860-864+870.DOI:10.15986/j.1006-7930.2014.06.017.

[11]戴静兰,陈志杨,叶修梓.ICP算法在点云配准中的应用[J].中国图象图形学报,2007(03):517-521.

作者简介：

康玉溪，生于2000年6月，男，汉族，贵州毕节人，江苏大学本科在读，测控技术与仪器方向

【基金项目】本文系江苏大学江苏大学2020年度大学生创新创业训练计划项目，项目编号：202010299144Y