智能车辆并道行为轨迹规划及跟踪控制
时间：2023-01-15 12:20:11 来源：柠檬阅读网 本文已影响 人 
刘思超，杨铁梅
(太原科技大学 电子信息工程学院，太原 030024)
智能车辆可以自动完成一些基本的驾驶操作行为，从而可以避免人为因素引起的交通事故，不仅交通事故率可以大大降低，而且道路拥堵问题也能得到改善[1]。并道行为会影响交通通行效率和道路安全，研究智能车辆的并道行为有重要意义。
轨迹规划模块主要是智能车根据车辆所处的环境、上层决策任务与自身所处位置，在满足运动学约束与安全性条件下，规划出一段时间内车辆期望的运动轨迹，包括行驶速度、方向和路径等[2]。王赵辉等[3]分析了目标车道上是否有行驶的车辆，在无车换道工况下选取梯形加速度换道轨迹，有车工况下则选择五次多项式换道轨迹。张新锋等[4]使用贝塞尔曲线作为参考轨迹，利用遗传算法实现了最优避障路径规划。潘兵宏等[5]通过对高速出口车辆换道轨迹数据的分析，使用双曲正切函数拟合出换道轨迹。范柄尧等[6]利用人工势场与差分进化算法相结合的方法，实现了移动机器人的轨迹规划问题。使用五次多项式作为并道轨迹平滑且连续，不存在曲率突变等问题，而且只需要已知智能车辆起点和终点状态，能够直接有效的规划出并道轨迹。
路径跟踪是指智能汽车控制器对规划出的参考轨迹跟踪行驶的能力。控制器的设计大多与汽车模型有关。跟踪控制目前较为常用的算法有比例积分微分控制算法，滑模控制算法，LQR控制算法，MPC算法等。李伟等[7]采用滑模控制方法实现了无人驾驶汽车路径跟踪控制。尹晓丽等[8]基于横向位置和参考轨迹的横摆角误差设计模糊控制器，实现了无人驾驶车辆的轨迹跟踪控制。季杰等[9]将车辆动力学模型和运动学模型相结合，设计多约束模型预测控制器实现无人驾驶汽车路径跟踪控制。模型预测方法对外界干扰和模型不确定性具有较强的鲁棒性，但目前模型预测方法设计的预测时域与控制时域大多是基于经验选取，在参数选取过程中，还需要考虑系统对参考轨迹的跟踪能力。
本文建立最优五次多项式并道轨迹，提出基于横摆角变化的自适应模型预测控制器，并证明算法的有效性。
智能车辆本身是一个非常复杂的系统，为使简化模型更加准确、方便的描述车辆运动状态，对车辆本身作如下假设：
(1)车辆在平坦无倾角的水平路面上行驶；
(2)忽略车体本身的振动及内部零件的相对运动；
(3)只考虑轮胎纯侧偏特性，并且工作在线性区；
(4)横向运动只由前轮转向引起，且纵向速度不变。
为了满足轮胎侧偏特性满足在线性区工作，对前轮偏角作小角度假设。车辆二自由度动力学模型如图1所示。其中oxy为车辆坐标系，OXY为地面惯性坐标系。
图1 智能车辆二自由度动力学模型Fig.1 Two-degree-of-freedom dynamic model of intelligent vehicle
对车辆模型进行受力分析，结合偏角小角度假设条件及车辆坐标系与地面惯性坐标系之间的关系，可以得到简化后的二自由度前轮小偏角假设智能车辆动力学模型如式(1)所示。
(1)
智能车辆的并道行为就是沿着一条已经规划好的轨迹稳定安全的从本车道驶向目标车道。为了保证车辆并道过程的稳定性，一般性的认为车辆以恒定速度完成并道过程。
在轨迹规划模块中，首先根据本车道与目标车道车辆速度，预测一段时间内前方与目标车道的车距，计算安全距离，保证并道过程中无碰撞，这是安全并道的前提；
其次，规划出合适的轨迹，不仅要符合车辆速度，角速度，前轮偏角等运动学约束，还要考虑质心侧偏角，轮胎侧偏角，横向加速度等动力学的约束，保证车辆的稳定性；
最后，根据并道类型，安全距离模型及驾驶员模型，动态调整并道时间，尽量减少交通干扰的同时，保证驾驶者与乘车人的舒适性。
2.1 五次多项式
并道轨迹选取五次多项式，保证车辆平顺行驶，不会发生突然转向。在惯性坐标系下建立五次多项式：
y=a0+a1x1+a2x2+a3x3+a4x4+a5x5
(2)
假设已知车辆变道起始点坐标A(XA，YA)与终点坐标B(XB，YB).根据车辆运动学模型，有
(3)
在起点和终点，车辆的纵向速度与纵向加速度均为0.
y′|(XA，YA)=0，y′|(XB，YB)=0
y″|(XA，YA)=0，y″|(XB，YB)=0
(4)
根据上述条件，可以得到一条完整的五次多项式轨迹。如图2可以看出，加速度、曲率连续无突变，轨迹平滑，适合作为智能车辆的并道参考轨迹。
图2 五次多项式轨迹图(左)及曲率图(右)Fig.2 Fifth degree polynomial trajectory graph(left)and curvature graph(right)
2.2 最优并道轨迹
考虑在并道过程中，智能车辆始终能够以恒定的速度沿着期望轨迹完成并道行为。在本文中，智能车辆作匀速曲线运动，在每一个参考点上，车辆的纵向速度恒定。车辆的横向运动只由前轮方向的决定。
在并道过程中，横坐标X是与时间t成正比关系，即：
X=vreft
(5)
式(5)中，vref为惯性坐标系下的纵向参考速度。本文假设vref=(1-0.05)vx.
假设高速匝道入口处，小车P准备并道，前方大卡车Q与后方小轿车O保持匀速行驶，车P并道过程中不会影响主道车辆的正常行驶，智能车辆向左并道示意图如图3所示。
图3 智能车并道示意图Fig.3 Schematic diagram of intelligent vehicle merging
考虑本车P与后车O在并道终点处的最小安全距离分别为：
DPMRS=0.3vQ-0.5vP
DOMRS=0.3vP-0.5vO
(6)
根据相对距离公式建立安全性代价函数：
(7)
式(7)中，系数k由车辆的相对速度与车型有关及并道类型相关。若智能车辆向左并道，后车为小型车辆，则k=0.7，保证与后车的相对安全距离。若智能车辆向右并道，前车为大型车辆，则k=0.4，要增大与前车的间距，防止并道终点前车急刹车的情况发生。
同时，人体的舒适度用j来定义，表示加速度的变化率，用如下公式表示：
(8)
智能车辆的横向加速度与曲率之间的关系为：
(9)
五次多项式任一点曲率可由式(10)表示：
(10)
智能车辆作匀速曲线运动，只有法向加速度，由式(9)可知，曲率越大，横向加速度越小，人体的舒适感越好。因此，要保证乘车人的舒适程度，约束函数还需要与j相关。
舒适度代价函数由式(11)表示。
(11)
式(11)中，μ为舒适度系数，ρmax为最大曲率，Δρmax为最大曲率变化率。
同时，考虑车辆并道时间对并道效率的影响，建立并道效率代价函数：
(12)
根据式(7)、(11)、(12)，建立最优目标函数：
(13)
式(13)中，λi为权重系数，且λ1+λ2+λ3=1.目标函数同时考虑了智能车辆并道安全性，乘车人舒适性及并道效率。
将目标函数转化为关于并道时间t的多约束优化问题：
s.t. 3≤t≤6
vPt |a|≤amax ρ≤ρmax j≤jmax w1+w2+w3+w4=1 (14) 式(14)中，Dmax表示并道所允许的最大距离，amax，ρmax，jmax分别表示最大横向加速度，最大曲率及最大横向加加速度。 遗传算法是一种基于概率的群体优化算法，具有并行，高效的优点，采用Matlab中的遗传算法工具箱对上述优化问题进行求解，得到最优并道轨迹。 设置并道时间上下限(3～6)s，初始化种群数目20，个体数目5，交叉概率0.4，变异概率0.1，最大进化代数20.求解结果如图4所示，并道时间t取3.366 s时，并道轨迹目标函数取值最小。 图4 遗传算法求最优轨迹图Fig.4 Genetic algorithm for optimal trajectory graph 模型预测控制具有前馈-反馈的功能，能够处理非线性多约束优化问题，根据整个预测内的参考输入以及状态量的约束求解输出最优的控制量。其主要有三个模块组成，构建预测模型，优化问题求解，之后将解的第一个元素作用于下一时刻控制量。 根据第一节所建立的动力学模型，建立车辆非线性状态方程： (15) 由于非线性模型计算非常复杂，参考文献[10]的方法对状态方程进行线性化及离散化，得到离散的状态空间表达式： η(k)=Cξ(k) (16) 其中，A(k)=I+TA(t)，B(k)=I+TB(t). 目标函数的设计要保证智能车快速且平稳的追踪上期望轨迹。本文采用如下形式的目标函数： J(ξ(t)，U(t-1)，ΔU(t))= (17) 式(17)中，第一项为预测时域内系统状态与参考轨迹的反馈误差，第二项为控制时域内对控制量的约束，能够使保证控制有效性，防止控制量的突变与超出控制范围，最后一项为软约束，避免了无解的情况发生。 同时，优化函数还需满足如下约束： (1)控制量约束 (2)控制增量约束 (3)质心侧偏角约束 -12°<β<12° (4)附着条件约束 ay，min+ε≤ay≤ay，max+ε (5)轮胎侧偏角约束 -2.5°<αf，t<2.5° 综合上述的目标函数和约束条件，基于动力学模型的模型预测控制器需解决如下优化问题： s.t. ΔUmin≤ΔU≤ΔUmax Umin≤AΔU+U≤Umax yhmin≤yh≤yhmax ysmin-ε≤ys≤ysmax+ε ε>0 (18) 式(18)中，yh为硬约束输出； 上述优化问题可转化为对二次规划问题求解，得到控制时域内一系列的控制增量，将第一个元素作为实际的控制增量作用于系统，即： ut+1=ut+ΔU(1) (19) 对每个控制周期重复上述过程，即可实现轨迹跟踪。 由多次实验发现，改变预测时域Np和控制时域Nc对控制效果有很大的影响。如果Np过大，可能导致求解时间过长，在控制周期内不能完成优化问题的求解，出现控制器失效的问题，Nc过大，则会导致系统的输出超过预期，产生过拟合现象。因此，本文提出横摆角系数自适应的模型预测控制器，根据车身的横摆角自适应调整Np，Nc. Np=k1NP，Nc=k2NC (20) (21) 在开始并道阶段，车身横摆角小于轨迹切线，Nc应该增大，保证跟踪精度。在并道中期，车身横摆角与轨迹切线斜率几乎相等，控制时域和预测时域应相对减小。在并道末期，智能车处于轨迹下方，横摆角大于切线斜率，要减小Nc，防止出现过拟合现象。 本文提出了基于五次多项式的并道轨迹及曲率自适应模型预测控制。左并道一般从低速车道变向较高车速车道，多见于高速入口及道路合流等情形。本文采用向左并道工况，对上述所提出的跟踪控制算法与斯坦利方法[11]进行Carsim/Simulink联合仿真对比实验。Carsim提供智能车辆的速度，位置，横摆角等状态量，轨迹规划模提供参考轨迹，再经过跟踪控制模块求解出下一时刻车辆的前轮偏角输入到Carsim中，形成闭环控制系统，验证算法的有效性。 Carsim中车辆模型参数表1所示。 表1 车辆模型参数 本文使用Carsim中的b-class，轮胎选择225/60 R18，车辆速度72 km/h，并道距离84 m.仿真结果如下图所示。 图5 Carsim/Simulink联合仿真示意图Fig.5 Carsim/Simulink co-simulation diagram 图6表示控制器的跟踪效果图，图7表示控制器的误差对比图。可以看出，三种控制器都有着很好的跟踪效果，文献[11]斯坦利方法在并道终点处会有较小的超调，之后会逐渐稳定。比传统的MPC控制器而言，本文提出的控制算法有更小的误差，更好的跟踪精度。 图6 轨迹跟踪效果图Fig.6 Trajectory tracking effect diagram 图7 轨迹跟踪误差图Fig.7 Trajectory tracking error graph 图8表示并道过程中车辆横摆角的变化图，文献[11]斯坦利方法是基于几何跟踪的控制方法，在终点处会有一段时间的微调。图9反映了变道过程中前轮偏转角变化，可以看出三种控制器均满足小角度约束条件，本文提出的控制器所解得的前轮偏角连续且平稳，能够有效的实现智能车辆的并道行为。综上所述，本文提出的控制器保证了并道的安全性，稳定性，相比具有更好的跟踪性能。 图8 横摆角变化图Fig.8 Yaw angle variation diagram 图9 前轮偏角变化图Fig.9 Variation diagram of front wheel deflection angle (1)基于五次多项式提出了一种最优并道轨迹，在最优轨迹规划中综合考虑并道类型，车辆的安全性、乘车人舒适度及并道效率，设置不同的权重系数，使用遗传算法优化得到最优变道轨迹。 (2)提出了基于横摆角的自适应MPC，详细分析了并道过程中横摆角的变化对控制器参数的影响，并且通过仿真结果说明了本文算法的有效性。
3.1 模型预测控制器
ys为软约束输出，可动态调节约束范围。3.2 基于横摆角系数自适应的MPC
