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