空間冗余機械臂零反作用最優路徑規劃

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

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

0 引言

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

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

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

本文針對已有研究存在的不足，提出一種基于冗余機械臂反作用零空間的最優路徑規劃算法。采用高斯偽譜法求解最優路徑問題，在零反作用軌跡的解集中尋找滿足邊界條件和路徑約束的使性能指標最優的機械臂運動規律。最后，通過數值仿真分析得到運動對基座姿態的干擾，以驗證其有效性。

1 機械臂零反運動

1.1 機械臂運動學方程

空間鏈式冗余機械臂系統如圖1 所示，系統由1 個基座航天器和n節剛性桿組成。圖中，S0為基座本體系，SI為慣性系，Si（i=1，…，n）為臂桿連體坐標系，ro為基座質心位置矢量，rg為系統質心位置矢量，rog為基座質心到系統質心矢量，pi為臂桿i關節位置矢量，ai為臂桿i質心在其連體系中位置矢量，為臂桿i角速度矢量。

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

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

1.2 機械臂零反運動解

由于本文只考慮基座的姿態運動而不考慮其平動，因此，不失一般性，假設系統線動量P=0，此時式（1）中角動量方程部分可寫為

從式（3）可以看出，為了滿足機械臂運動對基座姿態的干擾為零，則必須使得反作用力矩Fm=0，也就要求耦合動量Lm為常值。考慮到Hωφ∈Rm×n，其中，m為基座自由度，n為機械臂關節自由度，當機械臂滿足運動學冗余，也即n＞m時，滿足耦合動量Lm=const 的關節運動的解為

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

不失一般性，假設Lm=0，則有

將式（5）對時間積分，則可得到零反作用機械臂在關節空間中的運動路徑，所有滿足該條件的路徑稱為零反路徑集。

2 最優路徑規劃

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

定義狀態量x=［ξTφT］T，控制變量為

取關節角約束為

式中：φmin=［φ1minφ2min…φnmin］T；φmax=［φ1maxφ2max…φnmax］T。將式（5）對時間求導，可得

取關節角加速度約束為

結合式（5）和式（6），優化問題的動力學方程可以寫為

式中：

接下來將針對不同的目標函數，采用高斯偽譜法求解最優問題。

2.1 高斯偽譜法

高斯偽譜法是求解連續Bolza 型問題的直接法中配點法的一種，其基本原理是在一系列離散的Legendre Gauss 節點上，采用Lagrange 全局插值多項式來近似狀態變量和控制變量。然后，通過對插值多項式的求導來近似動力學方程中狀態變量對時間的導數，并使得離散點處滿足動力學方程的右函數約束條件。性能指標中的積分項采用近似精度最高的Gauss 積分來近似。經過一系列變換，從而將本文的最優路徑規劃問題轉化具有一系列代數約束的離散的非線性規劃問題。

因此，本文的最優路徑規劃問題可以描述為：求解離散狀態變量xi、控制變量ui，使對應的性能指標達到最小。

2.2 時間最優解

時間最優路徑規劃問題可描述為尋找控制變量u的一組解，使得具有形如

目標函數在滿足如下約束條件時取最小值（式中t0為初始時刻，tf為終端時刻）：1）動態約束見式（10）；2）邊界條件見式（7）；3）路徑約束見式（9）；4）初始條件為初始時刻t0=0，關節角φ(t0)=φ0；5）終端約束為φ(tf)=φf。

2.3 沿軌跡最小加速度解

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

目標函數在滿足2.2 節中約束條件1）～5）的同時取最小值。

3 仿真算例

平面3 自由度自由漂浮空間機械臂系統如圖2所示，由于只考慮基座的姿態運動，此時基座自由度m=1，機械臂自由度n=3，機械臂冗余自由度為n-m＞0，滿足零反運動必要條件。機械臂和基座的質量特性參數、幾何參數、初始構型以及期望達到的終端構型（見表1），在表1 中詳細列出，給定的關節角約束以及關節角加速度約束同樣在表1 中給出。

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

在仿真過程中，采用由弗洛里達大學的Rao等［21-23］開發開源免費軟件包GPOPS（Gauss Pseudospectral Optimization Software）對最優問題進行求解。初始時刻姿態角和角速度都為0，給出了針對兩個性能指標的仿真結果。

3.1 時間最優仿真結果

設置高斯節點數變化區間為4～12，容許偏差為10-4，求解過程中目標函數的梯度信息和約束方程的雅克比矩陣求解方式選為“有限差分”法，狀態變量和控制變量的初始猜測值可在各自的約束區間內任意選取。考慮到關節和關節角加速度約束，選取ξ的上下限分別為［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，最后求得機械臂最快到達終端構型的時間為tf，min=127.120 4 s。

表1 機械臂系統參數Tab.1 Parameters of the manipulator system

整個過程中各關節角的變化曲線如圖3 所示。由圖可見，各關節都在各自的容許范圍內。

圖3 關節角-時間最優Fig.3 Joint angle-time optimal

整個過程中各關節角加速度的變化曲線如圖4所示。同樣地，從圖中可以看出各關節角加速度滿足路徑約束。

圖4 關節角加速度-時間最優Fig.4 Joint acceleration-time optimal

為了證明在整個運動過程中機械臂運動對基座姿態沒有影響，機械臂姿態角速率隨時間變化的曲線如圖5所示，姿態角速率量級為10-17rad/s。圖6給出了機械臂運動對基座引起的反作用力矩，為10-15N?m 量級，也即干擾力矩為0，這也就說明基座姿態不受機械臂運動的干擾。

圖5 基座角速率-時間最優Fig.5 Angular velocity of base-time optimal

圖6 基座反作用力矩-時間最優Fig.6 Reaction torque of base-time optimal

3.2 沿軌跡最小加速度仿真結果

選取終端時刻tf=300 s，其他仿真參數的選取與時間最優規劃問題相同。經仿真求解，最后得到的目標函數值為0.151 3。

整個過程中各關節角的變化曲線如圖7 所示，整個過程中各關節角加速度的變化曲線如圖8 所示。同樣地，兩者都滿足約束條件。

從圖7 和圖8 中還可以看出，與時間最優問題不同的是，加速度最小目標下的軌跡更加平緩，關節角速度較小，所以到達期望狀態所需的時間也更長。時間最優目標下加速度大部分時間處于正向最大或反向最大階段，類似于Bang-Bang 控制，所以也能更快地達到期望狀態。

機械臂姿態角速率隨時間變化的曲線如圖9 所示，機械臂運動對基座引起的反作用力矩如圖10 所示。同樣可以說明，基座姿態不受機械臂運動的干擾，從而驗證了零反最優路徑規劃算法的有效性。

圖7 關節角-最小加速度Fig.7 Joint angle-minimum acceleration

圖8 關節角加速度-最小加速度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 結束語

本文提出了一種空間冗余機械臂最優路徑規劃算法，算法在保證機械臂達到期望末端位置狀態的同時對基座航天器姿態無干擾。基于冗余機械臂的反作用零空間，該算法求解得到的最優解是以零反作用機械臂運動的理論解析解為動力學約束條件的，因此，具有非常高的精度。此外，在采用高斯偽譜法求解最優路徑解的過程中考慮了兩個實際工程問題中的重要約束，分別為關節角運動范圍物理約束以及驅動關節運動的電機輸出加速度約束，因此，具有很高的工程實用價值。