CN111845774B - 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法 - Google Patents

Abstract

本发明涉及一种基于横纵向协调的自动驾驶汽车轨迹动态规划及跟踪方法，首先在车辆行驶的Frenet空间中对运动状态进行密集采样，完成横、纵向的局部轨迹重规划。在横纵向轨迹协调规划中，考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点，引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，并设计轨迹的平顺性、舒适性、行使效率等轨迹评价指标体系对轨迹质量进行评估。在轨迹跟踪阶段，采用Brush轮胎模型设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

Description

一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪 方法

技术领域

本发明属于智能交通自动驾驶汽车领域，具体涉及一种基于横纵向协调的自动驾驶汽车轨迹动态规划及跟踪方法。

背景技术

自动驾驶车辆作为智能交通系统的重要组成环节，把传统车辆驾驶过程中“人-车-环境”的闭环变成了“车-环境”系统，通过赋予汽车足够的智能，使车辆部分地乃至完全地取代人类驾驶者的操作，避免了人类驾驶者的操作失误，可有效提高交通系统的效率和安全性，自动驾驶车辆的研发已经逐渐受到国内外各大汽车厂商和研究机构的重视。

自动驾驶汽车通过摄像头、雷达等外部传感器感知与识别周围环境并做出相应的决策规划反应，再利用先进的硬件设备和电子信息技术控制车辆的自动运行。轨迹规划算法是自动驾驶技术的关键之一，与人类正常驾驶的汽车一样，自动驾驶汽车的轨迹规划主要是拟规划出一条从起始位置抵达目标位置，能够避开障碍物的安全、可靠可行路径，其性能的好坏将直接影响汽车的行驶安全性和无人驾驶汽车的智能化程度。目前存在的轨迹规划算法大致可以分为图搜索、数值优化和采样等三类；大多数研究集中于前两类轨迹规划方法。事实上,基于图搜索的方法将环境空间映射到加权图上从而将避障路径规划转换为图搜索问题，其计算时间会随着地图规模的增大快速增长。基于数值优化的算法是把目标函数和约束条件转化成凸优化形式，然后使用数值优化迭代器计算出其最优解，该方法最大的优点在于最优解空间是连续的，然而要将周围环境抽象成凸优化的目标函数这一条件难以满足，同时求解最优解往往依赖于计算开销较大的梯度迭代；这两类方法难以满足自动驾驶的高实时性要求。

另外，目前自动驾驶车辆的轨迹规划方法多针对横向运动规划的静态障碍物场景，但在自动驾驶实际行驶场景复杂，会随机存在临时障碍物以及动态障碍物，基于静态障碍物场景的单一横向轨迹规划难以满足自动驾驶的高可靠性要求，如何在综合交通环境中规划出能应对动、静态障碍物的横纵向协调的自动驾驶汽车动态轨迹，且满足无人驾驶车辆行驶的安全性、舒适性和通行效率等条件。同时，规划所得的轨迹跟踪还需要考虑自动驾驶车辆动力学操纵极限约束，保证其可执行性，也是应该对待的重要问题。

发明内容

本发明的目的是针对已有技术的不足，提供一种基于横纵向协调的自动驾驶汽车轨迹动态规划及跟踪方法，首先进行车辆轨迹Frenet坐标映射，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划，然后引入超平面理论进行障碍物碰撞检测，综合考虑横纵向协调的轨迹并对轨迹的平顺性、舒适性、行使效率进行评估，在设计轨迹操纵动力学极限约束规则下采用AFS+DYC集成系统对轨迹进行MPC跟踪控制，具有高实时、安全、可靠的优点。

为了实现上述目的，本发明提供如下方案：

一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，包括如下步骤：

S1、车辆行驶状态空间轨迹采样：

将自动驾驶车辆轨迹规划问题从笛卡尔坐标系映射至Frenet坐标系下进行，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划；

S2、横纵向轨迹协调规划与车辆轨迹评估：

在横纵向轨迹协调规划中，考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物的形状、大小、朝向，引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，并设计轨迹的平顺性、舒适性、行使效率轨迹评价指标体系对轨迹质量进行评估；

S3、车辆操纵极限约束下的轨迹跟踪控制：

在轨迹跟踪阶段，为保证自动驾驶车辆在自身物理条件限制之内轨迹跟踪行驶可达，采用Brush轮胎模型设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

优选地，一种基于横纵向协调的自动驾驶汽车轨迹动态规划及跟踪方法，包括如下步骤：

(1)车辆行驶状态空间轨迹采样：

将自动驾驶车辆轨迹位置从笛卡尔坐标系映射至Frenet坐标系，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划。

优选地，上述步骤(1)中，所建立的将自动驾驶车辆轨迹位置从笛卡尔坐标系[n_x,t_x]映射至Frenet坐标系[n_c,t_c]转换关系表达式如下：

x(s(t),d(t))＝r(s(t))+d(s)n_c(s(t))

其中，计算主车的横向运动速度v_lat和纵向运动速度v_lon，表达式为：

计算主车的横向运动加速度a_lat和纵向运动加速度a_lon，表达式为：

上述步骤(1)中，所通过采样得横向规划轨迹簇轨迹集合为：

d_set＝Ψ(t,d₀,d₁),t＝{t₀,t₁,t₂,…,t_N-1}

式中，Ψ(t,d₀,d₁)代表所有横向规划所得的轨迹集合，t表示N个时刻采样点，Ψ(t_i,d₀,d₁),0≤i≤N-1表示某一条状态确定的横向运动轨迹，车辆按该轨迹行驶，将使其从初始状态d₀转移至末状态d₁。

在横向轨迹规划中使用的五次多项式表达式为：

d(t)＝c_d1+c_d2t+c_d3t²+c_d4t³+c_d5t⁴+c_d6t⁵

横向规划轨迹求解中，其可以利用获取车辆的当前横向运动状态

和根据配置的规划时域末端运动状态

求解，求解的表达式如下：

d(t₀)＝c_d1+c_d2t₀+c_d3t₀ ²+c_d4t₀ ³+c_d5t₀ ⁴+c_d6t₀ ⁵

d(t₁)＝c_d1+c_d2t₁+c_d3t₁ ²+c_d4t₁ ³+c_d5t₁ ⁴+c_d6t₁ ⁵

求解矩阵条件及方式如下：

C＝M^-1O

其中C＝[c_d1,c_d2,c_d3,c_d4,c_d5,c_d6]^T

车辆在横向规划时间段内的规划轨迹求解结果表达式为：

通过令t₀＝0,简化方程组的求解，可直接求得

令T＝t₁-t₀，则剩下的三个系数可以通过求解如下矩阵方程得到：

优选地，上述步骤(1)中，所通过采样得纵向规划轨迹簇轨迹集合为：

s_set＝Φ(t,s₀,s₁),t＝{t₀,t₁,t₂,…,t_N-1}

式中，Φ(t,s₀,s₁)代表所有纵向规划所得的轨迹集合，t表示N个时刻采样点，Φ(t_i,s₀,s₁),0≤i≤N-1表示某一条状态确定的纵向运动轨迹，车辆按该轨迹行驶，将使其从初始状态s₀转移至末状态s₁。

在纵向轨迹规划使用的是四次多项式轨迹表达式为：

s(t)＝c_s1+c_s2t+c_s3t²+c_s4t³+c_s5t⁴

纵向规划轨迹求解中，利用获取车辆的当前纵向运动状态

其配置的规划时域末端运动状态在高速环境采用速度跟随模式中使用

在低速环境中车距跟随模式下使用

对规划轨迹进行求解的表达式为：

令T＝t₁-t₀，求解矩阵方程就可解得四次多项式曲线的五个系数。

将具有相同规划时域和插值点的横、纵向轨迹曲线按时刻点t_i∈[t₀,t₁],i＝1,2,...,N一一对应组合，得到[s(t_i),d(t_i)]，再根据转换关系表达式完成从Frenet坐标系到笛卡尔坐标系的映射及轨迹合成。

(2)横纵向轨迹协调规划与车辆轨迹评估：

在横纵向轨迹协调规划中，设置轨迹曲线的平顺性代价、安全性代价、行驶效率等代价对规划轨迹质量进行评价，并引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，选择最优规划轨迹用于底层的跟踪参考。

优选地，上述步骤(2)中，所设计的横纵向轨迹协调规划过程为：

评价横向规划轨迹的指标设置包括以下的两项：

其中k_j为权重系数，此项意在使横向规划轨迹jerk最小，保证舒适性。

C_e＝k_e(d(t))²

其中k_e为权重系数，此项意在使横向规划接近参考车道中心线，即让主车完成避障后能够回到初始参考车道行驶。

评价纵向规划轨迹的指标设置包括以下的三项：

其中k_j为权重系数，与横向轨迹的该指标一样，此项意在使纵向规划轨迹jerk最小，保证舒适性。

其中k_e为权重系数，v_target为目标车速，在车速跟随模式下需要使车速接近并保持目标速度时，纵向轨迹的末端速度参考采用上式所示的表达式。

其中s_target为目标距离，在车距跟随模式下需要使主车与移动的交通车辆保持一定的车距时，纵向轨迹的末端距离采用如上所示的表达式。

在对横纵向轨迹规划进行单独初步评价后，进一步设计评价横、纵向合成协调轨迹的指标包括以下的四项：

优选地，考虑车辆及环境静、动态障碍物都不能简单地等效为质点，它们的形状、大小、朝向等均可能给行驶带来风险，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物，引入超平面理论进一步确定这些车辆轨迹是否与周围交通环境中的静态或者动态的障碍物发生干涉及是否有发生碰撞风险如下：

定义车辆与障碍物不发生干涉及不发生碰撞风险的条件为

其中dist表示轨迹距离障碍物的最小距离，H(τ)为自动驾驶车辆的轨迹坐标，

为静态或者动态障碍物的圆中心位置，μ为静态或动态的障碍物与车辆干涉的安全因子,m为障碍物数。

优选引入轨迹避碰的超平面为

则自动驾驶车辆矩形顶点可以描述为

其中，T(τ)为坐标旋转矩阵，定义为

其中，

θ(τ)为自动驾驶车辆朝向。

进一步推导超平面中的车辆与障碍物不发生干涉及不发生碰撞风险的条件得

其中，

预测的为静态或者动态障碍物的圆中心位置，

为静态或者动态障碍物的圆半径。

定义横纵向协调轨迹的行驶效率指标为

C_t＝k_tT_i

其中k_t为权重系数，此项为轨迹覆盖的时间，与行驶效率有关。

定义横纵向协调轨迹的舒适性指标为

其中

其中k_a为权重系数，此项描述了规划轨迹的合加度，与舒适性有关。

定义横纵向协调轨迹的平顺性指标为

其中k_k为权重系数，此项描述了转换至大地坐标系下的合成轨迹曲率，与行驶平顺性有关。

(3)车辆操纵极限约束下的轨迹跟踪控制：

在轨迹跟踪阶段，采用Brush轮胎模型等设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

优选地，上述步骤(3)中，所设计的轨迹跟踪过程为：

在设计车辆跟踪动力学操纵极限物理约束规则时，用于描述车辆物理特征的车辆动力学模型表达式如下：

式中，

为车身坐标系下的横向加速度，

为车身纵坐标下的纵向加速度，

为横摆角，

为横摆角速度，

为横摆角加速度，

为车辆的纵向速度，

为车辆的横向速度，δ_f为前轮转角，M_z为直接横摆力矩，a,b分别为前、后轴到质心的距离，I_z为车辆的转动惯量，

为全局坐标系下的纵向速度，

为全局坐标系下的横向速度，X为全局坐标系下的纵向位置，Y为全局坐标系下的横向位置，m为车辆的总质量。

采用的Brush轮胎模型刻画运动的自动驾驶车辆与地面交互作用及路面附着约束如下如下

上式中

其中前后轮胎侧偏角为：

上述式，μ为地面附着系数,κ_i为滑移率，C_α,C_x分别为轮胎侧偏、纵向刚度，F_z,i为轮胎的垂直载荷。

采用稳态车辆质心侧偏角与横摆角速度来设计自动驾驶车辆动力学操纵极限，首先假设在转弯过程中纵向力作用很小，则稳态横摆角速度满足

为改善驾驶员前轮转向角输入激励下的车辆横摆角速度的瞬态响应，理想的横摆角速度极限修正为

同时，自动驾驶车辆动力学操纵极限严重受制于地面条件约束，导致的轮胎-路面交互作用的饱和效应，则后轮Brush轮胎模型产生临界侧向侧偏角进一步推导得

进一步可以推导车辆行使过程的横向速度极限为

其中，η为制动的依赖系数，可以表示为

利用模型预测控制(MPC)原理完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制如下：

定义的自动驾驶AFS、DYC集成跟踪控制系统的状态向量

定义第输入向量分别为u＝[δ_f,M_z]

因此用于车辆路径跟踪的预测模型表达式写作：

其中

定义模型预测控制的最优化问题的代价函数设计为：

其中，J(ξ(t),u(t-1),ΔU(t))为优化指标，Δu(t+i|t)，t＝0,1,...,N_c-1为输入控制增量向量，N_p为预测时域，N_c为控制时域，η_r(t+i|t),i＝1,2,...,N_p为参考输出量，Q,R,ρ分别为权重矩阵。

模型预测控制的约束条件设计为：

ΔU_min≤ΔU(t)≤ΔU_max

U_min-U(t-1)≤MΔU(t)≤U_max-U(t-1)

0<ε<ε_max

上述式中，Y_cmin(t),Y_cmax(t)分别为硬约束输出量的下限和上限，Y_scmin(t),Y_scmax(t)分别为软约束输出量的下限和上限，Ξ＝ε1_pn，其中1_pn为p_u维列向量。

在当前时刻，求解上述优化问题得到最优控制增量序列，将控制增量序列的第一个量与上一时刻的控制量累积，得到实际控制量，作为被控系统的输入。舍弃除第一个控制增量以外的序列中的其他控制增量。下一个优化时刻到来时，重复以上过程，实现滚动优化。在下一个规划周期更新车辆的初状态，重复步骤(1)至步骤(3)的操作，实现实时重规划与主动转向及直接横摆力矩集成跟踪控制。

本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著的技术进步：

1.本发明将自动驾驶车辆轨迹规划问题由笛卡尔坐标系映射至Frenet坐标系下进行，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划，能满足自动驾驶的高实时性要求；

2.在横纵向轨迹协调规划中，本发明考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物的形状、大小、朝向，引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，并设计轨迹的平顺性、舒适性、行使效率等轨迹评价指标体系对轨迹质量进行评估；

3.本发明在轨迹跟踪阶段，为保证自动驾驶车辆在自身物理条件限制之内轨迹跟踪行驶可达，采用Brush轮胎模型等设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

附图说明

图1是本发明一种基于横纵向协调的自动驾驶汽车动态轨迹规划方法的算法流程图。

图2是本发明中采用的Frenet坐标映射及跟踪控制动力学模型示意图。

图3是本发明的一种变道场景采样规划结果轨迹图。

具体实施方式

下面结合附图和优选实施例对本发明详细说明：

实施例一：

参见图1，一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，步骤如下：

S1、车辆行驶状态空间轨迹采样：

将自动驾驶车辆轨迹规划问题从笛卡尔坐标系映射至Frenet坐标系下进行，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划。

S2、横纵向轨迹协调规划与车辆轨迹评估：

在横纵向轨迹协调规划中，考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物的形状、大小、朝向，引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，并设计轨迹的平顺性、舒适性、行使效率轨迹评价指标体系对轨迹质量进行评估。

S3、车辆操纵极限约束下的轨迹跟踪控制：

在轨迹跟踪阶段，为保证自动驾驶车辆在自身物理条件限制之内轨迹跟踪行驶可达，采用Brush轮胎模型等设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

实施例二：

本实施例与实施一基本相同，特别之处在于：

如图1所示，一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，包括以下步骤：

1.车辆行驶状态空间轨迹采样：

将车辆位置从笛卡尔坐标系映射至Frenet坐标系，具体包括以下步骤：

全局地图信息包括车道路径点坐标(x_i,y_i),i∈N，自动驾驶主车的位置(x₀,y₀)，车速v₀，横摆角

加速度a₀，横摆角速度

计算主车的横向运动速度v_lat和纵向运动速度v_lon，表达式为：

计算主车的横向运动加速度a_lat和纵向运动加速度a_lon，表达式为：

找出全局坐标点(x_i,y_i),i∈N中与主车的位置坐标(x₀,y₀)最近的点作为自动驾驶主车的全局定位点。将在笛卡尔坐标系中的信息映射至Frenet坐标系中，如图2所示，笛卡尔坐标系[n_x,t_x]到Frenet坐标系[n_c,t_c]的转换关系为：

x(s(t),d(t))＝r(s(t))+d(s)n_c(s(t))

2.横纵向轨迹协调规划与车辆轨迹评估：

根据交通环境和行驶需要，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划。具体包括：

在横向规划时域中配置末端运动位置d(t₁)，运动速度

运动加速度

一般地，将

和

设为0，这样可使得最终的规划轨迹末端朝向与车道平行，有利于行驶；在纵向规划时域中配置末端运动位置s(t₁)，运动速度

运动加速度

利用规划时域内车辆的初状态和采样所得的末状态计算多项式曲线系数，用时刻点插值分别完成横、纵向轨迹的规划，并进行轨迹合成。具体步骤为：

在横向规划中，横向轨迹使用五次多项式描述，其表达式为：

d(t)＝c_d1+c_d2t+c_d3t²+c_d4t³+c_d5t⁴+c_d6t⁵

获取车辆的当前横向运动状态

再根据配置的规划时域末端运动状态

代入式可以对规划轨迹进行求解，具体为：

d(t₀)＝c_d1+c_d2t₀+c_d3t₀ ²+c_d4t₀ ³+c_d5t₀ ⁴+c_d6t₀ ⁵

d(t₁)＝c_d1+c_d2t₁+c_d3t₁ ²+c_d4t₁ ³+c_d5t₁ ⁴+c_d6t₁ ⁵

将式转换为矩阵形式，令：

则有：

C＝M^-1O

其中，C＝[c_d1,c_d2,c_d3,c_d4,c_d5,c_d6]^T,可求解得到无人驾驶车辆在规划时间段内的规划轨迹表达式。

通过令t₀＝0,简化方程组的求解，可直接求得

令T＝t₁-t₀，则剩下的三个系数可以通过求解如下矩阵方程得到：

通过采样得横向规划轨迹簇轨迹集合为：

d_set＝Ψ(t,d₀,d₁),t＝{t₀,t₁,t₂,…,t_N-1}

式中，Ψ(t,d₀,d₁)代表所有纵向规划所得的轨迹集合，t表示N个时刻采样点，Ψ(t_i,d₀,d₁),0≤i≤N-1表示某一条状态确定的纵向运动轨迹，车辆按该轨迹行驶，将使其从初始状态d₀转移至末状态d₁。

纵向轨迹规划使用的是四次多项式描述其轨迹，其表达式为：

s(t)＝c_s1+c_s2t+c_s3t²+c_s4t³+c_s5t⁴

获取车辆的当前纵向运动状态

步骤S2中配置的规划时域末端纵向运动状态

的使用与横向不同，在速度跟随模式中使用

在车距跟随模式下使用

可以对规划轨迹进行求解，具体表达式为：

令T＝t₁-t₀，求解矩阵方程就可解得四次多项式曲线的五个系数。

通过采样得纵向规划所得轨迹集合为：

s_set＝Φ(t,s₀,s₁),t＝{t₀,t₁,t₂,…,t_N-1}

式中，Φ(t,s₀,s₁)代表所有横向规划所得的轨迹集合，t表示N个时刻采样点，Φ(t_i,s₀,s₁),0≤i≤N-1表示某一条状态确定的横向运动轨迹，车辆按该轨迹行驶，将使其从初始状态s₀转移至末状态s₁。

将具有相同规划时域和插值点的横、纵向轨迹曲线按时刻点t_i∈[t₀,t₁],i＝1,2,...,N一一对应组合，得到[s(t_i),d(t_i)]，再根据步骤1中转换关系表达式完成从Frenet坐标系到笛卡尔坐标系的映射及轨迹合成。

设置轨迹曲线的平顺性代价、安全性代价、行驶效率等代价对规划轨迹质量进行评价，并校核轨迹的碰撞的风险，选择最优规划轨迹用于底层的跟踪参考，具体的轨迹代价包括：

评价横向规划轨迹的指标设置包括以下的两项：

其中k_j为权重系数，此项意在使横向规划轨迹jerk最小，保证舒适性。

C_e＝k_e(d(t))²

其中k_e为权重系数，此项意在使横向规划接近参考车道中心线，即让主车完成避障后能够回到初始参考车道行驶。

评价纵向规划轨迹的指标设置包括以下的三项：

其中k_j为权重系数，与横向轨迹的该指标一样，此项意在使纵向规划轨迹jerk最小，保证舒适性。

其中k_e为权重系数，v_target为目标车速，在车速跟随模式下需要使车速接近并保持目标速度时，纵向轨迹的末端速度参考采用上式所示的表达式。

其中s_target为目标距离，在车距跟随模式下需要使主车与移动的交通车辆保持一定的车距时，纵向轨迹的末端距离采用如上所示的表达式。

在对横纵向轨迹规划进行单独初步评价后，进一步设计评价横、纵向合成协调轨迹的指标包括以下的四项：

针对前面横向、纵向规划轨迹的指标设置生成的局部车辆运动参考轨迹簇，对规划轨迹进行碰撞检测以进一步保证安全性，本发明中采用了不同于其他轨迹规划算法中检测碰撞的方法，在本方案中，考虑车辆及环境静、动态障碍物都不能简单地等效为质点，它们的形状、大小、朝向等均可能给行驶带来风险，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物，引入超平面理论进一步确定这些车辆轨迹是否与周围交通环境中的静态或者动态的障碍物发生干涉及是否有发生碰撞风险如下：

定义车辆与障碍物不发生干涉及不发生碰撞风险的条件为

其中dist表示轨迹距离障碍物的最小距离，H(τ)为自动驾驶车辆的轨迹坐标，

为静态或者动态障碍物的圆中心位置，μ为静态或动态的障碍物与车辆干涉的安全因子,m为障碍物数。

进一步引入轨迹避碰的超平面为

则自动驾驶车辆矩形顶点可以描述为

其中，T(τ)为坐标旋转矩阵，定义为

其中，

θ(τ)为自动驾驶车辆朝向。

进一步推导超平面中的车辆与障碍物不发生干涉及不发生碰撞风险的条件得

其中，

预测的为静态或者动态障碍物的圆中心位置，

为静态或者动态障碍物的圆半径。

定义横纵向协调轨迹的行驶效率指标为

C_t＝k_tT_i

其中k_t为权重系数，此项为轨迹覆盖的时间，与行驶效率有关。

定义横纵向协调轨迹的舒适性指标为

其中

其中k_a为权重系数，此项描述了规划轨迹的合加度，与舒适性有关。

定义横纵向协调轨迹的平顺性指标为

其中k_k为权重系数，此项描述了转换至大地坐标系下的合成轨迹曲率，与行驶平顺性有关。

根据设计的轨迹评价指标，每条规划生成的备选轨迹都具有对应的评价分值，首先根据各备选轨迹的分值，对其升序排列后从第一条轨迹开始依次进行碰撞检测，通过检测则该轨迹成为最终的参考轨迹，不通过则删除该轨迹后检测次优的轨迹，如此循环直至通过。这样，就无需对每一条备选规划轨迹都进行碰撞检测，而仅检测其中的一部分，减小了计算量。

3.车辆操纵极限约束下的轨迹跟踪控制：

设计车辆跟踪动力学操纵极限物理约束规则，建立如图2所示的车辆动力学模型用于描述车辆物理特征，具体的车辆动力学模型表达式如下：

式中，

为车身坐标系下的横向加速度，

为车身纵坐标下的纵向加速度，

为横摆角，

为横摆角速度，

为横摆角加速度，

为车辆的纵向速度，

为车辆的横向速度，δ_f为前轮转角，M_z为直接横摆力矩，a,b分别为前、后轴到质心的距离，I_z为车辆的转动惯量，

为全局坐标系下的纵向速度，

为全局坐标系下的横向速度，X为全局坐标系下的纵向位置，Y为全局坐标系下的横向位置，m为车辆的总质量。

车辆的轨迹跟踪控制需要考虑车辆动力学操纵极限，以保证自动驾驶车辆在自身物理条件限制之内轨迹行驶可达。为刻画运动的自动驾驶车辆与地面交互作用及路面附着约束，采用Brush轮胎模型来建立轮胎模型如下

上式中

其中前后轮胎侧偏角为：

上述式，μ为地面附着系数,κ_i为滑移率，C_α,C_x分别为轮胎侧偏、纵向刚度，F_z,i为轮胎的垂直载荷。

进一步采用稳态车辆质心侧偏角与横摆角速度来推导自动驾驶车辆动力学操纵极限，首先假设在转弯过程中纵向力作用很小，则稳态横摆角速度满足

为改善驾驶员前轮转向角输入激励下的车辆横摆角速度的瞬态响应，理想的横摆角速度

极限修正为

同时，自动驾驶车辆动力学操纵极限严重受制于地面条件约束，导致的轮胎-路面交互作用的饱和效应，则后轮Brush轮胎模型产生临界侧向侧偏角进一步推导得

进一步可以推导车辆行使过程的横向速度极限为

其中，η为制动的依赖系数，可以表示为

利用模型预测控制(MPC)原理完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

定义自动驾驶AFS、DYC集成跟踪控制系统的状态向量

定义输入向量分别为u＝[δ_f,M_z]

假设自动驾驶AFS、DYC集成跟踪控制系统的某个工作点为[ξ₀,u₀]，

为始终施加控制量u₀后得到的系统状态量，存在如下关系：

将该非线性系统在任意点(ξ_r,u_r)处进行泰勒展开，仅保留一阶项，忽略高阶项，可以得到：

考虑到该状态表达式是连续的，为进一步模型预测控制器的设计，采用近似离散化方法对其进行离散化后可得到：

式中，k＝t,t+1,…,t+N-1。

根据之前定义状态向量如下：

因此用于车辆路径跟踪的预测模型表达式写作：

定义模型预测控制的最优化问题的代价函数设计为：

其中，J(ξ(t),u(t-1),ΔU(t))为优化指标，Δu(t+i|t)，t＝0,1,...,N_c-1为输入控制增量向量，N_p为预测时域，N_c为控制时域，η_r(t+i|t),i＝1,2,...,N_p为参考输出量，Q,R,ρ分别为权重矩阵。

模型预测控制的约束条件设计为：

ΔU_min≤ΔU(t)≤ΔU_max

U_min-U(t-1)≤MΔU(t)≤U_max-U(t-1)

0<ε<ε_max

上述式中，Y_cmin(t),Y_cmax(t)分别为硬约束输出量的下限和上限，Y_scmin(t),Y_scmax(t)分别为软约束输出量的下限和上限，Ξ＝ε1_pn，其中1_pn为p_u维列向量。

在当前时刻，求解上述优化问题得到最优控制增量序列，将控制增量序列的第一个量与上一时刻的控制量累积，得到实际控制量，作为被控系统的输入。舍弃除第一个控制增量以外的序列中的其他控制增量。下一个优化时刻到来时，重复以上过程，实现滚动优化。在下一个规划周期更新车辆的初状态，重复步骤1至步骤5的操作，实现实时重规划与主动转向及直接横摆力矩集成跟踪控制。根据具体实施方式进行仿真验证，仿真结果如图3所示：

综合上述实施例可知，本发明涉及一种基于横纵向协调的自动驾驶汽车轨迹动态规划及跟踪方法，首先将自动驾驶车辆轨迹规划问题从笛卡尔坐标系映射至Frenet坐标系下进行，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划。在横纵向轨迹协调规划中，考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点，分别采用移动的矩形及圆描述运行中的自动驾驶车辆及障碍物的形状、大小、朝向，引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，并设计轨迹的平顺性、舒适性、行使效率等轨迹评价指标体系对轨迹质量进行评估。在轨迹跟踪阶段，为保证自动驾驶车辆在自身物理条件限制之内轨迹跟踪行驶可达，采用Brush轮胎模型等设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。本发明方法能满足自动驾驶的高实时性要求，具有高实时、安全、可靠的优点。

上面对本发明实施例结合附图进行了说明，但本发明不限于上述实施例，还可以根据本发明的发明创造的目的做出多种变化，凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化，均应为等效的置换方式，只要符合本发明的发明目的，只要不背离本发明的技术原理和发明构思，都属于本发明的保护范围。

Claims (7)

1.一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，包括如下步骤：

S1、车辆行驶状态空间轨迹采样：

将自动驾驶车辆轨迹规划问题从笛卡尔坐标系映射至Frenet坐标系下进行，在车辆行驶的状态空间中对规划时域内车辆末端运动状态进行密集采样，分别利用五次和四次多项式曲线对车辆的状态转移过程进行插值完成横、纵向的局部轨迹重规划；

S2、横纵向轨迹协调规划与车辆轨迹评估：

在横纵向轨迹协调规划中，考虑复杂交通环境中车辆及环境中的静、动态障碍物都不能简单地等效为质点，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物的形状、大小、朝向，引入超平面理论评估车辆轨迹是否与周围交通环境中的静、动态障碍物发生干涉及发生碰撞的风险，并设计轨迹的平顺性、舒适性、行使效率轨迹评价指标体系对轨迹质量进行评估；

S3、车辆操纵极限约束下的轨迹跟踪控制：

在轨迹跟踪阶段，为保证自动驾驶车辆在自身物理条件限制之内轨迹跟踪行驶可达，采用Brush轮胎模型设计自动驾驶车辆动力学操纵极限约束规则，利用模型预测控制(MPC)完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制。

2.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，在所述步骤S1中，

将车辆位置从笛卡尔坐标系映射至Frenet坐标系，全局地图信息包括车道路径点坐标(x_i,y_i),i∈N，自动驾驶主车的位置(x₀,y₀)，车速v₀，横摆角

加速度a₀，横摆角速度

将自动驾驶车辆轨迹位置从笛卡尔坐标系[n_x,t_x]映射至Frenet坐标系[n_c,t_c]转换关系表达式如下：

x(s(t),d(t))＝r(s(t))+d(s)n_c(s(t))

其中，计算主车的横向运动速度v_lat和纵向运动速度v_lon，表达式为：

计算主车的横向运动加速度a_lat和纵向运动加速度a_lon，表达式为：

找出全局坐标点(x_i,y_i),i∈N中与主车的位置坐标(x₀,y₀)最近的点作为自动驾驶主车的全局定位点。将在笛卡尔坐标系中的信息映射至Frenet坐标系中，笛卡尔坐标系[n_x,t_x]到Frenet坐标系[n_c,t_c]的转换关系为：

x(s(t),d(t))＝r(s(t))+d(s)n_c(s(t))。

3.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，在所述步骤S1中，通过采样得横向规划轨迹簇轨迹集合为：

d_set＝Ψ(t,d₀,d₁),t＝{t₀,t₁,t₂,…,t_N-1}

式中，Ψ(t,d₀,d₁)代表所有横向规划所得的轨迹集合，t表示N个时刻采样点，Ψ(t_i,d₀,d₁),0≤i≤N-1表示某一条状态确定的横向运动轨迹，车辆按该轨迹行驶，将使其从初始状态d₀转移至末状态d₁；

在横向轨迹规划中使用的五次多项式表达式为：

d(t)＝c_d1+c_d2t+c_d3t²+c_d4t³+c_d5t⁴+c_d6t⁵

横向规划轨迹求解中，利用获取车辆的当前横向运动状态

和根据配置的规划时域末端运动状态

求解，求解的表达式如下：

d(t₀)＝c_d1+c_d2t₀+c_d3t₀ ²+c_d4t₀ ³+c_d5t₀ ⁴+c_d6t₀ ⁵

d(t₁)＝c_d1+c_d2t₁+c_d3t₁ ²+c_d4t₁ ³+c_d5t₁ ⁴+c_d6t₁ ⁵

求解矩阵条件及方式如下：

C＝M^-1O

其中C＝[c_d1,c_d2,c_d3,c_d4,c_d5,c_d6]^T

车辆在横向规划时间段内的规划轨迹求解结果表达式为：

4.根据权利要求2所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，在所述步骤S1中，通过采样得纵向规划轨迹簇轨迹集合为：

s_set＝Φ(t,s₀,s₁),t＝{t₀,t₁,t₂,…,t_N-1}

式中，Φ(t,s₀,s₁)代表所有纵向规划所得的轨迹集合，t表示N个时刻采样点，Φ(t_i,s₀,s₁),0≤i≤N-1表示某一条状态确定的纵向运动轨迹，车辆按该轨迹行驶，将使其从初始状态s₀转移至末状态s₁；

在纵向轨迹规划使用的是四次多项式轨迹表达式为：

s(t)＝c_s1+c_s2t+c_s3t²+c_s4t³+c_s5t⁴

纵向规划轨迹求解中，利用获取车辆的当前纵向运动状态

其配置的规划时域末端运动状态在高速环境采用速度跟随模式中使用

在低速环境中车距跟随模式下使用

对规划轨迹进行求解的表达式为：

将具有相同规划时域和插值点的横、纵向轨迹曲线按时刻点t_i∈[t₀,t₁],i＝1,2,...,N一一对应组合，得到[s(t_i),d(t_i)]，再根据转换关系表达式完成从Frenet坐标系到笛卡尔坐标系的映射及轨迹合成。

5.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，在所述步骤S2中，横纵向轨迹协调规划设计过程为：

评价横向规划轨迹的指标设置包括以下的两项：

其中k_j为权重系数，此项意在使横向规划轨迹jerk最小，保证舒适性；

C_e＝k_e(d(t))²

其中k_e为权重系数，此项意在使横向规划接近参考车道中心线，即让主车完成避障后能够回到初始参考车道行驶；

评价纵向规划轨迹的指标设置包括以下的三项：

其中k_j为权重系数，与横向轨迹的该指标一样，此项意在使纵向规划轨迹jerk最小，保证舒适性；

其中k_e为权重系数，v_target为目标车速，在车速跟随模式下需要使车速接近并保持目标速度时，纵向轨迹的末端速度参考采用上式所示的表达式；

其中s_target为目标距离，在车距跟随模式下需要使主车与移动的交通车辆保持一定的车距时，纵向轨迹的末端距离采用如上所示的表达式；

在对横纵向轨迹规划进行单独初步评价后，进一步设计评价横、纵向合成协调轨迹的指标包括以下的四项：

考虑车辆及环境静、动态障碍物都不能简单地等效为质点，它们的形状、大小、朝向均可能给行驶带来风险，分别采用移动的矩形及圆描述运行中的自动驾驶车辆、障碍物，引入超平面理论进一步确定这些车辆轨迹是否与周围交通环境中的静态或者动态的障碍物发生干涉及是否有发生碰撞风险如下：

定义车辆与障碍物不发生干涉及不发生碰撞风险的条件为：

其中dist表示轨迹距离障碍物的最小距离，H(τ)为自动驾驶车辆的轨迹坐标，

为静态或者动态障碍物的圆中心位置，μ为静态或动态的障碍物与车辆干涉的安全因子,m为障碍物数；

进一步引入轨迹避碰的超平面为：

则自动驾驶车辆矩形顶点描述为：

其中，T(τ)为坐标旋转矩阵，定义为：

其中，

θ(τ)为自动驾驶车辆朝向；

进一步推导超平面中的车辆与障碍物不发生干涉及不发生碰撞风险的条件得

其中，

预测的为静态或者动态障碍物的圆中心位置，

为静态或者动态障碍物的圆半径；

定义横纵向协调轨迹的行驶效率指标为：

C_t＝k_tT_i

其中k_t为权重系数，此项为轨迹覆盖的时间，与行驶效率有关；

定义横纵向协调轨迹的舒适性指标为

其中

其中w_a、w_θ为权重系数，此项描述了规划轨迹的合加度，与舒适性有关；

定义横纵向协调轨迹的平顺性指标为：

其中k_k为权重系数，此项描述了转换至大地坐标系下的合成轨迹曲率，与行驶平顺性有关。

6.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，在所述步骤S3中，在轨迹跟踪阶段，所设计的轨迹跟踪过程为：

在设计车辆跟踪动力学操纵极限物理约束规则时，用于描述车辆物理特征的车辆动力学模型表达式如下：

式中，

为车身坐标系下的横向加速度，

为车身纵坐标下的纵向加速度，

为横摆角，

为横摆角速度，

为横摆角加速度，

为车辆的纵向速度，

为车辆的横向速度，δ_f为前轮转角，M_z为直接横摆力矩，a,b分别为前、后轴到质心的距离，I_z为车辆的转动惯量，

为全局坐标系下的纵向速度，

为全局坐标系下的横向速度，X为全局坐标系下的纵向位置，Y为全局坐标系下的横向位置，m为车辆的总质量；

采用的Brush轮胎模型刻画运动的自动驾驶车辆与地面交互作用及路面附着约束如下

上式中

其中前后轮胎侧偏角为：

上述式，μ为地面附着系数,κ_i为滑移率，C_α,C_x分别为轮胎侧偏、纵向刚度，F_z,i为轮胎的垂直载荷；

采用稳态车辆质心侧偏角与横摆角速度来设计自动驾驶车辆动力学操纵极限，首先假设在转弯过程中纵向力作用很小，则稳态横摆角速度满足

为改善驾驶员前轮转向角输入激励下的车辆横摆角速度的瞬态响应，理想的横摆角速度极限修正为

同时，自动驾驶车辆动力学操纵极限严重受制于地面条件约束，导致的轮胎-路面交互作用的饱和效应，则后轮Brush轮胎模型产生临界侧向侧偏角进一步推导得

推导车辆行使过程的横向速度极限为：

其中，η为制动的依赖系数，表示为：

7.根据权利要求1所述基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法，其特征在于，在所述步骤S3中，在轨迹跟踪阶段，利用模型预测控制(MPC)原理完成自动驾驶车辆主动转向(AFS)与直接横摆力矩(DYC)集成轨迹跟踪控制如下：

定义的自动驾驶AFS、DYC集成跟踪控制系统的状态向量

定义输入向量分别为u＝[δ_f,M_z]

因此用于车辆路径跟踪的预测模型表达式写作：

其中

定义模型预测控制的最优化问题的代价函数设计为：

其中，J(ξ(t),u(t-1),ΔU(t))为优化指标，Δu(t+i|t)，t＝0,1,...,N_c-1为输入控制增量向量，N_p为预测时域，N_c为控制时域，η_r(t+i|t),i＝1,2,...,N_p为参考输出量，Q,R,ρ分别为权重矩阵；

模型预测控制的约束条件设计为：

ΔU_min≤ΔU(t)≤ΔU_max

U_min-U(t-1)≤MΔU(t)≤U_max-U(t-1)

0<ε<ε_max

上述式中，Y_cmin(t),Y_cmax(t)分别为硬约束输出量的下限和上限，Y_scmin(t),Y_scmax(t)分别为软约束输出量的下限和上限，Ξ＝ε1_pn，其中1_pn为p_u维列向量；

在当前时刻，求解上述优化问题得到最优控制增量序列，将控制增量序列的第一个量与上一时刻的控制量累积，得到实际控制量，作为被控系统的输入；舍弃除第一个控制增量以外的序列中的其他控制增量；下一个优化时刻到来时，重复以上过程，实现滚动优化；在下一个规划周期更新车辆的初状态，重复步骤S1至步骤S3的操作，实现实时重规划与主动转向及直接横摆力矩集成跟踪控制。

Images