李國洪，周琪鈞

（天津理工大學(xué) 天津市復(fù)雜系統(tǒng)控制理論及應(yīng)用重點實驗室，天津 300384）

0 引 言

工業(yè)機器人作為一種經(jīng)典的機電一體化機械數(shù)字裝備，體現(xiàn)著人類智慧與機器人技術(shù)的發(fā)展成果，可實現(xiàn)生產(chǎn)制造的模塊化、智能化，技術(shù)附加值高，應(yīng)用范圍廣。作為信息化社會的新興產(chǎn)業(yè)和先進制造業(yè)的支撐技術(shù)，全球各國都將其視為工業(yè)自動化發(fā)展的重點方向，勢必將對未來生產(chǎn)生活和社會發(fā)展起著愈發(fā)重要的作用。

在工業(yè)機器人的運動控制方面，國內(nèi)外眾多學(xué)者都進行了廣泛的研究。如Metin Toz[1]最新研究了一種基于混沌理論的全新渦旋搜索算法來求解帶偏置腕的串行機械臂逆運動學(xué)問題，該算法具有較好的逆解尋優(yōu)精度和收斂速度，但目前還停留在理論仿真尚未應(yīng)用于實際。Matulis Marius和Harvey Carlo[2]利用數(shù)字孿生建模技術(shù)通過強化學(xué)習(xí)方式對機械臂進行虛擬現(xiàn)實分析，將數(shù)字世界與實體世界進行了完美融合。在國內(nèi)，肖國紅等人[3]開發(fā)了一款基于PLC控制的5自由度機械臂實驗平臺，該平臺具有底座旋轉(zhuǎn)、垂直與水平移動、手爪旋轉(zhuǎn)與抓取等5個自由度。朱鵬程等人[4]運用Arduino控制器設(shè)計了一款五自由度機械臂與LabVIEW上位機，該系統(tǒng)可實現(xiàn)五自由度機械臂的基本功能，解決了實際本體實驗成本較高的問題。相較于國內(nèi)，國外研究者不論是算法還是研究方式均領(lǐng)先我國，再者我國機器人研究人才缺口巨大，高校還是采用傳統(tǒng)大型工業(yè)示教系統(tǒng)的教學(xué)方式讓學(xué)生普遍有畏難情緒，且系統(tǒng)本身也具有一定的危險性，學(xué)生的學(xué)習(xí)積極性并不高，所以，開發(fā)一種經(jīng)濟性、直觀性好的工業(yè)機器人控制試驗系統(tǒng)具有很強的現(xiàn)實意義[5]。

本文根據(jù)工業(yè)生產(chǎn)中5R（轉(zhuǎn)動副）工業(yè)機器人結(jié)構(gòu)特點，首先采用DH參數(shù)法建模，建立起工業(yè)機器人運動學(xué)模型，采取坐標變換矩陣推導(dǎo)出機器人運動學(xué)方程，進行正逆運動學(xué)求解，運用Matlab軟件中的Robotics Toolbox進行機器人工作空間和軌跡規(guī)劃的仿真求解。最后通過搭建基于STM32F407的機器人控制系統(tǒng)與PC上位機進行實物聯(lián)合試驗，進一步驗證了運動學(xué)模型的正確性和機器人控制系統(tǒng)設(shè)計的合理性。

1 機器人DH參數(shù)建模

本文搭建的是5R抓取機器人系統(tǒng)，該系統(tǒng)的末端執(zhí)行器具有5個自由度，即n末=5。其中，從下往上數(shù)的前3個轉(zhuǎn)動關(guān)節(jié)主要控制執(zhí)行器的空間位置，而后2個轉(zhuǎn)動關(guān)節(jié)主要控制執(zhí)行器的姿態(tài)，通常來講，控制執(zhí)行器抓手開合的自由度一般不包含在n末中。圖1所示為5R抓取機械臂的DH坐標系機構(gòu)圖。

抓取機械臂可以視為是一系列由運動關(guān)節(jié)串接起來的連桿組成的，為每一連桿構(gòu)建一個坐標系，并用齊次變換來描述這些坐標系之間的相對姿態(tài)和位置。本文5R機械臂DH參數(shù)如表1所示。

表1 5R機械臂DH參數(shù)

由于Matlab擁有強勁的矩陣運算能力和特有的Robotics Toolbox，采用該工具箱中的Link函數(shù)和Robot函數(shù)即可構(gòu)建機械臂各個關(guān)節(jié)的模型方程，該函數(shù)的一般表達式為：

使用Robotics Toolbox中的Link函數(shù)創(chuàng)建該機器人的命令為：

L1 = Link（[0 85 0 0 0],‘modified’）；

L2 = Link（[–π/2 0 10 –π/20],‘modified’）；

L3 = Link（[0 0 105 0 0],‘modified’）；

L4 = Link（[0 0 90 0 0],‘modified’）；

L5 = Link（[0 100 0 –π/20],‘modified’）；

robot = SerialLink（[L1,L2,L3,L4,L5]）；

robot.name = ‘5R抓取機器人’；

robot.plot（[0,–π/2,0,0,0]）；

robot.display（）；

運行如上Matlab代碼后，即可獲得如圖2所示的機械臂模型圖[6]。若加入teach指令，則可通過左側(cè)滑竿框調(diào)節(jié)各個關(guān)節(jié)的角度位置，實現(xiàn)姿態(tài)調(diào)整，如圖3所示。

圖2 機械臂DH參數(shù)模型姿態(tài)圖

圖3 姿態(tài)調(diào)整圖

2 機器人運動學(xué)分析

2.1 5R機械臂正運動學(xué)分析

已知機械臂各個關(guān)節(jié)的轉(zhuǎn)動角度，求取機械臂工具端OT的姿態(tài)和位置，即為機械臂的正解。用坐標變換來描述從坐標系0～0T5的變換。由圖1可知，從坐標系1到坐標系0的變換矩陣為0T1。以此類推1T2、2T3、3T4和4T5。

已知相鄰連桿i與i–1之間的連桿變換通式為：

其中，s和c分別表示三角函數(shù)sin和cos。

根據(jù)連桿變換的齊次矩陣式（2）和表1的連桿參數(shù)，運用D-H法求得各連桿變換矩陣如下：

其中，ci=cosθi、si=sinθi。

5R機械臂的運動模型是由以上5個坐標變換矩陣相乘得到的，這樣即可得到該機械臂的運動學(xué)模型：

機械臂末端位置方程為：

其中，sij=sin（θi+θj）、cij=cos（θi+θj）、sijk=sin（θi+θj+θk）、cijk=cos（θi+θj+θk）。

2.2 5R機械臂逆運動學(xué)分析

逆向運動學(xué)分析指已知末端執(zhí)行器（機械爪）位姿的前提下，求解運動方程，即求得機械臂相應(yīng)各個關(guān)節(jié)角度。這對于機械臂的運動控制至關(guān)重要。根據(jù)0T5可以知道機械臂的運動位置和姿態(tài)，需要獲得各關(guān)節(jié)的坐標值，用以機械臂能夠被驅(qū)動至這個位姿。

其中，K1=c1px+s1py?a1?d5c1ax?d5s1ay、K2=pz?

d1?d5az、K3=c1px+s1py?a1?a2c2?d5（c1ax+s1ay）+d5az+d1?pz?a2s2。

根據(jù)上面各式的結(jié)果來看，機械臂的運動反解可能存在多種解。但由于機械臂結(jié)構(gòu)的約束，例如各關(guān)節(jié)變量可能不會全都在360°范圍內(nèi)可運動，有些解不可能實現(xiàn)。在其存在多種解的情況下，應(yīng)擇其最滿意的一組解，例如：選擇一組與機械臂當(dāng)前位置最接近的解，或者考慮避障要求等，以滿足機械臂實際工作需求。

3 機器人運動學(xué)仿真

3.1 機械臂工作空間仿真

通常來講，機械臂的工作空間指的是末端執(zhí)行器經(jīng)歷所有可能的運動后末端遍歷的全部體積，它是衡量一款機械臂工作能力的一項重要運動學(xué)指標。將本文5R機械臂的工作空間記為S（P）。則工作空間和各個關(guān)節(jié)變量的關(guān)系可以用下式表達：

式中：θ=[θ1，θ2，θ3，θ4，θ5]——關(guān)節(jié)變量；

Q——約束空間，切莫隨意取值，應(yīng)考慮實際情況確定約束空間：

圖4 機械臂工作空間云圖

由圖4可以直觀地看出，機械臂在笛卡爾坐標系中三個方向上的運動基本上可以達到最大范圍內(nèi)的所有空間。在實際的生產(chǎn)作業(yè)中，根據(jù)機械臂的位置數(shù)值可以分析得到其工作空間的邊界條件繼而知曉其運動范圍，通過將機械臂及相關(guān)工件進行合理配置，可實現(xiàn)場所利用率的最大化利用。

3.2 機器人軌跡規(guī)劃仿真

根據(jù)以上邊界約束條件，即可得到插值函數(shù)中b0～b5各值。

jtraj函數(shù)調(diào)用的格式為：[q qd qdd]=jtraj（qz,qr,t）其中，q為關(guān)節(jié)位置，qd為關(guān)節(jié)角速度，qdd為關(guān)節(jié)角加速度；qz為初始位置關(guān)節(jié)角的值，qr為終止位置關(guān)節(jié)角的值，t為軌跡運行的時間。

本文規(guī)劃的路徑軌跡是從 q1=[0，–π/2，0，0，0]運動到 q2=[π/2，–π/3，–π/4，–π，0]的曲線。

則有 qz=q1，qr=q2，設(shè)定時間t=[0∶0.1∶5]，總運行時間為5 s，采樣周期為0.1 s。

對該機械臂各關(guān)節(jié)角度位移、速度及加速度進行規(guī)劃仿真，可得到各關(guān)節(jié)物理量（角位移、角速度、角加速度）隨時間的變化曲線和軌跡路徑如圖5所示。

圖5 機械臂關(guān)節(jié)角位移、角速度、角加速度變化曲線及軌跡路徑

由圖5可以觀察到各個關(guān)節(jié)三個物理量（角位移、角速度、角加速度）隨時間變化的情況。從三幅圖中可以直觀地看到每個關(guān)節(jié)獨立運動互不干擾，并且各關(guān)節(jié)起點和末點速度與加速度為零，軌跡保持平滑連續(xù)。

4 機器人控制系統(tǒng)設(shè)計

4.1 控制器系統(tǒng)電路設(shè)計

以STM32F407嵌入式單片機為核心，設(shè)計了控制機械臂六組電機（包括工具端執(zhí)行器電機）的硬件電路。該電路包含STM32F407最小系統(tǒng)、JTAG調(diào)試電路、芯片供電電路、濾波電路、FLASH存儲電路、USB轉(zhuǎn)串口電路、電機接口電路、RS485和CAN通信電路、按鍵、蜂鳴器和LED指示燈電路等。硬件電路原理圖由Altium公司發(fā)布的 Altium Designer（in AD19）軟件設(shè)計?？刂葡到y(tǒng)框圖如圖6所示。

圖6 控制系統(tǒng)框圖

4.2 控制系統(tǒng)軟件設(shè)計及上位機介紹

該控制系統(tǒng)采用C程序設(shè)計進行軟件開發(fā)，KEIL MDK5為開發(fā)工具。圖7為控制系統(tǒng)軟件流程示意圖，該流程主要由主程序和中斷服務(wù)子程序構(gòu)成。主程序包含系統(tǒng)初始化、機械臂初始位姿設(shè)定及其控制參數(shù)初始化，系統(tǒng)初始化又包含LED和蜂鳴器初始化、定時器TIM初始化、ADC初始化、中斷配置和串口通信配置等。中斷服務(wù)子程序主要由3個中斷服務(wù)函數(shù)構(gòu)成，包括判定電機供電是否正常的定時器中斷TIM2服務(wù)子函數(shù)、接收判斷PC上位機監(jiān)控軟件指令的串口中斷USART1服務(wù)子函數(shù)和輸出電機控制PWM的定時器中斷TIM3服務(wù)子函數(shù)。在main函數(shù)主循環(huán)中，TIM2中斷優(yōu)先級最高，全程一旦檢測到電機供電異常，蜂鳴器就會報警提示；串口中斷USART1優(yōu)先級次之，其主要任務(wù)是接收并判斷上位機指令是否符合既定控制指令并將其提取出來；TIM3中斷優(yōu)先級最低，其主要任務(wù)是將提取到的控制指令轉(zhuǎn)換為相應(yīng)的PWM輸出，驅(qū)動機械臂對應(yīng)關(guān)節(jié)運動，使之完成相應(yīng)指令。

圖7 控制系統(tǒng)軟件流程框圖

PC上位機監(jiān)控軟件采用微軟發(fā)行的Visual Studio軟件C#語言開發(fā)。軟件界面如圖8所示。采用上位機控制機械臂前，首先將micro-USB線纜連接控制板USB串口和計算機USB接口，再選定所用的串口號并設(shè)置好波特率后，即可對機械臂進行操作。該上位機設(shè)計有2種模式，分別為正、逆運動學(xué)演示模式。在正運動學(xué)模式中，上位機可以控制每臺電機的運動并且用戶能自主設(shè)計機械臂的運行動作；在逆運動學(xué)演示中，輸入起、終點坐標，機械臂可以平滑地由起點運行到終點，若運行不到或失敗，則有相應(yīng)的警告提示，體現(xiàn)出逆運動學(xué)。

圖8 上位機監(jiān)控軟件界面

5 系統(tǒng)試驗

根據(jù)上節(jié)所設(shè)計的機器人控制系統(tǒng)，完成機械臂與控制板及PC上位機的連接與安裝，圖9為STM32機械臂控制板實物圖，圖10為機械臂控制試驗系統(tǒng)連接圖。

圖9 STM32F4控制板

圖10 機器人控制系統(tǒng)連接圖

首先，連接機械臂與控制板。整個過程中要確保各連線準確無誤，同時保證電源開關(guān)等關(guān)鍵部件是否工作正常。當(dāng)一切準備就緒后，則可對機械臂進行控制。

為了驗證前文所述軌跡規(guī)劃的正確性，設(shè)計一條從起點[0–π/2 0 0 0]運動到終點[π/4–π/2–π/2–π/3 0]的曲線。運用Matlab中所搭建的模型仿真，并取軌跡中的5個點為測試點，通過Matlab可獲得這5個點各自所對應(yīng)的各電機角度理論值（見表2）。

表2 測試點關(guān)節(jié)角度（Matlab獲得）

然后將這5個點換算成對應(yīng)的空間坐標，輸入到PC上位機逆運動學(xué)界面的坐標輸入窗口中（見圖8），運行后可根據(jù)Visual Studio的控制臺窗口查看測試點所對應(yīng)的各電機實際要驅(qū)動的角度（見表3）。通過將兩表進行對比，可發(fā)現(xiàn)各測試點仿真與實際中各電機轉(zhuǎn)動角度差別很小，考慮到系統(tǒng)開發(fā)過程中實物平臺的測量誤差和實物剛性的問題，可證明系統(tǒng)逆運動學(xué)編程開發(fā)的正確性和軌跡規(guī)劃的合理性。

表3 測試點關(guān)節(jié)驅(qū)動角度（Visual Studio算得）

整個試驗過程中，機械臂運轉(zhuǎn)正常，較準確經(jīng)過所要求的路徑點，響應(yīng)速度快，各關(guān)節(jié)之間運行平穩(wěn)、連續(xù)無停頓，表明該機械臂控制試驗系統(tǒng)基本符合要求。

6 結(jié)束語

首先采用D-H參數(shù)建模法建立了工業(yè)機器人運動學(xué)模型，進而采用坐標變換矩陣推導(dǎo)出其運動學(xué)方程并進行正逆運動學(xué)求解。接著運用Matlab進行運動學(xué)仿真，根據(jù)Monte Carlo法和jtraj函數(shù)對機械臂的工作空間和軌跡規(guī)劃進行求解。在最后的實物驗證中，雖然試驗結(jié)果基本符合預(yù)期，但實驗過程都是在無障礙無干擾的理想環(huán)境中進行的，而實際情況并沒有這樣簡單，所以接下來的計劃是：1）在存在干擾因素的情況下對機器人進行軌跡規(guī)劃；2）解運動方程過程中若涉及方程非線性問題，可能導(dǎo)致無法推導(dǎo)出解析解，這時又應(yīng)作何處理；3）文本研究的內(nèi)容，暫不涉及動力學(xué)，而實際情況需將其考慮進去以達到更優(yōu)的控制效果。