高添 吳云華 張梟 岳程斐

1(哈爾濱工業(yè)大學(xué)(深圳) 空間科學(xué)與應(yīng)用技術(shù)研究院 深圳 518055)

2(南京航空航天大學(xué)航天學(xué)院 南京 211106)

0 引言

隨著航天器在軌服務(wù)技術(shù)的飛速發(fā)展，空間機械臂和裝配有空間機械臂的航天器已成為空間在軌操作的主要對象，將承擔(dān)在軌裝配、維修、燃油加注、運輸和空間碎片清理等諸多工作[1,2]。在軌操控作業(yè)中，機械臂的運動規(guī)劃和控制始終是研究的熱點，例如在笛卡爾空間進行空間機械臂點對點的規(guī)劃[3,4]等。隨著在軌操控任務(wù)復(fù)雜性的提升，如在軌靈巧操作、移動爬行等，未來操控任務(wù)均要求空間機械臂能多臂協(xié)同地開展工作，例如蜘蛛制造（SpiderFab）[5]等。這類任務(wù)除了要求航天器各機械臂實現(xiàn)精確的位置控制外，還要保證各臂自身和相互之間不發(fā)生碰撞。因此，考慮自避障的多臂路徑規(guī)劃是實現(xiàn)精準在軌操控的前提和基礎(chǔ)。

目前，針對機械臂路徑規(guī)劃及避障問題的研究有多種方法。Warren等[6,7]提出利用人工勢場法（Artificial Potential Field，APF）實現(xiàn)機械臂避障，該方法能快速有效地提供安全避障路徑，但是易陷入局部最小點處。Riky等[8]改進人工勢場算法，通過建立輔助函數(shù)求解局部極小值問題。Bohlin等[9]提出概率路線規(guī)劃（Probabilistic Roadmap Planners，PRMs）方法，通過最小化規(guī)劃期間執(zhí)行的碰撞檢查次數(shù)，達到最小化規(guī)劃運行時間的目的。Jia等[10]提出一種利用A-star 算法在機械臂的自由運動空間進行路徑搜索，從而實現(xiàn)機械臂無碰撞路徑規(guī)劃的方法。Duchoň等[11]對A-star 算法在計算時間和路徑優(yōu)化方面進行一些改進。Yao等[12]也對傳統(tǒng)A-star 算法進行評價函數(shù)加權(quán)處理的改進，同時引入人工搜索標記，可以有效快速擺脫障礙陷阱。上述方法多適用于單機械臂路徑規(guī)劃及避障等情形，針對多機械臂的研究較少。

與傳統(tǒng)單機械臂僅考慮路徑規(guī)劃及過程中實現(xiàn)外部避障的目標有所不同，多機械臂還需要考慮各條臂之間的協(xié)同規(guī)劃以及自身避障，特別是動態(tài)環(huán)境下的規(guī)劃與自避障。針對多機械臂的路徑規(guī)劃問題，可考慮將各臂看成智能體，通過博弈算法等智能策略進行設(shè)計。Guo等[13]利用博弈原理改進Q-Learning 算法進行智能體的避障協(xié)作路徑規(guī)劃。Shen等[14]提出一種博弈論數(shù)據(jù)融合輔助算法用于無人機在對抗環(huán)境中的協(xié)作和智能路徑規(guī)劃。Lee等[15]將混合博弈策略用于多目標進化算法，應(yīng)用于無人機系統(tǒng)任務(wù)路徑規(guī)劃。目前，關(guān)于博弈論應(yīng)用于機械臂路徑規(guī)劃的算法還較少。Wang等[16]提出一種基于博弈的快速拓展隨機樹（Rapidly-exploring Random Tree，RRT）算法，將博弈論用于尋找隨機樹的新網(wǎng)格。該方法僅考慮單機械臂路徑規(guī)劃，不適用于多機械臂自避障約束下的路徑協(xié)同規(guī)劃。

為解決多臂航天器的智能規(guī)劃問題，本文從多機械臂協(xié)同圍捕任務(wù)出發(fā)，同時考慮圍捕過程中各機械臂的避障問題，提出一種基于雙層博弈的多機械臂路徑規(guī)劃方法。本文給定了多臂航天器的基本構(gòu)型及建立的多機械臂運動學(xué)模型；在博弈原理的基礎(chǔ)上建立了多機械臂的博弈模型，通過機械臂末端追蹤目標的博弈實現(xiàn)對目標的圍捕，同時在圍捕過程中嵌套肘關(guān)節(jié)博弈實現(xiàn)避障，通過雙層博弈實現(xiàn)多條機械臂之間的協(xié)同規(guī)劃；給出了雙層博弈的基本算法流程及其納什均衡解的求解策略；對多機械臂圍捕案例進行仿真分析，驗證了所提出算法在末端精確跟蹤和肘部避障方面的有效性和實用性。

1 模型及數(shù)學(xué)原理

1.1 機械臂模型

研究的多臂航天器由一個基座和四條相同的仿人型7-DOF 機械臂組成，機械臂均勻分布在基座四周，其中第1，2，3 關(guān)節(jié)構(gòu)成肩關(guān)節(jié)，第4 關(guān)節(jié)為肘關(guān)節(jié)，第5，6，7 關(guān)節(jié)構(gòu)成腕關(guān)節(jié)。利用標準D-H（Denavit-Hartenberg）參數(shù)化方法建立機械臂模型，多臂航天器整體構(gòu)型以及各臂DH 坐標系如圖1 所示。單臂連桿參數(shù)列于表1。

圖1 多臂航天器整體構(gòu)型及坐標系定義Fig.1 Configuration of the multi-arm spacecraft and the definition of the coordinates

表1 機械臂DH 參數(shù)Table 1 DH parameters of the manipulator

針對該仿人型7-DOF 機械臂，定義其基座點B與肩關(guān)節(jié)點S之間的連線和末端腕關(guān)節(jié)點W形成的平面為參考平面，其肩關(guān)節(jié)點S，肘關(guān)節(jié)點E和腕關(guān)節(jié)點W形成的平面為臂平面，定義臂平面和參考平面之間的夾角為臂型角φ[17]，機械臂臂型角示意如圖2 所示。由此得知，在給定末端位姿的情況下，仿人型機械臂的臂平面可以繞SW運動，臂型角的相關(guān)應(yīng)用將在1.3 節(jié)闡述。

圖2 機械臂臂型角Fig.2 Arm angle of manipulator

1.2 博弈原理

博弈論是研究多個具備進行決策的行為主體在相互作用時，如何選擇合適的決策以達到總體收益最大的方法，也即達到一種均衡狀態(tài)，稱之為納什均衡。其中，具備決策行為的主體為博弈方，主體所選擇的決策形成的組合稱之為策略集。根據(jù)已制定的收益數(shù)學(xué)模型，對策略集里的任一組合均能求得一組收益值，其組成的集合稱之為收益集。通過對收益集求納什均衡解得到的一組策略組合即為本次博弈中各博弈方所采取的最佳決策。

綜上所述，博弈過程可以描述如下。

（1）假設(shè)共有n個博弈方參與博弈，記為p1，p2，...，pn，每個博弈方可供選擇的策略集分別為S1，S2，...，Sn，每個策略集內(nèi)的可選擇策略數(shù)量分別為n1，n2，...，nn。

（2）假設(shè)各博弈方在本次博弈中所采取策略為s1∈S1，s2∈S，...，sn∈Sn。

（3）根據(jù)制定的收益數(shù)學(xué)模型得到博弈方采取的一組策略對應(yīng)的一組收益(r1，r2，...，rn)=f(s1，s2，...，sn)，所有的收益組合形成收益集R，對收益集R求得納什均衡解，記作

其中，sij表示第i個博弈方選擇對應(yīng)策略集里的第j個策略。易知，納什均衡解存在唯一解、多解以及無解的情況。

當(dāng)存在唯一解時，可知此時該唯一解即為本次博弈各方采取的最優(yōu)策略。

當(dāng)無解時，此時博弈無法達到均衡，無法保證取得最大收益。為了求得一組策略使得總體收益不至于最差，因此采用max-min 原則進行選取，對于任意s1∈S1，...，si-1∈Si-1，si+1∈Si+1，...，sn∈Sn，滿足

1.3 多機械臂雙層博弈模型

在機械臂模型和博弈理論基礎(chǔ)上給出多機械臂航天器的雙層博弈模型。多臂航天器的四條仿人型機械臂分別為四個博弈方。為簡化模型的復(fù)雜程度，將機械臂工作空間柵格化，機械臂末端軌跡視為離散點，機械臂末端的運動視為點到點之間的直線運動。易知，從當(dāng)前一點運動到下一點共有18 個方向可供選擇，如圖3 所示，加上停止運動共19 個策略，即構(gòu)成了各條機械臂追捕博弈的策略集。

圖3 由19 個可能動作構(gòu)成的機械臂追捕博弈策略集Fig.3 Strategy set of the manipulator tracking game formed by the 19 potential actions

為評估各機械臂所選擇策略的優(yōu)劣，引入收益數(shù)學(xué)模型，構(gòu)建策略集對應(yīng)的收益集。收益函數(shù)可表示如下：

其中，f1為評價各機械臂末端與目標距離的收益函數(shù)，f2為評價各機械臂末端之間避障的收益函數(shù)，f3為評價各機械臂末端圍捕中心與目標距離的收益函數(shù)；α1,α2,α3為權(quán)重系數(shù)。

定義評價各機械臂末端與目標距離收益函數(shù)f1如下：

其中，Lsafe1為機械臂末端與目標之間的安全距離，Li-g為第i條機械臂與目標之間的距離。

定義評價各機械臂末端之間避障的收益函數(shù)f2如下：

其中，Lsafe2為各機械臂末端之間的安全距離，Lij為第i條 機械臂末端與第j條機械臂末端之間距離的最小值。

定義評價各機械臂末端圍捕中心與目標距離的收益函數(shù)f3如下：

其中，(L1-g+L2-g+L3-g+L4-g)/4 為各機械臂末端形成的四邊形中心與目標的距離，Lr為中心到目標的收益參考距離。

值得注意，安全距離Lsafe1、Lsafe2、參考收益距離Lr及各項分收益占總收益的比例α1，α2，α3均為常數(shù)，具體數(shù)值由末端離散點之間的步長決定，在此不做深入研究。

在完成多機械臂末端追捕目標博弈后，考慮多機械臂在運動過程中可能發(fā)生臂與臂之間的碰撞，增加肘關(guān)節(jié)之間的避障博弈，考慮在機械臂末端位姿不變的情況下，機械臂的肘關(guān)節(jié)點E可以繞肩關(guān)節(jié)點S和腕關(guān)節(jié)點W之間的連線旋轉(zhuǎn)，旋轉(zhuǎn)角度φ即為臂型角，因此，定義機械臂的臂型角為肘關(guān)節(jié)避障博弈的策略集。同理，構(gòu)建機械臂肘關(guān)節(jié)避障博弈的收益集，收益函數(shù)表示如下：

其中，g1為評價各機械臂肘關(guān)節(jié)之間距離的收益函數(shù)，g2為評價各機械臂肘關(guān)節(jié)之間均勻分布的收益函數(shù)。β1和β2為權(quán)重系數(shù)。

定義評價各機械臂肘關(guān)節(jié)之間距離的收益函數(shù)g1如下：

定義評價各機械臂肘關(guān)節(jié)之間均勻分布的收益函數(shù)g2如下：

其中，?r為機械臂肘關(guān)節(jié)均勻分布參考收益角度，θitoj為第i條 機械臂肘關(guān)節(jié)與第j條機械臂肘關(guān)節(jié)之間在水平面投影的對心夾角。機械臂肘關(guān)節(jié)對心夾角如圖4所示。

圖4 機械臂肘關(guān)節(jié)對心夾角Fig.4 Angle against the center of manipulator’s elbow joint

同樣，肘關(guān)節(jié)避障博弈收益模型中的安全距離，參考收益角度?r和各項分收益所占比例β1，β2均為常數(shù)，具體數(shù)值由臂型角變化范圍決定。

在機械臂實際運動過程中，考慮機械臂末端運動速度不能突變，以及機械臂臂型角度不能突變，由此引入約束集對策略集加以約束。假定某一機械臂在上一步博弈中末端運動方向為D0，在本次博弈中運動方向為D；在上一步博弈中臂型角為φ0，在本次博弈中臂型角為φ。則由最優(yōu)化理論，博弈問題可以描述為

其中，C為臂型角不引起突變的閾值。

同時考慮到機械臂末端為離散點之間的直線運動，為保證末端運動性能，對末端進行梯形速度曲線的直線運動規(guī)劃。

2 博弈問題求解

2.1 博弈算法流程

將單步博弈拓展至連續(xù)博弈，同時為提高算法的效率，增加肘關(guān)節(jié)碰撞檢測算法，即各機械臂在進行末端追捕博弈時，檢測當(dāng)各肘關(guān)節(jié)之間距離在碰撞安全閾值內(nèi)時，才執(zhí)行肘關(guān)節(jié)避障博弈。由此得到多臂航天器雙層博弈算法流程如下。

（1）設(shè)定博弈初始場景，生成柵格模型，初始化各機械臂構(gòu)型，隨機生成追捕目標位置。

（2）由各機械臂末端與目標之間的相對位置判斷是否捕獲成功（條件為某一機械臂末端與目標距離保持在給定捕獲范圍內(nèi)）。若捕獲成功，則博弈結(jié)束，反之執(zhí)行（3）。

（3）由各機械臂肘關(guān)節(jié)相對位置判斷是否需要執(zhí)行避障博弈（條件為各機械臂肘關(guān)節(jié)之間距離在碰撞安全閾值內(nèi)）。若判斷需要，則執(zhí)行（8），反之執(zhí)行（4）。

（4）分析各機械臂末端當(dāng)前約束，得到下一步各機械臂末端可選擇策略，形成末端策略集。

（5）根據(jù)式（4）的收益數(shù)學(xué)模型計算得到末端策略集對應(yīng)的末端收益集。

（6）對末端收益集求取納什均衡解，得到各機械臂末端下一步最優(yōu)策略組合。

（7）各機械臂執(zhí)行該策略組合運動到下一步，并轉(zhuǎn)到（2）判斷捕獲是否成功。

（8）分析各機械臂末端和肘關(guān)節(jié)當(dāng)前約束，得到下一步各機械臂末端和肘關(guān)節(jié)可選擇策略，形成末端和肘關(guān)節(jié)策略集。

（9）根據(jù)式（4）和式（8）的收益數(shù)學(xué)模型計算得到末端和肘關(guān)節(jié)策略集對應(yīng)的末端和肘關(guān)節(jié)收益集。

（10）對末端和肘關(guān)節(jié)收益集分別求取納什均衡解，得到各機械臂末端和肘關(guān)節(jié)下一步最優(yōu)策略組合。

（11）各機械臂執(zhí)行該策略組合運動到下一步，并轉(zhuǎn)到（2）判斷捕獲是否成功。

如此，完成機械臂追捕目標和肘關(guān)節(jié)避障雙層博弈策略，機械臂雙層博弈流程如圖5 所示。

圖5 雙層博弈算法流程Fig.5 Flow chart of the two-level game algorithm

2.2 納什均衡求解算法

在給出納什均衡解滿足的數(shù)學(xué)表達后，以四方博弈為例，給出求取納什均衡解的完整算法如下。

算法1 納什均衡求解算法

由此，針對多機械臂博弈模型，結(jié)合多機械臂雙層博弈算法及多機械臂末端追捕博弈和肘關(guān)節(jié)博弈的納什均衡解求解策略，即可得到所需的機械臂路徑規(guī)劃結(jié)果。

3 仿真分析

為驗證上述雙層博弈算法的有效性和實用性，對提出的模型和算法進行仿真分析。以仿人型7-DOF多機械臂為研究對象，設(shè)定博弈場景空間范圍為5 m×5 m×5 m，假定多臂航天器能進行精確的姿態(tài)位置控制，各機械臂根部的空間位置坐標以及初始構(gòu)型關(guān)節(jié)角見表2。

表2 機械臂仿真初始參數(shù)Table 2 Initial parameters of the simulation

3.1 機械臂末端追捕博弈仿真

按照多機械臂末端追捕博弈算法對機械臂末端追捕目標的博弈行為進行仿真，得到機械臂末端追捕博弈軌跡如圖6 所示，其中機械臂末端到目標的距離變化如圖7 所示。從圖6 可看出，各機械臂末端對于移動的被追捕目標均能表現(xiàn)出有效的追捕。

圖6 機械臂末端追捕博弈軌跡Fig.6 Trajectory diagram of the end effector in the hunting game

圖7 機械臂末端至目標的距離Fig.7 Distance between multi-manipulator end-effectors and target

3.2 機械臂雙層博弈仿真

按照多機械臂末端追捕和肘關(guān)節(jié)避障雙層博弈算法進行機械臂雙層博弈全過程仿真實驗，截取的部分機械臂博弈行為如圖8 所示。

圖8 部分機械臂雙層博弈行為Fig.8 Partial schematic diagram of the manipulators in the two-level game

從圖中可以看出，機械臂在雙層博弈下末端可以快速準確地接近運動目標進行追捕，同時肘關(guān)節(jié)運動能夠保證避障，臂與臂之間相對于基座的空間分布較為均勻。為了說明均勻性問題，分別提取在有無肘關(guān)節(jié)避障博弈情況下各機械臂肘關(guān)節(jié)之間在水平面投影的對心夾角進行對比分析，如圖9 所示。從圖9 可看出，無肘關(guān)節(jié)避障博弈時，各機械臂肘關(guān)節(jié)之間對心夾角隨著博弈的進行呈發(fā)散狀態(tài)；在增加肘關(guān)節(jié)避障博弈之后，各機械臂肘關(guān)節(jié)之間對心夾角均能保持在9 0°±10°范圍內(nèi)，肘關(guān)節(jié)分布均勻性良好。

圖9 各機械臂肘關(guān)節(jié)均勻分布指標Fig.9 Index of the distribution of elbow joint

4 結(jié)論

針對航天器在軌服務(wù)多臂協(xié)同操作路徑規(guī)劃和避障問題，提出了一種基于雙層博弈理論的多機械臂路徑規(guī)劃算法。研究結(jié)果表明該算法在實現(xiàn)多機械臂末端追捕目標的同時，可以實現(xiàn)多機械臂肘關(guān)節(jié)之間的避障功能，且運動過程中各機械臂空間分布呈現(xiàn)均勻的態(tài)勢。取得的成果可為多臂航天器智能路徑規(guī)劃提供新的思路。