力矩優(yōu)化的擴展雅可比方法機械手避障研究*

彭 芳，黎 萍，吳澤帆，劉春銘

(電子科技大學中山學院 機電工程學院，廣東 中山 528402)

0 引言

機械手已廣泛應用于自動化生產(chǎn)中，代替人類完成重復繁重的勞動。當機械手放置空間有限時，障礙物在工作空間形成了幾何約束，必須進行避障控制。機械手避障控制是確保機械手末端在完成軌跡跟蹤主任務時機械手連桿能避開障礙物，機械手的正確姿態(tài)需要進行無碰撞路徑規(guī)劃作為期望關節(jié)軌跡。

人們已經(jīng)在機械手避障這一領域做了很多研究，其中，主任務雅克比矩陣偽逆的零空間投影是實現(xiàn)機械手避障的一種重要方法，對于避障機械手，其工作任務包括末端執(zhí)行機構的路徑規(guī)劃任務(主任務)和連桿的避障任務(次級任務)，在不影響主任務的情況下，次級任務可以通過偽逆零空間投影實現(xiàn)[1-8]，這樣，避障問題轉化為優(yōu)化目標函數(shù)問題。盡管文獻[1-8]所述控制器利用零空間投影實現(xiàn)了機械手避障，但在實際應用中依然遇到問題。一個有效的運動/動力學控制方法必須考慮執(zhí)行器飽和問題，關節(jié)力矩需要限定在工作范圍內，否則將很難用于實踐[9]。

本文提出機械手在實現(xiàn)末端軌跡跟蹤的前提下，利用雅克比矩陣擴展方法，引入關節(jié)位移能量優(yōu)化與避障約束自運動，同時采用峰值力矩減小法，實現(xiàn)在力矩約束下的避障控制。

1 機械手逆運動學

假設一個機械手有n個關節(jié)，操作空間為m維，有n>m。若Q為n維關節(jié)空間，W為m維任務空間，則正向運動函數(shù)表示為f：Q→W。設q=(q1,q2,…qn)T為關節(jié)角度，x=(x1,x2,…,xm)T為機械手在操作空間的位置，則機械手的運動方程有:

x=f(q)

(1)

(2)

這里J(q)∈Rm×n稱為機械手的雅克比矩陣，由于n>m，J(q)是一個長方形矩陣。應用零空間投影法，式(2)可改寫為：

(3)

假設h：Q→R是一個優(yōu)化每一位置的實數(shù)函數(shù)，則利用梯度投影法:

z=±α▽h

系統(tǒng)能夠漸進趨向最優(yōu)，這里α是任意正定增益常數(shù)。雖然式(3)僅需要計算▽h，但增益α的選擇很困難，α太小，自運動項會導致系統(tǒng)反應遲鈍，α太大，控制系統(tǒng)變得不穩(wěn)定。Baillieul[ 10]提出的擴展雅可比矩陣方法解決了這一問題：

假設ni(i=1,…,n-m)是張成J零空間的一組正交向量，N=(n1,…,nn-m)，在滿足末端執(zhí)行器位置的情況下最小化標量函數(shù)h，有:

NT▽h=0

(4)

N∈Rn×(n-m)，它的梯度可用于擴展原來的雅克比矩陣。

(5)

擴展后的雅克比矩陣成為方陣，可直接求逆，而不需要再用偽逆求解。

2 擴展雅克比矩陣避障控制設計

2.1 帶避障約束的擴展雅克比矩陣

應用擴展雅克比矩陣方法，避障約束任務可以被定義為：

(6)

機械手的雅克比矩陣可通過自運動向量擴展，結合式(2)與式(4)，有：

(7)

(8)

式(8)中函數(shù)h(q)必然包含障礙物限制的影響，通常以障礙物到機械手每根連桿間的歐幾里得距離di為基礎，構造防撞約束Di。這樣，由避障相關約束形成的標量函數(shù)定義為：

(9)

(10)

由此，式(9)標量函數(shù)可以寫成：

(11)

根據(jù)式(11)，可得h(q)的梯度表示為：

其中，

2.2 避障約束下的末端軌跡跟蹤控制律設計

已知n關節(jié)機械手動力學方程有：

(12)

由式(7)和式(12)，擴展雅克比矩陣避障控制律設計為[11]：

(13)

式中，

末端軌跡跟蹤采用PD控制，xd為末端期望軌跡，x為末端實際軌跡，Kp、Kd分別為比例與微分系數(shù)。

3 關節(jié)力矩優(yōu)化策略

力矩優(yōu)化是機械手動力學主要的優(yōu)化目標之一，但在許多情況下，例如跟蹤控制問題，限制峰值轉矩比轉矩最小化更重要，由于大的需求轉矩可以導致轉矩飽和，從而導致跟蹤精度的降低。本文提出利用機械手末端執(zhí)行器運動的局部信息得到一個力矩局部優(yōu)化方法，以減小峰值力矩。

最小化性能指標定義為：

(14)

滿足條件:

(15)

和

(16)

在這里W是斜對稱正定權重矩陣，

(1)根據(jù)當前狀態(tài)i的信息計算下一狀態(tài)i+1的信息：

(17)

(18)

(4)計算下一個狀態(tài)的速度 。計算如下：

(19)

(5)在i+1狀態(tài)重復上述操作。

具有峰值力矩優(yōu)化的機械手避障控制框圖如圖1所示。在給定末端軌跡的情況下，通過擴展雅克比矩陣方法設計避障自運動向量，同時在滿足閾值條件進行力矩優(yōu)化，閾值條件設置為各連桿距離障礙物的最小距離(安全距離)，安全距離大于所設閾值則采用力矩優(yōu)化算法，否則依然采用避障優(yōu)先控制。

圖1 機械手避障控制結構框圖

4 避障算法求解與仿真

算法仿真以平面3連桿機械手為例，亦可看成是常用6軸機械手在垂直平面的規(guī)劃，此時θ1、θ4和θ6為鎖緊狀態(tài)，只有另外三個關節(jié)是自由的，即:

(20)

平面3連桿機械手如圖2所示，機械手的末端自由度是2(m=2)，冗余度則為1(r=n-m=1)。障礙物被包裹在以圓心為(x1b,x2b)、半徑為rb的圓內。

圖2 平面3連桿機械手避障

4.1 算法求解

(1)求解標量函數(shù)h(q)

現(xiàn)以連桿1為例計算防撞約束D1。首先定義兩個向量v1=OA和v2=OC，連桿1至障礙物圓心的歐式距離為d1，則有：

(21)

由向量計算公式，有:

v1·v2=v1v2cos(v1,v2)

(22)

由式(22)有：

(23)

利用公式(21)～式(23)，可以計算出歐式距離d1：

這樣，防撞約束D1為：

(24)

這里ds為防撞安全余量。

用相同方法，可求得連桿2和連桿3的防撞約束D2和D3：

(25)

(26)

將式(24)～式(26)帶入式(11)即可求得h(q)。

(27)

(3)求解擴展雅克比矩陣Je

求解Je的關鍵在于Ja的求解，對于式(8)，Klein等提出了一種更易于數(shù)值化處理的Ja計算公式[19]：

Ja=NT▽(▽h)-W

在這里，W是一個向量，它的元素wi計算方法如下：

此處，▽(▽h)是hessian矩陣，即：

4.2 仿真結果

以圖3所示的實驗室LT950-A-6六軸機械手為例，θ2、θ3和θ5關節(jié)所對應的連桿長度分別為L1=0.45m，L2=0.45 m，L3=0.109 m；初始條件q0=[1.5;-1.08;-0.6]；障礙物是半徑為0.03m的圓，其圓心位于(0.4,0.44)。機械手末端軌跡為一條由上而下的直線：

圖3 六軸機械手與障礙物位置演示

(1)無避障算法的機械手末端軌跡跟蹤控制如圖4、圖5所示，末端軌跡跟蹤采用PD控制，參數(shù)選擇Kp=diag[2600,4500]，Kd=diag[30,21]，t=4s。由圖可見，采用PD控制算法基本能夠實現(xiàn)末端軌跡跟蹤控制要求，但是機械手連桿穿越障礙物而過，如果障礙物存在，必定直接損壞機械手結構。

(2)采用雅克比擴展避障算法的機械手軌跡跟蹤控制如圖6～圖8所示，末端軌跡跟蹤仍采用PD控制，參數(shù)選擇Kp=diag[1855,1840,1450]，Kd=diag[30,35,30]，避障參數(shù)設置為ρ1=0.1，ρ2=0.1，ρ3=0.4；機械手連桿距離障礙物的安全距離設置為ds=0.01。由圖8可見，雅克比擴展避障算法在實現(xiàn)末端軌跡跟蹤的同時實現(xiàn)了機械手的避障。但輸出的峰值力矩較大，如不加以限制，可能超出電機實際轉矩。

(3)在雅克比擴展避障算法基礎上，加入峰值力矩減小算法限定機械手關節(jié)出現(xiàn)的峰值力矩。PD參數(shù)與避障參數(shù)設置同上，機械手的實際關節(jié)力矩極限值分別為±60Nm、±40Nm、±20Nm。設置安全距離為0.02m，即只有在連桿距離障礙物大于0.02m時，采用力矩優(yōu)化算法。圖9～圖11所示為本文所提算法優(yōu)化結果。由圖可見，在實現(xiàn)避障同時，關節(jié)控制力矩峰值有效的限定在了極限值內。

從仿真結果看，在擴展雅克比算法中引入避障約束自運動，能在完成末端軌跡跟蹤的同時有效實現(xiàn)障礙物的回避，只要機械手關節(jié)力矩Kp和Kd參數(shù)設置合適，采用PD控制算法，末端軌跡能夠得到較好跟蹤，但是關節(jié)轉矩過大時，末端跟蹤誤差將隨之增加，由圖(7)和圖(10)的結果比較可見，峰值力矩減小算法的加入提高了避障時的軌跡跟蹤精度，同時將關節(jié)力矩限定在了工作范圍內。如果需要進一步提高跟蹤精度，還可采用自適應神經(jīng)網(wǎng)絡控制[1]、自適應模糊控制[8]等算法實現(xiàn)。

圖4 無避障的末端軌跡誤差

圖5 無避障機械手運動軌跡

圖6 避障控制下的關節(jié)控制力矩

圖7 避障控制下的末端軌跡跟蹤誤差

圖8 避障控制下的機械手軌跡

圖9 峰值力矩減小法應用后的關節(jié)控制力矩

圖10 峰值力矩減小法應用后的末端軌跡跟蹤誤差

圖11 峰值力矩減小法應用后的機械手軌跡

5 結論

本文利用機械手冗余度解決避障控制問題，在機械手逆運動學中利用擴展雅克比矩陣方法，引入實現(xiàn)避障任務的自運動向量，通過設計能量優(yōu)化與防撞約束的標量函數(shù)計算自運動向量，從而在完成軌跡跟蹤主任務的同時實現(xiàn)避障控制。為了達到實際控制需求，在完成避障控制的前提下，提出了峰值減小的力矩優(yōu)化算法，避免飽和轉矩對軌跡跟蹤的不良影響。本文算法在平面3連桿機械手上進行了仿真實驗，仿真結果很好的驗證了算法的有效性，該算法可直接擴展應用于六軸機械手的避障控制中。

[1] Daachi B, Madani T, Benallegue A. Adaptive neural controller for redundant robot manipulators and collision avoidance with mobile obstacles[J]. Neurocomputing, 2012, 79(3):50-60.

[2] Ding H, Chan S P. A Real-Time Planning Algorithm for Obstacle Avoidance of Redundant Robots[J]. Journal of Intelligent & Robotic Systems, 1996, 16(3):229-243.

[3] 封岸松, 戴炬. 冗余自由度機械手的避障控制[J]. 機器人, 2002, 24(3):213-216.

[4] 謝宗武, 孫奎, 劉宏. 擴展雅克比方法的冗余度機器人逆運動學應用[J]. 哈爾濱工業(yè)大學學報, 2009,41(5):34-36.

[5] Guo D, Zhang Y. A New Inequality-Based Obstacle-Avoidance MVN Scheme and Its Application to Redundant Robot Manipulators[J]. IEEE Transactions on Systems Man & Cybernetics Part C, 2012, 42(6):1326-1340.

[6] Yen-Chen Liu, Nikhil Chopra. Control of semi-autonomous teleoperation system with time delays [J]. Automatica, 2013, 49(6):1553-1565.

[7] Liu Y C, Chopra N. Semi-autonomous teleoperation in task space with redundant slave robot under communication delays[J]. 2011, 32(14):679-684.

[8] Benzaoui M, Chekireb H, Tadjine M, et al. Trajectory tracking with obstacle avoidance of redundant manipulator based on fuzzy inference systems[J]. Neurocomputing, 2016, 196: 23-30.

[9] Salinas A, Moreno-Valenzuela J, Kelly R. A family of nonlinear PID-like regulators for a class of torque-driven robot manipulators equipped with torque-constrained actuators[J]. Behavior modification,1997,21(4):470.

[10] Baillieul J. Kinematic programming alternatives for redundant manipulators[C]//Robotics and Automation. Proceedings. 1985 IEEE International Conference on. IEEE, 1985, 2: 722-728.

[11] Benzaoui M, Chekireb H, Tadjine M. Redundant robot manipulator control with obstacle avoidance using extended Jacobian method[C]// Control & Automation (MED), 2010 18th Mediterranean Conference on,2010:371-376.