来源motion.cpp仿真刚体圆周运动模拟 IMU 姿态积分过程。整体逻辑每一步仿真周期 dt 内先做位置更新再做姿态旋转更新最后推送数据给 Pangolin 可视化。注意执行顺序先更新位置后更新姿态这个顺序是本仿真的设定。完整代码块拆解while(ui.ShouldQuit()false){// 循环直到用户关闭窗口// ---------- 1. 更新位置 ----------Vec3d v_worldpose.so3()*v_body;// 将 body 系速度变换到世界 (world) 系: v_w R_wb * v_bpose.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 qpose.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 Hzusleep(dt*1e6);}1、位置更新部分Vec3d v_worldpose.so3()*v_body;// v_w R_wb * v_bpose.translation()v_world*dt;算法向量坐标变换 欧拉向前积分pose.so3()返回RwbR_{wb}Rwb​车体→世界的旋转矩阵vbv_bvb​车体坐标系恒定速度本仿真固定向前(v,0,0)vwRwbvbv_w R_{wb} v_bvw​Rwb​vb​把车体速度转换到世界坐标系。欧拉积分更新世界位置pk1pkvw⋅Δt\boldsymbol p_{k1} \boldsymbol p_k \boldsymbol v_w \cdot \Delta tpk1​pk​vw​⋅Δt关键点位置pose.translation()存储在世界坐标系必须使用世界坐标系速度做积分不能直接用v_body。注意本代码顺序使用更新前的旧旋转矩阵计算 v_w更新位置之后再更新姿态 R。物理含义这一整个 dt 时间内姿态保持旧值末尾时刻发生旋转。2、姿态更新两套并行算法if‑else 二选一ω\omegaω车体坐标系下 Z 轴角速度相当于 IMU 测量出来的载体角速度。增量旋转发生在车体坐标系所以采用右乘增量。方案 A四元数一阶泰勒近似--use_quaterniontrueQuatd qpose.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);算法公式qk1≈qk⊗[112ωΔt]q_{k1} \approx q_k \otimes \begin{bmatrix}1 \\ \frac12 \boldsymbol\omega \Delta t\end{bmatrix}qk1​≈qk​⊗[121​ωΔt​]这是四元数微分方程的一阶泰勒近似只有当ωΔt\omega\Delta tωΔt单步旋转角度很小时误差才小。0.5系数是四元数微分方程固有系数不可省略。q.normalize()只修正四元数模长为 1不能消除一阶截断带来的角度误差。问题大角速度 / 大 dt 时单步旋转角度大会出现姿态漂移轨迹变成螺旋无法闭合。方案 BSO3 李群指数映射罗德里格斯解析解默认--use_quaternionfalsepose.so3()pose.so3()*SO3::exp(omega*dt);算法公式Rk1Rk⋅Exp(ωΔt)R_{k1}R_k \cdot Exp(\boldsymbol\omega \Delta t)Rk1​Rk​⋅Exp(ωΔt)ϕωΔt\boldsymbol\phi\boldsymbol\omega \Delta tϕωΔt李代数 so (3) 旋转矢量SO3::exp()指数映射内部执行罗德里格斯公式李代数 so (3) → 李群 SO (3) 旋转矩阵✅解析精确解无论单步旋转角度多大都没有截断近似误差。圆周轨迹可以完美闭合。右乘含义角速度定义在车体坐标系 (b 系)新旋转叠加在车体自身旧姿态右乘增量旋转。如果角速度定义在世界坐标系就要左乘增量。3、后续可视化与时序控制LOG(INFO)pose: pose.translation().transpose();ui.UpdateNavState(sad::NavStated(0,pose,v_world));usleep(dt*1e6);LOG(INFO)终端打印世界坐标系位置调试用UpdateNavState把时间戳、SE3 位姿、世界速度送入 Pangolin UI绘制 3D 轨迹、右侧时序曲线usleep(dt*1e6)dt 单位秒转为微秒本项目 dt0.05s循环 20Hz 仿真。4、重点本代码执行顺序带来的细节顺序先用旧 R 计算 v_w 更新位置 → 再更新姿态 R含义在dt这一段时间间隔内姿态保持旧姿态不变时间片结束时刻才完成姿态旋转。IMU 仿真里这是常用离散方式如果调换顺序先更新姿态再算速度积分轨迹会有微小相位偏移。5、对比总结表项目四元数一阶近似SO3 指数映射罗德里格斯算法一阶泰勒近似解析解无近似误差来源单步旋转角度ωΔt\omega\Delta tωΔt大时截断误差明显不存在截断误差归一化必须normalize()维持单位四元数不需要归一化输出天然合法旋转矩阵现象大角速度轨迹螺旋漂移任意角速度轨迹完美闭合工程使用场景IMU 高频采样每步角度很小dt 必须很小仿真、大角度增量场景通用6、问题反思为什么R_wb * v_body位置积分在世界坐标系载体速度必须通过旋转矩阵变换到世界坐标系。四元数normalize()为什么还会漂移normalize 只约束模长不能修复泰勒一阶截断带来角度本身误差。角速度在车体坐标系姿态更新为什么是右乘增量增量旋转施加在载体局部坐标系旧姿态右乘增量旋转。