离散化运动学递推与轨迹积分理论

一、连续模型的离散化需求

1.1 数值计算的动机

连续时间状态方程：

$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$$

在计算机中实现时需要离散化：

• 数字控制器（采样周期 $T_s$）
• 轨迹规划算法（离散时间点）
• 仿真与预测（步进递推）

1.2 离散化方法分类

方法	精度	计算复杂度	适用场景
欧拉法	一阶	低	快速原型
改进欧拉法	二阶	中	实时控制
龙格 - 库塔法	四阶	高	高精度仿真
精确积分	解析	中	恒定输入

二、欧拉离散化

2.1 前向欧拉法

最简单的离散化方法：

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+T_{s}f({\mathbf{x}}_k,{\mathbf{u}}_k)$$

其中 $T_s$ 为采样周期。

2.2 单轨模型的欧拉递推

对于单轨模型：

$$\begin{array}{rl}{x}_{k+1}& =x_k+T_s{v}_k\cos {\psi }_k\\ y_{k+1}& =y_k+T_s{v}_k\sin {\psi }_k\\ {\psi }_{k+1}& ={\psi }_k+T_s\frac{v_k}{L}\tan {\delta }_k\end{array}$$

2.3 截断误差

欧拉法的局部截断误差：

$$e_k=\mathbf{x}(t_{k+1})-{\mathbf{x}}_{k+1}=O(T_s^2)$$

全局累积误差：

$$E_N=\sum _{k=0}^{N-1}{e}_k=O(T_s)$$

误差阶数

欧拉法是一阶方法，误差与步长成正比。

三、改进欧拉法（Heun 法）

3.1 预测 - 校正格式

预测步：

$${\tilde{\mathbf{x}}}_{k+1}={\mathbf{x}}_k+T_{s}f({\mathbf{x}}_k,{\mathbf{u}}_k)$$

校正步：

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\frac{T_s}{2}\left[f({\mathbf{x}}_k,{\mathbf{u}}_k)+f({\tilde{\mathbf{x}}}_{k+1},{\mathbf{u}}_{k+1})\right]$$

3.2 单轨模型的改进欧拉递推

预测：

$$\begin{array}{rl}{\tilde{x}}_{k+1}& =x_k+T_s{v}_k\cos {\psi }_k\\ {\tilde{y}}_{k+1}& =y_k+T_s{v}_k\sin {\psi }_k\\ {\tilde{\psi }}_{k+1}& ={\psi }_k+T_s\frac{v_k}{L}\tan {\delta }_k\end{array}$$

校正：

$$\begin{array}{rl}{x}_{k+1}& =x_k+\frac{T_s}{2}{v}_k(\cos {\psi }_k+\cos {\tilde{\psi }}_{k+1})\\ y_{k+1}& =y_k+\frac{T_s}{2}{v}_k(\sin {\psi }_k+\sin {\tilde{\psi }}_{k+1})\\ {\psi }_{k+1}& ={\psi }_k+\frac{T_s}{2}\frac{v_k}{L}(\tan {\delta }_k+\tan {\tilde{\delta }}_{k+1})\end{array}$$

3.3 精度

改进欧拉法为二阶方法：

• 局部误差：$O(T_s^2)$
• 全局误差：$O(T_s)$

四、四阶龙格 - 库塔法（RK4）

4.1 标准 RK4 公式

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\frac{T_s}{6}(k_1+2k_2+2k_3+k_4)$$

其中：

$$\begin{array}{rl}{k}_1& =f({\mathbf{x}}_k,{\mathbf{u}}_k)\\ k_2& =f({\mathbf{x}}_k+\frac{T_s}{2}{k}_1,{\mathbf{u}}_{k+1/2})\\ k_3& =f({\mathbf{x}}_k+\frac{T_s}{2}{k}_2,{\mathbf{u}}_{k+1/2})\\ k_4& =f({\mathbf{x}}_k+T_s{k}_3,{\mathbf{u}}_{k+1})\end{array}$$

4.2 单轨模型的 RK4 实现

对于每个中间步骤，计算：

$$\begin{array}{rl}{k}_{1,x}& =v_k\cos {\psi }_k\\ k_{1,y}& =v_k\sin {\psi }_k\\ k_{1,\psi }& =\frac{v_k}{L}\tan {\delta }_k\end{array}$$

类似计算 $k_2,k_3,k_4$，然后加权平均。

4.3 精度

RK4 为四阶方法：

• 局部误差：$O(T_s^4)$
• 全局误差：$O(T_s^3)$

五、恒定输入下的精确积分

5.1 解析解

当 $v$ 和 $\delta$ 在 $[t_k,t_{k+1}]$ 内恒定时，存在解析解。

定义角速度：

$$\omega =\frac{v}{L}\tan \delta$$

5.2 直线运动（$\delta =0$）

当 $\delta =0$ 时，$\omega =0$：

$$\begin{array}{rl}{x}_{k+1}& =x_k+v{T}_s\cos {\psi }_k\\ y_{k+1}& =y_k+v{T}_s\sin {\psi }_k\\ {\psi }_{k+1}& ={\psi }_k\end{array}$$

5.3 圆弧运动（$\delta \ne 0$）

当 $\delta \ne 0$ 时：

$$\begin{array}{rl}{x}_{k+1}& =x_k+\frac{v}{\omega }[\sin ({\psi }_k+\omega T_s)-\sin {\psi }_k]\\ y_{k+1}& =y_k-\frac{v}{\omega }[\cos ({\psi }_k+\omega T_s)-\cos {\psi }_k]\\ {\psi }_{k+1}& ={\psi }_k+\omega T_s\end{array}$$

5.4 数值稳定性

精确积分方法：

• 无截断误差
• 保持几何约束（圆弧轨迹）
• 推荐使用于路径规划

六、轨迹积分的应用

6.1 前向仿真

给定初始状态和输入序列，计算轨迹：

for k = 0 to N-1:
    x[k+1] = integrate(x[k], u[k], T_s)

6.2 轨迹预测

在模型预测控制（MPC）中，预测未来 $N$ 步轨迹：

$$\{{\mathbf{x}}_1,{\mathbf{x}}_2,\dots ,{\mathbf{x}}_N\}=\text{propagate}({\mathbf{x}}_0,\{{\mathbf{u}}_0,\dots ,{\mathbf{u}}_{N-1}\})$$

6.3 路径规划

逆向积分生成可行路径：

$${\mathbf{x}}_k=\text{backward\_integrate}({\mathbf{x}}_{k+1},{\mathbf{u}}_k)$$

七、离散化的稳定性分析

7.1 数值稳定性

不同积分方法的稳定性区域：

方法	稳定性区域
欧拉法	小（$
RK4	大
精确积分	无条件稳定

7.2 步长选择原则

步长 $T_s$ 的选择需满足：

• 数值稳定性
• 精度要求
• 实时性约束

经验法则：

$$T_s<\frac{L}{10v_{\max }\tan {\delta }_{\max }}$$

八、相关内容

• 01-连续时间位姿状态空间的严格构建
• 03-运动学模型适用边界与理论局限
• 01-平面旋转矩阵与方向余弦的数学构建