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

技术特征：

1.一种基于里程计的机器人定位恢复方法，其特征在于，包括以下步骤：

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

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

2.根据权利要求1所述的基于里程计的机器人定位恢复方法，其特征在于，所述定位恢复，包括以下步骤：

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

3.根据权利要求2所述的基于里程计的机器人定位恢复方法，其特征在于，控制机器人行走到上一关键点位置时，使用周期纠正流程，通过对机器人运动过程中每一帧数据进行目标点与当前位置的对比，重新进行速度规划，对行走过程中位置偏差进行纠正。

4.根据权利要求3所述的基于里程计的机器人定位恢复方法，其特征在于，所述速度规划使用速度平滑流程，根据机器人离目标点的距离来规划速度和加速度。

5.根据权利要求4所述的基于里程计的机器人定位恢复方法，其特征在于，所述速度平滑流程，遵循以下原则：

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

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

6.根据权利要求5所述的基于里程计的机器人定位恢复方法，其特征在于，所述速度平滑流程中速度v的函数为：

v＝a*d^b

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

7.根据权利要求1～6任一所述的基于里程计的机器人定位恢复方法，其特征在于，所述关键点信息包括里程计信息中第一帧数据、机器人行走固定距离或转向固定角度后的里程计信息。

8.根据权利要求1～6任一所述的基于里程计的机器人定位恢复方法，其特征在于，所述定位丢失是指定位置信度小于阈值。

9.根据权利要求2所述的基于里程计的机器人定位恢复方法，其特征在于，所述定位恢复的具体步骤如下：

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

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

3)开启回退定时器；

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

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

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

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

10.根据权利要求1～6任一所述或权利要求9所述的基于里程计的机器人定位恢复方法，其特征在于，所述正常导航过程中维护里程计信息和关键点信息，包括以下步骤：

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

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

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

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