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?V?v?Δt?+loss速度?Δt?=0?s?V?v+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
u0?min?{?s?V?v+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?V?v+loss速度(hamilon公式)
那么实际上HJB方程就是
min
?
u
H
=
0
\min_{u} H =0
umin?H=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?J?T(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?)+uiT?Rui?}+(xkref??xk?)TQ(xkref??xk?)+ukT?Ruk?
这个时候的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.
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！