从小白到自动驾驶系统工程师21——横纵向控制

自动驾驶整个系统分为：感知层、规划层、控制层，控制是无人驾驶系统最底层的软件算法模块，包括：横向控制（主要用于车辆方向盘的控制）和纵向控制（主要用于车辆油门、刹车的控制）。

纵向控制主要为速度控制，通过控制刹车、油门、档位等实现对车速的控制，对于自动挡车辆来说，控制对象其实就是刹车和油门。

横向控制根据上层运动规划输出的路径、曲率等信息进行跟踪控制，以减少跟踪误差，同时保证车辆行驶的稳定性和舒适性。

横、纵向控制需要协同工作以使无人车按照预定的参考轨迹行驶。

PID 控制算法

PID控制是工业控制中应用得最为广泛的一种控制方法。其原因是：

（1）其结构简单，鲁棒性和适应性较强；

（2）其调节整定很少依赖于系统的具体模型；

（3）各种高级控制在应用上还不完善；

（4）大多数控制对象使用常规PID控制即可以满足实际的需要；

（5）高级控制难以被企业技术人员掌握。

PID控制器是一种线性控制器，它根据给定值r(t)与实际输出值y(t)构成偏差：e(t)=r(t)-y(t)。将偏差的比例(P)、积分(I)和微分(D)通过线性组合构成控制量，对受控对象进行控制。其控制规律为：

传递函数为：

式中，Kp为比例系数，Ti为积分时间常数，Td为微分时间常数；Ki=Kp/Ti，为积分系数；Kd=Kp*Td，为微分系数。

PID控制器各校正环节的作用

比例环节：即时成比例地反应控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用以减小误差。当偏差e=0时，控制作用也为0。因此，比例控制是基于偏差进行调节的，即有差调节。

积分环节：能对误差进行记忆，主要用于消除静差，提高系统的无差度，积分作用的强弱取决于积分时间常数Ti，Ti越大，积分作用越弱，反之则越强。

微分环节：能反映偏差信号的变化趋势(变化速率)，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减小调节时间。

从时间的角度讲，比例作用是针对系统当前误差进行控制，积分作用则针对系统误差的历史，而微分作用则反映了系统误差的变化趋势，这三者的组合是“过去、现在、未来”的完美结合。

衡量一个PID控制系统性能好坏的指标主要有：上升时间 、超调量 、调节时间 和稳态误差。其中：

（1）上升时间是指系统实际输出从正常输出的10%上升到正常输出的90%时所需的时间；

（2）调节时间是指系统实际输出值稳定在正常输出值的5%或2%范围以内时所需的时间，增大Kp可以缩短调节时间，但比例作用过大会使系统动态性能变坏，甚至会使闭环系统不稳定；

（3）超调量是指系统实际输出的最大值与正常值的差与正常值的比值，微分作用的引入使系统能够根据偏差变化的趋势做出反应，适当的微分作用可加快系统响应，有效地减小超调，改善系统的动态特性，增加系统的稳定性。不利之处是微分作用对干扰敏感，使系统抑制干扰能力降低；

（4）稳态误差是指系统达到稳态时的输出值与正常值差的绝对值与正常值的比值，积分作用的引入有利于消除稳态误差，但使系统的稳定性下降，尤其在大偏差阶段的积分往往会使系统产生过大的超调，调节时间变长。

这四个参数反映了系统的响应能力和稳定性，通过它们就可以判定一个系统性能的好坏。

参数整定找最佳，从小到大顺序查

先是比例后积分，最后再把微分加

曲线振荡很频繁，比例度盘要放大

曲线漂浮绕大湾，比例度盘往小扳

曲线偏离回复慢，积分时间往下降

曲线波动周期长，积分时间再加长

曲线振荡频率快，先把微分降下来

动差大来波动慢。微分时间应加长

理想曲线两个波，前高后低4比1

一看二调多分析，调节质量不会低

将规划的目标速度和自车车速作为输入量，对目标速度和自车车速偏差进行比例(Proportion)、积分(Integration)和微分(Differentiation)控制得到油门、刹车控制量。

将车辆当前的路径跟踪偏差作为输入量，对跟踪偏差进行比例(Proportion)、积分(Integration)和微分(Differentiation)控制得到转向控制量。

积分项的作用：消除稳态误差，也叫静态误差，即纠正过去。

微分项的作用：管控未来，减少控制过程中的震荡。

基于模型的横向控制

基于模型的横向控制分为基于车辆运动学模型的横向控制方法以及基于车辆动力学模型的横向控制方法。

常用的基于车辆运动学模型的控制算法有：纯跟踪控制(Pure Pursuit)算法、前轮反馈控制(Front wheel feedback)算法、后轮反馈控制(Rear wheel feedback)算法。

常用的基于车辆动力学模型的控制算法有：线性二次型调节器(Linear Quadratic Regulator，LQR)控制算法。

这里的x,y,φ都是大地坐标系，为了和后面推导中的车体坐标系区分，加上下标

纯跟踪控制(Pure Pursuit)是一种典型的横向控制方法，最早由 R. Wallace 在1985年提出，具有较好的鲁棒性。

基本思想：基于车辆后轮中心位置，在参考路径上选取一个预瞄点，计算预瞄点和路径偏差，对方向盘转向控制量进行计算。

前轮反馈控制（Front wheel feedback）又称 Stanley 控制，利用 前轮中心 的 路径跟踪偏差量 对方向盘转向控制量进行计算。

后轮反馈控制（Rear wheel feedback）是利用后轮中心的路径跟踪偏差量来进行转向控制量计算的方法。

基于车辆运动学模型的方法忽略了车辆的动力学特性 因此在车速过快 或者曲率变化率过大的情况下该算法无法满足车辆的稳定性控制要求。这时就需要用到基于车辆动力学模型的横向控制方法，通常将车辆动力学模型简化为二自由度的动力学模型。

对于基于车辆动力学模型的控制方法，首要的工作是对车辆动力学进行建模 由于车辆动力学模型具有很强的非线性时变特性，因此精确建模整个车辆动力学十分困难。在横向控制中，主要关心的是车辆的横向运动特性，假设车身的纵向速度Vx 保持不变, 其横向动力学模型的两个自由度为：横向运动和横摆运动。

由于精确的二自由度动力学模型是非线性的，为了便于进行实时的跟踪控制计算 通常还需要在精确的二自由度动力学模型基础上进行一些简化近似，得到线性二自由度动力学模型。

在给定的前轮转角输入下，可以得到车辆的横向位移、横向速度、横摆角以及横摆角速度的响应。但是横向跟踪控制的目的是为了减小跟踪偏差需要的状态方程是能够分析在给定的前轮转角下车辆跟踪偏差的响应， 这些偏差通常是：横向位置偏差、横向位置偏差变化率；横摆角偏差、横摆角偏差变化率。

前轮转角控制输入作用下，车辆路径跟踪偏差的响应特性、期望的响应特性就是 跟踪偏差能够快速、稳定地趋近于零，并保持平衡 同时前轮转角控制输入又尽可能小，这就是一个典型的多目标优化最优控制问题 且优化的目标函数可以表示为跟踪过程累计的跟踪偏差与累计的控制输入的加权和。