车辆的轨迹规划任务是指在车辆的起始时刻运动状态与终止时刻运动状态之间计算出符合约束条件的行驶轨迹，其中约束条件主要体现在车辆内在的运动能力限制。另一方面，满足上述约束条件的轨迹往往不止一条，应通过某一指标式筛选优质的轨迹作为最终结果。由于轨迹规划任务中存在硬性约束条件和用于寻优的指标式，因此适合通过建立最优控制表达式来进行轨迹规划。求解连续最优控制问题，即求取控制变量 u ( t ) 并确定终止时刻 t f ，使得代价函数如下所示：

J = ϕ ( x ( 0 ) , u ( t f ) , t f ) + ∫ 0 t f L ( x ( t ) , u ( t ) , t ) d t (1)

沿着相应的状态变量所形成的集合 x ( t ) 取得最小值。其中 x ( t ) 产生制约的因素包括：

车辆运动学约束：

d x ( t ) d t = f ( x ( t ) , u ( t ) , t ) ,   t ∈ [ 0 , t f ] (2)

起始点约束：

φ ( t f , x ( 0 ) , x ( t f ) , u ( 0 ) , u ( t f ) ) = 0 (3)

车辆速度及加速度约束：

g ( x ( t ) , u ( t ) , t ) ≤ 0 ,   t ∈ [ 0 , t f ] (4)

车辆在车速不高的工况下，应用车辆运动学模型已能够符合实际需求，将车辆的前后轮分别向车体纵轴方向合并为虚拟单轮，通过确定虚拟前轮的转动角速度以及虚拟后轮的线加速度变量，可以以间接确定车辆的前轮转角、行驶速度等，进而实现车辆运动。具体车辆运动学模型如图1所示：

图1. 车辆运动学模型

将自动驾驶车辆称为车辆i。根据运动学模型，车辆i在惯性坐标系中的运动过程受到以下微分方程组的限制：

d d t [ x i ( t ) y i ( t ) θ i ( t ) v i ( t ) ϕ i ( t ) ] = [ v i ( t ) cos θ i ( t ) v i ( t ) sin θ i ( t ) v i ( t ) tan ϕ i ( t ) / L w a i ( t ) ω i ( t ) ] (5)

式中， t ∈ [ 0 , t f ] ； ( x i ( t ) , y i ( t ) ) 代表车辆的后轮轴中点坐标； v i ( t ) 及 a i ( t ) 分别代表车体纵轴方向的速度及加速度，以使车辆前进的方向为正方向； ϕ i ( t ) 为车辆前轮偏转角； ω i ( t ) 为前轮偏转角速度； θ i ( t ) 代表车辆姿态角， L f 为车辆前悬距离， L r 为后悬距离， L w 为前后轮轴距， L b 为车宽。

在车辆运动的起始时刻 t = 0 ，指定车辆所处的运动状态。以车辆i为例，则有

[ v i ( 0 ) , ϕ i ( 0 ) , x i ( 0 ) , y i ( 0 ) , θ i ( 0 ) ] = [ v 0 i , p 0 i , x 0 i , y 0 i , θ 0 i ] (6)

式中， [ v 0 i , p 0 i , x 0 i , y 0 i , θ 0 i ] 对应着车辆的运动状态信息。

在车辆运动的终止时刻，对车辆运动状态进行限制，车辆 在终止时刻停止可描述为：

v i ( t f ) = 0 (7)

a i ( t f ) = 0 (8)

ω i ( t f ) = 0 (9)

ϕ i ( t f ) = 0 (10)

车辆在运动过程中，它们能够将状态、控制变量限制在一定区间内，一般包括：

| ϕ ( t ) | ≤ Φ max (11)

| a i ( t ) | ≤ a max (12)

| v i ( t ) | ≤ v max (13)

| ω i ( t ) | ≤ Ω max (14)

式中， Φ max 、 a max 、 v max 、 Ω max 分别为各状态、控制变量的最值。 Φ max 代表车辆前轮转角 ϕ i ( t ) 的偏转角度最值； v max 是车辆在低速场景中的最大安全行驶速度； a max 与 Ω max 分别为线加速度、前轮转角速度设置的最值。

满足车辆轨迹规划任务约束条件的轨迹不止一条，因此还需要设计特定的代价函数求解出最优的轨迹，可通过代价函数来进行最优轨迹的选取。

J = w 1 J 1 + w 2 J 2 (15)

式中 w 1 、 w 2 为各性能指标函数对应的权重函数，相对较大的权重表示在整个代价函数中该部分所占的比重更大。

为解决最优控制问题式中的代价函数式包含不利于统一求解的积分项部分，引入新的状态变量将积分型性能指标统一转化为末值型性能指标，因此最优控制问题式则可进一步抽象为以下形式：

min J ( x ( t f ) , u ( t f ) ) (16)

d x d t = F ( x ( t ) , u ( t ) ) (17)

G ( x ( t ) , u ( t ) ) ≤ 0 ,   t ∈ [ 0 , t f ] (18)

式中， x ( t ) 代表被微分的状态变量； u ( t ) 代表控制变量；J为代价函数；F代表常微分方程组中的代数函数部分；G包含了车辆起始点约束及车辆速度及加速度约束； t f 代表动态过程的终止时刻。求解最优控制问题时，需在满足约束条件的控制变量 u ( t ) 以及 t f 的前提下，使得设计的代价函数最小，从而实现最优命题的求解。

实验采用直接法将最优控制命题中的连续变量全部离散化，将其转化为非线性规划问题，通过求解非线性规划问题的数值最优解可间接得到最优控制命题的最优解。

全联立正交配置有限元法是用配置点上的插值函数来逼近原问题，在配置点上无离散化误差拉格朗日插值函数用于数值逼近，由于高阶插值会导致龙格现象，因此分段低阶插值是常用方式，通过选择正交配置点可使拉格朗日插值提高求解精度。同时将最优控制命题中的状态变量以及控制变量进行离散化，将其全部视为决策变量来进行求解，具体方法如下。

将时域 [ 0 , t f ] 划分为 N f e 段时长相等的有限元 [ t i − 1 , t i ] ：

t i − t i − 1 = t f N f e ,   i = 1 , ⋯ , N f e (19)

在有限元 [ t i − 1 , t i ] 上，采用拉格朗日插值函数形式对各变量进行描述，其中K取3进行计算。

x ( t ) = ∑ j = 0 K     l x j ( τ ) x i , j (20)

u ( t ) = ∑ j = 1 K     l u j ( τ ) u i , j (21)

t = t i − 1 + ( t i − t i − 1 ) τ ,   τ ∈ [ 0 , 1 ] (22)

式中， l x j ( τ ) 为微分变量 x ( t ) 的拉格朗日基函数； x i , j 为微分变量的离散配置点； l u j ( τ ) 为非微分变量 u ( t ) 对应的拉格朗日基函数； u i , j 为非微分变量 u ( t ) 的离散配置点；其中拉格朗日基函数 l x j ( τ ) 、 l u j ( τ ) 应满足以下关系：

l x j ( τ ) = ∏ k = 0 , k ≠ j K τ − τ k τ j − τ k (23)

l x j ( τ ) = { 1 ,     k = j 0 ,   k ≠ j (24)

l u j ( τ ) = ∏ k = 1 , k ≠ j K τ − τ k τ j − τ k (25)

l u j ( τ ) = { 1 ,     k = j 0 ,   k ≠ j (26)

拉格朗日插值多项式的具有一定的优势，其优势在于变量在各个配置点上的值恰好与其系数相等，如下式所示：

t i , j = t i − 1 + ( t i − t i − 1 ) τ j (27)

x ( t i , j ) = x i , j (28)

u ( t i , j ) = u i , j (29)

由于状态变量可导，所以相邻有限元连接处的节点上状态变量值也应该连续，故有下面的连续性条件：

x i + 1 , 0 = x i , k = ∑ j = 0 K     l x j ( 1 ) x i , j ,   j = 1 , ⋯ , N f e − 1 (30)

完整的非线性规划问题如下：

min J ( t f ) (31)

∑ k = 0 K ( d ( ∏ m = 0 , m ≠ j K τ − τ m τ j − τ m ) d τ | τ = τ j x i , k ) = ( t i − t i − 1 ) F ( x i , j , u i , j ) (32)

G ( x i , j , u i , j ) ≤ 0 (33)

t i − t i − 1 = t f N f e ,   t 0 = 0 ,   t N f e = t f (34)

i = 1 , ⋯ , N f e ;     j = 0 , ⋯ , K (35)

求解各有限元配置点 x i , j 、 u i , j 、 t f ，使得代价函数取得极小值，即完成非线性规划问题的求解。

为了验证算法的有效性，在Matlab平台下进行仿真实验。设计代价函数如下：

J = 10 t f + ∫ 0 t f ω i 2 ( t ) d t (36)

选择车辆运动前轮转角角速度作为控制变量的代表性变量，选择车辆运动速度作为状态变量的代表性变量。将上述轨迹规划命题在Matlab中进行输出，可得到车辆运动前轮转角角速度与运动时间曲线、车辆运动速度与运动时间曲线以及车辆行驶轨迹曲线。从车辆运动前轮转角角速度曲线当中可以看出，角速度变化率相对较大，车辆泊车活动过程中平顺性相对较差，仿真实验结果如图2所示。

图2. 参数优化前的车辆运动速度曲线

上述实验中车辆运动前轮转角角速度变化较大，超出了边界值。对代价函数进行参数优化，调大车辆运动前轮转角角速度权重系数，如下所示。

J = 10 t f + 10 ∫ 0 t f ω i 2 ( t ) d t (37)

对代价函数参数进行优化后的车辆运动前轮转角角速度与运动时间曲线可以得出，车辆运动前轮转角角速度变化率相对较小，车辆运动平顺性相对较好，有利于增加驾驶员或乘客的舒适性，仿真实验结果如图3所示。

图3. 参数优化后的车辆运动速度曲线