卡爾曼濾波在兩輪自平衡代步車姿態檢測中的應用

張團善,何穎

(西安工程大學 電子信息學院，西安 710048)

?

卡爾曼濾波在兩輪自平衡代步車姿態檢測中的應用

張團善,何穎

(西安工程大學 電子信息學院，西安 710048)

介紹了一種MENS加速度計、陀螺儀與嵌入式微控制器相結合的兩輪自平衡代步車姿態檢測系統。針對加速度計和陀螺儀測量分別存在噪聲干擾和隨機漂移誤差，采用卡爾曼濾波實現傳感器數據融合，補償傳感器測量誤差，得到車體姿態的最優估計。將該算法移植到姿態檢測系統的微控制器中，測試結果表明卡爾曼信息融合可以有效提高系統檢測精度。

姿態檢測；STM32F103RET6；卡爾曼濾波；數據融合

引 言

兩輪自平衡代步車作為一種新型輕便的交通工具，具有運動靈活、結構簡單、自平衡控制的特點。代步車在運動過程中需要不斷地對車體的運動速度、角度等姿態信息進行及時準確的采集與估計，以實現車體的運動規劃調整和動態平衡控制。

常用的姿態檢測傳感器為加速度計和陀螺儀。加速度計用于測量與慣性有關的加速度，經轉化后獲得角度值，其誤差小，精度高，但動態角度測量噪聲干擾較大。陀螺儀的作用是測量角速度信號，其本身極易受噪聲干擾，同時由于溫度變化、不穩定力矩等因素，陀螺儀會產生隨機漂移誤差，并隨著時間的推移而累加變大[1]。參考文獻[2]采用互補濾波的方法，根據傳感器輸出特性選擇相應的濾波器，達到加速度計與陀螺儀信號相互補償的作用，具有理論簡單、響應快的特點，但對橫向加速度和陀螺儀漂移不夠敏感，所以無法得到姿態信息的準確估計。本文提出了應用卡爾曼濾波算法實現加速度計與陀螺儀的檢測信息融合，得到俯仰角和俯仰角速度的最優估計，并在以STM32F103RET6為控制核心的兩輪自平衡代步車姿態檢測系統上加以驗證。

1 系統組成

本文所采用的姿態檢測系統主要由加速度計、陀螺儀、微控制器、無刷電機等部分組成[2]。姿態檢測系統框圖如圖1所示,陀螺儀和加速度計信號送入微控制器中，通過卡爾曼濾波器對傳感器數據進行補償和融合,獲得準確的姿態信號,再通過PID控制算法輸出控制量，經過三相逆變電路，生成PWM波驅動電機運轉。

圖1 姿態檢測系統框圖

1.1 加速度計

ADXL345是ADI公司2008年推出的采用MEMS技術的具有SPI和I2C總線數字輸出功能的三軸加速度計，最大可感知±16g的加速度，感應精度可達3.9 mg/LSB,傾角測量典型誤差小于1°，具有小巧輕薄、超低功耗、可變量程、高分辨率等特點。ADXL345可在傾斜敏感應用中測量靜態重力加速度，也可在運動甚至振動環境中測量動態加速度。將加速度計垂直于車體俯仰軸安裝時，通過反正切函數運算能得到準確的俯仰角信息，其表達式為:

(1)

其中，θ為俯仰角；Ax和Ay分別為橫軸和縱軸的重力加速度輸出。圖2為加速度計檢測原理圖，檢測數據通過I2C總線(SCL和SDA端口)送入微控制器。

圖2 加速度計檢測原理圖

ITG3205是單一芯片并以數字方式輸出的三軸MEMS陀螺儀，將數字輸出X、Y、Z軸角速度值傳感器整合在單一電路上。圖3為陀螺儀檢測原理圖，檢測數據通過高達400 kHz的I2C總線串行接口(SCL和SDA)與微控制器通信。

圖3 陀螺儀檢測原理圖

1.3 微控制器

采用ST公司的STM32F103RET6作為控制芯片，它的最高工作頻率可以高達72 MHz，并且片內集成了高速的存儲器(高達512K字節的閃存和64K字節的SRAM)，具有豐富的增強I/O端口和聯接到兩條APB總線的外設[3]。STM32F103RET6內部包含3個12位的ADC、4個通用16位定時器、2個16位帶死區控制和緊急剎車功能，用于電機控制的PWM高級控制定時器、2個I2C總線接口利于檢測數據的串行傳輸，適合應用于電機驅動和控制的場合。

2 卡爾曼濾波車體姿態信息融合

2.1 車體姿態模型

本系統采用的加速度計和陀螺儀都是三軸傳感器，可以對車體運行過程中的俯仰、偏航、橫滾狀態的信息進行檢測。本文主要研究俯仰狀態，應用卡爾曼濾波對俯仰角和俯仰角速度進行數據融合[4-5]。車體的俯仰角與俯仰角速度存在導數關系，所以選取車體的俯仰角作為狀態向量的一個元素。系統采用加速度計來估計陀螺儀的隨機漂移gyro_bias，將此偏差作為另一個狀態向量。

車體的俯仰角與俯仰角速度存在如下等式關系：

(2)

式中，anglek為估算的k時刻俯仰角,anglek-1為k-1時刻的俯仰角估計值, gyrok-1為陀螺儀所測得的k-1時刻角速度；gyro_biask-1為k-1時刻陀螺儀的位偏差,dt為采樣時間。

如果認為前一時刻與后一時刻的漂移誤差變化很小，即：

(3)

可得系統狀態方程為

試驗所得數據運用SPSS Statistics 19.0進行統計分析，作圖運用Origin 8.0軟件。

(4)

其中，wk是過程噪聲,并假定其符合均值為零、協方差矩陣為R的正態分布。

測量方程為:

(5)

其中,θk是k時刻加速度計的俯仰角觀測值；vk是觀測噪聲，假設符合均值為零、協方差為R的正態分布。

2.2 卡爾曼濾波器設計

圖4 卡爾曼濾波算法框圖

(6)

Q_angle和Q_gyro分別是俯仰角狀態和陀螺儀漂移狀態的過程噪聲的協方差，相對較小的取值，體現更高的信任程度，一般取Q_gyro的值比Q_angle小三個數量級左右。觀測噪聲的協方差矩陣：

(7)

R_angle為加速度計測量噪聲vk的協方差，R_angle越大，則觀測值中被卡爾曼濾波器視為噪聲的成分越大。

卡爾曼濾波以最小均方誤差估計為準則，通過一串含有噪聲的測量數據，對系統的內部狀態進行估計[6]。通過對上述算法的循環使用，可得到車體姿態的最優估計。

3 實驗驗證

為了驗證卡爾曼濾波融合車體姿態信息的可行性與有效性，將該數據融合算法移植到STM32F103RET6微控制器中，通過對兩輪自平衡代步車姿態檢測系統電路板的調試，實現對姿態數據的實時檢測與處理。軟件設置卡爾曼濾波器初始參數如表1所列。

表1 卡爾曼濾波器參數設置

實驗結果如圖5所示，在未經卡爾曼濾波器前，加速度計觀測的俯仰角度存在較大的噪聲干擾，角度輸出為±10°之間；經卡爾曼濾波后，角度輸出為±5°之間，有效濾除噪聲干擾信號。

圖5 俯仰角濾波結果

圖6中卡爾曼濾波器對陀螺儀工作過程中的隨機漂移誤差進行估計，并用估計結果來更正陀螺儀測量的角速率值，有效補償陀螺儀的漂移誤差。經濾波后，系統達到更高的檢測精度，滿足實際需要。

圖6 角速率濾波結果

結 語

[1] 徐東升.慣性導航中加速度計和陀螺儀性能研究[J].佳木斯大學學報,2012(3).

[2] 張吉昌,程凱.單軸雙輪自平衡車姿態檢測方案設計[J].中國海洋大學學報,2009(9).

[3] 李寧.基于MDK的STM32處理器開發應用[M].北京：北京航空航天大學出版社,2008:7-14.

[4] Rich chi Ooi.Balancing a Two-Wheeled Autonomous Robot[D]. Perth：The University of Western Australia, 2003.

[5] 郜園園,阮曉剛.兩輪自平衡機器人慣性傳感器濾波問題的研究[J].傳感技術學報,2010(5).

[6] 彭丁聰.卡爾曼濾波的基本原理及應用[J].軟件導刊,2009(11).

何穎(碩士研究生)，研究方向為檢測技術與自動化裝置。

Application of Kalman Filter in Two Rounds of Self-Balancing Walking Vehicle Attitude Detection

Zhang Tuanshan, He Ying

(College of Electronic Information, Xi’an Polytechnic University, Xi’an 710048,China)

The two rounds of self-balancing walking vehicle attitude detection system is introduced; MENS accelerometer and gyroscope combine with Embedded microcontroller are used.In view of accelerometer and gyroscope measurement respectively with noise and random drift error,kalman filter is used to realize sensors data fusion. In this way the sensor measurement error is compensated and the optimum estimate of car body posture is obtained.The algorithm is ported to the microcontroller of attitude detection system,the experimental results show that Kalman information fusion can improve the system detection accuracy effectively.

attitude detection；STM32F103RET6；Kalman filter；data fusion

TP391

A

珍

2013-11-29)