一種基于行為的自主/遙控水下機器人共享控制方法

王興華，田 宇

(1. 中國科學院沈陽自動化研究所 機器人學國家重點實驗室，遼寧 沈陽 110016；2. 中國科學院機器人與智能制造創新研究院，遼寧 沈陽 110016；3. 中國科學院大學，北京 100049)

0 引 言

水下機器人是探索海洋的重要工具，其主要分為自主水下機器人和遙控水下機器人。為綜合自主水下機器人和遙控水下機器人的特點并克服各自的不足，研究人員提出了自主/遙控水下機器人（ARV），它可以在自主、半自主、遙控等不同模式下作為自主水下機器人和遙控水下機器人執行搜索、觀察以及作業等任務。鑒于其獨特的特點和技術優勢，ARV的研發和在海洋觀測、探測中的應用成為近年來國際上水下機器人發展的熱點[1-2]。

由于ARV既可由操作人員遙控，又具備一定程度的智能和自主。因此，如何在其工作過程中融合人的感知、控制與ARV的自主感知、控制，以實現操作人員遙控和ARV自主控制協調的共享控制，一方面通過ARV的自主控制降低操作人員操作ARV的復雜性、保障其安全，另一方面通過操作人員的監控和輔助提高ARV對環境和任務變化相適應的工作能力，是ARV的高效應用需要研究和解決的重要問題[3-7]。

本文針對ARV在二維水平面運動進行環境探索和目標觀察，提出一種基于行為的共享控制方法。針對環境探索，設計了遙控行為和自主避障行為；針對目標觀察，設計了人機協同路徑跟蹤控制行為。通過基于優先級的行為組織和融合方法，實現了ARV基于設計的行為以“人主機輔”模式執行環境探索繼而以“機主人輔”模式執行目標觀察過程的有效共享控制。

1 控制結構

為了便于設計和實現基于行為的共享控制方法，本文采用一種模塊化的基于行為的控制結構，由相互串聯的行為管理模塊、行為模塊、行為融合模塊、運動控制模塊組成。行為管理模塊根據控制任務打開或關閉要求的行為模塊中的行為，即設置行為是否參與執行給定的控制任務；行為模塊為設計的ARV控制行為的集合，各行為的輸入為傳感器感知信息以及操作人員的控制信息，各行為輸出為ARV的期望艏向角ψc和期望前向速度vc；行為融合模塊負責融合行為模塊中各個行為輸出的ψc和vc，并將融合結果輸出給運動控制模塊；運動控制模塊根據行為融合模塊輸出的ψc和vc，計算ARV舵、推進器等執行機構控制指令，使ARV實現期望的運動。在本文中，運動控制模塊采用基于反饋線性化的方法進行ARV的前向速度控制、采用混合模糊P+ID控制方法進行ARV的首向角控制[8]。

圖1 控制結構Fig. 1 Control architecture

2 環境探索共享控制

近海底環境探索和目標搜尋是ARV所執行的重要任務。執行任務過程中，操作人員需實時控制ARV按操作人員的意圖對未知環境進行探索和搜尋感興趣的目標，因此本文設計了ARV的操作人員遙控行為，操作人員通過控制手柄實時控制ARV的運動速度和方向。由于ARV也具備通過傳感器如聲吶感知周邊環境并基于感知信息進行自主運動控制能力，為進一步保障環境探索過程中ARV在操作人員遙控下的航行安全，本文設計了ARV的自主避障行為。通過遙控行為和自主避障行為的融合，實現環境探索任務的共享控制。

2.1 遙控行為

遙控行為實現操作人員通過控制手柄實時控制ARV的航行速度和首向角。該行為的輸入為控制手柄在x和y方向的控制輸入量Jx和Jy，其中Jx控制ARV的首向角，Jy控制ARV的航行速度，并且Jx∈[-1，1]，Jy∈[0，1]。該行為的輸出為ARV的期望首向角ψc和期望的航行速度vc。

圖2 控制手柄輸出信號示意圖Fig. 2 Illustration of control commands from the joystick

ψc和vc分別按下式計算：

其中：ψ(t)為ARV的當前首向角；kψ為調節首向角增量的增益系數；Jxd＞0和Jyd＞0為去除控制手柄在0位置死區影響設定的閾值；vmax為ARV的最大航行速度。

2.2 自主避障行為

由于ARV具有基于聲吶等傳感器檢測環境障礙物并基于感知信息進行自主運動控制能力，因此本文設計ARV自主避障行為，輔助操作人員通過遙控行為控制ARV過程中避免與環境中的障礙物發生碰撞，進一步保障ARV的航行安全。自主避障行為的輸入為ARV和障礙物的當前位置和，輸出為ARV的期望艏向角ψc和期望的航行速度vc，分別計算如下：

其中atan(·)為反正切函數。

其中：lo=‖Pv-Po‖為ARV與障礙物之間的距離；lmax＞0為障礙物對ARV航行安全產生影響的最大距離；lmin＞0為保障ARV與障礙物之間安全的距離閾值；ko＞0為比例系數；lmax，lmin，ko為行為中的設計參數。

若環境中存在多個障礙物，則自主避障行為的輸出為各個障礙物對ARV影響的綜合：

其中：Vci為自主避障行為輸出的ARV對第i個障礙物避障的速度向量，其值為vci、方向為ψci；n為環境中對ARV航行安全產生影響的障礙物數量。

2.3 行為融合

環境探索任務的共享控制通過遙控行為和自主避障行為的融合實現，即在控制結構的行為融合模塊融合遙控行為輸出的ψc，vc以及自主避障行為輸出的ψc和vc，得到ARV運動控制模塊輸入的ψc和vc。為了保障ARV的安全，本文設計自主避障行為與遙控行為相比具有高優先級，并設計基于行為優先級的融合方法。

考慮到如下的共享控制要求：1）當ARV與障礙物之間的距離lo小于設定的ARV與障礙物之間安全距離閾值lmin時，自主避障行為需抑制遙控行為，控制ARV自主避障以保障其安全；2）當ARV與障礙物之間的距離lo大于障礙物對ARV航行安全產生影響的最大距離lmax時，自主避障行為無輸出，ARV僅在操作人員遙控下運動；3）當ARV與障礙物之間的距離lo大于lmin但小于lmax時，自主避障行為發揮部分作用，對操作人員遙控的ARV運動施加影響，使其遠離障礙物。針對該共享控制要求，參考文獻[9]，本文設計行為輸出融合方法如下：

其中：∠Vch=ψch，‖Vch‖=vch，∠Vcl=ψcl，‖Vcl‖=vcl，∠Vcf=ψcf，‖Vcf‖=vcf，下標h、l和f分別表示高優先級行為（自主避障行為）、低優先級行為（遙控行為）、和融合后的行為輸出結果；αf=αh+αl(1-αh)，其中αh、αl(1-αh)分別為高優先級行為和低優先級行為輸出結果的加權系數，αh∈[0，1]根據下式計算：αl＞0為低優先級行為輸出結果加權系數的調節參數（當αl＞1時，增加融合結果中遙控行為輸出的權重；當αl＜1時，增加融合結果中自主避障行為的輸出的權重），αl可根據操作人員操作ARV的技術水平以及期望的ARV運動對操作人員遙控的依從度來進行設置。通過式（6）和式（7），即實現了上述的共享控制要求。

3 目標觀察共享控制

近海底環境探索和尋找到感興趣的目標后，ARV對目標進行圍繞觀察也是ARV所需執行的重要任務。由于ARV具有自主運動控制能力，且其控制精度要高于操作人員的遙控。因此，本文設計路徑跟蹤控制行為實現ARV自主精確的跟蹤期望的目標圍繞觀察路徑。然而，由于ARV的自主感知和理解能力限制，其難以在目標觀察過程中根據環境、目標的信息和操作人員的觀察意圖自主調整路徑。因此，本文在自主路徑跟蹤控制中融入操作人員對路徑參數及路徑跟蹤運動的控制，即設計人機協同路徑跟蹤控制行為，實現在圍繞目標觀察過程中，以ARV自主路徑跟蹤控制為主、操作人員調節路徑參數及路徑跟蹤運動為輔助的共享控制，提高ARV執行目標觀察路徑跟蹤過程中對環境、目標以及觀測要求變化的適應能力。

3.1 超橢圓路徑

為了便于對不同形狀和尺度目標的圍繞觀察，本文設計ARV執行超橢圓路徑圍繞目標進行觀察。超橢圓是介于橢圓和矩形之間的一類曲線，具有良好的幾何性質，形狀可以在橢圓和矩形之間連續變化，其參數方程[10]：

其中：sign(·)為符號函數；t為超橢圓參數方程的參數；a和b分別超橢圓的長半軸和短半軸的長度；n為形狀系數，通過調節n即可得到不同形狀的曲線。

3.2 自主路徑跟蹤控制

自主路徑跟蹤控制的輸入為ARV需要跟蹤的路徑P（即超橢圓路徑的參數a，b和n），ARV的位置速度等狀態信息，輸出為ARV的期望艏向角ψc和期望的航行速度vc。

在路徑跟蹤控制中，vc為操作人員給定的ARV沿路徑運動的速度。下面說明本文自主路徑跟蹤控制中ψc的計算。

圖3 a=b但不同n值的超橢圓曲線Fig. 3 Superellipses with a=b and different n values

由于大多數的水下機器人為欠驅動，因此本文考慮欠驅動ARV的路徑跟蹤控制。令S為預規劃的水平面航行路徑P上的運動參考點，該路徑在參考點S處的單位切向量T和單位法向量N，在該點處張成直角坐標系SF。ARV以重心G為原點的載體坐標系為Gxyz，繞載體坐標系的z軸旋轉漂角β，得到ARV的速度坐標系W，ψWS為從SF坐標系到W坐標系的旋轉歐拉角。ARV跟蹤路徑P，即其速度坐標系以期望速度vc跟蹤運動坐標系SF（vc方向跟蹤T方向）；因此，路徑跟蹤誤差包括速度跟蹤誤差vc-vt，和在SF坐標系中表示的位置跟蹤誤差，ARV的重心位置G在SF坐標系中的位置向量[lT,lN]T，和角度跟蹤誤差ψWS。

ARV路徑跟蹤，即控制其速度vt趨于期望速度vc，同時使位置誤差和角度誤差全局漸進收斂到零。對于欠驅動ARV，不能直接將路徑跟蹤位置誤差用于閉環反饋控制。因此引入趨近角：

其中sign(·)為符號函數，0＜ψl≤π/2和kN＞0為設計參數。可證明，運動控制模塊的控制器保證vt跟蹤vc，角度誤差ψWS跟蹤趨近角ψa，并設計合適的路徑參考點S的運動速度[8]，可以保證路徑跟蹤位置和角度誤差全局漸進收斂到0。

趨近角ψa跟蹤誤差可以表示為：

其中：ψSF為SF坐標系T向量在大地坐標系中的角度，因此將ψa(t)+ψSF(t)-β(t)作為ARV自主路徑跟蹤控制的首向角的參考跟蹤信號：

3.3 人機協同路徑跟蹤控制行為

ARV通過自主路徑跟蹤控制跟蹤超橢圓路徑對目標進行圍繞觀察過程中，操作人員可通過ARV搭載的攝像機等傳感器反饋信息實時獲得目標的觀察信息。因此，操作人員可根據目標觀察要求實時調節超橢圓路徑以及ARV的路徑跟蹤運動，實現可根據環境、目標、以及觀察需要相應變化的更好目標觀察。

本文針對該要求，在上述ARV自主路徑跟蹤控制基礎上，設計人機協同路徑跟蹤控制行為實現目標觀察的共享控制。該行為的輸入為ARV需要跟蹤的路徑P（即超橢圓路徑的參數a，b和n，由操作人員根據獲取的目標信息確定）、ARV的位置速度等狀態信息、以及操作人員通過控制手柄輸出的信號Jx和Jy（Jx∈[-1，1]，Jy∈[0，1]），行為的輸出為ARV的期望首向角ψc和期望的航行速度vc。

設計vc由操作人員通過Jy來實時調節：

設計Jx來實時調節超橢圓路徑的a和b參數，即相應的調節ARV與目標的觀察距離，當Jx＞0時ARV遠離目標，當Jx＜0時ARV接近目標：

其中：a0，b0為該行為執行時a和b參數的初始值；ka＞0，kb＞0分別為調節a和b參數變化的增益系數。當a和b參數調節后，該行為由自主路徑跟蹤控制算法實時計算ψc（t）：

不同于環境探索通過融合遙控行為和自主避障行為來實現共享控制（即共享控制在行為間實現），在目標圍繞觀察中本文通過人機協同路徑跟蹤控制一個行為實現了ARV圍繞目標觀察運動的共享控制（即共享控制在行為內實現）。

4 行為綜合管理與融合

針對環境探索和目標觀察的共享控制需求，本文設計了自主避障行為、遙控行為、和人機協同路徑跟蹤控制行為，它們置于控制結構的行為模塊中。為了對行為模塊中各行為的輸出進行融合，以上述自主避障行為和遙控行為融合的方法為基礎，設計控制結構的行為融合模塊中的行為融合方法。

行為融合模塊中，對行為模塊中的各行為賦予優先級并按優先級從高到底進行順序排序。以ARV進行環境探索、對目標進行觀察、完成任務后返航這一完整作業過程為例，行為優先級從高到低分別為自主避障行為、人機協同路徑跟蹤控制行為、遙控行為、返航行為（返航行為控制ARV自主返回到期望位置Ph=(xh,yh)T，其輸出vc為行為中設定的參數，ψc=atan(yv-yh,xv-xh)，如圖4所示。

圖4 行為優先級Fig. 4 Priorities of the behaviours

基于行為的優先級，利用式（6）對各行為的輸出進行融合。在自主避障行為中，αh由式（7）進行計算，而其他行為的αh和αl則由操作人員在行為管理模塊中根據任務要求進行設置：當人機協同路徑跟蹤控制行為和遙控行為的αh均設置為0時，則行為融合模塊融合自主避障行為和返航行為的輸出，ARV自主返航；當遙控行為的αh設置為1時，則其將抑制返航行為，進而融合模塊融合自主避障行為和人機協同路徑跟蹤控制行為輸出，實現ARV進行環境探索的共享控制；當路徑跟蹤行為的αh設置為1時，則其將抑制遙控行為和返航行為的輸出，進而融合模塊融合自主避障行為和人機協同路徑跟蹤控制行為的輸出，實現ARV進行目標觀察的共享控制同時通過自主避障行為保障其安全。

因此，通過行為管理模塊對行為融合模塊中各行為αh和αl的設置，實現了對行為模塊中行為的協調和其輸出的融合，使ARV基于設計的行為執行不同的任務。

5 計算機仿真

為了對本文提出的基于行為的ARV共享控制方法進行驗證，研發了ARV共享控制計算機仿真研究環境。仿真環境主要包括ARV動力學仿真、ARV共享控制、障礙物和目標仿真、以及視景顯示4個模塊。其中，ARV的動力學模型采用REMUS AUV的六自由度動力學模型[11]，ARV的遙控輸入來自操作人員控制的圖馬斯特T.16000M操作桿。

為了驗證基于行為的ARV共享控制方法的有效性，本文在仿真環境中進行了仿真實驗。仿真中任務分為3個階段：第1階段為環境探索，操作人員使用“人主機輔”模式進行環境探索以尋找目標；第2階段為目標觀察，操作人員在發現目標后使用“機主人輔”模式進行觀察；第3階段為返航，ARV在返航行為和自主避障行為的控制下返回出發點。

仿真中ARV的控制周期為0.1 s，仿真區域大小為900×600 m，障礙物為直徑40 m的圓形，觀察的目標為150×100 m的矩形。基于行為的ARV共享控制方法中的參數設置為：vmax=2.5 m/s，控制手柄閾值Jxd=0.25，Jyd=0.05；遙控行為中2；自主避障行為中lmax=37.5 m，lmin=12.5 m，ko=1；自主路徑跟蹤控制器參數，kN=0.2；人機協同路徑跟蹤控制行為中a0=100 m，b0=125 m，ka=20 m，kb=20 m，n=5；環境探索階段人機協同路徑跟蹤控制行為、遙控行為和返航行為的αh分別設置為0，1，0，目標觀察階段分別設置為1，0，0，返航階段分別設置為0，0，1。

仿真結果如圖5所示。圖中A點為出發點，黑色圓形代表障礙物，矩形表示目標。曲線為ARV的運動路徑，其中AB段為環境探索階段，BC段為目標觀察階段，CA段為返航階段。

圖5 共享控制方法仿真結果Fig. 5 A simulation result of the shared control method

圖6 環境探索階段不同行為控制信號權值與loFig. 6 The weights of control signals from different behaviors and the value of lo during the environment exploration

在環境探索階段，ARV在“人主機輔”模式下安全的穿過障礙物區域并到達了目標附近，較好實現了操作人員的控制意圖。該階段的仿真時間為0～433.1 s，圖6為式（6）中環境探索階段遙控行為輸出Vcl的權值、自主避障行為輸出Vch的權值與ARV到障礙物的距離lo的變化。lo≥lmax時，自主避障行為的αh=0，為便于觀察，圖6中lo≥lmax的數據均標注為lmax。由圖6可知，在ARV靠近障礙物的過程中，避障行為輸出信號的權值增大，遙控行為輸出信號的權值減小，使ARV進行有效的避障，保證了ARV離障礙物的距離。

目標觀察階段開始觀察目標的仿真時間段為500～1 000 s，操作人員通過控制手柄修改路徑參數，控制手柄x軸的輸入信號Jx和ARV到目標的距離變化如圖7所示。操作人員輸入的路徑參數分為3部分，第1部分Jx＜Jxd，此時a(t)=a0,b(t)=b0，ARV與目標的期望距離為50 m；第2部分a（t）=80 m，b（t）=105 m，ARV與目標的期望距離為30 m；第3部分a（t）=120 m，b（t）=145 m，ARV與目標的期望距離為70 m。

圖7 目標觀察階段Jx和ARV與目標距離的變化Fig. 7 The distance between the ARV and the target，and the value of Jx during the target observation

由圖5和圖7可以看出，ARV在“機主人輔”模式下觀察目標時可以適應目標的形狀，同時可以根據操作人員輸入的路徑參數及時改變ARV的運動軌跡，以保持ARV與目標間相對穩定的距離，便于操作人員對目標進行觀察。此外，操作人員在“機主人輔”模式下觀察目標時輸入的控制信號比較簡單，有效降低了操作人員操作ARV的復雜性。

6 結 語

針對ARV在近海底環境探索和目標觀察中的共享控制需求，采用基于行為的控制思想，提出一種基于行為的ARV共享控制方法。基于設計的遙控行為、自主避障行為及其融合，實現了“人主機輔”模式執行環境探索過程的有效共享控制；基于設計的人機協同路徑跟蹤控制行為，實現了“機主人輔”模式執行目標觀察過程的有效共享控制。通過設計的基于優先級的行為組織和融合方法，實現了ARV基于設計的行為執行環境探索繼而目標觀察過程的有效共享控制。基于構建的水下機器人共享控制仿真環境對設計的共享控制方法進行的仿真實驗，驗證了提出的共享控制方法的有效性。