基于多目标优化的车辆编队仿真研究

Research on Vehicle Formation Simulation Based on Multi Objective Optimization

摘要: 随着车联网技术的发展，出现了车路协同、多车协同等多项汽车智能化技术。智能网联汽车编队在城市道路场景下有着良好的应用前景，在城市道路场景下，路况复杂，对车辆编队的灵活性提出了很高的要求。为了提高在城市街道场景下车辆编队的灵活性，提出了一种分散运动控制方法。该方法实现了编队当中每一个车辆的轨迹的优化和车间距的控制。首先，提出了一种车辆轨迹的生成方法，这种方法将车辆的横向轨迹和纵向轨迹解耦。同时，采用五次多项式生成轨迹族，并使用多目标优化选择最优轨迹。其次，针对车间距和车速的控制，设计了PD控制器，使得编队当中的车间距和车速能够达到一致。最后，通过Matlab仿真验证了分散式运动规划方法在不同场景下的优越性。结果表明，该方法提高了编队的灵活性和一致性。

Abstract: With the development of vehicle networking technology, multiple intelligent technologies for automobiles have emerged, such as vehicle road collaboration and multi vehicle collaboration. Intelligent connected vehicle formations have good application prospects in urban road scenarios, where complex road conditions require high flexibility for vehicle formations. In order to improve the flexibility of vehicle formation in urban street scenes, a decentralized motion control method is proposed. This method optimizes the trajectory of each vehicle in the formation and controls the distance between vehicles. Firstly, a method for generating vehicle trajectories is proposed, which decouples the lateral and longitudinal trajectories of the vehicle. Meanwhile, a fifth degree polynomial is used to generate trajectory families, and multi-objective optimization is employed to select the optimal trajectory. Secondly, a PD controller was designed to control the distance and speed between vehicles in the formation, ensuring consistency between the distance and speed. Finally, the superiority of the decentralized motion planning method in different scenarios was verified through Matlab simulation. The results indicate that this method improves the flexibility and efficiency of the formation.

1. 引言

近年来的研究表明，对车辆进行编队有利于提高城市街道交通流的稳定性，缓解交通拥堵，降低能耗[1]。智能车队的研究始于上世纪八十年代，美国加州大学的PATH项目，该项目证明了车队在提高驾驶安全和交通效率方面的应用潜力。从那时起，世界各国都部署了智能车队领域。著名的案例包括日本的能源智能交通系统项目，欧洲SARTRE项目等。

关于如何控制和利用好车队，众多学者进行了许多研究。孔慧芳等人[2]提出了一种V2X环境下的网联车辆编队控制策略，通过优化控制区内编队速度，并采用分布式模型预测控制理论，将速度跟随控制问题转化为易于求解的凸二次规划问题。仿真结果表明，该策略能有效避免车辆碰撞并减少编队车辆在信号灯路口的通行时间，从而提升通行效率。熊奔等人[3]提出了一种针对执行器故障的异构车辆编队分布式容错控制方案，通过反馈线性化和自适应控制协议，确保在故障情况下编队仍能保持稳定间距和期望速度。王树凤等人[4]改进了智能驾驶员模型(IDM)和优化速度模型(OVM)，通过引入驾驶员误差缓解速度波动。仿真结果表明，改进后的模型在速度响应时间、跟驰间距、油耗、安全性和稳定性方面表现更佳。张意豪等人[5]提出了一种固定时间控制策略，通过领航者–跟随者模式和固定时间理论设计的控制器，确保车辆编队误差在固定时间内收敛，并形成期望阵型。蒋金[6]提出了一种结合模型预测控制(MPC)和同步定位与地图构建(SLAM)的耦合控制策略。与传统方法相比，该策略显著改善了编队的稳定性和可靠性，特别是在复杂环境和高速条件下，实验结果表明其跟随效果优于其他控制方法。巩丹阳等人[7]提出了一个改进的卡车–小汽车混合流跟驰模型，利用可变时距和多前车信息，提升了混合交通流的稳定性和通行能力。郭晓刚等人[8]提出了一种基于目标点的虚拟道路车辆编队算法，通过插值计算和安全距离考虑，实现了车辆在直线和弯道上的有效编队，并通过仿真实验验证了其可行性。刘济铮等人[9]提出了一种面向异构智能网联汽车的延迟补偿控制方法，利用加速度信息实现纵向跟踪控制，并通过Smith预测器减少执行器和通讯延迟对编队稳定性的影响，使跟车误差降低80.7%。江浩斌等人[10]基于鱼群集群运动理论，采用滑模控制器和领航跟随法实现智能车辆在高速公路上的安全有序编队，仿真结果显示车辆能够从随机位置聚拢并有序跟随领航车。

俞志英等人[11]提出了一种基于车辆运动学模型的直接速度控制方法，通过最少通信量实现多车辆协同路径跟踪，仿真结果验证了其有效性。

目前的研究中，主要集中在对车辆编队纵向控制的研究，忽视了对横向控制的研究，这会导致车辆编队面对突发状况时，只能被迫减速或者停止。如图1所示，车辆编队的队形固定，很影响交通效率，适应差。

Figure 1. Current situation of vehicle formation

图1. 车辆编队现状

本研究中每一辆编队成员都可以进行运动轨迹的规划，这样做可以极大地提高车辆编队的灵活性，能够实现车辆编队队形的自适应调整。在驶过事故发生区域后，每一个车辆再可以再重新组合为一个车辆编队，图2为编队调整过程。图3为编队调整结果。

Figure 2. Formation adjustment process

图2. 编队调整过程

Figure 3. Formation adjustment results

图3. 编队调整结果

每一辆车的路径规划步骤如下：利用Frenet坐标系，将横向控制与纵向控制解耦，然后在参考线上进行横纵向采样，再把采样点用多项式曲线连接起来，最后经过轨迹评估，碰撞检测，轨迹合并等步骤，形成最终的规划轨迹。

2. 单个车辆运动学建模

2.1. 模型建立

采用自行车模型的好处是它简化了无人车前轮转向角与曲率之间的几何关系。汽车运动学模型如图4所示。其中，c是前后轮中心点连线上的一个参考点，一般取车辆的质心位置。v是汽车速度。其中，质心速度矢量与车身之间的角度为质心侧角β，车辆与x轴之间的角度是车辆横摆角θ。δ为汽车的前轮转角，$l_f$ 为质心与轮中心距离，$l_r$ 为质心与后轮中心距离。

Figure 4. Automotive kinematic model

图4. 汽车运动学模型

2.2. 模型推导

将速度分解为横向和纵向：

$\dot{x}=v\cos \left(\theta +\beta \right)$ (1)

$\dot{y}=v\sin \left(\theta +\beta \right)$ (2)

角加速度：

$\omega =\dot{\theta }=\frac{v}{R}$ (3)

有两个未知参数，即β和$1/R$ ，根据图中角度关系和三角形几何关系有：

$\tan \left(\delta \right)=\frac{l_r+l_c}{\overline{R}}$ (4)

$\cos \left(\beta \right)=\frac{\overline{R}}{R}$ (5)

$\tan \left(\beta \right)=\frac{l_r}{\overline{R}}$ (6)

由上面三个方程可以解出：

$\omega =\dot{\theta }=\frac{v}{R}=\frac{v}{l_r+l_f}\cos \left(\beta \right)\tan \left(\delta \right)$ (7)

$\beta =\arctan \left(\frac{l_r}{l_f+l_r}\tan \left(\delta \right)\right)$ (8)

由此可以得到模型的状态转移方程：

${\beta }_t=\arctan \left(\frac{l_r}{l_f+l_r}\tan \left({\delta }_t\right)\right)$ (9)

$x\left(t+1\right)=x\left(t\right)+v_t\cos \left(\theta \left(t\right)+{\beta }_t\right)\cdot dt$ (10)

$y\left(t+1\right)=y\left(t\right)+v_t\sin \left(\theta \left(t\right)+{\beta }_t\right)\cdot dt$ (11)

$\theta \left(t+1\right)=\theta \left(t\right)+\frac{v}{l_r+l_f}\cos \left({\beta }_t\right)\tan \left({\delta }_t\right)\cdot dt$ (12)

输入是当前时刻的速度，前轮转向角以及汽车位姿(横纵坐标和汽车航向角)，输出为下一时刻位姿。

3. 道路参考线

道路参考线贯穿了整个的决策规划过程，是轨迹规划中最重要的结构之一，在路径规划中，参考线是路径生成的重要依据，当车辆沿着车道线行驶时，所生成的路径和参考线几乎重合。道路参考线由地图读取，由一组路点几何组成：$\left(x_1,y_1,{\theta }_1\right),\left(x_2,y_2,{\theta }_2\right),\cdots ,\left(x_N,y_N,{\theta }_N\right)$ 。

由于各个路点是离散的，直接连接起来的话不够平滑，因此需要对参考线进行平滑。

3.1. 生成回旋线

Figure 5. Cyclotron curve

图5. 回旋曲线

回旋曲线是一种曲率与长度成线性关系的曲线，它的优点是曲率平滑过渡，本文选择回旋线对道路参考线进行平滑。回旋线的公式如下：

$\dot{\kappa }=as$ (13)

对上式进行积分得到直角坐标系下的参数方程：

$x=\frac{1}{a}{\displaystyle {\int }_0^{\tau }\cos }\left(t^2\right)\text{d}t$ (14)

$y=\frac{1}{a}{\displaystyle {\int }_0^{\tau }\sin }\left(t^2\right)\text{d}t$ (15)

上面方程是一个超越方程，没有精确解。想要得到回旋曲线上的点，绘制出回旋线就需要通过数值积分来近似计算。本文使用了Matlab中的菲涅尔积分来计算，绘制出的回旋曲线如图5所示。绘制出的回旋曲线的曲率如图6所示：

Figure 6. Curvature of cyclotron curve

图6. 回旋曲线的曲率

从图6中可以看出回旋曲线的曲率过度十分的平滑，适合对离散的路点进行平滑。

3.2. 回旋线G1拟合

想要用回旋线来连接路点，就要用到G1拟合。G1拟合的目的就是用分段回旋曲线来连接各个点，让整个曲线能够满足G1连续性(G1连续：相切连续，航向角连续)。图7为G1拟合后的道路参考线，图8为G1拟合后的航向角和曲率。

Figure 7. G1 fitted road reference line

图7. G1拟合后的道路参考线

Figure 8. Heading angle and curvature after fitting G1

图8. G1拟合后的航向角和曲率

从图7和图8中可以看出G1拟合后的道路参考线不光滑，并不满足G2连续，这是因为G1拟合的时候除了提供路点坐标以外，还要提供路点的航向角才能让参考路线光滑，路点信息可以由地图读取，而航向角可以由G2算法根据路点信息计算得到。

3.3. 回旋线G2拟合

回旋线G2拟合的过程是求出来这一组特定的航向角作为路点数据，从而让G1拟合出来的分段回旋曲线达到G2连续(G2连续：曲率连续)。有了G2算法提供的航向角后，回旋线G1拟合的道路参考线如图9所示，航向角和曲率如图10所示。

Figure 9. Road reference line

图9. 道路参考线

Figure 10. Heading angle and curvature

图10. 航向角和曲率

由图9和图10可以看出，在利用G2算法计算出给定的航向角数据后，经过G1拟合得到的分段回旋曲线不仅满足G1连续，还满足G2连续。

4. Frenet坐标系

车辆在道路上行驶的时候，需要关心的状态量：

1) 车沿着道路开了多远；

2) 偏离中心线多远。

这2个状态量无法通过笛卡尔坐标直观看出。为了解决这一问题，引入frenet坐标系。在Frenet坐标系中：S表示沿着参考线的里程，L表示偏离参考线的横向距离。

笛卡尔坐标系到Frenet坐标系的转换

只要知道道路参考线，就能够将笛卡尔坐标系转换到frenet坐标系。其中Frenet状态量为$\left(s,\dot{s},\ddot{s},l,\dot{l},\ddot{l}\right)$ ，笛卡尔状态量为$\left(x,y,\theta ,v_x,a_x,C{r}_k\right)$ 。

转换公式如下所示：

$\{\begin{array}{l}s=s_r\hfill \\ \dot{s}=\frac{v_x\cos \left({\theta }_x-{\theta }_r\right)}{1-k_{r}l}\hfill \\ \ddot{s}=\frac{a_x\cos \left({\theta }_x-{\theta }_r\right)-{\dot{s}}^2\left[l^{\prime }\left(k_x\frac{1-k_{r}l}{\cos \left({\theta }_x-{\theta }_r\right)}-k_r\right)-\left(k_r^{\prime }l+k_r{l}^{\prime }\right)\right]}{1-k_{r}l}\hfill \\ l=\mathrm{sign}\left(\left(x_x-x_r\right)\cos \left({\theta }_r\right)-\left(y_x-y_r\right)\sin \left({\theta }_r\right)\right)\sqrt{{\left(x_x-x_r\right)}^2+{\left(y_x-y_r\right)}^2}\hfill \\ l^{\prime }=\left(1-k_{r}l\right)\tan \left({\theta }_x-{\theta }_r\right)\hfill \\ l^{″}=-\left({k^{\prime }}_{r}l+k_r{l}^{\prime }\right)\tan \left({\theta }_x-{\theta }_r\right)+\frac{1-k_{r}l}{{\cos }^2\left({\theta }_x-{\theta }_r\right)}\left(\frac{1-k_{r}l}{\cos \left({\theta }_x-{\theta }_r\right)}{k}_x-k_r\right)\hfill \end{array}$ (16)

5. 多项式拟合

车辆在道路上行驶需要沿着平滑的轨迹才能保证行驶平稳，本文用加速度的变化即jerk来表示轨迹的平滑。

设p是车辆的位置，那么加速度的变化为$\stackrel{⃛}{p}$ ，那么一段轨迹的总jerk可以用积分来计算：

$J_t\left(p\left(t\right)\right)={\displaystyle {\int }_{t_0}^{t_1}{\stackrel{⃛}{p}}^2}\left(\tau \right)\text{d}\tau$ (17)

要使得jerk最小，根据最小作用量原理：

$C=\frac{1}{2}{\displaystyle {\int }_0^{T}L}dt$ (18)

$L={\left(\frac{{\text{d}}^{3}x}{\text{d}{t}^3}\right)}^2+{\left(\frac{{\text{d}}^{3}y}{\text{d}{t}^3}\right)}^2$ (19)

要求C的最小值，那么L需要满足：

$\frac{\partial L}{\partial x}-\frac{\text{d}}{\text{d}t}\left(\frac{\partial L}{\partial \dot{x}}\right)\cdots +{\left(-1\right)}^n\frac{{\text{d}}^n}{\text{d}{t}^n}\frac{\partial L}{\partial \left(x^{(n)}\right)}=0$ (20)

$\frac{\partial L}{\partial y}-\frac{\text{d}}{\text{d}t}\left(\frac{\partial L}{\partial \dot{y}}\right)\cdots +{\left(-1\right)}^n\frac{{\text{d}}^n}{\text{d}{t}^n}\frac{\partial L}{\partial \left(y^{(n)}\right)}=0$ (21)

由上式可以得到：

$\frac{{\text{d}}^3}{\text{d}{t}^3}\frac{\partial \left({\ddot{x}}^2\right)}{\partial (\ddot{x})}=0$ (22)

$\frac{{\text{d}}^3}{\text{d}{t}^3}\frac{\partial \left({\ddot{y}}^2\right)}{\partial (\ddot{y})}=0$ (23)

$\frac{{\text{d}}^{6}x}{\text{d}{t}^6}=0$ (24)

$\frac{{\text{d}}^{6}y}{\text{d}{t}^6}=0$ (25)

可以推导出五次多项式是其通解：

$x\left(t\right)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$ (26)

$y\left(t\right)=b_0+b_{1}t+b_2{t}^2+b_3{t}^3+b_4{t}^4+b_5{t}^5$ (27)

在Frenet坐标系中，可以很容易获得当前的Frenet状态量，同时根据环境状况也容易得出目标的Frenet状态(跟车，换道，借道绕行等)，但是如何从当前状态连接到目标的Frenet状态是个问题，本文采用用五次多项式从当前状态规划一段轨迹连接到目标状态。在本文中采用的五次多项式如下：

$s\left(t\right)=M_{s,5}\cdot t^5+M_{s,4}\cdot t^4+M_{s,3}\cdot t^3+M_{s,2}\cdot t^2+M_{s,1}\cdot t+M_{s,0}$ (28)

$d\left(t\right)=M_{d,5}\cdot t^3+M_{d,4}\cdot t^4+M_{d,3}\cdot t^3+M_{d,2}\cdot t^2+M_{d,1}\cdot t+M_{d,0}$ (29)

在等式28中，$M_{s,1},M_{s,2},M_{s,3},M_{s,4},M_{s,5}$ 是纵向轨迹多项式系数。同时，在等式29中 $M_{d,1},M_{d,2},M_{d,3},M_{d,4},M_{d,5}$ 是横向轨迹多项式系数。

构造起点的横行约束如下：

$\{\begin{array}{l}d\left(0\right)=M_{d,0}=d_0\hfill \\ \dot{d}\left(0\right)=M_{d,1}={\dot{d}}_0\hfill \\ \ddot{d}\left(0\right)=2\cdot M_{d,2}={\ddot{d}}_0\hfill \end{array}$ (30)

同样地，起点的纵向约束如方程式：

$\{\begin{array}{l}s\left(0\right)=M_{s,0}=s_0\hfill \\ \dot{s}\left(0\right)=M_{s,1}=v_0\hfill \\ \ddot{s}\left(0\right)=2\cdot M_{s,2}=a_0\hfill \end{array}$ (31)

在等式30和31中，$s_0,v_0,a_0$ 分别是起动的纵向位移、纵向速度和纵向加速度。车辆在道路上以固定的时间间隔$t_{\text{int }}$ 均匀采样，采样后的离散时域为$t_k\in [t_{\min },t_{\min }+t_{\text{int}}\cdots ,t_{\max }]$ ，在横向上以固定距离间隔$d_{\text{int }}$ 进行均匀采样，采样后得到的离散域如下所示$d_k\in [d_{\min },d_{\min }+d_{\text{int}}\cdots ,d_{\max }]$ ，在纵向上以固定的时间间隔$S_{\text{int }}$ 进行均匀采样，采样后得到的离散域如下所示$s_k\in [s_{\min },s_{\min }+s_{\text{int}}\cdots ,s_{\max }]$ ，此外，当$t_k$ 被确定时，$S_k$ 和$d_k$ 被唯一地确定。终点横向的约束如等式32所示。

$\{\begin{array}{l}d\left(t_k\right)=M_{d,5}\cdot t_k^5+M_{d,4}\cdot t_k^4+M_{d,3}\cdot t_k^3+M_{d,2}\cdot t_k^2+M_{d,1}\cdot t_k+M_{d,0}\hfill \\ \dot{d}\left(t_k\right)=5M_{d,5}\cdot t_k^4+4M_{d,4}\cdot t_k^3+3M_{d,3}\cdot t_k^2+2M_{d,2}\cdot t_k+M_{d,1}\hfill \\ \ddot{d}\left(t_k\right)=20M_{d,5}\cdot t_k^3+12M_{d,4}\cdot t_k^2+6M_{d,3}\cdot t_k+2M_{d,2}\hfill \end{array}$ (32)

终点纵向的约束如等式33所示。

$\{\begin{array}{l}s\left(t_k\right)=M_{s,5}\cdot t_k^5+M_{s,4}\cdot t_k^4+M_{s,3}\cdot t_k^3+M_{s,2}\cdot t_k^2+M_{s,1}\cdot t_k+M_{s,0}=s_m\hfill \\ \dot{s}\left(t_k\right)=5M_{s,5}\cdot t_k^4+4M_{s,4}\cdot t_k^3+3M_{s,3}\cdot t_k^2+2M_{s,2}\cdot t_k+M_{s,1}=v_m\hfill \\ \ddot{s}\left(t_k\right)=20M_{s,5}\cdot t_k^3+12M_{s,4}\cdot t_k^2+6M_{s,3}\cdot t_k+2M_{s,2}=a_m\hfill \end{array}$ (33)

最后，结合方程30和32得到横向轨迹多项式系数。同时，结合方程31和33得到纵向轨迹多项式系数。此外，根据$t_k$ 得到横向和纵向轨迹。对所有离散点的系数解进行积分，得到轨迹簇。在轨迹簇中，总共有$tr{j}_{\text{num }}=A\cdot B\cdot C$ 条轨迹。

$A=\left(t_{\max }-t_{\min }\right)/t_{\text{int}}$ (34)

$B=\left(s_{\max }-s_{\text{min }}\right)/s_{\text{int }}$ (35)

$C=\left(d_{\max }-d_{\text{min }}\right)/d_{\text{int }}$ (36)

其中$S_{\text{int }}$ 和$d_{\text{int }}$ 是经验值。本文使用$t_{\text{int}}=0.2\text{ s}$ ，$S_{\text{int }}=10\text{ m}$ 和$d_{\text{int }}=0.6\text{ m}$ 。综上所述，引导轨迹簇统一表示为以下形式：

$H=\left\{{\left(t_k,s_k,d_k,x_k,y_k,{\theta }_k,C{r}_k,\text{ Coef }\right)}_{trj}|k=1,2,\cdots ,{\text{point}}_{\text{num}},trj=1,2,\cdots ,tr{j}_{\text{num }}\right\}$ (37)

利用Frenet坐标系与笛卡尔坐标系之间的转换关系，可以从Frenet坐标系中的每个离散点$(s,\dot{s},\ddot{s},l,\dot{l},\ddot{l})$ 计算出相应的笛卡尔坐标系位置$\left(x,y,\theta ,v_x,a_x,\kappa \right)$ 。此外，${\text{point}}_{\text{num }}=t_k/T_{\text{ctrl }}$ 是轨迹的离散点的数目，其中$T_{\text{ctrl }}$ 是控制周期。Coef是横向和纵向轨迹的多项式系数，H而是生成的轨迹族。

在Matlab中生成的轨迹族如图11所示。

Figure 11. Generated Trajectory Family

图11. 生成的轨迹族

6. 最优轨迹的选择

在生成轨迹族后，需要对轨迹进行筛选。本文采用汽车运动约束、位移、快速性等指标来消除不合格轨迹。

6.1. 校验约束

轨迹筛选在轨迹点生成之后，轨迹筛选之前需要校验每一条轨迹是否满足约束，不满足约束则标记为无效。每条轨迹需要校验以下约束：

$\{\begin{array}{l}0<{\dot{s}}_k<v_{\max }\hfill \\ a_{\min }\le {\ddot{s}}_k\le a_{\max }\hfill \\ \text{abs}\left(C{r}_k\right)\le C{r}_k{}_{\max }\hfill \end{array}$ (38)

由方程式35，${\dot{s}}_k$ 和${\ddot{s}}_k$ 分别代表客车的纵向速度和纵向加速度。最大允许速度为$v_{\max }$ ，而最大纵向加速度为$a_{\max }$ 。$a_{\min }$ 表示最小纵向加速度，$C{r}_k{}_{\max }$ 表示最大曲率。

6.2. 基于多目标评价的轨迹选择

本文考虑了舒适度、位移、速度和是否与领航车在同一车道。因此，成本函数Cost的设计如下：

$\text{Cost}={\omega }_1{\mathrm{cost}}_{\text{Lon}}+{\omega }_2{\mathrm{cost}}_{\text{Lat}}+{\omega }_3{\mathrm{cost}}_{\text{Ld}}+{\omega }_4{\mathrm{cost}}_{\text{Pt}}+{\omega }_5\cos t_{\text{Lane }}$ (39)

在方程式36中，ω₁、ω₂、ω₃、ω₄、ω₅、ω₆为加权因子。基于等式36选择Cost值最小的轨迹作为最优路径轨迹。

1) 纵向舒适度

纵向舒适度是由纵向急动度来表示。纵向急动度越小，意味着加速度和减速度越小，这导致了更高的纵向舒适性。因此，采用纵向急动度的平均值来评价车辆的纵向舒适性。

${\text{cost}}_{\text{Lon }}=\left({\displaystyle \sum _{ls=1}^{{\text{point}}_{\text{num}}}\left|{\text{jerk}}_k\right|}\right)/{\text{point}}_{\text{num }}$ (40)

其中${\text{jerk}}_k$ 是轨迹中第K个轨迹点的加加速度。

2) 侧向舒适度

与纵向舒适性类似，横向加速度的平均值被用作横向舒适性的评价。

${\text{cost}}_{\text{Lon }}=\left({\displaystyle \sum _{ls=1}^{{\text{point}}_{\text{num}}}\left|{\text{jerk}}_k\right|}\right)/{\text{point}}_{\text{num }}$ (41)

3) 横向位移偏差

横向位移偏差是指目标和道路参考线之间的横向位移之间的偏差。

${\text{cost}}_{\text{Ld}}=\left|d_{\text{pointnum}}\right|$ (42)

4) 规划时间

实际规划时间和最短规划时间之间的差值是用于衡量横向和纵向规划的速度。

${\text{COST}}_{\text{Pt}}=t_{\text{pointnum}}-t_{\min }$ (43)

在等式39中，$t_{\text{pointnum}}$ 是实际的计划最优轨迹的时间。

5) 是否与领航车在同一车道

用来衡量是否与位置靠前的编队车辆在同一个车道。

${\text{COST}}_{\text{Lane }}=\text{basecost}\times \left(1-\text{lanematch}\right)$ (44)

7. 碰撞检测

碰撞检测通过动态胶囊来实现。胶囊作为车辆或者障碍物的碰撞边界的好处是计算速度快，计算2个胶囊是否碰撞只需要计算2条线段的距离即可。

7.1. 胶囊边界

车辆边界用一个胶囊来包围，胶囊的几何形状通过长度、半径和长度方向的偏移来定义。胶囊中的线段通过起点坐标，方向，长度来定义。这样，在知道车辆的位姿以后，就能够确定胶囊中线段，并且胶囊方向和车辆方向一致。车辆位姿和胶囊——对应。胶囊如图12所示。

Figure 12. Capsule boundary

图12. 胶囊边界

7.2. 胶囊碰撞分配

一个轨迹点的边界用1个胶囊来表示，那么一段轨迹的边界就用一个胶囊集合来表示(轨迹点之间时间间隔相同)。那么检测2段轨迹是否碰撞，只需要检测2个胶囊集合是否会碰撞。它的步骤是，首先分别计算出编队车辆轨迹的胶囊集合和其余车轨迹的胶囊集合，将胶囊两两之间进行分配，然后再循环寻找2个胶囊的最短距离。和2个胶囊的半径和进行比较，就能判定2个胶囊是否碰撞了。车辆编队开始列队如图13所示。编队车辆分散避让如图14所示。

Figure 13. Starting to line up for driving

图13. 开始列队行驶

Figure 14. Dispersed avoidance of formation vehicles

图14. 编队车辆分散避让

8. 一致性控制

Figure 15. Speed Consistency

图15. 速度一致性

Figure 16. Consistency of vehicle spacing

图16. 车间距一致性

一致性是指，在车队中，车队成员的速度、车间距能够保持一致，并且能够达到期望的速度与车间距。一致性控制在车队中至关重要，因为它能有效减少追尾事故和提高车队的整体流动性。精确的间距控制帮助车辆在变道或调整速度时保持稳定，防止出现局部阻塞问题，并优化交通流量。本文针对编队当中，车辆与车辆之间的间距控制采用PD控制器。

设计PD控制器的目的是使车辆的速度与车间距达到期望的速度与车间距。设置PD控制器的目标值是期望速度$v_d=20\text{ m/s}$ ，期望间距$D_d=10\text{ m}$ 。反馈值为当前速度与期望速度的和当前车间距和期望车间距的差值，计算出的输出为汽车的驱动力。再根据汽车驱动力计算出车辆的加速度从而对车辆编队的车间距和车速进行控制，以达到期望车速和期望的车间距。控制器设计如下：

$Ft=k_P\left(v_k-v_d\right)+k_d\left(D-D_d\right)$ (45)

在Matlab中进行仿真实验，速度一致性结果如图15所示，车间距一致性如图16所示。由图15可以看出车辆速度逐渐收敛到期望速度20 m/s。由图16可以看出车间距逐渐收敛到10 m。仿真实验证明了PD控制器的有效性。

9. 结论

为提升城市街道场景下车辆编队的灵活性，本文提出了一种分散运动控制方法。这种方法优化了编队中每辆车的轨迹，并有效地控制了车间距。具体而言，本文首先提出了一种车辆轨迹生成方法，该方法通过解耦车辆的横向和纵向轨迹，并利用五次多项式生成轨迹族，通过多目标优化选择最优轨迹。选择出的最优轨迹可以保证与场景中其余车辆的轨迹不发生碰撞。其次，为了控制车间距和车速，本文设计了PD控制器，以确保编队中车辆的车间距和车速能够保持一致。设计的PD控制器可以将编队车辆的速度收敛到期望速度，可以将编队当中的车间距收敛到期望的车间距。最后，通过Matlab仿真验证了分散式运动规划方法在城市街道场景下的优越性。仿真结果表明，该方法提高了编队的灵活性和一致性。