王 勇，唐 強(qiáng)，徐拴鋒，朱志斌，何英姿，魏春嶺

(1.北京控制工程研究所，北京 100190； 2.空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100190)

?

串并混聯(lián)空間機(jī)械臂全自由度軌跡規(guī)劃算法*

王 勇1,2，唐 強(qiáng)1,2，徐拴鋒1,2，朱志斌1,2，何英姿1,2，魏春嶺1,2

(1.北京控制工程研究所，北京 100190； 2.空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100190)

目前，國內(nèi)外對串并混聯(lián)空間機(jī)械臂的研究較少；而且大多在舍棄冗余自由度的情況下進(jìn)行軌跡規(guī)劃.針對7自由度串并混聯(lián)空間機(jī)械臂，提出了一種基于解析解、并且充分放開全部7個自由度的機(jī)械臂軌跡規(guī)劃算法.該規(guī)劃算法既擴(kuò)大了串并混聯(lián)空間機(jī)械臂的規(guī)劃可達(dá)空間，又保證了規(guī)劃的實(shí)時性，同時還保留了結(jié)構(gòu)承載能力強(qiáng)的優(yōu)點(diǎn)，可以充分發(fā)揮其作為空間抓捕用臂的優(yōu)勢.通過仿真和地面空間操作抓捕試驗(yàn)，驗(yàn)證了算法的有效性.

空間機(jī)器人；空間操作；軌跡規(guī)劃；串并混聯(lián)；冗余自由度.

0 引 言

近年來，以美歐俄日為代表的航天大國正在大力發(fā)展空間操作控制技術(shù)，并開展了相關(guān)的在軌飛行演示驗(yàn)證[1-4].因此，大力推進(jìn)空間操作關(guān)鍵技術(shù)攻關(guān)非常重要，這將有助于中國在空間維護(hù)和操作領(lǐng)域中占據(jù)優(yōu)勢地位.

與地面固定基座機(jī)器人不同，空間機(jī)器人處于失重環(huán)境下，且由于任務(wù)需要，空間機(jī)械連桿多為細(xì)長，材料也多選用輕質(zhì)材料.對于傳統(tǒng)的串聯(lián)機(jī)械臂，其工作空間大、結(jié)構(gòu)靈巧，但承載能力不足.串聯(lián)構(gòu)型的空間機(jī)械臂在運(yùn)行過程中，難免會給機(jī)器人系統(tǒng)引入較大振動，機(jī)械臂的顫動或變形可能會導(dǎo)致目標(biāo)抓取失敗、設(shè)備損壞等問題.并聯(lián)機(jī)械臂承載能力強(qiáng)、結(jié)構(gòu)緊湊，但其缺點(diǎn)是工作空間小.串并混聯(lián)機(jī)械臂則兼顧上述兩種機(jī)械臂的優(yōu)點(diǎn)[5].將一部串聯(lián)機(jī)械臂安裝于另一部并聯(lián)機(jī)械臂上，這種結(jié)構(gòu)不僅具有較高的剛度，而且末端定位精度也有較大提升[6-7].

在對目標(biāo)進(jìn)行近距離操作時，考慮到安全性和節(jié)省燃料等要求，空間機(jī)械臂的基座通常是自由漂浮的[8].無論是否對基座進(jìn)行控制，由于參數(shù)不確定性和各種干擾等因素，在抓捕過程中，基座的實(shí)際位姿與期望位姿相比均會具有一定的偏差[9]，這就對空間機(jī)械臂的性能，尤其是其運(yùn)動范圍提出了較高要求.即使采用了串并混聯(lián)的結(jié)構(gòu)設(shè)計(jì)，并聯(lián)機(jī)構(gòu)部分也會對機(jī)械臂系統(tǒng)的運(yùn)動空間帶來一定的影響，若不針對其運(yùn)動范圍進(jìn)行改進(jìn)，較大的基座?？课蛔似羁赡芫蜁诳陀^上直接導(dǎo)致機(jī)械臂無法對目標(biāo)完成抓捕.通常來講，機(jī)械臂一旦設(shè)計(jì)完成，其可運(yùn)動范圍就由軌跡規(guī)劃及控制算法來決定了.常見的機(jī)械臂軌跡規(guī)劃大都是針對時間或能量最優(yōu)進(jìn)行設(shè)計(jì)的[10-11]，這種算法計(jì)算復(fù)雜度高、也很少特別針對可運(yùn)動范圍進(jìn)行優(yōu)化.

為此，本文設(shè)計(jì)了一條7自由度串并混聯(lián)空間機(jī)械臂原理樣機(jī)，其具有一個冗余自由度.同時，為了擴(kuò)大機(jī)械臂的工作空間，提出了一種基于解析解、并且充分利用全部7個自由度的機(jī)械臂軌跡規(guī)劃算法.進(jìn)而，通過仿真以及在6自由度氣浮臺上進(jìn)行的空間抓捕試驗(yàn)，對算法的有效性進(jìn)行了分析和驗(yàn)證.

1 串并混聯(lián)機(jī)械臂的逆運(yùn)動學(xué)解算

1.1 串并混聯(lián)機(jī)械臂的模型及坐標(biāo)系定義

人類的手臂由肩部、上臂、前臂、手腕、手等部分組成，其中，在進(jìn)行運(yùn)動時，通常是肩部受力最大，這與機(jī)械臂在進(jìn)行抓捕等操作時一致，為了適應(yīng)這種力的分配情況，設(shè)計(jì)了一種新型的7自由度串并混聯(lián)機(jī)械臂，并將其作為空間抓捕機(jī)械臂的原型樣機(jī)，如圖1所示.機(jī)械臂由下臂、上臂兩部分組成，具有7個自由度.其中，下臂采用3支鏈并聯(lián)形式，具有3個自由度，其中主支鏈為UP形式，2根輔支鏈為UPS形式(U表示虎克鉸，P表示移動副，S表示球鉸)；上臂采用4自由度串聯(lián)形式.

圖1 7自由度串并混聯(lián)機(jī)械臂Fig.1 7 DOFs series-parallel manipulator

圖1中可以看到，UP支鏈的兩個轉(zhuǎn)動軸與肘部的方位轉(zhuǎn)動軸交于一點(diǎn)，因此，該構(gòu)型的機(jī)械臂具有解析形式的反解結(jié)果.解析解一個很大的優(yōu)勢就是使機(jī)械臂的控制和規(guī)劃(如力控制等)可以充分滿足實(shí)時性的需求.由于機(jī)構(gòu)具有7個自由度，當(dāng)給定末端位姿，理論上各關(guān)節(jié)的輸入值具有無數(shù)個解，因此，求解時必須固定其中一個自由度.考慮到控制實(shí)時性要求，將并聯(lián)部分UP支鏈伸長量給定，并聯(lián)臂此時僅具有2個轉(zhuǎn)動自由度(可視為繞UP支鏈虎克鉸的兩個轉(zhuǎn)動)，此時機(jī)械臂等效為一個6自由度的串聯(lián)機(jī)械臂.等效的機(jī)械臂模型及坐標(biāo)系定義如圖2所示.

圖2 機(jī)械臂模型及坐標(biāo)系定義Fig.2 Model and reference frame definition of manipulator

1.2 串并混聯(lián)機(jī)械臂的逆運(yùn)動學(xué)解算

與運(yùn)動學(xué)解算過程相反，當(dāng)機(jī)器人末端工具坐標(biāo)系與目標(biāo)位姿矩陣重合時，求各關(guān)節(jié)轉(zhuǎn)角θi的過程稱為逆運(yùn)動學(xué)解算，下面將簡要敘述求取過程.首先，根據(jù)圖2列出DH參數(shù)[12]，如表1所示.其中，表中各變量含義已在圖2中進(jìn)行標(biāo)注.

表1 機(jī)械臂模型的DH參數(shù)

(1)

(1)求解θ3

令

定義：

r=A(1,4)2+A(2,4)2+A(3,4)2

其中，A(i,j)表示A矩陣的第i行、第j列元素.借助于坐標(biāo)系4、5、6的原點(diǎn)匯于一點(diǎn)這一特點(diǎn)，可以得到關(guān)于x3的一元四次方程，形式如下：

(2)

其中，

C4θ3= 4e2l2-4e2r+4e2z2-4el3+4elr-4els2+

l4-2l2r+2l2s2+r2-2rs2+s4，

C3θ3=8l3s-16el2s+8ls3-8rls，

C2θ3=-8e2l2-8e2r+8e2z2+2l4-4l2r+

20l2s2+2r2-4rs2+2s4，

C1θ3=8l3s+16el2s+8ls3-8rls，

C0θ3=4e2l2-4e2r+4e2z2+4el3-4elr+4els2+

l4-2l2r+2l2s2+r2-2rs2+s4.

利用牛頓迭代法，即可實(shí)現(xiàn)x3的求取，進(jìn)而可以得到θ3=2arctan(x3).

(2)求解θ2

(3)

其中，

C2θ2= -2e2-l2+2le-2e2m+2esn+

2elm-s2-2lsn，

C1θ2=4sme+4e2n，

C0θ2=r-2e2-l2-2le+2e2m-2esn+

2elm-s2-2lsn.

通過式(3)可以解出x2，求得的多根需要再代入式(4)中進(jìn)行驗(yàn)證：

(4)

其中，

(3)求解θ1

(5)

其中，

C2θ1=qsm+qen-e+pl-pem+psn+p4x,

C1θ1=0，

C0θ1=p4x-qsm-qen+e-pl+pem-psn.

p4x為4、5、6三個坐標(biāo)系相交的原點(diǎn)在機(jī)械臂基坐標(biāo)系下的x坐標(biāo)，可由DH參數(shù)直接計(jì)算得到.

通過式(5)可以解出x1，求得的多根需要再代入下式進(jìn)行驗(yàn)證

(6)

其中，

選取滿足要求的解，可以解出θ1=2arctan(x1).

(4)求解θ4、θ5、θ6

令

(7)

θ5=-arccos(-Tp(2,3))，θ5≠kπ(k為整數(shù))，

(8)

其中，Tp(i,j)表示Tp矩陣的第i行、第j列元素.

在1.1節(jié)中已經(jīng)提到，解算時UP支鏈伸長量是給定的，θ6和θ5是為了求解虛擬出來的轉(zhuǎn)角，因此，還需要給出UPS支鏈對應(yīng)的長度.圖3是并聯(lián)部分的模型及坐標(biāo)系定義.

圖3 機(jī)械臂并聯(lián)結(jié)構(gòu)模型及坐標(biāo)系Fig.3 Model and reference frame definition of manipulator parallel structure

根據(jù)幾何關(guān)系很容易得到UPS支鏈長度

(9)

(10)

2 全自由度軌跡規(guī)劃算法

2.1 固定的UP支鏈對機(jī)械臂可達(dá)空間的影響

為了得到唯一的解析解，在求解過程中，固定了UP支鏈的長度.根據(jù)圖3中并聯(lián)結(jié)構(gòu)模型示意圖可知，機(jī)械臂并聯(lián)部分的可達(dá)空間由2個虛擬的轉(zhuǎn)角θ6和θ5反映.這兩個轉(zhuǎn)角不僅會隨著UPS支鏈的變化而變化，UP支鏈的變化對也會對其造成影響.圖4為θ6和θ5隨著UP支鏈長度s的變化而變化情況.

圖4 虛擬轉(zhuǎn)角受UP支鏈長度的影響Fig.4 Influence on virtual DOFs from UP-chain length

可以看到，當(dāng)UP支鏈長度s取不同數(shù)值時，機(jī)械臂的運(yùn)動范圍是不同的.在傳統(tǒng)的軌跡規(guī)劃算法中，UP支鏈長度s往往是固定的，這樣帶來的問題就是，無法控制機(jī)械臂到達(dá)能力所及的極限范圍.為此，本文提出了一種基于解析解、并充分放開UP支鏈長度的軌跡規(guī)劃算法.

2.2 軌跡規(guī)劃算法

通過上面的分析可以知道，在規(guī)劃機(jī)械臂的運(yùn)動軌跡時，為了盡可能的擴(kuò)大其運(yùn)動范圍，需要在規(guī)劃過程中將冗余的自由度s放開.一旦放開s以后，從理論上看，機(jī)械臂末端的可達(dá)空間將擴(kuò)大為不同s情況下機(jī)械臂可達(dá)空間的合集.此時，雖然機(jī)械臂末端的可達(dá)空間增大了，但若在規(guī)劃時讓末端按照固定軌跡，如直線連續(xù)運(yùn)動，可以發(fā)現(xiàn)，由于運(yùn)動過程中無法穿越奇異區(qū)域，機(jī)械臂的連續(xù)運(yùn)動空間依然不會很大，而若采用復(fù)雜的軌跡規(guī)劃又會給實(shí)際運(yùn)行帶來不必要的困難.因此，在機(jī)械臂軌跡規(guī)劃過程中，將規(guī)劃做在了關(guān)節(jié)空間上，而不是笛卡爾空間上.基于上述思路，本文提出的機(jī)械臂軌跡規(guī)劃算法流程如圖5所示.

圖5 全自由度軌跡規(guī)劃算法Fig.5 Trajectory-planning algorithm based on the full DOFs

利用逆運(yùn)動學(xué)解析公式得到目標(biāo)機(jī)器人構(gòu)型之后，與當(dāng)前機(jī)器人構(gòu)型做差值等分，計(jì)算得到的步長即可作為機(jī)械臂各關(guān)節(jié)的控制量輸入.每個周期在更新機(jī)器人最新構(gòu)型后，循環(huán)執(zhí)行差值等分，直至機(jī)器人運(yùn)動到目標(biāo)點(diǎn)和目標(biāo)姿態(tài).

3 仿真和試驗(yàn)

在仿真中，通過計(jì)算雅克比矩陣的條件數(shù)[13]，給出了當(dāng)s分別為不同數(shù)值、以及根據(jù)本文算法動態(tài)變化時，機(jī)械臂的運(yùn)動范圍，如圖6所示.仿真過程中，機(jī)械臂末端期望Z坐標(biāo)取固定值，機(jī)械臂期望姿態(tài)亦保持恒定不變，只針對XY平面進(jìn)行分析比較；淺色區(qū)域表示機(jī)械臂在該點(diǎn)處奇異，深色區(qū)域表示機(jī)械臂在該點(diǎn)處存在唯一的逆運(yùn)動學(xué)解；圖中坐標(biāo)系為機(jī)械臂基坐標(biāo)系.

圖6(a)是當(dāng)UP支鏈的長度s=550 mm時，機(jī)械臂末端在XY平面上可達(dá)空間的奇異性情況；圖6(b)是s=580 mm時的情況；圖6(c)是s=600 mm時的情況；圖6(d)是s=620 mm時的情況；圖6(e)是s=650 mm時的情況；圖6(f)是本文算法提出的s動態(tài)變化時的情況.可以看到，s的動態(tài)變化使得機(jī)械臂的可達(dá)空間實(shí)現(xiàn)了不同s情況下可達(dá)空間的集合，因而，也就增大了機(jī)械臂的運(yùn)動范圍.仿真結(jié)果證實(shí)了本文提出的軌跡規(guī)劃算法的有效性.

在試驗(yàn)中，同樣固定機(jī)械臂末端期望Z坐標(biāo)和姿態(tài)，控制機(jī)械臂前伸，并對其前伸軌跡進(jìn)行分析.圖7記錄了前伸過程中，機(jī)械臂末端X、Y坐標(biāo)在機(jī)械臂基坐標(biāo)系下的軌跡.

圖7 機(jī)械臂末端軌跡Fig.7 Manipulator end-effector trajectory

從圖7中可以看到，機(jī)械臂沿著X方向前伸了大約410 mm，而從圖6的機(jī)械臂XY平面可達(dá)空間上可以看出，當(dāng)機(jī)械臂前伸(末端Y坐標(biāo)從350 mm變?yōu)?80 mm過程條件下)時，無論UP支鏈的長度s固定為何值，沿著X方向的可移動距離大概都在320 mm左右，而本文提出的軌跡規(guī)劃算法可使機(jī)械臂沿X方向的移動距離超過500 mm，大大增加了其運(yùn)動空間.

此外，還在大型花崗巖平臺上，利用6自由度氣浮臺、手眼雙目相機(jī)等設(shè)備，進(jìn)行了空間操作抓捕全物理試驗(yàn).試驗(yàn)結(jié)果表明本文軌跡規(guī)劃算法明顯提高了抓捕機(jī)械臂的運(yùn)動能力.

4 結(jié) 論

本文通過將7自由度串并混聯(lián)機(jī)械臂的UP支鏈伸長量固定，將其等效為一個6自由度形式的串聯(lián)機(jī)械臂，進(jìn)而得到解析的逆運(yùn)動學(xué)反解.在規(guī)劃過程中，又將UP支鏈伸長量進(jìn)行了釋放，提出了一種基于解析解的全自由度軌跡規(guī)劃算法.

解析解的存在，保證了規(guī)劃算法的實(shí)時性，將為以后引入力控制等復(fù)雜策略奠定基礎(chǔ)；而全自由度軌跡規(guī)劃則在最大程度上擴(kuò)大了串并混聯(lián)機(jī)械臂的可達(dá)空間.

試驗(yàn)結(jié)果表明，本文算法在保留串并混聯(lián)機(jī)械臂承載能力強(qiáng)、結(jié)構(gòu)緊湊的優(yōu)點(diǎn)之上，使機(jī)械臂的規(guī)劃可達(dá)空間大大提升，客觀上提升了對目標(biāo)星進(jìn)行抓捕和連接的能力，在未來空間機(jī)械臂在軌維修維護(hù)任務(wù)中具有廣泛的應(yīng)用前景.

[1] STOLL E, LETSCHNIK J, WALTER U，et al. On-orbit servicing[J]. IEEE Robotics and Automation Magazine, 2009,16(4): 29-33.

[2] YOSHIDA K. Engineering test satellite VII flight experiments for space robot dynamics and control: theories on laboratory test beds ten years ago, now in orbit[J]. The International Journal of Robotics Research,2003,22(5): 321-335.

[3] WEISMULLER T, LEINZ M. GNC technology demonstrated by the orbital express autonomous rendezvous and capture sensor system[C]//The 29thAAS Guidance and Control Conference, Breckenridge, CO, USA: AAS, 2006：6-16.

[4] BROOK S, DAVID B. DARPA Phoenix payload orbital delivery (POD) system: “FedEx to GEO”[R]. San Diego, USA: AIAA, 2013.

[5] WALDRON K J， HUNT K H. Series-parallel dualities in actively coordinated mechanisms [J]. The International Journal of Robotics Research, 1991，10(5): 437-480.

[6] ROTH B, WALDRON K J， RAGHAVAN M. Kinematics of a hybrid series-parallel manipulator system [J]. Journal of Dynamic System, Measurement, and Control, 1989，111: 211-221.

[7] ZOPPI M, ZLATANOV D， Molfino R. On the velocity analysis of interconnected chains mechanisms[J]. Mechanism and Machine Theory, 2006，41(11): 1346-1358.

[8] PAPADOPOULOS E. On the dynamics and control of space manipulators [D]. Cambridge:Department of Mechanical Engineering, Massachusetts Institute of Technology, 1990.

[9] SLOTINE J J E, LI W. Composite adaptive control of robot manipulators [J]. Automatica, 1989, 25(4): 509-519.

[10] FOURQUET J Y. Optimal control theory and complexity of the time optimal problem for rigid manipulators [C]// Proceedings of the IEEE/RSJ Conference on Intelligent Robots and Systems, Yokohama, Japan: IEEE Industrial Electronics Society, 1993,84-90.

[11] LATOMBE J C. Robot motion planning [M]. Norwell: Kluwer, 1991：22-37.

[12] SPONG M W， VIDYASAGAR M. Robot dynamics and control [M]. New York, USA: John Willey & Sons, Inc., 1989：57-94.

[13] 張賢達(dá). 矩陣分析與應(yīng)用[M]. 北京: 清華大學(xué)出版社, 2004：341-400.

Trajectory-Planning Algorithm for Series-Parallel Space Manipulator Based on the Full DOFs

WANG Yong1,2, TANG Qiang1,2, XU Shuanfeng1,2, ZHU Zhibin1,2,HE Yingzi1,2, WEI Chunling1,2

(1.BeijingInstituteofControlEngineering，Beijing100190，China；2.ScienceandTechnologyonSpaceIntelligentControlLaboratory，Beijing100190，China)

There are few researches so far focusing on the series-parallel manipulator for space manipulators, where also the trajectory-planning algorithms are usually without resort to the redundant DOFs. This paper proposes a novel trajectory-planning algorithm for a 7 DOFs series-parallel space manipulator, which is based on the analytical solutions of the inverse kinematics, and makes use of all the 7 DOFs. The algorithm enlarges the reachable space of the manipulator, ensures real-time implementation of the planning, and also maintains the high load capacity due to the series-parallel structure. These facilitate the series-parallel space manipulator to capture the targets during the space operation. Through simulations and experiments for space operation on the ground, the efficiency of the algorithm is verified.

space Robot; space operation; trajectory-planning; series-parallel; redundant DOFs

*重點(diǎn)實(shí)驗(yàn)室基金資助項(xiàng)目(9140C59020611HT05).

2015-05-29

TP241.3

A

1674-1579(2015)05-0013-06

10.3969/j.issn.1674-1579.2015.05.003

王 勇(1982—)，男，工程師，博士，研究方向?yàn)榭臻g機(jī)器人在軌操作控制技術(shù)，移動機(jī)器人建圖、定位、路徑規(guī)劃、導(dǎo)航技術(shù)等；唐 強(qiáng)(1981—)，男，工程師，博士，研究方向?yàn)榭臻g機(jī)器人在軌操作控制技術(shù)，高穩(wěn)定度航天器控制技術(shù)，高速、高精度機(jī)器人運(yùn)動控制技術(shù)等；徐拴鋒(1983—)，男，工程師，博士，研究方向?yàn)榭臻g機(jī)器人在軌操作控制技術(shù)，空間機(jī)械臂動力學(xué)與控制技術(shù)等；朱志斌(1981—)，男，高級工程師，博士，研究方向?yàn)榭臻g機(jī)器人在軌操作控制技術(shù)，地面試驗(yàn)驗(yàn)證技術(shù)等.