基于卡爾曼濾波的陀螺儀電參數測量系統設計

［摘 要］ 為提高陀螺儀傳感器電參數測量值的精度與可靠性，設計一套基于卡爾曼濾波的陀螺儀傳感器電參數測量系統。采用STM32F407芯片作為上下位機的主控制器，通過無線通訊實現數據交互。下位機使用AD轉換電路測量陀螺儀傳感器的關鍵電參數；上位機采用7吋LCD顯示屏提供直觀的數據展示和觸摸按鍵輸入功能。利用卡爾曼濾波算法對測量數據進行誤差補償處理，以提高陀螺儀傳感器測得的偏轉角度和俯仰角度等多通道電壓、電流測量數據的準確性和穩定性。進行測量系統與高精度傾角儀的對比實驗，結果表明，該測量系統對陀螺儀傳感器的電壓、電流、偏轉角和俯仰角的最大測量誤差分別為0.07 V、0.18 mA、0.05°和0.042°，在允許誤差范圍內。

［關鍵詞］ 陀螺儀傳感器； 卡爾曼濾波； 電參數； 測量系統

［中圖分類號］ TP212 ［文獻標識碼］ A

陀螺儀傳感器是一種用于測量角速度的設備，也被稱為角速度傳感器。它通過旋轉慣性來測量旋轉運動。當物體轉動時，陀螺儀在重力力矩的作用下產生一個輸出信號，微控制器對陀螺儀輸出信號進行讀取和解碼，將之轉換成角度數據以便用于控制和導航[1-3]。陀螺儀傳感器測量精度可能受到溫度、磁場、振動和沖擊，以及時間漂移等環境因素的影響，會造成導航錯誤、控制不穩定等問題，如何提高陀螺儀傳感器測量的精度和可靠性成為研究熱點[4-6]。

陀螺儀傳感器測量系統廣泛應用于多種場景。曹鵬勇[7]等設計了一款結合紅外線傳感器、舵機和陀螺儀的智能巡航小車，該車控制器通過IIC通信讀取陀螺儀傳感器位姿信息，實現了對障礙物的識別、避讓，車體轉向和車身偏角預判等功能；張聰[8]等使用陀螺儀傳感器獲取汽車的三維角速度數據，數據經A/D轉換器處理后可顯示車身不同時刻的俯仰角、側傾角和橫擺角等動態參數。長時間使用陀螺儀進行姿態測量會導致積累的漂移誤差增加，從而顯著降低測量精度，通常需要對測量數據進行誤差補償處理。石煒[9]等通過在系統中加入霍爾傳感器矯正模塊消除陀螺儀累計漂移誤差，以獲得較高測量精度；張凱澤 [10] 等采用自適應濾波器消除信號噪聲，實現對不同波高和周期海浪的精確測量；韋昌銘 [11] 等則使用互補濾波和卡爾曼濾波對傳感器數據進行誤差修正，其獲得的姿態數據經濾波算法后可以得到穩定的平滑數據曲線。系統上下位機通常采用有線傳輸，數據傳輸線限制設備使用的靈活性，而無線傳輸則可以解決使用靈活性問題，陳元枝 [12] 等設計的位姿傳感器數據無線傳輸系統通過射頻模塊將數據發送到上位機，采集的數據值與參考值誤差較小，且在一定的距離內藍牙模塊數據傳輸性能可靠。

本文設計的陀螺儀傳感器電參數測量系統，融合卡爾曼濾波算法，可實現對陀螺儀傳感器偏轉角度和俯仰角度等進行精確測量，采用無線通訊方式進行上下位機數據交互。

1 系統硬件設計

1.1 系統結構

陀螺儀傳感器電參數測量系統硬件結構包括上位機和下位機兩部分，均采用STM32F407芯片作為主控制器實現對陀螺儀傳感器電參數的測量、處理和顯示等功能。下位機通過AD轉換電路采集陀螺儀傳感器的電參數信號，經處理后顯示為電壓、電流、偏轉角度和俯仰角度等數據；上位機接收和顯示測量數據，并可通過虛擬觸摸按鍵控制下位機的電壓輸出。上下位機通過無線通訊模塊實現遠距離的數據傳輸。

圖1展示了測量系統的硬件架構。在下位機部分。硬件配置涵蓋了聚合物鋰電池、陀螺儀傳感器、控制板以及藍牙無線通訊模塊。聚合物鋰電池負責為傳感器和控制板供電?？刂瓢迮c陀螺儀傳感器通信，以收集位姿信息。通過下位機的Iin和Vin接口，系統可測量陀螺儀傳感器的電流和電壓值。而Vout接口用于輸出電壓信號。所有收集到的數據通過藍牙無線通訊模塊發送至上位機。上位機的硬件組成包括顯示屏、開關電源、控制板和藍牙無線通訊模塊。藍牙無線通訊模塊的主要功能是接收下位機發送的測量數據。顯示屏不僅用于直觀展示測量數據，還能實時監控系統的工作狀態。

1.2 硬件電路

測量系統下位機電路如圖2所示。微處理器通過IIC總線與陀螺儀傳感器進行數據傳輸，使用8 MHz晶振，可以超頻到168 MHz來提高AD電路采樣頻率。無線通訊模塊采用的是一款具有高穩定性的工業級無線串口模塊，采用LORA擴頻調制，傳輸距離可達5000 m，且該模塊有指令控制、正常工作和休眠模式三種模式，指令控制模式可以對無線通訊模塊進行配置。鋰電池采用5 V升壓鋰電池，通過電源供電穩壓電路對陀螺儀傳感器、控制板和無線通訊模塊進行供電。測量系統上位機電路（圖3）包括7吋顯示屏、控制板和無線通訊模塊。其中7吋顯示屏分辨率為800×480，采用3.3 V或5 V供電，可與單片機進行連接使用。

2 系統軟件設計

2.1 通訊協議

為了保證上下位機通訊的穩定性，需要自定義通訊協議，其格式如圖4所示。該通訊協議為8字節結構。

起始字節（START）：作為協議數據幀起始標志，該字節固定為0x55。當串口接收到0x55時，即表示通訊開始。

幀頭：位于第2字節，上下位機通訊協議的幀頭統一設置為0xaa。

數據長度：第3字節，用于指示當前數據包的大小。這一長度由用戶自定義。

標識：第4字節，用于區分不同類型的數據包。例如：在電壓讀取模式下，該字節為電壓讀取標識；在電流讀取模式下，此處為電流讀取標識。

數據：第5字節，包括實際傳輸的數據內容，如實際電流值、電壓值、偏轉和俯仰角度等數據。

數據校驗：第6、7字節，采用CRC（循環冗余校驗）算法來檢測數據在傳輸過程中可能出現的錯誤。

幀尾： 第8字節，作為協議數據結束的標志，保留字節為0xff。

下位機遵循自定義通訊協議的規定，將數據精心組織并裝入發送緩沖區。將所有需要發送的數據集中于一個緩沖區，并利用發送中斷機制將緩沖區的數據發送出去。上位機則通過中斷接收數據，提高了數據處理的實時性和系統的響應速度。數據包的解析策略根據協議的復雜度靈活設置。對于簡單的協議，如果系統僅處理一些簡單的命令，可以直接把數據包的解析過程放入中斷處理函數中，一旦收到正確的數據包，即置位相應的標志，在主程序中再對命令進行處理。如果協議稍微復雜，則將接收的數據存放于緩沖區中，主程序讀取數據后進行解析。本系統通過接收中斷機制來實現數據接收協議，具體來說，每當系統接收到一次中斷（即接收到1個字節）時，就會將該數據存儲起來，直到收集到一個完整的數據幀。如果接收的數據幀是正確的，系統將設置一個標志位（flag），以便在主程序中進行命令解析。如果數據幀存在錯誤，就會被系統自動丟棄。

2.2 Kalman濾波算法設計

卡爾曼濾波是一種高效的動態系統狀態估計方法，廣泛應用于動態系統的精確狀態推斷。它結合傳感器測量和模型預測，以實現系統狀態的精確估計。這種方法適用于跟蹤目標、濾除噪聲和干擾等應用場景?？柭鼮V波算法通過將當前時刻的測量值與上一時刻的系統預測值及其相關誤差結合起來，通過計算得出當前時刻的最優解，再預測下一時刻的值，這相當于在線性條件下Ｋ值（卡爾曼增益）動態變化的互補濾波形式?？柭鼮V波的狀態方程

式中：Xk為k時刻的狀態變量；Ak|k-1為系統k-1時刻到k時刻的狀態轉移矩陣；Bk-1和Wk-1分別為k-1時刻系統的系統噪聲矩陣和過程噪聲；Zk、Hk和Vk分別為k時刻系統的觀測向量值、觀測轉移矩陣和觀測噪聲。

卡爾曼濾波算法包括時間更新和觀測更新，用于將上一個時刻的狀態轉移到當前時刻，并根據當前時刻的觀測值對狀態進行校正，得到最優的估計值。交替進行這兩個過程，可以實現對狀態的優化估計，達到濾波的目的。圖5為卡爾曼濾波算法更新流程。

將陀螺儀傳感器的角速度ω和靜態偏移φ作為系統的輸入值，并建立觀測方程

式中：θk為k時刻載體角度值；θk-1為k-1時刻的陀螺儀輸出角度值，φk-1為k-1時刻陀螺儀的靜態漂移，Wk-1為觀測噪聲。

系統狀態下一步預測方程和觀測方程分別為：

式中：Ｔ為積分時間；Zk為加速度計量測角度；Vk為系統測量的噪聲。

在陀螺儀傳感器電參數測量系統中，卡爾曼濾波算法被應用于電參數的處理和優化電參數。該算法通過模型預測方式對連續性信號進行濾波和預測，有效降低噪聲和誤差的影響，從而提高了估計精度和系統的可靠性。處理步驟包括建立數學模型、計算最優估計值和狀態誤差協方差矩陣、更新狀態估計值和誤差協方差矩陣，以及預測下一時刻的電參數值。

2.3 上位機軟件設計

測量系統下位機承擔著對陀螺儀傳感器電流值和電壓值的測量任務。這些數據進行卡爾曼濾波處理后，通過無線模塊傳輸給上位機。上位機作為系統的控制中心，通過無線模塊接收下位機發送的電壓值、電流值、偏轉角度和俯仰角度等關鍵數據，并在界面上直觀顯示。上位機顯示界面（圖6）部署在7吋LCD屏上，用戶可通過觸摸虛擬按鍵設置下位機電壓輸出值。在測量參數方面，上位機提供了5 V和30 V兩個量程的電壓測量選項，電流測量則統一設置為100 mA量程。此外，系統還能夠顯示陀螺儀的偏轉角度和俯仰角度數值。

3 實驗及結果分析

3.1 實驗方法

為了驗證陀螺儀傳感器電參數測量系統的可靠性，在硬件和軟件開發完成之后對其進行實驗驗證。考慮到陀螺儀傳感器在日常使用過程中的實際工況，采用模擬平臺控制陀螺儀傳感器在三維空間的偏轉角度；通過改變控制平臺X軸和Y軸的傾斜度來模擬實際場景的偏轉角度和俯仰角度。實驗采取對比法，分別使用本設備和高精度專業儀器對陀螺儀數據進行測量。本實驗將分為三部分進行：第一部分是上位機設置電壓，控制下位機輸出相應的電壓值；第二部分是下位機在5 V、30 V、100 mA 3個不同檔位測量陀螺儀傳感器的電壓和電流等電參數信息，并在上位機界面顯示；第三部分是下位機將處理后的偏轉角度和俯仰角度信息傳輸給上位機并在上位機界面上顯示。圖7為下位機實物正反面，圖8為上位機實物顯示界面和硬件電路。

3.2 實驗結果分析

圖9為設置電壓與輸出電壓對比曲線。通過上位機觸摸按鍵輸入設置電壓值，在下位機的Vout端口測量電壓值，共采樣10次，設置電壓范圍為0～5 V。由圖9可知，設置電壓曲線與輸出電壓曲線重合度較好，誤差在0.05 V以內，符合精度要求。

分別采用本設備和高精度電壓電流儀對陀螺儀傳感器的電壓和電流進行測量，獲得電壓和電流的參考值和測量值。圖10為測量電壓對比曲線及電壓測量誤差曲線。電壓測量共采樣20次，電壓為0～30 V。結合電壓測量誤差曲線可知，電壓測量最大誤差值為0.07 V。

圖11為測量電流對比曲線及電流測量誤差曲線。電流測量共采樣20次，電流為0～100 mA。由電流測量誤差曲線可知，電流測量最大誤差值為0.18 mA。

圖12為陀螺儀傳感器偏轉角與俯仰角對比曲線及誤差值曲線。陀螺儀傳感器偏轉角和俯仰角的參考值采用高精度傾角儀測得，測量值曲線與參考值曲線重合度較好，其中偏轉角最大測量誤差為0.05°，俯仰角最大測量誤差為0.042°。

4 結論

為了提高陀螺儀傳感器電參數的測量精度，設計了基于STM32F407的陀螺儀傳感器電參數測量系統（包括上下位機的軟硬件部分）。該系統實現了輸出電壓、測量電壓和電流、測量偏轉角和俯仰角，以及上位機顯示等功能。實驗結果表明，該測量系統能夠對陀螺儀傳感器的電壓、電流、偏轉角和俯仰角進行精確測量，最大測量誤差分別為0.07 V、0.18 mA、0.05°和0.042°，顯示出較高的測量精度。

［ 參 考 文 獻 ］

［1］ 駱曼箬，李紹良，黃藝明，等. 原子陀螺研究進展及展望[J]. 測控技術：1-10.

[2] 張風雷，柳奎，陸澤晃，等. 大型測地激光陀螺儀研究綜述[J]. 華中科技大學學報（自然科學版），2023，51（03）：120-131.

[3] PASSARO V M N， CUCCOVILLO A， VAIANI L， et al. Gyroscope technology and applications： a review in the industrial perspective[J]. Sensors， 2017， 17（10）： 22.

[4] 張強，吳志強，云啟凡.基于MIMU/磁傳感器/雙天線RTK的姿態測量方法研究[J].電子測量與儀器學報，2023，37（05）：230-239.

[5] XIA D Z， YU C， KONG L. The development of micromachined gyroscope structure and circuitry technology[J]. Sensors， 2014， 14（01）： 1394-1473.

[6] FENG R， WANG J， QIAO W， et al. Real-time built-in self-test of mems gyroscope based on quadrature error signal[J]. Micromachines， 2021， 12（09）： 16.

[7] 曹鵬勇，王建文. 基于STM8S105的智能車結構及控制系統的研究[J]. 工程設計學報，2020，27（04）：516-523.

[8] 張聰，陳翀，周亭亭. 便攜式車身動態姿態檢測系統的設計[J]. 測控技術，2016，35（06）：18-20.

[9] 石煒，朱光宇. 基于陀螺儀的方向盤轉角傳感器[J]. 儀表技術與傳感器，2014（09）：6-8.

[10] 張澤凱，梁旗，唐健，等. 基于MEMS慣性傳感器的波浪測量系統設計與實現[J]. 電子測量技術，2022，45（10）：170-176.

[11] 韋昌銘， 王勇軍." 基于慣性傳感器的姿態實時測量系統設計[J]. 桂林航天工業學院學報， 2021， 26（03）：265-270.

[12] 陳元枝，孫浩，趙國如. 基于CC254x與STM32的慣性傳感器無線傳輸系統設計[J]. 傳感器與微系統，2016，35（07）：103-106.

Design of Gyroscope Electrical Parameter MeasurementSystem Based on Kalman Filtering

LIU Xu1， YANG Zhiyong1， ZHANG Daode1，2， HU Xinyu1， WEI Qiong1， LU Enshun2

（1 School of Mechanical Engineering， Hubei Univ. of Tech.， Wuhan 430068，" China；2 Agricultural Machinery Engineering Research and Design Institute，Hubei Univ. of Tech.， Wuhan 430068， China）

Abstract： To improve the accuracy and reliability of gyroscope sensor electrical parameter measurements， a gyroscope sensor electrical parameter measurement system based on Kalman filtering was designed. Firstly， the system employed the STM32F407 chip as the main controller for both the upper and lower computers， enabling data interaction through wireless communication. The lower computer used an AD conversion circuit to measure the crucial electrical parameters of the gyroscope sensor. The upper computer featured a 7-inch LCD display screen， providing intuitive data visualization and touch button input functionality. Subsequently， the measured data was processed using the Kalman filtering algorithm to compensate for errors， enhancing the accuracy and stability of the gyroscope sensor's measured values， including deflection angle， pitch angle， and other multi-channel voltage and current measurements. Finally， a comparative experiment was conducted between the measurement system and a high precision inclinometer. The results showed that the maximum measurement errors of the gyroscope sensor's voltage， current， deflection angle， and pitch angle were 0.07V， 0.18mA， 0.05 degrees， and 0.042 degrees， respectively， all within the allowable error range.

Keywords： gyroscope sensor; Kalman filter; electrical parameters; Angular velocity measurement system

［責任編校： 張 眾］

［收稿日期］ 2023-05-11

［基金項目］ 國家自然科學基金 （51907055）; 湖北省教育廳重點項目（D20221404）

［第一作者］ 劉 旭（1999-）， 男， 湖北襄陽人， 湖北工業大學碩士研究生， 研究方向為機器人技術。