張團善,何穎

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

?

卡爾曼濾波在兩輪自平衡代步車姿態(tài)檢測中的應(yīng)用

張團善,何穎

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

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

姿態(tài)檢測；STM32F103RET6；卡爾曼濾波；數(shù)據(jù)融合

引 言

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

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

1 系統(tǒng)組成

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

圖1 姿態(tài)檢測系統(tǒng)框圖

1.1 加速度計

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

(1)

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

圖2 加速度計檢測原理圖

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

圖3 陀螺儀檢測原理圖

1.3 微控制器

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

2 卡爾曼濾波車體姿態(tài)信息融合

2.1 車體姿態(tài)模型

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

車體的俯仰角與俯仰角速度存在如下等式關(guān)系：

(2)

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

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

(3)

可得系統(tǒng)狀態(tài)方程為

試驗所得數(shù)據(jù)運用SPSS Statistics 19.0進行統(tǒng)計分析，作圖運用Origin 8.0軟件。

(4)

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

測量方程為:

(5)

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

2.2 卡爾曼濾波器設(shè)計

圖4 卡爾曼濾波算法框圖

(6)

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

(7)

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

卡爾曼濾波以最小均方誤差估計為準則，通過一串含有噪聲的測量數(shù)據(jù)，對系統(tǒng)的內(nèi)部狀態(tài)進行估計[6]。通過對上述算法的循環(huán)使用，可得到車體姿態(tài)的最優(yōu)估計。

3 實驗驗證

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

表1 卡爾曼濾波器參數(shù)設(shè)置

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

圖5 俯仰角濾波結(jié)果

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

圖6 角速率濾波結(jié)果

結(jié) 語

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

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

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

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

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

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

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

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)