CN110209167A - 一种实时的完全分布式的多机器人系统编队的方法 - Google Patents

Abstract

本发明属于机器人协同控制系统技术领域，特别涉及一种实时的完全分布式的多机器人系统编队的方法,本发明是适时探测周围环境的，所以不需要提前知道全局环境，克服了集中控制存在的缺陷；同组机器人之间有通信，并且用相对坐标系做数学规划，各个机器人之间会绝对保证相对位置不变，避免机器人之间的碰撞；采用分布式的方法代替Leader‑Follower的方法可以保证任何机器人遇到障碍物队形都不会乱。

Description

一种实时的完全分布式的多机器人系统编队的方法

技术领域

本发明属于机器人协同控制系统技术领域，特别涉及一种实时的完全分布式的多机器人系统编队的方法。

背景技术

随着科技的不断发展，机器人被应用到许多领域，如自动制造、柔性生产、搜索营救、环境监测、安全健康等。但是单机器人在信息获取、处理及控制能力等方面都有限制。对于复杂的工作、任务及多变的工作环境，单个机器人的能力明显不足。相对于单个机器人，多个机器人可以协调合作完成单一机器人难以完成的复杂任务，比如环境监督、灾难营救和雷区映射等等。

多机器人编队控制是多机器人系统研究领域一类常见的协作问题，采用多个机器人组成编队具有较多优点，例如在军事侦察、搜索、排雷等应用中能够获取更多的环境信息，在飞行器、卫星编队飞行等应用中能够完成更多、更复杂的任务。

近年来，随着网络技术、通信技术以及信息技术的发展，关于多机器人系统编队的研究有长足的进展，但有些方法仍存在一些不足，具体体现如下：1.集中控制方法缺乏健壮性和灵活性，当环境发生变化时控制器就需要重新制定全局的控制方案。2.采用分散式控制方法缺乏统一的规则，每个人机器人不能获取一起工作的其他机器人的状态和信息，可能会发生碰撞，后果不堪设想。3.采用Leader-Follower的控制方法缺乏普适性，如果Follower机器人遇到障碍物，控制器不会做出相应的决策保持队型。

发明内容

本发明的目的在于提供一种具有普适性、健壮性及灵活性的一种实时的完全分布式的多机器人系统编队的方法。

本发明提供的技术方案如下：一种实时的完全分布式的多机器人系统编队的方法,至少包括多机器人,其特征是：多机器人按如下步骤编队：

步骤1)若则执行下一步，否则跳出本算法；

步骤2)取stop_P_i,和position_i[n]，并且初始化

步骤3)机器人i判断在当前时刻k₀是否可以探测到静态或者动态障碍物，若有，则添加静态障碍物相对位置坐标到添加动态障碍物相对位置坐标到

步骤4)机器人i广播自己的障碍物集合给同组的其他机器人；

步骤5)若执行下一步，否则跳到步骤8；

步骤6)机器人i接收机器人j发来的虚拟障碍物的相对坐标分别添加到并且m₁＝m₁+1；

步骤7)若m₁＝n-1,则并且执行下一步，否则转到步骤6；

步骤8)机器人i根据stop_P_i和用iscp算法计算下一个时间点k₀+1的速度和位置及k₀时刻的加速度，如果计算成功则

步骤9)若则机器人i广播自己的计算状态给同组的其他机器人；

步骤10)若执行下一步，否则转到步骤13；

步骤11)机器人i接收机器人j发来的计算状态，置并且m₂＝m₂+1；

步骤12)若m₂＝n-1，则转到下一步，否则转到步骤11；

步骤13)机器人i把自己的计算结果送到执行器执行。

其中机器人i在时间点k₀的iscp算法步骤如下：

步骤1)根据前一个时间点的计算结果初始化初始位置序列⁰x_i(k₀+1:T_i),迭代次数s＝0,以及

步骤2)判断s是否满足s≤s_max，若满足则执行下一步，否则转到步骤13；

步骤3)初始化HasAdded＝false；

步骤4)判断k是否满足若满足则执行下一步，否则，跳到步骤9；

步骤5)判断k是否满足k∈PosiIndex，若满足则执行下一步，否则转到步骤7；

步骤6)碰撞避免的约束条件凸优化CVXConstr＝ApproxCVX(^sx_i[k],O)，将凸优化后的约束条件加到避碰约束条件集合CurConstr＝CurConstr∪CVXConstr，转到步骤9；

步骤7)判断HasAdded＝false和^sflag_new_i[k]＝true两个条件是否均满足，若满足则添加障碍物位置^sp_i[k]到集合O中并且执行下一步，否则转到步骤9；

步骤8)HasAdded＝true,PosiIndex＝PosiIndex∪{k},CVXConstr＝ApproxCVX(^sx_i[k-1],O),CVXConstr＝CurConstr∪CVXConstr；

步骤9)根据前边的结果构造数学规划CVX_P_i[k₀]＝Approx(P_i[k₀],CurConstr),并且求解该数学规划(aa,vv,xx)＝Solve(CVX_P_i[k₀]),利用计算结果当中的位置xx初始化下一次迭代初始位置序列^s+1x_i[k₀+1:T_i]＝xx；

步骤10)判断是否满足max||^s+1x_i[k]-^sx_i[k]||_∞≤∈，若满足执行下一步，否则转到步骤12，其中∈为精度；

步骤11)返回下一个时间点的信息a_i[k₀]＝aa[1],v_i[k₀+1]＝vv[1]，x_i[k₀+1]＝xx[1]，跳出本算法；

步骤12)迭代次数加一s＝s+1并且转到步骤2；

步骤13)报告错误，并采取紧急行动，例如紧急制动。

前述步骤9当中的数学规划模型如下：

subject to：

v_i[k+1]＝v_i[k]+a_i[k]h_i

v_i[k]∈[v_min,v_max],a_i[k]∈[a_min,a_max]

||x_i[k]-x₀||₂≥ρ′

其中x_i[k]、v_i[k]、a_i[k]分别表示机器人i在k时刻的位置、速度和加速度，h_i表示离散的时间步长，表示机器人i的目标位置，||·||₂表示二范数，对于静态障碍物ρ′＝ρ+ρ₀，对于动态障碍物ρ′＝2ρ，其中ρ表示机器人的安全半径，ρ₀表示静态障碍物的半径，x₀表示静态或动态障碍物的位置。其中||x_i[k]-x₀||₂≥ρ′在计算时需要凸优化为

x_i[k′]是一个位置序列包括其中表示在第k个时间点的预测界域，^sx_i[k′]表示机器人i在第k′个时间点的第s次迭代的结果。

步骤中的符号定义：

在给定的环境当中，多个机器人只知道自己的目标位置和同一个系统当中与其他机器人的相对位置。在移动的过程当中，每个机器人探测自己周围的环境，当某一个或者多个机器人探测到有障碍物时，其会广播障碍物和自己的相对位置给同组的机器人，同组其他机器人接收到障碍物的信息时把其当作虚拟障碍物添加到自己约束当中进行数学规划求解，这样就保证了在每次规划当中每个机器人用同样的数学规划方法，同样的目标函数和同样的约束条件，在同样的精度要求下计算出下一个时间点的相对位置是一样的。近而保证适时避障并且队形不变，最终到达各自的目标位置。

与现有的技术相比，本发明的有益效果具体体现在：

1、与采用集中控制的方法相比更具有灵活性，机器人可以适时地自动地根据不同的环境做出相应的决策进而执行，不会由于环境的变化需要重新制定整个方案；另外本发明不像集中控制方法需要获取全部环境情况，只需要获取自己的局部环境即可。

2、与采用分散控制的方法相比更具有安全性，同组的机器人可以保证自己内部不会发生碰撞，分散式控制由于机器人之间没有通信，可能在两个机器人遇到不同障碍物时由于同时避障而导致两个机器人发生碰撞。本发明不会发生这种情况，因为机器人之间有通信，两个机器人遇到不同障碍物之后，每个机器人会把另一个机器人遇到的障碍物考虑到自己的规划当中。

3、与采用Leader-Follower方法相比更具有普适性，本发明不设立主机和从机，每个机器人既是主机又是从机，完全可以避免Leader-Follower中从机遇到障碍物而不能保持队形的缺陷。

附图说明

下边结合实施例附图对本发明作进一步说明：

图1是采用分散式控制方法，机器人之间会发生碰撞；

图2与图1对比，采用分布式的方法可以避免机器人之间的碰撞；

图3是五个机器人编队的环境信息；

图4表示在k₀＝16时，4号机器人探测到障碍物O₂，此时4号机器人会把O₂和自己的相对位置(3.8,3.2)-¹⁶current_P₄广播给1,2,3,5号机器人，当其它四个机器人接收到相对位置坐标后，会把此相对坐标作为虚拟障碍物加到自己的数学规划里进行计算，当五个机器人都计算结束后，各自执行器分别按照计算结果执行。

图5表示在k₀＝19时，1号机器人和4号机器人分别遇到障碍物O₁和O₂。1号机器人广播O₁和自己的相对位置(3,4.3)-¹⁹current_P₁给2,3,4,5号机器人，4号机器人广播O₂和自己的相对位置(3.8,3.2)-¹⁹current_P₄给1,2,3,5号机器人，每个机器人都接收到所有广播后，把接收到的位置作为一个障碍物加到自己的数学规划里进行计算。

图6表示在k₀＝21时，1号、4号、5号机器人分别探测到障碍物O₁、O₂、O₂，1、4、5号机器人分别广播(3,4.3)-²¹current_P₁，(3.8,3.2)-²¹current_P₄，(3.8,3.2)-²¹current_P₅给同组其它四个机器人。

图7表示1、3、5号机器人均可以探测到障碍物，处理方式同上。

图8表示k₀＝27时，3号机器人探测到障碍物O₂，并且进行避障。

图9表示k₀＝29时，3号机器人遇到动态障碍物O₃，即此时3号机器人同时遇到静态障碍物O₂和动态障碍物O₃，3号机器人会把两个障碍物和自己的相对位置广播给1、2、4、5号机器人。

图10表示3号机器人处理图九遇到的障碍物的路径，五个机器人经过图11所示的路径最终到达图12所示的位置。

具体实施方式

实施例1

结合表1定义的符号内容说明本发明具体实施过程：一种实时的完全分布式的多机器人系统编队的方法,至少包括多机器人,多机器人按如下步骤编队：

步骤1)若则执行下一步，否则跳出本算法；

步骤2)获取stop_P_i,和position_i[n]，并且初始化

步骤3)机器人i判断在当前时刻k₀是否可以探测到静态或者动态障碍物，若有，则添加静态障碍物相对位置坐标到添加动态障碍物相对位置坐标到

步骤4)机器人i广播自己的障碍物集合给同组的其他机器人；

步骤5)若执行下一步，否则跳到步骤8；

步骤6)机器人i接收机器人j发来的虚拟障碍物的相对坐标分别添加到并且m₁＝m₁+1；

步骤7)若m₁＝n-1,则并且执行下一步，否则转到步骤6；

步骤8)机器人i根据 和用iscp算法计算下一个时间点k₀+1的速度和位置及k₀时刻的加速度，如果计算成功则

步骤9)若则机器人i广播自己的计算状态给同组的其他机器人；

步骤10)若执行下一步，否则转到步骤13；

步骤11)机器人i接收机器人j发来的计算状态，置并且m₂＝m₂+1；

步骤12)若m₂＝n-1，则转到下一步，否则转到步骤11；

步骤13)机器人i把自己的计算结果送到执行器执行。

其中机器人i在时间点k₀的iscp算法步骤如下：

步骤1)根据前一个时间点的计算结果初始化初始位置序列⁰x_i(k₀+1:T_i),迭代次数s＝0,以及

步骤2)判断s是否满足s≤s_max，若满足则执行下一步，否则转到步骤13；

步骤3)初始化HasAdded＝false；

步骤4)判断k是否满足若满足则执行下一步，否则，跳到步骤9；

步骤5)判断k是否满足k∈PosiIndex，若满足则执行下一步，否则转到步骤7；

步骤6)碰撞避免的约束条件凸优化CVXConstr＝ApproxCVX(^sx_i[k],O)，将凸优化后的约束条件加到避碰约束条件集合CurConstr＝CurConstr∪CVXConstr，转到步骤9；

步骤7)判断HasAdded＝false和^sflag_new_i[k]＝true两个条件是否均满足，若满足则添加障碍物位置^sp_i[k]到集合O中并且执行下一步，否则转到步骤9；

步骤8)HasAdded＝true,PosiIndex＝PosiIndex∪{k},CVXConstr＝ApproxCVX(^sx_i[k-1],O),CVXConstr＝CurConstr∪CVXConstr；

步骤9)根据前边的结果构造数学规划CVX_P_i[k₀]＝Approx(P_i[k₀],CurConstr),并且求解该数学规划(aa,vv,xx)＝Solve(CVX_P_i[k₀]),利用计算结果当中的位置xx初始化下一次迭代初始位置序列^s+1x_i[k₀+1:T_i]＝xx；

步骤10)判断是否满足max||^s+1x_i[k]-^sx_i[k]||_∞≤∈，若满足执行下一步，否则转到步骤12，其中∈为精度；

步骤11)返回下一个时间点的信息a_i[k₀]＝aa[1],v_i[k₀+1]＝vv[1]，x_i[k₀+1]＝xx[1]，跳出本算法；

步骤12)迭代次数加一s＝s+1并且转到步骤2；

步骤13)报告错误，并采取紧急行动，例如紧急制动。

前述步骤9当中的数学规划模型如下：

subject to：

v_i[k+1]＝v_i[k]+a_i[k]h_i

v_i[k]∈[v_min,v_max],a_i[k]∈[a_min,a_max]

||x_i[k]-x₀||₂≥ρ′

其中x_i[k]、v_i[k]、a_i[k]分别表示机器人i在k时刻的位置、速度和加速度，h_i表示离散的时间步长，表示机器人i的目标位置，||·||₂表示二范数，对于静态障碍物ρ′＝ρ+ρ₀，对于动态障碍物ρ′＝2ρ，其中ρ表示机器人的安全半径，ρ₀表示静态障碍物的半径，x₀表示静态或动态障碍物的位置。其中||x_i[k]-x₀||₂≥ρ′在计算时需要凸优化为

x_i[k′]是一个位置序列包括其中表示在第k个时间点的预测界域，^sx_i[k′]表示机器人i在第k′个时间点的第s次迭代的结果。

步骤中的符号定义：表1

实施例2

在上述实施例基础上，本实施例以五个机器人为例阐述编队过程，具体实现如图2。

步骤1)各机器人检查是否到达目标位置，即的状态，如果为false则执行下一步，其中i＝1,2,3,4,5，比如本例i＝1；

步骤2)各机器人获取自己当前位置、速度以及上一个位置的加速度自己的目标位置stop_P_i,以及和同组其他机器人的相对位置position_i[n]并且初始化

步骤3)机器人i在当前时刻k₀探测周围环境，确定障碍物的位置，并且添加静态障碍物的相对位置到添加动态障碍物的相对位置到

步骤4)机器人i广播自己的障碍物集合给同组的其它机器人j，j＝2,3,4,5；

步骤5)机器人i判断是否收到了同组所有机器人发来的障碍物集合，即判断的状态，如果为false，则执行下一步，否则转到步骤8；

步骤6)机器人i接收来自同组机器人j发来的障碍物的相对坐标并把其当做自己遇到的障碍物分别添加和到然后记录数m₁＝m₁+1；

步骤7)如果机器人i接收了同组其它所有机器人的广播，此时m₁＝4，则使并且执行下一步，否则转到步骤5；

步骤8)机器人i根据stop_P_i和用iscp算法计算下一个时间点k₀+1的速度和位置及k₀时刻的加速度，如果计算成功则

步骤9)若则机器人i广播自己的计算状态给同组的其他机器人j；

步骤10)若即执行下一步，否则转到步骤13；

步骤11)机器人i接收机器人j发来的计算状态，置并且计数m₂＝m₂+1；

步骤12)若m₂＝4，即收到了同组所有其它机器人广播的状态，则令转到下一步，否则转到步骤11；

步骤13)机器人i把自己的计算结果送到执行器执行。

其中，上述步骤8中的iscp算法具体如下：

步骤1)根据前一个时间点的计算结果初始化初始位置序列⁰x_i(k₀+1:T_i),迭代次数s＝0,以及

步骤2)判断s是否满足s≤s_max，若满足则执行下一步，否则转到步骤13；

步骤3)初始化HasAdded＝false；

步骤4)判断k是否满足若满足则执行下一步，否则，跳到步骤9；

步骤5)判断k是否满足k∈PosiIndex，若满足则执行下一步，否则转到步骤7；

步骤6)碰撞避免的约束条件凸优化CVXConstr＝ApproxCVX(^sx_i[k],O)，将凸优化后的约束条件加到避碰约束条件集合CurConstr＝CurConstr∪CVXConstr，转到步骤9；

步骤7)判断HasAdded＝false和^sflag_new_i[k]＝true两个条件是否均满足，若满足则添加障碍物位置^sp_i[k]到集合O中并且执行下一步，否则转到步骤9；

步骤8)HasAdded＝true,PosiIndex＝PosiIndex∪{k},CVXConstr＝ApproxCVX(^sx_i[k-1],O),CVXConstr＝CurConstr∪CVXConstr；

步骤9)根据前边的结果构造数学规划CVX_P_i[k₀]＝Approx(P_i[k₀],CurConstr),并且求解该数学规划(aa,vv,xx)＝Solve(CVX_P_i[k₀]),利用计算结果当中的位置xx初始化下一次迭代初始位置序列^s+1x_i[k₀+1:T_i]＝xx；

步骤10)判断是否满足max||^s+1x_i[k]-^sx_i[k]||_∞≤∈，若满足执行下一步，否则转到步骤12，其中∈为精度；

步骤11)返回下一个时间点的信息a_i[k₀]＝aa[1],v_i[k₀+1]＝vv[1]，x_i[k₀+1]＝xx[1]，跳出本算法；

步骤12)迭代次数加一s＝s+1并且转到步骤2；

步骤13)报告错误，并采取紧急行动，例如紧急制动。

本例当中要实现的任务是：五个机器人一直保持矩形队形(其中一个在矩形的中心)并且各机器人的相对位置不变，适时避免静态和动态障碍物，从初始到达目标位置。

结合附图对算法的执行做具体的说明：

如图3所示，本例编号为1,2,3,4,5的五个机器人的初始位置分别为(1.5,3)，(2.5,3)，(1.5,2)，(2.5,2)，(2,2.5)，5号机器人的目标位置为O_bj＝(8,8)。运行环境中存在三个障碍物，分别是两个静态障碍物O₁＝(3,4.3)，O₂＝(3.8,3.2)和一个动态障碍物O₃＝(5,3.8)，动态障碍物会在运行时显示。五个机器人需要保持现在的队形避过环境当中的所有障碍物到达目标位置。

在本例当中，整个任务等时间离散为50个时间点，时间步长为0.1，一次可以规划10个时间点的路径，传感半径为0.65，最大速度为5，机器人的安全半径为0.3(此处把障碍物看做一个点，障碍物的半径加到机器人的安全半径)，在各个时间点上机器人和相对应的障碍物分别为：表2

时间点	机器人(遇到的障碍物)
		14～17	4(O<sub>2</sub>)
18～20	1(O<sub>1</sub>),4(O<sub>2</sub>)
		21～24	1(O<sub>1</sub>),4(O<sub>2</sub>),5(O<sub>2</sub>)
25～26	1(O<sub>1</sub>),3(O<sub>2</sub>),5(O<sub>2</sub>)
		27～28	3(O<sub>2</sub>)

本发明中机器人运动时会把整个过程等时间离散(比如50个时间点到达目标位置)，机器人在每个时间点上会进行探测，并根据探测到障碍物的位置用数学规划进行传感器范围内(比如10个时间点)的路径规划，但是在执行的时候只执行一个时间点的结果，通过这种方式就可以保证机器人适时规划路径，并且可以保证是最优的路径。

在编队的时候，同组的机器人之间互相通信，并且在运动的过程中，每个机器人根据自己适时位置建立相对坐标系保证规划和运动的一致性。如果某一个机器人甲遇到障碍物，甲会把障碍物和自己的相对位置广播给同组其他机器人乙，乙接收到障碍物相对坐标后，会把此相对坐标当做一个障碍物处理，实际上乙没有遇到障碍物，只是因为甲的广播乙自己虚拟出一个障碍物，当乙遇到障碍物时也是同样的方法。这种方法保证了同组机器人在每一个点上的数学规划是一样的，用同样的数学规划方法同样的精度，最后的结果是一样的，即每个机器人以当前位置为参考点，下一个位置的加速度、速度、方向和位置是一样的，如图2。

本发明是适时探测周围环境的，所以不需要提前知道全局环境，克服了集中控制存在的缺陷；同组机器人之间有通信，并且用相对坐标系做数学规划，各个机器人之间会绝对保证相对位置不变，避免机器人之间的碰撞；采用分布式的方法代替Leader-Follower的方法可以保证任何机器人遇到障碍物队形都不会乱。

Claims (3)

1.一种实时的完全分布式的多机器人系统编队的方法,其特征是：至少包括如下步骤：

步骤1)若则执行下一步，否则跳出本算法；

步骤2)获取stop_P_i,和position_i[n]，并且初始化

步骤3)机器人i判断在当前时刻是否可以探测到静态或者动态障碍物，若有，则添加静态障碍物相对位置坐标到添加动态障碍物相对位置坐标到

步骤4)机器人i广播自己的障碍物集合给同组的其他机器人；

步骤5)若执行下一步，否则跳到步骤8；

步骤6)机器人i接收机器人j发来的虚拟障碍物的相对坐标分别添加到并且m₁＝m₁+1；

步骤7)若m₁＝n-1,则并且执行下一步，否则转到步骤6；

步骤8)机器人i根据stop_P_i和用iscp算法计算下一个时间点k₀+1的速度和位置及k₀时刻的加速度，如果计算成功则

步骤9)若则机器人i广播自己的计算状态给同组的其他机器人；

步骤10)若执行下一步，否则转到步骤13；

步骤11)机器人i接收机器人j发来的计算状态，置并且m₂＝m₂+1；

步骤12)若m₂＝n-1，则转到下一步，否则转到步骤11；

步骤13)机器人i把自己的计算结果送到执行器执行。

步骤中的符号定义：

i,j,n 系统中有n个机器人，i,j分别表示第i,j个机器人，1≤i,j≤n

机器人i在k₀时刻的位置

机器人i在k₀时刻的速度

机器人i在k₀时刻的加速度

stop_P_i 机器人i的目标位置

position_i[n] 机器人i与同一个系统中其他机器人的相对位置

机器人i在k₀时刻是否到达终点

机器人i在k₀时刻是否结束了当前的iscp计算

机器人i在k₀时刻是否接收了同组所有机器人广播的障碍物

机器人i在k₀时刻是否接收了同组所有机器人计算状态的广播

记录机器人i在k₀时刻记录计算结束的机器人的集合

机器人i在k₀时刻探测到的静态和动态障碍物(以当前位置建立坐标系的坐标)

在k₀时刻中机器人i要处理的静态障碍物

在k₀时刻中机器人i要处理的动态障碍物

s,s_max s表示迭代次数，s_max表示允许最大的迭代次数

^sx_i(k₀+1:T_i), 机器人i在第s次迭代中规划的从k₀+1到T_i时刻的位置坐标，k₀表示当前的时刻，T_i表示在整个规划当中离散的时间点总数

机器人i在k₀时刻可以规划的最大时间点

PosiIndex 在k₀时中被添加约束条件的时间点的集合

CurConstr 第s次迭代当中内各时间点添加的避碰约束条件集合

HasAdded 第s次迭代中是否有新的时间点被添加到PosiIndex

ApproxCVX(^sx_i[k],O) 对机器人i在第s次迭代中的第k个时间点的避碰约束条件进行凸优化，其中O表示其在第k个时间点探测到障碍物的集合

^sflag_new_i[k] 机器人i在第s次迭代中第k个时间点是否发现新的障碍物

^sp_i[k] 机器人i在第s次迭代中第k个时间点新添加的障碍物

Approx(P_i[k₀], 根据约束条件CurConstr构造机器人在k₀时间点的

CurConstr) 数学规划模型P_i[k₀]

Solve(CVX_P_i[k₀]) 求解数学规划P_i[k₀]，返回探测范围内加速度、速度和位置序列。

2.根据权利要求1所述的一种实时的完全分布式的多机器人系统编队的方法,其特征是：步骤8)具体包括如下步骤：

步骤1)根据前一个时间点的计算结果初始化初始位置序列⁰x_i(k₀+1:T_i),迭代次数s＝0,以及

步骤2)判断s是否满足s≤s_max，若满足则执行下一步，否则转到步骤13；

步骤3)初始化HasAdded＝false；

步骤4)判断k是否满足若满足则执行下一步，否则，跳到步骤9；

步骤5)判断k是否满足k∈PosiIndex，若满足则执行下一步，否则转到步骤7；

步骤6)碰撞避免的约束条件凸优化CVXConstr＝ApproxCVX(^sx_i[k],O)，将凸优化后的约束条件加到避碰约束条件集合CurConstr＝CurConstr∪CVXConstr，转到步骤9；

步骤7)判断HasAdded＝false和^sflag_new_i[k]＝true两个条件是否均满足，若满足则添加障碍物位置^sp_i[k]到集合O中并且执行下一步，否则转到步骤9；

步骤8)HasAdded＝true,PosiIndex＝PosiIndex∪{k},CVXConstr＝ApproxCVX(^sx_i[k-1],O),CVXConstr＝CurConstr∪CVXConstr；

步骤9)根据前边的结果构造数学规划CVX_P_i[k₀]＝Approx(P_i[k₀],CurConstr),并且求解该数学规划(aa,vv,xx)＝Solve(CVX_P_i[k₀]),利用计算结果当中的位置xx初始化下一次迭代初始位置序列^s+1x_i[k₀+1:T_i]＝xx；

步骤10)判断是否满足max||^s+1x_i[k]-^sx_i[k]||_∞≤∈，若满足执行下一步，否则转到步骤12，其中∈为精度；

步骤11)返回下一个时间点的信息a_i[k₀]＝aa[1],v_i[k₀+1]＝vv[1]，x_i[k₀+1]＝xx[1]，跳出本算法；

步骤12)迭代次数加一s＝s+1并且转到步骤2；

步骤13)报告错误，并采取紧急行动，例如紧急制动。

步骤中的符号定义：

i,j,n 系统中有n个机器人，i,j分别表示第i,j个机器人，1≤i,j≤n

机器人i在k₀时刻的位置

机器人i在k₀时刻的速度

机器人i在k₀时刻的加速度

stop_P_i 机器人i的目标位置

position_i[n] 机器人i与同一个系统中其他机器人的相对位置

机器人i在k₀时刻是否到达终点

机器人i在k₀时刻是否结束了当前的iscp计算

机器人i在k₀时刻是否接收了同组所有机器人广播的障碍物

机器人i在k₀时刻是否接收了同组所有机器人计算状态的广播

记录机器人i在k₀时刻记录计算结束的机器人的集合

机器人i在k₀时刻探测到的静态和动态障碍物(以当前位置建立坐标系的坐标)

在k₀时刻中机器人i要处理的静态障碍物

在k₀时刻中机器人i要处理的动态障碍物

s,s_max s表示迭代次数，s_max表示允许最大的迭代次数

^sx_i(k₀+1:T_i), 机器人i在第s次迭代中规划的从k₀+1到T_i时刻的位置坐标，k₀表示当前的时刻，T_i表示在整个规划当中离散的时间点总数

机器人i在k₀时刻可以规划的最大时间点

PosiIndex 在k₀时中被添加约束条件的时间点的集合

CurConstr 第s次迭代当中内各时间点添加的避碰约束条件集合

HasAdded 第s次迭代中是否有新的时间点被添加到PosiIndex

ApproxCVX(^sx_i[k],O) 对机器人i在第s次迭代中的第k个时间点的避碰约束条件进行凸优化，其中O表示其在第k个时间点探测到障碍物的集合

^sflag_new_i[k] 机器人i在第s次迭代中第k个时间点是否发现新的障碍物

^sp_i[k] 机器人i在第s次迭代中第k个时间点新添加的障碍物

Approx(P_i[k₀], 根据约束条件CurConstr构造机器人在k₀时间点的

CurConstr) 数学规划模型P_i[k₀]

Solve(CVX_P_i[k₀]) 求解数学规划P_i[k₀]，返回探测范围内加速度、速度和位置序列。

3.根据权利要求1所述的一种实时的完全分布式的多机器人系统编队的方法,其特征是：前述步骤9当中的数学规划模型是：

subject to：

v_i[k+1]＝v_i[k]+a_i[k]h_i

v_i[k]∈[v_min,v_max],a_i[k]∈[a_min,a_max]

v_i[T_i]＝0

||x_i[k]-x₀||₂≥ρ′

其中x_i[k]、v_i[k]、a_i[k]分别表示机器人i在k时刻的位置、速度和加速度，h_i表示离散的时间步长，表示机器人i的目标位置，||·||₂表示二范数，对于静态障碍物ρ′＝ρ+ρ₀，对于动态障碍物ρ′＝2ρ，其中ρ表示机器人的安全半径，ρ₀表示静态障碍物的半径，x₀表示静态或动态障碍物的位置。其中||x_i[k]-x₀||₂≥ρ′在计算时需要凸优化为

x_i[k′]是一个位置序列包括其中表示在第k个时间点的预测界域，^sx_i[k′]表示机器人i在第k′个时间点的第s次迭代的结果。