动态障碍环境下的多车协同编队控制方法、终端及介质

1.本发明涉及无人系统领域的一种编队控制技术，具体地，涉及一种动态障碍环境下多车协同编队控制方法、终端及介质。


背景技术：

2.多车编队控制是多无人车系统协同的核心研究内容之一，编队控制的任务是控制多个无人车在向目标点行进过程中保持一定的队形，同时保证无人车与障碍物不发生碰撞，另外还需考虑无人车间的避碰问题以及队形的切换和重构等问题。多车维持一定队形可以提高执行任务的效率，增加系统的鲁棒性与可靠性。在各类任务场景中，环境中往往存在各类障碍物，需要无人车在维持队形的同时避开障碍物。目前研究针对的障碍物环境较为简单，大多为全局信息已知的静态障碍环境，但是实际的任务场景更加复杂，全局环境信息往往难以获得且存在动态障碍物，如在军事侦察任务中，侦察区域内的环境信息未知，且存在敌方车辆等动态障碍。在动态障碍环境下，无人系统只能获取有限的局部环境信息且环境信息动态变化，需要对动态障碍物快速反应，这对无人系统的编队控制带来了很大挑战。因此研究动态障碍环境下多车协同编队控制问题具有重要的实际意义和应用价值。
3.目前的编队控制算法在处理静态障碍物避障问题时常采用人工势场法和基于优化的方法。其中人工势场法的思想是将环境抽象为虚拟力场，无人车在目标点引力场以及障碍物斥力场的共同作用下，避开障碍并到达目标点。基于优化的方法是根据无人车路径规划问题建立优化问题，针对障碍物建立避障约束，通过求解优化问题实现路径规划以及避障。对于动态障碍物，目前的工作很少，主要避障算法是基于静态障碍避障的改进算法，如在人工势场法中引入障碍物的速度与加速度因子，提高算法处理动态障碍的能力，但是改进人工势场算法在动态多障碍的环境下避障效果较差，没有考虑无人车的运动学模型，存在局部极值和目标点不可达等问题。而基于优化的动态障碍避障算法是根据障碍物移动速度信息对障碍物进行膨化处理，设置以障碍物为中心、面积大于障碍物的规避区域，通过规避该区域避免与动态障碍物发生碰撞，但是这种膨化处理增加了无人车需要规避的障碍区域范围，在障碍物密集环境下无人车可通行区域被进一步压缩，因此这种算法对障碍物较密集的复杂环境适应性较差。
4.因此，设计一种可以有效处理动态障碍避障约束，且能结合无人车运动学模型保证编队队形以及控制性能的多无人车系统协同编队控制方法对于多无人车协同系统的实现具有重要意义。目前没有发现同本发明类似技术的说明或报道，也尚未收集到国内外类似的资料。


技术实现要素：

5.本发明针对现有技术中存在的上述不足，提供了一种动态障碍环境下的多车协同编队控制方法、终端及介质。
6.根据本发明的一个方面，提供了一种动态障碍环境下的多车协同编队控制方法，
包括：
7.s1，无人车编队中的所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离；
8.s2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点；优化目标设计如下：
[0009][0010]
式中：
[0011]
t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，为全局目标点坐标，q
local
为领航者局部目标点优化问题中代价函数的权矩阵，a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数；
[0012]
求解所述优化目标后，获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点；
[0013]
s3，领航者通过s2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量以及相应的运动轨迹；所述模型预测控制方法采用在线滚动优化算法，在每个控制周期优化目标设计如下：
[0014][0015]
其中，
[0016][0017][0018]
式中：
[0019]
为领航者控制量，,a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，为领航者到达局部目标点位置的期望状态,为代价函数与惩罚函数中的权矩阵；
[0020]
求解所述优化目标后，获得领航者在预测时域内的控制量以及相应的运动轨迹；
[0021]
s4，领航者通过s3获得预测时域内的运动轨迹后，将领航者运动轨迹以及s1中确定的编队距离参数传递给跟随者；跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；所述模型预测控制方法采用优化算法，优化目标设计如下：
[0022][0023]
其中，代价函数与惩罚函数的表达式为：
[0024][0025][0026]
式中：
[0027]
为跟随者控制量，,为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，为跟随者的参考轨迹，为代价函数与惩罚函数中的权矩阵；
[0028]
求解所述优化目标后，获得跟随者在预测时域内的控制量；
[0029]
s5，将s3中确定的领航者在当前时刻的控制量施加给领航者，将s4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代s1至s5，直到领航者到达全局目标点。
[0030]
本发明所提供的动态障碍环境下的多车协同编队控制方法，针对动态障碍环境下的多车编队问题，采用领航跟随的编队策略，由领航者收集所有无人车检测到的障碍物信息并给出规划路径，引导无人车避障并接近全局目标点，跟随者负责跟踪领航者运动轨迹形成队形。领航者根据障碍物密集程度确定当前的编队参数，根据编队避障约束在检测范围内优化出一个局部目标点，并基于模型预测控制方法进行路径规划，控制领航者靠近局部目标点，利用局部目标点引导领航者提前避开障碍。跟随者根据领航者路径以及编队参数计算出自身的参考路径，并通过模型预测控制的方法跟踪参考路径，实现多车整体编队控制。
[0031]
根据本发明的另一个方面，提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时可用于执行上述任一项所述的方法。
[0032]
根据本发明的第三个方面，提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行上述任一项所述的方法。
[0033]
由于采用了上述技术方案，本发明与现有技术相比，具有如下任意一项的有益效果：
[0034]
1、本发明提供的动态障碍环境下的多车协同编队控制方法、终端及介质，采用模型预测控制算法处理编队控制问题，其滚动优化的特点能够适应动态障碍物带来的动态约束，对动态障碍快速反应；采用领航跟随的编队策略，并设计了编队参数调整策略，能够在障碍物较稀疏的环境下很好的保持队形，同时在障碍物较密集的环境下可以通过调整编队参数提高适应性。
[0035]
2、本发明提供的动态障碍环境下的多车协同编队控制方法、终端及介质，能够适应无人车的运动学模型与控制量约束，可以为无人车规划出合理的运动路径并得到相应的控制量，具有优异的控制性能。
[0036]
3、本发明提供的动态障碍环境下的多车协同编队控制方法、终端及介质，在滚动优化框架下建立动态障碍的避障约束，通过模型预测控制方法，实现多无人车系统对动态障碍环境的适应性。
附图说明
[0037]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0038]
图1是本发明一实施例中动态障碍环境下的多车协同编队控制方法流程图。
[0039]
图2是本发明一优选实施例中动态障碍环境下的多车协同编队控制方法流程图。
[0040]
图3是本发明一优选实施例中无人车各时刻状态示意图。
[0041]
图4是本发明一优选实施例中无人车各时刻跟随者与领航者距离图。
[0042]
图5是本发明一优选实施例中无人车各时刻控制量图。
具体实施方式
[0043]
下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。
[0044]
图1为本发明一实施例中动态障碍环境下的多车协同编队控制方法流程图。
[0045]
如图1所示，本实施例提供的动态障碍环境下的多车协同编队控制方法，可以包括以下步骤：
[0046]
s1，无人车编队中的所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离；
[0047]
s2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点；优化目标设计如下：
[0048][0049]
式中：
[0050]
t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，为全局目标点坐标，q
local
为领航者局部目标点优化问题中代价函数的权矩阵，a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数；
[0051]
求解优化目标后，获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点；
[0052]
s3，领航者通过s2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量以及相应的运动轨迹；模型预测控制方法采用在线滚动优化算法，在每个控制周期优化目标设计如下：
[0053][0054]
其中，
[0055]
[0056][0057]
式中：
[0058]
为领航者控制量，,a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，为领航者到达局部目标点位置的期望状态，为代价函数与惩罚函数中的权矩阵；
[0059]
求解优化目标后，获得领航者在预测时域内的控制量以及相应的运动轨迹；
[0060]
s4，领航者通过s3获得预测时域内的运动轨迹后，将领航者运动轨迹以及s1中确定的编队距离参数传递给跟随者；跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；模型预测控制方法采用优化算法，优化目标设计如下：
[0061][0062]
其中，代价函数与惩罚函数的表达式为：
[0063][0064][0065]
式中：
[0066]
为跟随者控制量，,为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，为跟随者的参考轨迹，为代价函数与惩罚函数中的权矩阵；
[0067]
求解优化目标后，获得跟随者在预测时域内的控制量；
[0068]
s5，将s3中确定的领航者在当前时刻的控制量施加给领航者，将s4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代s1至s5，直到领航者到达全局目标点。
[0069]
本发明实施例提供的动态障碍环境下多车协同编队控制方法，针对以下编队控制问题以及基于以下原理实施。
[0070]
动态障碍环境下多车编队协同控制问题，可以描述为：
[0071]
已知全局目标点位置，环境中存在具有凸多边形结构的障碍物，障碍物匀速运动或保持静止，n辆无人车需要从初始位置出发，形成队形并到达全局目标点，且在前进过程中避免与障碍物发生碰撞。无人车可以检测自身周围一定范围内的障碍物信息，检测窗口为半径为r的圆形窗口，各无人车之间可以互相通讯。
[0072]
考虑未知环境中均为凸多边形结构的障碍物，障碍物只做匀速直线运动或保持静止，且运动速度不超过无人车最大线速度，无人车可以检测自身周围半径为r的圆形区域内的障碍物信息，领航者与跟随者均需要检测自身周围的障碍点信息，并且跟随者需要将检测到的障碍点信息提供给领航者。
[0073]
无人车运动学模型：
[0074]
考虑双轮差动驱动式无人车，这类无人车通过安装在公共轴上的两个驱动轮驱动前进，这两个驱动轮可以独立地前后运动，从而驱动无人车转动。
[0075]
现定义一个广义坐标zi(t)来描述无人车在笛卡尔系下的状态：
[0076][0077]
其中,xi(t)为t时刻无人车i在笛卡尔系下的横坐标，yi(t)为t时刻无人车i在笛卡尔系下的纵坐标，θi(t)为t时刻无人车i在笛卡尔系下的航向角。无人车i在t时刻的状态量为zi(t)，控制量ui(t)为无人车i的线速度与角速度,有：
[0078][0079]
受无人车驱动系统限制，无人车i的线速度与角速度需满足一定的约束限制：
[0080][0081][0082]
其中,为无人车i的最大角速度与最大线速度，si为无人车i控制量约束维度可以得到无人车的运动学模型：
[0083][0084]
本发明实施例所提供的动态障碍环境下多车编队协同控制方法，其构思如下：采用领航跟随的编队策略，由领航者收集所有无人车检测到的障碍物信息并给出规划路径，引导无人车避障并接近全局目标点，跟随者负责跟踪领航者运动轨迹形成队形。领航者根据障碍物密集程度确定当前的编队参数，根据编队避障约束在检测范围内优化出一个局部目标点，并基于模型预测控制方法进行路径规划，控制领航者靠近局部目标点，利用局部目标点引导领航者提前避开障碍。跟随者根据领航者路径以及编队参数计算出自身的参考路径，并通过模型预测控制的方法跟踪参考路径，实现多车整体编队控制。
[0085]
领航跟随编队策略：
[0086]
指定编队中的某一个无人车作为领航者向目标点行进，其他无人车作为跟随者跟踪领航者行进。编队参数为各跟随者与领航者之间的距离li以及相对角度通过控制li、趋于跟随者i与领航者的期望距离与期望相对角度实现队形控制，即：
[0087][0088][0089]
基于以上规划问题及原理，本发明一优选实施例提供了一种动态障碍环境下的多车编队协同控制方法，如图2所示，其具体技术方案步骤如下。
[0090]
s1:所有无人车收集障碍点信息，传递给领航者。所有无人车检测自身周围的障碍
物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离。
[0091]
s2:领航者优化局部目标点。领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点。优化目标设计如下：
[0092][0093]
上述各式中：
[0094]
t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，为全局目标点坐标，q
local
为领航者局部目标点优化问题中代价函数的权矩阵，a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数。
[0095]
该优化问题为二次规划问题，可直接通过matlab中的求解器求解，求解该优化问题后可获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点。
[0096]
s3:领航者路径规划。领航者通过s2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量，并根据无人车运动学模型计算出相应的运动轨迹。模型预测控制方法是一种优化算法，优化目标设计如下：
[0097][0098]
其中，代价函数与惩罚函数的表达式为：
[0099][0100][0101]
上述各式中：
[0102]
为领航者控制量，,a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，为领航者到达局部目标点位置的期望状态，为代价函数与惩罚函数中的权矩阵。
[0103]
该优化目标是最小化预测时域内领航者位置与局部目标点的位置之差，使得领航者可以快速靠近局部目标点。该优化问题为二次规划问题，可直接通过matlab求解器求解，求解该优化问题可获得领航者在预测时域内的控制量及相应的运动轨迹。
[0104]
s4:跟随者路径跟踪。领航者通过s3获得预测时域内的运动轨迹后，将领航者运动轨迹以及s1中确定的编队距离参数传递给跟随者。跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量。模型预测控制方法是一种优化算法，优化目标设计如下：
[0105]
[0106]
其中，代价函数与惩罚函数的表达式为：
[0107][0108][0109]
上述各式中：
[0110]
为跟随者控制量，,为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，为跟随者的参考轨迹，为代价函数与惩罚函数中的权矩阵。
[0111]
该优化目标是最小化预测时域内跟随者轨迹与参考轨迹之差，使得跟随者可以准确地跟踪参考轨迹，维持编队队形。该优化问题为二次规划问题，可直接通过matlab求解器求解，求解该优化问题可获得跟随者在预测时域内的控制量。
[0112]
s5:判断领航者是否到达全局目标点。将s3中确定的领航者在当前时刻的控制量施加给领航者，将s4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代s1-s5，直到领航者到达全局目标点。
[0113]
作为一优选实施例，s1中，无人车检测自身周围的障碍物信息，包括：
[0114]
以无人车自身为圆心的圆形窗口为检测范围，其中障碍物为凸多边形结构；障碍物位置信息为障碍点信息，包括无人车检测范围内的障碍物顶点以及障碍物与检测窗口交点的坐标；无人车通过比较前后时刻的障碍点位置，计算出障碍物的运动速度信息：当无人车检测到障碍物顶点时，障碍物速度信息为障碍物顶点速度；当无人车只检测到障碍物边时，障碍物速度信息为障碍物沿垂直于边方向的运动速度。
[0115]
作为一优选实施例，s1中，确定无人车之间的编队距离的方法，包括：
[0116]
编队距离为无人车编队沿领航者速度方向从相邻障碍物之间通过的最大距离；若在该编队距离下，无人车编队内部会发生碰撞，说明当前障碍分布过于密集，则编队距离取所设的初始值，该初始值由用户设定，需要保证无人车间不会相互碰撞。
[0117]
作为一优选实施例，s2中，采用编队避障约束，包括：
[0118][0119][0120][0121][0122][0123][0124][0125]
||a
l,n
(t)||2≤1
[0126]
n≤ni(t)
[0127]
式中：
[0128]
t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，r
safe
为无人车安全距离，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，ni(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，t
l
为预测时域；
[0129]
编队避障约束将无人车当前位置、局部目标点以及对应的编队位置约束到超平面的一侧，将障碍点以及运动t
l
时间后的障碍点约束到超平面的另一侧，保证无人车在维持编队的情况下从当前位置到达局部目标点并且在预测时域内不与动态障碍物发生碰撞。
[0130]
作为一优选实施例，领航者到达局部目标点的期望朝向为领航者当前位置到局部目标点的连线方向，用于引导领航者提前转向。
[0131]
作为一优选实施例，s3中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的领航者运动轨迹以及相应的控制量，并将当前时刻的控制量作用于领航者。
[0132]
作为一优选实施例，s3中，采用编队超平面避障约束，包括：
[0133]
τ∈(t,t t
p
),
[0134][0135][0136][0137][0138][0139][0140]
||a
l,n
(τ|t)||2≤1,n≤ni(t)；
[0141]
式中：
[0142]
为领航者控制量，,a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，其中为领航者笛卡尔系下的横纵坐标，为领航者朝向，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，ni(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，r
safe
为无人车安全距离，为领航者到达局部目标点位置的期望状态，有：
[0143][0144]
编队超平面避障约束将预测时刻中每一时刻的领航者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，领航者跟踪局部目标点
过程中不会与动态障碍物发生碰撞。
[0145]
作为一优选实施例，s4中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的控制量，并将当前时刻的控制量作用于跟随者。
[0146]
作为一优选实施例，s4中，跟随者根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，包括：
[0147]
根据领航者在预测时域内的运动轨迹以及编队距离参数，计算如下：
[0148][0149]
式中：为跟随者fi的参考轨迹，为领航者在预测时域内的运动轨迹，为领航者运动轨迹坐标，为领航者运动轨迹朝向，也即跟随者fi的朝向，为跟随者fi的编队距离参数以及编队角度参数。
[0150]
作为一优选实施例，s4中，采用跟随者超平面避障约束，包括：
[0151]
τ∈(t,t t
p
),
[0152][0153][0154][0155][0156][0157][0158][0159]
式中：
[0160]
为跟随者控制量，,为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，为跟随者笛卡尔系下的横纵坐标，为跟随者朝向，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，ni(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，r
safe
为无人车安全距离；
[0161]
跟随者超平面避障约束将预测时刻中每一时刻的跟随者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，跟随者跟踪参考轨迹
过程中不会与动态障碍物发生碰撞。
[0162]
本优选实施例提供的动态障碍环境下多车协同编队控制方法，能够在动态障碍环境下形成并维持编队，避开动态障碍物并到达全局目标点。
[0163]
以下结合附图并通过仿真举例对本发明实施例所提供的技术方案进一步说明。
[0164]
请参阅图2，本优选实施例的动态障碍环境下多车协同编队控制方法，基于滚动优化，通过领航者优化局部目标点、领航者路径规划、跟随者路径跟踪以及迭代等步骤，在动态障碍环境下形成并维持编队，避开动态障碍物并到达全局目标点。
[0165]
请参阅图3，考虑一个领航者f
l
,两个跟随者f1、f2，共三个无人车的编队控制问题。图中两个三角形为障碍物，其中下方的障碍物为静态障碍物，上方的障碍物为动态障碍物。图中小圆圈表示无人车安全半径，大圆圈表示对应无人车的检测窗口半径。右上角较大的五角星为全局目标点，较小的五角星为领航者的局部目标点以及跟随者对应的期望位置。
[0166]
各项参数设置如下：无人车安全半径r
safe
＝0.05m，检测窗口半径r＝0.65m。领航者线速度约束为角速度约束为跟随者线速度约束为角速度约束为期望的编队参数为
[0167]
无人车的控制周期为0.1s，领航者局部目标点优化问题中，参数矩阵q
local
＝diag(60,60),目标点选取的范围约束中的参数λ＝0.7,预测障碍物的运动时间为t
l
＝0.3s。领航者局部目标点跟踪问题的预测时域t
p
＝0.3s，即未来三个控制周期。各参数矩阵分别为＝0.3s，即未来三个控制周期。各参数矩阵分别为跟随者轨迹跟踪问题中，预测时域与领航者局部目标点跟踪算法中的预测时域相同，t
p
＝0.3s，各参数矩阵分别为＝0.3s，各参数矩阵分别为
[0168]
在t＝0s时刻，三辆无人车坐标分别为在t＝0s时刻，三辆无人车坐标分别为不符合编队要求。环境中存在两个障碍物，其中第一个障碍物为静态障碍物，障碍点坐标为(0.15,0),(0.2,0.2),(0.3,0)；第二个障碍物为动态障碍物，障碍点初始坐标为(0.25,1.05),(0.35,0.85),(0.45,1.05),从t＝0s到t＝10s时间内，障碍物会以(0.07/s,-0.035/s)的速度向右下方匀速移动。无人车需要在避障的同时靠近全局目标点并调整队形。右上方的动态障碍物最初在无人车与全局目标点连线的上方，会逐渐运动至连线下方。无人车需要在考虑障碍物运动信息的同时避开动态障碍物并到达全局目标点。
[0169]
下面将以t＝7s时刻的迭代过程说明，请参阅图2，本优选实施例中，动态障碍环境下多车编队协同控制方法包括以下步骤：
[0170]
s1，所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包
括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离。
[0171]
在t＝7s时刻领航者检测到的障碍点为第二个障碍物的障碍点：(0.7330,0.8085),(0.8330,0.6085),(0.9330,0.8085)；跟随者f1检测到的第一个障碍物中的障碍点为：(0.2,0.2),(0.1894,0.1575),(0.2276,0.1447)，第二个障碍物中的障碍点为：(0.7330,0.8085),(0.8330,0.6085),(0.9330,0.8085)；跟随者f2检测到的第一个障碍物中的障碍点为：(0.2,0.2),(0.1736,0.0945),(0.2879,0.0241)，第二个障碍物中的障碍点为：(0.7330,0.8085),(0.8330,0.6085),(0.9330,0.8085)。
[0172]
由于两个障碍物都检测到了障碍物顶点，障碍物顶点速度即为障碍物速度，因此可以根据障碍物顶点在当前以及上一采样周期的坐标计算障碍物速度：
[0173][0174]
其中，t为无人车的检测周期，为障碍点ε
j,n
(t)的移动速度，为第n个障碍物的速度。对于第一个障碍物，在t＝7s时刻障碍物顶点坐标为(0.2,0.2)，在t＝6.9s时刻，障碍物顶点坐标为(0.2,0.2)，因此第一个障碍物速度为0.对于第二个障碍物，在t＝7s时刻，检测到的三个障碍点均为障碍物顶点，取第一个顶点，其坐标为(0.7330,0.8085)，该顶点在t＝6.9s时刻的坐标为(0.7260,0.8120)，根据上式可计算出第二个障碍物速度为(0.07/s,-0.035/s)。
[0175]
t＝7s时刻，领航者坐标为(0.5592,0.7475,0.7252),领航者计算障碍物间沿领航者前进方向的距离，由于只有第二个障碍物在领航者前方，且障碍点到领航者前进方向直线的相对距离可由如下公式计算：
[0176][0177]
其中，ε(t)为障碍点坐标，p
l
(t)为领航者坐标，θ
l
为领航者前进方向，有ε(t)＝(0.7330,0.8085)，p
l
(t)＝(0.5592,0.7475),θ
l
＝0.7252。代入数据计算可得相对距离为d
ref
＝0.0696,由于无人车另一侧没有障碍物，无人车检测范围r＝0.65m，因此无人车编队距离参数应使得无人车可以沿领航者前进方向通过宽度为d
ref
r＝0.7196的区域，由于期望的编队距离参数为望的编队距离参数为可以满足要求，故领航者确定当前的编队距离参数为0.18m。
[0178]
s2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点。优化目标设计如下：
[0179][0180]
使得，使得，
[0181][0182][0183]
[0184][0185][0186][0187][0188]
||a
l,n
(t)||2≤1
[0189]
n≤ni(t).
[0190]
上述各式中：
[0191]
t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，为全局目标点坐标，q
local
为领航者局部目标点优化问题中代价函数的权矩阵，a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，r
sight
为无人车障碍物检测窗口半径，λ为局部目标点优化范围参数，r
safe
为无人车安全距离，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，ni(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，t
l
为预测时域。
[0192]
该优化问题为二次规划问题，可直接通过matlab中的求解器求解，求解该优化问题后可获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点。代入数据求解出t＝7s时刻领航者局部目标点坐标为
[0193]
s3，领航者获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的运动轨迹与对应的控制量。模型预测控制方法是一种优化算法，优化目标设计如下：
[0194][0195]
使得，τ∈(t,t t
p
),
[0196][0197][0198][0199][0200][0201][0202]
||a
l,n
(τ|t)||2≤1,n≤ni(t).
[0203]
其中，代价函数与惩罚函数的表达式为：
[0204]
[0205][0206]
上述各式中：
[0207]
为领航者控制量，,a
l,n
、b
l,n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，有为领航者在笛卡尔系下的广义坐标，有为领航者笛卡尔系下的横纵坐标，为领航者朝向，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，ni(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，r
safe
为无人车安全距离，为领航者到达局部目标点位置的期望状态，有航者到达局部目标点位置的期望状态，有航者到达局部目标点位置的期望状态，有为代价函数与惩罚函数中的权矩阵。
[0208]
该优化目标是最小化预测时域内领航者位置与局部目标点的位置之差，使得领航者可以快速靠近局部目标点。该优化问题为二次规划问题，可直接通过matlab求解器求解，求解该优化问题可获得领航者在预测时域内的运动轨迹以及相应的控制量。代入数据求解可得到领航者在预测时域内的轨迹：(0.5690,0.7561,0.8752),(0.5773,0.7661,0.9468),(0.5849,0.7767,0.9469)，以及对应的控制量：(0.1300,1.5000),(0.1300,0.7158),(0.1300,0.0014)。
[0209]
s4，领航者通过s3获得预测时域内的运动轨迹后，将领航者运动轨迹以及s1中确定的编队距离参数传递给跟随者。跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹：
[0210][0211]
式中：为跟随者fi的参考轨迹，为领航者在预测时域内的运动轨迹，为领航者运动轨迹坐标，为领航者运动轨迹朝向，也即跟随者fi的朝向，为跟随者fi的编队距离参数以及编队角度参数。代入数据后可计算出跟随者f1的参考轨迹为：(0.3874,0.7081,0.8238),(0.3934,0.7146,0.9265),(0.3976,0.7201,0.9469)；参考者f2的参考轨迹为：(0.5482,0.5705,0.8329)，(0.5590,0.5823,0.9506)，
(0.5683,0.5954,0.9469)。
[0212]
跟随者随后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量。模型预测控制方法是一种优化算法，优化目标设计如下：
[0213][0214]
使得，τ∈(t,t t
p
),
[0215][0216][0217][0218][0219][0220][0221][0222]
其中，代价函数与惩罚函数的表达式为：
[0223][0224][0225]
上述各式中：
[0226]
为跟随者控制量，,为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，有为跟随者在笛卡尔系下的广义坐标，有为跟随者在笛卡尔系下的广义坐标，有为跟随者笛卡尔系下的横纵坐标，为跟随者朝向，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，ni(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，r
safe
为无人车安全距离，为代价函数与惩罚函数中的权矩阵。
[0227]
该优化目标是最小化预测时域内跟随者轨迹与参考轨迹之差，使得跟随者可以准确地跟踪参考轨迹，维持编队队形。该优化问题为二次规划问题，可直接通过matlab求解器求解，求解该优化问题可获得跟随者在预测时域内的控制量。代入数据求解可得到跟随者f1在预测时域内的控制量：(-0.1600,-0.0861),(0.0877,1.0269),(0.0698,0.2039)；跟随者f2在预测时域内的控制量：(0.0589,2.0000),(0.1600,1.1774),(0.1600,-0.0372)。
[0228]
s5，将s3中确定的领航者在当前时刻的控制量施加给领航者，将s4中确定的跟随者在当前时刻的控制量施加给跟随者，施加控制量后，领航者在t＝7.1s时刻的坐标为：(0.5690,0.7561,0.8752)；跟随者f1坐标为：(0.3874,0.7081,0.8238)；跟随者f2坐标为：
(0.5482,0.5705,0.8329)。
[0229]
由于领航者未到达全局目标点，则迭代s1～s5，直到领航者到达全局目标点。
[0230]
下面从整体流程介绍本实例，请参阅图3，t＝0s时刻，无人车初始位置不满足编队要求，且右方存在障碍物，优化出的局部目标点位于无人车右上方，引导无人车避障，无人车在跟踪局部目标点时会逐渐调整队形。t＝2.5s时刻，无人车已经避开了下方的障碍物，队形经过调整已经接近编队队形，在检测到障碍物后优化出的局部目标点在动态障碍物下方，引导无人车从障碍物下方进行避障。t＝4s时刻，无人车朝全局目标点移动，并针对动态障碍物不断调整队形。t＝7s时刻,此时无人车已经靠近动态障碍物。障碍物位于无人车与全局目标点连线的下方，优化出的局部目标点位于障碍物上方，引导无人车从障碍物上方避障。t＝8.5s时刻，无人车已经避开了障碍物，局部目标点在无人车与全局目标点连线上，引导无人车靠近全局目标点。t＝16s时刻，无人车维持队形并到达了全局目标点。
[0231]
图4展示了各时刻跟随者与领航者的距离，其中表示跟随者与领航者的期望编队距离，由于无人车的初始位置不满足编队需求，且无人车初始位置附近存在障碍物，无人车在初期需要在避障的同时调整队形，可以看到，跟随者与领航者间的距离逐渐逼近期望的编队距离。t＝4s至t＝8.5s期间，为了避开动态障碍物，无人车不断调整编队队形。t＝8.5s后，无人车避开动态障碍物后，在靠近全局目标点的过程中不断调整队形至期望值。
[0232]
图5展示了动态障碍下多车编队过程中各时刻无人车的控制量，其中，(a)为各车各时刻线速度，(b)表示各车各时刻角速度。可以看出，领航者线速度在基本上在线速度约束的上界，保持高速行驶。跟随者除了在初期为了调整队形外，也都保持接近约束上界的线速度。全过程中领航者与跟随者的控制量都满足约束。
[0233]
本发明上述实施例提供的动态障碍环境下的多车协同编队控制方法，考虑匀速运动的凸多边形障碍环境下的多车编队避障问题，采用领航跟随的编队策略以及模型预测控制方法实现编队控制。针对领航者，所有无人车收集自身周围的障碍物信息并传递给领航者，领航者根据障碍物分布确定无人车之间的编队距离；随后在领航者环境探测范围内优化出局部目标点引导编队提前避障；确定局部目标点后，领航者利用模型预测控制算法跟踪局部目标点，得到领航者在预测时域内的运动轨迹与对应的控制量。针对跟随者，领航者将当前时刻的编队距离与领航者在预测时域内的运动轨迹传递给跟随者；跟随者计算出预测时域内自身的参考轨迹，随后通过模型预测控制算法跟踪参考轨迹。本发明上述实施例提供的动态障碍环境下的多车协同编队控制方法，通过领航跟随的编队策略与基于模型预测控制的控制方法实现了动态环境下多车在避障的同时尽可能保持编队队形。
[0234]
本发明另一实施例提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时可用于执行本发明上述实施例中任一项的方法。
[0235]
可选地，存储器，用于存储程序；存储器，可以包括易失性存储器(英文：volatile memory)，例如随机存取存储器(英文：random-access memory，缩写：ram)，如静态随机存取存储器(英文：static random-access memory，缩写：sram)，双倍数据率同步动态随机存取存储器(英文：double data rate synchronous dynamic random access memory，缩写：ddr sdram)等；存储器也可以包括非易失性存储器(英文：non-volatile memory)，例如快闪存储器(英文：flash memory)。存储器用于存储计算机程序(如实现上述方法的应用程
序、功能模块等)、计算机指令等，上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
[0236]
上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
[0237]
处理器，用于执行存储器存储的计算机程序，以实现上述实施例涉及的方法中的各个步骤。具体可以参见前面方法实施例中的相关描述。
[0238]
处理器和存储器可以是独立结构，也可以是集成在一起的集成结构。当处理器和存储器是独立结构时，存储器、处理器可以通过总线耦合连接。
[0239]
本发明第三个实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行本发明上述实施例中任一项的方法。
[0240]
本发明上述实施例提供的动态障碍环境下的多车协同编队控制方法、终端及介质，通过各无人车收集障碍物信息、领航者优化局部目标点、领航者路径规划、跟随者路径跟踪、迭代等步骤，在动态障碍环境下形成并维持编队，避开动态障碍物并到达全局目标点。本发明上述实施例提供的动态障碍环境下多车协同编队控制方法，针对动态障碍环境下的多车协同编队问题，采用领航跟随的编队策略，各无人车收集并向领航者提供自身周围的环境信息，由领航者确定编队参数并优化局部目标点，通过局部目标点引导无人车提前避开动态障碍。在获得局部目标点后，领航者通过模型预测控制算法进行路径规划，并将编队参数与参考路径传送给跟随者，跟随者可根据编队参数与领航者的参考路径确定自身的参考路径，并通过模型预测控制算法跟踪参考路径，实现多车整体编队控制。本发明上述实施例考虑了无人车的避障约束、无人车运动学模型以及控制量约束等约束条件，无人车可以形成并维持编队，对动态障碍物快速反应，最终到达全局目标点。
[0241]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
[0242]
以上仅为发明的较佳实例而已，并非用来限定本发明的实施范围。即凡依本发明申请专利范围的内容所作的等效变化与修饰，都应该为本发明的技术范畴。