家庭服务机器人自主定位控制

王刚 周军 苏晓明

摘 要：家庭服务机器人是目前机器人领域的一个研究热点，家庭服务机器人的研发可以减轻社会和家庭负担，缓解人口老龄化等问题。自主定位技术是家庭服务机器人研发的核心技术之一。本文针对家庭服务机器人定位技术研究，进行了其机械结构的设计以及传感器的选型提供方案，也对ROS编译环境进行介绍。最重要的是设计出一种基于激光SLAM技术为核心的传感器数据融合方法适用于家庭的微动态环境下的定位方法。在对系统进行运动学模型和坐标系模型建模的基础上，提出一种传统的Hector SLAM算法并对其进行改进成自适应无迹卡尔曼（AUKF）融合算法，提高了定位的稳定性和准确性。

关键词：服务机器人;定位导航;数据融合;激光SLAM

Abstract：Home service robot is a research hotspot in the field of robotics. The research and development of family service robot can reduce the burden of the family and the aging of the population. Autonomous positioning technology is one of the core technologies in the research and development of home service robots. This paper focuses on the research of home service robot positioning technology， the design of its mechanical structure and the selection of sensors， and also introduces the ROS compilation environment. The most important thing is to design a sensor data fusion method based on laser SLAM technology， which is suitable for home positioning in micro-dynamic environment. Based on the kinematics model and coordinate system model of the system， a traditional Hector SLAM algorithm is proposed and improved into an adaptive unscented Kalman （AUKF） fusion algorithm to improve the stability of the positioning and accuracy.

Key words：service robot;positioning;data fusion;laser SLAM

机器人技术是一个集机械，信息，计算机科学和自动控制理论于一体的多学科系统[1-2]。 它不仅具有自身技术的高附加值，而且还具有广泛的产品。这已说明机器人已是成为重要的技术辐射平台，增强了军事防御力量，提高了操控性，提高事态发展水平，推动经济整体发展，很大程度上改善人民生活水平具有重要意义[3-5]。

我国2015年提出《中国制造2025》中更是把机器人作为未来发展的十大重点领域之一[6-8]。其中服务机器人更是本世纪最有前途的应用之一。有人预测，未来对服务机器人的需求务机器人的未来发展去制定国家短期和长期的研究计划[9-11]。在2006年我国也提出了863计划，把服务机器人的“智能机器人技术”视为研究方向[12-14]。对移动机器人的基本智能问题，Durrant-Whyte在1992年得出结论，机器人移动主要反映在三个问题上：

（1）“我现在在哪里？”（2）“我要去哪里？”（3）“如何到达那里”，对应着移动机器人的定位，路径规划和运动控制问题[15-19]。研究了家庭定位机器人的自主定位技术。自主定位问题是移动机器人实现特定功能的基础和前提，贯穿于自主导航之中，是众多问题中最先需要解决的一个问题，因为在一定程度上是反应了机器人的智能情况，如果其问题没有解决好，就可能导致无法实现机器人本身需要执行的功能[20-21]。

1 机器人系统和定位方案的设计

5对家庭服务机器人总体的系统架构进行设计和介绍，针对其定位技术硬件部分的核心传感模块进行构架出合适搭配，并对其编译环境ROS操作系统进行介绍。在对机器人定位技术的研究现状进行分析，对其室内定位方案进行设计。

1.1 机器人总体架构

家庭服务机器人总体框架主要为机械模块、传感模块、控制模块、驱动模块、软件模块五大模块。机器模块是机器人的外部机械结构。传感模块是检查周围环境和机器人本身位姿状态的传感器集合，也是定位技术的核心所在。控制模块则是机器人的核心，包含着许多硬件和控制器，负责实现机器人正常运行所有功能。驱动模块保证机器人运动的位置、方向、速度。软件模块则是保证系统控制模块的有效运行。其结构示意图如图1所示。

传感器模块内部选择以下4种传感器：

（1）激光雷达：作为获取外部环境信息的主要传感器，其数据的精确度就是构建地图和导航精度的基础。激光雷达需要对返回的数据进行预处理并作为绘制栅格地图的数据重要来源。

（2）惯性测量单元（IMU）：内部封装三个方向的陀螺仪和加速度计装置，来表示機器人的位姿，这也是在现代SLAM技术中机器人解决自主定位技术的关键信息。

（3）超声波测距：用于增加机器人定位的稳定性，通过检测机器人与物体之间的间距以防止雷达模块数据出现错误。

（4） 红外测距：应用于机器人在进行自身定位时发生物体间的碰撞。

1.2 家庭服务机器人定位方案

早期室内机器人定位主要为传感器定位，如惯性导航法和航迹推算法等[22-24]。然而，这些方法存在缺点，难以单个直接应用于家庭微动态环境进行自主定位，并且一些传感器可能产生一些累积误差。特别是家庭环境这种动态环境，由于无法使用GPS，定位精度要求高等特点和限制，更是增加了定位的难度，使得单一的定位方法难以适用。

针对单个传感器所需精度不满足实际情况的问题许多学者近年来进行融合多个传感器的数据问题研究，通过传感器间相互进行数据补偿调整精度，从而实现更好的机器人定位算法。当前来讲，多个传感器融合进行相互补偿定位数据方法主要以卡尔曼滤波，粒子滤波和贝叶斯理论三种方法为主。其中卡尔曼滤波与贝叶斯理论相比具有更好的动态性能，与粒子滤波相比又进行了简化，不需要庞大的计算量和考虑粒子的有效性和多样性问题。因此卡尔曼滤波算法下的多个传感器数据补偿已经在多种场合作为高效实用的最优估计算法。卡尔曼滤波算法发展过程示意图如图2所示。

2 定位算法研究

基于卡尔曼滤波的EKF-SLAM方法提出的Hector SLAM算法进行逐步优化成自适应性无迹卡尔曼算法，并进行仿真对比，查看优化后对定位精确度纠正的分析。

2.1 Hector SLAM算法

2.2 无迹卡尔曼算法

虽然Hector SLAM利用多分辨率地图等方法使得避免陷入局部极值，但是在客厅较大的环境中依然无法准确定位，只依靠激光雷达数据计算位置容易产生较大偏差。所以要使用多传感器的融合，如利用卡尔曼滤波算法，融合激光数据与里程计数据来减小误差。又由于真实环境下是处于非线性状态，所以无迹卡尔曼滤波器（UKF）来执行传感器数据融合估计得到时刻的在线位姿估计。

2.3 自适应无迹卡尔曼算法

无迹卡尔曼滤波方法在一般简单的环境下可以很准确估计出机器人当前时刻的状态。但是，在现实生活中，根据环境不同会导致模型的不准确性使得系统噪声发生变化，导致滤波效果会变差，此时预测的状态结果会变得不准确。使用Sage-Husa自适应滤波算法，在EKF的原有基础上对机器人的位姿状态估计的精度和鲁棒性进行改善。主要步骤如下：

（1）自适应调整系统噪声的统计特性。

（2）然后当除去量测噪声协方差矩阵估计的无偏估计。

（3）在UKF框架下，加入多维加权系数。

完成对无迹卡尔曼的改进，增加其算法的环境适应能力。其流程图如图4所示。

2.4 实验仿真

根据上文分析，我们认为激光SLAM的定位相对稳定，特别是角度的估计在室内环境认为相对准确。而里程计的实际量测特性容易发生变化。故取自适应向量，不同算法的定位轨迹如图5所示。

由图2.3可见，AUKF对定位位置精确度和运行轨迹平滑性上而言对UKF都是有所改进的，UKF和AUKF都可以通过抑制累积误差，能与真实值的轨迹近似相符。

进一步的，对单纯使用航迹推算、Hector SLAM和引入UKF、AUKF后的位姿误差进行统计分析，其位置误差和角度误差的概率密度如图6和7，均值如表1。

分析图6、图7和表1，使用AUKF比单纯的激光定位，位置精度提高了56%，角度精度提高了40%;与使用传统UKF融合算法相比，位置精度提高约14%，角度精度提高68%。从仿真结果来看，通过AUKF融合Hector SLAM的定结果和里程计数据，可以有效提高定位的结果。

3 结 论

机器人作为《中国制造2025》中未来发展的十大重点领域之一。机器人深度学习等相关性的理论知识和技术逐渐变得成熟，使得机器人技术得到迅速的发展。尤其是服务机器人，因其能直接造福人类的日常生活，正成为机器人领域新的蓝海和重点发展方向。而其家庭服务机器人真正实现自主定位所涉及问题有很多，而针对每个问题都会有多样的解决办法，来满足不同情况下的需求问题。且随着自主定位技术的逐渐成熟，机器人可以具有更好的环境适应能力，鲁棒性和效率也会有很大提升，从而实现服务人们的最终目的。

参考文献

[1] 王田苗，陶永，陈阳.服务机器人技术研究现状与发展趋势[J].中国科学（信息科学），2012，42（9）：1049-1066.

[2] 与人共融的家庭服务机器人定位与导航[D].济南：山东大学，2015.

[3] 徐景硕，周胜明，蒋华君.惯性导航系统的发展及其关键技术综述[J].科技信息，2009，（35）：865-866.

[4] LUCAS B D， KANADE T. An iterative image registration technique with an application to stereo vision （DARPA）[J]. Nutrient Cycling in Agroecosystems， 1981， 83（1）：13-26.

[5] ZHANG Z. Iterative point matching for registration of free form curves and surfaces[J]. International Journal of Computer Vision，1994， 13（2）：119-152.

[6] 吳乃亮，闫飞，卜春光.基于视觉里程计的移动机器人三维场景重构[J].华中科技大学学报（自然科学版），2015，43（S1）：337-340.

[7] GUO B，DAI H，LI Z.A visual-attention-based 3D mapping method for mobile robots[J].Acta Automatica Sinica，2017，43（7）：1248-1256.

[8] ZHOU R，YUAN Q，GU X，et al.Spatial pyramid VLAD[C]// Proc. of Visual Communications and Image Processing Conference.Valletta：IEEE，2015：342-345.

[9] 杨俊驹.动态环境下基于 ROS的移动机器人路径规划技术研究[D].苏州：苏州大学，2016.

[10]ZHOU H， ZOU D， PEI L， et al. StructSLAM： visual SLAM with building structure lines[J]. IEEE Transactions on Vehicular Technology， 2015， 64（4）： 1364-1375.

[11]边浩然， 资新运， 王洪涛， 等. 基于变形履带的搜救侦察机器人设计与试验[J]. 兵器装备工程学报， 2017（3）：143-146.

[12]马闪闪， 吕昭阳， 王迪， 等. 仿人足球机器人目标定位与追踪算法改进[J]. 电子技术， 2014 （11）： 9-13.

[13]VAN DER MERWE R， DOUCET A， DE FREITAS N， et al.The unscented particle filter[C]// Denver： NIPS， 2010：584-590.

[14]顾文华， 周波， 戴先中. 基于 ICP 匹配算法的室内移动机器人定位[J]. 华中科技大学学报（自然科学版），2013， 41（s1）： 262-266.

[15]SHEN G W， ZETIK R， THOMA R S. Performance comparison of TOA and TDOA based locatin estimation algorithms in LOS environment[C]// 5th Workshop on Positioning， Navigation and Communication， Hannover， Germany. USA： IEEE Computer Society， 2014： 71-78.

[16]ALQUDAH R， BASAQNI S. On the effects of multiple beacon on localization for wireless sensor networks[C]// IEEE International Conference on Wireless Telecommunications Symposium， Pomona， CA， USA， April 2013. USA： IEEE， 2013： 304-309.

[17]陳杰，杨朝，贺子琦，等.电缆隧道中巡检机器人实现及应用[J].电子技术与软件工程，2019（17）：73-74.

[18]刘送永，崔玉明.煤矿井下定位导航技术研究进展[J].矿业研究与开发，2019，39（7）：114-120.

[19]廖自威.激光雷达/微惯性室内自主建图与导航技术研究[D].南京：南京航空航天大学，2016.

[20]王志文，郭戈.移动机器人导航技术现状与展望[J].机器人，2003，25（5）：470-474.

[21]陈延国，于澎，高振东，等.自主移动机器人定位方法的研究现状[J].应用科技，2002（11）：41-43.

[22]MURARTAL R，TARDS 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.

[23]PENG Yan-fei，PENG Jian-jun，LI Ji-ping，et al. Design and development of the fall detection system based on point cloud[J]. Procedia Computer Science，2019，147：271-275.

[24]ENGEL J，KOLTUN V，CREMERS D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.