基于FPGA的串聯機器人控制系統設計

張道德， 尹 洋, 宋成龍

(湖北工業大學機械工程學院， 湖北 武漢 430068)

?

基于FPGA的串聯機器人控制系統設計

張道德， 尹 洋, 宋成龍

(湖北工業大學機械工程學院， 湖北 武漢 430068)

通過Pro/Enginee構建5自由度機械手硬件機械結構模型，設計基于FPGA嵌入式處理器的NiosII的SOPC架構系統的軟件平臺，以及FPGA控制器與步進電機驅動模塊，并分析機械臂運動學的正解、逆解問題，通過VC、MATLAB等工具實現機械臂的最優解的選取、軌跡規劃的仿真，以及運用Robotics Toolbox工具箱進行正逆運動學仿真，驗證算法可行性和求解的正確性。

現場可編程邏輯門陣列; 機器人控制; 機械臂

機械臂的控制方式主要有3種方案：第一種是以PC機作為硬件平臺，利用VC軟件實現機械臂的算法控制，PC的硬件配置要求較高；第二種是PC機和運動控制卡的結合方案，控制卡安裝在PC機中，處理器完成部分控制算法，主要任務由運動控制卡完成，控制系統比較穩定；第三種是機器人控制模塊與PC機的結合，模塊化具有高效、可移植性好的特點，但價格昂貴。本文提出的控制方式是通過FPGA作為硬件平臺，在FPGA 里完成算法控制，將控制結果輸出到驅動模塊，實現機械臂的實時控制。由于FPGA具有高效、并行執行等特點，對機器臂的控制具有很好的效果。

1 硬件結構模型的構建

1.1 機械臂模型建立

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

圖1 機械臂機械結構模型

機械臂由若干關節和連桿串聯組成，采用D-H參數法來確定每個關節上的坐標姿態和相鄰兩個坐標系之間的相對平移距離和旋轉角度[1]。機器人各關節坐標關系及機構簡圖如圖2所示。D-H參數由連桿長度a、連桿扭角αi、連桿距離di和連桿轉角θi構成。D-H參數如表1所示。

圖2 機器人坐標關系

表1 機械臂D-H參數

連桿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 串聯機械臂工作空間的分析和仿真

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

圖3 SimMechanics模型

圖4 機械臂工作空間三維圖

2 系統方案的設計

2.1 系統工作流程

5自由度串聯機械臂控制系統方案比較復雜。數據采集模塊采集到起始位姿和終止位姿數據后，通過串口發送到FPGA 內部，在FPGA內部實現運動學的逆解和解的最優化，然后進行軌跡規劃，在將指令發送到脈沖驅動模塊驅動機械臂(圖5)。

圖5 系統工作流程圖

由于在FPGA內部采用Verilog HDL實現運動學逆解及其最優化，矩陣運算等運算非常復雜，因此采用FPGA 內部的Nios II軟核處理器C語言處理復雜運算，脈沖驅動模塊等簡單運算可以運用Verilog HDL硬件語言實現。兩種方式相互結合會使方案實現起來可行性更高。

2.2 SOPC系統的構建

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

圖6 SOPC系統

2.3 自定義Avalon-MM總線

Avalon總線由ALTERA公司提出，用于在基于FPGA的片上系統中連接片內處理器和片內外設的總線結構。 連接到Avalon總線的設備分為主從設備，并各有其工作模式。Avalon總線的讀/寫時序是指外設之間產生的一次讀/寫數據總線接口各個信號之間的時序關系。在clk的第一個上升沿，Avalon總線傳遞address,byteenable_n和read_n信號到目標外設Avalon總線模塊內部對address進行譯碼，產生片選并驅動從端口的chipselect信號，從而輸出readdata[3]。從組件的基本讀時序如圖8所示。

圖7 系統RTL視圖

圖8 從組件的基本讀時序

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

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

2.4 機械臂運動學逆解及優化

機器人運動學的逆向求解問題是指已知機器人末端操作器(手部)要到達的目標位姿的情況下，通過坐標逆變換來求出所需的各個關節變量平移和旋轉值，以驅動各關節的伺服電機或步進電機旋轉，使手部的位姿得到滿足.這就是機器人的反向運動學問題。

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

依據表1 機械臂D-H參數,求解機械臂的整體坐標變換矩陣公式如下

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

(1)

求解機器人的運動學方程：

(2)

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

通過解析法可以求得機械臂的逆向運動學方程，最終求得每個轉角的值。已知運動學方程的位姿矩陣

(3)

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

(4)

(5)

(6)

(7)

(8)

通過算運,可以分別求出機械臂每個電機的轉角，但是由于有反三角函數存在，因此運算存在多解情況。

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

圖11 5自由度機械臂逆解

需要讓每個步進電機耗能最小，則選用距離最優的算法。該方法使機械臂的每個關節運動量最少。建立的最優解目標函數F1和F2，F1是優化關節1、關節2和關節3的目標函數，F2是優化關節4和關節5的目標函數，如

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

圖12 逆解最優化流程圖

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

圖13 距離最優化結果

Mtalab下的RoboticsToolbox可以對其進行仿真和驗證，Ikine()函數作用是求解逆向運動學，函數調用格式如下

(9)

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

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

2.5 軌跡規劃的仿真

軌跡規劃是工業機器人的控制基礎，其控制目標是平穩、快速和精確實現所規劃的軌跡運動，否則將對機械臂的機械結構部件造成一定的磨損，并可能導致機械臂運動時的震動和沖擊，最終影響機械臂的運動精度和定位精度。

圖14 5R機械臂逆解程序及結果

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

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

(10)

其中：q0、qa、qh、qb-a和qb分別是對應t0、ta、th、tb-a和tb時間點的關節位置。

運用MATLAB進行仿真，通過拋物線過渡的線性插值法，得到結果見圖15。

圖15 機械臂關節角位置、角速度和角加速度曲線

3 系統PCB板設計

3.1 機械臂FPGA控制系統PCB板設計

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

圖16 機械臂Fpga控制系統原理圖

3.2 步進電機驅動板設計

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

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

圖17 電機控制信號隔離電路

圖18 電機驅動PCB

4 結束語

本文構建了機械臂三維模型以仿真機械臂的工作空間，并且提出了更加方便、高效的機械臂控制方案，構建了SOPC控制系統，分析并仿真了機械臂運動學的逆解、優化和軌跡規劃等問題，最后設計出了FPGA的控制版和步進電機驅動板。本文較為完整地設計了串聯機器人的控制方案，并驗證了其可行性。

[1] 王 強.基于FPGA的5自由度工業機械臂控制系統研究[D].武漢:湖北工業大學，2012.

[2] 許衛斌，平雪良.6R型串聯機器人控制系統的軟件實現[J].中國制造業信息化,2012,41(17):60-63.

[3] 劉福奇.VerilogHDL設計與實戰[M]北京：北京航空航天大學出版社，2012.

[4] 華清遠見嵌入式培訓中心.FPGA應用開發入門與典型實例[M].北京：人民郵電出版社，2008.

[5] 吳厚航.愛上FPGA開發——特權和你一起學NIOSII[M].北京：北京航空航天大學出版社，2011.

[6] 韓建海.工業機器人[M]. 第二版.武漢：華中科技大學出版社，2014.

[7] 周國義，謝明紅.6 自由度解耦機器人運動學逆解優化的研究[J].機電產品開發與創新，2009(05)：21-23.

[8] 盧 貺，宋 霞.基于TB6560 步進電機驅動系統的設計[J].武漢船舶職業技術學院學報,2013(04):36-41.

[責任編校： 張 眾]

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-)， 男， 湖北黃梅人，工學博士，湖北工業大學教授，研究方向為智能控制，圖像識別

1003-4684(2015)01-0059-06

TP391

A