本发明属于协同控制技术领域，具体涉及一种基于ctl的应用实时启发式算法的多机器人协同运动控制方法。
背景技术
随着科学技术的发展，移动机器人的应用越来越普遍，人们研制了可在各种环境下作业的移动机器人系统，如无人驾驶飞机(unmannedarielvehicle)，无人驾驶汽车(unmannedgroundvehicle)，无人潜航器(unmannedunderwatervehicle)等。同时，随着工业化的进展，诸如自动制造、柔性生产、搜索营救、环境监测、安全健康等许多领域面临着大量操作复杂、规模庞大的任务。因此，单一的机器人已经无法很好地完成这些任务。相比于单机器人系统，多机器人系统由于多个机器人的相互协作而具有一系列显著的优点，例如多机器人系统可以降低任务求解的复杂性、促进任务完成的高效性、增加系统的可靠性、简化系统的设计等。由于这些优越的特性，多机器人系统越来越受到人们的关注，吸引众多学者对其进行广泛而深入的研究。
使用形式化方法非常受益的一个机器人领域是路径规划。一般来说，通过形式化方法解决路径规划问题需要机器人工作空间的先验知识。时序逻辑如计算树逻辑(computationtreelogic，ctl)、连续随机逻辑(continuousstochasticlogic，csl)、线性时序逻辑(lineartemporallogic，ltl)已被有效地应用于表达复杂高级规划规范。
在过去，有许多研究学者研究了基于时序逻辑的多机器人运动规划问题，但仍存在一些缺陷，具体体现在：1.直接将目标写为非线性数学规划的形式，cvx、matlab等无法求解，借助smt、snopt等非线性数学规划求解器的方法，可能会出现求解时间长、线性求解不成功等问题；2.传统机器人的运动控制通常依赖于机器人的动力学方程(如速度、加速度、力矩等)，然而随着系统中机器人数量的增加，基于动力学方程的系统模型将变得高度耦合、高度复杂，从而使得系统不易求解；3.采用中心化控制会导致系统缺乏健壮性和可扩展性；4.大部分研究依靠于已知的全局环境信息，提出的离线路径规划算法，无法避免实时可能出现的障碍物。
技术实现要素：
本发明的目的在于提供一种多机器人协同运动控制方法，该控制方法是一种基于ctl的应用实时启发式算法的方法，克服现有技术中假设全局环境信息已知而提出的离线路径规划算法存在的问题。
本发明提供的技术方案如下：
一种多机器人协同运动控制方法，用二维网格图表示机器人工作区，将工作区分解为块，通过协同控制算法和实时启发式算法，使每个机器人均能找到总花费最少、满足总ctl且可运动到目标位置的下一位置，实时避免静态障碍物，通过协同运动到达目标位置；
其中，总花费通过实时启发式算法得到，总ctl通过采用机器人的通信和栅格法实现多机器人的协同运动的协同控制算法来满足，i为机器人的编号。
所述协同控制算法包括以下步骤：
步骤1)初始化starti，close_listi，is_endi＝false，其中，starti表示机器人i的当前位置，close_listi表示机器人i不再检索的位置点，is_endi表示机器人i是否到达目标位置，i＝1，2，3，4，......，n；
步骤2)若is_endi＝false，则继续执行以下步骤，若is_endi＝true则跳至步骤8)；
步骤3)判断是否实时添加静态障碍物，若添加，则将静态障碍物位置加入obsrealtime；obsrealtime为实时添加的静态障碍物位置集合；
步骤4)将各机器人当前位置starti的周围八个点坐标，分别按照启发式算法计算花费后从小到大排序添加进neari，排序结果为neari[h]，h＝1，2，……，7，8；near1[h]代表机器人1当前位置start1的周围八个点坐标；
步骤5)机器人1找到花费最优且可运动到该点的邻近点near1[h]，若邻近点near1[h]为机器人1的目标位置，则is_end1＝true，否则is_end1＝false；
步骤6)若机器人2至机器人n均找到花费最少、满足ctl1i且可运动到该点的邻近点neari[h]，则执行步骤7)，若机器人2至机器人n任一机器人找不到花费最少、满足ctl1i且可运动到该点的邻近点neari[h]，则此时机器人1不可运动到步骤5)中找到的点neari[h]，返回步骤5)重新执行；其中，ctl1i为机器人1与机器人i之间的计算树逻辑；
步骤7)各机器人运动至满足总ctl且花费最少的下一步邻近点，更新各机器人起始状态，将各机器人比该步花费更高且不为终点的其他步neari放入close_listi中，若neari[h]为机器人i的目标位置，则is_endi＝true，跳至步骤8)，否则is_endi＝false，回到步骤2)继续执行，i＝1，2，3，4，......，n；
步骤8)各机器人到达目标位置。
所述实时启发式算法包括以下步骤：
1)若运动元为u或d或l或r，则从当前位置到下一状态位置的花费g定义为10；其中，u、d、l、r分别代表向上、向下、向左、向右运动一个栅格；
2)若运动元为ru或rd或ld或lu，则从当前位置到下一状态位置的花费g定义为14；其中，ru、rd、ld、lu分别代表向右上、向右下、向左下、向左上运动一个栅格；
3)得到从下一状态位置至目标位置的预计花费h；
4)从当前位置到目标位置的总花费f定义为f＝g+h；
5)根据f值，对neari从小到大排序。
当n＝4时，总ctl公式为：
其中，obs1为静态障碍物1的位置；r1f为机器人1的终止位置；r1·row为机器人1的横坐标值；r1·col为机器人1的纵坐标值；m表示两个机器人之间的横坐标或纵坐标的距离为m个单位。
工作区的每个块都被分配一个唯一的标识符，块的标识符为id＝(ix,iy),其中ix和iy是非负整数，相邻块的标识符是通过将相应组件的标识符id加或减1来获得的；ix代表栅格对应的横坐标，iy代表栅格对应的纵坐标。ctl1i＝(r1·row＝ri·row)∧(ri·col-r1·col＝m)，m表示两个机器人之间的横坐标或纵坐标的距离为m个单位。
从下一状态位置至目标位置的预计花费h通过下式得到：h＝abs(endi·row-neari·row)*10+abs(endi·col-neari·col)*10,i＝1,2,3,4,......，n；其中，neari·row表示机器人i下一个位置的横坐标值，neari·col表示机器人i下一个位置的纵坐标值，endi·row表示机器人i目标位置的横坐标值，endi·col表示机器人i目标位置的纵坐标值。
四个机器人始终满足矩形编队，实时避免静态障碍物，同时以最少的花费到达目标位置。
总ctl中的公式分为路径公式和状态公式。
与现有的技术相比，本发明的有益效果具体体现在：
1.无需借助求解器进行求解，本发明从栅格法和ctl、ltl的角度来考虑简化问题的求解，基于栅格的地图表示方法，即将整个环境分为若干相同大小的栅格；2.本发明实现了多机器人的协同运动，克服了中心化控制易导致系统缺乏健壮性和可扩展性的缺陷；3.本发明不需要提前已知全局环境信息，各机器人只能感知传感器范围内的环境信息和与通信范围内的机器人进行通信。
下面将结合附图做进一步详细说明。
附图说明
图1是机器人传感器探测范围示意图；
图2是环境信息示意图。
具体实施方式
实施例1：
现有技术的大部分研究都假设全局环境信息已知，然而在实际系统中，一般机器人只能知道传感器范围内的环境信息，且环境中随时有可能新增静态障碍物。因此，已知全局环境信息而提出的离线路径规划算法并不适用于实际系统。
为解决现有技术中存在的问题，本实施例提供了一种多机器人协同运动控制方法，用二维网格图表示机器人工作区，将工作区分解为块，通过协同控制算法和实时启发式算法，使每个机器人均能找到总花费最少、满足总ctl且可运动到目标位置的下一位置，实时避免静态障碍物，通过协同运动到达目标位置；
其中，总花费通过实时启发式算法得到，总ctl通过采用机器人的通信和栅格法实现多机器人的协同运动的协同控制算法来满足，i为机器人的编号。
工作区即全局环境信息。工作区被表示为二维网格图，使用统一网格将工作区分解为块。通过每个维度的块数指定工作空间的大小。每个块都被分配一个唯一的标识符，左上方块的标识符为(0,0)。
本发明无需借助求解器进行求解，从栅格法和ctl、ltl的角度来考虑简化问题的求解，基于栅格的地图表示方法，即将整个环境分为若干相同大小的栅格。不需要提前已知全局环境信息，各机器人只能感知传感器范围内的环境信息和与通信范围内的机器人进行通信。克服了中心化控制易导致系统缺乏健壮性和可扩展性的缺陷，实现了多机器人的协同运动。
实施例2：
在实施例1的基础上，本实施例提供了一种多机器人协同运动控制方法，所述协同控制算法包括以下步骤：
步骤1)初始化starti，close_listi，is_endi＝false，其中，starti表示机器人i的当前位置，close_listi表示机器人i不再检索的位置点，is_endi表示机器人i是否到达目标位置，i＝1，2，3，4，......，n；
步骤2)若is_endi＝false，则继续执行以下步骤，若is_endi＝true则跳至步骤8)；
步骤3)判断是否实时添加静态障碍物，若添加，则将静态障碍物位置加入obsrealtime；obsrealtime为实时添加的静态障碍物位置集合；
步骤4)将各机器人当前位置starti的周围八个点坐标，分别按照启发式算法计算花费后从小到大排序添加进neari，排序结果为neari[h]，h＝1，2，……，7，8；near1[h]代表机器人1当前位置start1的周围八个点坐标；
步骤5)机器人1找到花费最优且可运动到该点的邻近点near1[h]，若邻近点near1[h]为机器人1的目标位置，则is_end1＝true，否则is_end1＝false；
步骤6)若机器人2至机器人n均找到花费最少、满足ctl1i且可运动到该点的邻近点neari[h]，则执行步骤7)，若机器人2至机器人n任一机器人找不到花费最少、满足ctl1i且可运动到该点的邻近点neari[h]，则此时机器人1不可运动到步骤5)中找到的点neari[h]，返回步骤5)重新执行；其中，ctl1i为机器人1与机器人i之间的计算树逻辑；
步骤7)各机器人运动至满足总ctl且花费最少的下一步邻近点，更新各机器人起始状态，将各机器人比该步花费更高且不为终点的其他步neari放入close_listi中，若neari[h]为机器人i的目标位置，则is_endi＝true，跳至步骤8)，否则is_endi＝false，回到步骤2)继续执行，i＝1，2，3，4，......，n；
步骤8)各机器人到达目标位置。
所述实时启发式算法包括以下步骤：
步骤1)若运动元为u或d或l或r，则从当前位置到下一状态位置的花费g定义为10；其中，u、d、l、r分别代表向上、向下、向左、向右运动一个栅格；
步骤2)若运动元为ru或rd或ld或lu，则从当前位置到下一状态位置的花费g定义为14；其中，ru、rd、ld、lu分别代表向右上、向右下、向左下、向左上运动一个栅格；
步骤3)得到从下一状态位置至目标位置的预计花费h，估算h值的方法采用曼哈顿方法，计算从当前格到目的格之间水平和垂直的方格的数量总和，忽略对角线方向和一切障碍物，然后将结果乘以10；
步骤4)从当前位置到目标位置的总花费f定义为f＝g+h；
步骤5)根据f值，对neari从小到大排序。
实施例3：
在前述实施例的基础上，本实施例以四个机器人的协同运动作为示例进行陈述。
协同控制算法具体步骤如下：
步骤1)初始化starti，close_listi，is_endi＝false，其中，starti表示机器人i的当前位置，close_listi表示机器人i不再检索的位置点，is_endi表示机器人i是否到达目标位置，i＝1，2，3，4；；
步骤2)若is_endi＝false，则继续执行以下步骤，若is_endi＝true则跳至步骤10；
步骤3)判断是否实时添加静态障碍物，若添加，则将静态障碍物位置加入obsrealtime；obsrealtime为实时添加的静态障碍物位置集合；
步骤4)将各机器人当前位置starti的周围八个点坐标，分别按照启发式算法计算花费后从小到大排序添加进neari，排序结果为neari[h]，h＝1，2，……，7，8；near1[h]代表机器人1当前位置start1的周围八个点坐标；
步骤5)机器人1找到花费最优且可运动到该点的邻近点near1[h]，若邻近点near1[h]为机器人1的目标位置，则is_end1＝true，否则is_end1＝false；
步骤6)若机器人2找到花费最少、满足ctl12且可运动到该点的邻近点near2[h]，则执行步骤7)，若机器人2找不到花费最少、满足ctl12且可运动到该点的邻近点near2[h]，则此时机器人1不可运动到步骤5)中找到的点near1[h]，返回步骤5)重新执行；
步骤7)若机器人3找到花费最少、满足ctl13且可运动到该点的邻近点near3[h]，则执行步骤8)，若机器人3找不到花费最少、满足ctl13且可运动到该点的邻近点near3[h]，则此时机器人1不可运动到步骤5)中找到的点near1[h]，返回步骤5)重新执行；
步骤8)若机器人4找到花费最少、满足ctl14且可运动到该点的邻近点near4[h]，则执行步骤9)，若机器人4找不到花费最少、满足ctl14且可运动到该点的邻近点near4[h]，则此时机器人1不可运动到步骤5)中找到的点near1[h]，返回步骤5)重新执行；
步骤9)各机器人运动至满足总ctl且花费最少的下一步，更新各机器人起始状态，将各机器人比该步花费更高且不为终点的其他步neari放入close_listi中，若neari[h]为机器人i的目标位置，则is_endi＝true,跳至步骤10)，否则is_endi＝false，回到步骤2)继续执行，i＝1，2，3，4；；
步骤10)各机器人到达目标位置。
其中，实时启发式算法包括以下步骤：
步骤1)若运动元为u或d或l或r，则从当前位置到下一状态位置的花费g定义为10；
步骤2)若运动元为ru或rd或ld或lu，则从当前位置到下一状态位置的花费g定义为14；
步骤3)从下一状态位置至终点位置的预计花费h，估算h值的方法采用曼哈顿方法，计算从当前格到目的格之间水平和垂直的方格的数量总和，忽略对角线方向和一切障碍物，然后将结果乘以10，定义为h＝abs(endi·row-neari·row)*10+abs(endi·col-neari·col)*10，i＝1，2，3，4；
步骤4)从当前位置到终点位置的总花费f定义为f＝g+h；
步骤5)根据f值，对neari从小到大排序。
机器人协同控制的任务是：四个机器人始终满足矩形编队，实时避免静态障碍物，同时以最少的花费最终到达目标位置。
上述任务可被转换为如下总ctl公式：
其中，2泛指两个机器人之间的横坐标或纵坐标的距离为2个单位，该值可根据需要进行修改。通过增加或减少机器人数量，改变两个机器人横纵坐标距离的方法，均属于本发明的保护范围。
协同控制算法中的ctl12、ctl13、ctl14是总ctl公式的一部分，分别如下所示：
ctl12＝(r1·row＝r2·row)∧(r2·col-r1·col＝2)
ctl13＝(r3·row-r1·row＝2)∧(r1·col＝r3·col)
ctl14＝(r4·row-r1·row＝2)∧(r4·col-r1·col＝2)
结合附图对算法执行的具体步骤详细说明如下：
如图1所示，ri表示机器人i的当前位置。在当前位置下，ri的传感器范围为p1～p24。
如图2所示，p18、p42、p20、p44分别代表机器人1～4的初始位置，p114、p138、p116、p140分别代表机器人1～4的终点位置，p66、p78分别代表静态障碍物的位置。在如图2所示的当前环境信息下，若不实时添加障碍物，则按照本发明提出的实时启发式算法，在满足总ctl以及最少花费的前提下，四个机器人的协同运动的运动路径如下分析。r1处于p18时，到p6、p17、p30、p19的实际花费g为10，到p5、p29、p31、p7的实际花费为14。例如，p17到p114的预估花费h为90，p18经过p17到p114的总花费f为100。对p18周围八个点排序后的顺序依次为：p30、p31、p29、p17、p19、p6、p5、p7，总花费依次为80、94、94、100、100、100、114、114。同理可算出r2、r3、r4的情况。r1求得的最优的下一步为p30，r2找不到与r1满足ctl12的下一步可行路径。与p30满足ctl12的p54的下一步p66∈obs，将p54压入close_list1中。r1改变方向，选择次优步为p31。r2找到与r1满足ctl12的下一步为p55，r3找到与r1满足ctl13的下一步为p33，r4找到与r1满足ctl14的下一步为p44。之后各步依次类推。
因此，根据算法所求得的各机器人路径如下所示。机器人1的运动路径为p18→p31→p43→p55→p67→p79→p91→p103→p114，机器人2的运动路径为p42→p55→p67→p79→p91→p103→p115→p127→p138，机器人3的运动路径为p20→p33→p45→p57→p69→p81→p93→p105→p116，机器人4的运动路径为p44→p57→p69→p81→p93→p105→p129→p140，各机器人最优花费均为88。
在如图2所示的当前环境信息下，若实时添加障碍物，则按照本发明提出的实时启发式算法，在满足总ctl以及最少花费的前提下，四个机器人的协同运动的运动路径如下所示。
机器人1的运动路径为：
p18→p31→p43(在p91、p103处放置障碍物)→p56→p68→p81→p93→p105→p117→p129→p128→p127→p114，
机器人2的运动路径为：
p42→p55→p67(在p91、p103处放置障碍物)→p80→p92→p105→p117→p129→p141→p153→p152→p151→p138，
机器人3的运动路径为：
p20→p33→p45(在p91、p103处放置障碍物)→p58→p70→p83→p95→p107→p119→p131→p130→p129→p116，
机器人4的运动路径为：
p44→p57→p69(在p91、p103处放置障碍物)→p82→p94→p107→p119→p131→p143→p155→p154→p153→p140。
各机器人最优花费均为136。实时添加障碍物时，不限于仅当r1、r2、r3、r4分别运动至p43、p67、p45、p69时，在p91、p103处放置障碍物，可实时在多机器人运动前方添加静态障碍物。
本发明中涉及的定义做如下说明：
定义1：工作区
工作区即全局环境信息。工作区被表示为二维网格图，使用统一网格将工作区分解为块。我们通过每个维度的块数指定工作空间的大小。每个块都被分配一个唯一的标识符，左上方块的标识符为(0，0)。如果块的标识符是id＝(ix，iy)，其中ix和iy是非负整数，相邻块的标识符是通过将相应组件的标识符id加或减1来获得的。
定义2：运动元(motionprimitive，简写为mp)
每个机器人在当前位置的下一步，有八个可能的运动元，分别是：u(向上运动一个栅格，定义花费为10)、d(向下运动一个栅格，定义花费为10)、l(向左运动一个栅格，定义花费为10)、r(向右运动一个栅格，定义花费为10)、ru(向右上运动一个栅格，定义花费为14)、rd(向右下运动一个栅格，定义花费为14)、ld(向左下运动一个栅格，定义花费为14)、lu(向左上运动一个栅格，定义花费为14)。
定义3：ltl公式
在有限的轨迹中，我们使用线性时序逻辑表示多机器人系统的行为规范。假设ii表示原子命题的集合，原子命题π∈ii，任何ltl公式都可以根据以下语法制定：
给出上面的语法，我们能够通过以下方式来定义false和true：
定义4：ltl语义
ltl公式是在公式中使用的命题的真值赋值的有限序列σ上定义的。用length(σ)表示序列的长度。令σ(i)表示在σ的第i个位置，当0≤i≤length(σ)-1时，原子命题的集合为真。
对于一个ltl公式
σ，i|＝πiffπ∈0(i)
如果
定义5：ltl与ctl
线性时序逻辑将时间看作线性的，即每个时刻系统只有一个可能的后继状态，因此，每个时刻只有唯一一个可能的将来。如果每个时刻可能有多个不同的将来，ltl则无法处理，因此引入ctl。
定义6：ctl公式
令π是原子命题，ctl中的公式分为路径公式和状态公式。路径公式根据以下语法制定：
状态公式根据以下语法制定：
路径量词e表示对每条路径，a表示对所有路径。
定义7：ctl语义
令π∈ii是原子命题，状态公式的可满足关系
s|＝πiffπ∈s
路径公式的可满足关系
定义8：无标志状态s
s＝m\(rii∨rif∨obsi∨obsrealtime)，m代表环境中的所有状态集合。is_endi为true时表示机器人i已按任务要求到达目标位置，i＝1，2，3，4，.....，n；meari[h]为机器人i当前位置的下一个位置，i＝1，2，3，4，.....，n，h＝1，2，3，4，.....，7，8；diag表示机器人对角移动时会穿过的两个位置。
其中，图1、图2中的栅格为正方形。
以上例举仅仅是对本发明的举例说明，并不构成对本发明的保护范围的限制，凡是与本发明相同或相似的设计均属于本发明的保护范围之内。实施例没有详细叙述的方法属本行业的公知技术，这里不一一叙述。