自动驾驶（七十二）---------LQR控制算法

之前有写过MPC的控制算法，主要介绍的也是理论部分，在实际使用过程中发现C++没有高效的优化方法，类似python中的cvxpy的库，所以想绕过去，研究一下LQR控制算法。

1. 控制系统

这里我们先介绍常用的控制系统逻辑：

假设我们现在状态是x0，我们有状态方程 : （u为控制矩阵）

特别的，这里我们是对偏差建立方程，x是偏差的状态，优化的目的是x=0，针对我们通过一些假设可以得到详细的方程，这里我直接先给出其中,

详细的推导有兴趣可以看最后。

再假设有一个反馈控制器：这里很重要，可以认为是当前的控制量是通过当前的状态量计算出来的。

通过这套方法，我们就能得到一个稳定的系统：

当然这是基本的理论，再进一步，我们就会思考，通过这些控制量作为自变量，再设计一个代价函数，来优化这些控制量？

2. LQR控制算法

讲到这里就很自然引出LQR了，首先的问题代价函数是什么？一方面我们希望系统达到稳定状态，及偏差最小; 另一方面我们希望控制量较小，即付出较小的代价达到我们的目的。这里我直接给出：

其中x为状态量，u为控制量，Q为状态权重矩阵，R为控制权重矩阵。

特别的x和u中取值有正有负，所以需要平方和最小，在矩阵中没有平方，这里我们采用转置乘以本身的做法模拟矩阵的平方，如x^T*x 。这里状态量x和控制量u都是多维向量，上式计算的结果是一个标量。

其实我们可以把看作是的多维扩展表达式，这里我们需要Q为半正定，就是希望Q能起到a≥0的效果，R为正定矩阵就是希望矩阵R能够起到a>0的效果。

一般的我们认为状态量x为：横向偏差、横向变化率、角度偏差、角度变化率。Q为我们提前标定的对角矩阵，标定值对应以上不同维度的权重，也可以为非对角矩阵，考虑不同维度之间的相互关系。

控制向量u为：前轮转角、加速度。同样的R也为提前标定的对角矩阵。也可以考虑相互关系。

我的理解是Q11选取较大的值，会让x1很快到0；另外一方面，加大R的值，会使得对应的控制量减小，控制器执行更少的动作，意味着系统的状态衰减将变慢。所以要综合看具体的实际应用场景来调节，鱼和熊掌不可兼得。建议在不同场景下采用不同的参数。

3. 公式求解

有了上面的优化目标后，就是如何求解最优的控制量了。下面我们先进行一些转换：

1. 将u=−Kx代入代价函数后：，u=−Kx即我们认为当前的控制量可以通过当前的状态量计算出来。如果我们能计算出-K就很简单了，可以直接套出控制量。

2. 假设纯在一个常量矩阵P使得，，这里就直接这么假设，接着往下看，有需要深入的同学可以看后面详细推导，总之，在等式成立的情况下，J取最小值，达到我们的优化目标。

3. 把上式代入得到：

4. 把2中的方程微分展开：

5. 状态变量x的微分用式表示：

整理得到：

通过矩阵得知上式要想有解只有中间部分为零，即：

6. 把代入上式：

整理得到：

7. 这里我们再令这里需要思考一下，为什么可以这样假设，凑出结果？为了不增加复杂度，把这里的推导我放在最后，感兴趣的朋友可以继续研究。总之在上面等式成立的情况下，可以得到。

代入：

整理得到：，得到这一步就很简单了，式中A、B、Q、R都是已知量，很容易就能计算出P的结果。

8. 计算出P再通过就可以计算出K，也就是达到了我们一开始的目标，轻松得到当前的控制量。

4. MPC与LQR比较

首先，LQR的研究对象是现代控制理论中的状态空间方程给出的线性系统，而MPC的研究对象可以是线性系统，也可以是非线性系统。不过现在很多的做法都是将非线性系统线性化，然后进行相关计算，具体要根据自己的工程情况来确定哪种方式比较好，比如之前做MPC的时候，线控车底层速度控制接口就是加速度，那就没必要根据IMU再套嵌个一层PID。

其次，既然是优化问题，那就离不开目标函数的设计，LQR的目标函数在上面已经有描述，MPC的目标函数，多数都是多个优化目标乘以不同权重然后求和的方式。虽然方式不同，不过都是对达到控制目标的代价累计。

最后，工作时域上的不同，LQR的计算针对同一工作时域，在一个控制周期内，LQR只计算一次，并将此次计算出的最优解下发给控制器即可；而MPC是滚动优化的，计算未来一段时间内，每个采样周期都会经过计算，得出一组控制序列，但是只将第一个控制值下发给控制器。

对于LQR所有的理论部分都介绍完毕，下面是之前的一些公司推导部分，有兴趣的可以阅读：

1. 运动学模型的推导

根据车辆的二自由度动力学模型：

在小角度偏角的情况下有,轮胎的侧向力与轮胎的偏离角成正比. ,分别为前、后轮的侧偏刚度：

(3)在小角度的情况下有所以有(4)

因此上述车辆的动力学模型可以简化写成(5)

(6)期望横摆角角速度；

横摆角角度偏差；

横向偏差变化率求导数；

(8)横向偏差变化率

车辆模型的连续状态空间方程

状态变量X的选择分别为横向偏差、横向偏差变化率，横摆角角度偏差，横摆角角度偏差变化率。控制量u为前轮偏角。

选择合适的状态变量后得到A，B，B1，B2矩阵分别如下

由于只对横摆角角度偏差变化率的导数产生影响，在横向控制中主要是控制横向偏差、横向偏差变化率，横摆角角度偏差，横摆角角度偏差变化率，因而忽略了公式中的项。车辆系统的状态空间方程表示为 (10)

2. 最优化目标的等价条件

首先控制理论是一门学科，我建议其他专业同学，直接使用以上的结论，如果你非要想完全理解，你需要具备以下知识：泛函极值、哈密尔顿方程、正则方程、无约束条件泛函、变分法、拉格朗日方程、黎卡提微分方程、控制域、欧拉方程、等周长积分方程......等等知识点。如果说你都具备，欢迎你往下看，不然你还是多看几遍上面把。

1. 对于的最小化问题，

3.对于的推导

在前面的公式中，我们假设，到底这部分假设有没有数学推导呢？

回到之前的公式：，

同时我们知道R是正定对称矩阵，则一定存在一个矩阵M使得：，因为R的每一项大于零，M也为对角矩阵，数字为其平方根。

代入R得到：

自动驾驶（七十二）---------LQR控制算法相关推荐

Mobileye 自动驾驶策略（二）
Mobileye 自动驾驶策略(二) 与多方都成功进行了合作,其中比较大型的合作包括法雷奥.百度和中国 ITS. 法雷奥是最近的的 Tier 1 合作伙伴,法雷奥和 Mobileye 签署协议,表示未 ...
从零开始做自动驾驶定位（二）：轨迹精度评估
从零开始做自动驾驶定位(二):轨迹精度评估一.EVO工具的使用 1.evo简介 evo [1] 是一个很好的测评工具,它可以根据时间戳将轨迹进行对齐,同时可以将不同尺度的轨迹按照你指定的标准轨迹进行 ...
【正点原子Linux连载】第七十二章 RGB转HDMI实验 -摘自【正点原子】I.MX6U嵌入式Linux驱动开发指南V1.0
1)实验平台:正点原子阿尔法Linux开发板 2)平台购买地址:https://item.taobao.com/item.htm?id=603672744434 2)全套实验源码+手册+视频下载地址: ...
七十二、Python | Leetcode字符串系列（下篇）
@Author:Runsen @Date:2020/7/3 人生最重要的不是所站的位置,而是内心所朝的方向.只要我在每篇博文中写得自己体会,修炼身心:在每天的不断重复学习中,耐住寂寞,练就真功,不畏艰 ...
JavaScript学习（七十二）—严格模式
JavaScript学习(七十二)-严格模式 JavaScript除了提供正常模式外,还提供了严格模式,ES5的严格模式是采用具有限制性JavaScript变体的一种方式,即在严格条件下运行JS代码, ...
[ Coding七十二绝技 ] 如何利用Java异常快速分析源码
[ Coding七十二绝技 ] 如何利用Java异常快速分析源码参考文章: (1)[ Coding七十二绝技 ] 如何利用Java异常快速分析源码 (2)https://www.cnblogs.co ...
信息系统项目管理师必背核心考点（七十二）V模型
科科过为您带来软考信息系统项目管理师核心重点考点(七十二)V模型,内含思维导图+真题 [信息系统项目管理师核心考点]V模型一.软件开发瀑布模型的变种,反映了测试阶段和开发过程期间各阶段的对应关系,左 ...
第七十二章 Caché 函数大全 $WISWIDE 函数
文章目录第七十二章 Caché 函数大全 $WISWIDE 函数大纲参数描述示例第七十二章 Caché 函数大全 $WISWIDE 函数返回一个标志,该标志指示字符串是否包含代理对. 大 ...
【Microsoft Azure 的1024种玩法】七十二.使用NodeJS手动编写调用Azure Cognitive Services Translator API代码获取其支持翻译的语言集
[简介] Azure Cognitive Services Translato 是一种基于云的神经网络机器翻译服务,同时也是 REST API 的 Azure 认知服务系列的一部分. 本篇文章将会主要 ...
[生存志] 第58节孔门十哲七十二贤
孔门十哲七十二贤孔子本着"首都市民"精神力图恢复周公"礼乐"游戏,自始至终以"周公继承人" ...

自动驾驶（七十二）---------LQR控制算法

自动驾驶（七十二）---------LQR控制算法相关推荐

最新文章

热门文章