空間機器人抓捕碰撞分析與軌跡規劃鎮定控制

樊 茂，湯 亮,2

(1. 北京控制工程研究所，北京 100190；2. 空間智能控制技術重點實驗室，北京 100190)

0 引 言

近些年來，空間機器人在空間操作方面得到了廣泛的應用[1-3]，例如國際空間站的組裝[4]、哈勃望遠鏡的維修[5]以及國際空間站貨運倉的交會對接等空間操作都是空間機械臂的經典應用場景。但是隨著空間操作任務需求與難度的不斷提升，空間機械臂也面臨著對廢棄衛星、空間碎片與小型隕石等非合作目標的抓捕問題[6]。在抓捕非合作目標的過程中，機械臂末端與被捕獲目標進行接觸碰撞，為了增大抓捕過程的成功率，在抓捕過程中需要關閉基座的姿態控制系統，因此抓捕產生的碰撞沖擊會對空間機器人系統造成沖擊[7]，嚴重時甚至會造成抓捕目標后所形成組合體的翻滾、失穩；除此之外，抓捕過程對機械臂系統產生的沖擊會帶動機械臂一起運動，為了消除抓捕后機械臂受到的沖擊效應，有必要設計相應的控制策略，在減小碰撞沖擊的同時維持目標與平臺之間的相對穩定，避免系統失穩。

針對抓捕目標后組合體受到的沖擊問題，許多學者提出了不同的方法來實現對組合體系統的鎮定控制[8-11]。為了減少機械臂控制過程中對平臺姿態的擾動，文獻[8]利用零反作用操作在日本空間機器人ETS-VII上進行了在軌實驗，該方法有效減少了機械臂在運動時對衛星姿態的影響，同時還降低了由于約束反力帶來的關節角速度限制。文獻[9]對碰撞前的機械臂構型及軌跡進行優化，實現了機械臂碰撞脈沖力的最小化。文獻[10]研究了航天器抓捕目標后，系統質量發生變化所引起的失穩問題，提出了關節阻尼控制策略和關節函數參數化協調控制方法，對碰撞后狀態發生突變的系統進行了基于角動量守恒的協調控制。文獻[11]用遞推最小二乘算法在線估計目標角動量，利用角動量守恒定律提出了自適應零反作用空間算法，減少了機械臂運動對平臺姿態的影響。

雖然以上學者都利用基于角動量守恒的零反作用空間算法進行機械臂關節的軌跡規劃，但所提出的方法都只以減少對衛星平臺的擾動為目標，忽略了初始角動量對系統的影響，因此許多學者轉而利用優化算法求解機械臂運動軌跡[12-18]。文獻[12]提出了基于混沌粒子群算法的關節空間點對點軌跡規劃方法，該方法考慮了機械臂運動過程中對衛星平臺的反作用軌跡規劃問題。文獻[13]將機械臂軌跡用七次多項式的正弦函數參數化，利用空間機器人的非完整特性提出了同時考慮機械臂末端與平臺基座擾動的點對點軌跡規劃方法。文獻[14-15]對抓捕目標的消旋路徑進行了設計，利用優化算法求解得到同時考慮末端接觸力矩與阻尼時間的軌跡。文獻[16]提出了基于角動量轉移的優化方法，利用粒子群算法設計了機械臂軌跡，在保證組合體相對穩定且角速度較小的同時，有效抑制了組合體柔性部件的振動。文獻[17]利用混沌粒子群算法，研究了基座姿態擾動最小時的機械臂軌跡規劃問題，使機械臂軌跡平滑運動的同時減少了對衛星姿態的影響。為了盡快使抓捕后的非合作目標穩定，文獻[18]研究了空間機器人最優消旋策略和協調控制方法，提出了一種同時使平臺姿態和抓捕目標一起穩定的控制方法。以上學者在對機械臂軌跡規劃的過程中，大都只考慮了對衛星平臺的擾動，忽略了機械臂關節控制力矩的能量消耗問題。

針對上述方法的不足，本文分析了抓捕瞬態碰撞對機器人系統產生的影響，并提出了一種控制力矩能量消耗少、對衛星平臺基座擾動小的鎮定控制方法。首先，利用ADAMS軟件建立并分析了空間機器人抓捕目標時的瞬態碰撞過程，為后續鎮定控制策略的設計提供了相應的初始仿真參數；其次，采用四次多項式實現了機械臂關節空間軌跡的參數化，設定了基于控制力矩能量與基座擾動的加權目標函數，并利用差分進化算法(Differential evolution,DE)求解得到滿足機械臂控制力矩能量消耗小、平臺基座擾動少的機械臂關節空間軌跡；最后，基于空間七自由度漂浮機器人的數值仿真結果驗證了所提出方法的有效性。

1 動力學建模

本文從兩個方面研究了空間機器人的動力學建模問題。其一是利用ADAMS軟件建立了七自由度空間機器人的瞬態碰撞模型，作為分析抓捕瞬態碰撞的仿真模型；其二是利用Kane方程建立了七自由度空間機器人抓捕目標后的組合體動力學模型，作為系統鎮定控制器的設計基礎。

1.1 空間機器人碰撞動力學建模

本文建立了七自由度剛體自由漂浮機器人動力學模型。機械臂各關節由球形鉸鏈連接，機械臂末端附有抓手，充當抓捕工具。利用ADAMS軟件的建模功能，并根據空間機器人參數，建立七自由度機械臂抓捕碰撞模型，如圖1所示。該模型由機械臂、衛星平臺和目標組成。在機械臂末端建立抓捕工具，并在目標上建立相應的抓手，如圖2所示。

碰撞動力學的建模不僅涉及系統的動力學、彈性參數，還與碰撞物體的幾何形狀、相互嵌入量有關，很難有解析表達式。因此利用ADAMS軟件中的等效彈簧阻尼函數對碰撞進行數值仿真[19]

(1)

式中：K表示模型的等效接觸剛度，K的取值通常與接觸體的材料剛度和幾何形狀選取有關；q表示兩物體之間的穿透量；e表示非線性系數指數，當e≠1時表示非線性彈性，e=1表示線性彈性；Dmax表示碰撞物體的最大穿透量；Cmax表示接觸時的最大阻尼；step為階躍函數，其函數示意圖如圖3所示，利用三次多項式逼近階梯函數，模擬等效彈簧阻尼函數的非線性阻尼模型，函數中q為自變量，其定義域為[0,Dmax]，函數值域為[0,Cmax]，當碰撞剛剛發生時，穿透量為0，阻尼為0，當穿透量為Dmax時阻尼為Cmax[20]。

圖1 七自由度空間機器人抓捕碰撞模型Fig.1 Seven-DOF space robot impact model

圖2 機械臂末端抓手模型Fig.2 End effector model of space robot

圖3 非線性阻尼項特性曲線示意圖Fig.3 Diagram of nonlinear damping characteristic curve

1.2 組合體動力學建模

為了描述抓捕目標后的組合體動力學特性，建立如圖4所示的組合體動力學數學模型。其中B表示衛星平臺，Li(i=1,…,7)表示機械臂的七個連桿，連桿之間由單自由度的旋轉鉸接關節ki(i=1,…,7)連接，b表示抓捕目標。

建立如圖4所示的坐標系，在衛星平臺質心處建立坐標系Ob表示衛星本體坐標系，Oi(i=1,…,7)表示連桿Li的本體坐標系，其坐標系原點位于旋轉關節ki的鉸接中心；Oiref表示關節ki的參考坐標系，其原點與Oi原點重合，且隨鉸接關節轉動;r0表示平臺質心在慣性系下的位置；ri(i=1,…,7)表示連桿質心在慣性系下的位置；pi(i=1,…,7)表示關節參考坐標系在慣性系下的位置。

圖4 組合體系統模型Fig.4 Model of compound system

文獻[21]中利用Kane方程[22]建立了具有幾何約束的雙臂空間機器人動力學模型，利用文獻[21]中的動力學建模方法，本文建立了單臂七自由度抓捕目標后的空間機器人組合體剛體動力學模型

(2)

(3)

為了保證抓捕的成功，空間機器人系統在抓捕瞬態過程中對平臺姿態不進行控制，最終組合體系統動力學可寫為如下形式

(4)

(5)

1.3 系統動量與角動量

當空間機器人系統穩定抓捕目標后，所形成的組合體系統不受外力作用，因此系統遵循動量守恒定律。文獻[24]的方法建立系統動量、角動量守恒方程。文獻[24]中描述的空間機器人系統具有l部機械臂，在l部機械臂中的第h個機械臂具有nk個自由度。系統動量和關于平臺質心的角動量為

(6)

(7)

(8)

(9)

(10)

r0g=rg-r0

(11)

(12)

本文建立了單臂七自由度空間機器人模型，因此h=1。抓捕前目標與衛星平臺之間存在相對運動，抓捕后目標將進一步帶動機械臂各臂桿以及平臺一起運動，因此抓捕后形成的衛星-機械臂-目標組合體系統具有初始動量與角動量。

由式(6)可以求解出平臺速度表達式

(13)

將式(13)代入式(6)中得到

(14)

(15)

從以上的分析中可以得出：由于系統遵守動量守恒定律，因此由抓捕引起的系統初始動量、角動量大小無法改變，只能通過對關節角與關節角速度的優化來減少平臺所受到的擾動。

2 抓捕瞬態仿真分析

碰撞初始時刻選取目標相對平臺運動參數見表1，ADAMS碰撞參數見表2，機械臂D-H參數與空間機械臂各部分質量慣量見表3、4。其中仿真時間1s，仿真步長10-3s。采用GSTIFF積分器，SI2積分格式，積分誤差為10-3。在碰撞仿真過程中，機械臂關節及平臺不施加控制。碰撞前機械臂初始構型為[75,70,110,10,110,-96,94]T(°)，關節角速度與平臺角速度均為零。

圖5表示碰撞過程中機械臂關節角速度的變化情況。從圖5可以看出第一次碰撞發生在0.007 s，末端抓手碰撞，造成關節角速度突變；碰撞發生后，由于沒有采取鎮定控制策略，所以在0.2 s時發生第二次碰撞且各關節開始隨動，造成末端抓手受力、關節角速度以及平臺狀態的突變，因此需要在二次碰撞之前進行鎮定控制。選取0.1 s時機械臂關節角速度[-0.19,0.54,0.01,0.02,-1.11,3.19,-2.05]T(°)/s作為后續鎮定控制策略的初始仿真參數。

表1 碰撞初始時刻目標相對平臺角速度、速度Table 1 Relative velocity and angular velocity between target and platform at initial time of impact

表2 碰撞參數Table 2 Parameters of collision

圖5 碰撞過程關節角速度變化曲線Fig.5 The curve of joints angular velocity during impact

圖6表示碰撞時平臺角速度的變化曲線。從圖中可以看出抓捕瞬態對衛星平臺本體影響有限，在碰撞過程中平臺總體保持穩定。第一次碰撞對平臺造成的角速度突變小于10-4(°)/s量級；第二次碰撞對平臺造成的角速度突變超過0.02(°)/s。

圖6 碰撞過程平臺角速度變化Fig.6 The curve of platform angular velocity during impact

圖7 末端碰撞力大小Fig.7 The magnitude force of end effector during impact

圖8 末端碰撞力矩大小Fig.8 The magnitude torque of end effector during impact

圖7、8分別表示碰撞時末端抓手所受碰撞力、力矩大小變化曲線。力矩的突變證明了碰撞的發生，碰撞力的兩次突變對應系統狀態的兩次突變。

綜合以上抓捕碰撞仿真結果，可以得出以下結論：

1) 末端抓手在抓捕過程中與目標發生碰撞，造成關節角速度的突變；在碰撞發生后，由于沒有采取相應的鎮定控制策略，機械臂各關節開始隨動并發生第二次碰撞，造成抓手受力過大，關節與平臺狀態變化劇烈，因此需要在二次碰撞之前設計相應的隨動控制策略進行鎮定控制。

2) 抓捕沖擊對衛星本體影響較小，衛星平臺在抓捕瞬態保持穩定。

因此針對抓捕目標后的組合體系統，根據抓捕碰撞仿真分析結果，做出如下假設以輔助設計組合體鎮定控制策略：

假設1.抓捕結束后，末端抓手與目標之間不產生晃動，抓手與目標之間為剛性連接，目標附著在末端抓手上隨機械臂一起運動。

從碰撞仿真結果可以看出，碰撞過程時間很短，根據文獻[25]中的分析，可用沖量定理描述抓捕碰撞對系統的沖擊效應，假設碰撞在短時間Δt內完成，則末端抓手碰撞力在碰撞瞬間的沖量可等效為

(16)

利用沖量原理對瞬態抓捕碰撞進行分析：系統受到的碰撞沖擊與碰撞后系統速度變化的大小相關，碰撞沖擊大則系統速度變化大，碰撞沖擊小則速度變化小。由碰撞仿真分析結果可以得出，第一次瞬態碰撞對衛星本體基座影響較小，因此針對抓捕瞬態碰撞對系統產生的影響做出如下假設：

假設2.抓捕瞬態碰撞對空間機器人系統產生的沖擊效應以機械臂關節角速度的形式體現。

3 基于差分進化算法的軌跡規劃鎮定控制

為了輔助設計機械臂鎮定控制策略，基于假設1與假設2可以將鎮定控制問題等效為：根據已知的初始狀態和終止狀態，求解一條機械臂運動軌跡，在滿足一定約束條件的情況下，達到消除目標相對運動與鎮定組合體系統的目的。本文將抓捕沖擊產生的關節角速度看作軌跡設計的初始狀態，將關節角速度、角加速度為零看作終止狀態進行軌跡的優化設計。

3.1 關節空間軌跡參數化

在機械臂軌跡規劃問題中，常采用多項式函數插值的方法對軌跡進行逼近。在本文中機械臂初始與終點狀態的關節角、關節角速度已知，因此利用多項式設計關節軌跡如下

θi=a0i+a1it+a2it2+a3it3+a4it4

(17)

對式(17)求一階導數與二階導數，分別求得關節角速度和角加速度的表達式

(18)

(19)

式中：T,θTi分別表示軌跡規劃時間與終點關節角構型。

從式(19)中可以看出，設計的四次插值多項式軌跡由阻尼時間T與終點關節角θTi決定。針對這一特點，本文通過對T,θTi的優化，求解得到滿足相關約束條件的運動軌跡。

為了實現組合體鎮定過程中控制力矩能量消耗小、對平臺擾動少的目的，聯合式(5)與式(15)，選取控制力矩能量消耗與衛星平臺角速度累計變化作為評價指標：

(20)

除此之外，在設計機械臂軌跡的過程中，各關節還應滿足如下約束條件

(21)

3.2 差分進化算法

差分進化算法由Storn等在文獻[26]中提出，該方法利用群體進化理論通過種群內個體間的競爭與全局搜索來實現對問題的優化計算。針對式(22)所示的優化問題[27]，DE算法利用變異、交叉、選擇操作實現問題的優化求解。

(22)

其中:z(x),h(x)表示與優化變量x有關的函數約束條件。

配電自動化的核心是饋線自動化(Feeder Automation,FA),即配電線路自動化,在線路開關上添加饋線自動化終端設備,以實現遠程監控。饋線自動化終端可分為具有遙測、遙信、遙控功能的“三遙”配電終端;具有遙測、遙信功能的“二遙”配電終端;僅有遙測功能的“一遙”配電終端[3]。不同地區的終端配置可根據各自的可靠性要求進行優化選擇。通過對線路中開關的控制可以優化網絡結構和提高供電可靠性[4],而饋線自動化終端優化配置也必須要統籌考慮投資與可靠性的關系,從而達到配電網建設運行的經濟性與可靠性的目的。

(23)

式中：i,r1,r2,r3=1,2,…,N，且r1,r2,r3為隨機選中的互不相同的數；F為縮放比例因子，取值范圍在[0,1]。

(24)

式中：j=1,…,D表示種群中元素的序列數；jrand表示[1,D]之間的隨機整數；rand(j)表示[0,1]之間均勻分布的隨機數；C為變異常數因子。

變異因子C越大則越有利于提高算法的局部搜索能力，且收斂速率快；C越小則越有利于保持種群的多樣性，且全局搜索能力強。

為了讓優化變量都處在規定的范圍內，在標準差分進化算法中加入邊界條件限制：

(25)

(26)

根據以上步驟，本文利用差分進化算法對式(20),(21)所示的問題進行優化計算。根據式(19)定義優化變量x

(27)

式中：Δθi(i=1,…,7)表示機械臂終止狀態相對初始狀態的關節角偏移量，θTi=θ0i+Δθi；T表示軌跡規劃時間。

其中優化變量中的約束條件為：

(28)

綜合任務需求考慮，本文定義適應度函數如下：

J=Jv+Jp

(29)

其中，Jv表示評價指標

(30)

評價指標中包括了機械臂關節控制力矩的能量消耗以及對平臺的累計擾動。除此之外，根據約束條件，設置懲罰函數Jp如下所示

(31)

(32)

(33)

最終，利用差分進化算法對軌跡進行優化的流程如下：

1)設置種群維數D與種群數量N，并選取優化變量x。

5)設定種群的上限與下限，保證種群在設定的范圍內進行搜索。

6)將以上五個步驟產生的種群代入適應度函數，并利用貪婪法則對種群進行篩選。

7)重復2)～6)步驟直到最大進化代數G，最終得到適應度函數值以及優化變量x。

阻尼軌跡優化流程如圖9所示，差分進化算法計算流程如圖10所示。

圖9 阻尼軌跡優化流程框圖Fig.9 Flow diagram of damping trajectory

圖10 差分進化算法流程圖Fig. 10 Flow chart of DE algorithm

3.3 機械臂控制器設計

為了跟蹤優化后的軌跡，根據式(5)設計機械臂關節控制器：

(34)

(35)

選取合適的正定矩陣Kp,Kd，證明控制器的穩定性，構造李雅普諾夫方程如下

(36)

根據李雅普諾夫穩定性原理，對李雅普諾夫函數求導：

(37)

又因為V≥0，所以根據李雅普諾夫穩定性原理可證明系統全局漸近穩定，因此設計的控制器可以實現對期望軌跡的穩定跟蹤。

4 仿真校驗

本文利用圖4所示的七自由度空間機器人組合體模型對所提出的控制策略進行仿真校驗。組合體機械臂D-H參數見表3。

表3 機械臂D-H參數Table 3 D-H parameter of manipulator

4.1 軌跡優化結果

表4 系統質量慣量Table 4 Mass and inertia of system

(38)

將給定的關節角速度和機械臂構型作為初始仿真參數，并利用DE算法求解關節角偏移量和阻尼時間。

圖11 適應度函數收斂曲線Fig.11 The convergence curve of fitness function

圖11表示適應度函數值隨進化代數的收斂曲線。最終得到適應度函數優化值J=3149.7，阻尼時間T=123.44 s，關節角相對初始構型偏移量Δθ=[31.96,31.54,21.64,-4.12,39.99,-15.05,-27.49]T(°)。

結合式(27)將Δθ、T代入式(19)得到軌跡曲線，最后利用式(34)的控制器對軌跡進行跟蹤，得到關節角、關節角速度、平臺角速度和關節控制力矩曲線如圖12～15所示。從圖12關節角速度曲線可以看到，利用文中提出的鎮定控制方法使關節角速度最終收斂到零，消除了目標的相對運動，達到了維持被捕獲目標與衛星本體之間相對穩定的目的。特別注意圖14中機械臂控制力矩的變化，由于衛星平臺為自由漂浮狀態，當衛星平臺運動時關節仍需保持對期望軌跡的跟蹤，因此控制力矩始終不為零。

圖12 關節角速度曲線Fig.12 The curve of joints angular velocity

圖13 關節角軌跡Fig.13 The trajectory of joint angles

圖14 關節控制力矩Fig.14 The control torques of joints

圖15 平臺角速度Fig.15 The angular velocity of platform

4.2 優化與未優化結果的對比

為了驗證本文所提出算法的有效性與優越性，將其與未進行優化的軌跡結果進行對比分析。

另給定區別于本文優化結果的四次多項式軌跡作為期望軌跡進行跟蹤。在相同仿真時間下利用相同控制器，即式(34)對軌跡進行跟蹤，并設定如下指標進行對比：

機械臂運動對平臺的累計擾動：

(39)

機械臂關節控制力矩能量消耗：

(40)

平臺角速度擾動范數：

(41)

由圖16和圖18平臺擾動對比曲線可以看到，優化后的軌跡在阻尼鎮定過程中對平臺的擾動更小，達到了對平臺擾動少的目的；由圖17關節控制力矩能量消耗對比曲線可以看出，優化后的控制能量消耗更少。

圖16 平臺累計擾動Fig.16 Accumulation disturbance of platform

圖17 關節控制力矩能量消耗Fig.17 The energy cost of control torque

圖18 平臺角速度擾動范數Fig.18 The norm of platform angular velocity

根據以上仿真對比曲線可以得出，本文提出的鎮定控制策略對抓捕目標后的空間機器人系統具有良好的阻尼鎮定效果，達到了控制力矩能量消耗小、對平臺基座擾動少的控制目標。

5 結 論

本文針對空間機器人抓捕目標后對系統產生的沖擊問題，利用ADAMS軟件建立并分析了抓捕瞬態沖擊對空間機器人系統產生的影響，為鎮定控制策略的設計提供了初始仿真參數。采用四次多項式插值的方法實現了機械臂關節空間軌跡的參數化,利用差分進化算法求解得到滿足控制力矩能量消耗小、對平臺基座擾動少的軌跡。與未優化的仿真結果相比，本文提出的鎮定控制策略與軌跡規劃方法在減少控制力矩能量消耗和對平臺擾動的同時，實現了對抓捕目標后組合體系統的阻尼鎮定，減小了碰撞沖擊對系統產生的影響，維持了被捕獲目標與衛星本體之間的相對穩定。