俞炅?xí)F，吳 超，趙 敏，葛 彤

(上海交通大學(xué) 船舶海洋與建筑工程學(xué)院，上海 200240)

重于水型AUV模糊路徑規(guī)劃與優(yōu)化

俞炅?xí)F，吳 超，趙 敏，葛 彤

(上海交通大學(xué) 船舶海洋與建筑工程學(xué)院，上海 200240)

為了實(shí)現(xiàn)重于水型自治潛器（Heavier-Than-Water Autonomous Underwater Vehicle, AUV）在未知水下環(huán)境中的局部路徑規(guī)劃。首先根據(jù)AUV的運(yùn)動(dòng)方程和機(jī)翼升力特性，在M atlab中建立動(dòng)力學(xué)模型。設(shè)計(jì)模糊控制器，以聲吶對(duì)障礙物探測(cè)得到的距離和方位信息作為輸入量，實(shí)現(xiàn)了AUV在水下障礙物環(huán)境中的路徑規(guī)劃。最后，為提高AUV的能量利用效率，用遺傳算法對(duì)模糊控制器的隸屬度函數(shù)進(jìn)行優(yōu)化，并通過(guò)仿真試驗(yàn)證明了優(yōu)化后AUV在避障期間能源消耗更少，說(shuō)明該路徑規(guī)劃方法具有實(shí)際應(yīng)用價(jià)值。

重于水型AUV；模糊路徑規(guī)劃；遺傳算法；避障

0 引 言

重于水型自治潛器（簡(jiǎn)稱AUV）的局部路徑規(guī)劃是指在未知的水下環(huán)境中，潛水器根據(jù)傳感器對(duì)環(huán)境的探測(cè)，自行規(guī)劃得到一條無(wú)碰撞路徑。AUV如何根據(jù)規(guī)劃算法實(shí)時(shí)地推理出下一步的行進(jìn)路線，是局部路徑規(guī)劃的關(guān)鍵。比較典型的方法有Khatib提出的人工勢(shì)場(chǎng)法[1]，但該算法易陷入局部最小點(diǎn)，存在目標(biāo)不可達(dá)問(wèn)題[2]。另一種方法是根據(jù)Zadeh在1965年提出的模糊集合理論發(fā)展而來(lái)的模糊路徑規(guī)劃方法，通過(guò)把人的經(jīng)驗(yàn)歸納為模糊規(guī)則來(lái)控制AUV在水下未知環(huán)境中進(jìn)行路徑規(guī)劃。目前，陳衛(wèi)東[3]、付宜利[4]通過(guò)與傳統(tǒng)規(guī)劃方法的對(duì)比, 論證了模糊路徑規(guī)劃的有效性。孫兵[5]、楊敬輝[6]等學(xué)者使用智能算法對(duì)模糊控制器進(jìn)行優(yōu)化，獲得了更好的路徑規(guī)劃效果。但是上述

優(yōu)化大多以優(yōu)化路徑為目標(biāo)，未考慮系統(tǒng)的節(jié)能等因素，且應(yīng)用對(duì)象很少是AUV。本文針對(duì)水下潛器的運(yùn)動(dòng)特性設(shè)計(jì)了模糊控制器，實(shí)現(xiàn)了AUV的局部路徑規(guī)劃?？紤]到模糊控制器很大程度上依賴于專家經(jīng)驗(yàn)，可能會(huì)造成能源的浪費(fèi)，故將遺傳算法應(yīng)用于模糊控制器的優(yōu)化設(shè)計(jì)，通過(guò)選擇AUV的主要耗能裝置螺旋槳的轉(zhuǎn)速作為評(píng)價(jià)指標(biāo)，實(shí)現(xiàn)了AUV局部路徑規(guī)劃時(shí)消耗更少的系統(tǒng)能源。該方法的有效性通過(guò)仿真得到驗(yàn)證。

1 模型

1.1 重于水型AUV模型

重于水型AUV配置了機(jī)翼，不依靠浮力而是通過(guò)潛水器航行中獲取的流體升力平衡其在水中的剩余重量。不需要安裝龐大的浮力部件，其體積遠(yuǎn)小于常規(guī)的潛水器，能夠在速度、成本、機(jī)動(dòng)性等方面體現(xiàn)其優(yōu)勢(shì)。大范圍巡航作業(yè)過(guò)程中使升力大致與AUV水中重量相等，并由操縱面對(duì)AUV姿態(tài)進(jìn)行調(diào)整[7]。模型以上海交通大學(xué)水下工程研究所的“飛魚”Ⅱ重于水型AUV為原型，其外形如圖1所示。

潛水器的運(yùn)動(dòng)方程可以表示為非線性微分方程[8]，見(jiàn)式（1）。

其中：M為剛體質(zhì)量和附加質(zhì)量矩陣； C(v)為剛體轉(zhuǎn)動(dòng)慣量和科氏力矩陣； D(v)為線性和二次水動(dòng)力項(xiàng)矩陣；g(η)為重力和浮力項(xiàng)；；；Min, Nin]T。

η為大地坐標(biāo)系下AUV的位置和歐拉角信息；ν為體坐標(biāo)系中的（角）速度信息；τ為輸入的力（矩）。其中，η和ν之間存在轉(zhuǎn)換關(guān)系，為大地坐標(biāo)系與體坐標(biāo)系之間的映射矩陣，表示為式（2），具體形式參考文獻(xiàn)[9]。

“飛魚”Ⅱ型AUV在水中重力 P 大于浮力 B，且方向總是鉛錘的，故在大地坐標(biāo)系中分量為。根據(jù)大地坐標(biāo)系到體坐標(biāo)系的轉(zhuǎn)換矩陣（具體參考文獻(xiàn)[10]），將其轉(zhuǎn)換到體坐標(biāo)系中，表示為靜力項(xiàng)，見(jiàn)式（3）。其中，浮心坐標(biāo)為，重心坐標(biāo)為（已省略）。

“飛魚”Ⅱ型的機(jī)翼布置于AUV重心正上方，在運(yùn)動(dòng)中將產(chǎn)生升力，在體坐標(biāo)系中的分量表示為，再將其加入到式（1）的輸入項(xiàng)τ中。升力L由式（4）給出。其中，升力系數(shù)CL估算公式由式（5）給出，是攻角為0時(shí)機(jī)翼的升力系數(shù)。為不考慮機(jī)翼和艇體干擾的升力系數(shù)隨攻角的變化率。根據(jù)機(jī)翼面積和潛水器的主尺度l，可將其轉(zhuǎn)換為考慮相互干擾的升力系數(shù)變化率。

對(duì)式（1）線性化簡(jiǎn)和移項(xiàng)，得到重于水型AUV的空間運(yùn)動(dòng)狀態(tài)方程，見(jiàn)式（6）。其中是線性化簡(jiǎn)后的矩陣，由于AUV速度較慢，故忽略。根據(jù)式（6）建立動(dòng)力學(xué)模型，迭代求解微分方程可解得在特定輸入下，潛水器的速度狀態(tài)ν，繼而由轉(zhuǎn)換關(guān)系可以得到潛水器的位姿狀態(tài)。

1.2 聲吶模型

未知的水下環(huán)境中，AUV需要通過(guò)聲吶獲取周圍障礙物的分布情況，并根據(jù)障礙物的距離推理出適當(dāng)?shù)霓D(zhuǎn)速與舵角，進(jìn)行局部路徑規(guī)劃。仿真中依據(jù)實(shí)際聲吶參數(shù)建立聲吶模型，模擬聲吶對(duì)未知水下障礙物的探測(cè)。重于水型AUV使用某型避障聲吶，聲吶水平開角范圍，最大探測(cè)距離200 m，如圖3所示。聲吶返回的數(shù)據(jù)為障礙物3個(gè)邊緣點(diǎn)（A, B,C點(diǎn)）與聲吶的距離[11]。在仿真中，只取首向（0°）、+45°和–45°三個(gè)方向障礙物距AUV最近點(diǎn)的距離信息作為模糊控制器的輸入量，若障礙物距離大于聲吶探測(cè)距離則返回最大探測(cè)距離。

2 模糊控制器計(jì)與優(yōu)化

AUV在水下的低速運(yùn)動(dòng)可以解耦為水平面和垂直面2個(gè)平面空間的運(yùn)動(dòng)，本文設(shè)計(jì)的模糊控制器實(shí)現(xiàn)的是水平面內(nèi)的局部路徑規(guī)劃，AUV的深度由單獨(dú)的深度控制器控制，擬采用S面控制器[12]。模糊控制器的輸出為垂直舵的舵角和螺旋槳轉(zhuǎn)速。模糊控制器的主要結(jié)構(gòu)包括：模糊化模塊、知識(shí)庫(kù)、模糊推理模塊和清晰化模塊4部分，如圖4中虛線框部分。圖4為遺傳算法優(yōu)化的模糊路徑規(guī)劃系統(tǒng)。

2.1 模糊語(yǔ)言變量定義

模糊控制器5個(gè)輸入量為：

2）目標(biāo)方向（Direction）表示目標(biāo)位置和體坐標(biāo)原點(diǎn)連線與AUV首向的夾角，如圖2所示，包含{Left, M iddle, Right}3個(gè)集合。

3）AUV當(dāng)前的“速度（Speed）”包含{Slow，F(xiàn)ast}2個(gè)集合。

模糊控制器2個(gè)輸出量為：

“舵角（Rudder）”和螺旋槳的“轉(zhuǎn)速（RPM）”，為了實(shí)現(xiàn)對(duì)輸出量較為精細(xì)地控制，舵角和轉(zhuǎn)速分別劃分為5個(gè)模糊集合，分別為{BRight, SRight, M idd le,SLeft, BRight}和{NB, NS, ZZ, PS, PB}。

2.2 隸屬度函數(shù)設(shè)計(jì)

AUV具有轉(zhuǎn)向半徑較大和欠驅(qū)動(dòng)等特點(diǎn)。所以，需要根據(jù)其特點(diǎn)專門設(shè)計(jì)隸屬度函數(shù)和模糊規(guī)則庫(kù)。設(shè)計(jì)如下：

1）語(yǔ)言變量“距離”的隸屬度函數(shù)采用不對(duì)稱的設(shè)計(jì)，形狀為梯形，論域?yàn)閇–1，1]，如圖5（a）所示，整體偏向右側(cè)，即AUV更加傾向于將障礙物距離判斷為Near，保證其與障礙物之間留有足夠的安全距離。但是這樣做也可能導(dǎo)致規(guī)劃路徑變長(zhǎng)、能耗增加，因此后面將對(duì)隸屬度函數(shù)進(jìn)行優(yōu)化。

2）目標(biāo)方向隸屬度函數(shù)為對(duì)稱設(shè)計(jì)，障礙物在首向左側(cè)為負(fù)，右側(cè)為正，如圖5（b）所示。

3）速度隸屬度函數(shù)如圖5（c）所示。

4）輸出量舵角（Rudder）和轉(zhuǎn)速（RPM）的隸屬度函數(shù)形狀一致，皆如圖5（d）所示。

2.3 模糊控制規(guī)則庫(kù)

完成隸屬度函數(shù)的設(shè)計(jì)后，根據(jù)經(jīng)驗(yàn)定義模糊規(guī)則庫(kù)，作為AUV路徑規(guī)劃的指導(dǎo)規(guī)則。該模糊控制器總共有5個(gè)輸入，每個(gè)輸入量包含的模糊集合數(shù)量分別為 2，2，2，3，2，所以該模糊規(guī)則庫(kù)共有 2×2×2×3×2=48條規(guī)則，部分模糊規(guī)則見(jiàn)表1。

2.4 遺傳算法優(yōu)化

上面設(shè)計(jì)的隸屬度函數(shù)很大程度上依賴于專家的經(jīng)驗(yàn)知識(shí)[13]，可能無(wú)法適應(yīng)所有的水下障礙物環(huán)境，且浪費(fèi)系統(tǒng)能源。對(duì)此本文采用遺傳算法來(lái)優(yōu)化隸屬度函數(shù)的設(shè)計(jì)，進(jìn)一步提高模糊路徑規(guī)劃的可靠性和能量利用率。

遺傳算法優(yōu)化的主要運(yùn)行流程如圖6所示：

1）初始種群生成。首先產(chǎn)生包含30個(gè)個(gè)體的“種群”，個(gè)體的基因選擇為隸屬度函數(shù)圖5中標(biāo)注拐點(diǎn)處的橫坐標(biāo)值，表示為向量,?；騇為實(shí)數(shù)向量，代表一個(gè)個(gè)體，對(duì)應(yīng)著一種模糊控制器。初始種群的生成是在設(shè)定的定義域內(nèi)隨機(jī)產(chǎn)生實(shí)數(shù)向量M。

2）仿真測(cè)試。調(diào)用每個(gè)個(gè)體設(shè)置模糊控制器，控制AUV進(jìn)行路徑規(guī)劃，并返回AUV的運(yùn)行結(jié)果。

3）計(jì)算適應(yīng)度。在單個(gè)個(gè)體控制AUV進(jìn)行避障路勁規(guī)劃后，選取推進(jìn)器在運(yùn)動(dòng)期間的總旋轉(zhuǎn)圈數(shù)（簡(jiǎn)稱總轉(zhuǎn)數(shù)）作為種群中每個(gè)個(gè)體的適應(yīng)度評(píng)價(jià)指標(biāo)，見(jiàn)式（7）。并線性地給每個(gè)個(gè)體分配適應(yīng)度，見(jiàn)式（8）?？傓D(zhuǎn)數(shù)越低，適應(yīng)度越大，即個(gè)體基因有更大概率遺傳到下一代；反之就越小，更容易被淘汰（碰到障礙物的個(gè)體直接被淘汰）。

表1 模糊規(guī)劃規(guī)則表Tab. 1 Fuzzy control rules

5）選擇。根據(jù)每個(gè)個(gè)體的適應(yīng)度，運(yùn)用輪盤賭選擇法對(duì)個(gè)體進(jìn)行選擇。

6）最后進(jìn)行復(fù)制、變異和交叉操作，產(chǎn)生新種群?；蚪徊娴母怕试O(shè)置為80%，變異概率為8%。然后重復(fù)步驟 2 ～ 步驟 5。

3 結(jié)果與分析

為了加速優(yōu)化過(guò)程，首先在簡(jiǎn)單障礙物環(huán)境中進(jìn)行路徑規(guī)劃和遺傳算法的優(yōu)化。優(yōu)化仿真中種群繁衍至第12代停止，每一代的總能耗(使用種群平均的總轉(zhuǎn)數(shù)表示)如圖7所示。可以看出，經(jīng)過(guò)一代進(jìn)化，由于初始種群中一些適應(yīng)度很低的個(gè)體被淘汰，種群的總能耗已有了大幅降低；隨后每代種群總能耗平穩(wěn)收斂，直到滿足種群差異性的要求，結(jié)束優(yōu)化。末代種群中的最優(yōu)個(gè)體總轉(zhuǎn)數(shù)為，選取此個(gè)體的基因作為優(yōu)化后的隸屬度函數(shù)。

優(yōu)化前，模糊控制器隸屬度函數(shù)的定義如圖5所示，其基因（拐點(diǎn)橫坐標(biāo)）為M=[0.4 0.4 0.8 0.8, 0.25 0 0.25, 0.2 0.2 0.8 0.8]；優(yōu)化后的基因?yàn)閇0.581 0 0.553 5 0.997 7 0.956 8 0.216 1 0.042 9 0.762 2 0.064 2 0.017 7 0.716 5 0.761 1]。優(yōu)化前后的模糊控制器控制下AUV路徑規(guī)劃期間的轉(zhuǎn)速和路徑對(duì)比如圖8和圖9?？梢钥闯鰞?yōu)化后AUV在路徑規(guī)劃期間轉(zhuǎn)速明顯降低，且路徑規(guī)劃運(yùn)動(dòng)總時(shí)間與優(yōu)化前基本持平，均為180 s左右。

為了驗(yàn)證優(yōu)化后模糊路徑規(guī)劃的可靠性和節(jié)能特性，本文設(shè)置了更加復(fù)雜的環(huán)境，檢驗(yàn)AUV在不同環(huán)境中是否能達(dá)到快速、節(jié)能的目標(biāo)。圖10為復(fù)雜環(huán)境中AUV路徑規(guī)劃期間轉(zhuǎn)速的對(duì)比，其具體性能比較見(jiàn)表2?？梢钥闯霰苷蠙C(jī)動(dòng)期間（曲線凹陷部分），優(yōu)化后的AUV轉(zhuǎn)速更低。雖然時(shí)間有略微增加，但是總體來(lái)說(shuō)總轉(zhuǎn)數(shù)降幅更加明顯。圖11是優(yōu)化前后AUV規(guī)劃路徑的對(duì)比。說(shuō)明優(yōu)化后的模糊控制器可以適應(yīng)復(fù)雜的水下環(huán)境，并控制AUV更加節(jié)能地完成局部路徑規(guī)劃任務(wù)。

表2 路徑規(guī)劃性能比較Tab. 2 Comparison of path planning performance

4 結(jié) 語(yǔ)

本文建立了重于水型AUV的動(dòng)力學(xué)模型，為其設(shè)計(jì)了模糊控制器，并使用遺傳算法對(duì)控制器進(jìn)行優(yōu)化。優(yōu)化后的模糊控制器使重于水型AUV可以在未知水域中更加節(jié)能地完成路徑規(guī)劃任務(wù)，對(duì)于提高能量利用效率具有重要意義。模糊控制器輸入輸出量嚴(yán)格對(duì)應(yīng)實(shí)際潛水器，可通過(guò)離散化的方式將控制器翻譯為C語(yǔ)言，并移植到水下嵌入式平臺(tái)上，對(duì)于實(shí)現(xiàn)重于水型AUV在水下環(huán)境中的局部路勁規(guī)劃具有重要的應(yīng)用價(jià)值。

[1]KHATIB O.Real time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics and Research,1986,5(1),90–98.

[2]LIY ing,HONG Sheng,HUANG Jun,et al.The new environment model building of penetration mission based on the artificial potencial field approach[C].Proc of 2012 IEEE International Muti-Disciplinay Conference on Congnitive Methods in Situation Awareness and Decision Support,2012:121–124.

[3]陳衛(wèi)東,朱奇光.基于模糊算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].電子學(xué)報(bào),2011,(04):971–974,980.

[4]付宜利,靳保,王樹國(guó),等.未知環(huán)境下基于行為的機(jī)器人模糊路徑規(guī)劃方法[J].機(jī)械工程學(xué)報(bào),2006,(05):120–125.

[5]孫兵,朱大奇,楊元元.基于粒子群優(yōu)化的自治水下機(jī)器人模糊路徑規(guī)劃[J].高技術(shù)通訊,2013,12:1284–1291.

[6]楊敬輝,洪炳镕,樸松昊.基于遺傳模糊算法的機(jī)器人局部避障規(guī)劃[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2004,07:946–948.

[7]顏翚,葛彤,王彪.一種新型重于水的自治式潛水器及其先進(jìn)性分析[J].上海交通大學(xué)學(xué)報(bào),2012,08:1173–1177.

[8]SARKARM,NANDY S,K VADALIS.R,etal.Modelling and simulation of a robust energy effcient AUV controller[J].Mathematics and Computers in Simulation,2016,121:34–47.

[9]FOSSEN.guidance and control of ocean vehicles[M].Norway,1994.

[10]施生達(dá).潛艇操縱性[M].北京:國(guó)防工業(yè)出版社,1995.

[11]毛宇峰,龐永杰,李曄,等.速度矢量坐標(biāo)系下水下機(jī)器人動(dòng)態(tài)避障方法[J].哈爾濱工程大學(xué)學(xué)報(bào),2010,02:159–164.

[12]劉建成,于華男,徐玉如,等.水下機(jī)器人改進(jìn)的 S 面控制方法[J].哈爾濱工程大學(xué)報(bào),2002,23(1):33–36.

[13]孫增圻.智能控制理論與技術(shù)[M].北京:清華大學(xué)出版社,1996.

Fuzzy path planning and optimization of a heavier-than-water AUV

YU Jiong-min, WU Chao, ZHAO M in, GE Tong
(School of Naval Architecture, Ocean and Civil Engineering, Shanghai Jiaotong University, Shanghai 200240, China)

In order to achieve the goal of local path planning of a Heavier-Than-Water AUV in unknown underwater environment. A matlab kinematic model of the AUV was built based on its equation of motion and lift characteristics. A fuzzy controller was designed that enables AUV to plan path in real time. The controller takes obstacles' distance and orientation obtained from the sonar as input to control the AUV. At last, genetic algorithm was used to optimize the membership function of the fuzzy controller for improving energy utilization. And simulation tests showed that AUV consumed less energy during obstacle avoidance stage after optimization, which proved that the path planning method has practical value.

Heavier-Than-Water AUV；fuzzy path planning；genetic algorithm；obstacle avoidance

TP242

A

1672–7649(2017)12–0076–05

10.3404/j.issn.1672–7649.2017.12.016

2017–04–06

國(guó)家重大儀器研制資助項(xiàng)目(41427806)

俞炅?xí)F(1992–)，男，碩士研究生，研究方向?yàn)樗潞叫衅鞯倪\(yùn)動(dòng)控制和基于視覺(jué)的水下對(duì)接。