数值计算 | 图解基于龙格库塔法的微分方程计算与连续系统离散化(附Python实现)

0 专栏介绍

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

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

1 连续系统离散化

定义在时域的连续时间系统方程为

$$\{\begin{array}{l}\dot{\mathbf{x}}(t)=\mathbf{A}\mathbf{x}(t)+\mathbf{B}\mathbf{u}(t)\\ \mathbf{y}(t)=\mathbf{C}\mathbf{x}(t)+\mathbf{D}\mathbf{u}(t)\end{array}$$

经过$z$变换后可得$z$ 域系统方程及传递函数：
$$\{\begin{array}{l}z\mathbf{X}(z)={\mathbf{A}}_d\mathbf{X}(z)+{\mathbf{B}}_d\mathbf{U}(z)\\ \mathbf{Y}(z)={\mathbf{C}}_d\mathbf{X}(z)+{\mathbf{D}}_d\mathbf{U}(z)\end{array}⟹G(z)={\mathbf{C}}_d(z\mathbf{I}-{\mathbf{A}}_d{)}^{-1}{\mathbf{B}}_d+{\mathbf{D}}_d$$

连续系统离散化的核心在于用差分方法估计连续系统的微分量 $\dot{\mathbf{x}}(t)$，并建立离散状态矩阵与连续矩阵的关系。不妨将对微分$\dot{\mathbf{x}}(t)$的估计问题转变为对积分

$$\mathbf{x}(t)=\int \dot{\mathbf{x}}(t)$$

的估计问题，$\mathbf{x}(t)$的物理意义是从初始时刻到$t$时刻$\dot{\mathbf{x}}(t)$的积分值——$\dot{\mathbf{x}}(t)$曲线下方的面积。接下来，基于上述概念推导常用的连续系统离散化方法。

2 欧拉法

如图所示，采用左端点矩形近似$\mathbf{x}(t)$，则$\mathbf{x}(kT+T)=\mathbf{x}(kT)+\dot{\mathbf{x}}(kT)\cdot T$，对方程两边应用$z$变换和Laplace变换有：

$$z\mathbf{X}(z)=\mathbf{X}(z)+sT\mathbf{X}(s)$$

令$X(s)=X(z)$（即频域映射关系），即得欧拉法的映射：

$$s=\frac{z-1}{T}$$

将该式代入连续系统方程，使连续系统映射到离散系统：

$${\mathbf{G}}_d(z)=\mathbf{C}{\left(\frac{z-1}{T}\mathbf{I}-\mathbf{A}\right)}^{-1}\mathbf{B}+\mathbf{D}\Rightarrow {\mathbf{G}}_d(z)=\mathbf{C}(z\mathbf{I}-(\mathbf{I}+T\mathbf{A}){)}^{-1}\mathbf{B}T+\mathbf{D}$$

对比离散系统标准形式，即得欧拉法离散化公式：

$${\mathbf{A}}_d=\mathbf{I}+T\mathbf{A},{\mathbf{B}}_d=BT,{\mathbf{C}}_d=\mathbf{C},{\mathbf{D}}_d=\mathbf{D}$$

以及离散化差分方程：

$$\begin{array}{c}\mathbf{x}(k+1)=(\mathbf{I}+T\mathbf{A})\mathbf{x}(k)+BT\mathbf{u}(k)\\ \mathbf{y}(k)=\mathbf{C}\mathbf{x}(k)+D\mathbf{u}(k)\end{array}$$

根据泰勒展开：

$$\mathbf{x}(t+T)=\mathbf{x}(t)+T\dot{\mathbf{x}}(t)+\frac{1}{2}{T}^2\ddot{\mathbf{x}}(t)+\cdots$$

可知，欧拉法近似误差为采样周期的一阶精度：

$$\frac{\mathbf{x}(t+T)-\mathbf{x}(t)}{T}-\dot{\mathbf{x}}(t)\approx \frac{1}{2}T\ddot{\mathbf{x}}(t)=O(T)$$

3 双线性变换

如图所示, 采用梯形近似$\mathbf{x}(t)$, 则

$$\mathbf{x}(kT+T)=\mathbf{x}(kT)+\frac{T}{2}(\hat{\mathbf{x}}(kT)+\mathbf{x}(kT+T))$$

对方程两边应用$z$变换和Laplace 变换有

$$2z\mathbf{X}(z)=2\mathbf{X}(z)+T(s\mathbf{X}(s)+sz\mathbf{X}(s))$$

令$\mathbf{X}(s)=\mathbf{X}(z)$即得双线性变换映射

$$s=\frac{2(z-1)}{T(z+1)}$$

将该式代入 (8.2) 使连续系统映射到离散系统

$${\mathbf{G}}_d(z)=\mathbf{C}{\left(z\mathbf{I}-{\left(\mathbf{I}-\frac{1}{2}T\mathbf{A}\right)}^{-1}\left(\mathbf{I}+\frac{1}{2}T\mathbf{A}\right)\right)}^{-1}\frac{1}{2}{\left(\mathbf{I}-\frac{1}{2}T\mathbf{A}\right)}^{-1}\mathbf{B}T(z+1)+\mathbf{D}$$

进一步得到

$$\begin{gathered} A_d={\left(I-\frac{1}{2}TA\right)}^{-1}\left(I+\frac{1}{2}TA\right) \\ {B}_d=\frac{1}{2}{\left(I-\frac{1}{2}TA\right)}^{-1}BT \\ {C}_d=C\left(I+{\left(I-\frac{T}{2}A\right)}^{-1}\left(I+\frac{T}{2}A\right)\right) \\ {D}_d=D+\frac{CBT}{2}{\left(I-\frac{T}{2}A\right)}^{-1} \end{gathered}$$

和离散化差分方程组

$$\{\begin{array}{l}x(k+1)={\left(I-\frac{T}{2}A\right)}^{-1}\left(I+\frac{T}{2}A\right)x(k)+\frac{BT}{2}{\left(I-\frac{T}{2}A\right)}^{-1}u(k)\\ y(k)=C\left(I+{\left(I-\frac{T}{2}A\right)}^{-1}\left(I+\frac{T}{2}A\right)\right)x(k)+\left(D+\frac{CBT}{2}{\left(I-\frac{T}{2}A\right)}^{-1}\right)u(k)\end{array}$$

基于泰勒展开的公式：

$$\frac{\mathbf{x}(t+T)-\mathbf{x}(t)}{T}=\dot{\mathbf{x}}(t)+\frac{1}{2}T\ddot{\mathbf{x}}(t)+\frac{1}{6}{T}^2\ddot{\mathbf{x}}(t)+\cdots$$

对比双线性变换公式：

$$\begin{array}{rl}\frac{\mathbf{x}(t+T)-\mathbf{x}(t)}{T}& =\frac{1}{2}(\dot{\mathbf{x}}(t)+\dot{\mathbf{x}}(t+T))\\ & =\frac{1}{2}\dot{\mathbf{x}}(t)+\frac{1}{2}\left(\dot{\mathbf{x}}(t)+T\ddot{\mathbf{x}}(t)+\frac{1}{2}{T}^2\ddot{\mathbf{x}}(t)+\cdots \right)\\ & =\dot{\mathbf{x}}(t)+\frac{1}{2}T\ddot{\mathbf{x}}(t)+\frac{1}{4}{T}^2\ddot{\mathbf{x}}(t)+\cdots \end{array}$$

可得双线性变换近似误差为采样周期的二阶精度$O(T^2)$。

4 龙格库塔法

由拉格朗日中值定理，若函数 $f(x)$ 满足在闭区间$[a,b]$上连续，在开区间 $(a,b)$ 上可导，则在 $(a,b)$上至少存在一点 $\xi$ 使等式 $f(b)-f(a)=f^{\prime }(\xi )(b-a)$ 成立。

因此，$\mathbf{x}(kT+T)$ 的准确解为 $\mathbf{x}(kT+T)=\mathbf{x}(kT)+\dot{\mathbf{x}}(\xi )T$。前向差分法和后向差分法相当于分别用区间端点的斜率近似，即 $\dot{\mathbf{x}}(\xi )=\dot{\mathbf{x}}(kT)$ 与 $\dot{\mathbf{x}}(\xi )=\dot{\mathbf{x}}(kT+T)$，显然不够准确。基于此，龙格库塔法的核心思想是通过区间多点采样逼近 $\dot{\mathbf{x}}(\xi )$ 实现更精确的离散化估计，其中采样点数定义为龙格库塔法的阶数。欧拉法可视为一阶龙格库塔法。

一般地，令 $\dot{\mathbf{x}}(t)=f(\mathbf{x},t)$，$\mathbf{x}(kT)={\mathbf{x}}_k$， $kT=t_k$ ，则泰勒展开

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+T{\dot{\mathbf{x}}}_k+\frac{1}{2}{T}^2{\ddot{\mathbf{x}}}_k+O(T^3)$$

其中 $\ddot{\mathbf{x}}(t)=\mathrm{d}f(\mathbf{x},t)/\mathrm{d}t=f_t(\mathbf{x},t)+f(\mathbf{x},t)\cdot f_{\mathbf{x}}(\mathbf{x},t)\Rightarrow {\ddot{\mathbf{x}}}_k=f_t({\mathbf{x}}_k,t_k)+f({\mathbf{x}}_k,t_k)\cdot f_{\mathbf{x}}({\mathbf{x}}_k,t_k)$。代入上式可得

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+Tf+\frac{1}{2}{T}^2(f_t+f{f}_{\mathbf{x}})+O(T^3)$$

二阶龙格库塔法在区间$[t_k,t_{k+1}]$上采样两个点，分别记为

$$\{\begin{array}{l}{k}_1=f(x_k,t_k)\\ k_2=f(x_k+c_1{k}_{1}T,t_k+c_{2}T)\end{array}⟹x_{k+1}=x_k+({\lambda }_1{k}_1+{\lambda }_2{k}_2)T$$

其中${\lambda }_i$与$c_i$均为待定系数。对$k_2$一阶泰勒展开有：

$$k_2=f(x_k+c_1{k}_{1}T,t_k+c_{2}T)=f+c_{2}T{f}_t+c_{1}T{f}_x+\mathrm{O}(T^3)$$

将$k_1$和$k_2$代入$x_{k+1}$可得：

$$x_{k+1}=x_k+({\lambda }_1+{\lambda }_2)Tf+\frac{T^2}{2}\left(2{\lambda }_2{c}_2{f}_t+2c_1{\lambda }_2{f}_{xx}\right)+\mathrm{O}(T^3)$$

对比系数可知：

$$\{\begin{array}{l}{\lambda }_1+{\lambda }_2=1\\ {\lambda }_2{c}_1={\lambda }_2{c}_2=\frac{1}{2}\end{array}$$

这个不定方程的一组可行解为${\lambda }_1={\lambda }_2=1/2，c_1=c_2=1$，所以二阶龙格库塔法表述为：

$$x_{k+1}=x_k+\left(\frac{1}{2}{k}_1+\frac{1}{2}{k}_2\right)T,\{\begin{array}{l}{k}_1=f(x_k,t_k)\\ k_2=f(x_k+T,t_k+1)\end{array}$$

二阶龙格库塔法也称为改进欧拉法。依次类推，可以得到高阶龙格库塔法公式。四阶龙格库塔法达到了精度和效率的最佳平衡，因此工程中应用最广泛。接下来推导基于状态方程的四阶龙格库塔法积分。假定状态方程时不变，则

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\frac{T}{6}\left({\mathbf{k}}_1+2{\mathbf{k}}_2+2{\mathbf{k}}_3+{\mathbf{k}}_4\right),\{\begin{array}{l}{\mathbf{k}}_1=\mathbf{f}\left({\mathbf{x}}_k,{\mathbf{u}}_k\right)\\ {\mathbf{k}}_2=f\left({\mathbf{x}}_k+\frac{{\mathbf{k}}_1}{2}T,{\mathbf{u}}_k\right)\\ {\mathbf{k}}_3=f\left({\mathbf{x}}_k+\frac{{\mathbf{k}}_2}{2}T,{\mathbf{u}}_k\right)\\ {\mathbf{k}}_4=f\left({\mathbf{x}}_k+{\mathbf{k}}_{3}T,{\mathbf{u}}_k\right)\end{array}$$

5 Python实验

定义微分方程

$$y^{\prime }=f(x,y)=-\frac{0.9y}{1+2x}$$

其解析解为

$$y_{\text{exact}}(x)=(1+2x{)}^{-0.45}$$

• 欧拉法实现

  def euler(f, x0, y0, h, n):
      x = np.zeros(n + 1)
      y = np.zeros(n + 1)
      x[0], y[0] = x0, y0
      for i in range(n):
          y[i + 1] = y[i] + h * f(x[i], y[i])
          x[i + 1] = x[i] + h
      return x, y
  

• 改进欧拉法实现

  def rk2(f, x0, y0, h, n):
      x = np.zeros(n + 1)
      y = np.zeros(n + 1)
      x[0], y[0] = x0, y0
      for i in range(n):
          k1 = f(x[i], y[i])
          k2 = f(x[i] + h, y[i] + h * k1)
          y[i + 1] = y[i] + h * (k1 + k2) / 2
          x[i + 1] = x[i] + h
      return x, y
  

• 三阶龙格库塔法

  def rk3(f, x0, y0, h, n):
      x = np.zeros(n + 1)
      y = np.zeros(n + 1)
      x[0], y[0] = x0, y0
      for i in range(n):
          k1 = f(x[i], y[i])
          k2 = f(x[i] + h / 2, y[i] + h * k1 / 2)
          k3 = f(x[i] + h, y[i]  - h * (k1 + 2 * k2))
          y[i + 1] = y[i] + h * (k1 + 4 * k2 + k3) / 6
          x[i + 1] = x[i] + h
      return x, y
  

• 四阶龙格库塔法

  def rk4(f, x0, y0, h, n):
      x = np.zeros(n + 1)
      y = np.zeros(n + 1)
      x[0], y[0] = x0, y0
      for i in range(n):
          k1 = f(x[i], y[i])
          k2 = f(x[i] + h / 2, y[i] + h * k1 / 2)
          k3 = f(x[i] + h / 2, y[i] + h * k2 / 2)
          k4 = f(x[i] + h, y[i] + h * k3)
          y[i + 1] = y[i] + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6
          x[i + 1] = x[i] + h
      return x, y
  

完整工程代码请联系下方博主名片获取

🔥 更多精彩专栏：

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