基于慣性傳感器的姿態測量系統設計與實現

摘要：針對微機電系統陀螺儀姿態傳感器輸出速率不高、靜態精度低、有靜漂等問題，文章設計了一種基于磁力計、陀螺儀、溫度傳感器和單片機的姿態測量系統。將磁力計、陀螺儀數據融合解算后得到三軸姿態角，使用溫度傳感器做溫度補償修正。通過角度靜態準確性測試及零漂測試結果表明，系統靜態精度誤差為±0.2°，零漂方差最大值為0.002°，有效提高了姿態測量的精度和穩定性。

關鍵詞： 慣性傳感器；擴展卡爾曼濾波；傳感器數據融合；溫度補償

中圖分類號：TP311 文獻標識碼：A

文章編號：1009-3044（2025）04-0137-04 開放科學（資源服務） 標識碼（OSID） ：

0 引言

隨著人工智能技術的快速發展，自動駕駛技術已被廣泛應用于無人駕駛汽車和自主移動機器人等領域。自動駕駛技術的實現需要解決未知環境中的即時定位、地圖構建與導航問題[1]，其中導航的精準度在很大程度上依賴于姿態檢測系統的準確性。然而，慣性傳感器的漂移誤差以及系統噪聲會導致慣性測量單元（IMU） 數據及姿態解算出現偏差[2]，使自主移動機器人平臺的坐標變換及導航角出現嚴重誤差，從而影響導航效果。

針對上述問題，本文擬構建一種適用于運動載體姿態測量的慣性檢測系統，并設計姿態檢測系統的軟硬件實施方案。針對慣性傳感器存在的漂移誤差和系統噪聲，選擇合適的姿態數據解算方法，結合卡爾曼濾波等技術完成傳感器數據融合[3-5]，并對IMU姿態誤差進行校正。通過實際系統測試驗證，系統采集的姿態測量值的頻率最高可達1 kHz，在滿足姿態矩陣修正閾值的條件下，姿態測量誤差小于1°。

1 系統總體設計

設計一個最高以1 kHz速率輸出并具有溫度補償的高精度姿態傳感系統，并具備二次開發和擴展性，系統環境適應性強。

系統框圖如圖1所示，主要由3個核心傳感器（磁力計RM3100、陀螺儀ICM-42688-P 和溫度傳感器LMT70） 、單片機STM32以及兩個低壓差線性穩壓器構成。磁力計RM3100具有較低的噪聲密度和較高的輸出速率，陀螺儀ICM-42688-P的溫漂小，而溫度傳感器LMT70則具備高精度，這三種傳感器能夠滿足本設計的高速率和低噪聲輸出要求。

電源部分實現了5 V轉3.3 V和3.3 V轉1.8 V的功能。3.3 V電源為磁力計、陀螺儀、溫度傳感器和單片機提供電源，1.8V電源則為陀螺儀的內核和模擬電源供電。陀螺儀向單片機提供采集到的角速度和加速度數據，單片機通過擴展卡爾曼濾波器得到角度數據。磁力計將地磁場數據傳送給單片機，單片機將地磁場數據與擴展卡爾曼濾波器得到的角度數據進行進一步融合，以獲得更高可靠性的航向角數據。溫度傳感器將采集的溫度數據傳遞給單片機，單片機經過處理后進行角度的溫度補償。單片機通過外部接口中的串口及通用輸入輸出接口與外部進行數據交互。

2 硬件設計

2.1 單片機最小系統板設計

單片機最小系統板設計主要包括單片機外圍電路、電源電路、串口轉換為通用串口總線的電路以及按鍵等電路。單片機的外圍電路主要包括復位電路、外部晶振電路和退耦電容電路[6]，原理圖如圖2所示。復位電路由電容C6 和按鍵組成，晶振電路采用16MHz晶振及其配套電容，退耦電容電路由電容C7 和C8構成。電源電路采用低壓差線性穩壓器AP2112 芯片實現5V轉3.3V的功能。串口轉通用串口總線電路采用轉換芯片CH340x和Type-C接口，可直接使用Type-C數據線進行電腦與STM32芯片之間的通信。

2.2 磁力計測試板設計

磁力計RM3100 的測試板設計主要包括5 V 轉3.3 V電源和磁力計外圍電路兩個部分。測試板的原理圖如圖3 所示。電源部分在低壓差線性穩壓器AP2112 芯片的電路中增加了瞬態電壓抑制二極管ESD5451N，以吸收電源的高尖峰并提供靜電防護。磁力計的外圍電路包括三路RL振蕩器，其阻值分別為R1、R2、R3、R5、R6和R7。R4為磁力計內部振蕩器的外部電阻，C4至C7為磁力計的退耦和濾波電容。

2.3 陀螺儀測試板設計

陀螺儀ICM-42688-P的測試板主要包括5 V轉3.3 V電源、3.3 V轉1.8 V電源和陀螺儀外圍電路3個部分。電源部分分別采用低壓差線性穩壓器AP2112-3.3和AP2112-1.8，實現5 V轉3.3 V及3.3 V 轉1.8 V的功能。5 V轉3.3 V電源電路的輸入端采用肖特基二極管D1和瞬態電壓抑制二極管D2，以提高電路的輸出穩定性。同時，在輸出端設計的鉭電容參數為4.7 μF，以改善輸出與下一級3.3 V轉1.8 V的寄生電感，從而獲得更小紋波的1.8 V 電源輸出。在3.3 V轉1.8 V電源電路中，采用500 Ω的磁珠及電容E2、C5、C6來抑制電源紋波，為陀螺儀提供低于1.5 mA 的模擬電壓輸入。ICM-42688-P測試板的原理圖如圖4所示。

3 軟件設計

3.1 單片機配置及初始化

本設計使用了兩個串口，UART1和UART2，分別用于接收和發送數據，其中UART1用于接收，UART2 用于發送。串口1的接收同時采用DMA接收，配合隊列的緩沖接收和空閑中斷。在完成單片機初始化（包括UART 和DMA 的初始化） 后，從堆中獲取大小為100字節的空間作為串口的DMA接收緩存，然后等待中斷。

3.2 磁力計配置

磁力計RM3100的初始化時先進行自檢。首先向相應的寄存器寫入控制指令以執行傳感器自檢，判斷標志位DRDY是否變高。如果在超出等待時間后標志位無反應，則自檢失敗，可以讀取自檢寄存器以判斷哪個軸未通過自檢。如果標志位變高且讀取自檢寄存器的值為0xfa，則表示自檢成功。自檢成功后，設置三軸循環計數寄存器為155，使磁力計在連續模式下的輸出頻率為220 Hz。同時，設置數據更新速率為600 Hz，啟用傳感器的三個軸并設置為連續模式。最后，啟用單片機的外部中斷，完成磁力計的初始化。初始化流程圖如圖5所示。

3.3 陀螺儀配置及零偏矯正

陀螺儀ICM-42688-P的初始化過程首先進行自檢，以確保傳感器正常工作。完成自檢后，配置寄存器以輸出信號，開啟三軸陀螺儀和三軸加速度計的電源，并設置SPI模式以及旁路FIFO。接著，設置溫度輸出的內部數字濾波器截止頻率為40 Hz，并在配置陀螺儀和加速度輸出速率后完成初始化。

初始化完成后，芯片可以輸出三軸角速度和三軸加速度數據。然而，角速度的輸出可能存在零偏、高頻噪聲及加速度輸出不對稱等誤差。零偏校準的方法是以1 kHz的頻率連續讀取1 000個角速度數據，并計算出平均值作為偏置數據。為了保證零偏校準的精確性，將計算出的偏置數據與預設值進行比較。如果偏差過大，則須重新進行零偏矯正。

3.4 溫度傳感器配置及擬合

溫度傳感器的測試配置流程圖如圖6所示。在單片機外設完成初始化后，開啟DMA傳輸，使能定時器4通道4，然后開始采集溫度傳感器的輸出。當定時器4通道4發出上升沿信號后，ADC開始采集數據。采集完成后，由DMA自動將數據傳輸到設定的內存中，并觸發DMA的傳輸完成中斷。

初始化完成后，設計了溫度傳感器的IIR低通濾波器并對其進行了數據擬合。由于測試環境受限，所使用的溫度計在顯示實時性方面與輸出的精準度有限。但在實際系統應用中，陀螺儀溫漂矯正時會對溫度進行一定的修正。

4 實驗結果與分析

4.1 角度靜態準確性測試

為評估系統的靜態精度，使用可調式角度規銑床墊塊作為角度標準器進行靜態準確度測試。具體測試方案為：使用一個固定板作為支架，將墊塊的可移動測量面伸出，并在固定板上打上螺絲孔，以將整個系統固定于墊塊上。由于墊塊的測量范圍限制（以45°為零點，單邊75°） ，因此需要改變4次系統的結構才能完成一次完整的測量，實物圖如圖7所示。在角度測量時，以5°為步進值，先將墊塊帶動陀螺儀旋轉后固定，隨后讀取姿態測量系統輸出的數據并記錄。

俯仰角測試數據和標準值如圖8和圖9所示，橫滾角測試數據和標準值如圖10和圖11所示。圖中橫坐標為標準角度，縱坐標為測量角度。圓點代表姿態測量系統的測量值，星號代表角度標準值，直線為擬合后的測量值直線。從圖中可以看出，擬合后的直線基本穿過標準值。通過計算，將擬合后的測量值與標準值進行比較，結果顯示兩軸角度測量誤差均小于0.2°。

4.2 零漂測試

在俯仰角、橫滾角和航向角的零漂測試中，系統的輸出頻率設置為0.00333Hz（每5分鐘輸出一次） ，靜置放置5個半小時。三個軸的角度變化如圖12～圖14 所示。從圖中可以看出，輸出角度基本無零漂現象，偏移值較小。三軸角度測量的誤差方差均小于0.002，俯仰角和橫滾角的最大偏差均小于0.08°，航向角的偏差為0.1°，達到了設計預期。然而，由于測試條件的限制和機械結構的安裝誤差，三軸角度靜態輸出的初始值并不為零。

5 結論

本文設計并實現了一種基于慣性傳感器的姿態測量系統，該系統結合了磁力計、陀螺儀和溫度傳感器。通過數據融合和姿態解算算法，有效提高了姿態測量的精度和穩定性。經測試，該系統的輸出速率最高可達1 kHz，靜態精度誤差為±0.2°，零漂方差的最大值為0.002°。然而，由于測試時機械結構安裝存在誤差，無法進行精確的角度校準和修正。同時，缺乏可調的恒溫環境使得溫度傳感器的擬合效果有所欠缺。在后續研究中，需要探索更有效的溫度補償方法，以增強系統在復雜環境下的適應性。

參考文獻：

[1] 景元泉，王躍輝，韓偉，等.無人駕駛車輛與移動機器人SLAM 方法綜述[J].電子世界，2021（13）：4-5.

[2] 賈浩男.室內行人自主定位算法的研究與實現[D].哈爾濱： 哈爾濱工程大學，2017.

[3] 樊志強，董朝軼，王啟來，等.基于多傳感器數據融合的巡檢機器人測姿系統研究[J].自動化與儀器儀表，2021（3）：77-82，86.

[4] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理[M].2 版.西安：西北工業大學出版社，2012.

[5] 宋國東，姜守達，林連雷.復雜有色噪聲廣義系統信息融合Kalman濾波器[J].儀器儀表學報，2013，34（5）：1195-1200.

[6] 周潤景，張麗娜，丁莉.基于PROTEUS的電路及單片機設計與仿真[M].2版.北京：北京航空航天大學出版社，2010.

【通聯編輯：梁書】

基金項目：基于SLAM 技術的倉儲運輸小車的路徑規劃及調度（項目編號：2022ZR033）