基于Clamped B 樣條的空間非合作目標(biāo)抓捕策略研究1)

王明明 羅建軍 余 敏

(西北工業(yè)大學(xué)深圳研究院,廣東深圳 518057)

(西北工業(yè)大學(xué)航天學(xué)院,西安 710072)

引言

空間機(jī)器人將在衛(wèi)星維修、在軌裝配、碎片清除等任務(wù)中發(fā)揮著重要作用.典型的在軌演示試驗(yàn)如“Robot Technology Experiment(ROTEX)”、“Engineering Test Satellite VII(ETS-VII)”、“Orbital Express(OE)”和“鳳凰號(hào)”等項(xiàng)目驗(yàn)證了空間機(jī)器人執(zhí)行在軌服務(wù)任務(wù)的可行性[1-4].然而,在軌服務(wù)任務(wù)最具挑戰(zhàn)性的工作之一是如何高效、可靠地抓捕目標(biāo)衛(wèi)星.此外,空間機(jī)械臂與基座之間的動(dòng)力學(xué)耦合引起的非完整約束對(duì)于抓取規(guī)劃提出了新的挑戰(zhàn).

過(guò)往文獻(xiàn)已提出多種空間機(jī)械臂軌跡規(guī)劃方法.增強(qiáng)干擾圖(EDM)的概念[5]首先被提出并應(yīng)用于空間機(jī)器人的啟發(fā)式軌跡規(guī)劃問(wèn)題求解.然而,EDM很難計(jì)算,尤其是對(duì)于具有較高自由度的空間機(jī)械臂.此后,文獻(xiàn)[6]提出零反作用空間(RNS)的概念,被應(yīng)用于運(yùn)動(dòng)學(xué)非冗余和冗余空間機(jī)械臂的軌跡生成.文獻(xiàn)[7-8]考慮角動(dòng)量守恒,使用RNS 的概念設(shè)計(jì)關(guān)節(jié)軌跡抓捕翻滾目標(biāo).然而,對(duì)于6 自由度機(jī)械臂而言,RNS 的體積十分有限.文獻(xiàn)[9]中,提出等效平衡臂和動(dòng)態(tài)平衡控制(DBC)相結(jié)合的方案減少基座姿態(tài)干擾.文獻(xiàn)[10]提出了一種新的組合策略,將DBC 和RNS 集成到基于任務(wù)優(yōu)先級(jí)的求解框架中.此后,文獻(xiàn)[11]提出了空間雙臂機(jī)器人點(diǎn)到點(diǎn)零反作用操作的規(guī)劃策略.文獻(xiàn)[12]建模和分析了空間機(jī)器人系統(tǒng)的動(dòng)力學(xué)耦合,并將動(dòng)力學(xué)耦合應(yīng)用于最小化基座姿態(tài)干擾的運(yùn)動(dòng)軌跡規(guī)劃.此外,可采用不同的關(guān)節(jié)軌跡參數(shù)化方法,例如周期性均勻B 樣條[13]、Bezier 曲線[14]等;文獻(xiàn)[15]給出了一種笛卡爾軌跡參數(shù)化方法,使得自由漂浮空間機(jī)器人的軌跡規(guī)劃問(wèn)題轉(zhuǎn)化為非線性優(yōu)化問(wèn)題.非線性優(yōu)化問(wèn)題可以采用遺傳算法、差分進(jìn)化算法等進(jìn)行求解.

以上研究主要是解決捕獲前階段空間機(jī)械臂的軌跡規(guī)劃問(wèn)題,而未考慮抓捕目標(biāo)后的任務(wù).通常,機(jī)械臂欲抓捕移動(dòng)的目標(biāo),需要感知環(huán)境并生成適當(dāng)?shù)淖ゲ恫呗?最終實(shí)現(xiàn)機(jī)械臂末端執(zhí)行器和目標(biāo)抓捕點(diǎn)之間建立穩(wěn)固的連接.文獻(xiàn)[16]描述了一種在真實(shí)環(huán)境中工作的移動(dòng)機(jī)械手的抓捕規(guī)劃方法,利用當(dāng)前物體模型優(yōu)化機(jī)器人手的抓握位姿.文獻(xiàn)[17]提出了一種BiSpace 規(guī)劃算法,結(jié)合了雙向快速探索隨機(jī)樹(RRT)和RRT-JT 算法,通過(guò)同時(shí)探索多個(gè)空間來(lái)實(shí)現(xiàn)快速規(guī)劃.在此基礎(chǔ)上,文獻(xiàn)[18]提出了改進(jìn)的RRT*與四次樣條曲線結(jié)合的協(xié)調(diào)路徑規(guī)劃方法.文獻(xiàn)[19]中提出了一種基于目標(biāo)外形和操作任務(wù),學(xué)習(xí)人類抓取策略的抓捕集成方法.前期的抓捕規(guī)劃方法為在軌抓捕規(guī)劃提供了參考.通過(guò)在接近階段使用偏置動(dòng)量方法,在接觸碰撞階段使用阻抗控制,以及在捕獲后階段進(jìn)行分布式動(dòng)量控制,文獻(xiàn)[20]提出了一種可行的控制序列,用于成功完成捕獲操作.文獻(xiàn)[21]基于視覺(jué)反饋,引入了一種引導(dǎo)操作機(jī)械臂先攔截,后消旋的非合作目標(biāo)抓捕策略,形成了機(jī)器人捕獲具有未知漂移和翻滾目標(biāo)的預(yù)測(cè)和規(guī)劃組合方案.考慮抓捕過(guò)程中碰撞引起機(jī)器人基座姿態(tài)變化的問(wèn)題,文獻(xiàn)[22]提出一種碰撞前的機(jī)械臂構(gòu)型優(yōu)化方法.針對(duì)空間機(jī)械臂抓捕目標(biāo)后的控制問(wèn)題,文獻(xiàn)[23-24]設(shè)計(jì)了空間機(jī)器人在軌捕獲非合作目標(biāo)的柔性機(jī)構(gòu),并分別基于神經(jīng)網(wǎng)絡(luò)的全階滑模避障柔順控制和動(dòng)態(tài)緩沖柔順控制,實(shí)現(xiàn)抓捕過(guò)程的抗沖擊與彈性振動(dòng)主動(dòng)抑制,有效避免了機(jī)械臂關(guān)節(jié)電機(jī)的過(guò)載.文獻(xiàn)[25]設(shè)計(jì)了一類目標(biāo)消旋策略并同時(shí)穩(wěn)定基座的協(xié)調(diào)控制方法.文獻(xiàn)[26]以三臂空間機(jī)器人為對(duì)象,提出一種基于位姿誤差反饋的軌跡規(guī)劃算法用于應(yīng)對(duì)奇異位形出現(xiàn)時(shí)的位姿誤差問(wèn)題.文獻(xiàn)[27]系統(tǒng)地給出了空間雙臂機(jī)器人的協(xié)同測(cè)量及軌跡規(guī)劃方法.文獻(xiàn)[28]提出了空間雙臂機(jī)器人捕獲目標(biāo)后輔助對(duì)接操作的協(xié)調(diào)控制方案.

前期研究在抓捕可行性和抓捕規(guī)劃方面存在以下缺點(diǎn):(1)空間機(jī)器人抓捕策略未充分考慮目標(biāo)的運(yùn)動(dòng)和機(jī)器人的抓捕能力,無(wú)法評(píng)估抓取可行性,并確定最佳抓捕時(shí)機(jī);(2)非合作目標(biāo)的抓捕規(guī)劃是一個(gè)多目標(biāo)、多約束的非線性優(yōu)化問(wèn)題,為評(píng)估抓捕規(guī)劃策略的有效性,在規(guī)劃過(guò)程中須充分考慮捕獲后的操作.如何實(shí)現(xiàn)對(duì)翻滾目標(biāo)的安全可靠抓捕對(duì)開展后續(xù)的在軌服務(wù)任務(wù)至關(guān)重要.迄今為止,空間機(jī)械臂的抓取規(guī)劃仍然是一項(xiàng)極具挑戰(zhàn)性的工作.本文的目標(biāo)是設(shè)計(jì)運(yùn)動(dòng)學(xué)冗余空間機(jī)械臂的抓捕規(guī)劃策略,同時(shí)充分考慮空間機(jī)械臂的抓捕能力、目標(biāo)運(yùn)動(dòng)、關(guān)節(jié)限制和防碰撞等約束.選擇運(yùn)動(dòng)學(xué)冗余機(jī)械臂的原因在于其存在無(wú)窮多個(gè)逆運(yùn)動(dòng)學(xué)解,可用于滿足多約束條件,例如最小化基座姿態(tài)干擾、防碰撞、最大化力可操作度等.最終通過(guò)對(duì)抓捕策略的仿真驗(yàn)證,為后續(xù)空間機(jī)器人抓捕動(dòng)態(tài)非合作目標(biāo)提供技術(shù)支撐.

1 動(dòng)力學(xué)建模

1.1 空間機(jī)器人動(dòng)力學(xué)

如圖1 所示,空間機(jī)器人系統(tǒng)由航天器基座與n個(gè)自由度的操作機(jī)械臂組成,共包括n+1 個(gè)對(duì)象.基于Lagrange 動(dòng)力學(xué)方程,空間機(jī)器人系統(tǒng)的動(dòng)力學(xué)方程可表示如下

圖1 抓捕前空間機(jī)器人示意圖Fig.1 Schematic diagram of space robot in pre-capture phase

式中,Jg被稱為廣義雅可比矩陣[29].基于式(2)和式(3)可以看出,自由漂浮模式下基座和末端執(zhí)行器的運(yùn)動(dòng)僅依賴于關(guān)節(jié)軌跡及動(dòng)力學(xué)耦合效應(yīng).因此,通過(guò)適當(dāng)?shù)淖ゲ恫呗砸?guī)劃可實(shí)現(xiàn)多個(gè)優(yōu)化指標(biāo),如末端執(zhí)行器任務(wù),最小化基座干擾或最大化力可操作度等.

1.2 非合作目標(biāo)動(dòng)力學(xué)

圖2 抓捕后目標(biāo)示意圖Fig.2 Schematic diagram of target in post-capture phase

如圖2 所示,目標(biāo)衛(wèi)星的動(dòng)力學(xué)可描述如下mt和It分別為目標(biāo)衛(wèi)星的質(zhì)量和慣性張量.是單位矩陣.抓捕前階段,目標(biāo)上的外力旋量ht=0,目標(biāo)捕獲后,外力旋量ht=Gtehe,he∈R6.力旋量同時(shí)作用于末端執(zhí)行器,抓捕矩陣Gte 為

1.3 動(dòng)靜態(tài)對(duì)偶性分析

根據(jù)虛功原理,作用于末端執(zhí)行器力旋量的虛功和關(guān)節(jié)力矩計(jì)算的虛功相等.參考式(1),末端執(zhí)行器力旋量與關(guān)節(jié)力矩間的關(guān)系可通過(guò)廣義雅可比矩陣的轉(zhuǎn)置來(lái)建立

結(jié)合式(3)與式(8),可以給出動(dòng)靜態(tài)對(duì)偶性的關(guān)系.目標(biāo)一旦被捕獲,參考式(8)和ht=Gtehe,可以得出

下標(biāo)?可以是e 或t;相應(yīng)地,⊙可以對(duì)應(yīng)設(shè)置為g 或c.從式(11)和式(12)可以看出,力可操作橢球的主軸與速度可操作橢球的主軸一致,而各軸的長(zhǎng)度互為倒數(shù).根據(jù)動(dòng)靜態(tài)對(duì)偶性分析,良好的力可操作性方向?qū)?yīng)較差的速度可操作性方向,反之亦然.這些特征將用于后續(xù)抓捕規(guī)劃策略設(shè)計(jì)中.

2 抓捕時(shí)機(jī)與可行性

一般而言,空間非合作目標(biāo)上沒(méi)有固定的抓捕機(jī)構(gòu),因此,需要選擇星上特定的結(jié)構(gòu)作為可行的抓捕點(diǎn).非合作目標(biāo)上可以抓捕的位置包括:遠(yuǎn)地點(diǎn)發(fā)動(dòng)機(jī)邊緣、對(duì)接環(huán)、太陽(yáng)能帆板支架等.為了輔助構(gòu)建空間機(jī)械臂的抓捕規(guī)劃策略,本文提出如下假設(shè):

(1)空間機(jī)器人系統(tǒng)和目標(biāo)均為剛體,在執(zhí)行抓捕任務(wù)之前,空間機(jī)器人已通過(guò)軌道機(jī)動(dòng)至相對(duì)于目標(biāo)衛(wèi)星的某一固定位姿;

(2)空間機(jī)器人系統(tǒng)的初始角動(dòng)量為0,抓捕過(guò)程中系統(tǒng)質(zhì)量特性不發(fā)生變化,目標(biāo)的體坐標(biāo)系處于目標(biāo)質(zhì)心處;

(3)空間機(jī)器人系統(tǒng)的運(yùn)動(dòng)狀態(tài)和質(zhì)量特性已知,目標(biāo)上的抓捕點(diǎn)事先已確定,并在抓捕點(diǎn)上附著有抓捕坐標(biāo)系;

(4)抓捕過(guò)程中忽略環(huán)境力(重力梯度、地磁、太陽(yáng)光壓等)以及相對(duì)軌道動(dòng)力學(xué)效應(yīng).

2.1 抓捕能力圖譜

針對(duì)單一機(jī)械臂,其抓捕能力依賴于機(jī)械臂的運(yùn)動(dòng)學(xué)構(gòu)型、安裝位置等約束.為了完成對(duì)目標(biāo)的抓捕和操作,機(jī)械臂需要更完備的抓捕能力描述.因此,結(jié)合機(jī)械臂的可達(dá)性和靈巧性概念[30],構(gòu)建機(jī)械臂的抓捕能力圖譜.

首先,目標(biāo)上的抓捕點(diǎn)需要位于機(jī)械臂的工作空間WS之中,機(jī)械臂工作空間定義如下

圖3 SE(3)離散化Fig.3 Discretization of SE(3)

其中,θ是機(jī)械臂的構(gòu)型空間,F K(θ)是正向運(yùn)動(dòng)學(xué)映射.機(jī)械臂的可達(dá)性圖譜描述了機(jī)器人工具中心點(diǎn)(TCP)坐標(biāo)系能夠到達(dá)的所有可能位姿.如圖3所示,該圖譜的建立是通過(guò)在6 自由度空間中結(jié)構(gòu)化離散機(jī)械臂的工作空間完成的.首先,R3中的可達(dá)工作空間被分割成等體積的立方體單元,每個(gè)單元的姿態(tài)可達(dá)性通過(guò)單元內(nèi)接球上均勻分布空間點(diǎn)來(lái)實(shí)現(xiàn).可達(dá)性圖譜可存儲(chǔ)為二進(jìn)制矩陣,每個(gè)塊的值代表TCP 局部區(qū)域的可達(dá)性,值為1 表示可達(dá),為0 表示末端執(zhí)行器不可達(dá).可達(dá)性圖譜的計(jì)算可以基于機(jī)械臂的逆運(yùn)動(dòng)學(xué).本文中,工作空間離散為nv個(gè)立方體單元,每個(gè)內(nèi)接球上平均分布了nd個(gè)點(diǎn),每個(gè)內(nèi)接球上的點(diǎn)進(jìn)行了nz次旋轉(zhuǎn).

靈巧性為機(jī)械臂的操作能力提供了更為豐富的信息,每個(gè)立方體單元的靈巧性指標(biāo)可以定義如下

Vi(a)表示可達(dá)性圖譜中第i個(gè)立方體單元的二進(jìn)制值.Di有效地給出了每個(gè)立方體單元中不同位姿的可達(dá)概率并給出了其靈巧性評(píng)估.結(jié)合可達(dá)性與靈巧性,將每個(gè)立方體單元的靈巧性值用相應(yīng)的顏色表征,可以構(gòu)建機(jī)械臂的抓捕能力圖譜,如圖4 所示.抓捕能力圖譜給出了機(jī)械臂工作空間中可以靈巧操作目標(biāo)的程度,結(jié)合目標(biāo)的運(yùn)動(dòng)預(yù)測(cè),可以確定抓捕時(shí)間窗口并評(píng)估抓捕的可行性.

2.2 抓捕時(shí)機(jī)確定

以往的研究未充分考慮抓捕過(guò)程中機(jī)械臂的抓捕能力、目標(biāo)的運(yùn)動(dòng)、末端執(zhí)行器的接近方向等問(wèn)題,如圖5 所示,抓捕時(shí)機(jī)確定依賴于機(jī)械臂的抓捕能力分析、初始構(gòu)型、抓捕坐標(biāo)系以及目標(biāo)的運(yùn)動(dòng)預(yù)測(cè).首先,基于工作空間WS分析,確定抓捕時(shí)間窗口;此后,給定目標(biāo)的初始運(yùn)動(dòng)條件,搜索最優(yōu)的未來(lái)時(shí)刻,保證目標(biāo)上的抓捕點(diǎn)位于能力圖譜內(nèi)且具有更高的靈活性指標(biāo),此外,末端執(zhí)行器與抓捕點(diǎn)的距離要盡可能小,位置矢量ρeg=rg(t)?和抓捕坐標(biāo)系矢量nx之間的夾角亦需盡可能小,從而保證接近方向處于接近走廊內(nèi).因此,抓捕時(shí)機(jī)確定問(wèn)題可描述如下

圖4 抓捕能力圖譜及切面等高線圖Fig.4 Capability map and section contour map

圖5 抓捕時(shí)機(jī)確定示意圖Fig.5 Schematic diagram of determining optimal grasping time

其中,Dirg(t)是抓捕點(diǎn)在工作空間WS的靈巧性指標(biāo),nx是旋轉(zhuǎn)矩陣Rg(t)的第一列.給定初始狀態(tài),rg(t)和Rg(t)的計(jì)算可以根據(jù)式(4)得到.

3 抓捕規(guī)劃器設(shè)計(jì)

3.1 問(wèn)題描述

空間機(jī)械臂的抓捕規(guī)劃與控制受到動(dòng)力學(xué)耦合的影響,自由漂浮模式下由于非完整約束的存在,機(jī)械臂各關(guān)節(jié)的構(gòu)型是路徑依賴的.抓取規(guī)劃策略的目標(biāo)是確定最優(yōu)的抓捕時(shí)機(jī)并生成可行的,不違反各類約束的關(guān)節(jié)運(yùn)動(dòng)軌跡θ(t).一般而言,抓捕策略可描述為不等式約束gi(θ)與等式約束hi(θ)下的非凸優(yōu)化問(wèn)題:

3.2 約束與目標(biāo)函數(shù)

抓捕規(guī)劃中的約束可以分為等式約束與不等式約束.運(yùn)動(dòng)狀態(tài)x帶有上標(biāo)s,f,d 分別表示初始、終端和期望的位姿.等式約束主要用于描述關(guān)節(jié)的初始位置、初始速度和初始加速度

同樣地,抓捕規(guī)劃的不等式約束可列寫如下.

(1)關(guān)節(jié)限幅:抓捕過(guò)程中需要考慮關(guān)節(jié)的運(yùn)動(dòng)能力,保證各關(guān)節(jié)不超出其運(yùn)動(dòng)范圍

(2)抓捕走廊:參考圖1,末端執(zhí)行器的接近方向必須限制在一個(gè)抓捕走廊內(nèi)

(3)碰撞規(guī)避:抓捕過(guò)程中需要保證機(jī)械臂與目標(biāo)之間、各關(guān)節(jié)之間不發(fā)生碰撞

由于自由漂浮空間機(jī)器人非完整約束的存在,航天器基座和末端執(zhí)行器的終端狀態(tài)不僅依賴于逆運(yùn)動(dòng)學(xué),也依賴于動(dòng)力學(xué)耦合效應(yīng).假定和xb=(rb,qb)分別表示末端執(zhí)行器和基座的位姿,姿態(tài)描述采用四元數(shù)q={η,ε} ∈R4表示,相對(duì)四元數(shù)的計(jì)算公式如下

δε=0 表示兩個(gè)旋轉(zhuǎn)坐標(biāo)系重合.基于單位四元數(shù),并考慮式(2)和式(3),基座與末端執(zhí)行器的終端位姿計(jì)算如下

Jq=通過(guò)應(yīng)用正向運(yùn)動(dòng)學(xué),避免了動(dòng)力學(xué)奇異的發(fā)生.終端位姿與期望位姿的偏差可計(jì)算如下

基于上述分析,空間機(jī)械臂抓捕規(guī)劃策略考慮如下的優(yōu)化指標(biāo).

(1)調(diào)整末端執(zhí)行器狀態(tài):抓取規(guī)劃要求機(jī)械臂末端執(zhí)行器的終端位姿、速度要盡可能的與目標(biāo)上的抓捕點(diǎn)相匹配.因此,定義如下的優(yōu)化指標(biāo)

(2)最小化基座干擾:為保持通信、觀測(cè)等要求,期望在抓捕過(guò)程中最小化基座干擾.因此,定義如下的優(yōu)化指標(biāo)

(3)最大化力可操作度:抓取完成后,需要對(duì)目標(biāo)進(jìn)行消旋、調(diào)姿等操作,因此,需要最大化抓捕后的力可操作度

3.3 關(guān)節(jié)參數(shù)化

為保證抓取過(guò)程中關(guān)節(jié)位置、速度、加速度的連續(xù)性,參考文獻(xiàn)[14],各關(guān)節(jié)的軌跡θi(t)可以參數(shù)化為多項(xiàng)式曲線,如Bezier 曲線,正弦多項(xiàng)式曲線等.本文中,采用B 樣條對(duì)關(guān)節(jié)進(jìn)行參數(shù)化.B 樣條相比于Bezier 曲線的構(gòu)建需要更多的信息,但是它具備如下優(yōu)點(diǎn):首先,B 樣條曲線是一類Bezier 曲線;其次,B 樣條曲線具備Bezier 曲線的所有特性;再次,B 樣條曲線相比Bezier 曲線提供了更好的控制靈活性.B樣條曲線的階次與其控制點(diǎn)的個(gè)數(shù)無(wú)關(guān),亦即,可采用低階曲線和更多控制點(diǎn)來(lái)構(gòu)建運(yùn)動(dòng)曲線.此外,B樣條曲線具有局部修正特性,改變某一個(gè)控制點(diǎn)并不會(huì)全局的改變曲線的形狀,此特性對(duì)于非線性優(yōu)化過(guò)程中的求解十分有利.

給定d+1 個(gè)控制點(diǎn)Pi,0,Pi,1,···,Pi,d和有m+1 個(gè)節(jié)點(diǎn)的節(jié)點(diǎn)矢量U={u0,u1,···,um},u0≤u1≤··· ≤um,對(duì)于p階次的Clamped B 樣條,首節(jié)點(diǎn)與末節(jié)點(diǎn)具有重復(fù)度p+1,因此,對(duì)應(yīng)于m+1 個(gè)節(jié)點(diǎn)的節(jié)點(diǎn)矢量,u0=u1=···=up=0 和um?p=um?p+1=···=um=0.關(guān)節(jié)軌跡θi(t)可以描述為歸一化的Clamped B 樣條曲線

其中Nj,p是B 樣條的基函數(shù),可以遞歸計(jì)算如下

上式通常被稱為Cox-de Boor 遞歸公式.值得注意的是d,m和p滿足m=d+p+1.由于u是歸一化時(shí)間,對(duì)于軌跡的執(zhí)行時(shí)間T=tf?ts,如果定義t=u·T,關(guān)節(jié)速度和加速度可以計(jì)算如下

式中

本文中,選擇四次Clamped B 樣條曲線p=4,d=7,m=12 對(duì)關(guān)節(jié)軌跡進(jìn)行參數(shù)化.B 樣條曲線的基函數(shù)計(jì)算可參考圖6.式(18)中關(guān)節(jié)速度和加速度約束可通過(guò)調(diào)整執(zhí)行時(shí)間滿足

圖6 B 樣條基函數(shù)計(jì)算Fig.6 B spline basic functions calculation

其中,C′=dθ/du且C′′=d2θ/du2.tg?ts≥T意味著軌跡執(zhí)行時(shí)間足夠長(zhǎng)且滿足式(18),終端時(shí)間可以設(shè)定為tf=tg;否則,需要更新Pi,j重新構(gòu)建關(guān)節(jié)軌跡.將式(17)中的等式約束代入式(28)和式(31)可得Pi,0=Pi,1=Pi,2=.由于已知,描述關(guān)節(jié)軌跡的Clamped B 樣條曲線形狀僅依賴于剩余的控制點(diǎn)Pi,3,Pi,4,···,Pi,d.考慮到空間機(jī)械臂的n個(gè)關(guān)節(jié),定義p=[P1,3,P1,4,···,P1,d,P2,3,P2,4,···,Pn,3,Pn,4,···,Pn,d]T作為設(shè)計(jì)變量,一旦p確定,即可確定每個(gè)關(guān)節(jié)的軌跡,進(jìn)而得到抓捕規(guī)劃策略.因此,式(16)中的抓捕規(guī)劃問(wèn)題可轉(zhuǎn)化為如下的優(yōu)化問(wèn)題

上述問(wèn)題將利用自適應(yīng)慣性權(quán)重的粒子群優(yōu)化算法(PSO)進(jìn)行求解,算法基本步驟如下:首先在可行的搜索區(qū)域內(nèi)初始化具有隨機(jī)初始值的粒子群.單個(gè)粒子維度即為設(shè)計(jì)變量的維數(shù).然后,各粒子對(duì)其適應(yīng)度函數(shù)進(jìn)行評(píng)估,在搜索空間內(nèi)搜索目前已知的自身最佳粒子(局部)和整群中的最佳粒子(全局).各粒子的運(yùn)動(dòng)由局部及全局最佳粒子指導(dǎo),每一代更新一次.當(dāng)發(fā)現(xiàn)更好的粒子時(shí),就選作新一代粒子群運(yùn)動(dòng)的指導(dǎo)粒子.此過(guò)程不斷重復(fù),直到滿足特定收斂條件或發(fā)現(xiàn)有更好的解決方案.PSO 算法執(zhí)行過(guò)程中,需要考慮如何處理約束及選擇合適的適應(yīng)度函數(shù),參考文獻(xiàn)[31-32],采用修復(fù)不可行解集的方法處理本文中的不等式約束違反;適應(yīng)度函數(shù)選取式(25)用于評(píng)估每個(gè)粒子的優(yōu)劣,并存儲(chǔ)可行的空間機(jī)械臂抓捕規(guī)劃解集,而后基于式(26)和式(27)進(jìn)行多目標(biāo)優(yōu)化解集的選取與確定.

4 仿真結(jié)果

為驗(yàn)證本文提出的抓捕規(guī)劃策略,本節(jié)給出了抓捕翻滾目標(biāo)的仿真驗(yàn)證.空間機(jī)器人系統(tǒng)由一個(gè)7 DOF 運(yùn)動(dòng)學(xué)冗余機(jī)械臂和6 DOF 航天器組成,運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)參數(shù)在表1 中給出,其中ai,bi和Ii在各運(yùn)動(dòng)體的體坐標(biāo)系中進(jìn)行描述.在抓捕過(guò)程中,首先基于機(jī)械臂的抓捕能力圖譜、初始構(gòu)型、目標(biāo)運(yùn)動(dòng)預(yù)測(cè)等信息確定最優(yōu)抓捕時(shí)機(jī)tg.此后利用PSO 算法搜索求解式(33),構(gòu)建基于Clamped B 樣條的關(guān)節(jié)軌跡,生成空間機(jī)械臂的抓捕規(guī)劃策略.

表1 空間機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)參數(shù)Table 1 Kinematic and dynamic parameters of space robot

基于PSO 的求解結(jié)果,共選擇三組記錄來(lái)評(píng)價(jià)非合作目標(biāo)抓捕規(guī)劃策略.三組記錄對(duì)應(yīng)的解集可參見(jiàn)表2.針對(duì)式(26)和式(27),不同優(yōu)化指標(biāo)所構(gòu)成的Pareto 前沿如圖7 所示.圖8～圖10 給出了三組不同解集對(duì)應(yīng)的關(guān)節(jié)軌跡、末端執(zhí)行器與基座的運(yùn)動(dòng)情形.可以看出,所有解集均滿足仿真設(shè)定的終端狀態(tài)誤差要求.此外,抓捕過(guò)程中保證了空間機(jī)器人系統(tǒng)的動(dòng)量守恒.在終端抓捕時(shí)刻,對(duì)于Test 1,最終的力可操作度為0.818,總的基座姿態(tài)干擾為0.630;對(duì)于Test 2,最終的力可操作度為0.588,總的基座姿態(tài)干擾為0.083;對(duì)于Test 3,最終的力可操作度為0.736,總的基座姿態(tài)干擾為0.271.可以看出,末端執(zhí)行器最終實(shí)現(xiàn)了對(duì)目標(biāo)上抓捕點(diǎn)位姿和速度的匹配,同時(shí)最小化基座姿態(tài)干擾和最大化末端執(zhí)行器力可操作度兩個(gè)優(yōu)化指標(biāo)均有考慮,而選擇不同的優(yōu)化指標(biāo)最終將影響空間機(jī)器人系統(tǒng)的終端狀態(tài).

表2 針對(duì)不同優(yōu)化指標(biāo)的解集Table 2 Optimal solutions to different objectives

圖7 Pareto 前沿Fig.7 Pareto front

5 結(jié)論

空間機(jī)器人抓捕規(guī)劃過(guò)程中,需要考慮非合作目標(biāo)的運(yùn)動(dòng)和機(jī)械臂的抓捕能力;此外,充分利用機(jī)械臂與基座間的動(dòng)力學(xué)耦合特性,可有效提高空間機(jī)器人系統(tǒng)的燃料利用率,論文的主要工作列寫如下:

圖8 Test1 對(duì)應(yīng)的關(guān)節(jié)軌跡、基座與末端執(zhí)行器狀態(tài)Fig.8 Joint trajectories,base and end-effector’s states for Test1

圖8 Test1 對(duì)應(yīng)的關(guān)節(jié)軌跡、基座與末端執(zhí)行器狀態(tài)(續(xù))Fig.8 Joint trajectories,base and end-effector’s states for Test1(continued)

圖9 Test2 對(duì)應(yīng)的關(guān)節(jié)軌跡、基座與末端執(zhí)行器狀態(tài)Fig.9 Joint trajectories,base and end-effector’s states for Test2

圖10 Test3 對(duì)應(yīng)的關(guān)節(jié)軌跡、基座與末端執(zhí)行器狀態(tài)Fig.10 Joint trajectories,base and end-effector’s states for Test3

圖10 Test3 對(duì)應(yīng)的關(guān)節(jié)軌跡、基座與末端執(zhí)行器狀態(tài)(續(xù))Fig.10 Joint trajectories,base and end-effector’s states for Test3(continued)

(1)考慮機(jī)械臂抓捕能力、目標(biāo)運(yùn)動(dòng)、末端執(zhí)行器接近方向及TCP 與抓捕點(diǎn)相對(duì)距離等因素,確定了空間機(jī)械臂的最優(yōu)抓捕時(shí)機(jī),并給出了抓捕點(diǎn)在抓捕時(shí)刻的終端位姿和速度;

(2)考慮自由漂浮空間機(jī)器人的非完整約束,末端執(zhí)行器的運(yùn)動(dòng)不僅依賴于逆運(yùn)動(dòng)學(xué),也依賴于歷史運(yùn)動(dòng)路徑.由于本文所提方法無(wú)需對(duì)Jacobian 矩陣求逆,運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)奇異均得到了規(guī)避;

(3)采用Clamped B 樣條曲線對(duì)機(jī)械臂關(guān)節(jié)的軌跡進(jìn)行參數(shù)化,并對(duì)末端執(zhí)行器的終端狀態(tài)進(jìn)行了優(yōu)化;此外,將最小化基座干擾和最大化力可操作度作為抓捕后的優(yōu)化指標(biāo).

仿真結(jié)果驗(yàn)證了所提算法可應(yīng)用于抓捕時(shí)機(jī)的快速確定并生成優(yōu)化的抓捕關(guān)節(jié)軌跡,算法可有效擴(kuò)展其應(yīng)用于多臂機(jī)器人在軌抓捕過(guò)程之中.具有測(cè)量和參數(shù)不確定性的魯棒協(xié)調(diào)控制在本文中未充分考慮,將是論文的未來(lái)工作之一.