基于里程计的巡检机器人定位恢复方法

林 欢

(亿嘉和科技股份有限公司，江苏 南京 210012)

传统变电站的巡检由专门巡检人员完成，但由于主观因素的存在，会导致对检测标准的理解发生偏差，进而影响到检测质量。近年来，智能巡检机器人实现了对室外变电站全时段、全方位、全自主的巡检，机器人通过对采集到的图像进行视觉分析，能及时地发现变电站设备存在的隐患，从而为智能变电站提供创新性的技术检测手段和全方位的安全保障[1]。定位导航是巡检机器人最底层的技术，同时也是核心、关键[2]。在已知精确初始定位的前提下，确定巡检机器人在移动过程中的实时位置并由此规划运动轨迹，已经成为当前自主式移动机器人研究领域的关键问题。巡检机器人的定位不丢失，能够顺利导航并且返回充电，是室外机器人持续稳定运行的保障。

智能巡检机器人采用激光定位扫描的方式辅助机器人进行行走控制。但是在实际运行中，激光定位方式会受到变电站真实环境变化等因素的影响，造成机器人稳定性的降低。同时，当路段环境较为相似、便于机器人记忆的参照物较少时，机器人容易出现迷路的情况，并且有一定几率丢失定位，进而造成机器人无法行走，直至电量耗尽，很大程度上影响了机器人运行的续航能力及稳定性，并增加了用户的运营维护成本，因此需要对巡检机器人进行定位恢复。

目前大部分研究都是关注对实时定位算法的改进，但由于系统误差的存在，定位丢失不可避免，因此需要提出新的定位恢复方法。王炜[1]通过建立粒子滤波观测模型，并与实际环境模型比较，用修正粒子滤波的方法实现了定位恢复，但是该方法所需模型复杂，计算量大。薛瑞范[2]采用“双编码器+陀螺仪”实现了自身航迹推算的定位恢复功能，但成本较高，所采用的卡尔曼滤波组合较为复杂。伊星星[3]基于外观的位置辨识算法，通过词袋模型实现了系统的回环检测，从而纠正增量式系统的漂移误差，但是同样存在模型复杂，不利于实际使用的问题。针对上述定位恢复模型复杂、计算量大、无法适应动态环境变化等问题，本文提出了基于里程计的巡检机器人定位恢复方法，主要依靠记录机器人行走距离的里程计数据，同时为了控制机器人以平稳的速度回退到里程计中记录的位置点，采用了周期纠正流程。该方法对提高定位精度、保障机器人的续航能力有非常重要的意义。

1 巡检机器人里程计模型

里程计模型采用光电编码器来获得当前时刻巡检机器人在地图坐标系下的位姿[4-6]。通常情况下，光电编码器安装在机器人的两个差速主动轮上，随车轮同步转动。

通过光电编码器的检测，可以得到主动轮在Δt时间内转过的弧度[7]，从而计算出机器人车轮移动的距离Δd。

Δd=2×(N/p)×πr

(1)

式中：N为Δt时间内光电编码器输出的脉冲数；p为光电编码器分辨率；r为车轮半径。

根据式(1)，可以计算出机器人左右两个主动轮的移动距离，分别用ΔdL和ΔdR表示。那么机器人从当前位姿Pk(xk,yk,θk)移动到下一位姿Pk+1(xk+1,yk+1,θk+1)的实际移动距离ΔD和转过的角度Δθ都可用ΔdL和ΔdR表示。

(2)

(3)

式中：a为两主动轮之间的距离。

本文实际采用四轮驱动的机器人，为了简化模型，只对安装有光电编码器的两个主动轮进行计算分析。考虑到在变电站内巡检机器人直线行走和转弯的两种运动方式，建立如下两种模型。

1.1 直线模型

在行进过程中，若光电编码器得到的左右轮移动距离ΔdL和ΔdR近似相等，则可以认为巡检机器人当前以直线方式前进，如图1所示。

图1 直线模型

根据图1几何关系，在时间Δt内可以计算出巡检机器人在地图坐标系下由Pk(xk,yk,θk)移动到Pk+1(xk+1,yk+1,θk+1)的增量，由此可得到直线模型，如式(4)所示。

(4)

式中：ΔDk和Δθk分别为机器人在k时刻的实际移动距离和转过的角度增量。

1.2 圆弧模型

在行进过程中，若光电编码器检测得到左右轮移动距离ΔdL和ΔdR相差较大，则可以认为巡检机器人当前处于转弯阶段，如图2所示。

图2 圆弧模型

根据图2几何关系，在时间Δt内可以计算出巡检机器人在圆弧运动过程中的终点位姿Pk+1(xk+1,yk+1,θk+1)，由此可得到圆弧模型，如式(5)所示。

(5)

将根据直线和圆弧模型计算得到的机器人位姿Pk+1(xk+1,yk+1,θk+1)作为里程计信息保存，这个位姿独立于激光定位所得到的位姿，它是通过光电编码器的行走轨迹来确定的。但是因为里程计存在较大的累积误差，所以在实时定位中仍以激光定位为主，里程计仅作为定位恢复时位姿估计的辅助手段[8]。

2 定位恢复流程

2.1 正常导航中的历史数据维护

巡检机器人在运动过程中可以实时判断并反馈它的累积运动距离或者转向角度，一旦达到所设置的距离和角度，会自动为其添加一个关键点。将第一次收到的里程计信息作为关键点的第一帧数据，即作为关键点0并存入关键点队列中。

机器人每行走0.3s，就记录当前的位置信息并存入里程计队列中。在行走的0.3s内，机器人将当前里程计信息与关键点队列最后一帧数据进行几何求值，即每行走固定距离0.3m或者转向动作达到固定角度10°时，就要将里程计信息存入关键点队列数据中，并记录新的关键点。

为保证关键点队列数据的有限长度，机器人每行走0.3s，就要将关键点队列中与当前里程计信息距离大于设定的最大距离1.0m或者转向角度大于60°的数据清除出关键点队列，防止因为累积距离过大而对编码器的精确度造成影响。

2.2 定位丢失后的恢复流程

定位丢失是指激光传感器在进行定位时，因环境过于复杂(如重复性高、道路颠簸、过于空旷等)致使机器人定位算法失效，出现置信度过低的现象[9]。一旦置信度小于阈值0.4时，就会发出定位丢失信号。根据变电站实际环境，如果连续3s定位置信度小于阈值，即可确定当前情况为定位丢失。机器人定位丢失后，应立即暂停当前任务流程，并触发定位恢复流程，流程如图3所示。定位恢复是指机器人在定位丢失的情况下，借助外部传感器帮助机器人恢复定位算法的置信度，使机器人重新进入定位流程并继续运动。

图3 定位恢复流程

step1，首先判断记录机器人行走的里程计队列中是否有存储数据(即判断有无回退数据)。若无可恢复数据，定位失败；若有可恢复数据，则开始定位恢复。

step2，开启回退计时器，默认时间为1s。计时未超过1s，将重复运行step3，执行里程计控制机器人回溯动作。一旦超过1s，则执行step4，尝试定位恢复操作。

step3，获取机器人在运行过程中的当前位置，同时读取关键点队列中对应的数据。利用周期纠正流程对机器人进行速度平滑控制，直到机器人由当前位置运动到达队尾对应的关键点位置，随后将队尾数据移出关键点队列。为了不影响之后的重新定位，机器人回退到的实际位置与里程计中队尾关键点位置之间的误差应控制在5cm之内。

step4，根据里程计累积运动距离进行回退时移动距离估计，以此为移动量预测当前位置，并重新定位。

step5，若定位未恢复，则重复执行上述步骤；若定位恢复，机器人将重新进行路径规划，避开定位丢失路段继续运行。

2.2.1周期纠正流程

为解决机器人行走或旋转过程中，由于机械问题或者路面不平造成的行走误差，从而造成定位恢复失败的问题[10]，本文在step3中引入周期纠正流程，即在行走、旋转过程中，及时纠正机器人本体误差，提高定位恢复的几率，如图4所示。图中虚线代表机器人行走出现偏差，粗实线代表周期纠正流程的行走纠正。

图4 周期纠正示意图

在运动过程中要对里程计信息进行保护，避免行走误差影响到里程计队列中存储的位置点信息。机器人在每一帧的运动过程中，都会进行目标点与当前位置的比较，并利用速度平滑流程重新规划行走速度，进行行走过程中位置偏差的纠正[11]，具体流程如图5所示。

图5 周期纠正流程图

2.2.2速度平滑流程

巡检机器人在运行过程中，会由于惯性持续行走一段距离或旋转一定角度，导致实际停止位置与目标点有所偏差[12]。为解决此类问题，引入速度平滑流程，使机器人在行走或旋转过程中，随着与目标点距离的减小，逐步降低行走或转向的速度和加速度，减少由于惯性造成的误差，提高定位恢复的几率。

速度平滑流程主要是保证机器人在靠近目标时以较低的加速度和速度进行停靠，从而保证机器人控制的平滑性和稳定性[13]。假设机器人离目标距离为d，速度为v，则设计函数形式为

v=m×dn

(6)

式中：m,n是函数待确定的参数，可以根据在实际测试过程中机器人所能运动的速度上下限和运动距离来定义。

机器人在距离目标点0.05m时应采用最小速度匀速行走，最小速度为0.02m/s；机器人在距离目标点2.90m时应采用最大速度匀速行走，最大速度为0.80m/s。而在距离目标点0.05m到2.92m之间应实现速度平滑过渡。将以上参数代入式(6)，可得到m，n分别为0.647和0.4。函数曲线图如图6所示。

图6 速度平滑曲线图

3 实验结果

实验采用亿嘉和科技股份有限公司研制的变电站智能巡检机器人E100。目前E100已经投入实际使用，为智能变电站运维系统提供环境和工业设备状态的监测数据。实验模拟变电站实际的稀疏环境，实验环境的局部示意图如图7所示。

图7 变电站稀疏环境示意图

为了验证本文所提定位恢复方法的优越性和可靠性，将本文方法与经典定位方法(只使用激光传感器)进行比较。实验记录回退到可重新定位位置点的实际坐标以及回退时间，具体实验数据见表1、表2。在实际检测情况中，巡检机器人会根据置信度自行判断当前是否处于定位丢失状态。由于定位丢失为偶发情况，需要在实验中设置定位丢失点。在示意图中标记3处位置点(如图7所示)，一旦巡检机器人到达3个位置点中任意一个，机器人就会模拟定位丢失情况，从而触发定位恢复流程。

表1 仅使用激光传感器的实验结果

表2 基于里程计的实验结果

从表中可以看出，基于激光传感器的巡检机器人的定位恢复过程与基于里程计相比，所需要的时间更长，而且会出现定位无法恢复的情况。基于里程计的定位恢复除了恢复时间短，可靠性高之外，其实际回退到的可重新定位的位置点与里程计中保存的理论位置点之间的距离小于5cm，提高了后续重新定位的精度。

根据实验可以看出，基于里程计的定位恢复具有较好的位姿估计，其定位恢复准确，并且可靠性高，能够抵抗环境干扰，具有较高的鲁棒性。

4 结束语

本文提出的基于里程计的定位恢复方法，可以在偶发的定位丢失情况下，让巡检机器人逐步回退到里程计中保存的位置点，直到定位恢复并能够重新规划路径继续运行。实验结果表明，该方法可以有效地解决巡检机器人在变电站稀疏环境下迷路以及定位丢失的问题，并且在回退过程中利用周期纠正流程进行速度平滑控制，有效地提高了定位恢复的几率，保障了机器人的正常运行，对巡检机器人的实际应用具有重要的意义。目前基于里程计的巡检机器人已经在多个变电站实际使用，机器人定位恢复并持续运行(继续任务或者返回充电)的成功率达到96%以上，基本上避免了运维人员出工对电量耗尽的巡检机器人进行返回运输的情况。