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

　　4.3 互補濾波算法

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

　　互補濾波算法公式為：

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

　　4.4 小車平衡控制算法

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

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

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

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

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

　　5.結語

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