李文濤,葛 彤

(上海交通大學(xué)海洋工程國家重點實驗室,上海 200030)

懸停式AUV控制策略研究

李文濤,葛 彤

(上海交通大學(xué)海洋工程國家重點實驗室,上海 200030)

以懸停式AUV“MM-01”為試驗平臺,設(shè)計一種基于行為的控制體系結(jié)構(gòu)。將AUV的行為劃分為漫游、巡航和避障三個基本行為,并將模糊控制引入避障行為中,通過基于優(yōu)先級的仲裁方式實現(xiàn)三種行為的有序協(xié)調(diào),完成預(yù)定任務(wù)。同時,針對AUV的懸停需求,提出了增加內(nèi)環(huán)速度反饋實現(xiàn)AUV穩(wěn)定懸停的控制方法,定義一種新的高層自治控制系統(tǒng)與底層運動控制系統(tǒng)的接口。最后,通過建立整個系統(tǒng)的Simulink仿真模型,驗證了該控制策略的有效性。

AUV;懸停;基于行為;優(yōu)先級仲裁;仿真

Abstract:A control architecture based on behaviors is designed for the hovering-type AUV test bed“MM-01”.Three basic behaviors,including Wander,Cruise and Obstacle-Avoid,realized by fuzzy logic control,are defined.AUV′spredefinedmission can be performed by the sequential coordination of those behaviors based on priority.Besides,this paper proposes to introduce inner-loop velocity feedback into motion control to realize AUV′shovering function and definesa new kind of interface between high-level autonomous control block and low-levelmotion control block.Finally the simulation of thewhole system is executed in Simulink and the results validate the efficiency of the designed control strategy.

Key words:AUV;hovering;behavior-based;coordination based on priority;simulation

根據(jù)使命任務(wù)的不同,AUV(無人自治潛水器)可以分為巡航式AUV和懸停式AUV,前者用于大范圍的海域調(diào)查,后者用于小范圍內(nèi)的精確定點調(diào)查和作業(yè)[1]。巡航式AUV在海洋調(diào)查中已經(jīng)獲得越來越廣泛的應(yīng)用,而懸停式AUV是近年來出現(xiàn)的一種新型潛水器,它能依靠自身的推力系統(tǒng),在控制系統(tǒng)的指揮下抵抗海流和自身作業(yè)等引起的內(nèi)外擾動,使其保持某一姿態(tài)懸停于空間某一特定位置[2],即具備一種動力定位能力。懸停式AUV既有AUV的自治能力,又有ROV的機動能力,其發(fā)展目標(biāo)是成為一種高度自治并能夠?qū)崿F(xiàn)精確定點調(diào)查和作業(yè)的新型潛水器。由于在海底熱液噴口的觀察取樣和海洋結(jié)構(gòu)物探測方面具有不可替代的重要作用,懸停式AUV的研究具有顯而易見的重要意義,各國相繼積極開展了相關(guān)研究[3-4]。

控制策略是AUV自主地完成各種任務(wù)的指導(dǎo)思想,因而控制策略研究是設(shè)計懸停AUV的首要步驟。傳統(tǒng)的機器人控制方法是建立在嚴(yán)格精確的控制系統(tǒng)數(shù)學(xué)模型的基礎(chǔ)上,無法適用于具有強耦合性和非線性特點的AUV及其復(fù)雜多變的未知環(huán)境,而基于行為的控制結(jié)構(gòu)是一種反應(yīng)式結(jié)構(gòu),不需建立全局世界模型,因而非常適用于AUV的控制體系。AUV控制體系包括自治控制系統(tǒng)和運動控制系統(tǒng),二者的功能劃分及其接口定義是整個體系設(shè)計的基礎(chǔ)。此外,AUV的動力定位是依靠垂推和側(cè)推實現(xiàn)的,對具體的控制方法進行深入探究有助于提高動力定位的性能。在控制策略的上述方面進行了綜合研究,并完成了仿真,結(jié)果驗證了設(shè)計的控制策略的有效性。

1 懸停式AUV使命任務(wù)與本體結(jié)構(gòu)

為了實現(xiàn)精確定點作業(yè)和調(diào)查的功能,AUV必須能夠在三維空間內(nèi)從起始點自治地駛向目標(biāo)終點并懸停,其中起始點、子目標(biāo)點和終點預(yù)先定義,如圖1所示。航行過程中若發(fā)現(xiàn)前方有障礙物,AUV須自動避障,之后尋找原軌跡繼續(xù)駛向目標(biāo)點。若丟失對當(dāng)前位置導(dǎo)航信息的感知,則以大范圍回轉(zhuǎn)方式漫游,直至重新獲得導(dǎo)航信息。

AUV的本體“MM-01”如圖2所示,由主艇體和左右兩個電池艙組成。主艇體分為前后艙,分別為設(shè)備艙和電控艙。AUV共有4個推進器,包括兩個縱向主推、一個側(cè)推和一個垂推。兩個主推方向與縱軸平行,垂推和側(cè)推均布置在AUV中橫剖面位置,推進器的布置符合對稱原則,這樣保證了AUV具有四個自由度的機動能力:進退、側(cè)移、潛浮和轉(zhuǎn)首。值得注意的是:設(shè)計側(cè)推的目的是使AUV具備懸停能力,這是與常規(guī)的巡航式AUV的推進器布置方面的最大區(qū)別,也是懸停式AUV結(jié)構(gòu)方面的最本質(zhì)特征。

圖1 AUV的使命任務(wù)Fig.1 AUV′smission

圖 2 “MM-01”本體Fig.2 Main body of“MM-01”

2 AUV控制策略設(shè)計

2.1 AUV控制體系結(jié)構(gòu)

“MM-01”的控制體系結(jié)構(gòu)如圖3所示,包括任務(wù)使命、行為決策、底層運動控制、AUV和傳感器模塊,其中任務(wù)使命和行為決策模塊共同構(gòu)成了AUV的高層自治控制系統(tǒng)。任務(wù)使命模塊內(nèi)部保存航行任務(wù)參數(shù),由于比較簡單,該模塊實際被分解到了行為決策模塊的不同行為中。行為決策模塊包括基本行為和仲裁器,前者包括漫游、巡航、避障,仲裁器用于協(xié)調(diào)多個行為的沖突。自治控制系統(tǒng)將計算得到的控制指令送給底層運動控制器,后者得到各自由度的推力和力矩,經(jīng)推力分配送給AUV。

2.2 具備懸停功能的運動控制系統(tǒng)設(shè)計

非智能潛水器的經(jīng)典運動控制模式如圖4所示。其中ξ、η、ζ、Ψ分別表示潛水器在絕對坐標(biāo)系中的三維坐標(biāo)和首向角,下標(biāo)g表示期望值,i表示實際值;ex、ey、ez、eΨ分別表示潛水器與目標(biāo)點在運動坐標(biāo)系中的三維坐標(biāo)偏差和首向角偏差。由于懸停式AUV常在低速下運動,可以將運動控制分為水平面控制和垂直面控制,并近似認(rèn)為各自由度之間不存在耦合關(guān)系,然后分別設(shè)計控制器。為了實現(xiàn)AUV的懸停功能,在水平面運動控制回路中,除對偏差信號進行比例控制外,提出引入內(nèi)環(huán)反饋即速度(角速度)反饋的思想,這將大大改善閉環(huán)性能,使系統(tǒng)更易穩(wěn)定。

為了更好地實現(xiàn)清晰分工的模塊化思想,定義高層自治控制系統(tǒng)與運動控制系統(tǒng)的接口為e=[exeyezeΨ],并將圖4虛線框中的部分定義為運動控制模塊。

水平面和垂直面運動控制律:

式中:Kex、Key、KeΨ、Kez為偏差比例調(diào)節(jié)系數(shù) ,符號為正;Ku、Kv、Kr為速度反饋增益系數(shù) ,符號為負(fù)。

由于四個推進器按前述方式對稱布置,Tx和MTz經(jīng)過推力分配得到兩個水平主推的推力,Ty和Tz則分別對應(yīng)側(cè)推和垂推的推力。

圖3 MM-01控制體系結(jié)構(gòu)Fig.3 Control structure of“MM-01”

圖4 運動控制原理Fig.4 Motion control scheme

2.3 基于行為的自治控制系統(tǒng)設(shè)計

2.3.1 行為接口與仲裁

基本行為與仲裁器的接口指令eb同時包括相應(yīng)的行為激活指示符和目標(biāo)點與潛水器在運動坐標(biāo)系中的三維位置偏差和首向偏差,即eb=[bisactiveexeyezeΨ],其中bisactive是布爾變量,對漫游、巡航、避障行為而言分別是Wanderactive、Cruiseactive、Aroidactive,表示某行為是否激活,是則bisactive=1,否則bisactive=0。

若在同一時刻有多個行為被激活,則由仲裁器決定哪個行為的指令此刻將得到執(zhí)行。采用基于固定優(yōu)先級機制的仲裁器,事先設(shè)定不同行為的優(yōu)先級。當(dāng)多個行為同時發(fā)生時,優(yōu)先級高的行為將首先得到執(zhí)行,低優(yōu)先級的行為被抑制,直到?jīng)]有優(yōu)先級比它更高的行為激活時,低優(yōu)先級的行為才得到執(zhí)行。換言之,仲裁器的輸出為e=[exeyezeΨ],對應(yīng)當(dāng)前時刻激活行為中優(yōu)先級最高的行為送出的指令偏差。優(yōu)先級的高低根據(jù)行為的重要性和緊急情況設(shè)定,中的三個行為按優(yōu)先級從低到高依次是漫游、巡航、避障。漫游行為是底層的本能行為,始終激活,用于在失去位置導(dǎo)航信息的時候隨機漫游,以等待優(yōu)先級更高的任務(wù)被激活。巡航行為用于完成三維空間內(nèi)的軌跡遍歷,在當(dāng)前位姿可測時激活。如果前進路徑上出現(xiàn)障礙物,且障礙物危及到了潛水器的自身安全(到潛水器的距離小于5 m),則避障行為被激活。巡航行為具有目標(biāo)導(dǎo)向功能,而避障行為具有實時反應(yīng)能力,通過二者的協(xié)同作用,潛水器可以實現(xiàn)繞過障礙物抵達目標(biāo)點的任務(wù)。

2.3.2 漫游行為

漫游行為有以下兩種模式:

1)當(dāng)潛水器能獲得當(dāng)前絕對位置導(dǎo)航信息時,游向固定點。行為自身存儲目標(biāo)點坐標(biāo)以引導(dǎo)潛水器的運動。只需實時計算潛水器與目標(biāo)點在絕對坐標(biāo)系下的三維位置偏差和首向角偏差,進行坐標(biāo)變換得到ewander=[WanderactiveexwandereywanderezwandereΨwander]即可。

2)當(dāng)潛水器不能獲得當(dāng)前絕對位置導(dǎo)航信息時,按固定方式運動。由于本研究中采用水池中央頂部的攝像頭對AUV進行絕對定位,當(dāng)AUV在視野邊緣或光線不足時,可能導(dǎo)致攝像頭無法捕獲AUV當(dāng)前絕對坐標(biāo)ξ、η,此時漫游行為輸出常值偏差指令ewander=[WanderactivelcoseΨlsineΨ0eΨ],eΨ可設(shè)定為范圍在[0]內(nèi)的任意值,即潛水器以一定的半徑大范圍回轉(zhuǎn)(半徑大小可由調(diào)整),直至攝像頭重新獲得潛水器的絕對坐標(biāo)。

2.3.3 巡航行為

行為實時比較當(dāng)前位姿和由子目標(biāo)點決定的期望位姿得到偏差指令,如式(2)所示。

式中各變量的含義與2.2節(jié)相同。行為自身預(yù)存航行任務(wù)參數(shù),包括各目標(biāo)點位置坐標(biāo)和遍歷順序。設(shè)計中規(guī)定兩個相鄰子目標(biāo)點須保證在同一水平面上或不同水平面的同一垂線上,這樣AUV就只有兩種基本運動狀態(tài):定深航行和垂直潛浮。二者的控制策略如下:

1)定深航行:當(dāng)潛水器距離當(dāng)前子目標(biāo)點較遠(yuǎn)時,Ψg為當(dāng)前位置和目標(biāo)點連線的方向角,該值隨AUV的運動實時變化;當(dāng)潛水器距離目標(biāo)點非常近時,Ψg設(shè)定為某一常值(如上一子目標(biāo)點與當(dāng)前子目標(biāo)點的連線在絕對坐標(biāo)系中的方向角),即進行定向航行。這樣可以避免潛水器在距離目標(biāo)點非常近時頻繁調(diào)整首向的情況,從而最終懸停在目標(biāo)點。

2)垂直潛浮:類似地,設(shè)定Ψg為某一常值(潛水器在上一子目標(biāo)點處的懸停首向角)。

每次到達當(dāng)前子目標(biāo)點,則從數(shù)據(jù)庫中取出下一個點作為新的當(dāng)前子目標(biāo)點,直至到達終點。巡航行為的輸出為

2.3.4 避障行為

借鑒人類避障的思想,利用模糊控制方法設(shè)計模糊避障控制器[5],解決避障問題。

巡航行為中的當(dāng)前目標(biāo)點是固定的,AUV只需實時比較當(dāng)前位置與當(dāng)前目標(biāo)點之間的偏差,即可得到控制指令eb。而避障行為則完全不同:由于障礙物外形是不可預(yù)測的,AUV的航行路線也是不可預(yù)測的,所以航行過程中的當(dāng)前目標(biāo)點是實時變化的,將其定義為“虛目標(biāo)點”。為簡單起見,將“虛目標(biāo)點”始終設(shè)定為AUV左前方或右前方2 m處的點,目標(biāo)點方向與當(dāng)前首向的夾角大小和正負(fù)由控制器決定。避障示意如圖5所示。避障傳感器為前視攝像機和激光器,激光器安裝在AUV的頭部兩側(cè),間距0.5 m,安裝方向與AUV的軸線平行,配合攝像機可以測得左右兩側(cè)到障礙物的距離dl、dr。設(shè)計模糊避障控制器,輸入量為dl、dr。輸出量為轉(zhuǎn)向角增量ΔΨ(為方便計,后面用Ψ表示),如圖6所示。

輸入量的論域為{0,5},模糊集合為di={nearmedfar},i=l,r;輸出量Ψ的論域為模糊集合為{NBNSZOPSPB}。其中near,med,far分別表示距離近、距離適中和距離遠(yuǎn);NB,NS,ZO,PS,PB表示向左急轉(zhuǎn)、向左轉(zhuǎn)、不轉(zhuǎn)、向右轉(zhuǎn)、向右急轉(zhuǎn)。di和Ψ的隸屬函數(shù)均采用三角形函數(shù),如圖7所示。

根據(jù)人類避障的經(jīng)驗,設(shè)計針對圓柱形障礙物的模糊避障控制規(guī)則(見表1),并采用最大隸屬度法對輸出量進行清晰化處理。

圖5 避障過程示意Fig.5 Sketchmap of obstacle-avoiding behavior

圖6 模糊避障控制器Fig.6 Fuzzy logic controller

圖7di和Ψ的隸屬函數(shù)Fig.7 Member function fordiandΨ

避障的原則是往沒有障礙物或障礙物距離更遠(yuǎn)的一側(cè)轉(zhuǎn)向,當(dāng)兩側(cè)障礙物距離相等時,默認(rèn)向右轉(zhuǎn)。轉(zhuǎn)向的程度與最近的障礙物距離相關(guān),與障礙物的距離越近,轉(zhuǎn)向角度越大,反之越小。行為輸出為eavoid=[Avoidactiveexavoideyavoid

ezavoideΨavoid],顯然有下列關(guān)系:

于是,只要得到了Ψ就可以得到避障行為的輸出指令。AUV避障過程:當(dāng)視野前方?jīng)]有障礙物時,AUV執(zhí)行巡航行為,趨向目標(biāo)點;當(dāng)前方出現(xiàn)障礙物并且危及到自身安全時,避障行為被激活,AUV向遠(yuǎn)離障礙物的方向運動,當(dāng)前方?jīng)]有障礙物時,巡航行為再次被激活。AUV就是通過這兩種行為間的反復(fù)切換來實現(xiàn)避障和到達目標(biāo)的雙重目的。

3 控制策略仿真與分析

為了驗證控制策略的有效性,進行了AUV全系統(tǒng)的Simulink仿真。其中AUV動力學(xué)模型是依據(jù)試驗平臺“MM-01”的參數(shù)建立的狀態(tài)方程模型 ,模型的狀態(tài)量為u、v、w、q、r、θ、Ψ、ξ、η、ζ,即 AUV 的運動(角)速度和在絕對坐標(biāo)系中的位置和姿態(tài)(除橫傾和橫傾角速度外)。

3.1 漫游行為

設(shè)定AUV的初始狀態(tài)為u=0.514 4m/s,v=w=0,q=r=0,在原點出發(fā),目標(biāo)點PG=(5,5)。仿真時間t=200 s,假設(shè)在時間t=5 s至t=30 s內(nèi),AUV失去了位置導(dǎo)航信息,而在t=30 s以后又重新獲得該信息。運動軌跡如圖8所示。可見,當(dāng)失去導(dǎo)航信息時AUV進行了大范圍回轉(zhuǎn)的漫游,在獲得導(dǎo)航信息后重新駛向目標(biāo)點,最終在PG實現(xiàn)了懸停。

3.2 巡航行為

設(shè)定子目標(biāo)點序列為:P1=(5,5,0),P2=(10,5,0),P3=(10,5,5),P4=(15,10,5),P5=(20,10,5),P6=(25,15,5),初始狀態(tài)與漫游行為中的設(shè)置相同,仿真時間t=1 500 s。

仿真結(jié)果如圖9～11所示,其中圖9為三維運動軌跡,圖10～11分別為其頂視圖和側(cè)視圖?？梢?AUV可靠地完成了定深航行和垂直潛浮的動作,準(zhǔn)確地遍歷了6個目標(biāo)點,最終在終點實現(xiàn)了穩(wěn)定的懸停,這與我們的設(shè)計思想是吻合的。

3.3 避障行為

研究水平面內(nèi)的避障,通過避障和簡化的巡航行為結(jié)合實現(xiàn)避障目的。設(shè)定目標(biāo)點PG=(20,0),初始狀態(tài)與漫游行為中的設(shè)置相同。設(shè)置圓柱形障礙物,圓心O=(10,0),分別設(shè)半徑R=0.5、1、2 m,以演示控制策略在面對大小不同的障礙物時的避障能力。

仿真結(jié)果如圖12所示。AUV成功地避開了R=1m和R=2m的圓形障礙物,卻撞上了R=0.5m的障礙物,這是由于兩側(cè)激光器之間的距離相對較大,無法較好地識別體積較小的障礙物,該問題可以通過在AUV頭部中央加裝激光器加以解決。AUV躲過障礙物后,都重新規(guī)劃航線,最終順利地到達了預(yù)定的目標(biāo)點PG。

圖8 漫游行為的運動軌跡Fig.8 Moving trajectory of wander behavior

圖9 巡航行為的三維運動軌跡Fig.9 3D moving trajectory of cruise behavior

圖10 頂視圖(XY)Fig.10 Top view

圖11 側(cè)視圖(XZ)Fig.11 Lateral view

圖12 避障軌跡Fig.12 Moving trajectory of obstacle-avoiding behavior

3.4 多行為仿真

設(shè)定AUV的初始狀態(tài)與前述設(shè)置相同,子目標(biāo)點序列:P1=(5,5,0),P2=(10,5,0),P3=(10,5,5),P4=(15,10,5),P5=(25,10,5),P6=(30,15,5),在前進的必經(jīng)路徑上設(shè)置圓柱形障礙物,圓心O=(22,10),半徑R=1 m,高度h=6 m。假設(shè)在時間t=5 s至t=30 s內(nèi),AUV失去了位置導(dǎo)航信息,在t=30 s以后又重新獲得該信息。仿真結(jié)果如圖13～14所示。

可見,在行為仲裁器的有序協(xié)調(diào)下,AUV完成了大范圍回轉(zhuǎn)和軌跡點遍歷,并成功地躲過了障礙物,最終順利到達航行任務(wù)的終點,并完成了穩(wěn)定的懸停。結(jié)果有力地證明了控制策略的有效性。

圖13 多行為結(jié)合的三維運動軌跡Fig.13 3D moving trajectory ofmultiple behaviors

圖14 運動軌跡頂視圖(XY)Fig.14 Top view

4 結(jié) 語

主要討論了實現(xiàn)AUV的懸停、大范圍回轉(zhuǎn)漫游、三維空間內(nèi)的軌跡點遍歷和自主避障的控制方法。仿真結(jié)果驗證了基于行為的控制結(jié)構(gòu)應(yīng)用于AUV控制的有效性,同時也表明內(nèi)環(huán)反饋在AUV的懸?？刂浦芯哂嘘P(guān)鍵意義。提出的懸停運動及自治控制策略對懸停式AUV今后的研究開發(fā)具有重要的啟示作用,可以應(yīng)用于工程實際。

[1] Tamaki Ura.History of R&D and deploymentsof AUVs constructed at ura laboratory during 1984-2009[R].Shanghai:Shanghai Jiao Tong Univesity,2009.

[2] 郝穎明,吳清瀟,周 船,等.基于單目視覺的水下機器人懸停定位技術(shù)與實現(xiàn)[J].機器人,2006,28(6):656-661.

[3] Cuf X,Garcia R,Ridao P.An approach to vision-based station keeping for an unmanned underwater vehicle[C]∥Proceedingsof the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems.2002:799-804.

[4] Doucy O,Brutzman D,Healey A.Near surfacemanoeuvring and station-keeping for an autonomous underwater vehicle[C]∥NATO Symposium.2000.

[5] 石辛民,郝整清.模糊控制及其Matlab仿真[M].北京:清華大學(xué)出版社,北京交通大學(xué)出版社,2008:89-113.

Research on control strategy of a hovering-type AUV

LIWen-tao,GE Tong
(State Key Laboratory of Ocean Engineering,Shanghai Jiao Tong University,Shanghai 200030,China)

TP242.6

A

1005-9865(2011)01-0075-07

2010-04-12

上海交通大學(xué)海洋工程國家重點實驗室自主研究課題資助項目

李文濤(1986-),男,山東泰安人,碩士生,主要從事水下機器人的運動控制方面的研究。