基于視覺(jué)的雙輪競(jìng)速自平衡小車(chē)的設(shè)計(jì)

符長(zhǎng)友, 李 政, 王銘亮, 徐 飛

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

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

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

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

1 工作原理

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

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

圖1 自平衡車(chē)受力分析圖

1.1 倒立擺分析

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

(1)

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

(2)

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

1.2 卡爾曼濾波

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

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

(3)

卡爾曼濾波測(cè)量修正方程為:

(4)

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

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

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

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

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

3.1 微處理器

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

圖3 TMS320F28235設(shè)計(jì)圖

3.2 傾角傳感器

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

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

3.3 Wi-Fi通信模塊

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

3.4 視覺(jué)檢測(cè)模塊

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

4 程序設(shè)計(jì)

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

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

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

4.2 傾角子程序

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

voidAngle_Dip(void)

{//---加速度計(jì)算---

//范圍為2g時(shí),換算為16384 LSB/g

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

//若x>=sinx,則乘以1.3作適當(dāng)放大

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

g_AcceData_Y=Acce_y;

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

零點(diǎn)偏移,以計(jì)算角度(弧度)

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 卡爾曼濾波子程序

卡爾曼濾波以實(shí)現(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ù)測(cè)試結(jié)果

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

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

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

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

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

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

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

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

6 結(jié)語(yǔ)

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