李國洪，周琪鈞

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

0 引 言

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

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

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

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

本文搭建的是5R抓取機(jī)器人系統(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抓取機(jī)械臂的DH坐標(biāo)系機(jī)構(gòu)圖。

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

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

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

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

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抓取機(jī)器人’；

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

robot.display（）；

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

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

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

2 機(jī)器人運(yùn)動學(xué)分析

2.1 5R機(jī)械臂正運(yùn)動學(xué)分析

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

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

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

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

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

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

機(jī)械臂末端位置方程為：

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

2.2 5R機(jī)械臂逆運(yùn)動學(xué)分析

逆向運(yùn)動學(xué)分析指已知末端執(zhí)行器（機(jī)械爪）位姿的前提下，求解運(yùn)動方程，即求得機(jī)械臂相應(yīng)各個關(guān)節(jié)角度。這對于機(jī)械臂的運(yùn)動控制至關(guān)重要。根據(jù)0T5可以知道機(jī)械臂的運(yùn)動位置和姿態(tài)，需要獲得各關(guān)節(jié)的坐標(biāo)值，用以機(jī)械臂能夠被驅(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é)果來看，機(jī)械臂的運(yùn)動反解可能存在多種解。但由于機(jī)械臂結(jié)構(gòu)的約束，例如各關(guān)節(jié)變量可能不會全都在360°范圍內(nèi)可運(yùn)動，有些解不可能實現(xiàn)。在其存在多種解的情況下，應(yīng)擇其最滿意的一組解，例如：選擇一組與機(jī)械臂當(dāng)前位置最接近的解，或者考慮避障要求等，以滿足機(jī)械臂實際工作需求。

3 機(jī)器人運(yùn)動學(xué)仿真

3.1 機(jī)械臂工作空間仿真

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

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

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

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

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

3.2 機(jī)器人軌跡規(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為軌跡運(yùn)行的時間。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 系統(tǒng)試驗

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

圖9 STM32F4控制板

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

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

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

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

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

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

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

6 結(jié)束語

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