黨浩明， 周亞麗， 張奇志

(北京信息科技大學(xué) 自動化學(xué)院，北京 100192)

0 引 言

機(jī)械臂是由一系列通過關(guān)節(jié)連接的連桿所組成的運(yùn)動鏈。1955年，Denavit等[1]用4個參數(shù)來描述機(jī)械臂連桿坐標(biāo)系，并建立了連桿坐標(biāo)系間的通用變換，成為了機(jī)械臂建模和運(yùn)動學(xué)描述的標(biāo)準(zhǔn)方法。

機(jī)械臂運(yùn)動學(xué)研究各關(guān)節(jié)運(yùn)動與機(jī)械臂末端執(zhí)行器位姿之間的關(guān)系，主要包括正向運(yùn)動學(xué)和逆向運(yùn)動學(xué)。對于給定的一組關(guān)節(jié)角，其正向運(yùn)動學(xué)解總是存在且唯一；而逆向運(yùn)動學(xué)的求解則較為困難。正向運(yùn)動學(xué)可以理解為從關(guān)節(jié)空間到操作空間的映射，關(guān)節(jié)空間中任一位形，在操作空間中可以找到唯一的解與之對應(yīng)。而對于逆運(yùn)動學(xué)，操作空間中任一位形，在關(guān)節(jié)空間中可能存在無解、唯一解或多解的情況。1961年，Pieper[2]證明了帶有3個相鄰關(guān)節(jié)軸交于一點(diǎn)的機(jī)械臂存在逆運(yùn)動學(xué)解析解，為機(jī)械臂結(jié)構(gòu)設(shè)計提供了理論依據(jù)，目前大多數(shù)工業(yè)機(jī)器人都具有此種構(gòu)型。

針對逆運(yùn)動學(xué)的求解，許多研究人員提出了不同的方法，主要包括封閉解法、數(shù)值解法和智能算法。其中，數(shù)值解法和智能算法在求解過程中不依賴于具體的機(jī)械臂結(jié)構(gòu)。但數(shù)值方法[3-4]主要基于雅可比矩陣的迭代優(yōu)化，在奇異點(diǎn)附近可能存在不穩(wěn)定，不能保證算法一定收斂，且對于一組初始值只能得到一組逆解。智能算法如遺傳算法[5-6]、粒子群算法[7-8]和神經(jīng)網(wǎng)絡(luò)[9-10]等，存在計算量大、局部收斂和涉及大量參數(shù)選擇等問題。

封閉解法主要有幾何法與代數(shù)法。其中，幾何方法[11-12]適用于結(jié)構(gòu)簡單且自由度數(shù)較少的機(jī)械臂，對3個以上自由度的機(jī)械臂應(yīng)用幾何法求解逆運(yùn)動學(xué)通常比較困難。代數(shù)方法依賴于特定的機(jī)械臂結(jié)構(gòu)，但對于滿足Pieper準(zhǔn)則的機(jī)械臂構(gòu)型，能夠得到關(guān)節(jié)變量的符號表達(dá)式，故計算速度快且結(jié)果更可靠。1981年，Paul[13]提出反變換法解耦關(guān)節(jié)變量，得到了帶有球形腕的斯坦福機(jī)械臂封閉解，對逆運(yùn)動學(xué)代數(shù)求解具有指導(dǎo)作用。姜宏超等[14]采用矩陣逆乘方法得到六自由度機(jī)械臂的完整解析解。李憲華等[15]針對后3個關(guān)節(jié)軸交于一點(diǎn)的模塊化機(jī)械臂，運(yùn)用幾何法和反變換法分組求得逆運(yùn)動學(xué)解析解。崔玉潔等[16]對前3個關(guān)節(jié)軸交于一點(diǎn)的六自由度臂采用變量分離法得到逆運(yùn)動學(xué)封閉解。

本文針對前三軸交于一點(diǎn)的六自由度機(jī)械臂，首先利用標(biāo)準(zhǔn)DH法建立機(jī)械臂的數(shù)學(xué)模型，進(jìn)而得到正向運(yùn)動學(xué)解。根據(jù)機(jī)械臂的構(gòu)型特點(diǎn)，采用逆向解耦方法，利用末端位置求出機(jī)械臂后3個關(guān)節(jié)變量的值，利用末端朝向和已知關(guān)節(jié)角求出前3個關(guān)節(jié)變量的值，得到機(jī)械臂在非奇異狀態(tài)下的8組解析解。并通過在齊次矩陣相乘過程中設(shè)置中間變量以及利用旋轉(zhuǎn)矩陣的正交性，得到正逆運(yùn)動學(xué)的快速求解算法。最后，通過對正逆運(yùn)動學(xué)算法的仿真和機(jī)械臂可達(dá)工作空間的分析，證明了所提運(yùn)動學(xué)算法的有效性和此種構(gòu)型機(jī)械臂的實用性。

1 機(jī)械臂建模

本文以實驗室自主研發(fā)的六自由度串聯(lián)機(jī)械臂為研究對象，機(jī)械臂主要由6個模塊化旋轉(zhuǎn)關(guān)節(jié)、中間連接件和末端夾持器組成，相鄰關(guān)節(jié)的關(guān)節(jié)軸相互垂直，如圖1所示。

采用標(biāo)準(zhǔn)DH法建立連桿坐標(biāo)系。為簡化機(jī)械臂數(shù)學(xué)模型，將世界坐標(biāo)系{B}固定在兩肩連線的中點(diǎn)上，坐標(biāo)系{0}和坐標(biāo)系{1}、{2}固定在肩部，坐標(biāo)系{3}、{4}固定在肘部，坐標(biāo)系{5}、{6}固定在腕部，工具坐標(biāo)系{7}固定在末端夾持器的中心。機(jī)械臂3D模型和連桿坐標(biāo)系分布如圖2所示，DH參數(shù)如表1所示。其中：d0=0.262 m為機(jī)械臂肩部長度；d3=0.244 5 m；d5=0.274 m分別為機(jī)械臂上臂和前臂長度；d7=0.331 5 m為末端夾持器長度。

圖1 串聯(lián)機(jī)械臂實物圖

圖2 機(jī)械臂3D模型和連桿坐標(biāo)系分布

連桿αi/(°)aidiθi關(guān)節(jié)范圍/(°)1 9000θ1(-180,180]2-9000θ2[-90,90]3 900-d3θ3(-180,180]4-9000θ4[-120,120]5 900-d5θ5(-180,180]6000θ6[-120,120]

2 正向運(yùn)動學(xué)求解

由機(jī)械臂DH參數(shù)可以得到連桿坐標(biāo)系{i}相對于坐標(biāo)系{i-1}的變換矩陣：

trans(xi,ai)rot(xi,αi)=

(1)

將表1中的各連桿參數(shù)代入可得：

(2)

式中：ci代表cosθi；si代表sinθi。

由變換矩陣定義可知，坐標(biāo)系{0}相對于世界坐標(biāo)系{B}的變換矩陣和工具坐標(biāo)系{7}相對于坐標(biāo)系{6}的變換矩陣分別為：

(3)

通過變換矩陣連乘可以得到末端工具坐標(biāo)系{7}相對于世界坐標(biāo)系{B}的變換矩陣：

(4)

式中：[noa]和p分別代表末端工具坐標(biāo)系相對世界坐標(biāo)系的旋轉(zhuǎn)矩陣和原點(diǎn)的位置向量。

注意到在矩陣相乘過程中，存在很多重復(fù)的乘積項，且計算工具坐標(biāo)系{7}相對世界坐標(biāo)系{B}位姿的過程中，不需要計算中間矩陣的每個元素[17]。因此，這里定義中間乘積矩陣：

(5)

用中間變量Uimn代替矩陣Ui中第m行第n列的乘積項或多項式，由式(4)和(5)可以得到末端執(zhí)行器期望位姿：

(6)

式中：?代表不需要直接計算出的項。旋轉(zhuǎn)矩陣的第1列可以由第2和第3列的叉積得到：

a=n×o

(7)

計算工具坐標(biāo)系位姿的過程中所需的中間變量如下：

(8)

3 逆向運(yùn)動學(xué)求解

對機(jī)械臂進(jìn)行控制時，通常對逆運(yùn)動學(xué)更感興趣，即已知機(jī)械臂的連桿參數(shù)，給定末端工具坐標(biāo)系的期望位置和姿態(tài)，找到相應(yīng)的關(guān)節(jié)變量，使末端工具坐標(biāo)系能夠到達(dá)期望位姿。

本文所研究的機(jī)械臂前3個關(guān)節(jié)軸交于一點(diǎn)，滿足Pieper準(zhǔn)則，因此逆運(yùn)動學(xué)解析解存在。逆運(yùn)動學(xué)求解思路如下：首先，將關(guān)節(jié)變量逆向解耦為后3個關(guān)節(jié)變量和前3個關(guān)節(jié)變量。然后，利用末端位置求出后3個關(guān)節(jié)變量的值，利用末端朝向和已知關(guān)節(jié)角求出前3個關(guān)節(jié)變量的值，即可得到機(jī)械臂的8組完整解析解。

3.1 求解后3個關(guān)節(jié)變量

(9)

進(jìn)而可以計算得到坐標(biāo)系{0}的原點(diǎn)在坐標(biāo)系{7}下的位置向量：

(10)

式中

(11)

已知坐標(biāo)系{0}和{2}的原點(diǎn)重合，可以建立向量7P0ORG關(guān)于后3個關(guān)節(jié)變量θ4,θ5,θ6的方程：

(12)

分別對式(10)和(12)的右邊取平方和，可以得到：

(13)

當(dāng)θ4≠0時，求得θ4的兩種可能取值：

(14)

根據(jù)式(10)和(12)右邊的第2個元素相等可以得到：

(15)

求得θ5的兩種可能取值：

(16)

根據(jù)式(10)和(12)右邊的第1個和第3個元素相等可得：

(17)

3.2 求解前3個關(guān)節(jié)變量

已知關(guān)節(jié)角θ4、θ5和θ6，可以得到：

(18)

式中，Uimn可以通過矩陣相乘代入關(guān)節(jié)變量θ4、θ5和θ6求得，需要計算的中間變量如下：

(19)

又因為

(20)

令式(18)和(20)右邊的元素(3, 2)相等，可得：

c2=nxU421-axU422+oxU423

(21)

當(dāng)θ2≠0時，求得θ2的兩種可能取值：

(22)

令式(18)和(20)右邊的元素(2, 2)和元素(1, 2)分別相等，可得：

(23)

令式(18)和(20)右邊的元素(3, 3)和元素(3, 1)分別相等，可得:

(24)

以上是非奇異狀態(tài)下機(jī)械臂的8組逆運(yùn)動學(xué)解析解。由關(guān)節(jié)范圍可知，θ2和θ4不可能等于π，則奇異位形有以下3種情況：當(dāng)θ4=0時，此時只能求出關(guān)節(jié)變量θ3與θ5的和；當(dāng)θ2=0時，此時只能求出關(guān)節(jié)變量θ1與θ3的和；當(dāng)θ2=0且θ4=0時，此時只能求出關(guān)節(jié)變量θ1、θ3與θ5的和。當(dāng)機(jī)械臂處于奇異位形時，逆運(yùn)動學(xué)存在無數(shù)組解。

4 仿真驗證

為驗證正逆運(yùn)動學(xué)算法的有效性，在Windows系統(tǒng)下基于VC++和OpenGL對機(jī)械臂進(jìn)行了可視化仿真。仿真的硬件環(huán)境為：Intel Core i5-8250U處理器，主頻1.6 GHz，8 GB內(nèi)存。

4.1 正向運(yùn)動學(xué)仿真

給定關(guān)節(jié)向量的初始位形qv=[0° -90° 0° 0° 0° 0°]T，此時機(jī)械臂應(yīng)處于垂直向下的狀態(tài)。給定關(guān)節(jié)向量的水平位形qh=[0° 0° 0° 0° 0° 0°]T，此時機(jī)械臂應(yīng)處于水平伸展?fàn)顟B(tài)。當(dāng)關(guān)節(jié)向量等于qv時，根據(jù)正運(yùn)動學(xué)求解算法可以得到末端位姿：

當(dāng)關(guān)節(jié)向量等于qh時，根據(jù)正運(yùn)動學(xué)求解算法可以得到末端位姿：

機(jī)械臂仿真結(jié)果如圖3所示，正運(yùn)動學(xué)結(jié)果符合機(jī)械臂實際運(yùn)動規(guī)律，驗證了正向運(yùn)動學(xué)算法的有效性。

(a) 豎直狀態(tài)

(b) 水平狀態(tài)

4.2 逆向運(yùn)動學(xué)解仿真

給定末端期望位姿矩陣，對得到的8組逆解分別應(yīng)用正向運(yùn)動學(xué)，得到8個末端執(zhí)行器位姿矩陣，分別計算它們與期望位姿之間的誤差，若誤差滿足控制精度要求，則說明逆運(yùn)動學(xué)算法有效。

在關(guān)節(jié)范圍內(nèi)給定任意一個的關(guān)節(jié)向量，例如q0=[30° 40° 50° 60° 70° 80°]T，根據(jù)正向運(yùn)動學(xué)算法可以得到期望位姿：

將變換矩陣應(yīng)用于逆向運(yùn)動學(xué)算法可以得到8組逆運(yùn)動學(xué)解如表2所示，結(jié)果保留2位小數(shù)。

由仿真結(jié)果可知，逆運(yùn)動學(xué)算法求得的8組解對應(yīng)機(jī)械臂同一末端期望位姿，其位姿誤差主要由舍入誤差引起，能夠滿足機(jī)械臂控制的精度要求，且8組逆解中包含初始關(guān)節(jié)向量q0，驗證了逆運(yùn)動學(xué)算法的有效性和完備性。

表2 8組逆運(yùn)動學(xué)解 (°)

表3 8組逆運(yùn)動學(xué)解的位姿誤差

分別運(yùn)用矩陣連乘方法和設(shè)置中間變量的方法求解正逆運(yùn)動學(xué)，在C++環(huán)境下對比兩種方法的平均計算時間，如圖4所示。由圖中可以看出，通過設(shè)置中間變量的方法，能夠顯著減少算法運(yùn)算量，縮短程序運(yùn)行時間。正向運(yùn)動學(xué)的平均運(yùn)算時間約縮短為原來的44.87%，逆向運(yùn)動學(xué)的平均運(yùn)算時間約縮短為原來的64.00%。

圖4 運(yùn)動學(xué)平均計算時間

4.3 工作空間仿真

機(jī)械臂的工作空間指操作空間中末端執(zhí)行器能夠到達(dá)的點(diǎn)的集合。工作空間是機(jī)械臂結(jié)構(gòu)設(shè)計的一個重要指標(biāo)，分析其特點(diǎn)對研究機(jī)械臂的性能及障礙物回避等有著實際意義。

生成工作空間的方法主要包括解析法和數(shù)值法。其中，數(shù)值法更加簡單且靈活，不依賴于具體的機(jī)械臂結(jié)構(gòu)，能夠得到工作空間的近似邊界曲面。蒙特卡洛法基于隨機(jī)理論，是求解工作空間的常用方法。其基本思想是，所有關(guān)節(jié)變量在取值范圍內(nèi)遍歷取值后，末端執(zhí)行器所處位置的點(diǎn)的集合即構(gòu)成工作空間。

由表1確定的關(guān)節(jié)范圍內(nèi)，對每個關(guān)節(jié)變量取N=10×104個隨機(jī)值，組成N個關(guān)節(jié)向量。對每個關(guān)節(jié)向量運(yùn)用正運(yùn)動學(xué)算法，可以得到由腕關(guān)節(jié)與末端執(zhí)行器在操作空間中的位置所形成的點(diǎn)云，分別如圖5與圖6所示。

(a) 3維工作空間(b) z=0附近,xy截面(c) x=-d0附近,yz截面(d) y=0附近,xz截面

圖5 腕關(guān)節(jié)工作空間點(diǎn)云圖

圖6 末端執(zhí)行器工作空間點(diǎn)云圖

可以看出，腕關(guān)節(jié)與末端執(zhí)行器工作空間近似為空心的橢球體，機(jī)械臂工作空間受連桿長度和關(guān)節(jié)范圍影響較大。其中，腕關(guān)節(jié)工作空間沿x軸的移動范圍約為[-0.780 5 m, -0.011 2 m]，沿y軸和z軸的移動范圍為[-0.518 5 m, -0.518 5 m]。末端執(zhí)行器工作空間沿x軸的移動范圍約為[-1.112 0 m, 0.340 7 m]，沿y軸和z軸的移動范圍為[-0.85 m, -0.85 m]。

5 結(jié) 語

針對自主研發(fā)的六自由度串聯(lián)機(jī)械臂進(jìn)行了DH建模，并根據(jù)機(jī)械臂的構(gòu)型特點(diǎn)，得到其正向運(yùn)動學(xué)解和逆向運(yùn)動學(xué)8組完整解析解。在運(yùn)動學(xué)求解過程中，利用齊次矩陣連乘時存在重復(fù)乘積項的特點(diǎn)，定義中間變量得到了正逆運(yùn)動學(xué)的快速求解算法。最后對所提算法和機(jī)械臂的工作空間進(jìn)行了仿真實驗，驗證了正逆運(yùn)動學(xué)算法的正確性和高效性，該建模和運(yùn)動學(xué)求解算法也可應(yīng)用于具有相似結(jié)構(gòu)的機(jī)械臂。