月面人機協同作業機械臂安全軌跡規劃

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

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

0 引 言

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

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

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

1 軌跡規劃分析與問題描述

面向月表人機聯合作業場景,解決月面機械臂在進行抓取、采樣等工作時融合機器人動力學特性的安全軌跡規劃問題。首先,建立如下月面作業機械臂在關節角度q坐標系下的動力學歐拉方程[20]：

(1)

圖1 人機協同采樣軌跡規劃示意圖

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

(2)

式中:τi和τmax,i分別代表機械臂第i關節的驅動力矩以及其最大值。基于此,構建如下考慮動力學特性(1)的優化問題以生成整個作業過程路徑:

(3)

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

注1.時間約束和能量約束的權值α的選擇需要考慮具體的作業任務,可以通過構造基于模糊邏輯的專家系統進行設計并在線調整,或者將時間最優和能量最優構建成一個博弈問題,通過帕累托前沿理論構建時間-能量綜合性能函數,以適應不同的任務場景。

機器人動力學方程(1)呈現出非線性導致優化問題(3)很難進行直接求解,故對其進行簡化處理。利用標量軌跡坐標s降低系統維度,簡化求解過程。因此,引入如下參數變換[21]：

(4)

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

進而將機器人的動力學方程(1)轉化成基于坐標s的形式:

(5)

式中:

(6)

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

(7)

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

根據坐標變換(7),機器人動力學方程(1)可進一步化簡為

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

(8)

q?(s)b(s))

(9)

(10)

以及

(11)

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

(12)

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

2 無人環境下軌跡規劃方法

(13)

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

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

(14)

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

(15)

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

(16)

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

(17)

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

3 人機共融下安全軌跡規劃動態調節

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

(18)

(19)

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

(20)

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

圖2 人機協同作業安全軌跡規劃流程圖

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

(21)

式中:δ∈[δmin,δmax]且ζ∈[ζmin,ζmax]。此時,非凸優化問題成功地轉化為凸優化問題。

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

4 數值仿真

本小節以月面人機協同采樣和搬運的作業場景為例進行數值仿真,驗證機器人安全軌跡規劃算法的有效性。

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

表1 機械臂D-H參數

表2 機械臂動力學參數

表3 機械臂相關約束

圖3 機械臂D-H坐標系

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

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

圖4 期望軌跡示意圖

在YALMIP環境下對優化問題進行建模,選取離散點數目K=100即劃分整個軌跡為100個網格。軌跡坐標s的步長Δs=0.01,網格密度足以達到精度要求。單個網格內機械臂軌跡很短,可認為此段中航天員運動變化細微,其影響可忽略。因此實時的安全評估可簡化為對此時刻網格點狀態的評估計算。

軌跡規劃求解過程按網格點進行迭代的順序計算和評估。從起始網格點開始進行凸優化問題的求解,利用結果中下一網格點的狀態進行評估得到新的約束。一次迭代結束,機械臂行進至下一網格點,重復過程至機械臂行至最終的網格點,結束迭代。

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

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

圖6 無人情況下關節運動變量曲線

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

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

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

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

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

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

(22)

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

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

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

圖10 有人情況下關節運動變量曲線

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

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

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

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

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

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

通過數值仿真可以看出本論文提出的安全路徑規劃算法可以有效地保證人機聯合作業的安全。

5 結 論

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