一種自主∕遙控水下機(jī)器人共享控制方法

王興華，田 宇

（1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽(yáng) 110016；2.中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院，遼寧 沈陽(yáng) 110016；3.中國(guó)科學(xué)院大學(xué)，北京100049）

1 引言

自主∕遙控水下機(jī)器人（Autonomous and Remotely operated underwater Vehicle，ARV）是一種綜合自主水下機(jī)器人和遙控水下機(jī)器人特點(diǎn)的新型水下機(jī)器人，可以在自主、半自主、遙控等模式下執(zhí)行搜索、觀察及作業(yè)任務(wù)，其研發(fā)和在海洋觀測(cè)、探測(cè)中的應(yīng)用成為近年來(lái)國(guó)際上水下機(jī)器人發(fā)展的熱點(diǎn)[1-2]。目前，由于ARV自主控制技術(shù)水平的限制，ARV執(zhí)行諸多環(huán)境探索特別是作業(yè)等任務(wù)仍需操作人員的監(jiān)控或遙控[3]。因此，實(shí)現(xiàn)高效融合操作人員遙控和ARV自主控制的共享控制[4]，以充分發(fā)揮ARV所具有的人機(jī)協(xié)同控制特色優(yōu)勢(shì)進(jìn)而提高ARV的任務(wù)性能，成為近年來(lái)ARV高效應(yīng)用需要研究和解決的重要問(wèn)題。

共享控制指人和自主控制系統(tǒng)之間相互協(xié)調(diào)，協(xié)同對(duì)機(jī)器人進(jìn)行控制的控制策略[5]。共享控制的關(guān)鍵是人和自主控制系統(tǒng)控制權(quán)重的分配，其主要分為仲裁法和融合法兩種。仲裁法通過(guò)在任務(wù)執(zhí)行過(guò)程中根據(jù)任務(wù)需求選擇遙控或自主控制來(lái)實(shí)施對(duì)機(jī)器人的共享控制。仲裁法在控制信號(hào)切換時(shí)往往存在控制命令和機(jī)器人狀態(tài)的不連續(xù)和不穩(wěn)定問(wèn)題[6]，因此為避免仲裁法存在的問(wèn)題，研究人員更多的應(yīng)用融合法研究和實(shí)施共享控制，即根據(jù)任務(wù)和控制要求設(shè)計(jì)有效的算法實(shí)時(shí)融合操作人員和自主控制系統(tǒng)的控制命令以對(duì)機(jī)器人進(jìn)行共享控制。基于融合法的共享控制在智能輪椅[7-8]、移動(dòng)機(jī)器人[9-10]等領(lǐng)域已經(jīng)開(kāi)展了許多研究，結(jié)果表明可以在有效避免仲裁法存在的問(wèn)題同時(shí)取得良好的共享控制效果（提升機(jī)器人的任務(wù)性能和減輕操作人員的工作負(fù)擔(dān)）。

在水下機(jī)器人領(lǐng)域，針對(duì)水下機(jī)器人共享控制的研究相對(duì)較少，且多采用仲裁法研究和設(shè)計(jì)共享控制方法[3-4，11]。因此，面向ARV環(huán)境探索任務(wù)過(guò)程中的共享控制需求，針對(duì)仲裁法存在的問(wèn)題，采用融合法研究和設(shè)計(jì)了一種基于多目標(biāo)優(yōu)化的共享控制方法，并對(duì)其性能進(jìn)行計(jì)算機(jī)仿真驗(yàn)證和評(píng)估。

2 任務(wù)與控制結(jié)構(gòu)

未知環(huán)境探索是ARV所需執(zhí)行的重要任務(wù)之一。通常情況下，該任務(wù)中全局環(huán)境信息未知，無(wú)法預(yù)先設(shè)定ARV的運(yùn)動(dòng)目標(biāo)和運(yùn)動(dòng)路徑，需要依靠ARV攜帶的聲學(xué)、光學(xué)等傳感器獲取的實(shí)時(shí)局部環(huán)境信息進(jìn)行決策和規(guī)劃。由于ARV自主能力的限制，在復(fù)雜未知環(huán)境的探索任務(wù)中，ARV按照任務(wù)需求探索環(huán)境和搜索操作人員感興趣的目標(biāo)時(shí)仍需操作人員實(shí)時(shí)遙控。因此，針對(duì)ARV在全局環(huán)境信息未知的二維水平面運(yùn)動(dòng)執(zhí)行環(huán)境探索任務(wù)開(kāi)展共享控制研究，實(shí)時(shí)融合ARV自主控制與操作人員通過(guò)操作桿遙控的控制命令，以發(fā)揮融合法共享控制的優(yōu)勢(shì)使ARV服從操作人員的控制意圖、提高ARV的安全性、降低操作人員操作的復(fù)雜性并優(yōu)化ARV的運(yùn)動(dòng)路徑。

為便于實(shí)現(xiàn)ARV在環(huán)境探索任務(wù)中的共享控制，設(shè)計(jì)了模塊化的控制結(jié)構(gòu)，由傳感系統(tǒng)模塊、操作人員遙控模塊、自主控制模塊和共享控制模塊組成，如圖1所示。傳感系統(tǒng)模塊用于獲取并輸出與環(huán)境和ARV運(yùn)動(dòng)狀態(tài)相關(guān)的信息，包括局部環(huán)境中障礙物的分布（n為障礙物的數(shù)量）、ARV當(dāng)前的位置P ARV和艏向角ψARV；操作人員遙控模塊的功能是由操作人員根據(jù)傳感系統(tǒng)模塊輸出的P ARV、ψAR V和P obs通過(guò)遙控操作桿的x和y軸分別輸出ARV艏向角控制命令ψh和前向速度的控制命令v h；自主控制模塊根據(jù)傳感系統(tǒng)模塊輸出的P obs和P ARV，使用人工勢(shì)場(chǎng)法計(jì)算ARV艏向角的控制命令ψapf；共享控制模塊根據(jù)其他模塊輸出的ψh、ψapf、ψAR V、P A RV和P obs，使用基于多目標(biāo)優(yōu)化的共享控制方法產(chǎn)生艏向角的控制命令ψsh are。該結(jié)構(gòu)中，ARV的前向速度由操作人員直接控制，艏向角由共享控制模塊控制。

圖1 控制結(jié)構(gòu)Fig.1 Control Architecture

3 基于多目標(biāo)優(yōu)化的共享控制方法

3.1 方法概述

在控制結(jié)構(gòu)中，共享控制模塊使用基于多目標(biāo)優(yōu)化的共享控制方法產(chǎn)生ψshar e控制ARV。基于多目標(biāo)優(yōu)化的共享控制方法由目標(biāo)函數(shù)、約束條件和優(yōu)化方法三部分組成，使用目標(biāo)函數(shù)和約束條件將ARV艏向角的共享控制轉(zhuǎn)化為以艏向角控制命令變量ψ作為決策變量的多目標(biāo)優(yōu)化問(wèn)題，通過(guò)優(yōu)化方法求解該問(wèn)題得到ψ的最優(yōu)解即ψshare。目標(biāo)函數(shù)包括服從度函數(shù)ob edience(ψ)、自 主 度 函 數(shù)autonomy(ψ)和 穩(wěn) 定 度 函 數(shù)stab ility(ψ)，其作用是根據(jù)任務(wù)需求對(duì)ψ的不同取值進(jìn)行評(píng)估；約束條件為ψ可行解取值區(qū)間的集合I safe=[I1，I2，…，I i]（i為取值區(qū)間的個(gè)數(shù)），根據(jù)局部環(huán)境中障礙物的分布確定以保障ARV安全；優(yōu)化方法用于求解多目標(biāo)優(yōu)化問(wèn)題以得到ψshare，使用最小最大法作為優(yōu)化方法。該方法中，ARV艏向角的共享控制表示為公式（1）所示的極大化的多目標(biāo)優(yōu)化問(wèn)題。

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

文獻(xiàn)[7-8]根據(jù)反應(yīng)式控制的思想，設(shè)計(jì)了效率函數(shù)評(píng)估操作人員的控制命令，并使用函數(shù)值計(jì)算操作人員控制命令的權(quán)重。借鑒文獻(xiàn)[7-8]中效率函數(shù)的設(shè)計(jì)方法，目標(biāo)函數(shù)設(shè)計(jì)為以自然常數(shù)作為底數(shù)的負(fù)指數(shù)函數(shù)形式，值域均在區(qū)間(0，1]內(nèi)，便于對(duì)不同的目標(biāo)函數(shù)進(jìn)行比較。該形式的目標(biāo)函數(shù)在對(duì)稱(chēng)軸處取得最大值1并向兩側(cè)遞減，通過(guò)調(diào)整形狀系數(shù)（形狀系數(shù)非負(fù)）可以改變遞減的速度，形狀系數(shù)越大，遞減的速度越快；特別的，當(dāng)形狀系數(shù)為零時(shí)，函數(shù)值恒為1。采用最小最大法求解時(shí)優(yōu)化結(jié)果由函數(shù)值最小的函數(shù)確定[8]，因此，目標(biāo)函數(shù)的形狀系數(shù)越大，對(duì)優(yōu)化結(jié)果的影響越大；當(dāng)形狀系數(shù)為0時(shí)對(duì)優(yōu)化結(jié)果無(wú)影響，便于調(diào)整目標(biāo)函數(shù)在優(yōu)化中的作用。

3.2.1 服從度函數(shù)

在環(huán)境探索任務(wù)中，ARV的運(yùn)動(dòng)需要服從操作人員的控制意圖以滿(mǎn)足觀測(cè)需求，為簡(jiǎn)化計(jì)算，采用ψh表示操作人員的控制意圖，因此設(shè)計(jì)了式（2）所示的服從度函數(shù)評(píng)估ψ與ψh的差別。

式中：exp()—以自然常數(shù)作為底數(shù)的指數(shù)函數(shù)；ψ=ψh—對(duì)稱(chēng)軸；

α—服從度函數(shù)的形狀系數(shù)，其值由式（3）確定。

式中：αmax—大于0的常數(shù)；dmin—ARV到障礙物的最小距離；d shar e—障礙物影響ARV運(yùn)動(dòng)的距離閾值；d saf e—保障ARV與障礙物之間安全的距離閾值；d sha re和d sa fe根據(jù)ARV的運(yùn)動(dòng)半徑確定。

式（3）根據(jù)dmin調(diào)整α的值從而改變操作人員影響ARV運(yùn)動(dòng)的程度。

3.2.2 自主度函數(shù)

ARV在任務(wù)中還應(yīng)該服從自主控制的控制命令以輔助操作人員的操作并提高ARV的安全性，自主控制的控制命令使用ψapf以輔助操作人員避障，因此設(shè)計(jì)了式（4）所示的自主度函數(shù)評(píng)估ψ與ψapf的差別。

式中：γ—自主度函數(shù)的形狀系數(shù)，其值由式（5）確定。

式中：γmax—大于0的常數(shù)。

式（5）根據(jù)dmin調(diào)整γ的值，使ψapf僅在dmin

3.2.3 穩(wěn)定度函數(shù)

適當(dāng)?shù)臏p少控制命令相對(duì)于當(dāng)前運(yùn)動(dòng)方向的突變可以使ARV的運(yùn)動(dòng)狀態(tài)更穩(wěn)定，從而降低操作人員操作的復(fù)雜性并使ARV的運(yùn)動(dòng)路徑更加平順，因此設(shè)計(jì)了式（6）所示的穩(wěn)定度函數(shù)評(píng)估ψ相對(duì)于ψAR V的變化。

式中：β>0—穩(wěn)定度函數(shù)的形狀系數(shù)，β越大，穩(wěn)定度函數(shù)對(duì)優(yōu)化結(jié)果的影響越大，優(yōu)化產(chǎn)生的ψsh a re相對(duì)于ψA RV的變化越小。過(guò)大的β會(huì)使ARV不易改變運(yùn)動(dòng)方向，因此將β設(shè)置為較小的正常數(shù)。

3.3 約束條件

約束條件I safe為保障ARV安全的ψ的取值區(qū)間的集合，根據(jù)ARV在不同運(yùn)動(dòng)方向上的安全性確定，在基于多目標(biāo)優(yōu)化的共享控制方法中，安全性由安全性評(píng)估函數(shù)表示。在環(huán)境探索任務(wù)中，ARV與障礙物的距離越大越安全，因此，安全性評(píng)估函數(shù)由ARV攜帶的聲學(xué)、光學(xué)等傳感器獲取的周?chē)系K物分布的方向和距離信息確定。

安全性評(píng)估函數(shù)根據(jù)局部環(huán)境中障礙物分布的柵格地圖計(jì)算，計(jì)算過(guò)程的示意圖，如圖2所示。其中，虛線交叉產(chǎn)生的小正方形為柵格，黑色柵格為障礙物柵格，白色柵格為自由柵格，Δ為柵格粒度；實(shí)線標(biāo)出的大正方形為活動(dòng)窗口，邊長(zhǎng)為wm，活動(dòng)窗口內(nèi)的柵格地圖用于計(jì)算安全性評(píng)估函數(shù)；橢圓表示ARV，位于活動(dòng)窗口的中心；以θ為夾角使用以ARV所在的位置為中心的一組射線將活動(dòng)窗口劃分為2π∕θ個(gè)區(qū)域，單個(gè)區(qū)域內(nèi)ARV到障礙物柵格的最小距離為dm，當(dāng)區(qū)域內(nèi)不存在障礙物柵格時(shí)d=(w-Δ)∕2 m；I′1為符合安全性要求即安全性評(píng)估函數(shù)取值要求的ψ的一個(gè)取值區(qū)間，IΔ為安全間隔，I1為約束條件中ψ的一個(gè)取值區(qū)間。

安全性評(píng)估函數(shù)設(shè)計(jì)為分段函數(shù)，其自變量ψ的定義域?yàn)閰^(qū)間(-π，π]。在極坐標(biāo)系中，根據(jù)圖2中夾角為θ的區(qū)域劃分將定義域分段，每段定義域內(nèi)的函數(shù)值為常數(shù)，其根據(jù)圖2中對(duì)應(yīng)區(qū)域內(nèi)的d確定，使用式（7）計(jì)算。d越大，函數(shù)值越小，該段定義域內(nèi)ψ的取值越安全。

式中：security(ψ)—安全性評(píng)估函數(shù)；λ—改變函數(shù)值大小的比例因子，設(shè)置為大于0的常數(shù)；dmax—障礙物開(kāi)始引起security(ψ)函數(shù)值變化的距離閾值，且dmax=(w-Δ)∕2 m。約束條件I s a fe根據(jù)安全性評(píng)估函數(shù)分兩步計(jì)算，首先確定符合安全性要求的運(yùn)動(dòng)方向的取值區(qū)間的集合I′saf e=[I′1，I′2，…，I′j](j為取值區(qū)間的個(gè)數(shù)），使ARV在I′saf e的區(qū)間內(nèi)的運(yùn)動(dòng)方向上與障礙物保持安全的距離：設(shè)置d share為保障ARV安全的距離閾值，對(duì)應(yīng)的安全性評(píng)估函數(shù)的函數(shù)值閾值為λ(dmaxd share)，由于在安全性評(píng)估函數(shù)的函數(shù)值計(jì)算中，距離越大，函數(shù)值越小，ψ的取值越安全，因此使用securit y(ψ)<λ(dmax-d share)計(jì)算得到I′safe。然后，縮小I′sa fe中的取值區(qū)間以得到I saf e，以保障ARV在I sa fe的區(qū)間內(nèi)的運(yùn)動(dòng)方向上與兩側(cè)的障礙物也保持相對(duì)安全的距離：如圖2所示。設(shè)置了保障ARV安全的安全間隔IΔ，在I′sa fe中的所有取值區(qū)間（以I′1為例）的兩端分別縮小IΔ以得到I sa fe（以I1為例）。

圖2 安全性評(píng)估函數(shù)計(jì)算方法示意圖Fig.2 Illustration of the Calculation Method of Security Evaluation Function

3.4 優(yōu)化方法

在環(huán)境探索任務(wù)中，ARV通常在全局環(huán)境信息未知的水下環(huán)境中工作，對(duì)可靠性的要求很高，因此借鑒文獻(xiàn)[8]中多目標(biāo)優(yōu)化的方法，選擇最小最大法作為優(yōu)化方法計(jì)算最優(yōu)解，以在最壞的情況下尋求最好的優(yōu)化結(jié)果，保證ARV工作的可靠性。最小最大法將式（1）所示的多目標(biāo)優(yōu)化問(wèn)題轉(zhuǎn)化為式（8）所示的單目標(biāo)優(yōu)化問(wèn)題，通過(guò)求解即可得到共享控制方法產(chǎn)生的艏向角控制命令ψshare。

4 計(jì)算機(jī)仿真

為了驗(yàn)證基于多目標(biāo)優(yōu)化的共享控制方法的效果，研發(fā)了ARV共享控制計(jì)算機(jī)仿真研究環(huán)境。仿真環(huán)境主要由視景顯示、ARV共享控制算法、ARV動(dòng)力學(xué)仿真與運(yùn)動(dòng)控制、障礙物仿真四個(gè)模塊組成。其中，操作人員通過(guò)圖馬斯特T.16000M操作桿輸入控制信息；ARV的動(dòng)力學(xué)模型采用REMUS AUV的六自由度動(dòng)力學(xué)模型[12]，該AUV使用一個(gè)主推進(jìn)器控制前向速度，一對(duì)垂直舵控制艏向，最大前向速度為2.5 m∕s。

為驗(yàn)證基于多目標(biāo)優(yōu)化的共享控制方法的效果，在仿真環(huán)境中對(duì)基于多目標(biāo)優(yōu)化的ARV共享控制、遙控和加權(quán)共享控制三種方法進(jìn)行了對(duì)比仿真。其中，遙控方式可以仿真操作人員直接遙控和仲裁法共享控制中由操作人員控制的情況；加權(quán)共享控制通過(guò)線性加權(quán)融合人機(jī)控制信號(hào)實(shí)現(xiàn)共享控制，如式（9）所示，在融合法共享控制的設(shè)計(jì)思路中具有一定的代表性[8-9]，式（9）中μ（μ∈[0，1]）為操作人員遙控的權(quán)值，根據(jù)ARV到障礙物的最小距離確定，使用式（10）計(jì)算，為便于比較，式（10）的參數(shù)設(shè)置與服從度函數(shù)中形狀系數(shù)的計(jì)算方法一致。

如圖3（a）所示，仿真實(shí)驗(yàn)設(shè)計(jì)為ARV由起點(diǎn)A出發(fā)穿過(guò)障礙物區(qū)域并到達(dá)終點(diǎn)B的過(guò)程以仿真ARV的環(huán)境探索任務(wù)，終點(diǎn)B僅為路徑點(diǎn)，不用于ARV的路徑規(guī)劃。仿真中ARV的控制周期設(shè)置為0.1 s，運(yùn)動(dòng)控制采用文獻(xiàn)[13]中的混合模糊P+ID控制方法，仿真區(qū)域大小為（400×600）m。仿真中基于多目標(biāo)優(yōu)化的共享控制方法的參數(shù)設(shè)置如下：服從度函數(shù)中，d share=50 m，d sa fe=15 m，αmax=1；穩(wěn)定度函數(shù)中β=0.1；自主度函數(shù)中γmax=1；活動(dòng)窗口中，Δ=1 m，w=121m，用于劃分區(qū)域的θ=2π∕180，將活動(dòng)窗口劃分為180個(gè)區(qū)域；安全性評(píng)估函數(shù)中λ=1，計(jì)算約束條件時(shí)IΔ=2π∕18。仿真結(jié)果，如圖3、表1所示。

圖3 仿真結(jié)果Fig.3 Simulation Results

表1 仿真結(jié)果Tab.1 Simulation Results

圖3中：A—起點(diǎn)；B—終點(diǎn)，黑色圖形——障礙物；曲線—ARV的運(yùn)動(dòng)路徑。由圖3和表1可以看出，與遙控和加權(quán)共享控制相比，基于多目標(biāo)優(yōu)化的共享控制方法產(chǎn)生的ARV路徑更平順，在任務(wù)中的路徑長(zhǎng)度和所需時(shí)間也更短，有效的優(yōu)化了ARV的運(yùn)動(dòng)。

仿真中用于控制ARV艏向角的操作桿x軸的輸入信號(hào)變化，如圖4所示。

圖4 操作人員的輸入信號(hào)Fig.4 The Operator’s Input Signal

由圖4可知，與其他方式相比，基于多目標(biāo)優(yōu)化的共享控制方法有效減少了操作桿x軸輸入信號(hào)的變化，使操作人員通過(guò)操作桿輸入的控制信號(hào)更簡(jiǎn)單，有效降低了操作人員操作的復(fù)雜性，從而降低了操作人員遙控的工作負(fù)擔(dān)。

仿真中ARV到障礙物的最小距離dmin的變化，如圖5所示。仿真中使用的ARV動(dòng)力學(xué)模型的最小轉(zhuǎn)彎半徑不超過(guò)15m，因此，加入適當(dāng)余量后設(shè)定dmin不小于20 m時(shí)ARV處于安全狀態(tài)。使用遙控仿真時(shí)ARV僅由操作人員控制，圖5（b）中180 s左右出現(xiàn)了dmin過(guò)小的情況，此時(shí)圖4（b）中操作桿信號(hào)的變化比較劇烈，說(shuō)明遙控方式在操作較復(fù)雜的情況下發(fā)生碰撞的風(fēng)險(xiǎn)增加，安全性下降；使用加權(quán)共享控制控制時(shí)dmin保持在30m以上，雖然ARV保持安全狀態(tài)，但與基于多目標(biāo)優(yōu)化的ARV共享控制相比，減小了ARV在障礙物周?chē)顒?dòng)的范圍，同時(shí)dmin的變化比較劇烈，結(jié)合圖3（c）可知，ARV的運(yùn)動(dòng)路徑出現(xiàn)了明顯的抖動(dòng)；而使用基于多目標(biāo)優(yōu)化的共享控制方法時(shí)ARV始終處于安全狀態(tài)，克服了上述兩種方法中存在的問(wèn)題，改善了ARV的安全性，優(yōu)化了ARV的運(yùn)動(dòng)路徑，從而提高了ARV的任務(wù)性能。

圖5 ARV與障礙物的最小距離Fig.5 The Minimum Distance Between the ARV and the Obstacle

5 結(jié)論

針對(duì)ARV在環(huán)境探索任務(wù)中的艏向角控制，提出一種基于多目標(biāo)優(yōu)化的共享控制方法。將ARV艏向角控制命令作為決策變量，使用設(shè)計(jì)的服從度、穩(wěn)定度和自主度函數(shù)作為目標(biāo)函數(shù)，根據(jù)設(shè)計(jì)的安全性評(píng)估函數(shù)確定約束條件，通過(guò)最小最大法計(jì)算環(huán)境探索任務(wù)中ARV艏向角的控制命令。基于構(gòu)建的ARV共享控制仿真環(huán)境對(duì)設(shè)計(jì)的共享控制方法進(jìn)行的仿真實(shí)驗(yàn)表明，基于多目標(biāo)優(yōu)化的共享控制方法有效的提高了ARV的任務(wù)性能并降低了操作人員遙控的工作負(fù)擔(dān)。在將來(lái)的研究工作中，將通過(guò)識(shí)別操作人員意圖、分析操作人員的任務(wù)表現(xiàn)以及豐富操作人員與ARV系統(tǒng)的交互方式（力反饋、虛擬現(xiàn)實(shí)等），進(jìn)一步提高基于多目標(biāo)優(yōu)化的共享控制方法的控制效果。