基于改进RBPF的变电站巡检机器人建图方法研究.pdf

第6 0 卷第6 期2023年6 月15日电测与仪 表Electrical Measurement&InstrumentationVol.60 No.6Jun.15,2023基于改进RBPF的变电站巡检机器人建图方法研究于志浩，张艳荣，张安迪，徐雪莲（1.西南交通大学机械工程学院，成都6 10 0 31；2.燕山大学信息科学与工程学院，河北秦皇岛0 6 6 0 0 4）摘要：为解决传统粒子滤波（Rao-BlackwellizedParticleFilter,RBPF）巡检机器人建图方法在非结构化环境中计算精度低、计算量大等问题，文章提出一种基于点云匹配的改进RBPF变电站巡检机器人建图方法。基于RBPF方法设计子图构建策略，引人Adaboost学习算法识别相邻子图；针对传统点云匹配方法在拼接仅有部分重合的子图时，拼接精度低的现象,将NDT和ICP算法相结合，设计匹配算法，求解子图间相对位姿。然后，根据子图间相对位姿，通过图优化（General GraphOptimization，G 2 0）算法对子图全局位姿进行优化求解，得到完整的变电站地图；采用Gazebo软件进行仿真,对不同方法的建图效果进行对比。结果表明：改进RBPF方法在变电站的复杂环境下能够降低硬件成本、提高建图精度，可以为无人值守变电站的设计提供参考。关键词：地图构建；变电站；粒子滤波；点云匹配；机器人巡检D0I:10.19753/j.issn1001-1390.2023.06.004中图分类号：TM769Research on mapping method of substation inspection robot based on(1.School of Mechanical Engineering,Southwest Jiaotong University,Chengdu 610031,China.2.School of Information Science and Engineering,Yanshan University,Qinhuangdao 066004,Hebei,China)Abstract:In order to solve the problems of traditional Rao-Blackwellized particle filter(RBPF)inspection robot mappingmethod in the unstructured environment,such as low calculation accuracy and large calculation amount,an improved RB-PF substation inspection robot mapping method based on point cloud matching is proposed in this paper.Firstly,based onthe RBPF method,the subgraph construction strategy is designed,and the Adaboost learning algorithm is introduced to i-dentify adjacent subgraphs.Secondly,in view of the low splicing accuracy of traditional point cloud matching methodswhen splicing only partially overlapping sub-images,the NDT and ICP algorithms are combined to design a matching algo-rithm to solve the relative poses between sub-images.Thirdly,according to the relative poses between the sub-graphs,theglobal pose of the sub-graphs is optimized through the general graph optimization(G20)algorithm to obtain a completesubstation map.Finally,the Gazebo software is used for simulation to compare the mapping effects of different methods.The results show that the improved RBPF method can reduce costs of hardware,improve the accuracy of mapping in thecomplex environment of substations,and can provide a reference for the design of unattended substations.Keywords:mapping construction,substation,Rao-Blackwellized particle filter,point cloud matching,robot inspection0引 言自主巡检的前提是能够在未知环境中运用激光雷达等近年来,用于变电站的自主巡检机器人技术逐渐传感器建立机器人所在环境的栅格地图。栅格地图的成为机器人领域的研究热点13。变电站机器人实现精度和效率直接影响巡检机器人的运行效果。因此，对变电站巡检机器人建图方法的研究是极其必要的。基金项目：四川省科技计划项目（2 0 17 GZ0164）；四川省科委科在变电站环境下建图需要面对环境物体分散、特征区研基金项目（2 0 2 0 YJ0035）分度小、建图场景大等问题。当前机器人建图通常采一2 6 一文献标识码：Aimproved RBPFYu Zhihao,Zhang Yanrong,Zhang Andi,Xu Xuelian?文章编号：10 0 1-139 0(2 0 2 3)0 6-0 0 2 6-0 6第6 0 卷第6 期2023年6 月15日用RBPF方法和点云匹配方法。一方面,基于RBPF的建图方法因其采用蒙特卡洛采样理论，将机器人的建图过程转化为多个粒子（样本点)求解均值的过程，具有较高的建图速度,受到了业内的广泛关注。但RBPF方法具有精度发散快、粒子数目多而占用大量内存等缺陷。为改善RBPF方法的缺陷，目前较多学者在减少粒子数目、提高粒子精度等方面尝试对RBPF方法进行改进。文献4 提出了一种基于Dirichlet过程非参贝叶斯学习的高斯箱粒子滤波快速地图构建方法，该方法有效降低地图创建所需的粒子数,并提高了定位精度和实时性。文献5 则通过正则化滤波区域重采样的改进策略，提高了RBPF方法的精度和计算效率。文献6 引人相对熵概念,将粒子建议分布与相对嫡集成，用于优化粒子的权重和数量。通过对RBPF方法中采样粒子的优化，在一定程度上提高了建图精度和效率，但没有解决RBPF方法缺少回环的问题，建图精度仍然会随着地图的扩大而不断发散7 ,这使得机器人在变电站的环境下无法利用栅格地图实现定位导航等功能。另一方面，点云匹配算法是起源于激光重建的点云拼接算法，通过迭代计算查找能使两个点云相互重合的位姿变换，进而将点云拼接为栅格地图。点云匹配算法占用内存少，但存在计算量大、鲁棒性低的缺陷。针对点云匹配方法的缺陷，有学者进行了相关研究。文献8 提出了一种改进三维正态分布变换点云配准算法,有效提高了点云匹配速度。文献9 提出了一种无需检测动态目标且可以适应不同初始位置误差的快速、鲁棒的配准方法，能够在复杂的动态场景和较大的初始位置误差下完成精确可靠的点云配准。文献10在ICP算法基础上设计了滤波算法,避免了噪音点导致的错误匹配。文献11 针对三维重合度点云，运用曲率差值约束和法线差值约束实现高精度拼接。通过以上可知，当前点云匹配研究提高了算法的匹配速度、降低了噪音对拼接的影响。当前基于三维的低重合度点云拼接研究取得一定进展 。但用于二维点云匹配的算法以完全匹配两个激光点云为目标设计8-10 ,在面对重合部分占比较小的局部子图，以及在具有重复特征的变电站环境下，不能有效提取两个点云间的对应点。而计算过程中非对应点的加人会导致变电站相似特征间的错误匹配。鉴于此,拟提出一种基于RBPF和点云匹配方法的改进RBPF方法，将传统RBPF方法和点云匹配方法相结合,用RBPF方法构建子图,基于点云匹配方法计电测与仪表Electrical Measurement&Instrumentation算相邻子图的相对位姿，通过相对位姿求解子图全局位姿，以期建立精度较高的栅格地图,为后续变电站巡检工作提供参考。1子图构建算法设计机器人建图问题被描述为机器人从开始运动的0时刻到当前t时刻下,运用0 t-1时刻的里程计数据集和1t时刻的激光雷达观测数z=z，,z,计算机器人位姿集合s=s1，,s,和机器人所在环境的栅格地图m。这个过程等价于对联合概率密度p(s,mz,u)的求解。RBPF方法将联合概率密度分解为：p(s,m z,u)=p(m,I s,z)p(sl z,u)(1)由此将建图问题分解为估计位姿。以及在位姿和观测数据s的基础上创建地图mt，具体流程如图1所示。上一时刻机器人位姿si从已知分布p(s,Is-1,u-)中采样当前时刻机器人位姿s更新粒子权重Wi计算有效粒子数量Nf判断Nf是否大于2 N/3IY更新地图mi-上一时刻地图m-1图1RBPF方法建图流程框图Fig.1Block diagram of RBPF method mapping process利用上一时刻位姿估计结果对应的N个粒子(s-hi=和里程计的数据u-1,从已知分布p(s,Ist-1,ut-1）采样得到新的粒子集1si=N。此后更新每个粒子（采样点)的权重wi（s i）以及有效粒子个数Nefr：Wi(si)=Wi-l(si-)p(z,I ut-1)(2)NNefr=1/Z(wi)?i=1式中wi（s i）为t时刻第i个粒子的累计权重；Ner为有效粒子个数。当有效粒子Nerusub时，保存当前地图m,以及机器人权重最大的粒子的初始位置位姿1。然后令ubot等于0，重置粒子集（si)i=N，开始下一幅子图的构建。在机器人运动结束之后,获得子图集合m=m，,ms 。2基基于点云匹配的子图位姿计算方法设计在建立子图集合m=（m l，,m 之后,为将子图拼接为全局地图，需要获取相邻子图间的相对位姿。该算法基于点云匹配方法，将两顿子图分别视为待匹配点集Q=q1,qml和目标点集P=Pi,Pml。基于NDT算法,设计两组点集的粗匹配算法,在获取匹配结果的基础上,提出一种基于ICP的精匹配算法，用于计算两组点集完整匹配下的相对位姿。2.1粗匹配算法设计NDT算法假设点云服从正态分布，算法原理是查一2 8 一电测与仪表Electrical Measurement&Instrumentation找使得当前扫描点位于参考扫描平面上的可能性最大的姿态。NDT算法首先将点集P分割为若干个格子（c e l l），然后计算格子内部粒子的分布概率。定义正态分布：1px(2m)D/2/ZP1mmk1=1Z(Pe-)(P-)Tmm-1式中x为格子内部坐标点；p（）为坐标点对应的点集分布概率；D为点集维数。计算点集D中的各点在D分布下的累积概率,并获取里程计数据u构建负对数目标函数,求解使点集D的累积概率最大的变换位姿：继续下一时刻NVol.60 No.6Jun.15,2023(x-)Z(x-)exp2R,T =armin(-Z log(p(Rg+T)(6)R,T式中R*为粗匹配后，点集Q变换到点集P的最佳旋转角度对应的旋转矩阵;T则为最佳平移矩阵。运用高斯牛顿法对式（6)进行迭代计算，可以求得粗匹配后的旋转矩阵R和平移矩阵T。2.2精匹配算法设计ICP算法基于最小二乘理论,通过迭代计算,求解点集Q和点集P使对应点对平均距离最小的刚体变换参数。为提高计算效率,对应点设定为点集Q中各点到点集P中欧氏距离最近的点组成的集合，对应点间(4)的欧氏距离为作用距离。由于点集Q和点集P之间仅为部分重合点云，未重合部分各点通过计算欧式距离得到的对应点，在ICP算法计算过程中会导致匹配结果收敛至错误位姿。因此,为避免错误的收敛结果,文中设计了一种基于有效邻近点的改进ICP算法,通过剔除未重合部分各点的对应点集，提高匹配精度。在获得粗匹后位姿的基础上，首先定义点集Q和点集P间对应点的作用距离由小到大排列的集合Ra=ai,a,绘制辅助线an-a1(=(x-1)+aln-1定义Ra中第k个点到辅助线的距离Das：Dat=*%_-(h-1)+-an-1当（Da-Dak-1）O 时,取 Threshold=ak-1为作(5)=(7)(8)第6 0 卷第6 期2023年6 月15日用距离的阈值。当aThreshold时,该对应点作为有效邻近点带人ICP算法计算,若：T h r e s h o l d 则将该对应点舍弃。在获取有效临近点的后,进行基于ICP的精匹配算法设计。构建目标函数：Ri,T =a r g mi n(I R:+T -p:l l)(9)R,T式中R*为精匹配后，点集QP的最佳旋转角度对应的旋转矩阵；T*则为最佳平移矩阵。为进行求解，需定义两组点集的质心。和p：mqm=1P;m-1将式(10)带入式(9)得到：Ri=arg min I lp;=p-R(g-,)/mRi=1T=-Rua运用高斯牛顿法对式(11）进行迭代计算,求得精匹配后的旋转矩阵R*,将结果带人式（12）,可得平移矩阵T*。R*,T 即为子图间的相对位姿矩阵。基于点云匹配的子图位姿计算方法具体流程如图3所示。相邻子图立NDT粗匹配文是否收敛Y粗匹配位姿变换矩阵R，T 计算阅值ThresholdICP精匹配文是否收敛输出位置变换矩阵R,T图3子图位姿计算流程框图Fig.3 Block diagram of the sub-graph posecalculation process3建图算法总体设计在时间序列上相邻的子图可以通过位姿计算获取相对位姿，进而得到子图全局位姿，完成栅格地图的创电测与仪 表Electrical Measurement&Instrumentation建。但随着机器人运动历程的增加，当两副实际相邻子图因里程计的误差致使其期望位姿s1相距较远，无法识别为相邻子图，这种现象会使全局地图细节缺失。为改善位姿计算的局限性，根据相关研究12 16 ，采用Adaboost方法提取相邻子图，并利用G20方法进行优化，具体程如图4所示。机器人开始运动构建子图m：(10)构建子图m机器人完成运动(11)(12)子图初始位置NNVol.60 No.6Jun.15,2023Adaboost学习算法提取相邻子图计算相邻子图相对位姿R,列构建因子图G20算法优化子图位姿子图拼接为全局地图图4总体建图算法流程框图Fig.4 Block diagram of the overall mapping algorithm flow机器人开始运动后，连续的建立子图至机器人运动结束。采用Adaboost方法提取相邻子图。运用子图位姿计算方法求解相邻子图的相对位姿R,T*，将其作为因子图的边，而子图中机器人初始位姿s作为节点构建因子图。引人G20图优化算法对因子图中子图的位姿进行优化，降低子图定位误差，将子图拼接为全局地图。4仿真实验4.1仿真环境构建与实验设计为对比说明传统RBPF方法与文中改进RBPF方法在建图效果上的优劣,基于ROS系统中Gazebo机器人仿真软件对算法进行仿真分析。根据2 2 0 kV变电站的建筑特点,在Gazebo中建立由控制室、变压器、2 2 0kV设备区、避雷针等设施组成的DAE模型文件，用于模拟变电站环境，模型如图5所示。图5仿真环境示意图Fig.5 Schematic diagram of simulation environment一2 9 一：第6 0 卷第6 期2023年6 月15日通过URDF文件为机器人配置单线激光雷达、IMU和轮式里程计等传感器。其中激光雷达最大测距范围为2 0 m,16000次/s的采样频率，角分辨率0.54度。在仿真过程中,在Gazebo仿真软件中,操控机器人在变电站环境下移动，并记录传感器数据。沿既有路线行驶结束后，将传感器数据保存为数据集。通过回放数据集，使用改进RBPF方法与传统方法进行建图，对建图结果进行分析对比。4.2仿真结果与分析仿真过程中，机器人运动时长为6 4min，移动里程964.22m，平均移动速度1m/s。通过数据回放，运用改进RBPF建图方法、传统RBPF（G ma p p i n g）建图方法以及同样具有回环机制的Cartographer算法进行建图，并将建图结果与标准地图进行对比。记录各算法建立的地图中各点对标准地图中临近点的误差如图6 所示。依据数据集的建图效果如图7 所示。(1)建图精度由图6 及表1,改进RBPF方法所建地图由于采用多个子图互相拼接而成，避免了地图精度持续发散导致的累积误差，而传统RBPF方法由于误差的持续累计,在变电站仿真环境下所建地图的误差增长趋势最为陡峭，建图结果极度失真，不能反映机器人所在环境的地图特征。同时,改进RBPF方法由于所建子图精度较高,基于点云匹配的子图位姿计算方法可以较好地计算子图间相对位姿，所以其各点距标准地图临近点误差增长趋势最为平缓，误差的均值和方差相较于Cartographer算电测与仪表Electrical Measurement&Instrumentation1412w/1086420（a）临近点误差升序曲线图15H1050改进RBPFCartographer传统RBPF（b）临近点误差箱型图图6 建图误差对比图Fig.6Comparison chart of mapping errors口口口口口日口口Vol.60 No.6Jun.15,2023改进RBPFCartographet传统RBPF2345671点集序号/10 3丰HH法所建地图分别降低了56.4%和36.9%。在图7 中，传统RBPF建图方法在有连续的直线特征的场景下建图精度较高，但面对多组离散点时，由于缺少回环机制，局部误差持续积累，致使机器人期望位置与实际位置偏差过大，无法通过粒子滤波机制优化误差，不能有效控制地图延伸方向，导致地图错位。改进RBPF方法继承了传统RBPF方法地图局部精度高的特点，由于引人了基于点云匹配的子图拼接算法,其建图累计误差没有随着运动距离的增加而显著增长，完整准确地建立了巡检场景的二维地图。与改进RB-PF方法相比,具有回环机制的Cartographer算法构建完成的地图虽然可以反映巡检场景的整体特征，但局部保留细节较少,不能精准反映巡检环境的地形特征。(2）建图速度及计算资源消耗表2 列出了传统RBPF方法、改进RBPF方法和Cartographer算法的建图耗时和最大占用内存的多次实验下的平均值对比结果。显然，传统RBPF方法由于缺一30 一(a)标准地图品口(c)Cartographer图7 建图效果对比图Fig.7Comparison chart of mapping effect表1建图误差指标对比Tab.1Comparison of mapping error indicators改进 RBPF31.615 4Cartographer0.349 1传统RBPF2.760 5(b)改进 RBPF(d)传统RBPF(Gmapping)均值/m0.152 2U.uu方差50.07077 887.4第6 0 卷第6 期2023年6 月15日少针对位姿的回环求解，致使其计算时间较短，但因为粒子滤波的计算机制需要在建图过程中维系大量粒子,导致其内存占用较大。Cartographer算法通过相关性匹配建立了回环机制，但其采用激光帧至子图的回环方式，优化迭代过程耗时过长，比传统RBPF方法多了1.4倍。改进RBPF方法通过子图限制了每个粒子的内存占用体积,故相比于传统RBPF方法和Cartogra-pher算法分别节约了35.7%和30.8%的内存。由于采用子图至子图的回环方式，改进RBPF方法减少了优化对象,进而优化所需迭代次数较少，计算时间仅为Cartographer算法用时的8 5.6%。尽管改进RBPF方法比传统RBPF方法计算用时增长10 4.7%，但较好的建图精度和有限增长的建图时间证明该方法是可行有效的。表2 建图性能指标对比Tab.2(Comparison of mapping performance indicators最大内存占用/CB改进RBPF43.93Cartographer51.35传统RBPF21.015结束语(1)改进RBPF变电站巡检机器人建图方法，解决了巡检机器人在复杂环境下地图构建精度问题，满足变电站等复杂场景下的精度需求；（2)改进RBPF方法建图比传统RBPF方法建图时间增长10 4.7%，但仅为Cartographer算法用时的85.6%，可以满足巡检机器人的建图用时需求；（3）改进RBPF巡检机器人建图方法与传统RBPF方法相比节约35.7%的内存空间，可有效降低巡检机器人的硬件成本；(4)考虑到改进RBPF方法仍然需要配置多个传感器获取里程计数据，并将里程计数据作为先验信息带入算法计算。因此，如何在保持建图精度的前提下，减少传感器的使用是未来探索的方向。参考文献1黄山，吴振升，任志刚，等电力智能巡检机器人研究综述J电测与仪表，2 0 2 0,57(2)：2 6-38.Huang Shan,Wu Zhensheng,Ren Zhigang,et al.Review of electricpower intelligent inspection robot J.Electrical Measurement&Instru-mentation,2020,57(2):26-38.2戴永东，王茂飞，唐达樊，等基于双视卷积神经网络的输电线路自动巡检J.电力科学与技术学报，2 0 2 1，36（5）：2 0 1-2 10.电测与仪表Electrical Measurement&Instrumentation【3关家华，孙广慧，陆凯烨，等蛙跳式充电的无人机自主巡线技术与系统（一）：基于GPS/RTK的无人机自主定位J.电力科学与技术学报，2 0 2 1，36(5）：19 5-2 0 0.4罗景文，秦世引基于Dirichlet过程非参贝叶斯学习的高斯箱粒子滤波快速SLAM算法J.机器人，2 0 19，41(5）：6 6 0-6 7 5.Luo Jingwen,Tai Shiyin.Fast slam algorithm based on the non-para-metric bayesian learning of dirichlet process for gauss box particle filte-ringJ.Robot,2019,41(5):660-675.5李安旭.移动机器人即时定位与地图构建技术研究D成都：电子科技大学，2 0 19.6 Guan R P,Ristic B,Wang L,et al.KLD sampling with Gmapping pro-posal for Monte Carlo localization of mobile robots J.Information Fu-sion,2019(49):79-88.7 Kaijaluoto R.Precise indoor localization for mobile laser scanner D.Otakari 1,Espo0:Aalta University Masters Degree Thesis,2015.8 余洪山，付强，孙健，等面向室内移动机器人的改进3D-NDT点云配准算法J.仪器仪表学报，2 0 19，40（9）：151-16 1.【9 王任栋，徐友春，齐尧，等一种鲁棒的城市复杂动态场景点云配准方法J.机器人，2 0 18，40（3）：2 57-2 6 5.10Li X,Du S,Li G,et al.Integrate point-Cloud segmentation with 3D建图耗时/t/minLiDAR scan-matching for mobile robot localization and mapping J.0.9Sensors,2020,20(1):237.1.311甘璐豪，贺赛先低重叠度点云拼接方法研究J激光杂志，2019,40(3):84-90.1.412 Kaleci B,Senler C M,Dutagaci H,et al.Semantic classification of mo-bile robot locations through 2D laser scans J.Intelligent Service Ro-botics,2020,13(1):63-85.13李岩，刘玉娇，李国亮，等基于改进BEMD与Res-LSTM电气设备故障辨识方法J高压电器，2 0 2 2，58（11）：8 2-8 7，10 5.14杜文娇，叶齐政，袁哲，等基于可见光图像和机器学习的金具温升识别方法J高压电器，2 0 2 2，58（10）：2 2 1-2 2 9.15易晓东，杨思宁，杨绍武.实时更新的全局一致三维栅格建图J.模式识别与人工智能，2 0 19，32（1）：43-50.16刘彪，袁文海，董小顺，等基于改进边缘连接Canny算法的绝缘子憎水性图像分割研究J高压电器，2 0 2 2，58（1）：16 2-16 9.作者简介：于志浩（19 9 6 一）,男，硕士研究生，从事智能机器人控制技术研究。张艳荣（19 7 1一）,女，副教授，博士，从事嵌人式技术研究。Email:yrzhang 张安迪（19 9 3一），男，硕士研究生，从事嵌人式技术研究。徐雪莲（19 9 6 一），女，硕士研究生，从事数据库研究。Vol.60 No.6Jun.15,2023收稿日期：2 0 2 0-12-0 4；修回日期：2 0 2 2-12-2 8（任美辉编发）一31一