袁 帥，王義宇，張澤旭，李亞楠

(1. 哈爾濱工業(yè)大學(xué)航天學(xué)院，哈爾濱 150001；2. 塞薩克斯大學(xué)工程與設(shè)計學(xué)院，英國布萊頓 BN1 9RH)

0 引 言

隨著人類進(jìn)入太空的步伐逐漸加快,大規(guī)模常態(tài)化空間探測與開發(fā)時代的大幕徐徐拉開。新一輪的探月計劃、載人登月計劃甚至月球基地建設(shè)的宏偉藍(lán)圖也被提上日程。面對未知、復(fù)雜的空間和月面環(huán)境,人和機器人協(xié)同作業(yè)技術(shù)(簡稱空間人機協(xié)作技術(shù))將有力地支撐人類探索和利用空間資源,甚至輔助未來人類進(jìn)行長時間的太空旅行[1]?？臻g人機協(xié)作技術(shù)是空間機器人技術(shù)、傳感器技術(shù)、控制論和人工智能的深度交叉融合[2]。NASA、ESA和DLR等航天機構(gòu)從20世紀(jì)90年代就開始對空間人機協(xié)作技術(shù)的交互接口、感知、認(rèn)知和安全等多個維度開展廣泛且深入的研究工作[3-6],而其中的安全人機協(xié)作是需要解決的關(guān)鍵問題[7]。中國在空間人機協(xié)作方面的研究起步較晚,也有著迫切的需求,近地在軌服務(wù)甚至在月球/行星駐留科學(xué)探測與資源開發(fā)利用中也有廣闊的應(yīng)用前景[8]。2016年10月19日,天宮二號與神舟十一號對接后,航天員與機械手協(xié)同完成了拿電動工具擰螺釘、拆除隔熱材料等試驗,演示了空間人機協(xié)作技術(shù)的應(yīng)用場景和價值。

空間機器人的軌跡規(guī)劃是決定機器人執(zhí)行任務(wù)是否成功的關(guān)鍵問題?？臻g技術(shù)不斷發(fā)展以及機器人的重要應(yīng)用價值推動了國內(nèi)外學(xué)者對空間機器人軌跡規(guī)劃問題的廣泛研究工作。常見的研究思路是將軌跡規(guī)劃問題轉(zhuǎn)換成在動力學(xué)特性約束下目標(biāo)函數(shù)(能量、時間、位置)的優(yōu)化問題。岳程斐等[9]研究了空間站機器人操控任務(wù)的路徑規(guī)劃問題,提出了基于拓鄰域搜索蟻群算法三維全局規(guī)劃方法。胡忠華等[10]針對剛?cè)峄旌想p臂空間機器人抓持問題,提出了基于受限空間可操作度優(yōu)化的路徑規(guī)劃方法。Wang等[11]研究了在多關(guān)節(jié)運動特性約束下的空間雙機械臂軌跡規(guī)劃問題,并提出了基于粒子群優(yōu)化算法的求解方法。Wu等[12]提出了基于強化學(xué)習(xí)算法的空間雙機械臂系統(tǒng)軌跡規(guī)劃方法。廖一寰等[13]提出了基于Gauss偽譜方法和直接打靶法的混合規(guī)劃策略,并利用遺傳算法和SQP算法進(jìn)行無擾運動規(guī)劃。樊茂等[14]針對空間機器人抓捕碰撞問題,提出了基于多項式和加權(quán)目標(biāo)函數(shù)的機械臂關(guān)節(jié)軌跡規(guī)劃策略。Li等[15]引入深度強化學(xué)習(xí)策略構(gòu)建了針對冗余機器臂的運動規(guī)劃框架。Rybus等[16]利用B樣條曲線對軌跡進(jìn)行參數(shù)化簡化問題建模,進(jìn)而利用數(shù)值優(yōu)化方法實現(xiàn)了機器人軌跡生成。金榮玉等[17]針對空間機器人動力學(xué)奇異的回避問題,利用混合整數(shù)規(guī)劃的混沌粒子群優(yōu)化算法構(gòu)建路徑規(guī)劃策略。除上面提到的智能算法之外,也有部分學(xué)者[18-19]利用傳統(tǒng)或改進(jìn)的A*搜索算法、快速搜索隨機數(shù)方法(RRT)實現(xiàn)空間機械臂的避障安全路徑規(guī)劃。可見,現(xiàn)有的研究成果主要利用智能優(yōu)化方法、機器學(xué)習(xí)和傳統(tǒng)的搜索方法實現(xiàn)無人干預(yù)下機器人的軌跡規(guī)劃,對人機聯(lián)合作業(yè)的場景下充分考慮人行為安全的空間機器人軌跡規(guī)劃方法關(guān)注較少。

月面人機協(xié)作的安全路徑規(guī)劃需要重點考慮航天員行為因素,避免人和機器人在協(xié)同作業(yè)時發(fā)生危險,利用簡單高效的方式實現(xiàn)機器人在線調(diào)整軌跡以適應(yīng)人機共融的場景是研究的難點。鑒于此,本文面向未來載人登月任務(wù)月面人機協(xié)同作業(yè)的需求,針對月面人機協(xié)同采樣、協(xié)同搬運、安裝維修等作業(yè)場景,研究人機共融情況下的空間機械臂安全軌跡規(guī)劃算法。通過將安全軌跡規(guī)劃問題建模成凸優(yōu)化問題,利用其能快速求解的特點完成人機協(xié)同下的機械臂軌跡實時規(guī)劃,并構(gòu)建基于人體運動學(xué)特征的安全調(diào)節(jié)機制對凸優(yōu)化迭代求解過程進(jìn)行實時調(diào)控,實現(xiàn)安全軌跡規(guī)劃,保障空間人機協(xié)作的安全。

1 軌跡規(guī)劃分析與問題描述

面向月表人機聯(lián)合作業(yè)場景,解決月面機械臂在進(jìn)行抓取、采樣等工作時融合機器人動力學(xué)特性的安全軌跡規(guī)劃問題。首先,建立如下月面作業(yè)機械臂在關(guān)節(jié)角度q坐標(biāo)系下的動力學(xué)歐拉方程[20]：

(1)

圖1 人機協(xié)同采樣軌跡規(guī)劃示意圖

在人機聯(lián)合作業(yè)的背景下,機器人作業(yè)時間受到航天員月面外出作業(yè)時間的限制,以及自身能源的約束。因此,月面作業(yè)機械臂需要在綜合考慮時間最優(yōu)和能量最優(yōu)的情況下完成采樣、搬運等月面任務(wù)?？紤]到能量的消耗與機械臂的轉(zhuǎn)矩有著直接關(guān)系,本文采用如下有限積分形式表示機器人整個作業(yè)過程中的能量耗散:

(2)

式中:τi和τmax,i分別代表機械臂第i關(guān)節(jié)的驅(qū)動力矩以及其最大值?；诖?構(gòu)建如下考慮動力學(xué)特性(1)的優(yōu)化問題以生成整個作業(yè)過程路徑:

(3)

式中:Ξ(·)表示關(guān)節(jié)速度、加速度、加速度變化率以及轉(zhuǎn)矩約束;α>0表示時間約束和能量約束之間的權(quán)值。

注1.時間約束和能量約束的權(quán)值α的選擇需要考慮具體的作業(yè)任務(wù),可以通過構(gòu)造基于模糊邏輯的專家系統(tǒng)進(jìn)行設(shè)計并在線調(diào)整,或者將時間最優(yōu)和能量最優(yōu)構(gòu)建成一個博弈問題,通過帕累托前沿理論構(gòu)建時間-能量綜合性能函數(shù),以適應(yīng)不同的任務(wù)場景。

機器人動力學(xué)方程(1)呈現(xiàn)出非線性導(dǎo)致優(yōu)化問題(3)很難進(jìn)行直接求解,故對其進(jìn)行簡化處理。利用標(biāo)量軌跡坐標(biāo)s降低系統(tǒng)維度,簡化求解過程。因此,引入如下參數(shù)變換[21]：

(4)

式中:q′=?q/?s,q″=?2q/?s2,q″=?3q/?s3。

進(jìn)而將機器人的動力學(xué)方程(1)轉(zhuǎn)化成基于坐標(biāo)s的形式:

(5)

式中:

(6)

進(jìn)一步,引入中間變量a(s),b(s),c(s)使得如下等式成立:

(7)

式中:b′(s)=?b/?s,b″(s)=?2b/?s2。

根據(jù)坐標(biāo)變換(7),機器人動力學(xué)方程(1)可進(jìn)一步化簡為

τ(s)=m(s)a(s)+k(s)b(s)+g(s)+f(s)

(8)

q?(s)b(s))

(9)

(10)

以及

(11)

因此,根據(jù)參數(shù)坐標(biāo)變換(7)和上述指標(biāo)函數(shù),軌跡規(guī)劃(3)可以簡化為如下形式:

(12)

通過求解含角速度、角加速度、轉(zhuǎn)矩等多維約束的軌跡規(guī)劃問題(12),可以獲得機器人在期望路徑上的最優(yōu)運行軌跡。由于空間不確定性因素的影響,航天員需現(xiàn)場參與或調(diào)整機器人的作業(yè)過程,安全問題顯得尤為重要。為了保證航天員的安全,需要對式(12)中的機械臂動態(tài)特性約束和轉(zhuǎn)矩約束進(jìn)行動態(tài)調(diào)整,即在機器人運行過程中降低機器人的運行速度和轉(zhuǎn)矩大小,保證航天員能夠順暢地融入機器人作業(yè)任務(wù)。接下來的內(nèi)容將依次解決航天員在作業(yè)范圍外時機器人作業(yè)軌跡規(guī)劃問題,以及當(dāng)航天員進(jìn)入機器人作業(yè)范圍內(nèi)時保證人機安全的軌跡重新規(guī)劃問題。

2 無人環(huán)境下軌跡規(guī)劃方法

(13)

下面利用直接配點法將優(yōu)化問題(13)對s進(jìn)行離散化處理,構(gòu)造成一組優(yōu)化問題,即將軌跡在區(qū)間上進(jìn)行離散,獲得K+1個離散點:

0≤Sk≤1=SK,k=0,…,K

(14)

在這些離散點的基礎(chǔ)上對變量a(s),b(s),η(s),τ(s),δ(s)進(jìn)行離散化處理,分別構(gòu)建一組離散變量ak,bk,ηk,τk和δk,其中k=0,…,K,離散點的中間值可以通過插值獲得。下面對變量b(s)進(jìn)行舉例說明。當(dāng)sk≤s≤sk+1,利用如下線性插值關(guān)系

(15)

求解變量b(s)在任意兩離散點之間的數(shù)值。在直接配點法的基礎(chǔ)上,利用離散的變量bk以及其線性插值關(guān)系對指標(biāo)函數(shù)進(jìn)行如下離散化處理:

(16)

式中:Δsk=sk+1-sk。通過上述直接配點和線性插值處理,優(yōu)化問題(13)可以進(jìn)一步轉(zhuǎn)換成如下形式:

(17)

利用YALMIP等成熟的求解器可以高效在線求解軌跡規(guī)劃問題(17),即可實現(xiàn)在無人干預(yù)下的能量和時間混合最優(yōu)的機械臂軌跡規(guī)劃。但是,機器人在運行過程中航天員的介入會影響機械臂已經(jīng)規(guī)劃好的后續(xù)運行軌跡。因此,需要構(gòu)建考慮航天員行為特性的調(diào)整機制,動態(tài)完成機器人后續(xù)的軌跡規(guī)劃過程,以實現(xiàn)安全的人機融合。

3 人機共融下安全軌跡規(guī)劃動態(tài)調(diào)節(jié)

航天員出現(xiàn)在機器人的工作范圍內(nèi)時,機器人需要對動力學(xué)行為進(jìn)行實時調(diào)整,以確保航天員和機械臂的安全。在上一節(jié)內(nèi)容提出的軌跡規(guī)劃方法的基礎(chǔ)上,通過構(gòu)造安全機制來管理機器人動力學(xué)行為。具體地,通過改變無人干預(yù)情況下路徑規(guī)劃方法(17)的動力學(xué)約束來表征航天員進(jìn)入機器人的工作區(qū)域后的機器人動力學(xué)行為變化。為了在保證安全的同時實現(xiàn)效率的最優(yōu)化,受文獻(xiàn)[22]的啟發(fā),基于機器人末端和航天員相對速度以及航天員和機器人的相對距離來調(diào)整速度約束和力矩約束,構(gòu)造如下安全調(diào)節(jié)機制:

(18)

(19)

式中:λq,i,λτ,i,i=1,…,N,為分段函數(shù)的系數(shù),N為預(yù)先設(shè)定的運動狀態(tài)空間分區(qū)數(shù)量;Ωi代表運動狀態(tài)空間劃分區(qū)域。需要強調(diào)的是,空間分區(qū)數(shù)量的增加可以提高機器人運動變化的順滑度,但會相應(yīng)地增加算法的復(fù)雜度。因此,人機協(xié)作下安全軌跡規(guī)劃可以在無人干預(yù)情況下規(guī)劃方法(17)的基礎(chǔ)上對速度約束條件和力矩約束條件進(jìn)行調(diào)整,獲得如下的軌跡規(guī)劃方法:

(20)

當(dāng)機器人從在無人干涉的情況下按照(17)進(jìn)行迭代求解時,航天員突然進(jìn)入安全范圍以內(nèi),需要對數(shù)值求解過程進(jìn)行中斷,在下一次進(jìn)行迭代時考慮新的動力學(xué)約束,整個流程如圖2所示。

圖2 人機協(xié)同作業(yè)安全軌跡規(guī)劃流程圖

優(yōu)化問題(20)中含有δ(s)ξ(s)雙線性項,導(dǎo)致無法通過凸優(yōu)化求解工具對其進(jìn)行求解。下面將利用McCormick包絡(luò)對雙線性約束進(jìn)行放松處理。定義一個新變量ψ=δξ。根據(jù)文獻(xiàn)[24],將雙線性約束重新構(gòu)造成如下形式:

(21)

式中:δ∈[δmin,δmax]且ζ∈[ζmin,ζmax]。此時,非凸優(yōu)化問題成功地轉(zhuǎn)化為凸優(yōu)化問題。

注2.當(dāng)機器人和航天員之間產(chǎn)生物理接觸行為時,可以利用文獻(xiàn)[25]中提出的基于微分博弈論的人機阻抗控制方法以及航天員運動參數(shù)的自適應(yīng)估計方法,實現(xiàn)在保證安全前提下人和機器人的協(xié)同作業(yè)任務(wù)。

4 數(shù)值仿真

本小節(jié)以月面人機協(xié)同采樣和搬運的作業(yè)場景為例進(jìn)行數(shù)值仿真,驗證機器人安全軌跡規(guī)劃算法的有效性。

本算例所研究的機器人系統(tǒng)由月球車平臺及1個四自由度機械臂組成。機械臂末端用于采樣的關(guān)節(jié)只需在起始點根據(jù)樣品實際位置進(jìn)行調(diào)整,從而完成樣品的抓取任務(wù),而在樣品移動的過程中機械臂末端關(guān)節(jié)角度不變動,可將其認(rèn)為是三自由度。機械臂基座安裝在距地面高0.2 m的平臺上,設(shè)定基座平面為0系面,再采用標(biāo)準(zhǔn)D-H參數(shù)建模方法對整體機械臂坐標(biāo)系建模,如圖3所示。各關(guān)節(jié)的D-H參數(shù)表如表1所示,動力學(xué)參數(shù)如表2所示,另外,針對此機械臂的相關(guān)約束如表3所示。

表1 機械臂D-H參數(shù)

表2 機械臂動力學(xué)參數(shù)

表3 機械臂相關(guān)約束

圖3 機械臂D-H坐標(biāo)系

其中,ri表示臂桿i-1與臂桿i質(zhì)心之間距離,JLi和JTi分別表示臂桿i縱軸和橫軸的慣量矩陣。

設(shè)定機器人執(zhí)行采樣任務(wù)的目標(biāo)為采取樣品質(zhì)量約60 g,折算為末端作用力fext約為0.1 N,將樣品從月面轉(zhuǎn)移至距月面0.8 m高的樣品儲存罐中。設(shè)計機械臂末端軌跡為螺旋線作為仿真的期望軌跡,如圖4所示。

圖4 期望軌跡示意圖

在YALMIP環(huán)境下對優(yōu)化問題進(jìn)行建模,選取離散點數(shù)目K=100即劃分整個軌跡為100個網(wǎng)格。軌跡坐標(biāo)s的步長Δs=0.01,網(wǎng)格密度足以達(dá)到精度要求。單個網(wǎng)格內(nèi)機械臂軌跡很短,可認(rèn)為此段中航天員運動變化細(xì)微,其影響可忽略。因此實時的安全評估可簡化為對此時刻網(wǎng)格點狀態(tài)的評估計算。

軌跡規(guī)劃求解過程按網(wǎng)格點進(jìn)行迭代的順序計算和評估。從起始網(wǎng)格點開始進(jìn)行凸優(yōu)化問題的求解,利用結(jié)果中下一網(wǎng)格點的狀態(tài)進(jìn)行評估得到新的約束。一次迭代結(jié)束,機械臂行進(jìn)至下一網(wǎng)格點,重復(fù)過程至機械臂行至最終的網(wǎng)格點,結(jié)束迭代。

當(dāng)航天員始終在機械臂安全距離之外時,對優(yōu)化問題(18)進(jìn)行仿真求解,可得到運動軌跡如圖5所示,關(guān)節(jié)角度、關(guān)節(jié)角速度、關(guān)節(jié)角加速度以及關(guān)節(jié)扭矩的曲線如圖6所示,機械臂末端速度曲線如圖7所示。

圖5 實際軌跡曲線(無人)

圖6 無人情況下關(guān)節(jié)運動變量曲線

圖7 機械臂末端速度曲線(無人)

由圖6和圖7可得,無人環(huán)境下機械臂將嚴(yán)格按表3所示各約束進(jìn)行求解,36.2 s完成了沿期望軌跡從起點到終點的全過程,關(guān)節(jié)角速度、關(guān)節(jié)角加速度、關(guān)節(jié)扭矩的變化均在約束之內(nèi)。

當(dāng)有航天員進(jìn)入到安全距離內(nèi)時,機械臂將以人員進(jìn)入時刻狀態(tài)作為起始狀態(tài),按新的約束條件重新計算各參量的變化。在實際考慮了航天員位置、速度和體型的影響下進(jìn)行安全評估:

1) 將航天員在月面行走的過程近似為一個長0.5 m、寬0.5 m、高1.8 m的長方體在空間中平移,質(zhì)心取在距月表高1.0 m的切面幾何中心,根據(jù)在網(wǎng)格節(jié)點處的長方體位置和機械臂末端位置,可計算機械臂末端至長方體的最短距離作為相對位置l;

2) 航天員速度上限即長方體移動的最大速度取為月面行走的平均速度2.2 km/h,為了盡可能模擬航天員行動,在靠近過程中速度vh線性減小至0,遠(yuǎn)離過程逐漸加速至速度上限;

進(jìn)一步考慮安全性,為機械臂設(shè)置安全半徑為0.2 m,危險半徑為0.1 m,并在原安全評估公式中進(jìn)行分段處理:

(22)

通過求解優(yōu)化問題(21),可以得到有人情況下新的運動軌跡如圖8、圖9所示;關(guān)節(jié)角度、關(guān)節(jié)角速度、關(guān)節(jié)角加速度以及關(guān)節(jié)扭矩的曲線如圖10所示,機械臂末端速度曲線如圖11所示。

圖8 實際軌跡曲線(t=0～14.536 s,有人)

圖9 實際軌跡曲線(t=14.536～68.50 s,有人)

圖10 有人情況下關(guān)節(jié)運動變量曲線

圖11 機械臂末端速度曲線(有人)

圖8、圖9表示了實際考慮航天員影響的機械臂運動軌跡,其中實線為機械臂已完成的運動軌跡,虛線為未完成的期望軌跡。另用曲線描述了航天員的運動軌跡,箭頭方向為航天員運動方向。圖8模擬了機械臂0～14.536 s的運動過程,此時航天員逐漸靠近機械臂但還未進(jìn)入到工作空間之內(nèi)。圖9模擬了機械臂14.536～68.50 s的運動過程,航天員在此時間段進(jìn)入到工作空間,機械臂進(jìn)行減速。68.50 s之后,航天員逐漸遠(yuǎn)離,機械臂沿期望軌跡運動至終點。

圖10、圖11的曲線均在考慮航天員影響下得到,與無人環(huán)境下得到的圖6、圖7進(jìn)行對比分析:

(1) 在14.536 s前,即航天員未進(jìn)入工作空間中時,圖10、圖11各變量的變化與圖6、圖7中相同,表示此時段機械臂按無人環(huán)境下得到的解進(jìn)行運動;

(2) 在14.536 s時即有人介入之后,進(jìn)一步限制了表3中所示各約束。因此,圖10和圖11中關(guān)節(jié)角速度、關(guān)節(jié)角加速度以及末端速度都快速下降,機械臂在此階段以非常緩慢的速度進(jìn)行運動,符合預(yù)期;

(3) 在68.50 s后即航天員離開工作空間之后,各約束重新放寬為表3中各值。由圖10、圖11可得,在68.50 s時關(guān)節(jié)角速度、關(guān)節(jié)角加速度以及末端速度都快速上升,表明此時刻之后機械臂將重新加速,快速運動至終點,滿足安全性的要求。

通過數(shù)值仿真可以看出本論文提出的安全路徑規(guī)劃算法可以有效地保證人機聯(lián)合作業(yè)的安全。

5 結(jié) 論

本文面向未來月面人機協(xié)同作業(yè)任務(wù),提出了一種保證協(xié)作安全的機器人軌跡規(guī)劃方法。該方法考慮多種機器人動力學(xué)約束,構(gòu)建了基于時間和能量的組合最優(yōu)函數(shù),利用凸優(yōu)化理論和簡潔的在線安全調(diào)節(jié)機制實現(xiàn)了人機共融下的機器人軌跡規(guī)劃。利用人機月面協(xié)同采樣這一具體任務(wù)場景進(jìn)行了仿真驗證,通過對無人和有人兩種情況的仿真對比,表明本文提出的安全軌跡規(guī)劃機制能夠保障機器人作業(yè)效率的同時兼顧航天員的安全,可為未來月面航天員協(xié)作機器人的開發(fā)提供理論支撐。