self_drive car_学习笔记--第11课：控制理论

前言：这节课，主要介绍了无人车里面基本控制理论，然后介绍常用基本级控制器PID，也介绍以下进阶版本控制器，最后介绍一下matlab里面的控制工具箱。很菜现在，有些理解错误地方，还望大大们不吝赐教。觉得写得还行，麻烦赏个赞哈。好了，不瞎扯了，开始主题。

概要
1控制理论基础
2 PID控制器
3进阶控制器设计
4 MATLAB控制工具箱

---

1 控制理论基础

1.1 control基本介绍

【control目前，起到接管与自动驾驶转换的关键点；control直接与车协议沟通的，图中其他模块没有这种功能】【图中没有显示的就是，HD map对控制也有一定的作用】【control从planning接收轨迹参考信息，从localization接收自身定位信息，从HD Map接收周围环境信息，以及从图中没有显示的车底盘获取反馈信息，综合处理上述信息后，输出油门、刹车、方向盘信息】

1.2control控制模块

【淡茶绿色方框内的canbus，其是一个控制和车辆底层紧密相关的模块；在canbus设置好相应的协议，control根据协议发送指令，实现对车辆的控制】

1）输入：
----轨迹线reference【速度、加速度、位置】
----定位信息【自身位置状态】
----地图信息【环境信息，自身在环境位置信息】
----车辆反馈信息

2）输出：
油门、刹车、方向盘

3）canbus:车辆底盘交互协议（由车厂决定）
----Chassis（底盘）
----速度、四轮转速
----底盘健康状况
----底盘报错
----自动驾驶状态

4）仿真：
----运动学模型【车，根据轨迹以及运动方向，判断该车下一时刻会出现在哪里】
----动力学模型【车，在整个环境中的受力情况，包括干扰、风力、摩擦系数等造成的力学信息】

1.3 control控制任务


【上图左，表示轨迹点有位置、速度、加速度信息；上图中表示比较好的control，因为基本符合planning要求；上图右control就是比较差的，因为偏离planning越来越大，发散型control是危险的】
【下图左1，橙红色控制轨迹不合理的；下图左2控制轨迹也是不合理的，图中的三个点距离非常短，表示速度减少非常快，属于急刹，舒适性没有保证；下图左3的控制轨迹也是不合理的，因为其是严重摇摆影响舒适性，没有把累计的误差保证在一定范围导致严重摇摆的发生，业界称之为画龙现象（可能planning给的轨迹正常，但是control没有跟上，导致摇摆发生）；下图左4的控制轨迹基本合理】

1）【跟踪好】Planning的轨迹信息

2）不同环境下的轨迹跟踪
----雨雪、陡坡、石头路【这些细微因素使得轨迹跟踪变得不简单；由于planning并不会对这些信息进行处理，只是control来处理】

3）保证车辆可行性

4）保证车辆舒适性

5）实时性要求高（100Hz）【跟定位实时性要求一个级别，要求非常高】【对应时间复杂度比较高的算法，也就是运行时间比较长的算法，并不适合在control使用】

6）控制重要的原因：
----控制是对车辆油门、刹车、方向盘的精细控制
----是安全行驶的最后关卡
----克服外界各种不确定性环境因素（风速、湿滑）

7）控制模块：让车辆在既定轨迹上平稳运行的学问

控制问题很复杂，是一套严格的数学分支，本课程以下原则介绍控制：
----简化问题
----重点介绍概念性知识
----少用数学公式（推理、证明）

1.4 控制模块细分

1）预处理
----数据清洗：不正常信号、planning异常值【对于异常数据，去除或者报错】
----合理性检查：定位信息、车辆底盘信号

----紧急处理：前方突然出现障碍物（emergency stop）【例如无人车前侧安装的超声传感器，直接连接到control模块，应付紧急障碍物】
----信号smooth：去除信号噪声【传感器的信号都是带有噪声】，lag Compensation【滞后信号补偿，这是由于信号传递过程中，信号传递过程中有些滞后，需要做一些补偿，消除一些传递误差】

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

3）control控制器设计：
----建模【建立一个车辆模型，目的提供一个控制对象】
----系统参数辨识【控制可修复一些细微细小的错误，而这些错误对应的值是无法精确得到，这时候就需要系统辨识，利用一些方法，猜算出这些值也就是对应的系统参数】

----控制观测器设计【观测器，指的是有些控制方法在反馈的基础上，要添加更多观测信息，才能确定其反馈值是否符合控制输入；控制观测器可理解为整个控制器的一套方法；其设计非常重要，决定控制器性能】
----参数Fine Tuning【参数调节，与深度学习的超参的调差方式（一般是试验试错方式）不一样，而其一般是依据一些数学原理对参数调整进行指导，但也有试错试验方式来调整的】

1.5 控制被需要的原因

1）控制系统：消除数学模型和物理模型差异性的过程
【现实中的物理结构受很多复杂性因素影响，不能直接运行完美的数学结果；完美的数学结果与复杂的物理结构如何达到一个匹配，就是通过控制系统；控制系统就是缩小数学模型与物理模型的误差】【任何机械任务，数学模型看上去都是很美好的，但到机器层就会出现很多问题】

2）对于无人车：

----stability：稳定性，对于任何环境都需要稳定（安全）【稳定是安全的前提】
----稳态误差：尽可能小，保证执行上层命令【控制实际运行的偏差范围】

----动态误差：在动态工程中也需要保证很多性能
【上述的说的左右摇摆的画龙现象就是动态误差较大情况；特别在乘用车上，动态误差要求更高（舒适性要求高），而且比较难，因为其需要一些比较复杂的东西才能保证动态误差】

参考 J Doyle,Feedback Control Theory【http://static.gest.unipd.it/~oboe/psc/testi/dft.pdf】
【y(t)表示控制系统的输出；虚线表示控制系统理想状态下达到的位置（我们期望的）；t表示时间】【上图中的图（b）就是我们追求的控制系统效果；此外，a到达比较慢，c波动但没有收敛，d波动呈发散属于最不好的控制】

1.6 控制理论基础

参考 J Doyle,Feedback Control Theory 【http://static.gest.unipd.it/~oboe/psc/testi/dft.pdf】
【上左侧用于理解时间域；上右侧用于理解频率域，其横坐标表示频率，纵坐标表示增益】
【上右图中，波峰对应的是一个共振频率】

1）控制系统：

1-1）时间域（time domain）
----超调量：指的是目标值与可达到的最高位置的范围
----超调时间：指到达最高超调量所花时间

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

1-2）频率域（frequency domain）
----通过频率（pass band）：通过（增益大于一定程度）对应的频率
----停止频率（stop band）：增益下降到一定值所对应的频率

----带宽：增益上升和增益下降对应的频率范围大小
----截至频率：表示增益达到给定的截止值时所对应的频率

【时间域可通过傅里叶或者拉普拉斯变换，变成频率域】

1-3）离散域（z变换）

2)傅里叶变换（FT，FFT）
时域–>频域转换
【傅里叶变换，可以将任何信号转换成使用sin和cos的加权表示的】【任何信号都是可拆分为傅里叶变换】

3）拉普拉斯变换
----时域->频域

4）傅里叶变换（FT，FFT）

----信号–>正弦转换


右侧图参考：https://en.wikipedia.org/wiki/Fourier_series
【上表中，拉氏变换（频域）、时间函数（时域）、Z变换（离散域），三者是可以相互转换；时间函数相对比较常用，Google或百度搜索拉氏变换应该就用了】【右侧图表示的是傅里叶变换，不断叠加】

5）三域小结：
----控制系统三域是可以相互变换的
--------不同领域对特定的需求有不同的解法
---------控制算法也正对不同域有不同优势
---------三域认识，可扩展了人对物理系统的深刻理解

2 PID控制器

2.1 PID基本介绍

参考：Introduction:PID controller design

1）PID：【所有控制系统中】最简单但是应用最广最有效的控制器
2)Model Free
----通用性，任何模型都可以
----【PID属于】反馈控制
----输入可以是速度、位置等

3）PID控制器设计很简单【对系统没有要求，基本上没有什么控制系统，PID是搞不掂的】

2.2 PID解析

1）P

1-1）P：proportional比例
----跟误差成正比，误差越大，作用越强

1-2）容易出现震荡，无法收敛
----【我们希望】越靠近目标，应该作用越小，才能稳定
----注意在误差为0时候，控制器的输出也是0


2）D

2-1）D：derivative微分
----阻尼项，减少控制器输出的变化率

2-2）【D控制器】考虑将来误差，计算误差的一阶导，并和一个正常值的常数相乘
----微分控制可以提升整定时间及系统稳定性

【d,适合变化比较快的系统，例如波士顿机械狗底层就是pd控制；如果在变化不快系统使用d，会造成一些尾部的恒定误差，产生一些震荡，尾部很难收敛；由于无人车希望的是慢慢变，尽量平滑，所以d信号使用不多】

3）I

3-1）I：integral积分
----纠正系统累计偏差
----缩小稳态误差

3-2）【I控制】考虑过去的误差，将误差值过去一段时间和（误差和）乘以一个正值的常数
----积分控制会加速系统趋近设定值的过程，并且消除纯比例控制器会出现的稳态误差

【单纯pd控制时候，容易造成一个恒定（稳态）误差，如上图黄曲线距离直行大小；这时候就需要一个I控制，其可以将稳态误差收敛】

2.3 PID小结

1）PID控制器是世界上【综合性能】最好用的控制器

2）“没有什么是PID解决不了的”
----需要的信息少：当前状态、目标
----无人车使用pid控制方向和加速度基本满足

3）PID是线性控制器【性质决定了其不足】
----【单纯使用pid】复杂系统是不够的：四轴飞行器、多自由度机器人

参考：https://zh.wikipedia.org/wiki/File:PID_Compensation_Animated.gif
参考Kiam Heong Ang etc. PID Control system analysis,design,and technology,2005
【https://www.researchgate.net/publication/3207704_PID_control_system_analysis_and_design/link/02e7e51a8302235425000000/download】
【PID的调参不是一个简单事情】

2.4 常见PID种类

1）根据控制器输出和执行机构的关系划分：
----位置式PID
----增量式PID

1-1）位置式：输出控制量大小的实际值

1-2）增量式：输出只是控制量的增量
----递推式算法

【上式子为位置式pid;下式为增量式pid】

1-3）无人车应用：
----速度使用的是位置式
----方向盘使用的是位置式

2)串联PID

2-1）PID控制器对单一变量的控制是非常有效的
----无人车可以控制量：速度、加速度、位置、heading

2-2）串联控制作用
----改善控制质量的有效方法
----应对多变量控制和多干扰项

2-3）外环控制器的输出作为内环控制器的设定值
----内环控制器的输出去操纵控制阀

【虚线框表示内环控制器；调参时，先内环再外环】

3 进阶控制器设计

3.1 复杂控制器设计

1）控制任务
----按照planning要求到达目标
----适应不同路况
----适配不同车型
【这就是pid的弊端，在不同的环境，需要重新调参才能获取最佳效果】【这也是无人车难以广泛应用的原因之一】

2）动力学建模
----横向模型和纵向模型

【简单的PID利用的信息不充分，连控制对象是什么都不知道，只是简单利用数学方法来做，所以需要添加更多信息，控制器才能做的更好。因此，这里复杂控制器设计添加了动力学建模】
【纵向模型相对简单，因为车是一般是笔直往前开的，只是加速和减速，存在一些小误差也影响不大，所以相对简单，基本一个PI控制就能够满足基本要求；横向模型，不是指打方向盘的横向，而是要考虑加减速、轮胎转角的横向（这些映射到油门方向盘的控制），在无人车控制里面属于比较复杂而具有挑战性的工作】

3）控制器设计
----横向控制和纵向控制

3.2 车辆模型

1）模型表征一个车辆的物理特性
----单车模型

2）评估每一个控制输入应用于车辆后会发生什么

【车状态+控制输入–>车辆模型–>（估计）车辆轨迹】

3.3 单车模型

1）主要物理定律


【牛顿第二定律F=ma;刚体力矩分析，前轮转角稳定也就是转向力矩要一致；后轮转角，其左右转角稳定，符合转向角速度；横向没有侧向位移的，除非打滑】

2）横向动力学

【公式中线性化，抖动角度不大情况下，取cos近似为1简化】

3）小误差模型


【对于xy坐标系做近似会产生较大误差；这里再path坐标系下做的近似】【如果没有做近似的系统，复杂度非常高，基本上没法求解，或者计算量过大导致无法实用】【做近似时，需要一定的合理性】

4）模型表征一个车辆的物理属性
----单车模型

5）评估每一个控制输入应用于车辆后会发生什么

6）横向动力学模型
----heading误差
----横向误差


【图上方表示的是heading误差例子；下方展示的是横向误差；横向误差需要消除】

7）横向模型推导参考：
http://intheworld.win/2018/11/24/lqr%E4%B8%8E%E6%B1%BD%E8%BD%A6%E6%A8%AA%E5%90%91%E5%8A%A8%E5%8A%9B%E5%AD%A6/


参考：Rajamani,Vehicle dynamics and control.2011
【式子表示的是简化的横向动力学模型表示，上下两公式可以参数匹对的】

3.4 系统辨识模型

1）存在缘由：系统有些参数是不清楚【比如前轮转角和方向盘转角的实际关系，这时候就需要实验方法来获取】

2）系统辨识：通过实验的方法采集参数
----White Box:汽车大多是白箱辨识
----Black Box:有点像神经网络
----Grey Box

3)Learning-Based 方法【进行系统辨识】
----机器学习
【Apollo目前就应用到一些机器学习方法来做系统辨识，获取不清楚参数】

3.5 最优控制理论

【kalman fiter卡尔曼滤波，在控制领域被称为线性二次估计器】
【LQR，线性二次调节器，最基础的控制方法，好用且成熟，直接代入公式求解就可以了】
【LQE+LQR->LQG,LQG被称为线性二次高斯型】

【LQG+constraint条件限制->LMPC,LMPC线性模型预测控制器】
【LMPC+nonlinearity非线性条件+优化->MPC，MPC模型预测控制】
【整个框图是控制的优化领域概括】

1）optimal control【基本思想】
----给定一个系统和环境扰动
----根据需求条件（Objective）
----实现最优的控制量决策

2）复杂度从左往右递增
【控制最重要的是实时性，结合上图，所以并不是越往右侧控制越好，如果做一些简化能达到实时性的MPC也是可以使用的】

3）精准度从左往右递增

3.6 LQR（线性二次调节器）

1）LQR=linear quadratic regulator

2）基于模型的控制器
----通过车辆状态和模型推理，使得误差最小
----可用于设计横向控制器

3）状态X：
----横向误差及其变化率
----heading误差及其变化率
【横向误差及其变化率、heading误差及其变化率，如图x数组从上到下依次排列】

4）控制量U：
----方向盘（steering）
----油门（throttle）
----刹车（brake）
【如图中u的数组从上到下依次排列】

5）LQR状态方程
----线性等式（L）

【式子中，x表示状态，u表示控制量；状态x就是表示误差，u就是输入;状态与输入之和对时间积分最小就是cost】
【cost写成积分形式，也就是二次型原因是，求解方便，由于误差有正负，平方之和的误差最小才是真正误差最小】

6）尽可能减少误差，同时使用最小输入量U
----可以做到省油、平滑等特性

7）cost函数
----Q和R为正定矩阵
【Q、R表示的是使得两者之和最小，误差以及输入量对应的权重】【Q也可为半正定矩阵】
【R比较大，意味着，更加关注让输入小一些，此时状态衰减会慢一些】

8）最优控制理论的方法求解LQR

8-1）最小cost的解为：u=-Kx 【这个是数学推导得出的】

8-2）其中，K的公式：

----无人车中使用LQR的N一般为0

8-3）上述公式，P是连续空间中Riccati方程的解：

【Riccati方程在控制领域比较重要的，使用也比较广泛】

8-4）上述的是LQR的连续空间求解方法，下面是离散空间的表达式：

3.7 MPC（模型预测控制）

1）其是更为复杂控制器

2）基本步骤：

----建立模型
----计算有限时间范围内的最优控制输入

----执行第一组控制输入
【LQR没有时间限制，其累加当前状态下，未来所有环境内，所要达到最优的效果】

3）迭代优化的思想：

----先计算一系列的控制输入并优化该序列输入
----执行第一组控制输入

----循环执行上面两步
【不直接计算所有序列，选择迭代原因是，保证实时性和精确性；模型中做了近似，如果一次性执行所有，会造成很大误差，每次序列优化，选择迭代方式计算可以获取较好效果】

4）LQR VS MPC
----LQR针对固定的时间区间（一般为无限长时间）进行最佳化
----MPC针对不同的时间区间分别进行最佳化

5）时效性vs准确性
【MPC考虑未来时间越长，效果越好，当然耗费时间会越多】

6）【MPC也是一个】搜索问题
----通过车辆动力学模型来减少解空间

7）模型、约束、优化组合【MPC可理解为这三者的结合】

【cost 的求解使用的是QP】

【MPC相对于PID来说，比较精准；并且，MPC可以设定不同的cost，当然对应的计算过程更加复杂】

8）MPC控制流程图

8-1）预测系统
----车辆动力学模型

8-2）滚动优化【表示上流程图的优化方式】

8-3）反馈矫正

【MPC比PID优秀的原因，PID只是针对此刻而没有考虑未来】【比如给定一个轨迹进行实际拟合，PID可能是围着给定轨迹进行类似画龙方式进行拟合；而MPC会考虑综合误差最小方式慢慢拟合，也就是不会一次性拟合过头这些，这就是MPC和PID的拟合核心思想区别之处】

【画龙波浪线表示PID方式拟合；慢慢拟合是MPC方式拟合】

9）MPC车辆误差跟踪状态方程

【x矩阵，从上往下看，横向误差及其变化率，heading的角度误差及其变化率？，位置误差及其变化率（位置和速度）】
【A矩阵对应的是一个简化模型获取的】

【控制量u矩阵，从上往下，第一个为转向速度（对应方向盘），第二个是加速度（对应油门）】
【C的右侧矩阵，从上往下，第一个参数表示横向角变化率，第二个参数表示加速度参考值】
【MPC是可以不解耦的，包含横、纵向误差，可以直接输出两者的综合者】

9-1）X
----横向误差
----heading误差
----位置误差
----速度误差

9-2）U
----转向角
----加速度

9-3）C（扰动）
----横向角变化率
----加速度参考值

9-4）离散形式

10）MPC 未来N个周期的状态方程预测

【迭代预测、滚动计算结合】

10-1）目标函数

【目标函数使控制量、控制误差最小】

10-2）约束

【t 时刻控制量的大小范围；这里也是使用二次规划QP求解】

10-3）二次规划形式

10-4）求解出一组控制序列

【MPC是比较主要的控制方式，也是最优控制思想】

【控制领域比较成熟，一般利用现成公式以及工具，不需要怎么修改参数就可以了】

4 MATLAB控制工具箱

【matlab是做控制的必备工具；实际控制开发不使用python做的】

4.1 APOLLO控制算法

1）纵向PID控制器

2）横向LQR控制器【实际加了约束，所以更像是LMPC】

3）MPC控制器【其优势是可以处理非线性模型，可以使用QP求解出非线性最优解】

4）Apollo控制源码简单解读
【https://github.com/ApolloAuto/apollo/tree/master/modules/control/controller】

4.2 Matlab控制算法工具箱

1）Matlab

2)Simulink【位于matlab里面的】【下面是一些无人车相关工具箱】
----control system toolbox【控制】
----robust control toolbox【机器人】
----automated driving toolbox【无人车】
----model predictive control toolbox【MPC】

参考：https://ww2.mathworks.cn/?s_tid=gn_logo

https://ww2.mathworks.cn/help/mpc/ug/automated-driving-using-model-predictive-control.html 【MPC工具箱基本介绍】
【matlab官网对这些工具箱介绍非常好，如果想学习的，很有必要看这些官网说明性文件】

3)carsim【这个工具用于商业汽车仿真的，很贵（几百万一套）；其建模可以跟车一模一样】

4）https://ww2.mathworks.cn/help/mpc/automated-driving-applications.html
【老师说，把这些案例都搞明白了，进入无人驾驶开发没有问题的，虽然只是针对L2级别的】

5）MPC车道跟随案例
https://ww2.mathworks.cn/help/mpc/ug/lane-following-using-nonlinear-model-predictive-control.html

6）老师强烈推荐的MPC讲解视频
https://ww2.mathworks.cn/help/mpc/getting-started-with-model-predictive-control-toolbox.html

5 文献以及案例推荐

Omead Amidi. Integrated mobile robot control.1990【一本书，介绍机器人建模思路】
【https://pdfs.semanticscholar.org/2453/8d912da84a128f73cb4bca68a31ba8388781.pdf?_ga=2.92043610.1910327742.1611058982-127953921.1611058982】

Goodwin. Control system design.2001【一本书，控制系统设计介绍】

Coding：（optional）
Simulink: Adaptive Cruise Control System Using Model Predictive Control
【https://ww2.mathworks.cn/help/mpc/ug/adaptive-cruise-control-using-model-predictive-controller.html】
Simulink: Lane Following Using Nonlinear Model Predictive Control
【https://ww2.mathworks.cn/help/mpc/ug/lane-following-using-nonlinear-model-predictive-control.html】

答疑环节：
1）pid控制如何进行车辆的横纵向耦合？
答：一般情况下，将无人车系统简化，实现横纵分离，不要耦合

2）PID调参属于玄学，但是还是能够调好的，因为有一些规律的。

3）内外环pid会相互影响，设计时候，内环到外环应是从低维到高维的过程，如内环控制加速度，外环控制速度。再比如无人机，先偏航这些，然后速度这些，最后位置这些，共三层控制。

4）稳定误差，执行机构的终止状态和理想的参数状态不一致。比如方向盘一直有往左偏的误差，即使方向盘打正了，我认为打正了，方向盘应该正常输出，但是总会有一点点左偏。这种情况下，pd控制可能发现不了这个稳定误差，当误差减少到一定程度以后，如果p控制器没有进一步提高控制量的话，会被干扰项或者系统滞后项给过滤掉。因此，出现稳态误差，这个误差会累加到很大，加上I控制器以后，给足够的控制量，从而纠正这个误差。

5）稳定误差，就是系统本身造成的。

6）LQR核心是有了约束

7）控制在无人车里面位置不低，薪资待遇不比视觉这些低

8）强化学习对无人车的决策有用，但是做的难度比较大（老师团队开发经验），研究还行，但是实际使用难度较大，但是这是一个未来方向。

9）无论哪个领域，进入一定深度，都是有难度的

个人小结：这节课，首先介绍了基本的控制理论基础知识，当然也是结合无人车情况进行讲解。接着，介绍了PID控制器，P比例控制，I微分控制，D积分控制，个人理解，这是一个应用很广泛的线性控制器，因此需要解耦，介绍了应用在无人车里面的横向和纵向的引用情况。接着，介绍了比较复杂的控制器设计MPC这些思想啥的。然后，简单介绍了APOLLO的控制器是怎么做的，纵向PID、横向LQR以及MPC这些。最后介绍了matlab跟控制相关的工具箱，当然，重点介绍MPC工具箱，也介绍了两个案例。其实，很多概念这一节。信息量很大。突然感觉，本科时候杨叔子的那本控制论课本应该要好好重新学一下才行。很迷！可能太菜了。

#####################
感恩授课老师的付出
图片版权归原作者所有
不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈