ilqr 算法说明
1 Introduction
希望能用比较简单的方式将ilqr算法进行整理和总结。
2 HJB方程
假定我们现在需要完成一个从A点到B点的任务，执行这段任务的时候，每一步都需要消耗能量，可以用下面这个图表示。
我们在执行这个A点到B点的任务的时候，需要建立一个评价评价系统，有了明显的评价指标，我们才能对各种决策进行对比。现在问题来了，应该选择什么样的数据作为评价指标。我们在构建我们的最优决策的时候，借用动态规划的思想，通过分解的方法去反向求解我们这个最优决策序列。显然我们会使用能量损耗作为评价指标(值函数)，我们希望用当前步开始，到终点所用的总能量最小。
V k − 1 ( x ) = V k ( x ) + l o s s （ b e l l m a n 方程） V_{k-1}(x)=V_k(x)+loss （bellman方程） Vk−1(x)=Vk(x)+loss（bellman方程）
这是一个离散的表达形式，在一个连续的系统上，上面这个公式可以看成是，从时间维度上能量消耗的速度 * 时间=单位距离能量消耗 * 距离，根据上面动态规划的策略，随着距离终点越远，我们的值函数是越大的，所以
∂ V ∂ s Δ s + l o s s = 0 ∂ V ∂ s v ∗ Δ t + l o s s 速度 ∗ Δ t = 0 ∂ V ∂ s v + l o s s 速度 = 0 \begin{aligned} \frac{\partial V}{\partial s}\Delta_s + loss=0 \\ \frac{\partial V}{\partial s} v*\Delta_t + loss速度*\Delta_t=0 \\ \frac{\partial V}{\partial s} v + loss速度=0 \end{aligned} ∂s∂VΔs+loss=0∂s∂Vv∗Δt+loss速度∗Δt=0∂s∂Vv+loss速度=0
但是我们知道，这是我们在动态规划的贪婪策略下，才能取得的结果，并不是所有的u都能得到这样好的结果，所以有
min u 0 { ∂ V ∂ s v + l o s s 速度 } = 0 \min_{u_0} \{ \frac{\partial V}{\partial s} v + loss速度 \}=0 u0min{∂s∂Vv+loss速度}=0
现在说了很多了，看起来和hamilton毫无关系。实际上ilqr推导过程中，使用了hamilonian变量就是
H = ∂ V ∂ s v + l o s s 速度 ( h a m i l o n 公式 ) H = \frac{\partial V}{\partial s} v + loss速度 (hamilon 公式) H=∂s∂Vv+loss速度(hamilon公式)
那么实际上HJB方程就是
min u H = 0 \min_{u} H =0 uminH=0
如果hamilonian变量可导，这个公式可以拆成两个公式：
H = 0 ∂ H ∂ u = 0 ( 导数等于 0 ，才能取到极值 ) \begin{aligned} H & =0 \\ \frac{\partial H}{\partial u} &= 0 (导数等于0，才能取到极值) \end{aligned} H∂u∂H=0=0(导数等于0，才能取到极值)
3 LQR
在我们开始我们的推导之前，我们先把一些知识复习和梳理一下。
1）首先值函数V，可以看成是从当前状态到终点的损失累计J
2）LQR问题中，假定终点是无穷大时间，那么
J = ∫ ( x r e f − x ) T Q ( x r e f − x ) + u T R u J = \int (x_{ref}-x)^TQ(x_{ref}-x)+u^TRu J=∫(xref−x)TQ(xref−x)+uTRu
3) 我们现在的瞬时loss(loss速度)是
g ( x , u ) = ( x r e f − x ) T Q ( x r e f − x ) + u T R u g(x,u)=(x_{ref}-x)^TQ(x_{ref}-x)+u^TRu g(x,u)=(xref−x)TQ(xref−x)+uTRu
4) Hamilonian变量中的速度，一般由动力学系统(微分系统)给出。
x ˙ = f ( x , u ) = A ( x r e f − x ) + B u \dot{x}=f(x,u)=A(x_{ref}-x)+Bu x˙=f(x,u)=A(xref−x)+Bu
那么HJB方程有
∂ J ∂ x T ( A ( x r e f − x ) + B u ) + ( x r e f − x ) T Q ( x r e f − x ) + u T R u = 0 B ∂ J ∂ x + 2 R u = 0 \begin{aligned} \frac{\partial J}{\partial x}^T(A (x_{ref}-x)+Bu)+(x_{ref}-x)^TQ(x_{ref}-x)+u^TRu & =0 \\ B\frac{\partial J}{\partial x}+2Ru & = 0 \end{aligned} ∂x∂JT(A(xref−x)+Bu)+(xref−x)TQ(xref−x)+uTRuB∂x∂J+2Ru=0=0
这个公式中 ∂ J ∂ x \frac{\partial J}{\partial x} ∂x∂J没法直接求，但是根据 J = ∫ ( x r e f − x ) T Q ( x r e f − x ) + u T R u J = \int (x_{ref}-x)^TQ(x_{ref}-x)+u^TRu J=∫(xref−x)TQ(xref−x)+uTRu，可以猜测
∂ J ∂ x = 2 S ( x r e f − x ) ( 有个 2 为了计算方便 ) \frac{\partial J}{\partial x}=2S(x_{ref}-x)(有个2为了计算方便) ∂x∂J=2S(xref−x)(有个2为了计算方便)
并且我们采用最简单的线性控制策略
u = K ( x r e f − x ) u=K(x_{ref}-x) u=K(xref−x)
我们把这两个假设代入到刚才那个方程中，虽然我们有两个未知量，但是我们有两个方程，大概率能求解。
2 ( x r e f − x ) T S T ( A ( x r e f − x ) + B K ( x r e f − x ) ) + ( x r e f − x ) T Q ( x r e f − x ) + ( x r e f − x ) T K T R K ( x r e f − x ) = 0 2 B S ( x r e f − x ) + 2 R K ( x r e f − x ) = 0 \begin{aligned} 2(x_{ref}-x)^TS^T(A (x_{ref}-x)+BK(x_{ref}-x))+(x_{ref}-x)^TQ(x_{ref}-x)+(x_{ref}-x)^TK^TRK(x_{ref}-x) & =0 \\ 2BS(x_{ref}-x)+2RK(x_{ref}-x) & =0 \end{aligned} 2(xref−x)TST(A(xref−x)+BK(xref−x))+(xref−x)TQ(xref−x)+(xref−x)TKTRK(xref−x)2BS(xref−x)+2RK(xref−x)=0=0
进行简化，得到
2 S A + 2 S B K + Q + K T R K = 0 K = − R − 1 B S 2 S A − 2 S B R − 1 B S + Q + S B T R − 1 B S = 0 2 S A − S B R − 1 B S + Q = 0 \begin{aligned} 2SA+2SBK+Q+K^TRK &=0 \\ K &=-R^{-1}BS \\ 2SA-2SBR^{-1}BS+Q+SB^TR^{-1}BS &=0 \\ 2SA-SBR^{-1}BS+Q&=0 \\ \end{aligned} 2SA+2SBK+Q+KTRKK2SA−2SBR−1BS+Q+SBTR−1BS2SA−SBR−1BS+Q=0=−R−1BS=0=0
这就是Ricaati方程，先把riccati方程借出来，得到S，然后再用$K =-R^{-1}BS $, 就能得到我们控制器所用的参数了。
注意，我这里推到和传统lqr推导不一样，我使用了 x r e f − x x_{ref}-x xref−x
最后我们总结一下，原来lqr，就是对HJB方程进行展开而已。
3 ilqr
ilqr 和lqr不同之处，在于需要在有限长度的情况下，做出决策。
对于当前状态的值函数，满足
J = ∑ 0 k − 1 { ( x i r e f − x i ) T Q ( x i r e f − x i ) + u i T R u i } + ( x k r e f − x k ) T Q ( x k r e f − x k ) + u k T R u k J = \sum_0^{k-1} \{(x_{iref}-x_i)^TQ(x_{iref}-x_i)+u_i^TRu_i \}+(x_{kref}-x_k)^TQ(x_{kref}-x_k)+u_k^TRu_k J=0∑k−1{(xiref−xi)TQ(xiref−xi)+uiTRui}+(xkref−xk)TQ(xkref−xk)+ukTRuk
这个时候的Hamlitonian变量中的 ∂ J ∂ x \frac{\partial J}{\partial x} ∂x∂J，物理意义是单位距离内，能量的变化。[1]
因为
∂ P k ∂ u k = 0 \frac{\partial P^k}{\partial u_k}=0 ∂uk∂Pk=0
根据这个公式很容易推导得到8，
因为
V x k = ∂ P k ∂ x k V_x^k=\frac{\partial P^k}{\partial x_k} Vxk=∂xk∂Pk
并且因为 δ u k \delta_{uk} δuk中有 δ x k \delta_{xk} δxk的分量。
同理
V x x k = ∂ 2 P k ∂ 2 x k V_{xx}^k=\frac{\partial^2 P^k}{\partial ^2x_k} Vxxk=∂2xk∂2Pk
Reference
[1] Chen, Jianyu, Wei Zhan, and Masayoshi Tomizuka. “Autonomous driving motion planning with constrained iterative LQR.” IEEE Transactions on Intelligent Vehicles 4.2 (2019): 244-254.
[2] You, Changxi. “Real Time Motion Planning Using Constrained Iterative Linear Quadratic Regulator for On-Road Self-Driving.” arXiv preprint arXiv:2202.08400 (2022).
[3] https://github.com/pparmesh/Constrained_ILQR
[4] Howell, Taylor A., Brian E. Jackson, and Zachary Manchester. “ALTRO: A fast solver for constrained trajectory optimization.” 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019.
