王興華，田 宇

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

0 引 言

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

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

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

1 控制結構

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

圖1 控制結構Fig. 1 Control architecture

2 環(huán)境探索共享控制

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

2.1 遙控行為

遙控行為實現(xiàn)操作人員通過控制手柄實時控制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ψ為調節(jié)首向角增量的增益系數(shù)；Jxd＞0和Jyd＞0為去除控制手柄在0位置死區(qū)影響設定的閾值；vmax為ARV的最大航行速度。

2.2 自主避障行為

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

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

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

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

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

2.3 行為融合

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

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

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

3 目標觀察共享控制

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

3.1 超橢圓路徑

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

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

3.2 自主路徑跟蹤控制

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

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

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

由于大多數(shù)的水下機器人為欠驅動，因此本文考慮欠驅動ARV的路徑跟蹤控制。令S為預規(guī)劃的水平面航行路徑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，不能直接將路徑跟蹤位置誤差用于閉環(huán)反饋控制。因此引入趨近角：

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

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

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

3.3 人機協(xié)同路徑跟蹤控制行為

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

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

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

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

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

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

4 行為綜合管理與融合

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

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

圖4 行為優(yōu)先級Fig. 4 Priorities of the behaviours

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

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

5 計算機仿真

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

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

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

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

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

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

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

目標觀察階段開始觀察目標的仿真時間段為500～1 000 s，操作人員通過控制手柄修改路徑參數(shù)，控制手柄x軸的輸入信號Jx和ARV到目標的距離變化如圖7所示。操作人員輸入的路徑參數(shù)分為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在“機主人輔”模式下觀察目標時可以適應目標的形狀，同時可以根據(jù)操作人員輸入的路徑參數(shù)及時改變ARV的運動軌跡，以保持ARV與目標間相對穩(wěn)定的距離，便于操作人員對目標進行觀察。此外，操作人員在“機主人輔”模式下觀察目標時輸入的控制信號比較簡單，有效降低了操作人員操作ARV的復雜性。

6 結 語

針對ARV在近海底環(huán)境探索和目標觀察中的共享控制需求，采用基于行為的控制思想，提出一種基于行為的ARV共享控制方法。基于設計的遙控行為、自主避障行為及其融合，實現(xiàn)了“人主機輔”模式執(zhí)行環(huán)境探索過程的有效共享控制；基于設計的人機協(xié)同路徑跟蹤控制行為，實現(xiàn)了“機主人輔”模式執(zhí)行目標觀察過程的有效共享控制。通過設計的基于優(yōu)先級的行為組織和融合方法，實現(xiàn)了ARV基于設計的行為執(zhí)行環(huán)境探索繼而目標觀察過程的有效共享控制?；跇嫿ǖ乃聶C器人共享控制仿真環(huán)境對設計的共享控制方法進行的仿真實驗，驗證了提出的共享控制方法的有效性。