3. 数学原理
本文推导 Autonomy 仿真涉及的运动学模型与数值积分公式。
nav_test实现见 06_nav_test.md；车辆限幅见 08_vehicle_stage.md。
3.1 差速驱动运动学
Step 1：本体速度
差速机器人在本体坐标系下 \(v_y^{body} = 0\)。控制输入 \(u = (v, \omega)^\top\)，其中 \(v\) 为线速度，\(\omega\) 为角速度。
Step 2：世界坐标系速度
\[\begin{split}
\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix}
=
\begin{bmatrix} \cos\theta \\ \sin\theta \end{bmatrix} v
\end{split}\]
\[
\dot{\theta} = \omega
\]
Step 3：连续时间状态方程
\[\begin{split}
\frac{d}{dt} \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}
=
\begin{bmatrix} \cos\theta \\ \sin\theta \\ 0 \end{bmatrix} v
+
\begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \omega
\end{split}\]
Step 5：四元数姿态
航向 \(\theta\) 转四元数（绕 \(z\) 轴）：
\[
q_z = \sin(\theta/2), \quad q_w = \cos(\theta/2)
\]
3.2 轮速逆运动学（差速）
轮距 \(L\)，左右轮半径 \(r\)，角速度 \(\omega_L, \omega_R\)：
\[
v = \frac{r(\omega_R + \omega_L)}{2}, \quad
\omega = \frac{r(\omega_R - \omega_L)}{L}
\]
正运动学（轮速指令）：
\[
\omega_R = \frac{v + \omega L/2}{r}, \quad
\omega_L = \frac{v - \omega L/2}{r}
\]
Gazebo 差速插件内部执行此映射。
3.3 全向驱动（Holonomic）
\[\begin{split}
\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix}
= R(\theta) \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}
\end{split}\]
\(R(\theta)\) 为 2D 旋转矩阵。KinematicsControl 对 \(v_x, v_y, \omega\) 分别限幅。
3.4 Ackermann 运动学
轴距 \(L\)，前轮转角 \(\delta\)：
\[
\omega = \frac{v \tan\delta}{L}, \quad
R_{min} = \frac{L}{\tan\delta_{max}}
\]
最小转弯半径约束：\(|\omega| \leq |v| / R_{min}\)。
3.5 里程计模型
离散时刻 \(k\)，带噪声的里程计观测：
\[\begin{split}
\mathbf{z}_k = \begin{bmatrix} x_k \\ y_k \\ \theta_k \end{bmatrix} + \mathbf{n}_k, \quad
\mathbf{n}_k \sim \mathcal{N}(0, \Sigma_{odom})
\end{split}\]
nav_test 为理想仿真（无噪声）；扩展时可加高斯噪声。
3.6 速度限幅（KinematicsControl）
给定模型上限 \(v_{max}, \omega_{max}, a_{max}\)：
\[
v^{cmd} = \operatorname{clip}(v^{raw}, -v_{rev}, v_{max})
\]
\[
\omega^{cmd} = \operatorname{clip}(\omega^{raw}, -\omega_{max}, \omega_{max})
\]
加速度限幅（离散）：
\[
v_k = \operatorname{clip}\big(v_{k-1} + a_{max}\Delta t, \; v_{min}, v_{max}\big)
\]
3.7 仿真时间
仿真时钟（use_sim_time:=true）：
\[
t_{sim} = t_0 + \sum_k \Delta t_{sim}
\]
与墙钟解耦，由 Gazebo /clock 或 Autolink MODE_SIMULATION 驱动。
3.8 符号表
符号 |
含义 |
|---|---|
\(v, \omega\) |
线速度、角速度 |
\(\Delta t\) |
仿真步长 |
\(L\) |
轮距 / 轴距 |
\(\theta\) |
航向角 |
\(R_{min}\) |
最小转弯半径 |