CN112650214B - 一种集群***动态编队的队形控制方法 - Google Patents
一种集群***动态编队的队形控制方法 Download PDFInfo
- Publication number
- CN112650214B CN112650214B CN202011307984.XA CN202011307984A CN112650214B CN 112650214 B CN112650214 B CN 112650214B CN 202011307984 A CN202011307984 A CN 202011307984A CN 112650214 B CN112650214 B CN 112650214B
- Authority
- CN
- China
- Prior art keywords
- path
- formation
- cluster
- obstacle
- following
- 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.)
- Active
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 97
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000004891 communication Methods 0.000 claims abstract description 21
- 230000008569 process Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims abstract description 14
- 230000007613 environmental effect Effects 0.000 claims abstract description 13
- 230000004888 barrier function Effects 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 23
- 239000000178 monomer Substances 0.000 claims description 19
- 239000013598 vector Substances 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 8
- 230000005855 radiation Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000002147 killing effect Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000000465 moulding Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
Images
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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- 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
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
技术领域
本发明涉及集群智能体***编队控制技术领域,特别涉及一种集群***动态编队的队形控制方法。
背景技术
集群***采用集群的方式可以拓展单体***的能力,高效合作协调完成任务,各单体之间互相功能备份,可以避免单体失效造成任务失败。集群***一般都需要通过保持一定的编队队形来完成任务,且编队队形的精度可能会直接影响任务的完成与否。目前集群编队可分为静态与动态两种。静态编队通过在地面站根据编队造型与变换预先为各单体规划好航迹,并为各航迹点设置统一的时间戳,将规划的航迹存储在各单体的内部存储设备中,在编队任务开始时由地面站广播发送启动命令,后续每个集群单体可以按照时间戳同步跟踪预设路径,可以获得很高的编队精度,且运动过程无需通信,抗干扰能力强,但是在编队过程中无法更改编队信息,只能用于编队表演或者固定路径巡检等。
对于动态编队,目前集群编队路径大部分算法研究中,均采用从起点到终点的最短路径搜索,一般只考虑避开障碍的空间位置,缺乏障碍的类型判断与预处理。因此寻找一种合适的方法对障碍信息进行更合理的分类判断,与路径规划前的预处理,可以使得规划的路径更为合理安全。而动态编队需要的高精度的队形保持与跟踪,通常由集群***中的各单体之间获取位置信息后保持相对位置,但是这样的方式需要时刻获取集群***中其余单体之间的状态信息,在通信延时或者短暂丢失均会对编队队形与精度造成影响。因此改进动态编队中的编队信息的获取方式,提升复杂曲线编队精度是一个急需解决的问题。
发明内容
本发明的目的是提供一种集群***动态编队的队形控制方法,以改进无人机、无人车、无人船等集群***编队队形精度不高的问题。
为了解决以上问题,本发明通过以下技术方案实现:
一种集群***动态编队的队形控制方法,包括:步骤S1、建立环境障碍威胁地图。步骤S2、根据所述环境障碍威胁地图为集群***中的领航主机获取最优路径。步骤S3、对所述最优路径进行参数化处理,得到第一路径信息。步骤S4、对所述第一路径信息进行优化处理得到第二路径信息,所述第二路径信息满足预设的运动学约束条件与避障要求条件。步骤S5、根据所述第二路径信息为所述集群***中的每个跟随从机分配独立的跟随路径,并将所述跟随路径参数化后再通过通信数据链在所述集群***中同步。步骤S6、所述领航主机以恒定速度Vd跟随所述第二路径信息,若在所述集群***内通信丢失且无法获得所述集群***的状态信息时,跟随从机也以所述恒定速度Vd跟随路径来保持编队;若在所述集群***内通信正常时,则在所述恒定速度Vd的基础上,通过速度协调控制器得到跟随从机的协调速度Vr。
优选地,还包括:步骤S7、所述集群***通过搭载的敏感器在运动过程中实时探测环境信息,探测到新的障碍物后,为所述障碍物设定影响半径ri,为进入此影响半径ri的集群单体进行避障控制,在所述恒定速度Vd或者所述协调速度Vr基础上,增加避障加速度ai。
优选地,还包括:步骤S8、在编队过程中,根据环境复杂性与地面控制输入信息,定周期或者变周期搜索新的所述最优路径,并根据最新的所述最优路径重复步骤S3~S7。
优选地,所述步骤S1包括:将从起点到终点的方形区域划分为n*n的网格,每个所述网格为一个节点。
基于障碍类型确定不可通过区域,根据所述障碍类型的距离方位信息为每一网格设定不同的威胁系数,将每个所述威胁系数进行叠加处理后得到一个n*n的障碍威胁系数矩阵。
优选地,采用启发式算法获取所述最优路径:以叠加了所述障碍威胁系数矩阵中相应的障碍威胁系数与到终点的距离函数作为启发值。以从起点按照最小启发值路径到达当前位置的路径长度作为耗费值。将所述耗费值与所述启发值之和作为每一所述节点的代价值。从起点开始计算当前节点的周围节点的代价值,并从其中最小代价值点继续搜索,直到达到目标点,将各最小代价值点连接,得到所述最优路径。
优选地,所述第一路径信息H(x)采用如下公式进行计算:
式中,xi和yi分别是第i个原始规划路径点的x轴坐标和y轴坐标;qi表示原始规划路径点i的导数;xi+1和yi+1分别是第i+1个原始规划路径点的x轴坐标和y轴坐标;qi+1表示第i+1个原始规划路径点的导数。
优选地,所述步骤S4包括:
步骤S4.1、在第一路径信息中发生碰撞的下一路径点,新增该路径点前后两条原始路径点的直线连线中点。
步骤S4.2、再重新搜索新的路径得到所述第二路径信息。
步骤S4.3、重复所述步骤S4.1~步骤S4.2,直到所述第二路径信息满足所述运动学约束与避障要求。
优选地,所述步骤S5包括:以所述领航主机的质心为原点,速度切线方向为x轴,法线方向为y轴,在领航主机的所述第二路径信息的基础上叠加坐标偏移量,得到每一所述跟随从机的参数化的所述跟随路径。
所述坐标偏移量dij采用如下公式进行表示:
dij=(xoff_ij,yoff_ij,zoff_ij)
式中,xoff_ij表示表示集群单机i到j之间的x坐标向量,yoff_ij表示表示集群单机i到j之间的y坐标向量,zoff_ij表示表示集群单机i到j之间的z坐标向量。
优选地,所述步骤S6包括:所述跟随从机的协调速度Vr通过如下公式计算:
式中,||Vd||∈(vmin+Δv,vmax-Δv),f(x)=2/(1+e(-x))-1,f(x)∈(-1,1),最终的协调速度满足Vr∈(vmin,vmax),vmin表示集群单机的最小运动速度;vmax表示集群单机的最大运动速度;Δv表示可调速度量大小;e(-x)表示表示自然指数e的-x次方。
优选地,所述步骤S7包括:所述避障加速度ai通过如下公式计算:
式中,b和c为控制力系数,ρij为集群单体i和障碍j的距离;所述避障加速度ai的方向由障碍点指向所述集群单体i。
当探测的所述新的障碍物存在运动速度时,需要调整避障加速度ai的方向。
调整方法为当进入所述障碍物的影响半径ri的集群单体与所述障碍物的运动速度矢量相交时:从受影响的所述集群单体速度方向观察,如果所述集群单体的跟随路径在受影响的所述集群单体的右方,则将避障加速度ai的方向逆时针转动90度。从受影响的所述集群单体速度方向观察,如果所述集群单体的跟随路径在受影响的所述集群单体的左方,则将避障加速度ai的方向顺时针转动90度。
本发明至少具有以下优点之一:
本发明相比于传统的编队控制方法,本发明通过将障碍信息分类,引入威胁系数假设,因此规划的路径信息更合理安全;对编队过程中的动态障碍,根据障碍运动方向进行合理的优化规避,路径更优;将搜索的路径信息以分段的参数化数据描述并周期性更新,可以保证在丢失通信的一段时间内编队队形不受影响,且参数化数据可以实现对编队路径精准的描述,编队控制器可以达到更高的编队控制精度。因此可以降低通信延时与短暂丢失对编队队形的干扰,并大大提升集群***跟踪复杂曲线路径的精度与稳定性。本发明应用于无人机、无人车、无人船等集群***的高精度编队队形控制。
附图说明
图1是本发明一实施例提供的一种集群***动态编队的队形控制方法流程图;
图2是本发明一实施例提供的分段曲线公共点导数求解示意图;
图3是本发明一实施例提供的参数化路径优化调整示意图;
图4是本发明一实施例提供的集群编队队形坐标系示意图;
图5是本发明一实施例提供的动态障碍避障加速度调整方向示意图。
具体实施方式
以下结合附图和具体实施方式对本发明提出的一种集群***动态编队的队形控制方法作进一步详细说明。根据下面说明,本发明的优点和特征将更清楚。需要说明的是,附图采用非常简化的形式且均使用非精准的比例,仅用以方便、明晰地辅助说明本发明实施方式的目的。为了使本发明的目的、特征和优点能够更加明显易懂,请参阅附图。须知,本说明书所附图式所绘示的结构、比例、大小等,均仅用以配合说明书所揭示的内容,以供熟悉此技术的人士了解与阅读,并非用以限定本发明实施的限定条件,故不具技术上的实质意义,任何结构的修饰、比例关系的改变或大小的调整,在不影响本发明所能产生的功效及所能达成的目的下,均应仍落在本发明所揭示的技术内容能涵盖的范围内。
如图1所示,本实施例提供的一种集群***动态编队的队形控制方法,包括:步骤S1、建立环境障碍威胁地图。即建立环境障碍威胁地图,得到n*n的障碍威胁系数矩阵。所述步骤S1包括:将从起点到终点的方形区域划分为n*n的网格。每个所述网格为一个节点;每一所述网格的大小根据集群***具体组成单元的物理尺寸与编队精度确定。
基于障碍类型(障碍物类型)确定不可通过区域(例如将树木、建筑等障碍的物理空间范围设为不可通过区域),根据所述障碍类型(不同障碍的)的距离方位信息为每一网格设定不同的威胁系数,将每个所述威胁系数进行叠加处理后得到一个n*n的障碍威胁系数矩阵(将为每个网格内的障碍物分配的威胁系数叠加处理后得到一个n*n的障碍威胁系数矩阵)。离障碍(障碍物)越近威胁系数越大,表明路径规划算法越应该避开此区域。例如对于辐射源障碍,各障碍的物理空间不可通过区域大小相等,但是辐射强度不同,因此通过为辐射强度更大的辐射源障碍设定更大的威胁系数,可以在两个不同强度辐射源之间规划更为合理的路径。
步骤S2、根据所述环境障碍威胁地图为集群***中的领航主机获取最优路径(基于环境障碍威胁地图为领航主机搜索最优路径)。
所述步骤S2包括:在进行最优路径搜索时,只考虑目前已知的环境障碍信息,不考虑后续集群***的敏感器探测到的障碍。避开这些障碍到达终点存在多条可用的路径,但是考虑到集群***的性能约束,需要寻找一条最优路径。
采用启发式算法搜索所述最优路径:以叠加了所述障碍威胁系数矩阵中相应的障碍威胁系数与到终点的距离函数作为启发值h(n):
式中,a为权重系数,wi为第i个障碍的威胁系数,i=1,2,…,k,dmax是路径搜索过程中集群单体(例如:无人机)与障碍中心距离可能的最大值,d(pi)是集群单体与第i个障碍的中心距离,x,y为当前搜索位置坐标,xd,yd为终点坐标,n表示第n个节点。
以从起点按照最小启发值h(n)路径到达当前位置的路径长度作为耗费值,耗费值与启发值之和为每一所述节点的代价值。从起点开始计算周围节点(周围节点是指当前节点上下左右以及4个斜对角一共8个所述节点。)的代价值,并从其中最小代价值点继续搜索,直到达到目标点,将各最小代价值点连接即可得到搜索的所述最优路径。
即,所述步骤S2基于威胁地图为领航主机进行最优路径搜索,综合考虑最短路径与最小障碍威胁获得综合的最优路径。
步骤S3、对所述最优路径进行参数化处理,得到第一路径信息(参数化规划路径信息)。
上述得到的最优路径是离散化的,且各离散路径是由分段折线组成,离散路径可能存在较大转角。因此将上述最优路径分段采用多项式拟合后实现参数化,为了保证集群***在满足运动学约束的前提下跟踪规划路径,参数化后的路径需要满足以下2点:第一点为经过原始路径点;第二点为参数化后的各分段路径在原始公共路径点处导数相同。如图2所示,参数化后的路径在原始路径点处的切线与离散路径点折线的角平分线垂直,可由此求出该导数。参数后的路径(第一路径信息)H(x)由此求出:
式中,xi和yi分别是第i个原始规划路径点的x轴坐标和y轴坐标;qi表示原始规划路径点i的导数;xi+1和yi+1分别是第i+1个原始规划路径点的x轴坐标和y轴坐标;qi+1表示第i+1个原始规划路径点的导数。
即,所述步骤S3通过多项式拟合,将上述路径进行参数化,以便通过通信网传播。
步骤S4、对所述第一路径信息进行优化处理得到第二路径信息(参数化后的路径信息再优化),所述第二路径信息满足预设的运动学约束条件与避障要求条件。
具体的,运动学约束条件:||a||≤amax,||v||≤vmax,v、a和λ分别为集群单机(例如无人机)速度、加速度和转弯半径,vmax、amax和λmax分别为最大速度、最大加速度和最大转弯半径。避障要求条件:d<Rmin。d和Rmin分别是集群单机与障碍的距离和障碍的影响半径,进入此半径会发生碰撞。
所述步骤S4包括:如图3所示,参数化后的路径可能存在与障碍物再次碰撞的可能。采用如下优化方案:
步骤S4.1、在第一路径(第一路径信息)中发生碰撞的下一路径点,新增该路径点前后两条原始路径点的直线连线中点。
步骤S4.2、再重新搜索新的路径得到所述第二路径(第二路径信息)。
这里搜索就是增加两个路径点后按照上述公式H(x)重新进行计算得到新的第二路径信息。
步骤S4.3、重复所述步骤S4.1~步骤S4.2,直到所述第二路径信息满足所述运动学约束与避障要求。
即,所述步骤S4是将上述参数化后的路径信息再优化,直到优化后的曲线满足运动学约束与避障要求。
步骤S5、根据所述第二路径信息为所述集群***中的每个跟随从机分配独立的跟随路径,并将所述跟随路径参数化后再通过通信数据链在所述集群***中同步(集群***的编队队形分配)。
所述步骤S5包括:在上述为领航主机搜索获得的参数化路径基础上,增加坐标偏移量,为集群***中每个跟随从机分配独立的跟随路径,然后将此跟随路径的分段曲线参数以参数矩阵的形式通过通信数据链在集群***中同步。
如图4所示,以领航主机质心为原点,速度切线方向为x轴,法线方向为y轴,在所述领航主机的第二路径信息的基础上叠加坐标偏移量dij=(xoff_ij,yoff_ij,zoff_ij),得到每一所述跟随从机的参数化的所述跟随路径(得到其余跟随从机的路径);
所述坐标偏移量dij采用如下公式进行表示:
dij=(xoff_ij,yoff_ij,zoff_ij) (3)
式中,xoff_ij表示表示集群单机i到j之间的x坐标向量,yoff_ij表示表示集群单机i到j之间的y坐标向量,zoff_ij表示表示集群单机i到j之间的z坐标向量。
即,所述步骤S5通过编队信息为其余跟随从机分配跟随路径并参数化。所述编队信息指的是集群***的队形信息,即集群***应该按照什么样的队形布置,如一字型,人字形,五角星型等。编队信息是通过坐标偏移量dij来体现的。
步骤S6、编队路径协调控制,所述领航主机以恒定速度Vd跟随编队路径(第二路径信息),若在所述集群***内通信丢失且无法获得集群***的状态信息时,跟随从机也以所述恒定速度Vd跟随路径来保持编队;若在所述集群***内通信正常时,则在所述恒定速度Vd的基础上,通过速度协调控制器得到跟随从机的协调速度Vr,以进一步提高编队精度。即,所述步骤S6是根据通信条件,通过速度控制量为编队路径完成队形协调控制。
所述集群***的状态信息指集群中各单机的姿态、位置、速度、数据链信号强度、电量、任务进度等信息。
所述跟随从机的协调速度Vr通过如下公式计算:
式中,||Vd||∈(vmin+Δv,vmax-Δv),f(x)=2/(1+e(-x))-1,f(x)∈(-1,1),最终的协调速度满足Vr∈(vmin,vmax),vmin表示集群单机的最小运动速度;vmax表示集群单机的最大运动速度;Δv表示可调速度量大小;e(-x)表示表示自然指数e的-x次方。
本实施例还包括:步骤S7、障碍避障控制,即所述集群***通过搭载的敏感器在运动过程中实时探测环境信息,探测到新的障碍物后,为所述障碍物设定影响半径ri,为进入此影响半径ri的集群单体进行避障控制,在所述恒定速度Vd或者所述协调速度Vr基础上,增加避障加速度ai。
所述避障加速度ai通过如下公式计算:
式中,b和c为控制力系数,ρij为集群单体i和障碍j的距离;所述避障加速度ai的方向由障碍点指向所述集群单体i。
如图5所示,图a、b情况下斥力逆时针转动90度;图c、d情况下顺时针转动90度。
具体的,当探测的所述新的障碍物存在运动速度时,需要调整避障加速度ai的方向。
调整方法为当进入所述障碍物的影响半径ri的集群单体与所述障碍物的运动速度矢量相交时:
从受影响的所述集群单体速度方向观察,如果目标路径(所述集群单体的跟随路径)在受影响的所述集群单体的右方,则将避障加速度ai的方向逆时针转动90度(则将避障加速度乘以90度方向余弦矩阵R(90))。
从受影响的所述集群单体速度方向观察,如果目标路径(所述集群单体的跟随路径)在受影响的所述集群单体的左方,则将避障加速度ai的方向顺时针转动90度(则将避障加速度乘以-90度方向余弦矩阵R(-90))。
即,所述步骤S7是对编队过程中后加入的障碍完成避障控制。
本实施例还包括:步骤S8、更新路径信息,即在编队过程中,根据环境复杂性与地面控制输入信息,定周期或者变周期搜索新的所述最优路径,并根据最新的所述最优路径重复步骤S3~S7(并将最新的路径参数化矩阵在集群***中更新同步)。所述步骤S8是指周期更新路径信息,并将最新的路径参数化矩阵在集群***中更新同步。
本实施例研究的对象基于动态编队。动态编队通过数据链可以在运动过程中根据地面控制输入与地理环境信息,实时改变编队队形与航迹,因此编队更加灵活,具备更强的适用性,但是通信延迟、丢失,带宽以及外界干扰都会影响编队精度。由此可知,本实施例是一种适用于无人机、无人车、无人船等集群***在复杂空间,避开障碍情况下实现复杂曲线路径跟踪的高精度编队队形控制方法。
本实施例通过将障碍信息分类,引入威胁系数假设,因此规划的路径信息更合理安全;对编队过程中的动态障碍,根据障碍运动方向进行合理的优化规避,路径更优;将搜索的路径信息以分段的参数化数据描述并周期性更新,可以保证在丢失通信的一段时间内编队队形不受影响,且参数化数据可以实现对编队路径精准的描述,编队控制器可以达到更高的编队控制精度。因此可以降低通信延时与短暂丢失对编队队形的干扰,并大大提升集群***跟踪复杂曲线路径的精度与稳定性。
需要说明的是,在本文中,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
应当注意的是,在本文的实施方式中所揭露的装置和方法,也可以通过其他的方式实现。以上所描述的装置实施方式仅仅是示意性的,例如,附图中的流程图和框图显示了根据本文的多个实施方式的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用于执行规定的功能或动作的专用的基于硬件的***来实现,或者可以用专用硬件与计算机指令的组合来实现。
尽管本发明的内容已经通过上述优选实施例作了详细介绍,但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后,对于本发明的多种修改和替代都将是显而易见的。因此,本发明的保护范围应由所附的权利要求来限定。
Claims (7)
1.一种集群***动态编队的队形控制方法,其特征在于,包括:
步骤S1、建立环境障碍威胁地图;
步骤S2、根据所述环境障碍威胁地图为集群***中的领航主机获取最优路径;
步骤S3、对所述最优路径进行参数化处理,得到第一路径信息;
所述第一路径信息H(x)采用如下公式进行计算:
式中,xi和yi分别是第i个原始规划路径点的x轴坐标和y轴坐标;qi表示原始规划路径点i的导数;xi+1和yi+1分别是第i+1个原始规划路径点的x轴坐标和y轴坐标;qi+1表示第i+1个原始规划路径点的导数;
步骤S4、对所述第一路径信息进行优化处理得到第二路径信息,所述第二路径信息满足预设的运动学约束条件与避障要求条件;
步骤S5、根据所述第二路径信息为所述集群***中的每个跟随从机分配独立的跟随路径,并将所述跟随路径参数化后再通过通信数据链在所述集群***中同步;
步骤S6、所述领航主机以恒定速度Vd跟随所述第二路径信息,若在所述集群***内通信丢失且无法获得所述集群***的状态信息时,跟随从机也以所述恒定速度Vd跟随路径来保持编队;若在所述集群***内通信正常时,则在所述恒定速度Vd的基础上,通过速度协调控制器得到跟随从机的协调速度Vr;
所述步骤S6包括:所述跟随从机的协调速度Vr通过如下公式计算:
式中,||Vd||∈(vmin+Δv,vmax-Δv),f(x)=2/(1+e(-x))-1,f(x)∈(-1,1),最终的协调速度满足Vr∈(vmin,vmax),vmin表示集群单机的最小运动速度;vmax表示集群单机的最大运动速度;Δv表示可调速度量大小;e(-x)表示表示自然指数e的-x次方;
步骤S7、所述集群***通过搭载的敏感器在运动过程中实时探测环境信息,探测到新的障碍物后,为所述障碍物设定影响半径ri,为进入此影响半径ri的集群单体进行避障控制,在所述恒定速度Vd或者所述协调速度Vr基础上,增加避障加速度ai。
2.如权利要求1所述的集群***动态编队的队形控制方法,其特征在于,还包括:步骤S8、在编队过程中,根据环境复杂性与地面控制输入信息,定周期或者变周期搜索新的所述最优路径,并根据最新的所述最优路径重复步骤S3~S7。
3.如权利要求2所述的集群***动态编队的队形控制方法,其特征在于,所述步骤S1包括:
将从起点到终点的方形区域划分为n*n的网格,每个所述网格为一个节点;
基于障碍类型确定不可通过区域,根据所述障碍类型的距离方位信息为每一网格设定不同的威胁系数,将每个所述威胁系数进行叠加处理后得到一个n*n的障碍威胁系数矩阵。
4.如权利要求3所述的集群***动态编队的队形控制方法,其特征在于,
采用启发式算法获取所述最优路径:以叠加了所述障碍威胁系数矩阵中相应的障碍威胁系数与到终点的距离函数作为启发值;
以从起点按照最小启发值路径到达当前位置的路径长度作为耗费值;
将所述耗费值与所述启发值之和作为每一所述节点的代价值;
从起点开始计算当前节点的周围节点的代价值,并从其中最小代价值点继续搜索,直到达到目标点,将各最小代价值点连接,得到所述最优路径。
5.如权利要求4所述的集群***动态编队的队形控制方法,其特征在于,所述步骤S4包括:
步骤S4.1、在第一路径信息中发生碰撞的下一路径点,新增该路径点前后两条原始路径点的直线连线中点;
步骤S4.2、再重新搜索新的路径得到所述第二路径信息;
步骤S4.3、重复所述步骤S4.1~步骤S4.2,直到所述第二路径信息满足所述运动学约束与避障要求。
6.如权利要求5所述的集群***动态编队的队形控制方法,其特征在于,所述步骤S5包括:
以所述领航主机的质心为原点,速度切线方向为x轴,法线方向为y轴,在领航主机的所述第二路径信息的基础上叠加坐标偏移量,得到每一所述跟随从机的参数化的所述跟随路径;
所述坐标偏移量dij采用如下公式进行表示:
dij=(xoff_ij,yoff_ij,zoff_ij)
式中,xoff_ij表示表示集群单机i到j之间的x坐标向量,yoff_ij表示表示集群单机i到j之间的y坐标向量,zoff_ij表示表示集群单机i到j之间的z坐标向量。
7.如权利要求1所述的集群***动态编队的队形控制方法,其特征在于,所述步骤S7包括:所述避障加速度ai通过如下公式计算:
式中,b和c为控制力系数,ρij为集群单体i和障碍j的距离;所述避障加速度ai的方向由障碍点指向所述集群单体i;
当探测的所述新的障碍物存在运动速度时,需要调整避障加速度ai的方向;
调整方法为当进入所述障碍物的影响半径ri的集群单体与所述障碍物的运动速度矢量相交时:
从受影响的所述集群单体速度方向观察,如果所述集群单体的跟随路径在受影响的所述集群单体的右方,则将避障加速度ai的方向逆时针转动90度;
从受影响的所述集群单体速度方向观察,如果所述集群单体的跟随路径在受影响的所述集群单体的左方,则将避障加速度ai的方向顺时针转动90度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011307984.XA CN112650214B (zh) | 2020-11-20 | 2020-11-20 | 一种集群***动态编队的队形控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011307984.XA CN112650214B (zh) | 2020-11-20 | 2020-11-20 | 一种集群***动态编队的队形控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112650214A CN112650214A (zh) | 2021-04-13 |
CN112650214B true CN112650214B (zh) | 2022-12-23 |
Family
ID=75349539
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011307984.XA Active CN112650214B (zh) | 2020-11-20 | 2020-11-20 | 一种集群***动态编队的队形控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112650214B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114061573B (zh) * | 2021-11-16 | 2024-03-22 | 中国人民解放军陆军工程大学 | 地面无人车辆编队定位装置及方法 |
CN113848957B (zh) * | 2021-11-17 | 2023-11-10 | 中国人民解放军陆军工程大学 | 地面无人车辆编队控制装置及方法 |
CN114384929B (zh) * | 2021-12-02 | 2023-09-12 | 上海航天控制技术研究所 | 一种基于偏差优化启发式算法的无人集群编队控制方法 |
CN114020036B (zh) * | 2021-12-03 | 2023-12-12 | 南京大学 | 一种多无人机编队阵型变换时的防碰撞方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101685309A (zh) * | 2008-09-24 | 2010-03-31 | 中国科学院自动化研究所 | 多机器人协调队形控制方法 |
CN103941728A (zh) * | 2014-04-24 | 2014-07-23 | 北京航空航天大学 | 一种无人机密集自主编队的队形变换方法 |
CN108594846A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种障碍环境下多auv编队队形优化控制方法 |
CN108594845A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种通信限制下基于预测控制的多auv编队方法 |
CN108663939A (zh) * | 2018-06-12 | 2018-10-16 | 哈尔滨工程大学 | 考虑通讯丢包的uuv集群水平面路径跟踪控制方法 |
CN111399537A (zh) * | 2020-03-27 | 2020-07-10 | 西北工业大学 | 一种基于航路点的分布式无人机动态编队切换方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9927816B2 (en) * | 2016-05-13 | 2018-03-27 | Macau University Of Science And Technology | System and method for operating a follower vehicle in a vehicle platoon |
-
2020
- 2020-11-20 CN CN202011307984.XA patent/CN112650214B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101685309A (zh) * | 2008-09-24 | 2010-03-31 | 中国科学院自动化研究所 | 多机器人协调队形控制方法 |
CN103941728A (zh) * | 2014-04-24 | 2014-07-23 | 北京航空航天大学 | 一种无人机密集自主编队的队形变换方法 |
CN108594846A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种障碍环境下多auv编队队形优化控制方法 |
CN108594845A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种通信限制下基于预测控制的多auv编队方法 |
CN108663939A (zh) * | 2018-06-12 | 2018-10-16 | 哈尔滨工程大学 | 考虑通讯丢包的uuv集群水平面路径跟踪控制方法 |
CN111399537A (zh) * | 2020-03-27 | 2020-07-10 | 西北工业大学 | 一种基于航路点的分布式无人机动态编队切换方法 |
Non-Patent Citations (3)
Title |
---|
Explicit Model Following Distributed Control Scheme for Formation Flying of Mini UAVs;Qasim Ali;《IEEE Access》;20160113;第397-406页 * |
基于领航通信的多机器人队形控制的算法设计;刘宪力;《企业导报》;20090415;第189-190页 * |
多无人机航迹规划的自适应B样条算法;彭皓月;《计算机工程与应用》;20190517;摘要,第264-265页第4.1-4.3节 * |
Also Published As
Publication number | Publication date |
---|---|
CN112650214A (zh) | 2021-04-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112650214B (zh) | 一种集群***动态编队的队形控制方法 | |
CN110440811B (zh) | 一种通用型自主导航控制方法、装置及设备终端 | |
CN111208829B (zh) | 基于分布式预设时间状态观测器的多移动机器人编队方法 | |
Syed et al. | Coordinated vehicle platoon control: Weighted and constrained consensus and communication network topologies | |
Marin et al. | Event-based localization in ackermann steering limited resource mobile robots | |
CN112904723B (zh) | 非匹配干扰下的空地固定时间协同容错编队控制方法 | |
Xu et al. | Two-layer distributed hybrid affine formation control of networked Euler–Lagrange systems | |
CN110580740A (zh) | 多智能体协同三维建模方法及装置 | |
CN112859110B (zh) | 一种基于三维激光雷达的定位导航方法 | |
CN114706421B (zh) | 一种多无人机编队穿越框形障碍物方法 | |
CN116501056A (zh) | 车辆轨迹跟踪控制方法和装置、无人车和存储介质 | |
Kanchwala et al. | Development of an intelligent transport system for EV | |
Chen et al. | A formation control method for AUV group under communication delay | |
WO2021217341A1 (zh) | 避障方法、可移动平台、控制设备和存储介质 | |
Chen et al. | Provably safe and robust drone routing via sequential path planning: A case study in san francisco and the bay area | |
CN116466356A (zh) | 一种多激光全局定位方法和*** | |
CN112506203B (zh) | 一种机器人运动动态反馈方法及*** | |
Ritter et al. | Decentralized dynamic data-driven monitoring of atmospheric dispersion processes | |
Zhang et al. | A modified FastSLAM for an autonomous mobile robot | |
Lacroix et al. | Integration of concurrent localization algorithms for a planetary rover | |
CN112747752A (zh) | 基于激光里程计的车辆定位方法、装置、设备和存储介质 | |
Chen et al. | An Intelligent Navigation Control Approach for Autonomous Unmanned Vehicles via Deep Learning-Enhanced Visual SLAM Framework | |
Wang et al. | A dynamics controller design method for car-like mobile robot formation control | |
Aguiar et al. | Coordinated path-following of multiple underactuated autonomous vehicles with bidirectional communication constraints | |
Yang et al. | Optimal multi-sensor multi-vehicle (MSMV) localization and mobility tracking |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |