最近把S系列雙目相機接到工控機上跑VINS-Mono原本以為半個小時內(nèi)就能看到漂亮的軌跡圖結(jié)果第一版跑起來設備還沒走兩步軌跡直接往天上一拐接著像喝醉了一樣在原地畫圈。反復調(diào)了一周最后發(fā)現(xiàn)問題的根子全在幾個不起眼的配置項里。這篇就來記錄一下這個“軌跡飄飛解決版”的完整過程包括現(xiàn)象分析、標定細節(jié)和可直接復制的配置參數(shù)給同樣用S系列或者其它雙目IMU組合跑VINS-Mono的朋友一個參考。先說結(jié)論軌跡飄飛不是某一個參數(shù)壞了而是標定外參、時間戳、IMU噪聲、圖像曝光等多個因素的疊加。單獨改任何一個參數(shù)都只能壓制一部分癥狀只有把數(shù)據(jù)鏈路從頭捋一遍讓VINS-Mono拿到“干凈”的輸入軌跡才能穩(wěn)下來。下面按我實際踩坑的順序來寫沒有廢話全是操作。1. 先認清軌跡飄飛的現(xiàn)象和根因1.1 飄飛不是一個現(xiàn)象而是好幾種癥狀我一開始以為“軌跡飄飛”就只有一種畫面后來反復測試才發(fā)現(xiàn)它通常以四種形態(tài)出現(xiàn)而且每一種對應的排查方向完全不一樣。第一種是豎直方向直接飛走。軌跡原本應該貼著地面走結(jié)果跑了沒幾秒就往Z軸正方向沖像是坐電梯直接上天。這種問題多半出在重力加速度對齊上也就是IMU加速度計數(shù)據(jù)和相機坐標系之間的旋轉(zhuǎn)關系沒對齊。第二種是原地打轉(zhuǎn)軌跡畫成一團亂麻。設備明明在往前推軌跡卻像在繞圈速度越快圈越小。這一般是IMU角速度和圖像運動之間出現(xiàn)了系統(tǒng)性偏差常見原因是外參里的旋轉(zhuǎn)矩陣方向反了或者角速度噪聲參數(shù)設置得太小讓VINS過信IMU的角速度積分。第三種是水平方向上緩慢漂移。設備走完一圈回到起點軌跡卻沒有閉合誤差隨時間累積越來越明顯。這種通常不是突然爆掉而是慢慢偏常見原因是加速度計噪聲偏大、陀螺儀零偏估計不準或者圖像幀率太低導致視覺約束不足。第四種是運行幾秒后直接跳到另一個位置像瞬移一樣。這種往往是圖像特征點瞬間丟失回環(huán)檢測又錯誤匹配或者是時間戳突然出現(xiàn)跳變導致VINS把兩幀圖像當成了同一時刻的數(shù)據(jù)。1.2 我的最終排查結(jié)論我剛開始總想找一個“核心參數(shù)”覺得改一改就能徹底解決。后來把標定、配置、launch文件全部重新走了一遍才發(fā)現(xiàn)是四個問題疊在一起出廠外參和實際安裝位置存在毫厘級偏差、圖像與IMU時間戳沒有嚴格同步、IMU噪聲參數(shù)沿用默認值、相機自動曝光導致圖像亮度跳變。這四個問題單獨拿出來每一個都會引起小幅漂移放在一起就直接“放飛自我”。所以這個“解決版”要做的不是打某個神秘補丁而是把數(shù)據(jù)鏈路里最容易出錯的環(huán)節(jié)全部校準到位。2. 硬件與驅(qū)動準備先讓數(shù)據(jù)本身是對的2.1 把數(shù)據(jù)頻率和時間戳打到臺面上核實跑VINS-Mono之前我建議先用rostopic把相機和IMU的原始數(shù)據(jù)頻率都過一遍。S系列相機驅(qū)動起來之后先看圖像話題和IMU話題到底叫什么名字、頻率是多少、時間戳是不是以秒為單位連續(xù)遞增。這一步很多人會跳過直接打開launch文件就開跑飄飛之后再來找配置問題其實順序反了。我自己實際用的命令是這樣的rostopic hz /camera/color/image_raw rostopic hz /imu/data_raw rostopic echo /imu/data_raw -n 3rostopic hz能看出頻率是否穩(wěn)定。像IMU話題如果一會兒200Hz一會兒180HzVINS的預積分就會出現(xiàn)額外誤差。rostopic echo則重點看header.stamp和header.seq。如果seq連續(xù)遞增但stamp偶爾回跳或出現(xiàn)零值說明驅(qū)動里時間戳處理有問題需要在驅(qū)動層修復而不是指望VINS-Mono自己扛過去。這里還要注意一個細節(jié)S系列相機的硬件時間戳可能和系統(tǒng)時鐘不是同一個源。如果驅(qū)動設置不當圖像和IMU話題上的時間戳一個是傳感器時鐘另一個是系統(tǒng)接收時鐘兩種時間戳混在一起哪怕頻率完全正常VINS也會認為數(shù)據(jù)時序錯亂。2.2 時間戳偏差到底要控制在多少才算安全VINS-Mono的視覺慣性對齊對時間戳偏差非常敏感。很多資料會告訴你“毫秒級誤差可以接受”我在實際測試里發(fā)現(xiàn)這個說法太寬松了。圖像時間戳和IMU時間戳之間的偏差如果穩(wěn)定在3毫秒以內(nèi)影響還不明顯一旦到了10毫秒量級軌跡就會在轉(zhuǎn)彎時出現(xiàn)肉眼可見的橫向漂移超過20毫秒旋轉(zhuǎn)過程中直接飄飛。我用S系列相機做的一個簡單測試把圖像時間戳人為平移30毫秒短距離直線行走的軌跡誤差從0.5%漲到了4%以上而且在大幅轉(zhuǎn)動時出現(xiàn)Z軸跳變。這說明時間同步問題優(yōu)先度極高必須先處理。驗證時間偏差的方法很簡單在相機前快速晃動設備錄制一段bag然后離線對比圖像曝光中間時刻和IMU數(shù)據(jù)的變化趨勢。也可以用VINS自帶的在線時差估計參數(shù)estimate_td來粗評但后面我會說明這個參數(shù)不能替代硬件層面的同步修正。如果驅(qū)動層面無法直接統(tǒng)一時鐘退一步的辦法是錄bag時用record的--duration配合外部時間服務或者寫一個節(jié)點把IMU時間戳和圖像時間戳按最小二乘擬合對齊。最省事的方式是直接用驅(qū)動提供的傳感器時間戳前提是驅(qū)動文檔里明確說明圖像和IMU共用一個時鐘源。3. 修改VINS-Mono配置文件把每個參數(shù)講明白3.1 內(nèi)參、畸變、外參不能直接抄出廠標定S系列相機出廠時會提供一份標定文件里面包含左右目的相機內(nèi)參、畸變系數(shù)以及IMU到左目相機的外參。很多人直接把這些數(shù)值填進VINS配置里我就是這么做的結(jié)果軌跡飄給我看。原因很簡單出廠標定是在特定溫度、特定應力狀態(tài)下得到的相機模組在裝配和運輸過程中可能產(chǎn)生微小位移。特別是外參里的旋轉(zhuǎn)矩陣哪怕只偏了1度跑幾十米后橫向誤差就非?？捎^。IMU到相機的平移即使偏了1毫米同樣會在轉(zhuǎn)動手腕時被放大成明顯的漂移。因此強烈建議拿到相機后自己重新標定一次。用棋盤格走一遍Kalibr抑或其它標定工具算出來的內(nèi)參、畸變、相機與IMU外參比任何出廠值都可靠。如果你完全沒標定經(jīng)驗也至少要做一個“外參粗檢查”讓相機固定記錄IMU靜止時的重力方向再對比相機觀察到的靜止環(huán)境調(diào)整外參旋轉(zhuǎn)部分使兩者一致。在VINS配置文件中內(nèi)參長這樣cam0: camera_model: pinholedistortion distortion_coeffs: [0.0, 0.0, 0.0, 0.0] intrinsics: [450, 450, 320, 240]這里intrinsics依次是fx, fy, cx, cydistortion_coeffs順序是k1, k2, p1, p2。如果用魚眼鏡頭要把模型改成equidistant或?qū)Ｐ头駝t圖像邊緣畸變會直接誤導特征匹配。外參部分放在VINS配置的body_T_cam0字段。注意這個矩陣表示的是IMU本體坐標系相對于相機坐標系的變換還是相機相對于IMU的變換不同版本VINS的約定略有差異一定要確認清楚。如果方向?qū)懛窜壽E會立刻以螺旋形式飄走這是我見過最直接的外參錯誤表現(xiàn)。3.2 IMU噪聲參數(shù)初始值怎么給VINS配置里的acc_n、gyr_n、acc_w、gyr_w這四個參數(shù)決定了VINS對IMU數(shù)據(jù)有多信任。給太大會讓視覺主導軌跡容易抖動給太小則會讓IMU積分主導漂移會迅速累積。用S系列這類消費級IMU時我常用的初始經(jīng)驗值如下表參數(shù)含義經(jīng)驗值acc_n加速度計噪聲密度0.05gyr_n陀螺儀噪聲密度0.005acc_w加速度計隨機游走0.0005gyr_w陀螺儀隨機游走0.000005這四個值的單位是連續(xù)時間密度不是離散采樣的標準差。如果驅(qū)動輸出的IMU數(shù)據(jù)做了濾波平滑噪聲密度會比原始指標更小這時可以適當調(diào)低gyr_n和acc_n如果數(shù)據(jù)里明顯有振動干擾就要調(diào)大一點。VINS在靜止時會自己估計零偏所以噪聲參數(shù)不用特別精確但也不能差一個量級以上。我踩過的一個具體坑是把gyr_n從0.005改成0.0005后軌跡在旋轉(zhuǎn)時表現(xiàn)出“過剛”的傾向明明已經(jīng)停下來軌跡還會維持一小段慣性漂移。后來恢復默認值并配合準確外參情況立刻好轉(zhuǎn)。3.3 estimate_extrinsic設0還是設1VINS的estimate_extrinsic參數(shù)控制是否在線估計相機與IMU外參。設成1表示讓VINS在運行過程中自行優(yōu)化外參設成0則直接讀取配置里的body_T_cam0。很多教程為了省事建議直接把estimate_extrinsic設成1。我的實際經(jīng)驗是在標定準確度未知的情況下設為1確實能讓軌跡不飄得那么夸張但它收斂到的結(jié)果未必是真實外參而且會在某些運動激勵不足的場景下收斂到錯誤局部解。更穩(wěn)的操作是分兩步走先用一段弱光照、多旋轉(zhuǎn)的bag把estimate_extrinsic設為1運行完成后記錄它估計出的外參然后把收斂后的外參寫回配置并把estimate_extrinsic設回0正式跑。為什么最后要設回0因為在線外參估計需要充分的旋轉(zhuǎn)激勵一旦常規(guī)使用中運動類型單一VINS就會反向干擾外參導致結(jié)果反而變差。固定外參至少讓系統(tǒng)少一個自由變量穩(wěn)定度會高很多。這里還要注意外參通常涵蓋旋轉(zhuǎn)和平移兩部分。VINS對旋轉(zhuǎn)的敏感性遠高于平移所以哪怕平移給得粗略一些旋轉(zhuǎn)部分也必須精確。我拿到的出廠標定文件里旋轉(zhuǎn)矩陣看起來是單位陣附近的值實際標定后發(fā)現(xiàn)繞Z軸偏了約1.8度這就是軌跡旋轉(zhuǎn)飄飛的重要原因。3.4 estimate_td到底要不要開estimate_td控制是否在線估計圖像和IMU的時間偏差。如果驅(qū)動已經(jīng)做到5毫秒以內(nèi)的同步我建議設成0避免額外估計造成抖動。如果不太確定時間同步質(zhì)量先設成1讓VINS自己把時間對齊軌跡會有明顯改善。我用S系列實際測試時發(fā)現(xiàn)estimate_td在視覺初始化成功后大概能修正10毫秒量級的偏差但修正過程會伴隨短暫的前幾秒軌跡晃動之后趨于穩(wěn)定。如果你在啟動時看到軌跡剛起步有點扭曲過了兩三秒又好了這個參數(shù)通常在起作用。需要注意estimate_td不能替代驅(qū)動層的時間同步修復。如果時間戳本身就有時而連續(xù)時而跳變的壞點在線估計反而可能被壞點帶偏。所以規(guī)范順序依然是先保證驅(qū)動層時間戳干凈再決定是否開在線估計。4. 實操流程從頭到尾跑通一個解決版4.1 錄制數(shù)據(jù)的三條鐵律跑VINS-Mono解決飄飛不要一上來就拿實時數(shù)據(jù)試后面改參數(shù)會很痛苦。我習慣先錄一段bag在固定環(huán)境里反復測試配置每改一個參數(shù)都回放同一段數(shù)據(jù)對比軌跡。這樣能確?？吹降淖兓敲钜鸬亩皇黔h(huán)境變化造成的。錄制bag的時候有三條鐵律任何一條不滿足都可能為后續(xù)排查埋雷。第一啟動錄制后保持設備完全靜止至少2秒再開始移動。VINS初始化階段需要靜止來估計重力方向和陀螺儀零偏如果一上來就動初始零偏就會帶偏整條軌跡。哪怕后面算法再有回環(huán)修正起始段的誤差也會持續(xù)影響全局。第二關閉相機的自動曝光和自動白平衡。S系列相機的驅(qū)動里如果默認開啟自動曝光移動過程中圖像亮度會不斷變化特征點的像素梯度也跟著變嚴重影響光流跟蹤的穩(wěn)定性。我處理的方式是在驅(qū)動參數(shù)里固定曝光時間和增益并確認室內(nèi)光照恒定。第三錄制環(huán)境要保證有足夠紋理。純白墻、大面積玻璃、空蕩走廊都是VINS的坑。紋理不足時特征點數(shù)量掉到幾十個光流稍微一斷軌跡就開始飄。至少保證畫面里有二十個以上穩(wěn)定角點最好能覆蓋圖像的四個象限。錄制命令可以參考rosbag record /camera/color/image_raw /imu/data_raw -O s_walk.bag4.2 launch文件與topic名替換VINS-Mono自帶的示例launch文件默認訂閱的是相機的仿真話題直接跑肯定報找不到話題。需要新建一個launch文件把圖像話題和IMU話題替換成S系列實際發(fā)布的名稱。我的做法是復制一份示例launch然后只改配置路徑和話題名盡量不動原有結(jié)構。launch文件里大致是這樣launch arg nameconfig_path default$(find vins_estimator)/../config/s_series/s_robust.yaml / arg namevins_driver default / node namevins_estimator pkgvins_estimator typevins_estimator outputscreen param nameconfig_file typestring value$(arg config_path) / param nameoutput_path typestring value$(find vins_estimator)/output_path / /node /launch注意上面的launch文件里沒有直接寫圖像話題名VINS-Mono的圖像和IMU訂閱話題是在配置yaml里通過image_topic和imu_topic指定的。所以更關鍵的修改位置在yamlimage_topic: /camera/color/image_raw imu_topic: /imu/data_raw如果你在S系列驅(qū)動里看到的話題名帶命名空間或者圖像是雙目的兩個話題請把要用的左目話題名填進去。不要想當然地把兩個話題都塞給VINS-Mono它是單目視覺慣性算法只接受一個圖像輸入。4.3 啟動VINS-Mono并保存軌跡改完配置后按這個順序啟動roslaunch vins_estimator s_series.launch roslaunch vins_rviz vins_rviz.launch rosbag play s_walk.bag啟動順序上先把VINS和Rviz起來再播放bag。如果bag播放太快VINS可能出現(xiàn)初始化沒完成就開始運動的情況可以用rosbag play --pause控制播放速度等Rviz里出現(xiàn)了穩(wěn)定的特征點軌跡和重力方向再恢復播放。軌跡最終可以保存下來VINS-Mono提供保存路徑的服務rosservice call /vins_estimator/save_path {}調(diào)用后會在output_path下生成vins_result_path.txt。這個文件保存的是每個時間戳對應的位置和姿態(tài)可以直接用來和真值比較也可以用EVO之類的工具畫誤差曲線。一個經(jīng)驗是保存軌跡時要注意VINS輸出的話題時間戳和bag時間戳是否一致。如果二者有個固定的偏置畫圖之前先把時間對齊否則會認為軌跡嚴重漂移其實是時間柵格錯位。4.4 怎么判斷軌跡算不算正常軌跡飄飛解決沒解決不能只看“沒飛上天”就說成功。我用三個標準來驗收。第一個標準是靜止起點?；胤舃ag的前兩秒VINS剛完成初始化軌跡應該基本停在一個點上抖動幅度小于1厘米。如果靜止時軌跡都在勻速移動說明陀螺儀零偏或者加速度計重力度量有問題。第二個標準是直線段。在桌面上緩慢推動設備走一條直線軌跡應該是一條接近直線的平滑曲線不能出現(xiàn)明顯的波浪狀擺動。波浪狀擺動通常意味著圖像特征點選擇太分散、外參旋轉(zhuǎn)不準或者圖像和IMU時間戳偏差仍然存在。第三個標準是回到原點。走一個四邊形回到起點理想情況下軌跡終點和起點應重合實測中漂移在總路程的1%到2%以內(nèi)就算可以接受。如果超過這個量級先不要改VINS內(nèi)部參數(shù)回去檢查標定和時間戳。我實際跑S系列時把這三條標準全部跑通后軌跡已經(jīng)完全不會“上天”緩慢行走和旋轉(zhuǎn)都能保持穩(wěn)定放在室內(nèi)的鋼筋桌子上反復平移和轉(zhuǎn)動終點誤差控制在0.8%左右。雖然不能和專業(yè)慣導比但作為視覺慣性里程計已經(jīng)進入可用狀態(tài)。5. 常見問題速查表5.1 癥狀、原因和對策對照表整理了一張我在實際運行中遇到的問題表遇到同類現(xiàn)象可以直接按行排查?，F(xiàn)象最可能原因優(yōu)先處理方式軌跡往Z軸方向飛升外參旋轉(zhuǎn)矩陣錯誤或重力對齊異常重新標定外參檢查body_T_cam0方向原地打轉(zhuǎn)畫圈陀螺儀與外參方向不一致時差偏大檢查時間戳偏差修正外參旋轉(zhuǎn)水平方向緩慢漂移IMU噪聲過大或相機幀率低調(diào)低acc_n/gyr_n提升圖像幀率剛起步幾秒后軌跡跳變靜止初始化不充分或自動曝光開啟錄bag前靜止2秒固定曝光參數(shù)轉(zhuǎn)彎后軌跡明顯歪時間戳偏差大于10毫秒驅(qū)動層同步開estimate_td紋理稀少處軌跡亂飛特征點數(shù)量不足換紋理豐富場景或降低特征點閾值回放同一bag軌跡不一樣時間戳回跳或多線程調(diào)度抖動核對驅(qū)動時間戳保證數(shù)據(jù)源穩(wěn)定5.2 我的排查順序如果你不想一張表一張表看我給出一個可以直接照做的排查鏈條按這個順序走能省掉至少一半的時間。第一步看時間戳。把rostopic echo打出來確認時長是否連續(xù)遞增偏差是否在幾毫秒內(nèi)。第二步看外參。用標定工具重新標定或者至少用靜止時的重力和視覺方向粗對齊把外參旋轉(zhuǎn)部分修正到1度以內(nèi)。第三步查噪聲參數(shù)和estimate_extrinsic。按上一節(jié)的經(jīng)驗值填入在線估計外參一次并固化。第四步關自動曝光。錄一段固定曝光、紋理正常的數(shù)據(jù)重復播放觀察軌跡。第五步嚴格靜止初始化。每次回放bag的前幾秒不要立刻播放用--pause控制節(jié)奏。這個順序的核心邏輯是先確保傳感器數(shù)據(jù)本身無歧義再讓VINS去優(yōu)化。如果一上來就調(diào)VINS內(nèi)部參數(shù)很容易陷入“調(diào)好A又帶歪B”的循環(huán)。我親測過先處理時間戳和外參之后有一半的飄飛現(xiàn)象已經(jīng)自然消失。6. 個人心得與幾個實用習慣這套“解決版”最終落地時我對S系列相機的感受是硬件本身不差但默認數(shù)據(jù)和VINS所需的輸入之間還有一段“調(diào)試距離”。廠商給的標定參數(shù)只能作為起始參考真正想在具體場景里跑出穩(wěn)定軌跡標定和時間同步這兩個基本功繞不開。我還養(yǎng)成一個習慣每次修改配置前先在文件里加注釋記錄本次改動和當時的軌跡截圖編號。VINS參數(shù)聯(lián)調(diào)很容易出現(xiàn)“各參數(shù)相互補償”的假象比如外參調(diào)錯時把IMU噪聲調(diào)大視覺主導反而會讓軌跡勉強不發(fā)散但代價是細微抖動和回環(huán)閉合變差。如果沒有記錄過兩天再回看時根本不知道當時為什么這樣改。另一件值得做的事是把最終驗證通過的一套配置單獨存成一個文件夾連同launch文件、bag段和軌跡截圖一起歸檔。以后換一臺同樣的設備可以把這套配置作為起點再針對新設備的標定差異做增量調(diào)整而不是從零開始踩坑。如果你后續(xù)想進一步壓低漂移還可以考慮給S系列加回環(huán)檢測的觸發(fā)條件或者在場景中布設一些標志物輔助視覺特征。不過那已經(jīng)超出“解決飄飛”的范圍了。先把這篇里的數(shù)據(jù)鏈路、標定和時間同步處理好至少九成以上的軌跡飄飛問題都能落地穩(wěn)住。剩下的就是算法參數(shù)精細調(diào)優(yōu)的漫長了。