數(shù)據(jù)融合的兩輪自平衡車姿態(tài)角度檢測

余慶輝，蔡志端，張文莉，程似錦，吳海霞

（湖州師范學(xué)院工學(xué)院，浙江湖州313000）

數(shù)據(jù)融合的兩輪自平衡車姿態(tài)角度檢測

余慶輝，蔡志端，張文莉，程似錦，吳海霞

（湖州師范學(xué)院工學(xué)院，浙江湖州313000）

根據(jù)陀螺儀和加速計(jì)兩類慣性傳感器在姿態(tài)角度檢測中的優(yōu)缺點(diǎn)，提出了將多傳感器數(shù)據(jù)加權(quán)和卡爾曼濾波相結(jié)合的兩輪自平衡車姿態(tài)角度檢測數(shù)據(jù)融合算法.首先對傳感器輸出進(jìn)行分組加權(quán)運(yùn)算，然后采用卡爾曼濾波算法對加權(quán)計(jì)算結(jié)果進(jìn)行估計(jì).實(shí)驗(yàn)結(jié)果表明，該檢測方法可行有效，相對單一傳感器測量，可提升姿態(tài)角度檢測精度.

自平衡車；角度測量；數(shù)據(jù)加權(quán)；卡爾曼濾波；數(shù)據(jù)融合

兩輪自平衡車是一種可自行保持平衡運(yùn)動(dòng)的運(yùn)輸載具.其以輕巧、便捷、節(jié)能、轉(zhuǎn)向靈活等優(yōu)點(diǎn)得到了廣泛應(yīng)用.兩輪自平衡車是一種強(qiáng)耦合、欠驅(qū)動(dòng)、類似于倒立擺的非線性系統(tǒng)［1-2］.平衡車在運(yùn)動(dòng)過程中控制器根據(jù)車體姿態(tài)角實(shí)時(shí)向兩車輪驅(qū)動(dòng)電路發(fā)控制信號(hào)，控制車輪的速度與方向，以便通過兩輪差速保持車體平衡運(yùn)行狀態(tài)［3-4］.因此，兩輪自平衡車姿態(tài)角的測量是車體平衡控制的基礎(chǔ)與關(guān)鍵.近年來，許多學(xué)者對兩輪自平衡車控制及其姿態(tài)角檢測技術(shù)進(jìn)行了大量研究.文獻(xiàn)［1-5］研究了平衡車的控制技術(shù)；文獻(xiàn)［6-12］研究了平衡車姿態(tài)角的檢測技術(shù)，且都取得了有效的成果.本文主要研究車體姿態(tài)角的傳感器測量方法及基于數(shù)據(jù)融合算法的最優(yōu)值估計(jì)技術(shù)，并進(jìn)行實(shí)驗(yàn)研究與分析.

1 兩輪自平衡車姿態(tài)角度檢測方法

圖1為本文所設(shè)計(jì)的兩輪自平衡車控制系統(tǒng)圖.控制系統(tǒng)主要由姿態(tài)檢測傳感器組、單片機(jī)核心控制芯片、電機(jī)驅(qū)動(dòng)電路、車輪電機(jī)、光電編碼器等組成.系統(tǒng)采用陀螺儀和加速度計(jì)傳感器組同時(shí)獲取車體姿態(tài)角度，并送單片機(jī)A/D轉(zhuǎn)換端口，完成角度值的采集.單片機(jī)通過程序設(shè)計(jì)數(shù)據(jù)融合算法，計(jì)算車體姿態(tài)角的最優(yōu)估計(jì)值，以提升姿態(tài)角檢測精度.同時(shí)單片機(jī)根據(jù)姿態(tài)角度檢測值，通過PID控制算法計(jì)算電機(jī)控制信號(hào)，并通過PWM口輸出脈寬調(diào)制信號(hào)至電機(jī)驅(qū)動(dòng)電路，實(shí)現(xiàn)對兩輪電機(jī)速度的控制，并最終實(shí)現(xiàn)車體平衡運(yùn)動(dòng).在電機(jī)平衡控制過程中，需要知道車體姿態(tài)角與車輪電機(jī)控制信號(hào)間的關(guān)系.圖2為車體傾斜運(yùn)動(dòng)過程的示意圖.假設(shè)平衡車質(zhì)量為m，重心高度為l，車體姿態(tài)角為θ并以α加速度向前運(yùn)動(dòng).為使θ傾角度變?yōu)榱?，需要外力F產(chǎn)生加速度x（t）［67］.姿態(tài)角度θ一般比較小，可近似認(rèn)為sinθ≈θ，車體運(yùn)動(dòng)方程為［67］：

設(shè)車輪角速度為ω（t），根據(jù)文獻(xiàn)［3-4］得：

由（2）式和（3）式可知，通過控制電機(jī)速度ω，使得

從而保持電機(jī)平衡運(yùn)動(dòng).

Fig. 1 The control structure diagram of two- wheel self- balanced vehicle

Fig. 2 Diagrammatic sketch of two- wheel self- balanced vehicle

本文陀螺儀采用AD公司的ADIS6100，加速度計(jì)采用飛思卡爾的MMA7260.陀螺儀的輸出為旋轉(zhuǎn)角速度值，對角速度進(jìn)行積分運(yùn)算得到角度值：

在程序設(shè)計(jì)過程中可采用累加的方法代替積分運(yùn)算：

加速度計(jì)的輸出是重力加速度在敏感軸上的分量值，通過分量值反三角函數(shù)計(jì)算，可測得角度值：

2 姿態(tài)角度數(shù)據(jù)融合算法

在角度檢測過程中，由于陀螺儀和加速度計(jì)分別存在低頻積分誤差干擾和高頻振動(dòng)干擾，單獨(dú)使用其中一類傳感器進(jìn)行姿態(tài)角度檢測都會(huì)產(chǎn)生較大的測量誤差，所以本文采用多類多傳感器同時(shí)檢測角度，并通過數(shù)據(jù)融合算法估算更精確的車體姿態(tài)角度，以彌補(bǔ)單類、單個(gè)傳感器測量誤差.圖3為本文所提出的姿態(tài)角度數(shù)據(jù)融合算法框圖.首先在車體上同時(shí)安裝多個(gè)陀螺儀和加速度傳感器，再對同類多傳感器進(jìn)行數(shù)據(jù)加權(quán)處理得到各類傳感器的初始估計(jì)值，最后采用卡爾曼濾波算法對兩類傳感器初始估計(jì)值進(jìn)行最終估計(jì)，得到兩輪自平衡車姿態(tài)角度的最終估計(jì)值.多傳感器數(shù)據(jù)加權(quán)處理過程中加權(quán)值由各傳感器實(shí)時(shí)測量誤差進(jìn)行分配，分配方案為：

式中：λa、λb分別為a、b兩個(gè)傳感器加權(quán)值；Δθa、Δθb分別為兩個(gè)傳感器的測量誤差.

卡爾曼濾波是由R.E.Kalman提出的以最小均方誤差估計(jì)的實(shí)時(shí)遞推的最優(yōu)估計(jì)算法.其算法以系統(tǒng)當(dāng)前狀態(tài)值作為輸入量，以估計(jì)值作為輸出量.輸入輸出量之間由時(shí)間更新和觀測更新統(tǒng)一起來.由前一狀態(tài)計(jì)算得到的估計(jì)值和當(dāng)前狀態(tài)的測量實(shí)際值更新狀態(tài)量的估計(jì)，最終計(jì)算當(dāng)前狀態(tài)的最優(yōu)估計(jì)值.

設(shè)系統(tǒng)狀態(tài)方程為：

其中：A、B、H為系統(tǒng)系數(shù)矩陣；x（k）、x（k-1）分別為k和k-1時(shí)刻系統(tǒng)狀態(tài)量；u（k）、z（k）分別為輸入量與輸出量.

卡爾曼濾波算法估計(jì)值方程為：

Fig. 3 The total structure diagram of data fusion algorithm

卡爾曼濾波算法過程中的修正方程為：

其中：x^（k）為當(dāng)前最優(yōu)估計(jì)狀態(tài)值；x^（k，k-1）為由前一狀態(tài)計(jì)算得到的估計(jì)值；P為狀態(tài)對應(yīng)的協(xié)方差；Q為系統(tǒng)過程的協(xié)方差.

3 實(shí)驗(yàn)與結(jié)果分析

為了方便快捷獲取實(shí)驗(yàn)數(shù)據(jù)，在平衡車主控制器上接入ESP8266 WIFI無線模塊.實(shí)驗(yàn)過程中，無線通信模塊與計(jì)算機(jī)聯(lián)接通信，實(shí)驗(yàn)數(shù)據(jù)實(shí)時(shí)上傳至上位機(jī)軟件并保存.獲取實(shí)驗(yàn)數(shù)據(jù)后，通過Matlab對數(shù)據(jù)進(jìn)行分析處理，并對本文提出的算法進(jìn)行驗(yàn)證分析.圖4～圖6為實(shí)驗(yàn)結(jié)果.圖4為靜止?fàn)顟B(tài)和動(dòng)態(tài)下陀螺儀和加速度計(jì)兩類傳感器直接測量值在時(shí)間域中的變化曲線.其中，x1、x2分別為陀螺儀和加速度計(jì)在靜態(tài)下輸出角度值；x3、x4分別為動(dòng)態(tài)下陀螺儀和加速度計(jì)輸出角度值.由圖4可知，無論在靜態(tài)還是在動(dòng)態(tài)，傳感器的輸出都存在干擾誤差.陀螺儀存在低頻干擾誤差，加速度計(jì)存在高頻干擾誤差.所以，單獨(dú)使用一類傳感器進(jìn)行兩輪自平衡車姿態(tài)角測量是不夠精確的，必須同時(shí)應(yīng)用多傳感器并采用數(shù)據(jù)融合算法對車體姿態(tài)角進(jìn)行優(yōu)化估算，以提升角度測量精度.圖5和圖6分別為靜態(tài)和動(dòng)態(tài)下數(shù)據(jù)融合算法角度測量效果.其中，x1、x2為陀螺儀和加速度計(jì)直接測量值；x3為本文的最終估計(jì)值.實(shí)驗(yàn)結(jié)果表明，無論是在靜態(tài)還是動(dòng)態(tài)環(huán)境下，本文所提出的算法都能有效去除加速度計(jì)高頻毛刺干擾誤差和陀螺儀低頻誤差，提升車體姿態(tài)角測量精度.

Fig. 4 Angle curve diagram in static and dynam ic state

Fig. 5 Static angle test result after data fusion algorithm

4 結(jié)論

本文分析了兩輪自平衡車姿態(tài)角度測量的原理以及角度測量精度的提升技術(shù)，研究了結(jié)合多傳感器數(shù)據(jù)加權(quán)和卡爾曼濾波的數(shù)據(jù)融合最優(yōu)估計(jì)算法，并將其應(yīng)用于兩輪自平衡車姿態(tài)角的實(shí)時(shí)檢測中.通過Matlab軟件對本文提出的算法進(jìn)行實(shí)驗(yàn)驗(yàn)證與分析.結(jié)果表明，本文提出的車體姿態(tài)角度檢測方法與數(shù)據(jù)融合算法可行有效，能夠高精度地測量出平衡車姿態(tài)角度，為平衡車控制性能的提升提供了良好的條件.

Fig. 6 Dynam ic angle test result after data fusion algorithm

［1］LIN S C，TAI C H，HUANG S C.Adaptive robust self-balancing and steering of a two wheeled human transportation vehicle［J］.Intel Robot System，2011，27（2）：103-123.

［2］XU J X，GUO Z Q，LEE T H.Design and implementation of a takagi-sugeno type fuzzy logic controller on a twowheeled mobile robot［J］.IEEE Transaction on Industrial Electronics，2013，60（12）：5 717-5 728.

［3］LIN S C，TAI C H，HUANG S C.Development of self-balancing human transportation vehicle for the teaching of feedback control［J］.IEEE Transactions on Education，2009，26（2）：1-52.

［4］梁文宇，周惠興，曹榮敏，等.雙輪載人自平衡控制系統(tǒng)研究綜述［J］.控制工程，2010，17（1）：139-144.

［5］武俊峰，孫雷.兩輪自平衡機(jī)器人的控制方法研究［J］.哈爾濱理工大學(xué)學(xué)報(bào)，2014，34（6）：22-26.

［6］孟騰，章政.基于陀螺儀與加速度計(jì)的二輪自平衡控制系統(tǒng)設(shè)計(jì)［J］.傳感器與微系統(tǒng)，2014，33（10）：61-64.

［7］馬思遠(yuǎn)，魯庭勇，張麗君.兩輪自平衡車運(yùn)動(dòng)姿態(tài)的測量和控制［J］.測控技術(shù)，2015，34（4）：71-73.

［8］郜園園，阮曉鋼，寧洪軍，等.兩輪自平衡機(jī)器人慣性傳感器濾波問題的研究［J］.傳感器學(xué)報(bào)，2010，23（5）：696-700.

［9］姜香菊，劉二林.兩輪自平衡機(jī)器人角度檢測數(shù)據(jù)融合算法［J］.計(jì)算機(jī)工程與應(yīng)用，2013，49（8）：203-205.

［10］張吉昌，程凱，鄭榮兒.單軸雙輪自平衡車姿態(tài)檢測方案設(shè)計(jì)［J］.中國海洋大學(xué)學(xué)報(bào)，2009，39（9）：467-470.

［11］魏文，段晨東，高精隆，等.基于數(shù)據(jù)融合的兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)［J］.電子技術(shù)應(yīng)用，2013，39（5）：79-81.

［12］傅忠云，朱海霞，孫金秋，等.兩輪載人自平衡車姿態(tài)測量單元設(shè)計(jì)［J］.科學(xué)技術(shù)與工程，2015，15（15）：66-71.

Attitude Measurement of Two-Wheel Self-Balanced Vehicle Based on Data Fusion Algorithm

YU Qinghui，CAI Zhiduan，ZHANG Wenli，CHENG Sijin，WU Haixia
（School of Engineering，Huzhou University，Huzhou 313000，China）

Attitude measurement is the critical step in two-wheel self-balancing vehicle control. The advantages and disadvantages of the gyroscope and accelerometer sensors are analyzed.Data fusion algorithm for Attitude angle measurement is proposed based on multi-sensor data weighting method and kalman filter algorithm.Firstly，the output of the sensor is processed by weighting operation.Lastly，a kalman filter is proposed based on the measurement result.The experimental results show that the algorithm in the article is effective and more reliable than single sensor.The algorithm can enhance the precision of the self-balanced vehicle attitude angle measurement.

self-balanced vehicle；angle measurement；data weighting；Kalman filter；data fusion

TP291

A

1009-1734（2016）04-0061-04

［責(zé)任編輯 高俊娥］

2016-03-05

國家級大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目（201510347009）.

蔡志端，講師，研究方向：永磁同步電機(jī)控制技術(shù)、功率變換器故障診斷與容錯(cuò)控制.E-mail：czddule@163.com