一种基于障碍图和势场法的人机协同编队跟随及避障方法与流程

本发明属于机器人控制领域，尤其涉及一种协同编队跟随控制方法，将障碍图模型和势函数应用到有人/无人平台编队跟随及避障任务当中。

背景技术：

有人/无人跟随控制模式是无人平台配合有人平台作战中的一环，即通过有人平台对战场态势的分析，决定将要执行的任务并传达给无人平台，无人平台能够自主地集结在一起，并形成一个指定的队形，跟随有人平台共同完成任务。在无人平台自主跟随的过程中，编队和避障是其必需的两项基本功能。

传统的无人平台编队控制技术是各个无人平台以队伍虚拟领导者的轨迹为基准，按固定角度和距离生成各自的轨迹，从而形成队形。其缺点有二，首先队形参数需要事先设定，无法根据实际的避障情况合理进行调整；其次当部分无人平台损坏时，剩下的无人平台仍按原定队形参数进行编队，无法自主形成新的队形。而传统的无人平台避障控制技术通常采用势场法，以无人平台和障碍物为中心建立势场函数，通过两者之间的排斥力达到避障目的。其缺点是无人平台行进过程中可能陷入局部极小值点。

一种新的思路是利用势函数兼顾编队和避障控制器的设计。R.Olfati Saber教授及Richard M.Murray教授(Saber,R.O.,Murray,R.M.Flocking with obstacle avoidance:cooperation with limited communication in mobile networks[C].Decision and Control,2003.Proceedings.42nd IEEE Conference on.IEEE,2003,2:2022-2028.)提出了一种基于势函数的群集和避障设计思路。对于群集，用一个U形势函数，在无人平台彼此间的感应范围内，当靠得太近时相互排斥,离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。对于避障,用一般的排斥势函数，在无人平台相对于障碍的感应范围内，当靠得太近时相互排斥，从而达到避障的目的。但这一控制方法并不能有效地解决势场法固有的局部极小值问题，因此需要设计一个虚拟领导者，并对其进行轨迹规划，使其能够将队伍引开局部极值点。文献(姚立强,宋艳荣,张术东.带有领导者的多智能体避障队形控制[J].烟台大学学报:自然科学与工程版,2014,27(2):130-135.)和文献(罗小元,刘丹.基于势函数的多智能体群集与避障[C].Proceedings of the 29th Chinese Control Conference.2010,20:l.)研究了上述基于势函数的群集和避障设计思路，并简化了所用的势函数，但由于其虚拟领导者并未进行路径规划，因此仍存在局部极小值问题。

对于群集中的路径规划，文献(Zhou Z.W.,Zhou B.,Zhao X.Formation control for Multi robots based on improved artificial potential field[J].Technology Innovation and Application,2015,33:44-45)提出一种“基于队形变换的沿墙导航法”解决了局部极值问题，但这种遍历求解路径方法效率不高。文献(Jia Q,Wang X.An improved potential field method for path planning[C].2010Chinese Control and Decision Conference.IEEE,2010:2265-2270.)则通过实时改变目标势场的位置解决了局部极值问题，但如何选定新的目标势场位置又是一个待解决的问题。

技术实现要素：

为解决上述问题，本发明针对目前使用势函数存在局部极值点并需要进行轨迹规划的问题，提供一种基于障碍图模型和势函数的有人/无人平台编队跟随及避障方法，解决了在一个复杂环境下无人平台编队跟随有人平台前往预定地点，行进中无人平台自主形成队形并调节队形位置对未知障碍进行规避的控制问题。本发明实现了有人平台对实际障碍环境进行图模型构建和联体结构识别，并据此规划虚拟领导者的无碰路径，以及无人平台跟随虚拟领导者自主形成队形，并对检测到的障碍加以规避。本发明中编队及避障方法采用势场法，原理类似于异种电荷之间相互吸引，同种电荷之间相互排斥。对于编队，在每个无人平台中心建立势函数，使得在近距离内无人平台之间有数值上的排斥力，而远距离内又有数值上的吸引力。无人平台彼此间靠得太近时相互排斥，离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。对于避障，在障碍中心建立势函数，使得障碍对无人平台有数值上的排斥力，无人平台与障碍靠得太近时受到排斥，从而达到避障的目的。同时，避障过程不破坏无人平台自身的预定任务，即无人平台在避障后仍能自主编队跟随有人平台。

一种基于障碍图和势场法的人机协同编队跟随及避障方法，包括以下步骤：

步骤1：构建障碍图模型，具体包括以下步骤：

步骤11：假设实际环境中障碍总数为s，无人平台总数为n；

步骤12：假设r′为检测半径，无人平台检测以自身中心点为圆心，检测半径范围内的障碍，并将障碍位置和半径信息传递给有人平台；

步骤13：有人平台根据障碍位置和半径信息后构建障碍图模型；

所述障碍图模型构建方法为：每个障碍都作为障碍图的一个节点，若两个障碍之间的距离小于或等于最小阈值r_safe，无人平台无法从这两个障碍之间通过，且对应障碍图的两个节点之间有一条边；其中最小阈值r_safe的确定方法为：任意两个障碍的半径之和再加上无人平台的最大直径；

步骤2：识别障碍图模型的结构，遍历搜索各个节点，找到直接或间接与之相连的所有节点，依次确定障碍图模型中的由相连节点构成的各个联体结构；

步骤3：利用可视图化法规划虚拟领导者的路径，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：

1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；

2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：

步骤31：将虚拟领导者与最相近联体结构最外围的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；

步骤32：在各个切线节点与有人平台之间做虚线，其中：

如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；

如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最外围的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；

步骤33：重复步骤32，直到得到所有满足虚拟领导者无碰连接到有人平台的折线段路径；

步骤34：选取最短的一条折线段路径，作为虚拟领导者的前进路径；

步骤4：构建无人平台之间的邻接矩阵A以及无人平台与障碍之间的邻接矩阵B；具体包括以下步骤：

步骤41：假设通信半径为r，无人平台能够与以自身中心点为圆心，通信半径范围内的其他无人平台进行相互通信；

无人平台之间邻接矩阵A＝[a_ij]∈R^n×n，其中a_ij为邻接矩阵A的第(i,j)个元素，i≠j，R为实数；同时：

如果任意两个无人平台i,j中心点位置之间的距离小于或等于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值a_ij为1；

如果任意两个无人平台i,j中心点位置之间的距离大于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值a_ij为0；

步骤42：无人平台与障碍之间邻接矩阵B＝[b_ik]∈R^n×s，其中b_ik为邻接矩阵B的第(i,k)个元素，同时：

如果任意无人平台i中心点位置与障碍k中心点位置之间的距离小于或等于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值b_ik为1；

如果任意无人平台i中心点位置与障碍k中心点位置之间的距离大于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值b_ik为0；

步骤5：根据无人平台之间的距离构建编队势场函数φ₁(||q_a,i-q_a,j||)以及根据无人平台与障碍之间的距离构建避障势场函数φ₂(||q_a,i-q_o,k||)；其中q_a,i和q_a,j分别表示无人平台i和无人平台j的中心点位置，q_o,k为障碍k的中心点位置；

步骤6：构建编队、跟随以及避障三个行为的控制量并将其进行叠加；

根据无人平台之间的邻接矩阵A、无人平台与障碍之间的邻接矩阵B、编队势场函数φ₁(||q_a,i-q_a,j||)以及避障势场函数φ₂(||q_a,i-q_o,k||)构建各个无人平台的控制量u_i：

u_i＝f_i^α+f_i^β+f_i^γ

其中，f_i^α为无人平台编队的控制量，f_i^β为无人平台避障的控制量，f_i^γ为无人平台跟随虚拟领导者的反馈控制量，具体的：

f_i^α＝-Σa_ij[φ₁(||q_a,i-q_a,j||)h(||q_a,i-q_a,j||)(q_a,i-q_a,j)+γ(p_a,i-p_a,j)]

f_i^β＝-∑b_ik[φ₂(||q_a,i-q_o,k||)h(||q_a,i-q_o,k||)(q_a,i-q_o,k)+βp_a,i]

f_i^γ＝-α[(q_a,i-q_r)+γ(p_a,i-p_r)]

其中，p_r中v_r、θ_r分别为虚拟领导者的速率和方向，q_r为虚拟领导者的位置，为调节函数，p_a,i和p_a,j分别为无人平台i、无人平台j的速度，α,β,γ,ε＞0为控制参数；将u_i作为第i个无人平台的控制量输入，实现有人/无人平台编队、跟随及避障任务。

步骤3所述的虚拟领导者的前进路径表征为与时间t相关的位置函数q_path(t)和方向函数θ_path(t)，并由近及远分为三个区域：弹道区、控制区以及死区；其中死区的中心为虚拟领导者的目标位置；

所述控制区和死区的半径根据有人平台速度、无人平台的最大速度和实际速度以及有人平台和无人平台的实际尺寸来确定；

有人平台的位置、速率和方向与时间t相关，分别为q_h(t)、v_h(t)和θ_h(t)；虚拟领导者的位置、速率和方向分别为q_r、v_r和θ_r；

1)当无人平台未检测到障碍时，

2)当无人平台检测到障碍时，虚拟领导者的位置q_r为当前时刻前进路径的位置q_path(t)，虚拟领导者的方向θ_r为当前时刻前进路径的方向θ_path(t)，同时：

如果虚拟领导者处于弹道区，则虚拟领导者以无人平台的最大速度v_max前进；

如果虚拟领导者处于控制区，则虚拟领导者将根据由外到内的距离远近降低自身的速度；

如果虚拟领导者处于死区，则虚拟领导者以有人平台的速度v_h(t)前进。

步骤5所述的编队势场函数φ₁(||q_a,i-q_a,j||)满足如下条件：

(a)在||q_a,i-q_a,j||∈(2r_a,max,d)上，φ₁(||q_a,i-q_a,j||)单调递减，在||q_a,i-q_a,j||∈(d,∞)上，φ₁(||q_a,i-q_a,j||)单调递增；且在这两个区间里φ₁(||q_a,i-q_a,j||)可微且非负；

(b)当||q_a,i-q_a,j||→2r_a,max时，φ₁(||q_a,i-q_a,j||)→∞；当||q_a,i-q_a,j||＝d时，φ₁(||q_a,i-q_a,j||)取得唯一的最小值；

其中，r_a,max为无人平台的最大半径，d表示在无人平台彼此间的通信半径r内，无人平台达到群集状态时的相邻两个无人平台之间的距离；

所述群集状态为任意两个无人平台之间的距离趋于定值。

步骤5所述的避障势场函数φ₂(||q_a,i-q_o,k||)满足如下条件：

(a)在||q_a,i-q_o,k||∈(r_a,max+r_o,k,∞)上，φ₂(||q_a,i-q_o,k||)单调递减；

(b)当||q_a,i-q_o,k||→r_a,max+r_o,k时，φ₂(||q_a,i-q_o,j||)→∞；

(c)避障势场函数φ₂(||q_a,i-q_o,k||)为一个关于||q_a,i-q_o,k||可微、非负、无界的函数；

其中，r_o,k为障碍k的半径。

有益效果：

1、本发明在势函数群集和避障设计思路的基础上，增加了利用可视图法规划参考轨迹的环节，有效的解决了传统势场法中固有的局部极值点，即无人平台会在吸引力和排斥力共同作用下卡在两个障碍之间的问题。

2、本发明相较于传统可视图路径规划方法，增加了障碍环境的图建模环节，使得后续的结构识别过程变得简便，而且得到的障碍联体结构类似于规则多边形，满足了传统可视图法对障碍的几何环境要求以及障碍之间有足够的无碰空间要求。

3、相较于传统编队跟随方法，本发明无需事先设定队形参数和参考轨迹，队列形成的同时兼顾避障任务，且部分无人平台损坏不影响队形的形成。

附图说明

图1为本发明原始障碍环境示意图；

图2为本发明障碍图模型示意图；

图3为本发明环境区域划分示意图；

图4为本发明路径规划方法示意图；

图5为本发明编队势函数示意图；

图6为本发明避障势函数示意图；

图7为本发明控制器控制流程示意图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明：

本发明组建了包含三个Pioneer 3-AT与一个上位机的实验平台。其中Pioneer3-AT(包括车载笔记本)作为无人平台，可以获取自身的位置速度信息，以及通过传感器检测障碍位置的能力，且有与邻居之间通信的能力。上位机作为有人平台，可以获得人的位置速度信息，并有与三个无人平台通信的能力。上位机程序以及车载程序均由C++语言写成。无人平台通过传感器检测障碍，并将障碍位置发给有人平台。有人平台规划路径，并将虚拟领导者状态发给无人平台控制器，控制器结合虚拟领导者、自身及邻居状态和障碍位置计算控制量发送给无人平台执行器，最终使无人平台按照控制要求前进。控制方法示意图如图7所示。

实现本发明所述控制方法的步骤为：

步骤一：构建障碍图模型

设实际环境中障碍物总数为s＝30，对于第k个(k＝1,2,...,30)障碍，位置为q_o,k，半径为r_o,k。设无人平台的最大半径为r_a,max＝0.4m。对于任两个障碍q_o,k和q_o,t无人平台能通过的最小阈值为r_safe＝r_o,k+r_o,t+2r_a,max。若障碍之间的距离小于r_safe，虽然有人平台可以从中通过，但无人平台会在吸引力和排斥力共同作用下卡在两个障碍之间，即出现局部极小值情况，无法从它们之间通过。因此，在这种情况下需要设计虚拟领导者代替有人平台领导无人平台绕过障碍。

设以无人平台中心点为圆心，半径为r′＝3m的范围内的障碍都能被无人平台检测到，并能够传给有人平台。由于有人平台带领无人平台从两个障碍之间通过时容易出现局部极小值情况，为了方便轨迹规划，要对障碍环境进行建模。本发明中有人平台搜集所有障碍信息后，需要以障碍物的间距是否小于无人平台能通过的最小阈值r_safe为条件形成一个无向图模型，即为障碍图模型。障碍图模型构建原则为：每个障碍都作为图的一个节点，若两个障碍之间距离||q_o,k-q_o,t||≤r_safe，则对应图的两个节点之间有一条边。图1为原始障碍环境示意图，图2为障碍图模型示意图，障碍图模型中距离小于r_safe障碍之间存在一条边。

步骤二：对障碍图进行结构识别

根据步骤一获得的障碍图模型，所有距离小于r_safe的障碍之间都存在一条边。本发明将障碍图模型中有边连接起来的障碍集合看作一个联体结构，联体之内无人平台无法通过，但联体和联体之间有足够的无碰空间供无人平台通过。

本发明将识别联体结构问题转化为搜索图中的全部连通分量问题，从某一个节点开始向下搜索，找到直接或间接与之相连的所有节点，即为一个联体结构，再从其他节点继续此步骤，可以很简便的获得障碍环境中的全部联体结构。如图1所示，原始障碍环境被分成图2中9个联体结构。对于C++语言来说，实现这一过程首先要构建链表，然后对链表进行深度优先遍历。

步骤三：可视图法规划虚拟领导者路径

本发明使用可视图法对虚拟领导者进行路径规划，设计所有可行路径，在其中选择最短的路径作为参考路径。障碍图中的联体结构类似于规则多边形，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：

1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；

2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：

步骤31：将虚拟领导者与最相近联体结构最外围的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；

步骤32：在各个切线节点与有人平台之间做虚线，其中：

如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；

如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最外围的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；

步骤33：重复步骤32，直到得到N条满足虚拟领导者无碰连接到有人平台的折线段路径；其中N与切线顶点的个数有关；

步骤34：取其中最短的一条，作为虚拟领导者的前进路径，路径节点位置q_path和方向θ_path为虚拟领导者的位置和方向。如图4所示，有4条折线段满足领导者无碰连接有人平台，分别为

1)l₁→l₂→l₃，

2)l₁→l₄→l₅→l₆→l₃，

3)l₁→l₄→l₇→l₈，

4)l₉→l₁₀→l₁₁。

其中，第1条折线段长度最短，因此取l₁→l₂→l₃为虚拟领导者的路径。若不进行虚拟领导者的路径规划，虚拟领导者就会按照图4所示虚线前进，此时无人平台极易卡在障碍之间，陷入局部极小值点。计算得到的路径信息可以表示为与时间t相关的两个函数——位置q_path(t)和方向θ_path(t)。

虚拟领导者的速度控制使用动态死区法，根据虚拟领导者和有人平台的相对距离来确定自己在环境中的位置。整个环境被分为三个区域：弹道区(Ballistic zone)、控制区(Control zone)和死区(Dead zone)，如图3所示，以上区域面积大到小，其中无人平台的速度由快到慢。死区的中心即虚拟领导者的目标位置，此处为区域而不是一个点。若虚拟领导者处于了弹道区(此区域非常远离有人平台的位置)，那么虚拟领导者会以最大速度前进，从而整个无人平台队形匀速前进；在控制区中时，虚拟领导者将根据外到内的距离远近降低它的速度，以更精确的进入死区，在死区中虚拟领导者的速度与有人平台的速度相同，即表现为整个无人平台队形跟上有人平台后以相同的速度前进。

有人平台的位置、速率和方向与时间t相关，分别为q_h(t)、v_h(t)和θ_h(t)。定义虚拟领导者位置、速率和朝向分别为q_r、v_r和θ_r，其值如下：

1)当无人平台未检测到障碍时，

2)当无人平台检测到障碍时，

其中，q_path(t)为当前时刻前进路径的位置，θ_path(t)为当前时刻前进路径的方向，v_max＝0.2m/s是本范例中无人平台的最大速度，R_control＝10m为控制区的半径，R_dead＝0.4m为死区的半径，k_r＝0.01为控制参数。在此法中控制区和死区的半径的选择时要考虑到有人平台速度，各个无人平台的最大速度和实际速度，还要考虑各平台的实际尺寸。死区的半径越小，无人平台跟踪有人平台时间越短，跟随效果越好。

步骤四：构建无人平台模型和邻接矩阵

设无人平台总数为n＝3，对于第i个(i＝1,2,3)无人平台，其控制模型如下：

其中，q_a,i、p_a,i代表第i个无人平台的位置和速度，代表第i个无人平台位置的导数和速度的导数，u_i代表第i个无人平台的控制量。

设无人平台自身半径为r_a＝0.4m，且以无人平台中心点为圆心，半径为r＝100m的范围内的无人平台都能相互通信。令||q_a,i-q_a,j||表示无人平台i和j之间的距离则无人平台之间邻接矩阵A＝[a_ij]∈R^n×n，定义为：

同理，a_ij为邻接矩阵A的第(i,j)个元素，R为实数，q_a,i和q_a,j分别表示无人平台i和无人平台j的中心点位置。

令||q_o,k-q_a,i||表示无人平台i和障碍k之间的距离则无人平台与障碍之间邻接矩阵B＝[b_ik]∈R^n×s定义为：

步骤五：构建编队势场函数和避障势场函数

对于编队，在无人平台彼此间的感应范围r＝100m内，当靠得太近时相互排斥，离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。用d＝1.6m表示达到群集时无人平台间希望达到的稳定距离，则势场函数值在每两个无人平台中心点之间的距离为d时达到最小值，此时编队完成。编队的势场函数定义如下：

定义1(编队的势场函数)令z＝||q_a,i-q_a,j||表示无人平台i和j之间的距离势场函数φ₁(z)必须满足：

(a)在z∈(2r_a,max,d)上，φ₁(z)单调递减，在z∈(d,∞)上，φ₁(z)单调递增。在这两区间里φ₁(z)可微且非负。

(b)当z→2r_a,max时，φ₁(z)→∞；当d_a＝d时，φ₁(z)取得唯一的最小值。

根据上述定义，建立无人平台之间的势场函数两个范例如下：

1)z∈(2r_a,max,∞)

其中，为φ₁(z)的导数，k₁＝10是调节变量。2)z∈(2r_a,max,∞)

其中，为φ₁(z)的导数，k₁＝10是调节变量。

范例1)的函数图像如图5所示，图像先下降后上升，在d＝1.6m处达到最小值，在2r_a,max＝0.8m处为无穷大。其中横坐标表示无人平台i和j之间的距离，纵坐标表示势场函数φ₁(z)的相对大小；

对于避障，在无人平台相对于障碍的检测范围r′内，当靠得太近时相互排斥，从而达到避障的目的。避障的势场函数定义如下:

定义2(避障的势场函数)令z′＝||q_a,i-q_o,k||表示无人平台i和障碍k之间的距离则势场函数φ₂(z′)必须是一个关于z′可微、非负、无界的函数，且满足：

(a)在z′∈(r_a,max+r_o,k,∞)上，φ₂(z′)单调递减。

(b)当z′→r_a,max+r_o,k时，φ₂(z′)→∞。

根据上述定义，建立无人平台与障碍之间的势场函数两个范例如下：

1)z′∈(r_a,max+r_o,k,∞)

其中，为φ₂(z′)的导数，k₂＝15是调节变量。

2)z′∈(r_a,max+r_o,k,∞)

其中，为φ₂(z′)的导数，k₂＝5是调节变量。

范例2)的函数图像如图6所示，图像一直单调减，在r_a,max+r_o,k＝0.5m处为无穷大。其中横坐标表示无人平台i和障碍k之间的距离，纵坐标表示势场函数φ₂(z′)的相大小；

步骤六：写出编队、跟随以及避障三个行为的控制律进行叠加

根据控制要求：①无人平台形成队列，且无人平台间不能发生碰撞；②无人平台整体追踪虚拟领导者,最终与之轨迹相同、速度一致；③无人平台实现避障。因此可以得到每个无人平台的控制量u_i(i＝1,2,3)，其为以上三部分行为的叠加。

u_i＝f_i^α+f_i^β+f_i^γ

其中，f_i^α为无人平台编队的控制量，f_i^β为无人平台避障的控制量，f_i^γ为跟随虚拟领导者的反馈控制量。

f_i^α＝-Σa_ij[φ₁(||q_a,i-q_a,j||)h(||q_a,i-q_a,j||)(q_a,i-q_a,j)+γ(p_a,i-p_a,j)]

f_i^β＝-∑b_ik[φ₂(||q_a,i-q_o,k||)h(||q_a,i-q_o,k||)(q_a,i-q_o,k)+βp_a,i]

f_i^γ＝-α[(q_a,i-q_r)+γ(p_a,i-p_r)]

其中，为调节函数，α,β,γ,ε＞0为控制参数，本范例取α＝2,β＝1,γ＝2,ε＝0.1，a_ij是无人平台所形成邻接矩阵A∈R^n×n的第(i,j)个元素b_ik是无人平台与障碍所形成邻接矩阵B∈R^n×s的第(i,k)个元素参数a_ij和b_ij决定无人平台之间以及无人平台与障碍之间是否产生交互。

可以看到，我们将最终的u_i作为第i个Pioneer 3-AT的控制输入，即可实现上述有人/无人平台编队跟随及避障任务。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。