CN106483958A - 一种基于障碍图和势场法的人机协同编队跟随及避障方法 - Google Patents

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

Info

Publication number
CN106483958A
CN106483958A CN201610989474.2A CN201610989474A CN106483958A CN 106483958 A CN106483958 A CN 106483958A CN 201610989474 A CN201610989474 A CN 201610989474A CN 106483958 A CN106483958 A CN 106483958A
Authority
CN
China
Prior art keywords
platform
obstacle
unmanned platform
unmanned
virtual leader
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201610989474.2A
Other languages
English (en)
Other versions
CN106483958B (zh
Inventor
方浩
陈杰
吴楚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201610989474.2A priority Critical patent/CN106483958B/zh
Publication of CN106483958A publication Critical patent/CN106483958A/zh
Application granted granted Critical
Publication of CN106483958B publication Critical patent/CN106483958B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明针对目前使用势函数存在局部极值点并需要进行轨迹规划的问题，提供一种基于障碍图模型和势函数的有人/无人平台编队跟随及避障方法，解决了在一个复杂环境下无人平台编队跟随有人平台前往预定地点，行进中无人平台自主形成队形并调节队形位置对未知障碍进行规避的控制问题。本发明实现了有人平台对实际障碍环境进行图模型构建和联体结构识别，并据此规划虚拟领导者的无碰路径，以及无人平台跟随虚拟领导者自主形成队形，并对检测到的障碍加以规避。同时，避障过程不破坏无人平台自身的预定任务，即无人平台在避障后仍能自主编队跟随有人平台。

Description

一种基于障碍图和势场法的人机协同编队跟随及避障方法
技术领域
本发明属于机器人控制领域，尤其涉及一种协同编队跟随控制方法，将障碍图模型和势函数应用到有人/无人平台编队跟随及避障任务当中。
背景技术
有人/无人跟随控制模式是无人平台配合有人平台作战中的一环，即通过有人平台对战场态势的分析，决定将要执行的任务并传达给无人平台，无人平台能够自主地集结在一起，并形成一个指定的队形，跟随有人平台共同完成任务。在无人平台自主跟随的过程中，编队和避障是其必需的两项基本功能。
传统的无人平台编队控制技术是各个无人平台以队伍虚拟领导者的轨迹为基准，按固定角度和距离生成各自的轨迹，从而形成队形。其缺点有二，首先队形参数需要事先设定，无法根据实际的避障情况合理进行调整；其次当部分无人平台损坏时，剩下的无人平台仍按原定队形参数进行编队，无法自主形成新的队形。而传统的无人平台避障控制技术通常采用势场法，以无人平台和障碍物为中心建立势场函数，通过两者之间的排斥力达到避障目的。其缺点是无人平台行进过程中可能陷入局部极小值点。
一种新的思路是利用势函数兼顾编队和避障控制器的设计。R.Olfati Saber教授及Richard M.Murray教授(Saber,R.O.,Murray,R.M.Flocking with obstacleavoidance: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 29thChinese Control Conference.2010,20:l.)研究了上述基于势函数的群集和避障设计思路，并简化了所用的势函数，但由于其虚拟领导者并未进行路径规划，因此仍存在局部极小值问题。
对于群集中的路径规划，文献(Zhou Z.W.,Zhou B.,Zhao X.Formation controlfor Multi robots based on improved artificial potential field[J].TechnologyInnovation and Application,2015,33:44-45)提出一种“基于队形变换的沿墙导航法”解决了局部极值问题，但这种遍历求解路径方法效率不高。文献(Jia Q,Wang X.An improvedpotential field method for path planning[C].2010Chinese Control and DecisionConference.IEEE,2010:2265-2270.)则通过实时改变目标势场的位置解决了局部极值问题，但如何选定新的目标势场位置又是一个待解决的问题。
发明内容
为解决上述问题，本发明针对目前使用势函数存在局部极值点并需要进行轨迹规划的问题，提供一种基于障碍图模型和势函数的有人/无人平台编队跟随及避障方法，解决了在一个复杂环境下无人平台编队跟随有人平台前往预定地点，行进中无人平台自主形成队形并调节队形位置对未知障碍进行规避的控制问题。本发明实现了有人平台对实际障碍环境进行图模型构建和联体结构识别，并据此规划虚拟领导者的无碰路径，以及无人平台跟随虚拟领导者自主形成队形，并对检测到的障碍加以规避。本发明中编队及避障方法采用势场法，原理类似于异种电荷之间相互吸引，同种电荷之间相互排斥。对于编队，在每个无人平台中心建立势函数，使得在近距离内无人平台之间有数值上的排斥力，而远距离内又有数值上的吸引力。无人平台彼此间靠得太近时相互排斥，离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。对于避障，在障碍中心建立势函数，使得障碍对无人平台有数值上的排斥力，无人平台与障碍靠得太近时受到排斥，从而达到避障的目的。同时，避障过程不破坏无人平台自身的预定任务，即无人平台在避障后仍能自主编队跟随有人平台。
一种基于障碍图和势场法的人机协同编队跟随及避障方法，包括以下步骤：
步骤1：构建障碍图模型，具体包括以下步骤：
步骤11：假设实际环境中障碍总数为s，无人平台总数为n；
步骤12：假设r′为检测半径，无人平台检测以自身中心点为圆心，检测半径范围内的障碍，并将障碍位置和半径信息传递给有人平台；
步骤13：有人平台根据障碍位置和半径信息后构建障碍图模型；
所述障碍图模型构建方法为：每个障碍都作为障碍图的一个节点，若两个障碍之间的距离小于或等于最小阈值rsafe，无人平台无法从这两个障碍之间通过，且对应障碍图的两个节点之间有一条边；其中最小阈值rsafe的确定方法为：任意两个障碍的半径之和再加上无人平台的最大直径；
步骤2：识别障碍图模型的结构，遍历搜索各个节点，找到直接或间接与之相连的所有节点，依次确定障碍图模型中的由相连节点构成的各个联体结构；
步骤3：利用可视图化法规划虚拟领导者的路径，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：
1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；
2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：
步骤31：将虚拟领导者与最相近联体结构最***的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；
步骤32：在各个切线节点与有人平台之间做虚线，其中：
如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；
如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最***的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；
步骤33：重复步骤32，直到得到所有满足虚拟领导者无碰连接到有人平台的折线段路径；
步骤34：选取最短的一条折线段路径，作为虚拟领导者的前进路径；
步骤4：构建无人平台之间的邻接矩阵A以及无人平台与障碍之间的邻接矩阵B；具体包括以下步骤：
步骤41：假设通信半径为r，无人平台能够与以自身中心点为圆心，通信半径范围内的其他无人平台进行相互通信；
无人平台之间邻接矩阵A＝[aij]∈Rn×n，其中aij为邻接矩阵A的第(i,j)个元素，i≠j，R为实数；同时：
如果任意两个无人平台i,j中心点位置之间的距离小于或等于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值aij为1；
如果任意两个无人平台i,j中心点位置之间的距离大于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值aij为0；
步骤42：无人平台与障碍之间邻接矩阵B＝[bik]∈Rn×s，其中bik为邻接矩阵B的第(i,k)个元素，同时：
如果任意无人平台i中心点位置与障碍k中心点位置之间的距离小于或等于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值bik为1；
如果任意无人平台i中心点位置与障碍k中心点位置之间的距离大于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值bik为0；
步骤5：根据无人平台之间的距离构建编队势场函数φ1(||qa,i-qa,j||)以及根据无人平台与障碍之间的距离构建避障势场函数φ2(||qa,i-qo,k||)；其中qa,i和qa,j分别表示无人平台i和无人平台j的中心点位置，qo,k为障碍k的中心点位置；
步骤6：构建编队、跟随以及避障三个行为的控制量并将其进行叠加；
根据无人平台之间的邻接矩阵A、无人平台与障碍之间的邻接矩阵B、编队势场函数φ1(||qa,i-qa,j||)以及避障势场函数φ2(||qa,i-qo,k||)构建各个无人平台的控制量ui：
ui＝fi α+fi β+fi γ
其中，fi α为无人平台编队的控制量，fi β为无人平台避障的控制量，fi γ为无人平台跟随虚拟领导者的反馈控制量，具体的：
fi α＝-Σaij[φ1(||qa,i-qa,j||)h(||qa,i-qa,j||)(qa,i-qa,j)+γ(pa,i-pa,j)]
fi β＝-∑bik[φ2(||qa,i-qo,k||)h(||qa,i-qo,k||)(qa,i-qo,k)+βpa,i]
fi γ＝-α[(qa,i-qr)+γ(pa,i-pr)]
其中，pr中vr、θr分别为虚拟领导者的速率和方向，qr为虚拟领导者的位置，为调节函数，pa,i和pa,j分别为无人平台i、无人平台j的速度，α,β,γ,ε＞0为控制参数；将ui作为第i个无人平台的控制量输入，实现有人/无人平台编队、跟随及避障任务。
步骤3所述的虚拟领导者的前进路径表征为与时间t相关的位置函数qpath(t)和方向函数θpath(t)，并由近及远分为三个区域：弹道区、控制区以及死区；其中死区的中心为虚拟领导者的目标位置；
所述控制区和死区的半径根据有人平台速度、无人平台的最大速度和实际速度以及有人平台和无人平台的实际尺寸来确定；
有人平台的位置、速率和方向与时间t相关，分别为qh(t)、vh(t)和θh(t)；虚拟领导者的位置、速率和方向分别为qr、vr和θr；
1)当无人平台未检测到障碍时，
2)当无人平台检测到障碍时，虚拟领导者的位置qr为当前时刻前进路径的位置qpath(t)，虚拟领导者的方向θr为当前时刻前进路径的方向θpath(t)，同时：
如果虚拟领导者处于弹道区，则虚拟领导者以无人平台的最大速度vmax前进；
如果虚拟领导者处于控制区，则虚拟领导者将根据由外到内的距离远近降低自身的速度；
如果虚拟领导者处于死区，则虚拟领导者以有人平台的速度vh(t)前进。
步骤5所述的编队势场函数φ1(||qa,i-qa,j||)满足如下条件：
(a)在||qa,i-qa,j||∈(2ra,max,d)上，φ1(||qa,i-qa,j||)单调递减，在||qa,i-qa,j||∈(d,∞)上，φ1(||qa,i-qa,j||)单调递增；且在这两个区间里φ1(||qa,i-qa,j||)可微且非负；
(b)当||qa,i-qa,j||→2ra,max时，φ1(||qa,i-qa,j||)→∞；当||qa,i-qa,j||＝d时，φ1(||qa,i-qa,j||)取得唯一的最小值；
其中，ra,max为无人平台的最大半径，d表示在无人平台彼此间的通信半径r内，无人平台达到群集状态时的相邻两个无人平台之间的距离；
所述群集状态为任意两个无人平台之间的距离趋于定值。
步骤5所述的避障势场函数φ2(||qa,i-qo,k||)满足如下条件：
(a)在||qa,i-qo,k||∈(ra,max+ro,k,∞)上，φ2(||qa,i-qo,k||)单调递减；
(b)当||qa,i-qo,k||→ra,max+ro,k时，φ2(||qa,i-qo,j||)→∞；
(c)避障势场函数φ2(||qa,i-qo,k||)为一个关于||qa,i-qo,k||可微、非负、无界的函数；
其中，ro,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)障碍，位置为qo,k，半径为ro,k。设无人平台的最大半径为ra,max＝0.4m。对于任两个障碍qo,k和qo,t 无人平台能通过的最小阈值为rsafe＝ro,k+ro,t+2ra,max。若障碍之间的距离小于rsafe，虽然有人平台可以从中通过，但无人平台会在吸引力和排斥力共同作用下卡在两个障碍之间，即出现局部极小值情况，无法从它们之间通过。因此，在这种情况下需要设计虚拟领导者代替有人平台领导无人平台绕过障碍。
设以无人平台中心点为圆心，半径为r′＝3m的范围内的障碍都能被无人平台检测到，并能够传给有人平台。由于有人平台带领无人平台从两个障碍之间通过时容易出现局部极小值情况，为了方便轨迹规划，要对障碍环境进行建模。本发明中有人平台搜集所有障碍信息后，需要以障碍物的间距是否小于无人平台能通过的最小阈值rsafe为条件形成一个无向图模型，即为障碍图模型。障碍图模型构建原则为：每个障碍都作为图的一个节点，若两个障碍之间距离||qo,k-qo,t||≤rsafe，则对应图的两个节点之间有一条边。图1为原始障碍环境示意图，图2为障碍图模型示意图，障碍图模型中距离小于rsafe障碍之间存在一条边。
步骤二：对障碍图进行结构识别
根据步骤一获得的障碍图模型，所有距离小于rsafe的障碍之间都存在一条边。本发明将障碍图模型中有边连接起来的障碍集合看作一个联体结构，联体之内无人平台无法通过，但联体和联体之间有足够的无碰空间供无人平台通过。
本发明将识别联体结构问题转化为搜索图中的全部连通分量问题，从某一个节点开始向下搜索，找到直接或间接与之相连的所有节点，即为一个联体结构，再从其他节点继续此步骤，可以很简便的获得障碍环境中的全部联体结构。如图1所示，原始障碍环境被分成图2中9个联体结构。对于C++语言来说，实现这一过程首先要构建链表，然后对链表进行深度优先遍历。
步骤三：可视图法规划虚拟领导者路径
本发明使用可视图法对虚拟领导者进行路径规划，设计所有可行路径，在其中选择最短的路径作为参考路径。障碍图中的联体结构类似于规则多边形，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：
1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；
2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：
步骤31：将虚拟领导者与最相近联体结构最***的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；
步骤32：在各个切线节点与有人平台之间做虚线，其中：
如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；
如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最***的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；
步骤33：重复步骤32，直到得到N条满足虚拟领导者无碰连接到有人平台的折线段路径；其中N与切线顶点的个数有关；
步骤34：取其中最短的一条，作为虚拟领导者的前进路径，路径节点位置qpath和方向θpath为虚拟领导者的位置和方向。如图4所示，有4条折线段满足领导者无碰连接有人平台，分别为
1)l1→l2→l3，
2)l1→l4→l5→l6→l3，
3)l1→l4→l7→l8，
4)l9→l10→l11。
其中，第1条折线段长度最短，因此取l1→l2→l3为虚拟领导者的路径。若不进行虚拟领导者的路径规划，虚拟领导者就会按照图4所示虚线前进，此时无人平台极易卡在障碍之间，陷入局部极小值点。计算得到的路径信息可以表示为与时间t相关的两个函数——位置qpath(t)和方向θpath(t)。
虚拟领导者的速度控制使用动态死区法，根据虚拟领导者和有人平台的相对距离来确定自己在环境中的位置。整个环境被分为三个区域：弹道区(Ballistic zone)、控制区(Control zone)和死区(Dead zone)，如图3所示，以上区域面积大到小，其中无人平台的速度由快到慢。死区的中心即虚拟领导者的目标位置，此处为区域而不是一个点。若虚拟领导者处于了弹道区(此区域非常远离有人平台的位置)，那么虚拟领导者会以最大速度前进，从而整个无人平台队形匀速前进；在控制区中时，虚拟领导者将根据外到内的距离远近降低它的速度，以更精确的进入死区，在死区中虚拟领导者的速度与有人平台的速度相同，即表现为整个无人平台队形跟上有人平台后以相同的速度前进。
有人平台的位置、速率和方向与时间t相关，分别为qh(t)、vh(t)和θh(t)。定义虚拟领导者位置、速率和朝向分别为qr、vr和θr，其值如下：
1)当无人平台未检测到障碍时，
2)当无人平台检测到障碍时，
其中，qpath(t)为当前时刻前进路径的位置，θpath(t)为当前时刻前进路径的方向，vmax＝0.2m/s是本范例中无人平台的最大速度，Rcontrol＝10m为控制区的半径，Rdead＝0.4m为死区的半径，kr＝0.01为控制参数。在此法中控制区和死区的半径的选择时要考虑到有人平台速度，各个无人平台的最大速度和实际速度，还要考虑各平台的实际尺寸。死区的半径越小，无人平台跟踪有人平台时间越短，跟随效果越好。
步骤四：构建无人平台模型和邻接矩阵
设无人平台总数为n＝3，对于第i个(i＝1,2,3)无人平台，其控制模型如下：
其中，qa,i、pa,i代表第i个无人平台的位置和速度，代表第i个无人平台位置的导数和速度的导数，ui代表第i个无人平台的控制量。
设无人平台自身半径为ra＝0.4m，且以无人平台中心点为圆心，半径为r＝100m的范围内的无人平台都能相互通信。令||qa,i-qa,j||表示无人平台i和j之间的距离则无人平台之间邻接矩阵A＝[aij]∈Rn×n，定义为：
同理，aij为邻接矩阵A的第(i,j)个元素，R为实数，qa,i和qa,j分别表示无人平台i和无人平台j的中心点位置。
令||qo,k-qa,i||表示无人平台i和障碍k之间的距离则无人平台与障碍之间邻接矩阵B＝[bik]∈Rn×s定义为：
步骤五：构建编队势场函数和避障势场函数
对于编队，在无人平台彼此间的感应范围r＝100m内，当靠得太近时相互排斥，离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。用d＝1.6m表示达到群集时无人平台间希望达到的稳定距离，则势场函数值在每两个无人平台中心点之间的距离为d时达到最小值，此时编队完成。编队的势场函数定义如下：
定义1(编队的势场函数)令z＝||qa,i-qa,j||表示无人平台i和j之间的距离势场函数φ1(z)必须满足：
(a)在z∈(2ra,max,d)上，φ1(z)单调递减，在z∈(d,∞)上，φ1(z)单调递增。在这两区间里φ1(z)可微且非负。
(b)当z→2ra,max时，φ1(z)→∞；当da＝d时，φ1(z)取得唯一的最小值。
根据上述定义，建立无人平台之间的势场函数两个范例如下：
1)z∈(2ra,max,∞)
其中，为φ1(z)的导数，k1＝10是调节变量。2)z∈(2ra,max,∞)
其中，为φ1(z)的导数，k1＝10是调节变量。
范例1)的函数图像如图5所示，图像先下降后上升，在d＝1.6m处达到最小值，在2ra,max＝0.8m处为无穷大。其中横坐标表示无人平台i和j之间的距离，纵坐标表示势场函数φ1(z)的相对大小；
对于避障，在无人平台相对于障碍的检测范围r′内，当靠得太近时相互排斥，从而达到避障的目的。避障的势场函数定义如下:
定义2(避障的势场函数)令z′＝||qa,i-qo,k||表示无人平台i和障碍k之间的距离则势场函数φ2(z′)必须是一个关于z′可微、非负、无界的函数，且满足：
(a)在z′∈(ra,max+ro,k,∞)上，φ2(z′)单调递减。
(b)当z′→ra,max+ro,k时，φ2(z′)→∞。
根据上述定义，建立无人平台与障碍之间的势场函数两个范例如下：
1)z′∈(ra,max+ro,k,∞)
其中，为φ2(z′)的导数，k2＝15是调节变量。
2)z′∈(ra,max+ro,k,∞)
其中，为φ2(z′)的导数，k2＝5是调节变量。
范例2)的函数图像如图6所示，图像一直单调减，在ra,max+ro,k＝0.5m处为无穷大。其中横坐标表示无人平台i和障碍k之间的距离，纵坐标表示势场函数φ2(z′)的相大小；
步骤六：写出编队、跟随以及避障三个行为的控制律进行叠加
根据控制要求：①无人平台形成队列，且无人平台间不能发生碰撞；②无人平台整体追踪虚拟领导者,最终与之轨迹相同、速度一致；③无人平台实现避障。因此可以得到每个无人平台的控制量ui(i＝1,2,3)，其为以上三部分行为的叠加。
ui＝fi α+fi β+fi γ
其中，fi α为无人平台编队的控制量，fi β为无人平台避障的控制量，fi γ为跟随虚拟领导者的反馈控制量。
fi α＝-Σaij[φ1(||qa,i-qa,j||)h(||qa,i-qa,j||)(qa,i-qa,j)+γ(pa,i-pa,j)]
fi β＝-∑bik[φ2(||qa,i-qo,k||)h(||qa,i-qo,k||)(qa,i-qo,k)+βpa,i]
fi γ＝-α[(qa,i-qr)+γ(pa,i-pr)]
其中，为调节函数，α,β,γ,ε＞0为控制参数，本范例取α＝2,β＝1,γ＝2,ε＝0.1，aij是无人平台所形成邻接矩阵A∈Rn×n的第(i,j)个元素bik是无人平台与障碍所形成邻接矩阵B∈Rn×s的第(i,k)个元素参数aij和bij决定无人平台之间以及无人平台与障碍之间是否产生交互。
可以看到，我们将最终的ui作为第i个Pioneer 3-AT的控制输入，即可实现上述有人/无人平台编队跟随及避障任务。
当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (4)

1.一种基于障碍图和势场法的人机协同编队跟随及避障方法，其特征在于，包括以下步骤：
步骤1：构建障碍图模型，具体包括以下步骤：
步骤11：假设实际环境中障碍总数为s，无人平台总数为n；
步骤12：假设r′为检测半径，无人平台检测以自身中心点为圆心，检测半径范围内的障碍，并将障碍位置和半径信息传递给有人平台；
步骤13：有人平台根据障碍位置和半径信息后构建障碍图模型；
所述障碍图模型构建方法为：每个障碍都作为障碍图的一个节点，若两个障碍之间的距离小于或等于最小阈值rsafe，无人平台无法从这两个障碍之间通过，且对应障碍图的两个节点之间有一条边；其中最小阈值rsafe的确定方法为：任意两个障碍的半径之和再加上无人平台的最大直径；
步骤2：识别障碍图模型的结构，遍历搜索各个节点，找到直接或间接与之相连的所有节点，依次确定障碍图模型中的由相连节点构成的各个联体结构；
步骤3：利用可视图化法规划虚拟领导者的路径，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：
1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；
2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：
步骤31：将虚拟领导者与最相近联体结构最***的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；
步骤32：在各个切线节点与有人平台之间做虚线，其中：
如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；
如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最***的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；
步骤33：重复步骤32，直到得到所有满足虚拟领导者无碰连接到有人平台的折线段路径；
步骤34：选取最短的一条折线段路径，作为虚拟领导者的前进路径；
步骤4：构建无人平台之间的邻接矩阵A以及无人平台与障碍之间的邻接矩阵B；具体包括以下步骤：
步骤41：假设通信半径为r，无人平台能够与以自身中心点为圆心，通信半径范围内的其他无人平台进行相互通信；
无人平台之间邻接矩阵A＝[aij]∈Rn×n，其中aij为邻接矩阵A的第(i,j)个元素，R为实数；同时：
如果任意两个无人平台i,j中心点位置之间的距离小于或等于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值aij为1；
如果任意两个无人平台i,j中心点位置之间的距离大于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值aij为0；
步骤42：无人平台与障碍之间邻接矩阵B＝[bik]∈Rn×s，其中bik为邻接矩阵B的第(i,k)个元素，同时：
如果任意无人平台i中心点位置与障碍k中心点位置之间的距离小于或等于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值bik为1；
如果任意无人平台i中心点位置与障碍k中心点位置之间的距离大于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值bik为0；
步骤5：根据无人平台之间的距离构建编队势场函数φ1(||qa,i-qa,j||)以及根据无人平台与障碍之间的距离构建避障势场函数φ2(||qa,i-qo,k||)；其中qa,i和qa,j分别表示无人平台i和无人平台j的中心点位置，qo,k为障碍k的中心点位置；
步骤6：构建编队、跟随以及避障三个行为的控制量并将其进行叠加；
根据无人平台之间的邻接矩阵A、无人平台与障碍之间的邻接矩阵B、编队势场函数φ1(||qa,i-qa,j||)以及避障势场函数φ2(||qa,i-qo,k||)构建各个无人平台的控制量ui：
ui＝fi α+fi β+fi γ
其中，fi α为无人平台编队的控制量，fi β为无人平台避障的控制量，fi γ为无人平台跟随虚拟领导者的反馈控制量，具体的：
fi α＝-Σaij[φ1(||qa,i-qa,j||)h(||qa,i-qa,j||)(qa,i-qa,j)+γ(pa,i-pa,j)]
fi β＝-∑bik[φ2(||qa,i-qo,k||)h(||qa,i-qo,k||)(qa,i-qo,k)+βpa,i]
fi γ＝-α[(qa,i-qr)+γ(pa,i-pr)]
其中，pr中vr、θr分别为虚拟领导者的速率和方向，qr为虚拟领导者的位置，为调节函数，pa,i和pa,j分别为无人平台i、无人平台j的速度，α,β,γ,ε＞0为控制参数；将ui作为第i个无人平台的控制量输入，实现有人/无人平台编队、跟随及避障任务。
2.如权利要求1所述的一种基于障碍图和势场法的人机协同编队跟随及避障方法，其特征在于，步骤3所述的虚拟领导者的前进路径表征为与时间t相关的位置函数qpath(t)和方向函数θpath(t)，并由近及远分为三个区域：弹道区、控制区以及死区；其中死区的中心为虚拟领导者的目标位置；
所述控制区和死区的半径根据有人平台速度、无人平台的最大速度和实际速度以及有人平台和无人平台的实际尺寸来确定；
有人平台的位置、速率和方向与时间t相关，分别为qh(t)、vh(t)和θh(t)；虚拟领导者的位置、速率和方向分别为qr、vr和θr；
1)当无人平台未检测到障碍时，
q r = q h ( t ) v r = v h ( t ) θ r = θ h ( t )
2)当无人平台检测到障碍时，虚拟领导者的位置qr为当前时刻前进路径的位置qpath(t)，虚拟领导者的方向θr为当前时刻前进路径的方向θpath(t)，同时：
如果虚拟领导者处于弹道区，则虚拟领导者以无人平台的最大速度vmax前进；
如果虚拟领导者处于控制区，则虚拟领导者将根据由外到内的距离远近降低自身的速度；
如果虚拟领导者处于死区，则虚拟领导者以有人平台的速度vh(t)前进。
3.如权利要求1或2所述的一种基于障碍图和势场法的人机协同编队跟随及避障方法，其特征在于，步骤5所述的编队势场函数φ1(||qa,i-qa,j||)满足如下条件：
(a)在||qa,i-qa,j||∈(2ra,max,d)上，φ1(||qa,i-qa,j||)单调递减，在||qa,i-qa,j||∈(d,∞)上，φ1(||qa,i-qa,j||)单调递增；且在这两个区间里φ1(||qa,i-qa,j||)可微且非负；
(b)当||qa,i-qa,j||→2ra,max时，φ1(||qa,i-qa,j||)→∞；当||qa,i-qa,j||＝d时，φ1(||qa,i-qa,j||)取得唯一的最小值；
其中，ra,max为无人平台的最大半径，d表示在无人平台彼此间的通信半径r内，无人平台达到群集状态时的相邻两个无人平台之间的距离；
所述群集状态为任意两个无人平台之间的距离趋于定值。
4.如权利要求1或2所述的一种基于障碍图和势场法的人机协同编队跟随及避障方法，其特征在于，步骤5所述的避障势场函数φ2(||qa,i-qo,k||)满足如下条件：
(a)在||qa,i-qo,k||∈(ra,max+ro,k,∞)上，φ2(||qa,i-qo,k||)单调递减；
(b)当||qa,i-qo,k||→ra,max+ro,k时，φ2(||qa,i-qo,j||)→∞；
(c)避障势场函数φ2(||qa,i-qo,k||)为一个关于||qa,i-qo,k||可微、非负、无界的函数；其中，ro,k为障碍k的半径。
CN201610989474.2A 2016-11-10 2016-11-10 一种基于障碍图和势场法的人机协同编队跟随及避障方法 Active CN106483958B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610989474.2A CN106483958B (zh) 2016-11-10 2016-11-10 一种基于障碍图和势场法的人机协同编队跟随及避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610989474.2A CN106483958B (zh) 2016-11-10 2016-11-10 一种基于障碍图和势场法的人机协同编队跟随及避障方法

Publications (2)

Publication Number Publication Date
CN106483958A true CN106483958A (zh) 2017-03-08
CN106483958B CN106483958B (zh) 2018-02-06

Family

ID=58272003

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610989474.2A Active CN106483958B (zh) 2016-11-10 2016-11-10 一种基于障碍图和势场法的人机协同编队跟随及避障方法

Country Status (1)

Country Link
CN (1) CN106483958B (zh)

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107015571A (zh) * 2017-05-12 2017-08-04 南京航空航天大学 一种编队无人机追踪与规避移动目标的算法
CN108628294A (zh) * 2017-03-20 2018-10-09 北京军石科技有限公司 一种多机器人靶标自主协同控制***及其控制方法
CN109597418A (zh) * 2019-02-27 2019-04-09 福州大学 基于独立虚拟中心点的机器人分散式协同避障方法
WO2019152118A1 (en) * 2018-02-01 2019-08-08 Didi Research America, Llc Probabilistic navigation system and method
CN110209167A (zh) * 2019-05-27 2019-09-06 西安电子科技大学 一种实时的完全分布式的多机器人***编队的方法
CN110488840A (zh) * 2019-08-31 2019-11-22 武汉理工大学 一种无人车编队控制方法
CN110782650A (zh) * 2019-11-06 2020-02-11 同济人工智能研究院(苏州)有限公司 基于自适应事件触发的车流分布式协同编队控制方法
CN111142533A (zh) * 2020-01-03 2020-05-12 大连民族大学 多个无人水面艇的多势场避障方法及复杂环境下多usv多模式编队避障控制方法
CN111399539A (zh) * 2020-03-27 2020-07-10 西北工业大学 一种基于航路点的无人机编队避障和防撞控制方法
CN111761581A (zh) * 2020-07-07 2020-10-13 上海木木聚枞机器人科技有限公司 路径规划方法及装置、狭窄空间行进方法及装置
CN112363528A (zh) * 2020-10-15 2021-02-12 北京理工大学 基于机载视觉的无人机抗干扰集群编队控制方法
CN113848883A (zh) * 2021-09-06 2021-12-28 武汉理工大学 智能船舶编队航行控制方法、***、装置及存储介质
CN115657704A (zh) * 2022-08-29 2023-01-31 广州建通测绘地理信息技术股份有限公司 一种飞行器被动式避障导航方法、装置及计算机设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011062094A1 (ja) * 2009-11-20 2011-05-26 村田機械株式会社 自律移動体とその制御方法
CN103017757A (zh) * 2012-12-06 2013-04-03 中联重科股份有限公司 工程机械入场路径规划方法和路径规划装置
CN104165627A (zh) * 2014-08-27 2014-11-26 电子科技大学 一种基于线性规划的实时动态航迹规划方法
CN105511494A (zh) * 2016-01-20 2016-04-20 浙江大学 一种多无人机分布式队形控制的方法
CN105629974A (zh) * 2016-02-04 2016-06-01 重庆大学 一种基于改进型人工势场法的机器人路径规划方法及***

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011062094A1 (ja) * 2009-11-20 2011-05-26 村田機械株式会社 自律移動体とその制御方法
CN103017757A (zh) * 2012-12-06 2013-04-03 中联重科股份有限公司 工程机械入场路径规划方法和路径规划装置
CN104165627A (zh) * 2014-08-27 2014-11-26 电子科技大学 一种基于线性规划的实时动态航迹规划方法
CN105511494A (zh) * 2016-01-20 2016-04-20 浙江大学 一种多无人机分布式队形控制的方法
CN105629974A (zh) * 2016-02-04 2016-06-01 重庆大学 一种基于改进型人工势场法的机器人路径规划方法及***

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
何锦璇: "考虑障碍环境下的多机器人编队控制研究", 《计算机技术与发展》 *
姚立强 等: "带有领导者的多智能体避障队形控制", 《烟台大学学报( 自然科学与工程版)》 *
张凤 等: "基于Leader—follower与人工势场的多移动机器人编队控制", 《沈阳建筑大学学报(自然科学版)》 *
梁宵 等: "基于流水避石原理的无人机三维航路规划方法", 《航空学报》 *
王祥科 等: "多智能体***编队控制相关问题研究综述", 《控制与决策》 *

Cited By (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108628294A (zh) * 2017-03-20 2018-10-09 北京军石科技有限公司 一种多机器人靶标自主协同控制***及其控制方法
CN107015571A (zh) * 2017-05-12 2017-08-04 南京航空航天大学 一种编队无人机追踪与规避移动目标的算法
CN107015571B (zh) * 2017-05-12 2020-06-16 南京航空航天大学 一种编队无人机追踪与规避移动目标的算法
WO2019152118A1 (en) * 2018-02-01 2019-08-08 Didi Research America, Llc Probabilistic navigation system and method
US11719546B2 (en) 2018-02-01 2023-08-08 Beijing Voyager Technology Co., Ltd. Probabilistic navigation system and method
US11029167B2 (en) 2018-02-01 2021-06-08 Beijing Voyager Technology Co., Ltd. Probabilistic navigation system and method
CN109597418A (zh) * 2019-02-27 2019-04-09 福州大学 基于独立虚拟中心点的机器人分散式协同避障方法
CN109597418B (zh) * 2019-02-27 2021-03-02 福州大学 基于独立虚拟中心点的机器人分散式协同避障方法
CN110209167A (zh) * 2019-05-27 2019-09-06 西安电子科技大学 一种实时的完全分布式的多机器人***编队的方法
CN110488840A (zh) * 2019-08-31 2019-11-22 武汉理工大学 一种无人车编队控制方法
CN110782650A (zh) * 2019-11-06 2020-02-11 同济人工智能研究院(苏州)有限公司 基于自适应事件触发的车流分布式协同编队控制方法
CN111142533B (zh) * 2020-01-03 2023-08-15 大连民族大学 多个无人水面艇的多势场避障方法及复杂环境下多usv多模式编队避障控制方法
CN111142533A (zh) * 2020-01-03 2020-05-12 大连民族大学 多个无人水面艇的多势场避障方法及复杂环境下多usv多模式编队避障控制方法
CN111399539A (zh) * 2020-03-27 2020-07-10 西北工业大学 一种基于航路点的无人机编队避障和防撞控制方法
CN111399539B (zh) * 2020-03-27 2022-06-24 西北工业大学 一种基于航路点的无人机编队避障和防撞控制方法
CN111761581A (zh) * 2020-07-07 2020-10-13 上海木木聚枞机器人科技有限公司 路径规划方法及装置、狭窄空间行进方法及装置
CN111761581B (zh) * 2020-07-07 2021-08-27 上海木木聚枞机器人科技有限公司 路径规划方法及装置、狭窄空间行进方法及装置
CN112363528A (zh) * 2020-10-15 2021-02-12 北京理工大学 基于机载视觉的无人机抗干扰集群编队控制方法
CN113848883A (zh) * 2021-09-06 2021-12-28 武汉理工大学 智能船舶编队航行控制方法、***、装置及存储介质
CN115657704A (zh) * 2022-08-29 2023-01-31 广州建通测绘地理信息技术股份有限公司 一种飞行器被动式避障导航方法、装置及计算机设备
CN115657704B (zh) * 2022-08-29 2023-12-01 广州建通测绘地理信息技术股份有限公司 一种飞行器被动式避障导航方法、装置及计算机设备

Also Published As

Publication number Publication date
CN106483958B (zh) 2018-02-06

Similar Documents

Publication Publication Date Title
CN106483958B (zh) 一种基于障碍图和势场法的人机协同编队跟随及避障方法
Dentler et al. Collision avoidance effects on the mobility of a UAV swarm using chaotic ant colony with model predictive control
Pothal et al. Navigation of multiple mobile robots in a highly clutter terrains using adaptive neuro-fuzzy inference system
Saska et al. Autonomous deployment of swarms of micro-aerial vehicles in cooperative surveillance
Ni et al. Bioinspired neural network for real-time cooperative hunting by multirobots in unknown environments
Jun et al. Path planning for unmanned aerial vehicles in uncertain and adversarial environments
CN108268031A (zh) 路径规划方法、装置及机器人
Gan et al. Real-time decentralized search with inter-agent collision avoidance
Pradhan et al. Robot crowd navigation using predictive position fields in the potential function framework
CN105487544A (zh) 基于模糊推理***的多机器人角度控制围捕方法
CN110412877A (zh) 一种基于nsp算法的舰载机甲板路径规划最优控制方法
Liu et al. Towards search-based motion planning for micro aerial vehicles
He et al. UAV autonomous collision avoidance approach
Hou et al. Dynamic compound shape control of robot swarm
Devi et al. A cohesive and well-spaced swarm with application to unmanned aerial vehicles
Liang et al. Bio-inspired self-organized cooperative control consensus for crowded UUV swarm based on adaptive dynamic interaction topology
Huang et al. Recoat: A deep learning-based framework for multi-modal motion prediction in autonomous driving application
Ahmed et al. An energy efficient IoD static and dynamic collision avoidance approach based on gradient optimization
Wang et al. Navdog: robotic navigation guide dog via model predictive control and human-robot modeling
Okereke et al. An overview of machine learning techniques in local path planning for autonomous underwater vehicles
Lei et al. A bio-inspired neural network approach to robot navigation and mapping with nature-inspired algorithms
Yoon et al. Collaborative mission and route planning of multi-vehicle systems for autonomous search in marine environment
Nagrare et al. Decentralized path planning approach for crowd surveillance using drones
Kucherov et al. Group behavior of UAVs in obstacles presence
Heidari et al. Improved black hole algorithm for efficient low observable UCAV path planning in constrained aerospace

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB03 Change of inventor or designer information

Inventor after: Fang Hao

Inventor after: Chen Jie

Inventor after: Wu Chu

Inventor after: Zhang Fan

Inventor before: Fang Hao

Inventor before: Chen Jie

Inventor before: Wu Chu

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant