周嘉文,劉相權,張曉磊,白宇珅

(1.北京信息科技大學 機電工程學院,北京 100192;2.北京思靈機器人科技有限責任公司,北京 100192)

0 引言

目前,七軸機械臂已廣泛應用到各行各業(yè)中,具有成本低、精度高,以及能夠替代危險工作等優(yōu)勢。機械臂在提高人們生活質量和解放生產力方面有著巨大作用[1]。機械臂在進行運動前要進行軌跡規(guī)劃,奇異問題的解決是機械臂軌跡規(guī)劃的前提[2]。當機械臂處在奇異位置時,可能有無數(shù)個運動學逆解[3]、自由度缺失[4],甚至關節(jié)加速度無窮大[5],速度跳變會產生巨大沖擊[6],對機械臂產生不可控的危害,使機械臂無法按照期望軌跡運動。避奇異的同時保證關節(jié)角度不超過關節(jié)極限,從而使機械臂能夠以良好的速度和姿態(tài)進行軌跡運動,具有重要的工程意義。

趙龍澤等[7]提出了一種微分項提取和二次擬合的奇異回避算法,但是未考慮到關節(jié)角的極限。金榮玉等[8]針對空間機械臂的動力學奇異進行了研究。段銳等[9]針對機器人拖動示教中的奇異問題提出了導納控制框架進行避奇異,但是在大阻尼情況下這種方法會造成機械臂靈活性降低。Zhao等[10]通過將機械臂引入到粒子群算法中,得到末端實際位姿。Wong等[11]利用了神經(jīng)網(wǎng)絡算法進行運動規(guī)劃,但神經(jīng)網(wǎng)絡方法的計算成本太高。Teshigawara等[12]則對產生奇異的機械結構提出了解決方法,但是并不適用于高自由度機械臂。

本文應用阻尼最小二乘法避奇異,并采用零空間下的自運動限制關節(jié)角到達關節(jié)極限,并提出一種新的自運動計算方法來計算自運動的關節(jié)速度。以思靈七軸機械臂為例,對上述方法進行了詳細的說明,最后進行了仿真和實驗驗證。

1 機械臂運動學分析

1.1 運動學建模

以思靈機器人公司的七軸Diana7機械臂為研究對象,機械臂模型如圖1所示,各關節(jié)均為旋轉關節(jié)。機械臂的改進D-H(modified Denavit-Hartenberg)參數(shù)見表1。機械臂的改進D-H坐標系如圖2所示。

圖1 機械臂模型Fig.1 Manipulator model

圖2 機械臂D-H坐標系示意圖Fig.2 Schematic diagram of the manipulator D-H coordinate system

表1 改進D-H參數(shù)Table 1 Modified D-H parameters

在笛卡爾空間中,根據(jù)機械臂的正運動學可以得到:

x(t)=f(θ(t))

(1)

式中:x(t)為t時刻下機械臂末端在空間中的位姿,包含位置和角度兩部分;θ(t)為機械臂在t時刻下各個關節(jié)的角度。

(2)

(3)

機械臂的雅可比矩陣J的維度為6×n,其中行數(shù)6對應空間中3個平移和3個旋轉的自由度數(shù)量和,而列數(shù)n與機械臂的n個關節(jié)數(shù)量相對應。六軸機械臂有6個關節(jié),所以雅可比矩陣為6×6的構型,雅可比矩陣為方陣,可以通過式(3)直接求得機械臂的關節(jié)速度。然而對于有7個關節(jié)的七軸機械臂,雅可比矩陣J為6×7的構型,不能直接應用矩陣求逆操作來計算其逆矩陣。所以我們需要求取J的偽逆矩陣來進行雅可比矩陣的求逆計算。

首先根據(jù)表1的參數(shù)建立轉換矩陣:

(4)

(5)

(6)

然后令式(6)每列前三行組成一個列向量:

a1=[a1x,a1y,a1z]T

(7)

b1=[b1x,b1y,b1z]T

(8)

c1=[c1x,c1y,c1z]T

(9)

d1=[d1x,d1y,d1z]T

(10)

最終得到雅可比矩陣第1列:

J1=[(d1×a1)z(d1×b1)z(d1×c1)za1zb1zc1z]T

(11)

式中:(d1×a1)z代表d1和向量a1叉乘后得到的向量中取z分量,(d1×b1)z和(d1×c1)z同理。

按照式(6)到式(11)的這種方法依次得到J2、J3、J4、J5、J6和J7,將它們按照順序把7組列向量組合到一起組成雅可比矩陣:

(12)

最后采用文獻[14]所使用的奇異值分解(singular value decomposition,SVD)方法求取雅可比矩陣的偽逆。SVD奇異值分解方法可以使矩陣J被分解為3個特殊矩陣的乘積,即J=UΛVT。其中,U和V均為正交矩陣;Λ為奇異值組成的對角矩陣。通過這樣的分解,可以得到雅可比矩陣J的偽逆矩陣J+:

J+=(UΛVT)-1=(VT)-1Λ-1U-1=VΛ-1UT

(13)

通過將式(3)中的普通逆矩陣J-1替換為偽逆矩陣J+,得到七軸機械臂通過偽逆方法求得的關節(jié)速度為

(14)

1.2 阻尼最小二乘法

(15)

J*=(J+)T(J+(J+)T+λ2I)-1

(16)

式中:I為單位矩陣;λ為阻尼系數(shù),規(guī)定其具體取值和奇異值的大小有關:

(17)

式中:λmax為設置的最大阻尼系數(shù);σ為最小奇異值;σ0為設置的閾值。

通過將式(14)中的偽逆矩陣J+替換為式(16)的奇異魯棒性逆矩陣J*,最終得到使用最小二乘法進行求解關節(jié)速度的表達式:

(18)

1.3 零空間自運動

(19)

因為不是所有的關節(jié)速度向量都在零空間內,所以假設I-J+J為機械臂的零空間投影矩陣。雅可比的偽逆J+和原矩陣J相乘結果是一個單位矩陣I,即J+J=I,把關節(jié)速度轉換到零空間得到:

(20)

(21)

(22)

(23)

機械臂關節(jié)1的關節(jié)范圍為-3.12～3.12 rad,假設最大關節(jié)速度為1.5 rad/s,依據(jù)式(22)通過在不同階段設置不同的k值,得到的關節(jié)1關節(jié)速度q1大小對應不同關節(jié)角度如圖3所示。

圖3 關節(jié)1關節(jié)角度和速度關系Fig.3 Relationship between joint angle and speed of joint 1

可見,在遠離關節(jié)極限位置時,關節(jié)速度為0;當關節(jié)轉至接近最小關節(jié)極限時,關節(jié)改變速度方向以大于0的速度運動,使關節(jié)角度遠離最小極限位置;當關節(jié)轉至接近最大關節(jié)極限時,關節(jié)改變速度方向以小于0的速度運動,使關節(jié)角度遠離最大關節(jié)極限,可以實現(xiàn)防止關節(jié)到達關節(jié)極限的目的。

(24)

最終結合式(18)、式(21)和式(24)得到應用阻尼最小二乘法和零空間自運動進行求解的關節(jié)速度為

(25)

2 運動學算法實現(xiàn)流程

機械臂的可操作度(manipulability measure)是評估機械臂在給定姿態(tài)下的運動能力的一個指標。依據(jù)文獻[18],在機械臂運動過程中,可以通過判斷機械臂的可操作度值的大小進行奇異判斷。機械臂的可操作度表達式為

(26)

O(θ)值越大,說明機械臂在當前姿態(tài)下的運動能力越強。當可操作度為0時,判斷機械臂當前時刻的位姿處于奇異狀態(tài)。為了提前預判奇異狀態(tài),需要定義一個閾值常量ε(ε>0)。當機械臂可操作度大于ε時,判斷當前機械臂遠離奇異狀態(tài);當可操作度小于等于ε,機械臂接近奇異狀態(tài)。算法具體步驟如下:

1) 根據(jù)任務目標確定機械臂的初始位置和終止位置。

2) 啟動機械臂,通過當前時刻的關節(jié)角度數(shù)據(jù)進行雅可比矩陣的求解,并設置可操作度閾值。

3) 通過式(26)計算運動過程中每個點的可操作度并進行奇異判斷,如果當前姿態(tài)的可操作度O(θ)大于閾值ε,則不處在奇異位置,機械臂采取式(14)中能夠保持運動精度的偽逆法計算關節(jié)速度并進行運動,否則,機械臂將會采取式(25)中阻尼最小二乘法和零空間自運動的計算方法進行移動。

4) 實時判斷運動過程中每個位置的機械臂是否奇異,并持續(xù)控制機械臂進行運動,直到機械臂運動到任務目標位置。

上述整個算法的流程如圖4所示。

圖4 運動學算法實現(xiàn)流程Fig.4 Implementation process of kinematic algorithm

3 仿真及實驗

3.1 機械臂可操作度

在機械臂關節(jié)范圍下,不同的關節(jié)角度能夠使末端在空間中達到不同的點,每個點對應的可操作度Oi(θ)可以根據(jù)式(26)求出。通過比較每個點的可操作度大小,可以識別出最大的可操作度Omax(θ)。隨后,每個點的可操作度Oi(θ)與Omax(θ)相除,從而將原始的可操作度歸一化到0～1的范圍,相應的仿真結果如圖5～8所示。圖中不同顏色代表著可操作度的不同,其中紅色代表可操作度最好,藍色代表可操作度最差,由圖7和圖8明顯可見在最外層和最內層為藍色,代表著機械臂在最大關節(jié)極限和最小關節(jié)極限位置的可操作度較差,藍色區(qū)域范圍大,所以避開極限位置非常重要。

圖5 機械臂可操作度點云圖Fig.5 Manipulability measure point cloud of manipulator

圖6 X-O-Y平面可操作度點云圖Fig.6 Manipulability measure point cloud of X-O-Y plane

圖7 X-O-Z平面可操作度點云圖Fig.7 Manipulability measure point cloud of X-O-Z plane

圖8 Y-O-Z平面可操作度點云圖Fig.8 Manipulability measure point cloud of Y-O-Z plane

3.2 算法實驗驗證

在機械臂第4關節(jié)運動范圍為0～3.05 rad基礎上,假設關節(jié)4靠近最小關節(jié)極限位置,同時設置其他關節(jié)角度適當大小以確保在初始位置P0時機械臂接近奇異狀態(tài),因此假設P0位置處7個關節(jié)角度用弧度表示分別為:0,-0.174 53,-1.221 73,0.087 27,-0.261 80,-2.443 46,-0.698 13。

根據(jù)機械臂正運動學得到機械臂末端坐標系原點的初始位置坐標為

P0(-0.123 085,0.174 492,1.149 312)

假設機械臂末端沿x和y正方向移動0.2 m,z方向保持不動,并保持末端姿態(tài)不變。得到的終止位置的坐標為

P1(0.076 915,0.374 492,1.149 312)

首先采用普通偽逆法求解關節(jié)速度,通過軟件仿真各個關節(jié)角度在1 s內的變化,得到的仿真結果如圖9所示?？梢?大概在0.65 s處多個關節(jié)角度發(fā)生關節(jié)突變,導致關節(jié)1、關節(jié)3和關節(jié)4后續(xù)運動超出了關節(jié)極限。發(fā)生突變前7個關節(jié)角度(弧度)數(shù)據(jù)如下:0.740 79,-0.102 41,-1.976 46,0.289 61,-0.091 14,-2.135 99,-0.487 98。

圖9 偽逆法關節(jié)角度隨時間變化曲線Fig.9 Curve of joint angle changing with time by pseudo inverse method

計算機械臂在此關節(jié)角度下的可操作度O(θ)。首先計算當前位姿下的雅可比矩陣J:

計算當前位姿下的可操作度為

可操作度非常接近0,此時機械臂是處于奇異狀態(tài)的,并且關節(jié)角度突破了關節(jié)極限的限制,因此這種求解方法不適用于機械臂處于奇異并且關節(jié)角度接近關節(jié)極限時的情況。

下面在真實的機械臂上對式(25)進行驗證。采用阻尼最小二乘法的方法進行避奇異,通過實時計算求解每個位置的阻尼系數(shù)λ進行關節(jié)速度求解,并且通過自運動來防止機械臂的關節(jié)到達極限位置。機械臂初始位置和終止位置(圖10)與前面仿真的數(shù)據(jù)保持一致。

圖10 機械臂初始位置和終止位置Fig.10 Initial position and terminal position of the manipulator

得到的關節(jié)角度隨時間變化關系如圖11所示。

圖11 兩種方法結合的關節(jié)角度隨時間變化曲線Fig.11 Curve of joint angle changing with time by combining two methods

由圖11可知,得到的關節(jié)速度沒有發(fā)生突變,同時關節(jié)4也沒有超出最小關節(jié)極限,速度在1 s內也較為平緩,驗證了阻尼最小二乘法和自運動結合的可行性。

3.3 自運動的驗證

3.2節(jié)中采取了阻尼最小二乘法和自運動相結合的方法來求關節(jié)速度。為了驗證所提出式(22)的自運動計算方法的正確性,需排除阻尼最小二乘法對自運動的干擾?？紤]Diana7機械臂的關節(jié)極限,假設關節(jié)1接近極限位置,在此基礎上設置其他關節(jié)使機械臂遠離奇異狀態(tài),消除了阻尼最小二乘法的影響,假設7個關節(jié)初始角度(弧度)為:-3.054 33,-0.174 53,0,1.221 73,-0.261 80,-1.745 33,-0.698 13。

根據(jù)正運動學得到機械臂末端對應的位置坐標為

Pm(-0.467 230,0.010 210,0.956 054)

讓機械臂沿y正向移動0.2 m,因此設置終止位置的坐標設為

Pn(-0.467 230,0.189 790,0.956 054)

通過進行添加和不添加自運動的兩次實驗來驗證自運動正確性。其中,添加自運動的實驗按照式(25)計算關節(jié)速度;未添加自運動的實驗按照式(18)計算關節(jié)速度。

得到關節(jié)1的角度隨時間變化關系如圖12所示,由于機械臂未處于奇異狀態(tài),因此關節(jié)1即使在關節(jié)極限附近,也沒有發(fā)生關節(jié)突變。關節(jié)1的最小關節(jié)極限為-3.12 rad,未添加自運動方法的關節(jié)角度變化如圖中點劃線所示,0.5 s后到達了關節(jié)1的最小關節(jié)極限,機械臂立即停止運動并且示教器報錯。而圖中實線為添加了自運動方法的關節(jié)角度變化情況,關節(jié)角度沒有到達最小關節(jié)極限,從而驗證了所提自運動關節(jié)速度計算方法的正確性。

圖12 關節(jié)1角度隨時間變化關系Fig.12 The relationship between angle and time of joint 1

4 結束語

本文以七軸機械臂為研究對象,針對規(guī)劃軌跡中存在奇異點的問題進行了研究。通過阻尼最小二乘法和零空間自運動計算方法,進行避奇異和避關節(jié)極限的雙重操作,同時提出了新的自運動計算方法。實驗結果表明,針對Diana7七軸機械臂,本文方法能夠有效求解機械臂關節(jié)速度,在路徑點存在機械臂奇異情況下能夠使機械臂關節(jié)遠離關節(jié)極限,有效調節(jié)關節(jié)速度以完成任務。后續(xù)的研究將聚焦到運動精度上,以滿足各種場合下的任務需求。