煤矿巡检机器人同步定位与地图构建方法研究.pdf

工矿自动化 Ind ustr ya nd MineAuto ma tio n 第45卷第9期 2019年9月 Vo l . 45 No . 9 Sep.2019 文章编号1671-251X201909-0018-07DOI10. 13272/j. issn. 1671-251x. 17444 煤矿巡检机器人同步定位与地图构建方法研究 杨林12 ,马宏伟12 ,王岩12 ,王川伟12 ,张珍珍12 （1.西安科技大学机械工程学院％陕西西安710054； 2.陕西省矿山机电装备智能监测重点实验室陕西西安710054） 扫码移动阅读 摘要针对煤矿井下无GPS环境下巡检机器人自主定位问题，研究了基于激光雷达的同步定位与地图 构建方法。首先建立激光雷达观测模型和里程计预测模型，将机器人定位和地图构建的实际问题转换为概 率数学模型的逻辑推理问题。同时采用自适应蒙特卡罗定位算法进行机器人实时位姿估计，提出了根据粒 子权重地图的匹配度进行重采样的方法，以去除权重小的粒子，实现了用较少、较好粒子精确表达机器人 位姿的后验概率分布，满足机器人利用传感器在栅格地图上实时定位的需求。通过对Fa st-SLAM算法进 行优化，减少了粒子数量，缓解了粒子耗散，提高了地图构建的精确性。实验结果表明，基于激光雷达的同步 定位与地图构建方法有效解决了巡检机器人实时位姿估计和环境地图构建的问题，结合自适应蒙特卡罗定 位算法和优化Fa st-SLAM算法提高了机器人定位的自适应性和地图构建的精确性 关键词煤矿巡检机器人；位姿估计；同步定位；地图构建；地图匹配；激光雷达 中图分类号TD67 文献标志码A Resea r c h o n meth o d o fs imul ta neo usl o c a l iza tio na nd ma pping o fc o a l mineins pec tio nr o b o t YANG Lin1,2 , MA Ho ng wei1,2, WANG Ya n1,2 , WANG Ch ua nwei1,2 , ZHANG Zh enzh en1,2 1.Coleg eo fMec h a nic a l Eng ineer ing ,Xia n Univer sity o f Sc ienc e a nd Tec h no l o g y, Xia n 710054,Ch ina ; 2. Sh a a nxi Key La b o r a to r y o f Mine El ec tr o mec h a nic a l Equipment Intel l ig ent Mo nito r ing , X\n 710054, Ch ina Ab str a c t In view o f pr o b l em o f a uto no mo us l o c a tio n o f inspec tio n r o b o t with o ut GPS in und er g r o und c o a l m1ne,a meth o d o fs1mul ta neo usl o c a l za t1o na nd ma pp1ng b a sed o nl d a r wa sstud 1ed .F1r stl y,th e o b ser va t1o n mo d el o fl d a r a nd pr ed 1c t1o n mo d el o fo d o meter a r ees ta b l s h ed ,a nd th ea c tua l pr o b l ems o f r o b o tl o c a l za t1o n a nd ma pp1ng a r e tr a nsfo r med 1nto th e l o g 1c a l r ea so n1ng pr o b l ems o f pr o b a b l s tc ma th ema t1c a l mo d el .Atth esa met1me,th ea d a pt1veMo nteCa r l o l o c a l za t1o na l g o r 1th m1sused to est1ma te th er ea l -t1mepo seo fth er o b o t,th er esa mpl ng meth o d b a sed o npa r t1c l ewe1g h tma psma tc h 1ng d eg r ee1s pr o po sed to r emo ve pa r t1c l es wth smal we1g h t, a c c ur a te r epr esenta to n o f po ster 1o r pr o b a b l ty d istr ib utio n o f r o b o t po stur e with fewer a nd b etter pa r tic l es is r ea l ized , r equir ement o f using senso r s to r ea l ize,h er ea l -,imepo s i,io ning o fr o b o ,s o nr a s ,er ma psis me,.Fa s,-SLAM a l g o r i h m is o p,imized ,o r ed uc eth enumb er o fpa r tic l es ,a nd mitig a tepa r tic l ed issipa tio n,so a sto impr o vea c c ur a c yo f ma pping . Th e exper imenta l r esul ts sh o w th a t th e meth o d e fec tivel y so l ves th e pr o b l em o f r ea l -time po se estima tio n a nd envir o nmentma pping o finspec tio nr o b o t,a nd impr o vesth esel f-a d a pta b il ityo fr o b o tl o c a l iza tio na nd a c c ur a c yo fma pping c o mb ining with a d a ptiveMo nteCa r l o l o c a l iza tio na l g o r ith ma nd o ptimized Fa st-SLAM a l g o r ith m. 收稿日期收稿日期019-05-21；修回日期修回日期019-08-19；责任编辑责任编辑张强。 基金项目基金项目国家自然科学基金资助项目50674075；道路施工技术与装备教育部重点实验室项目“00102259508陕西省科技统筹创新工程计 划项目2013KJTCL01-02。 作者简介作者简介杨林1991一，男，陕西西安人，博士研究生，主要研究方向为移动机器人定位与导航，E-ma il 327160256qq. c o m。 引用格式引用格式杨林，马宏伟，王岩，等.煤矿巡检机器人同步定位与地图构建方法研究*.工矿自动化，2019,4591824. YANG LinMA Ho ng weiWANG Ya n eta l 8Res ea r c h o n meth o d o fs imul ta neo us l o c a l iza tio na nd ma pping o fc o a l mineins pec tio n r o b o t*. Ind us tr y a nd Mine Auto ma tio n,2019,459 18-24. 2019年第9期杨林等煤矿巡检机器人同步定位与地图构建方法研究 19 Keywords c o a l mine inspec tio n r o b o t; po se estima tio n; simul ta neo us l o c a l iza tio n ; ma pping ; sc a n- ma tc h ing fo r ma p;l id a r 0引言 对煤矿井下巷道环境和设备运行状态进行安全 巡检，是煤矿企业安全生产的重要保障。传统煤矿 井下的巡检分为2种一是人工巡检，由于效率低、 强度大、危险性高而逐渐被淘汰；二是视频监控，存 在覆盖面积小、投入成本高、维护效率低等问题 随着机器人技术的快速发展，人们希望危险性、重复 性的巡检工作能由机器人替代自己完成，尤其是近 年来，在全世界引发了应用于煤矿巡检领域的特种 机器人的研究热潮定位导航技术是煤矿井下 巡检机器人一个非常重要的研究课题，也是实现真 正智能化和完全自主移动的关键所在，它的核心是 要解决关于机器人“在哪”和“要去哪”的问题［45］，解 决该问题的关键点在于如何实现巡检机器人的自主 定位。 早期的巡检机器人通过将已知地图存储在机器 中来实现自主定位和导航，但并不是所有的离线地 图都与实时环境信息能保持高度的一致性，因此，需 要根据实际情况来解决这种离线式工作的局限性问 题。为此，SLAM （ Simul ta neo us Lo c a l iza tio n a nd Ma pping，同步定位与地图构建）技术应运而生。 SLAM 是解决 矿 GPS 移动机器人 自主定位的最佳手段。SLAM最早于20世纪 80年代被提出，但不能准确地通过数学形式表达， 之后人们逐渐提出了基于卡尔曼滤波的SLAM算 法并引入概率论研究方法6。20世纪80年代末， R. Smith等⑺明确地通过概率随机估计方式来表 述SLAM问题,认为机器人在定位过程中会不可避 免产生误差的累积，于是提出采用状态向量及其协 方差矩阵来表示机器人位置和路标位置，称为随机 地图或扩展卡尔曼滤波。21世纪初期，SLAM的相 关研究得到了迅速发展，主要集中在降低算法的运 算量方面。J. E. Guiva nt等）9*提出了基于扩展卡 尔曼滤波的改进算法，能够有效地降低计算复杂度。 M. Mo ntemer l o 等）10* 提出的 Fa st-SLAM 算法使用 粒子滤波算法，其优点是可以通过控制粒子数量来 调整计算量，同时能够处理不确定数据之间的关联 问题⑴*。 国内对于SLAM的研究主要是通过激光雷达 建立栅格地图，采用多种定位与建图的不同组合进 行SLAM算法改进。徐曙）12提出了一种基于A 算法的逆向D算法。厉茂海等［13］提出了以Ra o- Bl a c kwel l ized粒子滤波为基础，结合融合自适应算 法的粒子重采样算法。张文玲［14］利用自适应的 SLAM算法更好地实现了移动机器人的自身定位 和环境地图建立。朱磊等）15*在未知环境SLAM中 使用了人工鱼群算法来更新预测粒子，使预期粒子 更接近机器人的实际姿态。罗元等）16*将RBPF （Ra o -Bl a c kweuized Pa r tic l e Fil ter）和退火参数优化 混合提议分布相结合，实现室内机器人的SLAM。 以上研究成果为研究煤矿井下巡检机器人的同步定 位与地图构建方法提供了理论依据。 基 上， 文研究 基 Fa st-SLAM 巡检机器人同步定位与地图构建方法，通过激光雷 达获取实时环境信息，并进行多特征匹配来达到定 位和减小定位误差的目的，同时构建出机器人周围 的环境地图。该方法是针对巡检机器人在煤矿井下 无GPS情况下实现自主定位的最佳解决方案。在 机器人自主导航过程中，无论是局部实时避障还是 全局规划，都需要精确地知道巡检机器人的当前位 置。定位是完成导航任务首先要解决的问题，同时 也是其他高级导航任务的基础，精确的定位是提高 巡检机器人性能的关键。 1煤矿井下巡检机器人控制系统设计 1. 1 系统总体架构 煤矿井下巡检机器人控制系统是以轮式移动机 器人为本体结构、以ROS （ Ro b o t Oper a ting System）开源机器人系统为总体架构，采用上下位 机二级式分布共同搭建的，上下位机之间采用WiFi 通信。整个系统由轮式机器人主体、车载计算机、双 目相机、激光雷达、电动机驱动器、直流电动机、红外 传感器、超声传感器、惯导元件、云台摄像机等硬件 构成，总体架构如图1所示。轮式机器人主体包含 2个驱动轮和2个同步轮，电动机驱动器与车载计 算机通过串口通信。双目相机用来获取图像信息, 以进行机器人位姿状态估计，通过USB与车载计算 机通信。激光雷达能够实时获取机器人与周围环境 的距离测量值，从而实现地图构建，通过TCP/IP与 车载计算机相连。云台摄像机负责监测区域的实时 监控。 1. 2 系统工作原理 系统工作原理以轮式机器人为主体，以高性能 计算机作为控制核心，以直流电动机、编码器、霍尔 传感器作为运动控制系统，以红外热像仪、可见光摄 ・20・ 工矿自动化2019年第45卷 图1煤矿井下巡检机器人控制系统总体架构 Fig 1 Over ala r c h itec tur eo fc o a l mineund er g r o und inspec tio nr o b o tc o ntr o l system 像机、气体 器等作为远程监测系统，以超声、红 外传感器作为应急避 统，以多线激光雷达和双 目相机结合而成的多 器为 知系统。其 中，激光雷达负责周围环境的地图构建，双目相机负 自 估计，与激光雷 建地图相结合，实现 机器人实 、构建地图的功能，并 地图环境 机器人 进行行为决策。其工理 如图2所示。 图2煤矿井下巡检机器人控制系统工作原理 Fig 2 Wo r king pr inc ipl eo fc o a l mineund er g r o und inspec tio nr o b o tc o ntr o l system 1. 3 系统软件实现 统软件功能如图3所示。根据不同的功能要 求将机器人控制系统划分为4个 知模 、地图构建 、行为决策 、运动控制 X 中, 知 能是感知机器人 ，并基 器 为 建立 详细 述;地图构建 任务是利用激光雷达采集 到的实时距离信息，结合SLAM算法和基于粒子滤 波的自适应蒙特卡罗（Ad a ptive Mo nte Ca r l o Lo c a l iza tio n, AMCL）算法构建环境栅格地图和定 位;行为决策模块的任务是利 入 计算得出行为层面的决策；运动控制 计 目标 让机器人 能地按照行为决策规划的动 作序列运动。 图3煤矿井下巡检机器人控制系统软件功能 Fig 3 So ftwa r efunc tio no fc o a l mineund er g r o und inspec tio nr o b o tc o ntr o l system 2巡检机器人同步定位与地图构建 SLAM可以描述如下机器人在未知环境中开 始移动，在移动过程中利用内部 器 进行自 估计，并结合外部 器感知 外部 i 进行自 ，同 自 基础上 建增量式地图，实现机器人 导航等功 能「17「18*。 程 学 上来说是非线性滤波 程，因此，求解 题可看作是最优 滤波。 中SLAM问题可以归纳为 器 情 况下，同时估计机器人 建 地图 题, 以学式为 “（1 ,| /m, I 11，11为栅格在t 1时刻的状态； /m,为栅格的先验值，此值对所有栅格相同，可 。 由此可见，t时刻的栅格地图估计问题可转换 成t时刻激光雷达的逆观测模型和t 1时刻栅格 地图 题。 3巡检机器人重定位 为了使机器人能够利用传感器将自己实际位置 信息表示在已构建的栅格地图上，需要进行重定位。 本文利用AMCL算法进行重定位，该算法在机器人 定位领域应用广泛。假设机器人在t时刻的位姿是 ，-，““为航向角，激光雷达传感器从开始到 t时刻为止的所有观测值为1 {1，2，，,},里 程计从开始到t时刻所获得运动信息的预测值为 改进pr o po sa l分布函数的方法，将 由里程计预测 动学模型转换为精确度 更高的激光雷 测高斯分 型进行 ，极大 程度上 重 量。 5 研究了激光SLAM的同步定位与地图构 建方法，实现了巡检机器人 地图构建。实验 果 ， 相 ， 自 适 特卡 算法提高 估计的自适 ，改进Fa s- SLAM 算法 量 解 ， 提 高了地图构建的精确 控制系统的鲁 。 参考文献References 1 *姜俊英，周展，曹现刚，等煤矿巷道悬线巡检机器 人结构设计及仿真［J*.工矿自动化，2018,445 76-81. 24 工矿自动化2019年第45卷 JIANG Junying , ZHOU Zh a n, CAO Xia ng a ng , et a l . Str uc tur ed esig no fsuspensio nl ineins pec tio nr o b o tin c o a l mine r o a d wa y a nd its simul a tio n [J*. Ind ustr y a nd Mine Auto ma tio n, 2018,445 76-81. 13* 武汉华中科技大学2014. 厉茂海，洪炳熔，罗荣华用改进的Ra o Bl a c kwel l ized粒子滤波器实现移动机器人同时定位 和地图创建［J*.吉林大学学报，2007,37 2 2*汤怀量，雷泽勇，雷林，等.巡检与核应急机器人研 究现状分析*.机械工程师，2018220-23. TANG Hua il ia ng, LEI Zeyo ng, LEI Lin, et a l Resea r c h sta tuso finspec tio na nd nuc l ea r emer g enc y r espo nser o b o tJ* Mec h a nic a l Eng ineer,2018220-23 401-406 LIMa o h a i HONG Bing r o ng LUO Ro ng h ua Impr o ved Ra o Bl a c kwelized pa r tic l efil ter sfo r mo b il e r o b o t simul ta neo us l o c a l iza tio n a nd ma pping J* Jo ur na l o f Jil in Univer sity2007 372 401-406 3*杨林，马宏伟，王川伟，等.校园巡检机器人智能导 航与控制*.西安科技大学学报，2018,38 6 14*张文玲.移动机器人同时定位与地图创建自适应算 法研究［D*.合肥中国科学技术大学，2009. 1013-1020 YANG Lin, MA Ho ng wei, WANG Ch ua nwei, et a l . Intelig ent na vig a tio n a nd c o ntr o l o f c a mpus inspec tio n r o b o t*. Jo ur na l o f Xia n Univer sity o f Sc ienc e a nd Tec h no l o g y 2018 3861013-10208 15* 朱磊，樊继壮，赵杰，等.改进粒子滤波器的移动机 器人同步定位与地图构建方法*.重庆大学学报, 2014,37439-45. ZHU Lei , FAN Jizh ua ng , ZHAO Jie , et a l . A meth o d fo r mo b il e r o b o t SLAM b a sed o n mo d ified 4* LEONARDJJDURRANT-WHYTE H F8Mo b il e r o b o t l o c a l iza tio n b y tr a c king g eo metr ic b ea c o ns*. pa r tic l e fil ter [J*. Jo ur na l o f Ch o ng qing Univer sity , 2014,37439-45. IEEE Tr a nsa c io ns o n Ro b o ic s a nd Au,o ma io n 1991,73376382. 16*罗元，苏琴，张毅,等.基于优化RBPF的同时定位 与地图构建*.华中科技大学学报自然科学版, 5*DURRANTWHYTE. Wh er e a m I/ a tuto r ia l o n mo b il e veh ic l e l o c a l iza ,io n J *.Ind us ,r ia l Ro b o , 1994,2121116. 2016 , 44530-34. LUO Yua n , SU Qin , ZHANG Yi , et l Simul ta neo us l o c a l iza tio n a nd ma pping 6*CADENA C, CARLONE L, CARRILLO H, et l Pa stpr esenta nd futur eo fsimul ta neo usl o c a l iza tio n a nd ma pping to wa r d th e r o b ust-per c eptio n a g e[J*. impl ementa tio n b a sed o n o ptimized RBPFJ* Jo ur na l o f Hua zh o ng Univer sity o f Sc ienc e a nd Tec h no l o g y Na tur a l Sc ienc e Ed itio n , 2016,445 30-34. IEEE Tr a nsa c tio ns o n Ro b o tic s, 2016 , 32 6 1309-13328 17*任祥华.激光雷达室内SLAM方法［D*.哈尔滨哈 工程大学 2018 7* SMITH RSELF MCHEESEMAN P8Estima ting unc er ta in spa tia l r el a tio nsh ips in r o b o tic s J *8 Ma c h ine Intel l ig enc e Pa tter n Rec o g nitio n, 201355 435-461. 18 *武二永，项志宇，沈敏一,等.大规模环境下基于激 光雷达的机器人SLAM算法*.浙江大学学报工 学版,2007,41121982-1986. WU Er yo ng , XIANG Zh iyu , SHEN Minyi , et l 8*GUIVANT J E, NEBOT E M. Optimiza tio n o f th e simul ,a neo usl o c a l iza io na nd ma p-b uil d ing a l g o r ih m fo r r ea l -imeimpl emen,a io nJ*.IEEE Tr a nsa c ,io ns o n Ro b o tic s Auto ma tio n, 2002,173 242-257. Ro b o tSLAM a l g o r ith m b a sed o nl a ser r a ng efind er fo r l a r g e sc a l e envir o nment [J*. Jo ur na l o f Zh ejia ng Univer sity Eng ineer ing Ed itio n 2007 41 12 1982-19868 9 * GUIVANT J ENEBOT E M.So l ving c o mpu,a io na l a nd memo r y r equir ements o f fea tur e-b a sed 19* 赵希宇救援机器人的同时定位建图与导航研究 ［D*.沈阳沈阳工业大学，2018 simul ta neo us l o c a l iza tio n a nd ma pping a l g o r ith msJ*. IEEE Tr a nsa c tio ns o n Ro b o tic s Auto ma tio n, 2003,194749-755. 20* 王依人，邓国庆，刘勇,等.基于激光雷达传感器的 RBPF-SLAM系统优化设计*.传感器与微系统, 2017,36977-80. 10*MONTEMARLO M.Fa stSLAM a fa c to r ed so l utio n to th es imul ta neo usl o c a l iza tio na nd ma pping pr o b l em [C*// Pr o c eed ing s o f th e AAAI Na tio na l Co nfer enc e o n Ar tific ia l Intel l ig enc e, Ed mo nto n, 2002 593-598. WANG Yir en , DENG Guo qing , LIU Yo ng , et l Optima l d esig n o f RBPF-SLAM system b a sed o n LIDAR senso r J *.Tr a nsd uc er a nd Mic r o system Tec h no l o g ies2017 36977-80. 11* NIETOJGUIVANTJNEBOTEeta l 8Rea l time d a ta a sso c ia tio n fo r Fa stSLAM C *//IEEE Inter na tio na l Co nfer enc e o n Ro b o tic s a nd Auto ma tio n2003 412-418. 21 *张亚楠，孙丰财，史旭华一种改进的RBPF激光 SLAM算法*.无线通信技术，201741620. ZHANG Ya na n SUN Feng c a i SHI Xuh ua 8 An impr o ved RBPF l a ser SLAM a l g o r ith mJ*8Wir el ess 12*徐曙.基于SLAM的移动机器人导航系统研究［D*. Co mmunic a tio nTec h no l o g y2017416-208