兩輪自平衡車姿態(tài)傳感器信息融合研究①

林 華， 徐宇寶, 唐家成, 崔昆峰, 張曉東, 曹昌勇

(皖西學(xué)院 機械與車輛工程學(xué)院，安徽 六安 237012)

0 引 言

自平衡車又稱體感車等，有獨輪和雙輪兩類，其設(shè)計思想來自于對倒立擺的研究，改變重心便可以實現(xiàn)車輛啟停、加減速、轉(zhuǎn)向等動作。兩輪平衡車是典型的欠驅(qū)動系統(tǒng)，是本質(zhì)不穩(wěn)定的、多變量和強耦合的非線性系統(tǒng)[1～2]。平衡車運行姿態(tài)信息即車模傾角和傾角速度等，實際中通常采用加速度傳感器來采集車身傾角、通過陀螺儀采集傾角速度。由于慣性MEMS傳感器的固有特性決定了測量值含有噪聲和漂移。為提高車?？刂频姆€(wěn)定性，必須對兩個姿態(tài)傳感器的信息進行信息融合以減小誤差，得到更準(zhǔn)確可靠的角度值[3～4]。全國大學(xué)生智能汽車競賽中設(shè)有兩輪直立自平衡車組別，以攝像頭或電感采集道路信息，姿態(tài)傳感器采集車身姿態(tài)信息，并由單片機進行處理，輸出控制兩個直流電機，在賽道中自主循跡并完成比賽。車模制作和調(diào)試難點主要是從機械、硬件電路及算法上實現(xiàn)和保障直立、方向和速度的穩(wěn)定高效自動控制。以競賽中電磁兩輪直立自平衡車為例，進行設(shè)計搭建、建立動力學(xué)模型、利用加速度計和陀螺儀采集姿態(tài)信息，分別用卡爾曼濾波和互補濾波進行信息融合，以得到更準(zhǔn)確的角度值，提高車模運行穩(wěn)定性與快速性。

1 電磁兩輪自平衡車的總體設(shè)計

電磁兩輪平衡車的整體設(shè)計框圖如圖1所示，實物圖如圖2所示。智能車賽道中央有一根電磁引導(dǎo)線，通有80kHz到120kHz的交變電流，從而產(chǎn)生交變磁場，電感在變化的磁場中產(chǎn)生感應(yīng)電動勢。車模上電感采集賽道信息、姿態(tài)傳感器采集車身傾角信息、編碼器采集速度等傳至KEAZ128單片機處理后輸出PWM波，控制直流電機旋轉(zhuǎn)使車模完成相關(guān)動作。還需要從以下幾個方面對智能車進行設(shè)計：

圖1 整體框圖

1)檢測部分：賽道檢測依靠車體前瞻上的5個10mH的電感，經(jīng)過匹配電容和檢波電路，將采集的信號經(jīng)OPA2350運算放大器放大后傳給KEAZ128單片機的AD檢測引腳，單片機通過AD檢測相應(yīng)電壓信息來判斷道路信息和智能車所處在賽道的位置。車模的運行直立姿態(tài)檢測選用MMA8451加速度計和MPU3050陀螺儀傳感器，安裝在車模的中軸線上并且盡量靠近車軸，兩傳感器采集的角度數(shù)據(jù)需要經(jīng)過融合得到準(zhǔn)確的角度。車模行走和轉(zhuǎn)向差速的車輪轉(zhuǎn)速采集選用512線編碼器。

圖2 車模實物圖

2)機械部分：使用智能車競賽官方提供的E車車模，搭建兩輪自平衡車車體，電磁桿前端伸出距車軸約40cm，在車模穩(wěn)定運行時電感距離地面約15cm，為了減輕前瞻重量采用單桿式結(jié)構(gòu)并進行特殊固定，還需對車模機械結(jié)構(gòu)設(shè)計和仔細調(diào)整，如主控電路板一體化設(shè)計[5]、輪胎和減速齒輪的調(diào)整、機械重心盡可能控制在靠近車輪軸心上方中點處等。

3)程序部分[5]：采用“速度環(huán)—角度環(huán)—角速度環(huán)”串級控制方法，設(shè)置1ms的中斷時間，并分步實現(xiàn)直立的1ms控制周期、方向的10ms控制周期、速度的20ms的控制周期以及電機的1ms控制輸出周期。直立環(huán)采用PD控制、速度環(huán)采用PI控制、方向環(huán)采用PD和補償控制。程序調(diào)試中采用山外串口藍牙模塊，通過串口藍牙將需要觀察的數(shù)據(jù)傳至PC機藍牙接收模塊，接收的數(shù)據(jù)在PC機串口調(diào)試助手軟件上進行顯示以進行程序調(diào)試和數(shù)據(jù)分析。

2 自平衡車動力學(xué)建模

兩輪自平衡車的數(shù)學(xué)模型可以通過實驗法建立但較難，最好采用解析法建模，方法一般有經(jīng)典力學(xué)分析法、分析動力學(xué)方法[6]和拉格朗日方法[7]。通過分析平衡車的物理結(jié)構(gòu)以及在平衡瞬間的力平衡，求得力學(xué)平衡方程，經(jīng)變換后可建立其數(shù)學(xué)模型。將車簡化成重心處擺長為L、質(zhì)量為m的簡單倒立擺，放置在可前后轉(zhuǎn)動的車輪上，如圖3所示。

圖3 自平衡車簡化模型

對車輪進行受力分析，列力和力矩平衡方程[6]，化簡可得：

(1)

式中，M為車輪質(zhì)量；R為車輪半徑；x為車輪的前進位移；φ為車輪轉(zhuǎn)角，Tm為單個電機的驅(qū)動轉(zhuǎn)矩；H為車輪水平方向作用力。

對擺桿進行水平和垂直方向受力分析可得平衡方程以及轉(zhuǎn)矩方程。當(dāng)θ角度較小時，進行線性化處理，忽略二次分量，可得兩輪平衡車的數(shù)學(xué)模型為[6]：

(2)

(3)

整理后可得：

(4)

式中，x為左右輪的平均位移即車模位移；θ為擺角；m為除輪子以外的車體質(zhì)量；Jφ、JP為輪和擺的轉(zhuǎn)動慣量；T為左右輪對車身產(chǎn)生的轉(zhuǎn)矩之和。

上式是關(guān)于車體傾角和直線運行位移的微分方程。可以看出，對車模的傾角及其角速度進行控制是其直立穩(wěn)定的關(guān)鍵。上式中系統(tǒng)，有一個在s平面右半平面的極點，因此直立車是不穩(wěn)定的，需對小車系統(tǒng)引入一個角度的比例微分反饋量，構(gòu)成一個可控的閉環(huán)系統(tǒng)。在本文小車中，車模的各項參數(shù)具體為：m=820.5g；M=34.1g；R=32.5mm；L=14mm。

3 車身姿態(tài)信息融合

數(shù)據(jù)融合是將某一目標(biāo)的多源信息進行融合,形成比單一傳感器更精確、更完全的估計和判決,即對各個傳感器在空間或時間上冗余或互補的數(shù)據(jù),依據(jù)某種準(zhǔn)則進行組合,以獲得對被測對象的一致性描述或理解。數(shù)據(jù)融合廣泛用于需要確定移動物體的姿態(tài)和位置估計中。通常有貝葉斯估計、卡爾曼濾波、神經(jīng)網(wǎng)絡(luò)、小波變換、互補濾波等[4]。

自平衡車行走的基礎(chǔ)是直立控制，主要就是消除車身的傾斜角度。驅(qū)動電機帶動車輪朝著車模傾斜的方向轉(zhuǎn)動，增加一個額外恢復(fù)力，并控制使其加速度大于g，即可維持平衡[1]。為了使車子能夠盡快地在垂直位置穩(wěn)定下來，還需要增加阻尼力。因此實時角度檢測至關(guān)重要，本車模姿態(tài)檢測選用MMA8451加速度計傳感器檢測角度，用MPU3050陀螺儀傳感器檢測角速度。

加速度計長時間采集數(shù)據(jù)較準(zhǔn)確，靜態(tài)性能好，適合于靜態(tài)測量傾角，而車模運行中的振動會引起加速度計產(chǎn)生瞬時尖峰毛刺，且無法排除動態(tài)加速度對輸出數(shù)據(jù)的影響，使得測量數(shù)據(jù)瞬時存在較大誤差。陀螺儀的動態(tài)性能好，但其有靜態(tài)漂移，且陀螺儀的數(shù)據(jù)需要經(jīng)過積分才能得到角度值，積分勢必帶來累積誤差。

兩個傳感器各有優(yōu)勢，也各有劣勢，故要得到較為準(zhǔn)確的直立姿態(tài)角度值，單純靠加速度計或陀螺儀測量是不夠的，需要對傳感器的輸出信息進行有效融合。分別利用卡爾曼濾波和互補濾波進行車模姿態(tài)信息融合，以得到可靠的角度。

3.1 卡爾曼濾波

1960年，卡爾曼發(fā)表了用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文[8]?？柭鼮V波可以在未知模型性質(zhì)的情況下，由上一時刻的狀態(tài)值和當(dāng)前的狀態(tài)觀測值來估計當(dāng)前狀態(tài)的估計值，從含有噪聲的測量值中得到系統(tǒng)狀態(tài)的最優(yōu)估計。它提供了一種高效可計算的方法來估計過程的狀態(tài)，并使估計均方誤差最小[9]?？柭鼮V波器結(jié)構(gòu)如下圖4所示。

對于離散域線性系統(tǒng)：

x(k)=Ax(k-1)+B(u(k)+w(k))

(5)

yv(k)=Cx(k)+v(k)

(6)

上一時刻的協(xié)方差為：

P(k)=AP(k-1)AT+Q

(7)

圖4 卡爾曼濾波器結(jié)構(gòu)圖

計算卡爾曼增益：

(8)

系統(tǒng)k時刻的最優(yōu)估計為：

x(k)=Ax(k-1)+Mn(k)(yv(k)-CAx(k-1))

(9)

更新k狀態(tài)下的協(xié)方差為：

P(k)=(In-Mn(k)C)P(k)

(10)

系統(tǒng)輸出方程為：

ye(k)=Cx(k)+v(k)

(11)

式中，w(k)為過程噪聲信號，v(k)為測量噪聲信號。Q為系統(tǒng)噪聲協(xié)方差陣，R測量噪聲協(xié)方差。

結(jié)合卡爾曼濾波融合原理，本研究中可利用加速度計傳感器的姿態(tài)數(shù)據(jù)輸出來糾正陀螺儀傳感器的姿態(tài)數(shù)據(jù)漂移，以提高智能車姿態(tài)的多傳感器數(shù)據(jù)采集精度。選取陀螺儀作為先驗估計變量，加速度計作為觀測變量，將陀螺儀得到的角速度信號和加速度計得到的角度信號送入卡爾曼濾波器中進行信號的融合，將融合后的更加精確的角度值信號進行PID處理后輸出至驅(qū)動電機即可實現(xiàn)車體的直立等動作。融合過程如圖5所示。

圖5 卡爾曼濾波融合過程示意圖

手持智能車，給車體一個傾斜角度階躍輸入，反復(fù)調(diào)試，當(dāng)調(diào)試參數(shù)：Q_acce=0.001；Q_gyro=0.003；R=2.5；dt=0.005時，提取傳感器的輸出信號數(shù)據(jù)，在MATLAB進行數(shù)據(jù)后處理，得到波形如圖6所示。

圖中可以看出，未濾波融合前的加速度計輸出求出的角度比較準(zhǔn)確，長時間獲取也不會有漂移，但是在短時間內(nèi)受噪聲影響大，波形含有高頻振蕩干擾，瞬時數(shù)據(jù)誤差大，若直接采用會導(dǎo)致車身的穩(wěn)定性差。陀螺儀的輸出角度值數(shù)據(jù)在圖上明顯可以看出和加速度計的值不重合，輸出角度有零漂。

圖6 階躍輸入經(jīng)Kalman融合后波形

圖中還可看出，在階躍輸入時，加速度計輸出響應(yīng)會經(jīng)過較長時間的過渡過程才能穩(wěn)定下來，而陀螺儀信號的角度計算結(jié)果響應(yīng)速度較快。兩個角度信號經(jīng)過參數(shù)調(diào)試好的卡爾曼濾波器進行信息融合后的波形曲線無高頻噪聲、變化平滑、響應(yīng)快速、濾波效果明顯, 波形能長時間較好地跟蹤陀螺儀信號且能糾正其零漂值，得到了車模傾角的最優(yōu)估計值。進一步保障了車模的直立穩(wěn)定性，使得車模能在復(fù)雜賽道中不斷變換姿態(tài)以高速穩(wěn)定完成比賽。

3.2 互補濾波

互補濾波器由三部分組成：低通濾波器、高通濾波器和積分器，分一階互補濾波和二階互補濾波?；パa濾波算法是根據(jù)測量同一個信號的不同傳感器相反的噪聲特性，從頻域來分辨和消除測量噪聲。討論一階互補濾波，一階互補濾波中低通和高通濾波器傳遞函數(shù)分別為[10]：

(12)

滿足：G1(s)+G2(s)=1，不同傳感器的測量值信號：

y1=x+u;y2=x+v

(13)

式中，u、v分別為高頻和低頻噪聲。將測量值y1通過互補濾波的低通濾波器消去高頻噪聲，將測量值y2通過互補濾波的高通濾波器消去低頻噪聲。相加后得到融合后的估計值為：

X(s)=Y1(s)G1(s)+Y2(s)G2(s)=

Y1(s)G1(s)+Y2(s)(1-G1(s))

(14)

由前面的分析可知，加速度計的高頻加速度干擾噪聲較多、誤差大；陀螺儀的低頻噪聲較多、誤差大。分別將加速度計的測量信號通過低通濾波器，將陀螺儀的測量信號通過高通濾波器后進行疊加。

可以將一階互補濾波公式進行離散化，得到程序公式為：

anglen=A*(anglen-1+angle_gyro*dt)+

(1-A)*angle_acce)

(15)

系統(tǒng)中dt=0.005，經(jīng)多次調(diào)試后，選擇A=0.99時濾波效果較好。在給定一個角度階躍輸入后，經(jīng)互補濾波融合后的波形如圖7所示。由圖中可看出，在經(jīng)過一階互補濾波融合后的角度誤差在1°左右，濾去了加速度計的高頻噪聲，曲線平滑、過沖量小、響應(yīng)迅速且輸出穩(wěn)定。

圖7 階躍輸入經(jīng)互補濾波融合后波形圖

圖8 階躍輸入時Kalman濾波和互補濾波對比波形圖

3.3 卡爾曼和互補濾波融合效果對比

給車模一個階躍角度輸入，觀察卡爾曼濾波和一階互補濾波融合效果如圖8所示?？煽闯?，Kalman濾波后的效果與互補濾波幾乎一致，都能實現(xiàn)較好的濾波效果。但是在波形放大后可看到Kalman的曲線平滑度和跟隨抖動要略好于互補濾波，互補濾波在階躍響應(yīng)后的數(shù)據(jù)有少量的過沖現(xiàn)象，誤差要大于卡爾曼濾波融合。另外在調(diào)試中，手動給車模角度快速連續(xù)變化，也能觀察到Kalman濾波的效果也要優(yōu)于一階互補濾波。在實際中具體選擇哪種可以結(jié)合應(yīng)用場合、成本和單片機計算時效等綜合考慮選用。

4 結(jié) 語

以全國大學(xué)生智能汽車競賽中的電磁兩輪直立自平衡車為對象，設(shè)計制作了車模，建立了小車的數(shù)學(xué)模型，并研究了兩輪自平衡車的姿態(tài)融合方法：卡爾曼濾波和互補濾波。實驗結(jié)果表明：經(jīng)融合后的姿態(tài)信息有效補償了加速度計噪聲、陀螺儀漂移以及測量噪聲對的測量結(jié)果影響，使得姿態(tài)角度測量誤差大大減小，得到了跟蹤快速、輸出準(zhǔn)確的角度值。有效提高了平衡車穩(wěn)定性和快速性。

文中給出了參數(shù)調(diào)試方法并給出了效果較好的參數(shù)值，發(fā)現(xiàn)卡爾曼濾波的效果要優(yōu)于互補濾波。采用卡爾曼濾波融合的電磁兩輪自平衡車在省賽中以2.6m/s的平均速度完成了比賽；在國賽中常規(guī)賽道完成比賽速度為2.3m/s，在公路賽中實現(xiàn)直道平均速度2.7m/s。

文中經(jīng)驗對多傳感器信息融合研究和自平衡車相關(guān)設(shè)計制作有一定的參考意義。