教你用互補(bǔ)濾波算法測量兩輪平衡車的姿態(tài)角度
發(fā)布時(shí)間:2015-02-03 責(zé)任編輯:sherryyu
【導(dǎo)讀】本文基于互補(bǔ)濾波算法,設(shè)計(jì)了兩輪平衡車姿態(tài)角度測量電路與數(shù)據(jù)處理算法,設(shè)計(jì)了信號濾波預(yù)處理,利用互補(bǔ)濾波算法融合兩種傳感器數(shù)據(jù),分析了互補(bǔ)濾波算法中關(guān)鍵參數(shù)的計(jì)算方法。并將此方法應(yīng)用于兩輪平衡車角度測量,進(jìn)行了驗(yàn)證性試驗(yàn),給出了實(shí)驗(yàn)測試數(shù)據(jù)。
兩輪平衡車具有廣闊的應(yīng)用前景, 使其成為了當(dāng)前研究的熱點(diǎn)。其中,兩輪平衡車的姿態(tài)角度測量是研究的關(guān)鍵問題之一。姿態(tài)角度測量是兩輪平衡車運(yùn)行和控制實(shí)現(xiàn)的前提。姿態(tài)角度測量的精度和速度,將直接影響兩輪平衡車控制算法的穩(wěn)定性和可靠性。隨著慣性測量元件的微型化與微處理器運(yùn)算能力的提高,兩輪平衡車姿態(tài)測量普遍采用低成本的慣性測量組合元件(Inertial Measurement Uint,IMU),結(jié)合微處理器數(shù)據(jù)處理算法實(shí)現(xiàn)高精度的姿態(tài)測量。IMU 主要由低成本的MEMS 陀螺儀和三軸加速度計(jì)組成。MEMS 陀螺儀有自主性好、功耗低、機(jī)電性能好易集成等優(yōu)點(diǎn)。但是,MEMS 陀螺儀具有溫度漂移特性,其測量誤差會隨著時(shí)間的累加而不斷的累積,從而影響測量精度。加速度計(jì)會受到平衡車振動的影響,混疊額外的振動量干擾。所以單一的傳感器測量難以得到精確的姿態(tài)角度。需采用多傳感器信號融合的方法,來獲得準(zhǔn)確的姿態(tài)角度量。
多傳感器數(shù)據(jù)的融合方法有神經(jīng)網(wǎng)絡(luò)、小波分析、卡爾曼濾波等姿態(tài)解算算法,但這些方法建立穩(wěn)定可靠的更新方程通常具有較高的階數(shù),且計(jì)算量大,不適合于低運(yùn)算能力系統(tǒng)的實(shí)時(shí)計(jì)算。相比以上方法,互補(bǔ)濾波算法對處理器運(yùn)算速度要求不高,且簡單可靠。本文基于互補(bǔ)濾波算法,設(shè)計(jì)了兩輪平衡車姿態(tài)角度測量電路與數(shù)據(jù)處理算法,設(shè)計(jì)了信號濾波預(yù)處理,利用互補(bǔ)濾波算法融合兩種傳感器數(shù)據(jù),分析了互補(bǔ)濾波算法中關(guān)鍵參數(shù)的計(jì)算方法。并將此方法應(yīng)用于兩輪平衡車角度測量,進(jìn)行了驗(yàn)證性試驗(yàn),給出了實(shí)驗(yàn)測試數(shù)據(jù)。
1 姿態(tài)角度測量原理
沿平衡車3 個(gè)機(jī)體軸即直立時(shí)正前、正右、正上方向定義為x、y、z 三軸參考坐標(biāo)系。所受的3 軸重力加速度分量定義為gx、gy、gz。假設(shè)兩輪平衡車處于靜止或勻速運(yùn) 行的狀態(tài)。得到重力加速度與平衡車姿態(tài)角度的關(guān)系如式1所示:
其中,Cbn為慣性坐標(biāo)系到載體坐標(biāo)系的變換矩陣;θ 為俯仰角;φ為橫滾角;g 為重力加速度; 可以通過測量重力加速度分量gx、gy、gz,計(jì)算出平衡車俯仰角θ1和橫滾角φ1估計(jì)值
若使用陀螺儀來測量平衡車姿態(tài)角度,設(shè)陀螺儀測量載體相對慣性坐標(biāo)系的x、y、z 三軸旋轉(zhuǎn)角速度分別為ωx、ωy、 ωz。并定義0 時(shí)刻平衡車直立靜止??傻玫礁┭鼋?theta;2和橫滾角φ2估計(jì)值與ωx、ωy之間的關(guān)系如式3 所示:
在實(shí)際應(yīng)用中,由于平衡車機(jī)體運(yùn)行時(shí)存在運(yùn)動加速度、測量噪聲, 以及陀螺儀本身存在漂移等因素的影響, 式(2)、(3)姿態(tài)角度測量方法失效,為了準(zhǔn)確的獲得姿態(tài)角度??蓪⒁陨系? 種姿態(tài)角度測量得到的姿態(tài)角度信息相融合。
2 慣性組合測量電路
該系統(tǒng)中慣性組合測量電路如圖1 所示, 由加速度計(jì)MMA7361、陀螺儀ENC-03 及放大電路組成。實(shí)現(xiàn)對加速度計(jì)和陀螺儀測得信號進(jìn)行放大。加速度計(jì)和陀螺儀信號經(jīng)放大,分別由angle 引腳和gyro 引腳輸出后,信號通過AD 采樣轉(zhuǎn)換為數(shù)字信號,傳遞到微控制器中,再利用互補(bǔ)濾波算法,得到姿態(tài)角度。
圖1 慣性組合角度測量電路圖
[page]
3 陀螺儀漂移和加速度傳感器數(shù)據(jù)的預(yù)處理
MEMS 陀螺儀的漂移誤差由常值漂移、隨機(jī)漂移組成,漂移信號不符合平穩(wěn)、正態(tài)、零均值的時(shí)間序列特征。圖2 為陀螺儀采集到的一段數(shù)據(jù), 對其積分后得到的俯仰角θ2,表現(xiàn)為誤差不斷累加,逐漸增大。通過式(4)所示算法進(jìn)行均值扣除。
圖2 角速度與角度的曲線
去掉陀螺儀信號的均值(即常值分量)后,這樣陀螺儀的漂移只含有隨機(jī)漂移,此時(shí)的陀螺儀信號將作為互補(bǔ)濾波融合的輸入。
加速度計(jì)受機(jī)體振動的影響,對其采用滑動均值濾波的方法對加速度傳感器原始數(shù)據(jù)進(jìn)行處理,濾波原理如式(5)所示:
圖3 濾波前后加速度計(jì)數(shù)據(jù)圖
[page]
4 基于互補(bǔ)濾波器的姿態(tài)角度測量設(shè)計(jì)
陀螺儀動態(tài)響應(yīng)特性優(yōu)良,解算姿態(tài)角時(shí),由于陀螺儀低頻漂移的影響,積分后低頻擾動會產(chǎn)生較大誤差;加速度計(jì)解算的姿態(tài)角會受到平衡車運(yùn)行中機(jī)體高頻振動的影響, 輸出角度中攜帶較大分量的高頻干擾。二者在頻域上具有互補(bǔ)特性,采用互補(bǔ)濾波器對這兩種傳感器數(shù)據(jù)融合,可提高姿態(tài)角度測量的精度和動態(tài)響應(yīng)的性能。
互補(bǔ)濾波器的基本原理圖如圖4 所示。
圖4 互補(bǔ)濾波器的原理圖
其中θ 為實(shí)際的角度值,ω 為陀螺儀測量的角速度, 互補(bǔ)濾波算法后估計(jì)的角度值為互補(bǔ)濾波算法后估計(jì)的角度值, 加速度計(jì)測量中引入的高頻噪聲n1,陀螺儀測量中引入的低頻噪聲n2,用低通濾波器G1(s)消除加速度計(jì)中的高頻噪聲n1,用高通濾波器G2(s)消除陀螺儀中的低頻噪聲n2。兩個(gè)濾波器的傳遞函數(shù), 被設(shè)計(jì)為(6)、(7)所示,圖4 結(jié)構(gòu)可化簡為圖5 結(jié)構(gòu)。
圖5 互補(bǔ)濾波器頻域圖
選用的濾波傳遞函數(shù)需滿足G1(s)+G2(s)=1,由加速度計(jì)得到θ1經(jīng)低通濾波器和陀螺儀得到θ2經(jīng)高通濾波器后的數(shù)據(jù)融合為角度估計(jì)值互補(bǔ)濾波算法后估計(jì)的角度值,適當(dāng)?shù)倪x取權(quán)重因子K 值,可以使系統(tǒng)中高、低通濾波器具有合適的截止率。得到穩(wěn)定的姿態(tài)角度。
由(8)式可看出,在小于截止頻率的低頻段,加速度計(jì)對姿態(tài)解算結(jié)果起主要作用;在大于截止頻率的高頻段,陀螺儀對姿態(tài)解算結(jié)果起主要作用。通過調(diào)整時(shí)間常數(shù),改變?yōu)V波器的截止頻率τ,實(shí)現(xiàn)對陀螺儀和加速度計(jì)權(quán)重的調(diào)整。
[page]
5 實(shí)驗(yàn)驗(yàn)證
為了驗(yàn)證上述設(shè)計(jì)方案的可行性, 利用直立兩輪小車為實(shí)驗(yàn)驗(yàn)證平臺。本實(shí)驗(yàn)使用慣性測量組合元件(IMU)中的慣性傳感器分別選用了ENC-03(測量范圍:±300(deg/s))陀螺儀,采樣頻率為1.25 kHz 和MMA7361 加速度計(jì)(測量范圍:±1.5 g)。角度更新頻率為1.25 kHz?;パa(bǔ)濾波器截止頻率為138 Hz。以俯仰角(θ)為例,進(jìn)行了測試。
5.1 角速度和角度用互補(bǔ)濾波算法融合的分析
把陀螺儀測得的角速度數(shù)據(jù)和加速度計(jì)測得的角度數(shù)據(jù)通過(10)式進(jìn)行融合后的波形如圖6 所示,從圖6 中可以看出, 經(jīng)互補(bǔ)濾波算法融合后得到角度消除了陀螺儀的漂移和加速度計(jì)的高頻擾動,可得以下結(jié)論:
1)單從陀螺儀獲取的角速度積分后得到的角度是不正確的, 要把加速度計(jì)測得的角度值和陀螺儀測得角速度積分后的角度進(jìn)行互補(bǔ)濾波算法融合,提高角度精度。
2)經(jīng)互補(bǔ)濾波后陀螺儀的隨機(jī)漂移得到較為明顯的抑制,表現(xiàn)出了此互補(bǔ)濾波算法的有效性和優(yōu)越性。
圖6 角速度與角度的融合曲線圖
5.2 經(jīng)互補(bǔ)濾波處理后的角度與沒有經(jīng)處理后的角度比較分析
經(jīng)互補(bǔ)濾波處理后的角度與直接由加速度計(jì)測得的角度時(shí)域比較,對其互補(bǔ)濾波處理前后的信號進(jìn)行FFT 變換其頻譜圖如圖7 所示, 通過互補(bǔ)濾波算法可以降低隨機(jī)噪聲的干擾,可以使測得角度的波形更加的平滑。
圖7 融合前后角度比較圖
6 結(jié)論
文中分析了兩輪平衡車姿態(tài)角度解算時(shí)陀螺儀漂移和加速度計(jì)高頻擾動的影響,針對陀螺儀漂移和加速度計(jì)高頻擾動采用互補(bǔ)濾波融合加速度計(jì)和陀螺儀信號?;パa(bǔ)濾波能有效消除陀螺儀的漂移,抑制加速度計(jì)的高頻擾動,減少輸出姿態(tài)角的動態(tài)誤差,提高了角度測量精度,能夠滿足兩輪平衡車的姿態(tài)控制需要。實(shí)驗(yàn)結(jié)果表明了該方法的有效性,可推廣應(yīng)用于車載導(dǎo)航、兩輪平衡車、微小型機(jī)器人的姿態(tài)角度測量系統(tǒng)。
特別推薦
- 克服碳化硅制造挑戰(zhàn),助力未來電力電子應(yīng)用
- 了解交流電壓的產(chǎn)生
- 單結(jié)晶體管符號和結(jié)構(gòu)
- 英飛凌推出用于汽車應(yīng)用識別和認(rèn)證的新型指紋傳感器IC
- Vishay推出負(fù)載電壓達(dá)100 V的業(yè)內(nèi)先進(jìn)的1 Form A固態(tài)繼電器
- 康佳特推出搭載AMD 銳龍嵌入式 8000系列的COM Express緊湊型模塊
- 村田推出3225尺寸車載PoC電感器LQW32FT_8H系列
技術(shù)文章更多>>
- “扒開”超級電容的“外衣”,看看超級電容“超級”在哪兒
- DigiKey 誠邀各位參會者蒞臨SPS 2024?展會參觀交流,體驗(yàn)最新自動化產(chǎn)品
- 提前圍觀第104屆中國電子展高端元器件展區(qū)
- 高性能碳化硅隔離柵極驅(qū)動器如何選型,一文告訴您
- 貿(mào)澤電子新品推薦:2024年第三季度推出將近7000個(gè)新物料
技術(shù)白皮書下載更多>>
- 車規(guī)與基于V2X的車輛協(xié)同主動避撞技術(shù)展望
- 數(shù)字隔離助力新能源汽車安全隔離的新挑戰(zhàn)
- 汽車模塊拋負(fù)載的解決方案
- 車用連接器的安全創(chuàng)新應(yīng)用
- Melexis Actuators Business Unit
- Position / Current Sensors - Triaxis Hall
熱門搜索
濾波電感
濾波器
路由器設(shè)置
鋁電解電容
鋁殼電阻
邏輯IC
馬達(dá)控制
麥克風(fēng)
脈沖變壓器
鉚接設(shè)備
夢想電子
模擬鎖相環(huán)
耐壓測試儀
逆變器
逆導(dǎo)可控硅
鎳鎘電池
鎳氫電池
紐扣電池
歐勝
耦合技術(shù)
排電阻
排母連接器
排針連接器
片狀電感
偏光片
偏轉(zhuǎn)線圈
頻率測量儀
頻率器件
頻譜測試儀
平板電腦