增广iLQR-时空联合规划算法代码简介与再开发（2）iLQR简介

智能驾驶破壁人

已于 2024-10-10 16:03:51 修改

阅读量505

点赞数 16

文章标签：算法

于 2024-10-10 15:42:22 首次发布

本文链接：https://blog.csdn.net/weixin_46006849/article/details/142683793

版权

增广iLQR-时空联合规划算法代码简介与再开发-前言_时空联合优化器-CSDN博客文章浏览阅读294次，点赞6次，收藏11次。简单来说就是同时求解路径与速度曲线。时空联合规划本质上是求解最优化问题，将路径和速度曲线作为优化问题的变量，同时得到二者在可行范围内的最优解。前言介绍LQR和iLQR介绍增广拉格朗日函数法配置开源代码，编译和plot展示正式开始介绍代码，先介绍iLQR再介绍增广拉格朗日部分iLQR（预计10月底完成iLQR更新）iLQR类和问题构建系统模型DiscreteDynamics 和ContinuousDynamics类连续模型离散化过程。_时空联合优化器https://blog.csdn.net/weixin_46006849/article/details/142681970?spm=1001.2014.3001.5501

一. 为什么使用iLQR

自动驾驶的路径规划系统表现出非线性运动特性，如何解决非线性模型的最优控制问题，就可以同样应用路径规划。

首先说下什么是非线性模型的最优控制问题：在于找到一个控制策略（找到u控制序列），使得给定系统（比如车辆运动学模型）在满足约束条件的情况下（比如避障）从初始状态到终态（车辆状态序列）的过程，并且保证某种要求的性能指标（目标函数）达到最小（或最大）。

iLQR 适用于解决非线性系统最优控制问题，通过设置系统状态方程，目标函数和约束条件找到一序列控制输入，使车辆从初始状态开始沿着最优路径到达目标状态或者目标轨迹。

二.iLQR和LQR的对比

首先比较一下LQR和iLQR：

LQR：
- 适用于线性系统和二次代价函数的最优控制问题。
- 系统动力学和代价函数必须是线性和二次的，LQR黎卡提方程可以找到解析解。
iLQR：
- 适用于非线性系统的最优控制问题。
- 通过迭代的方式不断逼近最优解，取近似解。转换成LQR框架解决问题，直到迭代收敛。

iLQR是LQR（线性二次调节器）的迭代版本，使用一阶泰勒展开系统状态方程完成线性化，二阶泰勒展开近似目标成本函数，然后使用LQR求解该近似问题。先给定一条初始轨迹，如果满足收敛条件则退出否则将新的状态和控制作为输入进行下一次迭代。

三. iLQR 的组成

一. 系统状态方程

车辆行驶的过程中系统状态转移方程为连续系统，但离散系统更适用于数字信号处理，控制领域，对于整个域控来说输入和输出信号都为固定运行周期的离散信号，常规上规划模块每20ms时间运行一次，所以需要把连续系统离散化。

离散时间非线性系统状态转移方程，可以表示为

$x_{k+1} = f(x_{k},u_{k})$

对于自动驾驶规划来说，x 为状态矩阵，u为控制量矩阵（如下所示）。下一个运行周期的请求状态量为本周期请求状态量和本周期的请求控制量共同作用的结果。

$x = \begin{Bmatrix} \\ x \\ y \\ heading \\ v \\ yawrate \\ a \end{Bmatrix}$ $u = \begin{bmatrix} \\ yawraterate \\jerk \end{bmatrix}$

二.目标函数

（公式2，上图来自于网络）

目标函数J 由终态成本和[0 ~ N -1] 阶段成本两部分组成，通过设置 $Q_{n}$ ,Q,R，p,r,q矩阵可以满足不同场景需求。

三.迭代过程

具体迭代步骤详见四.iLQR 迭代步骤.

四. 迭代结束条件

迭代主要涉及三个方面，迭代的开始，迭代的结束和迭代的最大次数。

最简单的是迭代的最大次数，一般情况下根据算力的不同需要进行调整，常规设置在20-50次会有明显的收敛效果。

迭代的开始：一般情况会把参考线轨迹作为初始轨迹用来计算k = 0->1的迭代。从仿真上看一个符合场景预期的初始轨迹对迭代收敛的时间有很明显的压缩。

假设设置iLQR迭代的次数为100次，那么每进行一次迭代都会都会根据上一次迭代的结果来更新系统的状态和控制输入序列。当连续n次迭代的目标成本值逐渐减小，并且本次目标成本值低于一个阈值则认为迭代收敛，控制序列逼近最优解。

四.iLQR 迭代步骤

iLQR迭代的最终目的是找到收敛的最小目标成本函数所对应的控制序列u。

求解最小值第一个想法就是目标成本函数对u序列控制量求导为0，但因为序列嵌套比较多求解难度较大。可以利用贝尔曼最优性原理解决该问题，贝尔曼原理认为一个策略如果是最优的则对于任何初始状态，从这个状态开始按照该策略所采取的所有后续决策也是最优的。这样就把一个复杂问题分解为N个子问题。

贝尔曼原理的推导比较深，只需要知道四个概念和一个公式即可，具体细节可以自行了解。

概念：

未来的状态只和当前的状态有关，和过去的状态无关；
最优部分的任何一部分也是最优的；
最优动作值函数 $g_{(x_{k},u_{k})}$ ：在 $x_{k}$ 状态下采取 $u_{k}$ 的输入后得到的价值信息。用于评估做出最优的动作选择。不同的 $u_{k}$ 带来不同的价值，可以用于改进策略。
最优价值函数 $V_{k}(x_{k})$ ：在 $x_{k}$ 状态下所获得的累积价值。用于评估当前状态的好坏程度，和控制无关。

注：迭代的次数和控制输入的序列不要搞混，iLQR的迭代步骤是一次迭代的步骤，需要进行多次迭代找到近似解， $u_{k}$ 和 $x_{k}$ 中的k为序列中 $u = {u_{0},u_{1}...u_{N}}$ 的第k阶段。

公式(2)：

$Q_{k} = g_{k}(x_{k},u_{k}) + V_{k+1}(x_{k+1})$

$V_{k}= min(Q_{k})$

在向后传播的过程中即k= N->0，上述公式表示在本次迭代中k阶段的成本价值Q等于k阶段的最优动作价值加上K+1阶段（上个阶段）的最优价值函数值。

又因为概念2， $Q_{k}$ 如果是最优的话那么K阶段的最优价值函数 $V_{k}(x_{k})$ 为 $Q_{k}$ 。

以下为具体步骤：

初始化

给定初始的控制输入序列 $u = {u_{0},u_{1}...u_{N}}$ 和初始状态 $x= {x_{0},x_{1}...x_{N + 1}}$ ，详见四.迭代结束条件。

后向传递

从终端序列开始，从后向前递推求解K和d。后向传递更新反馈增益矩阵K和开环控制增量d，K和d将用于前向传递中生成新的控制输入和状态轨迹。以下介绍如何求解K和d：

1. 首先把目标成本函数用泰勒函数展开，得到 $\delta Q$ 。

(公式3）

约定 $Q_{x}=\frac{\partial Q }{\partial x}$ ,以此类推得到公式4

$\delta Q = Q_{x}^{T}\delta x+ Q_{u}^{T}\delta u+ \frac{1}{2}(\delta x)^{T}Q_{xx}\delta x+ \frac{1}{2}(\delta u)^{T}Q_{ux}\delta x+ \frac{1}{2}(\delta x)^{T}Q_{xu}\delta u+ \frac{1}{2}(\delta u)^{T}Q_{uu}\delta u$

（公式4）

2.为了计算最优控制序列， $\delta Q$ 对 $\delta u$ 直接求导为0 得到极值 $\delta u_{k}^{*}$ 。

$\frac{\partial \delta Q}{\partial \delta u}= Q_{u}+ \frac{1}{2}Q_{ux}\delta x+ \frac{1}{2}Q_{xu}^{T}\delta x+Q_{uu}\delta u=Q_{u}+ Q_{ux}\delta x+Q_{uu}\delta u = 0$

（公式5）

注意：x 和u都是矩阵，矩阵的求导公式和变量不同。

公式5中包含 $\delta u_{k},\delta x_{k}$ ，通过移项可以得到最优控制序列 $\delta u_{k}^{*}$ 表达式

$\delta u_{k}^{*} = -Q_{uu}^{-1}(Q_{ux}\delta x_{k} +Q_{u}) = K\delta x_{k} + d$

(公式6）

如果知道 $Q_{uu},Q_{ux},Q_{u}$ 则可以求解反馈增益矩阵K和开环控制增量d。

3.求Q对x和u求导矩阵

以 $Q_{x}$ 为例。由公式2可知 $Q_{k} = g_{k}(x_{k},u_{k}) + V_{k+1}(x_{k+1})$ ，则

$\frac{\partial Q}{\partial x} =Q_{x_{i}} ^{T}= \frac{\partial (g(x_{i},u_{i}) + V(x_{i+1})) }{\partial x} \\= \frac{\partial(g(x_{i},u_{i})) }{\partial x} + \frac{\partial V(x_{i+1})}{\partial x}\\=\frac{\partial(g(x_{i},u_{i})) }{\partial x} + \frac{\partial V(x_{i+1} =f(x_{i},u_{i}))}{\partial x}\\=g_{x}^{T} + \frac{\partial V(x_{i+1})}{\partial x}\mid x_{i+1}\frac{\partial f}{\partial x}\\=g_{x}^{T}+V_{i+1}^{T}f_{x}$

再进行一次转置可以得到

$Q_{x}=g_{x}+f_{x}V_{i+1}$

同理可得

（公式7）

注：没有标注的部分均为i序列求导矩阵。

现在Q对x和u求导矩阵如公式7所示，虽然推导出了Q对x和u求导矩阵但是表达式出现了新的需要求解的内容 $V_{x_{i+1}},V_{x_{i+1},x_{i+1}}$ 。

4.求解 $V_{x_{i+1}},V_{x_{i+1},x_{i+1}}$

由公式2 可知当 $Q_{k}$ 为最优动作值函数的时候，有 $V_{k}= min(Q_{k})$ 那么增加扰动后也有

$V_{k}(x_{k})=Q_{k}(x_{k})\rightarrow \delta V=\delta Q$

（公式8）

联立公式（4）和公式（6）可以得到最优的动作价值函数Q的扰动项 $\delta Q$ 见公式9，

(公式4） $\delta u_{k}^{*} = -Q_{uu}^{-1}(Q_{ux}\delta x_{k} +Q_{u}) = K\delta x_{k} + d$

把公式4带入6

$\delta Q=\frac{1}{2}\delta x^{T}[Q_{x}+K^{T}Q_{u}K+Q_{ux}K+K^{T}Q_{ux}]\delta x+[Q_{x}+K^{T}Q_{u}d+Q_{xu}d+K^{T}Q_{u}]^{T}\delta x+\frac{1}{2}d^{T}Q_{u}d+Q_{u}^{T}d$

（公式9）

把 $\delta V$ 泰勒展开：

$\delta V=\frac{\partial V }{\partial x}\delta x+ \frac{1}{2}(\delta x)^{T}\frac{\partial^2 V}{\partial x^2}\delta x+ \Delta V=V_{x}\delta x+\frac{1}{2}(\delta x)^{T}V_{xx}\delta x+\Delta V$

(公式10）

联立公式10，公式9和公式8，可得 $V_{x},V_{x,x}$ 表达式见公式11

$V_{x}=Q_{x}- Q_{u}Q_{uu}^{-1}Q_{ux}$

$V_{xx}=Q_{xx}- Q_{xu}Q_{uu}^{-1}Q_{ux}$

$\Delta V=-\frac{1}{2}Q_{u}Q_{uu}^{-1}Q_{u}$

（公式11）

至此所有项都求解出来，可带入求得K和d。

总结：

反向传播如果知道K+1阶段 $V_{x_{k+1}},V_{x_{k+1},x_{k+1}}$ ,根据公式7就能得到K 阶段的 $Q_{uu},Q_{ux},Q_{u}$ ，再代入公式6得到K阶段的反馈增益K和开环控制增量d。同时用K阶段公式7 得到的 $Q_{x},Q_{xx},Q_{u}$ 等带入公式11得到K阶段的 $V_{x_{k}},V_{x_{k},x_{k}}$ 以此迭代递推。

K阶段最优控制序列 $\delta u_{k}^{*}$ 还需要知道 $\delta x_{k}$ ,在前向传播的过程中会进行计算。

前向传递

利用后向传递得到的反馈增益K和增量d更新控制输入序列并通过系统的动力学方程更新状态轨迹。这一步骤是从前向后0-N 模拟系统的演化过程，这一步可以计算出 $\delta x,\delta u_{k}^{*}$ 。

$\delta x_{k}= x_{k}^{i}-x_{k}^{i-1}$

(公式12）

$\delta x$ 是n维状态向量， $\delta x_{k}$ 为状态向量中的K序列,i表示迭代次数,需要注意下标表示的含义。

公式12 表示 $\delta x$ 是状态向量的在两次迭代过程中差值。

向前传递涉及到两条计算链路：

一条是从0-N+1 个状态之间的迭代计算，在同一次迭代下利用状态转移方程递推下个序列的状态 $x_{k+1}^{1} = f(x_{k}^{1} ,u_{k}^{1} )$ ，每次迭代开始时只需要直到自车的初始状态即可根据状态转移方程递推出N+1个序列的状态向量；
一条是相邻迭代之间的计算， $\delta x_{k+1}= x_{k+1}^{1}-x_{k+1}^{0}$ ，对于同一个状态序列扰动项为两次迭代的差值。

这样迭代计算完成后会得到本次迭代0-N+1个序列的 $\delta x_{k}$ ，把 $\delta x_{k}$ 带入公式6即可得到0-N 序列的 $\delta u_{k}^{*}$ 。本次迭代的最优控制是上一次迭代的控制量加上 $\delta u_{k}^{*}$ ，最后根据新的deta 控制量更新本次迭代的输出 $u_{k}^{i}$ 。