3．4 速度PID控制算法
機器人采用經典的PID算法進行平衡控制。控制系統采用典型的雙閉環控制結構和前饋控制方法。速度環路設計有利于提高電機輸出速度精度。前饋控制主要是為了補償電機轉軸與車輪轉動軸以及傳動系統的間隙誤差，這些誤差是穩定的、可以測量的。當電機在正反轉變換時，控制系統可以檢測到變換方向，經過位置誤差環節調整后把確定的偏差補償量疊加到驅動器輸出端。這樣電機轉動的角度在原理基礎上增加了補償量，機械誤差得以修正。
采用PID控制算法公式如下：

其中，u(k)為當前調節器輸出量；u(k-1)為上一次調節器輸出量；△u(k)為當前控制增量；e(k)為當前控制誤差量；Kp為比例系數；KI為積分系數；Kd為微分系數。PID控制算法流程如圖7所示。圖中，ε為調試過程中設定的誤差值。

4 實 驗
利用該控制方法，對原理樣機進行了多次實驗。實驗中，在機器人正上方水平位置安裝陀螺儀，以采集機器人的傾倒角度數據。實際測量角度信息隨時間變化的曲線如圖8所示。通過測得數據分析可以看到，機器人大致可以穩定在0°附近，最大偏差為±2°。

結 語
本文設計了以ATmage128單片機為控制核心，采用紅外測距傳感器監控的雙輪自平衡機器人，并搭建實驗平臺進行了控制試驗。實驗結果表明，該機器人可以通過有效的控制保持在豎直狀態，并且在一定的角度范圍內達到平衡。