符長友, 李 政, 王銘亮, 徐 飛

(1. 企業(yè)信息化與物聯(lián)網(wǎng)測控技術(shù)四川省高校重點實驗室, 四川 自貢 643000;2. 四川理工學院 計算機學院, 四川 自貢 643000)

雙輪競速自平衡小車實質(zhì)上是一個不穩(wěn)定的欠驅(qū)動系統(tǒng),其特點為強耦合、多參數(shù)變量、非線性、多輸入、多輸出[1-2]。該小車控制系統(tǒng)的平衡動態(tài)性與抗干擾性愈強,則系統(tǒng)的魯棒性愈優(yōu)良、系統(tǒng)的平穩(wěn)性就愈好[3]。無論是在工業(yè)領(lǐng)域還是軍事領(lǐng)域,雙輪自平衡車都有著極其重要的研究作用與研究價值。

在雙輪競速小車自平衡控制過程中,其姿態(tài)傾角測量的精準性、實時性決定了小車的控制精度與穩(wěn)定性。由于雙輪平衡小車的車體抖振與穩(wěn)態(tài)誤差,若僅僅使用陀螺儀或加速度計對小車的姿態(tài)傾角實時監(jiān)測,則難以保證測量數(shù)據(jù)的精確性與可靠性[4]?？柭鼮V波是一種多傳感器數(shù)據(jù)融合算法,它將陀螺儀、加速度計數(shù)據(jù)進行優(yōu)化處理、深度融合,對抖振與穩(wěn)態(tài)誤差進行有效抑制,并對陀螺儀的漂移誤差與加速度計的動態(tài)誤差進行補償,便可得到高精度、高可靠性的運動姿態(tài)傾角[5-6]。

在競速中,雙輪自平衡小車需要準確、快速分辨出賽道的左右邊界。由于紅外測距、超聲波測距方法,受環(huán)境影響大且存在盲區(qū),測量結(jié)果往往誤差偏大。為此采用視覺傳感器,通過對比二值灰度圖像,精準、快速分辨出賽道的左右邊界,以確保小車在賽道內(nèi)快速、平穩(wěn)運動。

1 工作原理

為了使雙輪競速自平衡小車在賽道上快速、平穩(wěn)、精準運動,最關(guān)鍵的是要從賽道的二值圖像中準確、實時分辨出賽道左右邊線及位置,進而演算出偏差等數(shù)據(jù),以便更好地完成方向及轉(zhuǎn)向控制。

雙輪自平衡小車是一個復(fù)雜的動力學系統(tǒng),涉及多變量、不穩(wěn)定、參數(shù)不確定性、耦合、時變的非線性高階方程[7-8]。雙輪競速自平衡小車受力情況如圖1所示。

圖1 自平衡車受力分析圖

1.1 倒立擺分析

從力學上講,平衡小車就是一個倒立擺。車體可設(shè)置成長度為L、質(zhì)量為m的倒立擺,放置于可左右移動的車輪上。設(shè)外力干擾引起車體產(chǎn)生的干擾加速度為x(t),則沿著垂直于車體底座方向進行受力分析,可得車輪傾角θ、車輪加速度a(t)及外力干擾加速度x(t)三者之間的運動方程[9]:

(1)

經(jīng)過一系列化簡以及引入比例微分反饋,最終將得到2個系統(tǒng)極點,即:

(2)

若需系統(tǒng)穩(wěn)定,則這兩個系統(tǒng)極點只能位于s平面的左半部分。所以當k1>g、k2>0時,自平衡小車便可處于穩(wěn)定狀態(tài)[10-11]。

1.2 卡爾曼濾波

卡爾曼濾波是最小均方誤差估計的一種遞推算法,其核心思想是采用信號與偏差的狀態(tài)方程,利用上一狀態(tài)得到的估計值與當前狀態(tài)的測量值來推算下一狀態(tài)量,以求得當前狀態(tài)的最佳估計值[4]。

卡爾曼濾波狀態(tài)預(yù)測過程方程為：

(3)

卡爾曼濾波測量修正方程為:

(4)

2 系統(tǒng)架構(gòu)

基于視覺的雙輪競速自平衡小車能精準地測量車體的傾角、傾角角速度、驅(qū)動2個獨立電機以及負反饋閉環(huán)控制,與中控系統(tǒng)通過Wi-Fi進行數(shù)據(jù)通信。雙輪競速自平衡小車由傾角傳感器、電機驅(qū)動、微處理器、賽道檢測、LCD顯示、Wi-Fi通信等功能模塊構(gòu)成,通過傾角傳感器實現(xiàn)自身運動姿態(tài)數(shù)據(jù)的實時監(jiān)測與顯示、車身位置監(jiān)測、電機控制等,并把姿態(tài)數(shù)據(jù)通過Wi-Fi實時傳送給中控系統(tǒng),如圖2所示。

圖2 雙輪競速自平衡小車系統(tǒng)功能圖

中控系統(tǒng)是一款安裝在PC機的管理軟件,負責接收雙輪競速自平衡小車傳送來的姿態(tài)數(shù)據(jù)并在線顯示,同時也控制雙輪競速自平衡小車的運動方向與運動狀態(tài)。

3 主要功能模塊設(shè)計

3.1 微處理器

微處理器采用TMS320F28235,該芯片是32位浮點DSP控制器,主頻可高達150 MHz。其內(nèi)部采用高性能CMOS技術(shù),含16×16、32×32介質(zhì)控制(MAC)運算。采用IEEE-754單精度浮點運算單元,6通道DMA處理器,256 KB×16閃存,高達9個32位定時器[12]。其電路設(shè)計如圖3所示。

圖3 TMS320F28235設(shè)計圖

3.2 傾角傳感器

傾角傳感器實時監(jiān)測自平衡小車運動姿態(tài)的變化,從而及時知曉自平衡小車的運動狀態(tài)。傾角傳感器采用FXAS21002芯片。該芯片是一款低功耗、微型且?guī)?6-bit A/DC角速度的三軸MEMS陀螺儀傳感器,其內(nèi)部集成了低通濾波器,能有效抑制數(shù)字信號帶寬和噪聲。該芯片可配置±250/500/1000/2000(°)/s的動態(tài)量程,其精度最高為0.0625(°)/s[13]。其電路設(shè)計如圖4所示。

圖4 傾角傳感器設(shè)計圖

3.3 Wi-Fi通信模塊

Wi-Fi無線通信選用超低功耗的ESP-01模塊。該模塊采用高性能SoC—ESP8266,內(nèi)嵌1 MB Flash,128 KB SRAM,內(nèi)置TCP/IP協(xié)議棧、TR開關(guān)、balun、LNA、功率放大器,支持IEEE 802.11 b/g/n,支持STA/AP/STA+AP工作模式,支持AT指令[14]。

3.4 視覺檢測模塊

視覺檢測采用OV7670模塊。該模塊采用數(shù)字輸出型CMOS攝像頭,該攝像頭含有30萬像素的圖像感光芯片、3.6 mm焦距的鏡頭與鏡頭座,同時含有幀同步、行同步信號,以及FIFO(先進先出)模塊,能有效保證系統(tǒng)微處理器進行圖像采集控制[15]。

4 程序設(shè)計

4.1 程序設(shè)計流程圖

基于視覺的雙輪競速自平衡小車系統(tǒng)程序設(shè)計流程如圖5所示。

圖5 系統(tǒng)程序流程圖

4.2 傾角子程序

傾角子程序部分代碼如下:

voidAngle_Dip(void)

{//---加速度計算---

//范圍為2g時,換算為16384 LSB/g

//角度較小時,x=sinx得到角度(弧度), deg= rad*180/3.14

//若x>=sinx,則乘以1.3作適當放大

Acce_y=GetDataFXAS(ACCE_YOUT_H); //讀取y軸加速度

g_AcceData_Y=Acce_y;

Angle_ay = (Acce_y_g_fAcceZeroY) /16384; //去除

零點偏移,以計算角度(弧度)

Angle_ay = Angle_ay*1.2*180/3.14; //弧度轉(zhuǎn)換為度

......

}

4.3 角速度子程序

角速度子程序部分代碼如下:

voidSpeedAngleControl(unsigned char Enable)

{

floatf_PValue,f_EValue;

if(Enable==1)

{f_EValue=g_fSpeedSet-(g_PulseCountL+g_Pulse

CountR)/2.0;

f_PValue=f_EValue*SpeedControl_P+

(f_EValue-SpeedErr1)*SpeedControl_D;

SpeedErr1=f_EValue;

SpeedAngleSet=f_PValue/100.0;

if(SpeedAngleSet

......

}

4.4 卡爾曼濾波子程序

卡爾曼濾波以實現(xiàn)數(shù)據(jù)融合,其子程序部分代碼如下:

voidKalman(void)

{float Measure_x,Measure_y,Post_data0,Post_data1,

Post_data2,Post_data3,Predict_x,Predict_y,G_predict_x,G_predict_y;

{ Measure_x=Post_data0;

Measure_y=Post_data2;

Predict_x=Post_data0;

Predict_y=Post_data2;

return(init_Kalman(Post_data0,Post_data1,

Post_data2,Post_data3));

......

}

5 數(shù)據(jù)測試結(jié)果

(1) 傾角傳感器數(shù)據(jù)。雙輪競速自平衡小車在賽道上運行呈平穩(wěn)態(tài)時,傾角傳感器測量的姿態(tài)數(shù)據(jù)如圖6所示。

圖6 小車平穩(wěn)時的姿態(tài)數(shù)據(jù)圖

(2) 視覺傳感器數(shù)據(jù)。視覺傳感器OV7670檢測賽道左右邊線位置時的二值圖像及數(shù)據(jù)如圖7所示。

圖7 OV7670檢測賽道左右邊線數(shù)據(jù)圖

(3) 卡爾曼濾波效果。對陀螺儀、加速度計傳感器進行卡爾曼濾波處理后,最終數(shù)據(jù)融合后的曲線如圖8所示。

圖8 卡爾曼濾波后數(shù)據(jù)融合圖

(4) 中控系統(tǒng)管理界面。中控系統(tǒng)接收雙輪競速自平衡小車發(fā)來的姿態(tài)數(shù)據(jù),控制管理界面如圖9所示。

圖9 中控系統(tǒng)顯示運行姿態(tài)數(shù)據(jù)界面圖

6 結(jié)語

基于視覺的雙輪競速自平衡小車有效實現(xiàn)了自平衡運動姿態(tài)、運動方向、運動平穩(wěn)性的高效、精準控制,不但姿態(tài)傾角精準; 而且運動平穩(wěn)性好、轉(zhuǎn)彎半徑小——可達零轉(zhuǎn)彎半徑,前進后退切換自如,因而可以廣泛應(yīng)用于實驗教學和更多領(lǐng)域的研究。