助行机器人定位关键技术研究

申请上海交通大学博士学位论文助行机器人定位关键技术研究专 业：机械电子工程博士生：朱笑笑导 师：曹其新教授上海交通大学机械与动力工程学院2014年 2月 Ph.D. Dissertation Submitted to Shanghai Jiao Tong UniversityResearch on Key Technologies of Localization Method for Walking Assistant Robot Specialty: Mechatronics EngineeringAuthor:Zhu XiaoxiaoAdvisor: Prof. Cao QixinSchool of Mechanical and Power Engineering Shanghai Jiao Tong University February，2014 学位论文版权使用授权书本学位论文作者完全了解学校有关保留、使用学位论文的规定，同意学校保留并向国家有关部门或机构送交论文的复印件和电子版，允许论文被查阅和借阅。本人授权上海交通大学可以将本学位论文的全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或扫描等复制手段保存和汇编本学位论文。（请在以上方框内打“”） 本人郑重声明：所呈交的学位论文，是本人在导师的指导下，独立进行研究工作所取得的成果。除文中已经注明引用的内容外，本论文不包含任何其他个人或集体已经发表或撰写过的作品成果。对本文的研究做出重要贡献的个人和集体，均已在文中以明确方式标明。本人完全意识到本声明的法律结果由本人承担。 摘要助行机器人定位关键技术研究摘要助行机器人是一种可以辅助老人行走的特殊服务机器人，它的目标是代替传统的助行器（如拐杖、助步架等）在保证老人行走安全的基础上，大大提高其独立生活能力及生活质量。除了能够提供基本的辅助行走功能，它还需具备多种智能化功能，如用户健康状态监控，语音交互，定位导航，用户识别，用户计划提醒，信息播报服务等等。定位系统作为助行机器人一个重要组件，是实现多项智能化功能的一个基础。本文针对助行机器人定位系统的几项关键技术开展研究，具体研究内容如下。（1）环境地图是助行机器人自主定位导航的重要先验知识，常用的 2D地图能够满足机器人自定位的需求，但由于存在高度方向的不确定性，在家居狭小空间内的自主导航应用中存在明显不足。而传统的 3D地图创建方法一般需要使用价格昂贵的激光传感器，操作也较为复杂，不适合家庭使用。本文对利用 RGB-D（颜色-深度）传感器的 3D地图创建方法进行研究，提出基于改进 KinectFusion算法的地图创建方法，使其可以较为方便对家庭环境进行创建。对 KinectFusion算法进行两个方面的改进，一方面提出使用环境中的边线特征点匹配来提高其定位鲁棒性，另一方面在点云模型中预设一个地面点云来减少累积误差提高精度。并且提出了基于标志物的子地图拼接方法，解决 KinectFusion算法只能创建小规模地图的问题。（2 ）助行机器人工作形式灵活，经常需要在自主运行和被动控制之间切换，同时其运行环境也会在室内和室外变化。因此连续定位会频繁的中断，这就要求助行机器人具有较强的全局定位功能，可以快速地重新定位。本文对全局搜索定位算法进行研究，提出利用旋转不变量首先进行位置空间搜索得到可行的机器人位置，然后在方向空间进行搜索得到机器人的朝向，对全局搜索定位方法进行降维，这样大大提升了全局定位的效率。（3）传统的 2D连续位置跟踪方法较为成熟但是需要使用激光传感器，对助行机器人的成本控制不利。本文尝试以 RGB-D传感器来代替 2D激光传感器进I 上海交通大学博士学位论文行连续定位，通过充分利用 RGB-D的 3D点云信息弥补其水平方向视野较小的缺点。而传统的 3D点云配准定位算法，无法达到实时定位的需求，本文提出了基于 3DLUT（Lookup Table）的 3D点云快速配准方法，该方法可以达到实时的处理速度，同时具有较高的定位精度。（4 ）助行机器人的用户定位功能是一项非常重要的功能，对提高助行机器人的易用性起很大的作用。传统的基于激光传感器的用户定位方法由于没有用户区分的能力，所以在行人干扰较多的情况下无法正常工作；基于视觉的方法则因为对光线变化，观察角度等非常敏感，所以鲁棒性不高。本文提出基于全向视觉及红外标志物识别的方法来定位用户。通过调节相机曝光值，使得系统在室内室外均能稳定的识别用户。在对全向视觉系统进行标定时，利用镜面基底圆轮廓的成像来计算镜面位姿确定内部参数的方法，并提出了使用镜面中心点来确定镜面位姿两组可能解中的真实解，同时提出了利用一种具有单一解的 Non-SVP（非单一中心）P3P解法来确定全向视觉系统的外部参数的方法。该标定方法简单快速，对标定物要求低，并具有较高的精度。（5）根据本文提出的几项关键技术解决方案，开发搭建了 WalkMateIII型助行机器人软硬件系统。为了方便系统软件的开发集成，提高系统的结构清晰度，本文利用基于模块化的方法来搭建助行机器人的软件系统。最后通过实现两个典型任务：开机用户查找，用户跟踪的，对整套系统的可行性进行验证。国内助行机器人的研究刚刚起步，而助行机器人由于其特殊的工作方法和一般的移动服务机器人有多方面的差别，还有很多问题有待解决。本文的研究目的是：通过对助行机器人的定位系统的研究，解决几项关键问题。论文的研究有助于助行机器人早日进入实用阶段，为解决人口老龄化带来的老人护理问题打下基础，具有重要的社会意义和经济价值。关键词：助行机器人，全向视觉标定，3D点云地图创建，全局定位，快速点云配准算法，机器人模块化，用户定位II 摘要Research on Key Technologies of Localization Method for Walking Assistant Robot ABSTRACT The walking assistant robot (WAR) is a special kind of service robot which canassist the elderly to walk. The reason behind the development of the WAR is toreplace traditional walking aids such as crutches and the walking frame. Moreover,besides the walking assistant function, it also offers more intelligent functions, such ashealth condition monitoring, voice interaction, navigation, user identification, userprogram reminders, information services, etc. The localization system which is animportant component of WAR acts as a base for many intelligent functions. Thisdissertation covers a study of the localization system and the research contents hereinare as follows.1，The map of the environment is key for a-priori knowledge for the autonomouslocalization and navigation function of the WAR. Although 2D maps can meet theneeds of the robot for self-localization, its drawback of uncertainty in the verticaldirection made it unable to meet the demand for autonomous navigation in narrowenvironment such as would be encountered in a typical home setting. Traditional 3Dmapping methods need expensive equipment and the operation is complex. In thisdissertation, we studied the 3D mapping method based on RGB-D (color- depth)sensor and proposed a map creation method based on improved “KinectFusion”,enabling users to more easily re-create the family environment. This dissertationadvances the KinectFusion algorithm with two improvements. On the one hand use ismade of the environment feature to point out matching edges and consequentlyimprove its positioning robustness, on the other hand ground point cloud is preset inthe point cloud model to reduce the accumulated error and hence improve accuracy.III 上海交通大学博士学位论文Additionally, a sub-map stitching method is proposed to solve the limitation of thesize of the map built by “KinectFusion”, based on the ground consistency and thecalibration marker.2，The working style of WAR is flexible and there is often the need to switch betweenpassive control and autonomous operation, especially when its operating environmentswitches between indoor and outdoor. In this manner the continuous positioning isfrequently interrupted. This requires the robot to have a strong global positioningfeature and the ability to quickly re-orient itself. In this dissertation, the global searchpositioning dimensionality reduction method is proposed using the rotationalinvariants for the initial search for a viable space robot position, and subsequently inthe direction of the search space to get the orientation of the robot thus greatlyenhancing the efficiency of its global positioning.3，The traditional 2D continuous location tracking method is mature, but it requiresthe use of laser sensors on the WAR which is unfavorable for purposes of minimizingcost. This article attempts to employ an RGB-D sensor instead of a 2D laser sensor forcontinuous positioning. The main idea is to exclusively use RGB-D information as a3D point cloud for position tracking, making up for its disadvantage by selecting asmall field of view in the horizontal direction. Traditional 3D point cloud registrationalgorithm, cannot fulfill the demands of real-time location. This dissertation presentsa study of three-dimensional point cloud registration using RGB-D sensors, andproposes a fast 3D point cloud registration method based on 3DLUT (Lookup Table)algorithm. This method can not only achieve real-time processing speeds, but also hasa high degree of accuracy.4，The human positioning function is a very important function and plays a great roletowards improving user-friendliness of the WAR. The traditional laser-basedlocalization method lacks the capacity to identify the user hence cannot work well inan environment full of people. Vision-based methods on the other hand are verysensitive to changes in light characteristics hence have low robustness. In thisdissertation, we propose a people positioning based on omni-dimensional visual andIV 摘要infrared markers for identification. The test results proved it to be stable both indoorsand outdoors. For the omni-directional vision system (odvs) calibration, we proposeto use the image of the base circle contour to compute the posture of the mirror anddetermine the internal parameters, and then use the center of the mirror to determinethe true solution from two possibilities. Next, we propose using a special uniquesolution -Non-SVP P3P method to determine the external parameters. Thecalibration method is simple and fast, and not only places low demand on thecalibration object, but also has high accuracy.5，Using the solutions for these key technologies, the hardware and software systemfor the WalkMate III WAR was designed and built. We proposed to use the modulebased system model to make the development of the software system easier, and makethe structure of the system clearer. Finally two intelligence functions were tested inorder to evaluate the Feasibility of the whole system. These are the users findingfunction (when the robot power is switched on) and user tracking.The domestic research of WAR has just started. Because of its special workingmethods and have many differences with general mobile service robots, there aremany issues to be resolved. The purpose of this study is: research on the localizationsystem of WAR, and solve several key issues. The research contained in thisdissertation should be useful towards helping the WAR enter the practical stage assoon as possible, laying the foundation for solving the problem of caring for theelderly caused by aging population problem. Eventually this and has important socialsignificance and results in economic value.Keywords: walking assistant robot, calibration of omnidirectional camera, 3Dpoint cloud creating, global localization, fast point registration algorithm, modularrobots, people localizationV 上海交通大学博士学位论文目录摘要 .IABSTRACT.III目录 .VI第一章绪论 .11.1课题来源、研究背景及意义.11.1.1课题来源.11.1.2课题研究背景及意义.11.2助行机器人国内外研究现状.31.2.1国外研究现状.31.2.2国内研究现状.91.3助行机器人定位关键技术.121.3.1助行机器人功能需求.121.3.2助行机器人定位系统关键技术.141.4研究内容与论文组织.241.4.1研究内容.241.4.2论文组织.25第二章基于改进 KinectFusion算法的 3D点云地图创建.262.1引言.262.2 RGB-D传感器介绍.272.3 KinectFusion算法.282.3.1 KinectFusion算法的 ICP定位方法.292.3.2 KinectFusion算法的 TSDF点云融合算法 .322.4改进的 KinectFusion算法.332.4.1 KinectFusion算法的两个问题分析.332.4.2边线点对应关系改进.352.4.3预设地面模型改进.41VI 目录2.5基于标志物的点云地图拼接.442.5.1标志物及其在子图中的布置.442.5.2子地图中标志物坐标提取.452.5.3相邻子地图位置计算.472.5.4基于 BA算法的闭环优化.482.6实验与分析.502.6.1边线点对应关系改进方法测试.502.6.2预设地面模型改进方法测试.532.6.3子地图拼接精度测试.532.7小结.55第三章基于旋转不变量的全局自定位方法 .563.1引言.563.2 2D距离传感器数据的旋转不变量.573.2.1 2D距离传感器数据的数学定义.573.2.2旋转不变量的定义.583.2.3基于旋转不变量的位置滤波及其阈值确定.583.2.4旋转不变量滤除率分析.603.3基于旋转不变量的二步定位法.623.3.1地图遍历.633.3.2 Omni_Scan采集.653.3.3欧几里得聚类.653.3.4基于相关度匹配的朝向确定.663.3.5重定位策略.673.3.6处理过程的中间结果示例.683.4实验与分析.683.4.1直方图数目和距离扫描采样数的实验确定.693.4.2环境中有动态物体时的定位精度测试.733.4.3地图尺寸对定位的影响测试.733.4.4与基于线段特征的全局定位方法对比实验.74VII 上海交通大学博士学位论文3.4.5机器人定位精度试验.763.5小结.77第四章基于 3DLUT点云快速配准算法的实时位置跟踪方法.784.1引言.784.2加速 ICP算法研究现状 .784.3 2DLUT（Look Up Table,查找表）算法.794.3.1配准问题定义与分析.804.3.2误差方程定义及基于 RPROP算法的误差优化.814.3.3 2D查找表的建立.824.3.4基于 2DLUT的连续位置跟踪算法.844.4 3DLUT算法.844.4.1 3D查找表的建立.854.4.2内存的优化.864.4.3 RGB-D传感器标定.874.5实验与分析.894.5.1与 ICP算法对比实验 .894.5.2实际环境定位精度测试.914.6小结.93第五章基于全向视觉及红外标志的用户定位方法 .945.1引言.945.2基于全向视觉及红外标志物的用户定位.945.2.1全向视觉系统.945.2.2红外标志物目标识别.955.2.3目标的位置确定.995.3全向视觉传感器标定.1005.3.1研究现状.1005.3.2参数定义.1015.3.3全向视觉成像模型建立.1025.3.4基于镜面轮廓的内部参数标定.103VIII 目录5.3.5基于单一解 Non-SVP问题的外部参数标定.1055.3.6多点优化.1075.4实验与分析.1085.4.1全向视觉标定实验.1085.4.2已知高度单点位置测量精度实验.1125.4.3未知高度的两点测量精度实验.1135.5小结.114第六章 WalkMateIII实验平台及功能测试.1156.1引言.1156.2 WalkmateIII硬件系统 .1156.3 WalkmateIII软件系统 .1166.4集成功能测试.1196.4.1开机用户查找功能.1196.4.2用户跟随功能.1216.5小结.122zhi ku quan 20150721第七章总结与展望 .1237.1全文总结.1237.2主要创新点.1247.3研究展望.124参考文献 .126攻读博士学位期间已发表或录用的论文及专利 .134攻读博士学位期间参与的科研项目 .135致谢 .136IX zhi ku quan 20150721 第一章绪论第一章绪论1.1课题来源、研究背景及意义1.1.1课题来源本论文受国家 863计划先进制造技术领域重点项目“助老/助残机器人概念样机研究与开发”（2006AA040203）资助。1.1.2课题研究背景及意义全球经济日益繁荣、科技不断进步的同时，世界各国也正在步入一个老龄化的阶段。据统计1950年至2010年期间，发达国家60岁以上老年人口升至8%17%，发展中国家也升至4%6%。到本世纪中叶，这两个数字将分别达到 26%和 14%1。而中国则面临着更加严峻的人口老龄化问题，据中国社科院发布的中国老龄事zhi ku quan 20150721业发展报告(2013)2中指出截至 2012年底，我国 60岁以上老年人口数量达到1