换流站巡检四足机器人建图定位系统优化

王旋, 侯贺伟, 富银芳, 胡雪亮, 张时, 张虞旭驹, 周燕鑫, 姚恒燕

(1.国网浙江省电力有限公司超高压分公司, 杭州 311100; 2.杭州云深处科技有限公司, 杭州 310030)

由于人工智能技术研究的突破,智能巡检机器人在一般变电站环境中已经得到较为广泛的应用[1-2]。这些应用加快了变电站智能化无人值守进程,解放了人力资源[3]。然而,对于超高压换流站场景,存在环境复杂恶劣、设备类型多、数量大等特点,需要跨越楼梯以及狭窄区域场景,巡检的设备复杂烦琐、检查点密集。传统人工巡检模式巡检效率低、运维成本高,无法达到巡检工作基本要求[4]。常规的巡检机器人一方面受限于运动方式,另一方面在多楼层楼梯场景以及狭窄区域场景建图定位较为困难,很难部署在换流站或复杂地形环境中[5]。因此研究一种智能机器人在换流站复杂环境中的建图定位技术对于换流站无人巡检发展具有重要意义。

机器人在换流站中所处的环境往往是未知、难以预测的,要实现机器人在换流站中的自主导航巡检,关键是要实现机器人在换流站中精确的建图定位[6]。其中建图的常用方式是采用实时定位与地图构建(simultaneous localization and mapping,SLAM)技术,SLAM技术能够实现移动机器人在未知环境中的自我运动估计,同时建立周围环境地图[7-8]。现有的SLAM算法依据主导传感器大致可以分为视觉SLAM与激光SLAM两大类,激光SLAM建图直观、精度高、不受光照变化影响、不存在尺度漂移现象,同时无需预先布置场景,可融合多传感器生成便于导航的环境地图[9-10]。以激光SLAM技术构建变电站点云图已有一些研究进展,其中,尹政等[11]分析对比了目前几种主流激光SLAM算法,以Cartographer SLAM作为研究基础,通过高精度校准方法标定两台2D激光仪的位姿关系,并对两组数据进行优化融合以获得高精度的三维数据,从而实现达到一定建图精度的新系统。鲁锦涛等[12]使用改进的点云匹配算法解决了变电站中相似环境误匹配的问题,提高了地图的可靠性以及可视性。然而上述研究中都没有涉及换流站中多楼层环境的点云图建立以及完全解决狭窄通道相似误匹配问题。在定位方面,巡检机器人定位方式大多采用有轨方式或全球定位系统[13],有轨方式成本较高,全球定位系统定位方式不能在室内以及有遮挡的区域进行定位,因此需要一种更为有效的定位方式,王鹏程等[14]通过Hough变换算法和最小二乘法算法将雷达点云数据拟合成直线,建立环境地图模型,应用扩展卡尔曼滤波算法结合点云数据和位姿数据进行实时定位和预测走向,但单独依靠激光点云匹配得到位姿不够鲁棒,需要融合其他传感器来提高定位的稳定性。

现通过分析换流站巡检环境的特点,融合激光雷达、深度相机、惯性测量单元(inertial measurement unit, IMU)以及足式里程计重点解决四足机器人在换流站中多层楼梯以及狭窄通道场景的建图定位问题。对激光雷达和深度相机进行优势互补,融合激光雷达2～85 m范围内和深度相机2 m范围内较高精度点云,提取边缘特征、地面面特征以及非地面面特征,改进基于特征的点云匹配算法,使用紧耦合的迭代卡尔曼滤波器(iterated extended Kalman filter, IEKF)来融合点云特征点和IMU观测,解决楼梯间相似场景误匹配以及狭窄通道特征匹配失效的问题,优化建图算法回环检测部分,解决大场景建图回环问题。通过UKF融合足式里程计与融合点云和三维环境地图NDT匹配位姿得到四足机器人高频稳定的全局位姿。

1 四足机器人模型及巡检系统

采用四足机器人作为巡检载体,它能够跨越楼梯以及穿越换流站狭窄通道区域,其模型图如图1所示,四足机器人上搭配3个RealSense D435i相机、1个16线三维激光雷达、1个高精度IMU传感器和12个电机驱动器。

图1 四足机器人模型

四足机器人的巡检系统框架如图2所示,传感器数据包括3个深度相机数据、1个16线激光雷达点云数据、IMU数据以及足式里程计数据,足式里程计数据由运动控制端融合IMU以及编码器数据得到。同步单元将各个传感器数据进行时间同步,预处理之后再输入建图以及定位模块中。可以看出,建图和定位模块是四足机器人巡检系统中非常重要的两个模块。

图2 四足机器人巡检框架

2 传感器标定及数据预处理

激光雷达、相机以及IMU之间外参和时间戳的标定是四足机器人实施建图与定位模块的首要条件。采用3D棋盘格对激光雷达和相机的外参进行标定[15],设相机到激光雷达的外参表达式为

PL=TPC

(1)

式(1)中:PL为激光雷达L坐标齐次坐标转换表示;T为相机到激光雷达的坐标变换矩阵;PC为相机C的坐标。可以得出3个相机到激光雷达的外参矩阵分别为TLC1、TLC2、TLC3,其中,TLC1为四足机器人左前相机到激光雷达的坐标变换矩阵,TLC2为四足机器人右前相机到激光雷达的坐标变换矩阵,TLC3为四足机器人后面相机到激光雷达的坐标变换矩阵。

标定完激光雷达与相机的外参后,还需要标定激光雷达与IMU的外参。采用基于轨迹重合的标定方法对激光雷达与IMU进行标定,对激光雷达在一段时间内进行较高精度的点云匹配,得到该时间段内高精度里程计信息,利用惯导预积分的方式获取同一段时间内位姿约束信息。其原理如图3所示。

L0为激光雷达初始坐标系;Lidar(t)和Lidar(t+1)分别为激光雷达在t时刻和t+1时刻在L0中的位置;B0为IMU初始坐标系;IMU(t)和IMU(t+1)分别为IMU在t时刻和t+1时刻在B0中的位置;C为两个传感器所形成的轨迹;I、J、K为直角坐标系基向量

图3中虚线条分别表示激光雷达和IMU通过点云匹配和预积分得到的里程计信息,对两条轨迹进行重合的思想优化求解IMU到激光雷达的变换矩阵,首先构建优化方程为

(2)

由于激光雷达、深度相机与IMU获取数据的频率不一样,深度相机与激光雷达采集数据的频率为10 Hz,而IMU采集数据的频率为200 Hz,因此还需要进行激光雷达、深度相机与IMU的时间同步。首先通过时间同步单元对激光雷达、深度相机与IMU的时间戳基准进行同步,然后获取激光雷达当前帧前后时刻的IMU数据,通过插值的方法计算出一个激光雷达当前帧时刻的等效值,后续每一帧激光雷达数据都以此方法得到IMU同一时刻的等效信息,从而实现激光雷达、深度相机与IMU的时间软同步。

3 多传感器融合SLAM建图及实现

通过融合激光雷达、深度相机以及IMU传感器构建四足机器人的SLAM建图框架,如图4所示,整个框架主要包括传感器数据接收及预处理、特征提取[16]、点云里程计、回环检测及全局位姿优化[17]和建图模块。其中传感器数据接收及预处理模块分为深度相机与激光雷达的数据获取、激光点云去畸变以及激光点云与相机点云融合部分,激光里程计模块分为融合点云匹配、位姿求解以及IEKF[18]数据融合部分。

图4 四足机器人多传感器融合SLAM建图框架

多传感器融合框架具体步骤如下。

3.1 传感器数据接收及预处理

图5 激光雷达与深度相机点云融合

3.2 特征提取

从融合点云Pk中提取边缘特征和面特征,其中面特征又包括地面面特征和非地面面特征。通过评估局部区域内点的平滑度来提取边缘特征和面特征,计算平滑度的公式如式(3)所示。设i是Pk中的一个点,i∈Pk,设S是同一次扫描中激光扫描仪返回的i的连续点的集合,则有

(3)

为解决狭窄区域以及楼梯场景相似环境下点云建图回环不上的问题,构建BoW词袋向量用于后续回环检测。首先在3个深度相机获取的RGB图像中提取加速分段试验(features from accelerated segment test,FAST)特征点,并计算特征点的ORB描述符,最终根据ORB描述符计算该帧图像的BoW向量。为了减少计算量和需维护的内存大小,仅计算关键帧的BoW向量用于建立关键帧数据库。非关键帧仅提取特征点及计算ORB描述符,用于计算当前追踪的特征点数量。

3.3 点云里程计

3.4 回环检测及全局位姿优化

为降低回环优化时的计算成本以及系统维护的内存,仅对关键帧进行回环检测。以成功匹配的特征点数量小于某一阈值或当前帧生成的新特征点数量大于某一阈值或当前帧与上一关键帧之间帧数大于某一阈值为条件构建关键帧数据库。在关键帧数据库中通过离线训练的BoW词典搜索候选回环匹配帧,通过时间和几何一致性检查找到最佳匹配。检测到回环后,启动全局位姿图对所有关键帧位姿进行优化。全局位姿图如图6所示。

L1为激光雷达t1时刻的点云里程计数据,以此类推,Lk为tk时刻的点云里程计数据

3.5 建图

多传感器融合SLAM建图方法融合了三个深度相机的点云来补足激光雷达点云,对融合点云进行地面分割,并提取三类特征点,通过提取RGB图像特征点的ORB描述符计算关键帧图像的BoW向量用于回环检测以及全局的位姿优化。

4 多传感器融合定位及实现

通过多传感器融合SLAM建图方法建立换流站的全局地图之后,还需要实时获取四足机器人在换流站中的精准位姿。与实时建图中的定位不同的是,四足机器人在定位导航过程中点云帧间的运动有可能非常剧烈,如在上下楼梯或快速奔跑执行巡检任务时,扫描特征匹配可能会由于大的位移对于估计四足机器人的运动位姿有延迟,导致错误判断四足机器人在点云图中的位姿。因此采用UKF[21]将NDT扫描匹配与运动控制端提供的足式里程计进行融合,以足式里程计预测位姿作为NDT匹配的初始值,加快NDT匹配时间,并采用足式里程计进行有效插值输出高频的四足机器人位姿。四足机器人多传感器融合的定位框架如图7所示。

图7 多传感器融合定位框架

四足机器人的多传感器融合定位框架主要分为NDT点云匹配得到运动位姿以及UKF数据融合。

首先定义待估计的四足机器人状态为

(4)

(5)

式(5)中:Δpt为四足机器人在t-1到t时间段内补偿里程计位置偏置的位移;Δqt为四足机器人在t-1到t时间段内补偿里程计方向偏置的旋转。

式(5)中利用UKF预测的机器人位姿作为NDT匹配的初始值,再执行NDT将观测到的点云与全局地图进行匹配,最后通过得到的NDT扫描匹配之后的机器人位姿对机器人状态进行校正。定位观测方程为

zt=[pt,qt]T

(6)

在每帧足式里程计数据传入之后都对四足机器人位姿做一次预测更新,有了足式里程计的预测作为NDT匹配的初值,可以大大减短NDT点云匹配的时间,降低算法计算资源的消耗,以点云的匹配位姿作为估计真值对四足机器人的位姿进行校正,最终通过多传感器融合定位框架可以理论实现四足机器人在换流站环境中200 Hz高频稳定的实时位姿输出。

5 实验与分析

为验证四足机器人“绝影X20”在换流站中实际建图定位效果,以图8所示的四足机器人平台开展了5个实验。对四足机器人建图算法在换流站中整体的建图能力包括狭窄通道场景进行测试,与LIO-SAM算法[22]进行对比,验证所提出的建图算法整体建图能力;对四足机器人在多楼层场景进行建图测试;对四足机器人定位算法以足式里程计为预测、IMU预测以及无预测时位移以及旋转的偏差进行测试;对四足机器人在以建图定位算法基础上的上下楼梯成功率进行测试;对四足机器人在换流站场景的重复定位精度进行测试。

图8 四足机器人

首先测试四足机器人建图算法在换流站场景的整体建图能力,分别控制四足机器人运行所提出的SLAM方法与LIO-SAM算法走完整个换流站,所述SLAM算法所建点云图如图9所示,可以看出,点云图没有出现重影以及漂移情况。图10为LIO-SAM建立的换流站点云图,可以看到四足机器人在进入一个狭窄通道的时候,点云出现了图10左边重影的情况。

图9 SLAM算法所建点云图

图10 LIO-SAM算法所建点云图

建图算法融合了3个相机点云并且结合地面检测提取了地面特征点以及非地面特征点进行匹配,对于抑制建图Z轴方向的漂移具有重要作用,观测四足机器人在换流站中的建图轨迹与LIO-SAM算法的建图轨迹进行对比,提出的建图算法的轨迹在Z轴方向几乎没有漂移,而LIO-SAM算法具有2.132 m的漂移。因此提出的建图算法对于换流站环境整体点云地图建立更为准确,尤其是具有狭窄通道的场景。

验证四足机器人在换流站中跨越多楼层室内楼梯场景的建图能力,应用提出的建图算法进行测试,如图11所示,可以较好地建立楼梯间的点云图,和只采用激光雷达与IMU传感器的LIO-SAM算法进行对比,LIO-SAM算法在楼梯间点云匹配失效,出现了点云位姿漂移的现象,如图12所示。

图11 多楼层楼梯点云图

图12 LIO-SAM建立楼梯间点云图

为了评估足式里程计对于四足机器人定位姿态预测的效果,假设NDT估计的姿态大部分是正确的,将足式里程计预测的四足机器人姿态与LNDT估计的姿态进行比较,计算出足式里程计预测的四足机器人姿态错误量,将IMU预测的姿态与NDT估计的姿态进行比较,计算出IMU预测的姿态错误量,在没有位姿预测的情况下,将先前的匹配结果作为初始猜测计算错误量。分别测试四足机器人在上楼梯的过程以及快速奔跑的过程中足式里程计对于定位姿态预测的效果,分别如图13和图14所示,可以看出,不管是四足机器人在上楼梯还是以3 m/s快速奔跑的情况下,足式里程计预测的姿态都更为精准。当帧间位移变大,在没有位姿预测或预测误差较差的情况下,NDT匹配需要更长的时间,大约超过56 ms,因为帧间的大位移使NDT需要更多的迭代才能收敛到局部最优解。通过足式里程计预测,由于良好的初始猜测,每帧匹配时间约为42 ms。结果表明,基于足式里程计的姿态预测使四足机器人姿态估计对上下楼梯剧烈波动以及快速运动具有鲁棒性和快速收敛性。

图13 上楼梯情景下预测姿态错误量

图14 快速奔跑下预测姿态错误量

四足机器人稳定精准地上下楼梯是换流站巡检中的一个难点,需要定位框架提供精准的四足机器人位姿估计,构建换流站中楼梯的地形图,给四足机器人提供可踩区域和不可踩区域信息,使四足机器人可以提前设置好落脚点。在绍兴一个换流站中选取一处6层阶梯使用手柄遥控四足机器人和使用实时位姿构建地形图辅助四足机器人进行上下楼梯成功率的对比试验,每层阶梯高为18 cm,宽为21 cm,长为140 cm,如图15所示,分别以手柄遥控的方式和以地形图辅助的方式,统计100次上下楼梯的成功率,其中手柄遥控以及地形图辅助上楼梯时机器狗前进的速度均为0.5 m/s。分析数据得出,以手柄遥控方式的平均成功率为46.4%,以实时位姿构建地形图辅助四足机器人上下楼梯方式的成功率为98%。后一种方式比前一种方式上下楼梯的成功率提高了51.6%。因此,所述的定位方法对于提高四足机器人上下楼梯成功率具有重要作用。

图15 四足机器人爬楼梯

四足机器人在换流站中的重复定位精度是实现无人自主巡检的重要保障,只有当重复定位精度较高才能保证装载在四足机器人上的云台能够准确识别各个表计的读数以及一些异常点。通过标定换流站中某一点的坐标,使四足机器人重复到达这一点,重复12次,计算每一次到达坐标与标定坐标的直线误差绝对值,从而验证出四足机器人建图定位算法的重复定位精度,图16是截取的其中一次实验,可以看出四足机器人在换流站中的重复定位精度最大误差小于6 cm。

图16 重复定位精度测试

经过反复5轮不同点的测试,四足机器人的重复定位误差均小于6 cm,可以满足换流站的巡检需求。

6 结论

对换流站中四足机器人的建图定位系统进行了细致的优化。通过融合深度相机点云作为点云输入,弥补以飞行时间为原理的三维激光雷达在2 m范围内点云具有较低精度的缺陷,对融合点云进行地面分割,提取边缘特征、非地面面特征以及地面面特征,优化基于特征的点云匹配算法,使用紧耦合的IEKF滤波器来融合点云特征点和惯导观测,使四足机器人在狭窄环境以及楼梯通道环境具有较佳的建图效果,优化回环检测使建图算法对于换流站具有整体精准的三维建图能力。通过UKF滤波器融合足式里程计预测四足机器人姿态作为NDT匹配初值,加快NDT点云匹配时间,通过实时匹配融合点云与三维环境地图得到全局位姿进行姿态更新,使四足机器人在换流站中具备稳定精准的重复定位精度。最终,经过换流站带有狭窄通道的整体建图以及多楼层建图与LIO-SAM算法进行对比,验证了提出的建图算法对于狭窄通道场景以及多楼层场景建图的有效性,通过里程计预测对比实验、四足机器人上下楼梯实验以及重复定位精度实验验证了所提出的定位算法可以缩短NDT匹配的时间至42 ms以及达到6 cm以内的重复定位精度。