李文龍，孔祥龍，2，裘 俊，馬 偉，張曉靜

（1.上海衛(wèi)星工程研究所，上海 200240；2.哈爾濱工業(yè)大學(xué) 航天學(xué)院，黑龍江 哈爾濱 150001；3.上海交通大學(xué) 航空航天學(xué)院，上海 200240）

0 引言

在紛繁復(fù)雜的空間任務(wù)中，空間機(jī)械臂越來(lái)越多地得到應(yīng)用。由于太空環(huán)境具有潛在的致命危險(xiǎn)，加上經(jīng)常需要進(jìn)行重復(fù)的操作，因此，空間機(jī)械臂成為空間操作任務(wù)中不可或缺的工具［1］。機(jī)械臂的運(yùn)動(dòng)學(xué)規(guī)劃問(wèn)題一直是研究的熱點(diǎn)［2-5］。對(duì)于自由漂浮空間機(jī)械臂系統(tǒng)來(lái)說(shuō)，系統(tǒng)的線動(dòng)量和角動(dòng)量都是守恒的，與固定基座機(jī)械臂系統(tǒng)不同的是，機(jī)械臂運(yùn)動(dòng)與基座運(yùn)動(dòng)是相互耦合的。也就是說(shuō)，由于系統(tǒng)需要遵循動(dòng)量守恒原理，那么機(jī)械臂動(dòng)量的改變勢(shì)必引起基座動(dòng)量的變化［2-3］。機(jī)械臂每一根桿的運(yùn)動(dòng)都會(huì)產(chǎn)生反作用力和反作用力矩，通過(guò)各個(gè)關(guān)節(jié)傳遞到基座，從而引起基座的平動(dòng)和轉(zhuǎn)動(dòng)，為了確保機(jī)械臂操作過(guò)程中的安全性和精確性，基座的受擾運(yùn)動(dòng)是我們所不期望的。例如，抓捕過(guò)程中需要確保抓捕目標(biāo)始終位于飛行器視場(chǎng)范圍內(nèi)［6］。此外，基座的位置和方向會(huì)影響末端執(zhí)行器的位置和方向，從而使得機(jī)械臂末端不能精確跟蹤事先規(guī)劃好的路徑，末端位置的變化也就可能導(dǎo)致機(jī)械臂不能完成抓捕任務(wù)［7］。

為了使得在機(jī)械臂運(yùn)動(dòng)過(guò)程中航天器的位置和姿態(tài)都保持不變，那么需要消除或補(bǔ)償所有的干擾力和干擾力矩。對(duì)于航天器的姿態(tài)運(yùn)動(dòng)來(lái)講，有兩種方法：第一種是利用主動(dòng)姿態(tài)控制系統(tǒng)。姿態(tài)控制系統(tǒng)通過(guò)施加主動(dòng)控制力矩從而使得航天器姿態(tài)相對(duì)于慣性坐標(biāo)系保持不變。然而，在實(shí)際的空間機(jī)械臂操作過(guò)程中，姿態(tài)推力控制系統(tǒng)通常處于關(guān)閉狀態(tài)［8］，一個(gè)主要原因是為了避免助推器突然點(diǎn)火時(shí)機(jī)械臂末端執(zhí)行器與目標(biāo)發(fā)生碰撞，如果依賴飛輪等執(zhí)行機(jī)構(gòu)，由于操作過(guò)程中機(jī)械臂動(dòng)態(tài)變化較大，基座飛輪易飽和需卸載，需姿控推力系統(tǒng)介入；另一個(gè)主要原因則是為了節(jié)省有限的空間燃料。考慮到這些原因，就需要采取其他的減小姿態(tài)干擾的方法。第二個(gè)方法可以通過(guò)純機(jī)械反作用補(bǔ)償?shù)姆绞?，或者通過(guò)機(jī)械臂自身的運(yùn)動(dòng)來(lái)實(shí)現(xiàn)對(duì)載體航天器姿態(tài)的零干擾。通過(guò)機(jī)械反作用補(bǔ)償存在一系列的缺點(diǎn)，如極大地增加了系統(tǒng)質(zhì)量，且其補(bǔ)償能力也是有限的［9］。為此，通過(guò)合理規(guī)劃?rùn)C(jī)械臂運(yùn)動(dòng)路徑，使其對(duì)基座干擾達(dá)到最小，這個(gè)理念越來(lái)越受到研究人員的重視［10-12］。

DUBOWSKY 等［13］在VAFA 等［9］提出的干擾圖法的基礎(chǔ)上，對(duì)多自由度機(jī)械臂系統(tǒng)提出了增強(qiáng)干擾圖法。該方法同樣適用于冗余機(jī)械臂，其缺點(diǎn)是計(jì)算量大，占用內(nèi)存高。戈新生等［14］結(jié)合各種控制理論與空間機(jī)械臂系統(tǒng)的非完整特性，對(duì)空間機(jī)械臂的軌跡進(jìn)行研究，利用機(jī)械臂運(yùn)動(dòng)控制載體姿態(tài)。付宜利等［15］分析了載體姿態(tài)無(wú)擾情況下機(jī)械臂的運(yùn)動(dòng)特性，分析了其可達(dá)空間。QUINN 等［16］針對(duì)冗余機(jī)械臂系統(tǒng)提出一種優(yōu)化算法，使得機(jī)械臂運(yùn)動(dòng)引起的反作用影響最小，其性能指標(biāo)選取的是基座反作用力的平方加權(quán)，分別采用了局部和全局最優(yōu)法求解問(wèn)題。但是，以上方法并不是基于零反運(yùn)動(dòng)解析解的。從運(yùn)動(dòng)學(xué)的角度，NENCHEV等［17］針對(duì)自由漂浮基空間機(jī)械臂系統(tǒng)提出一種零反作用控制方法，其反作用零空間具有解析形式。以此為基礎(chǔ)，PIERSIGILLI［18］和PUTILOVA［19］考慮冗余機(jī)械臂抓捕移動(dòng)目標(biāo)任務(wù)中機(jī)械臂最優(yōu)路徑規(guī)劃問(wèn)題，但未考慮關(guān)節(jié)約束問(wèn)題，且采用的是間接法求解。間接法存在求解過(guò)程復(fù)雜、收斂域小、對(duì)初值估計(jì)精度要求高的缺點(diǎn)，尤其是對(duì)有路徑約束的問(wèn)題的求解有一定困難。高斯偽譜法［20］作為一種直接法具有求解精度高，且適用于具有各種帶有動(dòng)力學(xué)約束、路徑約束和邊界條件約束的非線性最優(yōu)控制問(wèn)題的特點(diǎn)，在工程應(yīng)用中已獲取了極大的成功。

本文針對(duì)已有研究存在的不足，提出一種基于冗余機(jī)械臂反作用零空間的最優(yōu)路徑規(guī)劃算法。采用高斯偽譜法求解最優(yōu)路徑問(wèn)題，在零反作用軌跡的解集中尋找滿足邊界條件和路徑約束的使性能指標(biāo)最優(yōu)的機(jī)械臂運(yùn)動(dòng)規(guī)律。最后，通過(guò)數(shù)值仿真分析得到運(yùn)動(dòng)對(duì)基座姿態(tài)的干擾，以驗(yàn)證其有效性。

1 機(jī)械臂零反運(yùn)動(dòng)

1.1 機(jī)械臂運(yùn)動(dòng)學(xué)方程

空間鏈?zhǔn)饺哂鄼C(jī)械臂系統(tǒng)如圖1 所示，系統(tǒng)由1 個(gè)基座航天器和n節(jié)剛性桿組成。圖中，S0為基座本體系，SI為慣性系，Si（i=1，…，n）為臂桿連體坐標(biāo)系，ro為基座質(zhì)心位置矢量，rg為系統(tǒng)質(zhì)心位置矢量，rog為基座質(zhì)心到系統(tǒng)質(zhì)心矢量，pi為臂桿i關(guān)節(jié)位置矢量，ai為臂桿i質(zhì)心在其連體系中位置矢量，為臂桿i角速度矢量。

假設(shè)系統(tǒng)中各體為剛體，系統(tǒng)不受任何外力和外力矩作用，不考慮任何動(dòng)量交換裝置，如反作用飛輪、控制力矩陀螺等。基于這些假設(shè)，空間機(jī)械臂系統(tǒng)滿足線動(dòng)量和角動(dòng)量方程，也即線動(dòng)量P和角動(dòng)量L為常值，可表示為

式中：M為航天器系統(tǒng)質(zhì)量；E為單位矩陣；Hω為系統(tǒng)慣量；JTω為機(jī)械臂雅克比矩陣；Hωφ為基座與機(jī)械臂的耦合慣量；v0為基座線速度；ω0為基座角速度。

1.2 機(jī)械臂零反運(yùn)動(dòng)解

由于本文只考慮基座的姿態(tài)運(yùn)動(dòng)而不考慮其平動(dòng)，因此，不失一般性，假設(shè)系統(tǒng)線動(dòng)量P=0，此時(shí)式（1）中角動(dòng)量方程部分可寫(xiě)為

從式（3）可以看出，為了滿足機(jī)械臂運(yùn)動(dòng)對(duì)基座姿態(tài)的干擾為零，則必須使得反作用力矩Fm=0，也就要求耦合動(dòng)量Lm為常值。考慮到Hωφ∈Rm×n，其中，m為基座自由度，n為機(jī)械臂關(guān)節(jié)自由度，當(dāng)機(jī)械臂滿足運(yùn)動(dòng)學(xué)冗余，也即n＞m時(shí)，滿足耦合動(dòng)量Lm=const 的關(guān)節(jié)運(yùn)動(dòng)的解為

式中：E∈Rn×n為n階單位矩陣；（.）+表示矩陣偽逆；ξ為任意n維向量。

不失一般性，假設(shè)Lm=0，則有

將式（5）對(duì)時(shí)間積分，則可得到零反作用機(jī)械臂在關(guān)節(jié)空間中的運(yùn)動(dòng)路徑，所有滿足該條件的路徑稱為零反路徑集。

2 最優(yōu)路徑規(guī)劃

由式（5）可以看出，零反運(yùn)動(dòng)關(guān)節(jié)角速度由零空間投影算子和任意n維向量ξ共同決定。零空間投影算子僅與機(jī)械臂固有的質(zhì)量特性和關(guān)節(jié)構(gòu)型相關(guān)。因此，為了確定，需要設(shè)計(jì)ξ，一旦ξ給定，那么整個(gè)機(jī)械臂在其關(guān)節(jié)空間中的運(yùn)動(dòng)路徑也就確定了。選取ξ的方法有很多，本文考慮關(guān)節(jié)角（機(jī)構(gòu)物理限制）和關(guān)節(jié)角加速度（對(duì)應(yīng)電機(jī)執(zhí)行能力）約束，尋找一組滿足給定性能指標(biāo)的最優(yōu)解。

定義狀態(tài)量x=［ξTφT］T，控制變量為

取關(guān)節(jié)角約束為

式中：φmin=［φ1minφ2min…φnmin］T；φmax=［φ1maxφ2max…φnmax］T。將式（5）對(duì)時(shí)間求導(dǎo)，可得

取關(guān)節(jié)角加速度約束為

結(jié)合式（5）和式（6），優(yōu)化問(wèn)題的動(dòng)力學(xué)方程可以寫(xiě)為

式中：

接下來(lái)將針對(duì)不同的目標(biāo)函數(shù)，采用高斯偽譜法求解最優(yōu)問(wèn)題。

2.1 高斯偽譜法

高斯偽譜法是求解連續(xù)Bolza 型問(wèn)題的直接法中配點(diǎn)法的一種，其基本原理是在一系列離散的Legendre Gauss 節(jié)點(diǎn)上，采用Lagrange 全局插值多項(xiàng)式來(lái)近似狀態(tài)變量和控制變量。然后，通過(guò)對(duì)插值多項(xiàng)式的求導(dǎo)來(lái)近似動(dòng)力學(xué)方程中狀態(tài)變量對(duì)時(shí)間的導(dǎo)數(shù)，并使得離散點(diǎn)處滿足動(dòng)力學(xué)方程的右函數(shù)約束條件。性能指標(biāo)中的積分項(xiàng)采用近似精度最高的Gauss 積分來(lái)近似。經(jīng)過(guò)一系列變換，從而將本文的最優(yōu)路徑規(guī)劃問(wèn)題轉(zhuǎn)化具有一系列代數(shù)約束的離散的非線性規(guī)劃問(wèn)題。

因此，本文的最優(yōu)路徑規(guī)劃問(wèn)題可以描述為：求解離散狀態(tài)變量xi、控制變量ui，使對(duì)應(yīng)的性能指標(biāo)達(dá)到最小。

2.2 時(shí)間最優(yōu)解

時(shí)間最優(yōu)路徑規(guī)劃問(wèn)題可描述為尋找控制變量u的一組解，使得具有形如

目標(biāo)函數(shù)在滿足如下約束條件時(shí)取最小值（式中t0為初始時(shí)刻，tf為終端時(shí)刻）：1）動(dòng)態(tài)約束見(jiàn)式（10）；2）邊界條件見(jiàn)式（7）；3）路徑約束見(jiàn)式（9）；4）初始條件為初始時(shí)刻t0=0，關(guān)節(jié)角φ(t0)=φ0；5）終端約束為φ(tf)=φf(shuō)。

2.3 沿軌跡最小加速度解

沿軌跡最小加速度最優(yōu)路徑規(guī)劃問(wèn)題可描述為給定終端時(shí)刻tf，尋找控制變量u的一組解，使得具有形如

目標(biāo)函數(shù)在滿足2.2 節(jié)中約束條件1）～5）的同時(shí)取最小值。

3 仿真算例

平面3 自由度自由漂浮空間機(jī)械臂系統(tǒng)如圖2所示，由于只考慮基座的姿態(tài)運(yùn)動(dòng)，此時(shí)基座自由度m=1，機(jī)械臂自由度n=3，機(jī)械臂冗余自由度為n-m＞0，滿足零反運(yùn)動(dòng)必要條件。機(jī)械臂和基座的質(zhì)量特性參數(shù)、幾何參數(shù)、初始構(gòu)型以及期望達(dá)到的終端構(gòu)型（見(jiàn)表1），在表1 中詳細(xì)列出，給定的關(guān)節(jié)角約束以及關(guān)節(jié)角加速度約束同樣在表1 中給出。

圖2 平面3 自由度冗余空間機(jī)械臂Fig.2 Planar redundant space manipulator with 3 degrees of freedom

在仿真過(guò)程中，采用由弗洛里達(dá)大學(xué)的Rao等［21-23］開(kāi)發(fā)開(kāi)源免費(fèi)軟件包GPOPS（Gauss Pseudospectral Optimization Software）對(duì)最優(yōu)問(wèn)題進(jìn)行求解。初始時(shí)刻姿態(tài)角和角速度都為0，給出了針對(duì)兩個(gè)性能指標(biāo)的仿真結(jié)果。

3.1 時(shí)間最優(yōu)仿真結(jié)果

設(shè)置高斯節(jié)點(diǎn)數(shù)變化區(qū)間為4～12，容許偏差為10-4，求解過(guò)程中目標(biāo)函數(shù)的梯度信息和約束方程的雅克比矩陣求解方式選為“有限差分”法，狀態(tài)變量和控制變量的初始猜測(cè)值可在各自的約束區(qū)間內(nèi)任意選取?？紤]到關(guān)節(jié)和關(guān)節(jié)角加速度約束，選取ξ的上下限分別為［0.5 0.5 0.5］T和［-0.5 -0.5 -0.5］T，控制變量u的上下限分別為［0.1 0.1 0.1］T和［-0.1 -0.1 -0.1］T，最后求得機(jī)械臂最快到達(dá)終端構(gòu)型的時(shí)間為tf，min=127.120 4 s。

表1 機(jī)械臂系統(tǒng)參數(shù)Tab.1 Parameters of the manipulator system

整個(gè)過(guò)程中各關(guān)節(jié)角的變化曲線如圖3 所示。由圖可見(jiàn)，各關(guān)節(jié)都在各自的容許范圍內(nèi)。

圖3 關(guān)節(jié)角-時(shí)間最優(yōu)Fig.3 Joint angle-time optimal

整個(gè)過(guò)程中各關(guān)節(jié)角加速度的變化曲線如圖4所示。同樣地，從圖中可以看出各關(guān)節(jié)角加速度滿足路徑約束。

圖4 關(guān)節(jié)角加速度-時(shí)間最優(yōu)Fig.4 Joint acceleration-time optimal

為了證明在整個(gè)運(yùn)動(dòng)過(guò)程中機(jī)械臂運(yùn)動(dòng)對(duì)基座姿態(tài)沒(méi)有影響，機(jī)械臂姿態(tài)角速率隨時(shí)間變化的曲線如圖5所示，姿態(tài)角速率量級(jí)為10-17rad/s。圖6給出了機(jī)械臂運(yùn)動(dòng)對(duì)基座引起的反作用力矩，為10-15N?m 量級(jí)，也即干擾力矩為0，這也就說(shuō)明基座姿態(tài)不受機(jī)械臂運(yùn)動(dòng)的干擾。

圖5 基座角速率-時(shí)間最優(yōu)Fig.5 Angular velocity of base-time optimal

圖6 基座反作用力矩-時(shí)間最優(yōu)Fig.6 Reaction torque of base-time optimal

3.2 沿軌跡最小加速度仿真結(jié)果

選取終端時(shí)刻tf=300 s，其他仿真參數(shù)的選取與時(shí)間最優(yōu)規(guī)劃問(wèn)題相同。經(jīng)仿真求解，最后得到的目標(biāo)函數(shù)值為0.151 3。

整個(gè)過(guò)程中各關(guān)節(jié)角的變化曲線如圖7 所示，整個(gè)過(guò)程中各關(guān)節(jié)角加速度的變化曲線如圖8 所示。同樣地，兩者都滿足約束條件。

從圖7 和圖8 中還可以看出，與時(shí)間最優(yōu)問(wèn)題不同的是，加速度最小目標(biāo)下的軌跡更加平緩，關(guān)節(jié)角速度較小，所以到達(dá)期望狀態(tài)所需的時(shí)間也更長(zhǎng)。時(shí)間最優(yōu)目標(biāo)下加速度大部分時(shí)間處于正向最大或反向最大階段，類似于Bang-Bang 控制，所以也能更快地達(dá)到期望狀態(tài)。

機(jī)械臂姿態(tài)角速率隨時(shí)間變化的曲線如圖9 所示，機(jī)械臂運(yùn)動(dòng)對(duì)基座引起的反作用力矩如圖10 所示。同樣可以說(shuō)明，基座姿態(tài)不受機(jī)械臂運(yùn)動(dòng)的干擾，從而驗(yàn)證了零反最優(yōu)路徑規(guī)劃算法的有效性。

圖7 關(guān)節(jié)角-最小加速度Fig.7 Joint angle-minimum acceleration

圖8 關(guān)節(jié)角加速度-最小加速度Fig.8 Joint acceleration-minimum acceleration

圖9 基座角速率-最小加速度Fig.9 Angular velocity of base-minimum acceleration

圖10 基座反作用力矩-最小加速度Fig.10 Reaction torque of base-minimum acceleration

4 結(jié)束語(yǔ)

本文提出了一種空間冗余機(jī)械臂最優(yōu)路徑規(guī)劃算法，算法在保證機(jī)械臂達(dá)到期望末端位置狀態(tài)的同時(shí)對(duì)基座航天器姿態(tài)無(wú)干擾?；谌哂鄼C(jī)械臂的反作用零空間，該算法求解得到的最優(yōu)解是以零反作用機(jī)械臂運(yùn)動(dòng)的理論解析解為動(dòng)力學(xué)約束條件的，因此，具有非常高的精度。此外，在采用高斯偽譜法求解最優(yōu)路徑解的過(guò)程中考慮了兩個(gè)實(shí)際工程問(wèn)題中的重要約束，分別為關(guān)節(jié)角運(yùn)動(dòng)范圍物理約束以及驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)的電機(jī)輸出加速度約束，因此，具有很高的工程實(shí)用價(jià)值。