何浪，謝明紅，劉馳弋

（華僑大學(xué)機(jī)電及自動(dòng)化學(xué)院，福建廈門 361021）

0引言

隨著世界“工業(yè)4.0”進(jìn)程的不斷深化以及“中國(guó)制造2025”戰(zhàn)略的全面部署，世界制造業(yè)的發(fā)展將日漸趨于數(shù)字化、網(wǎng)絡(luò)化、智能化[1]。多自由度工業(yè)機(jī)械手臂在工業(yè)自動(dòng)化領(lǐng)域中扮演著一個(gè)不可或缺的重要角色。在切割、焊接、噴涂、裝配、碼垛等多個(gè)工業(yè)領(lǐng)域有著非常廣泛的應(yīng)用，有效地降低了生產(chǎn)過(guò)程中的勞動(dòng)力成本，提高了社會(huì)生產(chǎn)力，推動(dòng)人類社會(huì)的工業(yè)自動(dòng)化水平的發(fā)展。六自由度機(jī)械臂控制系統(tǒng)研究主要包括運(yùn)動(dòng)學(xué)分析和軌跡插補(bǔ)兩部分核心內(nèi)容，是機(jī)械臂精確、平穩(wěn)、高效工作的重要保障。

傳統(tǒng)的機(jī)械臂運(yùn)動(dòng)學(xué)控制系統(tǒng)的開(kāi)發(fā)是基于機(jī)械臂物理實(shí)體設(shè)計(jì)過(guò)程，缺乏計(jì)算機(jī)軟件的仿真模塊，需要通過(guò)花費(fèi)更多的時(shí)間力氣調(diào)試機(jī)械臂，而且最終結(jié)果往往也不夠準(zhǔn)確[2]。當(dāng)前隨著計(jì)算機(jī)技術(shù)的飛速發(fā)展，各種仿真技術(shù)也被廣泛應(yīng)用到機(jī)械臂的控制系統(tǒng)研究當(dāng)中，避免了直接在機(jī)械臂上調(diào)試，通過(guò)構(gòu)建精度較高的機(jī)械臂模型，在仿真平臺(tái)上模擬實(shí)現(xiàn)機(jī)械臂的實(shí)際運(yùn)動(dòng)情況，最后再通過(guò)對(duì)機(jī)械臂實(shí)體的調(diào)試，設(shè)計(jì)出更加精準(zhǔn)、高效的機(jī)械臂控制系統(tǒng)?；诖耍疚囊藻X江RH6型的六自度串聯(lián)工業(yè)機(jī)器人為研究對(duì)象，通過(guò)對(duì)機(jī)械臂的運(yùn)動(dòng)學(xué)分析及軌跡規(guī)劃的分析研究，在VS2017軟件開(kāi)發(fā)平臺(tái)上，開(kāi)發(fā)模擬機(jī)械臂運(yùn)動(dòng)過(guò)程的仿真軟件。

1 運(yùn)動(dòng)學(xué)建模與求解

機(jī)械臂的運(yùn)動(dòng)分析采用的是由Denavit和Hartenberg在1955年提出的D-H參數(shù)法[3]。在機(jī)械臂的每個(gè)連桿上固連一個(gè)坐標(biāo)系，以此來(lái)描述連桿在空間中的位置狀態(tài)，通過(guò)齊次變換矩陣來(lái)描述各個(gè)坐標(biāo)系之間的相對(duì)關(guān)系，從而來(lái)表示各連桿之間的相對(duì)位姿，在根據(jù)各連桿之間的位姿關(guān)系建立機(jī)械臂的運(yùn)動(dòng)學(xué)方程，求取機(jī)械臂的正逆解。

1.1 D-H模型構(gòu)建

本文研究的錢江RH6型機(jī)械臂由6個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)串聯(lián)而成。在對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)分析之前需要對(duì)機(jī)械臂進(jìn)行建模，確定每一個(gè)連桿上的坐標(biāo)系。為了方便對(duì)連桿的描述，對(duì)各連桿進(jìn)行標(biāo)號(hào)，從固定基座開(kāi)始為0號(hào)連桿，第一個(gè)可動(dòng)連桿為1號(hào)連桿，直到機(jī)械臂的最末端連桿為6號(hào)連桿，連桿編號(hào)完畢開(kāi)始確定連桿坐標(biāo)系，連桿坐標(biāo)系的建立需要遵循以下原則：1）按照笛卡爾坐標(biāo)系的規(guī)則自由設(shè)定0號(hào)連桿的坐標(biāo)系及其坐標(biāo)原點(diǎn)位置；2）機(jī)械臂各關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)，其連桿軸線由右手定則確定；3）將各連桿的上一個(gè)連桿的關(guān)節(jié)軸定義為該連桿坐標(biāo)系的Z軸，Z軸方向可自由選定，通常平行的Z軸的方向相同；4）將相鄰兩連桿i軸和i+1軸的軸線的公法線設(shè)置為i軸連桿坐標(biāo)系的X軸，X軸方向由i指向i+1，i軸和i+1軸的交點(diǎn)或公垂線與i軸的交點(diǎn)作為連桿坐標(biāo)系的原點(diǎn)；5）各連桿坐標(biāo)系的Y軸由右手定則確定。

機(jī)械臂的連桿坐標(biāo)系建立完畢之后，需要確定各連桿的參數(shù)。連桿參數(shù)主要包括連桿夾角θi、連桿扭角αi、連桿長(zhǎng)度ai、連桿偏距di。根據(jù)實(shí)驗(yàn)室機(jī)械臂的原始尺寸可以得到機(jī)械臂的D-H參數(shù)表，如表1所示。

表1 錢江RH6型機(jī)械臂連桿參數(shù)表

建立機(jī)械臂的D-H模型如圖1所示。

圖1 錢江RH6型機(jī)械臂D-H模型

根據(jù)所定義的連桿坐標(biāo)以及連桿參數(shù)，能夠推導(dǎo)出相鄰兩關(guān)節(jié)坐標(biāo)的矩陣變換關(guān)系為

1.2 正逆運(yùn)動(dòng)學(xué)分析

機(jī)械臂的正逆運(yùn)動(dòng)學(xué)分析主要包括正運(yùn)動(dòng)分析和逆運(yùn)動(dòng)學(xué)分析。正運(yùn)動(dòng)學(xué)分析是根據(jù)已知的各關(guān)節(jié)旋轉(zhuǎn)角度來(lái)獲取機(jī)械臂的末端位姿。逆運(yùn)動(dòng)學(xué)是根據(jù)機(jī)械臂的期望末端位姿反求各關(guān)節(jié)角的大小[4]。正運(yùn)動(dòng)學(xué)求解能夠得到機(jī)械臂末端位姿的唯一解，而逆運(yùn)動(dòng)學(xué)求解既有可能得到多組封閉解，也有可能出現(xiàn)沒(méi)有封閉解的情況。在應(yīng)用過(guò)程中需要結(jié)合機(jī)械臂的實(shí)際應(yīng)用場(chǎng)景及各關(guān)節(jié)轉(zhuǎn)角限制約束來(lái)選取最優(yōu)解。

機(jī)械臂的正運(yùn)動(dòng)學(xué)分析首先需要根據(jù)上文推導(dǎo)的相鄰連桿坐標(biāo)系的齊次變換矩陣及各連桿的參數(shù)來(lái)求解各相鄰連桿之間坐標(biāo)系的齊次變換矩陣。

由上述各個(gè)連桿坐標(biāo)之間的齊次變換矩陣,通過(guò)鏈?zhǔn)揭?guī)則可以求解得到機(jī)械臂末端點(diǎn)和基座之間的齊次變換矩陣，即為機(jī)械臂的正運(yùn)動(dòng)唯一解：

式中：ci、si分別表示sin（i）、cos（i）；cij、sij分別表示sin（i+j）、cos（i+j）。

機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析是通過(guò)給定機(jī)械臂的末端位姿來(lái)求解機(jī)械臂6個(gè)關(guān)節(jié)角的轉(zhuǎn)動(dòng)大小。由于本文所研究的錢江RH6型機(jī)械臂滿足Piper原則，即機(jī)械臂的3個(gè)關(guān)節(jié)軸相交于一點(diǎn)存在著封閉解。因此，通過(guò)求解封閉解的方式來(lái)進(jìn)行機(jī)械臂的逆運(yùn)動(dòng)求解分析。機(jī)械臂前3個(gè)關(guān)節(jié)角確定的是末端執(zhí)行器的位置坐標(biāo)，后3個(gè)關(guān)節(jié)角確定了末端執(zhí)行器的姿態(tài)坐標(biāo)，在逆解求解過(guò)程中先求解前3個(gè)位置關(guān)節(jié)角再求解后3個(gè)姿態(tài)關(guān)節(jié)角。

由上面的計(jì)算便可以求解出機(jī)械臂的前3個(gè)關(guān)節(jié)角，由計(jì)算結(jié)果可知θ1存在著兩個(gè)解，對(duì)應(yīng)的θ3存在著4個(gè)解，由于θ2能夠由θ1和θ3唯一確定，所以前3個(gè)關(guān)節(jié)角總共存在4組解。最后，還需要確定出空間位置的最優(yōu)解，根據(jù)機(jī)械臂各關(guān)節(jié)角的限制范圍以及角度變化的原則可以獲取機(jī)械臂空間位置的最優(yōu)解。

根據(jù)所求得的θ1、θ2、θ3進(jìn)一步求解機(jī)械臂的后3個(gè)關(guān)節(jié)角。在式（5）中，將等式兩邊的（1,3）和（3,3）元素對(duì)應(yīng)相等，可以求解得到θ4的大小：

令式（14）兩邊元素（1,1）和（3,1）對(duì)應(yīng)相等求解可得出θ6的大?。?/p>

通過(guò)上述求解，得到了機(jī)械臂的剩下3個(gè)關(guān)節(jié)角的大小。理論上，機(jī)械臂前3個(gè)關(guān)節(jié)角有4組不同的解，由于通過(guò)機(jī)械臂的腕關(guān)節(jié)的翻轉(zhuǎn)可以獲得同樣的姿態(tài)，所以機(jī)械臂后3個(gè)關(guān)節(jié)角有2組不同的解。因此，機(jī)械臂理論上有8組不同的封閉解。根據(jù)機(jī)械臂各關(guān)節(jié)角的限制范圍以及角度變化的原則可以從中選取出機(jī)械臂的最優(yōu)解，來(lái)控制機(jī)械臂達(dá)到指定的目標(biāo)位姿。

2 機(jī)械臂軌跡插補(bǔ)算法

插補(bǔ)算法是數(shù)控加工的指揮者，是機(jī)械臂控制系統(tǒng)的核心內(nèi)容[5]。插補(bǔ)的過(guò)程就是根據(jù)NC代碼離散成多段微線段的過(guò)程，常見(jiàn)的插補(bǔ)算法有空間直線插補(bǔ)算法和空間圓弧插補(bǔ)算法。插補(bǔ)算法按插補(bǔ)方式又有脈沖增量插補(bǔ)和數(shù)據(jù)采樣插補(bǔ)。本文采用的是數(shù)據(jù)采樣插補(bǔ)即時(shí)間分割法，將加工一段曲線的時(shí)間劃分為若干個(gè)相等的插補(bǔ)周期，每經(jīng)過(guò)一個(gè)插補(bǔ)周期就進(jìn)行一次插補(bǔ)運(yùn)算，計(jì)算出該插補(bǔ)周期內(nèi)機(jī)械臂各軸的進(jìn)給量。采用時(shí)間分隔法要求插補(bǔ)算法有較快的運(yùn)算速度，能夠在1 ms的插補(bǔ)周期內(nèi)完成插補(bǔ)運(yùn)算以及其他相關(guān)的運(yùn)算及動(dòng)作。因此，本文采用常見(jiàn)的空間直線和圓弧插補(bǔ)算法來(lái)實(shí)現(xiàn)機(jī)械臂的軌跡插補(bǔ)，插補(bǔ)周期設(shè)置為1 ms，每個(gè)插補(bǔ)周期的插補(bǔ)步長(zhǎng)Δl由插補(bǔ)周期和進(jìn)給確定。

2.1 空間直線插補(bǔ)

直線插補(bǔ)是根據(jù)已知直線軌跡的終點(diǎn)坐標(biāo)，分解出在每個(gè)插補(bǔ)周期各個(gè)軸的運(yùn)動(dòng)分量，如圖2所示。

圖2 直線插補(bǔ)

假設(shè)直線軌跡的終點(diǎn)坐標(biāo)為pe（xe，ye，ze），首先需要確定當(dāng)前插補(bǔ)周期的起點(diǎn)坐標(biāo)。當(dāng)當(dāng)前加工直線軌跡為第一個(gè)程序段，那么將坐標(biāo)原點(diǎn)設(shè)置為起點(diǎn)。否則，我們將上一段加工軌跡的終點(diǎn)設(shè)置為起點(diǎn)，將所確定的起點(diǎn)坐標(biāo)定義為po（xo，yo，zo），可以求出直線軌跡的總長(zhǎng)度l：

當(dāng)直線軌跡插補(bǔ)到最后一個(gè)插補(bǔ)周期時(shí)，剩下的直線長(zhǎng)度llast≤Δl時(shí)，將最后一個(gè)插補(bǔ)周期的步長(zhǎng)設(shè)置為Δlδ=llast，再分別投影到各個(gè)坐標(biāo)軸上，可以得到各軸的進(jìn)給量。最后直到直線余量llast=0時(shí)，完成直線插補(bǔ)過(guò)程。

2.2 空間圓弧插補(bǔ)

空間圓弧插補(bǔ)是將NC代碼所確定的圓弧軌跡離散為多段微線段。在空間坐標(biāo)系中，確定一段圓弧軌跡需要3個(gè)點(diǎn)，在NC代碼中給出的是圓弧的起點(diǎn)坐標(biāo)、終點(diǎn)坐標(biāo)及圓弧軌跡的圓心坐標(biāo)。假設(shè)圓弧軌跡的終點(diǎn)坐標(biāo)為pe（xe，ye，ze），圓心坐標(biāo)為po（xo，yo，zo），首先確定圓弧軌跡的起點(diǎn)坐標(biāo)，若當(dāng)前加工直線軌跡為第一個(gè)程序段，那么將坐標(biāo)原點(diǎn)設(shè)置為起點(diǎn)。否則，我們將上一段加工軌跡的終點(diǎn)設(shè)置為起點(diǎn)，將所確定的起點(diǎn)坐標(biāo)定義為ps（xs，ys，zs）。

根據(jù)已知的條件可以求得以下內(nèi)容：

當(dāng)剩余圓心角θδ小于步距角Δθ時(shí)，圓弧插補(bǔ)進(jìn)入到最后一個(gè)插補(bǔ)周期，此時(shí)令步距角等于剩余圓心角，計(jì)算出各軸進(jìn)給量。當(dāng)θδ=0時(shí)，圓弧插補(bǔ)結(jié)束。

六自由度機(jī)械臂不僅僅是計(jì)算機(jī)械臂空間位置的插補(bǔ)，還需要考慮機(jī)械臂空間姿態(tài)的插補(bǔ)。在本文采用的是均勻變化的方式進(jìn)行姿態(tài)插補(bǔ)，假設(shè)起點(diǎn)姿態(tài)為ps（αs，βs，γs），終點(diǎn)姿態(tài)為pe（αe，βe，γe），那么可以得到每個(gè)插補(bǔ)點(diǎn)pi的姿態(tài)公式為：

根據(jù)上面的直線與圓弧的插補(bǔ)算法，可以求得各個(gè)插補(bǔ)周期中插補(bǔ)點(diǎn)的空間位姿pi（xi，yi，zi，αi，βi，γi），通過(guò)逆運(yùn)動(dòng)學(xué)分析，將笛卡爾坐標(biāo)系下插補(bǔ)點(diǎn)的空間位姿求逆解得到關(guān)節(jié)坐標(biāo)系下各軸的轉(zhuǎn)動(dòng)角度pi′（θi1，θi2，θi3，θi4，θi5，θi6）,通過(guò)關(guān)節(jié)坐標(biāo)系下各軸的角度來(lái)模擬仿真機(jī)械臂的運(yùn)動(dòng)過(guò)程。

3 OpenGL仿真軟件開(kāi)發(fā)

在機(jī)械臂的控制系統(tǒng)研究過(guò)程中，仿真環(huán)節(jié)是必不可少的一環(huán)，通過(guò)對(duì)機(jī)械臂的運(yùn)動(dòng)過(guò)程的仿真能夠模擬出機(jī)械臂末端執(zhí)行器的實(shí)際運(yùn)行軌跡，驗(yàn)證運(yùn)動(dòng)學(xué)算法及插補(bǔ)算法的準(zhǔn)確性。本文采用的是基于MFC窗口框架下的OpenGL來(lái)進(jìn)行機(jī)械臂的實(shí)時(shí)仿真。OpenGL是一種純軟件的圖形庫(kù)函數(shù)，不受硬件條件和不同操作系統(tǒng)的限制，支持多種語(yǔ)言開(kāi)發(fā)，能夠?yàn)閳D形軟件開(kāi)發(fā)提供非常豐富的圖形API接口函數(shù)。本文的OpenGL仿真軟件開(kāi)發(fā)的主要流程包括三維建模和仿真實(shí)現(xiàn)兩部分。在調(diào)用OpenGL庫(kù)函數(shù)前，需要在基于MFC框架中配置OpenGL的開(kāi)發(fā)環(huán)境。在VS2017的軟件開(kāi)發(fā)平臺(tái)中已經(jīng)集成了OpenGL32.lib庫(kù)函數(shù)，再通過(guò)導(dǎo)入glut32.lib即可完成OpenGL開(kāi)發(fā)環(huán)境的配置。

3.1 三維建模

三維建模主要包括機(jī)械臂的運(yùn)動(dòng)軌跡建模和機(jī)械臂的實(shí)體建模兩部分內(nèi)容。機(jī)械臂的運(yùn)動(dòng)軌跡模型的建立是通過(guò)解析NC代碼獲取加工圖形每一工步的頂點(diǎn)坐標(biāo)，將其存儲(chǔ)到數(shù)組當(dāng)中，根據(jù)G01、G02、G03分別區(qū)分加工軌跡的直線、順圓、逆圓。直線軌跡的繪制直接通過(guò)調(diào)用OpenGL中的直線繪制函數(shù)glBegin（GL_LINES）和glEnd（）。圓弧軌跡的繪制需要對(duì)圓弧進(jìn)行離散，離散結(jié)果是許多微線段，當(dāng)線段數(shù)量足夠多的時(shí)候，軌跡便能夠無(wú)限逼近圓弧，將這些離散出來(lái)的點(diǎn)根據(jù)直線繪制的方式繪制出來(lái)便能夠?qū)崿F(xiàn)圓弧軌跡的建模。圖4是NC代碼馬單環(huán)的軌跡建模效果圖。

圖4 馬單環(huán)軌跡模型仿真

機(jī)械臂實(shí)體建模首先需要通過(guò)三維軟件分別畫(huà)出機(jī)械臂6個(gè)連桿的實(shí)體模型，將其保存為obj格式，obj文件是通過(guò)點(diǎn)的坐標(biāo)來(lái)描述三維模型，在obj文件中主要由關(guān)鍵字及點(diǎn)位坐標(biāo)組成，在通過(guò)obj文件導(dǎo)入三維模型的過(guò)程中需要先解析obj文件，提取不同關(guān)鍵字下的數(shù)據(jù)將它們分別保存到數(shù)組中，然后再通過(guò)調(diào)用OpenGL中的三角形繪制函數(shù)glBegin（GL_TRIANGLES）和glEnd（）來(lái)繪制模型的各個(gè)三角面，最后組合成機(jī)械臂的3D模型。

完成了機(jī)械臂的實(shí)體建模之后還需要實(shí)現(xiàn)其動(dòng)態(tài)仿真過(guò)程，機(jī)械臂的動(dòng)態(tài)仿真過(guò)程其實(shí)質(zhì)就是圖形的幾何變換問(wèn)題，可以通過(guò)調(diào)用OpenGL 庫(kù)函數(shù)中的函數(shù)glTranslatef（x，y，z）來(lái)實(shí)現(xiàn)機(jī)械臂模型在X、Y、Z方向的平移動(dòng)作，調(diào)用函數(shù)glRotatef（Angle_x，1，0，0）、glRotatef（Angle_y，0，1，0）、glRotatef（Angle_z，0，0，1）來(lái)分別實(shí)現(xiàn)機(jī)械臂繞基座標(biāo)系的X、Y、Z軸的轉(zhuǎn)動(dòng)，調(diào)用函數(shù)glScalef（m_xScalse, m_yScalse, m_zScalse）可以實(shí)現(xiàn)機(jī)械臂模型的放大和縮小動(dòng)作。

3.2 仿真實(shí)現(xiàn)

建立了機(jī)械臂的三維模型并實(shí)現(xiàn)了機(jī)械臂的動(dòng)態(tài)仿真的模擬，接下來(lái)便可以進(jìn)一步完成機(jī)械臂的仿真軟件的開(kāi)發(fā)。機(jī)械臂在工業(yè)領(lǐng)域的實(shí)際應(yīng)用中主要有點(diǎn)動(dòng)和聯(lián)動(dòng)兩種運(yùn)動(dòng)方式，對(duì)應(yīng)在仿真軟件中需要分別模擬出機(jī)械臂的點(diǎn)動(dòng)控制和聯(lián)動(dòng)控制。點(diǎn)動(dòng)控制的實(shí)現(xiàn)是通過(guò)給定機(jī)械臂各關(guān)節(jié)需要轉(zhuǎn)動(dòng)的關(guān)節(jié)角，通過(guò)運(yùn)動(dòng)學(xué)求正解算法確定機(jī)械臂末端執(zhí)行器的位姿，同時(shí)在仿真軟件界面上實(shí)時(shí)顯示機(jī)械臂末端執(zhí)行器的位姿。聯(lián)動(dòng)控制是控制機(jī)械臂的六軸聯(lián)動(dòng)到達(dá)指定的目標(biāo)位置，從而實(shí)現(xiàn)目標(biāo)軌跡的運(yùn)動(dòng)。

軟件客戶端指定機(jī)械臂的NC代碼，仿真軟件需要讀取并解析指定的NC代碼，通過(guò)直線和圓弧插補(bǔ)算法將NC代碼離散成許多間隔很小的點(diǎn)，通過(guò)機(jī)械臂逆運(yùn)動(dòng)學(xué)求出各軸的關(guān)節(jié)角，將它們保存到自定義的數(shù)組中實(shí)時(shí)傳遞給OpenGL的旋轉(zhuǎn)函數(shù)，并且在軟件界面實(shí)時(shí)顯示機(jī)械臂末端執(zhí)行器的動(dòng)態(tài)位姿，從而實(shí)現(xiàn)對(duì)機(jī)械臂按照目標(biāo)軌跡的動(dòng)態(tài)仿真。

4 結(jié)語(yǔ)

本文通過(guò)對(duì)錢江RH6型機(jī)械臂的運(yùn)動(dòng)學(xué)分析和插補(bǔ)算法的研究，采用OpenGL模擬仿真了機(jī)械臂在實(shí)際加工過(guò)程中的運(yùn)動(dòng)情況，開(kāi)發(fā)了一套基于MFC的六自由度機(jī)械臂仿真軟件。軟件能夠?qū)崿F(xiàn)機(jī)械臂的實(shí)時(shí)點(diǎn)動(dòng)控制和聯(lián)動(dòng)控制兩種不同的運(yùn)動(dòng)模式，實(shí)時(shí)跟蹤機(jī)械臂末端執(zhí)行器的運(yùn)動(dòng)軌跡及各軸的旋轉(zhuǎn)角度，能夠較好地實(shí)現(xiàn)機(jī)械臂動(dòng)作過(guò)程的模擬仿真，對(duì)后期在機(jī)械臂上的實(shí)操作業(yè)有非常重大的指導(dǎo)意義。