【自动驾驶/机器人控制】之坐标系&运动学仿真代码工程分析（一）

来源：motion.cpp，仿真刚体圆周运动，模拟 IMU 姿态积分过程。

整体逻辑：每一步仿真周期 dt 内，先做位置更新，再做姿态旋转更新，最后推送数据给 Pangolin 可视化 。

注意执行顺序：先更新位置，后更新姿态，这个顺序是本仿真的设定。

完整代码块拆解

while (ui.ShouldQuit() == false) {      // 循环直到用户关闭窗口
    // ---------- 1. 更新位置 ----------
    Vec3d v_world = pose.so3() * v_body; // 将 body 系速度变换到世界 (world) 系: v_w = R_wb * v_b
    pose.translation() += v_world * dt;  // 位置积分: t = t + v_w * Δt

    // ---------- 2. 更新旋转（两种方式） ----------
    if (FLAGS_use_quaternion) {
        // 方式一：四元数更新（一阶近似）
        // 四元数微分方程: q(t+Δt) ≈ q(t) ⊗ [1, 0.5ωΔt]
        // 其中 ω = [ωx, ωy, ωz] 是角速度矢量
        Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
        q.normalize();                     // 四元数需要归一化保持单位长度
        pose.so3() = SO3(q);               // 更新旋转部分
    } else {
        // 方式二：SO3 指数映射（李代数 → 李群）
        // 旋转矩阵的指数更新: R(t+Δt) = R(t) * exp(ωΔt)^∧
        // 其中 exp(ωΔt)^∧ 是李代数 so3 到李群 SO3 的指数映射
        pose.so3() = pose.so3() * SO3::exp(omega * dt);
    }

    // 将当前位姿打印到终端（便于调试观察）
    LOG(INFO) << "pose: " << pose.translation().transpose();
    // 将导航状态（时间戳、位姿、速度）发送给 UI 线程显示
    ui.UpdateNavState(sad::NavStated(0, pose, v_world));
    // 睡眠 0.05 秒，控制循环频率为 20 Hz
    usleep(dt * 1e6);
}

---

1、位置更新部分

Vec3d v_world = pose.so3() * v_body; // v_w = R_wb * v_b
pose.translation() += v_world * dt;

算法：向量坐标变换 + 欧拉向前积分

1. pose.so3() 返回 RwbR_{wb}Rwb：车体→世界的旋转矩阵

   • vbv_bvb：车体坐标系恒定速度，本仿真固定向前 (v,0,0)；

   • vw=Rwbvbv_w = R_{wb} v_bvw=Rwbvb：把车体速度转换到世界坐标系。

2. 欧拉积分更新世界位置：

   pk+1=pk+vw⋅Δt\boldsymbol p_{k+1}= \boldsymbol p_k + \boldsymbol v_w \cdot \Delta tpk+1=pk+vw⋅Δt

关键点：

位置pose.translation()存储在世界坐标系 ，必须使用世界坐标系速度做积分 ，不能直接用v_body。

注意本代码顺序：使用更新前的旧旋转矩阵计算 v_w，更新位置，之后再更新姿态 R。物理含义：这一整个 dt 时间内姿态保持旧值，末尾时刻发生旋转。

---

2、姿态更新：两套并行算法（if‑else 二选一）

ω\omegaω：车体坐标系下 Z 轴角速度 ，相当于 IMU 测量出来的载体角速度。

增量旋转发生在车体坐标系，所以采用右乘增量。

方案 A：四元数一阶泰勒近似 --use_quaternion=true

Quatd q = pose.unit_quaternion() * Quatd(1, 0.5*omega[0]*dt,0.5*omega[1]*dt,0.5*omega[2]*dt);
q.normalize();
pose.so3() = SO3(q);

算法公式：

qk+1≈qk⊗112ωΔtq_{k+1} \approx q_k \otimes \begin{bmatrix}1 \\ \frac12 \boldsymbol\omega \Delta t\end{bmatrix}qk+1≈qk⊗121ωΔt

1. 这是四元数微分方程的一阶泰勒近似 ；只有当 ωΔt\omega\Delta tωΔt（单步旋转角度）很小时误差才小。

2. 0.5系数是四元数微分方程固有系数，不可省略。

3. q.normalize()：只修正四元数模长为 1，不能消除一阶截断带来的角度误差。

4. 问题：大角速度 / 大 dt 时，单步旋转角度大，会出现姿态漂移，轨迹变成螺旋，无法闭合。

方案 B：SO3 李群指数映射（罗德里格斯解析解，默认） --use_quaternion=false

pose.so3() = pose.so3() * SO3::exp(omega * dt);

算法公式：

Rk+1=Rk⋅Exp(ωΔt)R_{k+1}=R_k \cdot Exp(\boldsymbol\omega \Delta t)Rk+1=Rk⋅Exp(ωΔt)

1. ϕ=ωΔt\boldsymbol\phi=\boldsymbol\omega \Delta tϕ=ωΔt：李代数 so (3) 旋转矢量；

2. SO3::exp()：指数映射，内部执行罗德里格斯公式，李代数 so (3) → 李群 SO (3) 旋转矩阵；

3. ✅解析精确解，无论单步旋转角度多大，都没有截断近似误差。圆周轨迹可以完美闭合。

右乘含义：角速度定义在车体坐标系 (b 系) ，新旋转叠加在车体自身，旧姿态右乘增量旋转 。

如果角速度定义在世界坐标系，就要左乘增量。

---

3、后续可视化与时序控制

LOG(INFO) << "pose: " << pose.translation().transpose();
ui.UpdateNavState(sad::NavStated(0, pose, v_world));
usleep(dt * 1e6);

1. LOG(INFO)：终端打印世界坐标系位置，调试用；

2. UpdateNavState：把时间戳、SE3 位姿、世界速度送入 Pangolin UI，绘制 3D 轨迹、右侧时序曲线；

3. usleep(dt*1e6)：dt 单位秒，转为微秒；本项目 dt=0.05s，循环 20Hz 仿真。

---

4、重点：本代码执行顺序带来的细节

顺序：先用旧 R 计算 v_w 更新位置 → 再更新姿态 R

1. 含义：在dt这一段时间间隔内，姿态保持旧姿态不变；时间片结束时刻才完成姿态旋转。

2. IMU 仿真里这是常用离散方式；如果调换顺序，先更新姿态再算速度积分，轨迹会有微小相位偏移。

5、对比总结表

项目	四元数一阶近似	SO3 指数映射（罗德里格斯）
算法	一阶泰勒近似	解析解，无近似
误差来源	单步旋转角度 ωΔt\omega\Delta tωΔt 大时，截断误差明显	不存在截断误差
归一化	必须normalize()维持单位四元数	不需要归一化，输出天然合法旋转矩阵
现象	大角速度轨迹螺旋漂移	任意角速度轨迹完美闭合
工程使用场景	IMU 高频采样，每步角度很小；dt 必须很小	仿真、大角度增量场景通用

6、问题反思

1. 为什么R_wb * v_body？

位置积分在世界坐标系，载体速度必须通过旋转矩阵变换到世界坐标系。

2. 四元数normalize()为什么还会漂移？

normalize 只约束模长，不能修复泰勒一阶截断带来角度本身误差。

3. 角速度在车体坐标系，姿态更新为什么是右乘增量？

增量旋转施加在载体局部坐标系，旧姿态右乘增量旋转。