基于Carsim与Simulink的自动变道联合仿真

许 阁

（东风汽车公司技术中心，湖北 武汉 430058）

1 前言

对于自动变道规划算法而言，其作用是接收来自决策模块的决策信号，处理后生成一条变道规划轨迹并由控制模块按此轨迹控制本车进行自动变道。而单纯对规划模块进行开环测试，只能证明本算法能在给定决策条件下能顺利运行，无法证明生成的规划轨迹的正确性与有效性，因此本文在Simulink环境下以自动变道规划算法为核心并编写控制算法模拟车辆运动，再联合Carsim搭建仿真平台，实现闭环仿真。

2 车辆环境建模

在Carsim中选择车型：包含车辆的类型、参数配置等各种信息。

配置两条直线车道，车道中间为单虚线，车道宽为4m；设置自车初始状态，以20m/s的速度沿右车道中心线行驶，变道的目标车道为左车道中心线，通过本车前方的摄像头可选任意一条虚线或实线车道线，采集车道线上的四个点，并输出此四个点在本车车体坐标系下的坐标，并在本车前方20m处设置一辆速度为15m/s匀速行驶的障碍车。

3 算法设计

3.1 多项式拟合规划算法

通过车体坐标系与Frenet坐标系的相互转换，在Frenet坐标系下以参考线为基准采用五次多项式拟合，由给定起始点和变道终点的信息进行五次多项式拟合，生成一条连续的满足车辆运动的规划曲线。参考线选取两车道的中心线，通过摄像头输出的四个点坐标进行三次多项式拟合得到。

3.1.1 车体坐标系转Frenet坐标系

车体坐标系指的是基于车纵向和横向建立的坐标系，Carsim输出给Simulink的信号为车体坐标系下的信号，需将车体坐标系转换到Frenet坐标系进行五次多项式规划。

Frenet坐标系是用s，d_s，dd_s，d，d_d，dd_d来描述车辆在t时刻的状态[1]。

s：车辆在参考线上的投影点经过时间t后的位移；d：车辆在t时刻的位置到车辆在参考线上投影点的距离。参考线还是基于车体的坐标系下的参考线。

d_s：s对时间t的一阶导；d_d：d对s的一阶导。

dd_s：s对时间t的二阶导；dd_d：d对s的二阶导。

车体坐标系下的待转规划点坐标（x，y）和其对应的参考线上投影点的坐标（r_x，r_y），待转规划点与x轴的方位角θ_x，投影点与x轴方位角θ_r，车辆的速度v_x，车辆的加速度a_x，参考线上的投影点的曲率kr，投影点曲率对s的一阶导kr’，投影点距离参考线起始投影点的位移s，待转规划点的曲率kx，将以上物理量作为输入，经过下面的坐标转换公式，得到待转规划点（x，y）对应的Frenet坐标信息[s，d_s，dd_s，d，d_d，dd_d]。

起始状态指的是待转规划点起始坐标点，而末状态点是人为规划时想要车经过设定的时间后所到达的位置点。

转换公式如下：

1）求d：

求d_d：

2）求dd_d：

3）求s：s=s

4）求d_s：

5）求dd_s：

3.1.2 五次多项式拟合

通过坐标转换将始末状态点转换到Frenet坐标系下，即可进行五次多项式拟合。

横纵向规划是指规划出的两条轨迹，用s-t的多项式描述纵向规划结果，用d-s的多项式来描述横向规划结果。

（1）先对纵向进行s-t的5次多项式拟合得到s=s0+s1t+s2t2+s3t3+s4t4+s5t5，过程如下：

1）根据定义可知，d_s=s1+2s2t+3s3t2+4s4t3+5s5t4；dd_s=2s2+6s3t+12s4t2+20s5t3；

2）起始状态对应t=0时刻，终点时刻t=t0，解出s0，s1，s2，s3，s4，s5。

（2）再对横向进行d-s的五次多项式拟合得到d=d0+d1s+d2s2+d3s3+d4s4+d5s5。

3.1.3 过路检测

车辆在变道过程中是实时变化的，随着车辆离目标车道越来越近，采用五次多项式拟合时，会产生较大的超调从而变得不可控，降低了系统的稳定性。因此需设置过路检测，即当五次多项式生成的轨迹超调过大时，即规划轨迹超出目标车道时，且最高点超出超过参考线50cm时，调节起始状态点的d_d和dd_d为零，使得其重新进行五次多项式规划（方法同上述）生成一条不超过相邻车道线的规划轨迹。

3.1.4 Frenet坐标系转车体坐标系

将上述规划轨迹d-s由Frenet转换为车体坐标系y-x。

需使用待转规划点[s，d_s，dd_s，d，d_d,dd_d]，及待转规划点在Frenet坐标系下的参考线上的投影点，在车体坐标系下的坐标（r_x，r_y），投影点与x轴方位角θ_r，投影点的曲率kr，投影点曲率对s的一阶导kr’，从而求出待转点在车体坐标系下的坐标（x，y），与x轴的方位角θ_x，曲率kx，速度v_x，加速度a_x。

1）求（x，y）：

2）求θx：

解得θx；

3）求kx：

4）求v_x：

5）求a_x：

3.2 前馈反馈控制算法

根据规划模块输出的车体坐标系规划轨迹y-x，结合自身车速和偏转的角度，从规划轨迹上选取合适预瞄点，并计算本车与预瞄点之间的角度差距和曲率差距，通过前馈加PID反馈的方式，得到控制模块地输出转角，并发送给EPS转向系统，使其按计算的转角进行转向，从而实现对规划轨迹的跟踪控制[2]。为了更加贴近实际情况，应设计成离散控制系统，因此本文后续算法公式中积分与微分采用离散点数值进行计算。

3.2.1 PID反馈控制

本算法将四轮汽车简化成两轮自行车模型，根据纯跟踪算法原理[3]，将预瞄点选取为规划轨迹的第5个点，并可通过修改预瞄点的选取观察控制效果。角度误差e由两部分组成，一是由横向位置误差造成的θ1，二是由车身方向造成的θ2。

PID控制器设计公式如下，其输出的δ1为反馈转角：

3.2.2 前馈控制

前馈控制器的控制输入为规划轨迹曲率C，用上述当前时刻的规划轨迹的多项式系数C2的两倍近似表示，前馈控制器设计为，其中u为当前车速，C为规划轨迹曲率，k为稳定性因素，常通过标定决定，L为本车轴距，i为转向系统传动比：

4 仿真分析

在本次仿真分析过程中，在Carsim中设置本车在双车道的右车道中心线处，沿中心线方向向前以20m/s的速度行驶，并在其正前方20m处配置一辆同方向运动但车速为15m/s的障碍车，参考线选取两车道的中心线通过摄像头输出4个点，变道目标车道为左车道中心线，车体坐标系以本车之心为原点，沿中心轴指向车体方向为X轴正方向，垂直于中心轴指向车身左侧为Y轴正方向。输出接口两个，分别为carstate[Vx，AVz]表示本车的纵向速度和横摆角速度，和points[X_S1_1，X_S1_2，X_S1_3，X_S1_4，Y_S1_1，Y_S1_2，Y_S1_3，Y_S1_4]表示参考线上采用的四个点的横纵坐标，输入控制接口一个为theta=IMP_STEER_SW表示方向盘转角。仿真周期为0.001s。

Simulink中则为具体算法的搭建，由三部分组成：Carsim交互、规划模块和控制模块。

规划周期为0.1s；控制周期为0，01s，PID反馈控制中Kp取4，Ti取2.5，Td取0.25；前馈控制中，k取0.003，L为Carsim中配置的本车的轴距为2.91m，i取5。

仿真结果：30s的时间，每次规划的轨迹都是平滑且连续的，能满足车辆的动力学要求；车辆在4s左右时完成变道，伴有些许振荡并逐渐趋于平缓。

综上可得，本次联合仿真能实现比较稳定且效果良好的闭环控制，说明采用五次多项式拟合进行的变道轨迹规划能满足车辆的跟踪条件，为将应用软件导入实际车辆进行实车测试奠定了理论基础。

5 总结

本文以模拟实际车辆规划与控制的关系的基础上，即规划周期设为0.1s，控制周期设为0.01s，通过将车体坐标系下的感知信息转换到Frenet坐标系下进行五次多项式轨迹规划，并设计过路检测防止规划轨迹越过目标先造成系统不稳定，并将规划结果还原到车体坐标系下输出给前馈反馈控制模块进行变道跟踪，仿真结果表示，车辆能在限定时间内按照规划轨迹进行自动变道。