吳鵬，洪娟，陳廣，徐博

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001； 2.北京航天控制儀器研究所，北京 100854)

串聯(lián)冗余自由度機(jī)構(gòu)關(guān)節(jié)角位置的規(guī)劃算法

吳鵬1，洪娟2，陳廣1，徐博1

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001； 2.北京航天控制儀器研究所，北京 100854)

為了建立一種實(shí)時(shí)求解串聯(lián)冗余自由度機(jī)構(gòu)關(guān)節(jié)角位置的方法，解決串聯(lián)冗余自由度機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)求解中角位置的求解問題，給出了逆運(yùn)動(dòng)學(xué)的數(shù)值解。根據(jù)加權(quán)偽逆理論，采用瞬時(shí)最優(yōu)速度表達(dá)關(guān)節(jié)空間角增量的方法，研究了關(guān)節(jié)空間角位置數(shù)值解法，給出了冗余自由度機(jī)構(gòu)連續(xù)跟蹤期望空間姿態(tài)時(shí)的關(guān)節(jié)空間角位置計(jì)算方法。在典型運(yùn)動(dòng)輸入條件下，分別恒定速比關(guān)節(jié)位置算法和加權(quán)逆速度積分方法獲得的關(guān)節(jié)位置和姿態(tài)誤差進(jìn)行仿真。為驗(yàn)證算法有效性，以衛(wèi)星天線姿態(tài)穩(wěn)定系統(tǒng)為研究對(duì)象，仿真結(jié)果表明，恒定速比關(guān)節(jié)位置算法在運(yùn)動(dòng)學(xué)逆解過程中能夠保證天線姿態(tài)誤差Frobenius(弗羅貝尼烏斯)范數(shù)小于10-4。仿真結(jié)果表明：本文提出的方法能夠連續(xù)給出冗余自由度機(jī)構(gòu)運(yùn)動(dòng)過程中的關(guān)節(jié)空間角位置的希望值，實(shí)現(xiàn)了逆運(yùn)動(dòng)學(xué)角位置層面的數(shù)值解和理想的控制精度。

冗余自由度機(jī)構(gòu)； 衛(wèi)星天線； 加權(quán)偽逆； 逆運(yùn)動(dòng)學(xué)； 運(yùn)動(dòng)規(guī)劃； 恒定轉(zhuǎn)速比； 關(guān)節(jié)角位置； 姿態(tài)穩(wěn)定； 姿態(tài)誤差； Frobenius范數(shù)

船舶在海上航行或無人飛行器執(zhí)行遠(yuǎn)距離任務(wù)時(shí)，與陸地或指揮中心的通信都需要依靠地球同步通信衛(wèi)星。上下行信號(hào)的傳送，需要天線接收主瓣指向衛(wèi)星，同時(shí)天線的極化方向與衛(wèi)星發(fā)射信號(hào)的極化方向需要保持一致[1]。因此天線姿態(tài)穩(wěn)定系統(tǒng)被廣泛使用，實(shí)現(xiàn)衛(wèi)星天線與地球同步衛(wèi)星之間的相對(duì)穩(wěn)定。接收天線主瓣方向與極化方向之間是垂直的，利用這兩個(gè)方向構(gòu)成了一個(gè)來描述天線姿態(tài)的坐標(biāo)系。理論上，坐標(biāo)系在空間的定點(diǎn)轉(zhuǎn)動(dòng)有3個(gè)自由度，具有3個(gè)正交自由度的運(yùn)動(dòng)機(jī)構(gòu)就可以實(shí)現(xiàn)天線姿態(tài)的伺服。現(xiàn)實(shí)中，為了克服運(yùn)動(dòng)機(jī)構(gòu)奇異點(diǎn)問題，增加系統(tǒng)靈活性，采用具有4個(gè)正交運(yùn)動(dòng)軸的框架結(jié)構(gòu)，本文研究對(duì)象是一個(gè)冗余自由度的天線姿態(tài)穩(wěn)定系統(tǒng)。

聯(lián)接天線與運(yùn)載器的穩(wěn)定系統(tǒng)將實(shí)現(xiàn)了天線相對(duì)于運(yùn)載器的姿態(tài)運(yùn)動(dòng)，同時(shí)也建立了天線姿態(tài)與穩(wěn)定系統(tǒng)關(guān)節(jié)空間的約束。對(duì)與任務(wù)空間而言使用n維向量X，X∈Rn描述規(guī)劃的任務(wù)；對(duì)于關(guān)節(jié)空間，使用m維向量Θ，Θ∈Cm描述關(guān)節(jié)變量。當(dāng)m>n時(shí)系統(tǒng)即為冗余自由度系統(tǒng)，可以實(shí)現(xiàn)關(guān)節(jié)的避障、克服奇異點(diǎn)、完成第二任務(wù)等要求，例如實(shí)現(xiàn)機(jī)器人更好的靈活性[2-3]，躲避障礙[4-7]、防止關(guān)節(jié)運(yùn)動(dòng)超界[8-11]、避免奇異性[12-15]、減小關(guān)節(jié)運(yùn)動(dòng)速度及優(yōu)化動(dòng)力學(xué)性能等[16-17]。對(duì)于本文中的姿態(tài)隨動(dòng)系統(tǒng)，當(dāng)系統(tǒng)自由度為m=4時(shí)，形成姿態(tài)冗余自由度系統(tǒng)。對(duì)于冗余自由度機(jī)器人而言，運(yùn)動(dòng)學(xué)逆解有無窮多組。從無窮多組解中選擇合適解的過程，也就是冗余自由度問題的求解過程。成果包括：幾何求解方法[18-19]；Pieper證明了對(duì)于連續(xù)3個(gè)軸交于一點(diǎn)的冗余自由度機(jī)構(gòu)，一定具有解析解[20]；Liegeois及梯度投影法把冗余自由度問題的逆解分為了特解和齊次解，利用關(guān)節(jié)的自運(yùn)動(dòng)實(shí)現(xiàn)冗余自由度機(jī)器人的次任務(wù)[21-23]；Whitney最早采用了這種廣義逆形式來求取一般機(jī)器人的逆運(yùn)動(dòng)學(xué)及軌跡規(guī)劃問題，獲得了關(guān)節(jié)角速度向量的模最小解，也成為基于速度層面求解冗余自由度問題的基礎(chǔ)[24]；Wampler最早提出了加權(quán)最小二乘法實(shí)現(xiàn)主任務(wù)與自運(yùn)動(dòng)的同步完成[25]；Lee和Badler提出了可以選擇合適的關(guān)節(jié)角度作為參數(shù)，表示其余的角度，進(jìn)行解析解的求解方法[26-27]。

以上方法中，除幾何方法和文獻(xiàn)外，主要集中在對(duì)關(guān)節(jié)空間的速度求解，對(duì)于關(guān)節(jié)空間的位置求解研究較少，在對(duì)于機(jī)構(gòu)末端軌跡有高精度要求的應(yīng)用場合，并不適用[19]。

本文提出了一種建立在加權(quán)偽逆速度解基礎(chǔ)上的恒定速比關(guān)節(jié)位置算法，使用該方法直接計(jì)算得到關(guān)節(jié)空間中的角位置值，計(jì)算誤差不累計(jì)，能夠保證天線姿態(tài)在運(yùn)動(dòng)過程中達(dá)到期望精度。

1 姿態(tài)穩(wěn)定系統(tǒng)的描述

運(yùn)載器、天線、穩(wěn)定系統(tǒng)和衛(wèi)星之間相互姿態(tài)關(guān)系與各坐標(biāo)系的定義如圖1所示。圖1中，建立當(dāng)?shù)氐乩碜鴺?biāo)系為東北天坐標(biāo)系XeYeZe；運(yùn)載器坐標(biāo)系為與其固聯(lián)的前右上坐標(biāo)系XbYbZb；與天線固連的坐標(biāo)系為XaYaZa，其中Za為天線接收主波瓣方向，Ya為天線極化方向；建立與衛(wèi)星固聯(lián)的坐標(biāo)系XsYsZs，其中Zs為衛(wèi)星發(fā)射信號(hào)主瓣的相反方向，Ys為衛(wèi)星發(fā)射天線極化方向。

天線姿態(tài)穩(wěn)定問題等效于將天線本體坐標(biāo)系XaYaZa穩(wěn)定在地理坐標(biāo)系XeYeZe中，當(dāng)運(yùn)載器坐標(biāo)系XbYbZb發(fā)生變化時(shí)，通過穩(wěn)定系統(tǒng)的伺服運(yùn)動(dòng)，保證坐標(biāo)系XaYaZa與衛(wèi)星固聯(lián)坐標(biāo)系XsYsZs的一致。

圖1 運(yùn)載器、天線及衛(wèi)星坐標(biāo)系相互關(guān)系描述圖Fig.1 Description of the relationship between carrier, antenna and satellite coordinate system

設(shè)每次一轉(zhuǎn)動(dòng)的姿態(tài)轉(zhuǎn)移矩陣分別為T1、T2、T3和T4，而每一個(gè)姿態(tài)轉(zhuǎn)移矩陣又僅與一個(gè)對(duì)應(yīng)的關(guān)節(jié)角度有關(guān)，得到運(yùn)載器坐標(biāo)系與天線本體坐標(biāo)系之間的姿態(tài)轉(zhuǎn)移矩陣為

bTa=T1·T2·T3·T4=f(θ1,θ2,θ3,θ4)

(1)

式中：θ1～θ4為姿態(tài)穩(wěn)定系統(tǒng)的4個(gè)運(yùn)動(dòng)軸角度值。嘗試建立圖1中各坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，這里用姿態(tài)轉(zhuǎn)移矩陣eTb表示坐標(biāo)系XbYbZb在坐標(biāo)系XeYeZe中的表達(dá)。同理利用姿態(tài)轉(zhuǎn)移矩陣寫出各坐標(biāo)系之間轉(zhuǎn)換關(guān)系為

bTs=bTe·eTs=bTa

(2)

式中：eTs為衛(wèi)星在地理坐標(biāo)系下的表示，對(duì)于給定的地球同步衛(wèi)星目標(biāo)，在地球上某一點(diǎn)觀察衛(wèi)星，可以認(rèn)為eTs保持不變；eTb為運(yùn)載器在地理坐標(biāo)系下的表示；bTa為天線在運(yùn)載器坐標(biāo)系下的表示；運(yùn)載器運(yùn)動(dòng)，eTb發(fā)生變化，系統(tǒng)完成的功能就是調(diào)整θ1～θ4，更新bTa使得(2)式保持成立[1]

bTa=bTe·eTs=N(t)

(3)

式中：將衛(wèi)星在運(yùn)載器坐標(biāo)系下的連續(xù)運(yùn)動(dòng)用矩陣序列N(t)表示。則本文問題轉(zhuǎn)化為如何在如式(4)的約束條件下，進(jìn)行冗余自由度機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)求解。

f(θ1,θ2,θ3,θ4)=N(t)

(4)

矩陣N(t)的值則可通過姿態(tài)的運(yùn)動(dòng)和衛(wèi)星參數(shù)求得：

這里假設(shè)衛(wèi)星方位角度為Ae，俯仰角度為Ee，極化角約定為繞衛(wèi)星姿態(tài)矩陣Zs軸經(jīng)過σe的轉(zhuǎn)動(dòng)后，使得Ys軸與地面平行，此時(shí)的σe為極化角，并約定極化角正方向?yàn)槟鏁r(shí)針方向，且確定極化角時(shí)以坐標(biāo)系的Ys軸正方向?yàn)闇?zhǔn)。ψ為運(yùn)載器航向角，φ為運(yùn)載器的縱搖角，γ為運(yùn)載器的橫搖角。

2 加權(quán)意義下的速度最優(yōu)解

2.1雅克比矩陣的建立

天線姿態(tài)穩(wěn)定是一個(gè)姿態(tài)跟蹤控制過程。將初始條件位置時(shí)刻定義為t0時(shí)刻，此時(shí)的天線姿態(tài)矩陣為N(t0)。隨著運(yùn)載器運(yùn)動(dòng)，在t1時(shí)刻運(yùn)載器坐標(biāo)系下的天線姿態(tài)應(yīng)更新至N(t1)，在任意時(shí)刻姿態(tài)矩陣為N(tn)。根據(jù)機(jī)構(gòu)的約束關(guān)系有：

f(Θn)=N(tn)

(5)

Θn=f-1(N(tn))

(6)

對(duì)于式(6)所描述的冗余自由度運(yùn)動(dòng)學(xué)逆解，f-1(·)代表函數(shù)f(·)的反函數(shù)。利用系統(tǒng)的Moore-Penrose的廣義逆[24](簡稱M-P逆)可求得速度層面的最優(yōu)解，本文中提出的恒定速比關(guān)節(jié)位置算法也是從偽逆速度的角度出發(fā)的。首先建立由雅克比矩陣J描述的關(guān)節(jié)運(yùn)動(dòng)與天線姿態(tài)運(yùn)動(dòng)的關(guān)系為

(7)

2.2天線坐標(biāo)系的轉(zhuǎn)動(dòng)速度

在前節(jié)中已經(jīng)獲得了衛(wèi)星姿態(tài)的描述矩陣eTs，且通過慣性測量單元得到了t時(shí)刻的運(yùn)載器姿態(tài)的歐拉角，求得了eTb。利用式(2)直接得到連續(xù)變化的矩陣序列N(t)。對(duì)于任意相連時(shí)刻的N(tn)和N(tn+1)=N(tn+Δt)有：

N(tn+Δt)=Rot(q,dθ)N(tn)

(8)

式中：

Rot(q,dθ)=Rot(x,δx)Rot(y,δy)Rot(z,δz)

(9)

Rot(q,dθ)代表坐標(biāo)系繞某一個(gè)軸q轉(zhuǎn)動(dòng)了dθ角度，Δt為兩次測量測量之間的時(shí)間間隔；Rot(x,δx)為繞當(dāng)前坐標(biāo)系的x軸轉(zhuǎn)動(dòng)δx的微分運(yùn)動(dòng)，相應(yīng)地Rot(y,δy)和Rot(z,δz)為繞另外兩個(gè)軸的微分運(yùn)動(dòng)。當(dāng)δx、δy和δz較小時(shí)，忽略二階小量可得

(10)

2.3瞬時(shí)關(guān)節(jié)最優(yōu)速度求解

許多文獻(xiàn)針對(duì)冗余自由度機(jī)構(gòu)在速度層面的解進(jìn)行了深入的探討，其中具有加權(quán)M-P逆解具有各關(guān)節(jié)同步運(yùn)動(dòng)，無關(guān)節(jié)自運(yùn)動(dòng)的特點(diǎn)，本文選用此方法作為關(guān)節(jié)速度的逆解方法[25]。對(duì)于四軸穩(wěn)定系統(tǒng)，第1～4軸所消耗的能量逐漸減小，按照權(quán)值系數(shù)逐步降低的原則為轉(zhuǎn)動(dòng)關(guān)節(jié)分配轉(zhuǎn)動(dòng)權(quán)值，建立權(quán)值向量

(11)

此時(shí)的關(guān)節(jié)速度可表示為

(12)

3 恒定速比位置更新方法

3.1姿態(tài)跟蹤系統(tǒng)的特點(diǎn)

姿態(tài)跟蹤系統(tǒng)處在另外一種情況，一方面對(duì)于末端姿態(tài)精度有較高要求；另一方面，在給定一個(gè)期望姿態(tài)的情況下，希望系統(tǒng)夠以最快的速度實(shí)現(xiàn)姿態(tài)更新，實(shí)現(xiàn)對(duì)目標(biāo)位置。

對(duì)于姿態(tài)跟蹤系統(tǒng)，姿態(tài)測量單元每秒更新20～50次姿態(tài)信息，每個(gè)更新的姿態(tài)信息對(duì)應(yīng)姿態(tài)矩陣序列N(t)中的一個(gè)矩陣。每當(dāng)新的姿態(tài)矩陣出現(xiàn)，要求跟蹤系統(tǒng)能夠盡可能快速地實(shí)現(xiàn)天線姿態(tài)與期望姿態(tài)N(t)的一致。對(duì)于兩個(gè)姿態(tài)矩陣之間的姿態(tài)轉(zhuǎn)移過程則不關(guān)注，不需要對(duì)兩個(gè)姿態(tài)之間的軌跡做精確規(guī)劃，而是將關(guān)注的重點(diǎn)放在對(duì)應(yīng)每一個(gè)期望姿態(tài)所對(duì)應(yīng)的關(guān)節(jié)角上，即對(duì)式(6)快速求解的方法，獲得運(yùn)動(dòng)學(xué)逆解。

3.2現(xiàn)有方法的不足

2)姿態(tài)跟蹤系統(tǒng)的控制不同于離線規(guī)劃或示教方式規(guī)劃的機(jī)器人系統(tǒng)，后者可以根據(jù)精度要求詳細(xì)地規(guī)劃軌跡上的每一個(gè)點(diǎn)。姿態(tài)跟蹤系統(tǒng)關(guān)注的是期望姿態(tài)的快速建立和姿態(tài)建立精度，而對(duì)于給定的期望姿態(tài)如何直接求解關(guān)節(jié)的角位置尚沒有成熟的方法。雖然可以建立姿態(tài)誤差的反饋，多次計(jì)算偽逆，直至末端位姿滿足精度要求。但計(jì)算量大，收斂速度慢，不適合進(jìn)行在線規(guī)劃。

3.3恒定速比位置算法

本文提出了一種基于關(guān)節(jié)最優(yōu)速度比恒定和姿態(tài)約束方程的關(guān)節(jié)位置算法，簡稱為恒定速比(CRR)位置算法。

對(duì)于給定末端坐標(biāo)系運(yùn)動(dòng)速度，基于式(12)可獲得關(guān)節(jié)瞬時(shí)最優(yōu)速度。冗余自由度最優(yōu)速度意義本質(zhì)上是將末端姿態(tài)運(yùn)動(dòng)合理地分解到各運(yùn)動(dòng)關(guān)節(jié)。角位移是有角速率和運(yùn)動(dòng)時(shí)間決定的，在相同的采樣時(shí)間間隔上，如果角速率保持不變，那么關(guān)節(jié)空間角位移將正比于角速率。各軸的瞬時(shí)速率值，也代表了各軸瞬時(shí)角度運(yùn)動(dòng)量的大小。各關(guān)節(jié)瞬時(shí)速度之比，即瞬時(shí)關(guān)節(jié)運(yùn)動(dòng)量之比。將姿態(tài)轉(zhuǎn)移過程中關(guān)節(jié)運(yùn)動(dòng)速度比設(shè)定為恒定，即恒定速比，是本文算法的核心思想。將每一個(gè)關(guān)節(jié)的運(yùn)動(dòng)速度通過某一個(gè)關(guān)節(jié)運(yùn)動(dòng)速度和關(guān)節(jié)之間速度的比值關(guān)系表示，并設(shè)定這一速比關(guān)系在每一次姿態(tài)轉(zhuǎn)移過程中保持恒定。通過這樣的表達(dá)，可以將采樣周期內(nèi)關(guān)節(jié)角位置增量表達(dá)出來，將這個(gè)待求角度增量的表達(dá)式直接帶入姿態(tài)約束方程，求得每個(gè)關(guān)節(jié)角的增量值，進(jìn)而獲得關(guān)節(jié)角位置。

將此方法應(yīng)用于天線姿態(tài)穩(wěn)定系統(tǒng)，求解關(guān)節(jié)角位置的步驟為：

1)根據(jù)衛(wèi)星的參數(shù)，計(jì)算矩陣eTs；

2)與運(yùn)載器固連的姿態(tài)測量單元檢測到運(yùn)載器的運(yùn)動(dòng)姿態(tài)，利用歐拉角ψ、φ和γ描述航向角、縱搖角和橫搖角，計(jì)算得到bTe，結(jié)合eTs求得此時(shí)衛(wèi)星在運(yùn)載器坐標(biāo)系下的姿態(tài)矩陣記為N(tn+1)，相應(yīng)的前一時(shí)刻的姿態(tài)矩陣記為N(tn);

3)在工作空間中進(jìn)行運(yùn)動(dòng)微分的提取。計(jì)算連續(xù)兩個(gè)位姿轉(zhuǎn)動(dòng)之間的差值，即兩位姿之間的姿態(tài)轉(zhuǎn)移矩陣為

N(tn+1)=Rot(q,dθ)N(tn)

(13)

(14)

5)根據(jù)建立的關(guān)節(jié)空間變量與工作空間之間的關(guān)系，在第n次姿態(tài)測量的時(shí)刻，有：

f(Θn)=N(tn)

(15)

那么對(duì)于tn+1有：

f(Θn+1)=f(Θn+ΔΘ)=

f(Θn+Φ·Δt)=N(tn+1)

(16)

6)建立恒等速比的關(guān)系式為

(17)

利用第一軸表示其余軸的速度，有：

(18)

式中:k2、k3、k4為關(guān)節(jié)速度比例系數(shù)，在單次關(guān)節(jié)位置計(jì)算過程中保持不變，即保持恒定速比。關(guān)節(jié)增量表達(dá)式可寫為

(19)

7)將式(18)的增量表達(dá)式代入式(15)的方程中，將Θ作為初始，利用牛頓迭代方法即可獲得ξ1。將ξ1代入式(19)，求得ΔΘ。

將算法總結(jié)為流程圖，如圖2所示。

圖2 恒定速比位置更新算法流程圖Fig.2 Flow chart of CRR position update algorithm

4 姿態(tài)穩(wěn)定系統(tǒng)運(yùn)動(dòng)仿真結(jié)果

4.1仿真條件

本文為了驗(yàn)證算法在運(yùn)載器大動(dòng)態(tài)特性下的結(jié)果，設(shè)定了一個(gè)跟蹤目標(biāo)，并仿真運(yùn)載器的運(yùn)動(dòng)規(guī)律，觀察關(guān)節(jié)空間的運(yùn)動(dòng)情況及末端位姿的誤差。仿真時(shí)間間隔為ΔT=0.01 s。

對(duì)跟蹤目標(biāo)做如下假設(shè)：目標(biāo)方位角度：223.49°，俯仰角：60°，極化角度：-28.8°。按照本文中建模規(guī)則，初始位置下目標(biāo)對(duì)應(yīng)的關(guān)節(jié)角度為

(20)

運(yùn)載器的航向角、縱橫搖角運(yùn)動(dòng)組照如下的描述為

ψ=40sin[(2π/30 000)t]+0.01t

φ=40sin[(2π/800)t]

γ=45sin[(2π/1 000)t]

(21)

4.2偽逆方法仿真結(jié)果

按照仿真條件，分別使用恒等速比方法和加權(quán)M-P逆方法進(jìn)行了穩(wěn)定系統(tǒng)關(guān)節(jié)角、關(guān)節(jié)角速度和姿態(tài)誤差的計(jì)算。

直接利用關(guān)節(jié)角速度積分或的關(guān)節(jié)角位置，如圖4所示。

將計(jì)算得到的各關(guān)節(jié)角度帶入運(yùn)動(dòng)學(xué)方程計(jì)算，獲得末端的姿態(tài)矩陣，將此姿態(tài)矩陣與期望姿態(tài)矩陣進(jìn)行做差，計(jì)算差矩陣的Frobenius范數(shù)值，如圖5所示。

可以看到單純利用偽逆速度積分獲得的末端姿態(tài)矩陣誤差會(huì)快速發(fā)散。

使用恒定速比方法進(jìn)行計(jì)算，計(jì)算結(jié)果為每一步的關(guān)節(jié)角更新位置，直接獲得關(guān)節(jié)角位置變化曲線如圖6所示。

圖3 根據(jù)M-P逆求得的各軸速度值Fig.3 The solution of joints velocity based on the M-P inverse

圖4 對(duì)M-P逆生成速度進(jìn)行積分得到的角度值Fig.4 The joints values obtained by using M-P inverse velocity integral

圖5 姿態(tài)誤差矩陣Frobenius范數(shù)值Fig.5 Frobenius norm values of attitude error matrix

本文解決的重點(diǎn)在關(guān)節(jié)空間角位置的計(jì)算，暫時(shí)不考慮關(guān)節(jié)的限位問題。在恒定速比方法求解過程中每一步計(jì)算得到的M-P逆速度如圖7所示。

同樣地根據(jù)各關(guān)節(jié)角度值進(jìn)行運(yùn)動(dòng)學(xué)正計(jì)算，將獲得姿態(tài)與期望姿態(tài)進(jìn)行做差，求得誤差矩陣的Frobenius范數(shù)曲線如圖8所示。由于是利用恒速比方法直接求得的位置值，關(guān)節(jié)角的速度值是通過位置差分得到的平均速度值，差分結(jié)果如圖9所示。

圖6 利用本文的恒速比方法獲得角度值Fig.6 The joints values obtained by using RCC approach

圖7 恒速比方法中的各關(guān)節(jié)的M-P逆轉(zhuǎn)速值Fig.7 The M-P inverse velocity of joints in RCC approach

圖8 姿態(tài)誤差矩陣Frobenius范數(shù)值Fig.8 Frobenius normal values of attitude error matrix

圖9 恒定速比方法中位置差分得到的速度值Fig.9 The velocity of joints obtained by positiondifference in RCC approach

從圖9誤差結(jié)果可以看出，利用恒定速比方法計(jì)算關(guān)節(jié)角位置，機(jī)構(gòu)的末端姿態(tài)誤差不累加。雖然在某些位置出現(xiàn)誤差增加點(diǎn)，但整體末端姿態(tài)始終保持較高精度。同時(shí)，姿態(tài)精度取決于算法中牛頓迭代的精度，換而言之，根據(jù)末端姿態(tài)精度要求的不同，可以靈活選擇牛頓迭代法的精度。

5 結(jié)論

1)建立了具有4個(gè)運(yùn)動(dòng)自由度的天線穩(wěn)定系統(tǒng)運(yùn)動(dòng)模型，給出了天線運(yùn)動(dòng)、運(yùn)載器運(yùn)動(dòng)及目標(biāo)衛(wèi)星之間的關(guān)系；給出了天線姿態(tài)矩陣序列的計(jì)算方法。

2)分析了現(xiàn)有的冗余自由度運(yùn)動(dòng)學(xué)逆解方法在軌跡跟蹤方面的不足。提出了一種基于關(guān)節(jié)恒定速比的關(guān)節(jié)位置算法，給出了算法的流程圖。

3)對(duì)基于M-P逆的速度求解方法和本文提出的方法進(jìn)行了仿真驗(yàn)證，仿真結(jié)果表明恒定速比方法得到的關(guān)節(jié)角，保證了系統(tǒng)末端姿態(tài)在運(yùn)動(dòng)過程中具有足夠的精度。

[1] 吳鵬. 船用衛(wèi)星天線姿態(tài)穩(wěn)定系統(tǒng)關(guān)鍵技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué), 2012： 17-23.

WU Peng. Key technologies of marine satellite antenna attitude stabilization system[D]. Harbin: Harbin Engineering University, 2012: 17-23.

[2] GUO Dongsheng, ZHANG Yunong. Acceleration-level inequality-based MAN scheme for obstacle avoidance of redundant robot manipulators[J]. IEEE transactions on industrial electronics, 2014, 61(12): 6903-6914.

[3] ZHANG Y, LI W, LIAO B,et al. Analysis and verification of repetitive motion planning and feedback control for omnidirectional mobile manipulator robotic systems[J]. Journal of intelligent & robotic systems, 2014,75 (3-4): 393-411.

[4] CAPISANI L M, FERRARA A. Trajectory planning and second-order sliding mode motion/interaction control for robot manipulators in unknown environments[J]. IEEE trans ind electron, 2012, 59(8): 3189-3198.

[5] XIAO L, ZHANG Y. A new performance index for the repetitive motion of mobile manipulators[J]. IEEE trans cybern, 2014,44(2)： 280-292.

[6] FLACCO F, DE LUCA A, KHATIB O. Control of redundant robots under hard joint constraints: saturation in the null space[J]. IEEE transactions on robotics, 2015, 31(3): 637-654.

[7] CHEN Dechao, ZHANG Yunong. A hybrid multi-objective scheme applied to redundant robot manipulators[J]. IEEE transactions on automation science and engineering, 2017，14(3)：1337-1350.

[8] FLACCO F, DE LUCA A. Optimal redundancy resolution with task scaling under hard bounds in the robot joint space[C]//Proc IEEE Int Conf Robot Seoul Korea, 2013: 3969-3975.

[9] ZHANG Dongsheng, XU Yundou , YAO Jiantao,et al. Kinematics, dynamics and stiffness analysis of a novel 3-DOF kinematically/actuation redundant planar parallel mechanism [J]. Mechanism and machine theory, 2017, 116:203-219.

[10] KUMAR A, KUMAR V. Evolving an interval type-2 fuzzy PID controller for the redundant robotic manipulator [J]. Expert systems with applications , 2017,73:161-177.

[11] KIM J, MARANI G, CHUNG W K, et al. A general singularity avoidance framework for robot manipulators: task reconstruction method[C]//IEEE International Conference on Robotics and Automation, New Orleas, 2004, 5: 4809-4814.

[12] MARANI G, KIM J, YUH J, et al. A real-time approach for singularity avoidance in resolved motion rate control of robotic manipulators[C]//IEEE International Conference on Robotics and Automation, Washington DC, 2002, 2: 1973-1978.

[13] ZORJAN M, HUGEL V. Generalized humanoid leg inverse kinematics to deal with singularities[C]∥IEEE International Conference on Robotics and Automation Karlsruhe Germany, 2013: 4791-4796.

[14] JAKUBIAK J, RATAJCZAK A. Kinematics and motion planning of the multi-bar system[J]. Journal of intelligent and robotic systems, 2014, 75(1): 5-15.

[15] RANDY C H, RODNEY G R, ANTHONY A M, et al. Designing a failure-tolerant workspace for kinematically redundant robots[J]. IEEE transactions on automation science and engineering, 2015, 12(4): 1421-1432.

[16] ZHANG Yunong, ZHANG Yinyan, CHEN Dechao, et al. From davidenko method to zhang dynamics for nonlinear equation systems solving[C]∥IEEE Transactions on Systems, Man, and Cybernetics: Systems. Budapest Hungary, 2016.

[17] LI S, LI Y. Nonlinearly activated neural network for solving timevarying complex Sylvester equation[J]. IEEE trans. cybern, 2014, 44(8): 1397-1407.

[18] ASFOUR T, DILLMANN R. Human-like motion of a humanoid robot arm based on a closed-form solution of the inverse kinematics problem[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems. Las Vegas Nevada, 2003, 2: 1407-1412.

[19] 王英石.冗余機(jī)器人的運(yùn)動(dòng)學(xué)及軌跡規(guī)劃的研究[D].天津：南開大學(xué), 2014.

WANG Yingshi. Research on the kinematics and trajectory planning of redundant robots[D].Tianjin: Nankai University, 2014.

[20] PEIPER D L. The kinematics of manipulators under computer control[R]. Stanford Univ Calif Dept of Computer Science, 1968: 32-66.

[21] LIEGEOIS A. Automatic supervisory control of the configuration and behavior of mutibody mechanisms[J]. IEEE trans sys man cyber,1977, SMC-7(12): 868-871.

[22] JIN Hongzhe, ZHANG Hui, LIU Zhangxing,et al. A synthetic algorithm for tracking a moving object in a multiple-dynamic obstacles environment based on kinematically planar redundant manipulators [J]. Mathematical problems in engineering, 2017(2): 1-15.

[23] DUBEY R V, EULER J A, BABCOCK S M. An efficient gradient projection optimization scheme for a seven-degree-of-freedom redundant robot with spherical wrist[C]∥IEEE international conference on robotics and automation, philadelphia, PA, USA, 1988: 28-36.

[24] WHITNEY D E. The mathematics of coordinated control of prosthetic arms and manipulators[J]. ASME journal of dynamic systems, measurement, and control, 1972, 94(4): 303-309.

[25] WAMPLER C W. Manipulator inverse kinematic solutions based on formulations and damped least-squares methods[J]. IEEE transactions on systems man and cybernetics, 1986, 16(1): 93-101.

[26] CHEN Yujen, JU Mingyi, HWANG Kaoshing. A Virtual torque-based approach to kinematic control of redundant manipulators[J]. IEEE Transactions on industrial electronics, 2017, 64(2): 1728-1736.

[27] BADLER N I, TOLANI D. Real-time inverse kinematics of the human arm[J]. Center for human modeling and simulation, 1996, 73: 392-401.

本文引用格式：吳鵬，洪娟，陳廣，等. 串聯(lián)冗余自由度機(jī)構(gòu)關(guān)節(jié)角位置的規(guī)劃算法[J]. 哈爾濱工程大學(xué)學(xué)報(bào)， 2017， 38(10): 1623 -1629，1641.

WU Peng, HONG Juan, CHEN Guang, et al. Joints position planning approach for series redundant DOF mechanisms[J]. Journal of Harbin Engineering University， 2017， 38(10): 1623 -1629，1641.

JointanglepositionplanningapproachforseriesredundantDOFmechanisms

WU Peng1, HONG Juan2, CHEN Guang1, XU Bo1

(1.College of Automation, Harbin Engineering University, Harbin 150001, China; 2.Beijing Institute of Aerospace Control Device, Beijing 100854, China)

This paper proposes a method that can solve the joint angle position of a mechanism with series redundant DOFs in real time, solve the problem of attaining angular position in the inverse kinematics of the mechanism with series redundancy, and provide the numerical solution of inverse kinematics. According to weighted pseudo-inverse theory, this paper adopted the instantaneous optimum speed to represent the angle increment of joint space, studied the numerical solution of the joint space position, and presented the computing method for the joint space's angle position as the redundant DOF mechanism tracks the desired space attitude continuously. Under typical motion input conditions, the joint position and attitude errors obtained by constant speed ratio joint position algorithm and weighted inverse velocity integral method were simulated. To verify the effectiveness of the algorithm, the satellite antenna attitude stabilization system was taken as the research target. Simulation results show that the constant speed ratio joint position algorithm can ensure that the attitude error of the antenna Fresenius norm is less than 10-4in the process of inverse kinematic solution. Simulation results show that the proposed method can give the desired value of joint space position continuously in the movement process of a mechanism with redundant DOFs. The numerical solution of the position of the inverse kinematics can be realized, and control precision is ideal.

redundant manipulators; satellite antenna; weighted pseudo inverse; inverse kinematics; motion planning; constant rotation ratio (CRR); joint position; attitude stabilization; attitude error; Frobenius norm

10.11990/jheu.201607014

http://www.cnki.net/kcms/detail/23.1390.U.20170816.1550.042.html

TP241.3

A

1006-7043(2017)10-1623-08

2016-07-05. < class="emphasis_bold">網(wǎng)絡(luò)出版日期

日期：2016-08-16.

國家自然科學(xué)基金項(xiàng)目(61203225，61633008)；黑龍江省自然科學(xué)基金項(xiàng)目(QC2014C0669)；中央高校專項(xiàng)基金項(xiàng)目(HEUCF041702)；海洋工程國家重點(diǎn)實(shí)驗(yàn)室開放課題項(xiàng)目(1616).

吳鵬(1978-)，男，講師，博士；徐博(1984-)，男，副教授，研究生.

徐博，E-mail:xubocarter@sina.com.