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.
