七軸機械臂避奇異軌跡規劃

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

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

0 引言

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

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

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

1 機械臂運動學分析

1.1 運動學建模

以思靈機器人公司的七軸Diana7機械臂為研究對象,機械臂模型如圖1所示,各關節均為旋轉關節。機械臂的改進D-H(modified Denavit-Hartenberg)參數見表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參數Table 1 Modified D-H parameters

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

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

(1)

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

(2)

(3)

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

首先根據表1的參數建立轉換矩陣:

(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+,得到七軸機械臂通過偽逆方法求得的關節速度為

(14)

1.2 阻尼最小二乘法

(15)

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

(16)

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

(17)

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

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

(18)

1.3 零空間自運動

(19)

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

(20)

(21)

(22)

(23)

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

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

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

(24)

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

(25)

2 運動學算法實現流程

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

(26)

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

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

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

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

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

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

圖4 運動學算法實現流程Fig.4 Implementation process of kinematic algorithm

3 仿真及實驗

3.1 機械臂可操作度

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

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

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

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

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

P1(0.076 915,0.374 492,1.149 312)

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

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

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

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

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

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

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

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

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

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

3.3 自運動的驗證

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

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

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)計算關節速度;未添加自運動的實驗按照式(18)計算關節速度。

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

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

4 結束語

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