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

　　4.3 互補(bǔ)濾波算法

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

　　互補(bǔ)濾波算法公式為：

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

　　4.4 小車平衡控制算法

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

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

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

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

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

　　5.結(jié)語

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