基于自适应采样的智能车辆轨迹规划方法

Trajectory planning for intelligent vehicles based on adaptive sampling

摘要

针对在三维空间（X-Y-T）进行智能车辆轨迹规划时计算复杂度高、耗时长的问题，本文提出了一种基于自适应采样的双层三阶段轨迹规划方法，基于Frenet坐标系把三维轨迹规划问题分解为路径规划和速度规划2个二维优化问题。首先，在路径规划方面，引入基于人工势场的自适应采样方法，以减少路径规划空间采样点数量，采用动态规划计算连接各采样点综合代价最低的路径曲线，构建二次规划问题进一步优化此路径，得到最终规划路径。其次，在速度规划方面，根据运动学约束确定速度规划空间自适应采样区域，使用动态规划方法计算连接各采样点综合代价最低的速度曲线，并依据此曲线构建二次规划问题，求解得到最终速度规划结果。最后，构建轨迹跟踪误差模型，设计横、纵向控制器对规划轨迹可跟踪性进行验证。仿真结果表明，本文所提方法能够将单次规划耗时降低至0.1 s左右，为智能车辆提供了更新频率为10 Hz的规划轨迹，实现了规划与控制的协同。

Abstract

To address the issue of high computational complexity and excessive time consumption of intelligent vehicle trajectory planning in three-dimensional space （X-Y-T）， this paper proposes a dual-layer， three-stage trajectory planning method based on adaptive sampling. By using the Frenet coordinate system， the three-dimensional trajectory planning problem is decomposed into two two-dimensional optimization problems： path planning and speed planning. Firstly， in terms of path planning， an adaptive sampling method based on artificial potential fields is introduced to reduce the number of path planning space sampling points. Dynamic programming is used to calculate the path curve that connects each sampling point with the lowest comprehensive cost， and a quadratic programming problem is constructed to further optimize this path to obtain the final planned path. Secondly，in terms of speed planning， the adaptive sampling area for speed planning space is determined according to kinematic constraints. The dynamic programming method is used to calculate the speed curve that connects each sampling point with the lowest comprehensive cost， and a quadratic programming problem is constructed based on this curve to obtain the final speed planning result. Finally， a trajectory tracking error model is constructed， and lateral and longitudinal controllers are designed to verify the trackability of the planned trajectory. Simulation results show that the proposed method in this paper can reduce the planning time to about 0.1 seconds per planning cycle， providing a planned trajectory with a frequency of 10 Hz for intelligent vehicles， achieving coordination between planning and control.

Graphical abstract

关键词

车辆工程 / 智能车辆 / 轨迹规划 / 自适应采样

Key words

vehicle engineering / intelligent vehicles / trajectory planning / adaptive sampling

0 引 言

随着大数据、多元传感和人工智能等领域的发展，融合这些先进技术的智能车辆正在以前所未有的方式彻底改变交通系统。而智能驾驶技术在发展中，也逐渐形成了感知、预测、决策、规划和控制等核心要素^［1］。其中，轨迹规划将离散决策命令转化为一条从当前位置到目标位置的局部行驶轨迹，是连接感知决策与控制执行的重要环节^［2，3］。

轨迹规划技术起源于机器人控制领域，典型方法包括：①基于采样的方法；②基于优化的方法；③基于搜索的方法；④基于几何曲线的方法^［4］。轨迹规划可分为路径规划和速度规划，在机器人应用中路径规划占据主导地位，期望速度可由路径一阶导数获得，此种思路也被应用于智能车辆轨迹规划^［5］。Chen等^［6］将智能车辆避障路径规划问题转化为三阶贝塞尔曲线参数约束优化问题，采用遗传算法求解此约束优化问题得到可行路径。Werling等^［7］在Frenet坐标系下对目标区域进行离散均匀采样，建立了一系列候选轨迹作为搜索空间，将其中最小代价轨迹作为规划结果。由上述研究可知，结合采样、优化和几何曲线等方法是轨迹规划重要的探索方向。

由于较低的计算复杂度和较高的灵活性，先采样、后优化的两阶段轨迹规划方法近几年在智能驾驶领域备受关注^［8］。基于采样的方法可以在短时间内生成多个候选路径，然后基于代价最低的路径进一步优化。此种轨迹规划方案有望在提升自动驾驶系统综合性能方面发挥关键作用。Fan等^［9］提出了基于Frenet坐标系的路径和速度迭代优化方法，通过先采样、后优化的设计思路处理智能车辆轨迹规划问题；Lim等^［10］提出了一种用于自动驾驶的分层轨迹规划算法，通过整合采样和序列二次规划，有效降低了轨迹规划的复杂性和计算负担。上述算法结合了采样和优化各自优点生成规划轨迹。然而，由于采样点是随机或均匀选取的，未能充分利用先验信息改善规划轨迹性能和优化求解效率。

障碍物、道路边界、历史轨迹等先验信息能够为车辆轨迹规划提供有价值的启发，本文提出了一种基于自适应采样的轨迹规划方法，利用障碍物和道路边界等先验信息剔除距离道路边界或障碍物较近的采样点，使用动态规划算法计算最低代价路径并构建二次规划问题进一步优化此路径。在速度规划中，基于运动学约束在速度规划空间进行自适应采样，使用动态规划确定最低代价位置-时间曲线，构建基于交通参与者预测轨迹和运动学约束的二次规划问题，求解最终速度曲线。

1 双层三阶段轨迹规划方法

本文提出的双层三阶段轨迹规划系统框架如图1所示。首先，基于Frenet坐标系分别进行路径规划和速度规划，形成解耦的双层结构。在每层中分别融合先验知识和运动学约束实现SL（station-lateral position）空间和ST（station time）空间的自适应采样。其次，使用动态规划（Dynmaic programming，DP）分别计算连接各采样点综合代价最低的路径和速度曲线。再次，构建二次规划（Quadratic programming，QP）问题进一步优化路径和速度曲线。最后，设计控制方法对所生成规划轨迹的综合性能进行验证。

2 Frenet坐标系与笛卡尔坐标系

在Frenet坐标系和笛卡尔坐标系下对车道中心线的描述如图2所示。受人类感知特点限制，人类驾驶员在驾驶中倾向以车道线作为实际参考，如图 2中Frenet坐标系s轴。Frenet坐标系以参考轨迹为基础，沿参考轨迹方向为纵轴（s），垂直于参考轨迹方向为横轴（l），横、纵轴相互垂直且参考轨迹与车道线平行。因此，基于Frenet坐标系进行轨迹规划时能够忽略道路曲率的影响，便捷地描述车辆沿车道线的纵向移动距离和车辆偏离车道中心线的横向移动距离，提高车辆轨迹规划效率。

在笛卡尔坐标系中，车辆运动状态可由元组{\left[x_x,y_x,{\theta }_x,v_x,a_x,{\kappa }_x\right]}^{\mathrm{T}}表示，其中，x_x 和y_x 分别为车辆的纵、横向位置，θ_x 、v_x 、a_x 分别为车辆的航向角、速度、加速度，κ_x 为车辆的行驶轨迹曲率。在Frenet坐标系中，车辆运动状态可由元组{\left[s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l^{\text{'}},l^{″}\right]}^{\mathrm{T}}表示，其中，s、\dot{s}、\ddot{s}分别为车辆沿s轴的纵向位移、纵向速度、纵向加速度，l、\dot{l}、\ddot{l}分别为车辆沿l轴的横向位移、横向速度、横向加速度，l^{\text{'}}、l^{″}分别为车辆横向位移对弧长ds的一阶和二阶导数。

Frenet坐标系是基于参考轨迹建立的，如图 3所示。本文以道路中心线为参考轨迹，以参考轨迹上的投影点为基准建立坐标系。记车辆当前位置向量为\vec{\mathit{x}}，对应参考轨迹点位置向量为\vec{\mathit{r}}，{\vec{\mathit{T}}}_x、{\vec{\mathit{N}}}_x分别为当前轨迹点单位切向量和单位法向量，{\vec{\mathit{T}}}_r、{\vec{\mathit{N}}}_r分别为参考轨迹点的单位切向量和单位法向量。根据向量关系可知横向位移l={[\vec{\mathit{x}}-\vec{\mathit{r}}]}^{\mathrm{T}}{\vec{\mathit{N}}}_r。进一步结合文献［11，12］，可以得到车辆笛卡尔坐标系下运动状态和Frenet坐标系下运动状态转换公式：

\left\{\begin{array}{l}s=s\\ \dot{s}=\frac{v_x\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_x-{\theta }_r\right)}{\mathrm{1}-{\kappa }_{r}l}\\ \ddot{s}=\frac{a_x\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_x-{\theta }_r\right)}{\mathrm{1}-{\kappa }_{r}l}-\frac{{\dot{s}}^{\mathrm{2}}}{\mathrm{1}-{\kappa }_{r}l}\left[l^{\text{'}}\left({\kappa }_x\frac{\mathrm{1}-{\kappa }_{r}l}{\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_x-{\theta }_r\right)}-{\kappa }_r\right)-\left({\kappa }_r^{\mathrm{\text{'}}}l+{\kappa }_r{l}^{\text{'}}\right)\right]\\ l=\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\left(\left(y_x-y_r\right)\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_r\right)-\left(x_x-x_r\right)\mathrm{s}\mathrm{i}\mathrm{n}\left({\theta }_r\right)\right)\sqrt[]{{\left(x_x-x_r\right)}^{\mathrm{2}}+{\left(y_x-y_r\right)}^{\mathrm{2}}}\\ l^{\text{'}}=\left(\mathrm{1}-{\kappa }_{r}l\right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\theta }_x-{\theta }_r\right)\\ l^{″}=-\left({\kappa }_r^{\mathrm{\text{'}}}l+{\kappa }_r{l}^{\text{'}}\right)\mathrm{t}\mathrm{a}\mathrm{n}\left({\theta }_x-{\theta }_r\right)+\frac{\mathrm{1}-{\kappa }_{r}l}{\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\left({\theta }_x-{\theta }_r\right)}\left({\kappa }_x\frac{\mathrm{1}-{\kappa }_{r}l}{\mathrm{c}\mathrm{o}\mathrm{s}\left({\theta }_x-{\theta }_r\right)}-{\kappa }_r\right)\\ \dot{l}=l^{\text{'}}\dot{s}\\ \ddot{l}=l^{\text{'}}\ddot{s}+l^{″}{\dot{s}}^{\mathrm{2}}\end{array}\right.（1）

式中：{\theta }_r、{\kappa }_r、x_r、y_r分别为参考轨迹点在笛卡尔坐标系下的航向角、曲率、纵向位置、横向位置。

3 SL空间自适应采样与路径规划

在Frenet坐标系下，轨迹规划可分为路径规划和速度规划，路径规划旨在确定车辆沿参考轨迹的理想路径。规划路径沿车道线的横纵向关系通常在SL图中能够被清晰刻画，通过在SL图中进行离散点采样并有序连接各列采样点，可以生成多条不含速度信息的候选路径。目前，许多轨迹规划研究重点是针对采样路径进行优化，而对提高单步计算效率考虑不足。此外，许多基于采样的轨迹规划方法直接对车辆周围一定区域进行均匀采样，未能充分利用现有的先验信息考虑局部区域的优化细节。因此，本文提出了一种根据先验信息调整采样区域的自适应采样方法，为后续使用动态规划求解代价最低路径提供更合理的采样点，提升计算效率。

3.1　SL空间自适应采样

在路径规划中，由于障碍物位置、道路边界和历史轨迹较易获取，且此类信息能够为轨迹规划提供启发，本节将充分考虑上述先验信息实现SL空间的自适应采样。

人工势场法因其实现简单、直观且有效，在路径规划领域得到了广泛应用和创新，Liu等^［13］通过融合道路边界和动态障碍物信息构建斥力势场，实现了不同类型障碍物不同速度下的路径规划。考虑到轨迹规划频率一般为10 Hz，而且为了满足车辆动力学限制和乘坐舒适性要求，当前计算周期规划轨迹与上一周期规划轨迹不应产生过大变化，本文在此基础上进一步考虑上一周期的路径规划结果构建历史轨迹引力场U_h。另外，借鉴文献［13］第2节中对于斥力场的设计思路，使用当前周期感知模块提供的障碍物和道路边界信息构建障碍物斥力场函数U_o和道路边界斥力场函数U_b，每个采样点的势场值U_APF由上述3项综合确定，即：

U_{\mathrm{A}\mathrm{P}\mathrm{F}}=U_{\mathrm{o}}+U_{\mathrm{b}}+U_{\mathrm{h}}（2）

3.1.1　障碍物斥力场U_o

用符号n_ij 标记采样节点，其中，i和j分别为采样点所在的行和列。障碍物在采样点n_ij 产生的斥力势场值取决于障碍物与采样点之间的距离。由于障碍物和车辆均具有一定几何尺寸，需要在障碍物斥力场函数U_o的设计中考虑障碍物宽度w_o和车辆宽度w_v，即：

U_{\mathrm{o}}=\left\{\begin{array}{l}\frac{\mathrm{1}}{\mathrm{2}}\eta \frac{\mathrm{1}}{{\left(l_{nij}-l_{\mathrm{o}}\right)}^{\mathrm{2}}},\left|l_{nij}-l_{\mathrm{o}}\right|>\frac{w_{\mathrm{o}}+w_{\mathrm{v}}}{\mathrm{2}}\\ +\mathrm{i}\mathrm{n}\mathrm{f},\left|l_{nij}-l_{\mathrm{o}}\right|\le \frac{w_{\mathrm{o}}+w_{\mathrm{v}}}{\mathrm{2}}\end{array}\right.（3）

式中：η=10，为障碍物斥力系数；l_nij 为采样点n_ij 横向位置；l_o为障碍物横向位置。当\left|l_{nij}-l_{\mathrm{o}}\right|\le \frac{w_{\mathrm{o}}+w_{\mathrm{v}}}{\mathrm{2}}时，说明采样点n_ij 与障碍物横向几何轮廓重叠，相应势场值应设为足够大，以避免碰撞。

3.1.2　道路边界斥力场U_b

与障碍物斥力场形式类似，根据采样点横向位置l_nij 与道路横向边界l_ub、l_lb间的距离定义道路边界斥力场如下：

（4）

式中：γ=5，为道路边界斥力系数；w_v为车辆宽度。l_nij ≤l_lb+w_v/2或l_nij ≥l_ub-w_v/2表明车辆驶出边界。

3.1.3　历史轨迹引力场U_h

实际应用中，获取上一周期（t-1）的规划轨迹较为方便，连续两帧的规划轨迹在0.1 s的采样间隔内应保持一定连续性，因此利用历史轨迹信息构建引力场压缩采样区域，定义历史轨迹引力场如下：

U_{\mathrm{h}}=\frac{\mathrm{1}}{\mathrm{2}}\mu {\left(l_{nij\left|t\right.}-l_{t-\mathrm{1}}\right)}^{\mathrm{2}}（5）

式中：μ=8，为历史轨迹引力系数；l_{nij\left|t\right.}-l_{t-\mathrm{1}}为t时刻采样点l_nij|t 与上一周期规划结果中同列规划点l_t-1的横向距离，其值越小表明本周期采样点n_ij 越接近同列历史最优轨迹点。

综合上述3项代价，基于障碍物、道路边界和历史最优轨迹等先验信息可得每个采样点的人工势场值，选取每列采样点中代价最小的5个构建自适应采样区域，着重关注本周期最优轨迹更可能所处的区域。

3.2　基于动态规划的路径规划

3.1节实现了采样点规模的降低，获得了基于先验信息的自适应采样区域，本节目标是在自适应采样区域内基于动态规划搜寻一条依次连接各列采样点代价最低的路径。通过构建二次规划问题可以迅速解决二维优化问题，但要求问题定义在凸空间内。然而，车辆轨迹规划任务通常是非凸问题，例如，当面对正前方的障碍物优化求解最优路径时，车辆左侧和右侧绕行路径的代价可能相同，从而构成了非凸问题。为了应对这一挑战，本节使用五次多项式连接采样区域内有限的采样点，并定义多目标评价函数评价采样点间连接路径的代价，使用动态规划以代价函数最低为目标，初步选出各连接路径组成的完整路径，获得一条粗略路径，如图4所示，这样就能够把问题转化为一个从有限离散值中求取最低值的问题。然后，基于此粗略路径构造二次规划问题，避免了直接在非凸空间内定义二次规划问题的复杂性。

两点间曲线的代价取决于3个性能指标：平滑性、安全性、相对参考轨迹偏移量。数学描述如下：

J_{\mathrm{d}\mathrm{p}}=J_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}+J_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}+J_{\mathrm{r}\mathrm{e}\mathrm{f}}（6）

式中：J_smooth为平滑性代价；J_safe为安全性代价；J_ref为相对参考轨迹的偏移量代价。

轨迹优化问题可以表述为寻找横向位移l的最优函数l=f（s），并记（s_i，l_i ）为五次多项式连接路径上均匀采样的序列点，定义平滑性代价J_smooth如下：

J_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}=w_{\mathrm{1}}\sum _i{\left(f^{\text{'}}(s_i)\right)}^{\mathrm{2}}+w_{\mathrm{2}}\sum _i{\left({f^{\text{'}}}^{\text{'}}(s_i)\right)}^{\mathrm{2}}+w_{\mathrm{3}}\sum _i{\left({{f^{\text{'}}}^{\mathrm{\text{'}}}}^{\text{'}}(s_i)\right)}^{\mathrm{2}}（7）

式中：f^{\text{'}}(s_i)为参考轨迹航向角与规划轨迹航向角之差；{f^{\text{'}}}^{\text{'}}(s_i)为规划轨迹曲率；{{f^{\text{'}}}^{\mathrm{\text{'}}}}^{\text{'}}(s_i)为曲率变化率；w₁、w₂、w₃为相应权重系数。

自适应采样已经排除了障碍物所占区域，但车辆与障碍物间应保证足够的安全冗余空间，因此在安全代价中定义了安全冗余距离d_safe。当车辆位于（s_i， l_i ），且（s_i， l_i ）与障碍物距离大于d_safe时，障碍物对采样点不构成安全威胁；当（s_i， l_i ）与障碍物距离小于d_safe时，构成安全威胁代价。定义安全性代价J_safe如下：

J_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}=\sum _{i}g\left(s_i,l_i\right)（8）

其中，

g\left(s_i,l_i\right)=\left\{\begin{array}{l}\mathrm{0},\sqrt[]{{\left(s_i-s_{\mathrm{o}}\right)}^{\mathrm{2}}+{\left(l_i-l_{\mathrm{o}}\right)}^{\mathrm{2}}}>d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\\ \frac{w_{\mathrm{4}}}{\sqrt[]{{\left(s_i-s_{\mathrm{o}}\right)}^{\mathrm{2}}+{\left(l_i-l_{\mathrm{o}}\right)}^{\mathrm{2}}}},\sqrt[]{{\left(s_i-s_{\mathrm{o}}\right)}^{\mathrm{2}}+{\left(l_i-l_{\mathrm{o}}\right)}^{\mathrm{2}}}\le d_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}\end{array}\right.（9）

式中：（s_o，l_o）为障碍物位置。

最后一项J_ref为车辆相对参考轨迹的偏移量代价，参考轨迹通常定义为车道中心线，此项代价可以保证在无障碍物时车辆沿车道中心线行驶：

J_{\mathrm{r}\mathrm{e}\mathrm{f}}=w_{\mathrm{5}}\sum _i{\left(l_i\right)}^{\mathrm{2}}（10）

式中：l_i 为车辆相对参考轨迹横向偏移量。

上述内容定义了连接路径的代价函数，从首列至最后列采样点间的各段连接路径可以形成多条完整候选路径，将第1列所有采样点代价均设定为0，由第1列采样点开始逐列计算至后续五次多项式序列点代价之和，直至计算出行驶至最后列采样点代价。然后，从最后1列逆向回溯至第1列采样点，获得最小代价路径r（s）。

3.3　基于二次规划的路径优化

上述内容通过结合自适应采样和动态规划得到一个粗略路径，将粗略路径作为二次规划问题的参考进行进一步优化，围绕该路径定义目标函数如下：

J_{\mathrm{q}{\mathrm{p}}_{-}\mathrm{p}\mathrm{a}\mathrm{t}\mathrm{h}}=w_{\mathrm{6}}\sum _i{\left(f^{\text{'}}(s_i)\right)}^{\mathrm{2}}+w_{\mathrm{7}}\sum _i{\left({f^{\text{'}}}^{\text{'}}(s_i)\right)}^{\mathrm{2}}+w_{\mathrm{8}}\sum _i{\left({{f^{\text{'}}}^{\mathrm{\text{'}}}}^{\text{'}}(s_i)\right)}^{\mathrm{2}}+w_{\mathrm{9}}\sum _i{\left(f\left(s_i\right)-r\left(s_i\right)\right)}^{\mathrm{2}}（11）

式中：r\left(s_i\right)为3.2节采用动态规划算法所得粗略路径在s_i处的横向位置。目标函数前3项用于保证参考轨迹航向角与规划轨迹航向角之差f^{\text{'}}(s_i)、规划轨迹曲率{f^{\text{'}}}^{\text{'}}(s_i)、曲率变化率{{f^{\text{'}}}^{\mathrm{\text{'}}}}^{\text{'}}(s_i)较小，以改善轨迹平滑性；最后一项用于限制规划路径与粗略路径在横向上的偏差。

所构造约束包括等式约束和不等式约束，等式约束用于保证轨迹连续性。假设相邻两点（s_i，l_i ）与（s_i+1，l_i+1）间横向位移的三阶导数为常数\frac{l_{i+\mathrm{1}}^{″}-l_i^{″}}{\Delta s}，其中\Delta s=s_i+1-s_i。在点（s_i+1，l_i+1）进行泰勒展开，即：

l_{i+\mathrm{1}}=l_i+l_i^{\mathrm{\text{'}}}\cdot \Delta s+\frac{\mathrm{1}}{\mathrm{2}}{l}_i^{″}\cdot \Delta s^{\mathrm{2}}+\frac{\mathrm{1}}{\mathrm{6}}\left(\frac{l_{i+\mathrm{1}}^{″}-l_i^{″}}{\Delta s}\right)\Delta s^{\mathrm{3}}（12）

对式（12）求导可得：

l_{i+\mathrm{1}}^{\mathrm{\text{'}}}=l_i^{\mathrm{\text{'}}}+l_i^{″}\cdot \Delta s+\frac{\mathrm{1}}{\mathrm{2}}\left(\frac{l_{i+\mathrm{1}}^{″}-l_i^{″}}{\Delta s}\right)\cdot \Delta s^{\mathrm{2}}（13）

进一步变换使等式右边为0，并写成矩阵形式：

\left(\begin{array}{cccccc}\mathrm{1}& \Delta s& \frac{\mathrm{1}}{\mathrm{3}}\Delta s^{\mathrm{2}}& -\mathrm{1}& \mathrm{0}& \frac{\mathrm{1}}{\mathrm{6}}\Delta s^{\mathrm{2}}\\ \mathrm{0}& \mathrm{1}& \frac{\mathrm{1}}{\mathrm{2}}\Delta s& \mathrm{0}& -\mathrm{1}& \frac{\mathrm{1}}{\mathrm{2}}\Delta s\end{array}\right)\left(\begin{array}{c}{l}_i\\ l_i^{\mathrm{\text{'}}}\\ l_i^{″}\\ l_{i+\mathrm{1}}\\ l_{i+\mathrm{1}}^{\mathrm{\text{'}}}\\ l_{i+\mathrm{1}}^{″}\end{array}\right)=\left(\begin{array}{l}\mathrm{0}\\ \mathrm{0}\end{array}\right)（14）

式（14）描述了保证相邻两点间轨迹连续性的等式约束。假设一条轨迹包含N个点，任意相邻两点均满足式（14）约束，则完整的等式约束可表示为：

{\mathit{A}}_{eq}x={\left[\begin{array}{lllllll}\mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \dots \end{array}\right]}^{\mathrm{T}}（15）

其中：

\begin{array}{l}{A}_{eq}={\left[\begin{array}{cccccccccc}\mathrm{1}& \Delta s& \frac{\Delta s^{\mathrm{2}}}{\mathrm{3}}& -\mathrm{1}& \mathrm{0}& \frac{\Delta s^{\mathrm{2}}}{\mathrm{6}}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \dots \\ \mathrm{0}& \mathrm{1}& \frac{\Delta s}{\mathrm{2}}& \mathrm{0}& -\mathrm{1}& \frac{\Delta s}{\mathrm{2}}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \dots \\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}& \Delta s& \frac{\Delta s^{\mathrm{2}}}{\mathrm{3}}& -\mathrm{1}& \mathrm{0}& \frac{\Delta s^{\mathrm{2}}}{\mathrm{6}}& \dots \\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}& \frac{\Delta s}{\mathrm{2}}& \mathrm{0}& -\mathrm{1}& \frac{\Delta s}{\mathrm{2}}& \dots \\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}& \Delta s& \frac{\Delta s^{\mathrm{2}}}{\mathrm{3}}& \dots \\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}& \frac{\Delta s}{\mathrm{2}}& \dots \\ \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}& \mathrm{︙}\end{array}\right]}_{(\mathrm{2}N-\mathrm{2})\times \mathrm{3}N}\\ \hspace{1em}\hspace{1em}\hspace{1em}x={[\begin{array}{cccccc}{l}_i& l_i^{\mathrm{\text{'}}}& l_i^{″}& l_{i+\mathrm{1}}& l_{i+\mathrm{1}}^{\mathrm{\text{'}}}& l_{i+\mathrm{1}}^{″}\end{array}\hspace{1em}\dots ]}_{\mathrm{3}N\times \mathrm{1}}^{\mathrm{T}}\end{array}（16）

基于x初步构造横向位置l的不等式约束如下：

{\left[\begin{array}{ccccccc}{l}_{\mathrm{m}\mathrm{i}\mathrm{n}\mathrm{1}}& -\mathrm{\infty }& -\mathrm{\infty }& l_{\mathrm{m}\mathrm{i}\mathrm{n}\mathrm{2}}& -\mathrm{\infty }& -\mathrm{\infty }& \dots \end{array}\right]}^{\mathrm{T}}\le x\le {\left[\begin{array}{ccccccc}{l}_{\mathrm{m}\mathrm{a}\mathrm{x}\mathrm{1}}& +\mathrm{\infty }& +\mathrm{\infty }& l_{\mathrm{m}\mathrm{a}\mathrm{x}\mathrm{2}}& +\mathrm{\infty }& +\mathrm{\infty }& \dots \end{array}\right]}^{\mathrm{T}}（17）

进一步考虑车辆几何尺寸，将车辆轮廓描述为矩形，如图5所示。

设置车辆轮廓4个顶点p₁、p₂、p₃、p₄横向位置的约束范围如下：

\left\{\begin{array}{c}{l}_{p_{\mathrm{1}}}=l_i+d_{\mathrm{1}}\cdot \mathrm{s}\mathrm{i}\mathrm{n}\theta +\frac{w_{\mathrm{v}}}{\mathrm{2}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}\theta \in \left[l_{\mathrm{m}\mathrm{i}\mathrm{n}p\mathrm{1}}{l}_{\mathrm{m}\mathrm{a}\mathrm{x}p\mathrm{1}}\right]\\ l_{p_{\mathrm{2}}}=l_i+d_{\mathrm{1}}\cdot \mathrm{s}\mathrm{i}\mathrm{n}\theta -\frac{w_{\mathrm{v}}}{\mathrm{2}}\mathrm{c}\mathrm{o}\mathrm{s}\theta \in \left[l_{\mathrm{m}\mathrm{i}\mathrm{n}p\mathrm{2}}{l}_{\mathrm{m}\mathrm{a}\mathrm{x}p\mathrm{2}}\right]\\ l_{p_{\mathrm{3}}}=l_i-d_{\mathrm{2}}\cdot \mathrm{s}\mathrm{i}\mathrm{n}\theta +\frac{w_{\mathrm{v}}}{\mathrm{2}}\mathrm{c}\mathrm{o}\mathrm{s}\theta \in \left[l_{\mathrm{m}\mathrm{i}\mathrm{n}p\mathrm{3}}{l}_{\mathrm{m}\mathrm{a}\mathrm{x}p\mathrm{3}}\right]\\ l_{p_{\mathrm{4}}}=l_i-d_{\mathrm{2}}\cdot \mathrm{s}\mathrm{i}\mathrm{n}\theta -\frac{w_{\mathrm{v}}}{\mathrm{2}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}\theta \in \left[l_{\mathrm{m}\mathrm{i}\mathrm{n}p\mathrm{4}}{l}_{\mathrm{m}\mathrm{a}\mathrm{x}p\mathrm{4}}\right]\end{array}\right.（18）

式中：d₁和d₂分别为质心至车辆几何轮廓前、后端距离；l_minp1和l_maxp1为顶点p₁处对应的边界值，顶点p₂、p₃、p₄同理；车辆与s轴夹角θ通常很小，令\mathrm{s}\mathrm{i}\mathrm{n}\theta \approx \mathrm{t}\mathrm{a}\mathrm{n}\theta \approx l_k^{\mathrm{\text{'}}},\mathrm{c}\mathrm{o}\mathrm{s}\theta \approx \mathrm{1}。记l_min为［l_minp1， l_minp2， l_minp3， l_minp4］中的最大元素，l_max为［l_maxp1， l_maxp2， l_maxp3， l_maxp4］中的最小元素，综合安全冗余和简便约束等设计目标，将不等式约束及其矩阵形式可表示为：

\left\{\begin{array}{l}{l}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le l_i+d_{\mathrm{1}}\cdot l_i^{\mathrm{\text{'}}}+\frac{w_{\mathrm{v}}}{\mathrm{2}}\le l_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ l_{\mathrm{m}\mathrm{i}\mathrm{n}}\le l_i+d_{\mathrm{1}}\cdot l_i^{\mathrm{\text{'}}}-\frac{w_{\mathrm{v}}}{\mathrm{2}}\le l_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ l_{\mathrm{m}\mathrm{i}\mathrm{n}}\le l_i-d_{\mathrm{2}}\cdot l_i^{\mathrm{\text{'}}}+\frac{w_{\mathrm{v}}}{\mathrm{2}}\le l_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ l_{\mathrm{m}\mathrm{i}\mathrm{n}}\le l_i-d_{\mathrm{2}}\cdot l_i^{\mathrm{\text{'}}}-\frac{w_{\mathrm{v}}}{\mathrm{2}}\le l_{\mathrm{m}\mathrm{a}\mathrm{x}},\end{array}\right.\hspace{1em}\hspace{1em}\left[\begin{array}{ccc}\mathrm{1}& d_{\mathrm{1}}& \mathrm{0}\\ \mathrm{1}& d_{\mathrm{1}}& \mathrm{0}\\ \mathrm{1}& -d_{\mathrm{2}}& \mathrm{0}\\ \mathrm{1}& -d_{\mathrm{2}}& \mathrm{0}\\ -\mathrm{1}& -d_{\mathrm{1}}& \mathrm{0}\\ -\mathrm{1}& -d_{\mathrm{1}}& \mathrm{0}\\ -\mathrm{1}& d_{\mathrm{2}}& \mathrm{0}\\ -\mathrm{1}& d_{\mathrm{2}}& \mathrm{0}\end{array}\right]\left[\begin{array}{c}{l}_i\\ l_i^{\mathrm{\text{'}}}\\ l_i^{″}\end{array}\right]\le \left[\begin{array}{c}{l}_{\mathrm{m}\mathrm{a}\mathrm{x}}-\frac{w_{\mathrm{v}}}{\mathrm{2}}\\ l_{\mathrm{m}\mathrm{a}\mathrm{x}}+\frac{w_{\mathrm{v}}}{\mathrm{2}}\\ l_{\mathrm{m}\mathrm{a}\mathrm{x}}-\frac{w_{\mathrm{v}}}{\mathrm{2}}\\ l_{\mathrm{m}\mathrm{a}\mathrm{x}}+\frac{w_{\mathrm{v}}}{\mathrm{2}}\\ \frac{w_{\mathrm{v}}}{\mathrm{2}}-l_{\mathrm{m}\mathrm{i}\mathrm{n}}\\ -\frac{w_{\mathrm{v}}}{\mathrm{2}}-l_{\mathrm{m}\mathrm{i}\mathrm{n}}\\ \frac{w_{\mathrm{v}}}{\mathrm{2}}-l_{\mathrm{m}\mathrm{i}\mathrm{n}}\\ -\frac{w_{\mathrm{v}}}{\mathrm{2}}-l_{\mathrm{m}\mathrm{i}\mathrm{n}}\end{array}\right]（19）

4 ST空间自适应采样与速度规划

前文完成了不含速度信息的路径规划，本节基于所得路径进行速度规划。速度规划负责为车辆分配合适的速度，使车辆沿着规划路径行驶。在ST图中，纵轴（S轴）坐标表示车辆沿规划路径的纵向位移s，横轴（T轴）坐标表示车辆行驶至s_i 所花费时间t_i，在ST图中进行离散点采样并连接各采样点，可以得到多个候选ST曲线，对其求一阶导即可得到速度曲线。与路径规划类似，速度规划包括ST空间自适应采样、基于动态规划的粗略速度规划以及基于二次规划的精细速度优化等3个步骤。

在实际测试中发现仅依靠车辆运动学约束就能对ST空间采样点进行有效剔除，施加约束如下：

\left\{\begin{array}{c}{\dot{s}}_i\in \left[v_{\mathrm{m}\mathrm{i}\mathrm{n}},v_{\mathrm{m}\mathrm{a}\mathrm{x}}\right]\\ {\ddot{s}}_i^{}\in \left[a_{\mathrm{m}\mathrm{i}\mathrm{n}},a_{\mathrm{m}\mathrm{a}\mathrm{x}}\right]\end{array}\right.（20）

4.1　基于动态规划的速度规划

速度规划的目标是在ST图上找到最佳速度曲线，而速度规划本身也是一个非凸优化问题。例如，在面对目标车道障碍物时，加速超车和减速避让均为可行策略，在不同可行策略所对应的凸空间内均能找到局部最优速度曲线，因此本节采用动态规划算法得到具有明显加速或减速趋势的粗略速度曲线。与路径规划不同，由于时间采样间隔较小，假设智能车辆在一个采样间隔内处于匀速运动状态，故采用直线段连接采样点。与路径规划中对于连接路径上采样点的命名一致，将直线段上的采样点（s_i，t_i ）称为序列点。

基于有限差分法对车辆运动状态描述如下：

\left\{\begin{array}{l}{\dot{s}}_i=v_i\approx \frac{s_i-s_{i-\mathrm{1}}}{dt}\\ {\ddot{s}}_i=a_i\approx \frac{s_i-\mathrm{2}{s}_{i-\mathrm{1}}+s_{i-\mathrm{2}}}{{(dt)}^{\mathrm{2}}}\\ {\stackrel{⃛}{s}}_i=jer{k}_i\approx \frac{s_i-\mathrm{3}{s}_{i-\mathrm{1}}-\mathrm{3}{s}_{i-\mathrm{2}}+s_{i-\mathrm{3}}}{{(dt)}^{\mathrm{3}}}\end{array}\right.（21）

基于3种状态定义目标函数如下：

J_{\mathrm{d}\mathrm{p}\_\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}=w_{\mathrm{10}}\sum _i{\left({\dot{s}}_i-V_{\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^{\mathrm{2}}+w_{\mathrm{11}}\sum _i{\left({\ddot{s}}_i\right)}^{\mathrm{2}}+w_{\mathrm{12}}\sum _i{\left({\stackrel{⃛}{s}}_i\right)}^{\mathrm{2}}+w_{\mathrm{13}}\sum _i\mathrm{c}\mathrm{o}\mathrm{s}{t}_{\mathrm{o}\mathrm{b}\mathrm{s}}（22）

式中：第1项促使车辆尽量遵循参考速度V_ref行驶，V_ref一般由道路车速限制或交通规则确定；第2项促使车辆速度保持稳定；第3项保证规划路径拥有较好的乘坐舒适性；第4项为障碍物代价。障碍物代价定义如下：

\mathrm{c}\mathrm{o}\mathrm{s}{t}_{\mathrm{o}\mathrm{b}\mathrm{s}}={\displaystyle \sum _{k=\mathrm{1}}^K}cos{t}_{collision}(k)（23）

（24）

式中：K为检测到的动态障碍物数量；d（k）_min为ST图上序列点到障碍物k的最短距离。

使用动态规划求解可初步求得代价最低的ST曲线。ST图中，当速度曲线位于动态障碍物投影区域左上方时，表明规划结果为本车更早抵达共享路段（s₁，s₂），意味着本车先行；当速度曲线位于动态障碍物投影区域右下方时，表明规划结果为本车更晚抵达共享路段（s₁，s₂），意味着本车让行。

动态障碍物在ST图中投影后构成的可行区域如图6所示。结合粗略速度曲线构造速度二次规划问题的约束，当所得粗略速度曲线为先行策略时，要求（t₁，t₂）区间规划点满足s_i>s_obs（图6中ST空间黄色区域）；当所得粗略速度曲线为让行策略时，要求（t₁，t₂）区间规划点满足s_i ≤s_obs（图6 ST空间蓝色区域）。

4.2　基于二次规划的速度优化

所设定二次规划问题的目标函数为：

J_{qp\_\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}=w_{\mathrm{14}}\sum _i{\left(s_i-{s_{dp}}_{\_i}\right)}^{\mathrm{2}}+w_{\mathrm{15}}\sum _i{\left({\ddot{s}}_i\right)}^{\mathrm{2}}+w_{\mathrm{16}}\sum _i{\left({\stackrel{⃛}{s}}_i\right)}^{\mathrm{2}}（25）

式中：第1项用于限制规划点与粗略曲线间纵向偏差过大；第2、3项用于限制加速度和加加速度过大。

除了4.1节所述由先行或让行构成的约束，还需设定纵向速度、加速度和加加速度等运动学约束，即：

\left\{\begin{array}{l}{v}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \dot{s}\le v_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ a_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \ddot{s}\le a_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ jer{k}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \stackrel{⃛}{s}\le jer{k}_{\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.（26）

5 横、纵向控制器

为了更好地验证规划轨迹在控制执行层的实际跟踪效果，首先在车辆二自由度动力学模型基础上构建了轨迹跟踪误差模型，然后分别设计考虑前馈的线性二次型（Linear quadratic regulator，LQR）横向控制器和考虑位置、速度误差的双比例-积分-微分（Proportional integral derivative，PID）纵向控制器。

5.1　轨迹跟踪误差模型

当前轮转角幅度较小时，轮胎的侧向力F_y 与偏转角α呈线性关系，即F_yf=C_f α_f，F_yr=C_r α^r［14］。其中，C_r和C_f分别为前、后轮侧偏刚度。根据车辆受力情况得到车辆沿y轴和绕z轴的受力平衡方程为：

\left\{\begin{array}{l}\sum F_y=m{a}_y=F_{y\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}\delta +F_{y\mathrm{r}}=C_{\mathrm{f}}{\alpha }_{\mathrm{f}}+C_{\mathrm{r}}{\alpha }_{\mathrm{r}}\\ \sum M_z=I_z\ddot{\phi }=L_{\mathrm{f}}{F}_{y\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}\delta -L_{\mathrm{r}}{F}_{y\mathrm{r}}=L_{\mathrm{f}}{C}_{\mathrm{f}}{\alpha }_{\mathrm{f}}-L_{\mathrm{r}}{C}_{\mathrm{r}}{\alpha }_{\mathrm{r}}\end{array}\right.（27）

式中：m为整车质量；a_y 为侧向加速度；δ为前轮转角；I_z 为车辆绕z轴的转动惯量；\ddot{\phi }为横摆角加速度。将a_y=\dot{v}+u\dot{\phi }代入式（27）得：

\left\{\begin{array}{c}m(\dot{v}+u\dot{\phi })=\frac{v}{u}\left(C_{\mathrm{f}}+C_{\mathrm{r}}\right)+\frac{\mathrm{1}}{u}\left(L_{\mathrm{f}}{C}_{\mathrm{f}}-L_{\mathrm{r}}{C}_{\mathrm{r}}\right)\dot{\phi }-C_{\mathrm{f}}\delta \\ I_z\ddot{\phi }=\frac{v}{u}\left(L_{\mathrm{f}}{C}_{\mathrm{f}}-L_{\mathrm{r}}{C}_{\mathrm{r}}\right)+\frac{\mathrm{1}}{u}\left(L_{\mathrm{f}}^{\mathrm{2}}{C}_{\mathrm{f}}+L_{\mathrm{r}}^{\mathrm{2}}{C}_{\mathrm{r}}\right)\dot{\phi }-L_{\mathrm{f}}{C}_{\mathrm{f}}\delta \end{array}\right.（28）

引入轨迹跟踪误差模型衡量车辆实际位置与规划轨迹间偏离，如图 7所示。为统一示意图风格，此处沿用了Frenet坐标系示意图（见图3），并对符号进行调整，二者均包含待优化轨迹和参考轨迹两种轨迹。此处待优化轨迹为行驶轨迹，需要施加控制算法优化跟踪误差，参考轨迹为规划轨迹。

首先定义轨迹跟踪误差模型状态量为{[\begin{array}{cccc}{e}_{\mathrm{1}}& {\dot{e}}_{\mathrm{1}}& e_{\mathrm{2}}& {\dot{e}}_{\mathrm{2}}\end{array}]}^{\mathrm{T}}，其中，e₁为横向位置误差l_c，即Frenet坐标系下实际车辆质心位置与规划轨迹点的横向距离，e_{\mathrm{2}}=\phi -{\theta }_x为横摆角误差。横向误差变化率{\dot{e}}_{\mathrm{1}}和横摆角误差e₂可表示为：

\begin{array}{l}{\dot{e}}_{\mathrm{1}}=v+u{e}_{\mathrm{2}}\\ e_{\mathrm{2}}=\phi -{\theta }_x\end{array}（29）

故有：

\left\{\begin{array}{l}{\ddot{e}}_{\mathrm{1}}=\dot{v}+u{\dot{e}}_{\mathrm{2}}\\ {\dot{e}}_{\mathrm{2}}=\dot{\phi }-{\dot{\theta }}_x\\ {\ddot{e}}_{\mathrm{2}}=\ddot{\phi }-{\ddot{\theta }}_x\approx \ddot{\phi }\end{array}\right.,\hspace{1em}\left\{\begin{array}{l}v={\dot{e}}_{\mathrm{1}}-u{e}_{\mathrm{2}}\\ \dot{v}={\ddot{e}}_{\mathrm{1}}-u{\dot{e}}_{\mathrm{2}}\\ \dot{\phi }={\dot{e}}_{\mathrm{2}}+{\dot{\theta }}_x\\ \ddot{\phi }={\ddot{e}}_{\mathrm{2}}\end{array}\right.（30）

将式（30）与二自由度车辆动力学模型状态方程联立，得到连续时间的轨迹误差模型状态方程：

{\dot{\mathit{X}}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(t)=A{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(t)+\mathit{B}U(t)+\mathit{C}{\dot{\theta }}_x(t)（31）

\begin{array}{l}\mathit{A}=\left[\begin{array}{cccc}\mathrm{0}& \mathrm{1}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \frac{C_{\mathrm{f}}+C_{\mathrm{r}}}{mu}& -\frac{C_{\mathrm{f}}+C_{\mathrm{r}}}{u}& \frac{L_{\mathrm{f}}{C}_{\mathrm{f}}-L_{\mathrm{r}}{C}_{\mathrm{r}}}{mu}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\\ \mathrm{0}& \frac{L_{\mathrm{f}}{C}_{\mathrm{f}}-L_{\mathrm{r}}{C}_{\mathrm{r}}}{I_{z}u}& \frac{L_{\mathrm{r}}{C}_{\mathrm{r}}-L_{\mathrm{f}}{C}_{\mathrm{f}}}{I_z}& \frac{L_{\mathrm{f}}^{\mathrm{2}}{C}_{\mathrm{f}}+L_{\mathrm{r}}^{\mathrm{2}}{C}_{\mathrm{r}}}{I_{z}u}\end{array}\right],{\mathit{X}}_{err}(t)=\left[\begin{array}{c}{e}_{\mathrm{1}}\\ \stackrel{.}{e_{\mathrm{1}}}\\ e_{\mathrm{2}}\\ \stackrel{.}{e_{\mathrm{2}}}\end{array}\right],\\ \mathit{B}=\left[\begin{array}{c}\mathrm{0}\\ -\frac{C_{\mathrm{f}}}{m}\\ \mathrm{0}\\ -\frac{L_{\mathrm{f}}{C}_{\mathrm{f}}}{I_z}\end{array}\right],\mathit{C}=\left[\begin{array}{c}\mathrm{0}\\ \frac{L_{\mathrm{f}}{C}_{\mathrm{f}}-L_{\mathrm{r}}{C}_{\mathrm{r}}}{mu}-u\\ \mathrm{0}\\ \frac{L_{\mathrm{f}}^{\mathrm{2}}{C}_{\mathrm{f}}+L_{\mathrm{r}}^{\mathrm{2}}{C}_{\mathrm{r}}}{I_{z}u}\end{array}\right],U(t)=\delta \end{array}（32）

式中： A 为状态转移矩阵； B 为输入矩阵； C 为干扰矩阵。

5.2　考虑前馈的LQR横向控制

前述内容融合二自由度车辆动力学方程得到了轨迹跟踪误差模型状态方程，下面基于LQR建立误差到控制输入（前轮转角\delta）的映射关系，同时引入了前馈控制量，实现了消除稳态误差的目的。将最优转角计算问题解耦δ_optimal=δ_lqr+δ_offset，其中，δ_lqr为路径曲率为0 m^-1时根据跟踪误差计算得到的前轮转角，δ_offset为规划路径曲率导致的附加前轮转角。

离散化的轨迹误差模型状态方程可以表示为：

\begin{array}{l}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k+\mathrm{1})={\mathit{A}}_{\mathrm{d}}{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)+{\mathit{B}}_{\mathrm{d}}\mathit{U}(k)+{\mathit{C}}_{\mathrm{d}}{\dot{\theta }}_x(k)\\ {\mathit{A}}_{\mathrm{d}}=\frac{{\mathrm{e}}^{\frac{\mathit{A}T}{\mathrm{2}}}}{-{\mathrm{e}}^{\frac{\mathit{A}T}{\mathrm{2}}}}=\frac{I+\frac{\mathit{A}T}{\mathrm{2}}}{I-\frac{\mathit{A}T}{\mathrm{2}}}={\left(I-\frac{\mathit{A}T}{\mathrm{2}}\right)}^{-\mathrm{1}}\left(I+\frac{\mathit{A}T}{\mathrm{2}}\right),{\mathit{B}}_{\mathrm{d}}=\mathit{B}T,{\mathit{C}}_{\mathrm{d}}=\mathit{C}T\end{array}（33）

式中： A_d为离散时间状态矩阵； B_d为离散时间输入矩阵； C_d为离散时间干扰矩阵；T为采样步长；{\dot{\theta }}_x(k)取决于规划路径曲率。

目标函数应同时考虑系统状态量偏差和控制量大小，以保证车辆快速、平稳地跟踪规划轨迹。因此，LQR控制器目标函数采用如下形式：

J={\displaystyle \sum _{\mathrm{0}}^{\mathrm{\infty }}}\left({\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}^{\mathrm{T}}(k)\mathit{Q}{X}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)+\mathit{U}{(k)}^{\mathrm{T}}\mathit{R}\mathit{U}(k)\right)（34）

式中：第1项为跟踪过程路径偏差累积大小；第2项为跟踪过程能量损耗。在LQR中，包含 Q 和 R 两个权重矩阵， Q 为半正定状态权重系数矩阵， R 为正定控制量权重系数矩阵，二者设置如下：

\mathit{Q}=\left[\begin{array}{cccc}\mathrm{25}& \mathrm{0}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{3}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{10}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{4}\end{array}\right]，\mathit{R}=\mathrm{15}

求解式（34）可以得到控制量：

\mathit{U}(k+\mathrm{1})={\delta }_{lqr}=-{\left(\mathit{R}+{\mathit{B}}_{\mathrm{d}}^{\mathrm{T}}{P}_{k+\mathrm{1}}{\mathit{B}}_{\mathrm{d}}\right)}^{-\mathrm{1}}{\mathit{B}}_{\mathrm{d}}^{\mathrm{T}}{P}_{k+\mathrm{1}}{\mathit{A}}_{\mathrm{d}}{x}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)=-\mathit{K}{x}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)（35）

式中：K为最优控制律反馈矩阵；P_k+1为黎卡提方程的解，可以通过迭代求解得到。

由于在LQR状态反馈控制求解过程中没有考虑规划路径本身的动态变化特性，δ_lqr为假设参考轨迹为直线时计算出的转角，所以后续应加上由规划轨迹参考点曲率引起的附加前轮转角δ_offset 以抵消稳态误差。将式（35）计算出的最优控制律代入式（33）中，得到系统的稳态反馈状态方程：

{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k+\mathrm{1})=\left({\mathit{A}}_{\mathrm{d}}-{\mathit{B}}_{\mathrm{d}}\mathit{K}\right){\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)+{\mathit{C}}_{\mathrm{d}}{\dot{\theta }}_x(k)（36）

将式（36）代入式（33）中，可得：

{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k+\mathrm{1})={\mathit{A}}_{\mathrm{d}}{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)+{\mathit{B}}_{\mathrm{d}}\left(-\mathit{K}{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)+{\delta }_{\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}\right)+{\mathit{C}}_{\mathrm{d}}{\dot{\theta }}_x(k)（37）

令 X_err（k+1）=0可得：

{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)={\left({\mathit{B}}_{\mathrm{d}}K-{\mathit{A}}_{\mathrm{d}}\right)}^{-\mathrm{1}}\left({\mathit{B}}_{\mathrm{d}}{\delta }_{\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}+{\mathit{C}}_{\mathrm{d}}{\dot{\theta }}_x(k)\right)（38）

记反馈矩阵 K=［K₁K₂K₃K₄］，计算式（38）得：

{\mathit{X}}_{\mathrm{e}\mathrm{r}\mathrm{r}}(k)=\left[\begin{array}{l}\frac{\mathrm{1}}{K_{\mathrm{1}}}\left\{{\delta }_{\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}-\frac{{\dot{\theta }}_x(k)}{u}\left[L_{\mathrm{f}}+L_{\mathrm{r}}-L_{\mathrm{r}}{K}_{\mathrm{3}}-\frac{m{u}_{}^{\mathrm{2}}}{L_{\mathrm{f}}+L_{\mathrm{r}}}\left(\frac{L_{\mathrm{r}}}{C_{\mathrm{f}}}+\frac{K_{\mathrm{3}}{L}_{\mathrm{f}}}{C_{\mathrm{r}}}-\frac{L_{\mathrm{f}}}{C_{\mathrm{r}}}\right)\right.\right\}\\ \mathrm{0}\\ -\frac{{\dot{\theta }}_x(k)}{u}\left(L_{\mathrm{r}}+\frac{L_{\mathrm{f}}}{L_{\mathrm{f}}+L_{\mathrm{r}}}\frac{m{u}^{\mathrm{2}}}{C_{\mathrm{r}}}\right)\\ \mathrm{0}\end{array}\right]（39）

由式（39）可知，通过调节δ_offset能使横向位置跟踪误差趋近0，所需施加的δ_offset大小为：

{\delta }_{\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}=\frac{{\dot{\theta }}_x(k)}{u}\left[L_{\mathrm{f}}+L_{\mathrm{r}}-L_{\mathrm{r}}{K}_{\mathrm{3}}-\frac{m{u}_{}^{\mathrm{2}}}{L_{\mathrm{f}}+L_{\mathrm{r}}}\left(\frac{L_{\mathrm{r}}}{C_{\mathrm{f}}}+\frac{K_{\mathrm{3}}{L}_{\mathrm{f}}}{C_{\mathrm{r}}}-\frac{L_{\mathrm{f}}}{C_{\mathrm{r}}}\right)\right]（40）

5.3　考虑纵向位置、速度跟踪误差的PID纵向控制

PID控制器设计、调整简便，适用范围广泛，是应用最广泛的工业控制器之一^{［15，16］}，本节选用PID控制器用于车辆纵向控制。由于所设计纵向控制器重点在于实现规划轨迹跟踪，因此除了考虑速度误差，还需要考虑位置误差以实现跟踪控制与轨迹规划的协同。如图8所示，s_d、v_d和a_d为轨迹规划模块输出的期望位置、速度和加速度；s、v为车辆当前时刻的位置、速度；\Deltav_d为第一个PID控制器生成的速度补偿；\Deltaa_d为第二个PID控制器生成的加速度补偿。PID控制器参数设置总结如表1所示。

6 仿真验证

本文所采用计算平台处理器为Intel（R） Core（TM） i7-8700 CPU（3.20 GHz），内存为16 GB。在软件方面，基于Prescan 2019.2.0、CarSim 2019.1和Matlab 2021a建立仿真平台。其中，Prescan用于构建驾驶环境，设置了分别包含静态障碍物和动态障碍物的两种场景；CarSim用于高保真车辆动力学建模；Matlab/Simulink用于实现所设计的规划和控制算法。在两种驾驶场景下验证了所提出智能车辆规划-控制方案的集成性，并对算法实时性优势进行分析。所使用车型为奥迪A8，车辆参数如表2所示。

6.1　多静态障碍物驾驶场景

针对存在多静态障碍物的驾驶工况，构建驾驶场景如图9所示，红色箱体为正方形障碍物，长、宽均为1 m，智能车辆依据导航信息在右侧车道直线行驶，由感知系统提供障碍物、车道线等信息，由所设计规划-控制算法进行局部轨迹规划和跟踪。

智能车辆和障碍物的轨迹时空图如图10所示，车辆能够进行横向调整以规避障碍物。规划路径、速度、航向角及其跟踪误差曲线如图11所示。图11（a）进一步展示了车辆行驶过程中规划路径、实际路径以及位置跟踪误差曲线。基于纵向位置可以将曲线大致分为5个阶段：（0，50）、（50，150）、（150，250）、（250，350）、（350，420），区间节点与障碍物位置一致。可以看出，前4个阶段车辆规划与控制的目标为避障，规划路径与实际路径基本重合，位置跟踪误差保持在0.05 m以内。仿真过程车辆始终在本车道内进行避障操作，车辆横向移动幅度约为0.9 m。图 11（b）展示了车辆行驶过程中规划速度、实际速度以及速度跟踪误差曲线，规划速度与实际速度基本重合，速度跟踪误差保持在1.5 m/s以内。避障操作会引起轻微的速度跟踪误差，在直行状态下跟踪误差保持在0 m/s附近。图11（c）展示了车辆行驶过程中规划航向角、实际航向角以及航向角误差曲线。航向角误差围绕0 rad波动，并保持在0.02 rad以内。

规划路径曲率以及横摆角速度曲线如图12所示，规划轨迹曲率保持在0.006 m^-1内，横摆角速度保持在0.04 rad/s内。

6.2　多动态障碍物驾驶场景

针对存在多动态障碍物的驾驶工况，所搭建场景如图13所示。被控车辆行驶于中间车道，初始速度为0 m/s，仿真开始后以25 m/s为目标速度加速行驶，前方一辆交通参与者以10 m/s匀速直行，左后、右后方各有一辆交通参与者，初始速度为0 m/s，仿真开始后分别以20 m/s和15 m/s为目标速度行驶。

智能车辆和交通参与者的轨迹时空曲线如图14所示。智能车辆通过换道规避交通参与者，此处基于文献［17］设计行为决策算法用于换道决策。

规划路径、速度、航向角及其跟踪误差曲线如图15所示，目标车道的改变会引起规划轨迹所参考道路中心线的变化。如图15（a）所示，换道路径平滑、连续，跟踪误差保持在0.06 m内。图15（b）展示了车辆行驶过程中规划速度、实际速度以及速度跟踪误差曲线。由于所设目标速度为25 m/s，前车速度为10 m/s，而0~3.6 s本车处于跟车状态，本车通过速度调整能够确保安全距离。3.6 s车辆开始换道，换道结束后速度误差趋近于0 m/s。图15（c）展示了规划航向角、实际航向角以及航向角误差曲线。航向角最大误差产生于换道初始时刻，仿真过程保持在0.025 rad以内。

规划路径曲率和横摆角速度曲线如图16所示。3.6 s以后的换道行为引起了规划路径曲率的上升，开始换道后曲率逐渐降低。仿真过程规划路径曲率保持在0.02 m^-1内。车辆横摆角速度最大值小于0.25 rad/s，换道结束后趋近于0 rad/s。

6.3　实时性分析

在轨迹规划和跟踪控制模块解耦的框架下，智能车辆规划轨迹时不用考虑复杂的动力学问题，只需基于几何运动学模型处理碰撞约束，同时跟踪控制模块也能基于复杂的动力学模型跟踪参考轨迹，实现100 Hz的高频反馈修正，所以5~10 Hz的轨迹规划频率被广泛采用，也是衡量轨迹规划算法实时性表现的重要指标^［18-20］。每列选用不同采样点数量的单次规划计算耗时对比图如图17所示，横轴表示仿真步数，纵轴表示计算耗时，可以看出，当每列选用5个采样点时，可将计算耗时限制在100 ms左右。采样5、7、9、11个点的计算耗时均值分别为102.3、119.9、177.5、375.6 ms。因此，每列选用5个采样点能够为车辆提供10 Hz左右的重规划能力，保证了轨迹规划-跟踪控制解耦框架的可行性。

基于均匀采样点、自适应采样点、模型预测控制算法以及快速随机树算法生成的路径规划结果如图18所示。基于均匀采样和自适应采样的路径规划结果完全一致，在动态规划处理后的粗略路径基础上，通过构建二次规划问题可以得到综合性能更出色的最终规划路径，如图18中蓝色曲线所示，进行此帧规划耗时95.6 ms；设定相同起始点和终止点，采用模型预测控制所生成轨迹如图18中橙黄色曲线所示，其中曲线连续性保障依靠二自由度车辆运动学模型约束实现，避撞性保障通过设置安全性约束实现，进行此帧规划耗时959.1 ms，实时性较差；使用快速随机树在迭代次数为1 000、搜索步长为0.5 m的情况下，进行此帧规划耗时50.1 ms，实时性较为理想，规划轨迹如图18中绿色曲线所示，规划轨迹抖动过大，这是由于快速随机树本身是一种随机采样方法，未考虑动力学约束，节点扩展方向不稳定。

7 结束语

本文提出了一种基于自适应采样的智能车辆分层轨迹规划方法。通过结合障碍物、车道线、历史规划轨迹等先验信息有效减少了SL空间和ST空间的采样点数量，保证了轨迹规划的效率和安全性。仿真结果验证了该方法在提高计算效率的同时，能够有效减少规划耗时，轨迹规划模块达到了10 Hz左右的快速响应，同时能够与以100 Hz运行的跟踪控制模块协同，完成动态交通场景下的轨迹规划任务。此外，通过所设计的横向和纵向控制器，实现了规划与控制的有效协同，进一步验证本文所提轨迹规划方法的拓展性。然而，本文所提方法还存在一定的局限性，比如在更复杂的交通环境中的适应性尚需验证。未来的研究将致力于在更多样化的场景中测试和优化该方法，以及探索更为高效的采样策略。

参考文献

[1] Qureshi K N, Abdullah A H. A survey on intelligent transportation systems[J]. Middle-East Journal of Scientific Research, 2013, 15(5): 629-642.

[2] 熊璐, 杨兴, 卓桂荣, 等. 无人驾驶车辆的运动控制发展现状综述[J]. 机械工程学报, 2020, 56(10): 127-143.

[3] Xiong Lu, Yang Xing, Zhuo Gui-rong, et al. Review on motion control of autonomous vehicles[J]. Journal of Mechanical Engneering, 2020, 56(10): 127-143.

[4] 彭浩楠, 唐明环, 查奇文, 等. 自动驾驶汽车双车道换道最优轨迹规划方法[J]. 吉林大学学报: 工学版, 2022, 52(12): 2852-2863.

[5] Peng Hao-nan, Tang Ming-huan, Zha Qi-wen, et al. Optimization-based lane changing trajectory planning approach for autonomous vehicles on two-lane road[J]. Journal of Jilin University (Engineering and Technology Edition), 2022, 52(12): 2852-2863.

[6] 张利鹏, 苏泰, 严勇. 基于采样区域优化的智能车辆轨迹规划方法[J]. 机械工程学报, 2022, 58(14): 276-287.

[7] Zhang Li-peng, Su Tai, Yan Yong. Trajectory planning method of intelligent vehicle based on sampling area optimization[J]. Journal of Mechanical Engneering, 2022, 58(14): 276-287.

[8] 张一鸣, 周兵, 吴晓建, 等. 基于前车轨迹预测的高速智能车运动规划[J]. 汽车工程, 2020, 42(5): 574-580.

[9] Zhang Yi-ming, Zhou Bin, Wu Xiao-jian, et al. Motion planning of high speed intelligent vehicle based on front vehicle trajectory prediction[J]. Automotive Engineering, 2020, 42(5): 574-580.

[10] Chen L, Qin D F, Xu X, et al. A path and velocity planning method for lane changing collision avoidance of intelligent vehicle based on cubic 3-D Bezier curve[J]. Advances in Engineering Software, 2019, 132: 65-73.

[11] Werling M, Kammel S, Ziegler J, et al. Optimal trajectories for time-critical street scenarios using discretized terminal manifolds[J]. The International Journal of Robotics Research, 2012, 31(3): 346-359.

[12] 余卓平, 李奕姗, 熊璐. 无人车运动规划算法综述[J]. 同济大学学报: 自然科学版, 2017, 45(8): 1150-1159.

[13] Yu Zhuo-ping, Li Yi-shan, Xiong Lu. A review of the motion planning problem of autonomous vehicle[J]. Journal of Tongji University (Natural Science), 2017, 45(8): 1150-1159.

[14] Fan H Y, Fan Z, Liu C C, et al. Baidu apollo EM motion planner[J]. Arxiv Preprint, 2018, 9: No.180708048.

[15] Lim W, Lee S, Sunwoo M, et al. Hierarchical trajectory planning of an autonomous car based on the integration of a sampling and an optimization method[J]. IEEE Transactions on Intelligent Transportation Systems, 2018, 19(2): 613-626.

[16] 唐志荣, 冀杰, 吴明阳, 等. 基于改进人工势场法的车辆路径规划与跟踪[J]. 西南大学学报: 自然科学版, 2018, 40(6): 174-182.

[17] Tang Zhi-rong, Ji Jie, Wu Ming-yang, et al. Vehicle path planning and tracking based on improved artificial potential field method[J]. Journal of Southwest University (Natural Science Edition), 2018, 40(6): 174-182.

[18] Lu B, Li G F, Yu H L, et al. Adaptive potential field-based path planning for complex autonomous driving scenarios[J]. IEEE Access, 2020, 8: 225294-225305.

[19] Liu Z X, Yuan X F, Huang G M, et al. Two potential fields fused adaptive path planning system for autonomous vehicle under different velocities[J]. ISA Transactions, 2021, 112: 176-185.

[20] Qu T, Chen H, Cao D P, et al. Switching-based stochastic model predictive control approach for modeling driver steering skill[J]. IEEE Transactions on Intelligent Transportation Systems, 2014, 16(1): 365-375.

[21] 许芳, 张君明, 胡云峰, 等. 智能车辆路径跟踪横纵向耦合实时预测控制器[J]. 吉林大学学报: 工学版, 2021, 51(6): 2287-2294.

[22] Xu Fang, Zhang Jun-ming, Hu Yun-feng, et al. Lateral and longitudinal coupling real-time predictive controller for intelligent vehicle path tracking[J]. Journal of Jilin University (Engineering and Technology Edition), 2021, 51(6): 2287-2294.

[23] Qu T, Zhao J W, Gao H H, et al. Multi-mode switching-based model predictive control approach for longitudinal autonomous driving with acceleration estimation[J]. IET Intelligent Transport Systems, 2020, 14(14): 2102-2112.

[24] Hang P, Lv C, Xing Y, et al. Human-like decision making for autonomous driving: a noncooperative game theoretic approach[J]. IEEE Transactions on Intelligent Transportation Systems, 2020, 22(4): 2076-2087.

[25] Polack P, Altché F, Andréa N B, et al. Guaranteeing consistency in a motion planning and control architecture using a kinematic bicycle model[C]∥Proceeding of the 2018 Annual American Control Conference (ACC). Piscataway, N J: IEEE, 2018: 3981-3987.

[26] Mouhagir H, Talj R, Cherfaoui V, et al. Evidential-based approach for trajectory planning with tentacles, for autonomous vehicles[J]. IEEE Transactions on Intelligent Transportation Systems, 2019, 21(8): 3485-3496.

[27] Ji J L, Yang T K, Xu C, et al. Real-time trajectory planning for aerial perching[C]∥Proceeding of the 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway, N J:IEEE, 2022: 10516-10522.

基金资助

吉林省科技厅重点研发项目(20230201109GX)

重庆市自然科学基金面上项目(CSTB2023NSCQ-MSX1019)