CN111554081A - 一种基于多级领导者鸽群理论的车队路口避障控制方法 - Google Patents

Abstract

本发明公开了一种基于多级领导者鸽群理论的车队路口避障控制方法，包括步骤1，建立车队领导者数目切换原则；步骤2，定义风险感知区与危险感应区范围；步骤3，确定交通流车辆的集合划分；步骤4，计算车队中智能车i的风险系数μ_i；步骤5，由风险系数μ_i确定二级领导者；步骤6，建立二级领导者的应急避障控制；步骤7，判断二级领导者是否成功完成避让，如果完成避让，则执行步骤8；步骤8，建立二级领导者的领航控制；步骤9，建立二级领导者后车的避让跟随控制；步骤10，车队通过路口并退级切换。本发明提升了智能车队路口的通行效率，提高车队对路口突发事件的避让成功率，解决了智能车队过路口危险多发的问题。

Description

一种基于多级领导者鸽群理论的车队路口避障控制方法

技术领域

本发明属于车联网智能车队协同与应急避障技术领域，特别涉及了一种基于多级领导者鸽群理论的车队路口避障控制方法。

背景技术

为满足安全、高效、智能化交通发展的要求，智能网联汽车多车协同避障策略成为近年来发展和研究的热点方向。交叉路口作为城市交通环境的典型场景，由于人车混杂且交通主体之间的交互和影响十分复杂，极易发生交通事故，是城市交通的“咽喉”所在。车队在交叉路口弯道避障的不确定性和风险性，均高于直线路段的行驶环境。针对此，国内外研究者开展了大量理论和实验探讨，研究重点已逐渐从单一的车载系统向路口人车路一体化协同方向发展，近年来，车车协同、车路协同、智能化车辆安全技术和安全辅助驾驶技术等成为多学科交叉的研究热点。

与此同时，随着仿生学的应用日益普及，鸽群的迁徙为解决交叉路口等问题提供了一种崭新的思路。鸽群在迁徙过程中飞行决策采用平等交互模式或层级交互模式，两种模式等同于车队中的无领导编队与领导车辆-从属车辆两种编队模式。平等交互模式下，车辆之间的行驶决策相互影响；层级交互模式下，车队存在领导者，会对其车队内其他跟随车辆的跟驰行为产生决定性影响。鸽群在迁徙的过程中，两种模式切换与鸽群轨迹曲率呈现一定的关联，当鸽群飞行状态平稳时为平等交互模式，运动曲率发生较大变化时，原鸽群个体转而采用层级交互模式，层级交互模式相比于平等交互模式，具有信息传递更快、效率更高的特点。

因此，本发明针对复杂交叉路口环境下，结合鸽群平等交互模式和层级交互模式，提出一种面向动态威胁车的车队协同应急避障方案。该方案基于两种鸽群迁徙模式及对应的模式切换原则，将该理论引入到多车协同的路口控制方法中，从而达到车队过弯时动态应急避障并保障车队安全顺利通过路口。

车队在路口进行转弯运动时，运动曲率较之前直线行驶发生较大变化，对应鸽群理论，即此刻进行模式切换，车队由转弯前的平等交互模式转为头车作为领导者的领航-跟随模式，在在领导者的层级交互模式下能够降低车辆之间的通讯延迟，更快的通过路口。但处在路口复杂交通场景下存在各种突发状况，当领航-跟随模式下领导者带领车队转弯时，出现旁侧车道的转弯车辆贴近我方车队或路口行人在行驶通过路口时逐渐逼近我方车队的情况，车队将存在较大安全隐患并应根据此刻危险系数的大小采取不同程度的避让行为，此刻，若车队中被贴近的车辆或被行人逼近的车辆不是作为头车的领导车时，贴近的车辆以及逼近的行人无法对领导者产生威胁，原有的一级领导者也因此无法产生避让措施。若此时贴近车辆或行人进一步逼近车队中从属车时，从属车辆将进行紧急避让并发生较大的航向角偏转，当偏转角度超过阈值将导致在队形之中丢失前车目标，导致在整个车队形位置丢失、破坏整个队形并对后方从属车辆产生极大的风险。

因此，当在复杂的路口环境下，行人、过路行驶车辆与越线行驶等混合交通流对车队产生了较大的威胁，当出现碰撞风险时，仅考虑单领导者的车队协同已显然不能够保证车队安全通过交叉路口。若此时引入鸽群理论，并采用可切换模式的多领导者控制策略，将能较好的解决上述问题。

发明内容

本发明提出的多级领导者切换的车队路口避障控制方法很好解决了上述问题。车队领导车转弯并进入直行后，旁车或人行道行人逼近车队从属车时，直接受到威胁的从属车将进行动态避让，运动曲率突然再一次发生较大改变，仿照鸽群模式互切换机制，此时满足新领导者产生的条件，计算车队每一辆车的风险系数，并定义此刻车队中风险系数最大的避让车成为二级领导者，其等级在车队中低于一级领导者但高于普通从属车，同时二级领导车后方跟随车辆不在以头车为领导者，转而以已经做出避让反应的二级领导者为目标进行跟随，可在跟驰二级领导者下提前做出避让反应，提前进行避让。当车队完成避让顺利并带领整个车队通过路口开始直线行驶时，二级领导者退出领导，切换为变为普通跟随者。车队再一次回到初始行驶状态。

该模式切换下的路口避让控制方法考虑到交通流对车队的不同影响，根据旁车贴近以及行人逼近时计算车队中各车的风险系数，依据危险系数确定二级领导者，同时对二级领导车和其后从属车采用不同的控制输入，使得二级领导者能够根据其自身风险系数的大小采取不同程度的避让措施，从而首先保障自身安全的情况下带领后方从属车及时进行有效的风险规避。

本发明的有益效果：

1、基于对鸽群迁徙的仿生原理研究，提出了一种二级领导者随场景产生并退出的多级领导者切换的车队协同避让方法，该方法能够有效提升智能车队在交叉路口的通行效率，提高车队对路口突发事件的避让成功率，在一定程度上解决了智能车队过路口危险多发的问题，而且达到了路口过弯时动态引导后方车队车辆避让、有效规避风险的同时保障车队安全通过路口的目的。

2、车队中的二级领导者根据场景的需要出现，由风险系数μ_i确定，并可根据实际交通场景进行更新并改变。

3、对于影响车队通过交叉路口的物体根据半径R_caution与R_danger大小划分为了三类集合，并根据三类集合物体对该智能车过路口的风险不同推导出车队中每一辆智能车i的风险系数μ_i，该计算方法基于实际交通流场景，能够实时更新风险系数μ_i并变更车队中的二级领导者，提升领导效率。

4、将风险系数引入μ_i人工势场并建立了基于速度变化人工势场的函数模型，更贴近于实际的交通流场景，提高模型的可用性与实际准确性。

5、对于二级领导者的领航控制引入基于人工势场法的PID反馈控制，加入目标点的轨迹跟踪预瞄误差反馈的两个特征量。基于时变参数人工势场的PID协调控制能够实现较高的控制精度以及较快的响应速度,并能够有效的削弱横向误差，进一步保证精度提高领航的安全性。

附图说明

图1为风险感知区与危险感应区示意图；

图2为交通流车辆的集合划分示意图；

图3为车队路口旁车及行人逼近示意图；

图4为车队路口后车轨迹跟踪示意图；

图5为转弯通过路口的避让行为示意图；

图6为旁车逼近时二级领导者与旁车斥力控制输入几何关系示意图；

图7为后车的PID跟随控制示意图；

图8为基于多级领导者鸽群理论的车队路口避障控制方法框图；

具体实施方式

下面结合附图对本发明作进一步说明。

如图8所示，本发明的实施包含如下步骤：

Step1:建立车队领导者数目切换原则

考虑有N辆直线行驶的无领导者智能车编队，每辆车辆个体i可视作一个质点，其动力学模型如下式所示：

其中p_i,q_i,u_i分别为编队车辆i的位置矢量、速度矢量、控制输入。其中||p_i||₂＜V_max，V_max＞0，V_max为编队中个体允许的最大的行驶速度，N为车辆的数量。

定义智能车行驶时车队中某一辆车轨迹的曲率改变量Δρ_i，车队中领导者数目标识符Leader_i，车队发生领导者数目切换的Δρ_i阈值为K_switch，其中K_switch为一常数。车队为在通过路口前直线行驶，为平等交互模式，Leader_i＝0。当车队即将通过路口并开始转弯时切换为头车领导的单领导者层级交互模式，此时领导者数目标识符Leader_i＝1，当旁车贴近或行人逼近时车队中从属车产生避让行为并产生双领导者交互模式，此时Leader_i＝2。Leader_i与K_switch具有如下关系：

在层级交互模式下，切换为Leader_i＝2必须以Leader_i＝1的层级交互模式为前提，不可跨级切换，也不可越级退出。

Step2:定义风险感知区与危险感应区范围

基于汽车行驶的安全距离考虑在三种Leader_i模式的车队行驶的过程中，车队中每一辆无人车均存在以自身(x,y)为圆心、半径R_caution和R_denger为大小的风险感知区与危险感应区，如图1所示，其中：

L为车身长度，δ为环境影响因子，晴朗交通正常情况下δ＝1且大小与天气恶劣程度和交通流量成正相关。

Step3:确定交通流车辆的集合划分

如图2所示，根据所定义风险感知圈和危险感应圈可以将车队智能车i旁的物体j(j＝1,2...N)根据距离智能车i的远近分为三类集合：

其中，||X_i-X_j||为智能车i与物体j的欧式距离。车队内部车一般为无风险车，属集合{j1}，对于旁道压线行驶的车，因距离较近有分险但暂无危险考虑到归为集合{j2}，对于骑轧车道线行驶车辆以及逼近行人距离过近，归为集合{j3}，三种集合的车辆由于距离不同因而对车队产生的威胁也不同。

Step4:车队中智能车i风险系数μ_i计算

考虑到由半径R_caution与R_danger大小划分的三类集合中的物体对该智能车的风险不同，为了区分处在不同集合内的物体对于车队中智能车的不同影响，推导出车队中每一辆智能车i的风险系数μ_i：

n为处在对应集合中检测到的障碍物数目，对于在R_caution外侧集合{j1}中的物体对车队构成的风险相对较小，暂不列入式中。逐渐贴近的旁车位于智能车i的集合{j2}中时，此时风险系数μ_i随两车之间的距离成一次反比增长，直至进入集合{j3}，则判定该车已经贴近车队且将对车队产生较大的危险，此时风险系数μ_i以不同于{j2}的反比平方速度飞速增长。

Step5:由风险系数μ_i确定二级领导者

根据比较此刻车队中每一辆车风险系数μ_i，由μ_i的最大值确定车队中产生的二级领导者：

secleader＝μ_iimax

Step6:建立二级领导者的应急避障控制

本发明所研究的智能车队为网联式，且能够在道路上通过组建的无线传感器网络进行车辆之间位置信息、速度以及加速度信息的交换，使得二级领导者能够及时获得危险车的位置信息、速度以及加速度等信息后及时进行计算分析并及时进行避让，图3为车队路口旁车及行人逼近示意图,如图5所示为转弯通过路口时二级领导者产生避让行为示意图。现基于人工势场法引入风险系数μ_i进一步考虑关于二级领导者的控制输入。

假设车队中每一辆车都处在一个势能场中，势场中的障碍物(如转弯时旁车与行人)均被斥力包围，行驶过程中障碍物对每一辆车呈现斥力，且斥力大小障碍物距离智能体的距离减小而不断增大，方向背离障碍物。势场中的目标对于智能体呈现吸引力作用，且吸引力的大小随二者之间距离的减小而减小，方向指向目标点。将势场中所有的斥力势场力和吸引势场力作和得势函数，取势函数梯度下降的方向来实现无路径规划，车队中智能车i引力场和势力场分别定义为：

Q_att＝0.5ξ₁||X_i-X_des||²

其中Q_att为吸引力场，Q_rej为斥力场，||X_i-X_des||为车X_i此刻与目标点X_des之间的距离，ξ₁,ξ₂均为定值，ξ₃为产生排斥力场的阈值，是一略小于R_caution的定值。当||X_i-X_des||≤ξ₃时无法产生斥力作用，当||X_i-X_des||≤ξ₃时产生斥力作用且排斥力随着两者之间距离减小而不断增大。

取势力场之和的梯度下降方向作为控制输入：

其中U_in为该智能车的控制输入。

考虑到基础人工势场法只采用比例环节作为输入，控制很不稳定，因此在其基础上引入风险系数μ_i建立基于相对速度和闯入距离的斥力势函数：

其中j＝1,2...n,ω₁，ω₂为相对速度和闯入距离之间的权重大小，V_i,V_j为第i辆车队中的智能车与第j个障碍物的绝对速度大小，|V_i-V_j|为二者的相对速度大小。引入风险系数后斥力势场函数能够同时考虑闯入风险区后距离二级领导者的距离大小与闯入时的相对速度大小，闯入障碍物的速度越大、距离二级领导者越近，则表现的斥力越大，取梯度下降的方向得到二级领导者的斥力控制输入，闯入障碍物与二级领导者斥力控制输入几何关系示意图请见图6，其中：

其中,K^h为斥力势场增益，U_rej为速度斥力输入；V_ij表示二级领导者i与物体j之间的相对速度(当障碍物是静态障碍物时，对静态障碍物动态化,赋予目标的速度)；θ表示智能体相对于障碍物的位置矢量与相对速度矢量的夹角，用来判断障碍物是否向二级领导者移动；ω₁，ω₂为大于零常量且ω₁+ω₂＝1，x_ij表示二级领导者i相对于物体j的水平位移，y_ij示二级领导者i相对于障碍物的竖直位移，ψ为V_ij与X轴的夹角。式中，当θ为

时,表示二级领导者是向靠近二级领导者的方向移动的，相应的当相对速度越大的时候，斥力越大；速度越小，斥力越小。式中θ角是是判断该物体运动方向及其未来位置是否对二级领导者有影响的关键因素，判断了物体是否是朝着车队运动，也就可以借此提前判断该物体能否对车队构成潜在的威胁。

Step7:判断二级领导者是否成功完成避让

二级领导者成功避让条件：

k为观测次数，ts为两次相隔的采样时间，X^t+ts _i(k)为经过t+ts采样时间后车队中第i辆智能车第k观测所在的位置，X^t+ts _j(k)为经过t+ts采样时间后车队中第j个障碍物第k观测所在的位置，R_sence为智能车感知半径。并规定连续观测次数k达到10次，存在于集合{j₁}∪{j₂}中的物体能够进入到集合{j₃}中则不在被判定为风险障碍物，视作二级领导者避让成功。

Step8:建立二级领导者的领航控制

目前二级领导者已完成避障，现对二级领导者的领航控制引入基于人工势场法的PID反馈控制，加入目标点的轨迹跟踪预瞄误差反馈的两个特征量：预瞄点处的横向位置偏差以及跟随车航向与道路期望航向之间的航向偏差，用以提前预瞄道路的横向与纵向偏差。从而同时调节这两个偏差来获得二级领导者的前轮转角输入。从而对二级领导者转向盘进行航向角进行调整，完成反馈条件下的目标追踪，图4为目标追踪时后车轨迹跟踪示意图。

在人工势场中增加预瞄信息的横向位置偏差，引力势场函数模型建立如下：

式中，Y(s)为道路势场引力，K^v为人工势场增益，y_s为预瞄点处的横向偏差，l_s为预瞄距离，

为身纵轴和道路中心线切线的夹角，y_y为质心处的横向偏差

在二级领导者动态避让后，二级领导者将以某一速度时刻改变位置，为此引入了纵向速度以及预瞄时间根据车速调整预瞄距离，建立引力势场函数模型如下：

其中，K^v1、K^v2为势场增益系数，v为目标车速，h₂为速度增益系数。即可得到引力作用下前轮转角为：

其中，K₁为一常数。加入预瞄点的横向偏差可得：

Q_rec(s)＝0.5ξ₅L² ₍y_c,y_r)

其中，ξ₅为势力场增益系数,L_(yc,yr)为无人车距离道路的距离,则对于二级领导者的整体输入为斥力输出与引入输入之和：

转化为二级领导者避让后的前轮转角为：

至此，二级领导者已成功完成路口避障与目标追踪，完成领航控制。

Step9:建立二级领导者后车的避让跟随控制

针对后车跟随车控制，采用PID跟随控制，如图7所示。

其中e(t)为输入，u(t)为输出，K_p为比例因子，T₁，T₂为时间常数以及积分微分时间常数，下面对三个常数取值进行确定。

(1)为由于积分项会导致跟随车转向延迟，故在转弯时跟随车将积分项系数

置零。并调节比例项使车身能够在转弯行驶时无振荡且保持车身稳定。

(2)保持车身稳定跟随车舵机在转弯中需要较好的动态响应能力，微分系数K_d使用定值。

(3)在后车转弯跟随时主要控制其比例系数K_p，考虑到比例系数K_p在遇到急弯要增大，为提高跟随车的转弯灵敏性，基于比例系数的变化引入跟随车的风险度因子μ_i，定义：

其中K_p与μ_i为线性关系，α、β为一定值。对于车队过弯道时，从属车风险系数增大，特别是考虑到有障碍物闯入时风险系数将随障碍物距离车队的距离变化产生瞬间的增长，最终导致K_a增大，能提高转向的灵敏度。更有利于追踪二级领导者的避让轨迹并提前做出反应规避路口风险。而对于直线正常行驶，风险因子相对于弯道小，旁车骑道行驶与闯入概率要小很多，则相对得出的比例系数K_p也大幅下降，使其在直线行驶时减少转向的灵敏度，也更利于车队的队形保持和正常行驶。

Step10:车队通过路口并退级切换

当整个队伍通过路口后，旁车贴近与行人逼近均已成功避让且车队安全过弯后，风险系数逐步下降，此刻Leader_i<K_switch，队伍开始退级切换，由过弯时的Leader_i＝2逐步退至过弯前的Leader_i＝0，并进入平等交互模式下的直线行驶，至此回到初始车队直线行驶。

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技术所创的等效方式或变更均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，包括如下步骤：

步骤1，建立车队领导者数目切换原则；

步骤2，定义风险感知区与危险感应区范围；

步骤3，确定交通流车辆的集合划分；

步骤4，计算车队中智能车i的风险系数μ_i；

步骤5，由风险系数μ_i确定二级领导者；

步骤6，建立二级领导者的应急避障控制；

步骤7，判断二级领导者是否成功完成避让，如果完成避让，则执行步骤8；

步骤8，建立二级领导者的领航控制；

步骤9，建立二级领导者后车的避让跟随控制；

步骤10，车队通过路口并退级切换。

2.根据权利要求1所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤1具体如下：

设有N辆直线行驶的无领导者智能车编队，每辆车辆个体i视作一个质点，其动力学模型如下式所示：

其中p_i,q_i,u_i分别为编队车辆i的位置矢量,速度矢量，控制输入；其中||p_i||₂＜V_max，V_max＞0，V_max为编队中个体允许的最大的行驶速度，m_i为车队中第i辆车的质量；

定义智能车行驶时车队中某一辆车轨迹的曲率改变量Δρ_i，车队中领导者数目标识符Leader_i，车队发生领导者数目切换的Δρ_i阈值为K_switch，其中K_switch为一常数。车队为在通过路口前直线行驶，为平等交互模式，Leader_i＝0；当车队即将通过路口并开始转弯时切换为头车领导的单领导者层级交互模式，此时领导者数目标识符Leader_i＝1，当旁车贴近或行人逼近时车队中从属车产生避让行为并产生双领导者交互模式，此时Leader_i＝2；Leader_i与K_switch具有如下关系：

在层级交互模式下，切换为Leader_i＝2必须以Leader_i＝1的层级交互模式为前提，不可跨级切换，也不可越级退出。

3.根据权利要求2所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤2中风险感知区与危险感应区的具体定义如下：

在三种Leader_i模式的车队行驶的过程中，车队中每一辆无人车均存在以自身(x,y)为圆心，R_caution和R_denger为大小的风险感知区与危险感应区，其中：

L为车身长度，δ为环境影响因子，其值大小与天气恶劣程度和交通流量成正相关，晴朗交通正常情况下δ＝1。

4.根据权利要求1所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤3具体如下：

根据风险感知圈和危险感应圈将车队智能车i旁的物体j(j＝1,2...N)根据距离智能车i的远近分为三类集合：

其中，||X_i-X_j||为智能车i与物体j的欧式距离，车队内部车一般为无风险车，属集合{j1}，对于旁道压线行驶的车，因距离较近有分险但暂无危险考虑到归为集合{j2}，对于骑轧车道线行驶车辆以及逼近行人距离过近，归为集合{j3}。

5.根据权利要求4所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤4中风险系数μ_i的具体计算如下：

对于在R_caution外侧集合{j1}中的物体对车队构成的风险相对较小，暂不列入式中；逐渐贴近的旁车位于智能车i的集合{j2}中时，此时风险系数μ_i随两车之间的距离成一次反比增长，直至进入集合{j3}，则判定该车已经贴近车队且将对车队产生较大的危险，此时风险系数μ_i以不同于{j2}的反比平方速度飞速增长。

6.根据权利要求5所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤5具体如下：

比较此刻车队中每一辆车风险系数μ_i，由μ_i的最大值确定车队中产生的二级领导者：

secleader＝μimax。

7.根据权利要求1所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤6采用基于人工势场法建立二级领导者的应急避障控制，具体如下：

假设车队中每一辆车都处在一个势能场中，势场中的障碍物均被斥力包围，行驶过程中障碍物对每一辆车呈现斥力，且斥力大小障碍物距离智能体的距离减小而不断增大，方向背离障碍物；势场中的目标对于智能体呈现吸引力作用，且吸引力的大小随二者之间距离的减小而减小，方向指向目标点，将势场中所有的斥力势场力和吸引势场力作和得势函数，取势函数梯度下降的方向来实现无路径规划，车队中智能车i引力场和势力场分别定义为：

Q_att＝0.5ξ₁||X_i-X_des||²

其中Q_att为吸引力场，Q_rej为斥力场，||X_i-X_des||为车X_i此刻与目标点X_des之间的距离，ξ₁,ξ₂均为定值，ξ₃为产生排斥力场的阈值，是一略小于R_caution的定值。当||X_i-X_des||≤ζ₃时无法产生斥力作用，当||X_i-X_des||≤ζ₃时产生斥力作用且排斥力随着两者之间距离减小而不断增大；

取势力场之和的梯度下降方向作为控制输入：

其中U_in为该智能车的控制输入；

引入风险系数μ_i建立基于相对速度和闯入距离的斥力势函数：

其中j＝1,2...n,ω₁，ω₂为相对速度和闯入距离之间的权重大小，引入风险系数后斥力势场函数能够同时考虑闯入风险区后距离二级领导者的距离大小与闯入时的相对速度大小，闯入障碍物的速度越大、距离二级领导者越近，则表现的斥力越大，取梯度下降的方向得到二级领导者的斥力控制输入：

其中,K^h为斥力势场增益，U_rej为速度斥力输入；V_ij表示二级领导者i与物体j之间的相对速度(当障碍物是静态障碍物时，对静态障碍物动态化,赋予目标的速度)；θ表示智能体相对于障碍物的位置矢量与相对速度矢量的夹角，用来判断障碍物是否向二级领导者移动；ω₁，ω₂为大于零常量且ω₁+ω₂＝1，x_ij表示二级领导者i相对于物体j的水平位移，y_ij示二级领导者i相对于障碍物的竖直位移，ψ为V_ij与X轴的夹角；式中，当θ为

时，表示二级领导者是向靠近二级领导者的方向移动的，相应的当相对速度越大的时候，斥力越大；速度越小，斥力越小；式中θ角是是判断该物体运动方向及其未来位置是否对二级领导者有影响的关键因素，判断了物体是否是朝着车队运动，就能够提前判断该物体能否对车队构成潜在的威胁。

8.根据权利要求1所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤7中二级领导者成功完成避让的条件是：

k为观测次数，R_sence为智能车感知半径，ts为采样时间，并规定连续观测次数k达到10次，存在于集合{j₁}∪{j₂}中的物体能够进入到集合{j₃}中则不在被判定为风险障碍物，视作二级领导者避让成功。

9.根据权利要求1所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤8具体如下：

对二级领导者的领航控制引入基于人工势场法的PID反馈控制，加入目标点的轨迹跟踪预瞄误差反馈的两个特征量：预瞄点处的横向位置偏差以及跟随车航向与道路期望航向之间的航向偏差，用以提前预瞄道路的横向与纵向偏差；同时调节这两个偏差来获得二级领导者的前轮转角输入，对二级领导者转向盘进行航向角进行调整，完成反馈条件下的目标追踪；具体地：

在人工势场中增加预瞄信息的横向位置偏差，引力势场函数模型建立如下：

式中，Y(s)为道路势场引力，K^v为人工势场增益，y_s为质心处的横向偏差，l_s为预瞄点处的横向偏差；

在二级领导者动态避让后，二级领导者将以某一速度时刻改变位置，为此引入纵向速度以及预瞄时间根据车速调整预瞄距离，建立引力势场函数模型如下：

即可得到引力作用下前轮转角为：

加入预瞄点的横向偏差可得：

Q_rec(s)＝0.5ξ₅L²(y_c,y_r)

则对于二级领导者的整体输入为斥力输出与引入输入之和：

转化为二级领导者避让后的前轮转角为：

至此，二级领导者已成功完成路口避障与目标追踪，完成领航控制。

10.根据权利要求1所述的一种基于多级领导者鸽群理论的车队路口避障控制方法，其特征在于，所述步骤9具体如下：

针对后车跟随车控制，采用PID跟随控制：

其中e(t)为输入，u(t)为输出，K_p为比例因子，T₁，T₂为时间常数以及积分微分时间常数，下面对三个常数取值进行确定；

(1)为由于积分项会导致跟随车转向延迟，故在转弯时跟随车将积分项系数

置零，并调节比例项使车身能够在转弯行驶时无振荡且保持车身稳定；

(2)保持车身稳定跟随车舵机在转弯中需要较好的动态响应能力，微分系数K_d使用定值；

(3)在后车转弯跟随时主要控制其比例系数K_p，考虑到比例系数K_p在遇到急弯要增大，为提高跟随车的转弯灵敏性，基于比例系数的变化引入跟随车的风险度因子μ_i，定义：

其中K_p与μ_i为线性关系，α、β为定值。

Images