一种基于图论与势场方法的无人车多车道护航编队方法与流程
本发明涉及智能汽车自动驾驶控制技术，尤其涉及一种基于图论与势场方法的无人车多车道护航编队方法。
背景技术：
目前世界上大多数的自动驾驶汽车均采用“感知-规划-控制”(sense-plan-act)三层结构模式，首先利用车身装载的计算机视觉系统和先进的传感装置构筑的感知系统采集车身周围的道路环境信息；再采用复杂高效的智能算法处理环境信息，根据交通法规制定安全可靠的行驶策略，实时规划出可行的参考轨迹；最后对规划出的局部参考轨迹进行精确地跟踪。同时，车辆编队驾驶能够有效的提高道路利用率以及降低油耗，多车编队驾驶具有重要的意义和价值。因此，如何在考虑道路环境信息以及满足编队驾驶需求的前提下，规划出安全、平滑、高效的车辆驾驶路径仍然是自动驾驶技术研究的一项挑战。目前国内外研究学者对编队驾驶路径规划的研究主要使用领导-跟随法、虚拟结构法、基于行为的控制方法。但在方法上侧重于保持编队的稳定和精度，缺乏对于环境因素的考虑，车辆编队灵活性较差。
技术实现要素：
本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于图论与势场方法的无人车多车道护航编队方法。
本发明解决其技术问题所采用的技术方案是：一种基于图论与势场方法的无人车多车道护航编队方法，包括以下步骤：
1)根据无人车队的编队控制向车辆发送预定义的编队结构信息，所述编队结构信息包括存在组队关系的车辆数量、车辆位置和车辆间的期望距离；
2)根据预定义的无人车队形和无人车之间的间距，使用图论方法，规划出保持编队所需的速度和位移；
3)根据无人车队队形规划获得无人车编队轨迹规划，对编队轨迹规划进行无人车避障轨迹规划；
4)将无人车编队轨迹规划和无人车编队轨迹规划的规划结果进行加权，得到车辆的最终规划结果，生成最终轨迹。
按上述方案，所述步骤1)中编队结构信息抽象为简单图结构，图中节点为车辆位置，图中的边表示两车间存在保持间距的组队关系，边的长度为车辆间的期望距离。
按上述方案，所述步骤2)中编队车辆将根据周围存在边的车辆的位置规划自车轨迹，调整自车位置，从而与周围车辆保持期望间距。基于图论方法，设计分布式的编队控制方程，以完成上述规划过程，具体过程如下：
编队结构的图定义为
其中：ek为图
根据关联矩阵定义拉普拉斯矩阵
式中，
根据图的相关理论和当前车辆与其他车辆间距，规划出保持编队所需的速度和位移；
其中，
按照上述方程计算下一时刻的车辆位置，则编队车辆可以形成并保持局部图结构的稳定，编队整体将趋于形成预定义的编队结构并保持稳定，在(s,l)坐标系下，方程为：
式中，i，j表示存在边的两个节点，即车辆i与车辆j，wi,j为边的权重，si,j和li,j为车辆i与车辆j的横纵向间距，bs和bl为图结构期望的横纵向距离；
s-l坐标系为根据道路曲线形状对道路进行建模，以车辆沿道路中心线前进方向为s轴，垂直于道路中心线方向为l轴，(s,l)表示车辆在s-l坐标系下的坐标。
按上述方案，所述步骤3)中避障轨迹规划为基于势场法的避障轨迹规划，根据无人车感知的当前环境，使用势场法对环境进行建模，量化行驶风险，调整编队结构和势场力与速度的比例系数，规划出安全避障的速度与路径。
按上述方案，所述根据无人车感知的当前环境，使用势场法对环境进行建模，具体如下：
受控车的总势场u定义为道路边界势场uroad、环境车势场uecar、编队车辆势场ufcar、保持车道势场ukeep_lane或换道势场uchange_lane、车辆前进势场ufront的总和：
u＝uroad+uecar+ufcar+ufront+(ukeep_laneoruchange_lane)
对于道路边界势场uroad，道路的边界是车辆不能逾越的区域，因而在车辆靠近道路边界时，势场值应迅速增大，并在道路边界达到最大值，具体势场函数如下：
其中：d(road)为车辆与道路边界的距离，kroad>0为道路势场比例系数，droad为道路边界的影响范围；
保持车道势场ukeep_lane如下：
其中，dmid_lane为车辆与当前车道线距离，λkeep_lane为保持车道势场的比例系数；
当车辆有换道需求时，势场应沿换道方向下降，驱使车辆换道，采用换道势场函数：
其中，λchange_lane为变道势场比例系数，wlane为车道宽度，
编队车辆势场ufcar对编队内车辆建立势场，能够保证车辆在编队动态调整时，编队内的车辆不发生碰撞，其势场函数为：
其中，i表示第i辆编队内车辆，kfcar>0为比例系数，di(fcar)为距离第i辆编队内车辆的距离，dfcar为编队内车辆的势场影响范围；
环境车势场如下：
其中，i表示第i辆环境车辆，kecar和λecar为比例系数，di(ecar)为受控车距离第i辆环境车辆的距离，decar为环境车辆势场影响的最大距离，只有车辆在环境车辆的影响范围内，势场才会作用于车辆，sr为受控车传感器感知范围。
按上述方案，所述调整编队结构和势场力与速度的比例系数，具体如下：
设定车辆受到的势场力和速度存在比例关系k，即
vtarget＝k*fapf
fapf为车辆受到的势场力，vtarget为规划出的目标速度，k为比例系数；
比例系数k的设定包括以下情况：
当车辆环境中不存在任何障碍时，给车辆施加一个向前的虚拟力ffront，驱使车辆匀速前进，则：
其中，vrefer为车辆巡航的期望速度；
当车辆前方存在障碍车却无法换道避让时，车辆速度降至与障碍车相同，同时与障碍车保持安全距离；根据前方障碍车势场和速度设定势场力和速度的比例系数k：
其中，vrefer为车辆巡航期望速度，tsafe为安全时距，vocar为前方障碍车速度，sr为车辆定位障碍车的最大感知范围。
按上述方案，所述基于图的规划结果和基于人工势场的规划结果的加权下式所示：
式中，a，b∈[0,1]为权重系数，a+b＝1，g为基于图的规划结果，k*fapf为基于人工势场的规划结果，fapf为车辆受到的势场力，是一个向量，δt为控制周期时长。
本发明产生的有益效果是：
本发明方法将无人车编队结构抽象为一个无向图结构，并设计相应的编队调整策略，使得编队在不同场景下合理的调整编队，完成不同的任务需求。基于图的相关结论，提出分布式车辆编队规划方程，计算维持编队结构的轨迹，同时，对行车环境建立势场模型，设计环境因素的势场函数，根据车辆受到的势场值计算避障所需的局部轨迹。最后将上述两部分轨迹进行加权，得到车辆最终的期望路径与速度，从而在保持编队结构的同时，完成避让障碍，安全高效的进行编队驾驶。
本发明方法基于图与势场法提出了一种多车道编队方法，设计了编队调整车辆，并对行车环境建立了势场模型，设计了不同影响因素的势场函数，同时引入势场力与车辆速度的比例系数，对无人车的路径和速度进行了统一规划，提高了多车道编队的灵活性和避障能力。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1是本发明实施例的方法流程图；
图2是本发明实施例的编队结构图节点与车辆映射示意图；
图3是本发明实施例的编队状态转移图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
如图1所示，一种基于图论与势场方法的无人车多车道护航编队方法，包括以下步骤：
1)根据无人车队的编队控制向车辆发送预定义的编队结构信息，所述编队结构信息包括存在组队关系的车辆数量、车辆位置和车辆间的期望距离；
如图2，编队结构信息可抽象为简单图结构，图中节点为车辆位置，图中的边表示两车间存在保持间距的组队关系，边的长度为车辆间的期望距离；
2)根据预定义的无人车队形和无人车之间的间距，使用图论方法，规划出保持编队所需的速度和位移；
步骤2)中编队车辆将根据周围存在边的车辆的位置规划自车轨迹，调整自车位置，从而与周围车辆保持期望间距。基于图论方法，设计分布式的编队控制方程，以完成上述规划过程，具体过程如下：
编队结构的图定义为
其中：ek为图
根据关联矩阵定义拉普拉斯矩阵
式中
根据图的相关理论和当前车辆与其他车辆间距，规划出保持编队所需的速度和位移；
其中，
按照上述方程计算下一时刻的车辆位置，则编队车辆可以形成并保持局部图结构的稳定，编队整体将趋于形成预定义的编队结构并保持稳定，在(s,l)坐标系下，方程为：
式中，i，j表示存在边的两个节点，即车辆i与车辆j，wi,j为边的权重，si,j和li,j为车辆i与车辆j的横纵向间距，bs和bl为图结构期望的横纵向距离；
s-l坐标系为根据道路曲线形状对道路进行建模，以车辆沿道路中心线前进方向为s轴，垂直于道路中心线方向为l轴，(s,l)表示车辆在s-l坐标系下的坐标。
至此，车辆编队能够完成任意合理预定义结构。但多车道编队尚无法满足不同的编队需求(车辆加入，脱离等),也无法完成对环境中障碍物的避让。下面将阐述编队调整策略；
多车道车辆编队在行驶过程中，可能会有新的车辆加入编队，或者编队内的车辆希望脱离编队，以及道路中的一个或多个车道存在障碍物的情况，多车道的编队无法继续保持预定义的编队结构，而需要进行动态调整。同时，当编队前方道路阻塞时，编队应整体减速并与障碍物保持安全距离，以及车辆编队主动切换编队结构时，同样需要进行调整。本文将编队的状态集合设为{保持编队，车辆加入，车辆脱离，编队避障，编队切换结构，编队减速}。
编队状态发生转移的触发事件主要有：
f1：有无人车申请加入编队；
f2：编队中有无人车申请脱离编队；
f3：道路中存在环境车辆阻塞车道，对编队造成阻碍。
f4：前方道路所有车道均被阻塞
f5：编队收到切换结构指令
f6：道路恢复通畅
f7：无人车脱离或加入完成
f8：编队结构恢复稳定
f9：部分道路恢复畅通
通过上述事件的触发，使得编队状态发生转换，以便采取正确的调整策略。无人车多车道编队状态转移图，如图3所示。
编队避障、编队切换结构、编队加入车辆，车辆脱离编队等状态下，编队的调整过程较为复杂，下面将具体介绍四种状态下的编队调整策略。
编队中存在一辆特殊车辆保存着整个编队中所有车辆和图节点的对应关系，该车作为编队管理车辆，维护着车辆和图节点的对应关系，仅当编队车辆期望位置变化时，向车辆发送信号，更新期望位置。本文称为编队管理车辆。该编队管理车辆可由任意车辆担任，并且随时可由其他车辆接替。
1.无人车加入状态
车辆编队在行驶过程中，当有新的无人车希望加入编队时，编队进入车辆加入状态，进行相应的调整。期望加入编队的无人车与编队管理车辆进行通信，将自车的位置与速度发送至管理车辆，随后管理车辆进行应答与动作，编队调整好后，无人车再加入编队中。
本实施例中无人车编队的期望编队结构为八车两车道的矩形结构，设定黄色车辆为待加入编队的无人车，绿色车辆为编队中的无人车，蓝色车辆为编队中的编队管理车辆，编队任意车辆都可作为编队管理车辆，虚线矩形框是编队结构中还没有真实车辆的空缺位置，车辆加入编队过程共四个步骤：
步骤一：当无人车与车辆编队进入适当范围时，待加入编队车辆与编队中的编队管理车辆进行通信，将自车速度与位置发送至编队管理车辆。编队管理车辆根据当前车辆编队中空缺的节点位置(即图中虚线矩形框)，和待加入的无人车位置，将距离待加入无人车最近的空缺节点作为新增的车辆位置。
步骤二：编队管理车辆与编队中的部分车辆(即位于待加入车辆与分配的空缺节点之间的编队车辆)进行通信，这些车辆将被编队管理车辆重新分配位置，将步骤一新增的车辆位置填满，为待加入车辆空出编队节点位置。
步骤三：待加入车辆根据编队空出的位置和当前自车位置，进行变道，切换到期望的车道。
步骤四：待加入车辆调整速度，缩短自车与期望位置的纵向距离，进入期望位置，车辆完成加入编队动作。
2.编队避障状态
当环境中存在其他环境车辆，且对无人车多车道编队形成阻碍时，车辆编队进入编队避障状态，位于阻塞车道的编队车辆并入通畅车道，编队结构进行调整以安全通过阻碍区域。
车辆编队避障的整个调整过程中，无需编队管理车辆的介入，编队中所有车辆无主从关系。设定橙红色车辆为慢速环境车，该车无法与编队中的无人车建立车间通信，编队中的无人车通过自身车载传感器，感知环境中阻碍车的存在。
当感知到某些车道被环境车阻塞的情况时，阻塞车道上的编队车辆向相邻的通畅车道上的车辆发起通信，发出并入请求。同时，与相邻车道上的车辆建立前后相接的链式结构，相邻车道上的车辆根据临时建立的链式结构，空出相应的空间以容纳并入的车辆。
随后，被阻塞车辆并入通畅车道，车辆编队整体进入临时避让状态，编队结构发生变化。当避让动作完成，阻塞车道变得通畅时，编队整体恢复预定义的结构，整个避障状态下的调整过程完成。
3.编队切换结构状态
无人车的多车道编队分布在多个车道，结构多样，实际编队中可能对编队结构有特殊要求，或需要编队主动切换编队结构以满足特定的需求，当车辆编队收到切换编队结构指令时，编队进入编队切换结构状态。
本文的车辆编队结构的切换主要指切换时车辆数目不发生变化的情况，即车辆数目相同的两种编队结构间的切换，不同结构不同车辆数目间的结构切换可以拆分为两步，同数目不同结构的切换、同结构编队车辆加入和脱离两步。
本实施例中提供8辆无人车组成的两种编队结构，编队结构1为八车两车道矩阵结构，编队结构2为八车三车道工字形结构。车辆编队主动切换队形结构时，需要重配置编队中所有车辆的位置，这一过程需要编队管理车辆的参与。当编队结构1向编队结构2切换时，编队管理车辆假定期望队形(编队结构2)存在于当前编队的前方，对编队车辆位置进行重配置时，按照距离最近原则给编队结构2中的位置分配实际车辆，并优先分配期望结构中(结构2)最前最左的位置。
换而言之，将编队结构2的车辆位置视为空缺的位置，需要编队结构1中的车辆填充，先选定待填充的位置，按照最前最左的原则选定先给数字1分配车辆，距离数字1位置最近的车辆为车辆a，则车辆a的期望位置为结构2中的位置1。类似的，再给位置2分配车辆，如此重复，直至所有车辆分配到所有的位置上，完成编队车辆的位置重配置。每辆车再调整实际位置，向期望的位置靠拢，形成编队结构2。将期望编队结构放置于当前编队前方，并按照最前最左原则进行位置的重配置，能够避免无人车在调整过程中发生车辆相互形成阻碍的死锁情况。
4.车辆脱离编队状态
在特定的情景下，编队内的车辆需要脱离编队，如车辆故障或能源不足，无法继续编队行驶，此时编队进入车辆脱离状态，车辆向编队管理车辆发出脱离编队请求，编队管理车辆从编队结构中删去脱离车辆节点，调整其他编队车辆位置，为脱离车辆空出空间，并重新配置剩余所有无人车位置。
设定蓝色车辆为编队管理车辆，绿色车辆为编队内车辆，黄色车辆为欲脱离编队车辆。整个脱离过程主要有4个步骤。步骤一：黄色车辆与编队管理车辆通信，发出脱离编队请求，编队管理车辆收到脱离请求后，与黄色车辆同车道的其他编队车辆通信，向其发送避让指令。步骤二：与黄色车辆同一车道的其他编队内车辆并入相邻车道，避让脱离车辆，为其空出空间。步骤三：黄色车辆所在车道前后通畅，该车可以加速或减速与编队拉开距离，当脱离距离超过设定值时，向编队管理车辆发送脱离成功信号。步骤四：黄色车辆加速脱离编队成功后，管理车辆重配置剩余车辆在编队内的位置，恢复编队结构。
3)根据无人车队队形规划获得无人车编队轨迹规划，对编队轨迹规划进行无人车避障轨迹规划；
所述步骤3)中避障轨迹规划为基于势场法的避障轨迹规划，根据无人车感知的当前环境，使用势场法对环境进行建模，量化行驶风险，调整编队结构和势场力与速度的比例系数，规划出安全避障的速度与路径；车辆在行驶过程中，无人车感知的当前环境影响因素主要包括道路边界，车道线，环境车，编队内车辆等；
1)根据无人车感知的当前环境，使用势场法对环境进行建模，具体如下：
受控车的总势场u定义为道路边界势场uroad、环境车势场uecar、编队车辆势场ufcar、保持车道势场ukeep_lane或换道势场uchange_lane、车辆前进势场ufront的总和：
uall＝uroad+ufcar+uecar+ufront+(ukeep_laneoruchange_lane)
道路的边界是车辆不能逾越的区域，因而在车辆靠近道路边界时，势场值应迅速增大，并在道路边界达到最大值，具体势场函数如下：
其中，kroad>0为道路势场比例系数，d(road)为车辆与道路边界最短距离，droad为道路边界的影响范围，只有车辆在道路边界的影响范围内，势场才会作用于车辆。假定车道宽为3.6m，边界影响范围为1.8m。
保持车道势场函数如下：
式中，dmid_lane为车辆与当前车道线距离，λkeep_lane为保持车道势场的比例系数，假定车道宽度为3.6m；
当车辆有换道需求时，势场应沿换道方向下降，驱使车辆换道，采用换道势场函数：
式中λchange_lane为变道势场比例系数，wlane为车道宽度，
对编队内车辆建立势场，能够保证车辆在编队动态调整时，编队内的车辆不发生碰撞，其势场函数与道路边界势场函数类似：
其中，i表示第i辆编队内车辆，kfcar>0为比例系数，di(fcar)为受控车与第i辆编队车辆车体的最短距离，dfcar为编队车辆势场的影响范围。
环境车势场如下：
式中，i表示第i辆环境车辆，kecar和λecar为比例系数，di(ecar)为受控车距离环境车辆的距离，decar为环境车辆势场影响范围，sr为受控车传感器感知范围。
调整编队结构和势场力与速度的比例系数，
引入势场力与车辆速度的比例系数k，即假定车辆收到的势场力与车辆下一时刻的速度正相关，并按照该比例系数规划车辆速度。势场值越大，车辆在该位置收到的势场力越大，而势场值大表明该驾驶区域的碰撞风险高，车辆需尽快驶离该区域。因此，车辆收到的势场力大小与车辆速度大小正相关，引入比例系数k来表示正相关的关系，即：
vtarget＝k*fapf
式中vtarget为车辆规划出的下一时刻的目标速度，k为势场力与速度的比例系数，fapf为车辆受到的势场力。
当确定比例系数k后，需要根据比例系数k和车辆期望巡航速度，给无人车一个沿道路前进方向的虚拟势场力，以驱动车辆按照期望速度向前行驶，即：
式中ffront为需要施加给车辆的向前虚拟势场力，vrefer为车辆期望的巡航速度，k为势场力与速度的比例系数。根据该式即可计算ffront的大小。
当受控车前方存在慢速车辆，且其他车道同样不通畅时，受控车采取减速跟车策略，受控车希望减速至与障碍车相同速度，同时与障碍车保持安全距离，这种情况下，由于受控车与环境车辆的速度产值并不确定，可能无法在保持最短安全距离的同时，将速度降低至环境车辆的速度。所以，这种情况下车辆需根据前方障碍车距离、速度，感知范围以及最短安全距离，重新设定比例系数k，如下式所示：
式中，vrefer为车辆期望巡航速度，vocar为障碍车速度，λecar为环境车势场系数，sr为受控车传感器感知范围，tsafe为无人车与环境车应保持的最短安全时距。
通过上式重新设定比例系数k后，车辆能够在与障碍车保持最短距离的同时将速度降至与障碍车相同，从而完成减速跟车的驾驶行为，当交通场景发生变化，车辆无需减速跟车时，只需将比例系数k重置回初始值即可。
规划出安全避障的速度与路径；
基于人工势场的避障路径规划，使用梯度下降法求得无人车所受势场力的大小，结合规划出的避障速度，得到车辆下一时刻的位置，其具体如下式：
其中，uall为车辆受到达总时长，vtarget为规划出的避障速度，fapf为车辆受到的势场力，δt为控制周期。
4)将无人车编队轨迹规划和无人车编队轨迹规划的规划结果进行加权，得到车辆的最终规划结果，生成最终轨迹。
基于人工势场的轨迹规划能够有效的量化车辆行车环境的驾驶风险，提高车辆的避障能力，基于图的编队控制，能够对多辆无人车进行编队控制，实现预定义编队结构，二者都是对车辆的速度与路径进行规划，完成相应的控制任务。本文对二者进行结合，将两种方法的规划结果进行加权，得到车辆的最终规划结果，即下一时刻的速度与路径，再由车辆控制器去跟踪规划出的下一时刻速度与路径，得到车辆控制量，完成无人车多车道编队的一个控制周期，下一周期重复进行类似的工作。
基于图的规划结果和基于人工势场的规划结果的加权下式所示：
式子中，a，b∈[0,1]为权重系数，g为基于图的规划结果，k*fapf为基于人工势场的规划结果，fapf为车辆受到的势场力，是一个向量。δt为控制周期时长。
根据车辆期望轨迹，通过车辆横纵向控制器计算车辆控制量(前轮转角，刹车油门等)，完成对期望轨迹的跟踪，至此完成一次对于车辆编队控制。
应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。
- 还没有人留言评论。精彩留言会获得点赞！