【备战算法岗】—— 控制模块复习（持续更新！！！）

1 控制理论基础

1.1 控制模块概述

在这里插入图片描述

输入：轨迹线Reference、地图信息、定位信息、车辆反馈信息
输出：刹车、油门、转向

CANBUS：车辆底盘交互协议
底盘、速度、四轮转速、健康状况、底盘报错、自动驾驶状态

运动学模型：刚体运动（低速）
动力学模型：力学分析（中高速）
详细推导见：车辆运动学和动力学模型

无人车控制任务：让无人车在既定的轨迹上平稳运行，保证车辆行驶的可行性、舒适性、控制实时性

控制模块：

(1) 预处理：

数据清洗：不正常信号、planning异常值【对于异常数据，去除或者报错】
合理性检查：定位信息、车辆底盘信号
紧急处理：前方突然出现障碍物（emergency stop）【例如无人车前侧安装的超声传感器，直接连接到control模块，应付紧急障碍物】
信号smooth：去除信号噪声【传感器的信号都是带有噪声】，lag Compensation【滞后信号补偿，这是由于信号传递过程中，信号传递过程中有些滞后，需要做一些补偿，消除一些传递误差】

(2) 后处理：

Saturatian/Limitation处理：执行器的固有缺陷补偿【由于执行器本身有能力限制，所以需要对输出信号做一定的限制，否则输出的信号有可能直接导致底盘报错或者执行器破坏】
信号Smoothing：简单的平滑处理和异常值检查

(3) Controller控制器设计：

建模：建立一个车辆模型，目的提供一个控制对象
系统参数辨识：控制可修复一些细微细小的错误，而这些错误对应的值是无法精确得到，这时候就需要系统辨识，利用一些方法，猜算出这些值也就是对应的系统参数
控制观测器设计：观测器，指的是有些控制方法在反馈的基础上，要添加更多观测信息，才能确定其反馈值是否符合控制输入；控制观测器可理解为整个控制器的一套方法；其设计非常重要，决定控制器性能
参数Fine Tuning：参数调节，与深度学习的超参的调差方式（一般是试验试错方式）不一样，而其一般是依据一些数学原理对参数调整进行指导，但也有试错试验方式来调整的

在这里插入图片描述

1.2 控制基础

（1）控制系统：消除物理模型和数学模型差异性的过程
对于无人车
稳定性：对于任何环境都需要稳定（安全）
稳态误差：尽可能小，保证执行上层命令【控制实际运行的偏差范围】
动态误差：在动态工程中也需要保证很多性能
在这里插入图片描述
y(t)表示控制系统的输出；虚线表示控制系统理想状态下达到的位置（我们期望的）；t表示时间
图(b)是我们追求的控制效果，(a)比较慢，©波动但是不收敛，(d)波动呈发散属于最不好的控制

（2）控制基础

时间域

超调量：目标值与可达到的最高位置的范围
超调时间：到达最高超调量所花时间
稳态调整时间（整定时间）：最初到达稳定在一定范围内所花的时间
上升时间：初次到达目标位置所花的时间，参与衡量上升速度大小
一般来说，希望超调量尽量小；超调时间尽量快；稳态调整时间越快越好；上升时间越快越好

频率域

通过频率（pass band）：通过（增益大于一定程度）对应的频率
停止频率（stop band）：增益下降到一定值所对应的频率
带宽：增益上升和增益下降对应的频率范围大小
截止频率：表示增益达到给定的截止值时所对应的频率
时间域可通过傅里叶和拉普拉斯变换变成频率域

离散域（z变换）

频域、时域、离散域是可以相互转换的；时间函数相对比较常用，Google或百度搜索拉氏变换应该就用了

2 仿真软件的使用

CARLA

3 常见规划控制 算法

纵向控制算法：PID
横向控制算法：PID、Pure pursuit、Stanley、LQR、MPC

3.1 横向控制 算法对比

（1）是否转弯内切（2）鲁棒性（3）对路径的要求（4）适用场景

横向控制算法
（1）PID：鲁棒性较差，对路径无要求，转弯不会内切，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径曲率较小及低速的跟踪场景
（2）Pure pursuit：鲁棒性较好，对路径无要求，转弯内切速度增加变得严重，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径连续或不连续或者低速的跟踪场景
（3）Stanley：鲁棒性好，对路径要求曲率连续，转弯不会内切，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径平滑的中低速跟踪场景
（4）LQR：鲁棒性较差，对路径要求曲率连续，不会转弯内切，曲率快速变化时超调严重，稳态误差小，除非速度特别大，适用场景：路径平滑的中高速城市驾驶跟踪场景

LQR vs MPC

	MPC	LQR
研究对象	线性和非线性系统	线性系统
状态方程：离散化	对非线性的状态方程进行线性化,对线性方程离散化	对线性方程离散化
目标函数	MPC目标函数一个是累计和	LQR目标函数一个是积分
求解方法	转化为二次规划问题，利用求解器进行求解，生成控制序列	采用变分法，通过求解黎卡提方程进行逼近，最终获取控制序列
工作时域	是求解预测时间段Np内的控制序列，并在下个周期后进行滚动优化	求解预测时间段内的控制序列，只求解一次，每个周期下取对应的控制量
LQR和MPC的优缺点	计算量大，实时性差，对于算力要求高，硬件成本较高	① LQR不滚动优化，预测时间段内的控制序列只求解一次，没有考虑实际与规划的误差② LQR求解整个预测时域内的控制序列，而MPC可在更小的时间窗口中求解优化问题，获得次优解，可大大提升求解速度③ 无约束，假设对控制量无约束

3.2 控制 算法概述

（1）PID
PID：调试比例增益、积分增益、微分增益来达到想要的控制效果
在这里插入图片描述
调试PID经验参考文章：
阿克曼小车位置式PID调试经验
pid的参数调试方法以及个人理解

（2）Pure pursuit
预瞄距离：后轴中心到目标路点的距离
$\alpha$ ：后轴中心到目标路点与车辆纵轴之间的夹角

纯追踪算法：基于自行车模型，以车辆后轴中心为切点，车辆纵向车身为切线。通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶
${\delta (t)=arctan(\frac{2Lsin\alpha (t)}{Ld} )} }$

（3）Stanley
核心思想：基于前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算

前轮转角控制量： $\delta =\theta _{\varphi }+\theta _{y }$
$\theta _{\varphi }$ ：当前车身方向与参考轨迹最近的点的切线方向的夹角
$\theta _{y }$ ：前轮中心到参考轨迹最近点的横向距离 $e_{y}$

控制器设计：
$\delta(k)=\theta _{e}(k)+arctan\frac{ke(k)}{v(k)}$
其中， $\theta _{e}(k)$ 为k时刻的航向角偏差，e(k)为横向跟踪误差，k为需要调节的参数，v为无人车当前速度。

输入：当前车辆位置、航向角 $ψ$ 、速度 $v$ 、当前目标路点和离车辆前轴中心最近目标路径点的航向角 $ψ_t$
计算横向误差 $e_y$
计算 $δ=ψ_t−ψ+arctan\frac{ke_y}{v}$
输出：前轮转角控制量 $\delta$

（4）LQR
离散系统：
$x (K + 1) = A x (k) + B u (k)$
代价函数：
在这里插入图片描述
设计步骤：
① 确定迭代范围N
② 设置迭代初始值 $P_N=Q$
③ $t = N, ..., 1$ ，从后向前循环迭代求解离散时间的代数Riccati方程
$P_{t-1}=Q+A^TP_tA-A^TP_tB(R+B^TP_{t+1}B)^{-1}B^TP_tA$
④ $t = 0, ..., N$ 循环计算反馈系数 $K_t=(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A$ 得到控制量 $u_t=-K_tx_t$

主要步骤：
（1）确定迭代范围N，预设精度EPS
（2）设置迭代初始值P = Qf，Qf = Q
（3）循环迭代， $t = 1, ..., N$
$P _{new} =Q+A ^TPA−A ^TPB(R+B ^T PB) ^{−1}B ^TPA$
若 $P_{new}-P||<EPS$ ：跳出循环；否则： $P=P_{new}$
（4）计算反馈系数 $K=(R + B^TP_{new}B)^{-1}B^TP_{new}A$
（5）最终的优化控制量 $u^*=-Kx$

（5）MPC
核心思想：利用三维的空间模型加上时间构成四维时空模型，然后在这个时空模型上求解最优控制器。

因为理论构建的模型与系统真实模型有误差；从而，更远未来的控制输出对系统控制的价值很低，MPC仅执行输出序列的第一个控制输出

模型预测控制在k时刻共有三步：
① 获取系统当前状态
② 基于u(k)，u(k+1)，u(k+2)，…，u(k+m)进行最优化处理
③ 只取u(k)作为控制输入施加在系统上
在下一时刻重复上面三步，在下一步进行预测时使用的就是下一步的状态值，我们将这样的方案称为滚动优化控制

4 控制 算法面试问题整理

（1）现在企业里的控制都是用什么做呢？

控制的话可能更多还是二自由度动力学模型+求解线性化后的QP问题

（2）为什么规划需要给控制输出一条尽可能平滑的曲线，控制不也是根据离散点来做的，比如纯跟踪这种做法，不也是选取预瞄点去跟踪？

车辆运动是连续性的动作，假如规划的曲线不平滑，这里的不平滑更多的值这曲线不符合车辆运动学，那控制跟踪计算的转角可能不连续。比如纯跟踪，上一帧选取的参考点计算出一个对应转角，下一帧的曲线上有个跳点（不平滑），计算出的转角较上一帧有较大变化，造成不舒适。
规划轨迹当然越平滑越好但实车运行需要考虑耗时平滑会耗费几十到上百毫秒，另一方面要参考控制是单点还是多点预瞄多点预瞄对规划轨迹的平滑程度要求相对来说不高其连续性约束相当于做了一次平滑能有效解决期望输出跳变的问题。
希望规划平滑的另一个原因是：控制模块做的是个轨迹跟踪的功能，一般不会去施加额外的安全性约束，因此规划给出的轨迹是否足够好的标准之一就是控制是否能跟得上，如果轨迹不平滑导致控制根本跟踪不上，那整个安全性假设就不成立了。预瞄点一般要跟车速关联，车速高的时候要相对预瞄地远一点，保证整体控制的稳定性，但相应地控制误差可能会大一点。

（3）实车控制问题若采用mpc 做，中间产生无解或是违反约束的情况怎么解决

可能要看下是违反了什么约束，如果是碰撞或者动力学上限这类的硬约束导致了无解，那可能需要额外的备份规划器来输出应急行为了，如果是速度限制这种约束导致无解可以考虑把他变成高权重的软约束，可以缓解无解的情况。备份规划器一般来说就是AES和AEB的领域了，方法都是类似的，不过设计目标是可以牺牲一定舒适性来消除短时域内的碰撞风险(1~2s)。你所应用的赛车控制问题应该不需要这个，还是检查检查有没有约束设计的不合理吧

（4）mpc做控制或者局部路径规划，这两者的区别

mpc优化问题的形式本身就可以理解为是一个规划问题，优化求解出来的最优状态序列可以理解为一条轨迹，而作为控制器使用的时候就只执行第一个周期内的控制指令，这是主要区别。

（5）MPC在低速场景（0.1m/s），进行路径跟踪，是否无法进行？

适用的，只是车辆动力学在低速场景下表现不好，如果是运动学模型+MPC没问题

（6）基于mpc方法能做泊车场景吗？跟图搜索和随机采样的方法有什么不同?为什么很多泊车场景很少用mpc去做？

搜索和采样只是分辨率下最优和概率最优，而优化可以做到连续空间下的最优，但问题就在于非凸问题的优化没那么好做，很容易陷入不好的局部最优解。理论上说肯定是mpc这种优化方法效果更好且理论更完备。

（7）经典控制理论：PID、ACC
现代控制理论：LQR、轨迹跟踪

（8）纵向的油门刹车标定表如何建立

理想的测试地点是平坦的长直路，且两边没有高大的建筑物遮挡。保证定位精度，这样标定的准确度才够。
标定目的：找到油门，速度，加速度之间的一个对应关系

参考：（八）实践出真知——apollo刹车油门标定

（9）误差状态方程的右边还有一项关于参考路径的航向角速率，但是 LQR 控制器中并没有考虑他对状态量的影响，那应该如何解决呢

增加前馈

（10）线控转向系统的控制延迟如何处理

引入预瞄，在偏差计算不计算当前时刻的偏差，而是计算当前时刻0.1-0.2s之后的偏差

（11）讲一讲自动驾驶中的一些坐标系以及转换关系
笛卡尔
Frenet
转换关系见：Apollo基础 - Frenet坐标系
（12）LQR怎么处理非线性目标和约束

将非线性系统线性化，约束的处理需要使用扩展的LQR方法：NMPC、NLQR

（13）自行车模型中具体是如何实现的？
在这里插入图片描述
（14）在实际问题中，应该根据什么去选择不同的控制算法进行控制器的设计？

首先要考虑的是系统的类型、性能要求、稳定性要求、精度要求、成本要求等。具体可以考虑以下几个方面：
①系统的类型：线性系统还是非线性系统，连续系统还是离散系统，是否有时间延迟等。
②稳定性要求：对于一些对稳定性要求较高的系统，可能需要使用更为复杂但具有更好稳定性的控制算法，如LQR、PID等。
③性能要求：考虑系统动态性能的要求，如指标响应时间、超调量大小等。如果要求响应速度快，需要考虑如状态反馈，模型预测控制等方式；对超调量有严格限制的，可能会比较适合使用比例-积分-微分方式的PID控制器。
④精度要求：对于需要高精度控制的系统，可能需要使用更为精细的控制方法。
⑤成本要求：对于需要考虑成本的应用，可能需要选择能满足需求但成本较低的算法。高级控制策略（如模型预测控制）可能会提供更好的控制性能，但可能需要更高的计算资源。
⑥实施的难易程度：有的控制算法可能理论上很优秀，但实施起来特别困难，这种情况下需要权衡其操作难度和收益。
最终，选择哪种控制算法需要根据具体系统的特性和对系统性能的要求来进行综合考虑和权衡。

（15）什么是自动控制？为什么要进行频域和时域的分析？各自的分析方法有哪些？

自动控制通常涉及到一些作用元件（即控制器）能按照一定的策略自动地调整控制输入，以使得系统的输出达到预期的目标。
频域和时域的分析是观察系统行为的不同角度
①时域分析：主要研究系统对单位阶跃、单位冲激等时间函数的响应，求解系统的时间响应并分析系统的稳定性、快速性、超调等性能指标。一般方法包括微分方程求解、根轨迹法、极点零点法等。
②频域分析：主要研究系统在各个频率下的响应特性，如振幅特性和相位特性等，并用来分析和设计控制系统的稳定性、鲁棒性。常用的方法有频率响应法、Nyquist稳定判据、Bode图法等。

为什么要进行这两种分析，主要是因为它们各自关注了系统的不同特性：时域分析关注系统响应随时间如何变化、系统能否在有限时间内达到稳定状态，频域分析关注系统对不同频率信号的响应特性，使我们能深入理解系统对输入信号的灵敏度以及在不同频率下的系统性能。通过时域和频域分析，可以全面了解系统的动态行为，为系统的设计和调整提供依据。

（16）QP一定是凸问题吗？