簡(jiǎn)介這份資源是《傳感器信息融合——MATLAB程序?qū)崿F(xiàn)》的隨書(shū)程序包面向?qū)W習(xí)多傳感器數(shù)據(jù)融合的本科生、研究生及工程技術(shù)人員幫助讀者把卡爾曼濾波、粒子濾波、貝葉斯估計(jì)等理論落到可運(yùn)行的代碼上。壓縮包共40個(gè)文件以34個(gè)m腳本為主體覆蓋數(shù)據(jù)預(yù)處理、傳感器建模、融合算法實(shí)現(xiàn)與結(jié)果評(píng)估等環(huán)節(jié)另含jpg、tif、bmp圖像素材用于圖像融合實(shí)驗(yàn)的輸入與對(duì)比展示整體約443KB輕量便于本地調(diào)試。目前已有3458人學(xué)習(xí)下載說(shuō)明其在教學(xué)與自學(xué)場(chǎng)景中具備一定參考價(jià)值。讀者可借助這些腳本理解Sobel邊緣檢測(cè)、小波分解、區(qū)域方差匹配等具體實(shí)現(xiàn)思路對(duì)照隨書(shū)章節(jié)復(fù)現(xiàn)實(shí)驗(yàn)流程并在此基礎(chǔ)上調(diào)整參數(shù)、替換數(shù)據(jù)集遷移到自動(dòng)駕駛、無(wú)人機(jī)導(dǎo)航或工業(yè)自動(dòng)化等實(shí)際項(xiàng)目中。1. 從一堆跳變的讀數(shù)說(shuō)起這套 MATLAB 傳感器融合程序到底能干什么做過(guò)嵌入式或者導(dǎo)航算法的朋友大概都遇到過(guò)這種場(chǎng)景加速度計(jì)單獨(dú)看還行陀螺儀積分一會(huì)兒就飄磁力計(jì)在電機(jī)旁邊直接發(fā)瘋GPS 在隧道里丟星。你手里有三四個(gè)傳感器每個(gè)都在輸出數(shù)據(jù)但每個(gè)都不可信。這時(shí)候真正要解決的問(wèn)題不是「怎么再買(mǎi)一個(gè)更貴的傳感器」而是「怎么把這幾路互相矛盾的數(shù)據(jù)揉成一個(gè)能用的估計(jì)值」。傳感器信息融合要干的就是這件事而這套 MATLAB 程序?qū)崿F(xiàn)就是把這套理論從公式變成能跑、能改、能看結(jié)果的代碼。它適合三類人正在做畢設(shè)或課程設(shè)計(jì)、需要一套能跑通的融合框架的學(xué)生剛轉(zhuǎn)做導(dǎo)航、姿態(tài)估計(jì)、組合導(dǎo)航想先拿仿真把卡爾曼濾波吃透的工程師以及手上有真實(shí)傳感器數(shù)據(jù)、想先用 MATLAB 驗(yàn)證算法再移植到嵌入式的開(kāi)發(fā)者。整套程序圍繞 MATLAB 展開(kāi)核心是濾波與狀態(tài)估計(jì)不依賴特定硬件你拿自己的數(shù)據(jù)也能替換進(jìn)去跑。2. 融合的骨架狀態(tài)方程、觀測(cè)方程與卡爾曼濾波的落地寫(xiě)法2.1 為什么選卡爾曼濾波而不是直接加權(quán)平均很多人第一反應(yīng)是「加權(quán)平均不就完了」給陀螺儀 0.7、加速度計(jì) 0.3湊一個(gè)輸出。這種做法在靜態(tài)、線性、噪聲穩(wěn)定的場(chǎng)景下確實(shí)能糊弄過(guò)去但只要傳感器特性一變、或者系統(tǒng)本身有動(dòng)態(tài)過(guò)程權(quán)重就沒(méi)法自適應(yīng)了?？柭鼮V波的價(jià)值在于它把「系統(tǒng)怎么演化」和「?jìng)鞲衅髟趺从^測(cè)」分開(kāi)建模用協(xié)方差矩陣動(dòng)態(tài)地決定每一時(shí)刻該信誰(shuí)多一點(diǎn)。陀螺儀短期準(zhǔn)、長(zhǎng)期飄加速度計(jì)長(zhǎng)期穩(wěn)、短期抖卡爾曼濾波恰好能利用這個(gè)互補(bǔ)特性短期信陀螺長(zhǎng)期用加速度計(jì)把漂移拉回來(lái)。這套程序里狀態(tài)量通常取姿態(tài)角或四元數(shù)加陀螺零偏觀測(cè)來(lái)自加速度計(jì)解算的傾角和磁力計(jì)解算的航向。狀態(tài)方程描述的是「上一時(shí)刻姿態(tài) 陀螺積分 - 零偏影響」觀測(cè)方程描述的是「當(dāng)前姿態(tài)投影到重力/地磁方向應(yīng)該得到什么讀數(shù)」。理解這兩條方程比背卡爾曼五個(gè)公式重要得多。2.2 狀態(tài)方程與觀測(cè)方程的代碼骨架下面這段是典型的離散卡爾曼預(yù)測(cè)與更新骨架姿態(tài)用四元數(shù)表示狀態(tài)向量取 7 維四元數(shù) 4 零偏 3。實(shí)際程序里矩陣維度會(huì)按你的傳感器數(shù)量調(diào)整但結(jié)構(gòu)一致。% 狀態(tài): x [q0 q1 q2 q3 bx by bz] 四元數(shù) 陀螺零偏 % 預(yù)測(cè)步: 用陀螺角速度推進(jìn)四元數(shù) function x_pred predict_state(x, omega, dt) q x(1:4); b x(5:7); w omega - b; % 去掉零偏后的真實(shí)角速度 % 四元數(shù)微分: q_dot 0.5 * q ? [0 w] Omega [0, -w; w, -skew(w)]; q_dot 0.5 * Omega * q; q_pred q q_dot * dt; q_pred q_pred / norm(q_pred); % 必須歸一化否則會(huì)漂 x_pred [q_pred; b]; % 零偏按隨機(jī)游走建模預(yù)測(cè)不變 end % 更新步: 用加速度計(jì)觀測(cè)重力方向做校正 function [x_upd, P_upd] update_acc(x_pred, P_pred, acc, R) g_ref [0; 0; 1]; % 參考重力方向(導(dǎo)航系) q x_pred(1:4); g_pred quat2dcm(q) * g_ref; % 預(yù)測(cè)的機(jī)體系重力方向 y acc / norm(acc) - g_pred; % 觀測(cè)殘差 H jacobian_acc(x_pred); % 觀測(cè)矩陣對(duì)四元數(shù)求偏導(dǎo) S H * P_pred * H R; K P_pred * H / S; % 卡爾曼增益 x_upd x_pred K * y; x_upd(1:4) x_upd(1:4) / norm(x_upd(1:4)); P_upd (eye(7) - K * H) * P_pred; end邏輯上分兩步predict_state負(fù)責(zé)用陀螺把狀態(tài)往前推update_acc負(fù)責(zé)用加速度計(jì)把預(yù)測(cè)拉回真實(shí)。參數(shù)里dt是采樣周期必須和你的傳感器實(shí)際輸出頻率一致寫(xiě)錯(cuò)這個(gè)值結(jié)果會(huì)整體偏慢或偏快R是觀測(cè)噪聲協(xié)方差加速度計(jì)越抖R對(duì)角線就該給越大濾波才會(huì)更信陀螺。skew(w)是反對(duì)稱矩陣函數(shù)quat2dcm是四元數(shù)轉(zhuǎn)方向余弦矩陣MATLAB 的 Aerospace Toolbox 里有現(xiàn)成的沒(méi)有的話自己寫(xiě)十幾行也能實(shí)現(xiàn)。2.3 觀測(cè)噪聲 R 與過(guò)程噪聲 Q 的整定方法新手最容易翻車(chē)的地方就是 Q 和 R 拍腦袋填。我的習(xí)慣是先做靜態(tài)實(shí)驗(yàn)傳感器靜止放十分鐘采集加速度計(jì)和陀螺的輸出算各自的方差。加速度計(jì)方差直接填進(jìn) R 的對(duì)角線陀螺的隨機(jī)游走方差填進(jìn) Q 里零偏對(duì)應(yīng)的位置。這樣起步至少不會(huì)離譜。然后做動(dòng)態(tài)調(diào)參如果融合后的姿態(tài)在靜止時(shí)還在緩慢畫(huà)圈說(shuō)明陀螺零偏沒(méi)被觀測(cè)約束住把 Q 里零偏項(xiàng)調(diào)小、或者確認(rèn)磁力計(jì)/加速度計(jì)觀測(cè)真的在起作用如果動(dòng)態(tài)時(shí)響應(yīng)明顯滯后說(shuō)明 R 給太大、太信陀螺了適當(dāng)減小 R。這個(gè)過(guò)程沒(méi)有解析解就是反復(fù)跑數(shù)據(jù)看曲線屬于典型的玄學(xué)調(diào)參但有了靜態(tài)方差做起點(diǎn)能少走很多彎路。3. 多傳感器接入加速度計(jì)、陀螺、磁力計(jì)的互補(bǔ)與沖突處理3.1 三路傳感器的角色分工單靠陀螺只能得到相對(duì)姿態(tài)時(shí)間一長(zhǎng)必然漂加速度計(jì)能提供絕對(duì)的水平參考重力方向但一有運(yùn)動(dòng)加速度就被污染磁力計(jì)能提供絕對(duì)的航向參考但周?chē)需F磁物質(zhì)或電流就會(huì)偏。三者融合的本質(zhì)是用加速度計(jì)和磁力計(jì)提供長(zhǎng)期絕對(duì)基準(zhǔn)用陀螺提供短期高動(dòng)態(tài)響應(yīng)。程序里通常把加速度計(jì)和磁力計(jì)分別做觀測(cè)更新而不是合成一個(gè)觀測(cè)。這樣做的好處是某一路失效時(shí)可以單獨(dú)關(guān)掉比如電機(jī)干擾大時(shí)先禁用磁力計(jì)只靠加速度計(jì)維持水平姿態(tài)航向暫時(shí)交給陀螺短時(shí)維持。3.2 磁力計(jì)橢球校正與航向解算磁力計(jì)原始輸出往往不是標(biāo)準(zhǔn)球面分布而是被硬鐵和軟鐵干擾拉成橢球。直接拿原始值算航向誤差能到幾十度。常見(jiàn)做法是先做橢球擬合校正% 采集多組姿態(tài)下的磁力計(jì)數(shù)據(jù) mag (N x 3) % 擬合橢球: (m - c) * A * (m - c) 1 % 簡(jiǎn)化做法: 用最小二乘擬合偏移和縮放 function [mag_cal] calib_mag(mag_raw) c mean(mag_raw); % 粗略取均值作為硬鐵偏移 m_centered mag_raw - c; scale max(abs(m_centered)); % 各軸縮放因子 mag_cal m_centered ./ scale; end % 用校正后的磁力計(jì)和加速度計(jì)算航向 function yaw compute_yaw(mag_cal, acc) % 先用加速度計(jì)把磁力計(jì)投影到水平面 roll atan2(acc(2), acc(3)); pitch atan2(-acc(1), sqrt(acc(2)^2 acc(3)^2)); mx mag_cal(1)*cos(pitch) mag_cal(2)*sin(roll)*sin(pitch) ... mag_cal(3)*cos(roll)*sin(pitch); my mag_cal(2)*cos(roll) - mag_cal(3)*sin(roll); yaw atan2(-my, mx); endcalib_mag里用均值代替硬鐵偏移是最省事的近似精度要求高時(shí)應(yīng)該用橢球擬合求偏移和變換矩陣。compute_yaw先把磁力計(jì)投影到水平面再算航向這一步不能省否則傾斜時(shí)航向會(huì)跟著姿態(tài)一起變。參數(shù)上scale取各軸最大絕對(duì)值是為了把橢球拉回球面如果你的磁力計(jì)三軸靈敏度差異大這里要換成逐軸縮放。3.3 觀測(cè)失效時(shí)的降級(jí)策略真實(shí)系統(tǒng)里傳感器會(huì)掉線、會(huì)飽和、會(huì)被干擾。程序里應(yīng)該給每路觀測(cè)加一個(gè)有效性判斷加速度計(jì)模值偏離 1g 太多就認(rèn)為有運(yùn)動(dòng)加速度暫時(shí)不更新磁力計(jì)模值突變就認(rèn)為受干擾跳過(guò)這次航向校正。降級(jí)后系統(tǒng)退化成純陀螺積分短時(shí)間還能用等觀測(cè)恢復(fù)再重新校正。這個(gè)邏輯不寫(xiě)融合結(jié)果在干擾出現(xiàn)時(shí)會(huì)直接被帶偏而且很難查。4. 避坑與排查融合結(jié)果不對(duì)時(shí)先看這幾處4.1 姿態(tài)緩慢畫(huà)圈或持續(xù)漂移現(xiàn)象靜止放置時(shí)融合輸出的航向或俯仰角以固定速率緩慢變化幾分鐘就偏出十幾度。原因通常是陀螺零偏沒(méi)有被有效觀測(cè)或者觀測(cè)更新根本沒(méi)生效。解決先確認(rèn)加速度計(jì)和磁力計(jì)的更新函數(shù)確實(shí)被調(diào)用再檢查 Q 矩陣?yán)锪闫珜?duì)應(yīng)的方差是不是給得太小導(dǎo)致濾波器認(rèn)為零偏不會(huì)變、不去估計(jì)它。把零偏項(xiàng)方差調(diào)大一個(gè)量級(jí)通常就能把漂移壓住。4.2 動(dòng)態(tài)響應(yīng)明顯滯后現(xiàn)象快速轉(zhuǎn)動(dòng)傳感器時(shí)融合輸出跟不上真實(shí)姿態(tài)松手后還要過(guò)一會(huì)兒才慢慢歸位。原因是 R 給得太大濾波器過(guò)度信任陀螺的預(yù)測(cè)、不敢用觀測(cè)去修正。解決減小加速度計(jì)和磁力計(jì)的觀測(cè)噪聲 R或者檢查觀測(cè)殘差y是不是被錯(cuò)誤地做了限幅。限幅本意是抗野值但閾值設(shè)太小會(huì)把正常動(dòng)態(tài)也砍掉。4.3 四元數(shù)不歸一化導(dǎo)致數(shù)值發(fā)散現(xiàn)象跑一段時(shí)間后姿態(tài)輸出突然變成 NaN 或者亂跳。原因是四元數(shù)在迭代中累積了數(shù)值誤差模長(zhǎng)偏離 1再繼續(xù)運(yùn)算就會(huì)放大。解決每次預(yù)測(cè)和更新之后都強(qiáng)制歸一化就是代碼里那句q q / norm(q)。這一步看著不起眼但漏掉它長(zhǎng)時(shí)間跑必翻車(chē)。4.4 采樣率不一致導(dǎo)致 dt 用錯(cuò)現(xiàn)象融合結(jié)果整體偏快或偏慢和真實(shí)運(yùn)動(dòng)對(duì)不上。原因是陀螺、加速度計(jì)、磁力計(jì)的更新頻率不同但代碼里用了同一個(gè)dt。解決給每路觀測(cè)單獨(dú)記錄時(shí)間戳預(yù)測(cè)步用陀螺的dt觀測(cè)更新時(shí)用「距上次更新的實(shí)際時(shí)間差」。如果嫌麻煩至少保證所有傳感器同步采樣或者把高頻數(shù)據(jù)降采樣到統(tǒng)一頻率再進(jìn)濾波器。4.5 坐標(biāo)系定義混亂現(xiàn)象某個(gè)軸的方向反了或者姿態(tài)整體旋轉(zhuǎn)了 90 度。原因是導(dǎo)航系和機(jī)體系的定義、傳感器安裝方向、四元數(shù)旋轉(zhuǎn)方向三者沒(méi)對(duì)齊。解決拿一個(gè)已知姿態(tài)做驗(yàn)證比如水平靜止時(shí)加速度計(jì)應(yīng)該輸出[0 0 1]或[0 0 -1]取決于你的約定磁力計(jì)指向北時(shí)航向應(yīng)該為 0。把這三個(gè)基準(zhǔn)點(diǎn)對(duì)齊坐標(biāo)系問(wèn)題基本就定位了。5. 從仿真到實(shí)測(cè)用真實(shí)數(shù)據(jù)驗(yàn)證融合效果的一個(gè)笨辦法程序跑通仿真只是第一步真正檢驗(yàn)它的是你自己的數(shù)據(jù)。我一般會(huì)用一個(gè)很笨但很有效的辦法把傳感器固定在一個(gè)能手動(dòng)復(fù)現(xiàn)的動(dòng)作上比如先靜止三十秒再繞某個(gè)軸勻速轉(zhuǎn)九十度停住再轉(zhuǎn)回來(lái)。全程錄數(shù)據(jù)然后拿融合輸出和這個(gè)已知?jiǎng)幼鲗?duì)比。具體操作上先把錄到的數(shù)據(jù)存成 MAT 文件字段名和程序里讀取的變量對(duì)齊% 假設(shè)錄到的數(shù)據(jù): t(時(shí)間), gyr(陀螺 rad/s), acc(g), mag(uT) load(my_sensor_log.mat); dt mean(diff(t)); % 實(shí)際平均采樣周期 N length(t); x_est zeros(7, N); % 存融合結(jié)果 x_est(:,1) [1;0;0;0;0;0;0]; % 初始四元數(shù) P eye(7) * 0.1; % 初始協(xié)方差 for k 2:N x_pred predict_state(x_est(:,k-1), gyr(k,:), dt); P_pred P Q * dt; % 簡(jiǎn)化過(guò)程噪聲傳播 if norm(acc(k,:)) 0.8 norm(acc(k,:)) 1.2 [x_pred, P_pred] update_acc(x_pred, P_pred, acc(k,:), R_acc); end if norm(mag(k,:)) 20 norm(mag(k,:)) 80 [x_pred, P_pred] update_mag(x_pred, P_pred, mag(k,:), R_mag); end x_est(:,k) x_pred; P P_pred; end % 把四元數(shù)轉(zhuǎn)成歐拉角看曲線 eul quat2eul(x_est(1:4,:), ZYX) * 180/pi; plot(t, eul); legend(Yaw,Pitch,Roll);這段代碼的關(guān)鍵在有效性判斷加速度計(jì)模值在 0.8g 到 1.2g 之間才更新磁力計(jì)模值在合理區(qū)間才更新超出范圍就跳過(guò)。參數(shù)Q和R_acc、R_mag用前面靜態(tài)實(shí)驗(yàn)整定出來(lái)的值。跑完把歐拉角曲線和你的已知?jiǎng)幼鲗?duì)照如果靜止段平直、轉(zhuǎn)動(dòng)段跟得上、回位后不漂說(shuō)明融合基本可用如果靜止段還在緩慢變化回到第 4 章排查零偏和觀測(cè)。驗(yàn)證通過(guò)之后再把這套邏輯往嵌入式移植就心里有底了。MATLAB 里能跑通不代表 C 里能跑通定點(diǎn)數(shù)、浮點(diǎn)精度、實(shí)時(shí)性都是新問(wèn)題但至少算法層面你已經(jīng)確認(rèn)它是對(duì)的。從那以后我每次拿到新的傳感器數(shù)據(jù)都強(qiáng)制先跑一遍這個(gè)「靜止—轉(zhuǎn)動(dòng)—回位」的驗(yàn)證流程不看到曲線對(duì)得上絕不往下做。希望這套程序能幫你把融合這件事從公式變成手上能跑的東西幫到你。本文還有配套的精品資源點(diǎn)擊獲取