簡介面向嵌入式、機器人與無人機方向的姿態(tài)解算學(xué)習(xí)者這份MATLAB工程圍繞九軸傳感器三軸陀螺儀、加速度計與磁力計的數(shù)據(jù)融合展開覆蓋陀螺儀角速度、線性加速度和地磁觀測的校準、濾波與姿態(tài)輸出適合已具備一定MATLAB基礎(chǔ)、希望把互補濾波、Mahony、擴展卡爾曼濾波等方法落實到代碼中的讀者。壓縮包共12個文件以10個.m腳本/函數(shù)為主體另有1個.mat數(shù)據(jù)文件和1個.asv自動備份文件整體約555KB便于直接運行、閱讀與二次修改。已有5071人學(xué)習(xí)說明其在慣導(dǎo)定位與姿態(tài)解算入門實踐中具有一定參考價值。資源按核心算法、傳感器校準、數(shù)據(jù)處理與演示腳本組織包含低通/高通濾波、磁力計到加速度坐標系轉(zhuǎn)換、歐拉角與四元數(shù)結(jié)果查看可幫助讀者理解從原始IMU數(shù)據(jù)到俯仰、偏航、翻滾角輸出的完整鏈路并據(jù)此搭建自己的實驗流程。1. 九軸姿態(tài)解算的真實起點靜止十分鐘航向漂了三十度前陣子幫人看一臺田間小車的日志機器停在原地十分鐘串口打印的航向角從 32° 一路爬到 61°一動起來 roll 和 pitch 又開始抽。傳感器沒壞問題出在從原始采樣到姿態(tài)四元數(shù)這條鏈路上標定沒做干凈、磁力計沒做傾角補償、陀螺零偏被當成真值一路積分。這份基于 MATLAB 的九軸傳感器姿態(tài)解算工程把這套鏈路完整攤開——加速度計、陀螺儀、磁力計三路數(shù)據(jù)怎么對齊零偏和橢球誤差怎么標四元數(shù)怎么積分Mahony 和 Madgwick 兩種互補濾波怎么選姿態(tài)穩(wěn)定后怎么接慣導(dǎo)定位推速度和位置。適合正在調(diào) IMU 模塊、做云臺/小車/機械臂末端姿態(tài)或者準備把 MATLAB 算法往嵌入式搬的開發(fā)者。2. 工程骨架與數(shù)據(jù)流從原始采樣到統(tǒng)一時間軸拿到一個姿態(tài)解算工程先別急著看濾波公式。九軸數(shù)據(jù)最容易翻車的地方不在算法本身而在“三路傳感器的時間戳對不上、坐標軸定義各說各話”。這份工程把 IO、標定、AHRS、INS、評估拆成五層每一層只做一件事改起來不會牽一發(fā)動全身。2.1 目錄結(jié)構(gòu)與腳本職責(zé)工程按功能分層而不是按“第幾章第幾節(jié)”平鋪這樣二次開發(fā)時替換某一層不會影響其他模塊nine_axis_ahrs/ ├── data/ │ ├── calib_static.mat % 靜止采集用于陀螺零偏 │ ├── calib_six_face.mat % 六面采集用于加速度計標定 │ ├── calib_mag_rotate.mat % 繞三軸慢速旋轉(zhuǎn)用于磁橢球擬合 │ └── run_dynamic.mat % 動態(tài)跑車/搖擺數(shù)據(jù) ├── src/ │ ├── io/ │ │ └── load_dataset.m % 統(tǒng)一讀入、單位換算、時間對齊 │ ├── calib/ │ │ ├── gyro_bias_calib.m │ │ ├── acc_six_face_calib.m │ │ └── mag_ellipsoid_calib.m │ ├── ahrs/ │ │ ├── quat_multiply.m │ │ ├── quat_to_dcm.m │ │ ├── dcm_to_euler.m │ │ ├── mahony_update.m │ │ └── madgwick_update.m │ ├── ins/ │ │ ├── ins_mechanize.m % 比力變換、重力補償、積分 │ │ └── zupt_detect.m % 零速檢測 │ └── eval/ │ ├── plot_attitude.m │ └── plot_trajectory.m ├── run_all.m % 一鍵跑通標定→解算→畫圖 └── README.mddata里放的是采集好的.mat不依賴硬件就能復(fù)現(xiàn)calib下的三個腳本各自獨立可以單獨拿出來給別的項目用ahrs只負責(zé)“給定角速度和比力輸出四元數(shù)”不碰文件讀寫ins拿四元數(shù)做機械編排eval只畫圖不改數(shù)據(jù)。這種切法的好處是你要換 Madgwick 為 EKF只動ahrs一個目錄run_all.m的調(diào)用方式不變。run_all.m我一般寫成顯式流水線而不是一堆函數(shù)互相調(diào)用方便打斷點% run_all.m % 1) 讀數(shù)據(jù) D load_dataset(data/run_dynamic.mat); % 2) 標定參數(shù)首次運行會從 calib_*.mat 里算出來并緩存 P load(data/calib_params.mat); % 3) 應(yīng)用標定 gyro (D.gyro - P.gyro_bias) * diag(P.gyro_scale); acc (D.acc - P.acc_bias) * diag(P.acc_scale); mag (D.mag - P.mag_center) * P.mag_W; % 4) 逐采樣遞推 q [1;0;0;0]; for k 2:numel(D.t) dt D.t(k) - D.t(k-1); q mahony_update(q, gyro(k,:), acc(k,:), mag(k,:), dt, 0.5, 0.02); eul(k,:) dcm_to_euler(quat_to_dcm(q)); end % 5) 畫圖 plot_attitude(D.t, eul);這里的mahony_update入?yún)㈨樞蚴?q, gyro, acc, mag, dt, Kp, Ki)Kp取 0.5、Ki取 0.02 是常見起步值后面第 4 章會解釋這兩個數(shù)怎么調(diào)。dt不要用固定 1/fs用相鄰時間戳相減采集卡丟包時能自動吸收誤差。2.2 數(shù)據(jù)格式與時間對齊九軸模塊常見的輸出是陀螺和加速度計 100~1000 Hz磁力計 10~100 Hz。如果直接把三路按行號一一對應(yīng)等于假設(shè)它們同時采樣磁力計那一行會整體滯后幾十毫秒航向在快速轉(zhuǎn)動時明顯拖尾。工程里約定每個數(shù)據(jù)集至少包含以下字段字段維度單位說明t_gyroN×1s陀螺時間戳作為主時間軸gyroN×3rad/s機體坐標系角速度accN×3m/s2比力含重力t_magM×1s磁力計時間戳magM×3μT機體坐標系磁場fs標量Hz標稱采樣率僅作參考對齊用interp1把磁力計插到陀螺時間軸上注意外推標志function D align_streams(D) % 以陀螺時間軸為主軸把磁力計線性插值過來 if isfield(D, t_mag) numel(D.t_mag) ~ numel(D.t_gyro) D.mag interp1(D.t_mag, D.mag, D.t_gyro, linear, extrap); end D.t D.t_gyro; % 去掉首尾各 1% 的數(shù)據(jù)避免插值邊界抖動 n numel(D.t); idx round(0.01*n)1 : n-round(0.01*n); D.t D.t(idx); D.gyro D.gyro(idx,:); D.acc D.acc(idx,:); D.mag D.mag(idx,:); endlinear對磁力計足夠因為磁場變化本來就慢extrap是為了防止t_mag比t_gyro短一截時首尾出現(xiàn)NaN但外推出來的點不可信所以后面裁掉首尾 1%。如果磁力計采樣率低于 10 Hz線性插值的誤差會變大這時改用零階保持previous更穩(wěn)代價是航向更新有臺階。還有一個容易忽略的點單位。不同模塊陀螺輸出可能是deg/s磁力計可能是gauss讀入時必須統(tǒng)一到rad/s和μT否則Kp調(diào)到頭也壓不住漂移。load_dataset.m里我習(xí)慣加一段顯式檢查assert(max(abs(D.gyro(:))) 20, 陀螺單位疑似 deg/s請先轉(zhuǎn) rad/s); assert(mean(vecnorm(D.mag,2,2)) 5 mean(vecnorm(D.mag,2,2)) 200, ... 磁力計模值異常檢查單位是否為 uT);2.3 坐標系約定與旋轉(zhuǎn)順序姿態(tài)解算里一半的“玄學(xué)”來自坐標系沒對齊。工程統(tǒng)一采用航空常用的約定并在 README 第一行寫死符號含義約定b 系機體坐標系x 向前y 向右z 向下FRDn 系導(dǎo)航坐標系北-東-地NEDq姿態(tài)四元數(shù)[w;x;y;z]表示 b 系到 n 系的旋轉(zhuǎn)歐拉角ZYX 順序先繞 z 轉(zhuǎn) yaw再繞 y 轉(zhuǎn) pitch最后繞 x 轉(zhuǎn) rollg重力加速度9.81 m/s2NED 下沿 z在這個約定下靜止時加速度計輸出約[0 0 -9.81]比力方向向上與 z 軸反向磁力計在水平放置時北向分量為正。如果拿到的模塊是“前右下”之外的軸向比如 x 向右、y 向前必須在load_dataset里做一次軸交換而不是等到濾波里再補否則后面所有叉積的符號都是錯的。歐拉角用 ZYX 順序反解時pitch在 ±90° 附近會出現(xiàn)萬向鎖atan2的分母趨近于零角度會跳變。這不是算法錯誤是歐拉角表示本身的奇點第 5 章會講怎么在畫圖和輸出時繞開它。四元數(shù)全程參與運算只在最后展示時轉(zhuǎn)歐拉角這樣積分過程不受奇點影響。3. 標定與預(yù)處理零偏、六面校準與磁橢球擬合很多人拿到模塊直接跑濾波看到曲線“大概跟著動”就認為成了結(jié)果航向一小時漂幾十度。九軸姿態(tài)解算的精度上限由標定決定濾波只負責(zé)把剩下的誤差壓平。這一章三個標定各管一段陀螺零偏管短期漂移加速度計六面管傾角基準磁橢球管航向絕對參考。3.1 陀螺零偏與溫度漂移陀螺靜止時輸出不是零而是一個幾十到幾百deg/h的偏置直接積分就是航向漂移。最直接的做法是開機靜止采一段求均值function [bias, sigma] gyro_bias_calib(gyro, fs, sec) % gyro: N×3 rad/s % sec : 靜止采集時長建議 30~60 s n min(round(sec*fs), size(gyro,1)); seg gyro(1:n, :); bias mean(seg, 1); sigma std(seg, 0, 1); % 用 3σ 剔除野點后重算避免開機瞬間的沖擊 keep all(abs(seg - bias) 3*sigma, 2); bias mean(seg(keep,:), 1); fprintf(gyro bias (rad/s): %.5f %.5f %.5f\n, bias); fprintf(gyro noise std : %.5f %.5f %.5f\n, sigma); endsec取 30 秒是經(jīng)驗值太短均值不穩(wěn)太長用戶等不及。sigma不是拿來好看的它是后面 Mahony 的Ki和 ZUPT 閾值的參考——噪聲標準差乘以采樣間隔就是單步積分的不確定度。如果sigma大于 0.01 rad/s先檢查是不是采集時桌子在抖或者模塊沒固定。零偏會隨溫度變冷機開機和跑十分鐘后的零偏能差 20% 以上。工程里沒有做完整溫補但留了一個線性模型接口% 若數(shù)據(jù)里帶溫度字段 D.temp攝氏度可以做一階擬合 p polyfit(D.temp(static_idx), gyro_static(:,1), 1); bias_x polyval(p, D.temp(k));我一般會在run_all.m里把冷機靜止段和熱機靜止段各存一份對比零偏差異超過 30% 就在 README 里注明“建議預(yù)熱 5 分鐘”。這是血淚經(jīng)驗不寫這句話下一個用工程的人一定會在冷機狀態(tài)下調(diào)參數(shù)然后懷疑算法有問題。3.2 加速度計六面標定加速度計的誤差模型是raw S * a_true bS是 3×3 的尺度加軸間耦合b是零偏。完整解需要十二面甚至更多姿態(tài)但工程里用六面法已經(jīng)能消掉主要誤差把模塊分別朝 x、-x、y、-y、z、-z 六個方向靜止放置每個方向采幾秒取均值。function [bias, scale] acc_six_face_calib(acc_faces, g) % acc_faces: 6×3行順序為 x,-x,y,-y,z,-z 的平均讀數(shù) % g: 當?shù)刂亓铀俣?.81 bias zeros(3,1); scale zeros(3,1); for i 1:3 pos acc_faces(2*i-1, i); % 該軸指向下時的讀數(shù) neg acc_faces(2*i, i); % 該軸指向上時的讀數(shù) % 理想情況 pos g, neg -g scale(i) 2*g / (pos - neg); bias(i) (pos neg) / 2; end % 校正a_cal scale .* (raw - bias) end這里假設(shè)六個面放置時敏感軸嚴格對準重力方向?qū)嶋H用手擺會有幾度偏差scale會有 1%~2% 的誤差。對姿態(tài)解算來說夠用因為加速度計只用來修正 roll 和 pitch 的長期漂移不參與高頻積分。bias的單位是 m/s2scale無量綱校正時逐軸相乘。如果模塊焊在板子上、板子又裝在結(jié)構(gòu)件里六個面很難擺正可以退而求其次只做零偏校正把模塊水平靜置取bias mean(acc) - [0 0 -g]。代價是傾角在±30° 以外會有幾度誤差適合對精度要求不高的場景。3.3 磁力計橢球擬合與硬磁/軟磁補償磁力計是航向的唯一絕對參考但它也是最容易被干擾的一路。硬磁干擾喇叭磁鐵、電機永磁體讓球心偏移軟磁干擾鐵質(zhì)結(jié)構(gòu)件讓球變成橢球。未標定的磁力計轉(zhuǎn)一圈模值可能從 20 μT 變到 80 μT航向自然亂跳。工程里用九參數(shù)橢球擬合。把磁力計繞三個軸各轉(zhuǎn)幾圈采到的點滿足一般二次曲面function [center, W] mag_ellipsoid_calib(mag) % mag: N×3單位 μT需覆蓋各個朝向 mx mag(:,1); my mag(:,2); mz mag(:,3); % 設(shè)計矩陣Ax2By2Cz22Dxy2Exz2Fyz2Gx2Hy2Iz 1 D [mx.^2, my.^2, mz.^2, ... 2*mx.*my, 2*mx.*mz, 2*my.*mz, ... 2*mx, 2*my, 2*mz]; p D \ ones(size(mx)); % 還原二次型矩陣與一次項 M [p(1) p(4) p(5); p(4) p(2) p(6); p(5) p(6) p(3)]; d p(7:9); center -M \ d; % 橢球球心即硬磁偏移 % 軟磁把 M 歸一化到單位球 [V, E] eig(M); s sqrt(diag(E) / min(diag(E))); W V * diag(1./s) * V; % 3×3 軟磁補償矩陣 endp是九維系數(shù)M是二次型矩陣d是一次項。完成配方后球心是-M\d也就是硬磁偏移W把橢球拉回球。應(yīng)用時mag_cal (mag - center) * W; % 檢查標定效果模值標準差應(yīng)小于均值的 5% fprintf(mag norm mean%.2f, std%.2f\n, ... mean(vecnorm(mag_cal,2,2)), std(vecnorm(mag_cal,2,2)));參數(shù)說明D的列順序必須和p的物理含義一一對應(yīng)改順序會得到完全錯誤的球心eig分解要求M正定如果采集點分布太集中比如只在一個平面轉(zhuǎn)M會接近奇異W里出現(xiàn)巨大數(shù)值。判斷方法是看cond(M)大于 1e6 就重新采數(shù)據(jù)讓模塊繞三個軸都轉(zhuǎn)到。標定后的磁力計還要做傾角補償才能算航向。簡單做法是把磁場旋到水平面function yaw mag_yaw(mag_b, roll, pitch) mx mag_b(1); my mag_b(2); mz mag_b(3); hx mx*cos(pitch) my*sin(roll)*sin(pitch) mz*cos(roll)*sin(pitch); hy my*cos(roll) - mz*sin(roll); yaw atan2(-hy, hx); % NED 下北向為 0順時針為正 end這段只在靜止或低動態(tài)時可信動態(tài)下更穩(wěn)的做法是把磁力計觀測直接送進 Mahony 或 Madgwick讓濾波器自己處理權(quán)重第 4 章會給出對應(yīng)代碼。4. 姿態(tài)解算與慣導(dǎo)定位四元數(shù)積分、Mahony、重力補償與 ZUPT標定把系統(tǒng)性誤差壓下去之后剩下的就是濾波器的活。這一章從四元數(shù)微分方程講到 Mahony 和 Madgwick 兩種互補濾波再往下接慣導(dǎo)機械編排。代碼都按“能直接抄進自己的工程”來寫參數(shù)含義和調(diào)法跟在每段后面。4.1 四元數(shù)更新與陀螺積分四元數(shù)q [w;x;y;z]表示 b 系到 n 系的旋轉(zhuǎn)微分方程是q_dot 0.5 * q ? [0; ω]其中ω是機體角速度。離散化后用一階歐拉積分再歸一化function q quat_integrate(q, omega, dt) % omega: 3×1 rad/s已扣除零偏并做濾波修正 wx omega(1); wy omega(2); wz omega(3); % 四元數(shù)乘法矩陣形式q_dot 0.5 * Omega * q Omega [0 -wx -wy -wz; wx 0 wz -wy; wy -wz 0 wx; wz wy -wx 0 ]; q q 0.5 * dt * (Omega * q); q q / norm(q); % 必須歸一化否則模值漂移會污染旋轉(zhuǎn)矩陣 enddt用相鄰時間戳之差不用固定值。一階積分在dt小于 10 ms 時誤差可忽略如果采樣率只有 50 Hz快速轉(zhuǎn)動下會有明顯錐誤差這時換二階龍格庫塔或圓錐補償。歸一化那行不能省浮點累積幾十萬步之后模值會偏離 1旋轉(zhuǎn)矩陣就不再正交重力方向會慢慢歪掉。四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣用標準公式注意這里是 b 系到 n 系function R quat_to_dcm(q) q q / norm(q); w q(1); x q(2); y q(3); z q(4); R [1-2*(y^2z^2), 2*(x*y-w*z), 2*(x*zw*y); 2*(x*yw*z), 1-2*(x^2z^2), 2*(y*z-w*x); 2*(x*z-w*y), 2*(y*zw*x), 1-2*(x^2y^2)]; endR的第三行[2(xz-wy), 2(yzwx), 1-2(x2y2)]正好是 n 系重力單位向量[0;0;1]在 b 系的投影Mahony 的重力修正直接用它不用再單獨推公式。4.2 Mahony 互補濾波的 PI 修正Mahony 的思路是陀螺積分 trustworthy 在短時間加速度計和磁力計 trustworthy 在長時間。用加速度計觀測到的重力方向與四元數(shù)預(yù)測的重力方向做叉積得到誤差向量PI 控制后反饋到角速度上。function [q, b] mahony_update(q, b, gyro, acc, mag, dt, Kp, Ki) % q : 4×1 四元數(shù) % b : 3×1 積分反饋項陀螺零偏的在線估計 % acc : 3×1 比力靜止時約 [0 0 -g] % mag : 3×1 磁場可為空 R quat_to_dcm(q); % 加速度計觀測取反歸一化得到 b 系下的“重力方向” a -acc(:) / norm(acc); % 四元數(shù)預(yù)測的重力方向R 的第三行 v R(3, :); e cross(a, v); % 磁力計修正把磁場轉(zhuǎn)到 n 系去掉垂直分量只保留水平北向 if nargin 5 ~isempty(mag) m mag(:) / norm(mag); h R * m; h(3) 0; if norm(h) 1e-6 h h / norm(h); v_m R * h; % 期望磁矢量在 b 系的方向 e e cross(m, v_m); end end % PI 反饋Kp 壓高頻誤差Ki 估零偏 if Ki 0 b b - Ki * e * dt; end omega gyro(:) Kp * e b; q quat_integrate(q, omega, dt); endKp決定收斂速度取 0.5~2.0Ki決定零偏跟蹤速度取 0.01~0.05。Kp太大姿態(tài)會跟著加速度計的振動抖Ki太大航向會在磁干擾下緩慢跑偏。調(diào)參順序是先把Ki置零調(diào)Kp到靜態(tài)傾角穩(wěn)定、動態(tài)不抖再加Ki讓長時間靜止的航向不漂。b的初值給[0;0;0]它會在運行中慢慢收斂到殘余零偏。注意b的符號b b - Ki*e*dt對應(yīng) Mahony 原文的b_dot -Ki*e如果寫成加號誤差會發(fā)散而不是收斂。4.3 Madgwick 梯度下降與 β 參數(shù)Madgwick 把姿態(tài)解算寫成優(yōu)化問題找一個四元數(shù)讓預(yù)測的重力方向和磁場方向與觀測的誤差最小用梯度下降迭代再和陀螺積分融合。它的計算量比 Mahony 略大但收斂更穩(wěn)尤其是在磁干擾間歇出現(xiàn)的場景。下面是加速度計部分的梯度磁力計部分結(jié)構(gòu)相同源碼里給了完整雅可比function q madgwick_update(q, gyro, acc, mag, dt, beta) q q / norm(q); a -acc(:) / norm(acc); % 與 Mahony 一致取反后 az≈1 ax a(1); ay a(2); az a(3); w q(1); x q(2); y q(3); z q(4); % 目標函數(shù) f預(yù)測重力方向 - 觀測重力方向 f [2*(x*z - w*y) - ax; 2*(w*x y*z) - ay; 2*(0.5 - x^2 - y^2) - az]; % 雅可比 J df/dq J [-2*y, 2*z, -2*w, 2*x; 2*x, 2*w, 2*z, 2*y; 0, -4*x, -4*y, 0 ]; grad J * f; if norm(grad) 1e-9 grad grad / norm(grad); end % 陀螺積分項 - β * 梯度修正項 q_dot 0.5 * quat_multiply(q, [0; gyro(:)]) - beta * grad; q q q_dot * dt; q q / norm(q); endbeta是梯度下降步長也是濾波器的“信任分配”旋鈕beta大更信加速度計收斂快但動態(tài)噪聲大beta小更信陀螺平滑但傾角恢復(fù)慢。常用范圍 0.02~0.1對應(yīng)約 1~5 秒的收斂時間。如果采樣率是 100 Hzbeta取 0.05 左右比較穩(wěn)采樣率降到 50 Hzbeta要相應(yīng)減小否則每一步修正過沖。quat_multiply就是標準四元數(shù)乘法輸入兩個 4×1 列向量輸出也是 4×1function q quat_multiply(p, r) pwp(1); pxp(2); pyp(3); pzp(4); rwr(1); rxr(2); ryr(3); rzr(4); q [pw*rw - px*rx - py*ry - pz*rz; pw*rx px*rw py*rz - pz*ry; pw*ry - px*rz py*rw pz*rx; pw*rz px*ry - py*rx pz*rw]; end實際使用時不必自己手寫 Madgwick 的完整雅可比源碼里madgwick_update.m已經(jīng)把加速度計和磁力計兩部分拼好調(diào)用方式和 Mahony 一致方便在同一份數(shù)據(jù)上對比兩條曲線。4.4 姿態(tài)轉(zhuǎn)歐拉角與萬向鎖四元數(shù)轉(zhuǎn)歐拉角只在展示和評估時做ZYX 順序function eul dcm_to_euler(R) % 返回 [roll; pitch; yaw]單位 rad roll atan2(R(3,2), R(3,3)); pitch -asin(max(-1, min(1, R(3,1)))); % 限幅防 NaN yaw atan2(R(2,1), R(1,1)); eul [roll; pitch; yaw]; endasin外面套max/min是防止數(shù)值誤差讓R(3,1)略超 ±1 導(dǎo)致NaN。pitch接近 ±90° 時roll和yaw的分母同時趨近零兩個角會互相“搶”畫出來就是跳變。這不是解算錯了是歐拉角在這個姿態(tài)下不唯一。評估時我一般把pitch限制在 ±85° 以內(nèi)做測試或者在圖上同時畫四元數(shù)分量避免被歐拉角跳變誤導(dǎo)。4.5 比力變換、重力補償與 ZUPT姿態(tài)穩(wěn)了之后慣導(dǎo)定位的鏈路是把機體比力轉(zhuǎn)到 n 系加回重力積分得速度再積分得位置function [vn, pn] ins_mechanize(q, acc, vn, pn, dt, g) % q : 當前姿態(tài)四元數(shù) % acc: 3×1 比力單位 m/s2 % g : 9.81 R quat_to_dcm(q); f_n R * acc(:); % 比力轉(zhuǎn)到 n 系 a_n f_n [0; 0; g]; % NED 下重力沿 z vn vn a_n * dt; pn pn vn * dt; end靜止時 本文還有配套的精品資源點擊獲取