高添 吳云華 張梟 岳程斐

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

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

0 引言

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

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

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

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

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

1.1 機(jī)械臂模型

研究的多臂航天器由一個(gè)基座和四條相同的仿人型7-DOF 機(jī)械臂組成，機(jī)械臂均勻分布在基座四周，其中第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é)。利用標(biāo)準(zhǔn)D-H（Denavit-Hartenberg）參數(shù)化方法建立機(jī)械臂模型，多臂航天器整體構(gòu)型以及各臂DH 坐標(biāo)系如圖1 所示。單臂連桿參數(shù)列于表1。

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

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

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

圖2 機(jī)械臂臂型角Fig.2 Arm angle of manipulator

1.2 博弈原理

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

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

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

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

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

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

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

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

1.3 多機(jī)械臂雙層博弈模型

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

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

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

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

定義評(píng)價(jià)各機(jī)械臂末端與目標(biāo)距離收益函數(shù)f1如下：

其中，Lsafe1為機(jī)械臂末端與目標(biāo)之間的安全距離，Li-g為第i條機(jī)械臂與目標(biāo)之間的距離。

定義評(píng)價(jià)各機(jī)械臂末端之間避障的收益函數(shù)f2如下：

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

定義評(píng)價(jià)各機(jī)械臂末端圍捕中心與目標(biāo)距離的收益函數(shù)f3如下：

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

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

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

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

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

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

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

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

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

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

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

同時(shí)考慮到機(jī)械臂末端為離散點(diǎn)之間的直線運(yùn)動(dòng)，為保證末端運(yùn)動(dòng)性能，對(duì)末端進(jìn)行梯形速度曲線的直線運(yùn)動(dòng)規(guī)劃。

2 博弈問題求解

2.1 博弈算法流程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 納什均衡求解算法

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

算法1 納什均衡求解算法

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

3 仿真分析

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

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

3.1 機(jī)械臂末端追捕博弈仿真

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

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

圖7 機(jī)械臂末端至目標(biāo)的距離Fig.7 Distance between multi-manipulator end-effectors and target

3.2 機(jī)械臂雙層博弈仿真

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

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

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

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

4 結(jié)論

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