0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

---

1 LQR算法的局限性

若系统动力学特性可以用一组线性微分方程表示，且性能指标为状态变量和控制变量的二次型函数，则此类最优控制问题称为线性二次型问题。线性二次调节器(Linear Quadratic Regulator, LQR)是求解线性二次型问题常用的求解方法之一，其数学形式优美且求解解析性强。然而，LQR固有的局限性也相当显著，主要体现在两个方面：

• LQR的核心理论建立在线性系统模型与二次型代价函数的基础之上，对非线性的被控对象应用LQR必须首先在工作点附近进行线性化，这种近似处理在状态偏离工作点较大或期望优化的轨迹远离线性化区域时，控制器性能会急剧下降甚至导致不稳定；
• LQR本质上是一种无限时间域的静态状态反馈策略，它求解的是一个全局统一的、固定的反馈增益矩阵，通常用于初解生成(这方面应用可以参考轨迹优化 | 基于最优控制的运动学约束路径平滑与粗轨迹生成(附ROS C++/Python实现))。静态反馈难以处理具有复杂约束、时变目标或需要在状态空间中进行大范围机动的优化问题。

从LQR的线性化局限可以看出，对于轨迹优化问题

$$\begin{gathered} J\left(\mathbf{u}\right)={\ell }_f\left({\mathbf{x}}_N\right)+\sum _{k=0}^{N-1}\ell \left({\mathbf{x}}_k,{\mathbf{u}}_k\right) \\ s.t.{\mathbf{x}}_{k+1}=\mathbf{f}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right),k=0,1,\cdots ,N-1 \end{gathered}$$

其中系统动力学和损失函数均为非线性函数。若仅在${\mathbf{x}}_0$处线性化，那么在给定轨迹的其他位置${\mathbf{x}}_i$都不容易满足该局部线性假设。自然地，如果在每个轨迹点都进行线性化，就能避免单一线性模型带来的误差，同时当轨迹点足够稠密时能取得接近非线性模型的精度。本节介绍应用该思想处理非线性轨迹优化问题的微分动态规划(Differential Dynamic Programming, DDP)算法及其特例迭代线性二次型调节器iLQR算法。

2 贝尔曼最优性原理

离散时间系统的控制问题本质上是基于迭代的多级决策问题，求解的核心是贝尔曼最优性原理(Bellman’s Principle of Optimality)，表述为：若

$$\mathbf{x}\left(1\right),\mathbf{x}\left(2\right),\cdots ,\mathbf{x}\left(N\right)$$

是0时刻起，以$\mathbf{x}\left(0\right)$为初始状态的控制问题最优解，则对$\forall k=0,1,\cdots ,N-1$，决策序列

$$\mathbf{x}\left(k+1\right),\mathbf{x}\left(k+2\right),\cdots ,\mathbf{x}\left(N\right)$$

是$k$为初始时刻，${\mathbf{x}}_k$为初始状态的子问题的最优解。贝尔曼最优性原理体现了动态规划在控制理论中的应用。

为了应用贝尔曼最优性原理，定义最优控制下最小化的性能指标

$$V\left(\mathbf{x}\left(k\right),k\right)\stackrel{\mathrm{def}}{=}\underset{\mathbf{u}\in \mathcal{U}}{\min }J\left(\mathbf{u};\mathbf{x}\left(k\right),k\right),k=0,1,\cdots$$

为最优控制问题的值函数(Value Function)。离散时间最优控制问题的最优控制满足以下关于值函数的贝尔曼方程(Bellman Equation)

{ V ( x ( N ) , N ) = h D ( x ( N ) , N ) V ( x ( k ) , k ) = min ⁡ u ∈ U { g D ( x ( k ) , u ( k ) , k ) + V ( x ( k + 1 ) , k + 1 ) }    k = N − 1 , ⋯   , 0 \begin{cases} V\left( \boldsymbol{x}\left( N \right) ,N \right) =h_D\left( \boldsymbol{x}\left( N \right) , N \right)\\ V\left( \boldsymbol{x}\left( k \right) ,k \right) =\underset{\boldsymbol{u}\in \mathcal{U}}{\min}\left\{ g_D\left( \boldsymbol{x}\left( k \right) ,\boldsymbol{u}\left( k \right) ,k \right) +V\left( \boldsymbol{x}\left( k+1 \right) , k+1 \right) \right\} \,\,k=N-1,\cdots ,0\\\end{cases} {V(x(N),N)=hD​(x(N),N)V(x(k),k)=u∈Umin​{gD​(x(k),u(k),k)+V(x(k+1),k+1)}k=N−1,⋯,0​

本质上是一个反向动态规划过程。若能解得贝尔曼方程的唯一解，即对$\forall k=0,1,\cdots ,N-1$都有已知的函数$V\left(\mathbf{x}\left(k\right),k\right)$，则对于任意时刻$k$和任意可能状态${\mathbf{x}}_k$，可得闭环最优控制

u ( k ) = a r g min ⁡ u ∈ U { g D ( x ( k ) , u ( k ) , k ) + V ( f D ( x ( k ) , u ( k ) , k ) , k + 1 ) } \boldsymbol{u}\left( k \right) =\mathrm{arg}\min _{\boldsymbol{u}\in \mathcal{U}}\left\{ g_D\left( \boldsymbol{x}\left( k \right) ,\boldsymbol{u}\left( k \right) ,k \right) +V\left( f_D\left( \boldsymbol{x}\left( k \right) ,\boldsymbol{u}\left( k \right) , k \right) , k+1 \right) \right\} u(k)=argu∈Umin​{gD​(x(k),u(k),k)+V(fD​(x(k),u(k),k),k+1)}

3 微分动态规划DDP原理

对于第$k$个轨迹点，将系统状态方程$\mathbf{f}$在$\mathbf{x}={\mathbf{x}}_k$、$\mathbf{u}={\mathbf{u}}_k$处二阶展开

$$\begin{gathered} \mathbf{f}\left({\mathbf{x}}_k+\delta \mathbf{x},{\mathbf{u}}_k+\delta \mathbf{u}\right)=\mathbf{f}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+{\mathbf{F}}_{\mathbf{x}}\delta \mathbf{x}+{\mathbf{F}}_{\mathbf{u}}\delta \mathbf{u} \\ +\frac{1}{2}{\mathbf{F}}_{\mathbf{xx}}\left(\delta \mathbf{x}\otimes {\mathbf{I}}_n\right)\delta \mathbf{x}+\frac{1}{2}{\mathbf{F}}_{\mathbf{uu}}\left(\delta \mathbf{u}\otimes {\mathbf{I}}_m\right)\delta \mathbf{u}+\frac{1}{2}{\mathbf{F}}_{\mathbf{xu}}\left(\delta \mathbf{x}\otimes {\mathbf{I}}_m\right)\delta \mathbf{u}+\frac{1}{2}{\mathbf{F}}_{\mathbf{ux}}\left(\delta \mathbf{u}\otimes {\mathbf{I}}_n\right)\delta \mathbf{x} \end{gathered}$$

同样地，将损失函数在$\mathbf{x}={\mathbf{x}}_k$、$\mathbf{u}={\mathbf{u}}_k$处二阶展开

$$\begin{gathered} J\left(\mathbf{u}\right)={\mathbf{\ell }}_{f,\mathbf{x}}^T\delta {\mathbf{x}}_N+\frac{1}{2}\delta {\mathbf{x}}_N^T{\mathbf{\ell }}_{f,\mathbf{xx}}\delta {\mathbf{x}}_N+ \\ \sum _{k=0}^{N-1}\left({\mathbf{\ell }}_{\mathbf{x}}^T\delta {\mathbf{x}}_k+{\mathbf{\ell }}_{\mathbf{u}}^T\delta {\mathbf{u}}_k+\frac{1}{2}\delta {\mathbf{x}}_k^T{\mathbf{\ell }}_{\mathbf{xx}}\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^T{\mathbf{\ell }}_{\mathbf{uu}}\delta {\mathbf{u}}_k+\frac{1}{2}\delta {\mathbf{x}}_k^T{\mathbf{\ell }}_{\mathbf{xu}}\delta {\mathbf{u}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^T{\mathbf{\ell }}_{\mathbf{ux}}\delta {\mathbf{x}}_k\right) \end{gathered}$$

根据贝尔曼最优性原理可得

{ V N ( x N ) = ℓ f ( x N ) V k ( x k ) = min ⁡ u k { ℓ ( x k , u k ) + V k + 1 ( x k + 1 ) } \begin{cases} V_N\left( \boldsymbol{x}_N \right) =\ell _f\left( \boldsymbol{x}_N \right)\\ V_k\left( \boldsymbol{x}_k \right) =\min _{\boldsymbol{u}_k}\left\{ \ell \left( \boldsymbol{x}_k, \boldsymbol{u}_k \right) +V_{k+1}\left( \boldsymbol{x}_{k+1} \right) \right\}\\\end{cases} {VN​(xN​)=ℓf​(xN​)Vk​(xk​)=minuk​​{ℓ(xk​,uk​)+Vk+1​(xk+1​)}​

对值函数在$\mathbf{x}={\mathbf{x}}_k$处二阶泰勒展开

$$V_k\left({\mathbf{x}}_k+\delta \mathbf{x}\right)=V_k\left({\mathbf{x}}_k\right)+{\mathbf{s}}_k^T\delta \mathbf{x}+\frac{1}{2}\delta {\mathbf{x}}^T{\mathbf{S}}_k\delta \mathbf{x}$$

设动作值函数

$$Q_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)=\ell \left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+V_{k+1}\left({\mathbf{x}}_{k+1}\right)=\ell \left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+V_{k+1}\left(\mathbf{f}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\right)$$

对动作-值函数在$\mathbf{x}={\mathbf{x}}_k$、$\mathbf{u}={\mathbf{u}}_k$处二阶泰勒展开

$$\begin{gathered} Q_k\left({\mathbf{x}}_k+\delta \mathbf{x},{\mathbf{u}}_k+\delta \mathbf{u}\right)=Q_k\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)+{\mathbf{Q}}_{k,\mathbf{x}}^T\delta \mathbf{x}+{\mathbf{Q}}_{k,\mathbf{u}}^T\delta \mathbf{u} \\ +\frac{1}{2}\delta {\mathbf{x}}^T{\mathbf{Q}}_{k,\mathbf{xx}}\delta \mathbf{x}+\frac{1}{2}\delta {\mathbf{u}}^T{\mathbf{Q}}_{k,\mathbf{uu}}\delta \mathbf{u}+\frac{1}{2}\delta {\mathbf{x}}^T{\mathbf{Q}}_{k,\mathbf{xu}}\delta \mathbf{u}+\frac{1}{2}\delta {\mathbf{u}}^T{\mathbf{Q}}_{k,\mathbf{ux}}\delta \mathbf{x} \end{gathered}$$

求导利用链式法则和乘法原理

$$\begin{gathered} {\mathbf{Q}}_{k,\mathbf{u}}={\mathbf{\ell }}_{\mathbf{u}}+{\mathbf{F}}_{\mathbf{u}}^T{\mathbf{s}}_{k+1} \\ {\mathbf{Q}}_{k,\mathbf{xx}}={\mathcal{L}}_{\mathbf{xx}}+{\mathbf{F}}_{\mathbf{x}}^T{\mathbf{S}}_{k+1}{\mathbf{F}}_{\mathbf{x}}+\underset{\text{二阶部分}}{\underbrace{T\left({\mathbf{F}}_{\mathbf{xx}}\right)\left({\mathbf{s}}_{k+1}^{}\otimes {\mathbf{I}}_n\right)}} \\ {\mathbf{Q}}_{k,\mathbf{uu}}={\mathcal{L}}_{\mathbf{uu}}+{\mathbf{F}}_{\mathbf{u}}^T{\mathbf{S}}_{k+1}{\mathbf{F}}_{\mathbf{u}}+\underset{\text{二阶部分}}{\underbrace{T\left({\mathbf{F}}_{\mathbf{uu}}\right)\left({\mathbf{s}}_{k+1}^{}\otimes {\mathbf{I}}_m\right)}} \\ {\mathbf{Q}}_{k,\mathbf{ux}}={\mathcal{L}}_{\mathbf{ux}}+{\mathbf{F}}_{\mathbf{u}}^T{\mathbf{S}}_{k+1}{\mathbf{F}}_{\mathbf{x}}+\underset{\text{二阶部分}}{\underbrace{T\left({\mathbf{F}}_{\mathbf{ux}}\right)\left({\mathbf{s}}_{k+1}^{}\otimes {\mathbf{I}}_n\right)}} \end{gathered}$$

当不考虑环境的二阶属性时，DDP退化为迭代LQR算法(Iterative Linear Quadratic Regulator, iLQR)。

对泛函增量 Δ V k ( x k + δ x ) = min ⁡ δ u { Δ Q k ( x k + δ x , u k + δ u ) } \varDelta V_k\left( \boldsymbol{x}_k+\delta \boldsymbol{x} \right) =\min _{\delta \boldsymbol{u}}\left\{ \varDelta Q_k\left( \boldsymbol{x}_k+\delta \boldsymbol{x}, \boldsymbol{u}_k+\delta \boldsymbol{u} \right) \right\} ΔVk​(xk​+δx)=minδu​{ΔQk​(xk​+δx,uk​+δu)}求最优扰动$\partial \Delta Q_k\left({\mathbf{x}}_k+\delta \mathbf{x},{\mathbf{u}}_k+\delta \mathbf{u}\right)/\partial \delta \mathbf{u}=0$可得

$$\delta {\mathbf{u}}^{\ast }={\mathbf{K}}_k\delta \mathbf{x}+{\mathbf{d}}_k$$

其中${\mathbf{K}}_k=-{\mathbf{Q}}_{k,\mathbf{uu}}^{-1}{\mathbf{Q}}_{k,\mathbf{ux}}$、${\mathbf{d}}_k=-{\mathbf{Q}}_{k,\mathbf{uu}}^{-1}{\mathbf{Q}}_{k,\mathbf{u}}$

将$\delta {\mathbf{u}}^{\ast }$代回$\Delta Q_k\left({\mathbf{x}}_k+\delta \mathbf{x},{\mathbf{u}}_k+\delta \mathbf{u}\right)$对比$\Delta V_k\left({\mathbf{x}}_k+\delta \mathbf{x}\right)={\mathbf{s}}_k^T\delta \mathbf{x}+\delta {\mathbf{x}}^T{\mathbf{S}}_k\delta \mathbf{x}/2$可得

$$\{\begin{array}{l}{\mathbf{s}}_k={\mathbf{Q}}_{k,\mathbf{x}}+{\mathbf{K}}_k^T{\mathbf{Q}}_{k,\mathbf{uu}}{\mathbf{d}}_k+{\mathbf{Q}}_{k,\mathbf{ux}}^T{\mathbf{d}}_k+{\mathbf{K}}_k^T{\mathbf{Q}}_{k,\mathbf{u}}\\ {\mathbf{S}}_k={\mathbf{Q}}_{k,\mathbf{xx}}+{\mathbf{K}}_k^T{\mathbf{Q}}_{k,\mathbf{uu}}{\mathbf{K}}_k+{\mathbf{Q}}_{k,\mathbf{xu}}^T{\mathbf{K}}_k+{\mathbf{K}}_k^T{\mathbf{Q}}_{k,\mathbf{ux}}\end{array}$$

至此，DDP与iLQR算法的核心原理推导完成

4 算法流程与图示

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇