一种基于里程计的机器人定位恢复方法与流程

本发明属于智能机器人技术领域，具体涉及一种基于里程计的机器人定位恢复方法。

背景技术：

变电站智能巡检机器人是一种室外轮式移动机器人，可携带可见光、红外等检测设备对室外变电站内的设备进行全时段的自主巡检。如何精确地确定系统的位置移动并由此规划机器人的运动轨迹，是自主式移动机器人研究领域非常关注的问题。自主导航是机器人进行自主巡检的关键步骤，机器人的准确定位是实现自主导航的前提，而机器人的定位不丢失，顺利返回充电，则是室外机器人持续稳定运行的保障。

定位导航作为最底层技术，是构建整个机器人的核心关键。室外智能巡检机器人采用激光定位的方式，辅助机器人进行行走控制。在室外智能巡检机器人实际运行中，激光定位方式即采用激光传感器采集的激光点云数据来作为定位算法的输入的定位方式，受真实环境变化及颠簸等因素，稳定性受影响很大。同时，当出现路段环境较为相似，可便于机器人记忆的参照物较少，不利于机器人的定位，也会出现机器人迷路的情况。在运维过程中机器人在定位过程中有一定机率丢失定位，进而造成机器人无法行走，直至电量耗尽，这极大程度上影响机器人运行的续航能力及稳定性，并增加了用户的运维成本。

技术实现要素：

本发明目的是：针对现有技术的不足，提供一种基于里程计的机器人定位恢复方法。该方法在确定机器人丢失定位的情况下，能够进行定位恢复，解决机器人行走过程中定位丢失的问题。

本发明提供的基于里程计的机器人定位恢复方法包括以下步骤：

正常导航过程中维护里程计信息和关键点信息；

判断机器人发生定位丢失后，进行定位恢复。

进一步而言，所述定位恢复包括以下步骤：

判断里程计队列和关键点队列中有存储数据时，控制机器人行走到上一关键点位置，根据当前位置重新设置机器人定位；直至定位恢复。

进一步而言，控制机器人行走到上一关键点位置时，使用周期纠正流程，即通过对机器人运动过程中每一帧数据进行目标点与当前位置的对比，重新进行速度规划，对行走过程中位置偏差进行纠正。

进一步而言，所述速度规划使用速度平滑流程，即根据机器人离目标点的距离来规划速度和加速度。

进一步而言，所述速度平滑流程，遵循以下原则：

(1)机器人离目标点越近时，速度越低；反之，速度越高；

(2)机器人离目标点越近时，加速度越低；反之，加速度越高。

进一步而言，所述速度平滑流程中速度v的函数为：

v＝a*d^b

其中a、b是函数待确定的参数，可以根据每个机器人所能运动的速度上下限和通常的运动距离来定义函数中的v和d，将v和d代入公式获取参数a、b。

进一步而言，所述关键点信息包括里程计信息中第一帧数据、机器人行走固定距离或转向固定角度后的里程计信息。

进一步而言，所述定位丢失是指定位置信度小于阈值。

进一步而言，所述定位恢复的具体步骤如下：

1)判断记录机器人行走数据的里程计队列中是否有存储数据，如果有存储数据，则进入步骤2)；否则，定位恢复失败；

2)判断关键点队列是否有历史关键点，如果有历史关键点，进入步骤3)；否则，定位恢复失败；

3)开启回退定时器；

4)根据里程计数据控制机器人行走到上一关键点位置：对于运行中的每一帧，获取机器人当前位置，同时读取关键点队列中队尾的关键点数据，利用周期纠正流程对机器人进行速度控制，直到到达关键点队列中队尾关键点对应的位置，随后关键点队列将队尾的关键点数据移出队列，进入步骤5)；

5)若回退定时器的计时到期，则进入步骤6)；否则，进入步骤4)；

6)根据当前位置重新设置机器人定位：根据里程计累积的运动距离估计倒退时移动的距离，以此为移动量预测当前位置，重新设置机器人定位；

7)判断定位是否恢复，如未恢复，则进入步骤2)；如已恢复，则定位恢复结束。

进一步而言，所述正常导航过程中维护里程计信息和关键点信息，包括以下步骤：

机器人行走时，将第一次收到的里程计信息作为关键点第一帧数据存入关键点队列中；

机器人每行走固定时间间隔，记录下机器人当前里程计信息，存入里程计队列中；

机器人在行走固定时间间隔内，将当前里程计信息与关键点队列最后一帧数据进行几何求值，将每行走固定距离后的里程计信息同时存入关键点队列中，并记录新关键点；或者机器人在转向流程中有转向动作达到固定角度时，则在转向结束时记录当前里程计信息，存入关键点队列中，并记录新关键点。

机器人每行走固定时间间隔内，将关键点队列中与当前里程信息距离大于最大距离或者转向角度大于最大转向角度的数据，清除出关键点队列。

本发明的有益效果如下：本发明的基于里程计的机器人定位恢复方法，可以实现在偶发的定位丢失情况下，机器人定位恢复并持续运行(继续任务或者返回充电)的成功率达到96％以上，基本上避免了需要运维人员出工对电量耗尽的机器人进行返回运输的额外运维成本的情况。

附图说明

图1是本发明实施例的定位恢复流程图。

图2是本发明实施例的周期纠正流程的行走路线示意图。

图3是本发明实施例的周期纠正流程图。

图4是本发明实施例的平滑速度曲线图。

具体实施方式

下面结合实施例并参照附图对本发明作进一步详细描述。

实施例1：

本发明的一个实施例，介绍了一种基于里程计的机器人定位恢复方法。

与本实施例相关的技术术语的定义如下：

定位丢失，是指在使用传感器(如激光、深度摄像机或者高清相机)进行定位时，因环境过于复杂(比如重复性高，道路颠簸，过于空旷等)致使机器人定位算法失效，出现置信度(对当前位置进行判断的算法的可信程度)过低的现象，这种情况认为机器人已经迷路，无法继续运动。

定位恢复，是指在机器人定位丢失的情况下，采用手段(比如借助外部传感器)来帮助机器人恢复定位算法的置信度，重新进入定位流程继续运动。

本实施例包括两部分：正常导航过程中的历史数据维护流程；机器人发生定位丢失，触发定位恢复流程。

正常导航过程中的历史数据(即里程计信息和关键点信息)维护流程，包括：

(1)机器人行走时，将第一次收到的里程计信息作为关键点第一帧数据即关键点0，存入关键点队列中。

(2)机器人每行走固定时间间隔(固定时间间隔默认为interval＝0.3s)，记录下机器人当前里程计信息，存入里程计队列中。

(3)机器人在行走interval时间内，将当前里程计信息与关键点队列最后一帧数据进行几何求值，即每行走固定距离后(固定距离默认为key_dis＝0.3m)，将里程计信息同时存入关键点队列数据中，并记录新关键点；或者机器人在转向流程中有转向动作达到固定角度(固定角度默认为key_angle＝10°)，则在转向结束时记录当前里程计信息，存入关键点队列中，并记录新关键点。

(4)为保证关键点队列数据有限长度，机器人每行走interval时间内，将关键点队列中与当前里程信息距离大于最大距离(最大距离默认为max_dis＝1m)，或者转向角度大于最大转向角度(最大转向角度默认为max_angle＝60°)的数据，清除出关键点队列。

(5)按照以上步骤，在导航过程中依次存入关键点1，2，3，……，数据的信息。

当机器人发生定位丢失后，触发定位恢复流程：

(1)机器人发出定位丢失信号后，经一段时间确认定位丢失，立即暂停当前任务流程，并触发定位恢复流程；

定位丢失信号，是指置信度小于阈值经验值(通常选取0.4)，此时认定机器人定位丢失。

从机器人发出定位丢失信号至确认定位丢失的时间，一般是根据机器人和现场环境而定，在本实施例中取值3秒，如果连续3秒定位置信度小于阈值，即确定定位丢失。

(2)执行定位恢复流程。

(3)恢复机器人导航流程，重新进行路径规划，避开定位丢失路段继续执行原有任务。

参照图1，定位恢复流程，包括：

(1)判断记录机器人行走数据的里程计队列中是否有存储数据(可用于定位恢复的数据)。如果有存储数据，则进入步骤(2)；否则，定位恢复失败。

(2)判断关键点队列是否有历史关键点。如果有历史关键点，进入步骤(3)；否则，定位恢复失败。

(3)开启回退定时器(默认时间为1秒)。

(4)根据里程计数据控制机器人行走到上一关键点位置：对于运行中的每一帧，获取机器人当前位置，同时读取关键点队列中队尾的关键点数据，利用周期纠正流程(参见周期纠正流程)对机器人进行速度控制(参见速度平滑流程)，直到到达关键点队列中队尾关键点对应的位置，随后关键点队列将队尾的关键点数据移出队列，进入步骤(5)。

(5)若回退定时器的计时到期，即回退定时器计时大于等于1秒，则进入步骤(6)；否则，进入步骤(4)。

(6)根据当前位置重新设置机器人定位：根据里程计累积的运动距离估计倒退时移动的距离，以此为移动量预测当前位置，重新设置机器人定位。

(7)判断定位是否恢复(即定位算法置信度大于阈值)。定位未恢复，则进入步骤(2)；定位恢复，则进入步骤(8)。

(8)定位恢复结束。

周期纠正流程：

为解决机器人行走与旋转过程中，由于机械问题或者路面不平造成行走误差，从而使定位恢复失败，为解决此类问题，引入周期纠正流程，即在行走、旋转过程中，及时纠正机器人本体误差，提高定位恢复的机率。

参照图2和图3，在机器人运动过程中对于里程计信息进行保存和维护，通过对机器人运动过程中每一帧数据进行目标点与当前位置的对比，重新进行速度规划，对行走过程中位置偏差进行纠正。

速度平滑流程：

机器人在运行到停止过程中，惯性会使机器人继续行走一定的距离或旋转一定的角度，从而其停止的位置与目标点有所偏差。为解决此类问题，引入速度平滑流程。使机器人在行走及旋转过程中，随着与目标点间距离的减少，逐步降低行走及转向速度，减少由于惯性造成的误差，提高定位恢复的机率。

速度平滑是根据机器人离目标点的距离来规划速度，其中遵循以下两点原则：

(1)机器人离目标点越近时，速度越低；反之，速度越高。

(2)机器人离目标点越近时，加速度越低；反之，加速度越高。

以上两点原则主要是为了保证机器人在靠近目标时以较低的加速度和速度进行停靠，且速度接近最高速度时不至于突兀，从而保证机器人运动的平滑性和稳定性。根据以上两点原则，假设机器人离目标距离为d，速度为v，速度平滑流程中速度v的函数为：

v＝a*d^b

其中，a、b是函数待确定的参数，可以根据每个机器人所能运动的速度上下限和通常的运动距离来定义函数中的v和d，将v和d代入公式获取参数a、b。

参照图3，举例进行说明。以下为测试过程中产生的经验值：

(1)0.02是机器人行走最小速度(单位为m/s)。

(2)0.8是机器人行走最大速度(单位为m/s)。

(3)0.05是机器人在多长距离应该采用最小速度行走。

(4)2.92是机器人在多长距离应该采用最大速度行走。

将(1)(3)和(2)(4)分别代入公式，获取参数a为0.647，b为0.4。

即机器人向目标点靠近时，距离目标点0.05米内采用最小速度(0.02m/s)匀速行走，距离目标点大于2.92米时，匀速以最高速度(0.8m/s)行走，而在0.05米到2.92米之间的这段距离则采用速度平滑流程。

虽然本发明已以较佳实施例公开如上，但实施例并不是用来限定本发明的。在不脱离本发明之精神和范围内，所做的任何等效变化或润饰，同样属于本发明之保护范围。因此本发明的保护范围应当以本申请的权利要求所界定的内容为标准。