CN112083727B - 基于速度障碍物的多自主体系统分布式避碰编队控制方法 - Google Patents

基于速度障碍物的多自主体系统分布式避碰编队控制方法 Download PDF

Info

Publication number
CN112083727B
CN112083727B CN202010925182.9A CN202010925182A CN112083727B CN 112083727 B CN112083727 B CN 112083727B CN 202010925182 A CN202010925182 A CN 202010925182A CN 112083727 B CN112083727 B CN 112083727B
Authority
CN
China
Prior art keywords
speed
obstacle
formation
autonomous
self
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
Application number
CN202010925182.9A
Other languages
English (en)
Other versions
CN112083727A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN202010925182.9A priority Critical patent/CN112083727B/zh
Publication of CN112083727A publication Critical patent/CN112083727A/zh
Application granted granted Critical
Publication of CN112083727B publication Critical patent/CN112083727B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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/0291—Fleet control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • 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/0088—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
    • 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/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Business, Economics & Management (AREA)
  • Game Theory and Decision Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种基于速度障碍物的多自主体分布式避碰编队控制方法，该方法包括以下步骤：步骤1：对环境动静态障碍物运动模型和自主体运动模型基于微分方程建模；步骤2：在无向联通通讯拓扑下，设计有限速度的标称分布式编队跟踪控制器及选择控制器参数；步骤3：非合作自主体及动静态障碍物间避免碰撞速度障碍物构造；步骤4：合作自主体间相互避免碰撞速度障碍物构造；步骤5：基于局部二次最优问题(QP)求解有限速度多自主体分布式避碰编队控制器，在自主体速度有界约束条件下，实现与环境中静动态障碍物以及其他自主体无碰撞情况下编队跟踪控制。

Description

基于速度障碍物的多自主体系统分布式避碰编队控制方法
技术领域：
本发明涉及一种速度受限多自主体系统分布式避碰编队控制器设计方法，基于提出的速度受限分布式避碰编队控制算法，多自主体系统可在有限速度约束下无碰撞的实现分布式编队跟踪控制。通过设置不同的目标编队构型以及跟踪轨迹，该方法能使多自主体依据初始位置匹配到最优理想编队位置，进而在有限速度约束下，与环境中静动态障碍物及其他自主体间无碰撞的形成指定编队队形并对目标轨迹完成跟踪，该发明属于协同控制技术领域。
背景技术：
随着传感、通信及计算技术的快速发展，各类自主体设备使用成本大大降低。多自主体系统通过协同合作，在勘测、测绘以及搜救等任务中相对单个自主体可大大缩减任务时间，提高任务完成效率。多自主体系统的诸多应用优势引起了研究人员对多自主体系统协同控制的广泛关注。随着多自主体系统规模的不断扩大，传统的集中式多自主体系统控制方法因其在通信以及计算能力上的瓶颈变得不再适用。分布式多自主体系统控制方法通过将通信以及计算要求分配到每个自主体，各自主体只需与部分邻居自主体进行通信，减轻了单个自主体的通信以及计算负担，因此更适合于大规模多自主体系统的协同控制。
分布式多自主体系统协同控制内容包括一致性控制、编队控制以及蜂拥控制等。编队控制作为一种基本的控制任务，其目标在于通过每个自主体与局部邻居间的相互作用，使得所有自主体形成指定的编队构型。编队控制应用广泛，除上面提到的搜救、测绘等任务，还可用于目标追踪以及协同搬运等方面。基于一致性的分布式编队控制方法因其简易性以及灵活性逐渐成为当前最常用的分布式编队控制方法。一种基于一致性的固定编队控制方法见[1](参见Ren W,Beard RW,Atkins EM,et al.Information consensus inmultivehicle cooperative control[J].IEEE Control Systems Magazine,2007,27(2):71-82.)。对于时变的编队构型，一种典型控制方法见[2](参见Brinonarranz L,Seuret A,Canudasdewit C,etal.Cooperative Control Design for Time-Varying Formations ofMulti-Agent Systems[J].IEEE Transactions on Automatic Control,2014,59(8):2283-2288.)。实际应用中，编队控制任务除了要求自主体形成并保持编队，通常还希望整个编队能够沿着期望轨迹移动，因此分布式编队跟踪控制研究尤为重要。一种编队跟踪控制方式为指定多自主体系统编队中心的期望轨迹，并在此过程中保证自主体系统满足理想相对位置约束，即保证队形稳定。一种编队跟踪控制方法见[3](参见Antonelli G,Arrichiello F,Caccavale F,etal.Decentralized centroid and formation controlfor multi-robot systems[C].international conference on robotics andautomation,2013:3511-3516.)上述方法从理论上考虑了多自主体系统编队形成以及编队中心对指定轨迹的跟踪，但是实际多自主体系统在运动过程中还需要考虑自主体与环境障碍物间的避碰要求。一种常见的避碰控制策略为引入势能函数来保证多自主体系统在实现编队的过程中避免相互碰撞，见[4](参见Mastellone S,
Figure BDA0002668224840000021
D M,Graunke C R,etal.Formation control and collision avoidance for multi-agent non-holonomicsystems:Theory and experiments[J].The International Journal of RoboticsResearch,2008,27(1):107-126.)。然而，由于物理约束，实际多自主体系统在控制过程中通常存在输入约束，如速度不能超过最大值。基于势能函数的避碰编队控制方法无法将有限输入约束考虑在内。另一种较为常见的避碰编队控制方法如模型预测控制能够处理速度限制，但是该方法对于初始可行控制输入的存在性假设较难进行验证，见[5](参见LiH,XieP,YanW.Receding horizon formation tracking control of constrainedunderactuated autonomous underwater vehicles[J].IEEE Transactions onIndustrial Electronics,2016,64(6):5004-5013.)。
通过上述背景技术的分析，考虑避碰以及有限速度约束的基于一致性的多自主体分布式避碰编队控制方法存在较高技术难度，目前还未见到有效方案。
发明内容：
针对现有多自主体系统分布式避碰编队控制方法的不足，本发明提出一种基于速度障碍物的多自主体系统分布式避碰编队控制方法，在自主体速度有界约束条件下，实现与环境中静动态障碍物以及其他自主体无碰撞情况下编队跟踪控制，本发明通过邻居自主体间交互相对信息，设计标称编队控制器，进而通过自主体与静动态障碍物间以及自主体系统间避碰要求，分别构造速度障碍物及互惠速度障碍物，最终通过求解局部二次最优问题得到分布式避碰编队控制输入。
为了实现上述目的，本发明基于速度障碍物的多自主体系统分布式避碰编队控制方法，包括以下步骤：
步骤1：对环境动静态障碍物运动模型和自主体运动模型基于微分方程建模，具体包括：
步骤1-1：确定自主体运动学的微分方程模型表示；
步骤1-2：确定环境中静动态障碍物运动学模型表示。
步骤2：在无向联通通讯拓扑下，设计有限速度的标称分布式编队跟踪控制器并选取控制器参数，具体包括：
步骤2-1：确定自主体编队位置分配；
步骤2-2：设计自主体标称编队控制器；
步骤2-3：分析标称编队控制器收敛时控制器参数需满足条件并选取标称控制器参数。
步骤3：非合作自主体及动静态障碍物间避免碰撞速度障碍物构造。
步骤4：合作自主体间相互避免碰撞速度障碍物构造。
步骤5：基于局部二次最优问题(QP)的有限速度多自主体分布式避碰编队控制器设计。具体包括：
步骤5-1：根据标称编队控制器，设计各自主体理想输入；
步骤5-2：求解二次最优问题，计算自主体分布式避碰编队控制器。
进一步地，步骤1中的N个自主体运动模型和静动态障碍物运动学模型由如下微分方程表示：
步骤1-1.确定自主体运动学的微分方程模型表示：
Figure BDA0002668224840000031
其中xi(t)∈R2为第i个自主体位置向量，vi(t)∈R2为第i个自主体速度输入向量。自主体i具有有界速度输入约束||vi(t)||∞≤vm，其中vm＞0为每个速度分量上界。假设每个自主体i均可由圆形外形近似，碰撞半径为Ri。
步骤1-2.环境中静动态障碍物运动学模型由如下微分方程模型表示：
Figure BDA0002668224840000032
其中vj(t)∈R2为障碍物j运动速度。对于静态障碍物有vj(t)＝0。对于动态障碍物vj(t)≠0。考虑到避碰过程要求自主体离障碍物周围任意方向距离都应大于一定避碰距离，因此假设静动态障碍物均可由圆形外形近似，障碍物j的碰撞半径为
Figure BDA0002668224840000033
对于更复杂外形障碍物，可用多个圆形叠加覆盖方式推广所设计的避碰控制方法。
此步骤中自主体及障碍物建模方式优势在于模型复杂度相对较低，同时具有较广阔适用性，对于大量实际多机器人系统，如多无人车、多无人车等均可由此框架建模。
步骤2中设计有限速度的标称分布式编队跟踪控制器及控制器参数选取方法具体为：
步骤2-1.假设自主体理想编队由向量组di，i＝1，2，..，N给出，并且向量组di满足
Figure BDA0002668224840000041
编队控制目标为使得编队中心/>
Figure BDA0002668224840000042
跟踪目标轨迹xd(t)，同时存在编队位置分配s1，s2，...，sN使得/>
Figure BDA00026682248400000415
其中s1，s2，...，sN为1，2，...，N的一个排序。为了确定编队位置分配s1，s2，...，sN，首先由多自主体初始位置分布xi(0)，计算
Figure BDA0002668224840000043
进而得到初始时刻理想编队位置集合，xc(0)+di，i＝1，2，...，N。然后对多自主体初始位置集合和理想编队位置集合进行最短移动距离分配，即使得分配后所有自主体和其对应理想编队位置间距离之和最小。
步骤2-2.得到每个自主体i对应理想编队位置
Figure BDA0002668224840000044
后，设计如下所示标称编队控制器：
Figure BDA0002668224840000045
Figure BDA0002668224840000046
Figure BDA0002668224840000047
其中pi∈R2为控制器状态，k1，k2，kp，kx＞0为控制器参数，
Figure BDA0002668224840000048
为理想编队运动速度，对于向量y＝[y1，...，yN]T∈RN，函数/>
Figure BDA0002668224840000049
对于标量yi，当|yi|≥1时，有/>
Figure BDA00026682248400000410
当|yi|＜1时，有/>
Figure BDA00026682248400000411
步骤2-3.记
Figure BDA00026682248400000412
则可得
Figure BDA00026682248400000413
其中L为通讯图拉普拉斯矩阵，构造李雅普诺夫函数
Figure BDA00026682248400000414
通过分析保证李雅普诺夫函数导数为负定条件推导标称编队控制器收敛时控制器参数需满足条件
k1+k2+vd≤vm。
kp+vm＜kx
此步骤所设计的标称控制器优势在于只使用了多自主体系统间相对位置测量信息，对于自主体间通讯能力要求较低。同时控制器利用饱和函数，能够实现自主体速度全局有界。通过选择合适的观测器参数和控制器参数，可从理论上保证编队控制误差渐进趋向于零。并且参数需满足的不等式条件形式较为简单，容易选择合适参数满足条件。
步骤3中非合作自主体及动静态障碍物间避免碰撞速度障碍物构造方法具体为：
设自主体由A表示，障碍物由B表示。自主体避碰半径为rA，速度为vA，障碍物避碰半径为rB，速度为vB。以障碍物B为圆心，rA+rB为半径做圆，由顶点A出发，可做出此圆的两条切线。当vA，vB保持不变，且相对速度vA-vB落入此扇形区域内时，经过有限时间后，自主体与障碍物将发生碰撞。以A圆心为顶点，将此扇形区域沿速度向量vB平移，则得到速度障碍物
Figure BDA0002668224840000056
其物理含义为，若自主体A速度向量vA落入此区域，则经过有限时间，自主体与障碍物会发生碰撞。而当自主体速度向量vA在速度障碍物以外时，自主体和障碍物保持速度不变将不会发生碰撞。为了增大可选自主体速度vA范围，考虑有限时间τ对应速度障碍物。类似前述分析，当自主体与障碍物速度相对运动速度不落入扇形区域时，自主体和障碍物保持运动速度不变，则在时间τ内不会发生碰撞。将此阴影区域定义为自主体A相对于障碍物B的有限时间τ速度障碍物/>
Figure BDA0002668224840000055
此步骤构造的速度障碍物优势在于仅利用自主体与障碍物当前速度信息即可确定自主体满足避碰时需避免选择的速度集合，便于与之后局部二次规划问题相结合，得到满足编队跟踪控制要求下的障碍物避碰速度输入。
步骤4中合作自主体间相互避免碰撞速度障碍物构造方法具体为：
设两个自主体分别由A和C表示，自主体A避碰半径为rA，速度为vA，自主体C避碰半径为rC，速度为vC。首先构造自主体A相对于自主体C的速度障碍物
Figure BDA0002668224840000051
由于自主体间将相互合作进行避障，每个自主体只需改变一半的相对运动速度即可合作避开速度障碍物。因此将速度障碍物沿向量/>
Figure BDA0002668224840000052
平移，得到互惠速度障碍物/>
Figure BDA0002668224840000053
进一步考虑有限时间互惠速度障碍物，类似有限时间速度障碍物构造方法，可得有限时间τ互惠速度障碍物/>
Figure BDA0002668224840000054
当自主体A和C的速度向量分别落在对应有限时间τ互惠速度障碍物之外时，自主体A和C保持运动速度不变，则可保证在有限时间τ内，两自主体不会发生碰撞。
此步骤构造的互惠速度障碍物优势在于仅利用两自主体当前速度信息即可确定每个自主体满足避碰时需避免选择的速度集合，便于与之后局部二次规划问题相结合，得到满足编队跟踪控制要求下的实现自主体间避碰的速度输入。
步骤5中基于局部二次最优问题(QP)的有限速度多自主体分布式避碰编队控制器设计具体为：
步骤5-1.根据标称编队控制器，设计每个自主体i的期望输入
Figure BDA0002668224840000061
其中
Figure BDA0002668224840000062
为障碍物j圆心位置向量，dis(x，y)代表两向量x，y间距离，/>
Figure BDA0002668224840000063
为自主体i感知范围内障碍物集合，δ＞0为碰撞安全裕度。
步骤5-2.根据每个自主体i与静动态障碍物间对应的有限时间τ速度障碍物
Figure BDA0002668224840000064
以及与其他自主体间对应的有限时间τ互惠速度障碍物/>
Figure BDA0002668224840000065
构造如下二次最优问题
Figure BDA0002668224840000066
Figure BDA0002668224840000067
Figure BDA0002668224840000068
||vi||∞≤vm
其中
Figure BDA0002668224840000069
自主体i感知范围内静动态障碍物集合，为自主体i感知范围内邻居自主体集合。此最优化问题解/>
Figure BDA00026682248400000610
即为多自主体系统(1)的分布式避碰编队控制器。
此步骤基于局部二次最优问题得到的编队控制器优势在于求取控制输入时计算量小同时能够在编队跟踪控制和避碰之间实现有效平衡。当自主体没有与环境中障碍物和邻居自主体碰撞危险时，由局部二次最优问题可知，最优控制器与标称编队控制器输入一致，因此自主体将完成编队控制目标；当自主体可能与周围障碍物或者其他自主体发生碰撞时，求解局部二次最优问题可以最小程度的修改标称编队控制器，从而在完成避碰的同时尽量朝实现编队任务方向运动。
相对于现有技术，本发明的优点如下：
1)相对于大部分没有考虑自主体间避碰要求的分布式编队控制器，本发明提出的基于速度障碍物的有界输入分布式避碰编队控制方法可以处理多自主体系统实际编队控制中避免碰撞的安全性要求，适用于多无人车、无人机等实际多机器人系统。
2)本发明提出的基于速度障碍物的有界输入分布式避碰编队控制方法可以显式处理自主体有界速度约束。相比于基于势函数的避碰控制方法，本发明提出的控制器通过求解局部二次最优问题，保证求出的最优控制输入始终满足无穷范数有界要求，适用于实际多自主体系统编队控制任务。
3)相对于基于模型预测控制的分布式避碰编队控制方法，本发明提出的基于速度障碍物的有界输入分布式避碰编队控制方法仅需每个自主体每时刻求解一个局部二次最优问题，得到避碰的编队跟踪控制器，计算量大大降低，可以满足实际应用时对控制器实时性的要求。
4)本发明所提出的避碰控制方法也可方便的拓展到其他外形的自主体和障碍物情形，以及其他分布式协同运动控制问题，具有较广阔的应用前景以及实用性。
综上所述，相对于已有技术，本发明的优点如下：本发明提出的基于速度障碍物的有界输入分布式避碰编队控制器，可以有效实现输入速度有界约束下，多自主体系统在存在静动态障碍物环境下无碰撞分布式编队跟踪控制，具有计算量小，应用范围广等优势，可适用于多类实际多自主体系统的编队跟踪控制任务。
附图说明
图1是本发明的基于速度障碍物的分布式避碰编队控制器设计步骤示意图；
图2是本发明中多自主体理想编队位置分配方案示意图；
图3是本发明中非合作自主体与动静态障碍物间避碰速度障碍物构造方法示意图；
图4是本发明中合作自主体间互惠避碰速度障碍物构造方法示意图；
图5是本发明的仿真实验中多自主体通讯拓扑示意图；
图6是本发明仿真实验中多自主体系统运动轨迹及障碍物示意图；
图7是本发明仿真实验中多自主体系统避碰效果示意图。
具体实施方式
下面将结合附图就本发明的发明目的、技术方案、发明优点作进一步详细说明。
现有的多自主体分布式编队控制方法主要采用基于一致性的编队控制方法。当前基于一致性的编队控制方法无法同时考虑自主体间以及自主体与障碍物间的避碰以及自主体本身的速度限制，因此此类编队控制方法在实用性上存在一定问题。
基于以上考虑，本发明首先对环境动静态障碍物运动模型和自主体运动模型基于微分方程建模，然后在无向联通通讯拓扑下，设计有限速度的标称分布式编队跟踪控制器并给出控制器参数选取条件，接着分析非合作自主体及动静态障碍物间避免碰撞速度障碍物构造方法以及合作自主体间相互避免碰撞速度障碍物构造方法，最后给出基于局部二次最优问题(QP)的有限速度多自主体分布式避碰编队控制器设计。
图1表示了本发明的基于速度障碍物的分布式避碰编队控制器设计步骤示意图。其构成具体如下执行：
步骤1.对N个自主体运动模型和静动态障碍物运动学模型用微分方程进行表示。该步骤的具体过程为：
步骤1-1.确定自主体运动学的微分方程模型表示：
Figure BDA0002668224840000081
其中xi(t)∈R2为第i个自主体位置向量，vi(t)∈R2为第i个自主体速度输入向量。自主体i具有有界速度输入约束||vi(t)||∞≤vm，其中vm＞0为每个速度分量上界。假设每个自主体i均可由圆形外形近似，碰撞半径为Ri。
步骤1-2.环境中静动态障碍物运动学模型由如下微分方程模型表示：
Figure BDA0002668224840000082
其中vj(t)∈R2为障碍物j运动速度。对于静态障碍物有vj(t)＝0。对于动态障碍物vj(t)≠0。假设静动态障碍物均可由圆形外形近似，障碍物j的碰撞半径为
Figure BDA0002668224840000083
步骤2设计有限速度的标称分布式编队跟踪控制器及控制器参数选取方法。该步骤的具体过程为：
步骤2-1.假设自主体理想编队由向量组di，i＝1，2，..，N给出，并且向量组di满足
Figure BDA0002668224840000084
编队控制目标为使得编队中心/>
Figure BDA0002668224840000085
跟踪目标轨迹xd(t)，同时存在编队位置分配s1，s2，...，sN使得/>
Figure BDA0002668224840000087
其中s1，s2，...，sN为1，2，...，N的一个排序。为了确定编队位置分配s1，s2，...，sN，首先由多自主体初始位置分布xi(0)，计算/>
Figure BDA0002668224840000086
进而得到初始时刻理想编队位置集合，xc(0)+di，i＝1，2，...，N。然后对多自主体初始位置集合和理想编队位置集合进行最短移动距离分配，即使得分配后所有自主体和其对应理想编队位置间距离之和最小，如图2所示。
步骤2-2.得到每个自主体i对应理想编队位置
Figure BDA0002668224840000091
后，设计如下所示标称编队控制器：
Figure BDA0002668224840000092
Figure BDA0002668224840000093
Figure BDA0002668224840000094
其中pi∈R2为控制器状态，k1，k2，kp，kx＞0为控制器参数，
Figure BDA0002668224840000095
为理想编队运动速度，对于向量y＝[y1，...，yN]T∈RN，函数/>
Figure BDA0002668224840000096
对于标量yi，当|yi|≥1时，有/>
Figure BDA0002668224840000097
当|yi|＜1时，有/>
Figure BDA0002668224840000098
步骤2-3.分析上述标称控制器下系统收敛条件。记
Figure BDA0002668224840000099
Figure BDA00026682248400000910
则可得
Figure BDA00026682248400000911
其中L为通讯图拉普拉斯矩阵。构造李雅普诺夫函数
Figure BDA00026682248400000912
沿系统
Figure BDA00026682248400000913
轨线求导得
Figure BDA00026682248400000914
其中利用到||v||∞≤k1+k2+vd。令
Figure BDA00026682248400000915
可验证/>
Figure BDA00026682248400000916
并且/>
Figure BDA00026682248400000917
因此有/>
Figure BDA00026682248400000918
进而可得
Figure BDA00026682248400000919
结合前面
Figure BDA0002668224840000101
的推导式可得/>
Figure BDA0002668224840000102
因此在条件
k1+k2+vd≤vm
kp+vm＜kx
成立下，V在有限时间内趋于零，进而可得
Figure BDA0002668224840000103
由/>
Figure BDA0002668224840000104
的微分方程可知，有/>
Figure BDA0002668224840000105
故pi经过有限时间收敛到零。考虑到/>
Figure BDA0002668224840000106
以及/>
Figure BDA0002668224840000107
则经过有限时间有/>
Figure BDA0002668224840000108
此时控制输入vi变为
Figure BDA0002668224840000109
令/>
Figure BDA00026682248400001010
Figure BDA00026682248400001011
其中/>
Figure BDA00026682248400001012
则有/>
Figure BDA00026682248400001013
考虑李雅普诺夫函数/>
Figure BDA00026682248400001014
求导可得/>
Figure BDA00026682248400001015
因此，ξ渐进趋于向量/>
Figure BDA00026682248400001016
其中c为一常数向量。由于/>
Figure BDA00026682248400001017
可得c＝0。因此当t→∞时，/>
Figure BDA00026682248400001020
注意到/>
Figure BDA00026682248400001018
则有t→∞时，/>
Figure BDA00026682248400001021
另外，
Figure BDA00026682248400001019
注意到当t→∞时，
Figure BDA00026682248400001022
则有t→∞时，xc(t)-xd(t)→0。因此有t→∞时，/>
Figure BDA00026682248400001023
即在控制器参数条件
k1+k2+vd≤vm
kp+vm＜kx
下，标称编队控制器vi可以实现编队跟踪控制。
步骤3中非合作自主体及动静态障碍物间避免碰撞速度障碍物构造方法具体为：
设自主体由A表示，障碍物由B表示。自主体避碰半径为rA，速度为vA，障碍物避碰半径为rB，速度为vB。如图3所示，以障碍物B为圆心，rA+rB为半径做圆，由顶点A出发，可做出此圆的两条切线。当vA，vB保持不变，且相对速度vA-vB落入此扇形区域内时，经过有限时间后，自主体与障碍物将发生碰撞。以A圆心为顶点，将此扇形区域沿速度向量vB平移，则得到速度障碍物
Figure BDA0002668224840000111
其物理含义为，若自主体A速度向量vA落入此区域，则经过有限时间，自主体与障碍物会发生碰撞。而当自主体速度向量vA在速度障碍物以外时，自主体和障碍物保持速度不变将不会发生碰撞。为了增大可选自主体速度vA范围，考虑有限时间τ对应速度障碍物。类似前述分析，当自主体与障碍物速度相对运动速度不落入图3中阴影区域时，自主体和障碍物保持运动速度不变，则在时间τ内不会发生碰撞。将此阴影区域定义为自主体A相对于障碍物B的有限时间τ速度障碍物/>
Figure BDA0002668224840000112
步骤4中合作自主体间相互避免碰撞速度障碍物构造方法具体为：
设两个自主体分别由A和C表示，自主体A避碰半径为rA，速度为vA，自主体C避碰半径为rC，速度为vC。首先构造自主体A相对于自主体C的速度障碍物
Figure BDA0002668224840000113
由于自主体间将相互合作进行避障，每个自主体只需改变一半的相对运动速度即可合作避开速度障碍物。因此将速度障碍物沿向量/>
Figure BDA0002668224840000114
平移，得到互惠速度障碍物/>
Figure BDA0002668224840000115
进一步考虑有限时间互惠速度障碍物，类似有限时间速度障碍物构造方法，可得有限时间τ互惠速度障碍物/>
Figure BDA0002668224840000116
当自主体A和C的速度向量分别落在对应有限时间τ互惠速度障碍物之外时，自主体A和C保持运动速度不变，则可保证在有限时间τ内，两自主体不会发生碰撞。
步骤5基于局部二次最优问题(QP)的有限速度多自主体分布式避碰编队控制器设计。该步骤的具体过程为：
步骤5-1.根据标称编队控制器，设计每个自主体i的期望输入
Figure BDA0002668224840000117
其中
Figure BDA0002668224840000118
为障碍物j圆心位置向量，dis(x，y)代表两向量x，y间距离，/>
Figure BDA0002668224840000119
为自主体i感知范围内障碍物集合，δ＞0为碰撞安全裕度。
步骤5-2.根据每个自主体i与静动态障碍物间对应的有限时间τ速度障碍物
Figure BDA00026682248400001110
以及与其他自主体间对应的有限时间τ互惠速度障碍物/>
Figure BDA00026682248400001111
构造如下二次最优问题
Figure BDA0002668224840000121
Figure BDA0002668224840000122
Figure BDA0002668224840000123
||vi||∞≤vm
其中
Figure BDA0002668224840000124
自主体i感知范围内静动态障碍物集合，/>
Figure BDA0002668224840000125
为自主体i感知范围内邻居自主体集合。此最优化问题解/>
Figure BDA0002668224840000126
即为多自主体系统(1)的分布式避碰编队控制器。
以下是本发明所设计的基于速度障碍物的分布式避碰编队控制方法仿真验证实验。假设有6个自主体，初始位置和理想编队由图2给出。其中理想编队队形为半径为6.5m的圆形编队。根据自主体初始位置，确定通讯拓扑如图5所示。每个自主体半径r＝3m，速度上界vm＝2m/s。编队跟踪轨迹xd(t)＝[35，35]T，即理想目的地为[35，35]T，
Figure BDA0002668224840000127
环境中假设有三个静态障碍物和一个动态障碍物，静态障碍物半径为6m，动态障碍物半径为3m。由控制器设计步骤2，设计标称编队控制器
Figure BDA0002668224840000128
Figure BDA0002668224840000129
Figure BDA00026682248400001210
其中k1＝1，k2＝1，kp＝2，kx＝5。由步骤5，设计
Figure BDA00026682248400001211
其中δ＝0.3m。进而构造二次最优问题得到分布式避碰编队控制器。仿真结果如图6，图7所示。图6显示，利用所设计的分布式避碰编队控制器，自主体系统顺利实现编队跟踪控制，同时运动过程中自主体自动避开环境中静态障碍物以及动态障碍物。从图7中每时刻每个自主体与最近物体(包括其他自主体及环境障碍物)间最小距离可看出，所设计控制器成功实现编队运动过程中避碰目标。
由以上仿真实验结果可看出，本发明所设计的基于速度障碍物的分布式避碰编队控制方法在存在静动态障碍物环境下可以有效实现带有速度约束的多自主体系统实现编队跟踪控制，并且运动轨迹较为平滑，适用于实际多自主体系统控制应用。
本发明提供了一种基于速度障碍物方法的分布式避碰编队控制方法，在速度有界约束下多自主体系统分布式避碰编队跟踪控制应用中有令人满意的结果。另外本发明提供了多自主体系统与非合作静动态障碍物避碰速度障碍物构造方法以及合作自主体间互惠速度障碍物构造方法。
以上仅是本发明的优选实施方式，应当指出，以上实施列对本发明不构成限定，相关工作人员在不偏离本发明技术思想的范围内，所进行的多样变化和修改，均落在本发明的保护范围内。

Claims (1)

1.基于速度障碍物的多自主体分布式避碰编队控制方法，其特征在于，该方法包括以下步骤：
步骤1：对环境动静态障碍物运动模型和自主体运动模型基于微分方程建模；
步骤2：在无向联通通讯拓扑下，设计有限速度的标称分布式编队跟踪控制器及选择控制器参数；
步骤3：非合作自主体及动静态障碍物间避免碰撞速度障碍物构造；
步骤4：合作自主体间相互避免碰撞速度障碍物构造；
步骤5：基于局部二次最优问题(QP)求解得到有限速度多自主体系统分布式避碰编队控制器；
步骤1对环境动静态障碍物运动模型和自主体运动模型基于微分方程建模具体如下：
步骤1-1.确定自主体运动学的微分方程模型表示：
Figure FDA0004046203220000011
其中xi(t)∈R2为第i个自主体位置向量，vi(t)∈R2为第i个自主体速度输入向量，自主体i具有有界速度输入约束||vi(t)||∞≤vm，其中vm＞0为每个速度分量绝对值上界；
步骤1-2.环境中静动态障碍物运动学模型由如下微分方程模型表示：
Figure FDA0004046203220000012
其中vj(t)∈R2为障碍物j运动速度，对于静态障碍物有vj(t)＝0，对于动态障碍物vj(t)≠0；
步骤2中无向联通通讯拓扑下设计有限速度的标称分布式编队跟踪控制器及控制器参数选取方法具体为：
步骤2-1.假设自主体理想编队由向量组di∈R2,i＝1,2,...,N给出，并且向量组di满足
Figure FDA0004046203220000013
编队控制目标为使得编队中心/>
Figure FDA0004046203220000014
跟踪目标轨迹xd(t)，同时存在编队位置分配s1,s2,...,sN使得/>
Figure FDA0004046203220000015
其中s1,s2,...,sN为1,2,...,N的一个排序；为了确定编队位置分配s1,s2,...,sN，首先由多自主体初始位置分布xi(0)，计算
Figure FDA0004046203220000016
进而得到初始时刻理想编队位置集合，xc(0)+di，i＝1,2,...,N；然后对多自主体初始位置集合和理想编队位置集合进行最短移动距离分配，即使得分配后所有自主体和其对应理想编队位置间距离之和最小，此分配过程可由匈牙利算法得到；
步骤2-2.得到每个自主体i对应理想编队位置
Figure FDA0004046203220000021
后，设计如下所示标称编队控制器：
Figure FDA0004046203220000022
其中k1,k2,kp,kx＞0为控制器参数；aij≥0表示自主体间通讯关系，aij＞0当且仅当自主体i和j在通讯图中存在连边；
Figure FDA0004046203220000023
为理想编队运动速度；对于向量y∈RN，函数sat(y)＝[sat(y1),...,sat(yN)]T,/>
Figure FDA0004046203220000024
对于标量yi，当|yi|≥1时，有sat(yi)＝sgn(yi),/>
Figure FDA0004046203220000025
当|yi|＜1时，有sat(yi)＝yi,/>
Figure FDA0004046203220000026
sgn(·)为符号函数；
步骤2-3.构造李雅普诺夫函数，通过分析标称编队控制器收敛性得到标称控制器参数选取区间
Figure FDA0004046203220000027
其中vd满足
Figure FDA0004046203220000028
步骤3中非合作自主体及动静态障碍物间避免碰撞速度障碍物构造方法具体为：
设自主体由A表示，障碍物由B表示，自主体避碰半径为rA，速度为vA，障碍物避碰半径为rB，速度为vB，以障碍物B中心为圆心，rA+rB为半径做圆，由自主体A中心出发，可做出此圆的两条切线，当vA,vB保持不变，以A中心为起点，相对速度向量vA-vB的终点落入此扇形区域内时，经过有限时间后，自主体与障碍物将发生碰撞，以A中心为起点，将此扇形区域沿速度向量vB平移，则得到速度障碍物
Figure FDA0004046203220000029
其物理含义为，若自主体A速度向量vA以A中心为起点，终点落入此区域时，则经过有限时间，自主体与障碍物会发生碰撞；而当自主体速度向量vA终点落在速度障碍物以外时，自主体和障碍物保持速度不变将不会发生碰撞；为了增大可选自主体速度vA范围，考虑有限时间τ对应速度障碍物，使得当自主体和障碍物保持运动速度不变且自主体与障碍物相对运动速度向量终点不落入此区域时，则在时间τ内不会发生碰撞，将此阴影区域定义为自主体A相对于障碍物B的有限时间τ速度障碍物/>
Figure FDA0004046203220000031
步骤4中合作自主体间相互避免碰撞速度障碍物构造方法具体为：
设两个自主体分别由A和C表示，自主体A避碰半径为rA，速度为vA，自主体C避碰半径为rC，速度为vC，首先构造自主体A相对于自主体C的速度障碍物
Figure FDA0004046203220000032
由于自主体间将相互合作进行避障，每个自主体只需改变一半的相对运动速度即可合作避开速度障碍物，因此将速度障碍物沿向量/>
Figure FDA0004046203220000033
平移，得到互惠速度障碍物/>
Figure FDA0004046203220000034
进一步考虑有限时间互惠速度障碍物，类似有限时间速度障碍物构造方法，可得有限时间τ互惠速度障碍物/>
Figure FDA0004046203220000035
当自主体A和C的速度向量终点分别落在对应有限时间τ互惠速度障碍物之外时，自主体A和C保持运动速度不变，则可保证在有限时间τ内，两自主体不会发生碰撞；
步骤5中基于局部二次最优问题(QP)的有限速度多自主体分布式避碰编队控制器设计具体为：
步骤5-1.根据标称编队控制器，设计每个自主体i的期望输入
Figure FDA0004046203220000036
其中dis(x,y)代表两向量x,y间距离，Ri为自主体i半径，
Figure FDA0004046203220000037
为障碍物j半径，/>
Figure FDA0004046203220000038
为自主体i感知范围内障碍物集合，δ＞0为碰撞安全裕度；
步骤5-2.根据自主体i与静动态障碍物间对应的有限时间τ速度障碍物
Figure FDA0004046203220000039
以及与其他自主体间对应的有限时间τ互惠速度障碍物/>
Figure FDA00040462032200000310
构造如下二次最优问题
Figure FDA0004046203220000041
其中
Figure FDA0004046203220000042
为自主体i感知范围内邻居自主体集合，此最优化问题解/>
Figure FDA0004046203220000043
即为多自主体系统(1)的分布式避碰编队控制器。
CN202010925182.9A 2020-09-06 2020-09-06 基于速度障碍物的多自主体系统分布式避碰编队控制方法 Active CN112083727B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010925182.9A CN112083727B (zh) 2020-09-06 2020-09-06 基于速度障碍物的多自主体系统分布式避碰编队控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010925182.9A CN112083727B (zh) 2020-09-06 2020-09-06 基于速度障碍物的多自主体系统分布式避碰编队控制方法

Publications (2)

Publication Number Publication Date
CN112083727A CN112083727A (zh) 2020-12-15
CN112083727B true CN112083727B (zh) 2023-07-07

Family

ID=73731548

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010925182.9A Active CN112083727B (zh) 2020-09-06 2020-09-06 基于速度障碍物的多自主体系统分布式避碰编队控制方法

Country Status (1)

Country Link
CN (1) CN112083727B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112698659A (zh) * 2020-12-29 2021-04-23 北京电子工程总体研究所 一种针对静态场景的编队避障方法
CN112731939A (zh) * 2020-12-29 2021-04-30 北京电子工程总体研究所 针对动态目标的编队避障方法
CN113093733B (zh) * 2021-03-17 2022-09-23 东南大学 一种无人艇集群对海打击方法
CN115047769B (zh) * 2022-06-29 2023-10-31 南京理工大学 一种基于约束跟随的无人作战平台避障-到达控制方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108594846A (zh) * 2018-03-23 2018-09-28 哈尔滨工程大学 一种障碍环境下多auv编队队形优化控制方法
CN108958289B (zh) * 2018-07-28 2021-08-13 天津大学 基于相对速度障碍的集群无人机避碰方法
CN110196599B (zh) * 2019-06-26 2020-08-18 华南理工大学 一种避碰与连接保持约束下的无人艇分布式编队控制方法

Also Published As

Publication number Publication date
CN112083727A (zh) 2020-12-15

Similar Documents

Publication Publication Date Title
CN112083727B (zh) 基于速度障碍物的多自主体系统分布式避碰编队控制方法
Bajcsy et al. A scalable framework for real-time multi-robot, multi-human collision avoidance
CN111240319B (zh) 室外多机器人协同作业系统及其方法
Liu et al. Coordinated motion planning for multiple mobile robots along designed paths with formation requirement
Mujumdar et al. Evolving philosophies on autonomous obstacle/collision avoidance of unmanned aerial vehicles
Cichella et al. Safe coordinated maneuvering of teams of multirotor unmanned aerial vehicles: A cooperative control framework for multivehicle, time-critical missions
Spurny et al. Cooperative transport of large objects by a pair of unmanned aerial systems using sampling-based motion planning
CN115033016B (zh) 一种异构无人集群编队避障方法及系统
CN114610066A (zh) 复杂未知环境下分布式集群无人机编队飞行轨迹生成方法
Xin et al. Overview of research on transformation of multi-AUV formations
Frew et al. Obstacle avoidance with sensor uncertainty for small unmanned aircraft
CN113900449A (zh) 多无人机轨迹规划方法、装置、无人机及存储介质
Wang et al. Decentralized MPC-based trajectory generation for multiple quadrotors in cluttered environments
CN114186859B (zh) 复杂未知环境多机协同多目标任务分配方法
Wu et al. Real-time three-dimensional smooth path planning for unmanned aerial vehicles in completely unknown cluttered environments
Yang et al. Cooperative deconflicting heading maneuvers applied to unmanned aerial vehicles in non-segregated airspace
Selje et al. Gps-denied three dimensional leader-follower formation control using deep reinforcement learning
CN114637301B (zh) 基于最优仿射队形变换的多机器人动态避障装置及方法
Deng et al. Multi-robot dynamic formation path planning with improved polyclonal artificial immune algorithm
Huang et al. Multi-robot collision avoidance based on buffered voronoi diagram
Ajami et al. Dynamic stabilization-based trajectory planning for drones
Zhang et al. Real-time path planning algorithms for autonomous UAV
Wang et al. Cooperative UAV formation flying with stochastic obstacle avoidance
ZHANG et al. AUV cluster path planning based on improved RRT* algorithm
Li et al. Fast Formation Transformation and Obstacle Avoidance Control for Multi-agent System

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