基于FPGA的工業(yè)機(jī)器人運(yùn)動(dòng)控制器設(shè)計(jì)

劉宏樂，李 兵

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

0 引言

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

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

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

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

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

(1)

(2)

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

2 模糊PID算法

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

kp=kp0+Δkp

ki=ki0+Δki

kd=kd0+Δkd，

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

3 運(yùn)動(dòng)控制器的硬件實(shí)現(xiàn)

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

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

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

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

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

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

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

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

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è)計(jì)

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

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

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

表2 A模糊控制表

表3 B模糊控制表

表4 C模糊控制表

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

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

(3)

圖4為A的MIF文件。

圖4 A的MIF文件

3.4 運(yùn)算模塊設(shè)計(jì)

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

3.5 頂層模塊設(shè)計(jì)

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

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

4 結(jié)語

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

圖5 乘加運(yùn)算模塊原理圖

圖6 模糊PID算法頂層模塊

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