基于飛思卡爾單片機的兩輪車(chē)控制系統設計

　　4.3 互補濾波算法

　　陀螺儀的動(dòng)態(tài)響應較好，可以檢測瞬態(tài)角度變化，但由于其本身存在累計漂移誤差，不適合長(cháng)時(shí)間單獨工作；加速度計的靜態(tài)性能較好，能夠準確地檢測靜態(tài)角度，但受動(dòng)態(tài)加速度影響較大，不適合測量動(dòng)態(tài)變化角度。因此本文采用互補濾波算法將陀螺儀和加速度計測量到的角度信息進(jìn)行融合，得到準確的車(chē)模傾角信息。

　　互補濾波算法公式為：

　　上式中θ_new是第n次濾波后的角度值，θ_old是第n次濾波前的角度值，α_gyro是陀螺儀濾波權重系數，在本系統中取值為0.97,β_acc是加速度計濾波權重系數，在本系統中取值為0.03,ω_gyro是第n次陀螺儀采樣測量的角速度值，θ_acc時(shí)第n次加速度計采樣測量的角度值。其中陀螺儀濾波權重系數和加速度計濾波權重系數純在如下關(guān)系：

　　4.4 小車(chē)平衡控制算法

　　本系統在小車(chē)平衡控制上采用了PD算法，小車(chē)平衡控制算法公式為：

　　上式中ν_temp是控制小車(chē)平衡的電機控制變量，θ_new是當前小車(chē)傾斜角度，ν_gyro是當前小車(chē)的角速度。通過(guò)上位機觀(guān)察在不同P、D參數情況下的響應曲線(xiàn)來(lái)確定具體的P、D參數。具體調節方法是首先改變P參數，得到一個(gè)超調量最小的響應曲線(xiàn)；然后改變D參數，使得響應曲線(xiàn)的反應速度快并且超調量小。

　　4.5 電機速度及轉向控制算法

　　由于本系統采用了兩個(gè)獨立的直流電機，并且在每個(gè)電機的齒輪上組裝了一只雙相編碼器作為速度反饋器件。因此兩個(gè)電機在控制上均是獨立的，這樣便可實(shí)現小車(chē)的前進(jìn)、后退及轉向。在轉向控制上，是通過(guò)控制左右電機差速的方法來(lái)實(shí)現。利用攝像頭采集賽道邊線(xiàn)位置信息，并通過(guò)賽道邊線(xiàn)位置信息來(lái)擬合賽道中心線(xiàn)的位置信息。由于賽道寬度是已知并且固定的，所以當攝像頭采集到左右兩邊賽道邊線(xiàn)時(shí)，采用中值法獲取賽道中心線(xiàn)；當攝像頭只能采集到左右一邊賽道邊線(xiàn)時(shí)，給當前賽道信息補上賽道寬度一半的量來(lái)獲取賽道中心線(xiàn)。最后通過(guò)計算，獲得小車(chē)所需的轉向比例。本系統在電機控制上采用了PI算法，左右電機控制算法公式為：

　　上式中Speed_New_Left是左電機速度，Speed_New_Right是右電機速度。K是小車(chē)轉向比例，即當小車(chē)在直道上時(shí)K=0,當小車(chē)需要左轉時(shí)K0,當小車(chē)需要右轉時(shí)K>0,K的取值由小車(chē)偏離賽道中心線(xiàn)的程度決定。Speed_Dev是上一次期望速度與實(shí)際速度的偏差。Speed_Dev_Add是速度偏差的累積。其中P、I參數的確定方法是首先調節P參數，通過(guò)上位機觀(guān)察響應曲線(xiàn)，直至得到反應快且超調量最小的響應曲線(xiàn)；然后調節I參數，以獲得滿(mǎn)意的響應曲線(xiàn)。

　　5.結語(yǔ)

　　本文介紹了一種自主循跡兩輪智能車(chē)的設計與實(shí)現。在動(dòng)態(tài)數據采集的基礎上，主要研究了對由加速度傳感器和陀螺儀采集的角度信號進(jìn)行融合的互補濾波算法，在路徑識別上對由攝像頭采集路徑信息的二值化處理方法。經(jīng)過(guò)實(shí)驗驗證，該系統具有工作穩定、循跡準確和運行速度快的特點(diǎn)。本系統方案也能擴展到類(lèi)似的機器人系統中，應用前景廣闊。