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

技术特征：
1.一种动态障碍环境下的多车协同编队控制方法，其特征在于，包括：s1，无人车编队中的所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离；s2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点；优化目标设计如下：式中：t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，为全局目标点坐标，q
local
为领航者局部目标点优化问题中代价函数的权矩阵，a
l，n
、b
l，n
为编队超平面避障约束中的超平面参数；求解所述优化目标后，获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点；s3，领航者通过s2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量以及相应的运动轨迹；所述模型预测控制方法采用在线滚动优化算法，在每个控制周期优化目标设计如下：其中，其中，式中：为领航者控制量，，a
l，n
、b
l，n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，为领航者到达局部目标点位置的期望状态，为代价函数与惩罚函数中的权矩阵；求解所述优化目标后，获得领航者在预测时域内的控制量以及相应的运动轨迹；s4，领航者通过s3获得预测时域内的运动轨迹后，将领航者运动轨迹以及s1中确定的编队距离参数传递给跟随者；跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；所述模型预测控制方法采用优化算法，优化目标设计如下：其中，代价函数与惩罚函数的表达式为：
式中：为跟随者控制量，为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，为跟随者的参考轨迹，为代价函数与惩罚函数中的权矩阵；求解所述优化目标后，获得跟随者在预测时域内的控制量；s5，将s3中确定的领航者在当前时刻的控制量施加给领航者，将s4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代s1至s5，直到领航者到达全局目标点。2.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述s1中，无人车检测自身周围的障碍物信息，包括：以无人车自身为圆心的圆形窗口为检测范围，其中障碍物为凸多边形结构；所述障碍物位置信息为障碍点信息，包括无人车检测范围内的障碍物顶点以及障碍物与检测窗口交点的坐标；无人车通过比较前后时刻的障碍点位置，计算出障碍物的运动速度信息：当无人车检测到障碍物顶点时，障碍物速度信息为障碍物顶点速度；当无人车只检测到障碍物边时，障碍物速度信息为障碍物沿垂直于边方向的运动速度；和/或所述s1中，确定无人车之间的编队距离的方法，包括：编队距离为无人车编队沿领航者速度方向从相邻障碍物之间通过的最大距离；若在该编队距离下，无人车编队内部会发生碰撞，说明当前障碍分布过于密集，则编队距离取由用户设定的初始值。3.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述s2中，采用编队避障约束，包括：s2中，采用编队避障约束，包括：s2中，采用编队避障约束，包括：s2中，采用编队避障约束，包括：s2中，采用编队避障约束，包括：s2中，采用编队避障约束，包括：s2中，采用编队避障约束，包括：||a
l，n
(t)||2≤1n≤n
i
(t)；式中：t为当前时刻，为领航者局部目标点在笛卡尔系下的坐标，a
l，n
、b
l，n
为编队超平面避障约束中的超平面参数，r
safe
为无人车安全距离，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，n
i
(t)为t时
刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，t
l
为预测时域；所述编队避障约束将无人车当前位置、局部目标点以及对应的编队位置约束到超平面的一侧，将障碍点以及运动t
l
时间后的障碍点约束到超平面的另一侧，保证无人车在维持编队的情况下从当前位置到达局部目标点并且在预测时域内不与动态障碍物发生碰撞。4.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，领航者到达局部目标点的期望朝向为领航者当前位置到局部目标点的连线方向，用于引导领航者提前转向。5.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述s3中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的领航者运动轨迹以及相应的控制量，并将当前时刻的控制量作用于领航者。6.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述s3中，采用编队超平面避障约束，包括：τ∈(t，t t
p
)，)，)，)，)，)，)，式中：为领航者控制量，，a
l，n
、b
l，n
为编队超平面避障约束中的超平面参数，t
p
为预测时域，为领航者在笛卡尔系下的广义坐标，其中为领航者笛卡尔系下的横纵坐标，为领航者朝向，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，n
i
(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，r
safe
为无人车安全距离，为领航者到达局部目标点位置的期望状态，有：所述编队超平面避障约束将预测时刻中每一时刻的领航者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，领航者跟踪局部目标点过程中不会与动态障碍物发生碰撞。7.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述s4中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的控制量，并将当前时刻的控制量作用于跟随者；和/或
所述s4中，跟随者根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，包括：根据领航者在预测时域内的运动轨迹以及编队距离参数，计算如下：式中：为跟随者f
i
的参考轨迹，为领航者在预测时域内的运动轨迹，为领航者运动轨迹坐标，为领航者运动轨迹朝向，也即跟随者f
i
的朝向，为跟随者f
i
的编队距离参数以及编队角度参数。8.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述s4中，采用跟随者超平面避障约束，包括：τ∈(t，t t
p
)，)，)，)，)，)，)，式中：为跟随者控制量，为跟随者超平面避障约束中的超平面参数，t
p
为预测时域，为跟随者在笛卡尔系下的广义坐标，为跟随者笛卡尔系下的横纵坐标，为跟随者朝向，为无人车i检测到的第n个障碍物中的第j个障碍点，为无人车i检测到的第n个障碍物的所有障碍点构成的集合，n
i
(t)为t时刻无人车i检测到的障碍物数量，为障碍点在t时刻的速度，r
safe
为无人车安全距离；所述跟随者超平面避障约束将预测时刻中每一时刻的跟随者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，跟随者跟踪参考轨迹过程中不会与动态障碍物发生碰撞。9.一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时可用于执行权利要求1-8中任一项所述的方法。10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器
执行时可用于执行权利要求1-8中任一项所述的方法。

技术总结
本发明提供了一种动态障碍环境下的多车协同编队控制方法，无人车检测障碍物信息，确定无人车之间的编队距离；根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的运动轨迹与对应的控制量；将运动轨迹以及编队距离参数传递给跟随者，计算出自身在预测时域内的参考轨迹，利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；将领航者在当前时刻的控制量施加给领航者，将跟随者在当前时刻的控制量施加给跟随者，直到领航者到达全局目标点。本发明在滚动优化框架下建立动态障碍避障约束，通过模型预测控制方法，实现多无人车系统对动态障碍环境的适应性。障碍环境的适应性。障碍环境的适应性。


技术研发人员：王高尚 邹媛媛 李少远 方浩
受保护的技术使用者：上海交通大学
技术研发日：2020.11.30
技术公布日：2022/6/7