來源motion.cpp仿真剛體圓周運動模擬 IMU 姿態(tài)積分過程。整體邏輯每一步仿真周期 dt 內(nèi)先做位置更新再做姿態(tài)旋轉(zhuǎn)更新最后推送數(shù)據(jù)給 Pangolin 可視化。注意執(zhí)行順序先更新位置后更新姿態(tài)這個順序是本仿真的設(shè)定。完整代碼塊拆解while(ui.ShouldQuit()false){// 循環(huán)直到用戶關(guān)閉窗口// ---------- 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. 更新旋轉(zhuǎn)兩種方式 ----------if(FLAGS_use_quaternion){// 方式一四元數(shù)更新一階近似// 四元數(shù)微分方程: 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();// 四元數(shù)需要歸一化保持單位長度pose.so3()SO3(q);// 更新旋轉(zhuǎn)部分}else{// 方式二SO3 指數(shù)映射李代數(shù) → 李群// 旋轉(zhuǎn)矩陣的指數(shù)更新: R(tΔt) R(t) * exp(ωΔt)^∧// 其中 exp(ωΔt)^∧ 是李代數(shù) so3 到李群 SO3 的指數(shù)映射pose.so3()pose.so3()*SO3::exp(omega*dt);}// 將當(dāng)前位姿打印到終端便于調(diào)試觀察LOG(INFO)pose: pose.translation().transpose();// 將導(dǎo)航狀態(tài)時間戳、位姿、速度發(fā)送給 UI 線程顯示ui.UpdateNavState(sad::NavStated(0,pose,v_world));// 睡眠 0.05 秒控制循環(huán)頻率為 20 Hzusleep(dt*1e6);}1、位置更新部分Vec3d v_worldpose.so3()*v_body;// v_w R_wb * v_bpose.translation()v_world*dt;算法向量坐標(biāo)變換 歐拉向前積分pose.so3()返回RwbR_{wb}Rwb?車體→世界的旋轉(zhuǎn)矩陣vbv_bvb?車體坐標(biāo)系恒定速度本仿真固定向前(v,0,0)vwRwbvbv_w R_{wb} v_bvw?Rwb?vb?把車體速度轉(zhuǎn)換到世界坐標(biāo)系。歐拉積分更新世界位置pk1pkvw?Δt\boldsymbol p_{k1} \boldsymbol p_k \boldsymbol v_w \cdot \Delta tpk1?pk?vw??Δt關(guān)鍵點位置pose.translation()存儲在世界坐標(biāo)系必須使用世界坐標(biāo)系速度做積分不能直接用v_body。注意本代碼順序使用更新前的舊旋轉(zhuǎn)矩陣計算 v_w更新位置之后再更新姿態(tài) R。物理含義這一整個 dt 時間內(nèi)姿態(tài)保持舊值末尾時刻發(fā)生旋轉(zhuǎn)。2、姿態(tài)更新兩套并行算法if?else 二選一ω\omegaω車體坐標(biāo)系下 Z 軸角速度相當(dāng)于 IMU 測量出來的載體角速度。增量旋轉(zhuǎn)發(fā)生在車體坐標(biāo)系所以采用右乘增量。方案 A四元數(shù)一階泰勒近似--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?]這是四元數(shù)微分方程的一階泰勒近似只有當(dāng)ωΔt\omega\Delta tωΔt單步旋轉(zhuǎn)角度很小時誤差才小。0.5系數(shù)是四元數(shù)微分方程固有系數(shù)不可省略。q.normalize()只修正四元數(shù)模長為 1不能消除一階截斷帶來的角度誤差。問題大角速度 / 大 dt 時單步旋轉(zhuǎn)角度大會出現(xiàn)姿態(tài)漂移軌跡變成螺旋無法閉合。方案 BSO3 李群指數(shù)映射羅德里格斯解析解默認--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李代數(shù) so (3) 旋轉(zhuǎn)矢量SO3::exp()指數(shù)映射內(nèi)部執(zhí)行羅德里格斯公式李代數(shù) so (3) → 李群 SO (3) 旋轉(zhuǎn)矩陣?解析精確解無論單步旋轉(zhuǎn)角度多大都沒有截斷近似誤差。圓周軌跡可以完美閉合。右乘含義角速度定義在車體坐標(biāo)系 (b 系)新旋轉(zhuǎn)疊加在車體自身舊姿態(tài)右乘增量旋轉(zhuǎn)。如果角速度定義在世界坐標(biāo)系就要左乘增量。3、后續(xù)可視化與時序控制LOG(INFO)pose: pose.translation().transpose();ui.UpdateNavState(sad::NavStated(0,pose,v_world));usleep(dt*1e6);LOG(INFO)終端打印世界坐標(biāo)系位置調(diào)試用UpdateNavState把時間戳、SE3 位姿、世界速度送入 Pangolin UI繪制 3D 軌跡、右側(cè)時序曲線usleep(dt*1e6)dt 單位秒轉(zhuǎn)為微秒本項目 dt0.05s循環(huán) 20Hz 仿真。4、重點本代碼執(zhí)行順序帶來的細節(jié)順序先用舊 R 計算 v_w 更新位置 → 再更新姿態(tài) R含義在dt這一段時間間隔內(nèi)姿態(tài)保持舊姿態(tài)不變時間片結(jié)束時刻才完成姿態(tài)旋轉(zhuǎn)。IMU 仿真里這是常用離散方式如果調(diào)換順序先更新姿態(tài)再算速度積分軌跡會有微小相位偏移。5、對比總結(jié)表項目四元數(shù)一階近似SO3 指數(shù)映射羅德里格斯算法一階泰勒近似解析解無近似誤差來源單步旋轉(zhuǎn)角度ωΔt\omega\Delta tωΔt大時截斷誤差明顯不存在截斷誤差歸一化必須normalize()維持單位四元數(shù)不需要歸一化輸出天然合法旋轉(zhuǎn)矩陣現(xiàn)象大角速度軌跡螺旋漂移任意角速度軌跡完美閉合工程使用場景IMU 高頻采樣每步角度很小dt 必須很小仿真、大角度增量場景通用6、問題反思為什么R_wb * v_body位置積分在世界坐標(biāo)系載體速度必須通過旋轉(zhuǎn)矩陣變換到世界坐標(biāo)系。四元數(shù)normalize()為什么還會漂移normalize 只約束模長不能修復(fù)泰勒一階截斷帶來角度本身誤差。角速度在車體坐標(biāo)系姿態(tài)更新為什么是右乘增量增量旋轉(zhuǎn)施加在載體局部坐標(biāo)系舊姿態(tài)右乘增量旋轉(zhuǎn)。