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

1 控制理论基础

1.1 控制模块概述

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

CANBUS ：车辆底盘交互协议

参考博客：Apollo CANBUS模块解析
apollo：canbus模块（1）引言

底盘、速度、四轮转速、健康状况、底盘报错、自动驾驶状态

运动学模型 ：刚体运动（低速）
动力学模型 ：力学分析（中高速）

详细推导见：车辆运动学和动力学模型

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

控制模块：

(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）控制基础

1. 时间域

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

2. 频率域

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

3. 离散域（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 α：后轴中心到目标路点与车辆纵轴之间的夹角

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

（3）Stanley

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

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

控制器设计：
δ ( k ) = θ e ( k ) + a r c t a n k e ( k ) v ( k ) \delta(k)=\theta _{e}(k)+arctan\frac{ke(k)}{v(k)} δ(k)=θe(k)+arctanv(k)ke(k)

其中， θ e ( k ) \theta _{e}(k) θe(k)为k时刻的航向角偏差，e(k)为横向跟踪误差，k为需要调节的参数，v为无人车当前速度。

输入：当前车辆位置、航向角 ψ ψ ψ、速度 v v v、当前目标路点和离车辆前轴中心最近目标路径点的航向角 ψ t ψ_t ψt

计算横向误差 e y e_y ey

计算 δ = ψ t − ψ + a r c t a n k e y v δ=ψ_t−ψ+arctan\frac{ke_y}{v} δ=ψt−ψ+arctanvkey

输出：前轮转角控制量 δ \delta δ

（4）LQR

离散系统：
x ( K + 1 ) = A x ( k ) + B u ( k ) x(K+1)=Ax(k)+Bu(k) x(K+1)=Ax(k)+Bu(k)

代价函数：

设计步骤：

① 确定迭代范围N

② 设置迭代初始值 P N = Q P_N=Q PN=Q

③ t = N , . . . , 1 t=N,...,1 t=N,...,1，从后向前循环迭代求解离散时间的代数Riccati方程
P t − 1 = Q + A T P t A − A T P t B ( R + B T P t + 1 B ) − 1 B T P t A P_{t-1}=Q+A^TP_tA-A^TP_tB(R+B^TP_{t+1}B)^{-1}B^TP_tA Pt−1=Q+ATPtA−ATPtB(R+BTPt+1B)−1BTPtA

④ t = 0 , . . . , N t=0,...,N t=0,...,N循环计算反馈系数 K t = ( R + B T P t + 1 B ) − 1 B T P t + 1 A K_t=(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A Kt=(R+BTPt+1B)−1BTPt+1A 得到控制量 u t = − K t x t u_t=-K_tx_t ut=−Ktxt

主要步骤：

（1）确定迭代范围N，预设精度EPS

（2）设置迭代初始值P = Qf，Qf = Q

（3）循环迭代， t = 1 , . . . , N t=1,...,N t=1,...,N
P n e w = Q + A T P A − A T P B ( R + B T P B ) − 1 B T P A P _{new} =Q+A ^TPA−A ^TPB(R+B ^T PB) ^{−1}B ^TPA Pnew=Q+ATPA−ATPB(R+BTPB)−1BTPA

若 ∣ ∣ P n e w − P ∣ ∣ < E P S ||P_{new}-P||<EPS ∣∣Pnew−P∣∣<EPS：跳出循环；否则： P = P n e w P=P_{new} P=Pnew

（4）计算反馈系数 K = ( R + B T P n e w B ) − 1 B T P n e w A K=(R + B^TP_{new}B)^{-1}B^TP_{new}A K=(R+BTPnewB)−1BTPnewA

（5）最终的优化控制量 u ∗ = − K x u^*=-Kx 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一定是凸问题吗？

QP问题不一定是凸问题。判断QP是不是凸的关键在于Hessian矩阵，如果Hessian矩阵半正定，那么QP是凸问题，否则是非凸问题。

（17）车辆是如何进行执行横向或者纵向控制的，如何匹配使得车辆跟踪上目标点？

（18）简述MPC开发过程

模型预测控制在k时刻共有三步：

① 获取系统当前状态

② 基于u(k)，u(k+1)，u(k+2)，...，u(k+m)进行最优化处理

③ 只取u(k)作为控制输入施加在系统上

在下一时刻重复上面三步，在下一步进行预测时使用的就是下一步的状态值

（19）简述MPC算法原理

在每一个采用时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻，重复上述过程：用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解 。

（20）讲一讲LQR和MPC的不同点，以及相应的应用场景

主要不同在于MPC有很多约束，求解的方法更加复杂。在应用上MPC多用于纵向控制，LQR多用于横向控制。

（21）自动驾驶中车辆有哪些模型，是如何建模的？

自动驾驶中通常考虑车辆的运动学模型和动力学模型，运动学模型中有自行车模型，分为前轮，后轮和质心模型。

（22）控制模块的输入输出是什么？和规划是什么关系

输入：轨迹线、地图信息、定位信息、车辆反馈信息

输出：刹车、油门、转向

规划和控制是上下游的关系

（23）你的算法在实际测试中发现的三个头部问题是什么，然后你是怎么解决的

（24）常见的碰撞检测方法
常见的碰撞检测方法
碰撞检测的向量实现

外接圆碰撞检测、AABC、OBB、GJK、EPA、比较面积的方法

（25）自己运用强化学习算法多点还是传统控制算法多点

传统算法多点，PID、LQR、MPC、Pure pursuit、Stanley

（26）在巡航控制算法中用的PID算法，如何根据被控对象设计算法，思路是什么

巡航控制算法使用PID思路：

（1）确定被控对象

（2）确定控制目标

（3）建立数学模型

（4）设计PID算法

（5）实现算法

（6）调试与优化

（27）你做的都是时间域上的控制，频域上的控制了解吗

频域控制通过调整系统的频率响应来实现对系统的控制，在实际应用中，需要根据系统的特性和性能要求，选择合适的频域控制策略，并对其进行实现和优化

（28）控制算法最看重的指标是什么，如何衡量这个指标

我觉得控制算法最看重的指标是控制精度，纵向控制看是否到达预定的位置和速度，横向控制看横向误差，航向误差是否到达预想的目标

（29）给出一个定义好的类，请用多态调用的方式调用这个类，写出一个PD位置控制算法

（30）在工程上经常会出现电机突然不转的情况，这时候会出现积分饱和，请问如何抵抗积分饱和，说出你的思路，并写代码解决

（31）假设一辆车沿一条直路向前行驶，初始位置为0m，速度为0m/s，每一秒该车都可以选择一次加速度： + 2 m / s 2 +2m/s^2 +2m/s2， − 2 m / s 2 -2m/s^2 −2m/s2，或者 0 m / s 2 0m/s^2 0m/s2。该车在1秒内开过的路程是(v(t)+v(t+1))/2m。终点在1000m处, 且到达终点时速度恰好为0m/s。路上有若干处限速，请问如何加减速才能保证最快到达终点且到达终点时速度为0m/s。

思路：编一个类似仿真的环境，一步=1秒；每一步都依次考察+1，0，-1 三种加速度是否满足限速要求。一旦找到满足要求的加速度，就不用再考察更慢的决策了，直接执行既可。

#include <iostream>
#include <cstdio>
#include <unordered_map>
#include <math.h>
#include <array>
 
 
int main()
{
    std::unordered_map<int, int> v_limit_vec{ {100,5}, {405,20} ,{1000,2} }; //<speed limit position, speed limit value>
    int pos = 0; //initial position
    int t = 0;//time
    int v = 0;//initial speed
    int v_next = 0; //next step speed
    int p_next = 0; //next step position
    std::array<int,3> a_vec{ 1,0,-1 };// three actions
    while (pos<1000){
        for (int a : a_vec){ //start with +1, then 0, then -1
            v_next = v + a;
            if (v_next < 0) {
                v_next = 0;
            }
            p_next = (v_next + v) / 2 + pos;
 
            bool action_good = true; //once find good action, break action loop
            for (std::pair<int,int> the_pair : v_limit_vec) {//loop over speed limits
                int v_lim_pos=the_pair.first; //get speed limit position
                if (v_lim_pos - pos <= 0)  {
                    continue;
                }
                int v_lim_v=the_pair.second; //get speed limit speed
                if (v_lim_pos - p_next > 0) { //if potential speeding at future step
 
                    //compute the acc of constant deceleration to satisfy speed limit
                    float acc = (static_cast<float>(v_lim_v) * v_lim_v - v_next * v_next) / (2 * (v_lim_pos - p_next));
                    if (acc < -1) {
                        action_good = false;
                    }
                } 
                else { //if speeding at current step
                    if (v_next > v_lim_v) {
                        action_good = false;
                    }
                }
                if (!action_good) { 
                    break; //this action does not satisfy this speed limit, no need to check other speed limits.
                }
            }
            if (action_good) {
                break;
            }
        }
        //simulate one step
        t = t + 1;
        v = v_next;
        pos = p_next;
    }
    std::cout << t <<std::endl;
    std::cout << v << std::endl;
    std::cout << pos << std::endl;
}

（32）轮胎的侧偏特性，侧偏角产生的原因

轮胎的侧偏特性是指当轮胎在行驶过程中，由于外部因素（如道路不平、车辆转向等）的影响，使得轮胎的侧面与地面之间的接触点产生相对滑动的现象。这种现象会导致轮胎的侧偏角产生，进而影响车辆的行驶稳定性和操控性能。

（33）当车辆上下乘客（车重发生变化时）对模型参数辨识带来的困难

当车辆上下乘客时，车辆的重量会发生变化，这可能会对模型参数的辨识带来一定的困难。具体表现在以下几个方面：

• 车辆动态特性的变化：车辆的重量变化会直接影响车辆的动态特性，如车辆的悬挂系统、刹车系统等。这将导致模型参数的变化，从而影响模型的预测精度和稳定性。
• 模型参数的不稳定性：当车辆上下乘客时，车辆的重量会发生变化，这可能导致模型参数的不稳定。例如，车辆的质心位置、悬挂系统的刚度等参数可能会随着车辆重量的变化而发生变化，从而影响模型的参数辨识。
• 参数辨识的复杂性：当车辆上下乘客时，车辆的动态特性可能会发生显著的变化，这将使得参数辨识问题变得更加复杂。例如，在车辆重量变化的情况下，需要更大量的实测数据来准确辨识模型参数，以保证模型的预测精度和稳定性。
  为了克服这些困难，可以采取以下一些措施：

1. 在模型设计和参数辨识过程中，考虑车辆重量变化对模型参数的影响，以提高模型的鲁棒性和稳定性。
2. 采用在线参数辨识方法，根据实测数据动态调整模型参数，以适应车辆重量的变化。
3. 通过合理的实验设计和数据处理方法，减小车辆重量变化对参数辨识的影响，提高参数辨识的准确性和稳定性。

（34）python面试常问的问题
算法工程师面经 ------ python 面试常问问题