基于FPGA的串聯(lián)機(jī)器人控制系統(tǒng)設(shè)計(jì)

張道德， 尹 洋, 宋成龍

(湖北工業(yè)大學(xué)機(jī)械工程學(xué)院， 湖北 武漢 430068)

?

基于FPGA的串聯(lián)機(jī)器人控制系統(tǒng)設(shè)計(jì)

張道德， 尹 洋, 宋成龍

(湖北工業(yè)大學(xué)機(jī)械工程學(xué)院， 湖北 武漢 430068)

通過Pro/Enginee構(gòu)建5自由度機(jī)械手硬件機(jī)械結(jié)構(gòu)模型，設(shè)計(jì)基于FPGA嵌入式處理器的NiosII的SOPC架構(gòu)系統(tǒng)的軟件平臺(tái)，以及FPGA控制器與步進(jìn)電機(jī)驅(qū)動(dòng)模塊，并分析機(jī)械臂運(yùn)動(dòng)學(xué)的正解、逆解問題，通過VC、MATLAB等工具實(shí)現(xiàn)機(jī)械臂的最優(yōu)解的選取、軌跡規(guī)劃的仿真，以及運(yùn)用Robotics Toolbox工具箱進(jìn)行正逆運(yùn)動(dòng)學(xué)仿真，驗(yàn)證算法可行性和求解的正確性。

現(xiàn)場(chǎng)可編程邏輯門陣列; 機(jī)器人控制; 機(jī)械臂

機(jī)械臂的控制方式主要有3種方案：第一種是以PC機(jī)作為硬件平臺(tái)，利用VC軟件實(shí)現(xiàn)機(jī)械臂的算法控制，PC的硬件配置要求較高；第二種是PC機(jī)和運(yùn)動(dòng)控制卡的結(jié)合方案，控制卡安裝在PC機(jī)中，處理器完成部分控制算法，主要任務(wù)由運(yùn)動(dòng)控制卡完成，控制系統(tǒng)比較穩(wěn)定；第三種是機(jī)器人控制模塊與PC機(jī)的結(jié)合，模塊化具有高效、可移植性好的特點(diǎn)，但價(jià)格昂貴。本文提出的控制方式是通過FPGA作為硬件平臺(tái)，在FPGA 里完成算法控制，將控制結(jié)果輸出到驅(qū)動(dòng)模塊，實(shí)現(xiàn)機(jī)械臂的實(shí)時(shí)控制。由于FPGA具有高效、并行執(zhí)行等特點(diǎn)，對(duì)機(jī)器臂的控制具有很好的效果。

1 硬件結(jié)構(gòu)模型的構(gòu)建

1.1 機(jī)械臂模型建立

5自由度機(jī)械臂的機(jī)械結(jié)構(gòu)系統(tǒng)組成包括：機(jī)身、大臂、小臂、手腕和末端操作器(手抓)等5部分。機(jī)械臂機(jī)械結(jié)構(gòu)模型見圖1。

圖1 機(jī)械臂機(jī)械結(jié)構(gòu)模型

機(jī)械臂由若干關(guān)節(jié)和連桿串聯(lián)組成，采用D-H參數(shù)法來確定每個(gè)關(guān)節(jié)上的坐標(biāo)姿態(tài)和相鄰兩個(gè)坐標(biāo)系之間的相對(duì)平移距離和旋轉(zhuǎn)角度[1]。機(jī)器人各關(guān)節(jié)坐標(biāo)關(guān)系及機(jī)構(gòu)簡(jiǎn)圖如圖2所示。D-H參數(shù)由連桿長度a、連桿扭角αi、連桿距離di和連桿轉(zhuǎn)角θi構(gòu)成。D-H參數(shù)如表1所示。

圖2 機(jī)器人坐標(biāo)關(guān)系

表1 機(jī)械臂D-H參數(shù)

連桿di/cma/cmαi/(°)θi/(°)i=100-900～135i=20100-90～90i=309．50-90～90i=403．2-90-90～90i=514．500-90～90

1.2 串聯(lián)機(jī)械臂工作空間的分析和仿真

機(jī)械臂的工作空間是機(jī)械臂的手腕中心或末端操作器所能達(dá)到空間點(diǎn)位置和合集，它是用于衡量串聯(lián)機(jī)械臂工作能力的重要指標(biāo)。其計(jì)算十分復(fù)雜多樣，主要有解析法、圖解法、數(shù)值法和仿真法，其中較常用的方法是數(shù)值法中的蒙特卡洛法，采用MATLAB中SimMechanics工具箱進(jìn)行工作空間的分析計(jì)算，仿真實(shí)現(xiàn)起來更快捷、方便。利用機(jī)器人坐標(biāo)關(guān)系圖和D-H參數(shù)建立SimMechanics模型，設(shè)置各關(guān)節(jié)轉(zhuǎn)動(dòng)驅(qū)動(dòng)，分別是角度、角速度、角加速度[2]。SimMechanics模型見圖3,仿真結(jié)果見圖4。

圖3 SimMechanics模型

圖4 機(jī)械臂工作空間三維圖

2 系統(tǒng)方案的設(shè)計(jì)

2.1 系統(tǒng)工作流程

5自由度串聯(lián)機(jī)械臂控制系統(tǒng)方案比較復(fù)雜。數(shù)據(jù)采集模塊采集到起始位姿和終止位姿數(shù)據(jù)后，通過串口發(fā)送到FPGA 內(nèi)部，在FPGA內(nèi)部實(shí)現(xiàn)運(yùn)動(dòng)學(xué)的逆解和解的最優(yōu)化，然后進(jìn)行軌跡規(guī)劃，在將指令發(fā)送到脈沖驅(qū)動(dòng)模塊驅(qū)動(dòng)機(jī)械臂(圖5)。

圖5 系統(tǒng)工作流程圖

由于在FPGA內(nèi)部采用Verilog HDL實(shí)現(xiàn)運(yùn)動(dòng)學(xué)逆解及其最優(yōu)化，矩陣運(yùn)算等運(yùn)算非常復(fù)雜，因此采用FPGA 內(nèi)部的Nios II軟核處理器C語言處理復(fù)雜運(yùn)算，脈沖驅(qū)動(dòng)模塊等簡(jiǎn)單運(yùn)算可以運(yùn)用Verilog HDL硬件語言實(shí)現(xiàn)。兩種方式相互結(jié)合會(huì)使方案實(shí)現(xiàn)起來可行性更高。

2.2 SOPC系統(tǒng)的構(gòu)建

SOPC(System On a Programmable Chip)能把整個(gè)可編程系統(tǒng)集成到一塊硅片上，是一種特殊的嵌入式系統(tǒng)。NIOS II 是一個(gè)用戶可配置的通用32位RISC嵌入式軟核處理器，它是SOPC片上可編程系統(tǒng)的核心。運(yùn)用Quartus軟件開發(fā)工具的SOPC Builder可以很方便地定義一個(gè)我們需要的系統(tǒng)。SOPC系統(tǒng)的構(gòu)建首先需要定義軟核時(shí)鐘100 MH,接著需要建立Nios IIProcessor，主要包括CPU模塊、EPCS控制器、SYSTEM ID 、JTAG UART，這是SOPC系統(tǒng)最基本組成部分。依據(jù)系統(tǒng)需要，還要構(gòu)建PIO口用于脈沖輸出，RS232口用于數(shù)據(jù)接收,還需構(gòu)建一個(gè)PLL鎖相環(huán)對(duì)時(shí)鐘倍頻。系統(tǒng)構(gòu)建的SOPC系統(tǒng)如圖6所示。系統(tǒng)構(gòu)建的Nios II的RTL視圖如圖7所示。

圖6 SOPC系統(tǒng)

2.3 自定義Avalon-MM總線

Avalon總線由ALTERA公司提出，用于在基于FPGA的片上系統(tǒng)中連接片內(nèi)處理器和片內(nèi)外設(shè)的總線結(jié)構(gòu)。 連接到Avalon總線的設(shè)備分為主從設(shè)備，并各有其工作模式。Avalon總線的讀/寫時(shí)序是指外設(shè)之間產(chǎn)生的一次讀/寫數(shù)據(jù)總線接口各個(gè)信號(hào)之間的時(shí)序關(guān)系。在clk的第一個(gè)上升沿，Avalon總線傳遞address,byteenable_n和read_n信號(hào)到目標(biāo)外設(shè)Avalon總線模塊內(nèi)部對(duì)address進(jìn)行譯碼，產(chǎn)生片選并驅(qū)動(dòng)從端口的chipselect信號(hào)，從而輸出readdata[3]。從組件的基本讀時(shí)序如圖8所示。

圖7 系統(tǒng)RTL視圖

圖8 從組件的基本讀時(shí)序

由于脈沖輸出模塊沒有復(fù)雜的運(yùn)算，因此運(yùn)用Avalon-MM總線自定義一個(gè)外設(shè)模塊具有更高的運(yùn)行效率。構(gòu)建的脈沖輸出模塊的RTL視圖如圖9所示，并且通過Modelsim仿真軟件驗(yàn)證了其正確性(圖10)。

圖9 脈沖輸出模塊的RTL視圖

2.4 機(jī)械臂運(yùn)動(dòng)學(xué)逆解及優(yōu)化

機(jī)器人運(yùn)動(dòng)學(xué)的逆向求解問題是指已知機(jī)器人末端操作器(手部)要到達(dá)的目標(biāo)位姿的情況下，通過坐標(biāo)逆變換來求出所需的各個(gè)關(guān)節(jié)變量平移和旋轉(zhuǎn)值，以驅(qū)動(dòng)各關(guān)節(jié)的伺服電機(jī)或步進(jìn)電機(jī)旋轉(zhuǎn)，使手部的位姿得到滿足.這就是機(jī)器人的反向運(yùn)動(dòng)學(xué)問題。

圖10 脈沖輸出模塊仿真波形

依據(jù)表1 機(jī)械臂D-H參數(shù),求解機(jī)械臂的整體坐標(biāo)變換矩陣公式如下

T50=T1·T2·T3·T4·T5=

(1)

求解機(jī)器人的運(yùn)動(dòng)學(xué)方程：

(2)

其中,-3.2、0和34分別代表機(jī)械臂的末端操作器相對(duì)于固定坐標(biāo)系的x軸、y軸和z軸三坐標(biāo)軸的平移量。

通過解析法可以求得機(jī)械臂的逆向運(yùn)動(dòng)學(xué)方程，最終求得每個(gè)轉(zhuǎn)角的值。已知運(yùn)動(dòng)學(xué)方程的位姿矩陣

(3)

且T50=T1T2T3T4T5，通過T1、T2、T3、T4和T5可以求出T1-1、T2-1、T3-1、T4-1和T5-1分別為

(4)

(5)

(6)

(7)

(8)

通過算運(yùn),可以分別求出機(jī)械臂每個(gè)電機(jī)的轉(zhuǎn)角，但是由于有反三角函數(shù)存在，因此運(yùn)算存在多解情況。

5自由度機(jī)械臂的逆向運(yùn)動(dòng)學(xué)共有8組解(圖11),但是由于機(jī)械臂實(shí)際存在約束條件，有些解不在末端操作器工作域內(nèi)，所以5自由度機(jī)械臂的最后運(yùn)動(dòng)情況要根據(jù)真實(shí)情況和最優(yōu)解的選擇來決定[1]。

圖11 5自由度機(jī)械臂逆解

需要讓每個(gè)步進(jìn)電機(jī)耗能最小，則選用距離最優(yōu)的算法。該方法使機(jī)械臂的每個(gè)關(guān)節(jié)運(yùn)動(dòng)量最少。建立的最優(yōu)解目標(biāo)函數(shù)F1和F2，F(xiàn)1是優(yōu)化關(guān)節(jié)1、關(guān)節(jié)2和關(guān)節(jié)3的目標(biāo)函數(shù)，F(xiàn)2是優(yōu)化關(guān)節(jié)4和關(guān)節(jié)5的目標(biāo)函數(shù)，如

其中，ai(i=1,2,3)是D-H參數(shù)中的連桿長度，θi(i=1,2,3)和θi(i=4,5)是D-H參數(shù)中的關(guān)節(jié)角。目標(biāo)函數(shù)F1體現(xiàn)了一種加權(quán)思想，而目標(biāo)函數(shù)F2直接取關(guān)節(jié)4和關(guān)節(jié)5的兩個(gè)關(guān)節(jié)角的平均值[1]。在實(shí)際應(yīng)用中，機(jī)械臂運(yùn)動(dòng)學(xué)逆解最優(yōu)化方法很多，一般有時(shí)間最優(yōu)、功率最優(yōu)、距離最優(yōu)、避障和受力等。應(yīng)該根據(jù)實(shí)際情況選擇，并建立最優(yōu)化函數(shù)。

圖12 逆解最優(yōu)化流程圖

將運(yùn)動(dòng)學(xué)逆解和最優(yōu)化程序整合后，在VC中仿真的效果如圖13所示。首先逆解出8個(gè)角后排出不合理的角度，然后通過距離最優(yōu)得到5個(gè)最優(yōu)解。由于NIOSII編輯環(huán)境是C語言，可以將VC的仿真程序直接移植到NIOSII編輯環(huán)境中。

圖13 距離最優(yōu)化結(jié)果

Mtalab下的RoboticsToolbox可以對(duì)其進(jìn)行仿真和驗(yàn)證，Ikine()函數(shù)作用是求解逆向運(yùn)動(dòng)學(xué)，函數(shù)調(diào)用格式如下

(9)

式中：ROBOT為一個(gè)機(jī)器人對(duì)象，T為要進(jìn)行反解的變換矩陣，Q是初始猜測(cè)關(guān)節(jié)角(默認(rèn)全是0)，M是忽略某個(gè)關(guān)節(jié)自由度。MATLAB程序及結(jié)果見圖14。

其結(jié)果可見，MATLAB仿真得到的結(jié)果與VC程序得到的5個(gè)輸出角度值一致。

2.5 軌跡規(guī)劃的仿真

軌跡規(guī)劃是工業(yè)機(jī)器人的控制基礎(chǔ)，其控制目標(biāo)是平穩(wěn)、快速和精確實(shí)現(xiàn)所規(guī)劃的軌跡運(yùn)動(dòng)，否則將對(duì)機(jī)械臂的機(jī)械結(jié)構(gòu)部件造成一定的磨損，并可能導(dǎo)致機(jī)械臂運(yùn)動(dòng)時(shí)的震動(dòng)和沖擊，最終影響機(jī)械臂的運(yùn)動(dòng)精度和定位精度。

圖14 5R機(jī)械臂逆解程序及結(jié)果

本文主要討論連續(xù)路徑的無障礙軌跡規(guī)劃方法[6]。主要方法有三次多項(xiàng)式插值法、五次多項(xiàng)式法和用拋物線過渡的線性插值法。而用拋物線過渡的線性插值法更加常用。時(shí)間t在不同的時(shí)間段中求出的關(guān)節(jié)位置函數(shù)

保證ta有解，則加速度a的值要選擇的足夠大，要滿足

(10)

其中：q0、qa、qh、qb-a和qb分別是對(duì)應(yīng)t0、ta、th、tb-a和tb時(shí)間點(diǎn)的關(guān)節(jié)位置。

運(yùn)用MATLAB進(jìn)行仿真，通過拋物線過渡的線性插值法，得到結(jié)果見圖15。

圖15 機(jī)械臂關(guān)節(jié)角位置、角速度和角加速度曲線

3 系統(tǒng)PCB板設(shè)計(jì)

3.1 機(jī)械臂FPGA控制系統(tǒng)PCB板設(shè)計(jì)

一般FPGA最小系統(tǒng)主要包括FPGA芯片、外部時(shí)鐘、下載電路、復(fù)位電路和電源[4]。如果需要使用NIOSII軟嵌入式處理器，還需要包括SDRAM和Flash這些組件。本系統(tǒng)選用的FPGA芯片為比較成熟的CycloneII芯片EPM2C8Q208C8。除了基本模塊外，還添加了串口通信模塊來完成硬件與PC機(jī)的通信，進(jìn)行人機(jī)交互。系統(tǒng)原理圖見圖16。

圖16 機(jī)械臂Fpga控制系統(tǒng)原理圖

3.2 步進(jìn)電機(jī)驅(qū)動(dòng)板設(shè)計(jì)

由于機(jī)械臂由步進(jìn)電機(jī)帶動(dòng)，因此選擇穩(wěn)定、高效的步進(jìn)電機(jī)驅(qū)動(dòng)方案十分重要。步進(jìn)電機(jī)總轉(zhuǎn)動(dòng)角度由輸入脈沖數(shù)決定，轉(zhuǎn)速由脈沖信號(hào)頻率決定，具有控制簡(jiǎn)單、慣量低、定位精度高等特點(diǎn)。TB6560芯片是東芝推出的低功耗、高集成兩相混合式步進(jìn)電機(jī)驅(qū)動(dòng)芯片[8]，集成雙全橋MOSFET驅(qū)動(dòng)，單相輸出最大電流3.5A(峰值)，有多種細(xì)分方式，具有自動(dòng)斷開輸出、過流保護(hù)等功能。

電機(jī)以TB6560為核心芯片驅(qū)動(dòng)電路，附加的電路有光電隔離電路外部接口電路、自動(dòng)半流電路，以及細(xì)分電路等組成。其中隔離電路采用兩片4N25高速光耦和一片PC817普通光耦。電機(jī)控制信號(hào)隔離電路如圖17所示。PCB圖如圖18所示。

圖17 電機(jī)控制信號(hào)隔離電路

圖18 電機(jī)驅(qū)動(dòng)PCB

4 結(jié)束語

本文構(gòu)建了機(jī)械臂三維模型以仿真機(jī)械臂的工作空間，并且提出了更加方便、高效的機(jī)械臂控制方案，構(gòu)建了SOPC控制系統(tǒng)，分析并仿真了機(jī)械臂運(yùn)動(dòng)學(xué)的逆解、優(yōu)化和軌跡規(guī)劃等問題，最后設(shè)計(jì)出了FPGA的控制版和步進(jìn)電機(jī)驅(qū)動(dòng)板。本文較為完整地設(shè)計(jì)了串聯(lián)機(jī)器人的控制方案，并驗(yàn)證了其可行性。

[1] 王 強(qiáng).基于FPGA的5自由度工業(yè)機(jī)械臂控制系統(tǒng)研究[D].武漢:湖北工業(yè)大學(xué)，2012.

[2] 許衛(wèi)斌，平雪良.6R型串聯(lián)機(jī)器人控制系統(tǒng)的軟件實(shí)現(xiàn)[J].中國制造業(yè)信息化,2012,41(17):60-63.

[3] 劉福奇.VerilogHDL設(shè)計(jì)與實(shí)戰(zhàn)[M]北京：北京航空航天大學(xué)出版社，2012.

[4] 華清遠(yuǎn)見嵌入式培訓(xùn)中心.FPGA應(yīng)用開發(fā)入門與典型實(shí)例[M].北京：人民郵電出版社，2008.

[5] 吳厚航.愛上FPGA開發(fā)——特權(quán)和你一起學(xué)NIOSII[M].北京：北京航空航天大學(xué)出版社，2011.

[6] 韓建海.工業(yè)機(jī)器人[M]. 第二版.武漢：華中科技大學(xué)出版社，2014.

[7] 周國義，謝明紅.6 自由度解耦機(jī)器人運(yùn)動(dòng)學(xué)逆解優(yōu)化的研究[J].機(jī)電產(chǎn)品開發(fā)與創(chuàng)新，2009(05)：21-23.

[8] 盧 貺，宋 霞.基于TB6560 步進(jìn)電機(jī)驅(qū)動(dòng)系統(tǒng)的設(shè)計(jì)[J].武漢船舶職業(yè)技術(shù)學(xué)院學(xué)報(bào),2013(04):36-41.

[責(zé)任編校： 張 眾]

Design of Robot Control System Based on FPGA

ZHANG Daode，YIN Yang，SONG Chenglong

(SchoolofMechanicalEngin.,HubeiUniv.ofTech.,Wuhan430068,China)

Through PROE robot hardware, the study first constructed the five degrees of freedom mechanical structure model, and designed the software platform based on FPGA embedded processor NiosII of SOPC framework, and FPGA controller and stepper motor driver module. It then analysed the positive and inverse problem solutions of manipulators’ kinematics. It also completed the selection of the optimal solution of manipulators by VC, MATLAB and other tools and trajectory planning simulation. It finally used the Robotics Toolbox Toolbox in positive and inverse kinematics simulation to verify the feasibility of the algorithm and the correctness of the solutions.

FPGA;NIOS II;MATLAB; manipulators

2014-09-10

張道德(1973-)， 男， 湖北黃梅人，工學(xué)博士，湖北工業(yè)大學(xué)教授，研究方向?yàn)橹悄芸刂?，圖像識(shí)別

1003-4684(2015)01-0059-06

TP391

A