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

（湖州師范學院工學院，浙江湖州313000）

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

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

（湖州師范學院工學院，浙江湖州313000）

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

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

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

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

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

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

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

從而保持電機平衡運動.

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

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

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

在程序設計過程中可采用累加的方法代替積分運算：

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

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

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

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

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

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

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

卡爾曼濾波算法估計值方程為：

Fig. 3 The total structure diagram of data fusion algorithm

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

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

3 實驗與結(jié)果分析

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

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

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

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

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

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

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

［12］傅忠云，朱海霞，孫金秋，等.兩輪載人自平衡車姿態(tài)測量單元設計［J］.科學技術(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

［責任編輯 高俊娥］

2016-03-05

國家級大學生創(chuàng)新創(chuàng)業(yè)訓練計劃項目（201510347009）.

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