CN110162035A - 一种集群机器人在有障碍物场景中的协同运动方法 - Google Patents

Abstract

本发明涉及机器人的技术领域，更具体地，涉及一种机器人集群在有障碍物场景中的协同运动方法。本发明提供的方法用于多机器人在有障碍物场景下实现队形变换、动静态障碍物规避和协同运动。该方法将集群机器人在复杂环境下的协同控制定义为一个分布式的模型预测控制问题，每个机器人根据局部邻居的信息，独立求解优化方程，有较好的鲁棒性和灵活性。同时，该方法提出队形图概念，将机器人队形用一个有向图来表示，在保证图连通性的前提下，通过改变队形图的边和权重来实现期望队形的局部变换，可以同时保证集群的连通性和灵活性。此外，该专利提出的方法具有层次化的结构，采用这种层次结构，可以减少模块间的耦合性，便于算法设计与拓展。

Description

一种集群机器人在有障碍物场景中的协同运动方法

技术领域

本发明涉及群体智能、机器人控制技术领域，更具体地，涉及一种集群机器人在有障碍物场景中的协同运动方法。

背景技术

随着机器人技术的不断发展，自动化的机器人***在越来越多的领域发挥着重要的作用，比如工业机器人，家用机器人，服务机器人等等。因此，机器人在结构化环境中的自动导航一直是一个热门的研究领域。与传统的人力***相比，机器人***有着多方面的优势，比如经济，时间和安全性等。而相比于单机器人***，多机器人***协同执行任务具有更大的优势和更广泛的应用场景，比如危险环境中的探索，农业中的监测以及极端条件下的军事任务。同一复杂任务，多机器人***可以比单机器人更高效，更低花销地完成，同时具有更好的容错性(其中一个机器人失效时不会影响整个***的运行)，自适应和灵活性。因为多机器人***具有这些优越性，并且当代处理器计算能力的不断增长，近年来多移动机器人***吸引了越来越多国内外学者们的关注。

编队控制指多个机器人组成的团队在向特定目标或方向运动的过程中，相互之间保持预定的几何形态(即队形)，同时又要适应环境约束(例如避开障碍)的控制问题。多机器人编队控制，需要解决以下问题：

(1)机器人如何确定自己在队形中的期望位置；

(2)机器人如何确定自己在队形中的实际位置；

(3)机器人怎样运动以维持队形；

(4)遇到障碍物时机器人如何应对。

现有技术中，提出了许多方法解决以上问题，有领航者-跟随者算法、虚拟结构法、基于行为法、人工势场法和基于优化的方法。但是在有障碍物场景中，机器人集群无法以一个固定队形运动，此时上述方法不完全适用。在有约束环境下，机器人集群应该根据传感器检测到的环境信息自适应地改变期望队形，而如何选择一个新的队形以及如何实现队形转换是一个具有挑战性的问题，对于分布式架构的集群来说，问题则更为复杂。

发明内容

本发明为克服上述现有技术所述的至少一种缺陷，提供一种集群机器人在有障碍物场景中的协同运动方法，提出了局部队形变换方法，控制集群机器人在有障碍物场景中的协同运动。

为解决上述技术问题，本发明采用的技术方案是：一种集群机器人在有障碍物场景中的协同运动方法，包括以下步骤：

S1.在机器人集群开始运动前，需要人工给定关键路径点或者通过规划算法规划出一条期望路径，以供集群的领航者跟随，跟随者通过对领航者的跟随实现整个集群的协同运动，并将期望队形和机器人间关系用一个队形图来表达；

S2.机器人在运动过程中利用机载传感器实时检测感知其周围环境的障碍物以及非邻居机器人的状态信息；

S3.每一架机器人通过局域网发布自己的位置和速度信息，同时得到其邻居机器人的位置和速度信息，根据邻居的状态改变队形图中边以及边的权重值以实施机器人间的相互避碰；

S4.将步骤S2和S3得到的信息作为分布式MPC(模型预测控制)算法的输入，通过求解最优化问题，得到控制序列和状态序列，选取第一个控制量作为机器人当前时刻的最优控制量，输入作用到机器人上，驱使机器人到达期望的目标点；

S5.重复步骤S2至S4，直至到达终点。

进一步的，所述的S1步骤中，利用图论对机器人集群进行建模，对于有n个机器人的集群，用队形图来表示集群关系，是顶点的集合，顶点代表机器人，ε是有向边的集合，表示机器人之间的信息流动方向，表示机器人之间的期望距离，表示机器人的期望的相对方位角。

在集群机器人协同运动任务中，每一个机器人需要根据邻居位置来确定自己的期望位置。根据领航者-跟随者算法，跟随者可以通过两种方法确定其在编队中相对于其他机器人的位置，第一种方法是距离-方位控制器，第二种是距离-距离控制器。对于有n个机器人的集群，在本发明中用队形图来表示集群关系，是顶点的集合，顶点代表机器人，ε是有向边的集合，表示机器人之间的信息流动方向，表示机器人之间的期望距离，表示机器人的期望的相对方位角。所述队形图是一个有向图，包含节点、边和距离三个元素，节点表示机器人个体，边代表邻居关系，即信息流动方向，距离代表邻居间期望距离。

进一步的，所述的S3步骤中边的权重变化具体包括以下步骤：

S311.通过在模型预测控制器中添加机器人间距离与期望距离的残差项来实现机器人间的队形保持，残差项表示为：

固定的权重ω_ij可以使得机器人有良好的距离保持性能，但是对于穿越障碍物环境的时候会缺乏灵活性，所以本发明提出了自适应改变编队权重ω_ij参数的方法；

S312.对于第i个机器人和第j个机器人，定义其安全距离余量为：

l_ij＝||p_i-p_j||-2r

其中p_i和p_j分别表示第i个机器人和第j个机器人位置，r是机器人的半径，由于硬约束的限制，可以保证l_ij≥0，对以上公式对时间求导，可以得到第i个机器人和第j个机器人的距离变化率为：

故对于第j个机器人，可以得到其与第i个机器人的碰撞时间为：

δt_ij＝l_ij/l_ij

碰撞时间δt_ij描述了两个机器人发生碰撞的时间紧急性，碰撞时间的大小和方向可以用来确定两个机器人之间的碰撞情况；当δt_ij＝0时，意味着l_ij＝0，这时候说明两个机器人很有可能发生了碰撞，这是我们应该要避免出现的情况；当δt_ij＞0，意味着l_ij＞0，这时候说明两个机器人之间的距离余量在逐渐变大，两个机器人正在远离，δt_ij越大，说明两个机器人远离得越快。当δt_ij＜0，意味着l_ij＜0，这时候说明两个机器人之间的距离余量在逐渐变小，两个机器人正在靠近，δt_ij的绝对值越小，说明两个机器人靠近得越快。所以碰撞时间δt_ij描述了两个机器人发生碰撞的时间紧急性；

S313.如果第i个机器人和第j个机器人在队形图中是相邻的，且两个机器人的碰撞时间δt_ij＜0且绝对值比较小的时候，相应的权重参数ω_ij就应该增大；可以用以零均值的高斯密度函数来表示ω_ij的变化：

其中k＞0是高斯密度函数的峰值，σ²表示对碰撞时间的敏感程度，越小的σ²使得权重参数ω_ij的变化越敏感。

进一步的，为了维持队形的稳定，减少权重调节的频率，只有当邻居进入机器人的危险范围内，才触发自适应权重变化。

进一步的，所述的S3步骤中，局部队形变换的方法具体包括以下步骤：

S321.定义第j个机器人的距离的畸变系数为：

其中s_ij∈S_f是第j个机器人相对于第i个机器人的期望距离，d_ij是第j个机器人与第i个机器人的实际距离；当η_ij＝0时，说明两个机器人的距离正好等于期望距离；当η_ij＞0时，说明第i个机器人对第j个机器人有牵引力；当η_ij＜0时，说明第i个机器人会对第j个机器人有排斥力；

S322.假如在队形图中，存在有向边(v_i,v_j)和(v_k,v_j)，意味着要求第j个机器人要与第i个机器人和第k个机器人这两个机器人保持期望距离；当η_ij＜0且η_kj＞η_threshold时，第j个机器人同时处于被牵引和被排斥的情况，在这种情况下，第j个机器人需要删除有向边(v_k,v_j)，解除与第k个机器人保持期望距离的要求，放宽对第j个机器人的运动约束，使得第j个机器人能够更容易在约束环境中找到可行解；

S323.设当第j个机器人通过传感器观测到感知范围内的非邻居机器人时，如观测到第u个机器人且这个机器人不是队形图中第j个机器人的邻居成员，但是通过观测计算得到的碰撞时间δt_uj＜0，这意味着对于第j个机器人来说，有一个先前不是其邻居成员的机器人正在向其靠近，在这种情况下，第j个机器人要与第u个机器人保持距离以避免发生碰撞，将有向边(v_u,v_j)添加到原有的编队拓扑结构图中，同时相应地要将期望距离s_uj添加到原有的期望距离集合中，从而改变原有的集群机器人编队队形图。

本发明提供的方法用于多机器人在有障碍物场景下实现队形变换、动静态障碍物规避和协同运动。该方法将集群机器人在复杂环境下的协同控制定义为一个分布式的模型预测控制问题，每个机器人根据局部邻居的信息，独立求解优化方程，有较好的鲁棒性和灵活性。同时，该方法提出队形图概念，将机器人队形用一个有向图来表示，在保证图连通性的前提下，通过改变队形图的边和权重来实现期望队形的局部变换，可以同时保证集群的连通性和灵活性。此外，该专利提出的方法具有层次化的结构：路径规划层、队形决策层和队形保持层，采用这种层次结构，可以减少模块间的耦合性，便于算法设计与拓展

与现有技术相比，有益效果是：

1.将单机器人路径规划算法拓展至多机器人***；在机器人集群中，定义一个领航者，领航者执行路径规划与导航功能，其余机器人(跟随者)执行队形保持与障碍物避碰功能，从而实现机器人群体的路径规划与导航。因此，路径规划算法与编队控制算法解耦，从而可以采用单机器人路径规划的各种算法；

2.设计了层次化的***结构；在有约束环境下，机器人集群需要根据环境自适应地改变队形；可以减少模块间的耦合性，便于算法设计与拓展；

3.提出自适应队形变换算法；机器人实时根据局部邻居的状态信息来动态更新队形图中边和边的权值，从而实现机器人队形的局部变换，而不会牵扯到整个期望队形的变换。

附图说明

图1是本发明的整体方法流程图。

图2是本发明的队形图的示意图。

图3是本发明的局部队形变换方法流程图。

图4是本发明局部队形变换方法的效果图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

如图1所示，一种集群机器人在复杂场景中的协同运动方法，包括如下步骤：

步骤1.在机器人集群开始运动前，需要人工给定关键路径点或者通过规划算法规划出一条期望路径，以供集群的领航者跟随，跟随者通过对领航者的跟随实现整个集群的协同运动。

在集群机器人协同运动任务中，每一个机器人需要根据邻居位置来确定自己的期望位置。利用图论的方法对机器人集群进行建模；对于有n个机器人的集群，用队形图来表示集群关系，是顶点的集合，顶点代表机器人，ε是有向边的集合，表示机器人之间的信息流动方向，表示机器人之间的期望距离，表示机器人的期望的相对方位角，如图2所示。

步骤2.机器人在运动过程中利用记载传感器实时检测感知其周围环境的障碍物以及非邻居机器人的状态信息；

步骤3.每一架机器人通过局域网通信发布自己的位置和速度信息，同时得到其邻居机器人的位置和速度信息，根据邻居的状态改变队形图中边以及边的权值；

其中，边的权重变化具体包括以下步骤：

S311.通过在模型预测控制器中添加机器人间距离与期望距离的残差项来实现机器人间的队形保持，残差项表示为：

固定的权重ω_ij可以使得机器人有良好的距离保持性能，但是对于穿越障碍物环境的时候会缺乏灵活性，所以本发明提出了自适应改变编队权重ω_ij参数的方法；

S312.对于第i个机器人和第j个机器人，定义其安全距离余量为：

l_ij＝||p_i-p_j||-2r

其中p_i和p_j分别表示第i个机器人和第j个机器人位置，r是机器人的半径，由于硬约束的限制，可以保证l_ij≥0，对以上公式对时间求导，可以得到第i个机器人和第j个机器人的距离变化率为：

故对于第j个机器人，可以得到其与第i个机器人的碰撞时间为：

δt_ij＝l_ij/l_ij

碰撞时间δt_ij描述了两个机器人发生碰撞的时间紧急性，碰撞时间的大小和方向可以用来确定两个机器人之间的碰撞情况；当δt_ij＝0时，意味着l_ij＝0，这时候说明两个机器人很有可能发生了碰撞，这是我们应该要避免出现的情况；当δt_ij＞0，意味着l_ij＞0，这时候说明两个机器人之间的距离余量在逐渐变大，两个机器人正在远离，δt_ij越大，说明两个机器人远离得越快。当δt_ij＜0，意味着l_ij＜0，这时候说明两个机器人之间的距离余量在逐渐变小，两个机器人正在靠近，δt_ij的绝对值越小，说明两个机器人靠近得越快。所以碰撞时间δt_ij描述了两个机器人发生碰撞的时间紧急性；

S313.如果第i个机器人和第j个机器人在队形图中是相邻的，且两个机器人的碰撞时间δt_ij＜0且绝对值比较小的时候，相应的权重参数ω_ij就应该增大；可以用以零均值的高斯密度函数来表示ω_ij的变化：

其中k＞0是高斯密度函数的峰值，σ²表示对碰撞时间的敏感程度，越小的σ²使得权重参数ω_ij的变化越敏感。

为了维持队形的稳定，减少权重调节的频率，对自适应权重变换设定了触发条件——当邻居进入机器人的危险范围(危险范围>安全范围>半径)内，才触发自适应权重变化。

另外，局部队形变换的方法具体包括以下步骤：

S321.定义第j个机器人的距离的畸变系数为：

其中s_ij∈S_f是第j个机器人相对于第i个机器人的期望距离，d_ij是第j个机器人与第i个机器人的实际距离；当η_ij＝0时，说明两个机器人的距离正好等于期望距离；当η_ij＞0时，说明第i个机器人对第j个机器人有牵引力；当η_ij＜0时，说明第i个机器人会对第j个机器人有排斥力；

S322.假如在队形图中，存在有向边(v_i,v_j)和(v_k,v_j)，意味着要求第j个机器人要与第i个机器人和第k个机器人这两个机器人保持期望距离；当η_ij＜0且η_kj＞η_threshold时，第j个机器人同时处于被牵引和被排斥的情况，在这种情况下，第j个机器人需要删除有向边(v_k,v_j)，解除与第k个机器人保持期望距离的要求，放宽对第j个机器人的运动约束，使得第j个机器人能够更容易在约束环境中找到可行解；

S323.设当第j个机器人通过传感器观测到感知范围内的非邻居机器人时，如观测到第u个机器人且这个机器人不是队形图中第j个机器人的邻居成员，但是通过观测计算得到的碰撞时间δt_uj＜0，这意味着对于第j个机器人来说，有一个先前不是其邻居成员的机器人正在向其靠近，在这种情况下，第j个机器人要与第u个机器人保持距离以避免发生碰撞，将有向边(v_u,v_j)添加到原有的编队拓扑结构图中，同时相应地要将期望距离s_uj添加到原有的期望距离集合中，从而改变原有的集群机器人编队队形图。

步骤4.将步骤2和步骤3得到的信息作为分布式MPC算法的输入，通过求解最优化问题，得到控制序列和状态序列，选取第一个控制量作为机器人当前时刻的最优控制量，输入作用到机器人上，驱使机器人到达期望的目标点。整体流程如图3所示，具体实施效果如图4所示。

步骤5.重复步骤2至步骤4，直至到达终点。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (5)

1.一种集群机器人在有障碍物场景中的协同运动方法，其特征在于，包括以下步骤：

S1.在机器人集群开始运动前，需要人工给定关键路径点或者通过规划算法规划出一条期望路径，以供集群的领航者跟随，跟随者通过对领航者的跟随实现整个集群的协同运动，并将期望队形和机器人间关系用一个队形图来表达；

S2.机器人在运动过程中利用机载传感器实时检测感知其周围环境的障碍物以及非邻居机器人的状态信息；

S3.每一架机器人通过局域网发布自己的位置和速度信息，同时得到其邻居机器人的位置和速度信息，根据邻居的状态改变队形图中边以及边的权重值以实施机器人间的相互避碰；

S4.将步骤S2和S3得到的信息作为分布式MPC算法的输入，通过求解最优化问题，得到控制序列和状态序列，选取第一个控制量作为机器人当前时刻的最优控制量，输入作用到机器人上，驱使机器人到达期望的目标点；

S5.重复步骤S2至S4，直至到达终点。

2.根据权利要求1所述的一种集群机器人在有障碍物场景中的协同运动方法，其特征在于，所述的S1步骤中，利用图论对机器人集群进行建模，对于有n个机器人的集群，用队形图来表示集群关系，是顶点的集合，顶点代表机器人，ε是有向边的集合，表示机器人之间的信息流动方向，表示机器人之间的期望距离，表示机器人的期望的相对方位角。

3.根据权利要求2所述的一种集群机器人在有障碍物场景中的协同运动方法，其特征在于，所述的S3步骤中边的权重变化具体包括以下步骤：

S311.通过在模型预测控制器中添加机器人间距离与期望距离的残差项来实现机器人间的队形保持，残差项表示为：

S312.对于第i个机器人和第j个机器人，定义其安全距离余量为：

l_ij＝||p_i-p_j||-2r

其中p_i和p_j分别表示第i个机器人和第j个机器人位置，r是机器人的半径，由于硬约束的限制，可以保证l_ij≥0，对以上公式对时间求导，可以得到第i个机器人和第j个机器人的距离变化率为：

故对于第j个机器人，可以得到其与第i个机器人的碰撞时间为：

碰撞时间δt_ij描述了两个机器人发生碰撞的时间紧急性，碰撞时间的大小和方向可以用来确定两个机器人之间的碰撞情况；

S313.如果第i个机器人和第j个机器人在队形图中是相邻的，且两个机器人的碰撞时间δt_ij＜0且绝对值比较小的时候，相应的权重参数ω_ij就应该增大；可以用以零均值的高斯密度函数来表示ω_ij的变化：

其中k＞0是高斯密度函数的峰值，σ²表示对碰撞时间的敏感程度，越小的σ²使得权重参数ω_ij的变化越敏感。

4.根据权利要求3所述的一种集群机器人在有障碍物场景中的协同运动方法，其特征在于，为了维持队形的稳定，减少权重调节的频率，只有当邻居进入机器人的危险范围内，才触发自适应权重变化。

5.根据权利要求3所述的一种集群机器人在有障碍物场景中的协同运动方法，其特征在于，所述的S3步骤中，局部队形变换的方法具体包括以下步骤：

S321.定义第j个机器人的距离的畸变系数为：

其中s_ij∈S_f是第j个机器人相对于第i个机器人的期望距离，d_ij是第j个机器人与第i个机器人的实际距离；当η_ij＝0时，说明两个机器人的距离正好等于期望距离；当η_ij＞0时，说明第i个机器人对第j个机器人有牵引力；当η_ij＜0时，说明第i个机器人会对第j个机器人有排斥力；

S322.假如在队形图中，存在有向边(v_i,v_j)和(v_k,v_j)，意味着要求第j个机器人要与第i个机器人和第k个机器人这两个机器人保持期望距离；当η_ij＜0且η_kj＞η_threshold时，第j个机器人同时处于被牵引和被排斥的情况，在这种情况下，第j个机器人需要删除有向边(v_k,v_j)，解除与第k个机器人保持期望距离的要求，放宽对第j个机器人的运动约束，使得第j个机器人能够更容易在约束环境中找到可行解；

S323.设当第j个机器人通过传感器观测到感知范围内的非邻居机器人时，如观测到第u个机器人且这个机器人不是队形图中第j个机器人的邻居成员，但是通过观测计算得到的碰撞时间δt_uj＜0，这意味着对于第j个机器人来说，有一个先前不是其邻居成员的机器人正在向其靠近，在这种情况下，第j个机器人要与第u个机器人保持距离以避免发生碰撞，将有向边(v_u,v_j)添加到原有的编队拓扑结构图中，同时相应地要将期望距离s_uj添加到原有的期望距离集合中，从而改变原有的集群机器人编队队形图。