移动机器人多信标SLAM技术

高沛林 高赟

摘 要：针对大型煤矿中的露天移动机器人自主行走问题，应用基于卡尔曼滤波的SLAM算法，研究了移动机器在不携带惯性导航设备，也无先验地图的情况下，通过自身携带的传感器与环境特征量进行应答式通信，建立环境地图，并利用该地图计算自身位置，从而实现自主导航与定位。仿真结果显示，SLAM算法的定位误差保持在±1 m以内，速度误差保持在±0.2 m/s以内，对环境特征量的定位误差随着机器人的移动逐渐减小，最终保持在±2 m以内。同时，通过对不同距离量测噪声与速度量测噪声的情况也进行了分析。仿真结果显示，当保持距离量测噪声不变，增大速度量测噪声时，或保持速度量测噪声不变，增大距离量测噪声时，SLAM算法的定位精度均会下降。研究表明，基于卡尔曼滤波的SLAM算法很好地控制了移动机器人在未知环境中的定位误差，保证了机器人的定位精度。

关键词：露天移动机器人;同时制图与定位;导航定位;随机信标

中图分类号：TD 05            文献标志码：A

文章编号：1672-9315（2019）05-0905-07

Abstract：This article aims at the problem of autonomous walking of outdoor mobile robots in large coal mines.It focuses on a situation that a robot，which doesn’t carry any inertial navigation equipment or without a priori map，communicates with the beacons，establishes the environmental maps，which is used to calculate its own position.The simulation results show that the positioning error of SLAM algorithm is keep within ±1 m，the velocity error is kept within ±0.2 m/s，the beacon positioning error is decreasing while the robot is moving，and eventually kept within ±2 m.The study also finds out that distance measurement noise or velocity measurement noise will directly affect the position accuracy of SLAM algorithm.Therefore，the influence of different measurement noises is also analyzed.The accuracy of SLAM algorithm is decrease while the distance measurement noise is increased or the velocity measurement noise is increased.The simulation results show that the SLAM algorithm based on Kalman Filtering controls the position error of mobile robot in the unknown environment well and improves the positioning accuracy of the robot effectively.

Key words：mobile robots in the open air;SLAM;navigation;random beacon

0 引 言

隨着计算机技术和人工智能的发展成熟，针对机器人进行的研究得到越来越多的关注和投入。无论是哪一种工作原理、工作目的的移动机器人，其移动都不是盲目的。在机器人的移动过程中需要考虑移动路径、避障、移动地图、路径规划、导航定位等许多实际问题[1-3]。对于在已知环境中的移动机器人自主定位和已知机器人位置的地图创建，已有了一些实际的解决方法。然而在很多情况下，需要人工进入变电站、发电厂、煤矿井下进行巡检，巡检困难、危险性高，难免会发生漏检、误检事件[4-7]。针对类似于变电站、发电厂、井下这类高温、高危、高辐射地区，机器人往往因为信号干扰不能利用GPS全局定位系统，这时就需要机器人在自身位置不确定、地图环境未知的情况下完成导航与定位，这就是移动机器人同时制图与定位问题，即SLAM（Simultaneous Localization and Mapping）问题[8]。

到目前为止，SLAM问题得到机器人界众多学者的重视与研究。Menegatti E等人构建了无线传感器网络，机器人通过接收到信号强度的不同与距离量测，使用REKF（鲁棒卡尔曼滤波：Robuts Extended Kalman Filter）解决了SLAM问题[9]。刘明雍等人研究了基于随机信标的水下SLAM问题，解决了信标位置不固定时的水下航行器定位问题，并将该方法的定位精度与最小二乘法进行了比较[10]。Dauson A J等人针对携带单目摄像头的机器人，设计了一种可以复现其在未知场景里随机运动的轨迹的算法，该工作重点在于深入分析单目摄像头的工作原理与工作方式，如何将摄像头采集到的数据进行更好地复现，并将复现后的数字信号应用于解决SLAM问题[11]。以上这些文章都针对SLAM问题的不同方面进行了研究与解决，但SLAM问题针对煤矿机器人的应用少之又少。2019年1月，国家煤监局发布了《煤矿机器人重点研发目录》，鼓励支持煤矿企业与国内外科研单位、机器人制造企业开展合作，大力研发应用煤矿机器人。其中就包括具备远程无线通信、自主行走、导航避障功能的露天矿卡车无人驾驶系统。

因此，针对大型煤矿中的露天移动机器人没有携带惯性导航设备，也无先验地图的情况[12-13]，通过自身携带的传感器与环境中随机布放的信标进行通信，在实现移动机器自身导航定位的基础上，对环境中随机信标的位置也进行了估计。同时，还分析了不同量测噪声对SLAM定位算法的影响。研究表明，基于卡尔曼滤波的SLAM算法很好地控制了移动机器人在未知环境中的定位误差，有效提升了机器人的定位精度。

1 露天移动机器人多信标SLAM问题  在某一大型露天矿场中存在一露天移动机器人，移动机器人在未知环境中，从一个未知位置开始移动，在移动过程中根据自身携带的传感器与环境中随機布放的信标进行应答式通信，建立增量式地图，进行同时制图与定位，从而完成不需要任何预先定义的地图或预先设置的位置已知的信标便可实现的机器人自主导航与定位[14-15]。露天移动机器人SLAM定位如图1所示。

将露天矿场环境简化为一个二维空间，在该空间内随机分布着多个位置未知的信标，露天机器人在该空间内移动。当机器人移动到路径中的某一点X时，此时可以观测到信标1、信标2与信标5.但信标3，信标4此时未被观测到。随着机器人的移动，信标3与信标4先后被观测到，从而将信标3，信标4的位置信息加入增广的观测矩阵中。对SLAM问题而言，机器人在环境特征未知的一个二维空间中运动，根据已知的运动模型，利用携带的传感器观测附近信标的位置，从而通过观测到的信标位置确定自身位置，实现导航与定位[16-17]。用数学方法将SLAM问题描述为

机器人的状态，被称为States，用Xp描述。

信标的位置，被称为Landmarks，用XjL描述，其中，j=1，2，…，m.

机器人的运动模型，被称为Motion，即XP（k）=f（XP，uk）+vk，其中，uk为机器人在k时刻的控制输入;vk为服从均值为零的高斯白噪声，且协方差矩阵为Qk.

机器人的观测模型，被称为Observation，即Zj（k）=h（XP，XjL）+wk，表示机器人在第k时刻观测到的第j个信标的位置;wk为服从均值为零的高斯白噪声，且协方差矩阵为Rk.

在SLAM问题中，指导机器人运动的信息有输入uk及观测量Zj（k），根据uk，Zj（k）求解XP，XjL的过程，就是解决SLAM问题的过程。

2 基于Kalman滤波的SLAM定位算法

图7～图8为信标landmarks=20时的SLAM算法仿真图。图7中绿色曲线为机器人移动的真实路径，红色曲线为SLAM算法估计出的机器人移动路径。图7中围绕着各个信标逐渐密集的圆圈表示随着机器人的移动、对环境特征量的不断观测与算法的迭代，对信标的定位误差也逐渐减小，越来越接近信标的真实位置。图8为landmarks=20时，SLAM算法的路径定位误差图。相较于landmarks=2时，信标个数的增多，无论是x轴方向还是y轴方向的定位误差都控制在±0.5 m以内，有效提高了定位精度。

除了随机信标的个数，在仿真过程中，信标量测噪声r，距离量测噪声rd，速度量测噪声rv，过程噪声rp都将直接影响初始协方差矩阵Pk，量测噪声矩阵Rk，过程噪声矩阵Qk，从而对SLAM算法的定位精度造成影响。而量测噪声对初始协方差矩阵Pk，量测噪声矩阵Rk的影响尤为严重。因此文中对相同前提下的不同距离量测噪声rd，速度量测噪声rv进行了仿真分析，仿真中假设随机信标个数为2个，仿真时间600 s，分析结果如图9，图10所示。

图9，图10针对不同距离量测噪声rd，速度量测噪声rv的情况进行了仿真分析。从图中可以看出，当保持速度量测噪声rv不变，距离量测噪声rd增大时，将图9和图10中的红色曲线与绿色曲线做比较，抑或将两图中的蓝色曲线与黑色曲线作比较，都能发现随着距离量测噪声rd增大，无论在x轴方向还是y轴方向，SLAM算法的路径定位误差都增大。同样，当保持距离量测噪声rd不变，速度量测噪声rv增大时，将图9和图10中的红色曲线与黑色曲线做比较，抑或将两图中的蓝色曲线与绿色曲线作比较，都能发现随着速度量测噪声rv的增大，SLAM算法的速度定位误差也增大。由此可见，通过降低量测噪声，在一定程度上可以提高SLAM算法的定位精度，但传感器噪声受限于传感器的制造工艺、使用环境等因素。因此，如何有效控制传感器噪声带来的影响可作为下一步的研究重点。

4 结 论

1）基于Kalman滤波的SLAM算法能很好地估计出露天移动机器人的位置信息。并针对信标个数不同、信标位置随机的情况进行了仿真分析，该方法克服了露天移动机器人无惯导设备与先验地图情况下无法导航的困难。

2）给出系统的初始协方差矩阵、量测噪声协方差矩阵、过程噪声协方差矩阵的表达式。并对不同距离量测噪声、速度量测噪声的情况进行了仿真分析。研究表明，基于卡尔曼滤波的SLAM算法很好地控制了移动机器人在未知环境中的定位误差，保证了机器人的定位精度。

3）不足之处在于，该方法没有针对如何更精确地确定信标量测位置，现有的研究也仅限于理论分析，后续研究应使用真实传感器以获得更准确的数据，在这些数据的基础上通过改进算法致力于降低导航误差，也应针对系统噪声未知、量测噪声多变的现实环境进行后续研究。

参考文献（References）：

[1] Dissanayake M W M G，Newman P，Clark S，et al.A solution to the simultaneous localization and map building （SLAM） problem[J].Csorba IEEE Transactions on Robotics and Automation，2001，17（3）：229-241.

[2]Bailey T，Durrant-Whyte H.Simultaneous localization and mapping（SLAM）：part Ⅱ[J].IEEE Robotics & Automation Magazine，2006，13（3）：108-117.

[3]Durrant-Whyte，Hugh，Tim Bailey.Simultaneous localization and mapping：part Ⅰ[J]. Robotics & Automation Magazine，IEEE，2006，13（2）：99-110.

[4]薛永勝.变电站巡检机器人SLAM算法及其应用研究[D].重庆：西南科技大学，2015.

XUE Yong-sheng.Substation inspection robot SLAM algorithm and its application research[D].Chongqing：Southwest University of Science and Technology，2015.

[5]张志超，郑之增，方海峰，等.矿井救灾机器人的导航定位研究[J].煤矿机械，2008，29（11）：41-43.

ZHANG Zhi-chao，ZHENG Zhi-zeng，FANG Hai-feng，et al.Research on navigation and localization technologies for mine rescue robots[J].Coal Mine Machinery，2008，29（11）：41-43.

[6]孙 振，胡金磊，罗建军，等.变电站智能巡检机器人导航定位技术设计[J].自动化技术与应用，2018，37（11）：86-89.

SUN Zhen，HU Jin-lei，LUO Jian-jun，et al.Design of navigation and positioning technology for intelligent inspection robot in substation[J].Techniques of Automation and Application，2018，37（11）：86-89.

[7]耿世毅.煤矿井下机器人准确定位研究[D].郑州：河南理工大学，2014.

GENG Shi-yi.Research on accurate localization of underground robot[D].Zhengzhou：Henan Polytechnic University，2014.

[8]王 怡.机器人同时定位与地图创建研究[D].上海：复旦大学，2009.

WANG Yi.Research on simultaneous localization of robots and construction of land maps[D].Shanghai：Fudan University，2009.

[9]Menegatti E，Zanella A，Zilli S，et al.Range-only SLAM with a mobile robot and a Wireless Sensor Netw[J].IEEE International Conference on Robotics & Automation，2009：1109-1115.

[10]刘明雍，董婷婷，张立川.随机信标的水下SLAM导航方法[J].系统工程与电子技术，2015，37（12），2830-2834.

LIU Ming-yong，DONG Ting-ting，ZHANG Li-chuan.Underwater SLAM navigation algorithm based on random beacons[J].Systems Engineering and Electronics，2015，37（12）：2830-2834.

[11]Davison A J，Reid I D，Molton N D，et al.Mono SLAM：Real-time single camera SLAM[J].IEEE Transactions on Pattern Analysis & Machine Intelligence，2007，29（6）：1052-1067.

[12]Micha R Nowicki，Dominik Belter，Aleksander Kostusiak，et al.An experimental study on feature-based SLAM for multi-legged robots with RGB-D sensors[J].Industrial Robot，2017，44（4）：428-441.

[13]Smith，Randall，Matthew Self，et al.Estimating uncertain spatial relationships in robotics[M].Springer New York：Autonomous Robot Vehicles，1990.

[14]周 武，赵春霞，沈亚强，等.基于全局观测地图模型的SLAM研究[J].机器人，2010，32（5）：647-654.

ZHOU Wu，ZHAO Chun-xia，SHEN Ya-qiang et al.SlAM research based on global observation map model[J].Robot，2010，32（5）：647-654.

[15]程见童，张为华，江振宇.同时定位与制图辅助的GPS/DR组合导航[J].国防科技大学学报，2014，36（3）：72-76.

CHENG Jian-tong，ZHANG Wei-hua，JIANG Zhen-yu.GPS/DR integrated navigation aided by simultaneous localization and mapping[J].Journal of National University of Defense Technology，2014，36（3）：72-76.

[16]Zhang J，Shi C，Sun D，et al.High-precision，limited-beacon-aided AUV localization algorithm[J].Ocean Engineering，2018，149：106-112.

[17]王宏健，王 晶，边信黔，等.基于组合EKF的自主水下航行器SLAM[J].机器人，2012，34（1）：56-64.

WANG Hong-jian，WANG Jing，BIAO Xin-qian，et al.Slam of AUV based on the combined EKF[J].Robot，2012，34（1）：56-64.

[18]季秀才，鄭志强，张 辉.SLAM问题中机器人定位误差分析与控制[J].自动化学报，2008，34（3）：323-330.

JI Xiu-cai，ZHENG Zhi-qiang，ZHANG Hui.Analysis and control of Robot Position Error in SLAM[J].Acta Automatica Sinica，2008，34（3）：323-330.

[19]曹 军，曾 碧，何元烈.基于路标观测的改进EKF-SLAM算法[J].自动化与信息工程，2014（1）：21-26.

CAO Jun，ZENG Bi，HE Yuan-lie.Improved EKF-SLAM algorithm based on road sign observation[J].Automation and Information Engineering，2014（1）：21-26.

[20]厉茂海，洪炳馆，罗荣华.移动机器人的同时定位和地图创建方法[J].哈尔滨工业大学学报，2004，36（7）：874-876.

LI Mao-hai，HONG Bing-guan，LUO Rong-hua.Simultaneous localization and map building for mobile robot[J].Journal of Harbin Institute Technology，2004，36（7）：874-876.

[21]陈卫东，张 飞.移动机器人的同步自定位与地图创建研究进展[J].控制理论与应用，2005，22（3）：455-460.

CHEN Wei-dong，ZHANG Fei.Review on achievements in simultaneous location and map building for mobile robot[J].Control Theory and Applications，2005，22（3）：455-460.

[22]Thrun S，Liu Y.Multi-robot SLAM with sparse extended information filers[J].Robotics Research Sienna，2005，15（5）：254-266.

[23]付梦印.Kalman滤波理论及其在导航系统中的应用[M].北京：科学出版社，2003.

FU Meng-yin.Kalman filtering theory and its application in navigation system[M].Beijing：Science Press， 2003.

[24]Chen S Y.Kalman filter for robot vision：a survey[J].IEEE Transactions on Industrial Electronics，2012，59（11）：4409-4420.