移动机器人跟随路径运动的控制方法及设备与流程

本发明涉及计算机领域，尤其涉及一种移动机器人跟随路径运动的控制方法及设备。

背景技术：

1、随着机器人技术的日趋完善，涌现出了很多种移动机器人的局部路径规划和控制方法，比如常见的有模型预测控制(mpc)，时间弹性带(teb)，动态窗口法(dwa)等。这些方法有的基于采样，有的基于优化算法。虽然运动比较灵活平滑，且能及时避障，但有时并不能严格遵循路径来移动，在有些要求机器人严格按照路径运动的应用场景中并不适用。

技术实现思路

1、本发明的一个目的是提供一种移动机器人跟随路径运动的控制方法及设备。

2、根据本发明的一个方面，提供了一种移动机器人跟随路径运动的控制方法，该方法包括：

3、步骤s1，基于机器人的预设全局路径，产生一系列固定间隔的路径点；

4、步骤s2，控制器基于上一次迭代的线速度和角速度，控制机器人行进的同时，根据机器人的当前位置，将已经经过的路径点清除掉；若还有未经过的路径点，则相对于机器人的当前位置，取一个未选择过的最远的路径点作为当前目标点，计算出从机器人的当前位置至当前目标点的一段移动轨迹；

5、步骤s3：基于各个路径点，判断计算出的移动轨迹是否是最优移动轨迹，若不最优移动轨迹，则回到步骤s2,继续选择离机器人的当前位置更近的一个路径点作为当前目标点；

6、步骤s4：若是最优移动轨迹，则基于最优移动轨迹，计算出下一次迭代的线速度和角速度。

7、进一步的，上述移动机器人跟随路径运动的控制方法中，相对于机器人的当前位置，取一个未选择过的最远的路径点作为当前目标点，计算出从机器人的当前位置至当前目标点的一段移动轨迹，包括：

8、根据机器人的当前位姿，将当前目标点的全局坐标转换成相对坐标(xt,yt)，相对坐标系的前方为x轴正方向，左侧为y轴正方向；

9、如果当前目标点的y轴相对坐标yt的绝对值约等于0，则该当前目标点在机器人的正前方或正后方，该段移动轨迹为直线轨迹，如果xt<0,则转弯半径r(radius)记为flt_min；否则记为flt_max；

10、如果当前目标点的y轴相对坐标yt不为0，则根据图2机器人的当前位置和当前目标点(xt,yt)和转弯半径(r)，计算弧线轨迹。

11、进一步的，上述移动机器人跟随路径运动的控制方法中，基于各个路径点，判断计算出的移动轨迹是否是最优移动轨迹，若不最优移动轨迹，则回到步骤s2,继续选择离机器人的当前位置更近的一个路径点作为当前目标点，包括：

12、判断当前目标点的x轴相对坐标xt是否小于0，或者弧线轨迹到机器人的当前位置和该目标点的两点连线的最远距离超过预设阈值，若是，则认为该直线轨迹或弧线轨迹已脱离路径，控制机器人原地旋转，放弃当前直线轨迹或弧线轨迹后，回到步骤s2，继续选择离机器人的当前位置更近的一个路径点作为当前目标点。

13、进一步的，上述移动机器人跟随路径运动的控制方法中，判断当前目标点的x轴相对坐标xt是否小于0，或者弧线轨迹到机器人的当前位置和该目标点的两点连线的最远距离超过预设阈值之后，还包括：

14、若否，则遍历在机器人当前位置至当前目标点之间的所有路径点，计算所有路径点到该直线轨迹或弧线轨迹的距离，

15、如果所有路径点距离该直线轨迹或弧线轨迹都大于等于预设的阈值，回到步骤s2，继续选择离机器人的当前位置更近的一个路径点作为当前目标点；

16、如果所有路径点距离该直线轨迹或弧线轨迹都小于预设的阈值，则该直线轨迹或弧线轨迹为最优移动轨迹。

17、进一步的，上述移动机器人跟随路径运动的控制方法中，若是最优移动轨迹，则基于最优移动轨迹，计算出下一次迭代的线速度和角速度，包括：

18、如果转弯半径等于flt_min，根据机器人的当前线速度，计算出下一次迭代的线速度和角速度；

19、如果转弯半径等于flt_max，根据直线轨迹长度，计算出下一次迭代的线速度和角速度；

20、如果转弯半径是其他值，根据弧线轨迹长度，计算出下一次迭代的线速度和角速度。

21、进一步的，上述移动机器人跟随路径运动的控制方法中，如果转弯半径等于flt_min，根据机器人的当前线速度，计算出下一次迭代的线速度和角速度，包括：

22、若转弯半径r等于flt_min，表示当前目标点在机器人身后，则控制机器人先停止并旋转到指定角度，其中，

23、如果机器人的当前线速度v0不为0则先减速，根据公式vnew＝v0-linear_deceleration*δt，计算出下一次迭代的线速度为vnew，同时下一次迭代的角速度规划为0；其中，如果计算出的线速度vnew小于0则设为0，表示机器人已经停止，以便于后续原地旋转到指定角度；公式中v0表示当前线速度，linear_deceleration表示预设线减速度，δt表示每次迭代的间隔；

24、如果机器人的当前线速度v0为0，则根据预设角加速度和预设角减速度，启动需要旋转的角度值，以及最终角速度为0，计算出下一次迭代的角速度，且设置下一次迭代的线速度为0，表示原地旋转，完成后回到步骤2，进行下一次迭代的计算。

25、进一步的，上述移动机器人跟随路径运动的控制方法中，如果转弯半径等于flt_max，根据直线轨迹长度，计算出下一次迭代的线速度和角速度，包括：

26、若转弯半径r等于flt_max，表示机器人进行直线行驶，因此下一次迭代的角速度设为0；根据如下方式计算下一次迭代的线速度，首先根据直线轨迹长度和如下公式，计算出当机器人在刚好到达当前目标点并停止时的最大速度speed1，

27、tracklength＝(vend+vstart)*t/2，t＝(vstart-vend)/linear_deceleration

28、其中，vend为0，因此speed1＝vstart＝sqrt(2*linear_deceleration*tracklength)；其中，vstart表示机器人在直线轨迹的起始点的线速度，vend表示机器人在直线轨迹的结束点的线速度，t表示机器人从直线轨迹的起始点移动到该直线轨迹的结束点的时长；linear_deceleration表示预设线减速度；

29、预设的机器人的最大线速度为speed2；

30、取min(speed1，speed2)作为下一次迭代的线速度，至此就完成了一次迭代规划，回到步骤2。

31、进一步的，上述移动机器人跟随路径运动的控制方法中，如果转弯半径是其他值，根据弧线轨迹长度，计算出下一次迭代的线速度和角速度，包括：

32、如果转弯半径是其他值，表示机器人进行弧线行驶，首先计算线速度：

33、根据弧线轨迹长度和如下公式，计算出当机器人在刚好到达当前目标点并停止时的最大速度speed1，

34、tracklength＝(vend+vstart)*t/2，t＝(vstart-vend)/linear_deceleration；

35、其中，vend为0，因此speed1＝vstart＝sqrt(2*linear_deceleration*tracklength)；vstart表示机器人在直线轨迹的起始点的线速度，vend表示机器人在直线轨迹的结束点的线速度，t表示机器人从直线轨迹的起始点移动到该直线轨迹的结束点的时长；

36、预设的机器人的最大线速度为speed2；

37、根据障碍物距离和预设线减速度linear_deceleration计算出允许的最大避障速度speed3；

38、根据当前线速度v0和预设线加速度linear_acceleration计算允许的最大规划速度speed4＝v0+linear_acceleration*δt；

39、根据机器人的当前角速度和角加速度计算出最大角速度，然后乘以转弯半径r得到speed5；

40、取linearspeed＝min(speed1，speed2，speed3，speed4，speed5)；

41、根据机器人的当前线速度和预设刹车减速度，计算出最小刹车速度brakingspeed，取linearspeed和brakingspeed的最大值，作为下一次迭代的线速度；

42、将下一次迭代的线速度除以转弯半径，计算出下一次迭代的角速度，并下发给控制器，至此就完成了一次迭代规划，然后回到步骤2。

43、根据本发明的另一方面，还提供一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现上述任一项所述的方法。

44、根据本发明的另一方面，还提供一种用于在网络设备端信息处理的设备，该设备包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该设备执行上述任一项所述的方法。

45、与现有技术相比，本发明从移动轨迹中的最远处的路径点作为当前局部轨迹的目标点开始处理，计算出从当前位置到目标点的轨迹，该轨迹可能是直线，也可能是弧线。如果该当前局部轨迹也能覆盖到对应的一段移动轨迹中途中的所有路径点，则是合法的轨迹；如果有部分路径点距离该轨迹的距离超过阈值，则认为该局部轨迹脱离路径，是非法轨迹，然后往回遍历，按照同样的算法将更近一个路径点作为当前局部轨迹中的目标点，直到找到最优的局部轨迹。然后可以根据局部轨迹长度，障碍物距离，机器人的加减速度，以及算法迭代一次的时间间隔，计算一个下一次迭代的合理的线速度和角速度作为新的控制输出。在机器人运动的过程中，会实时根据当前位置清理已经路过的路径点，以确保剩余路径点都是未到达的。

46、本发明公开一种移动机器人的运动控制方法，在给定路径的情况下，可以使机器人严格按照预定轨迹运行，同时保持机器人运动速度的连贯性和平滑性，可以使机器人严格按照预定路径(如工业环境、巡检场景)，根据预设的加/减速度平稳的移动到目的地，在途遇到障碍物时也能平稳的停下避让。

47、本发明可以将机器运动轨迹分为直线行走、弧线行走、原地转弯三种局部轨迹，从当前位置到目的地的移动轨迹会被分解为一段段的直线、弧线的局部轨迹，这些直线、弧线的线段会依次覆盖所有的路径点，以实现循迹运动的效果。在局部轨迹的规划的过程中，本发明会尽可能从当前的剩余移动轨迹中选择的最远的路径点作为当前局部轨迹中的目标点，使得局部轨迹尽可能长，以此让机器人有足够的加速空间，能够达到最高运行速度。