胡宇航 張尚盈② 張曉艷

（①武漢工程大學(xué)機(jī)電工程學(xué)院，湖北 武漢 430205；②武漢譽(yù)瓊科技有限公司，湖北 武漢 430205）

近些年機(jī)械臂廣泛應(yīng)用于多個(gè)生產(chǎn)領(lǐng)域，這是因?yàn)樗褂梅奖?、動作靈敏，并且能適應(yīng)各種復(fù)雜多變的工作環(huán)境，能有效地減少勞動力、提高生產(chǎn)效率。在工業(yè)拆卸及大物件搬運(yùn)等起重比要求高的環(huán)境中，大多使用液壓機(jī)械臂。

為了提高液壓機(jī)械臂的結(jié)構(gòu)性能、作業(yè)精度等，其運(yùn)動學(xué)的研究尤為重要。對于機(jī)械臂的建模及正逆運(yùn)動學(xué)的研究方法選擇上，不論是D-H 建模還是其他的建模方式；數(shù)值解析法還是幾何法，或是神經(jīng)網(wǎng)絡(luò)、遺傳算法求逆解，都各有利弊。大部分專家學(xué)者在進(jìn)行對機(jī)械臂運(yùn)動學(xué)的研究中都采取了不同的方法。對機(jī)械臂建模而言，郭發(fā)勇等[1]使用一種實(shí)體建模的方式（CFDH）使機(jī)器人建模更直觀準(zhǔn)確，但建模步驟會繁瑣一些；Li F T 等[2]采用改進(jìn)的D-H 法和幾何映射算法對液壓機(jī)械臂建模。對運(yùn)動學(xué)分析而言，郭志強(qiáng)等[3]和趙修琪等[4]采用矩陣逆乘的方式求逆解，此過程易懂明了，但出現(xiàn)了漏解現(xiàn)象；張普行等[5]用空間幾何求反解，避免了大量矩陣乘和求矩陣逆；Peng J Q 等[6]對6 自由度機(jī)械臂劃分成兩個(gè)3 自由度，用解析法對其求逆解；黨浩明等[7]用中間變量替代齊次矩陣連乘，有效縮短逆解運(yùn)算時(shí)間；Zarrin A 等[8]提出一種逆運(yùn)動學(xué)的優(yōu)化幾何解法，在笛卡爾坐標(biāo)系下增加2 個(gè)角度輸入，增加機(jī)械臂工作空間，優(yōu)化其效率，逆運(yùn)動學(xué)求解大大降低了定位誤差；Duka A V 等[9]采用基于神經(jīng)網(wǎng)絡(luò)方式求逆解，并對其軌跡進(jìn)行跟蹤驗(yàn)證，結(jié)果非常穩(wěn)定。

本文以6R 液壓機(jī)械臂為研究對象，針對其正逆運(yùn)動學(xué)進(jìn)行研究和分析。首先，用改進(jìn)的D-H 參數(shù)法構(gòu)建機(jī)械臂運(yùn)動學(xué)模型及正運(yùn)動學(xué)方程，運(yùn)用幾何法和解析法結(jié)合的方式求出運(yùn)動學(xué)逆解，然后調(diào)用機(jī)器人工具箱對正逆解進(jìn)行求解，分析對比計(jì)算值與真實(shí)值。最后，通過關(guān)節(jié)空間軌跡規(guī)劃進(jìn)行仿真，驗(yàn)證此建模及運(yùn)動學(xué)分析的機(jī)械臂運(yùn)動平穩(wěn)有效，為之后運(yùn)動規(guī)劃及實(shí)時(shí)控制打下堅(jiān)實(shí)基礎(chǔ)[10]。

1 機(jī)械臂運(yùn)動學(xué)分析

1.1 機(jī)械臂基本結(jié)構(gòu)及建模

液壓機(jī)械臂是一款6R 關(guān)節(jié)串聯(lián)機(jī)械臂，其三維模型如圖1 所示，它的主要組成包括腰部（回轉(zhuǎn)）、大小臂（俯仰）以及手腕等部分，各相鄰桿件通過轉(zhuǎn)動關(guān)節(jié)連接，在工業(yè)拆卸、搬運(yùn)等領(lǐng)域具有廣泛的應(yīng)用。機(jī)械臂全液壓驅(qū)動，除肩部采用直線液壓缸、腕部旋轉(zhuǎn)采用液壓馬達(dá)，其余關(guān)節(jié)全部采用擺動液壓缸。

圖1 液壓機(jī)械臂三維模型

根據(jù)圖1、2 的機(jī)械臂三維模型及其尺寸參數(shù)，通過改進(jìn)的D-H 參數(shù)法，建立機(jī)械臂坐標(biāo)系，i-1系與i系之間的變換關(guān)系可以用平移和旋轉(zhuǎn)來表示。其建模的具體步驟如下：

圖2 液壓機(jī)械臂平面尺寸

（1）令i-1系繞zi-1軸旋轉(zhuǎn) θi，使xi-1與xi平行，其算子為Rot(zi-1,θi)。

（2）沿著zi-1軸平移di，使xi-1軸與xi軸重合，其算子為Trans(0,0,di)。

（3）沿著xi軸平移ai-1，使二者原點(diǎn)重合，其算子為Trans(ai-1,0,0)。

（4）沿著xi軸旋轉(zhuǎn) αi-1，使得i-1系和i系重合，其算子為Rot(xi,αi-1)。

構(gòu)建的機(jī)械臂坐標(biāo)系簡圖如圖3 所示，構(gòu)建的連桿參數(shù)及關(guān)節(jié)變量如表1 所示，其中 αi-1表示連桿坐標(biāo)系之間的扭轉(zhuǎn)角度，θi表示各關(guān)節(jié)的旋轉(zhuǎn)角度，ai-1表示相鄰關(guān)節(jié)之間的連桿長度，di表示相鄰連桿之間的偏移量。在表中各參數(shù)之中，只有 θi為變值，ai-1、αi-1和di均為定值。

圖3 機(jī)械臂坐標(biāo)系簡圖

表1 液壓機(jī)械臂D-H 參數(shù)

1.2 機(jī)械臂運(yùn)動學(xué)正解

機(jī)械臂的正運(yùn)動學(xué)是通過兩連桿坐標(biāo)系間的變換矩陣相乘得到末端位姿矩陣的過程。由于本文運(yùn)用改進(jìn)的D-H 參數(shù)法，其變換矩陣的通式用式（1）表示。

將表1 中各連桿的D-H 的4 個(gè)參數(shù)代入式（1）中，可依次得到如下各連桿變換矩陣。

將以上各連桿變換矩陣依次相乘得到機(jī)械臂正解，如式（2）所示。

式（2）中：

其中：c234=cos(θ2+θ3+θ4)，s234=sin(θ2+θ3+θ4)，c23=cos(θ2+θ3)。

1.3 機(jī)械臂運(yùn)動學(xué)逆解

機(jī)械臂運(yùn)動學(xué)求逆解是通過已知的末端位姿矩陣來求解各個(gè)關(guān)節(jié)的旋轉(zhuǎn)角度。求逆解的方法一般包括幾何法、解析法等。幾何法一般是將D-H 參數(shù)通過空間幾何轉(zhuǎn)換到平面上來求解；而解析法是通過變換矩陣得到的三角函數(shù)方程來求解。本文則通過幾何法和解析法結(jié)合的方式來求解機(jī)械臂運(yùn)動學(xué)逆解，這樣可以大大減少其計(jì)算量。

（1）計(jì)算θ1,θ5,θ6

由于關(guān)節(jié)2～4 是俯仰關(guān)節(jié)，通過俯視角度觀其始終在一條直線上。關(guān)節(jié)5 為偏擺關(guān)節(jié)，關(guān)節(jié)6 為回轉(zhuǎn)關(guān)節(jié)連接在關(guān)節(jié)5 上，可知關(guān)節(jié)6 相對于關(guān)節(jié)5 的相對位姿是固定的。當(dāng)給定末端位姿，可通過齊次變換得到關(guān)節(jié)5 相對于基座下的位姿，再通過圖4 就能確定 θ1的表達(dá)式。

圖4 關(guān)節(jié)5 在基座坐標(biāo)系下的投影

通過式（3）可得

通過式（4）可得

或

（2）計(jì)算θ2,θ3,θ4

再根據(jù)（1,4）和（3,4）相等后移項(xiàng)得

令X1和X2分別與式（12）右部分相等得

從而通過（13）求解θ3

根據(jù)式（12）～（14）可求出θ2

再根據(jù)式（13）～（15）可求出θ4

通過以上幾何法和解析法結(jié)合的方式已求出此液壓機(jī)械臂全部關(guān)節(jié)角，從以上各式可看出存在多解的情況，經(jīng)過組合可生成8 組逆解，具體的求解流程如圖5 所示。大部分的機(jī)械臂都會存在多個(gè)逆解，會讓機(jī)械臂的末端位姿有多種選擇。但是在實(shí)際運(yùn)動過程中，要充分考慮到機(jī)械臂參數(shù)的約束，會有部分逆解在機(jī)械臂可達(dá)空間之外，或是末端位姿不符合操作任務(wù)的要求，所以對求得的逆解需要進(jìn)行驗(yàn)證。

圖5 運(yùn)動學(xué)求逆解流程圖

2 機(jī)械臂運(yùn)動學(xué)仿真

2.1 建立運(yùn)動學(xué)模型

在MATLAB 軟件中，使用機(jī)器人工具箱Robotics Toolbox 對機(jī)械臂進(jìn)行運(yùn)動學(xué)建模，它包含機(jī)器人所需要的多種運(yùn)動學(xué)函數(shù)。根據(jù)表1 所給出的數(shù)據(jù)，運(yùn)用機(jī)器人工具箱對機(jī)械臂建模，得到的初始模型如圖6 所示。

圖6 機(jī)械臂初始姿態(tài)

2.2 機(jī)械臂正運(yùn)動學(xué)驗(yàn)證

對機(jī)械臂進(jìn)行正運(yùn)動學(xué)的驗(yàn)證，只需要將一組關(guān)節(jié)角代入式（10），得到的位姿矩陣與MATLAB得到的是否一致，若一樣則正運(yùn)動學(xué)驗(yàn)證正確。

取隨機(jī)一組關(guān)節(jié)角q1=[0 0 0 0 0 0]，代入到運(yùn)動學(xué)正解公式中得到末端姿態(tài)矩陣T1，與機(jī)器人工具箱所得到的末端位姿矩陣（如圖6）一樣，驗(yàn)證了機(jī)械臂正運(yùn)動學(xué)方程與運(yùn)動學(xué)模型的正確性。

2.3 機(jī)械臂逆運(yùn)動學(xué)驗(yàn)證

取q2=[30 60 45 75 45 120]*pi/180代入到機(jī)器人工具箱中進(jìn)行求解，可得到末端位姿矩陣T2

將上述得到的位姿矩陣代入到（1.3）節(jié)中的計(jì)算公式，可計(jì)算出對應(yīng)的運(yùn)動學(xué)逆解。在MATLAB對機(jī)械臂逆解計(jì)算得出結(jié)果如表2 所示。

表2 運(yùn)動學(xué)逆解數(shù)值

根據(jù)表2 的數(shù)據(jù)可知，對于每組末端位姿均可求出8 組逆解。當(dāng)然在實(shí)際應(yīng)用中，對機(jī)械臂的約束不同，機(jī)械臂能到達(dá)的關(guān)節(jié)角度只會是其中的一部分。經(jīng)過數(shù)據(jù)對比，發(fā)現(xiàn)q2的取值與表2 第一組一致，至此驗(yàn)證了建立此種逆運(yùn)動學(xué)的正確性。

2.4 軌跡規(guī)劃及仿真

這里所指的軌跡規(guī)劃方法是指機(jī)械臂在運(yùn)動時(shí)，對其位置、角速度及角加速度進(jìn)行規(guī)劃，通過得到的曲線觀察其具體的運(yùn)動狀態(tài)[11-12]。

選用關(guān)節(jié)空間軌跡規(guī)劃，在空間中選擇任意兩點(diǎn)，在機(jī)器人工具箱中調(diào)用函數(shù)對機(jī)械臂進(jìn)行軌跡規(guī)劃。設(shè)關(guān)節(jié)空間中初始點(diǎn)所對應(yīng)的關(guān)節(jié)轉(zhuǎn)角為q1=[0 0 0 0 00]，末端點(diǎn)所對應(yīng)的關(guān)節(jié)轉(zhuǎn)角為q3=[45 -60 90 60 -36 -30]，得到的機(jī)械臂末端運(yùn)動軌跡如圖7 所示。

圖7 機(jī)械臂末端運(yùn)動軌跡

需要對機(jī)械臂的6 個(gè)關(guān)節(jié)進(jìn)行分析，分別對其位移、角速度及角加速度進(jìn)行仿真。由圖8 可知，機(jī)械臂在運(yùn)行過程中，6 個(gè)關(guān)節(jié)所有關(guān)節(jié)的角位移、角速度、角加速度隨時(shí)間變化曲線能平穩(wěn)運(yùn)行，不存在間斷和突變。由以上分析可知此液壓機(jī)械臂及末端執(zhí)行器可連續(xù)平穩(wěn)的完成作業(yè)任務(wù)。

圖8 液壓機(jī)械臂各關(guān)節(jié)仿真曲線

3 結(jié)語

本文以液壓機(jī)械臂為研究對象，基于改進(jìn)D-H法對其進(jìn)行運(yùn)動學(xué)建模，建立正運(yùn)動學(xué)方程。通過空間幾何法和數(shù)值解析法結(jié)合的方式求出其8 組逆解，在MATLAB 編寫程序驗(yàn)證了正逆運(yùn)動學(xué)的正確性。借助機(jī)器人工具箱，在關(guān)節(jié)空間中進(jìn)行機(jī)械臂運(yùn)動軌跡的仿真，通過觀察各關(guān)節(jié)角位移、角速度和角加速度的平滑曲線，驗(yàn)證了機(jī)械臂參數(shù)的合理性。