智能汽车规划控制常用控制方法详解

控制是驱使车辆前行的策略。控制的目标是使用可行的控制量，最大限度地降低与目标轨迹的偏差、最大限度地提供乘客的舒适度等。

如上图所示，与控制模块输入相关联的模块有规划模块、定位模块和车辆信息等。其中定位模块提供车辆的位置信息，规划模块提供目标轨迹信息，车辆信息则包括档位、速度、加速度等。控制输出量则为转向、加速和制动量。

控制模块主要分为横向控制和纵向控制，根据耦合形式的不同可以分为独立和一体化两种方法。

1 控制方法

1.1 解耦控制

所谓解耦控制，就是将横向和纵向控制方法独立分开进行控制。

1.2 耦合控制

耦合控制考虑到了横纵向控制时存在的耦合问题。一个典型的例子是汽车不能高速过弯，因为当纵向速度过高时，横向角速度需要有一定的限制，否则向心力无法满足向心加速度。

横纵一体的典型代表方法是线性时变模型预测控制，该方法在模型预测控制的基础上增加横纵向的联合约束。如最大向心加速度约束等。

1.3 横向控制

如上图横向控制可以分为几何方法、基于运动学模型方法和基于动力学模型的方法。

1.3.1 前馈控制

所谓前馈控制，即根据跟踪点信息对控制量进行提前适当的补偿。一个典型的例子是利用跟踪序列点中的曲率信息，对转角进行补偿。

1.3.2 Chained Form

链式系统将非线性系统进行多层线性化，它将系统进行逐层分解，进而可以将系统快慢化，类似于滤波系统[3]。

系统在frenet坐标下的模型：

经过反向积分预算，可以得到控制率：

1.3.3 Lyapunov

基于李雅普诺夫稳定性方法的设计可以运用在运动学和动力学模型上。其基本思想是首先建立运动学或者动力学模型，根据模型提出跟踪方法，然后建立李雅普诺夫函数，通过李雅普诺夫稳定性证明闭环系统的渐近稳定[4]。

• 运动学模型

如上图所示，小车当前点为P，跟踪目标点为Pr。 是当前位置和目标点的位姿差，  和  分别为参考速度和角速度。设计李雅普诺夫函数：

跟踪率设计：

最终通过限定约束设计参数，进而证明了跟踪率的渐近稳定性，即当 → ∞时，  → 0。

• 动力学模型

首先建立动力学模型：

其中：

令

则误差为：

设计代价函数：

设计控制率：

最终证明渐进稳定性。

1.3.4 Pure Pursuit

纯跟踪是一种几何路径跟踪控制器。该控制器使用车辆运动和参考路径的几何关系来跟踪参考路径的控制器。该控制方法使用车辆后轴中心用作参考点。

根据上图可推到出前轮转角指令：

其中R为转弯半径，L为车辆轴距，e为车辆当前姿态和目标路点在横向上的误差， 为前视距离且  。

根据上图实验数据所示前视距离增加，跟踪抖动越来越小。较短的前视距离可提供更精确的跟踪，而较长的前视距离可提供更平滑的跟踪。PurePursuit的另一个特征是，在转弯跟踪时，过大的前视距离会导致“cutting corners”。Pure Pursuit很难在稳定性和跟踪性能之间进行权衡。

1.3.5 Stanley

与以后轴为参考点的纯追赶纯跟踪方法不同，Stanley控制器以前轴为参考点。它同时考虑了航向误差和横向误差。Stanley控制器不仅考虑航向误差，而且还考虑了横向误差。

根据上图可推到出前轮转角指令：

根据上图实验数据所示随着k的增加，跟踪性能也会提高。当车速变大时Stanley不像Pure Pursuit有足够的稳定性。

1.3.6 LQR

基于车辆运动学模型的方法忽略了车辆的动力学特性，因此在车速过快或者曲率变化率过大的情况下该算法无法满足车辆的稳定性控制要求。对于基于车辆动力学模型的控制方法，首要的工作是对车辆动力学进行建模。由于精确的二自由度动力学模型是非线性的，为了便于进行实时的跟踪控制计算，通常还需要在精确的二自由度动力学模型基础上进行一些简化近似，得到线性二自由度动力学模型。

• 车辆二自由度动力学模型：

• LQR:

线性二次调节器（Linear Quadratic Regulator或LQR）是基于模型的控制器，它使用车辆的状态来使误差最小化。LQR理论是现代控制理论中发展最早也最为成熟的一种状态空间设计法。LQR可得到状态线性反馈的最优控制规律，易于构成闭环最优控制。

LQR 最优设计是指设计出的状态反馈控制器 K 要使二次型目标函数 J 取最小值，而 K 由权矩阵 Q 与 R 唯一决定，故此 Q、R的选择尤为重要。如下公式为LQR代价函数：

根据车辆动力学模型和LQR代价函数可以推到出代数里卡提方程：

最终通过迭代黎卡提方程计算反馈矩阵，再根据反馈矩阵获取最优控制量。

1.3.7 MPC

MPC(Model Prediction Control)是一种致力于将更长时间跨度,甚至于无穷时间的最优化控制问题, 分解为若干个更短时间跨度,或者有限时间跨度的最优化控制问题, 并且在一定程度上仍然追求最优解。

MPC由如下三个要素组成:

1. 预测模型: 预测模型能够在短时间内很好的预测系统状态的变化;
2. 在线滚动优化: 由于预测模型得到的结果与实际仍然存在偏差, 所以采用滚动优化找到每个时刻下的局部最优解,通常情况会设计一个目标(损失)函数并将其转化为二次规划问题进而找到最优解;
3. 反馈校正: 到下一个时间点根据新的状态重新进行预测和优化。

• 预测模型:

根据LQR中的车辆动力学模型可以推出预测模型。

• 滚动优化:

MPC代价函数：

根据预测模型、车辆横向约束、代价函数进行优化求解可以得到相应的控制指令。

1.3.8 横向控制算法对比

1.4 纵向

如上图所示，纵向控制一般采用采用串级pid控制方法。

2 详细设计

控制器的设计如上图所示，其中Controller为基类，LonController、LonController和MPCController继承该基类。LonController又衍生出LQRController、LyapunovController和StanleyController等子类。