技术领域technical field
本发明属于机器人控制领域，尤其涉及一种协同编队跟随控制方法，将障碍图模型和势函数应用到有人/无人平台编队跟随及避障任务当中。The invention belongs to the field of robot control, and in particular relates to a cooperative formation following control method, which applies an obstacle graph model and a potential function to manned/unmanned platform formation following and obstacle avoidance tasks.
背景技术Background technique
有人/无人跟随控制模式是无人平台配合有人平台作战中的一环，即通过有人平台对战场态势的分析，决定将要执行的任务并传达给无人平台，无人平台能够自主地集结在一起，并形成一个指定的队形，跟随有人平台共同完成任务。在无人平台自主跟随的过程中，编队和避障是其必需的两项基本功能。The manned/unmanned follow control mode is a part of the unmanned platform's cooperation with the manned platform, that is, through the manned platform's analysis of the battlefield situation, the task to be performed is determined and communicated to the unmanned platform, and the unmanned platform can autonomously gather in the Together, and form a designated formation, follow the manned platform to complete the task together. In the process of self-following of unmanned platforms, formation and obstacle avoidance are two basic functions necessary for it.
传统的无人平台编队控制技术是各个无人平台以队伍虚拟领导者的轨迹为基准，按固定角度和距离生成各自的轨迹，从而形成队形。其缺点有二，首先队形参数需要事先设定，无法根据实际的避障情况合理进行调整；其次当部分无人平台损坏时，剩下的无人平台仍按原定队形参数进行编队，无法自主形成新的队形。而传统的无人平台避障控制技术通常采用势场法，以无人平台和障碍物为中心建立势场函数，通过两者之间的排斥力达到避障目的。其缺点是无人平台行进过程中可能陷入局部极小值点。The traditional unmanned platform formation control technology is based on the trajectory of the virtual leader of the team, and each unmanned platform generates its own trajectory according to a fixed angle and distance, thereby forming a formation. There are two disadvantages. First, the formation parameters need to be set in advance, which cannot be adjusted reasonably according to the actual obstacle avoidance situation. Second, when some unmanned platforms are damaged, the remaining unmanned platforms still form according to the original formation parameters. Unable to form new formations autonomously. The traditional unmanned platform obstacle avoidance control technology usually adopts the potential field method to establish a potential field function centered on the unmanned platform and obstacles, and achieve the purpose of obstacle avoidance through the repulsive force between the two. Its disadvantage is that the unmanned platform may fall into a local minimum point during its travel.
一种新的思路是利用势函数兼顾编队和避障控制器的设计。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.)研究了上述基于势函数的群集和避障设计思路，并简化了所用的势函数，但由于其虚拟领导者并未进行路径规划，因此仍存在局部极小值问题。A new idea is to use the potential function to take into account the design of formation and obstacle avoidance controllers. Professor R.Olfati Saber and Professor 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.) proposed a design idea of clustering and obstacle avoidance based on potential functions. For clusters, use a U situation function, within the sensing range between unmanned platforms, when they are too close, they repel each other, and when they are too far away, they attract each other, and finally make the distance between them tend to a stable value. For obstacle avoidance, the general repulsive potential function is used. Within the sensing range of the unmanned platform relative to the obstacle, when they are too close to each other, they repel each other, so as to achieve the purpose of obstacle avoidance. However, this control method cannot effectively solve the inherent local minimum problem of the potential field method. Therefore, it is necessary to design a virtual leader and perform trajectory planning on it so that it can lead the team away from the local minimum point. Literature (Yao Liqiang, Song Yanrong, Zhang Shudong. Multi-agent obstacle avoidance formation control with a leader[J]. Journal of Yantai University: Natural Science and Engineering Edition, 2014,27(2):130-135.) and literature ( Luo Xiaoyuan, Liu Dan. Multi-agent swarming and obstacle avoidance based on potential function[C].Proceedings of the 29thChinese Control Conference.2010,20:l.) studied the above-mentioned potential function-based swarming and obstacle avoidance design ideas, and simplified However, since its virtual leader does not perform path planning, there is still a local minimum problem.
对于群集中的路径规划，文献(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.)则通过实时改变目标势场的位置解决了局部极值问题，但如何选定新的目标势场位置又是一个待解决的问题。For path planning in clusters, literature (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) proposed a "based on "Navigation along the wall method of formation transformation" solves the local extremum problem, but this method of traversal to solve the path is not efficient. The literature (Jia Q, Wang X. An improved potential field method for path planning [C]. 2010 Chinese Control and Decision Conference. IEEE, 2010: 2265-2270.) solves the local extremum problem by changing the position of the target potential field in real time, but How to select the new target potential field position is another problem to be solved.
发明内容Contents of the invention
为解决上述问题，本发明针对目前使用势函数存在局部极值点并需要进行轨迹规划的问题，提供一种基于障碍图模型和势函数的有人/无人平台编队跟随及避障方法，解决了在一个复杂环境下无人平台编队跟随有人平台前往预定地点，行进中无人平台自主形成队形并调节队形位置对未知障碍进行规避的控制问题。本发明实现了有人平台对实际障碍环境进行图模型构建和联体结构识别，并据此规划虚拟领导者的无碰路径，以及无人平台跟随虚拟领导者自主形成队形，并对检测到的障碍加以规避。本发明中编队及避障方法采用势场法，原理类似于异种电荷之间相互吸引，同种电荷之间相互排斥。对于编队，在每个无人平台中心建立势函数，使得在近距离内无人平台之间有数值上的排斥力，而远距离内又有数值上的吸引力。无人平台彼此间靠得太近时相互排斥，离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。对于避障，在障碍中心建立势函数，使得障碍对无人平台有数值上的排斥力，无人平台与障碍靠得太近时受到排斥，从而达到避障的目的。同时，避障过程不破坏无人平台自身的预定任务，即无人平台在避障后仍能自主编队跟随有人平台。In order to solve the above-mentioned problems, the present invention aims at the problem that there are local extremum points in the current use of the potential function and needs trajectory planning, and provides a manned/unmanned platform formation following and obstacle avoidance method based on the obstacle graph model and the potential function, which solves the problem of In a complex environment, the unmanned platform formation follows the manned platform to the predetermined location, and the unmanned platform independently forms a formation and adjusts the formation position to avoid unknown obstacles. The invention realizes that the manned platform constructs the graph model and recognizes the conjoined structure of the actual obstacle environment, and plans the non-collision path of the virtual leader accordingly, and the unmanned platform follows the virtual leader to form a formation independently, and the detected obstacles are avoided. The formation and obstacle avoidance method in the present invention adopts the potential field method, and the principle is similar to that charges of different types attract each other, and charges of the same type repel each other. For the formation, a potential function is established at the center of each unmanned platform, so that there is numerical repulsion between unmanned platforms in the short distance, and numerical attraction in the long distance. When the unmanned platforms are too close to each other, they repel each other, and when they are too far away, they attract each other, and finally the distance between them tends to a stable value. For obstacle avoidance, a potential function is established in the center of the obstacle, so that the obstacle has numerical repulsion to the unmanned platform. When the unmanned platform is too close to the obstacle, it will be repelled, so as to achieve the purpose of obstacle avoidance. At the same time, the obstacle avoidance process does not destroy the scheduled mission of the unmanned platform itself, that is, the unmanned platform can still follow the manned platform in formation after obstacle avoidance.
一种基于障碍图和势场法的人机协同编队跟随及避障方法，包括以下步骤：A human-machine collaborative formation following and obstacle avoidance method based on obstacle graph and potential field method, comprising the following steps:
步骤1：构建障碍图模型，具体包括以下步骤：Step 1: Build an obstacle graph model, specifically including the following steps:
步骤11：假设实际环境中障碍总数为s，无人平台总数为n；Step 11: Assume that the total number of obstacles in the actual environment is s, and the total number of unmanned platforms is n;
步骤12：假设r′为检测半径，无人平台检测以自身中心点为圆心，检测半径范围内的障碍，并将障碍位置和半径信息传递给有人平台；Step 12: Assuming that r' is the detection radius, the unmanned platform detects obstacles within the radius with its own center point as the center, and transmits the obstacle position and radius information to the manned platform;
步骤13：有人平台根据障碍位置和半径信息后构建障碍图模型；Step 13: The manned platform constructs the obstacle graph model according to the obstacle position and radius information;
所述障碍图模型构建方法为：每个障碍都作为障碍图的一个节点，若两个障碍之间的距离小于或等于最小阈值rsafe，无人平台无法从这两个障碍之间通过，且对应障碍图的两个节点之间有一条边；其中最小阈值rsafe的确定方法为：任意两个障碍的半径之和再加上无人平台的最大直径；The obstacle graph model construction method is: each obstacle is used as a node of the obstacle graph, if the distance between two obstacles is less than or equal to the minimum threshold rsafe , the unmanned platform cannot pass between the two obstacles, and There is an edge between two nodes corresponding to the obstacle graph; the determination method of the minimum threshold rsafe is: the sum of the radii of any two obstacles plus the maximum diameter of the unmanned platform;
步骤2：识别障碍图模型的结构，遍历搜索各个节点，找到直接或间接与之相连的所有节点，依次确定障碍图模型中的由相连节点构成的各个联体结构；Step 2: Identify the structure of the obstacle graph model, traverse and search each node, find all the nodes directly or indirectly connected to it, and determine the conjoined structures composed of connected nodes in the obstacle graph model in turn;
步骤3：利用可视图化法规划虚拟领导者的路径，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：Step 3: Use the visualization method to plan the path of the virtual leader, draw a dotted line starting from the position of the virtual leader and ending at the position of the manned platform, where:
1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；1) If the dotted line does not pass through the conjoined structure, then the dotted line is the path of the virtual leader;
2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：2) If the dotted line passes through the conjoined structure, the path determination of the virtual leader includes the following steps:
步骤31：将虚拟领导者与最相近联体结构最外围的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；Step 31: Connect the virtual leader to the outermost nodes of the closest conjoined structure, and then remove the polyline segment passing through the closest conjoined structure to obtain the polyline segment tangent to the closest conjoined structure and its corresponding tangent node;
步骤32：在各个切线节点与有人平台之间做虚线，其中：Step 32: Make dotted lines between each tangent node and the manned platform, where:
如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；If the dotted line does not pass through the conjoined structure, the polyline segment between the tangent node corresponding to the dotted line and the manned platform is the path of the virtual leader;
如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最外围的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；If the dotted line passes through the conjoined structure, connect the tangent nodes corresponding to the dotted line to the outermost nodes of the conjoined structure, and then remove the polyline segment passing through the conjoined structure to obtain the polyline segment tangent to the conjoined structure and its corresponding tangent node;
步骤33：重复步骤32，直到得到所有满足虚拟领导者无碰连接到有人平台的折线段路径；Step 33: Repeat step 32 until all polyline segment paths satisfying the non-touch connection of the virtual leader to the manned platform are obtained;
步骤34：选取最短的一条折线段路径，作为虚拟领导者的前进路径；Step 34: Select the shortest polyline segment path as the forward path of the virtual leader;
步骤4：构建无人平台之间的邻接矩阵A以及无人平台与障碍之间的邻接矩阵B；具体包括以下步骤：Step 4: Construct the adjacency matrix A between unmanned platforms and the adjacency matrix B between unmanned platforms and obstacles; specifically include the following steps:
步骤41：假设通信半径为r，无人平台能够与以自身中心点为圆心，通信半径范围内的其他无人平台进行相互通信；Step 41: Assuming that the communication radius is r, the unmanned platform can communicate with other unmanned platforms within the communication radius with its center point as the center;
无人平台之间邻接矩阵A＝[aij]∈Rn×n，其中aij为邻接矩阵A的第(i,j)个元素，i≠j，R为实数；同时：Adjacency matrix A=[aij ]∈Rn×n between unmanned platforms, where aij is the (i,j)th element of adjacency matrix A, i≠j, R is a real number; at the same time:
如果任意两个无人平台i,j中心点位置之间的距离小于或等于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值aij为1；If the distance between the center points of any two unmanned platforms i and j is less than or equal to the communication radius r, the element value aij of the adjacency matrix A corresponding to the two unmanned platforms is 1;
如果任意两个无人平台i,j中心点位置之间的距离大于通信半径r，则这两个无人平台对应的邻接矩阵A的元素值aij为0；If the distance between the center points of any two unmanned platforms i and j is greater than the communication radius r, the element value aij of the adjacency matrix A corresponding to the two unmanned platforms is 0;
步骤42：无人平台与障碍之间邻接矩阵B＝[bik]∈Rn×s，其中bik为邻接矩阵B的第(i,k)个元素，同时：Step 42: The adjacency matrix B=[bik ]∈Rn×s between the unmanned platform and the obstacle, where bik is the (i,k)th element of the adjacency matrix B, at the same time:
如果任意无人平台i中心点位置与障碍k中心点位置之间的距离小于或等于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值bik为1；If the distance between the center point position of any unmanned platform i and the center point position of obstacle k is less than or equal to the detection radius r', the element value bik of the adjacency matrix B corresponding to the unmanned platform and obstacle is 1;
如果任意无人平台i中心点位置与障碍k中心点位置之间的距离大于检测半径r′，则该无人平台和障碍对应的邻接矩阵B的元素值bik为0；If the distance between the center point position of any unmanned platform i and the center point position of obstacle k is greater than the detection radius r', the element value bik of the adjacency matrix B corresponding to the unmanned platform and obstacle is 0;
步骤5：根据无人平台之间的距离构建编队势场函数φ1(||qa,i-qa,j||)以及根据无人平台与障碍之间的距离构建避障势场函数φ2(||qa,i-qo,k||)；其中qa,i和qa,j分别表示无人平台i和无人平台j的中心点位置，qo,k为障碍k的中心点位置；Step 5: Construct the formation potential field function φ1 (||qa,i -qa,j ||) according to the distance between the unmanned platforms and construct the obstacle avoidance potential field function according to the distance between the unmanned platform and the obstacle φ2 (||qa,i -qo,k ||); where qa,i and qa,j represent the center point positions of unmanned platform i and unmanned platform j respectively, and qo,k are obstacles The position of the center point of k;
步骤6：构建编队、跟随以及避障三个行为的控制量并将其进行叠加；Step 6: Construct the control amount of the three behaviors of formation, following and obstacle avoidance and superimpose them;
根据无人平台之间的邻接矩阵A、无人平台与障碍之间的邻接矩阵B、编队势场函数φ1(||qa,i-qa,j||)以及避障势场函数φ2(||qa,i-qo,k||)构建各个无人平台的控制量ui：According to the adjacency matrix A between unmanned platforms, the adjacency matrix B between unmanned platforms and obstacles, the formation potential field function φ1 (||qa,i -qa,j ||) and the obstacle avoidance potential field function φ2 (||qa,i -qo,k ||) constructs the control quantity ui of each unmanned platform:
ui＝fiα+fiβ+fiγui =fiα +fiβ +fiγ
其中，fiα为无人平台编队的控制量，fiβ为无人平台避障的控制量，fiγ为无人平台跟随虚拟领导者的反馈控制量，具体的：Among them, fiα is the control amount of unmanned platform formation, fiβ is the control amount of unmanned platform obstacle avoidance, fiγ is the feedback control amount of unmanned platform following the virtual leader, specifically:
fiα＝-Σaij[φ1(||qa,i-qa,j||)h(||qa,i-qa,j||)(qa,i-qa,j)+γ(pa,i-pa,j)]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β ＝-∑bik [φ2 (||qa,i -qo,k ||)h(||qa,i -qo,k ||)(qa,i -qo ,k )+βpa,i ]
fiγ＝-α[(qa,i-qr)+γ(pa,i-pr)]fiγ ＝-α[(qa,i -qr )+γ(pa,i -pr )]
其中，pr中vr、θr分别为虚拟领导者的速率和方向，qr为虚拟领导者的位置，为调节函数，pa,i和pa,j分别为无人平台i、无人平台j的速度，α,β,γ,ε＞0为控制参数；将ui作为第i个无人平台的控制量输入，实现有人/无人平台编队、跟随及避障任务。in, vr and θr in pr are the speed and direction of the virtual leader respectively, qr is the position of the virtual leader, is the adjustment function, pa,i and pa,j are the speeds of unmanned platform i and unmanned platform j respectively, α,β,γ,ε>0 are the control parameters; take ui as the ith unmanned platform The control quantity input realizes manned/unmanned platform formation, following and obstacle avoidance tasks.
步骤3所述的虚拟领导者的前进路径表征为与时间t相关的位置函数qpath(t)和方向函数θpath(t)，并由近及远分为三个区域：弹道区、控制区以及死区；其中死区的中心为虚拟领导者的目标位置；The advancing path of the virtual leader described in step 3 is characterized by a position function qpath (t) and a direction function θpath (t) related to time t, and is divided into three areas from near to far: the ballistic area and the control area and a dead zone; where the center of the dead zone is the target position of the virtual leader;
所述控制区和死区的半径根据有人平台速度、无人平台的最大速度和实际速度以及有人平台和无人平台的实际尺寸来确定；The radius of the control zone and the dead zone is determined according to the speed of the manned platform, the maximum speed and the actual speed of the unmanned platform, and the actual size of the manned platform and the unmanned platform;
有人平台的位置、速率和方向与时间t相关，分别为qh(t)、vh(t)和θh(t)；虚拟领导者的位置、速率和方向分别为qr、vr和θr；The position, velocity and direction of the manned platform are related to time t, which are qh (t), vh (t) and θh (t) respectively; the position, velocity and direction of the virtual leader are qr , vr and θr ;
1)当无人平台未检测到障碍时，1) When the unmanned platform does not detect obstacles,
2)当无人平台检测到障碍时，虚拟领导者的位置qr为当前时刻前进路径的位置qpath(t)，虚拟领导者的方向θr为当前时刻前进路径的方向θpath(t)，同时：2) When the unmanned platform detects an obstacle, the position qr of the virtual leader is the position qpath (t) of the advancing path at the current moment, and the direction θr of the virtual leader is the direction θpath (t) of the advancing path at the current moment ,at the same time:
如果虚拟领导者处于弹道区，则虚拟领导者以无人平台的最大速度vmax前进；If the virtual leader is in the ballistic zone, the virtual leader advances at the maximum velocity vmax of the unmanned platform;
如果虚拟领导者处于控制区，则虚拟领导者将根据由外到内的距离远近降低自身的速度；If the virtual leader is in the control zone, the virtual leader will reduce its own speed according to the distance from the outside to the inside;
如果虚拟领导者处于死区，则虚拟领导者以有人平台的速度vh(t)前进。If the virtual leader is in the dead zone, the virtual leader advances at the velocity vh (t) of the manned platform.
步骤5所述的编队势场函数φ1(||qa,i-qa,j||)满足如下条件：The formation potential field function φ1 (||qa,i -qa,j ||) described in step 5 satisfies the following conditions:
(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||)可微且非负；(a) On ||qa,i -qa,j ||∈(2ra,max ,d), φ1 (||qa,i -qa,j ||) decreases monotonically, and in || On |qa,i -qa,j ||∈(d,∞), φ1 (||qa,i -qa,j ||) increases monotonously; and in these two intervals φ1 ( ||qa,i -qa,j ||) is differentiable and non-negative;
(b)当||qa,i-qa,j||→2ra,max时，φ1(||qa,i-qa,j||)→∞；当||qa,i-qa,j||＝d时，φ1(||qa,i-qa,j||)取得唯一的最小值；(b) When ||qa,i -qa,j ||→2ra,max , φ1 (||qa,i -qa,j ||)→∞; when ||qa, Wheni -qa,j ||＝d, φ1 (||qa,i -qa,j ||) obtains the unique minimum value;
其中，ra,max为无人平台的最大半径，d表示在无人平台彼此间的通信半径r内，无人平台达到群集状态时的相邻两个无人平台之间的距离；Among them, ra,max is the maximum radius of the unmanned platform, and d represents the distance between two adjacent unmanned platforms when the unmanned platforms reach the cluster state within the communication radius r between the unmanned platforms;
所述群集状态为任意两个无人平台之间的距离趋于定值。The cluster state is that the distance between any two unmanned platforms tends to a constant value.
步骤5所述的避障势场函数φ2(||qa,i-qo,k||)满足如下条件：The obstacle avoidance potential field function φ2 (||qa,i -qo,k ||) described in step 5 satisfies the following conditions:
(a)在||qa,i-qo,k||∈(ra,max+ro,k,∞)上，φ2(||qa,i-qo,k||)单调递减；(a) On ||qa,i -qo,k ||∈(ra,max +ro,k ,∞), φ2 (||qa,i -qo,k ||) Monotonically decreasing;
(b)当||qa,i-qo,k||→ra,max+ro,k时，φ2(||qa,i-qo,j||)→∞；(b) When ||qa,i -qo,k ||→ra,max +ro,k , φ2 (||qa,i -qo,j ||)→∞;
(c)避障势场函数φ2(||qa,i-qo,k||)为一个关于||qa,i-qo,k||可微、非负、无界的函数；(c) The obstacle avoidance potential field function φ2 (||qa,i -qo,k ||) is a differentiable, non-negative, unbounded function about ||qa,i -qo,k || ;
其中，ro,k为障碍k的半径。Among them, ro,k is the radius of obstacle k.
有益效果：Beneficial effect:
1、本发明在势函数群集和避障设计思路的基础上，增加了利用可视图法规划参考轨迹的环节，有效的解决了传统势场法中固有的局部极值点，即无人平台会在吸引力和排斥力共同作用下卡在两个障碍之间的问题。1. On the basis of potential function clustering and obstacle avoidance design ideas, the present invention adds the link of using the visual map method to plan the reference trajectory, which effectively solves the inherent local extreme points in the traditional potential field method, that is, the unmanned platform will The problem of getting stuck between two barriers due to a combination of attractive and repulsive forces.
2、本发明相较于传统可视图路径规划方法，增加了障碍环境的图建模环节，使得后续的结构识别过程变得简便，而且得到的障碍联体结构类似于规则多边形，满足了传统可视图法对障碍的几何环境要求以及障碍之间有足够的无碰空间要求。2. Compared with the traditional visual map path planning method, the present invention adds the graph modeling link of the obstacle environment, making the subsequent structure identification process easier, and the obtained obstacle conjoined structure is similar to a regular polygon, which satisfies the traditional possible The view method requires the geometric environment of obstacles and sufficient non-collision space between obstacles.
3、相较于传统编队跟随方法，本发明无需事先设定队形参数和参考轨迹，队列形成的同时兼顾避障任务，且部分无人平台损坏不影响队形的形成。3. Compared with the traditional formation following method, the present invention does not need to set formation parameters and reference trajectories in advance. The formation of the formation takes into account the task of obstacle avoidance, and the damage of some unmanned platforms does not affect the formation of the formation.
附图说明Description of drawings
图1为本发明原始障碍环境示意图；Fig. 1 is a schematic diagram of the original obstacle environment of the present invention;
图2为本发明障碍图模型示意图；Fig. 2 is a schematic diagram of the obstacle map model of the present invention;
图3为本发明环境区域划分示意图；Fig. 3 is a schematic diagram of the division of environmental regions in the present invention;
图4为本发明路径规划方法示意图；Fig. 4 is a schematic diagram of the path planning method of the present invention;
图5为本发明编队势函数示意图；Fig. 5 is the schematic diagram of formation potential function of the present invention;
图6为本发明避障势函数示意图；Fig. 6 is a schematic diagram of the obstacle avoidance potential function of the present invention;
图7为本发明控制器控制流程示意图。Fig. 7 is a schematic diagram of the control flow of the controller of the present invention.
具体实施方式Detailed ways
下面结合附图和实施例对本发明做进一步说明：Below in conjunction with accompanying drawing and embodiment the present invention will be further described:
本发明组建了包含三个Pioneer 3-AT与一个上位机的实验平台。其中Pioneer3-AT(包括车载笔记本)作为无人平台，可以获取自身的位置速度信息，以及通过传感器检测障碍位置的能力，且有与邻居之间通信的能力。上位机作为有人平台，可以获得人的位置速度信息，并有与三个无人平台通信的能力。上位机程序以及车载程序均由C++语言写成。无人平台通过传感器检测障碍，并将障碍位置发给有人平台。有人平台规划路径，并将虚拟领导者状态发给无人平台控制器，控制器结合虚拟领导者、自身及邻居状态和障碍位置计算控制量发送给无人平台执行器，最终使无人平台按照控制要求前进。控制方法示意图如图7所示。The present invention sets up an experimental platform including three Pioneer 3-ATs and a host computer. Among them, Pioneer3-AT (including car notebook) is an unmanned platform, which can obtain its own position and speed information, as well as the ability to detect the position of obstacles through sensors, and has the ability to communicate with neighbors. As a manned platform, the upper computer can obtain the position and speed information of the man, and has the ability to communicate with the three unmanned platforms. Both the host computer program and the vehicle program are written in C++ language. The unmanned platform detects obstacles through sensors and sends the location of the obstacles to the manned platform. The manned platform plans the path, and sends the state of the virtual leader to the controller of the unmanned platform. The controller combines the virtual leader, the state of itself and its neighbors, and the obstacle position to calculate the control amount and send it to the actuator of the unmanned platform. Finally, the unmanned platform follows the Control demands to move forward. The schematic diagram of the control method is shown in Figure 7.
实现本发明所述控制方法的步骤为：The step that realizes control method described in the present invention is:
步骤一：构建障碍图模型Step 1: Build an obstacle graph model
设实际环境中障碍物总数为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，虽然有人平台可以从中通过，但无人平台会在吸引力和排斥力共同作用下卡在两个障碍之间，即出现局部极小值情况，无法从它们之间通过。因此，在这种情况下需要设计虚拟领导者代替有人平台领导无人平台绕过障碍。Assuming that the total number of obstacles in the actual environment is s=30, for the kth (k=1,2,...,30) obstacle, the position is qo,k and the radius is ro,k . Suppose the maximum radius of the unmanned platform is ra,max =0.4m. For any two obstacles qo,k and qo,t The minimum threshold for the unmanned platform to pass is rsafe =ro,k +ro,t +2ra,max . If the distance between obstacles is less than rsafe , although the manned platform can pass through it, the unmanned platform will be stuck between the two obstacles under the joint action of attraction and repulsion, that is, there will be a local minimum, and it will not be possible to pass through them. pass between. Therefore, in this case, it is necessary to design a virtual leader to replace the manned platform to lead the unmanned platform to bypass obstacles.
设以无人平台中心点为圆心，半径为r′＝3m的范围内的障碍都能被无人平台检测到，并能够传给有人平台。由于有人平台带领无人平台从两个障碍之间通过时容易出现局部极小值情况，为了方便轨迹规划，要对障碍环境进行建模。本发明中有人平台搜集所有障碍信息后，需要以障碍物的间距是否小于无人平台能通过的最小阈值rsafe为条件形成一个无向图模型，即为障碍图模型。障碍图模型构建原则为：每个障碍都作为图的一个节点，若两个障碍之间距离||qo,k-qo,t||≤rsafe，则对应图的两个节点之间有一条边。图1为原始障碍环境示意图，图2为障碍图模型示意图，障碍图模型中距离小于rsafe障碍之间存在一条边。Assuming that the center point of the unmanned platform is the center of the circle, the obstacles within the radius of r′=3m can be detected by the unmanned platform and can be transmitted to the manned platform. Since the manned platform leads the unmanned platform to pass between two obstacles, it is easy to have a local minimum value. In order to facilitate trajectory planning, the obstacle environment should be modeled. In the present invention, after the manned platform collects all obstacle information, it needs to form an undirected graph model based on whether the obstacle distance is smaller than the minimum threshold rsafe that the unmanned platform can pass through, which is the obstacle graph model. The principle of constructing the obstacle graph model is: each obstacle is regarded as a node of the graph, if the distance between two obstacles is ||qo,k -qo,t ||≤rsafe , then the distance between two nodes of the corresponding graph is There is a side. Figure 1 is a schematic diagram of the original obstacle environment, and Figure 2 is a schematic diagram of the obstacle graph model. In the obstacle graph model, there is an edge between obstacles whose distance is less than rsafe .
步骤二：对障碍图进行结构识别Step 2: Identify the structure of the obstacle map
根据步骤一获得的障碍图模型，所有距离小于rsafe的障碍之间都存在一条边。本发明将障碍图模型中有边连接起来的障碍集合看作一个联体结构，联体之内无人平台无法通过，但联体和联体之间有足够的无碰空间供无人平台通过。According to the obstacle graph model obtained in step 1, there is an edge between all obstacles whose distance is less thanrsafe . The invention regards the obstacle set connected by edges in the obstacle graph model as a conjoined structure, the unmanned platform cannot pass through the conjoined body, but there is enough non-collision space between the conjoined bodies for the unmanned platform to pass through .
本发明将识别联体结构问题转化为搜索图中的全部连通分量问题，从某一个节点开始向下搜索，找到直接或间接与之相连的所有节点，即为一个联体结构，再从其他节点继续此步骤，可以很简便的获得障碍环境中的全部联体结构。如图1所示，原始障碍环境被分成图2中9个联体结构。对于C++语言来说，实现这一过程首先要构建链表，然后对链表进行深度优先遍历。The present invention transforms the problem of identifying conjoined structures into the problem of searching for all connected components in the graph, searches downward from a certain node, and finds all nodes directly or indirectly connected to it, which is a conjoined structure, and then starts from other nodes Continuing this step, you can easily obtain all conjoined structures in the obstacle environment. As shown in Figure 1, the original obstacle environment is divided into nine joint structures in Figure 2. For the C++ language, to implement this process, we must first build a linked list, and then perform a depth-first traversal on the linked list.
步骤三：可视图法规划虚拟领导者路径Step 3: Planning the path of virtual leaders with visual diagrams
本发明使用可视图法对虚拟领导者进行路径规划，设计所有可行路径，在其中选择最短的路径作为参考路径。障碍图中的联体结构类似于规则多边形，以虚拟领导者位置为起点，有人平台位置为终点做一条虚线，其中：The present invention uses a visualization method to plan the path of the virtual leader, designs all feasible paths, and selects the shortest path as a reference path. The conjoined structure in the obstacle diagram is similar to a regular polygon, with the virtual leader as the starting point and the manned platform as the end point, draw a dotted line, where:
1)如果虚线不经过联体结构，则该虚线为虚拟领导者的路径；1) If the dotted line does not pass through the conjoined structure, then the dotted line is the path of the virtual leader;
2)如果虚线经过联体结构，则虚拟领导者的路径确定包括以下步骤：2) If the dotted line passes through the conjoined structure, the path determination of the virtual leader includes the following steps:
步骤31：将虚拟领导者与最相近联体结构最外围的节点分别相连，再去掉穿过最相近联体结构的折线段，得到与最相近联体结构相切的折线段及其对应的切线节点；Step 31: Connect the virtual leader to the outermost nodes of the closest conjoined structure, and then remove the polyline segment passing through the closest conjoined structure to obtain the polyline segment tangent to the closest conjoined structure and its corresponding tangent node;
步骤32：在各个切线节点与有人平台之间做虚线，其中：Step 32: Make dotted lines between each tangent node and the manned platform, where:
如果虚线不经过联体结构，则该虚线对应的切线节点与有人平台之间的折线段为虚拟领导者的路径；If the dotted line does not pass through the conjoined structure, the polyline segment between the tangent node corresponding to the dotted line and the manned platform is the path of the virtual leader;
如果虚线经过联体结构，则将该虚线对应的切线节点与联体结构最外围的节点分别相连，再去掉穿过联体结构的折线段，得到与联体结构相切的折线段及其对应的切线节点；If the dotted line passes through the conjoined structure, connect the tangent nodes corresponding to the dotted line to the outermost nodes of the conjoined structure, and then remove the polyline segment passing through the conjoined structure to obtain the polyline segment tangent to the conjoined structure and its corresponding tangent node;
步骤33：重复步骤32，直到得到N条满足虚拟领导者无碰连接到有人平台的折线段路径；其中N与切线顶点的个数有关；Step 33: Repeat step 32 until N pieces of polyline segment paths satisfying the non-touch connection between the virtual leader and the manned platform are obtained; wherein N is related to the number of vertices of the tangent line;
步骤34：取其中最短的一条，作为虚拟领导者的前进路径，路径节点位置qpath和方向θpath为虚拟领导者的位置和方向。如图4所示，有4条折线段满足领导者无碰连接有人平台，分别为Step 34: Take the shortest one as the forward path of the virtual leader, and the position qpath and direction θpath of the path node are the position and direction of the virtual leader. As shown in Figure 4, there are 4 broken line segments that satisfy the leader’s non-touch connection to the manned platform, which are respectively
1)l1→l2→l3，1) l1 →l2 →l3 ,
2)l1→l4→l5→l6→l3，2) l1 →l4 →l5 →l6 →l3 ,
3)l1→l4→l7→l8，3) l1 →l4 →l7 →l8 ,
4)l9→l10→l11。4) l9 →l10 →l11 .
其中，第1条折线段长度最短，因此取l1→l2→l3为虚拟领导者的路径。若不进行虚拟领导者的路径规划，虚拟领导者就会按照图4所示虚线前进，此时无人平台极易卡在障碍之间，陷入局部极小值点。计算得到的路径信息可以表示为与时间t相关的两个函数——位置qpath(t)和方向θpath(t)。Among them, the length of the first polyline segment is the shortest, so l1 → l2 → l3 is taken as the path of the virtual leader. If the path planning of the virtual leader is not carried out, the virtual leader will move forward according to the dotted line shown in Figure 4. At this time, the unmanned platform is very easy to get stuck between obstacles and fall into a local minimum point. The computed path information can be expressed as two functions related to time t—position qpath (t) and direction θpath (t).
虚拟领导者的速度控制使用动态死区法，根据虚拟领导者和有人平台的相对距离来确定自己在环境中的位置。整个环境被分为三个区域：弹道区(Ballistic zone)、控制区(Control zone)和死区(Dead zone)，如图3所示，以上区域面积大到小，其中无人平台的速度由快到慢。死区的中心即虚拟领导者的目标位置，此处为区域而不是一个点。若虚拟领导者处于了弹道区(此区域非常远离有人平台的位置)，那么虚拟领导者会以最大速度前进，从而整个无人平台队形匀速前进；在控制区中时，虚拟领导者将根据外到内的距离远近降低它的速度，以更精确的进入死区，在死区中虚拟领导者的速度与有人平台的速度相同，即表现为整个无人平台队形跟上有人平台后以相同的速度前进。The virtual leader's speed control uses the dynamic dead zone method to determine its own position in the environment according to the relative distance between the virtual leader and the manned platform. The whole environment is divided into three areas: Ballistic zone, Control zone and Dead zone, as shown in Figure 3, the above areas are from large to small, and the speed of the unmanned platform is determined by Fast to slow. The center of the dead zone is the target position of the virtual leader, which is an area rather than a point. If the virtual leader is in the ballistic area (this area is very far away from the position of the manned platform), then the virtual leader will advance at the maximum speed, so that the entire unmanned platform formation will advance at a uniform speed; when in the control area, the virtual leader will move forward according to The distance from the outside to the inside reduces its speed to enter the dead zone more accurately. In the dead zone, the speed of the virtual leader is the same as that of the manned platform, that is, the entire unmanned platform formation catches up with the manned platform. Go forward at the same speed.
有人平台的位置、速率和方向与时间t相关，分别为qh(t)、vh(t)和θh(t)。定义虚拟领导者位置、速率和朝向分别为qr、vr和θr，其值如下：The position, velocity, and direction of the manned platform are related to time t, and are qh (t), vh (t) and θh (t), respectively. Define the position, velocity and orientation of the virtual leader as qr , vr and θr respectively, and their values are as follows:
1)当无人平台未检测到障碍时，1) When the unmanned platform does not detect obstacles,
2)当无人平台检测到障碍时，2) When the unmanned platform detects obstacles,
其中，qpath(t)为当前时刻前进路径的位置，θpath(t)为当前时刻前进路径的方向，vmax＝0.2m/s是本范例中无人平台的最大速度，Rcontrol＝10m为控制区的半径，Rdead＝0.4m为死区的半径，kr＝0.01为控制参数。在此法中控制区和死区的半径的选择时要考虑到有人平台速度，各个无人平台的最大速度和实际速度，还要考虑各平台的实际尺寸。死区的半径越小，无人平台跟踪有人平台时间越短，跟随效果越好。Among them, qpath (t) is the position of the forward path at the current moment, θpath (t) is the direction of the forward path at the current moment, vmax =0.2m/s is the maximum speed of the unmanned platform in this example, Rcontrol =10m is the radius of the control zone, Rdead =0.4m is the radius of the dead zone, and kr =0.01 is the control parameter. In this method, the selection of the radius of the control zone and the dead zone should take into account the speed of the manned platform, the maximum speed and actual speed of each unmanned platform, and the actual size of each platform. The smaller the radius of the dead zone, the shorter the time for the unmanned platform to track the manned platform, and the better the tracking effect.
步骤四：构建无人平台模型和邻接矩阵Step 4: Build the unmanned platform model and adjacency matrix
设无人平台总数为n＝3，对于第i个(i＝1,2,3)无人平台，其控制模型如下：Assuming that the total number of unmanned platforms is n=3, for the i-th (i=1,2,3) unmanned platform, its control model is as follows:
其中，qa,i、pa,i代表第i个无人平台的位置和速度，代表第i个无人平台位置的导数和速度的导数，ui代表第i个无人平台的控制量。Among them, qa,i and pa,i represent the position and velocity of the i-th unmanned platform, Represents the derivative of the position and velocity of the i-th unmanned platform, ui represents the control amount of the i-th unmanned platform.
设无人平台自身半径为ra＝0.4m，且以无人平台中心点为圆心，半径为r＝100m的范围内的无人平台都能相互通信。令||qa,i-qa,j||表示无人平台i和j之间的距离则无人平台之间邻接矩阵A＝[aij]∈Rn×n，定义为：Assume that the radius of the unmanned platform itself is ra =0.4m, and the center point of the unmanned platform is taken as the center, and the unmanned platforms within the radius of r=100m can communicate with each other. Let ||qa,i -qa,j || represent the distance between unmanned platform i and j Then the adjacency matrix A=[aij ]∈Rn×n between unmanned platforms is defined as:
同理，aij为邻接矩阵A的第(i,j)个元素，R为实数，qa,i和qa,j分别表示无人平台i和无人平台j的中心点位置。Similarly, aij is the (i,j)th element of the adjacency matrix A, R is a real number, qa,i and qa,j represent the center point positions of unmanned platform i and unmanned platform j respectively.
令||qo,k-qa,i||表示无人平台i和障碍k之间的距离则无人平台与障碍之间邻接矩阵B＝[bik]∈Rn×s定义为：Let ||qo,k -qa,i || represent the distance between unmanned platform i and obstacle k Then the adjacency matrix B=[bik ]∈Rn×s between the unmanned platform and the obstacle is defined as:
步骤五：构建编队势场函数和避障势场函数Step 5: Construct formation potential field function and obstacle avoidance potential field function
对于编队，在无人平台彼此间的感应范围r＝100m内，当靠得太近时相互排斥，离得太远时又相互吸引，最终使它们之间的距离趋于一个稳定的值。用d＝1.6m表示达到群集时无人平台间希望达到的稳定距离，则势场函数值在每两个无人平台中心点之间的距离为d时达到最小值，此时编队完成。编队的势场函数定义如下：For the formation, within the sensing range r=100m between the unmanned platforms, they repel each other when they are too close, and attract each other when they are too far away, and finally the distance between them tends to a stable value. Use d=1.6m to represent the desired stable distance between unmanned platforms when they reach the cluster, then the potential field function value reaches the minimum value when the distance between the center points of every two unmanned platforms is d, and the formation is completed at this time. The potential field function of the formation is defined as follows:
定义1(编队的势场函数)令z＝||qa,i-qa,j||表示无人平台i和j之间的距离势场函数φ1(z)必须满足：Definition 1 (potential field function of formation) let z=||qa,i -qa,j || denotes the distance between unmanned platforms i and j The potential field function φ1 (z) must satisfy:
(a)在z∈(2ra,max,d)上，φ1(z)单调递减，在z∈(d,∞)上，φ1(z)单调递增。在这两区间里φ1(z)可微且非负。(a) On z∈(2ra,max ,d), φ1 (z) decreases monotonically, and on z∈(d,∞), φ1 (z) increases monotonically. In these two intervals, φ1 (z) is differentiable and non-negative.
(b)当z→2ra,max时，φ1(z)→∞；当da＝d时，φ1(z)取得唯一的最小值。(b) When z→2ra,max , φ1 (z)→∞; when da =d, φ1 (z) obtains the unique minimum value.
根据上述定义，建立无人平台之间的势场函数两个范例如下：According to the above definition, two examples of establishing the potential field function between unmanned platforms are as follows:
1)z∈(2ra,max,∞)1) z∈(2ra,max ,∞)
其中，为φ1(z)的导数，k1＝10是调节变量。2)z∈(2ra,max,∞)in, is the derivative of φ1 (z), and k1 =10 is the adjustment variable. 2) z∈(2ra,max ,∞)
其中，为φ1(z)的导数，k1＝10是调节变量。in, is the derivative of φ1 (z), and k1 =10 is the adjustment variable.
范例1)的函数图像如图5所示，图像先下降后上升，在d＝1.6m处达到最小值，在2ra,max＝0.8m处为无穷大。其中横坐标表示无人平台i和j之间的距离，纵坐标表示势场函数φ1(z)的相对大小；The function image of Example 1) is shown in Fig. 5, the image first decreases and then increases, reaches the minimum value at d=1.6m, and reaches infinity at 2ra,max =0.8m. Wherein the abscissa represents the distance between the unmanned platform i and j, and the ordinate represents the relative size of the potential field function φ1 (z);
对于避障，在无人平台相对于障碍的检测范围r′内，当靠得太近时相互排斥，从而达到避障的目的。避障的势场函数定义如下:For obstacle avoidance, within the detection range r′ of the unmanned platform relative to the obstacle, when they are too close to each other, they will repel each other, so as to achieve the purpose of obstacle avoidance. The potential field function for obstacle avoidance is defined as follows:
定义2(避障的势场函数)令z′＝||qa,i-qo,k||表示无人平台i和障碍k之间的距离则势场函数φ2(z′)必须是一个关于z′可微、非负、无界的函数，且满足：Definition 2 (potential field function for obstacle avoidance) let z′=||qa,i -qo,k || denotes the distance between unmanned platform i and obstacle k Then the potential field function φ2 (z′) must be a differentiable, non-negative, unbounded function with respect to z′, and satisfy:
(a)在z′∈(ra,max+ro,k,∞)上，φ2(z′)单调递减。(a) On z′∈(ra,max +ro,k ,∞), φ2 (z′) decreases monotonously.
(b)当z′→ra,max+ro,k时，φ2(z′)→∞。(b) When z′→ra,max +ro,k , φ2 (z′)→∞.
根据上述定义，建立无人平台与障碍之间的势场函数两个范例如下：According to the above definition, two examples of establishing the potential field function between the unmanned platform and the obstacle are as follows:
1)z′∈(ra,max+ro,k,∞)1) z′∈(ra,max +ro,k ,∞)
其中，为φ2(z′)的导数，k2＝15是调节变量。in, is the derivative of φ2 (z′), and k2 =15 is the adjustment variable.
2)z′∈(ra,max+ro,k,∞)2) z′∈(ra,max +ro,k ,∞)
其中，为φ2(z′)的导数，k2＝5是调节变量。in, is the derivative of φ2 (z′), and k2 =5 is the adjustment variable.
范例2)的函数图像如图6所示，图像一直单调减，在ra,max+ro,k＝0.5m处为无穷大。其中横坐标表示无人平台i和障碍k之间的距离，纵坐标表示势场函数φ2(z′)的相大小；The function image of example 2) is shown in Fig. 6, the image always decreases monotonously, and becomes infinity at ra,max +ro,k =0.5m. Wherein the abscissa represents the distance between the unmanned platform i and the obstacle k, and the ordinate represents the phase size of the potential field function φ2 (z′);
步骤六：写出编队、跟随以及避障三个行为的控制律进行叠加Step 6: Write down the control laws of the three behaviors of formation, following and obstacle avoidance for superposition
根据控制要求：①无人平台形成队列，且无人平台间不能发生碰撞；②无人平台整体追踪虚拟领导者,最终与之轨迹相同、速度一致；③无人平台实现避障。因此可以得到每个无人平台的控制量ui(i＝1,2,3)，其为以上三部分行为的叠加。According to the control requirements: ①The unmanned platforms form a queue, and no collisions between unmanned platforms can occur; ②The unmanned platforms track the virtual leader as a whole, and finally have the same trajectory and speed as them; ③The unmanned platforms realize obstacle avoidance. Therefore, the control quantity ui (i=1, 2, 3) of each unmanned platform can be obtained, which is the superposition of the above three parts of behavior.
ui＝fiα+fiβ+fiγui =fiα +fiβ +fiγ
其中，fiα为无人平台编队的控制量，fiβ为无人平台避障的控制量，fiγ为跟随虚拟领导者的反馈控制量。Among them, fiα is the control amount of unmanned platform formation, fiβ is the control amount of unmanned platform obstacle avoidance, and fiγ is the feedback control amount of following the virtual leader.
fiα＝-Σaij[φ1(||qa,i-qa,j||)h(||qa,i-qa,j||)(qa,i-qa,j)+γ(pa,i-pa,j)]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β ＝-∑bik [φ2 (||qa,i -qo,k ||)h(||qa,i -qo,k ||)(qa,i -qo ,k )+βpa,i ]
fiγ＝-α[(qa,i-qr)+γ(pa,i-pr)]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决定无人平台之间以及无人平台与障碍之间是否产生交互。in, is the adjustment function, α, β, γ, ε>0 are the control parameters, this example takes α=2, β=1, γ=2, ε=0.1, aij is the adjacency matrix A∈Rn formed by the unmanned platform The (i,j)th element of×n bik is the (i,k)th element of the adjacency matrix B∈Rn×s formed by the unmanned platform and obstacles The parameters aij and bij determine whether there is interaction between unmanned platforms and between unmanned platforms and obstacles.
可以看到，我们将最终的ui作为第i个Pioneer 3-AT的控制输入，即可实现上述有人/无人平台编队跟随及避障任务。It can be seen that we use the final ui as the control input of the i-th Pioneer 3-AT to realize the above manned/unmanned platform formation following and obstacle avoidance tasks.
当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。Of course, the present invention can also have other various embodiments, and those skilled in the art can make various corresponding changes and deformations according to the present invention without departing from the spirit and essence of the present invention, but these corresponding Changes and deformations should belong to the scope of protection of the appended claims of the present invention.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610989474.2ACN106483958B (en) | 2016-11-10 | 2016-11-10 | A kind of man-machine coordination formation based on obstacle figure and potential field method follows and barrier-avoiding method |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610989474.2ACN106483958B (en) | 2016-11-10 | 2016-11-10 | A kind of man-machine coordination formation based on obstacle figure and potential field method follows and barrier-avoiding method |
| Publication Number | Publication Date |
|---|---|
| CN106483958A CN106483958A (en) | 2017-03-08 |
| CN106483958Btrue CN106483958B (en) | 2018-02-06 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610989474.2AActiveCN106483958B (en) | 2016-11-10 | 2016-11-10 | A kind of man-machine coordination formation based on obstacle figure and potential field method follows and barrier-avoiding method |
| Country | Link |
|---|---|
| CN (1) | CN106483958B (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108628294A (en)* | 2017-03-20 | 2018-10-09 | 北京军石科技有限公司 | A kind of autonomous cooperative control system of multirobot target and its control method |
| CN107015571B (en)* | 2017-05-12 | 2020-06-16 | 南京航空航天大学 | An Algorithm for Tracking and Avoiding Moving Targets for Formation UAVs |
| US10480952B2 (en) | 2018-02-01 | 2019-11-19 | Didi Research America, Llc | Probabilistic navigation system and method |
| CN109597418B (en)* | 2019-02-27 | 2021-03-02 | 福州大学 | Distributed cooperative obstacle avoidance method for robots based on independent virtual center point |
| CN110209167B (en)* | 2019-05-27 | 2021-07-16 | 西安电子科技大学 | A real-time fully distributed multi-robot system formation method |
| CN110488840A (en)* | 2019-08-31 | 2019-11-22 | 武汉理工大学 | A kind of unmanned vehicle formation control method |
| CN110782650B (en)* | 2019-11-06 | 2021-11-23 | 同济人工智能研究院(苏州)有限公司 | Traffic flow distributed cooperative formation control method based on self-adaptive event triggering |
| CN111142533B (en)* | 2020-01-03 | 2023-08-15 | 大连民族大学 | Multi-potential field obstacle avoidance method for multiple unmanned surface vehicles and multi-USV multi-mode formation obstacle avoidance control method in complex environment |
| CN111399539B (en)* | 2020-03-27 | 2022-06-24 | 西北工业大学 | Unmanned aerial vehicle formation obstacle avoidance and collision avoidance control method based on waypoints |
| CN111761581B (en)* | 2020-07-07 | 2021-08-27 | 上海木木聚枞机器人科技有限公司 | Path planning method and device, and narrow space traveling method and device |
| CN112363528B (en)* | 2020-10-15 | 2022-06-14 | 北京理工大学 | Anti-jamming swarm formation control method of UAV based on airborne vision |
| CN113848883B (en)* | 2021-09-06 | 2023-04-18 | 武汉理工大学 | Intelligent ship formation navigation control method, system and device and storage medium |
| CN115657704B (en)* | 2022-08-29 | 2023-12-01 | 广州建通测绘地理信息技术股份有限公司 | Passive obstacle avoidance navigation method and device for aircraft and computer equipment |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5398488B2 (en)* | 2009-11-20 | 2014-01-29 | 村田機械株式会社 | Autonomous mobile object and its control method |
| CN103017757B (en)* | 2012-12-06 | 2015-05-20 | 中联重科股份有限公司 | Engineering machinery entry path planning method and path planning device |
| CN104165627B (en)* | 2014-08-27 | 2017-12-26 | 电子科技大学 | A kind of real-time dynamic route planning method based on linear programming |
| CN105511494B (en)* | 2016-01-20 | 2018-06-19 | 浙江大学 | A kind of method of multiple no-manned plane distributed formation control |
| CN105629974B (en)* | 2016-02-04 | 2018-12-04 | 重庆大学 | A kind of robot path planning method and system based on modified Artificial Potential Field Method |
| Publication number | Publication date |
|---|---|
| CN106483958A (en) | 2017-03-08 |
| Publication | Publication Date | Title |
|---|---|---|
| CN106483958B (en) | A kind of man-machine coordination formation based on obstacle figure and potential field method follows and barrier-avoiding method | |
| CN106444769B (en) | A kind of optimum path planning method of indoor mobile robot increment type environmental information sampling | |
| CN115033016B (en) | Heterogeneous unmanned cluster formation obstacle avoidance method and system | |
| Zhang et al. | Spill detection and perimeter surveillance via distributed swarming agents | |
| CN115373399A (en) | A Path Planning Method for Ground Robots Based on Air-Ground Coordination | |
| CN110162035B (en) | A collaborative motion method for swarm robots in scenes with obstacles | |
| CN113848984B (en) | Unmanned aerial vehicle cluster control method and system | |
| CN115494866A (en) | A method and system for multi-UAV global and local path intelligent planning | |
| CN119469168B (en) | Autonomous navigation method and system of quadruped robot for special environments | |
| Li et al. | A behavior-based mobile robot navigation method with deep reinforcement learning | |
| CN113156954A (en) | Multi-agent cluster obstacle avoidance method based on reinforcement learning | |
| Ahmed et al. | An energy efficient IoD static and dynamic collision avoidance approach based on gradient optimization | |
| AbuJabal et al. | A comprehensive study of recent path-planning techniques in dynamic environments for autonomous robots | |
| Yang et al. | Smart autonomous moving platforms | |
| Zhang et al. | A formation cooperative reconnaissance strategy for multi-UGVs in partially unknown environment | |
| Politi et al. | Path planning and landing for unmanned aerial vehicles using ai | |
| Saska et al. | Roads sweeping by unmanned multi-vehicle formations | |
| CN111580563B (en) | A Seed Search-Based Autonomous Obstacle-avoiding Flight Method for Unmanned Aerial Vehicles | |
| Jung et al. | Enabling operational autonomy for unmanned aerial vehicles with scalability | |
| Chen et al. | A novel navigation system for an autonomous mobile robot in an uncertain environment | |
| Oyana et al. | Three-layer multi-uavs path planning based on ROBL-MFO | |
| CN115390589B (en) | Unmanned aerial vehicle cluster control method and device, electronic equipment and storage medium | |
| CN111897340A (en) | A method for long-distance autonomous navigation of intelligent robots | |
| CN117666556A (en) | A collaborative path planning and obstacle avoidance method for heterogeneous unmanned clusters | |
| CN116718190A (en) | Mobile robot path planning method in long-distance dense crowd scene |
| 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 |