一文搞懂《擴(kuò)展卡爾曼濾波融合IMU和ODOM》

觀測(cè)數(shù)據(jù)里的 “噪聲和干擾”，就像瞄準(zhǔn)鏡前的樹葉、霧氣、樹木遮擋

當(dāng)樹葉擋住視線時(shí)（觀測(cè)數(shù)據(jù)突然不準(zhǔn)），卡爾曼濾波不會(huì)完全相信這一幀的觀測(cè)，而是更多依賴 “基于目標(biāo)運(yùn)動(dòng)規(guī)律的預(yù)判”（比如 “剛才目標(biāo)在勻速跑，不可能突然跳 10 米，這幀數(shù)據(jù)肯定是被樹葉擋了，忽略它”）。

擴(kuò)展卡爾曼濾波（EKF）是卡爾曼濾波（KF）的 “升級(jí)版”，專門用來解決卡爾曼濾波搞不定的場(chǎng)景。我們可以繼續(xù)用 “狙擊槍瞄準(zhǔn)移動(dòng)靶” 的比喻來理解它們的關(guān)系：

1. 卡爾曼濾波（KF）：只能對(duì)付 “規(guī)矩的目標(biāo)”

卡爾曼濾波的核心前提是：目標(biāo)的運(yùn)動(dòng)規(guī)律是 “線性的”。
比如移動(dòng)靶在勻速直線運(yùn)動(dòng)（速度不變、方向不變），或者勻加速運(yùn)動(dòng)（每秒多跑 1m/s）—— 這些運(yùn)動(dòng)可以用簡單的線性公式描述（比如 “下一秒位置 = 現(xiàn)在位置 + 速度 × 時(shí)間”）。
這時(shí)候，狙擊手只要用 “勻速 / 勻加速模型” 就能精準(zhǔn)預(yù)判，卡爾曼濾波的公式也能直接套用，計(jì)算簡單又高效。

2. 擴(kuò)展卡爾曼濾波（EKF）：能搞定 “調(diào)皮的目標(biāo)”

但現(xiàn)實(shí)中，很多目標(biāo)的運(yùn)動(dòng)是 “非線性的”：

• 比如移動(dòng)靶突然轉(zhuǎn)彎、繞圈，甚至做 S 形走位（運(yùn)動(dòng)方向和速度的關(guān)系不是簡單的線性公式）；

• 或者傳感器的觀測(cè)方式是非線性的（比如用雷達(dá)測(cè)角度，角度和位置的關(guān)系是三角函數(shù)，不是直線關(guān)系）。
  

總結(jié)：關(guān)系就像 “基礎(chǔ)款計(jì)算器” 和 “科學(xué)計(jì)算器”

• 卡爾曼濾波是 “基礎(chǔ)款”：只能算加減乘除（線性問題），簡單直接，速度快。
• 擴(kuò)展卡爾曼濾波是 “科學(xué)計(jì)算器”：能處理三角函數(shù)、指數(shù)等復(fù)雜運(yùn)算（非線性問題），通過 “近似線性化” 擴(kuò)展了適用范圍，但計(jì)算更復(fù)雜一點(diǎn)。
  
  
• 兩者的核心邏輯（“預(yù)判 + 修正” 的濾波思想）完全一致，只是 EKF 能應(yīng)對(duì)更貼近現(xiàn)實(shí)的非線性場(chǎng)景。
  
  

咱們可以把 EKF 融合 IMU 和 ODOM 的過程，想象成一個(gè) “猜位置” 的游戲，你需要結(jié)合兩種 “線索” 來不斷修正自己的猜測(cè)，讓結(jié)果越來越準(zhǔn)。

IMU（Inertial Measurement Unit）和 ODOM（Odometry）在機(jī)器人領(lǐng)域以及其他涉及運(yùn)動(dòng)感知和定位的系統(tǒng)中是非常重要的概念，以下是它們的詳細(xì)介紹：

IMU（慣性測(cè)量單元）

• 定義

  IMU 是一種能夠測(cè)量物體三軸加速度和三軸角速度的組合式傳感器，有的 IMU 還集成了磁力計(jì) ，可以測(cè)量物體的磁場(chǎng)信息，輔助確定物體的方向。它是一個(gè)小型化、高度集成的系統(tǒng)，能夠?qū)崟r(shí)提供物體的運(yùn)動(dòng)狀態(tài)信息。

• 工作原理

• 加速度計(jì)

  ：基于牛頓第二定律，通過檢測(cè)內(nèi)部質(zhì)量塊在加速度作用下產(chǎn)生的力，來測(cè)量加速度。比如，當(dāng)物體加速時(shí)，質(zhì)量塊會(huì)相對(duì)于傳感器內(nèi)部的固定結(jié)構(gòu)產(chǎn)生位移，通過檢測(cè)這種位移并將其轉(zhuǎn)換為電信號(hào)，就可以得到加速度的大小和方向。

• 陀螺儀

  ：利用科里奧利力原理工作。當(dāng)陀螺儀的殼體發(fā)生旋轉(zhuǎn)時(shí)，內(nèi)部的旋轉(zhuǎn)質(zhì)量塊會(huì)受到科里奧利力的作用，從而產(chǎn)生微小的振動(dòng)或偏移，通過檢測(cè)這些變化就可以測(cè)量出角速度。

ODOM（里程計(jì)）

• 定義

  里程計(jì)是一種用于測(cè)量物體在運(yùn)動(dòng)過程中移動(dòng)距離和方向的裝置或算法，通過對(duì)運(yùn)動(dòng)過程中的一些參數(shù)進(jìn)行累計(jì)和計(jì)算，來確定物體的位置變化。

• 工作原理

• 輪式里程計(jì)

  ：常見于輪式機(jī)器人，通過安裝在輪子上的編碼器來記錄輪子的轉(zhuǎn)動(dòng)圈數(shù)。結(jié)合輪子的半徑，就可以計(jì)算出機(jī)器人在平面上移動(dòng)的距離。同時(shí)，通過一些額外的傳感器（如安裝在差速驅(qū)動(dòng)機(jī)器人兩個(gè)輪子上的編碼器，對(duì)比兩個(gè)輪子的轉(zhuǎn)動(dòng)圈數(shù)差異），可以推算出機(jī)器人的轉(zhuǎn)向角度和方向變化。

先給兩個(gè) “線索源” 做個(gè)簡單畫像：

• IMU：像你身上的 “運(yùn)動(dòng)傳感器”，能實(shí)時(shí)感覺到自己在怎么動(dòng) —— 比如走得快還是慢、有沒有轉(zhuǎn)彎（用角速度測(cè)轉(zhuǎn)彎，用加速度測(cè)加速）。但它有個(gè)毛?。翰露嗔藭?huì) “跑偏”（比如站著不動(dòng)，它可能會(huì)慢慢覺得你在挪位置，這叫 “漂移”）。

• ODOM：像你手里的 “路標(biāo)導(dǎo)航”，每隔一會(huì)兒告訴你當(dāng)前的位置和速度（比如輪式里程計(jì)通過輪子轉(zhuǎn)了多少圈算位置，或者視覺里程計(jì)通過攝像頭看環(huán)境算位置）。它相對(duì)穩(wěn)定，不會(huì)輕易跑偏，但更新沒那么快（比如輪子轉(zhuǎn)半圈才出一次數(shù)據(jù)），而且偶爾會(huì) “騙你”（比如輪子打滑時(shí)，它算的位置就不準(zhǔn)了）。

EKF 做的事，就是用 IMU 的 “實(shí)時(shí)感覺” 先“猜”，再用 ODOM 的 “路標(biāo)信息” 來修正，最后得到一個(gè)又快又準(zhǔn)的結(jié)果。具體分兩步：

第一步：用 IMU “猜”（預(yù)測(cè)階段）

假設(shè)你現(xiàn)在要走路去超市，EKF 先根據(jù) IMU 的信息，實(shí)時(shí)猜你每一秒的位置和速度。比如：

• 上一秒你在 “小區(qū)門口”（上一時(shí)刻的位置），速度是 “1 米 / 秒向前”（上一時(shí)刻的速度）。

• 這一秒 IMU 告訴你：“你沒轉(zhuǎn)彎（角速度為 0），也沒加速（加速度為 0）”（IMU 的測(cè)量值）。

• 那 EKF 就會(huì)猜：“這一秒你應(yīng)該在小區(qū)門口往前 1 米的地方，速度還是 1 米 / 秒”（預(yù)測(cè)的當(dāng)前位置和速度）。

  
  

但 EKF 知道 IMU 會(huì) “跑偏”，所以猜的時(shí)候會(huì)留個(gè) “誤差范圍”：比如 “位置大概在 1 米左右，誤差可能有 0.5 米”（這個(gè)誤差會(huì)隨著 IMU 用得越久慢慢變大，就像猜多了越來越?jīng)]譜）。

第二步：用 ODOM “修正”（更新階段）

走了一會(huì)兒，ODOM 這個(gè) “路標(biāo)” 說話了：“根據(jù)輪子轉(zhuǎn)的圈數(shù)，你現(xiàn)在應(yīng)該在‘離小區(qū)門口 1.2 米’的地方”（ODOM 的測(cè)量值）。

這時(shí)候 EKF 要做兩件事：

• 對(duì)比 “猜的位置（1 米）” 和 “ODOM 說的位置（1.2 米）”，算出差了 0.2 米。

• 看看這兩個(gè)線索誰更可信，再?zèng)Q定怎么修正。

比如：

• 如果地面很平，輪子沒打滑，ODOM 很靠譜（權(quán)重高），那就多聽 ODOM 的，把猜測(cè)從 1 米調(diào)成 1.15 米（靠近 1.2 米）。

• 如果地面坑坑洼洼，輪子可能打滑了，ODOM 不太準(zhǔn)（權(quán)重低），就少聽 ODOM 的，調(diào)成 1.05 米（靠近原來的猜測(cè)）。

循環(huán)往復(fù)，越猜越準(zhǔn)

之后就重復(fù)這兩步：

• 沒 ODOM 數(shù)據(jù)時(shí)，用 IMU 實(shí)時(shí)猜（預(yù)測(cè)），誤差慢慢變大；

• 有 ODOM 數(shù)據(jù)時(shí)，用它修正猜測(cè)（更新），誤差縮小。

• 讓 IMU 當(dāng) “實(shí)時(shí)報(bào)信員”，隨時(shí)喊 “現(xiàn)在往哪動(dòng)、動(dòng)多快”；
• 讓 ODOM 當(dāng) “定期校準(zhǔn)員”，每隔一會(huì)兒喊 “實(shí)際位置在這”；
• 裁判根據(jù)兩者的靠譜程度，綜合出一個(gè)最準(zhǔn)的結(jié)果。

這個(gè)方程是擴(kuò)展卡爾曼濾波器（EKF）的核心預(yù)測(cè)方程，確實(shí)比較抽象。我來一步步解釋，讓它變得更容易理解。

狀態(tài)預(yù)測(cè)方程的直觀理解

這個(gè)方程描述了如何根據(jù)上一時(shí)刻的狀態(tài)估計(jì)和當(dāng)前的控制輸入，來預(yù)測(cè)當(dāng)前時(shí)刻的系統(tǒng)狀態(tài)。

我們可以用一個(gè)生活中的例子來類比：

上一時(shí)刻的狀態(tài)：就像你現(xiàn)在知道自己的位置和速度

數(shù)學(xué)符號(hào)詳細(xì)解釋

讓我們拆解這個(gè)方程：

符號(hào)含義：

• x?：帶帽子的 x 表示 "估計(jì)值"（不是真實(shí)值，因?yàn)橛姓`差）

• k|k-1：豎線后面的數(shù)字表示 "基于哪個(gè)時(shí)刻的信息"

• x?_k|k-1

  表示：基于 k-1 時(shí)刻的信息，對(duì) k 時(shí)刻狀態(tài)的預(yù)測(cè)

• x?_k-1|k-1

  表示：基于 k-1 時(shí)刻的信息，對(duì) k-1 時(shí)刻狀態(tài)的估計(jì)（這是上一輪更新得到的結(jié)果）

• f()：這是一個(gè)函數(shù)，表示狀態(tài)如何從一個(gè)時(shí)刻轉(zhuǎn)移到下一個(gè)時(shí)刻

• u_k：控制輸入，例如 IMU 測(cè)量的加速度和角速度

• w_k：過程噪聲，表示系統(tǒng)的不確定性（例如，你的動(dòng)作可能不完全按照計(jì)劃執(zhí)行）

更具體的例子：移動(dòng)機(jī)器人

假設(shè)我們有一個(gè)在平面上移動(dòng)的機(jī)器人，它的狀態(tài)包括：

• 位置：x, y

• 朝向：θ（角度）
• 速度：v_x, v_y（x 和 y 方向的速度）

總結(jié)

狀態(tài)預(yù)測(cè)方程x?_k|k-1 = f(x?_k-1|k-1, u_k, w_k)的核心思想是：

• 基于上一時(shí)刻的狀態(tài)估計(jì)

• 加上控制輸入的影響
  
• 考慮系統(tǒng)的不確定性
  
• 預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)
  

1. 矩陣結(jié)構(gòu)與狀態(tài)對(duì)應(yīng)

矩陣的行和列嚴(yán)格對(duì)應(yīng) EKF 狀態(tài)向量的順序，通常是：
[x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]

每個(gè)元素 process_noise_covariance[i][j] 的含義：

• 當(dāng) i=j（對(duì)角線元素）：表示單個(gè)狀態(tài)量的預(yù)測(cè)噪聲強(qiáng)度（不確定性大小）。
• 當(dāng) i≠j（非對(duì)角線元素）：表示兩個(gè)狀態(tài)量之間的噪聲相關(guān)性（通常設(shè)為 0，假設(shè)各狀態(tài)噪聲獨(dú)立）。

2. 逐行 / 列解讀（以你的配置為例）

下面結(jié)合常見機(jī)器人運(yùn)動(dòng)場(chǎng)景（如差速底盤、機(jī)械臂），解釋對(duì)角線元素（非對(duì)角線多為 0，代表噪聲獨(dú)立）的物理意義：

3. 如何調(diào)參？核心邏輯

EKF 的本質(zhì)是 **“信任模型預(yù)測(cè)” vs “信任傳感器觀測(cè)”** 的平衡，process_noise_covariance 控制對(duì) “模型預(yù)測(cè)” 的信任度：

• 對(duì)角線值越小

  ：認(rèn)為 “模型預(yù)測(cè)的狀態(tài)越準(zhǔn)”，濾波會(huì)更依賴預(yù)測(cè)（適合模型精確的場(chǎng)景，比如高精度機(jī)械臂）。

• 對(duì)角線值越大

  ：認(rèn)為 “模型預(yù)測(cè)的不確定性越高”，濾波會(huì)更依賴傳感器觀測(cè)（適合干擾多、模型粗糙的場(chǎng)景，比如地面機(jī)器人打滑）。

一句話總結(jié)

process_noise_covariance 是 EKF 的 “模型信任度旋鈕”—— 值越小，越相信模型預(yù)測(cè)；值越大，越依賴傳感器。調(diào)參時(shí)，先保持默認(rèn)值跑場(chǎng)景，遇到 “收斂慢”（比如轉(zhuǎn)向后角度回不來）就增大對(duì)應(yīng)維度的噪聲，遇到 “太敏感”（比如傳感器一飄就跟丟）就減小噪聲，反復(fù)迭代即可。