第二军医大学学报  2019, Vol. 40 Issue (5): 507-511   PDF    
引用本文
杨晶东, 郭远首. 基于联合点线特征的医疗服务机器人同时定位与地图构建算法研究[J]. 第二军医大学学报, 2019, 40(5): 507-511  
YANG Jing-dong, GUO Yuan-shou. Simultaneous localization and mapping algorithm based on point and line features for medical service robots[J]. Academic Journal of Second Military Medical University, 2019, 40(5): 507-511 (in Chinese with English abstract)  
基于联合点线特征的医疗服务机器人同时定位与地图构建算法研究
杨晶东, 郭远首     
上海理工大学光电信息与计算机工程学院自主机器人实验室, 上海 200093
收稿日期: 2018-11-07    接受日期: 2018-12-07
基金项目: 国家自然科学基金（61374039），上海市自然科学基金（15ZR1429100），沪江基金（C14002）.
作者简介: 杨晶东, 博士, 副教授, 硕士生导师.E-mail:eerfriend@yeah.net
通信作者(Corresponding author): 郭远首.Tel:021-55271064, E-mail:guo-yuans@hotmail.com
摘要: 目的 为提高医疗服务机器人同时定位与地图构建（SLAM）算法全局定位精度和实时性，提出基于点线特征SLAM（PL-SLAM）算法，并与ORB（oriented FAST and rotated BRIEF）-SLAM2算法进行比较。方法 PL-SLAM算法在特征提取过程中在点特征的基础上增加线段特征，根据融合后的点线特征，在复杂医疗环境内进行地图创建与全局定位。利用公开数据集EuRoc和KITTI对比PL-SLAM算法与ORB-SLAM2算法，测试医疗服务机器人的自主导航综合性能。结果 与ORB-SLAM2算法相比，PL-SLAM算法在弱纹理环境下能够提取较多的点线特征，定位精度和实时性均有较大提升。其中旋转误差较ORB-SLAM2算法减小42.2%，运算速度提高55.9%。结论 PL-SLAM算法能够有效提高医疗服务机器人全局定位精度和实时性。
关键词: 医疗服务机器人    同时定位与地图创建    ORB算法    弱纹理环境    
Simultaneous localization and mapping algorithm based on point and line features for medical service robots
YANG Jing-dong, GUO Yuan-shou     
Autonomous Robot Lab, School of Optical-Electrical and Computer Engineering, University of Shanghai for Science and Technology, Shanghai 200093, China
Supported by National Natural Science Foundation of China (61374039), Natural Science Foundation of Shanghai (15ZR1429100), and Hujiang Foundation (C14002).
Abstract: Objective To propose a point and line (PL)-simultaneous localization and mapping (SLAM) algorithm and to compare it with oriented FAST and rotated BRIEF (ORB)-SLAM2, so as to improve the global localization accuracy and real-time performance of SLAM algorithm for medical service robots. Methods The PL-SLAM algorithm added line features based on point feature in the process of feature extraction, and carried out mapping and global localization in the complex medical environment according to the point and line features after fusion. The public datasets (EuRoc and KITTI) were used to compare the PL-SLAM and ORB-SLAM2 algorithms, and the comprehensive performance of autonomous navigation of the medical service robots was tested. Results Compared with the ORB-SLAM2 algorithm, PL-SLAM algorithm extracted more point and line features in weak texture scenario, and effectively enhanced the global localization accuracy and real-time performance. The rotation error of the PL-SLAM algorithm decreased by 42.2% and the runtime increased by 55.9%. Conclusion PL-SLAM algorithm can effectively improve global localization accuracy and the real-time performance of medical service robots.
Key words: medical service robotics    simultaneous localization and mapping    oriented FAST and rotated BRIEF    weak texture scenario    

服务型机器人进入医疗领域后，有效地提高了医疗服务水平。常见的医疗服务机器人多为手术机器人、康复机器人、护理机器人、救援机器人和转运机器人。手术机器人有Da Vinci、Remebot等，主要应用于微创手术；康复机器人主要用来帮助老年人和残疾人更好地适应日常工作和生活，部分补偿弱化的机体功能，以及帮助患者恢复机体功能；护理机器人辅助护士完成护理工作，实时监控并记录患者身体、情绪状况等；救援机器人在意外事故发生、人力无法快速完成任务的情况下执行救援任务；转运机器人主要用于挪动病床、术前术后接送患者，避免患者的二次伤害[1]。所有医疗服务机器人均与同时定位与地图构建（simultaneous localization and mapping，SLAM）技术相关。

SLAM最早由Smith、Self和Cheeseman提出，一直被认为是实现全自主移动机器人的核心技术[2-3]。SLAM技术常用传感器为激光和摄像头传感器，故可分为激光SLAM和视觉SLAM两类。激光传感器制图的精度较高，但是地图包含物体元素较少，观察不够直观，并且激光制图容易受到阳光、玻璃等外界因素影响；摄像头制图包含的三维信息较多，但制图精度不高，含有较多噪声，易受环境光照、视角等因素影响。

目前对SLAM的研究方向主要是提高计算效率、回环检测优化等。我国的SLAM研究起步较晚，但在地图构建、定位和导航领域已取得不少成果，对无人机、服务机器人、扩增实境等的研究较多。国外对SLAM的研究方向主要是后端优化和前端的特征提取优化，主要集中在无人驾驶、服务机器人和军事领域。

常用视觉SLAM算法有ORB（oriented FAST and rotated BRIEF）-SLAM、DSO（direct sparse odometry）-SLAM、SVO（semidirect visual odometry）-SLAM。ORB-SLAM算法是基于视觉场景中的点特征，其优点是在静态环境中定位准确，可实现单目和双目实时定位，缺点是创建地图点云稀疏，不易识别移动物体。DSO-SLAM算法可以生成稠密点云，运算速度较快，一般为20~30帧/s，但是对于环境光照要求较高，要求保持光照稳定，对移动物体不灵敏。SVO-SLAM算法运算速度快，可达到300帧/s，但创建地图和定位误差大。

基于点线特征（point and line，PL）-SLAM算法不同于基于点特征算法，其能够在弱纹理环境中稳定运行。弱纹理即环境中存在大量特征不明显的特征点，该点的相近领域内不存在角点、边界等较明显的纹理特征[4]。该算法联合了点线特征，使地图中含有更丰富的几何物体特征，可以提供更多的结构信息。

1 PL-SLAM算法

PL-SLAM算法流程包含3个线程：特征跟踪、局部地图构建和回环检测，流程图见图 1。

图 1 PL-SLAM算法流程 Fig 1 PL-SLAM algorithm flow PL-SLAM: Point and line-simultaneous localization and mapping; BA: Bundle adjustment

1.1 特征追踪

特征追踪是相连帧之间的视觉测量估计和卡尔曼滤波（Kalman filter，KF）决策策略[5]。点和线段均沿着立体帧序列进行追踪，通过最小化误差函数获得摄像机三维运动。特征追踪包括点特征、线段特征、关键帧匹配和运动估计。

（1）点特征：PL-SLAM算法使用ORB特征提取方法[6]，具有较好的关键点检测性能，基于二进制特征描述符可以进行快速、有效的关键点匹配。

（2）线段特征[7]：采用直线检测算法（line segment detection，LSD）提取线段特征[8]，具有较高的精度和重复性，利用线段本身的几何信息过滤方向和长度不匹配、差异度较高线段，保留更多的线性结构，使地图内容更丰富。

（3）运动估计：建立了两立体帧对应关系后，将第一帧关键点和线段与第二帧进行投影，通过高斯牛顿法优化，并采用Pseudo-Huber损失函数[9]，获得两连续帧之间的运动增量估计，建立正态分布模型为

(1)
1.2 局部地图构建

（1）三角化测量：双目相机可以获得像素深度信息，需要三角化测量估计地图点深度。线段由像素点组成，因此线段和点深度信息的获取采用相同方法。其原理见图 2。

图 2 三角化测量法获取点深度 Fig 2 Point depth measurement by triangulation method O1 and O2 are camera spots; P is spatial point; p1 is the feature point in I1, corresponding to the feature point p2 in I2; t is the translation matrix of I1 to I2. I1 and I2 are binocular cameras

图 2中p1、p2为地图点P分别在双目相机I1和I2中的投影，设其归一化坐标为x1与x2，其关系为

(2)

其中R为相机内参，t为变换矩阵，s1与s2为2个特征点的深度。欲求2个特征点深度，可将上式两侧左乘即得

(3)

（2）最小化投影误差：由于噪声的存在，直线O1p1与O2p2在场景中不会相交于一点，这是一个光束平差法（bundle adjustment，BA）问题，即最小化重投影误差[10]。将误差求和，通过最小二乘法使其误差最小化，公式为

(4)
1.3 回环检测

回环检测又称闭环检测，为找到一个与当前处理环境相似的环境[11]，通过对比得到当前位姿估计，减少累积位姿估计误差漂移问题。与历史位姿数据关联，在发生碰撞或绑架后，机器人可以通过回环检测对自身位姿进行重定位。因此，回环检测提升了机器人SLAM精度与稳定性。

可以通过词袋模型实现回环检测[12]，词袋即BoW，一张图片特征对应BoW单词，许多单词组成“字典”。例如：1个“字典”包含3个单词即特征，分别为w1、w2、w3，图片特征可用下式表示

(5)

若使用向量表示图片，则上式可以用向量[1, 1, 0]T表示。

2 实验结果和分析

针对EuRoc和KITTI公开数据集，本研究对比分析了PL-SLAM算法与ORB-SLAM2算法性能。为保证实验客观性，所有实验均在Inter Core i7-7700 CPU @ 2.80GHz笔记本电脑上运行。图 3A、3B分别为针对EuRoc数据集采用ORB-SLAM2和PL-SLAM算法运行的局部地图，由图可见，PL-SLAM算法可提取到更多弱纹理特征，如墙壁棱角、物体边线。图 4A、4B分别为针对KITTI-07数据集采用ORB-SLAM2和PL-SLAM算法运行的全局地图。从全局地图分析，PL-SLAM算法创建的地图中除了包含点特征外，还包含大量线段特征。

图 3 ORB-SLAM2（A）和PL-SLAM（B）算法分别运行EuRoc数据集结果图 Fig 3 ORB-SLAM2 (A) and PL-SLAM (B) algorithms for EuRoc dataset The green point is the point feature extracted by ORB-SLAM2 algorithm, and the green line is the line segment feature extracted by PL-SLAM algorithm. ORB-SLAM: Oriented FAST and rotated BRIEF-simultaneous localization and mapping; PL-SLAM: Point and line-simultaneous localization and mapping

图 4 ORB-SLAM2（A）和PL-SLAM（B）算法运行KITTI-07数据集 Fig 4 ORB-SLAM2 (A) and PL-SLAM (B) algorithms for KITTI-07 dataset The red point is the current environmental feature extracted by the robot, and the blue route is the moving path of the robot. ORB-SLAM: Oriented FAST and rotated BRIEF-simultaneous localization and mapping; PL-SLAM: Point and line-simultaneous localization and mapping

PL-SLAM算法测试KITTI-01数据集时，估计轨迹和groundtruth绝对误差见图 5，图中绿色线为估计轨迹，黑色虚线为数据集groundtruth。由图 5可见，环境中含有大量回环，回环检测提供了当前数据与历史数据的关联，图中机器人运动一段时间后会回到经过的位置，当机器人到达该位置后，进行回环检测，消除平移和旋转误差，提高地图创建和全局定位精度。此外，由于PL-SLAM算法较传统SLAM算法多了线段特征，有效降低了机器人在移动过程中产生的旋转误差。

图 5 KITTI-01数据集在PL-SLAM算法上的误差 Fig 5 PL-SLAM algorithm error for KITTI-01 dataset The green line is the estimated trajectory, and the black dotted line is the dataset groundtruth. PL-SLAM: Point and line-simultaneous localization and mapping

针对8个KITTI数据集，测试对比PL-SLAM和ORB-SLAM2算法的平移误差和旋转误差，结果见表 1。ORB-SLAM2算法的平均平移误差为2.27%，平均旋转误差为3.77%；PL-SLAM算法的平均平移误差为2.26%，平均旋转误差为2.18%，旋转误差较ORB-SLAM2算法减小42.2%。可见PL-SLAM与ORB-SLAM2算法有相似的平移误差，但PL-SLAM算法的旋转误差远小于ORB-SLAM2算法。由于医疗服务机器人自主导航过程中经常改变转向，结果表明PL-SLAM算法更适合医疗服务机器人自主导航过程。

表 1 PL-SLAM与ORB-SLAM2算法的误差对比 Tab 1 Comparison of errors between PL-SLAM and ORB-SLAM2 algorithms

此外，PL-SLAM算法在特征追踪、局部地图构建、回环检测3个线程的平均运行时间分别为58.8、39.7、23.0 ms，总运行时间为121.5 ms；而ORB-SLAM2算法3个线程的平均运行时间分别为83.3、185.7、6.8 ms，总运行时间为275.8 ms。PL-SLAM相对ORB-SLAM算法运算速度提高了55.9%，可见PL-SLAM算法的运行速度较ORB-SLAM2算法更快。这是因为PL-SLAM算法中添加了线段特征，线段特征需要先提取特征点，通过对比上下帧过滤不匹配的特征点，再构成线段特征，因此回环检测有效减少了特征匹配时间。结果表明PL-SLAM算法可提高医疗服务机器人自主导航实时性。

3 结论

SLAM技术是医疗服务机器人研究热点。本研究提出的PL-SLAM算法在ORB-SLAM算法基础上增加了线段特征提取，包含了更丰富的特征，具有更好的定位精度，在弱纹理环境下可构建更精确的地图。PL-SLAM算法虽然在回环检测环节耗费较多时间，但其对特征提取和局部地图构建环节均进行了优化，减少了运算时间，因此，整体上提高了算法实时性。本研究提出的PL-SLAM算法旋转误差较小，在复杂特征环境中具有较好的定位精度，但还不适合在特征较少的对称环境（如楼道等）中准确定位，后期还需改进与提高。

参考文献
[1]
黄敦华, 李勇, 陈容红. 医疗服务机器人应用与发展研究报告[J]. 机电产品开发与创新, 2014, 27: 5-8. DOI:10.3969/j.issn.1002-6673.2014.01.002
[2]
邓伟.服务机器人的同步定位与地图构建(SLAM)问题研究[D].乌鲁木齐: 新疆大学, 2014. http://cdmd.cnki.com.cn/Article/CDMD-10755-1014384048.htm
[3]
NIETO J, GUIVANT J, NEBOT E. DenseSLAM:simultaneous localization and dense mapping[J]. Int J Robot Res, 2006, 25: 711-744. DOI:10.1177/0278364906067379
[4]
PUMAROLA A, VAKHITOV A, AGUDO A, SANFELIU A, MORENO-NOGUER F. PL-SLAM: real-time monocular visual SLAM with points and lines[C]//IEEE International Conference on Robotics and Automation (ICRA).[S.l.]: IEEE, 2017: 4503-4508.
[5]
杜航原, 郝燕玲, 赵玉新. 基于模糊自适应卡尔曼滤波的SLAM算法[J]. 华中科技大学学报(自然科学版), 2012, 40: 58-62.
[6]
DAVISON A J. Real-time simultaneous localisation and mapping with a single camera[C]//Proceedings Ninth IEEE International Conference on Computer Vision.[S.l.]: IEEE, 2003, 2: 1403-1410.
[7]
韩锐, 李文锋. 一种基于线特征的SLAM算法研究[J]. 自动化学报, 2006, 32: 43-46.
[8]
郭克友, 王艺伟, 郭晓丽. LDA与LSD相结合的车道线分类检测算法[J]. 计算机工程与应用, 2017, 53: 219-225.
[9]
BARRON J T. A more general robust loss function[Z/OL]. arXiv: 1701.03077[cs.CV]. (2017-01-11). https://arxiv.org/abs/1701.03077v1.
[10]
徐帆, 王宏远, 方磊, 田文. 最小化重投影误差的PFR三维射影重建[J]. 华中科技大学学报(自然科学版), 2008, 36: 52-55.
[11]
XIA Y, LI J, QI L, YU H, DONG J. An evaluation of deep learning in loop closure detection for visual SLAM[C]//2017 IEEE International Conference on Internet of Things (iThings) and IEEE Green Computing and Communications (GreenCom) and IEEE Cyber, Physical and Social Computing (CPSCom) and IEEE Smart Data (SmartData).[S.l.]: IEEE, 2017: 85-91.
[12]
曾桂萍, 孙作雷, 潘盼. 闭环检测中词袋与词对袋的对比研究[J]. 微型机与应用, 2017, 36: 21-23, 30.