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

摘 要:以RoboCup中型組足球機器人為實驗平臺，提出一種基于FPGA的全方位移動足球機器人運動控制系統的實現方法。首先分析和研究三輪全方位移動機器人的運動學特性，建立其運動控制模型，然后以FPGA為主要處理器，設計了PID速度閉環控制算法，實現了對機器人的精確控制。實驗發現，該設計方法具有很好的實時性，能夠對全方位移動機器人進行快速、準確的控制。

關鍵詞:RoboCup; 全方位移動機器人;FPGA; PID

中圖分類號:TN911; TP242 文獻標識碼:A

文章編號:1004-373X(2010)14-0127-04

Medium-sized Group of Robot Motion Control System Based on FPGA

WANG Quan-zhou， PEI Dong， TAO Zhong-xing， YANG Shuo， CUI Tao， LIU Ping-he

(College of Physics and Electronic Engineering，Northwest Normal University，Lanzhou 730070，China)

Abstract: Making the medium-sized group of RoboCup soccer robot as experimental platform， an implementation of the omni-directional mobile robot motion control system based on FPGA is provided. In order to achieve precise control of the robot， the kinematics characteristics of the omni-directional robot with three orthogonal-wheels is analyzed and studied， the robot motion control model is built， and the speed closed-loop PID control algorithm using FPGA as its main processor is designed. It is found that the design method of theFPGA-based omni-directional mobile robot motion control system has good real-time performance， and can control the omni-directional mobile robot rapidly and accurately.

Keywords: RoboCup; omni-directional mobile robot; FPGA; PID

0 引 言

目前，全方位移動機器人由于具有出色的靈活性，已經成為RoboCup中型組足球機器人比賽中最理想的選擇。而機器人的運動控制一直以來都是直接影響機器人性能的主要因素，也是移動機器人研究的熱點之一[1]。本文研究了一種用FPGA技術實現三輪全方位移動機器人運動控制系統的方法，與雙DSP結構[2]，DSP+CPLD結構[3]，以及DSP+專用集成電路結構等相比，該方法具有簡單可靠，擴展性強等特點。且FPGA設計簡單，使用方便，開發周期短，能夠實現真正的SOPC系統。

1 全方位移動機器人運動模型

設世界坐標系下機器人的速度為ε=[vx，vy，]，則當vx=0，vy≠0，=0時，機器人做前后方向的直線運動，當vx≠0，vy=0，=0時，機器人做左右方向的直線運動，當vx=0，vy=0，≠0時，機器人做自轉運動。

圖1中，ω1，ω2，ω3為3個主動輪的轉動角速度，R為全向輪半徑;L1，L2，L3為機器人車體中心到3組全向輪中心的水平距離，設有L1=L2=L3=L。α為前兩輪之間的夾角，另外2個夾角均為180°-α/2。則機器人坐標系下的速度到三輪速度之間的關系如下[4]:

ω1

ω2

ω3=1Rcos(α/2)sin(α/2)L

-10L

cos(α/2)-sin(α/2)Lvxvyω

(1)

由式(1)可以看到:知道了機器人在平面世界坐標系中的速度要求后，便可以得到主動輪的速度要求，進而對電機發出相應的控制信號。

2 運動控制方案

本系統總體設計思路[5]如圖2所示，首先通過RS 232接口，實現PC機與底層控制芯片FPGA的通信，FPGA在接收到相關的機器人坐標系下的速度后，將機器人坐標系下的速度值轉化成機器人3個全向輪子的角速度，將得到的角速度值計算出相應的占空比，生成相應占空比的PWM波形，輸出信號接到直流伺服電機驅動器，然后通過FPGA采集正交編碼盤信號，計算出輪子實際的角速度值，做PID速度閉環控制。鑒于FPGA模塊復制的優勢，這里對每個全向輪分別做了PID閉環控制。

圖1 全方位移動機器人的運動學模型

圖2 運動控制框圖

3 系統硬件設計

采用的三輪全方位移動機器人系統框圖如圖3所示[6]，上位機主要完成圖像信息的采集、處理、路徑規劃，并實現與場外裁判盒的通信。下位機主要是FPGA，主 要實現三輪編碼信號的采集，PID速度閉環控制，踢球控制，電機控制信號的產生，還有其他的傳感器 信息的采集等，并負責與上位機之間的信息交互。本設計只是完成了下位機運動控制部分。

圖3 全方位移動機器人控制系統

3.1 正交編碼信號采集與測速實現

增量式光電編碼器輸出信號如圖4所示。

圖4 編碼器信號圖

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

圖5 4倍頻電路仿真圖

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

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

v=360°fM1/(2NM2) (2)

3.2 增量式PID控制原理及其FPGA實現

實際機器人的數學模型不可避免地存在一定程度的參數不確定性，且三輪全方位移動機器人的正交全向輪在行走時會與地面交替接觸而產生一些不確定摩擦轉矩，這些都會給機器人的精確控制帶來難度[8]。為了對三輪全方位移動機器人進行精確的控制，系統采用PID速度閉環控制算法對機器人的3個全向輪進行速度調節。

圖6 變M/T法測速原理圖

令采樣周期為TS，將連續PID公式離散化后可得到數字PID算法表達式[9]:

u(k)= KPe(k)+TSTI∑kj=0e(j)+

TDTS[e(k)-e(k-1)](3)

其中，令:KI=KP(TS/TI)，KD=(TD/TS)得:

u(k)=KPe(k)+KI∑kj=0e(j)+KD[e(k)-e(k-1)] (4)

式中:k為采樣序號;u(k)為第k個采樣時刻的計算機輸出值;e(k)為第k個采樣時刻的計算機輸入誤差值;e(k-1)為第k-1個采樣時刻的輸入誤差值;KP為比例系數;KI為積分系數;KD為微分系數。

這種算法雖然比較直觀，但由于是全量輸出，所以每次輸出均與過去的所有狀態有關，計算時要對e(k)進行累加，計算機運算量大。

于是產生了增量式PID算法:

Δu(k)=u(k)-u(k-1) (5)

Δu(k) =KP[e(k)-e(k-1)]+KIe(k)+KD[e(k)-2e(k-1)+e(k-2)]

=(KP+KI+KD)e(k)-(KP+2KD)e(k-1)+KDe(k-2)

令:

A=(KP+KI+KD)，B=(KP+2KD)，C=KD

所以有:

Δu(k)=Ae(k)-Be(k-1)+Ce(k-2) (6)

u(k)=u(k-1)+Δu(k) (7)

上述公式(7)為增量式PID控制算法。只輸出控制增量，誤動作影響較小，且控制增量只與最近幾次的采樣值有關，容易通過加權處理獲得比較好的控制效果[2]。

根據以上公式推導，結合FPGA的工作特點，本文設計了適合FPGA的增量式PID實現結構[10]。

由圖7可以看出，增量式PID控制算法程序結構，只要最近的3個誤差采樣值就可以加權計算。這在FPGA內部完全可以并行實現，移位部分結構類似FIR濾波器的實現結構，難點是FPGA設計時對有符號數的熟練操作和保證累加器不能溢出。

圖7 增量式PID實現框圖

一種高效的硬件測試手段和系統測試方法，它能夠獲取并顯示可編程片上系統(SOPC)的實時信號，它可以隨設計文件一起下載到FPGA中，用于捕捉FPGA內部節點和I/O引腳的狀態，就如同使用真的邏輯分析儀一樣，對設計進行在線仿真，但又不影響硬件系統的工作[11]。

為了檢驗測得的全向輪實際速度值是否準確，對設計的測速模塊進行了在線仿真。設定每個全向輪以固定的速度轉動，對比測得的實際速度值和設定的速度值，如圖8所示。

圖8 測速模塊在線仿真圖

在嵌入式邏輯分析儀中，對PID模塊也進行了在線測試。實驗條件:在空載條件下，頻繁變化電機的速度，通過嵌入式邏輯分析儀觀察FPGA內部PID調節后的速度值和設定值，圖9所示為一號全向輪的速度設定值與反饋速度值。

三輪全方位移動機器人與雙輪差速不同，具有很大的靈活性，況且由于3個全向輪的負載的不同，使得機器人不能走出精確的直線。而要實現機器人的精確控制，一個前提就是讓它能夠走出很直的直線。為檢驗機器人控制性能，設計了如下實驗:機器人以固定速度分別向前后左右4個方向行走，先觀察沒有加入PID控制算法時的情況，然后再觀察加入PID控制算法時的情況。實驗的結果如表1所示。

表1 PID控制算法對機器人性能的影響

前后直線運動左右直線運動

沒有PID控制算法直線運動(4 m)劃出直徑約為3 m的圓圈

加入PID控制算法直線運動(6 m)直線運動(6 m)

圖9 PID控制器硬件仿真

分析:由于機器人的3個全向輪所承受的負載不一樣，即在相同的占空比的PWM下，3個輪子的實際速度并不相同，這就使得三輪速度不可能準確合成機器人的速度，進而影響機器人的控制軌跡。根據圖1所示的機器人1號輪和3號輪負載相當，2號輪子承受的負載較大，沒有加入PID控制器時，前后運動雖然在一定范圍內近似直線，但是機器人運行的速度達不到預期設定的速度，左右運動軌跡就是一個圓，而且設定的機器人左右移動速度大小還決定了機器人是朝順時針方向還是逆時針方向轉圈。加入PID控制算法后，輪子的速度得到校正，機器人能夠以預期設定的速度前后左右運動，特別是左右運動在一定范圍內近似為直線，不再是圓圈。可見PID閉環控制算法明顯提高了機器人的控制性能。

5 結 語

針對目前常見的以DSP為核心實現足球機器人底層運動控制系統的方案，提出了一種采用FPGA實現三輪全方位移動足球機器人的底層運動控制系統的方法。通過在三輪足球機器人上的應用實踐，發現這種采用FPGA實現的方案有很好的實時性，精確度較高，而且由于FPGA本身的引腳多特點，其可擴展性較強，比如可以通過串口配置數字羅盤等外圍信息傳感器等其他傳感器，同時，本設計對于研究多電機的機器人運動控制系統的實現方案有重要的參考價值和實用價值。

另外，由于全向輪的隨動性較強，且易打滑，在實行精確控制的時候方向容易受到影響，而且PID閉環控制算法反應時間較長，參數還需要更多時間的調試，在以后的研究中，我們將研究更為精確的控制算法，實現對機器人的精確控制。

參考文獻

[1]TU Kuo-yang， LUO Shi-chao.Design and implementation of omni-directional soccer robots for roboCup[J].IEEE International Conference on System，Man and Cybernetics， 2006，10:2000-2005.

[2]黃永賢.全自主足球機器人運動控制系統的研制[D].廣州:廣東工業大學，2008.

[3]冷春濤，曹其新，黃彥文，等.基于SoC的全方位移動機器人運動控制系統設計與實現[C].2005年中國智能自動化會議論文集，2005.

[4]聶曉璐.全方位小型足球機器人運動性能研究[D].天津:天津大學，2006.

[5]CHARITKHUAN Chirot， BHURIPANYO Janjai， CHOO-MUANG Rerngwut. FPGA implementation of closed-loop control system for small-sized roboCup[J].IEEE Conference on Robotics，Automation and Mechatronics， 2006，6:1-6.

[6]羅真，曹其新，陳衛東.中型組自主式足球機器人平臺設計回顧[J].機器人，2003，25(4):378-384.

[7]李擁軍，楊文淑.光電編碼器測速算法的IP核實現[J].長春理工大學學報:自然科學版，2008，31(3):35-37.

[8]李永新.RoboCup小型組足球機器人結構設計與分析[D].北京:中國科技大學，2003.

[9]楊文超，張榮標，張春艷.基于FPGA的PID控制器的研究[J].微計算機信息，2005，21(11):135-137.

[10]Chakravarthy Narashiman， XIAO Ji-zhong. FPGA-based control system for miniature robots[J].Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and System， 2005， 10: 3399-3404.

[11]劉波.精通Verilog HDL語言編程[M].北京:電子工業出版社，2007.