动态场景中的移动机器人安全运动规划方法及装置

本发明涉及动态场景中的移动机器人安全运动规划方法及装置，属于移动机器人的运动规划。

背景技术：

1、在密集人群中导航的机器人应该基于附近行人的运动做出相应的反应，然而导航算法如果设计得不合理可能会导致不安全、低效、难以辨认的机器人动作。其中，现有的在动态环境中的机器人导航工作主要分为三类，分别是基于反应的方法、基于学习的方法和基于优化的方法。

2、在基于反应的方法中，机器人根据当前环境中动态障碍物的信息，一步向前地做出当前最优的动作。经典的方法以速度障碍(velocity obstacle,vo，)为代表，根据环境中所有动态障碍物的速度来分割速度空间得到当前最优的机器人速度动作。在一些现有技术中，基于vo提出了rvo(reciprocal velocity obstacle，互惠速度障碍)，将避障责任均匀分摊给两个智能体以减少避障过程中的振荡情况。以及，在rvo的基础上提出了orca(optimal reciprocal collision avoidance，最优互惠避障算法)，新增了时间窗口的概念，增大了机器人的可行速度空间，从而使得机器人的动作更优。但这种方法仅采用当前的环境信息将会导致短视、振荡和不自然的行为，不利于了解机器人的运动意图。

3、在基于学习的方法中，机器人试图重现适当的导航策略以模仿行人的交互运动，从而以更符合社会的方式在密集的人群中导航。其中，强化学习(reinforcementlearning,rl)通常用于训练计算效率高的导航策略，该策略隐式编码行人之间的交互和协作。最近基于学习的工作取得了重大进展，这些工作通常将人群的集体影响通过成对交互的简化聚合来建模，如现有技术的极大极小算子或lstm，但同时可能会降低复杂拥挤场景下合作规划的性能。又譬如，一些现有技术中，将自我注意机制加入人-机器人两两交互，并在强化学习框架中联合建模人-机器人和人-人相互作用以增强对人群行为的理解。然而，传统的基于学习的方法仅仅依赖于离线训练，十分依赖场景属性，面临着从仿真模拟到现实世界的泛化问题，不能保证在与训练数据不同的情况下的性能。

4、在基于优化的方法中，通常由每个时间步长的两个连续步骤组成，即预测和规划，首先使用行人运动模型来预测附近行人的运动，然后通过优化规划合适的轨迹。机器人的运动学约束和静态/动态碰撞约束都可以被集成为一个优化问题。譬如，一些现有技术提出一种基于模型预测的轮廓控制(model predictive contouring control,mpcc)的局部运动规划方法，可以在无结构化动态环境中进行运动规划，同时考虑了对静态和移动障碍物的避障问题。又譬如，一些现有技术在mpcc的基础上提出了一种基于安全走廊模型预测轮廓控制(corridor-based mpcc,cmpcc)的运动规划器，用于解决无人机在面对不可预测干扰时生成动态可行的、无碰撞的轨迹。又譬如，一些现有技术在mpcc的基础上结合控制障碍函数，在基于激光雷达的基础上实现安全关键控制的动态避障。这些基于优化的方法证明了在机器人避障方面具有鲁棒性和安全性，但很大的挑战在于如果预测的运动和行人的真实运动不同，机器人在密集的人群中可能无法找到一条可行的路径，如果优化问题设计得不合理，机器人在密集的人群中存在一定的安全隐患。

5、近年来，基于控制障碍函数(control barrier function,cbf)的方法被广泛应用于安全关键型控制系统。譬如，一些现有技术提出了一种安全关键控制器(clf-cbf-qp)，通过二次规划将保证稳定性的控制李雅普诺夫函数(control lyapunov function,clf)和保证安全性的控制障碍函数(cbf)统一起来，实现连续时域自适应巡航控制。这种方法在优化问题中将潜在冲突的控制目标和安全准则相耦合，很好地将系统的收敛性和安全性协调在一个平衡的位置，从而保证移动机器人的自主系统满足安全性要求的同时能够最快地到达目标位置。模型预测控制由于考虑了系统未来的状态信息，从而可以得到一个相对不贪婪的控制策略。又譬如，一些现有技术提出了一种基于模型预测控制和离散时间控制障碍函数的安全关键模型预测控制策略(mpc-cbf)，旨在保证系统安全并通过模型预测控制实现最优性能，相对于只使用一步预测信息，该方法表现出了对障碍物的提前规避，提升了自主系统的安全性。

6、综上所述，由于基于优化的方法具有能加入未来环境估计信息和安全约束的优点，因此更能规划出一条合理的，能被周围行人所理解的路线。但在移动机器人在面对复杂的、环境感知不确定的情况时，仍会做出危险的行为，导致移动机器人与周围的障碍物发生碰撞。但最新的研究都是直接将控制障碍函数作为优化问题的硬约束，而直接作为硬约束作用于预测时域中可能会导致优化问题出现无解的情况，因此如何结合控制障碍函数来构建优化问题成为主要难点。因此，在使用基于优化的方法时，恰当地构建优化问题来提升系统的安全性是十分重要的。

技术实现思路

1、本发明提供动态场景中的移动机器人安全运动规划方法及装置，旨在至少解决现有技术中存在的技术问题之一。

2、本发明的技术方案涉及动态场景中的移动机器人安全运动规划方法，根据本发明的方法包括以下步骤：

3、s100、在动态环境里，通过定位模块获取移动机器人的状态信息，通过感知模块获取周围的动态障碍物的状态信息；

4、s200、在动态环境里，根据当前的移动机器人的状态信息和动态障碍物的信息，结合控制障碍函数和模型预测控制构建避障约束；

5、s300、将所述避障约束转换为避障代价，加入到模型预测控制的代价方程中；

6、s400、根据所述避障约束相对于系统控制输入的相对度构建一步安全约束，以作为系统的安全保证约束；

7、s500、求解模型预测控制的优化问题，将该优化问题的最优解作为控制输入，发送到移动机器人的运动控制模块。

8、进一步，还包括所述步骤s600中：

9、若步骤s400的优化问题无可行解，则启动移动机器人的制动装置。

10、进一步，所述步骤s100中，

11、在第t时刻，所述移动机器人状态信息表示为其中，(xt，yt)表示移动机器人在当前地图下的二维坐标，θt表示移动机器人的朝向角，r表示移动机器人半径，表示移动机器人当前分别沿x轴和y轴的速度；

12、所述动态障碍物的状态变量表示为其中，上标i表示第i个动态障碍物，前两个变量表示动态障碍物的二维坐标，后两个变量表示动态障碍物分别沿x轴和y轴的速度。

13、进一步，所述步骤s200中，

14、在t时刻，基于控制障碍函数和模型预测控制的优化问题表示如下：

15、

16、s.t.for all k＝0，...，n-1：

17、xt+k+1|t＝f(xt+k|t，ut+k|t)，

18、

19、

20、xt|t＝xt，

21、

22、其中，n表示预测时域大小；xt+k|t表示从当前状态xt开始的第t+k时刻的预测状态；输入序列ut+k|t＝{ut|t，...，ut+n-1|t}；代价函数方程的前一项p表示终端代价，后一项q表示过程代价；第一个约束xt+k+1|t是系统的运动学约束；第二个约束xt+k|t和第三个约束ut+k|t分别表示机器人沿预测时域的状态约束和输入约束；第四个约束xt|t是初始状态约束；第五个约束hi是基于控制障碍函数的距离约束；

23、其中，ri为移动机器人和动态障碍物的半径和，γ∈(0，1]为保守系数；

24、其中，在t时刻的最优解为输入序列，其表示为ut+k|t＝{ut|t，...，ut+n-1|t}，最优解输入序列的第一个元素作为控制输入发送给移动机器人。

25、进一步，所述步骤s300中，

26、在t时刻，将基于控制障碍函数的距离约束使用精确罚函数的方法，通过加入松弛变量将避障约束转化为避障代价后，加入到模型预测控制的代价方程中，获得的。

27、进一步，所述步骤s300中，

28、将所述避障约束转换为避障代价后，其优化问题表示如下：

29、

30、s.t.for all k＝0，...，n-1：

31、xt+k+1|t＝f(xt+k|t，ut+k|t)，

32、

33、

34、xt|t＝xt，

35、

36、其中，为加入的松弛变量，其具体形式为no为动态障碍物的数量；

37、其中，约束的最后一项为软约束，其具体形式如下：

38、

39、进一步，所述步骤s400中，

40、所述相对度d是控制输入ut出现在系统状态约束h(xt)中的延迟步长；

41、使用广义控制障碍函数作为单步约束加入到模型预测控制优化问题后，获得的系统的安全保证约束表示如下：

42、

43、其中，保守系数η∈(γ，1]。

44、进一步，所述步骤s500中，

45、在t时刻，基于控制障碍函数和模型预测控制的优化问题最终形式表示如下：

46、

47、s.t.for all k＝0，...，n-1：

48、xt+k+1|t＝f(xt+k|t，ut+k|t)，

49、

50、

51、xt|t＝xt，

52、

53、

54、通过求解上述优化问题获得在t时刻的最优解序列，其表示如下：

55、ut+k|t＝{ut|t，...，ut+n-1|t}

56、其中，最优解序列的第一个元素作为控制输入发布给所述移动机器人。

57、本发明的技术方案还涉及计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施上述的方法。

58、本发明的技术方案还涉及移动机器人安全运动规划装置，所述系统包括计算机装置，该计算机装置包含上述计算机可读存储介质。

59、本发明的有益效果如下：

60、本发明为应用于动态场景中的移动机器人安全运动规划方法及装置，其为结合控制障碍函数和模型预测控制的安全运动规划方法，应用于在动态环境的仿真场景里，有利于保证移动机器人在安全的前提下，能够更加高效地到达目的地。

61、当优化问题遇到违反原来硬约束的情况时，本发明将会以代价的形式反映到优化问题中，而不是求解失败，从而确保了优化问题对于控制输入约束中的任何输入序列都是可行的，使得在运动规划中不用保证得到的每个解都满足安全约束，移动机器人可在拥挤区域会更积极地探索新路径。

62、本发明只有最优解序列的第一个元素将会被作为控制输入发布给机器人，从而实现避开动态障碍物的同时能够使得机器人能够高效地到达目的地。

63、本发明为了避免可能发生的碰撞，当优化问题不可行时，可通过激活机器人上自带的制动器来确保机器人完全停止，而不是仅仅将控制输入设置为零或保持上一时刻的控制输入。