基于視覺的雙輪競速自平衡小車的設計

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

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

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

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

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

1 工作原理

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

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

圖1 自平衡車受力分析圖

1.1 倒立擺分析

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

(1)

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

(2)

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

1.2 卡爾曼濾波

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

卡爾曼濾波狀態預測過程方程為：

(3)

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

(4)

2 系統架構

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

圖2 雙輪競速自平衡小車系統功能圖

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

3 主要功能模塊設計

3.1 微處理器

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

圖3 TMS320F28235設計圖

3.2 傾角傳感器

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

圖4 傾角傳感器設計圖

3.3 Wi-Fi通信模塊

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

3.4 視覺檢測模塊

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

4 程序設計

4.1 程序設計流程圖

基于視覺的雙輪競速自平衡小車系統程序設計流程如圖5所示。

圖5 系統程序流程圖

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; //弧度轉換為度

......

}

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

卡爾曼濾波以實現數據融合,其子程序部分代碼如下:

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 數據測試結果

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

圖6 小車平穩時的姿態數據圖

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

圖7 OV7670檢測賽道左右邊線數據圖

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

圖8 卡爾曼濾波后數據融合圖

(4) 中控系統管理界面。中控系統接收雙輪競速自平衡小車發來的姿態數據,控制管理界面如圖9所示。

圖9 中控系統顯示運行姿態數據界面圖

6 結語

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