數據融合的兩輪自平衡車姿態角度檢測

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

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

數據融合的兩輪自平衡車姿態角度檢測

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

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

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

自平衡車；角度測量；數據加權；卡爾曼濾波；數據融合

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

1 兩輪自平衡車姿態角度檢測方法

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

設車輪角速度為ω（t），根據文獻［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.陀螺儀的輸出為旋轉角速度值，對角速度進行積分運算得到角度值：

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

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

2 姿態角度數據融合算法

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

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

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

設系統狀態方程為：

其中：A、B、H為系統系數矩陣；x（k）、x（k-1）分別為k和k-1時刻系統狀態量；u（k）、z（k）分別為輸入量與輸出量.

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

Fig. 3 The total structure diagram of data fusion algorithm

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

其中：x^（k）為當前最優估計狀態值；x^（k，k-1）為由前一狀態計算得到的估計值；P為狀態對應的協方差；Q為系統過程的協方差.

3 實驗與結果分析

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

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

Fig. 5 Static angle test result after data fusion algorithm

4 結論

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

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］梁文宇，周惠興，曹榮敏，等.雙輪載人自平衡控制系統研究綜述［J］.控制工程，2010，17（1）：139-144.

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

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

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

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

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

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

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

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

國家級大學生創新創業訓練計劃項目（201510347009）.

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