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