基于FPGA的工業(yè)機器人運動控制器設(shè)計

劉宏樂，李 兵

(1.華北理工大學(xué)，河北 唐山 063210；2.唐山學(xué)院，河北 唐山 063000)

0 引言

隨著電子技術(shù)與信息技術(shù)的不斷發(fā)展，機器人正在逐漸地進入到工業(yè)領(lǐng)域中，尤其是在高端制造業(yè)和工業(yè)自動化生產(chǎn)中發(fā)揮著重要作用。運動控制系統(tǒng)是工業(yè)機器人的核心，它向驅(qū)動裝置發(fā)出任務(wù)信號，控制末端的執(zhí)行裝置實現(xiàn)對被控對象的位置、速度等參數(shù)的精準控制。為此，就需要設(shè)計一個行之有效的運動控制器對給定軌跡進行實施跟蹤。

根據(jù)控制器控制核心的不同，運動控制器主要有三大類型：PC型、嵌入式型、全軟件型，并向多軸化、網(wǎng)絡(luò)化、開放式、智能化、可重構(gòu)性等方向發(fā)展[1]。

依據(jù)運動控制器的上述類型及特點，為了達到良好的控制效果，專家們已經(jīng)進行了很多的嘗試，在運動定位策略方面，根據(jù)不同的工況環(huán)境，有針對性地提出了決策樹、模糊邏輯、遺傳算法、人工神經(jīng)網(wǎng)絡(luò)等智能算法與傳統(tǒng)PID相結(jié)合的方法[2]。傳統(tǒng)的PID控制因為結(jié)構(gòu)簡單易懂，在大部分的工業(yè)自動化系統(tǒng)中都有它的存在，但是在碰到復(fù)雜的系統(tǒng)時，由于無法獲得準確的數(shù)學(xué)模型，難以整定PID的參數(shù)，因而無法獲得預(yù)期的效果。本文的研究對象為六自由度工業(yè)機器人，目前還無法建立準確的模型。考慮運算的實時性、控制的準確性、硬件設(shè)計的復(fù)雜性及可重構(gòu)性，本設(shè)計選擇模糊PID作為運動控制器的核心算法。在運算的實時性方面，模糊算法實際上是一種離散的算法，無論選擇軟件執(zhí)行還是硬件執(zhí)行，在執(zhí)行速度方面優(yōu)于其他算法。因為模糊算法是一種經(jīng)驗性算法，一般情況下決策樹、遺傳算法、人工神經(jīng)網(wǎng)絡(luò)算法在運動控制的準確性方面要優(yōu)于模糊算法，但如果模糊控制規(guī)則選擇得當(dāng)，該算法也能夠滿足控制需求。在硬件設(shè)計的法則性方面，無論是采用PLC、單片機、DSP、嵌入式還是可編程邏輯芯片，模糊算法的實現(xiàn)都是最為容易的。在可重構(gòu)性方面，如果采用的是純軟件設(shè)計方法(PC機)、PLC、單片機、DSP或嵌入式等方法，要根據(jù)重構(gòu)需求重新修改軟件程序，同時還要更換相應(yīng)的數(shù)據(jù)采集或通信接口等設(shè)備，成本比較高，周期也比較長[3]。如果采用的是FPGA，根據(jù)重構(gòu)需求，通過修改程序即可設(shè)計出相應(yīng)的電路系統(tǒng)，數(shù)據(jù)采集、通信接口電路的更改也可以通過修改程序來實現(xiàn)，可大大降低重構(gòu)成本和周期。因此依托于FPGA設(shè)計模糊PID運動控制器更為簡單、易懂，開發(fā)周期更短，設(shè)計成本也更低。

1 伺服系統(tǒng)的傳遞函數(shù)

本設(shè)計中選擇的六自由度工業(yè)機器人為ABB的IRB-2600ID，在伺服電機的驅(qū)動下，相鄰的關(guān)節(jié)之間的動態(tài)性能可由下面的方程表示[4]：

(1)

(2)

該微分方程即為六自由度工業(yè)機器人的傳遞函數(shù)。

2 模糊PID算法

由于六自由度工業(yè)機器人的伺服控制系統(tǒng)為非線性控制系統(tǒng)，傳統(tǒng)的PID控制在位置控制效果方面不能滿足要求，因此要將PID與模糊邏輯相結(jié)合對系統(tǒng)進行控制。模糊PID控制是根據(jù)PID控制器的3個參數(shù)與偏差e和偏差的變化ec之間的模糊關(guān)系，在運行時不斷檢測e及ec，通過事先確定的關(guān)系，利用模糊推理的方法，在線修改PID控制器的3個參數(shù)，讓PID參數(shù)可自整定[5]。在線實時調(diào)整控制器的kp，ki，kd這3個參數(shù)，得到整理后的Δkp，Δki，Δkd作為輸出。因為Δkp，Δki，Δkd這3個數(shù)一直是動態(tài)變化的，所以它們會根據(jù)系統(tǒng)的實時變化來整定自己的參數(shù)。PID控制參數(shù)的計算用到下列公式：

kp=kp0+Δkp

ki=ki0+Δki

kd=kd0+Δkd，

其中kp0，ki0，kd0為初始數(shù)據(jù)。

3 運動控制器的硬件實現(xiàn)

目前工業(yè)機器人的主流控制器有DSP、嵌入式、工控機、PLC等，這些控制器的結(jié)構(gòu)與外設(shè)相對固定，使得控制缺乏靈活性，運算速率相對較低，實時性較差。雖然FPGA發(fā)展較晚，但它具有強大的計算能力及優(yōu)異的可重構(gòu)性，在控制器的開發(fā)設(shè)計以及升級改造等方面有著其他控制器無法匹及的優(yōu)勢[6]。圖1為基于FPGA的模糊PID控制器電路結(jié)構(gòu)。

圖1 基于FPGA的模糊PID控制器電路結(jié)構(gòu)

系統(tǒng)分為4個模塊：誤差產(chǎn)生模塊、量化模塊、模糊查找表模塊以及運算模塊。誤差產(chǎn)生模塊生成e和ec的精確值；量化模塊就是把精確的e，ec變成模糊值；模糊值進一步經(jīng)過模糊查找表，輸出PID參數(shù)，最后通過一定運算，輸出最終的Δu值。本設(shè)計采用的FPGA型號為Altera公司的EP4CE40F29C8N。

3.1 誤差及誤差變化量模塊設(shè)計

本模塊的設(shè)計中輸入是誤差e以及誤差變化率ec，利用連續(xù)的兩個誤差e得到誤差變化率ec。整體設(shè)計采用的是原理圖設(shè)計方法，利用FPGA中固有的運算模塊來進行浮點運算，精確度更高，與傳統(tǒng)的編程語句實現(xiàn)方法相比較，該方法在消耗的硬件資源方面不占有優(yōu)勢，但是在運算精度上卻要高出傳統(tǒng)方法。由于所選擇的FPGA具有豐富的邏輯單元，在電路結(jié)構(gòu)上的消耗可以忽略。在時鐘CLK作用下同時將給定值與輸出反饋值送入兩個LPM_ADD_SUB模塊，同時接入兩個鎖存器，這樣送入第二個LPM_ADD_SUB模塊的值相對于第一個來說要差一個周期，那么兩個LPM_ADD_SUB模塊分別產(chǎn)生第k次和k-1次誤差。最后將這兩個誤差送入第三個LPM_ADD_SUB模塊中，輸出誤差變化量。在離散化論域中，誤差變化率的值通常是用誤差變化量的值來替代的。設(shè)計該模塊主要實現(xiàn)的是一個減法功能，于是調(diào)用了QuqrtusⅡ模塊庫中的LPM_ADD_SUB。圖2為誤差模塊設(shè)計原理圖。

圖2 誤差模塊設(shè)計原理圖

3.2 模糊量化模塊設(shè)計

上一個模塊中輸出的e與ec是精確的量，但是模糊控制器的輸入是模糊值。為了得到模糊值就需要將精確值經(jīng)過量化。設(shè)計此模塊，首先是將e與ec的模糊論域與實際論域根據(jù)實際情況設(shè)定出來；其次將模糊論域依據(jù)各個成分，把實際論域分成幾個段，每一段對應(yīng)于一個模糊論域中的一個成分。劃分論域時，如果劃分過細，會造成規(guī)則數(shù)目太多而使得電路結(jié)構(gòu)過于復(fù)雜，運算速度降低；如果劃分過粗的話，則無法達到精確的控制。IRB-2600ID型六自由度機械手臂的執(zhí)行機構(gòu)為伺服電機，由于伺服電機本身的控制為閉環(huán)控制，精度較高，因此在模糊論域的劃分上，權(quán)衡了控制精度與系統(tǒng)的復(fù)雜程度后，采用的是七級編碼機制，模糊語言變量劃分為7個以下論域集合{NB，NM，NS，ZO，PS，PM，PB}，即{負大，負中，負小，零，正小，正中，正大}，因此本文中將誤差大小量化為7個等級。當(dāng)從現(xiàn)場采集到數(shù)值后，將機器人關(guān)節(jié)的旋轉(zhuǎn)角度誤差e與誤差變化率ec作為輸入。根據(jù)ABB IRB-2600ID工業(yè)機器人產(chǎn)品手冊可知，軸1的工作范圍是-180°～180°，因此將e與ec的實際論域范圍定為[-180，180]，量化因子為60，采取四舍五入的方式，把實際論域分成7段，分別是[-180，-150]，[-150，-90]，[-90，-30]，[-30，30]，[30，90]，[90，150]，[150，180]，對應(yīng)到模糊論域范圍[-3，3]上，即{-3，-2，-1，0，1，2，3}。假設(shè)誤差的精確值是57，此精確值在[30，90]的論域段，誤差經(jīng)過量化后的值就是1，量化地址為100。ec和e的量化過程相同，表1為e/ec的量化地址。

表1 模糊量化后的e/ec的量化地址

這部分采用VHDL語言進行設(shè)計，其部分代碼如下顯示：

PROCESS(E，EC，CLK)

BEGIN

IF CLK'EVENT AND CLK='1'THEN

IF E<=-180 THEN A<="111"；

ELSIF E>=-180 AND E<=-150 THEN A<="000"；

ELSIF E>=-150 AND E<=-90 THEN A<="001"；

ELSIF E>=-90 AND E<=-30 THEN A<="010"；

ELSIF E>=-30 AND E<=30 THEN A<="011"；

ELSIF E>=30 AND E<=90 THEN A<="100"；

ELSIF E>=90 AND E<=150 THEN A<="101"；

ELSIF E>=150 AND E<=180 THEN A<="110"；

ELSIF E>=180 THEN A<="111"；

END IF；

…

END IF；

END IF；

END PROCESS；

C<=A&B；

Q<=C；

END ART；

該模塊如圖3所示。

圖3 量化模塊

3.3 模糊查找表模塊設(shè)計

模糊控制器中，模糊查找表是至關(guān)重要的。由于在現(xiàn)實中模糊化運算、模糊推理和清晰化計算是非常繁復(fù)的，在線的演算量較少，故本文主要運用離線運算方式，這種方式不僅改善了FPGA的工作效率，而且編程的困難也有所消減。

為了得到Δkp，Δki，Δkd的值，主要通過Matlab建立模糊PID模型，在Matlab中得到相應(yīng)的值。為了減少難度，可設(shè)A=Δkp+Δki+Δkd，B=-(Δkp+2Δkd)，C=Δkd。

通過計算得到以e和ec為輸入，以A，B，C為輸出的表格。高3位是e的量化地址，低3位是ec的量化地址。表2-表4即為A，B，C的模糊控制表。

表2 A模糊控制表

表3 B模糊控制表

表4 C模糊控制表

為了更好地運用A，B，C模糊控制表，需要將它們制作成ROM模塊，而ROM模塊的核心就是MIF文件。將這些設(shè)計完后，此時只需要計算：

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

(3)

圖4為A的MIF文件。

圖4 A的MIF文件

3.4 運算模塊設(shè)計

乘法運算模塊調(diào)用了函數(shù)庫里的LPM_MUITA。讓e(k)，B與e(k-1)，C與e(k-2)相乘，進一步使用parallel_add，使這3個參數(shù)通過加法功能獲得最后的結(jié)果Δu。圖5為運算模塊原理圖。

3.5 頂層模塊設(shè)計

圖6就是模糊PID算法的頂層模塊。首先輸入通過誤差模塊獲得的精確值，量化模塊把e和ec進行量化，得到模糊值；其次將量化后的值送入到下一個單元以查表的方式進行離線查詢，得出控制參數(shù)的增量；最后將計算得到的3個參數(shù)A，B，C輸入到下一個模塊，通過計算得到輸出。

取一組數(shù)據(jù)，如da=11，db=9，則可以得到e=2，e1=2，e2=5，ec=-3，e精確值是2，屬于[-180，180]這個論域，其量化值是0，量化編碼是011。同理可得ec的量化編碼是011。把e的量化編碼設(shè)為高3位，ec的量化編碼設(shè)為低3位，查表可知A=3，B=0，C=0，通過公式(3)計算得到Δu=6。電路仿真結(jié)果如圖7所示。

4 結(jié)語

由電路仿真結(jié)果可以看出，電路運算結(jié)果與理論計算得出的結(jié)果是一致的，說明設(shè)計中所采用的方法能夠滿足控制需求。電路運行的延遲時間為11 ns，確保了執(zhí)行的實時性?？刂破髡w占用FPGA資源的58.5%，確保了后續(xù)升級重構(gòu)需求。

圖5 乘加運算模塊原理圖

圖6 模糊PID算法頂層模塊

圖7 電路仿真結(jié)果