[发明专利]一种基于障碍图和势场法的人机协同编队跟随及避障方法

摘要

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

主权项

一种基于障碍图和势场法的人机协同编队跟随及避障方法，其特征在于，包括以下步骤：步骤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个无人平台的控制量输入，实现有人/无人平台编队、跟随及避障任务。