ilqr算法原理推导及代码实践

目录

    • 一. ilqr原理推导
      • 1.1 ilqr问题描述
      • 1.2 ilqr算法原理
      • 1.3 ilqr算法迭代过程
    • 二. ilqr实践代码

一. ilqr原理推导

1.1 ilqr问题描述

本文参考知乎博主: LQR与iLQR:从理论到实践【详细】

基础LQR只能处理线性系统 (指可以使用 x ( k + 1 ) = A x ( k ) + B u ( k ) x(k+1) = Ax(k) + Bu(k) x(k+1)=Ax(k)+Bu(k)来描述系统 ) ,对目标代价函数也做了要求,而在实际问题中非线性系统占了绝大部分,iLQR(Iterative Linear Quadratic Regulator,迭代线性二次型调节器)则能够通过迭代的方式有效地求解非线性最优控制问题,考虑如下问题表述形式:
min ⁡ u 0 , … , u N − 1 J = l f ( x N ) + ∑ k = 0 N − 1 l ( x k , u k ) s.t. x k + 1 = f ( x k , u k ) , x 0 given \begin{gather} \min_{u_0, \ldots, u_{N-1}} & \quad J = l_f(x_N) + \sum_{k=0}^{N-1} l(x_k, u_k) \\ \text{s.t.} & \quad x_{k+1} = f(x_k, u_k), \quad x_0 \text{ given} \end{gather} u0,,uN1mins.t.J=lf(xN)+k=0N1l(xk,uk)xk+1=f(xk,uk),x0 given
其中 J J J是目标函数,需要在控制变量 u 0 , . . . , u N − 1 u_0,...,u_{N-1} u0,...,uN1下进行最小化。目标函数由两部分组成:终端状态 x N x_N xN的代价 l f ( x N ) l_f(x_N) lf(xN) 和 从 k = 0 k = 0 k=0 N − 1 N - 1 N1 的每一个时间步的代价 l ( x k , u k ) l(x_k,u_k) l(xk,uk)的总和,约束条件是状态转移方程 x k + 1 = f ( x k , u k ) x_{k+1} = f(x_k,u_k) xk+1=f(xk,uk),并且初始状态 x 0 x_0 x0是给定的。

1.2 ilqr算法原理

与LQR问题求解过程类似,定义问题的最优状态值函数 J ^ i ( x i ) \hat{J}_{i}(x_{i}) J^i(xi)和动作值函数 J ~ i ( x i , u i ) \tilde{J}_{i}(x_{i}, u_{i}) J~i(xi,ui)
J ^ i ( x i ) = min ⁡ u i { l ( x i , u i ) + J ^ i + 1 ( x i + 1 ) } = min ⁡ u i { l ( x i , u i ) + J ^ i + 1 ( f ( x i , u i ) ) } = min ⁡ u i J ~ i ( x i , u i ) \begin{align*} \hat{J}_{i}(x_{i}) &= \min_{u_{i}}\left\{ l(x_{i},u_{i}) + \hat{J}_{i+1}(x_{i+1}) \right\} \\ &= \min_{u_{i}}\left\{ l(x_{i},u_{i}) + \hat{J}_{i+1}(f(x_{i},u_{i})) \right\} \\ &= \min_{u_{i}} \tilde{J}_{i}\left( x_{i},u_{i} \right) \tag{3} \end{align*} J^i(xi)=uimin{l(xi,ui)+J^i+1(xi+1)}=uimin{l(xi,ui)+J^i+1(f(xi,ui))}=uiminJ~i(xi,ui)(3)
下面详细解释(3)是怎么来的:

  1. 最优状态值函数 J ^ i ( x i ) \hat{J}_{i}(x_{i}) J^i(xi) 定义为给定当前状态 x i x_i xi,从时间步 i i i N N N 的最小总代价,举个例子,现在状态是 x N − 1 x_{N - 1} xN1,那么可以预期的最优状态值 J ^ N − 1 ( x N − 1 ) \hat{J}_{N - 1}(x_{N - 1}) J^N1(xN1) 为当前 N − 1 N-1 N1 时刻的最小即时代价 l ( x N − 1 , u N − 1 ) l(x_{N-1},u_{N-1}) l(xN1,uN1) 加上最小终端代价 J ^ N ( x N ) \hat{J}_{N}(x_{N}) J^N(xN),就像一个人开车从A市去B市,计算最优燃油消耗量Y,那么他最省油的开法肯定是当前时刻采用最省油的开法,并在到达B市之前都一直采用最省油的开法。
  2. 动作值函数 J ~ i ( x i , u i ) \tilde{J}_{i}\left( x_{i},u_{i} \right) J~i(xi,ui) 定义为在当前状态 x i x_i xi下采取某个控制输入 u i u_i ui并遵循最优策略直到终端状态的总代价,动作值函数为最优需要满足采取的控制输入 u i u_i ui为最优
  3. 状态转移:状态 x i + 1 x_{i + 1} xi+1 由当前状态 x i x_{i} xi u i u_i ui通过状态转移函数 f ( x i , u i ) f(x_{i},u_{i}) f(xi,ui)决定,因此,可以将当前状态 x i x_i xi的最优状态值函数 J ^ i ( x i ) \hat{J}_{i}(x_{i}) J^i(xi) 表达为:
    J ^ i ( x i ) = min ⁡ u i { l ( x i , u i ) + J ^ i + 1 ( x i + 1 ) } = min ⁡ u i { l ( x i , u i ) + J ^ i + 1 ( f ( x i , u i ) ) } = min ⁡ u i J ~ i ( x i , u i ) \begin{align*} \hat{J}_{i}(x_{i}) &= \min_{u_{i}} \left\{ l(x_{i}, u_{i}) + \hat{J}_{i+1}(x_{i+1}) \right\} \\ &= \min_{u_{i}} \left\{ l(x_{i}, u_{i}) + \hat{J}_{i+1}(f(x_{i}, u_{i})) \right\} \\ &= \min_{u_{i}} \tilde{J}_{i}(x_{i}, u_{i}) \tag{4} \end{align*} J^i(xi)=uimin{l(xi,ui)+J^i+1(xi+1)}=uimin{l(xi,ui)+J^i+1(f(xi,ui))}=uiminJ~i(xi,ui)(4)
  4. 动作值函数 J ~ i ( x i , u i ) \tilde{J}_{i}\left( x_{i},u_{i} \right) J~i(xi,ui) 可以定义为:
    J ~ i ( x i , u i ) = l ( x i , u i ) + J ^ i + 1 ( f ( x i , u i ) ) \begin{align*} \tilde{J}_{i}\left( x_{i},u_{i} \right) = l(x_{i},u_{i}) + \hat{J}_{i+1}(f(x_{i},u_{i})) \tag{5} \end{align*} J~i(xi,ui)=l(xi,ui)+J^i+1(f(xi,ui))(5)
  5. 最小化动作值函数:最优状态值函数也可以通过最小化所有可能的动作值函数得到:
    J ^ i ( x i ) = min ⁡ u i J ~ i ( x i , u i ) \begin{align*} \hat{J}_{i}(x_{i}) = \min_{u_{i}} \tilde{J}_{i}(x_{i}, u_{i}) \tag{6} \end{align*} J^i(xi)=uiminJ~i(xi,ui)(6)
    坐稳了,下面开始推导,终端最优状态值函数 J ^ N ( x N ) = l f ( x N ) \hat{J}_{N}(x_{N})={l}_{f}(x_{N}) J^N(xN)=lf(xN) ,很好理解,因为到终端状态就没有输入了,所以只有状态代价,没有控制代价。
    使用泰勒展开将系统在当前状态 x i x_{i} xi附近线性化,进而转换为LQR问题进行求解:
    状态转移函数展开,这里需要了解下非线性系统是如何线性化的,链接: 雅可比矩阵几何意义的直观解释及应用
    通过雅可比矩阵得到 A i A_{i} Ai B i B_{i} Bi
    f ( x i , u i ) + δ f ( x i , u i ) = f ( x i + δ x i , u i + δ u i ) ≈ f ( x i , u i ) + ∂ f ( x i , u i ) ∂ x i δ x i + ∂ f ( x i , u i ) ∂ u i δ u i ≜ f ( x i , u i ) + A i δ x i + B i δ u i \begin{align*} f\left(x_{i}, u_{i}\right)+\delta f\left(x_{i}, u_{i}\right) &= f\left(x_{i}+\delta x_{i}, u_{i}+\delta u_{i}\right) \\ &\approx f\left(x_{i}, u_{i}\right)+\frac{\partial f\left(x_{i}, u_{i}\right)}{\partial x_{i}}\delta x_{i}+\frac{\partial f\left(x_{i}, u_{i}\right)}{\partial u_{i}}\delta u_{i} \\ &\triangleq f\left(x_{i}, u_{i}\right)+A_{i}\delta x_{i}+B_{i}\delta u_{i}\tag{7} \end{align*} f(xi,ui)+δf(xi,ui)=f(xi+δxi,ui+δui)f(xi,ui)+xif(xi,ui)δxi+uif(xi,ui)δuif(xi,ui)+Aiδxi+Biδui(7)
    状态值函数展开,这里也很好理解,注意这个 p i T p_{i}^T piT P i P_{i} Pi,后面backward pass的时候通过更新它们俩来实现状态递推
    J ^ i ( x i ) + δ J ^ i ( x i ) = J ^ i ( x i + δ x i ) ≈ J ^ i ( x i ) + ∂ J ^ i ( x i ) ∂ x i δ x i + 1 2 δ x i T ∂ 2 J ^ i ( x i ) ∂ x i 2 δ x i ≜ J ^ i ( x i ) + p i T δ x i + 1 2 δ x i T P i δ x i \begin{align*} \hat{J}_{i}(x_{i}) + \delta \hat{J}_{i}(x_{i}) &= \hat{J}_{i}(x_{i} + \delta x_{i}) \\ &\approx \hat{J}_{i}(x_{i}) + \frac{\partial \hat{J}_{i}(x_{i})}{\partial x_{i}} \delta x_{i} + \frac{1}{2} \delta x_{i}^T \frac{\partial^2 \hat{J}_{i}(x_{i})}{\partial x_{i}^2} \delta x_{i} \\ &\triangleq \hat{J}_{i}(x_{i}) + p_{i}^T \delta x_{i} + \frac{1}{2} \delta x_{i}^T P_{i} \delta x_{i}\tag{8} \end{align*} J^i(xi)+δJ^i(xi)=J^i(xi+δxi)J^i(xi)+xiJ^i(xi)δxi+21δxiTxi22J^i(xi)δxiJ^i(xi)+piTδxi+21δxiTPiδxi(8)
    动作值函数展开,常规的泰勒一阶展开和二阶展开,没有什么难理解的
    J ~ i ( x i , u i ) + δ J ~ i ( x i , u i ) = J ~ i ( x i + δ x i , u i + δ u i ) ≈ J ~ i ( x i , u i ) + ∂ J ~ i ( x i , u i ) ∂ x i δ x i + ∂ J ~ i ( x i , u i ) ∂ u i δ u i + 1 2 δ x i T ∂ 2 J ~ i ( x i , u i ) ∂ x i 2 δ x i + 1 2 δ u i T ∂ 2 J ~ i ( x i , u i ) ∂ u i 2 δ u i + 1 2 δ x i T ∂ 2 J ~ i ( x i , u i ) ∂ x i ∂ u i δ u i + 1 2 δ u i T ∂ 2 J ~ i ( x i , u i ) ∂ u i ∂ x i δ x i ≜ J ~ i ( x i , u i ) + Q x i T δ x i + Q u i T δ u i + 1 2 δ x i T Q x i 2 δ x i + 1 2 δ u i T Q u i 2 δ u i + 1 2 δ x i T Q x i u i δ u i + 1 2 δ u i T Q u i x i T δ x i ( 9 ) \begin{align*} \tilde{J}_{i}\left(x_{i},u_{i}\right)+\delta\tilde{J}_{i}\left(x_{i},u_{i}\right) &= \tilde{J}_{i}\left(x_{i}+\delta x_{i},u_{i}+\delta u_{i}\right) \\ &\approx \tilde{J}_{i}\left(x_{i},u_{i}\right)+\frac{\partial\tilde{J}_{i}\left(x_{i},u_{i}\right)}{\partial x_{i}}\delta x_{i}+\frac{\partial\tilde{J}_{i}\left(x_{i},u_{i}\right)}{\partial u_{i}}\delta u_{i} \\ &\quad+\frac{1}{2}\delta x_{i}^{T}\frac{\partial^{2}\tilde{J}_{i}\left(x_{i},u_{i}\right)}{\partial x_{i}^{2}}\delta x_{i}+\frac{1}{2}\delta u_{i}^{T}\frac{\partial^{2}\tilde{J}_{i}\left(x_{i},u_{i}\right)}{\partial u_{i}^{2}}\delta u_{i} \\ &\quad+\frac{1}{2}\delta x_{i}^{T}\frac{\partial^{2}\tilde{J}_{i}\left(x_{i},u_{i}\right)}{\partial x_{i}\partial u_{i}}\delta u_{i}+\frac{1}{2}\delta u_{i}^{T}\frac{\partial^{2}\tilde{J}_{i}\left(x_{i},u_{i}\right)}{\partial u_{i}\partial x_{i}}\delta x_{i} \\ &\triangleq \tilde{J}_{i}\left(x_{i},u_{i}\right)+Q_{x_{i}}^{T}\delta x_{i}+Q_{u_{i}}^{T}\delta u_{i}+\frac{1}{2}\delta x_{i}^{T}Q_{x_{i}^{2}}\delta x_{i} \\ &\quad+\frac{1}{2}\delta u_{i}^{T}Q_{u_{i}^{2}}\delta u_{i}+\frac{1}{2}\delta x_{i}^{T}Q_{x_{i} u_{i}}\delta u_{i} + \frac{1}{2}\delta u_{i}^{T}Q_{u_{i} x_{i}}^{T}\delta x_{i} \end{align*}\qquad(9) J~i(xi,ui)+δJ~i(xi,ui)=J~i(xi+δxi,ui+δui)J~i(xi,ui)+xiJ~i(xi,ui)δxi+uiJ~i(xi,ui)δui+21δxiTxi22J~i(xi,ui)δxi+21δuiTui22J~i(xi,ui)δui+21δxiTxiui2J~i(xi,ui)δui+21δuiTuixi2J~i(xi,ui)δxiJ~i(xi,ui)+QxiTδxi+QuiTδui+21δxiTQxi2δxi+21δuiTQui2δui+21δxiTQxiuiδui+21δuiTQuixiTδxi(9)
    根据公式(5),对 J ~ ( x , u ) \tilde{J}\left( x,u \right) J~(x,u)做泰勒展开有:
    J ~ i ( x i , u i ) + δ J ~ i ( x i , u i ) = J ~ i ( x i + δ x i , u i + δ u i ) = l ( x i + δ x i , u i + δ u i ) + J ^ i + 1 ( f ( x i + δ x i , u i + δ u i ) ) ( 10 ) \begin{align*} \tilde{J}_{i}(x_{i}, u_{i}) + \delta \tilde{J}_{i}(x_{i}, u_{i}) &= \tilde{J}_{i}(x_{i} + \delta x_{i}, u_{i} + \delta u_{i}) \\ &= l(x_{i} + \delta x_{i}, u_{i} + \delta u_{i}) + \hat{J}_{i+1}(f(x_{i} + \delta x_{i}, u_{i} + \delta u_{i})) \end{align*}\qquad(10) J~i(xi,ui)+δJ~i(xi,ui)=J~i(xi+δxi,ui+δui)=l(xi+δxi,ui+δui)+J^i+1(f(xi+δxi,ui+δui))(10)
    其中,即时代价展开为:
    l ( x i + δ x i , u i + δ u i ) = l ( x i , u i ) + δ l ( x i , u i ) ≈ l ( x i , u i ) + ∂ l ( x i , u i ) ∂ x i δ x i + ∂ l ( x i , u i ) ∂ u i δ u i + 1 2 δ x i T ∂ 2 l ( x i , u i ) ∂ x i 2 δ x i + 1 2 δ u i T ∂ 2 l ( x i , u i ) ∂ u i 2 δ u i + 1 2 δ x i T ∂ 2 l ( x i , u i ) ∂ x i ∂ u i δ u i + 1 2 δ u i T ∂ 2 l ( x i , u i ) ∂ u i ∂ x i δ x i ≜ l ( x i , u i ) + l x i δ x i + l u i δ u i + 1 2 δ x i T l x i 2 δ x i + 1 2 δ u i T l u i 2 δ u i + 1 2 δ x i T l x i u i δ u i + 1 2 δ u i T l u i x i δ x i ( 11 ) \begin{align*} l(x_{i}+\delta x_{i},u_{i}+\delta u_{i}) &= l(x_{i},u_{i})+\delta l(x_{i},u_{i}) \\ &\approx l(x_{i},u_{i})+\frac{\partial l(x_{i},u_{i})}{\partial x_{i}}\delta x_{i}+\frac{\partial l(x_{i},u_{i})}{\partial u_{i}}\delta u_{i} \\ &\quad+\frac{1}{2}\delta x_{i}^{T}\frac{\partial^{2} l(x_{i},u_{i})}{\partial x_{i}^{2}}\delta x_{i}+\frac{1}{2}\delta u_{i}^{T}\frac{\partial^{2} l(x_{i},u_{i})}{\partial u_{i}^{2}}\delta u_{i} \\ &\quad+\frac{1}{2}\delta x_{i}^{T}\frac{\partial^{2} l(x_{i},u_{i})}{\partial x_{i}\partial u_{i}}\delta u_{i}+\frac{1}{2}\delta u_{i}^{T}\frac{\partial^{2} l(x_{i},u_{i})}{\partial u_{i}\partial x_{i}}\delta x_{i} \\ &\triangleq l(x_{i},u_{i})+l_{x_{i}}\delta x_{i}+l_{u_{i}}\delta u_{i}+\frac{1}{2}\delta x_{i}^{T} l_{x_{i}^{2}}\delta x_{i} \\ &\quad+\frac{1}{2}\delta u_{i}^{T} l_{u_{i}^{2}}\delta u_{i}+\frac{1}{2}\delta x_{i}^{T} l_{x_{i} u_{i}}\delta u_{i}+\frac{1}{2}\delta u_{i}^{T} l_{u_{i} x_{i}}\delta x_{i} \end{align*}\qquad(11) l(xi+δxi,ui+δui)=l(xi,ui)+δl(xi,ui)l(xi,ui)+xil(xi,ui)δxi+uil(xi,ui)δui+21δxiTxi22l(xi,ui)δxi+21δuiTui22l(xi,ui)δui+21δxiTxiui2l(xi,ui)δui+21δuiTuixi2l(xi,ui)δxil(xi,ui)+lxiδxi+luiδui+21δxiTlxi2δxi+21δuiTlui2δui+21δxiTlxiuiδui+21δuiTluixiδxi(11)
    下一时刻的状态值函数展开为(这里参考式(8)):
    J ^ i + 1 ( f ( x i + δ x i , u i + δ u i ) ) = J ^ i + 1 ( f ( x i , u i ) + δ f ( x i , u i ) ) ≈ J ^ i + 1 ( f ( x i , u i ) ) + ∂ J ^ i + 1 ( x i + 1 ) ∂ x i + 1 δ f ( x i , u i ) + 1 2 δ f ( x i , u i ) T ∂ 2 J ^ i + 1 ( x i + 1 ) ∂ x i + 1 2 δ f ( x i , u i ) = J ^ i + 1 ( f ( x i , u i ) ) + p i + 1 T ( A i δ x i + B i δ u i ) + 1 2 ( A i δ x i + B i δ u i ) T P i + 1 ( A i δ x i + B i δ u i ) = J ^ i + 1 ( f ( x i , u i ) ) + p i + 1 T A i δ x i + p i + 1 T B i δ u i + 1 2 δ x i T A i T P i + 1 A i δ x i + 1 2 δ x i T A i T P i + 1 B i δ u i + 1 2 δ u i T B i T P i + 1 A i δ x i + 1 2 δ u i T B i T P i + 1 B i δ u i ( 12 ) \begin{align*} \hat{J}_{i+1}\left(f\left(x_{i}+\delta x_{i}, u_{i}+\delta u_{i}\right)\right) &= \hat{J}_{i+1}\left(f\left(x_{i}, u_{i}\right)+\delta f\left(x_{i}, u_{i}\right)\right) \\ &\approx \hat{J}_{i+1}\left(f\left(x_{i}, u_{i}\right)\right)+\frac{\partial\hat{J}_{i+1}\left(x_{i+1}\right)}{\partial x_{i+1}}\delta f\left(x_{i}, u_{i}\right) \\ &\quad+\frac{1}{2}\delta f\left(x_{i}, u_{i}\right)^{T}\frac{\partial^{2}\hat{J}_{i+1}\left(x_{i+1}\right)}{\partial x_{i+1}^{2}}\delta f\left(x_{i}, u_{i}\right) \\ &= \hat{J}_{i+1}\left(f\left(x_{i}, u_{i}\right)\right)+p_{i+1}^{T}\left(A_{i}\delta x_{i}+B_{i}\delta u_{i}\right) \\ &\quad+\frac{1}{2}\left(A_{i}\delta x_{i}+B_{i}\delta u_{i}\right)^{T} P_{i+1}\left(A_{i}\delta x_{i}+B_{i}\delta u_{i}\right) \\ &= \hat{J}_{i+1}\left(f\left(x_{i}, u_{i}\right)\right)+p_{i+1}^{T} A_{i}\delta x_{i}+p_{i+1}^{T} B_{i}\delta u_{i} \\ &\quad+\frac{1}{2}\delta x_{i}^{T} A_{i}^{T} P_{i+1} A_{i}\delta x_{i}+\frac{1}{2}\delta x_{i}^{T} A_{i}^{T} P_{i+1} B_{i}\delta u_{i} \\ &\quad+\frac{1}{2}\delta u_{i}^{T} B_{i}^{T} P_{i+1} A_{i}\delta x_{i}+\frac{1}{2}\delta u_{i}^{T} B_{i}^{T} P_{i+1} B_{i}\delta u_{i} \end{align*}\qquad(12) J^i+1(f(xi+δxi,ui+δui))=J^i+1(f(xi,ui)+δf(xi,ui))J^i+1(f(xi,ui))+xi+1J^i+1(xi+1)δf(xi,ui)+21δf(xi,ui)Txi+122J^i+1(xi+1)δf(xi,ui)=J^i+1(f(xi,ui))+pi+1T(Aiδxi+Biδui)+21(Aiδxi+Biδui)TPi+1(Aiδxi+Biδui)=J^i+1(f(xi,ui))+pi+1TAiδxi+pi+1TBiδui+21δxiTAiTPi+1Aiδxi+21δxiTAiTPi+1Biδui+21δuiTBiTPi+1Aiδxi+21δuiTBiTPi+1Biδui(12)
    其中, p i = ∂ J ^ i ( x i ) ∂ x i p_{i} = \frac{\partial \hat{J}_{i}(x_{i})}{\partial x_{i}} pi=xiJ^i(xi) P i = ∂ 2 J ^ i ( x i ) ∂ x i 2 P_{i} = \frac{\partial^2 \hat{J}_{i}(x_{i})}{\partial x_{i}^2} Pi=xi22J^i(xi)。 联合(8)(9)(10)(11)(12)可得 J ~ i ( x i , u i ) \tilde{J}_{i}(x_{i}, u_{i}) J~i(xi,ui) ( x i , u i ) (x_{i}, u_{i}) (xi,ui)附近泰勒展开的多项式系数矩阵:
    Q x i = l x i + p i + 1 T A i Q u i = l u i + p i + 1 T B i Q x i 2 = l x i 2 + A i T P i + 1 A i Q x i u i = l x i u i + A i T P i + 1 B i Q u i x i = l u i x i + B i T P i + 1 A i Q u i 2 = l u i 2 + B i T P i + 1 B i \begin{align*} Q_{x_{i}} &= l_{x_{i}} + p_{i+1}^{T} A_{i} \\ Q_{u_{i}} &= l_{u_{i}} + p_{i+1}^{T} B_{i} \\ Q_{x_{i}^{2}} &= l_{x_{i}^{2}} + A_{i}^{T} P_{i+1} A_{i} \\ Q_{x_{i} u_{i}} &= l_{x_{i} u_{i}} + A_{i}^{T} P_{i+1} B_{i} \\ Q_{u_{i} x_{i}} &= l_{u_{i} x_{i}} + B_{i}^{T} P_{i+1} A_{i} \\ Q_{u_{i}^{2}} &= l_{u_{i}^{2}} + B_{i}^{T} P_{i+1} B_{i} \tag{13} \end{align*} QxiQuiQxi2QxiuiQuixiQui2=lxi+pi+1TAi=lui+pi+1TBi=lxi2+AiTPi+1Ai=lxiui+AiTPi+1Bi=luixi+BiTPi+1Ai=lui2+BiTPi+1Bi(13)
    一般来说有 Q x i u i = Q x i u i T Q_{x_{i} u_{i}}=Q_{x_{i} u_{i}}^{T} Qxiui=QxiuiT,根据式(9),如果当前状态 x i x_{i} xi,输入是 u i u_{i} ui,希望 x i + δ x i x_{i} + \delta x_{i} xi+δxi的代价值函数 J ~ i ( x i + δ x i , u i + δ u i ) \tilde{J}_{i}\left(x_{i}+\delta x_{i},u_{i}+\delta u_{i}\right) J~i(xi+δxi,ui+δui)最小,则需要使 δ J ~ i ( x i , u i ) \delta \tilde{J}_{i}(x_{i}, u_{i}) δJ~i(xi,ui)最小,有:
    δ J ^ i ( x i ) = min ⁡ δ u i δ J ~ i ( x i , u i ) = min ⁡ δ u i { Q x i T δ x i + Q u i T δ u i + 1 2 δ x i T Q x i 2 δ x i + 1 2 δ u i T Q u i 2 δ u i + 1 2 δ x i T Q x i u i δ u i + 1 2 δ u i T Q u i x i T δ x i } = min ⁡ δ u i δ J ~ i ( x i , u i ) ( 14 ) \begin{align*} \delta \hat{J}_i(x_i) &= \min_{\delta u_i} \delta \tilde{J}_i(x_i, u_i) \\ &= \min_{\delta u_i} \left\{ Q_{x_i}^T \delta x_i + Q_{u_i}^T \delta u_i + \frac{1}{2} \delta x_i^T Q_{x_i^2} \delta x_i + \frac{1}{2} \delta u_i^T Q_{u_i^2} \delta u_i \right. \\ &\quad \left. + \frac{1}{2} \delta x_i^T Q_{x_i u_i} \delta u_i + \frac{1}{2} \delta u_i^T Q_{u_i x_i}^T \delta x_i \right\} \\ &= \min_{\delta u_i} \delta \tilde{J}_i(x_i, u_i) \end{align*} \qquad(14) δJ^i(xi)=δuiminδJ~i(xi,ui)=δuimin{QxiTδxi+QuiTδui+21δxiTQxi2δxi+21δuiTQui2δui+21δxiTQxiuiδui+21δuiTQuixiTδxi}=δuiminδJ~i(xi,ui)(14)
    δ J ~ i ( x i , u i ) \delta \tilde{J}_{i}(x_{i}, u_{i}) δJ~i(xi,ui) δ u i \delta u_{i} δui求偏导,使其取得最小值得到 δ J ^ i ( x i ) \delta \hat{J}_i(x_i) δJ^i(xi),并让其等于0有:
    ∂ δ J ~ i ( x i ) ∂ δ u i = Q u i + 1 2 Q u i x i T δ x i + Q u i 2 δ u i + 1 2 Q x i u i δ x i = Q u i + Q u i x i δ x i + Q u i 2 δ u i = 0 ( 15 ) \begin{align*} \frac{\partial \delta \tilde{J}_i(x_i)}{\partial \delta u_i} &= Q_{u_i} + \frac{1}{2} Q_{u_i x_i}^T \delta x_i + Q_{u_i^2} \delta u_i + \frac{1}{2} Q_{x_i u_i} \delta x_i \\ &= Q_{u_i} + Q_{u_i x_i} \delta x_i + Q_{u_i^2} \delta u_i = 0 \end{align*} \qquad(15) δuiδJ~i(xi)=Qui+21QuixiTδxi+Qui2δui+21Qxiuiδxi=Qui+Quixiδxi+Qui2δui=0(15)
    解出来在 i i i时刻的最优控制输入的变化量:
    δ u ^ i ∗ = − Q u i 2 − 1 ( Q u i + Q u i x i δ x i ) ≜ K i δ x i + d i ( 16 ) \begin{align*} \delta \hat{u}_i^* &= -Q_{u_i^2}^{-1} \left( Q_{u_i} + Q_{u_i x_i} \delta x_i \right) \\ &\triangleq K_i \delta x_i + d_i \end{align*} \qquad(16) δu^i=Qui21(Qui+Quixiδxi)Kiδxi+di(16)
    iLQR的核心思想就是在给定初解 u i u_i ui的基础上,求最优的补偿输入 δ u ^ i ∗ \delta \hat{u}_i^* δu^i 使得在 u i + δ u ^ i ∗ u_i+\delta \hat{u}_i^* ui+δu^i的作用下获得更优的解,并循环这个过程。
    K i K_i Ki为反馈增益, d i d_i di为前馈增益。将式(16)代入到式(9)整理得:
    δ J ~ i ( x i , u i ∗ ) = Q x i T δ x i + Q u i T ( K i δ x i + d i ) + 1 2 δ x i T Q x i 2 δ x i + 1 2 ( K i δ x i + d i ) T Q u i 2 ( K i δ x i + d i ) + 1 2 δ x i T Q x i u i ( K i δ x i + d i ) + 1 2 ( K i δ x i + d i ) T Q u i x i δ x i = 1 2 δ x i T [ Q x i 2 + K i T Q u i 2 K i + Q x i u i K i + K i T Q u i x i ] δ x i + [ Q x i + K i T Q u i 2 d i + Q x i u i d i + K i T Q u i d i ] T δ x i + 1 2 d i T Q u i 2 d i + Q u i T d i ( 17 ) \begin{align*} \delta \tilde{J}_i(x_i, u_i^*) &=Q_{x_i}^T \delta x_i + Q_{u_i}^T (K_i \delta x_i + d_i) + \frac{1}{2} \delta x_i^T Q_{x_i^2} \delta x_i \\ &\quad+ \frac{1}{2} (K_i \delta x_i + d_i)^T Q_{u_i^2} (K_i \delta x_i + d_i) \\ &\quad + \frac{1}{2} \delta x_i^T Q_{x_i u_i} (K_i \delta x_i + d_i) + \frac{1}{2} (K_i \delta x_i + d_i)^T Q_{u_i x_i} \delta x_i \\ &= \frac{1}{2} \delta x_i^T [Q_{x_i^2} + K_i^T Q_{u_i^2} K_i + Q_{x_i u_i} K_i + K_i^T Q_{u_i x_i}] \delta x_i \\ &\quad + [Q_{x_i} + K_i^T Q_{u_i^2} d_i + Q_{x_i u_i} d_i + K_i^T Q_{u_i} d_i]^T \delta x_i \\ &\quad + \frac{1}{2} d_i^T Q_{u_i^2} d_i + Q_{u_i}^T d_i \end{align*} \qquad(17) δJ~i(xi,ui)=QxiTδxi+QuiT(Kiδxi+di)+21δxiTQxi2δxi+21(Kiδxi+di)TQui2(Kiδxi+di)+21δxiTQxiui(Kiδxi+di)+21(Kiδxi+di)TQuixiδxi=21δxiT[Qxi2+KiTQui2Ki+QxiuiKi+KiTQuixi]δxi+[Qxi+KiTQui2di+Qxiuidi+KiTQuidi]Tδxi+21diTQui2di+QuiTdi(17)
    这里需要注意的是 δ J ~ i ( x i , u i ∗ ) = δ J ^ i ( x i ) \delta \tilde{J}_i(x_i, u_i^*)=\delta \hat{J}_i(x_i) δJ~i(xi,ui)=δJ^i(xi),结合式(8)式(17)可以得到 p i p_{i} pi P i P_{i} Pi的递推关系式:
    p i = Q x i + K i T Q u i 2 d i + Q x i u i d i + K i T Q u P i = Q x i 2 + K i T Q u i 2 K i + Q x i u i K i + K i T Q u i x i Δ J ^ i = 1 2 d i T Q u i 2 d i + Q u i T d i ( 18 ) \begin{align*} p_i &= Q_{x_i} + K_i^T Q_{u_i^2} d_i + Q_{x_i u_i} d_i + K_i^T Q_u \\ P_i &= Q_{x_i^2} + K_i^T Q_{u_i^2} K_i + Q_{x_i u_i} K_i + K_i^T Q_{u_i x_i} \\ \Delta \hat{J}_i &= \frac{1}{2} d_i^T Q_{u_i^2} d_i + Q_{u_i}^T d_i \end{align*} \qquad(18) piPiΔJ^i=Qxi+KiTQui2di+Qxiuidi+KiTQu=Qxi2+KiTQui2Ki+QxiuiKi+KiTQuixi=21diTQui2di+QuiTdi(18)
    其中, p N T = ∂ l f ( x N ) ∂ x N p_{N}^T = \frac{\partial l_f(x_{N})}{\partial x_{N}} pNT=xNlf(xN) P N = ∂ 2 l f ( x N ) ∂ x N 2 P_{N} = \frac{\partial^2 l_f(x_{N})}{\partial x_{N}^2} PN=xN22lf(xN) Δ J ^ i \Delta \hat{J}_i ΔJ^i J ^ i ( x i ) \hat{J}_i(x_i) J^i(xi)二阶泰勒展开的高阶无穷小项,与 δ x \delta x δx无关,不影响 δ u \delta u δu的计算。
    从上面的推导可以看出,如果确定 p N T p_{N}^T pNT P N P_{N} PN,就可以通过式(18)来递推得到 p N − 1 T p_{N-1}^T pN1T P N − 1 P_{N-1} PN1之所以有backward pass,就是为了从 N − 1 N-1 N1时刻,一直倒推到当前时刻,求出每个时刻的最优控制输入的变化量。

1.3 ilqr算法迭代过程

  1. Backward pass
    初始化 p N T = ∂ l f ( x N ) ∂ x N p_{N}^T = \frac{\partial l_f(x_{N})}{\partial x_{N}} pNT=xNlf(xN) P N = ∂ 2 l f ( x N ) ∂ x N 2 P_{N} = \frac{\partial^2 l_f(x_{N})}{\partial x_{N}^2} PN=xN22lf(xN)
    f o r for for i = N − 1 , . . . , 0 i = N-1,...,0 i=N1,...,0
    根据式(13)式(18)更新 p i ← p i + 1 p_{i} \leftarrow p_{i+1} pipi+1 P i ← P i + 1 P_{i} \leftarrow P_{i+1} PiPi+1
    根据式(16)计算 K i = Q u i 2 − 1 Q u i x i K_i = Q_{u_i^2}^{-1} Q_{u_i x_i} Ki=Qui21Quixi d i = Q u i 2 − 1 Q u i d_i = Q_{u_i^2}^{-1} Q_{u_i} di=Qui21Qui
  2. Forward pass
    初始条件 x 0 x_{0} x0,对于任意迭代次数 j j j x 0 j = x 0 j − 1 x_{0}^j = x_{0}^{j-1} x0j=x0j1,这里也好理解,每次迭代都是基于当前时刻的状态,所以不会变
    f o r for for i = 0 , . . . , N − 1 i = 0,...,N-1 i=0,...,N1
    δ x i = x i j − x i j − 1 \delta x_i = x_i^j - x_i^{j-1} δxi=xijxij1 δ x i \delta x_i δxi的含义就是当前轮迭代解和上一轮迭代解在 i i i时刻的差值(是不是有牛顿法那味了)
    δ u ^ i ∗ = K i δ x i + d i \delta \hat{u}_i^* = K_i \delta x_i + d_i δu^i=Kiδxi+di,更新控制输入 u i j = u i j − 1 + δ u ^ i ∗ u_{i}^j = u_{i}^{j-1} + \delta\hat{u}_i^* uij=uij1+δu^i
    更新状态 x i + 1 j = f ( x i j , u i j ) x_{i+1}^j = f(x_{i}^{j},u_{i}^{j}) xi+1j=f(xij,uij)
  3. 满足 ∣ J ( j + 1 ) − J ( j ) J ( j ) ∣ < ϵ \left| \frac{J^{(j+1)} - J^{(j)}}{J^{(j)}} \right| < \epsilon J(j)J(j+1)J(j) <ϵ或者迭代达到最大轮次终止迭代。

二. ilqr实践代码

参考这位博主: 最优化理论与自动驾驶(十一):基于iLQR的自动驾驶轨迹跟踪算法(c++和python版本)

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/890234.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

应该连续学一个科目,还是多学科切换?

https://www.zhihu.com/question/333420829https://www.zhihu.com/question/333420829

excel 使用vlook up找出两列中不同的内容

当使用 VLOOKUP 函数时&#xff0c;您可以将其用于比较两列的内容。假设您要比较 A 列和 B 列的内容&#xff0c;并将结果显示在 C 列&#xff0c;您可以在 C1 单元格中输入以下公式&#xff1a; 这个公式将在 B 列中的每个单元格中查找是否存在于 A 列中。如果在 A 列中找不到…

2024159读书笔记|《南山册页:齐白石果蔬册鱼虫册》节选

2024159读书笔记|《南山册页&#xff1a;齐白石果蔬册&鱼虫册》节选 1. 《南山册页&#xff1a;齐白石鱼虫册》2. 《南山册页&#xff1a;齐白石果蔬册》 1. 《南山册页&#xff1a;齐白石鱼虫册》 《南山册页&#xff1a;齐白石鱼虫册》南山书画&#xff0c;大家之作&…

【AI驱动的数据结构:包装类的艺术与科学】

&#x1f308;个人主页: Aileen_0v0 &#x1f525;热门专栏: 华为鸿蒙系统学习|计算机网络|数据结构与算法 ​&#x1f4ab;个人格言:“没有罗马,那就自己创造罗马~” 文章目录 包装类装箱和拆箱阿里巴巴面试题 包装类 在Java中基本数据类型不是继承来自Object&#xff0c;为了…

Nginx 双向链表 ngx_queue_t

目录 一、基本概述 二、数据结构 三、接口描述与实现 1、相关宏接口 2、ngx_queue_middle 3、ngx_queue_sort 四、使用案例 整理自 nginx 1.9.2 源码 和 《深入理解 Nginx&#xff1a;模块开发与架构解析》 一、基本概述 双向链表的优势是可以快速进行数据插入、删除与…

亚信安全春节14天双倍假期通告

亚信安全14天双倍假期来袭 “网安福利王”再次实至名归 2024年 8773小时&#xff0c;31582680秒 亚信安全一直驰骋于云网安世界 奋战在“安全 数智化”的壮阔征途上 如今&#xff0c;新春的脚步渐近 长达14天的春节长假 能让我们暂且放下忙碌的工作 去除班味&#xff0c…

【时间之外】IT人求职和创业应知【71】-专利费

目录 2025 ICT产业趋势年会召开&#xff0c;2024年度ICT十大新闻重磅揭晓 海纳致远数字科技申请定制化插件驱动的数据分析专利 阿波罗智联取得语音数据的处理方法、装置、设备和存储介质专利 心勿贪&#xff0c;贵知足。 感谢所有打开这个页面的朋友。人生不如意&#xff0…

游戏《姆吉拉的假面》启动时提示“xinput1_3.dll丢失”怎么办?“xinput1_3.dll丢失”要怎么解决?

《姆吉拉的假面》报错&#xff1a;xinput1_3.dll丢失&#xff1f;这里有解决之道&#xff01; 在畅游《姆吉拉的假面》这款经典游戏时&#xff0c;你是否遇到过“xinput1_3.dll丢失”的报错信息&#xff1f;这个错误不仅会影响你的游戏体验&#xff0c;还可能让你陷入无法继续…

数据分析实战—鸢尾花数据分类

1.实战内容 (1) 加载鸢尾花数据集(iris.txt)并存到iris_df中,使用seaborn.lmplot寻找class&#xff08;种类&#xff09;项中的异常值&#xff0c;其他异常值也同时处理 。 import pandas as pd from sklearn.datasets import load_iris pd.set_option(display.max_columns, N…

hive注释comment中文乱码解决

问题描述 当使用以下命令查看表的元数据信息时出现中文乱码&#xff08;使用的是idea连接hive&#xff09; desc formatted test.t_archer; 解决 连接保存hive元数据的MySQL数据库&#xff0c;执行以下命令&#xff1a; use hive3; show tables;alter table hive3.COLUMNS_…

maven项目运行时NoSuchMethodError问题排查记录(依赖冲突解决)

控制台异常如下&#xff1a; Handler dispatch failed; nested exception is java.lang.NoSuchMethodError: org.apache.commons.io.input.BoundedInputStream.builder()Lorg/apache/commons/io/input/BoundedInputStream$Builder;问题明显&#xff0c;根据NoSuchMethodError…

java 选择排序,涵盖工作原理、算法分析、实现细节、优缺点以及一些实际应用场景

选择排序的详细解析 更深入地探讨选择排序的各个方面&#xff0c;包括其工作原理、算法分析、实现细节、优缺点以及一些实际应用场景。 动画演示 1. 基本概念 选择排序是一种简单的比较排序算法。它的核心思想是将数组分为两个部分&#xff1a;已排序部分和未排序部分。每…

矩阵-向量乘法的行与列的解释(Row and Column Interpretations):中英双语

本文是学习这本书的笔记 网站是&#xff1a;https://web.stanford.edu/~boyd/vmls/ 矩阵-向量乘法的行与列的解释 矩阵-向量乘法&#xff08;Matrix-Vector Multiplication&#xff09;是线性代数中的基本操作&#xff0c;也是机器学习、数据科学和工程中常用的数学工具。本文…

基于海思soc的智能产品开发(巧用mcu芯片)

【 声明&#xff1a;版权所有&#xff0c;欢迎转载&#xff0c;请勿用于商业用途。 联系信箱&#xff1a;feixiaoxing 163.com】 对于开发车规级嵌入式软件的同学来说&#xff0c;socmcu这样的组合&#xff0c;他们并不陌生。但是传统的工业领域&#xff0c;比如发动机、医疗或…

带有 Elasticsearch 和 Langchain 的 Agentic RAG

作者&#xff1a;来自 Elastic Han Xiang Choong 讨论并实现 Elastic RAG 的代理流程&#xff0c;其中 LLM 选择调用 Elastic KB。 更多阅读&#xff1a;Elasticsearch&#xff1a;基于 Langchain 的 Elasticsearch Agent 对文档的搜索。 简介 代理是将 LLM 应用于实际用例的…

SmartX分享:NVMe-oF 介绍、SMTX ZBS 如何选择高性能场景解决方案与如何实现

目录 背景什么是 NVMe-oFZBS AccessiSCSI 与 iSERNMVe-oF 介绍NVMeNVMe-oFNVMe-oF 承载网络&#xff08;数据平面&#xff09; ZBS NVMe-oF 实现ZBS 接入策略ZBS 接入点分配策略性能测试 为什么要支持 RoCE引用 背景 前几篇文章&#xff0c;我们认识到了 SmartX 公司产品 SMTX…

【机器学习】机器学习的基本分类-强化学习(Reinforcement Learning, RL)

强化学习&#xff08;Reinforcement Learning, RL&#xff09;是一种基于试错的方法&#xff0c;旨在通过智能体与环境的交互&#xff0c;学习能够最大化累积奖励的策略。以下是强化学习的详细介绍。 强化学习的核心概念 智能体&#xff08;Agent&#xff09; 执行动作并与环境…

MyBatis-Plus中isNull与SQL语法详解:处理空值的正确姿势

目录 前言1. 探讨2. 基本知识3. 总结 前言 &#x1f91f; 找工作&#xff0c;来万码优才&#xff1a;&#x1f449; #小程序://万码优才/r6rqmzDaXpYkJZF 基本的Java知识推荐阅读&#xff1a; java框架 零基础从入门到精通的学习路线 附开源项目面经等&#xff08;超全&#x…

Spring Boot 项目创建

创建一个新项目&#xff1a; 打开 Spring Initializr 网址&#xff1a;https://start.spring.io/ &#xff0c;然后创建一个新项目&#xff1a; springboot3.3.5_jdk17&#xff1a; Project&#xff08;Maven&#xff09;编程语言&#xff08;Java 17&#xff09;Spring Boo…

基于蓝牙通信的手机遥控智能灯(论文+源码)

1.系统设计 灯具作为人们日常生活的照明工具为人们生活提供光亮&#xff0c;本次基于蓝牙通信的手机遥控智能灯设计功能如下&#xff1a; &#xff08;1&#xff09;用户可以通过蓝牙通信模块的作用下&#xff0c;在手机端遥控切换智能灯不同的工作模式&#xff1b; &#x…