基于飛思卡爾單片機的兩輪車控制系統(tǒng)設(shè)計

　　4.3 互補濾波算法

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

　　互補濾波算法公式為：

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

　　4.4 小車平衡控制算法

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

　　上式中ν_temp是控制小車平衡的電機控制變量，θ_new是當(dāng)前小車傾斜角度，ν_gyro是當(dāng)前小車的角速度。通過上位機觀察在不同P、D參數(shù)情況下的響應(yīng)曲線來確定具體的P、D參數(shù)。具體調(diào)節(jié)方法是首先改變P參數(shù)，得到一個超調(diào)量最小的響應(yīng)曲線；然后改變D參數(shù)，使得響應(yīng)曲線的反應(yīng)速度快并且超調(diào)量小。

　　4.5 電機速度及轉(zhuǎn)向控制算法

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

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

　　5.結(jié)語

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