六自由度機(jī)械手仿真與實(shí)訓(xùn)平臺(tái)開發(fā)

彭芳 于楚泓 楊亮 黎萍 鄧文彬

摘? 要 針對(duì)目前六自由度機(jī)械手的實(shí)驗(yàn)項(xiàng)目大多采用示教編程練習(xí)和二次開發(fā)使用，實(shí)驗(yàn)項(xiàng)目與機(jī)器人技術(shù)教學(xué)內(nèi)容脫節(jié)等問題，提出一種模塊化結(jié)構(gòu)的仿真平臺(tái)和實(shí)訓(xùn)平臺(tái)相結(jié)合的實(shí)驗(yàn)平臺(tái)建設(shè)方案，利用MATLAB與ADAMS開發(fā)仿真平臺(tái)，實(shí)現(xiàn)機(jī)械手的正逆運(yùn)動(dòng)學(xué)求解、軌跡規(guī)劃、運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)仿真，開發(fā)基于ARM+FPGA架構(gòu)的開放式底層控制平臺(tái)，設(shè)計(jì)HMI人機(jī)交互界面，編寫C語言算法程序?qū)崿F(xiàn)機(jī)械手軌跡控制。實(shí)驗(yàn)平臺(tái)具有層次遞進(jìn)、易于擴(kuò)展、深入底層控制等特點(diǎn)。實(shí)踐證明，該平臺(tái)在本科教學(xué)和畢業(yè)設(shè)計(jì)中取得良好效果。

關(guān)鍵詞 六自由度機(jī)械手;機(jī)器人技術(shù);仿真平臺(tái);實(shí)訓(xùn)平臺(tái);控制系統(tǒng);MATLAB

中圖分類號(hào)：TP391.9? ? 文獻(xiàn)標(biāo)識(shí)碼：B

文章編號(hào)：1671-489X（2019）10-0012-04

Abstract At present， most of the experiment items of the 6-DOF manipulator are implemented by teaching-programming practice or?secondary development， which is out of touch with the teaching con-tent of Robot Technology. A modular structure platform of combining?simulation and training is proposed. MATLAB and ADAMS are used?to develop the simulation platform which realizes the forward and reverse kinematics solution， trajectory planning， kinematics and dynamics simulation of the manipulator. An open low-level control platform based on ARM+FPGA architecture is also developed. HMI man-machine interactive interfaces are designed and the trajectory control of manipulator is realized by programming algorithm in C?language. The experimental platform has the characteristics of pro-gressive hierarchy， easy extension and deep low-level control. The?practice shows that the platform has achieved good results in under-graduate teaching and graduation design.Key words 6-DOF manipulator; robot technology; simulation plat-form; training platform; control system; MATLAB

1 前言

隨著我國(guó)從制造大國(guó)向制造強(qiáng)國(guó)的邁進(jìn)，工業(yè)機(jī)器人將逐步取代人力，成為生產(chǎn)中必不可少的自動(dòng)化裝備。六自由度機(jī)械手作為工業(yè)機(jī)器人的典型代表，廣泛用于工業(yè)搬運(yùn)、機(jī)械加工、焊接、裝配、打磨拋光、噴涂等工業(yè)生產(chǎn)過程[1]。然而國(guó)內(nèi)發(fā)展現(xiàn)狀是，機(jī)械手的“三大”核心部件伺服電機(jī)、減速機(jī)和控制器大部分依賴進(jìn)口[2]，嚴(yán)重制約了我國(guó)機(jī)器人產(chǎn)業(yè)的發(fā)展，本土機(jī)器人相關(guān)人才的培養(yǎng)已刻不容緩。

在面向本科教育的工業(yè)機(jī)器人教學(xué)實(shí)踐中，一般方法是購(gòu)買成套六自由度機(jī)械手平臺(tái)進(jìn)行示教編程練習(xí)和二次開發(fā)使用[3-4]，極大限制了學(xué)生對(duì)于機(jī)械手底層控制算法和控制方法的理解和研究。本文根據(jù)機(jī)器人技術(shù)教學(xué)目標(biāo)，提出一種模塊化結(jié)構(gòu)的仿真平臺(tái)和實(shí)訓(xùn)平臺(tái)，采用階梯遞進(jìn)式學(xué)習(xí)，一步一步引導(dǎo)學(xué)生從機(jī)械手的運(yùn)動(dòng)軌跡規(guī)劃求解入手，到實(shí)際控制電路和程序開發(fā)的訓(xùn)練，對(duì)機(jī)器人技術(shù)進(jìn)行深入剖析。

2 六自由度機(jī)械手的正逆運(yùn)動(dòng)學(xué)求解

機(jī)器人運(yùn)動(dòng)學(xué)是機(jī)器人技術(shù)的研究基礎(chǔ)。機(jī)器人運(yùn)動(dòng)學(xué)就是把機(jī)器人的空間位移解析地表示為時(shí)間的函數(shù)，研究關(guān)節(jié)變量空間和末端執(zhí)行器位姿之間的關(guān)系。本文以實(shí)驗(yàn)室的LT950六自由度機(jī)械手為例（如圖1所示），探討機(jī)械手正逆運(yùn)動(dòng)學(xué)求解算法，繼而在MATLAB中根據(jù)求解的結(jié)果進(jìn)行空間軌跡規(guī)劃。

正運(yùn)動(dòng)學(xué)解析? Denavit和Hartenberg提出的連桿坐標(biāo)系法是機(jī)器人運(yùn)動(dòng)學(xué)建模的通用方法。本文采用D-H前置坐標(biāo)系法，建立機(jī)械手連桿坐標(biāo)如圖2所示，其中d2=90 mm，

根據(jù)圖2坐標(biāo)系，確定各連桿的D-H參數(shù)和關(guān)節(jié)變量，得到機(jī)械手連桿參數(shù)如表1所示。

通過D-H方法，可以建立起機(jī)器人各個(gè)關(guān)節(jié)的坐標(biāo)系，相鄰兩桿件的坐標(biāo)變換關(guān)系用變換矩陣表示為：

利用式（2），若機(jī)器人結(jié)構(gòu)參數(shù)已知，則當(dāng)給出機(jī)械手各個(gè)運(yùn)動(dòng)關(guān)節(jié)的關(guān)節(jié)變量θi時(shí)，就可以確定出運(yùn)動(dòng)學(xué)方程中各位姿矩陣的值，從而求得機(jī)械手末端執(zhí)行器的位置和姿態(tài)。

逆運(yùn)動(dòng)學(xué)求解? 運(yùn)動(dòng)學(xué)逆解問題是給定機(jī)器人末端執(zhí)行器需要達(dá)到的位置和姿態(tài)，求解機(jī)器人各關(guān)節(jié)的關(guān)節(jié)變量，即已知T的值，求解θi。本文采用代數(shù)法進(jìn)行逆運(yùn)動(dòng)求解，代數(shù)法求逆解在很多機(jī)器人經(jīng)典教材和文獻(xiàn)中都有較詳細(xì)的論述[5-7]。根據(jù)代數(shù)法，如果連桿末端的位姿已知，即式（2）已知，則可以采用依次左乘的方法，把關(guān)節(jié)變量分離出來，進(jìn)而求解出全部關(guān)節(jié)角度。

在求機(jī)器人運(yùn)動(dòng)學(xué)逆解過程中，關(guān)節(jié)軸4和6重合，操作臂處于奇異形位，編程時(shí)需要進(jìn)行奇異形位判別后確定θ4的值[7]。

六自由度機(jī)器人存在八組逆解，而機(jī)器人的控制需要選擇其中一組作為最優(yōu)解，由于機(jī)器人結(jié)構(gòu)的限制，求得的逆解可能會(huì)超出關(guān)節(jié)變量范圍。運(yùn)動(dòng)學(xué)逆解的選擇并沒有統(tǒng)一的標(biāo)準(zhǔn)，應(yīng)根據(jù)具體的實(shí)際情況而定，一般情況下滿足行程最短、功率最省、回避障礙和受力最好等原則要求。

本文在不考慮避障的前提下，最優(yōu)解的求取算法如圖3所示：首先根據(jù)關(guān)節(jié)變量范圍，淘汰不在范圍內(nèi)的逆解;接著進(jìn)行奇異點(diǎn)的處理;最后根據(jù)“行程最短”準(zhǔn)則，選取一組最優(yōu)解。

3 機(jī)械手仿真平臺(tái)開發(fā)

MATLAB仿真平臺(tái)開發(fā)? 本文使用Robotics Toolbox工具箱的SerialLink（）函數(shù)，創(chuàng)建機(jī)械手模型，編寫自己的正逆解算法函數(shù)。為了實(shí)現(xiàn)良好的人機(jī)交互功能，開發(fā)相應(yīng)的GUI界面。圖4為正逆運(yùn)動(dòng)學(xué)測(cè)試系統(tǒng)。首先在文本框中輸入機(jī)械手的六個(gè)關(guān)節(jié)角，通過調(diào)用正解函數(shù)進(jìn)行正向運(yùn)動(dòng)學(xué)求解，得到機(jī)械手的當(dāng)前位姿;有了機(jī)械手位姿后，可點(diǎn)擊逆解求解調(diào)用逆解函數(shù)，求得最優(yōu)關(guān)節(jié)角度值。通過多次反復(fù)測(cè)試，正逆解結(jié)果與設(shè)定值完全一致，從而驗(yàn)證正逆解算法的正確性。

機(jī)械手軌跡規(guī)劃的一般方法是笛卡爾空間軌跡規(guī)劃[8]。學(xué)生可以編寫直線插補(bǔ)、圓弧插補(bǔ)、樣條插補(bǔ)等插補(bǔ)程序進(jìn)行軌跡規(guī)劃研究，Robotics Toolbox工具箱的ctraj函數(shù)也可以實(shí)現(xiàn)笛卡爾空間軌跡規(guī)劃。設(shè)計(jì)典型的軌跡規(guī)劃交互界面如圖5所示，可通過設(shè)定末端軌跡方程，進(jìn)行笛卡爾空間軌跡規(guī)劃。

在圖5窗口輸入具有代表性的空間螺旋線作為目標(biāo)軌跡，螺旋線參數(shù)方程為：

式中，k=0：pi/20：3*pi。需要對(duì)式（3）中的螺旋線進(jìn)行分段取點(diǎn)規(guī)劃。機(jī)械手的初始角度值為q0=[0? -pi/2 0 0 pi/2 0]，初始位姿為T0 =[1 0 0 450; 0 -1 0 -90; 0 0 -1 596; 0 0 0 1]，在機(jī)械手軌跡規(guī)劃中，末端姿態(tài)保持不變;接著運(yùn)用逆解函數(shù)對(duì)規(guī)劃出的所有末端位姿進(jìn)行關(guān)節(jié)角度求取;最后根據(jù)關(guān)節(jié)角度矩陣用plot（）函數(shù)繪制機(jī)械手運(yùn)動(dòng)軌跡。在圖形輸出窗口可觀察到機(jī)械手運(yùn)行的全部動(dòng)態(tài)過程，可從仿真結(jié)果對(duì)插補(bǔ)算法和正逆運(yùn)動(dòng)學(xué)算法程序的有效性進(jìn)行直觀判別。

ADAMS虛擬樣機(jī)開發(fā)? 在PRO/E中對(duì)機(jī)械手的各個(gè)零部件進(jìn)行精確的造型設(shè)計(jì)，確定底部模型、大臂模型、小臂模型、手部模型等的參數(shù)，對(duì)各模塊進(jìn)行組裝成型。再將整體模型導(dǎo)入ADAMS軟件，為各個(gè)零部件設(shè)置正確的傳動(dòng)方式、驅(qū)動(dòng)方式，從而進(jìn)行約束、驅(qū)動(dòng)的添加。ADAMS是虛擬樣機(jī)分析應(yīng)用軟件，用戶可以運(yùn)用該軟件非常方便地對(duì)虛擬機(jī)械系統(tǒng)進(jìn)行運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)分析。ADAMS還提供有與MATLAB的仿真接口，可以用Simulink實(shí)現(xiàn)機(jī)械手的動(dòng)力學(xué)控制算法仿真，也可以直接在各個(gè)關(guān)節(jié)導(dǎo)入MATLAB中規(guī)劃好的角度值，進(jìn)行運(yùn)動(dòng)學(xué)仿真[9-11]，如圖6所示。

4 機(jī)械手實(shí)訓(xùn)平臺(tái)開發(fā)

為了進(jìn)一步加強(qiáng)學(xué)生對(duì)機(jī)械手實(shí)際控制的理解，本文開發(fā)了針對(duì)底層控制的開放式控制平臺(tái)。機(jī)械手本體采用的是安川公司的伺服電機(jī)和伺服驅(qū)動(dòng)器。從學(xué)生實(shí)踐角度出發(fā)，伺服電機(jī)采用經(jīng)典的位置控制方式，此時(shí)伺服驅(qū)動(dòng)器的主要輸入信號(hào)有PULS脈沖驅(qū)動(dòng)信號(hào)、SIGN方向信號(hào)、SEN絕對(duì)值編碼器使能信號(hào)、伺服電機(jī)ON信號(hào);輸出信號(hào)有絕對(duì)值編碼器A、B、C相信號(hào)與旋轉(zhuǎn)量串行數(shù)據(jù)。

在分析比較各種方案之后，本文采用ARM與FPGA結(jié)合的控制方式，其總體架構(gòu)如圖7所示。由于FPGA是并行運(yùn)算，以硬件描述語言來實(shí)現(xiàn)，在信號(hào)發(fā)送和接收方面直接采用硬件電路實(shí)現(xiàn)，省去軟件上的延時(shí)，速度更快、更準(zhǔn)確。機(jī)械手控制系統(tǒng)主要包括以下四個(gè)部分：

第一部分是基于ARM的CortexM4系列內(nèi)核F4芯片的主控模塊;

第二部分是以FPGA為核心的Cyclone IV E系列的EP4CE30F23C8模塊;

第三部分是接口電路部分;

第四部分是人機(jī)交互系統(tǒng)。

主控模塊? ARM主控模塊主要負(fù)責(zé)控制系統(tǒng)初始化、任務(wù)調(diào)度、軌跡生成算法運(yùn)算、機(jī)器人正逆運(yùn)動(dòng)學(xué)求解、正交編碼信號(hào)處理與PD控制算法、控制模塊通信與人機(jī)交互系統(tǒng)的通信等。為了應(yīng)付如此繁雜任務(wù)的處理，主控模塊上運(yùn)行有實(shí)時(shí)的操作系統(tǒng)uCOS-III，通過實(shí)時(shí)操作系統(tǒng)對(duì)ARM的硬件資源進(jìn)行合理的分配調(diào)度，在合理的時(shí)間內(nèi)完成各項(xiàng)任務(wù)。主控模塊將處理后的數(shù)據(jù)通過FSMC并行方式發(fā)送到FPGA。

FPGA模塊? FPGA設(shè)計(jì)有伺服電機(jī)驅(qū)動(dòng)模塊和正交編碼器信號(hào)接收模塊。驅(qū)動(dòng)模塊的任務(wù)是生成伺服電機(jī)的控制信號(hào)，該模塊接收來自主控模塊發(fā)來的并行數(shù)據(jù)指令，包括脈沖頻率、脈沖個(gè)數(shù)、方向信號(hào)與位置完成信號(hào)等，將包含相對(duì)角度位置的旋轉(zhuǎn)指令轉(zhuǎn)化為PWM脈沖輸出信號(hào)，再發(fā)送至接口電路模塊進(jìn)行信號(hào)轉(zhuǎn)換與隔離，從而驅(qū)動(dòng)機(jī)械手各關(guān)節(jié)運(yùn)動(dòng);正交編碼器信號(hào)接收模塊的任務(wù)是接收伺服驅(qū)動(dòng)器發(fā)出的正交編碼信號(hào)，需要設(shè)計(jì)四倍頻電路來準(zhǔn)確捕捉A、B相信號(hào)，再將計(jì)數(shù)數(shù)據(jù)發(fā)送給主控模塊進(jìn)行角度運(yùn)算。

接口電路? 接口電路的主要任務(wù)是進(jìn)行差分和單端信號(hào)的轉(zhuǎn)換、信號(hào)電源的隔離。接口電路主要包括信號(hào)轉(zhuǎn)換電路、通信電路以及隔離電路。

人機(jī)交互系統(tǒng)? 人機(jī)交互系統(tǒng)主要是實(shí)現(xiàn)操作人員對(duì)機(jī)械手運(yùn)動(dòng)方式及軌跡的指定。人機(jī)交互界面的開發(fā)，選用的是輝度智能系統(tǒng)公司的WTH207A HMI人機(jī)界面，開發(fā)環(huán)境為WitGrid組態(tài)軟件，WinCE操作系統(tǒng)。通過人機(jī)交互界面，能夠?qū)崟r(shí)觀察反饋回來的機(jī)械手末端的空間坐標(biāo)位置及角度值;能夠采用關(guān)節(jié)點(diǎn)動(dòng)控制對(duì)空間點(diǎn)進(jìn)行采集，并選擇插補(bǔ)算法，實(shí)現(xiàn)運(yùn)動(dòng)軌跡的設(shè)定，機(jī)械手能夠根據(jù)畫面的設(shè)定按計(jì)算好的運(yùn)行軌跡運(yùn)動(dòng)，完成相應(yīng)的動(dòng)作。人機(jī)交互系統(tǒng)與ARM之間是通過Modbus協(xié)議進(jìn)行通信，采用組態(tài)軟件方式也易于學(xué)生進(jìn)行界面開發(fā)。

以學(xué)生開發(fā)的運(yùn)行界面為例，如圖8所示，通過點(diǎn)動(dòng)操作機(jī)械手各關(guān)節(jié)進(jìn)行末端坐標(biāo)值采樣，兩個(gè)采樣點(diǎn)確定后，點(diǎn)擊啟動(dòng)，就可以讓機(jī)械手末端以設(shè)定的速度從采樣點(diǎn)1直線運(yùn)行到采樣點(diǎn)2。

5 平臺(tái)的使用和運(yùn)行

學(xué)生首先在MATLAB和ADAMS下進(jìn)行軌跡規(guī)劃與仿真;然后在ARM中編寫C語言算法程序和開發(fā)HMI人機(jī)界面，編寫的機(jī)械手算法程序可以與MATLAB中的仿真結(jié)果相互印證，以確定算法的可靠性;同時(shí)，由于FPGA采用的是可編程邏輯單元陣列，學(xué)生也可以通過編程的方式改進(jìn)驅(qū)動(dòng)電路，訓(xùn)練電路設(shè)計(jì)能力。

6 結(jié)語

本文所開發(fā)的仿真與實(shí)訓(xùn)平臺(tái)，采用階梯遞進(jìn)方式，引導(dǎo)學(xué)生由淺入深，逐步掌握與機(jī)械手控制技術(shù)相關(guān)的原理與應(yīng)用。平臺(tái)采用模塊化開放式結(jié)構(gòu)，易于擴(kuò)展和改進(jìn)，可有效激發(fā)學(xué)生綜合運(yùn)用所學(xué)知識(shí)不斷創(chuàng)新，為進(jìn)一步的機(jī)器人控制研究打下基礎(chǔ)，培養(yǎng)機(jī)械手控制系統(tǒng)開發(fā)人才。實(shí)驗(yàn)平臺(tái)已應(yīng)用于本科機(jī)器人技術(shù)教學(xué)與畢業(yè)設(shè)計(jì)，收到良好的效果，培養(yǎng)的人才受到用人單位的極大肯定?！?/p>

參考文獻(xiàn)

[1]鄧必懋，朱濤.淺析我國(guó)工業(yè)機(jī)器人技術(shù)的產(chǎn)業(yè)化發(fā)展[J].機(jī)械工程與自動(dòng)化，2017（2）：222-224.

[2]孟明輝，周傳德，陳禮彬，等.工業(yè)機(jī)器人的研發(fā)及應(yīng)用綜述簡(jiǎn)[J].上海交通大學(xué)學(xué)報(bào)，2016（S1）：98-101.

[3]榮繼民.工業(yè)機(jī)器人教學(xué)實(shí)驗(yàn)工作站[J].實(shí)驗(yàn)技術(shù)與管理，2001，18（5）：17-20.

[4]李頎，強(qiáng)華.工業(yè)機(jī)器人實(shí)驗(yàn)教學(xué)平臺(tái)[J].實(shí)驗(yàn)技術(shù)與管理，2018，35（4）：166-170.

[5]Niku S B. Introduction to Robotics Analysis， Con-

trol， Applications[M].2nd Edition. Beijing， Publi-shing House of Electronics Industry，2013，3：66-70.

[6]Craig J J. Introduction to Robotics Mechanics and

Control[M].3rd Edition. Beijing， China Machine Press，

2013：83-86.

[7]熊有倫.機(jī)器人技術(shù)基礎(chǔ)[M].武漢：華中理工大學(xué)出版社，1996.

[8]林仕高，劉曉麟，歐元賢.機(jī)械手笛卡爾空間軌跡規(guī)劃研究[J].機(jī)械設(shè)計(jì)與制造，2013（3）：49-52.

[9]武金藝，黃金鳳，李煒，等.基于SolidWorks和ADAMS的裝車機(jī)械手運(yùn)動(dòng)仿真[J].機(jī)床與液壓，2016，44（9）：52-56.

[10]林海波，王延兵，張毅.基于ADAMS機(jī)械手的動(dòng)力學(xué)分析[J].制造業(yè)自動(dòng)化，2012，34（22）：80-83.

[11]董其維.采摘機(jī)器人機(jī)械手夾緊裝置優(yōu)化設(shè)計(jì)：基于Pro/E和ADAMS聯(lián)合仿真[J].農(nóng)機(jī)化研究，2017，39（5）：

226-230.