基于车辆运动学模型的轨迹跟踪控制

车辆路径跟踪包括主车的纵向和横向运动控制，纵向控制是通过调整主车的加速度，使得车辆以期望的速度行驶；横向控制是通过调整主车的转向，使主车沿预期的轨迹行驶。本文结合车辆运动学的知识对主车进行横纵向控制，基于PID控制策略设计路径跟踪控制器，并实现整个系统的闭环仿真与分析。

一、车辆运动学模型

首先，利用车辆运动学模型，来估计车辆实际运动轨迹相应点的位置。车辆转向运动学模型如下图所示。

其中坐标系为惯性坐标系XOY；δf为前轮转角；(Xf,Yf)和(Xr,Yr)分别为前轴轴心和后轴轴心的坐标；vf为前轴轴心速度，vr为后轴轴心速度；L为轴距；φ为航向角。

在后轴行驶轴心(Xr,Yr)处，速度为：

前后轴的运动学约束为：

由式（1）和式（2）联合得：

由前后轮的几何关系可得：

根据式（2）、（3）和（4）可得车辆横摆角速度为：

综上，得到车辆运动学模型状态方程如下：

由状态方程可知，输入为前轮转角δf和后轴轴心速度vr，状态为横向位移Xr、纵向位移Yr和航向角φ。给定后轴轴心初速度为0，初始航向角为0。

二、预瞄驾驶员模型

明尼苏达大学的Rajamani教授等指出，预瞄控制是提高车辆横向控制鲁棒性和稳定性最好的方法之一。因此，采用预瞄控制思想，即以车辆前方预瞄点处的运动状态作为反馈设计PID控制器。预瞄时距取0.1s，预瞄距离由以下公式计算：

我们记本时刻点的位置为(x0,y0)，将预瞄点的位置表示为(x1,y1)，则两点之间的坐标有以下关系

三、横向控制

本题给定轨迹为一百个离散的点，利用for循环语句依次求预瞄点与轨迹上各点的距离比较得到最近点的坐标。因为距离都是非负数，为了简便运算，此处我们可直接比较距离的平方值，记d为预瞄点到轨迹上点距离的平方，计算公式为：

1）横向误差计算

此处的横向误差不是纵坐标的差值，而是预瞄点到轨迹上最近点的距离。记横向误差为e1，计算公式如下：

2）航向误差计算

如要计算航向误差，需要得到期望轨迹相应点的航向角，与实际航向角作差得到航向误差。但给定的轨迹为一组离散点，为了得到航向角信息，我们可以对轨迹进行拟合。利用MATLAB曲线拟合工具箱 （cftool）进行拟合，发现拟合最佳曲线为正弦函数曲线，具体如下。

拟合得到的函数表达式为：

记导数为ds，利用diff求导函数得到导数表达式：

继而可以得到对应每个x取值得到的导数值，再对导数进行反正切计算，得到轨迹上对应点的航向角，记航向误差为e2，计算公式为：

以横向误差e1和航向误差e2作为反馈对方向盘转角或前轮转角进行PID控制，PID控制原理为：

其中，Kp、Ki、Kd 分别为误差的比例系数、积分系数和微分系数，调节三个参数来对系统进行控制。

当以横向误差e1为反馈部分取Kp、Ki均为1.5，微分系数为0；以航向误差e2为反馈部分取Kp为3，其余两系数均为0。得到轨迹如下：

四、纵向控制

继而进行纵向控制，车辆纵向控制系统的目的是车辆的纵向行驶速度跟踪，车辆初始速度为0，到达指定位置时速度也为0，当开始行驶时目标速度为Vset，当车辆即将到达目标位置时其目标速度为0。车辆纵向控制系统是一个简单的单输入单输出系统，以速度跟踪误差为反馈实现车辆速度跟踪控制。跟踪误差计算公式如下：

其中，目标参考速度为：

其中，d0为判断车辆是否接近目标位置的距离阈值，d为车辆当前位置到目标位置的距离，记目标位置坐标为(xdes,ydes)，则d的计算公式为：

以速度跟踪误差为反馈进行PID控制，取比例系数为0.8，积分系数为15，得到速度跟踪图如下：

至此，实现了对给定期望轨迹的跟踪控制，并用PID进行横纵向闭环控制，形成了车辆运动学的闭环系统仿真，得到了较为理想的运动轨迹。

关于清研车联

清研车联，是清华大学苏州汽车研究院重点孵化打造的汽车职业教育企业。

专注于汽车行业垂直产业链，致力于中国汽车产业新型人才的培养，为汽车产业的转型升级大规模输送合格人才，为院校和企业的人才培养提供综合解决方案。

积极应对汽车产业变革中科技、人才、服务等快速发展趋势，充分依托清华大学优质的教育资源，重点聚焦新能源汽车、智能网联汽车、汽车智能制造，汽车轻量化与NVH等汽车新技术领域，推陈出新，引领教育变革，从内容和模式方面协同国内汽车职业教育转型升级。

在线教育MOOC官网：www.auto-mooc.com