實(shí)踐)
做機(jī)器人定位與建圖的同學(xué)遲早會遇到一個很詭異的現(xiàn)象你用擴(kuò)展卡爾曼濾波器EKF跑同時定位與地圖構(gòu)建SLAM傳感器噪聲參數(shù)明明設(shè)置得很合理仿真跑了幾圈回頭看NEES指標(biāo)發(fā)現(xiàn)真實(shí)估計(jì)誤差比濾波器預(yù)測的方差大了好幾倍。這個現(xiàn)象叫不一致性本質(zhì)上是EKF在非線性系統(tǒng)中丟失了可觀測性約束。我在處理多個定位項(xiàng)目時被這個問題反復(fù)折磨過今天就從可觀測性的角度把這件事徹底拆開講清楚包含完整的Matlab實(shí)現(xiàn)思路和可以抄作業(yè)的代碼框架。這篇文章適合兩類人看一類是正在用EKF做SLAM、發(fā)現(xiàn)結(jié)果“看著還行但協(xié)方差不靠譜”的研究生或工程師另一類是準(zhǔn)備做狀態(tài)估計(jì)方向算法研究、想理解FEJ和可觀測性約束這類進(jìn)階概念的同學(xué)。我會先講清楚不一致性從哪來再給出一套能在Matlab里復(fù)現(xiàn)問題并驗(yàn)證改進(jìn)方案的完整流程最后把我踩過的坑整理成速查表盡量讓你繞開這些彎路。1. 揭開EKF-SLAM不一致性的面紗1.1 不一致性到底是什么先說結(jié)論一個濾波器被稱為“一致”的指的是它輸出的協(xié)方差矩陣能夠真實(shí)反映估計(jì)誤差的統(tǒng)計(jì)特性。換句話說協(xié)方差說這個誤差是±0.1米那么大量實(shí)驗(yàn)后你的真實(shí)誤差標(biāo)準(zhǔn)偏差就應(yīng)該接近0.1米。如果協(xié)方差說誤差很小實(shí)際誤差卻大得多那就是不一致也叫“過于自信”或“樂觀估計(jì)”。在EKF-SLAM里這個問題表現(xiàn)得非常突出。一個2D平面中帶N個路標(biāo)的SLAM系統(tǒng)狀態(tài)維度是32N機(jī)器人位姿3維每個路標(biāo)位置2維但理論可觀測維度只有2N。也就是說全局位置平移和全局旋轉(zhuǎn)這3個方向本質(zhì)上是不可以被觀測的。無論你裝了多少路標(biāo)、觀測了多少次機(jī)器人的絕對位置都無法被唯一確定因?yàn)檎麄€地圖平移旋轉(zhuǎn)一下觀測值完全不變。標(biāo)準(zhǔn)EKF的問題在于它在每一步都把非線性系統(tǒng)在當(dāng)前估計(jì)點(diǎn)做線性化然后用線性卡爾曼更新的方式處理。線性化點(diǎn)的誤差會破壞雅可比矩陣的結(jié)構(gòu)使得濾波器在那些“理論上不該獲得信息”的全局方向上產(chǎn)生了虛假的信息增益。結(jié)果就是協(xié)方差被持續(xù)壓縮系統(tǒng)對自己的絕對位姿越來越自信但真實(shí)誤差根本沒有變小。1.2 可觀測性約束被破壞的全過程為了把這個過程看得更具體想象你沿著一條環(huán)形路徑走路上不斷看到周圍的幾個路標(biāo)。理想的SLAM應(yīng)該做到你對路標(biāo)之間的相對位置、機(jī)器人與它們的相對幾何關(guān)系越來越確定但你對整個地圖在全局坐標(biāo)系中的絕對位置仍然一無所知。但標(biāo)準(zhǔn)EKF不是這么干的。當(dāng)機(jī)器人發(fā)生旋轉(zhuǎn)時狀態(tài)估計(jì)中的航向角誤差會讓觀測雅可比矩陣發(fā)生扭曲。原本應(yīng)該在可觀測性矩陣零空間里的方向被線性化雅可比矩陣投影到了非零空間。每一次更新后濾波器都“學(xué)到”了一點(diǎn)點(diǎn)關(guān)于絕對位置的信息盡管這些信息完全是虛假的。多次循環(huán)下來協(xié)方差矩陣會變得越來越小但真實(shí)誤差并不會按照這個速度收斂于是NEES指標(biāo)開始飆升。我剛開始跑仿真的時候看到協(xié)方差橢圓縮得很漂亮以為定位精度很高結(jié)果一對比真實(shí)軌跡差點(diǎn)沒嚇壞。協(xié)方差說位置誤差標(biāo)準(zhǔn)偏差只有0.05米實(shí)際卻已經(jīng)飄了0.3米以上。這就是不一致性最直接的工程危害它讓你的濾波器不再可信輸出結(jié)果用于決策比如路徑規(guī)劃、避障時會埋下巨大隱患。2. 從可觀測性理論看EKF-SLAM的深層次問題2.1 非線性系統(tǒng)可觀測性分析基礎(chǔ)談到可觀測性很多同學(xué)第一反應(yīng)是線性系統(tǒng)那套觀測矩陣的秩是否等于狀態(tài)維度能觀測就代表可以通過輸出反推狀態(tài)。但對于SLAM這種非線性系統(tǒng)嚴(yán)格的分析工具是非線性可觀測性理論通常用李導(dǎo)數(shù)構(gòu)建的可觀測性矩陣來判斷。在無噪聲的理想情況下2D SLAM系統(tǒng)的狀態(tài)空間是機(jī)器人位姿(localization)和路標(biāo)位置(mapping)的聯(lián)合。觀測模型是距離與方位它們只依賴機(jī)器人與路標(biāo)之間的相對位置。這意味著全局坐標(biāo)系的三個變換——水平平移、垂直平移、整體旋轉(zhuǎn)——不產(chǎn)生任何觀測差異。從李導(dǎo)數(shù)的角度求出來可觀測性矩陣的秩比狀態(tài)維度少3這3個自由度就是不可觀測方向。理解這一點(diǎn)非常重要SLAM的目標(biāo)從來不是絕對定位而是相對幾何關(guān)系的估計(jì)。機(jī)器人從原點(diǎn)出發(fā)還是從(100,100)出發(fā)對路標(biāo)相對位置、機(jī)器人相對路標(biāo)的估計(jì)結(jié)果完全等價?？捎^測性理論告訴我們這3個自由度根本上無法被任何傳感器觀測消除。2.2 線性化讓不可觀測方向“泄漏”了信息非線性系統(tǒng)的可觀測性分析給的是一個理論基準(zhǔn)理想狀況下哪些方向可觀測、哪些方向不可觀測。但EKF每次更新用的是線性化模型線性化模型的雅可比矩陣是否完美保持這些不可觀測方向就是問題核心了。假設(shè)理論可觀測性分析告訴我們某個方向向量 ( g ) 應(yīng)該在可觀測性矩陣的零空間里。但線性化時用的點(diǎn)是當(dāng)前狀態(tài)估計(jì)值 ( \mu ) 而真值是 ( x ) 二者之間存在誤差。這個誤差會讓雅可比矩陣在 ( g ) 方向上的投影不再是零。用大白話說濾波器本應(yīng)認(rèn)為“在全局平移方向上我一無所知”但由于線性化點(diǎn)偏了它誤以為觀測數(shù)據(jù)能提供這個方向的信息于是開始錯誤地修正這個方向上的狀態(tài)同時壓縮協(xié)方差。這種現(xiàn)象在機(jī)器人轉(zhuǎn)彎時尤其嚴(yán)重。因?yàn)楹较蚪钦`差在旋轉(zhuǎn)時會被放大觀測雅可比中的角度相關(guān)項(xiàng)偏移較大不可觀測方向的泄漏就愈發(fā)明顯。所以一個典型現(xiàn)象是EKF-SLAM跑直線時NEES基本正常一旦開始轉(zhuǎn)彎NEES立刻飆升而且轉(zhuǎn)得越頻繁、越多問題越嚴(yán)重。這也是很多人在做圓形軌跡實(shí)驗(yàn)時發(fā)現(xiàn)一致性差得離譜的原因。更深一層這種泄漏是持續(xù)累積的。每一次更新都往不可觀測方向注入一點(diǎn)虛假信息濾波器協(xié)方差越來越小而估計(jì)誤差中的不可觀測成分卻沒有減少最終表現(xiàn)為嚴(yán)重的過度自信。這也是標(biāo)準(zhǔn)EKF-SLAM在長距離運(yùn)行中表現(xiàn)不佳的關(guān)鍵內(nèi)因。3. Matlab環(huán)境下的標(biāo)準(zhǔn)EKF-SLAM實(shí)現(xiàn)與問題復(fù)現(xiàn)3.1 狀態(tài)向量與系統(tǒng)模型設(shè)計(jì)在研究改進(jìn)方案之前先把標(biāo)準(zhǔn)EKF-SLAM在Matlab里完整實(shí)現(xiàn)一遍把不一致性問題真實(shí)復(fù)現(xiàn)出來。這樣后面做對比時就有明確的基線了。我的仿真場景設(shè)置如下機(jī)器人在一個20m×20m的平面區(qū)域運(yùn)行沿半徑8m的圓形軌跡勻速運(yùn)動線速度 ( v 1.5 m/s )角速度 ( \omega v/R 0.1875 rad/s )。環(huán)境中隨機(jī)分布80個路標(biāo)機(jī)器人搭載一個距離-方位傳感器可以觀測到視場內(nèi)的路標(biāo)距離噪聲標(biāo)準(zhǔn)差0.1m方位噪聲標(biāo)準(zhǔn)差0.01rad。運(yùn)動模型使用速度模型線速度噪聲標(biāo)準(zhǔn)差0.2m/s角速度噪聲標(biāo)準(zhǔn)差0.05rad/s。狀態(tài)向量定義如下這是后續(xù)所有代碼的基礎(chǔ)% 狀態(tài)向量結(jié)構(gòu) % x [x_r; y_r; theta_r; x_l1; y_l1; x_l2; y_l2; ...; x_lN; y_lN] % 索引約定 % 1 : 機(jī)器人 x 坐標(biāo) % 2 : 機(jī)器人 y 坐標(biāo) % 3 : 機(jī)器人航向角 theta % 42k: 第 k 個路標(biāo)的 x 坐標(biāo) % 52k: 第 k 個路標(biāo)的 y 坐標(biāo) % 總維度 D 3 2*N運(yùn)動模型采用經(jīng)典的速度模型機(jī)器人接收到控制輸入 ( u [v, \omega]^T )經(jīng)過時間 ( \Delta t ) 后位姿更新為[ \begin{aligned} x_{t1} x_t v\Delta t \cos\theta_t \ y_{t1} y_t v\Delta t \sin\theta_t \ \theta_{t1} \theta_t \omega\Delta t \end{aligned} ]路標(biāo)位置假設(shè)是靜止的不參與運(yùn)動方程。觀測模型針對每一個被看到的路標(biāo) ( i )[ \begin{aligned} h_1 \sqrt{(x_i - x_r)^2 (y_i - y_r)^2} \ h_2 \text{atan2}(y_i - y_r, x_i - x_r) - \theta_r \end{aligned} ]也就是相對距離和相對方位角。兩個觀測量的噪聲相互獨(dú)立構(gòu)成觀測噪聲矩陣 ( R \text{diag}(0.1^2, 0.01^2) )。3.2 標(biāo)準(zhǔn)EKF的預(yù)測與更新核心代碼下面給出標(biāo)準(zhǔn)EKF-SLAM在Matlab中的核心代碼實(shí)現(xiàn)。這段代碼我經(jīng)過多次仿真驗(yàn)證結(jié)構(gòu)清晰方便你在此基礎(chǔ)上做改進(jìn)。預(yù)測步代碼function [mu_pre, P_pre] ekf_predict(mu, P, v, omega, dt, Q, N) % mu: 狀態(tài)向量 (32N) x 1 % P : 協(xié)方差矩陣 (32N) x (32N) % v, omega: 控制輸入 % Q: 過程噪聲協(xié)方差 (2x2) theta mu(3); D 3 2*N; % 運(yùn)動模型函數(shù) mu_pre mu; mu_pre(1) mu(1) v*dt*cos(theta); mu_pre(2) mu(2) v*dt*sin(theta); mu_pre(3) mu(3) omega*dt; % 狀態(tài)雅可比 F d(f)/d(x) F eye(D); F(1,3) -v*dt*sin(theta); F(2,3) v*dt*cos(theta); % 噪聲雅可比 G d(f)/d(u_noise) G zeros(D, 2); G(1,1) dt*cos(theta); G(2,1) dt*sin(theta); G(3,2) dt; % 預(yù)測協(xié)方差 P_pre F * P * F G * Q * G; end更新步代碼逐個路標(biāo)更新function [mu_est, P_est] ekf_update(mu_pre, P_pre, z, landmark_idx, R_obs, N) % z: 觀測向量 [距離; 方位] % landmark_idx: 當(dāng)前觀測到的路標(biāo)在狀態(tài)向量中的編號 % R_obs: 觀測噪聲協(xié)方差 (2x2) D 3 2*N; theta mu_pre(3); xr mu_pre(1); yr mu_pre(2); % 提取路標(biāo)位置 lk 3 2*(landmark_idx - 1) 1; xl mu_pre(lk); yl mu_pre(lk 1); dx xl - xr; dy yl - yr; q sqrt(dx^2 dy^2); % 預(yù)測觀測值 z_hat [q; atan2(dy, dx) - theta]; % 觀測雅可比 H d(h)/d(x) H zeros(2, D); H(1,1) -dx/q; H(1,2) -dy/q; H(1,lk) dx/q; H(1,lk1) dy/q; H(2,1) dy/q^2; H(2,2) -dx/q^2; H(2,3) -1; H(2,lk) -dy/q^2; H(2,lk1) dx/q^2; % 創(chuàng)新 innovation z - z_hat; innovation(2) wrapToPi(innovation(2)); % 角度歸一化 S H * P_pre * H R_obs; K P_pre * H / S; mu_est mu_pre K * innovation; P_est (eye(D) - K * H) * P_pre; end這里有個需要注意的細(xì)節(jié)角度差一定要用wrapToPi歸一化否則方位角的創(chuàng)新項(xiàng)會在±π附近產(chǎn)生跳變造成錯誤的更新。這是新手最容易忽略的點(diǎn)我當(dāng)年在這個問題上卡了整整一個下午。還有一個關(guān)鍵工程點(diǎn)上面的代碼是逐路標(biāo)更新的即每看到一個路標(biāo)就做一次更新。這樣做實(shí)現(xiàn)簡單但在信息形式下相當(dāng)于對觀測信息做了一次近似分解嚴(yán)格來說不如批量更新精確。批量更新的做法是把所有觀測到的路標(biāo)的雅可比和觀測值都堆疊起來一次更新完成% 批量更新示意 H_all zeros(2*n_obs, D); z_all zeros(2*n_obs, 1); R_all kron(eye(n_obs), R_obs); % ... 堆疊各個路標(biāo)的 H 和 z ... S H_all * P_pre * H_all R_all; K P_pre * H_all / S;批量更新的好處是理論更嚴(yán)格不會因?yàn)轫樞蚨绊懡Y(jié)果在路標(biāo)數(shù)量不多時計(jì)算量也可接受。如果你的仿真場景路標(biāo)數(shù)量比較多可以繼續(xù)用逐路標(biāo)更新但心里要清楚這個近似帶來的細(xì)微差別。3.3 一致性量化指標(biāo)與復(fù)現(xiàn)實(shí)驗(yàn)設(shè)計(jì)復(fù)現(xiàn)問題不能只靠肉眼觀察協(xié)方差橢圓必須用數(shù)學(xué)指標(biāo)量化。最常用的指標(biāo)是NEESNormalized Estimation Error Squared。在時刻t給定真實(shí)位姿、估計(jì)位姿和協(xié)方差NEES定義為[ \epsilon_t (x_t^{\text{true}} - x_t^{\text{est}})^T P_t^{-1} (x_t^{\text{true}} - x_t^{\text{est}}) ]這里的 ( x ) 通常只取機(jī)器人位姿的三維部分 ( (x, y, \theta) )。如果濾波器是一致的那么NEES應(yīng)該服從自由度為3的卡方分布其均值為3。無數(shù)次蒙特卡洛實(shí)驗(yàn)后平均NEES應(yīng)接近3。如果平均NEES顯著大于3說明濾波器不一致而且數(shù)值越大不一致越嚴(yán)重。Matlab中計(jì)算NEESfunction nees compute_nees(x_true, x_est, P_est) % 取三維位姿 d x_est(1:3) - x_true(1:3); d(3) wrapToPi(d(3)); nees d * inv(P_est(1:3,1:3)) * d; end為了得到統(tǒng)計(jì)上可靠的結(jié)論必須做蒙特卡洛仿真。我在實(shí)驗(yàn)中跑了50次獨(dú)立運(yùn)行每次重新生成隨機(jī)噪聲最后將50次NEES求平均得到平均NEES曲線。判斷標(biāo)準(zhǔn)很樸素如果平均NEES穩(wěn)定在3附近濾波器一致如果遠(yuǎn)高于3濾波器不一致如果遠(yuǎn)低于3濾波器雖然“保守”協(xié)方差過大但至少不虛假自信不過工程上過保守也是次優(yōu)的。我在標(biāo)準(zhǔn)EKF仿真中跑出來的結(jié)果是前40秒平均NEES還能維持在3.5附近一旦機(jī)器人繞完第一圈開始持續(xù)重復(fù)觀測路標(biāo)平均NEES直線上升到150秒時已經(jīng)飆到18以上。這說明標(biāo)準(zhǔn)EKF的不一致性在長時間重復(fù)觀測場景下非常嚴(yán)重。你不需要特殊調(diào)試只要跑一個閉合軌跡就能清晰復(fù)現(xiàn)這個現(xiàn)象。4. 基于可觀測性約束的改進(jìn)實(shí)現(xiàn)4.1 第一條思路固定線性化點(diǎn)FEJ理解了問題根源之后改進(jìn)思路就變得清晰了。既然不一致性來自線性化點(diǎn)漂移導(dǎo)致的不可觀測方向泄漏那就設(shè)法讓線性化點(diǎn)保持穩(wěn)定使系統(tǒng)的可觀測性矩陣在時間上一致。FEJFirst-Estimates Jacobian方法的核心做法是對于每一個路標(biāo)在它第一次被觀測到的時候記錄當(dāng)時的機(jī)器人狀態(tài)估計(jì)值。之后在對這個路標(biāo)做觀測更新時無論機(jī)器人當(dāng)前狀態(tài)估計(jì)變成了什么構(gòu)建觀測雅可比矩陣都使用那個“首發(fā)估計(jì)值”而不是當(dāng)前估計(jì)值。為什么這樣有效因?yàn)镾LAM系統(tǒng)中可觀測性矩陣的列空間變化主要來源于每個路標(biāo)被重復(fù)觀測時線性化點(diǎn)的不同。如果每次都用第一次觀測時的狀態(tài)來線性化那么濾波器對某個路標(biāo)的觀測雅可比就從一而終不會再產(chǎn)生虛假信息注入。從數(shù)學(xué)上講這保證了系統(tǒng)線性化模型的可觀測性屬性始終與非線性系統(tǒng)理論保持一致。在Matlab中實(shí)現(xiàn)FEJ需要額外維護(hù)一個“首發(fā)估計(jì)”字典% 初始化 first_estimates struct(xr, [], yr, [], theta, [], initialized, false); % 更新時 % 檢查路標(biāo)是否首次被觀測 if ~first_estimates(landmark_idx).initialized % 首次觀測用當(dāng)前估計(jì)作為首發(fā)估計(jì) first_estimates(landmark_idx).xr mu_pre(1); first_estimates(landmark_idx).yr mu_pre(2); first_estimates(landmark_idx).theta mu_pre(3); first_estimates(landmark_idx).initialized true; end % 構(gòu)建觀測雅可比時用首發(fā)估計(jì)而不是當(dāng)前估計(jì) xr_lin first_estimates(landmark_idx).xr; yr_lin first_estimates(landmark_idx).yr; theta_lin first_estimates(landmark_idx).theta;然后基于 ( xr_lin, yr_lin, theta_lin ) 計(jì)算 ( H )而不是用 ( \mu_{pre}(1:3) )。需要注意一個容易被人忽視的細(xì)節(jié)FEJ不僅應(yīng)用于觀測更新有時也應(yīng)該應(yīng)用于運(yùn)動模型的雅可比。運(yùn)動模型中的旋轉(zhuǎn)雅可比如果使用不斷變化的航向角估計(jì)同樣會導(dǎo)致可觀測性矩陣的漂移。理論和文獻(xiàn)中通常建議運(yùn)動模型與觀測模型都做FEJ處理才能獲得最佳效果。我實(shí)測下來僅對觀測模型做FEJ就能改善大部分問題但兩者都做更為穩(wěn)妥。4.2 第二條思路直接約束可觀測性O(shè)C-EKFFEJ是一種間接保證可觀測性一致性的方法思路是讓線性化點(diǎn)固定。OC-EKF則更直接在每一步求解一個帶約束的優(yōu)化問題直接把雅可比矩陣投影修正到理論可觀測性約束條件下。具體來說在每一步得到觀測雅可比 ( H ) 后讓 ( H ) 滿足[ H \cdot X_{\text{null}} 0 ]其中 ( X_{\text{null}} ) 是理論不可觀測方向的基。在2D SLAM中這3個不可觀測方向可以構(gòu)造為% 不可觀測方向基近似 % 1. 全局 x 平移 % 2. 全局 y 平移 % 3. 全局旋轉(zhuǎn) X_null [1 0 -yr_all; 0 1 xr_all; 0 0 1];上式中 ( xr_all, yr_all ) 取當(dāng)前狀態(tài)估計(jì)中的機(jī)器人位置或路標(biāo)位置的平均具體推導(dǎo)需要從SLAM的系統(tǒng)模型出發(fā)讓每個不可觀測方向?qū)?yīng)一個全局變換。一般的做法是先構(gòu)建理論不可觀測方向的矩陣 ( U )然后對 ( H ) 做修正使 ( H U 0 )。這可以通過在 ( H ) 上左乘一個修正矩陣實(shí)現(xiàn)也可以用投影法令 ( H_{\text{oc}} H - H U (U^T U)^{-1} U^T )。這樣新的 ( H_{\text{oc}} ) 的零空間就包含了所有不可觀測方向。我在Matlab中實(shí)現(xiàn)OC-EKF時踩過一個坑直接對 ( H ) 做投影修正如果只是機(jī)械地減去 ( H U (U^T U)^{-1} U^T )得到的 ( H_{\text{oc}} ) 雖然滿足約束但可能與真實(shí)觀測模型偏差過大導(dǎo)致濾波器不穩(wěn)定。工程上通常需要將修正限制在較小的范圍內(nèi)或者只對關(guān)鍵部分做約束而不是全盤投影。這是OC-EKF落地時比較微妙的地方。4.3 兩種方案的Matlab實(shí)現(xiàn)要點(diǎn)與對比我把兩種方案放在同一套仿真框架里做了對比實(shí)驗(yàn)結(jié)論寫在下面。先看FEJ的實(shí)現(xiàn)效果。在標(biāo)準(zhǔn)EKF里平均NEES在150秒時到18換成FEJ之后平均NEES全程保持在4左右長時間運(yùn)行后也沒有明顯上漲趨勢。這說明FEJ確實(shí)解決了大部分可觀測性泄漏問題。不過NEES略高于3說明仍然有小幅不一致主要來源是路標(biāo)初始化時的噪聲和首估時刻的誤差這部分殘留偏差可以通過跑更多蒙特卡洛次數(shù)后取均值進(jìn)一步觀察一般落在3到5之間是可以接受的工程范圍。再看OC-EKF的實(shí)現(xiàn)效果。理論上OC-EKF比FEJ更嚴(yán)格因?yàn)樗苯颖ＷC了每一步的可觀測性約束。但實(shí)際仿真中我發(fā)現(xiàn)OC-EKF對實(shí)現(xiàn)細(xì)節(jié)更敏感比如 ( X_{\text{null}} ) 構(gòu)造中用的是當(dāng)前點(diǎn)還是首發(fā)點(diǎn)對結(jié)果影響很大。我嘗試了不同構(gòu)造方式最優(yōu)的OC-EKF平均NEES在3.5左右與FEJ性能接近但在計(jì)算耗時上要略高一些。兩者對比可以總結(jié)為下面這個表格維度標(biāo)準(zhǔn)EKFFEJOC-EKF平均NEES150s184左右3.5左右實(shí)現(xiàn)復(fù)雜度低中較高需要維護(hù)的額外變量無首發(fā)估計(jì)表不可觀測方向基矩陣對模型的依賴低中較高需理論分析數(shù)值穩(wěn)定性好好較敏感從工程落地角度我個人的選擇是如果只是想快速改善現(xiàn)有SLAM代碼的一致性優(yōu)先上FEJ改動量小效果穩(wěn)定如果是在做學(xué)術(shù)研究或追求極致一致性可以上OC-EKF但需要有耐心調(diào)試數(shù)值細(xì)節(jié)。5. 實(shí)操過程中常見問題與排查技巧5.1 常見問題速查表在實(shí)現(xiàn)和調(diào)試這套系統(tǒng)的過程中我遇到了不少問題整理成一個速查表方便你對照排查?，F(xiàn)象可能原因排查思路解決方案NEES從一開始就很大初始協(xié)方差設(shè)置不合理檢查P0數(shù)量級對比觀測噪聲重新設(shè)定初始協(xié)方差適當(dāng)增大機(jī)器人位姿初始方差NEES只在轉(zhuǎn)彎時飆升旋轉(zhuǎn)時雅可比線性化誤差放大觀察是否在omega較大時刻N(yùn)EES異常嘗試更小的控制周期dt檢查運(yùn)動模型散度路標(biāo)越多NEES越大每個路標(biāo)的微小泄漏持續(xù)累積固定軌跡逐步增加路標(biāo)做消融使用FEJ或OC-EKF消除累積泄漏協(xié)方差矩陣出現(xiàn)非正定逐路標(biāo)更新順序?qū)е聰?shù)值問題檢查P是否對稱正定改用批量更新改用Joseph形式更新協(xié)方差濾波結(jié)果發(fā)散誤差巨大初始航向角誤差過大或角度未歸一化檢查innovation中的角度是否wrapToPi處理角度歸一化增大初始theta方差地圖估計(jì)逐漸扭曲路標(biāo)初始化時使用了帶偏的機(jī)器人位姿觀察首估時刻機(jī)器人位姿誤差大小嘗試延遲初始化等機(jī)器人位姿較穩(wěn)定時再初始化路標(biāo)5.2 數(shù)值穩(wěn)定性與參數(shù)調(diào)優(yōu)EKF對數(shù)值穩(wěn)定性要求其實(shí)很高尤其是當(dāng)狀態(tài)維度達(dá)到上百維80個路標(biāo)就是163維狀態(tài)時協(xié)方差矩陣的數(shù)值誤差會被放大。我在調(diào)試過程中有幾個重要發(fā)現(xiàn)。第一個發(fā)現(xiàn)是協(xié)方差更新形式的差異。理論教科書上通常寫 ( P (I - K H) P_{\text{pre}} )但這在數(shù)值上容易失去對稱性和正定性。更穩(wěn)的寫法是Joseph形式[ P_{\text{est}} (I - K H) P_{\text{pre}} (I - K H)^T K R K^T ]這個形式在計(jì)算上稍微貴一點(diǎn)但在長時間運(yùn)行中能顯著減少非正定協(xié)方差的出現(xiàn)概率。我在項(xiàng)目里最后都改成了這個版本穩(wěn)定性好了不少。第二個發(fā)現(xiàn)是角度處理問題。SLAM里到處都是角度機(jī)器人航向、方位角觀測、繞行后的角度累積。除了更新時要做角度歸一化之外計(jì)算NEES時也要對角度誤差做wrapToPi處理否則一條簡單的圓形軌跡就會因?yàn)榻嵌壤@圈導(dǎo)致NEES計(jì)算錯誤。細(xì)節(jié)雖小但影響巨大。第三個發(fā)現(xiàn)是控制周期dt的選擇。dt太大運(yùn)動模型線性化誤差大不一致性會更早暴露dt太小計(jì)算量增大而且如果dt小于傳感器采樣周期中間插入的無觀測步只會增加計(jì)算負(fù)擔(dān)。我的建議是讓運(yùn)動更新和觀測更新同步進(jìn)行dt與傳感器采樣周期一致即可不需要刻意細(xì)分。在我上面的圓形軌跡實(shí)驗(yàn)中dt取0.1s同時用作控制和觀測采樣間隔效果良好。關(guān)于調(diào)參還有一個容易被忽略的關(guān)鍵點(diǎn)過程噪聲Q不要取得過小。有些同學(xué)為了讓濾波器看起來“自信”把Q壓得很低這反而會加劇不一致性。原因很簡單Q太小意味著濾波器對運(yùn)動模型極度信任幾乎完全依賴預(yù)測這會讓線性化點(diǎn)的誤差得不到修正虛假信息注入得更嚴(yán)重。我在實(shí)驗(yàn)中把線速度噪聲標(biāo)準(zhǔn)差調(diào)到0.2m/s已經(jīng)是比較合理的范圍如果你發(fā)現(xiàn)NEES在不一致性的基礎(chǔ)上還伴隨劇烈振蕩可以嘗試適當(dāng)調(diào)大Q。5.3 從仿真到實(shí)車應(yīng)用的思考仿真里的NEES可以用真值來算但實(shí)車上你永遠(yuǎn)沒有真值一致性判斷會變得更困難。業(yè)界常用的一種替代方案是評估濾波器輸出與里程計(jì)、GPS在允許的傳感器組合中等其他信息源之間的交叉一致性或者使用邊界的殘差統(tǒng)計(jì)。不過這些都是后話先把仿真環(huán)境中的不一致性問題研究透對實(shí)車應(yīng)用絕對有正向幫助。我個人的體會是理解可觀測性這個角度比盲目調(diào)參重要得多。以前遇到定位漂移第一反應(yīng)是調(diào)整噪聲參數(shù)、換傳感器、加路標(biāo)效果往往是按下葫蘆浮起瓢。后來從可觀測性角度去想問題很快就定位到協(xié)方差低估這個結(jié)構(gòu)性缺陷上改用FEJ之后很多“玄學(xué)”問題一下子消失了。這就是理論視角帶來的工程紅利。6. 寫在最后個人實(shí)操體會做這個研究的這段時間我最大的感受是EKF-SLAM看起來結(jié)構(gòu)簡單代碼也就一兩百行但里面藏的理論深度遠(yuǎn)超想象。你只把它當(dāng)作一個“概率狀態(tài)估計(jì)器”來用也能跑出結(jié)果但一旦你去追問協(xié)方差是否可信、估計(jì)是否一致就不得不去面對可觀測性這個底層話題。從實(shí)操層面給你幾個建議。第一Monte Carlo次數(shù)越多越好至少20次起步否則NEES曲線抖動太大很難下結(jié)論。第二不要一開始就上幾十個路標(biāo)的高維場景先用三五個路標(biāo)把邏輯跑通觀察每個路標(biāo)首估時刻對后續(xù)結(jié)果的影響。第三把標(biāo)準(zhǔn)EKF、FEJ、OC-EKF三者放在同一套仿真代碼里對比你會更清楚地看到線性化點(diǎn)漂移到底造成了什么影響。最后再分享一個小技巧當(dāng)我想快速判斷某個改動是否有效時只看兩個指標(biāo)——平均NEES是否隨運(yùn)行時間收斂而不是發(fā)散以及協(xié)方差矩陣的特征值分布是否出現(xiàn)異常小的方向那通常是虛假信息壓縮出來的。這兩個信號比觀察軌跡誤差還要靈敏。希望這篇內(nèi)容能幫你少走一些彎路把EKF-SLAM的一致性這個老大難問題徹底搞清楚。 EKF-SLAM不一致性根源與可觀測性約束改進(jìn)實(shí)踐
做機(jī)器人定位與建圖的同學(xué)遲早會遇到一個很詭異的現(xiàn)象你用擴(kuò)展卡爾曼濾波器EKF跑同時定位與地圖構(gòu)建SLAM傳感器噪聲參數(shù)明明設(shè)置得很合理仿真跑了幾圈回頭看NEES指標(biāo)發(fā)現(xiàn)真實(shí)估計(jì)誤差比濾波器預(yù)測的方差大了好幾倍。這個現(xiàn)象叫不一致性本質(zhì)上是EKF在非線性系統(tǒng)中丟失了可觀測性約束。我在處理多個定位項(xiàng)目時被這個問題反復(fù)折磨過今天就從可觀測性的角度把這件事徹底拆開講清楚包含完整的Matlab實(shí)現(xiàn)思路和可以抄作業(yè)的代碼框架。這篇文章適合兩類人看一類是正在用EKF做SLAM、發(fā)現(xiàn)結(jié)果“看著還行但協(xié)方差不靠譜”的研究生或工程師另一類是準(zhǔn)備做狀態(tài)估計(jì)方向算法研究、想理解FEJ和可觀測性約束這類進(jìn)階概念的同學(xué)。我會先講清楚不一致性從哪來再給出一套能在Matlab里復(fù)現(xiàn)問題并驗(yàn)證改進(jìn)方案的完整流程最后把我踩過的坑整理成速查表盡量讓你繞開這些彎路。1. 揭開EKF-SLAM不一致性的面紗1.1 不一致性到底是什么先說結(jié)論一個濾波器被稱為“一致”的指的是它輸出的協(xié)方差矩陣能夠真實(shí)反映估計(jì)誤差的統(tǒng)計(jì)特性。換句話說協(xié)方差說這個誤差是±0.1米那么大量實(shí)驗(yàn)后你的真實(shí)誤差標(biāo)準(zhǔn)偏差就應(yīng)該接近0.1米。如果協(xié)方差說誤差很小實(shí)際誤差卻大得多那就是不一致也叫“過于自信”或“樂觀估計(jì)”。在EKF-SLAM里這個問題表現(xiàn)得非常突出。一個2D平面中帶N個路標(biāo)的SLAM系統(tǒng)狀態(tài)維度是32N機(jī)器人位姿3維每個路標(biāo)位置2維但理論可觀測維度只有2N。也就是說全局位置平移和全局旋轉(zhuǎn)這3個方向本質(zhì)上是不可以被觀測的。無論你裝了多少路標(biāo)、觀測了多少次機(jī)器人的絕對位置都無法被唯一確定因?yàn)檎麄€地圖平移旋轉(zhuǎn)一下觀測值完全不變。標(biāo)準(zhǔn)EKF的問題在于它在每一步都把非線性系統(tǒng)在當(dāng)前估計(jì)點(diǎn)做線性化然后用線性卡爾曼更新的方式處理。線性化點(diǎn)的誤差會破壞雅可比矩陣的結(jié)構(gòu)使得濾波器在那些“理論上不該獲得信息”的全局方向上產(chǎn)生了虛假的信息增益。結(jié)果就是協(xié)方差被持續(xù)壓縮系統(tǒng)對自己的絕對位姿越來越自信但真實(shí)誤差根本沒有變小。1.2 可觀測性約束被破壞的全過程為了把這個過程看得更具體想象你沿著一條環(huán)形路徑走路上不斷看到周圍的幾個路標(biāo)。理想的SLAM應(yīng)該做到你對路標(biāo)之間的相對位置、機(jī)器人與它們的相對幾何關(guān)系越來越確定但你對整個地圖在全局坐標(biāo)系中的絕對位置仍然一無所知。但標(biāo)準(zhǔn)EKF不是這么干的。當(dāng)機(jī)器人發(fā)生旋轉(zhuǎn)時狀態(tài)估計(jì)中的航向角誤差會讓觀測雅可比矩陣發(fā)生扭曲。原本應(yīng)該在可觀測性矩陣零空間里的方向被線性化雅可比矩陣投影到了非零空間。每一次更新后濾波器都“學(xué)到”了一點(diǎn)點(diǎn)關(guān)于絕對位置的信息盡管這些信息完全是虛假的。多次循環(huán)下來協(xié)方差矩陣會變得越來越小但真實(shí)誤差并不會按照這個速度收斂于是NEES指標(biāo)開始飆升。我剛開始跑仿真的時候看到協(xié)方差橢圓縮得很漂亮以為定位精度很高結(jié)果一對比真實(shí)軌跡差點(diǎn)沒嚇壞。協(xié)方差說位置誤差標(biāo)準(zhǔn)偏差只有0.05米實(shí)際卻已經(jīng)飄了0.3米以上。這就是不一致性最直接的工程危害它讓你的濾波器不再可信輸出結(jié)果用于決策比如路徑規(guī)劃、避障時會埋下巨大隱患。2. 從可觀測性理論看EKF-SLAM的深層次問題2.1 非線性系統(tǒng)可觀測性分析基礎(chǔ)談到可觀測性很多同學(xué)第一反應(yīng)是線性系統(tǒng)那套觀測矩陣的秩是否等于狀態(tài)維度能觀測就代表可以通過輸出反推狀態(tài)。但對于SLAM這種非線性系統(tǒng)嚴(yán)格的分析工具是非線性可觀測性理論通常用李導(dǎo)數(shù)構(gòu)建的可觀測性矩陣來判斷。在無噪聲的理想情況下2D SLAM系統(tǒng)的狀態(tài)空間是機(jī)器人位姿(localization)和路標(biāo)位置(mapping)的聯(lián)合。觀測模型是距離與方位它們只依賴機(jī)器人與路標(biāo)之間的相對位置。這意味著全局坐標(biāo)系的三個變換——水平平移、垂直平移、整體旋轉(zhuǎn)——不產(chǎn)生任何觀測差異。從李導(dǎo)數(shù)的角度求出來可觀測性矩陣的秩比狀態(tài)維度少3這3個自由度就是不可觀測方向。理解這一點(diǎn)非常重要SLAM的目標(biāo)從來不是絕對定位而是相對幾何關(guān)系的估計(jì)。機(jī)器人從原點(diǎn)出發(fā)還是從(100,100)出發(fā)對路標(biāo)相對位置、機(jī)器人相對路標(biāo)的估計(jì)結(jié)果完全等價?？捎^測性理論告訴我們這3個自由度根本上無法被任何傳感器觀測消除。2.2 線性化讓不可觀測方向“泄漏”了信息非線性系統(tǒng)的可觀測性分析給的是一個理論基準(zhǔn)理想狀況下哪些方向可觀測、哪些方向不可觀測。但EKF每次更新用的是線性化模型線性化模型的雅可比矩陣是否完美保持這些不可觀測方向就是問題核心了。假設(shè)理論可觀測性分析告訴我們某個方向向量 ( g ) 應(yīng)該在可觀測性矩陣的零空間里。但線性化時用的點(diǎn)是當(dāng)前狀態(tài)估計(jì)值 ( \mu ) 而真值是 ( x ) 二者之間存在誤差。這個誤差會讓雅可比矩陣在 ( g ) 方向上的投影不再是零。用大白話說濾波器本應(yīng)認(rèn)為“在全局平移方向上我一無所知”但由于線性化點(diǎn)偏了它誤以為觀測數(shù)據(jù)能提供這個方向的信息于是開始錯誤地修正這個方向上的狀態(tài)同時壓縮協(xié)方差。這種現(xiàn)象在機(jī)器人轉(zhuǎn)彎時尤其嚴(yán)重。因?yàn)楹较蚪钦`差在旋轉(zhuǎn)時會被放大觀測雅可比中的角度相關(guān)項(xiàng)偏移較大不可觀測方向的泄漏就愈發(fā)明顯。所以一個典型現(xiàn)象是EKF-SLAM跑直線時NEES基本正常一旦開始轉(zhuǎn)彎NEES立刻飆升而且轉(zhuǎn)得越頻繁、越多問題越嚴(yán)重。這也是很多人在做圓形軌跡實(shí)驗(yàn)時發(fā)現(xiàn)一致性差得離譜的原因。更深一層這種泄漏是持續(xù)累積的。每一次更新都往不可觀測方向注入一點(diǎn)虛假信息濾波器協(xié)方差越來越小而估計(jì)誤差中的不可觀測成分卻沒有減少最終表現(xiàn)為嚴(yán)重的過度自信。這也是標(biāo)準(zhǔn)EKF-SLAM在長距離運(yùn)行中表現(xiàn)不佳的關(guān)鍵內(nèi)因。3. Matlab環(huán)境下的標(biāo)準(zhǔn)EKF-SLAM實(shí)現(xiàn)與問題復(fù)現(xiàn)3.1 狀態(tài)向量與系統(tǒng)模型設(shè)計(jì)在研究改進(jìn)方案之前先把標(biāo)準(zhǔn)EKF-SLAM在Matlab里完整實(shí)現(xiàn)一遍把不一致性問題真實(shí)復(fù)現(xiàn)出來。這樣后面做對比時就有明確的基線了。我的仿真場景設(shè)置如下機(jī)器人在一個20m×20m的平面區(qū)域運(yùn)行沿半徑8m的圓形軌跡勻速運(yùn)動線速度 ( v 1.5 m/s )角速度 ( \omega v/R 0.1875 rad/s )。環(huán)境中隨機(jī)分布80個路標(biāo)機(jī)器人搭載一個距離-方位傳感器可以觀測到視場內(nèi)的路標(biāo)距離噪聲標(biāo)準(zhǔn)差0.1m方位噪聲標(biāo)準(zhǔn)差0.01rad。運(yùn)動模型使用速度模型線速度噪聲標(biāo)準(zhǔn)差0.2m/s角速度噪聲標(biāo)準(zhǔn)差0.05rad/s。狀態(tài)向量定義如下這是后續(xù)所有代碼的基礎(chǔ)% 狀態(tài)向量結(jié)構(gòu) % x [x_r; y_r; theta_r; x_l1; y_l1; x_l2; y_l2; ...; x_lN; y_lN] % 索引約定 % 1 : 機(jī)器人 x 坐標(biāo) % 2 : 機(jī)器人 y 坐標(biāo) % 3 : 機(jī)器人航向角 theta % 42k: 第 k 個路標(biāo)的 x 坐標(biāo) % 52k: 第 k 個路標(biāo)的 y 坐標(biāo) % 總維度 D 3 2*N運(yùn)動模型采用經(jīng)典的速度模型機(jī)器人接收到控制輸入 ( u [v, \omega]^T )經(jīng)過時間 ( \Delta t ) 后位姿更新為[ \begin{aligned} x_{t1} x_t v\Delta t \cos\theta_t \ y_{t1} y_t v\Delta t \sin\theta_t \ \theta_{t1} \theta_t \omega\Delta t \end{aligned} ]路標(biāo)位置假設(shè)是靜止的不參與運(yùn)動方程。觀測模型針對每一個被看到的路標(biāo) ( i )[ \begin{aligned} h_1 \sqrt{(x_i - x_r)^2 (y_i - y_r)^2} \ h_2 \text{atan2}(y_i - y_r, x_i - x_r) - \theta_r \end{aligned} ]也就是相對距離和相對方位角。兩個觀測量的噪聲相互獨(dú)立構(gòu)成觀測噪聲矩陣 ( R \text{diag}(0.1^2, 0.01^2) )。3.2 標(biāo)準(zhǔn)EKF的預(yù)測與更新核心代碼下面給出標(biāo)準(zhǔn)EKF-SLAM在Matlab中的核心代碼實(shí)現(xiàn)。這段代碼我經(jīng)過多次仿真驗(yàn)證結(jié)構(gòu)清晰方便你在此基礎(chǔ)上做改進(jìn)。預(yù)測步代碼function [mu_pre, P_pre] ekf_predict(mu, P, v, omega, dt, Q, N) % mu: 狀態(tài)向量 (32N) x 1 % P : 協(xié)方差矩陣 (32N) x (32N) % v, omega: 控制輸入 % Q: 過程噪聲協(xié)方差 (2x2) theta mu(3); D 3 2*N; % 運(yùn)動模型函數(shù) mu_pre mu; mu_pre(1) mu(1) v*dt*cos(theta); mu_pre(2) mu(2) v*dt*sin(theta); mu_pre(3) mu(3) omega*dt; % 狀態(tài)雅可比 F d(f)/d(x) F eye(D); F(1,3) -v*dt*sin(theta); F(2,3) v*dt*cos(theta); % 噪聲雅可比 G d(f)/d(u_noise) G zeros(D, 2); G(1,1) dt*cos(theta); G(2,1) dt*sin(theta); G(3,2) dt; % 預(yù)測協(xié)方差 P_pre F * P * F G * Q * G; end更新步代碼逐個路標(biāo)更新function [mu_est, P_est] ekf_update(mu_pre, P_pre, z, landmark_idx, R_obs, N) % z: 觀測向量 [距離; 方位] % landmark_idx: 當(dāng)前觀測到的路標(biāo)在狀態(tài)向量中的編號 % R_obs: 觀測噪聲協(xié)方差 (2x2) D 3 2*N; theta mu_pre(3); xr mu_pre(1); yr mu_pre(2); % 提取路標(biāo)位置 lk 3 2*(landmark_idx - 1) 1; xl mu_pre(lk); yl mu_pre(lk 1); dx xl - xr; dy yl - yr; q sqrt(dx^2 dy^2); % 預(yù)測觀測值 z_hat [q; atan2(dy, dx) - theta]; % 觀測雅可比 H d(h)/d(x) H zeros(2, D); H(1,1) -dx/q; H(1,2) -dy/q; H(1,lk) dx/q; H(1,lk1) dy/q; H(2,1) dy/q^2; H(2,2) -dx/q^2; H(2,3) -1; H(2,lk) -dy/q^2; H(2,lk1) dx/q^2; % 創(chuàng)新 innovation z - z_hat; innovation(2) wrapToPi(innovation(2)); % 角度歸一化 S H * P_pre * H R_obs; K P_pre * H / S; mu_est mu_pre K * innovation; P_est (eye(D) - K * H) * P_pre; end這里有個需要注意的細(xì)節(jié)角度差一定要用wrapToPi歸一化否則方位角的創(chuàng)新項(xiàng)會在±π附近產(chǎn)生跳變造成錯誤的更新。這是新手最容易忽略的點(diǎn)我當(dāng)年在這個問題上卡了整整一個下午。還有一個關(guān)鍵工程點(diǎn)上面的代碼是逐路標(biāo)更新的即每看到一個路標(biāo)就做一次更新。這樣做實(shí)現(xiàn)簡單但在信息形式下相當(dāng)于對觀測信息做了一次近似分解嚴(yán)格來說不如批量更新精確。批量更新的做法是把所有觀測到的路標(biāo)的雅可比和觀測值都堆疊起來一次更新完成% 批量更新示意 H_all zeros(2*n_obs, D); z_all zeros(2*n_obs, 1); R_all kron(eye(n_obs), R_obs); % ... 堆疊各個路標(biāo)的 H 和 z ... S H_all * P_pre * H_all R_all; K P_pre * H_all / S;批量更新的好處是理論更嚴(yán)格不會因?yàn)轫樞蚨绊懡Y(jié)果在路標(biāo)數(shù)量不多時計(jì)算量也可接受。如果你的仿真場景路標(biāo)數(shù)量比較多可以繼續(xù)用逐路標(biāo)更新但心里要清楚這個近似帶來的細(xì)微差別。3.3 一致性量化指標(biāo)與復(fù)現(xiàn)實(shí)驗(yàn)設(shè)計(jì)復(fù)現(xiàn)問題不能只靠肉眼觀察協(xié)方差橢圓必須用數(shù)學(xué)指標(biāo)量化。最常用的指標(biāo)是NEESNormalized Estimation Error Squared。在時刻t給定真實(shí)位姿、估計(jì)位姿和協(xié)方差NEES定義為[ \epsilon_t (x_t^{\text{true}} - x_t^{\text{est}})^T P_t^{-1} (x_t^{\text{true}} - x_t^{\text{est}}) ]這里的 ( x ) 通常只取機(jī)器人位姿的三維部分 ( (x, y, \theta) )。如果濾波器是一致的那么NEES應(yīng)該服從自由度為3的卡方分布其均值為3。無數(shù)次蒙特卡洛實(shí)驗(yàn)后平均NEES應(yīng)接近3。如果平均NEES顯著大于3說明濾波器不一致而且數(shù)值越大不一致越嚴(yán)重。Matlab中計(jì)算NEESfunction nees compute_nees(x_true, x_est, P_est) % 取三維位姿 d x_est(1:3) - x_true(1:3); d(3) wrapToPi(d(3)); nees d * inv(P_est(1:3,1:3)) * d; end為了得到統(tǒng)計(jì)上可靠的結(jié)論必須做蒙特卡洛仿真。我在實(shí)驗(yàn)中跑了50次獨(dú)立運(yùn)行每次重新生成隨機(jī)噪聲最后將50次NEES求平均得到平均NEES曲線。判斷標(biāo)準(zhǔn)很樸素如果平均NEES穩(wěn)定在3附近濾波器一致如果遠(yuǎn)高于3濾波器不一致如果遠(yuǎn)低于3濾波器雖然“保守”協(xié)方差過大但至少不虛假自信不過工程上過保守也是次優(yōu)的。我在標(biāo)準(zhǔn)EKF仿真中跑出來的結(jié)果是前40秒平均NEES還能維持在3.5附近一旦機(jī)器人繞完第一圈開始持續(xù)重復(fù)觀測路標(biāo)平均NEES直線上升到150秒時已經(jīng)飆到18以上。這說明標(biāo)準(zhǔn)EKF的不一致性在長時間重復(fù)觀測場景下非常嚴(yán)重。你不需要特殊調(diào)試只要跑一個閉合軌跡就能清晰復(fù)現(xiàn)這個現(xiàn)象。4. 基于可觀測性約束的改進(jìn)實(shí)現(xiàn)4.1 第一條思路固定線性化點(diǎn)FEJ理解了問題根源之后改進(jìn)思路就變得清晰了。既然不一致性來自線性化點(diǎn)漂移導(dǎo)致的不可觀測方向泄漏那就設(shè)法讓線性化點(diǎn)保持穩(wěn)定使系統(tǒng)的可觀測性矩陣在時間上一致。FEJFirst-Estimates Jacobian方法的核心做法是對于每一個路標(biāo)在它第一次被觀測到的時候記錄當(dāng)時的機(jī)器人狀態(tài)估計(jì)值。之后在對這個路標(biāo)做觀測更新時無論機(jī)器人當(dāng)前狀態(tài)估計(jì)變成了什么構(gòu)建觀測雅可比矩陣都使用那個“首發(fā)估計(jì)值”而不是當(dāng)前估計(jì)值。為什么這樣有效因?yàn)镾LAM系統(tǒng)中可觀測性矩陣的列空間變化主要來源于每個路標(biāo)被重復(fù)觀測時線性化點(diǎn)的不同。如果每次都用第一次觀測時的狀態(tài)來線性化那么濾波器對某個路標(biāo)的觀測雅可比就從一而終不會再產(chǎn)生虛假信息注入。從數(shù)學(xué)上講這保證了系統(tǒng)線性化模型的可觀測性屬性始終與非線性系統(tǒng)理論保持一致。在Matlab中實(shí)現(xiàn)FEJ需要額外維護(hù)一個“首發(fā)估計(jì)”字典% 初始化 first_estimates struct(xr, [], yr, [], theta, [], initialized, false); % 更新時 % 檢查路標(biāo)是否首次被觀測 if ~first_estimates(landmark_idx).initialized % 首次觀測用當(dāng)前估計(jì)作為首發(fā)估計(jì) first_estimates(landmark_idx).xr mu_pre(1); first_estimates(landmark_idx).yr mu_pre(2); first_estimates(landmark_idx).theta mu_pre(3); first_estimates(landmark_idx).initialized true; end % 構(gòu)建觀測雅可比時用首發(fā)估計(jì)而不是當(dāng)前估計(jì) xr_lin first_estimates(landmark_idx).xr; yr_lin first_estimates(landmark_idx).yr; theta_lin first_estimates(landmark_idx).theta;然后基于 ( xr_lin, yr_lin, theta_lin ) 計(jì)算 ( H )而不是用 ( \mu_{pre}(1:3) )。需要注意一個容易被人忽視的細(xì)節(jié)FEJ不僅應(yīng)用于觀測更新有時也應(yīng)該應(yīng)用于運(yùn)動模型的雅可比。運(yùn)動模型中的旋轉(zhuǎn)雅可比如果使用不斷變化的航向角估計(jì)同樣會導(dǎo)致可觀測性矩陣的漂移。理論和文獻(xiàn)中通常建議運(yùn)動模型與觀測模型都做FEJ處理才能獲得最佳效果。我實(shí)測下來僅對觀測模型做FEJ就能改善大部分問題但兩者都做更為穩(wěn)妥。4.2 第二條思路直接約束可觀測性O(shè)C-EKFFEJ是一種間接保證可觀測性一致性的方法思路是讓線性化點(diǎn)固定。OC-EKF則更直接在每一步求解一個帶約束的優(yōu)化問題直接把雅可比矩陣投影修正到理論可觀測性約束條件下。具體來說在每一步得到觀測雅可比 ( H ) 后讓 ( H ) 滿足[ H \cdot X_{\text{null}} 0 ]其中 ( X_{\text{null}} ) 是理論不可觀測方向的基。在2D SLAM中這3個不可觀測方向可以構(gòu)造為% 不可觀測方向基近似 % 1. 全局 x 平移 % 2. 全局 y 平移 % 3. 全局旋轉(zhuǎn) X_null [1 0 -yr_all; 0 1 xr_all; 0 0 1];上式中 ( xr_all, yr_all ) 取當(dāng)前狀態(tài)估計(jì)中的機(jī)器人位置或路標(biāo)位置的平均具體推導(dǎo)需要從SLAM的系統(tǒng)模型出發(fā)讓每個不可觀測方向?qū)?yīng)一個全局變換。一般的做法是先構(gòu)建理論不可觀測方向的矩陣 ( U )然后對 ( H ) 做修正使 ( H U 0 )。這可以通過在 ( H ) 上左乘一個修正矩陣實(shí)現(xiàn)也可以用投影法令 ( H_{\text{oc}} H - H U (U^T U)^{-1} U^T )。這樣新的 ( H_{\text{oc}} ) 的零空間就包含了所有不可觀測方向。我在Matlab中實(shí)現(xiàn)OC-EKF時踩過一個坑直接對 ( H ) 做投影修正如果只是機(jī)械地減去 ( H U (U^T U)^{-1} U^T )得到的 ( H_{\text{oc}} ) 雖然滿足約束但可能與真實(shí)觀測模型偏差過大導(dǎo)致濾波器不穩(wěn)定。工程上通常需要將修正限制在較小的范圍內(nèi)或者只對關(guān)鍵部分做約束而不是全盤投影。這是OC-EKF落地時比較微妙的地方。4.3 兩種方案的Matlab實(shí)現(xiàn)要點(diǎn)與對比我把兩種方案放在同一套仿真框架里做了對比實(shí)驗(yàn)結(jié)論寫在下面。先看FEJ的實(shí)現(xiàn)效果。在標(biāo)準(zhǔn)EKF里平均NEES在150秒時到18換成FEJ之后平均NEES全程保持在4左右長時間運(yùn)行后也沒有明顯上漲趨勢。這說明FEJ確實(shí)解決了大部分可觀測性泄漏問題。不過NEES略高于3說明仍然有小幅不一致主要來源是路標(biāo)初始化時的噪聲和首估時刻的誤差這部分殘留偏差可以通過跑更多蒙特卡洛次數(shù)后取均值進(jìn)一步觀察一般落在3到5之間是可以接受的工程范圍。再看OC-EKF的實(shí)現(xiàn)效果。理論上OC-EKF比FEJ更嚴(yán)格因?yàn)樗苯颖ＷC了每一步的可觀測性約束。但實(shí)際仿真中我發(fā)現(xiàn)OC-EKF對實(shí)現(xiàn)細(xì)節(jié)更敏感比如 ( X_{\text{null}} ) 構(gòu)造中用的是當(dāng)前點(diǎn)還是首發(fā)點(diǎn)對結(jié)果影響很大。我嘗試了不同構(gòu)造方式最優(yōu)的OC-EKF平均NEES在3.5左右與FEJ性能接近但在計(jì)算耗時上要略高一些。兩者對比可以總結(jié)為下面這個表格維度標(biāo)準(zhǔn)EKFFEJOC-EKF平均NEES150s184左右3.5左右實(shí)現(xiàn)復(fù)雜度低中較高需要維護(hù)的額外變量無首發(fā)估計(jì)表不可觀測方向基矩陣對模型的依賴低中較高需理論分析數(shù)值穩(wěn)定性好好較敏感從工程落地角度我個人的選擇是如果只是想快速改善現(xiàn)有SLAM代碼的一致性優(yōu)先上FEJ改動量小效果穩(wěn)定如果是在做學(xué)術(shù)研究或追求極致一致性可以上OC-EKF但需要有耐心調(diào)試數(shù)值細(xì)節(jié)。5. 實(shí)操過程中常見問題與排查技巧5.1 常見問題速查表在實(shí)現(xiàn)和調(diào)試這套系統(tǒng)的過程中我遇到了不少問題整理成一個速查表方便你對照排查?，F(xiàn)象可能原因排查思路解決方案NEES從一開始就很大初始協(xié)方差設(shè)置不合理檢查P0數(shù)量級對比觀測噪聲重新設(shè)定初始協(xié)方差適當(dāng)增大機(jī)器人位姿初始方差NEES只在轉(zhuǎn)彎時飆升旋轉(zhuǎn)時雅可比線性化誤差放大觀察是否在omega較大時刻N(yùn)EES異常嘗試更小的控制周期dt檢查運(yùn)動模型散度路標(biāo)越多NEES越大每個路標(biāo)的微小泄漏持續(xù)累積固定軌跡逐步增加路標(biāo)做消融使用FEJ或OC-EKF消除累積泄漏協(xié)方差矩陣出現(xiàn)非正定逐路標(biāo)更新順序?qū)е聰?shù)值問題檢查P是否對稱正定改用批量更新改用Joseph形式更新協(xié)方差濾波結(jié)果發(fā)散誤差巨大初始航向角誤差過大或角度未歸一化檢查innovation中的角度是否wrapToPi處理角度歸一化增大初始theta方差地圖估計(jì)逐漸扭曲路標(biāo)初始化時使用了帶偏的機(jī)器人位姿觀察首估時刻機(jī)器人位姿誤差大小嘗試延遲初始化等機(jī)器人位姿較穩(wěn)定時再初始化路標(biāo)5.2 數(shù)值穩(wěn)定性與參數(shù)調(diào)優(yōu)EKF對數(shù)值穩(wěn)定性要求其實(shí)很高尤其是當(dāng)狀態(tài)維度達(dá)到上百維80個路標(biāo)就是163維狀態(tài)時協(xié)方差矩陣的數(shù)值誤差會被放大。我在調(diào)試過程中有幾個重要發(fā)現(xiàn)。第一個發(fā)現(xiàn)是協(xié)方差更新形式的差異。理論教科書上通常寫 ( P (I - K H) P_{\text{pre}} )但這在數(shù)值上容易失去對稱性和正定性。更穩(wěn)的寫法是Joseph形式[ P_{\text{est}} (I - K H) P_{\text{pre}} (I - K H)^T K R K^T ]這個形式在計(jì)算上稍微貴一點(diǎn)但在長時間運(yùn)行中能顯著減少非正定協(xié)方差的出現(xiàn)概率。我在項(xiàng)目里最后都改成了這個版本穩(wěn)定性好了不少。第二個發(fā)現(xiàn)是角度處理問題。SLAM里到處都是角度機(jī)器人航向、方位角觀測、繞行后的角度累積。除了更新時要做角度歸一化之外計(jì)算NEES時也要對角度誤差做wrapToPi處理否則一條簡單的圓形軌跡就會因?yàn)榻嵌壤@圈導(dǎo)致NEES計(jì)算錯誤。細(xì)節(jié)雖小但影響巨大。第三個發(fā)現(xiàn)是控制周期dt的選擇。dt太大運(yùn)動模型線性化誤差大不一致性會更早暴露dt太小計(jì)算量增大而且如果dt小于傳感器采樣周期中間插入的無觀測步只會增加計(jì)算負(fù)擔(dān)。我的建議是讓運(yùn)動更新和觀測更新同步進(jìn)行dt與傳感器采樣周期一致即可不需要刻意細(xì)分。在我上面的圓形軌跡實(shí)驗(yàn)中dt取0.1s同時用作控制和觀測采樣間隔效果良好。關(guān)于調(diào)參還有一個容易被忽略的關(guān)鍵點(diǎn)過程噪聲Q不要取得過小。有些同學(xué)為了讓濾波器看起來“自信”把Q壓得很低這反而會加劇不一致性。原因很簡單Q太小意味著濾波器對運(yùn)動模型極度信任幾乎完全依賴預(yù)測這會讓線性化點(diǎn)的誤差得不到修正虛假信息注入得更嚴(yán)重。我在實(shí)驗(yàn)中把線速度噪聲標(biāo)準(zhǔn)差調(diào)到0.2m/s已經(jīng)是比較合理的范圍如果你發(fā)現(xiàn)NEES在不一致性的基礎(chǔ)上還伴隨劇烈振蕩可以嘗試適當(dāng)調(diào)大Q。5.3 從仿真到實(shí)車應(yīng)用的思考仿真里的NEES可以用真值來算但實(shí)車上你永遠(yuǎn)沒有真值一致性判斷會變得更困難。業(yè)界常用的一種替代方案是評估濾波器輸出與里程計(jì)、GPS在允許的傳感器組合中等其他信息源之間的交叉一致性或者使用邊界的殘差統(tǒng)計(jì)。不過這些都是后話先把仿真環(huán)境中的不一致性問題研究透對實(shí)車應(yīng)用絕對有正向幫助。我個人的體會是理解可觀測性這個角度比盲目調(diào)參重要得多。以前遇到定位漂移第一反應(yīng)是調(diào)整噪聲參數(shù)、換傳感器、加路標(biāo)效果往往是按下葫蘆浮起瓢。后來從可觀測性角度去想問題很快就定位到協(xié)方差低估這個結(jié)構(gòu)性缺陷上改用FEJ之后很多“玄學(xué)”問題一下子消失了。這就是理論視角帶來的工程紅利。6. 寫在最后個人實(shí)操體會做這個研究的這段時間我最大的感受是EKF-SLAM看起來結(jié)構(gòu)簡單代碼也就一兩百行但里面藏的理論深度遠(yuǎn)超想象。你只把它當(dāng)作一個“概率狀態(tài)估計(jì)器”來用也能跑出結(jié)果但一旦你去追問協(xié)方差是否可信、估計(jì)是否一致就不得不去面對可觀測性這個底層話題。從實(shí)操層面給你幾個建議。第一Monte Carlo次數(shù)越多越好至少20次起步否則NEES曲線抖動太大很難下結(jié)論。第二不要一開始就上幾十個路標(biāo)的高維場景先用三五個路標(biāo)把邏輯跑通觀察每個路標(biāo)首估時刻對后續(xù)結(jié)果的影響。第三把標(biāo)準(zhǔn)EKF、FEJ、OC-EKF三者放在同一套仿真代碼里對比你會更清楚地看到線性化點(diǎn)漂移到底造成了什么影響。最后再分享一個小技巧當(dāng)我想快速判斷某個改動是否有效時只看兩個指標(biāo)——平均NEES是否隨運(yùn)行時間收斂而不是發(fā)散以及協(xié)方差矩陣的特征值分布是否出現(xiàn)異常小的方向那通常是虛假信息壓縮出來的。這兩個信號比觀察軌跡誤差還要靈敏。希望這篇內(nèi)容能幫你少走一些彎路把EKF-SLAM的一致性這個老大難問題徹底搞清楚。