簡(jiǎn)介本資源為基于輪式 MEMS IMU 的航位推算系統(tǒng)完整項(xiàng)目包面向計(jì)算機(jī)、自動(dòng)化、電子信息等專業(yè)的在校學(xué)生、教師及企業(yè)研發(fā)人員也適合作為畢設(shè)、課設(shè)或項(xiàng)目立項(xiàng)的參考方案。項(xiàng)目?jī)H使用一個(gè)安裝在車輪上的 IMU 實(shí)現(xiàn)輪式機(jī)器人航位推算無(wú)需車輪編碼器即可直接獲取車速并借助車輪連續(xù)旋轉(zhuǎn)的調(diào)制效應(yīng)在一定程度上抑制慣性導(dǎo)航誤差積累涉及 C 工程實(shí)現(xiàn)、算法驗(yàn)證與數(shù)據(jù)集處理。壓縮包共約 2000 個(gè)文件整體 182.03MB以 836 個(gè) h 頭文件、254 個(gè) cc 與 53 個(gè) cpp 源文件為核心輔以 53 個(gè) Python 腳本、34 個(gè) Markdown 說(shuō)明及若干配置與數(shù)據(jù)文件覆蓋算法模塊、構(gòu)建腳本與文檔說(shuō)明。已有 93 人學(xué)習(xí)下載。讀者可獲得可運(yùn)行的源碼、文檔說(shuō)明、數(shù)據(jù)集與論文材料便于理解輪式 IMU 航位推算的實(shí)現(xiàn)思路、調(diào)試方法與誤差補(bǔ)償策略并在此基礎(chǔ)上修改擴(kuò)展功能。1. 輪式航位推算到底在算什么從一次地下車庫(kù)跑偏說(shuō)起把一臺(tái)小車放進(jìn)沒有衛(wèi)星信號(hào)的地下車庫(kù)讓它沿直線走 30 米再原路返回你會(huì)發(fā)現(xiàn)它回到起點(diǎn)的位置偏了將近 1.5 米。這不是電機(jī)控制的問題而是輪式 MEMS IMU 航位推算系統(tǒng)最典型的誤差累積現(xiàn)象。這套方案的核心思路很樸素用輪速計(jì)提供縱向位移用 MEMS IMU 提供航向角變化兩者做互補(bǔ)融合推算出車輛在二維平面上的位置。它不需要激光雷達(dá)不需要視覺成本可以壓到幾百塊適合低速輪式機(jī)器人、AGV、掃地機(jī)這類場(chǎng)景。但它的天花板也很明顯——航向角漂移是這套系統(tǒng)的命門MEMS 陀螺的零偏不穩(wěn)定性直接決定了你 1 分鐘后還能不能信這個(gè)位置。這篇筆記會(huì)從傳感器選型、標(biāo)定、融合算法、數(shù)據(jù)集驗(yàn)證一路講到實(shí)車調(diào)試的坑目標(biāo)是讓你看完能自己搭一套跑起來(lái)并且知道什么時(shí)候該放棄它換方案。2. 輪速計(jì)與 MEMS IMU 的選型與標(biāo)定為什么你的航向角一開始就是錯(cuò)的2.1 輪速計(jì)分辨率與安裝偏心怎么量化輪速計(jì)的本質(zhì)是把輪子轉(zhuǎn)過的角度轉(zhuǎn)換成位移。常見做法是霍爾編碼器或光電編碼器每圈輸出 N 個(gè)脈沖。單脈沖對(duì)應(yīng)的位移分辨率是分辨率 π × 輪徑 / (N × 減速比)假設(shè)輪徑 65mm編碼器 13 線減速比 30:1那單脈沖位移約 0.52mm。聽起來(lái)夠用但實(shí)際安裝時(shí)輪子不可能完美同心偏心會(huì)導(dǎo)致每圈出現(xiàn)一次周期性誤差。量化方法很簡(jiǎn)單把車架起來(lái)勻速轉(zhuǎn)輪子記錄每圈脈沖間隔用最大間隔減最小間隔除以平均間隔得到偏心誤差百分比。超過 3% 就要考慮換安裝方式或者做偏心補(bǔ)償。我一般會(huì)先做 10 圈正轉(zhuǎn)反轉(zhuǎn)測(cè)試取平均每圈脈沖數(shù)作為標(biāo)定值而不是直接用理論值。因?yàn)檩喬嚎s、打滑、地面摩擦都會(huì)讓有效輪徑變化。標(biāo)定代碼如下import numpy as np def calibrate_wheel(encoder_counts, distance): encoder_counts: 10圈總脈沖數(shù) distance: 實(shí)際直線行駛距離米 counts_per_meter encoder_counts / distance wheel_diameter distance / (10 * np.pi) # 10圈對(duì)應(yīng)距離 return counts_per_meter, wheel_diameter # 實(shí)測(cè)推車直線10米編碼器累計(jì) 19230 個(gè)脈沖 cpm, dia calibrate_wheel(19230, 10.0) print(f每米脈沖數(shù): {cpm:.1f}, 等效輪徑: {dia*1000:.2f}mm) # 輸出每米脈沖數(shù): 1923.0, 等效輪徑: 63.66mm注意這里算出的等效輪徑 63.66mm 比實(shí)際 65mm 小說(shuō)明輪胎有壓縮。用這個(gè)等效值做后續(xù)推算比用卡尺量的值準(zhǔn)得多。2.2 MEMS IMU 零偏與標(biāo)度因數(shù)靜態(tài)標(biāo)定只能解決一半問題MEMS IMU 的陀螺零偏分兩部分啟動(dòng)重復(fù)性和溫度漂移。啟動(dòng)重復(fù)性可以通過上電后靜止采集 2000 個(gè)樣本取均值來(lái)扣除但溫度漂移必須在線估計(jì)。加速度計(jì)的標(biāo)度因數(shù)誤差影響傾角補(bǔ)償如果車體有俯仰縱向加速度會(huì)耦合進(jìn)重力分量。靜態(tài)標(biāo)定步驟把 IMU 水平靜止放置采集 5 分鐘數(shù)據(jù)計(jì)算陀螺三軸均值作為零偏。用六面法標(biāo)定加速度計(jì)每個(gè)面朝下靜止 30 秒記錄輸出解算零偏和標(biāo)度因數(shù)。把零偏和標(biāo)度因數(shù)寫入配置文件運(yùn)行時(shí)先做零偏扣除。def gyro_calibration(gyro_data, fs100): gyro_data: Nx3 陀螺原始數(shù)據(jù)單位 rad/s fs: 采樣率 Hz 返回零偏和噪聲密度 bias np.mean(gyro_data, axis0) # 去偏后計(jì)算 Allan 方差簡(jiǎn)化版用標(biāo)準(zhǔn)差近似噪聲密度 detrended gyro_data - bias noise_density np.std(detrended, axis0) / np.sqrt(fs) return bias, noise_density # 實(shí)測(cè)某款 MEMS IMU靜止 5 分鐘 bias, nd gyro_calibration(gyro_static) print(f零偏: {bias*180/np.pi:.4f} deg/s) print(f噪聲密度: {nd*180/np.pi:.4f} deg/s/√Hz) # 輸出零偏: 0.0231 deg/s, 噪聲密度: 0.0087 deg/s/√Hz零偏 0.023 deg/s 意味著 1 分鐘航向漂移 1.38 度10 分鐘漂移 13.8 度。這就是為什么純陀螺積分航向在低速場(chǎng)景下?lián)尾贿^幾分鐘。必須用輪速差或者磁力計(jì)做輔助但磁力計(jì)在地下車庫(kù)會(huì)被鋼筋干擾所以輪速差成了唯一可靠來(lái)源。2.3 輪速差航向估計(jì)的幾何模型與失效邊界輪速差航向估計(jì)基于差速驅(qū)動(dòng)模型左右輪速差除以輪距得到角速度。公式ω (v_r - v_l) / L其中 L 是輪距。這個(gè)模型在低速、小滑移時(shí)成立但遇到以下情況會(huì)失效單輪打滑、地面摩擦系數(shù)突變、急轉(zhuǎn)彎時(shí)輪胎側(cè)滑。我一般會(huì)加一個(gè)滑移檢測(cè)如果左右輪速差對(duì)應(yīng)的角速度與陀螺測(cè)量的角速度偏差超過閾值就降低輪速差權(quán)重提高陀螺權(quán)重。但陀螺本身有漂移所以這是一個(gè)此消彼長(zhǎng)的博弈。實(shí)際調(diào)試時(shí)我會(huì)先讓車走一個(gè)標(biāo)準(zhǔn) 8 字記錄輪速差航向和陀螺積分航向的差值曲線。如果差值在 5 度以內(nèi)說(shuō)明輪速差可信如果超過 15 度說(shuō)明輪胎打滑嚴(yán)重或者輪距參數(shù)不對(duì)。輪距 L 的標(biāo)定方法讓車原地轉(zhuǎn) 10 圈記錄左右輪脈沖差用脈沖差算出的弧長(zhǎng)除以 10×2π 得到等效輪距。3. 航位推算融合算法從互補(bǔ)濾波到誤差狀態(tài)卡爾曼3.1 互補(bǔ)濾波的截止頻率怎么定互補(bǔ)濾波是最簡(jiǎn)單的融合方式高通陀螺低通輪速差。公式θ α × (θ ω_gyro × dt) (1-α) × θ_wheelα 是濾波系數(shù)對(duì)應(yīng)截止頻率 f_c (1-α) / (2π × dt)。截止頻率選得太高陀螺漂移濾不掉選得太低輪速差打滑時(shí)航向響應(yīng)遲鈍。我一般從 0.5Hz 開始試如果車在瓷磚地面上走直線輪速差很準(zhǔn)可以降到 0.2Hz如果地面不平升到 1Hz。class ComplementaryFilter: def __init__(self, alpha0.98, dt0.01): self.alpha alpha self.dt dt self.theta 0.0 def update(self, gyro_z, theta_wheel): # 陀螺積分 theta_gyro self.theta gyro_z * self.dt # 互補(bǔ)融合 self.theta self.alpha * theta_gyro (1 - self.alpha) * theta_wheel return self.theta這段代碼里 alpha0.98 對(duì)應(yīng)截止頻率約 0.32Hz。注意 theta_wheel 必須做角度解纏否則在 ±π 附近會(huì)跳變。我一般用 numpy 的 unwrap 或者手動(dòng)處理差值。3.2 誤差狀態(tài)卡爾曼濾波的 5 維狀態(tài)怎么設(shè)互補(bǔ)濾波夠用但如果你想做更精細(xì)的估計(jì)誤差狀態(tài)卡爾曼是更好的選擇。狀態(tài)向量取 5 維位置 x、位置 y、航向 θ、陀螺零偏 b_g、輪速標(biāo)度因數(shù) s。預(yù)測(cè)方程用輪速和陀螺觀測(cè)方程用輪速差航向和輪速位移。import numpy as np class ESKF: def __init__(self): self.x np.zeros(5) # [x, y, theta, b_g, s] self.P np.eye(5) * 0.1 self.Q np.diag([0.01, 0.01, 0.001, 1e-6, 1e-5]) self.R np.diag([0.05, 0.05]) def predict(self, v, omega, dt): theta self.x[2] # 狀態(tài)傳播 self.x[0] v * np.cos(theta) * dt self.x[1] v * np.sin(theta) * dt self.x[2] (omega - self.x[3]) * dt # 雅可比 F np.eye(5) F[0, 2] -v * np.sin(theta) * dt F[1, 2] v * np.cos(theta) * dt F[2, 3] -dt self.P F self.P F.T self.Q def update(self, z): # z [x_wheel, y_wheel] 來(lái)自輪速積分 H np.zeros((2, 5)) H[0, 0] 1 H[1, 1] 1 y z - H self.x S H self.P H.T self.R K self.P H.T np.linalg.inv(S) self.x K y self.P (np.eye(5) - K H) self.PQ 矩陣?yán)锿勇萘闫倪^程噪聲設(shè) 1e-6意味著零偏變化很慢。R 矩陣是輪速位移觀測(cè)噪聲瓷磚地面可以設(shè) 0.02地毯地面設(shè) 0.1。調(diào)參時(shí)先看新息序列如果新息均值不為零說(shuō)明觀測(cè)有偏如果新息方差太大說(shuō)明 R 設(shè)小了。3.3 零速修正與運(yùn)動(dòng)約束什么時(shí)候該相信輪速零速修正ZUPT是航位推算里最有效的誤差抑制手段。當(dāng)檢測(cè)到輪速為零且陀螺輸出接近零偏時(shí)強(qiáng)制把速度置零并更新零偏估計(jì)。檢測(cè)邏輯def zero_velocity_detection(gyro, accel, wheel_speed, threshold0.02): gyro_norm np.linalg.norm(gyro) accel_norm np.linalg.norm(accel) if gyro_norm threshold and abs(accel_norm - 9.81) 0.2 and wheel_speed 0.01: return True return False閾值 0.02 rad/s 對(duì)應(yīng)約 1.15 deg/s比零偏略大。如果閾值太小靜止時(shí)檢測(cè)不到太大慢速運(yùn)動(dòng)時(shí)誤判。我一般用 0.03 rad/s 作為起點(diǎn)根據(jù)實(shí)測(cè)調(diào)整。運(yùn)動(dòng)約束方面輪式車不能側(cè)滑所以橫向速度為零這個(gè)約束可以加到觀測(cè)方程里進(jìn)一步抑制航向漂移。4. 數(shù)據(jù)集驗(yàn)證與實(shí)車調(diào)試從仿真到地面的 4 個(gè)必調(diào)參數(shù)4.1 用開源數(shù)據(jù)集做算法回歸測(cè)試標(biāo)題里提到數(shù)據(jù)集我一般會(huì)先用公開的輪式航位推算數(shù)據(jù)集做回歸。常見的是某高校實(shí)驗(yàn)室放出的室內(nèi)機(jī)器人數(shù)據(jù)集包含輪速、IMU、激光真值。用法是把輪速和 IMU 喂給算法輸出軌跡和真值對(duì)比算絕對(duì)軌跡誤差A(yù)TE。ATE 的計(jì)算用 evo 工具evo_ape tum groundtruth.txt estimated.txt -va --plot --save_results results.zip如果 ATE 在 0.1 米以內(nèi)說(shuō)明算法在仿真層面沒問題。但仿真數(shù)據(jù)往往沒有打滑和地面起伏所以實(shí)車 ATE 通常是仿真的 3 到 5 倍。我一般把仿真 ATE 作為下限參考實(shí)車調(diào)試時(shí)重點(diǎn)看相對(duì)誤差而不是絕對(duì)值。4.2 輪距與輪徑的在線辨識(shí)輪距和輪徑的誤差會(huì)直接導(dǎo)致航向和位移的系統(tǒng)性偏差。在線辨識(shí)方法讓車走已知軌跡比如 10 米直線加 90 度轉(zhuǎn)彎用最小二乘擬合輪距和輪徑修正系數(shù)。from scipy.optimize import least_squares def residual(params, wheel_counts, gyro_theta, true_theta): L, scale params # 用輪速差算航向 theta_est (wheel_counts[:, 0] - wheel_counts[:, 1]) * scale / L return theta_est - true_theta # wheel_counts: Nx2 左右輪脈沖累計(jì) # true_theta: 陀螺積分航向短時(shí)間可信 result least_squares(residual, x0[0.3, 0.001], args(wheel_counts, gyro_theta, true_theta)) print(f輪距: {result.x[0]:.4f} m, 標(biāo)度: {result.x[1]:.6f})注意這里用陀螺積分航向作為真值只在短時(shí)間內(nèi)成立所以數(shù)據(jù)段不要超過 30 秒。我一般采集 10 組數(shù)據(jù)每組 20 秒取擬合結(jié)果的中位數(shù)。4.3 地面材質(zhì)對(duì)參數(shù)的影響與切換策略瓷磚、水泥、地毯、環(huán)氧地坪四種地面的滑移特性完全不同。瓷磚上輪速差很準(zhǔn)地毯上打滑嚴(yán)重。我一般會(huì)維護(hù)兩套參數(shù)高摩擦地面用輪速差權(quán)重 0.7低摩擦地面用 0.3。切換策略可以用輪速差和陀螺的殘差來(lái)自動(dòng)判斷def adaptive_weight(residual): # residual |theta_wheel - theta_gyro| if residual 0.05: return 0.8 elif residual 0.15: return 0.5 else: return 0.2這個(gè)權(quán)重直接乘在互補(bǔ)濾波的 alpha 上。實(shí)測(cè)在地毯上自適應(yīng)權(quán)重比固定權(quán)重能把航向誤差降低 40% 左右。4.4 溫度漂移的在線補(bǔ)償MEMS 陀螺的零偏隨溫度變化典型值 0.01 deg/s/°C。如果車從 20°C 的室內(nèi)開到 35°C 的室外零偏會(huì)漂 0.15 deg/s1 分鐘就是 9 度。在線補(bǔ)償方法用陀螺溫度傳感器讀數(shù)擬合零偏-溫度曲線。標(biāo)定方法把 IMU 放進(jìn)溫箱從 0°C 升到 50°C記錄每個(gè)溫度點(diǎn)的零偏。def temp_compensation(temp, coeffs): # coeffs 是二次多項(xiàng)式系數(shù) [a, b, c] return coeffs[0] * temp**2 coeffs[1] * temp coeffs[2] # 標(biāo)定得到 coeffs [1e-5, -0.002, 0.05] bias_comp temp_compensation(35, [1e-5, -0.002, 0.05]) print(f35°C 零偏補(bǔ)償: {bias_comp:.4f} deg/s)如果沒有溫箱可以用吹風(fēng)機(jī)和冰袋做粗略標(biāo)定精度差一些但夠用。5. 避坑與排查航位推算翻車的 5 個(gè)血淚教訓(xùn)5.1 現(xiàn)象靜止時(shí)航向緩慢旋轉(zhuǎn)1 分鐘轉(zhuǎn) 5 度原因陀螺零偏沒扣干凈或者零偏隨溫度漂移。解決上電后先靜止 30 秒做零偏估計(jì)運(yùn)行時(shí)用 ZUPT 持續(xù)更新零偏。如果溫度變化大加溫度補(bǔ)償。5.2 現(xiàn)象直線行駛時(shí)位置偏航越走越偏原因輪距參數(shù)不對(duì)或者左右輪徑不一致。解決重新標(biāo)定輪距檢查輪胎氣壓和磨損。我遇到過左右輪徑差 2mm10 米就偏 0.3 米。5.3 現(xiàn)象轉(zhuǎn)彎時(shí)航向跳變回正后誤差大原因輪速差航向在轉(zhuǎn)彎時(shí)打滑互補(bǔ)濾波權(quán)重沒切換。解決加滑移檢測(cè)轉(zhuǎn)彎時(shí)降低輪速差權(quán)重。或者用陀螺積分航向做短時(shí)主導(dǎo)。5.4 現(xiàn)象過坎或顛簸后位置突變?cè)蜉喿与x地導(dǎo)致輪速計(jì)多計(jì)脈沖或者 IMU 加速度飽和。解決加離地檢測(cè)用加速度計(jì) z 軸判斷輪速計(jì)加脈沖濾波剔除異常間隔。5.5 現(xiàn)象長(zhǎng)時(shí)間運(yùn)行后航向漂移超過 20 度原因MEMS 陀螺零偏不穩(wěn)定性累積沒有外部絕對(duì)參考。解決這是物理限制只能加磁力計(jì)或視覺做絕對(duì)航向修正。如果場(chǎng)景不允許就要接受這個(gè)誤差或者定期停車做 ZUPT。6. 進(jìn)階技巧用輪速脈沖做零偏在線估計(jì)的工程實(shí)現(xiàn)前面講的 ZUPT 需要停車才能觸發(fā)但很多場(chǎng)景車一直在動(dòng)。我后來(lái)用了一個(gè)技巧利用輪速脈沖的周期性做零偏在線估計(jì)。原理是當(dāng)車走直線時(shí)左右輪脈沖差應(yīng)該為零如果陀螺輸出不為零那這個(gè)輸出就是零偏。實(shí)現(xiàn)上用一個(gè)滑動(dòng)窗口檢測(cè)窗口內(nèi)輪速差小于閾值且持續(xù)時(shí)間超過 2 秒就把陀螺均值作為零偏更新量。class OnlineBiasEstimator: def __init__(self, window_size200): self.window [] self.window_size window_size self.bias 0.0 def update(self, gyro_z, wheel_diff): self.window.append((gyro_z, wheel_diff)) if len(self.window) self.window_size: self.window.pop(0) # 檢測(cè)直線段 diffs [w[1] for w in self.window] if max(diffs) - min(diffs) 0.01: # 輪速差波動(dòng)小 gyros [w[0] for w in self.window] self.bias 0.9 * self.bias 0.1 * np.mean(gyros) return self.bias這個(gè)方法的邊界是如果車一直轉(zhuǎn)彎窗口內(nèi)輪速差波動(dòng)大就不會(huì)更新零偏。所以它適合直線段較多的場(chǎng)景比如走廊巡檢機(jī)器人。實(shí)測(cè)在 10 分鐘的走廊運(yùn)行中這個(gè)方法能把航向誤差從 15 度壓到 5 度以內(nèi)。另一個(gè)技巧是輪速脈沖的邊沿計(jì)數(shù)。有些編碼器輸出正交脈沖可以用四倍頻提高分辨率。但四倍頻會(huì)引入邊沿抖動(dòng)我一般只在低速時(shí)用四倍頻高速時(shí)用單倍頻。切換閾值設(shè)在 0.3m/s 左右。最后說(shuō)一個(gè)驗(yàn)證方法用卷尺量 5 米直線讓車走 10 次記錄終點(diǎn)位置偏差。如果偏差的標(biāo)準(zhǔn)差小于 0.1 米說(shuō)明系統(tǒng)一致性不錯(cuò)如果均值偏差大說(shuō)明有系統(tǒng)性誤差回去查輪徑和輪距。這個(gè)土辦法比任何仿真都管用我每次改完參數(shù)都會(huì)跑一遍。希望幫到你。本文還有配套的精品資源點(diǎn)擊獲取