黨浩明, 周亞麗, 張奇志
(北京信息科技大學(xué) 自動化學(xué)院,北京 100192)
機(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ī)械臂的實用性。
本文以實驗室自主研發(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]
由機(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)
對機(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組完整解析解。
(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)
已知關(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ù)組解。
為驗證正逆運(yùn)動學(xué)算法的有效性,在Windows系統(tǒng)下基于VC++和OpenGL對機(jī)械臂進(jìn)行了可視化仿真。仿真的硬件環(huán)境為:Intel Core i5-8250U處理器,主頻1.6 GHz,8 GB內(nèi)存。
給定關(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)
給定末端期望位姿矩陣,對得到的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é)平均計算時間
機(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]。
針對自主研發(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ī)械臂。