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

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

Info

Publication number
CN114594756A
CN114594756A CN202011380399.2A CN202011380399A CN114594756A CN 114594756 A CN114594756 A CN 114594756A CN 202011380399 A CN202011380399 A CN 202011380399A CN 114594756 A CN114594756 A CN 114594756A
Authority
CN
China
Prior art keywords
obstacle
pilot
formation
follower
unmanned vehicle
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.)
Pending
Application number
CN202011380399.2A
Other languages
English (en)
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.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong 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 Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202011380399.2A priority Critical patent/CN114594756A/zh
Publication of CN114594756A publication Critical patent/CN114594756A/zh
Pending legal-status Critical Current

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
    • G05D1/0295—Fleet control by at least one leading vehicle of the fleet
    • 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/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/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点；优化目标设计如下：
Figure BDA0002808310690000021
式中：
t为当前时刻，
Figure BDA0002808310690000022
为领航者局部目标点在笛卡尔系下的坐标，
Figure BDA0002808310690000023
为全局目标点坐标，Qlocal为领航者局部目标点优化问题中代价函数的权矩阵，al,n、bl,n为编队超平面避障约束中的超平面参数；
求解所述优化目标后，获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点；
S3，领航者通过S2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量以及相应的运动轨迹；所述模型预测控制方法采用在线滚动优化算法，在每个控制周期优化目标设计如下：
Figure BDA0002808310690000031
其中，
Figure BDA0002808310690000032
Figure BDA0002808310690000033
式中：
Figure BDA0002808310690000034
为领航者控制量，,al,n、bl,n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000035
为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，
Figure BDA0002808310690000036
为领航者到达局部目标点位置的期望状态,
Figure BDA0002808310690000037
为代价函数与惩罚函数中的权矩阵；
求解所述优化目标后，获得领航者在预测时域内的控制量以及相应的运动轨迹；
S4，领航者通过S3获得预测时域内的运动轨迹后，将领航者运动轨迹以及S1中确定的编队距离参数传递给跟随者；跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；所述模型预测控制方法采用优化算法，优化目标设计如下：
Figure BDA0002808310690000038
其中，代价函数与惩罚函数的表达式为：
Figure BDA0002808310690000039
Figure BDA00028083106900000310
式中：
Figure BDA00028083106900000311
为跟随者控制量，,
Figure BDA00028083106900000312
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000041
为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，
Figure BDA0002808310690000042
为跟随者的参考轨迹，
Figure BDA0002808310690000043
为代价函数与惩罚函数中的权矩阵；
求解所述优化目标后，获得跟随者在预测时域内的控制量；
S5，将S3中确定的领航者在当前时刻的控制量施加给领航者，将S4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代S1至S5，直到领航者到达全局目标点。
本发明所提供的动态障碍环境下的多车协同编队控制方法，针对动态障碍环境下的多车编队问题，采用领航跟随的编队策略，由领航者收集所有无人车检测到的障碍物信息并给出规划路径，引导无人车避障并接近全局目标点，跟随者负责跟踪领航者运动轨迹形成队形。领航者根据障碍物密集程度确定当前的编队参数，根据编队避障约束在检测范围内优化出一个局部目标点，并基于模型预测控制方法进行路径规划，控制领航者靠近局部目标点，利用局部目标点引导领航者提前避开障碍。跟随者根据领航者路径以及编队参数计算出自身的参考路径，并通过模型预测控制的方法跟踪参考路径，实现多车整体编队控制。
根据本发明的另一个方面，提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时可用于执行上述任一项所述的方法。
根据本发明的第三个方面，提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行上述任一项所述的方法。
由于采用了上述技术方案，本发明与现有技术相比，具有如下任意一项的有益效果：
1、本发明提供的动态障碍环境下的多车协同编队控制方法、终端及介质，采用模型预测控制算法处理编队控制问题，其滚动优化的特点能够适应动态障碍物带来的动态约束，对动态障碍快速反应；采用领航跟随的编队策略，并设计了编队参数调整策略，能够在障碍物较稀疏的环境下很好的保持队形，同时在障碍物较密集的环境下可以通过调整编队参数提高适应性。
2、本发明提供的动态障碍环境下的多车协同编队控制方法、终端及介质，能够适应无人车的运动学模型与控制量约束，可以为无人车规划出合理的运动路径并得到相应的控制量，具有优异的控制性能。
3、本发明提供的动态障碍环境下的多车协同编队控制方法、终端及介质，在滚动优化框架下建立动态障碍的避障约束，通过模型预测控制方法，实现多无人车系统对动态障碍环境的适应性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1是本发明一实施例中动态障碍环境下的多车协同编队控制方法流程图。
图2是本发明一优选实施例中动态障碍环境下的多车协同编队控制方法流程图。
图3是本发明一优选实施例中无人车各时刻状态示意图。
图4是本发明一优选实施例中无人车各时刻跟随者与领航者距离图。
图5是本发明一优选实施例中无人车各时刻控制量图。
具体实施方式
下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。
图1为本发明一实施例中动态障碍环境下的多车协同编队控制方法流程图。
如图1所示，本实施例提供的动态障碍环境下的多车协同编队控制方法，可以包括以下步骤：
S1，无人车编队中的所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离；
S2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点；优化目标设计如下：
Figure BDA0002808310690000051
式中：
t为当前时刻，
Figure BDA0002808310690000061
为领航者局部目标点在笛卡尔系下的坐标，
Figure BDA0002808310690000062
为全局目标点坐标，Qlocal为领航者局部目标点优化问题中代价函数的权矩阵，al,n、bl,n为编队超平面避障约束中的超平面参数；
求解优化目标后，获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点；
S3，领航者通过S2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量以及相应的运动轨迹；模型预测控制方法采用在线滚动优化算法，在每个控制周期优化目标设计如下：
Figure BDA0002808310690000063
其中，
Figure BDA0002808310690000064
Figure BDA0002808310690000065
式中：
Figure BDA0002808310690000066
为领航者控制量，,al,n、bl,n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000067
为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，
Figure BDA0002808310690000068
为领航者到达局部目标点位置的期望状态，
Figure BDA0002808310690000069
为代价函数与惩罚函数中的权矩阵；
求解优化目标后，获得领航者在预测时域内的控制量以及相应的运动轨迹；
S4，领航者通过S3获得预测时域内的运动轨迹后，将领航者运动轨迹以及S1中确定的编队距离参数传递给跟随者；跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；模型预测控制方法采用优化算法，优化目标设计如下：
Figure BDA00028083106900000610
其中，代价函数与惩罚函数的表达式为：
Figure BDA0002808310690000071
Figure BDA0002808310690000072
式中：
Figure BDA0002808310690000073
为跟随者控制量，,
Figure BDA0002808310690000074
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000075
为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，
Figure BDA0002808310690000076
为跟随者的参考轨迹，
Figure BDA0002808310690000077
为代价函数与惩罚函数中的权矩阵；
求解优化目标后，获得跟随者在预测时域内的控制量；
S5，将S3中确定的领航者在当前时刻的控制量施加给领航者，将S4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代S1至S5，直到领航者到达全局目标点。
本发明实施例提供的动态障碍环境下多车协同编队控制方法，针对以下编队控制问题以及基于以下原理实施。
动态障碍环境下多车编队协同控制问题，可以描述为：
已知全局目标点位置，环境中存在具有凸多边形结构的障碍物，障碍物匀速运动或保持静止，n辆无人车需要从初始位置出发，形成队形并到达全局目标点，且在前进过程中避免与障碍物发生碰撞。无人车可以检测自身周围一定范围内的障碍物信息，检测窗口为半径为R的圆形窗口，各无人车之间可以互相通讯。
考虑未知环境中均为凸多边形结构的障碍物，障碍物只做匀速直线运动或保持静止，且运动速度不超过无人车最大线速度，无人车可以检测自身周围半径为R的圆形区域内的障碍物信息，领航者与跟随者均需要检测自身周围的障碍点信息，并且跟随者需要将检测到的障碍点信息提供给领航者。
无人车运动学模型：
考虑双轮差动驱动式无人车，这类无人车通过安装在公共轴上的两个驱动轮驱动前进，这两个驱动轮可以独立地前后运动，从而驱动无人车转动。
现定义一个广义坐标zi(t)来描述无人车在笛卡尔系下的状态：
Figure BDA0002808310690000078
其中,xi(t)为t时刻无人车i在笛卡尔系下的横坐标，yi(t)为t时刻无人车i在笛卡尔系下的纵坐标，θi(t)为t时刻无人车i在笛卡尔系下的航向角。无人车i在t时刻的状态量为zi(t)，控制量ui(t)为无人车i的线速度与角速度,有：
Figure BDA0002808310690000081
受无人车驱动系统限制，无人车i的线速度与角速度需满足一定的约束限制：
Figure BDA0002808310690000082
Figure BDA0002808310690000083
其中,
Figure BDA0002808310690000084
为无人车i的最大角速度与最大线速度，si为无人车i控制量约束维度可以得到无人车的运动学模型：
Figure BDA0002808310690000085
本发明实施例所提供的动态障碍环境下多车编队协同控制方法，其构思如下：采用领航跟随的编队策略，由领航者收集所有无人车检测到的障碍物信息并给出规划路径，引导无人车避障并接近全局目标点，跟随者负责跟踪领航者运动轨迹形成队形。领航者根据障碍物密集程度确定当前的编队参数，根据编队避障约束在检测范围内优化出一个局部目标点，并基于模型预测控制方法进行路径规划，控制领航者靠近局部目标点，利用局部目标点引导领航者提前避开障碍。跟随者根据领航者路径以及编队参数计算出自身的参考路径，并通过模型预测控制的方法跟踪参考路径，实现多车整体编队控制。
领航跟随编队策略：
指定编队中的某一个无人车作为领航者向目标点行进，其他无人车作为跟随者跟踪领航者行进。编队参数为各跟随者与领航者之间的距离li以及相对角度
Figure BDA0002808310690000086
通过控制li、
Figure BDA0002808310690000087
趋于跟随者i与领航者的期望距离
Figure BDA0002808310690000088
与期望相对角度
Figure BDA0002808310690000089
实现队形控制，即：
Figure BDA00028083106900000810
Figure BDA00028083106900000811
基于以上规划问题及原理，本发明一优选实施例提供了一种动态障碍环境下的多车编队协同控制方法，如图2所示，其具体技术方案步骤如下。
S1:所有无人车收集障碍点信息，传递给领航者。所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离。
S2:领航者优化局部目标点。领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点。优化目标设计如下：
Figure BDA0002808310690000091
上述各式中：
t为当前时刻，
Figure BDA0002808310690000092
为领航者局部目标点在笛卡尔系下的坐标，
Figure BDA0002808310690000093
为全局目标点坐标，Qlocal为领航者局部目标点优化问题中代价函数的权矩阵，al,n、bl,n为编队超平面避障约束中的超平面参数。
该优化问题为二次规划问题，可直接通过MATLAB中的求解器求解，求解该优化问题后可获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点。
S3:领航者路径规划。领航者通过S2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量，并根据无人车运动学模型计算出相应的运动轨迹。模型预测控制方法是一种优化算法，优化目标设计如下：
Figure BDA0002808310690000094
其中，代价函数与惩罚函数的表达式为：
Figure BDA0002808310690000095
Figure BDA0002808310690000096
上述各式中：
Figure BDA0002808310690000097
为领航者控制量，,al,n、bl,n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000098
为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，
Figure BDA0002808310690000101
为领航者到达局部目标点位置的期望状态，
Figure BDA0002808310690000102
为代价函数与惩罚函数中的权矩阵。
该优化目标是最小化预测时域内领航者位置与局部目标点的位置之差，使得领航者可以快速靠近局部目标点。该优化问题为二次规划问题，可直接通过MATLAB求解器求解，求解该优化问题可获得领航者在预测时域内的控制量及相应的运动轨迹。
S4:跟随者路径跟踪。领航者通过S3获得预测时域内的运动轨迹后，将领航者运动轨迹以及S1中确定的编队距离参数传递给跟随者。跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量。模型预测控制方法是一种优化算法，优化目标设计如下：
Figure BDA0002808310690000103
其中，代价函数与惩罚函数的表达式为：
Figure BDA0002808310690000104
Figure BDA0002808310690000105
上述各式中：
Figure BDA0002808310690000106
为跟随者控制量，,
Figure BDA0002808310690000107
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000108
为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，
Figure BDA0002808310690000109
为跟随者的参考轨迹，
Figure BDA00028083106900001010
为代价函数与惩罚函数中的权矩阵。
该优化目标是最小化预测时域内跟随者轨迹与参考轨迹之差，使得跟随者可以准确地跟踪参考轨迹，维持编队队形。该优化问题为二次规划问题，可直接通过MATLAB求解器求解，求解该优化问题可获得跟随者在预测时域内的控制量。
S5:判断领航者是否到达全局目标点。将S3中确定的领航者在当前时刻的控制量施加给领航者，将S4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代S1-S5，直到领航者到达全局目标点。
作为一优选实施例，S1中，无人车检测自身周围的障碍物信息，包括：
以无人车自身为圆心的圆形窗口为检测范围，其中障碍物为凸多边形结构；障碍物位置信息为障碍点信息，包括无人车检测范围内的障碍物顶点以及障碍物与检测窗口交点的坐标；无人车通过比较前后时刻的障碍点位置，计算出障碍物的运动速度信息：当无人车检测到障碍物顶点时，障碍物速度信息为障碍物顶点速度；当无人车只检测到障碍物边时，障碍物速度信息为障碍物沿垂直于边方向的运动速度。
作为一优选实施例，S1中，确定无人车之间的编队距离的方法，包括：
编队距离为无人车编队沿领航者速度方向从相邻障碍物之间通过的最大距离；若在该编队距离下，无人车编队内部会发生碰撞，说明当前障碍分布过于密集，则编队距离取所设的初始值，该初始值由用户设定，需要保证无人车间不会相互碰撞。
作为一优选实施例，S2中，采用编队避障约束，包括：
Figure BDA0002808310690000111
Figure BDA0002808310690000112
Figure BDA0002808310690000113
Figure BDA0002808310690000114
Figure BDA0002808310690000115
Figure BDA0002808310690000116
Figure BDA0002808310690000117
||al,n(t)||2≤1
n≤Ni(t)
式中：
t为当前时刻，
Figure BDA0002808310690000118
为领航者局部目标点在笛卡尔系下的坐标，al,n、bl,n为编队超平面避障约束中的超平面参数，rsafe为无人车安全距离，
Figure BDA0002808310690000119
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure BDA00028083106900001110
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure BDA00028083106900001111
为障碍点
Figure BDA0002808310690000121
在t时刻的速度，Tl为预测时域；
编队避障约束将无人车当前位置、局部目标点以及对应的编队位置约束到超平面的一侧，将障碍点以及运动Tl时间后的障碍点约束到超平面的另一侧，保证无人车在维持编队的情况下从当前位置到达局部目标点并且在预测时域内不与动态障碍物发生碰撞。
作为一优选实施例，领航者到达局部目标点的期望朝向为领航者当前位置到局部目标点的连线方向，用于引导领航者提前转向。
作为一优选实施例，S3中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的领航者运动轨迹以及相应的控制量，并将当前时刻的控制量作用于领航者。
作为一优选实施例，S3中，采用编队超平面避障约束，包括：
τ∈(t,t+Tp),
Figure BDA0002808310690000122
Figure BDA0002808310690000123
Figure BDA0002808310690000124
Figure BDA0002808310690000125
Figure BDA0002808310690000126
Figure BDA0002808310690000127
||al,n(τ|t)||2≤1,n≤Ni(t)；
式中：
Figure BDA0002808310690000128
为领航者控制量，,al,n、bl,n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000129
为领航者在笛卡尔系下的广义坐标，其中
Figure BDA00028083106900001210
为领航者笛卡尔系下的横纵坐标，
Figure BDA00028083106900001211
为领航者朝向，
Figure BDA00028083106900001212
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure BDA00028083106900001213
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure BDA00028083106900001214
为障碍点
Figure BDA00028083106900001215
在t时刻的速度，rsafe为无人车安全距离，
Figure BDA00028083106900001216
为领航者到达局部目标点位置的期望状态，有：
Figure BDA0002808310690000131
编队超平面避障约束将预测时刻中每一时刻的领航者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，领航者跟踪局部目标点过程中不会与动态障碍物发生碰撞。
作为一优选实施例，S4中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的控制量，并将当前时刻的控制量作用于跟随者。
作为一优选实施例，S4中，跟随者根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，包括：
根据领航者在预测时域内的运动轨迹以及编队距离参数，计算如下：
Figure BDA0002808310690000132
式中：
Figure BDA0002808310690000133
为跟随者fi的参考轨迹，
Figure BDA0002808310690000134
为领航者在预测时域内的运动轨迹，
Figure BDA0002808310690000135
为领航者运动轨迹坐标，
Figure BDA0002808310690000136
为领航者运动轨迹朝向，也即跟随者fi的朝向，
Figure BDA0002808310690000137
为跟随者fi的编队距离参数以及编队角度参数。
作为一优选实施例，S4中，采用跟随者超平面避障约束，包括：
τ∈(t,t+Tp),
Figure BDA0002808310690000138
Figure BDA0002808310690000139
Figure BDA00028083106900001310
Figure BDA00028083106900001311
Figure BDA00028083106900001312
Figure BDA0002808310690000141
Figure BDA0002808310690000142
式中：
Figure BDA0002808310690000143
为跟随者控制量，,
Figure BDA0002808310690000144
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA0002808310690000145
为跟随者在笛卡尔系下的广义坐标，
Figure BDA0002808310690000146
为跟随者笛卡尔系下的横纵坐标，
Figure BDA0002808310690000147
为跟随者朝向，
Figure BDA0002808310690000148
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure BDA0002808310690000149
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure BDA00028083106900001410
为障碍点
Figure BDA00028083106900001411
在t时刻的速度，rsafe为无人车安全距离；
跟随者超平面避障约束将预测时刻中每一时刻的跟随者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，跟随者跟踪参考轨迹过程中不会与动态障碍物发生碰撞。
本优选实施例提供的动态障碍环境下多车协同编队控制方法，能够在动态障碍环境下形成并维持编队，避开动态障碍物并到达全局目标点。
以下结合附图并通过仿真举例对本发明实施例所提供的技术方案进一步说明。
请参阅图2，本优选实施例的动态障碍环境下多车协同编队控制方法，基于滚动优化，通过领航者优化局部目标点、领航者路径规划、跟随者路径跟踪以及迭代等步骤，在动态障碍环境下形成并维持编队，避开动态障碍物并到达全局目标点。
请参阅图3，考虑一个领航者fl,两个跟随者f1、f2，共三个无人车的编队控制问题。图中两个三角形为障碍物，其中下方的障碍物为静态障碍物，上方的障碍物为动态障碍物。图中小圆圈表示无人车安全半径，大圆圈表示对应无人车的检测窗口半径。右上角较大的五角星为全局目标点，较小的五角星为领航者的局部目标点以及跟随者对应的期望位置。
各项参数设置如下：无人车安全半径rsafe＝0.05m，检测窗口半径R＝0.65m。领航者线速度约束为
Figure BDA00028083106900001412
角速度约束为
Figure BDA00028083106900001413
跟随者线速度约束为
Figure BDA0002808310690000151
角速度约束为
Figure BDA0002808310690000152
期望的编队参数为
Figure BDA0002808310690000153
无人车的控制周期为0.1s，领航者局部目标点优化问题中，参数矩阵Qlocal＝diag(60,60),目标点选取的范围约束中的参数λ＝0.7,预测障碍物的运动时间为Tl＝0.3s。领航者局部目标点跟踪问题的预测时域Tp＝0.3s，即未来三个控制周期。各参数矩阵分别为
Figure BDA0002808310690000154
Figure BDA0002808310690000155
跟随者轨迹跟踪问题中，预测时域与领航者局部目标点跟踪算法中的预测时域相同，Tp＝0.3s，各参数矩阵分别为
Figure BDA0002808310690000156
Figure BDA0002808310690000157
在t＝0s时刻，三辆无人车坐标分别为
Figure BDA0002808310690000158
Figure BDA0002808310690000159
不符合编队要求。环境中存在两个障碍物，其中第一个障碍物为静态障碍物，障碍点坐标为(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)的速度向右下方匀速移动。无人车需要在避障的同时靠近全局目标点并调整队形。右上方的动态障碍物最初在无人车与全局目标点连线的上方，会逐渐运动至连线下方。无人车需要在考虑障碍物运动信息的同时避开动态障碍物并到达全局目标点。
下面将以t＝7s时刻的迭代过程说明，请参阅图2，本优选实施例中，动态障碍环境下多车编队协同控制方法包括以下步骤：
S1，所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离。
在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)。
由于两个障碍物都检测到了障碍物顶点，障碍物顶点速度即为障碍物速度，因此可以根据障碍物顶点在当前以及上一采样周期的坐标计算障碍物速度：
Figure BDA0002808310690000161
其中，T为无人车的检测周期，
Figure BDA0002808310690000162
为障碍点εj,n(t)的移动速度，
Figure BDA0002808310690000163
为第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)。
t＝7s时刻，领航者坐标为(0.5592,0.7475,0.7252),领航者计算障碍物间沿领航者前进方向的距离，由于只有第二个障碍物在领航者前方，且障碍点到领航者前进方向直线的相对距离可由如下公式计算：
Figure BDA0002808310690000164
其中，ε(t)为障碍点坐标，pl(t)为领航者坐标，θl为领航者前进方向，有ε(t)＝(0.7330,0.8085)，pl(t)＝(0.5592,0.7475),θl＝0.7252。代入数据计算可得相对距离为dref＝0.0696,由于无人车另一侧没有障碍物，无人车检测范围R＝0.65m，因此无人车编队距离参数应使得无人车可以沿领航者前进方向通过宽度为dref+R＝0.7196的区域，由于期望的编队距离参数为
Figure BDA0002808310690000165
Figure BDA0002808310690000166
可以满足要求，故领航者确定当前的编队距离参数为0.18m。
S2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点。优化目标设计如下：
Figure BDA0002808310690000167
使得，
Figure BDA0002808310690000168
Figure BDA0002808310690000169
Figure BDA0002808310690000171
Figure BDA0002808310690000172
Figure BDA0002808310690000173
Figure BDA0002808310690000174
Figure BDA0002808310690000175
Figure BDA0002808310690000176
Figure BDA0002808310690000177
||al,n(t)||2≤1
n≤Ni(t).
上述各式中：
t为当前时刻，
Figure BDA0002808310690000178
为领航者局部目标点在笛卡尔系下的坐标，
Figure BDA0002808310690000179
为全局目标点坐标，Qlocal为领航者局部目标点优化问题中代价函数的权矩阵，al,n、bl,n为编队超平面避障约束中的超平面参数，Rsight为无人车障碍物检测窗口半径，λ为局部目标点优化范围参数，rsafe为无人车安全距离，
Figure BDA00028083106900001710
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure BDA00028083106900001711
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure BDA00028083106900001712
为障碍点
Figure BDA00028083106900001713
在t时刻的速度，Tl为预测时域。
该优化问题为二次规划问题，可直接通过MATLAB中的求解器求解，求解该优化问题后可获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点。代入数据求解出t＝7s时刻领航者局部目标点坐标为
Figure BDA00028083106900001714
S3，领航者获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的运动轨迹与对应的控制量。模型预测控制方法是一种优化算法，优化目标设计如下：
Figure BDA0002808310690000181
使得，τ∈(t,t+Tp),
Figure BDA0002808310690000182
Figure BDA0002808310690000183
Figure BDA0002808310690000184
Figure BDA0002808310690000185
Figure BDA0002808310690000186
Figure BDA0002808310690000187
||al,n(τ|t)||2≤1,n≤Ni(t).
其中，代价函数与惩罚函数的表达式为：
Figure BDA0002808310690000188
Figure BDA0002808310690000189
上述各式中：
Figure BDA00028083106900001810
为领航者控制量，,al,n、bl,n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA00028083106900001811
为领航者在笛卡尔系下的广义坐标，有
Figure BDA00028083106900001812
Figure BDA00028083106900001813
为领航者笛卡尔系下的横纵坐标，
Figure BDA00028083106900001814
为领航者朝向，
Figure BDA00028083106900001815
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure BDA00028083106900001816
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure BDA00028083106900001817
为障碍点
Figure BDA00028083106900001818
在t时刻的速度，rsafe为无人车安全距离，
Figure BDA00028083106900001819
为领航者到达局部目标点位置的期望状态，有
Figure BDA00028083106900001820
Figure BDA00028083106900001821
Figure BDA00028083106900001822
为代价函数与惩罚函数中的权矩阵。
该优化目标是最小化预测时域内领航者位置与局部目标点的位置之差，使得领航者可以快速靠近局部目标点。该优化问题为二次规划问题，可直接通过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)。
S4，领航者通过S3获得预测时域内的运动轨迹后，将领航者运动轨迹以及S1中确定的编队距离参数传递给跟随者。跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹：
Figure BDA0002808310690000191
式中：
Figure BDA0002808310690000192
为跟随者fi的参考轨迹，
Figure BDA0002808310690000193
为领航者在预测时域内的运动轨迹，
Figure BDA0002808310690000194
为领航者运动轨迹坐标，
Figure BDA0002808310690000195
为领航者运动轨迹朝向，也即跟随者fi的朝向，
Figure BDA0002808310690000196
为跟随者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)。
跟随者随后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量。模型预测控制方法是一种优化算法，优化目标设计如下：
Figure BDA0002808310690000197
使得，τ∈(t,t+Tp),
Figure BDA0002808310690000198
Figure BDA0002808310690000201
Figure BDA0002808310690000202
Figure BDA0002808310690000203
Figure BDA0002808310690000204
Figure BDA0002808310690000205
Figure BDA0002808310690000206
其中，代价函数与惩罚函数的表达式为：
Figure BDA0002808310690000207
Figure BDA0002808310690000208
上述各式中：
Figure BDA0002808310690000209
为跟随者控制量，,
Figure BDA00028083106900002010
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure BDA00028083106900002011
为跟随者在笛卡尔系下的广义坐标，有
Figure BDA00028083106900002012
Figure BDA00028083106900002013
Figure BDA00028083106900002014
为跟随者笛卡尔系下的横纵坐标，
Figure BDA00028083106900002015
为跟随者朝向，
Figure BDA00028083106900002016
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure BDA00028083106900002017
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure BDA00028083106900002018
为障碍点
Figure BDA00028083106900002019
在t时刻的速度，rsafe为无人车安全距离，
Figure BDA00028083106900002020
为代价函数与惩罚函数中的权矩阵。
该优化目标是最小化预测时域内跟随者轨迹与参考轨迹之差，使得跟随者可以准确地跟踪参考轨迹，维持编队队形。该优化问题为二次规划问题，可直接通过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)。
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)。
由于领航者未到达全局目标点，则迭代S1～S5，直到领航者到达全局目标点。
下面从整体流程介绍本实例，请参阅图3，t＝0s时刻，无人车初始位置不满足编队要求，且右方存在障碍物，优化出的局部目标点位于无人车右上方，引导无人车避障，无人车在跟踪局部目标点时会逐渐调整队形。t＝2.5s时刻，无人车已经避开了下方的障碍物，队形经过调整已经接近编队队形，在检测到障碍物后优化出的局部目标点在动态障碍物下方，引导无人车从障碍物下方进行避障。t＝4s时刻，无人车朝全局目标点移动，并针对动态障碍物不断调整队形。t＝7s时刻,此时无人车已经靠近动态障碍物。障碍物位于无人车与全局目标点连线的下方，优化出的局部目标点位于障碍物上方，引导无人车从障碍物上方避障。t＝8.5s时刻，无人车已经避开了障碍物，局部目标点在无人车与全局目标点连线上，引导无人车靠近全局目标点。t＝16s时刻，无人车维持队形并到达了全局目标点。
图4展示了各时刻跟随者与领航者的距离，其中
Figure BDA0002808310690000211
表示跟随者与领航者的期望编队距离，由于无人车的初始位置不满足编队需求，且无人车初始位置附近存在障碍物，无人车在初期需要在避障的同时调整队形，可以看到，跟随者与领航者间的距离逐渐逼近期望的编队距离。t＝4s至t＝8.5s期间，为了避开动态障碍物，无人车不断调整编队队形。t＝8.5s后，无人车避开动态障碍物后，在靠近全局目标点的过程中不断调整队形至期望值。
图5展示了动态障碍下多车编队过程中各时刻无人车的控制量，其中，(a)为各车各时刻线速度，(b)表示各车各时刻角速度。可以看出，领航者线速度在基本上在线速度约束的上界，保持高速行驶。跟随者除了在初期为了调整队形外，也都保持接近约束上界的线速度。全过程中领航者与跟随者的控制量都满足约束。
本发明上述实施例提供的动态障碍环境下的多车协同编队控制方法，考虑匀速运动的凸多边形障碍环境下的多车编队避障问题，采用领航跟随的编队策略以及模型预测控制方法实现编队控制。针对领航者，所有无人车收集自身周围的障碍物信息并传递给领航者，领航者根据障碍物分布确定无人车之间的编队距离；随后在领航者环境探测范围内优化出局部目标点引导编队提前避障；确定局部目标点后，领航者利用模型预测控制算法跟踪局部目标点，得到领航者在预测时域内的运动轨迹与对应的控制量。针对跟随者，领航者将当前时刻的编队距离与领航者在预测时域内的运动轨迹传递给跟随者；跟随者计算出预测时域内自身的参考轨迹，随后通过模型预测控制算法跟踪参考轨迹。本发明上述实施例提供的动态障碍环境下的多车协同编队控制方法，通过领航跟随的编队策略与基于模型预测控制的控制方法实现了动态环境下多车在避障的同时尽可能保持编队队形。
本发明另一实施例提供了一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时可用于执行本发明上述实施例中任一项的方法。
可选地，存储器，用于存储程序；存储器，可以包括易失性存储器(英文：volatilememory)，例如随机存取存储器(英文：random-access memory，缩写：RAM)，如静态随机存取存储器(英文：static random-access memory，缩写：SRAM)，双倍数据率同步动态随机存取存储器(英文：Double Data Rate Synchronous Dynamic Random Access Memory，缩写：DDR SDRAM)等；存储器也可以包括非易失性存储器(英文：non-volatile memory)，例如快闪存储器(英文：flash memory)。存储器用于存储计算机程序(如实现上述方法的应用程序、功能模块等)、计算机指令等，上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
上述的计算机程序、计算机指令等可以分区存储在一个或多个存储器中。并且上述的计算机程序、计算机指令、数据等可以被处理器调用。
处理器，用于执行存储器存储的计算机程序，以实现上述实施例涉及的方法中的各个步骤。具体可以参见前面方法实施例中的相关描述。
处理器和存储器可以是独立结构，也可以是集成在一起的集成结构。当处理器和存储器是独立结构时，存储器、处理器可以通过总线耦合连接。
本发明第三个实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可用于执行本发明上述实施例中任一项的方法。
本发明上述实施例提供的动态障碍环境下的多车协同编队控制方法、终端及介质，通过各无人车收集障碍物信息、领航者优化局部目标点、领航者路径规划、跟随者路径跟踪、迭代等步骤，在动态障碍环境下形成并维持编队，避开动态障碍物并到达全局目标点。本发明上述实施例提供的动态障碍环境下多车协同编队控制方法，针对动态障碍环境下的多车协同编队问题，采用领航跟随的编队策略，各无人车收集并向领航者提供自身周围的环境信息，由领航者确定编队参数并优化局部目标点，通过局部目标点引导无人车提前避开动态障碍。在获得局部目标点后，领航者通过模型预测控制算法进行路径规划，并将编队参数与参考路径传送给跟随者，跟随者可根据编队参数与领航者的参考路径确定自身的参考路径，并通过模型预测控制算法跟踪参考路径，实现多车整体编队控制。本发明上述实施例考虑了无人车的避障约束、无人车运动学模型以及控制量约束等约束条件，无人车可以形成并维持编队，对动态障碍物快速反应，最终到达全局目标点。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上仅为发明的较佳实例而已，并非用来限定本发明的实施范围。即凡依本发明申请专利范围的内容所作的等效变化与修饰，都应该为本发明的技术范畴。

Claims (10)

1.一种动态障碍环境下的多车协同编队控制方法，其特征在于，包括：
S1，无人车编队中的所有无人车检测自身周围的障碍物信息并传递给领航者，其中障碍物信息包括障碍物位置信息以及速度信息，领航者根据障碍物信息确定无人车之间的编队距离；
S2，领航者根据障碍物信息以及编队距离参数，通过优化算法确定领航者局部目标点；优化目标设计如下：
Figure FDA0002808310680000011
式中：
t为当前时刻，
Figure FDA0002808310680000012
为领航者局部目标点在笛卡尔系下的坐标，
Figure FDA0002808310680000013
为全局目标点坐标，Qlocal为领航者局部目标点优化问题中代价函数的权矩阵，al，n、bl，n为编队超平面避障约束中的超平面参数；
求解所述优化目标后，获得在领航者检测范围内满足编队避障约束且最靠近全局目标点位置的局部目标点；
S3，领航者通过S2获得局部目标点后，利用模型预测控制方法跟踪局部目标点，得到领航者在预测时域内的控制量以及相应的运动轨迹；所述模型预测控制方法采用在线滚动优化算法，在每个控制周期优化目标设计如下：
Figure FDA0002808310680000014
其中，
Figure FDA0002808310680000015
Figure FDA0002808310680000016
式中：
Figure FDA0002808310680000017
为领航者控制量，，al，n、bl，n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure FDA0002808310680000018
为领航者在笛卡尔系下的广义坐标，即为领航者相应的运动轨迹，其包含了预测时域内领航者各时刻的位置与速度信息，
Figure FDA0002808310680000021
为领航者到达局部目标点位置的期望状态，
Figure FDA0002808310680000022
为代价函数与惩罚函数中的权矩阵；
求解所述优化目标后，获得领航者在预测时域内的控制量以及相应的运动轨迹；
S4，领航者通过S3获得预测时域内的运动轨迹后，将领航者运动轨迹以及S1中确定的编队距离参数传递给跟随者；跟随者首先根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，然后利用模型预测控制方法跟踪参考轨迹，获得在预测时域内的控制量；所述模型预测控制方法采用优化算法，优化目标设计如下：
Figure FDA0002808310680000023
其中，代价函数与惩罚函数的表达式为：
Figure FDA0002808310680000024
Figure FDA0002808310680000025
式中：
Figure FDA0002808310680000026
为跟随者控制量，
Figure FDA0002808310680000027
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure FDA0002808310680000028
为跟随者在笛卡尔系下的广义坐标，即为跟随者的运动轨迹，
Figure FDA0002808310680000029
为跟随者的参考轨迹，
Figure FDA00028083106800000210
为代价函数与惩罚函数中的权矩阵；
求解所述优化目标后，获得跟随者在预测时域内的控制量；
S5，将S3中确定的领航者在当前时刻的控制量施加给领航者，将S4中确定的跟随者在当前时刻的控制量施加给跟随者，若领航者未到达全局目标点，则迭代S1至S5，直到领航者到达全局目标点。
2.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述S1中，无人车检测自身周围的障碍物信息，包括：
以无人车自身为圆心的圆形窗口为检测范围，其中障碍物为凸多边形结构；所述障碍物位置信息为障碍点信息，包括无人车检测范围内的障碍物顶点以及障碍物与检测窗口交点的坐标；无人车通过比较前后时刻的障碍点位置，计算出障碍物的运动速度信息：当无人车检测到障碍物顶点时，障碍物速度信息为障碍物顶点速度；当无人车只检测到障碍物边时，障碍物速度信息为障碍物沿垂直于边方向的运动速度；和/或
所述S1中，确定无人车之间的编队距离的方法，包括：
编队距离为无人车编队沿领航者速度方向从相邻障碍物之间通过的最大距离；若在该编队距离下，无人车编队内部会发生碰撞，说明当前障碍分布过于密集，则编队距离取由用户设定的初始值。
3.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述S2中，采用编队避障约束，包括：
Figure FDA0002808310680000031
Figure FDA0002808310680000032
Figure FDA0002808310680000033
Figure FDA0002808310680000034
Figure FDA0002808310680000035
Figure FDA0002808310680000036
Figure FDA0002808310680000037
||al，n(t)||2≤1
n≤Ni(t)；
式中：
t为当前时刻，
Figure FDA0002808310680000038
为领航者局部目标点在笛卡尔系下的坐标，al，n、bl，n为编队超平面避障约束中的超平面参数，rsafe为无人车安全距离，
Figure FDA0002808310680000039
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure FDA00028083106800000310
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure FDA00028083106800000311
为障碍点
Figure FDA00028083106800000312
在t时刻的速度，Tl为预测时域；
所述编队避障约束将无人车当前位置、局部目标点以及对应的编队位置约束到超平面的一侧，将障碍点以及运动Tl时间后的障碍点约束到超平面的另一侧，保证无人车在维持编队的情况下从当前位置到达局部目标点并且在预测时域内不与动态障碍物发生碰撞。
4.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，领航者到达局部目标点的期望朝向为领航者当前位置到局部目标点的连线方向，用于引导领航者提前转向。
5.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述S3中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的领航者运动轨迹以及相应的控制量，并将当前时刻的控制量作用于领航者。
6.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述S3中，采用编队超平面避障约束，包括：
τ∈(t，t+Tp)，
Figure FDA0002808310680000041
Figure FDA0002808310680000042
Figure FDA0002808310680000043
Figure FDA0002808310680000044
Figure FDA0002808310680000045
Figure FDA0002808310680000046
Figure FDA0002808310680000047
式中：
Figure FDA0002808310680000048
为领航者控制量，，al，n、bl，n为编队超平面避障约束中的超平面参数，Tp为预测时域，
Figure FDA0002808310680000049
为领航者在笛卡尔系下的广义坐标，其中
Figure FDA00028083106800000410
为领航者笛卡尔系下的横纵坐标，
Figure FDA00028083106800000411
为领航者朝向，
Figure FDA00028083106800000412
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure FDA00028083106800000413
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure FDA00028083106800000414
为障碍点
Figure FDA00028083106800000415
在t时刻的速度，rsafe为无人车安全距离，
Figure FDA00028083106800000416
为领航者到达局部目标点位置的期望状态，有：
Figure FDA0002808310680000051
所述编队超平面避障约束将预测时刻中每一时刻的领航者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，领航者跟踪局部目标点过程中不会与动态障碍物发生碰撞。
7.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述S4中，采用模型预测控制方法，随着迭代过程滚动优化出预测时域内的控制量，并将当前时刻的控制量作用于跟随者；和/或
所述S4中，跟随者根据领航者运动轨迹以及编队距离参数计算出自身在预测时域内的参考轨迹，包括：
根据领航者在预测时域内的运动轨迹以及编队距离参数，计算如下：
Figure FDA0002808310680000052
式中：
Figure FDA0002808310680000053
为跟随者fi的参考轨迹，
Figure FDA0002808310680000054
为领航者在预测时域内的运动轨迹，
Figure FDA0002808310680000055
为领航者运动轨迹坐标，
Figure FDA0002808310680000056
为领航者运动轨迹朝向，也即跟随者fi的朝向，
Figure FDA0002808310680000057
为跟随者fi的编队距离参数以及编队角度参数。
8.根据权利要求1所述的动态障碍环境下的多车协同编队控制方法，其特征在于，所述S4中，采用跟随者超平面避障约束，包括：
τ∈(t，t+Tp)，
Figure FDA0002808310680000058
Figure FDA0002808310680000059
Figure FDA00028083106800000510
Figure FDA00028083106800000511
Figure FDA0002808310680000061
Figure FDA0002808310680000062
Figure FDA0002808310680000063
式中：
Figure FDA0002808310680000064
为跟随者控制量，
Figure FDA0002808310680000065
为跟随者超平面避障约束中的超平面参数，Tp为预测时域，
Figure FDA0002808310680000066
为跟随者在笛卡尔系下的广义坐标，
Figure FDA0002808310680000067
为跟随者笛卡尔系下的横纵坐标，
Figure FDA0002808310680000068
为跟随者朝向，
Figure FDA0002808310680000069
为无人车i检测到的第n个障碍物中的第j个障碍点，
Figure FDA00028083106800000610
为无人车i检测到的第n个障碍物的所有障碍点构成的集合，Ni(t)为t时刻无人车i检测到的障碍物数量，
Figure FDA00028083106800000611
为障碍点
Figure FDA00028083106800000612
在t时刻的速度，rsafe为无人车安全距离；
所述跟随者超平面避障约束将预测时刻中每一时刻的跟随者位置约束在超平面的一侧，将对应时刻的障碍物约束在超平面的另一侧，保证在预测时域内，跟随者跟踪参考轨迹过程中不会与动态障碍物发生碰撞。
9.一种终端，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时可用于执行权利要求1-8中任一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时可用于执行权利要求1-8中任一项所述的方法。
CN202011380399.2A 2020-11-30 2020-11-30 动态障碍环境下的多车协同编队控制方法、终端及介质 Pending CN114594756A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011380399.2A CN114594756A (zh) 2020-11-30 2020-11-30 动态障碍环境下的多车协同编队控制方法、终端及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011380399.2A CN114594756A (zh) 2020-11-30 2020-11-30 动态障碍环境下的多车协同编队控制方法、终端及介质

Publications (1)

Publication Number Publication Date
CN114594756A true CN114594756A (zh) 2022-06-07

Family

ID=81803060

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011380399.2A Pending CN114594756A (zh) 2020-11-30 2020-11-30 动态障碍环境下的多车协同编队控制方法、终端及介质

Country Status (1)

Country Link
CN (1) CN114594756A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115686004A (zh) * 2022-10-27 2023-02-03 南京长峰航天电子科技有限公司 一种基于无人船分散式编队控制的路径动态规划方法
CN115993845A (zh) * 2023-03-23 2023-04-21 西北工业大学深圳研究院 一种集群智能系统协同运动规划与编队控制方法
CN116880193A (zh) * 2023-07-19 2023-10-13 合肥工业大学 基于乌卡模型的领航-跟随多无人车协同系统的控制方法
CN117631676A (zh) * 2024-01-25 2024-03-01 上海伯镭智能科技有限公司 一种矿区无人驾驶车辆自动引导前进的方法和装置

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115686004A (zh) * 2022-10-27 2023-02-03 南京长峰航天电子科技有限公司 一种基于无人船分散式编队控制的路径动态规划方法
CN115993845A (zh) * 2023-03-23 2023-04-21 西北工业大学深圳研究院 一种集群智能系统协同运动规划与编队控制方法
CN116880193A (zh) * 2023-07-19 2023-10-13 合肥工业大学 基于乌卡模型的领航-跟随多无人车协同系统的控制方法
CN116880193B (zh) * 2023-07-19 2024-02-23 合肥工业大学 多无人车协同系统的控制方法及其控制装置、终端、介质
CN117631676A (zh) * 2024-01-25 2024-03-01 上海伯镭智能科技有限公司 一种矿区无人驾驶车辆自动引导前进的方法和装置
CN117631676B (zh) * 2024-01-25 2024-04-09 上海伯镭智能科技有限公司 一种矿区无人驾驶车辆自动引导前进的方法和装置

Similar Documents

Publication Publication Date Title
CN114594756A (zh) 动态障碍环境下的多车协同编队控制方法、终端及介质
Stahl et al. Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios
CN112631294B (zh) 一种移动机器人智能路径规划方法
Kunz et al. Autonomous driving at Ulm University: A modular, robust, and sensor-independent fusion approach
CN110262495A (zh) 可实现移动机器人自主导航与精确定位的控制系统及方法
Ardiyanto et al. Real-time navigation using randomized kinodynamic planning with arrival time field
Fernández et al. Improving collision avoidance for mobile robots in partially known environments: the beam curvature method
Von Hundelshausen et al. Driving with tentacles: Integral structures for sensing and motion
Roussos et al. 3D navigation and collision avoidance for nonholonomic aircraft‐like vehicles
Li et al. A practical trajectory planning framework for autonomous ground vehicles driving in urban environments
CN109871031B (zh) 一种固定翼无人机的轨迹规划方法
CN109270933A (zh) 基于圆锥曲线的无人驾驶避障方法、装置、设备及介质
CN113341984A (zh) 基于改进rrt算法的机器人路径规划方法和装置
CN112577506B (zh) 一种自动驾驶局部路径规划方法和系统
CN104501816A (zh) 一种多无人飞行器协调避碰导引规划方法
Sisto et al. A fuzzy leader-follower approach to formation control of multiple mobile robots
CN113296523A (zh) 一种移动机器人避障路径规划方法
CN115933648A (zh) 一种机器人动态避障方法及系统
Mouhagir et al. A markov decision process-based approach for trajectory planning with clothoid tentacles
CN115309163A (zh) 基于改进方向评价函数dwa算法的局部路径规划方法
Li et al. Real-time local path planning for intelligent vehicle combining tentacle algorithm and B-spline curve
Xin et al. Coordinated motion planning of multiple robots in multi-point dynamic aggregation task
Xiong et al. Surrounding vehicle trajectory prediction and dynamic speed planning for autonomous vehicle in cut-in scenarios
Alsaab et al. Improving velocity obstacle approach for obstacle avoidance in indoor environments
Li et al. A real-time and predictive trajectory-generation motion planner for autonomous ground vehicles

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