張勤　范長湘

(華南理工大學 機械與汽車工程學院， 廣東 廣州 510640)

基于任務(wù)分解和速度分配的機器人運動規(guī)劃*

張勤范長湘

(華南理工大學 機械與汽車工程學院， 廣東 廣州 510640)

摘要：為了克服機器人在奇異點附近的運動障礙，提出基于任務(wù)分解和速度分配的機器人運動規(guī)劃方法：將控制任務(wù)按照優(yōu)先順序分解成若干項子任務(wù)分別進行控制；根據(jù)電機的驅(qū)動能力和任務(wù)優(yōu)先順序確定各項子任務(wù)的速度分配，以便在機器人處于奇異姿態(tài)或在奇異姿態(tài)附近，其靈活性降低的情況下，機器人也能夠協(xié)調(diào)運動，保證重要控制目標的實現(xiàn)，避免由于速度突變造成的機器人抖動、末端誤差過大和控制系統(tǒng)不穩(wěn)定現(xiàn)象.最后以PUMA機器人為例，說明了控制系統(tǒng)的構(gòu)成，并通過仿真證明了文中方法的可行性.

關(guān)鍵詞：機器人；運動規(guī)劃；任務(wù)分解；速度分配；奇異姿態(tài)

機器人的運動規(guī)劃是實現(xiàn)高速、高精度、高穩(wěn)定性操作的重要保證.當機器人處于奇異位形時，速度雅可比矩陣降秩，末端執(zhí)行器自由度減少，機器人的靈活性降低，在這種情況下，如何克服機器人在奇異點附近的運動障礙，保證重要控制目標，已經(jīng)成為了當前國內(nèi)外機器人領(lǐng)域的研究熱點.

奇異點的存在是機器人的一個固有特性，包括結(jié)構(gòu)邊界奇異問題和內(nèi)部奇異問題.針對機器人的結(jié)構(gòu)邊界奇異問題，目前比較常用的方法是把關(guān)節(jié)極限角度作為邊界約束，通過優(yōu)化對關(guān)節(jié)角的輸出進行控制，防止關(guān)節(jié)角超限.而對于內(nèi)部奇異問題，Wampler等[1]提出“阻尼最小方差”修正雅可比矩陣的方法，通過選擇合適的阻尼因子，保證奇異區(qū)域內(nèi)關(guān)節(jié)速度的連續(xù)性，實現(xiàn)奇異區(qū)域的平穩(wěn)過渡，但該方法無法避免由此產(chǎn)生的機器人末端的控制誤差；Chiaverini[2]對上述方法進行了改進，提出“阻尼最小方差+數(shù)值濾波”的方法，綜合考慮軌跡規(guī)劃精度和關(guān)節(jié)速度的關(guān)系，提高了機器人末端位置精度.Cheah等[3]通過奇異區(qū)域內(nèi)運動參數(shù)調(diào)整，奇異區(qū)域外偏差補償?shù)姆椒?，實現(xiàn)了機器人跨越奇異區(qū)域或者從奇異區(qū)域啟動.文獻[4-5]采用奇異分離和阻尼優(yōu)化的方法，分離出機器人運動中影響奇異的參數(shù)，實現(xiàn)機器人的關(guān)節(jié)速度在奇異區(qū)內(nèi)連續(xù)平滑的同時，還可以保證機械人末端在某些方向的控制精度.綜合以上分析可以看出，上述研究主要是考慮如何回避奇異點，保證奇異區(qū)域內(nèi)各關(guān)節(jié)速度的平穩(wěn)性，但一般是以犧牲機器人末端的控制精度為代價.

增加機器人的關(guān)節(jié)自由度數(shù)，能夠有效提高機器人的靈活性.通過冗余機器人的自運動能力，機器人能在完成作業(yè)目標的同時，盡力實現(xiàn)回避奇異位形、躲避障礙、容錯控制以及改善動力學性能等操作[6-7].在冗余機器人的控制中，逆運動學的求解是關(guān)鍵.目前普遍使用的求解方法主要有擴展雅可比矩陣法、基于四元數(shù)求解法和梯度投影法等.Baillieul[8]提出擴展雅可比矩陣方法，將機器人回避奇異位形和躲避障礙等任務(wù)作為約束加入機器人的求解運算中，把雅可比矩陣擴展為方陣進行求解.基于四元數(shù)求解法是基于旋量理論描述機械人的空間方位，可以在一定范圍內(nèi)回避奇異姿態(tài)，但容易引起系統(tǒng)的不穩(wěn)定[9-10].Liegeois[11]提出的梯度投影算法是應(yīng)用最為廣泛的方法，其將冗余機器人關(guān)節(jié)速度的通解表示為特解與對應(yīng)的齊次項之和，通過評價函數(shù)或者子任務(wù)來確定齊次解，優(yōu)化控制.以此為基礎(chǔ)，根據(jù)不同的控制目標，確定齊次解，提出了諸多的優(yōu)化方法.Yoshikawa、姚建初等[12-13]以機器人的可操作性作為評價，在奇異姿態(tài)的回避上得到很好的應(yīng)用.針對現(xiàn)有算法中普遍存在的奇異姿態(tài)附近關(guān)節(jié)速度過大的問題，Colome等[14]基于雅可比矩陣奇異值修正濾波和設(shè)定可變阻尼的方法，對逆運動學的求解進行了改進，使機器人奇異姿態(tài)附近的運動更加平穩(wěn)，但計算量大，限制了其應(yīng)用范圍.文獻[7,15]提出基于任務(wù)優(yōu)先級的冗余控制方法，根據(jù)冗余機器人的能力優(yōu)先保證優(yōu)先級別高的任務(wù)，再依次盡力完成級別低的任務(wù)，但也無法避免奇異區(qū)域內(nèi)關(guān)節(jié)速度過大的現(xiàn)象.針對這個問題，文獻[16]提出了冗余機器人的反饋運動控制方法.在以上研究的基礎(chǔ)上，本研究將基于任務(wù)分解的冗余控制思路應(yīng)用于非冗余機器人控制中，并進行了改進，提出基于任務(wù)分解和速度分配的機器人運動規(guī)劃方法，也就是TDSD(Task-Decomposition and Speed-Distribution)法.按照目標任務(wù)的優(yōu)先級別將控制任務(wù)分解為各項子任務(wù)，分別進行控制；根據(jù)電機的驅(qū)動能力和任務(wù)優(yōu)先級，確定各項子任務(wù)的速度分配，這樣在機器人處于奇異姿態(tài)或在奇異姿態(tài)附近時，即使靈活性降低，機器人也能夠協(xié)調(diào)運動，保證重要控制目標的同時緩慢通過其奇異位形.最后以PUMA機器人為例說明了控制系統(tǒng)的構(gòu)成，并通過仿真證明了所提出的方法的可行性.

1基于TDSD的控制系統(tǒng)構(gòu)成

1.1方法的提出及其數(shù)學描述

多自由度的機器人通常完成復(fù)雜的操作，在機器人控制任務(wù)的實現(xiàn)過程中，作業(yè)目標不同，機器人優(yōu)先保證的任務(wù)也不同.比如：在焊接操作中，通常機械手末端的位置比姿態(tài)更為重要；在一些旋轉(zhuǎn)裝配作業(yè)中，相對于機械手的末端軌跡，其姿態(tài)更為重要；在液體搬運操作中，經(jīng)常需要保證盛具的位置和液面方向，而對盛具自身轉(zhuǎn)動方向的自由度要求不高.針對不同的控制目標，將控制任務(wù)按照優(yōu)先順序分解成若干個子任務(wù)，分別進行控制，這樣當機器人處于奇異姿態(tài)或者在奇異姿態(tài)附近，自由度減少時，機器人將首先保證優(yōu)先級最高的任務(wù)，然后再根據(jù)機器人的能力，依次完成優(yōu)先級別低的任務(wù)，滿足控制的要求.

假設(shè)機器人具有n個自由度，其第i個關(guān)節(jié)角度表示為θi(i=1，2，…，n)，對應(yīng)的關(guān)節(jié)角度矩陣為θ，則對應(yīng)的機械手末端位置和姿態(tài)為關(guān)節(jié)角度的函數(shù)y =f(θ)，且

(1)

其中J為雅可比矩陣.根據(jù)優(yōu)先順序?qū)⒛繕巳蝿?wù)分成若干目標子任務(wù)，子任務(wù)可以是給定的目標軌跡y1d，y2d，…，yid(i

(2)

(3)

由式(3)解得

(4)

(5)

(6)

當?shù)?項子任務(wù)以評價函數(shù)p=V(θ)的形式給出時，通過保證p最大，確定通解中的k1值.可以設(shè)定k1= ξkp，其中ξ = [ξ1ξ2…ξn]T，而ξi= ?V(θ) /?θi，代入式(2)可得

(7)

綜合以上的分析，可以看出目標關(guān)節(jié)速度可以看成是實現(xiàn)各項子任務(wù)的關(guān)聯(lián)速度之和：

(8)

1.2控制系統(tǒng)的構(gòu)成

按照控制任務(wù)優(yōu)先級將機器人的作業(yè)目標分解為各項子任務(wù)，對其進行分別控制，完成控制目標的機器人的關(guān)節(jié)速度可以分解為完成各項子任務(wù)需要的各關(guān)節(jié)速度的綜合.以7自由度機器人為研究對象，以典型的空間6自由度作業(yè)任務(wù)為例，說明運動控制系統(tǒng)的構(gòu)成.

考慮作業(yè)任務(wù)的一般性，假設(shè)將控制任務(wù)按照優(yōu)先順序分解為位置控制、姿態(tài)控制和優(yōu)化控制3層子任務(wù)，各項任務(wù)分別控制，控制框圖如圖1所示.

圖1　基于TDSD的機器人運動控制框圖Fig.1　Block diagram of the kinematic control based on TDSD

(9)

(10)

根據(jù)以上設(shè)定，實際機器人目標關(guān)節(jié)速度為

(11)

通過速度分配系數(shù)Q1、Q2，驅(qū)動電機會優(yōu)先保證優(yōu)先級別高的位置控制需要的速度要求，然后是姿態(tài)，最后是優(yōu)化條件.控制過程中，無需判定奇異位形的位置，控制系統(tǒng)根據(jù)式(9)或式(10)，自動確定每個時刻Q1、Q2值，協(xié)調(diào)或者緩沖速度突變，保證重要的控制目標，避免通常的控制方法中機器人在奇異位形及奇異位形附近出現(xiàn)的目標軌跡誤差過大或者陷入奇異位形不能逃脫的現(xiàn)象.假設(shè)p表示機械手末端的實際位置矢量，a、o、n分別為機械手末端姿態(tài)矢量；同樣機械手末端的目標位置和姿態(tài)矢量分別為pd、ad、od、nd，則機械手末端的實際位置姿態(tài)和目標位置姿態(tài)的齊次變換矩陣分別為：

ep=[dx,dy,dz]T=pd-p

(12)

姿態(tài)誤差通過式(13)求出[17].

(13)

2仿真實例

根據(jù)第1節(jié)的分析可知，按照任務(wù)的優(yōu)先級別將控制任務(wù)分解為各項子任務(wù)，分別進行控制，根據(jù)電機的驅(qū)動能力和任務(wù)優(yōu)先級，確定各項子任務(wù)的速度分配系數(shù)Q1、Q2，這樣當機器人處于奇異姿態(tài)或在奇異姿態(tài)附近時，即使靈活性降低，機器人也能夠協(xié)調(diào)或者緩沖速度突變，緩慢通過其奇異位形，保證重要控制目標.

以PUMA機械臂為例，通過仿真證明文中提出的控制方法的可行性.

2.1奇異姿態(tài)附近的運動規(guī)劃

圖2　PUMA型機器人模型Fig.2　Model of PUMA robot

圖3　肩部奇異點附近機械臂的運動示意圖(單位：mm)Fig.3　Schematic diagram of motion of the manipulator near the shoulder singularity(Unit:mm)

PUMA型機械臂的結(jié)構(gòu)如圖2所示.圖中l(wèi)分別表示機械手各個連桿的長度.通常情況下，機械臂存在3種類型的奇異姿態(tài)：肩部、腕部和肘部奇異姿態(tài).其中肩部和手腕部的奇異姿態(tài)在機械臂可動空間內(nèi)的任何地方都可能存在.假設(shè)控制任務(wù)中需要保證機械手末端的位置和姿態(tài)，且位置控制更為重要.當機械臂末端從A點運動到B點時，如圖3所示，機械臂將通過肩部奇異姿態(tài)及其附近，此時機械臂的靈活性大大降低.圖中：la=671 mm,lb=ld=le=0,lc=433 mm,lf=433 mm,lg=50 mm.圖4、圖5示出了基于位置和姿態(tài)的一般控制方法和基于TDSA方法求得的機械臂末端從A點到B點運動過程中，其末端軌跡的位姿誤差[dx,dy,dz,δx,δy,δz]T和各關(guān)節(jié)角度q的變化.從圖4(a)、圖5(a)可以看出，機械臂在奇異點附近區(qū)域(A2—A5區(qū)間)各關(guān)節(jié)速度突變，機械手末端出現(xiàn)了較大誤差.為了避免這種情況發(fā)生，將控制任務(wù)分解為位置控制和姿態(tài)控制，并進行分別控制，根據(jù)各關(guān)節(jié)驅(qū)動速度容許值和式(9)確定Q1，可以緩沖速度突變，協(xié)調(diào)速度分配，使機械臂可以緩慢通過奇異姿態(tài).對比圖4、圖5中的(a)和(b)可以看出，機械手在通過其肩部奇異姿態(tài)附近時，即使各關(guān)節(jié)角度突變，由于任務(wù)分解，對位置和姿態(tài)分別進行控制，以及Q1的調(diào)節(jié)作用，機械臂末端的目標位置仍然可以完全保證，機械手末端的姿態(tài)誤差也大大減小.圖6為從A點到B點運動過程中Q1的變化.在肩部奇異姿態(tài)附近(A2—A5)，Q1<1，此區(qū)間內(nèi)，機械臂優(yōu)先滿足控制機械手末端位置需要的速度，在此前提下，再盡量保證控制姿態(tài)需要的速度，在t=1.76 s附近，Q1近似為0，表示此時只能保證其末端的位置，而沒有能力控制機械臂的姿態(tài)，此點對應(yīng)的姿態(tài)誤差應(yīng)該為最大.在奇異區(qū)域以外的正常工作區(qū)域，Q1=1，與通常的位姿控制方法相同.

圖4　肩部奇異點附近機械臂末端的位置和姿態(tài)誤差Fig.4　Positional and orientational errors of the end-effecter near the shoulder singularity

圖5　肩部奇異點附近的機械臂各關(guān)節(jié)角度變化Fig.5　Joint-angle changes of the manipulator near the shoulder singularity

圖6　速度分配過程中Q1值的變化Fig.6　Changes of Q1 during speed-distribution

圖7　PUMA機器人搬運液體示意圖Fig.7　Schematic diagram of PUMA robot’s motion of transporting liquid

2.2液體搬運任務(wù)的運動規(guī)劃

液體搬運過程中，筆者更關(guān)心的是搬運軌跡和保證盛具內(nèi)的液體不灑落，而不在意盛具沿其軸線方向的轉(zhuǎn)動.假設(shè)利用PUMA機械臂將水桶從a點經(jīng)過b點搬到c處(如圖7所示)，且機械手各關(guān)節(jié)可動范圍分別為q1∈[-160°,160°]，q2∈[-225°,45°]，q3∈[-45°,225°]，q4∈[-110°,170°]，q5∈[-100°,100°]，q6∈[-266°,266°].在通常的控制方法中，雖然可以求得完成上述控制任務(wù)需要的各關(guān)節(jié)轉(zhuǎn)角，如圖8(a)所示，但從圖中可以看出機械手腕部的角度超過了其關(guān)節(jié)的可動范圍，實際控制也是無法完成的.為此可以將控制任務(wù)分解為控制水桶的位置和其法向的姿態(tài)[001]T(圖7中的虛線框部分)，剩余的自由度盡量保證機械手腕部轉(zhuǎn)角q4、q5、q6在其可動角度范圍中間附近.設(shè)定評價函數(shù)為

(14)

根據(jù)1.1節(jié)可以求得完成控制任務(wù)過程中，各關(guān)節(jié)角度的變化，如圖8(b)所示.a、b、c點處的位姿分別為：

圖8　機械臂各關(guān)節(jié)角度變化Fig.8　Joint-angle changes of the manipulator——關(guān)節(jié)角度變化；對應(yīng)關(guān)節(jié)角度的可動范圍；從這點開始關(guān)節(jié)角度超出其可動范圍

對比圖8(a)、8(b)可見，各關(guān)節(jié)的轉(zhuǎn)角完全控制在其可動范圍內(nèi)，且滿足控制目標要求.

3結(jié)論

文中提出基于任務(wù)分解和速度分配的防御性機器人運動規(guī)劃方法.針對不同的控制目標，將控制任務(wù)按照優(yōu)先順序分解成若干項子任務(wù)，分別進行控制；根據(jù)電機的驅(qū)動能力和任務(wù)的優(yōu)先級，確定各項子任務(wù)的速度分配，這樣當機器人處于奇異姿態(tài)或在奇異姿態(tài)附近時，即使靈活性降低，也能夠保證重要控制目標；通過改變奇異姿態(tài)附近的機器人各關(guān)節(jié)運動速度，可以避免由于速度突變造成的機器人抖動和末端誤差過大以及控制系統(tǒng)不穩(wěn)定的現(xiàn)象發(fā)生.與現(xiàn)有的控制方法相比，文中方法不需要復(fù)雜的運算，可以最大限度地利用機器人的驅(qū)動能力(自由度數(shù))，協(xié)調(diào)機器人運動平穩(wěn)性與機械手末端精度的關(guān)系，能滿足實際工程中需要.最后以PUMA機器人為例，說明了文中方法的可行性.文中方法適用于任何類型機器人的冗余和非冗余控制.

參考文獻：

[1]WAMPLER C W.Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods [J]. IEEE Transactions on Systems,Man and Cybernetics,1986,16(1):93-101.

[2]CHIAVERINI Stefano.Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators [J]. IEEE Transactions on Robotics and Automation,1997,13(3):398-410.

[3]CHEAH C C,Li X.Singularity-robust task-space tracking control of robot [C]∥Proceediings of 2011 IEEE,International Conference on Robotics and Automation.Shanghai:IEEE,2011:5819-5824.

[4]徐文福,梁斌,劉宇,等.一種新的 PUMA 類型機器人奇異回避算法 [J]. 自動化學報,2008,34(6):670-675.

XU Wen-fu,LIANG Bin,LIU Yu,et al.A novel approach to avoid singularities of PUMA-type Manipulators [J]. Acta Automatica Sinica,2008,34(6):670-675.

[5]劉海濤.工業(yè)機器人的高速高精度控制方法研究 [D]. 廣州:華南理工大學,2012.

[6]HANAFUSA H,YOSHIKAWA T,NAKAMURA Y.Analy-sis and control of articulated robot with redundancy [C]∥Proceedings of IFAC Symposium on Robot Control,8th Triennal World Congress.[S.l.]:IFAC,1981:1927-1932.

[7]HANAFUSA H,YOSHIKAWA T,NAKAMURA Y.Redundancy analysis of articulated robot arms and its utilization for tasks with priority [J]. Transactions of the Society of Instrument and Control Engineers,1983,19(5):421- 426.

[8]BAILLIEUL J.Kinematic programming alternatives for redundant manipulators [C]∥Proceedings of the IEEE International Conference on Robotics and Automation.St Louis：IEEE,1985:722-728

[9]De OLIVEIRA A S,De PIERI E R,MORENO U F,et al.A new approach to singularity-free inverse kinematics using dual-quaternionic error chains in the Davies method [J]. Robotica,2014:1-15.DOI:10.1017/S026357471 4001982.[10]SARIYILSIZ E,TEMELTAS H.A new formulation me-thod for solving kinematic problems of multiarm robot systems using quaternion algebra in the screw theory framework [J]. Turkish Journal of Electrical Engineering and Computer Science,2012,20(4):607-628.

[11]LIEGEOIS A.Automatic supervisory control of the configuration and behavior of multibody mechanisms [J]. IEEE Transactions on Systems,Man,and Cybernetics,1977,7(12):868-871.

[12]YOSHIKAWA T.Analysis and control of robot manipulators with redundancy [C]∥Proceedings of Robotics Research:The First International Symposium.Cambridge,MA:MIT Press,1984:735-748.

[13]姚建初,丁希侖,戰(zhàn)強,等.冗余度機器人基于任務(wù)的方向可操作度研究 [J]. 機器人,2000,22(6):501-505.

YAO Jian-chu,DING Xi-lun,ZHAN Qiang,et al.On task-based directional manipulability measure of redundant Robot [J]. Robot,2000,22(6):501-505.

[14]COLOME A,TORRAS C.Closed-Loop inverse kinema-tics for redundant robots:comparative assessment and two enhancements [J]. IEEE/ASME Transactions on Mechatronics,2015，20(2):944-955.

[15]NAKAMURA Y,HANAFUSA H,YOSHIKAWA T.Task-priority based redundancy control of robot manipulators [J]. The International Journal of Robotics Research,1987,6(2):3-15.

[16]張勤,神谷好承,関弘明.冗長制御系の概念を応用したマニピュレータの運動制御 [C]∥ロボット學會論文集,東京，日本,2001.ZHANG Qin,YOSHITSUGU K,HIROAKI S.Motion control of manipulator from concept of redundancy [C]∥Proceedings of the Robotics Society of Japan.Tokyo:Japan[s.n.],2001.

[17]小林尚登,他.ロボット制御の実際 [C]∥計測自動制御學會.東京:1997.

KOBAYASHI H,et al.Practical control of robot manipulators [C]∥Proceedings of The Society of Instrument and Control Engineers.Tokyo:[s.n.],1997.

Motion Planning of Robot on the Basis of Task Decomposition and Speed Distribution

ZHANGQinFANChang-xiang

(School of Mechanical and Automotive Engineering, South China University of Technology, Guangzhou 510640, Guangdong, China)

Abstract:In order to overcome the motion difficulties of robot near its singularities, a new motion planning algorithm is proposed on the basis of task decomposition and speed distribution.In this algorithm, the control task is divided into several sub-tasks according to their priorities, and each sub-task is controlled independently.The velocity component of each sub-task is determined according to the driving capability of joint motors and the priority of each sub-task, which helps implement the coordinated movement of robot even in the condition of low flexibility when the robot moves near the singularities or stays in the singularities, thus achieving the control objective and avoiding the vibration, excessive end-effector error and system instability caused by the sudden change of joint relative velocity.By taking a PUMA robot for example, the composition of the control system is illustrated, and the feasibility of the proposed method is proved via simulation.

Key words:robot; motion planning; task decomposition; speed distribution; singularity

收稿日期：2015-09-29

*基金項目：廣東省教育部產(chǎn)學研結(jié)合項目(2012B091100145)；廣東省科技計劃項目(2014A020208018)

Foundation items: Supported by the Industry-Universities-Research Institutes Collaboration Project of Guangdong Province(2012B091100145) and the Science and Technology Planning Project of Guangdong Province(2014A020208018)

作者簡介：張勤(1964-)，女，博士,教授，碩士生導師，主要從事機器人及其應(yīng)用系統(tǒng)研究.E-mail：zhangqin@scut.edu.cn

中圖分類號：TP242

doi：10.3969/j.issn.1000-565X.2016.03.007