中型組機器人運動控制系統的FPGA設計

A、B兩相信號是相位相差90°的正交方波脈沖串，每個脈沖代表被測對象旋轉了一定的角度，A、B之間的相位關系則反映了被測對象的旋轉方向。在FPGA中設計4倍頻和鑒向電路，本設計采用2路輸出：一路輸出方向，另一路輸出脈沖，并對鑒向倍頻電路進行仿真，如圖5所示。

根據脈沖計數來測量轉速的方法有M法、T法以及M／T法3種。M法適用于高速測量場合，在低速時有較大的誤差；而T法，恰恰相反，在低速時測量準確，高速時誤差較大。

本設計采用文獻所描述的方法。該方法如圖6所示，設定參考閘門時間為固定的1個值，它只是作為參考信號和編碼信號共同確定實際的閘門時間。這樣確定的閘門時間為被測信號的整周期倍，能夠有效提高測量精度。則測得的速度為：

3．2 增量式PID控制原理及其FPGA實現
實際機器人的數學模型不可避免地存在一定程度的參數不確定性，且三輪全方位移動機器人的正交全向輪在行走時會與地面交替接觸而產生一些不確定摩擦轉矩，這些都會給機器人的精確控制帶來難度。為了對三輪全方位移動機器人進行精確的控制，系統采用PID速度閉環控制算法對機器人的3個全向輪進行速度調節。
令采樣周期為TS，將連續PID公式離散化后可得到數字PID算法表達式：


式中：k為采樣序號；u(k)為第k個采樣時刻的計算機輸出值；e(k)為第k個采樣時刻的計算機輸入誤差值；e(k-1)為第k-1個采樣時刻的輸入誤差值；Kp為比例系數；KI為積分系數；KD為微分系數。
這種算法雖然比較直觀，但由于是全量輸出，所以每次輸出均與過去的所有狀態有關，計算時要對e(k)進行累加，計算機運算量大。
于是產生了增量式PID算法：



上述公式(7)為增量式PID控制算法。只輸出控制增量，誤動作影響較小，且控制增量只與最近幾次的采樣值有關，容易通過加權處理獲得比較好的控制效果。
根據以上公式推導，結合FPGA的工作特點，本文設計了適合FPGA的增量式PID實現結構。
由圖7可以看出，增量式PID控制算法程序結構，只要最近的3個誤差采樣值就可以加權計算。這在FPGA內部完全可以并行實現，移位部分結構類似FIR濾波器的實現結構，難點是FPGA設計時對有符號數的熟練操作和保證累加器不能溢出。