同步定位与建图(Simultaneous Localization and Mapping，SLAM)作为移动机器人技术的核心，决定了移动机器人是否能够实现完全自主的关键。近年来，由于在二维平面内的SLAM技术的成熟及其应用限制，针对三维环境的SLAM研究越来越多。三维SLAM构建的地图模型接近真实环境，也包含了更多的环境信息，能够让机器人在三维环境中进行定位与路径规划，使机器人在三维环境中的自主移动成为可能。随着越来越多的三维SLAM算法被提出，三维SLAM面临着地图构建精度、实时性和缺乏语义模型等难题，也出现了许多解决方案。
室内环境多为结构化环境，具有许多平面区域，许多研究者据此提出了基于平面特征的SLAM算法，建立基于平面特征的地图。平面地图结构紧凑且具有语义模型的优点，形象地反映了室内环境结构化的特点，不仅可以用来提取语义地图，进行基于语义信息的导航，还可以用来辅助抓取等任务的实现。
研究者们提出了许多基于平面特征的SLAM算法。Weingarten和Siegwart[1]提取了基于扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的平面特征SLAM。使用移动机器人上的旋转激光雷达获取点云数据，采用区域增长法分割平面，用SP-model[2]表示平面特征，使用基于EKF的SLAM框架建立三维平面地图。Lee等[3]提出了一种在室内环境下从有噪声的RGB-D传感器中提取平面特征并建立三维地图的方法。该方法中的平面特征参数使用球坐标表示，同时使用点云中点的位置和误差估计平面的空间误差，显示了优于其他平面表现形式的建图效果。Kaess[4]提出了一种无限平面的最少参数表示形式，通过将齐次化的参数映射到四元数来表示无限平面，该表示方法非常适合进行最小二乘优化，如使用ISAM递增优化算法[5]对位姿图进行优化求解, 分别在仿真环境、真实环境中使用该表示方法进行了平面建图实验，实现了快速稳定的平面地图创建。但平面特征提取和匹配耗费了大量的计算时间，可以进一步进行改进。
尽管平面特征广泛存在于室内环境下，但由于传感器测距范围和视角的限制，往往获取很少的平面特征，同时获取的平面特征中往往存在平行的平面，仅仅使用平面来进行位姿估计常常出现失败的情况。因此，结合平面特征和其他特征的位姿估计引起研究者的兴趣，如平面与线结合、平面与点结合等。
为了解决仅使用平面特征进行SLAM带来的问题，Trevor等[6]使用RGB-D传感器和激光雷达，基于平面特征和直线特征进行位姿估计，利用激光雷达的常测距距离和大视角来弥补RGB-D传感器小视角短距离的缺陷，构建基于直线和平面特征的地图。Taguchi等[7]提出了基于点和平面特征的SLAM系统，使用点和平面的组合来进行位姿估计，一个基于RANSAC算法的配准算法用于估计相机的运动。但是该系统的运算量过大，同时不能有效地对重复纹理环境进行建图。在Taguchi等[7]的基础上，Ataer-Cansizoglu等[8]提出了一种基于预测的相机位姿跟踪算法，实现特征的快速匹配，提高了构建地图的效率，并能够很好地对重复纹理环境进行建图，构建基于点和平面特征的混合地图。Salas-Moreno等[9]提出了一种Dense Planar SLAM算法，同时使用带有边界的平面点和非平面点，建立密集的环境地图。该算法可以检测、拼接、压缩室内结构环境下的平面，而且通过使用GPU(Graphics Processing Unit)，能够达到实时建图效果。Salas-Moreno等[9]同时展示了该平面区域SLAM算法在人工智能领域的潜在应用前景。
因此，目前多数基于平面特征的SLAM系统存在着不能构建大范围地图、可靠性差和计算量大的问题。在前期研究[10]的基础上，本文同样使用点和平面特征进行位姿估计，构建大范围地图，提高SLAM的可靠性和实用性。但是不同的是，构建的是基于点和平面特征的混合地图。以所提出的SLAM算法为核心，搭建室内导航系统，并进行室内导航实验，分析所提出的SLAM算法的性能。
1 护理机器人样机护理机器人的概念设计如图 1所示。为了使护理机器人样机具备移动能力，设计了全向移动底盘。机器人具有里程计、激光雷达、IMU、Kinect和超声波等传感器。通过里程计可以使用航迹推算算法估计机器人的运动。Kinect固定在机器人顶部，作为SLAM的传感器。同时，为了能够实现抓取等对环境物体的操作任务，在服务机器人上集成了7自由度模块化机械臂。护理机器人样机如图 2所示，表 1列举出了样机指标，其他传感器包括里程计、IMU、激光雷达、超声波。
|
| 图 1 护理机器人概念设计图 Fig. 1 Care robot conceptual design sketch |
|
| 图 2 护理机器人样机 Fig. 2 Care robot prototype |
| 指标 | 数值 |
| 整机质量/kg | 120 |
| 外形尺寸/(m×m×m) | 0.9×0.65×1.35 |
| 机械臂自由度 | 7 |
| Kinect安装高度/m | 1.2 |
2 室内导航系统总体架构
采用ROS[11]导航程序包框架，搭建护理机器人的室内导航系统。针对不同的移动机器人平台，需要配置里程计信息、传感器输入和底盘控制器，同时需要提供定位和地图。室内导航系统的架构如图 3所示，由EKF位姿估计、定位与建图、障碍物地图、路径规划与运动控制几个部分组成。
|
| 图 3 室内导航系统架构图 Fig. 3 Indoor navigation system architecture diagram |
本文室内导航系统的定位与建图部分基于乔治亚理工学院的平滑建图工具(Georgia Tech Smoothing and Mapping，GTSAM)[12]。图 4概述了SLAM系统的框架，主要由特征提取、数据关联、因子图的建立和优化、地图的更新与改进等部分组成。
|
| 图 4 SLAM系统框架图 Fig. 4 SLAM system framework diagram |
SLAM系统基于点和平面2种特征，特征点提取使用ORB(Oriented FAST and Rotated BRIEF)特征点检测[13]，特征平面提取使用基于直线元素的快速平面提取方法[14]。每个数据帧中包含机器人当前的姿态估计、特征点信息、特征平面信息。特征点的信息有特征描述符、三维位置及其协方差，即
|
(1) |
式中：Pi为所提取的特征点；di为特征点的描述符；Ii(u, v)为特征点在图像中的位置，u、v分别为水平、垂直位置；pi(x, y, z)为特征点在相机坐标系下的三维位置；Σi为三维位置的协方差。
特征平面的信息有平面参数、平面曲率和表示平面区域的内点点云，即
|
(2) |
式中：Lj为所提取的特征平面；ξj(n, d)为平面参数，n、d分别为平面法向量、原点到平面的距离；κj为平面曲率；cj为包含平面的所有内点的点云。
由于平面内点的数量比较多，使用点云库(Point Cloud Library，PCL)[15]中的体素滤波器(voxel grid filter)对内点进行下采样，控制平面内点的数目。
在基于图优化的SLAM系统中，需要根据某一准则确定关键帧，以减少数据帧的数量，降低内存使用量和系统的计算量，提高定位与建图的效率。关键帧的确定通常根据相对位姿[16]或观测信息变化量[17]进行决策。所提出的SLAM系统的关键帧由机器人当前数据帧与上一关键帧的相对位姿进行判断，设定相对平移阈值和相对旋转阈值，当相对平移大于阈值或相对旋转大于阈值时，将当前数据帧作为关键帧。在实验验证中，选取平移阈值为0.1 m，旋转阈值为10°。
在SLAM过程中，需要找到当前观测值与地图中路标的一一对应关系，即数据关联问题[18]。数据关联的结果决定着SLAM系统定位和建图的效果，也影响SLAM系统的运算时间。为了实现观测特征与地图路标快速可靠的匹配，所提出的SLAM系统使用基于解释树[19]的数据关联方法。
使用GTSAM工具中的iSAM2[20]库进行图优化，根据优化后的结果，更新定位信息和地图信息。构建的环境地图模型为基于点和平面特征的混合地图。由于传感器视角和距离的限制，在地图构建过程中，往往会有属于同一环境平面区域的平面特征被重复地添加到地图中，导致地图中存在重复的平面路标，不仅破坏了SLAM系统的建图的效果，同时也影响定位精度。因此，需要检测地图中的重复平面路标，并将重复的平面路标合并，对地图进行改进。
3.2 数据关联方法本文所使用的数据关联方法基于解释树(interpretation tree)，其主要思想是考虑所有的匹配可能性，建立解释树，再使用特征之间的约束来剔除错误的子树。如图 5所示，{F1，F2，…，Fm}为观测特征，{L1，L2，…，Ln}为地图中的路标，用“*”代表没有在地图中找到对应的匹配路标。根据特征点或特征平面的所有可能数据关联结果建立解释树，再根据特征之间的约束关系，逐级向下删除错误的子树。约束关系可以是一元约束，即观测特征与地图路标之间的约束关系，如几何参数相似性、位置相关性等；也可以为二元约束，即2个特征匹配对中，2个观测特征的空间相对位姿与2个地图路标的空间相对位姿的约束关系，如平面的夹角约束、点的距离约束等。
|
| 图 5 基于解释树的数据关联 Fig. 5 Data association based on interpretation tree |
对于特征点的匹配，使用描述符的相似性、三维位置的相关性和点到点的距离约束来剔除错误的子树。
1) 描述符的相似性。ORB特征点的描述符为256维的向量。计算描述符间的汉明距离(Hamming distance)，设定最大汉明距离阈值，如果汉明距离小于阈值，则特征点和路标点符合描述符相似性约束。
2) 三维位置的相关性。根据机器人位置和相机的内参数，将地图中路标点的三维位置传递到相机坐标系下，对路标点的观测值进行预测。再计算特征点观测值与预测值间的马氏距离(Mahalanobis distance)，如果马氏距离小于阈值，则特征点和路标点的三维位置相关。
3) 点到点的距离约束。该约束属于二维约束，如图 6所示，f1和f2为观测值，l1和l2为路标的预测值。假设f1与l1的关联成立，判断f2与l2的关联是否成立。计算特征点f1与f2之间的距离df，路标点l1与l2之间的距离dl，如果满足|df－dl < δ，其中δ为允许偏差，则特征点和路标点之间符合点到点的距离约束，f2与l2的关联成立。
|
| 图 6 点到点的距离约束 Fig. 6 Point to point distance constraint |
对于特征平面的匹配，使用平面参数的相似性、平面区域的重叠和平面与平面间的夹角约束来剔除错误的子树。
1) 平面参数的相似性。将地图中的路标平面传递到机器人坐标系下，得到平面观测的预测值。计算预测值与观测值之间的角度偏差与距离偏差，如果角度偏差和距离偏差同时小于对应阈值，则特征平面和路标平面的平面参数相似。
2) 平面区域的重叠。匹配的平面应该在平面所表示的平面区域上存在着一定的重叠。采用八叉树(octree)[21]的数据结构，使用平面的内点，计算2个平面的重叠率。首先将路标平面的所有内点传递到机器人坐标系下，建立八叉树；再使用特征平面的每一个内点，判断是否与八叉树相交，计算相交的内点数；如果相交的内点数大于阈值，则特征平面与路标平面所代表的平面区域存在着重叠。
3) 平面与平面间的夹角约束。该约束属于二元约束，如图 7所示，假设观测值f1与预测值l1的关联成立，判断f2与l2是否关联。计算观测值f1与f2之间的夹角α，预测值l1与l2之间的夹角β，如果|α－β| < ε，其中ε为允许偏差，则特征平面和路标平面符合平面间的夹角约束，即f2与l2的关联成立。
|
| 图 7 平面与平面间的夹角约束 Fig. 7 Angle constraint between plane and plane |
根据优化结果，更新基于点和平面的混合地图。对于每一个点路标，地图中存储的信息有特征点的描述符、点的三维位置，即
|
(3) |
式中：P(w)i为在世界坐标系下的点路标；d(w)i为点路标的描述符；p(w)i为点路标的三维位置。
而对于每一个平面路标，地图中存储的信息有平面在世界坐标系下的平面参数、平面内点点云，即
|
(4) |
式中：L(w)j为在世界坐标系下的平面路标；ξ(w)j为在世界坐标系下的平面参数；c(w)j为在世界坐标系下的包含平面的所有内点的点云。点路标的三维位置p(w)i和平面路标的平面参数ξ(w)j可以从优化结果中直接获取。表示平面区域的内点点云可以将多个观测平面特征的内点传递到世界坐标系下再相加而得到。但是随着SLAM进行，内点点云会变的非常多，为了控制平面内点的数量，使用点云库中的体素网格滤波器对相加后的内点点云进行下采样。不仅减少内点的数量，也会提高平面数据关联的效率。经过体素网格滤波器下采样后的平面内点点云如图 8所示，椭圆标记部分为新观测到的平面内点。
|
| 图 8 平面路标点云更新 Fig. 8 Plane landmark point cloud update |
由于深度传感器视角和测距范围的限制，会有属于同一平面区域的重复路标被添加到地图中。如图 9所示为建立的一个房间内的平面地图，平面1被先观测到，并被添加入地图中；随着地图构建过程的进行，平面2被从另外一个视角观测到，也被添加入地图中。但是，平面1和平面2属于同一个环境平面区域，应当将平面1和平面2进行合并。
|
| 图 9 存在重复平面路标的平面地图 Fig. 9 Plane map with reduplicated plane landmarks |
重复的平面路标满足2个条件：
1) 平面参数相似。
2) 平面区域重叠。
判断平面参数的相似性、平面区域是否重叠采用与数据关联中平面匹配相似的方法。
将重复的平面路标合并得到一个平面路标，应该优先保留平面区域大的平面路标。在合并过程中，不仅需要将2个平面的内点合并，还需要在因子图中用合并平面路标变量替换原有路标变量，并替换相关因子。假设2个重复的平面路标中内点数较多的路标为L1，另一个平面路标为L2，它们在因子图中的识别符分别为S1和S2，重复平面的合并过程如下：
1) 从因子图中删除平面路标L2对应的变量。
2) 对因子图中的所有因子，用S1替换S2。
3) 平面路标L1和L2的内点相加，经过体素网格滤波器下采样后作为合并平面的内点。
4) 进行地图优化和更新。
图 9所示的重复平面路标合并后的平面地图见图 10，平面2被合并到平面1中。不仅正确地描述了环境平面区域，也正确地处理了变量间的约束关系，减少了变量优化误差，提高了建图效果。
|
| 图 10 重复平面路标合并后的平面地图 Fig. 10 Plane map after reduplicated plane landmarks merging |
室内导航系统采用2个障碍物地图来存储环境障碍物信息，即全局障碍物地图和局部障碍物地图。全局障碍物地图用来存储整个环境的障碍物信息，为了降低计算量，全局障碍物地图的更新频率很低；局部障碍物地图用来存储机器人附近一定区域内的障碍物信息，更新频率比较高，可以实时反应出环境信息，用于动态避障。
障碍物地图采用三维占栅格地图，如图 11所示，每个栅格有未知、占用、空闲3个状态。输入数据为Kinect传感器得到的深度点云测量值，根据深度测量值，对三维占栅格进行障碍物标记和清除操作。为了便于进行路径规划，通常将三维占栅格地图可以投影到二维平面上，生成二维栅格地图，如图 12所示。
|
| 图 11 三维占栅格地图 Fig. 11 3D occupancy grid map |
|
| 图 12 投影后的二维栅格地图 Fig. 12 2D occupancy grid map after projection |
路径规划采用全局和局部相结合的方式。全局路径在全局障碍物地图上规划出一条全局路径，局部路径的目标点跟随全局路径，在局部障碍物地图上进行规划，实现机器人的动态避障。
由于机器人的投影足迹具有一定的形状，不能把机器人看作一个点来进行路径规划。因此，在使用代价地图进行路径规划之前，需要根据机器人的足迹形状，对代价地图进行膨胀操作。
如图 13所示，红色多边形为机器人足迹，指定了内切圆半径Ri和外接圆半径Rc。对于代价地图中的每一个栅格，假设将机器人中心放在该栅格中心位置，栅格到障碍物的最小距离为dg。显然，dg越大，机器人距离障碍物越远，碰撞的可能越小。
|
| 图 13 膨胀半径示意图 Fig. 13 Schematic diagram of inflated radius |
在进行膨胀操作时，随着dg的增大赋予递减的代价值，对于dg < Ri的栅格，赋予一个致命的代价值；对于Ri < dg < Rc的栅格，赋予较高的代价值；对dg>Rc的栅格，采用指数函数递减的代价值。将图 12中的障碍物地图膨胀后的代价地图如图 14所示。显然，在进行路径规划时，应尽量选择代价值小的栅格，更能保证机器人的避障性能。
|
| 图 14 膨胀操作后的代价地图 Fig. 14 Costmap after inflation operation |
全局路径规划使用A*算法[22]，局部路径规划使用动态窗口法(Dynamic Window Approach，DWA)[23]。图 15显示了一个路径规划的结果，红色多边形为护理机器人投影到二维平面上的足迹，绿色曲线为全局规划路径，蓝色曲线为局部规划路径。
|
| 图 15 全局与局部路径规划 Fig. 15 Global and local path planning |
在室内走廊环境下进行基于点和平面特征的SLAM实验。走廊环境为典型的室内结构化环境，有许多的平面区域，但环境纹理比较少，提取的特征点会比较少。为了分析基于点和平面特征的SLAM的定位精度和建图效果，使用激光雷达传感器，应用GMapping[24]建立二维占栅格地图，将SLAM轨迹与GMapping轨迹进行比较，并将SLAM构建的混合地图与GMapping构建的占栅格地图进行对比。激光雷达的有效测距范围为30 m，测量角度范围为270°，构建的二维占栅格地图非常精确，常被作为评估其他SLAM算法的参考值。从建图效果、定位精度和运算时间3个方面分析实验结果。
所有的SLAM实验运行在单个线程内，CUP主频为2.9 GHz，内存为12 GB，没有使用GPU。
6.1 建图效果SLAM构建的基于点和平面特征的混合地图如图 16所示，不同颜色的平面区域代表不同的平面路标，不同的球形点代表不同的点路标。地图中含有45个平面路标和266个点路标。平面路标正确地描述了环境平面区域，三维地图与真实环境基本吻合。
|
| 图 16 基于点和平面特征的混合地图 Fig. 16 Hybrid map based on point and plane features |
GMapping[24]构建的二维占栅格地图如图 17所示，基本符合建筑物地图。将SLAM地图与二维占栅格进行比较，如图 18所示。可以看到，SLAM构建的混合地图很好地与栅格地图重叠，尤其是混合地图中的平面路标，与栅格地图可以很好地重合。
|
| 图 17 GMapping生成的占栅格地图 Fig. 17 Occupancy grid map generated by GMapping |
|
| 图 18 SLAM地图与占栅格地图对比 Fig. 18 Comparison of SLAM map and occupancy grid map |
以GMapping轨迹作为参考值，将SLAM轨迹与GMapping轨迹进行对比(见图 19)，来估计所提出SLAM算法的定位精度，并计算位姿的均方根误差(Root Mean Square Error，RMSE)[25]，SLAM实验的详细结果如表 2所示。可以看到，在长度达180 m的室内实验中，SLAM轨迹与GMapping轨迹基本吻合，SLAM轨迹的平移RMSE为0.106 3 m、旋转RMSE为1.194 1°。
|
| 图 19 SLAM轨迹与GMapping轨迹对比 Fig. 19 Comparison of SLAM trajectory and GMapping trajectory |
| 实验结果 | 数值 |
| 轨迹长度/m | 179.74 |
| 持续时间/s | 617.53 |
| 数据帧数 | 6 176 |
| 关键帧数 | 1 495 |
| 平移误差均值/m | 0.097 0 |
| 平移RMSE/m | 0.106 3 |
| 旋转误差均值/(°) | 1.084 0 |
| 旋转RMSE/(°) | 1.194 1 |
6.3 运算时间
SLAM过程中各步骤的运算时间如图 20所示，特征点提取和特征平面提取的详细运算时间如图 21所示。数据帧处理的时间保持稳定，约为30 ms。建地图的时间随着SLAM的进行而不断增加。进一步分析建图过程中各个步骤的运算时间，图 22描述了建图过程中平面匹配、点匹配、图优化、地图改进、路标更新的运算时间。
|
| 图 20 SLAM各步骤运算时间 Fig. 20 Each SLAM step runtime |
|
| 图 21 特征提取运算时间 Fig. 21 Feature extraction runtime |
|
| 图 22 建图各步骤运算时间 Fig. 22 Each mapping step runtime |
从图 22可以看出，平面匹配和点匹配的运算时间基本保持稳定，但会随着SLAM过程的进行而略有增加。图优化的运算时间存在着很大的波动性，而地图改进的运算时间中存在着稳定上升部分和跳变部分。稳定上升部分为检测重复平面路标所花费的时间，平面路标越多，运算时间越长；跳变部分为重复路标合并所花费的时间，只有在检测到重复平面时才进行，因此出现向上的跳变。路标更新的运算时间随着SLAM过程的进行而显著增加，主要由于平面路标的内点点云更新需要许多的计算量。SLAM过程中各个步骤的平均运算时间如表 3所示。
| 步骤 | 运算时间/ms |
| 特征平面提取 | 17.39 |
| 特征点提取 | 27.11 |
| 特征平面数据关联 | 9.66 |
| 特征点数据关联 | 22.78 |
| 因子图优化 | 116.07 |
| 地图改进 | 100.35 |
| 路标更新 | 193.50 |
| 总计 | 499.36 |
7 结论
本文提出了基于混合地图的护理机器人室内导航方法，具有如下特点：
1) 基于点和平面特征，构建混合地图，结合单独使用点或平面特征进行导航的优点，以提高导航系统的可靠性和实用性。
2) 以混合地图为核心，搭建护理机器人的室内导航系统，进行了室内导航实验，验证了所提出的基于混合地图的导航系统的可靠性和实用性。
在将来的研究中，可以加入闭环检测功能，提高建图效果；也可以对建图过程中的地图改进和更新算法进行改进，减少计算量。
| [1] | WEINGARTEN J, SIEGWART R. 3D SLAM using planar segments[C]//2006 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE Press, 2006: 3062-3067. |
| [2] | CASTELLANOS J A, TARDOS J D. Mobile robot localization and map building[M]. Berlin: Springer, 1999: 9-30. |
| [3] | LEE T K, LIM S, LEE S, et al. Indoor mapping using planes extracted from noisy RGB-D sensors[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE Press, 2012: 1727-1733. http://ieeexplore.ieee.org/document/6385909/ |
| [4] | KAESS M. Simultaneous localization and mapping with infinite planes[C]//2015 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2015: 4605-4611. http://ieeexplore.ieee.org/document/7139837/ |
| [5] | KAESS M, RANGANATHAN A, DELLAERT F. iSAM:Incremental smoothing and mapping[J]. IEEE Transactions on Robotics, 2008, 24 (6): 1365–1378. DOI:10.1109/TRO.2008.2006706 |
| [6] | TREVOR A J B, ROGERS J G, CHRISTENSEN H I. Planar surface SLAM with 3D and 2D sensors[C]//2012 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2012: 3041-3048. http://ieeexplore.ieee.org/document/6225287/ |
| [7] | TAGUCHI Y, JIAN Y D, RAMALINGAM S, et al. Point-plane SLAM for hand-held 3D sensors[C]//2013 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2013: 5182-5189. http://ieeexplore.ieee.org/document/6631318/ |
| [8] | ATAER-CANSIZOGLU E, TAGUCHI Y, RAMALINGAM S, et al. Tracking an RGB-D camera using points and planes[C]//2013 IEEE International Conference on Computer Vision Workshops. Piscataway, NJ: IEEE Press, 2013: 51-58. http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.407.5933 |
| [9] | SALAS-MORENO R F, GLOCKEN B, KELLY P H J, et al. Dense Planar SLAM[C]//2014 IEEE International Symposium on Mixed and Augmented Reality. Piscataway, NJ: IEEE Press, 2014: 157-164. |
| [10] | ZHANG L Z, CHEN D S, LIU W H. Point-plane SLAM based on line-based plane segmentation approach[C]//2016 IEEE International Conference on Robotics and Biomimetics. Piscataway, NJ: IEEE Press, 2016: 1287-1292. |
| [11] | QUIGLEY M, CONLEY K, GERKEY B P, et al. ROS: An open-source robot operating system[C]//ICRA Workshop on Open Source Software. Piscataway, NJ: IEEE Press, 2009: 1-6. |
| [12] | DELLAERT F. Factor graphs and GTSAM: A hands-on introduction: GT-RIM-CP&R-2012-02[R]. Altalanta: Georgia Institute of Technology, 2012: 1-26. |
| [13] | RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: An efficient alternative to SIFT or SURF[C]//2011 IEEE International Conference on Computer Vision. Piscataway, NJ: IEEE Press, 2011: 2564-2571. http://ieeexplore.ieee.org/document/6126544/?reload=true&arnumber=6126544 |
| [14] | ZHANG L Z, CHEN D S, LIU W H. Fast plane segmentation with line primitives for RGB-D sensor[J]. International Journal of Advanced Robotic Systems, 2016, 13 (6): 1–8. |
| [15] | RUSU R B, COUSINS S. 3D is here: Point cloud library (PCL)[C]//2011 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2011: 1-4. |
| [16] | KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[C]//IEEE and ACM International Symposium on Mixed and Augmented Reality. Piscataway, NJ: IEEE Press, 2008: 225-234. |
| [17] | MUR-ARTAL R, TARDOS J D. ORB-SLAM2:An open-source SLAM system for monocular, stereo and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33 (5): 1255–1262. DOI:10.1109/TRO.2017.2705103 |
| [18] | GAD A, MAJDI F, FAROOQ M. A comparison of data association techniques for target tracking in clutter[C]//2002 IEEE International Conference on Information Fusion. Piscataway, NJ: IEEE Press, 2002: 1126-1133. http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.462.1485 |
| [19] | POPPINGA J, VASKEVICIUS N, BIRK A, et al. Fast plane detection and polygonalization in noisy 3D range images[C]//2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE Press, 2008: 3378-3383. http://ieeexplore.ieee.org/document/4650729/ |
| [20] | KAESS M, JOHANNSSON H, ROBERTS R, et al. iSAM2:Incremental smoothing and mapping using the Bayes tree[J]. International Journal of Robotics Research, 2012, 31 (2): 216–235. |
| [21] | KAMMERL J, BLODOW N, RUSU R B, et al. Real-time compression of point cloud streams[C]//2012 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2012: 778-785. |
| [22] | HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science & Cybernetics, 1968, 4 (2): 100–107. |
| [23] | FOX D, BURGARD W, THRUN S. The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine, 1997, 4 (1): 23–33. |
| [24] | GRISETTIYZ G, STACHNISS C, BURGARD W. Improving grid-based SLAM with Rao-Blackwellized particle filters by adaptive proposals and selective resampling[C]//2005 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE Press, 2005: 2432-2437. |
| [25] | STURM J, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE Press, 2012: 573-580. http://ieeexplore.ieee.org/document/6385773/ |