离散LQR与iLQR的推导思路
离散LQR与iLQR的推导思路
- 一、最优控制
- 最优控制定义
- 数学基础
- 里程碑工作
- 二、LQR问题
- LQR定义
- 极小值原理方法求解
- 动态规划方法求解
- 两种方法的比较
- 三、iLQR框架
一、最优控制
最优控制定义
首先介绍一下LQR的背景，也就是最优控制，最优控制的背景在之前介绍HJB方程的时候也有介绍：HJB方程的一些简单理解和过程推导。假设现在有一辆自动驾驶汽车，我们要设置控制方法，让其沿着指定路线到达某一个位置。要完成这项工作，首要的任务就是了解这辆车当前的“状态”和关于“控制”的变化规律：比如我们一般最关心当前的位置和车速，通过当前位置和速度去进行控制，比如打方向盘，踩刹车，踩油门等，从而又会引起当前位置和车速的变化。与此同时，我们可能还希望省油，车速平稳，不超速等要求。要满足以上的需求，需要合理的设计控制方法，以上问题我们就可以把它写成一个最优控制问题。
最优控制问题的四个基本元素：
状态方程：描述系统（自动驾驶汽车）的运动学或动力学方程
x˙(t)=f(x(t),u(t),t),t∈[t0,tf].x(t0)=x0\dot{x}(t)=f(x(t),u(t),t) , t\in[t_0,t_f]. x(t_0)=x_0x˙(t)=f(x(t),u(t),t),t∈[t0,tf].x(t0)=x0
容许控制：控制与状态满足的约束
u∈U,x∈X.u\in U , x\in X.u∈U,x∈X.
目标集：在结束时间tft_ftf，被控对象的状态x(tf)x(t_f)x(tf)应符合的条件
S={x(tf):m(x(tf),tf)=0}S= \left\{ x(t_f):m(x(t_f),t_f)=0 \right\}S={x(tf):m(x(tf),tf)=0}
- 性能指标：hhh为终端成本，ggg为运行成本
J(u)=h(x(tf),tf)+∫t0tfg(x(t),u(t),t)dt.\displaystyle{J(u)=h(x(t_f),t_f)+\int_{t_0}^{t_f}g(x(t),u(t),t)dt.}J(u)=h(x(tf),tf)+∫t0tfg(x(t),u(t),t)dt.
最优控制可以理解为求解满足状态方程、容许控制，且能达到目标点的情况下使性能指标最优的控制。
数学基础
这里传统的最优控制方法主要有两个：一个是极小值原理，一个是动态规划。这两种方法的产生可谓是神仙打架。在冷战的最初十年中，美苏双方的空军部门都遇到了同一个问题——如何以最短的时间拦截敌方的飞机。双方各自组件了研究团队，寻找切合实际的通用数学工具。
美国自1948年开始进行研究，苏联1955年才开始正是组建团队。尽管苏联方面着手较晚，但是在1956-1957年间苏联团队提出适用范围极广的Pontryagin极小值原理而暂时胜出，美方在1952-1957年提出的动态规划方法则在20世纪60年代被发现可用于最优控制问题，在特定的情况下还可以得到和苏联相同的结果。
里程碑工作
在这场较量中，诞生出了许许多多的重要成果：
- Hestenes问题：寻求最优飞机攻角和坡度。首次使用了Hamiltonian函数
- Isaacs的问题：微分博弈
- Bellman的问题：Bang-Bang控制
- Bellman的多级决策问题与离散时间最优控制
- 开环控制与闭环控制
- Pontryagin极小值原理
- 连续时间最优控制问题与两点边值问题
- 二次性能指标与卡尔曼调节器问题
二、LQR问题
LQR定义
LQR问题其实是一种最简单的最优控制问题。L代表状态方程或者动力学方程是线性的。Q代表目标函数是二次的。具体形式如下：
对于一个路径规划问题，当我们拥有了状态方程，通过任意给出控制，就可以产生无数条路径，而这些路径的优劣需要根据目标函数去抉择。所以我们希望找到一个控制率，这个控制率起一个向导的作用，当一个机器人在某一个位置的时候，控制率会告诉你在现在所处的位置，根据目标函数的判断标准最优控制应该是什么，这种思想的一个简单的数学表达是：
uk=Fkxk+vku_k=F_kx_k+v_k uk=Fkxk+vk
这样可以理解为：状态方程+控制率=最优路径。所以在状态方程已知的情况下，我们的目标就是求解一个控制率，也就是控制对于位置的函数。
在搜索LQR问题的求解推导时，网上的方法不是很统一，这是因为一部分人使用的极小值原理，一部分人使用的是动态规划，下面分别对这两种方法进行简要说明。
极小值原理方法求解
在离散情况下的，极小值原理其实可以不必非要写成哈密尔顿方程的形式，更好理解的方式就是使用拉格朗日法，求解KKT条件。
我们将动力学约束放入目标函数中，分别对变量和拉格朗日乘子进行求导，得出以下的四个方程，其中最后一个是终值条件。
这里我们还需要根据这个终值条件做一个假设，在任意时刻的λkλ_kλk都具有类似终值条件那样的形式，这里绿色是表示已知量，红色代表未知量。如果我们将红色的PPP求解出来，那么将其带入到第二个式子中，不就把控制率求解出来了吗。我们将求解PPP的这个方程成为：RiccatiRiccatiRiccati方程。一般来说迭代几步就会收敛。
通过这几个式子的消除变量，可以将PkP_kPk的迭代式求解出来，而我们又已知了最后一个时刻的PPP,这样就可以迭代的求出任意时刻的PPP值，顺理成章的得出控制率：
具体的推导过程可以详见：https://zhuanlan.zhihu.com/p/465054562
动态规划方法求解
使用动态规划之前需要了解什么是最优性原理，可以参考：HJB方程的一些简单理解和过程推导
blog.csdnimg.cn/8bc06d608206439fa084555e331a12a5.png#pic_center)
我们的推导思路其实很简单，举个例子，已知从上海到北京的最短路径是上海-南京-北京，那么根据最优性原理，从南京到北京的最短路径是南京-北京，那么我们可以先求出南京-北京这一段的控制率，然后这段有了之后，再去求上海-南京-北京的最后路径，即从后往前求。
我们这里重新定义一下目标函数，因为要一个阶段一个阶段的求解，所以我们不妨设相邻两个状态的目标函数为:
动力学方程为：
同样我们希望根据上面两个式子找到一个控制率：
我们希望先求出南京-北京这段路的最优控制率，那么这一段的评判标准就是c(xT,uT)c(x_T,u_T)c(xT,uT),这里写成了他的展开形式：
希望对于控制最优，所以直接对控制进行求导等于零，得出：
这样我们就轻而易举的求出了这一阶段的控制率，尝到了甜头，那么我们希望继续用这种方法求上海-南京-北京的最优控制率，也即找到Q(xT−1,uT−1)Q(x_{T-1},u_{T-1})Q(xT−1,uT−1)，然后求导等于零，得出最优控制率。很自然的想法就是Q(xT−1,uT−1)Q(x_{T-1},u_{T-1})Q(xT−1,uT−1)的构建是：
但是这里我们需要将xTx_TxT和uTu_TuT用xT−1x_{T-1}xT−1和uT−1u_{T-1}uT−1替换掉。如何替换呢？
其实就是简单的变量消除。首先我们可以使用得到的最优控制，将uTu_TuT用xTx_TxT替换掉：
其中：
之后使用状态方程或者动力学防尘将：xTx_TxT替换成xT−1x_{T-1}xT−1和uT−1u_{T-1}uT−1：
其中：
这样我们就可以对其进行求导，如此向前迭代，求出最优控制率，整理成算法就呈现出了如下的反向迭代结构：
两种方法的比较
对于极小值原理，求解的一个开环问题。通过使用变分法，基于哈密尔顿方程组，一次性将最优曲线求解出来。
对于动态规划，他求解的是一个闭环问题。通过将问题变成一个多级决策问题，使用最优性原理，通过迭代的方式求出曲线。
两者虽然出发点不同，但是在求解问题的时候都有一个反馈-前馈结构。反馈主要体现在控制率的求解，前馈体现是使用控制率和状态方程计算轨迹。
三、iLQR框架
对于LQR问题，可以根据求解出来的控制率和状态方程，直接算出最优轨迹，如左边的示意图。
对于iLQR，其实就是目标函数和状态方程出现了非线性，这个时候控制率不是那么好求解。解决思路就是使用迭代的思想进行求解。首先给定一个初始的轨迹，如蓝色的直线，那么为了让其能够求解，可以将目标函数泰勒二次展开，状态方程泰勒一阶展开，之后使用LQR算法生成一条新的轨迹，如红色的虚线，之后反复迭代，直到轨迹的变化在一个规定的极小值内，就认为收敛，达到了最优。
具体的算法可以参考文献：Iterative Linear Quadratic Regulator Design for Nonlinear Biological Movement Systems。
推导思路是使用极小值原理。
离散LQR与iLQR的推导思路相关推荐
- 线控转向 四轮转向控制 4WIS CarSim与Simulink联合控制器为离散LQR控制器 带有完整详细的控制
线控转向 四轮转向控制 4WIS CarSim与Simulink联合控制器为离散LQR控制器 带有完整详细的控制器.二自由度稳定性控制目标推导说明 MATLAB版本为2018b CarSim版本为20 ...
- 四轮线控转向控制 4WIS CarSim与Simulink联合 控制器为离散LQR控制器
四轮线控转向控制 4WIS CarSim与Simulink联合 控制器为离散LQR控制器 带有完整详细的控制器.二自由度稳定性控制目标推导说明, MATLAB版本为2018b CarSim版本为201 ...
- 卡尔曼滤波推导思路总结
推导思路一: (1) 混合高斯 一维高斯函数形式: (1)N(x,μ,σ)=1σ2πe−(x−μ)22σ2\mathcal N(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2 ...
- 【核心内容及推导思路】人类记忆系统之谜，也许就是这么回事儿
文章目录 0. 前言 1. 推导思路 第1步(猜想的由来及核心内容): 第2步(解剖学上的"疑似证据"): 记忆输入通路示意图 记忆检出通路示意图 第3步(记忆特征上的" ...
- Apollo Control——超详细入门教程(二)：连续状态空间方程离散化与离散LQR公式推导
专栏文章列表 Apollo Control--超详细入门教程(一):基于道路误差的车辆动力学模型 Apollo Control--超详细入门教程(二):连续状态空间方程离散化与离散LQR公式推导 Ap ...
- 离散拉普拉斯算子与LOG推导
目录 离散拉普拉斯算子的推导 高斯拉普拉斯算子的推导 离散拉普拉斯算子的推导 连续函数的拉普拉斯算子如下: ∇2f=∂2f∂x2+∂2f∂y2\nabla^{2} f=\frac{\partial^{ ...
- MIT6.824-lab2A-2022篇（万字推导思路及代码构建）
目录 前言 一.学习背景 二.实验引入 三.结构体实现 3.1 State的定义 3.2 AppendEntries RPC的定义 3.3 RequestVote RPC的定义 四.领导选举 4.1初 ...
- MIT6.824-lab2B-2022篇（万字推导思路及代码构建）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 前言 一.整体流程思路 二.初始化,发送ticker 2.1.初始化 2.2.发送ticker 三.进行日志增量的RPC 3 ...
- lm曲线公式推导_菜鸟理解的IS-LM曲线推导思路，求指正(更新中)
小弟最近备考CFA一级,十二月份考,然后现在看到经济学宏观部分,看到了IS-LM曲线的推导,十分不解,以下是小弟理解的过程,希望大家批评指正,在这里错好过在考试的时候犯错,最后祝大家都顺利通过. 因为 ...
- 离散信号（四）| 周期信号 |离散傅里叶级数（DFS）推导 + 主要性质（周期卷积定理、帕斯瓦尔定理）
离散傅里叶变换(DFT)要解决两个问题:一是信号离散化后它的频谱情况:二是快速运算算法.第一个问题将涉及周期离散信号的傅里叶级数(DFS),以及由DFS得到非周期信号的离散时间傅里叶变换(DTFT)和 ...
最新文章
- 物联网电子标签助力无人便利店
- 基础学科如何不再“又难又穷”
- 一图理解腾讯本次组织结构重大变革的方向和目的
- Android之SharedPreferences 讲解
- linux top 历史,阿里工程师眼中的linux top命令
- deepin/ubuntu安装苹方字体
- 常用于评价回归模型优劣的统计量包括( )。_第四十一讲 R-判断回归模型性能的指标...
- Android 获取当前的时间。年月日，时分秒，星期几
- LeetCode 53. 最大子序和（动态规划）
- 我对 ASP.NET Web API 的随想
- android打印机字体大小,打印自定义文档 | Android 开发者 | Android Developers
- 局域网共享加密账号密码登录自动创建账户
- Win11新Bug任务栏图标不显示的解决方法
- 计算机上图片打不开,电脑上打不开jpg格式的图片是怎么回事？
- css将两张图片叠加（简易方法）
- 我深爱的Java，对不起，我出轨了！！！呸！渣男！
- 【Codecs系列】GDR(Gradual Decoder Refresh)帧
- linux设备驱动之pci设备的I/O和内存
- 平时用电脑需要注意什么
- 强制用户在下次登录Linux时更改密码