基于高斯伪谱法的自动驾驶车辆状态研究*

唐晓峰，杨 林，袁静妮

（上海交通大学机械与动力工程学院，上海 200240）

前言

自动驾驶车辆主要采用雷达、机器视觉、地图导航等传感器实现对道路环境的感知，应用决策、规划和控制技术实现避障功能，其中合理的决策与路径规划是实现自动驾驶车辆进行避障的核心环节之一。目前常用的自动驾驶路径规划方法主要有4类：第1类方法是基于几何模型搜索方法［1］，例如常用的单元体分解法、可视图法等，其原理是利用图像搜索方法搜索可行区域，其缺点是只关注避障路径本身，很难增加车辆动力学的安全性约束；第2类方法是基于力学场和导航方法，例如人工势场方法和谐波函数场等，其原理是将道路和动静障碍物抽象为具有引力或斥力的势能场，以过程中势能最小为目标进行规划路径，其缺点是算法只能产生瞬时指令，因此其行为不可预测［2］；第3类方法是基于元启发式方法，例如粒子群算法、遗传算法等，其原理是结合启发式算法，在复杂的非结构化约束下随机找到有效的近似最优解，缺点是由于搜索空间是离散的，所以生成的轨迹不平滑且计算量大［3］；第4类是基于数学优化方法，例如常见的线性二次型、模型预测控制等，其原理是采用数学优化的方法处理数值最小的成本函数，包含了严格且系统性较高的车辆动力学约束和各类别的安全约束等。将自动驾驶车辆的路径规划问题转换成性能指标的优化问题是未来的发展趋势。

模型预测控制方法是优化路径的一种常用算法，它具有建立约束模型、车辆动力学模型和道路模型的优点。然而，自动驾驶车辆存在传感器失效或不准确、车辆的位置与航向角等状态无法准确测量、车辆驶过特定道路时需要多阶段轨迹优化和不同状态的约束等情况，因此模型预测控制存在处理问题时要求状态边界准确性较高且容易陷入局部最优问题等缺点。据此，本文中提出了基于高斯伪谱法的自动驾驶车辆的多阶段轨迹优化方法。伪谱法最早应用于航空航天领域，例如当火箭发射升空时会呈现多阶段轨迹和每一阶段的状态约束变化，伪谱法能实现多阶段轨迹优化和路径规划的全局最优。将伪谱法应用到自动驾驶车辆的路径规划中，有助于实现自动驾驶在多个道路阶段的轨迹优化，且可根据每一阶段的要求设定车辆行驶状态的不同约束，以实现车辆在路径规划上的全局最优。伪谱法包括勒让德伪谱法、高斯伪谱法和切比雪夫伪谱法3种，其特点是采用全局正交多项式逼近状态变量和控制变量，具有快速收敛、对初值敏感度不高和可设立多阶段的轨迹优化等优点。其中，高斯伪谱法结合了有限元区间划分的思想进行自适应调节，求解精度更高［4］。本文中以车辆自动驶过特定的道路（容易发生交通事故的区域）为模型，研究基于高斯伪谱法的多阶段轨迹优化问题。

1 道路空间自动驾驶路径规划模型

1.1 环境建模

自动驾驶车辆通常根据预测的确定性障碍物的未来行驶意图规划其自身的轨迹，然而不确定性动态障碍物的行驶意图很难预测，因此通常将车辆置于特定的行驶环境中，进行环境建模来规划路径。环境模型通常分为决策性模型、基于集类、随机类和基于场景模型等4类，决策性环境建模为每个场景的障碍物提供单一的预测轨迹，其特点是简化了控制过程，但无法处理不确定的情况；基于集类的环境建模是给定一个预测模型和关系来近似目标车辆的未来空间；随机类环境建模是使用可能性分配函数将驾驶行为模型化，缺点是难以计及交通规则与车辆之间的关系，增加了复杂性；基于场景的环境建模可充分克服上述缺点，可描述不确定的行驶环境，同时增加了交通场景的理解程度［5］。本文中采用基于场景的环境建模来研究自动驾驶车辆在特定道路环境下的轨迹规划问题。

为不失一般性，本文中建立了如图1所示的场景模型。假设主车1要经过道路前方容易发生交通事故的区域，其中L段较长距离为主车1的起始行驶区域或周围车辆的换道行驶区域，在容易发生交通事故的区域有3辆车，主车2、6是临近车道的周围车辆，其车道保持行驶或换道行驶状态影响主车1的行驶状态约束设置，换道行驶须在L段距离内完成。为便于更好地实现主车对前方车辆的有效感知，在道路环境上设定了虚拟车辆，当主车临近虚拟车辆位置时，可通过通信技术通知主车实时调整路径轨迹和行驶状态，例如加减速、巡航或换道等行驶行为。从图1中可以看出，只要进行主车1在L1和L2段距离的轨迹规划和车辆自身及周围车辆的行驶状态约束设置，便可实现主车1对前方道路环境的有效感知。

图1 基于场景的自动驾驶环境建模

首先定义虚拟车辆与车辆4的相对安全距离，采用传统的Mazda模型［6］，当主车1行驶到虚拟车辆的位置时，须采取合理的行驶行为，保证车辆的安全行驶。

定义1：虚拟车辆位置与车辆4位置的相对制动安全距离设定为

式中：v为虚拟车辆的速度；vrel为相对车速；a1为虚拟车辆的最大减速度；a2为车辆4的最大减速度；t1为处理器等响应延迟时间；t2为制动器延迟时间；t3为传感器感知车辆4的延迟时间；d0为最小停车距离。

L3段距离的约束局限范围是：L3≥d0

其次定义虚拟车辆的形状。采用基于P准则的障碍物模型构建函数，可方便模拟车辆为正方形、矩形或者圆形［7］，主要形式为

1.2 车辆动力学建模

车辆的动力学特性会影响自动驾驶车辆的行驶轨迹和跟踪控制等任务，因此，本文中增加了车辆动力学模型，并将其简化为如图2所示的模型［8］，车辆动力学模型可描述如下：

式中：x、y、θ为车辆给定的位置和行驶方向；v、L为车辆速度和轴距；γ为横摆角；a、w为车辆的加速度和横摆角速度。令u为控制变量，u＝［a，w］T。

图2 车辆动力学模型

1.3 边界条件与约束

自动驾驶车辆在进行轨迹规划时须考虑车辆动力学约束、交通规则和每一阶段的起始和终了条件，以保证车辆可规划出一条安全有效的路径，因此在进行轨迹规划时须设定各种边界条件，包括初始边界条件和终了边界条件，初始边界条件是自动驾驶车辆起始的状态变量初值，终了边界条件是在轨迹规划的终点需要满足的条件，首先设定L1段的起始和终了条件，具体过程如下。

车辆在经过L1段距离的起始条件为

车辆在经过L1段距离后的终了条件为

车辆在L1段各状态的约束为

车辆经过L2段距离时状态的起始条件是L1段距离的终了条件，这种约束称为链接约束，即车辆在L2段距离的终了条件为

车辆在L2段各状态的约束为

主车在L2路段中行驶时，须设定虚拟车辆的路径约束，根据式（2）可知，为便于计算有效性，定义虚拟车辆的约束为

式中：（xc，yc）为障碍物中心坐标；a、b、c为确定参数。这样，路径约束为

h（x，y）≥0

1.4 性能指标

自动驾驶车辆的路径规划要从起始位置到终了位置，其性能指标可设置计算耗能最低、计算快速通行能力等。本文中研究车辆在低速行驶下对图1中L1和L2段道路环境感知情况，为保证两段道路环境的畅通性，采取快速通行能力的性能优化指标，目标是得到时间最短的轨迹，因此，性能指标函数为

2 高斯伪谱法

使用高斯伪谱法计算优化问题的离散解可满足间接法的1阶最优条件，通过一系列变换将连续优化问题离散为带有代数约束的参数优化问题，即非线性规划问题。高斯伪谱法在控制变量、状态变量和协调变量上的近似精度和收敛速度具有明显的优势［9］，具体步骤如下［10］。

（1）时间区间转换。高斯伪谱法实现最优控制问题的时间区间是τ∈［-1，1］，将姿态优化问题的时间区间［t0，tf］转换为［-1，1］，定义时间变量 t为

（2）离散状态变量与控制变量。高斯伪谱法主要采用拉格朗日插值和高斯求积公式将一系列最优问题在Legendre-Gauss（LG）点进行离散化处理。其中，LG点为K阶Legendre多项式：

则状态变量近似为

式中Li（τ）为Lagrange插值基函数：

高斯伪谱法控制变量近似为

（3）终了状态约束的离散状态方程约束中考虑了末端节点，因此须计算出末端节点方程。对动力学微分方程在区间［-1，1］上积分，得

式中：τk为 LG点；μk为 Gauss权重。离散化并用高斯积分来近似可得

（4）动力学微分方程的转换。高斯伪谱法的状态变量通过正交插值多项式来近似，因此其导数可通过对式（12）求导得到，即

令：

式中k＝1，2，…，K。再代入动力学微分方程，可得到离散表示式：

（5）目标函数。应用高斯积分来计算优化函数中目标函数的积分项，可得离散形式的近似目标函数：

3 仿真结果分析

主车1在 L1阶段的起始条件：x10＝10 m，y10＝0.1 m，θ10＝0.2 rad，v10＝5 m／s，γ10＝0.2 rad／s。

主车1在 L1阶段的终了条件：x1f＝50 m，y1f＝0，θ1f＝0，v1f＝8 m／s，γ1f＝0。

主车1在 L1段各状态的约束：x∈［0，200］，y∈

本试验在MATLAB2018b环境下，进行自动驾驶车辆的多阶段研究，设定终点约束为

E（x（t0），x（tf），t0，tf）＝tf

寻找满足约束条件的状态变量和控制变量，使J＝tf。计算主车1驶往L1段距离行驶过程中的状态，并考虑主车1在低速和高速两种行驶工况。

（1）首先，研究主车1的低速行驶情况，其仿真数据约束条件如下。［0，3］，θ∈［-3π，3π］，v∈［5，8］，γ∈［-3π，3π］，a∈［-3，3］。

主车1在L2阶段的起始条件：x20＝50 m，y20＝0，θ20＝0，v20＝8 m／s，γ20＝0。

主车1在 L2阶段的终了条件：x2f＝100 m，y2f＝0，θ2f＝0.1 rad，v2f＝10 m／s，γ2f＝0。

主车1在 L2段各状态的约束：x∈［0，200］，y∈［0，3］，θ∈［-3π，3π］，v∈［5，8］，γ∈［-3π，3π］，a∈［-3，3］。

联立式（3）～式（5）、式（9）和式（15），可建立自动驾驶车辆的优化模型，其结果如图3和图4所示。

图3 L1和L2阶段的轨迹和速度

图4 L1和L2阶段的车辆状态

从图3和图4可以看出：主车1在L1阶段时，车速、加速度和横摆角速度在状态约束范围内有效行驶，并设计出了L1阶段的行驶轨迹；当主车1在L2阶段时，随着车速、加速度和横摆角速度等车辆状态的临界约束条件的变化，L2阶段的行驶轨迹也随着变化。主车1在L1和L2阶段的车辆状态临界约束条件和起始、终了条件的变化以及在两个阶段的起始和终了状态约束的充分链接性，保证主车1在L1和L2阶段以不同的状态行驶，并能根据道路环境调整行驶状态约束，以产生合适的行驶路径。因此，当主车1在容易发生交通事故区域等特定的交通环境行驶时，多阶段路径优化有助于主车1在每一阶段调整合适的行驶状态及其临界约束条件，保证车辆在越来越临近特定的行驶环境时，能实现优化控制在全局上的最优，进而实现车辆对道路前方的有效感知。

（2）其次，考虑主车1在较高行驶速度环境下的状态，其仿真数据约束条件如下。

主车1在L1阶段的起始条件：x10＝10 m，y10＝0.1 m，θ10＝0.2 rad，v10＝15 m／s，γ10＝0.2 rad／s。

主车1在L1阶段的终了条件：x1f＝100 m，y1f＝0，θ1f＝0，v1f＝20 m／s，γ1f＝0。

主车1在 L1段各状态的约束：x∈［0，200］，y∈［0，3.5］，θ∈［-3π，3π］，v∈［15，25］，γ∈［-3π，3π］，a∈［-3，3］，w∈［-0.3，0.3］。

计算自动驾驶经过L2段距离中的状态，其仿真数据约束条件如下。

主车1在L2阶段的起始条件：x20＝100 m，y20＝0，θ20＝0，v20＝20 m／s，γ20＝0。

主车1在L2段各状态的约束：x2f＝250 m，y2f＝0.2 m，θ2f＝0，v2f＝25 m／s，γ2f＝0。

主车1在 L2段各状态的约束：x∈［0，200］，y∈［0，3］，θ∈［-3π，3π］，v∈［20，30］，γ∈［-3π，3π］，a∈［-3，3］，w∈［-0.3，0.3］。

联立式（3）、式（6）、式（7）、式（9）和式（16），可建立自动驾驶车辆的优化模型，其结果如图5和图6所示。

图5 L1和L2阶段的轨迹和速度

从图5中可以看出：当主车1以较高速度行驶时，其行驶轨迹和车辆状态在不同阶段实时变化；在L1阶段，车速以匀加速增加，直到轨迹达到L1段距离的末端约束；在L2阶段，车辆的初始位置从L1阶段的终点开始，且其初始条件与L1阶段的终了条件相同，主车1在L2阶段可实时调整其行驶轨迹，以适应具体的行驶环境。从图6中可以看出，主车1的加速度和横摆角速度随着不同阶段路径的变化而产生较好的效果，保证自动驾驶在对特定的行驶环境感知时，能实时调节其行驶状态，进而实现车辆对道路前方的有效感知。

4 结论

本文中以自动驾驶车辆的多阶段轨迹优化为目标，建立了车辆动力学模型和环境模型，在满足车辆的速度、加速度和横摆角速度等状态约束以及状态的起始和终了条件下，采用高斯伪谱法进行自动驾驶车辆的多阶段轨迹优化问题求解，将自动驾驶车辆的轨迹规划问题转化成优化问题。从本文研究中可以看出，在对多阶段轨迹优化过程中，高斯伪谱法可根据行驶环境设定具体的车辆状态约束及状态的起始、终了条件，进而实现自动驾驶车辆在全局求解性能上的最优，保证自动驾驶车辆对前方道路环境的实时性有效感知。该方法计算精度高、求解速度快，具有较强的鲁棒性，但仍存在一些挑战，例如伪谱法在节点上的插值部分可能出现不满足的情况。

图6 L1和L2阶段的车辆状态