六自由度串聯機械臂建模與運動學分析

黨浩明， 周亞麗， 張奇志

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

0 引 言

機械臂是由一系列通過關節連接的連桿所組成的運動鏈。1955年，Denavit等[1]用4個參數來描述機械臂連桿坐標系，并建立了連桿坐標系間的通用變換，成為了機械臂建模和運動學描述的標準方法。

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

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

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

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

1 機械臂建模

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

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

圖1 串聯機械臂實物圖

圖2 機械臂3D模型和連桿坐標系分布

連桿αi/(°)aidiθi關節范圍/(°)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 正向運動學求解

由機械臂DH參數可以得到連桿坐標系{i}相對于坐標系{i-1}的變換矩陣：

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

(1)

將表1中的各連桿參數代入可得：

(2)

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

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

(3)

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

(4)

式中：[noa]和p分別代表末端工具坐標系相對世界坐標系的旋轉矩陣和原點的位置向量。

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

(5)

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

(6)

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

a=n×o

(7)

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

(8)

3 逆向運動學求解

對機械臂進行控制時，通常對逆運動學更感興趣，即已知機械臂的連桿參數，給定末端工具坐標系的期望位置和姿態，找到相應的關節變量，使末端工具坐標系能夠到達期望位姿。

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

3.1 求解后3個關節變量

(9)

進而可以計算得到坐標系{0}的原點在坐標系{7}下的位置向量：

(10)

式中

(11)

已知坐標系{0}和{2}的原點重合，可以建立向量7P0ORG關于后3個關節變量θ4,θ5,θ6的方程：

(12)

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

(13)

當θ4≠0時，求得θ4的兩種可能取值：

(14)

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

(15)

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

(16)

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

(17)

3.2 求解前3個關節變量

已知關節角θ4、θ5和θ6，可以得到：

(18)

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

(19)

又因為

(20)

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

c2=nxU421-axU422+oxU423

(21)

當θ2≠0時，求得θ2的兩種可能取值：

(22)

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

(23)

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

(24)

以上是非奇異狀態下機械臂的8組逆運動學解析解。由關節范圍可知，θ2和θ4不可能等于π，則奇異位形有以下3種情況：當θ4=0時，此時只能求出關節變量θ3與θ5的和；當θ2=0時，此時只能求出關節變量θ1與θ3的和；當θ2=0且θ4=0時，此時只能求出關節變量θ1、θ3與θ5的和。當機械臂處于奇異位形時，逆運動學存在無數組解。

4 仿真驗證

為驗證正逆運動學算法的有效性，在Windows系統下基于VC++和OpenGL對機械臂進行了可視化仿真。仿真的硬件環境為：Intel Core i5-8250U處理器，主頻1.6 GHz，8 GB內存。

4.1 正向運動學仿真

給定關節向量的初始位形qv=[0° -90° 0° 0° 0° 0°]T，此時機械臂應處于垂直向下的狀態。給定關節向量的水平位形qh=[0° 0° 0° 0° 0° 0°]T，此時機械臂應處于水平伸展狀態。當關節向量等于qv時，根據正運動學求解算法可以得到末端位姿：

當關節向量等于qh時，根據正運動學求解算法可以得到末端位姿：

機械臂仿真結果如圖3所示，正運動學結果符合機械臂實際運動規律，驗證了正向運動學算法的有效性。

(a) 豎直狀態

(b) 水平狀態

4.2 逆向運動學解仿真

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

在關節范圍內給定任意一個的關節向量，例如q0=[30° 40° 50° 60° 70° 80°]T，根據正向運動學算法可以得到期望位姿：

將變換矩陣應用于逆向運動學算法可以得到8組逆運動學解如表2所示，結果保留2位小數。

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

表2 8組逆運動學解 (°)

表3 8組逆運動學解的位姿誤差

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

圖4 運動學平均計算時間

4.3 工作空間仿真

機械臂的工作空間指操作空間中末端執行器能夠到達的點的集合。工作空間是機械臂結構設計的一個重要指標，分析其特點對研究機械臂的性能及障礙物回避等有著實際意義。

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

由表1確定的關節范圍內，對每個關節變量取N=10×104個隨機值，組成N個關節向量。對每個關節向量運用正運動學算法，可以得到由腕關節與末端執行器在操作空間中的位置所形成的點云，分別如圖5與圖6所示。

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

圖5 腕關節工作空間點云圖

圖6 末端執行器工作空間點云圖

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

5 結 語

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