中型組機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的FPGA設(shè)計(jì)

0 引言
    目前，全方位移動(dòng)機(jī)器人由于具有出色的靈活性，已經(jīng)成為RoboCup中型組足球機(jī)器人比賽中最理想的選擇。而機(jī)器人的運(yùn)動(dòng)控制一直以來(lái)都是直接影響機(jī)器人性能的主要因素，也是移動(dòng)機(jī)器人研究的熱點(diǎn)之一。本文研究了一種用FPGA技術(shù)實(shí)現(xiàn)三輪全方位移動(dòng)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的方法，與雙DSP結(jié)構(gòu)，DSP+CPLD結(jié)構(gòu)，以及DSP+專(zhuān)用集成電路結(jié)構(gòu)等相比，該方法具有簡(jiǎn)單可靠，擴(kuò)展性強(qiáng)等特點(diǎn)。且FPGA設(shè)計(jì)簡(jiǎn)單，使用方便，開(kāi)發(fā)周期短，能夠?qū)崿F(xiàn)真正的SOPC系統(tǒng)。

1 全方位移動(dòng)機(jī)器人運(yùn)動(dòng)模型
    設(shè)世界坐標(biāo)系下機(jī)器人的速度為ε=[vx，vy，φ]，則當(dāng)vx=O，vy≠0，φ=O時(shí)，機(jī)器人做前后方向的直線運(yùn)動(dòng)，當(dāng)vx≠0，vy=0，φ=0時(shí)，機(jī)器人做左右方向的直線運(yùn)動(dòng)，當(dāng)vx=0，vy=0，φ≠0時(shí)，機(jī)器人做自轉(zhuǎn)運(yùn)動(dòng)。圖1中，ω1，ω2，ω3為3個(gè)主動(dòng)輪的轉(zhuǎn)動(dòng)角速度，R為全向輪半徑；L1，L2，L3為機(jī)器人車(chē)體中心到3組全向輪中心的水平距離，設(shè)有L1=L2=L3=L。α為前兩輪之間的夾角，另外2個(gè)夾角均為180°-α／2。則機(jī)器人坐標(biāo)系下的速度到三輪速度之間的關(guān)系如下：
   

    由式(1)可以看到：知道了機(jī)器人在平面世界坐標(biāo)系中的速度要求后，便可以得到主動(dòng)輪的速度要求，進(jìn)而對(duì)電機(jī)發(fā)出相應(yīng)的控制信號(hào)。
2運(yùn)動(dòng)控制方案本系統(tǒng)總體設(shè)計(jì)思路如圖2所示，首先通過(guò)RS 232接口，實(shí)現(xiàn)PC機(jī)與底層控制芯片F(xiàn)PGA的通信，F(xiàn)PGA在接收到相關(guān)的機(jī)器人坐標(biāo)系下的速度后，將機(jī)器人坐標(biāo)系下的速度值轉(zhuǎn)化成機(jī)器人3個(gè)全向輪子的角速度，將得到的角速度值計(jì)算出相應(yīng)的占空比，生成相應(yīng)占空比的PWM波形，輸出信號(hào)接到直流伺服電機(jī)驅(qū)動(dòng)器，然后通過(guò)FPGA采集正交編碼盤(pán)信號(hào)，計(jì)算出輪子實(shí)際的角速度值，做PID速度閉環(huán)控制。鑒于FPGA模塊復(fù)制的優(yōu)勢(shì)，這里對(duì)每個(gè)全向輪分別做了PID閉環(huán)控制。

3 系統(tǒng)硬件設(shè)計(jì)
    采用的三輪全方位移動(dòng)機(jī)器人系統(tǒng)框圖如圖3所示，上位機(jī)主要完成圖像信息的采集、處理、路徑規(guī)劃，并實(shí)現(xiàn)與場(chǎng)外裁判盒的通信。下位機(jī)主要是FPGA，主要實(shí)現(xiàn)三輪編碼信號(hào)的采集，PID速度閉環(huán)控制，踢球控制，電機(jī)控制信號(hào)的產(chǎn)生，還有其他的傳感器信息的采集等，并負(fù)責(zé)與上位機(jī)之間的信息交互。本設(shè)計(jì)只是完成了下位機(jī)運(yùn)動(dòng)控制部分。

3．1 正交編碼信號(hào)采集與測(cè)速實(shí)現(xiàn)
    增量式光電編碼器輸出信號(hào)如圖4所示。

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

    根據(jù)脈沖計(jì)數(shù)來(lái)測(cè)量轉(zhuǎn)速的方法有M法、T法以及M／T法3種。M法適用于高速測(cè)量場(chǎng)合，在低速時(shí)有較大的誤差；而T法，恰恰相反，在低速時(shí)測(cè)量準(zhǔn)確，高速時(shí)誤差較大。

    本設(shè)計(jì)采用文獻(xiàn)所描述的方法。該方法如圖6所示，設(shè)定參考閘門(mén)時(shí)間為固定的1個(gè)值，它只是作為參考信號(hào)和編碼信號(hào)共同確定實(shí)際的閘門(mén)時(shí)間。這樣確定的閘門(mén)時(shí)間為被測(cè)信號(hào)的整周期倍，能夠有效提高測(cè)量精度。則測(cè)得的速度為：
   
3．2 增量式PID控制原理及其FPGA實(shí)現(xiàn)
    實(shí)際機(jī)器人的數(shù)學(xué)模型不可避免地存在一定程度的參數(shù)不確定性，且三輪全方位移動(dòng)機(jī)器人的正交全向輪在行走時(shí)會(huì)與地面交替接觸而產(chǎn)生一些不確定摩擦轉(zhuǎn)矩，這些都會(huì)給機(jī)器人的精確控制帶來(lái)難度。為了對(duì)三輪全方位移動(dòng)機(jī)器人進(jìn)行精確的控制，系統(tǒng)采用PID速度閉環(huán)控制算法對(duì)機(jī)器人的3個(gè)全向輪進(jìn)行速度調(diào)節(jié)。
    令采樣周期為T(mén)S，將連續(xù)PID公式離散化后可得到數(shù)字PID算法表達(dá)式：


式中：k為采樣序號(hào)；u(k)為第k個(gè)采樣時(shí)刻的計(jì)算機(jī)輸出值；e(k)為第k個(gè)采樣時(shí)刻的計(jì)算機(jī)輸入誤差值；e(k-1)為第k-1個(gè)采樣時(shí)刻的輸入誤差值；Kp為比例系數(shù)；KI為積分系數(shù)；KD為微分系數(shù)。
    這種算法雖然比較直觀，但由于是全量輸出，所以每次輸出均與過(guò)去的所有狀態(tài)有關(guān)，計(jì)算時(shí)要對(duì)e(k)進(jìn)行累加，計(jì)算機(jī)運(yùn)算量大。
    于是產(chǎn)生了增量式PID算法：
   
   

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

    一種高效的硬件測(cè)試手段和系統(tǒng)測(cè)試方法，它能夠獲取并顯示可編程片上系統(tǒng)(SOPC)的實(shí)時(shí)信號(hào)，它可以隨設(shè)計(jì)文件一起下載到FPGA中，用于捕捉FPGA內(nèi)部節(jié)點(diǎn)和I／0引腳的狀態(tài)，就如同使用真的邏輯分析儀一樣，對(duì)設(shè)計(jì)進(jìn)行在線仿真，但又不影響硬件系統(tǒng)的工作。為了檢驗(yàn)測(cè)得的全向輪實(shí)際速度值是否準(zhǔn)確，對(duì)設(shè)計(jì)的測(cè)速模塊進(jìn)行了在線仿真。設(shè)定每個(gè)全向輪以固定的速度轉(zhuǎn)動(dòng)，對(duì)比測(cè)得的實(shí)際速度值和設(shè)定的速度值，如圖8所示。

    在嵌入式邏輯分析儀中，對(duì)PID模塊也進(jìn)行了在線測(cè)試。實(shí)驗(yàn)條件：在空載條件下，頻繁變化電機(jī)的速度，通過(guò)嵌入式邏輯分析儀觀察FPGA內(nèi)部PID調(diào)節(jié)后的速度值和設(shè)定值，圖9所示為一號(hào)全向輪的速度設(shè)定值與反饋速度值。

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

    分析：由于機(jī)器人的3個(gè)全向輪所承受的負(fù)載不一樣，即在相同的占空比的PWM下，3個(gè)輪子的實(shí)際速度并不相同，這就使得三輪速度不可能準(zhǔn)確合成機(jī)器人的速度，進(jìn)而影響機(jī)器人的控制軌跡。根據(jù)圖1所示的機(jī)器人1號(hào)輪和3號(hào)輪負(fù)載相當(dāng)，2號(hào)輪子承受的負(fù)載較大，沒(méi)有加入PID控制器時(shí)，前后運(yùn)動(dòng)雖然在一定范圍內(nèi)近似直線，但是機(jī)器人運(yùn)行的速度達(dá)不到預(yù)期設(shè)定的速度，左右運(yùn)動(dòng)軌跡就是一個(gè)圓，而且設(shè)定的機(jī)器人左右移動(dòng)速度大小還決定了機(jī)器人是朝順時(shí)針?lè)较蜻€是逆時(shí)針?lè)较蜣D(zhuǎn)圈。加入PID控制算法后，輪子的速度得到校正，機(jī)器人能夠以預(yù)期設(shè)定的速度前后左右運(yùn)動(dòng)，特別是左右運(yùn)動(dòng)在一定范圍內(nèi)近似為直線，不再是圓圈?？梢?jiàn)PID閉環(huán)控制算法明顯提高了機(jī)器人的控制性能。

5 結(jié)語(yǔ)
    針對(duì)目前常見(jiàn)的以DSP為核心實(shí)現(xiàn)足球機(jī)器人底層運(yùn)動(dòng)控制系統(tǒng)的方案，提出了一種采用FPGA實(shí)現(xiàn)三輪全方位移動(dòng)足球機(jī)器人的底層運(yùn)動(dòng)控制系統(tǒng)的方法。通過(guò)在三輪足球機(jī)器人上的應(yīng)用實(shí)踐，發(fā)現(xiàn)這種采用FPGA實(shí)現(xiàn)的方案有很好的實(shí)時(shí)性，精確度較高，而且由于FPGA本身的引腳多特點(diǎn)，其可擴(kuò)展性較強(qiáng)，比如可以通過(guò)串口配置數(shù)字羅盤(pán)等外圍信息傳感器等其他傳感器，同時(shí)，本設(shè)計(jì)對(duì)于研究多電機(jī)的機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的實(shí)現(xiàn)方案有重要的參考價(jià)值和實(shí)用價(jià)值。
    另外，由于全向輪的隨動(dòng)性較強(qiáng)，且易打滑，在實(shí)行精確控制的時(shí)候方向容易受到影響，而且PID閉環(huán)控制算法反應(yīng)時(shí)間較長(zhǎng)，參數(shù)還需要更多時(shí)間的調(diào)試，在以后的研究中，我們將研究更為精確的控制算法，實(shí)現(xiàn)對(duì)機(jī)器人的精確控制。