胡　紐，董春凱，王志強(qiáng)（中國船舶重工集團(tuán)公司 第七〇五研究所昆明分部，云南 昆明 650118）

基于避碰聲吶的遠(yuǎn)程魚雷水下自主障礙規(guī)避算法研究

胡紐，董春凱，王志強(qiáng)
（中國船舶重工集團(tuán)公司 第七〇五研究所昆明分部，云南 昆明 650118）

伴隨著航程可達(dá)上千千米的遠(yuǎn)程魚雷的出現(xiàn)，對魚雷遠(yuǎn)程航渡過程中自主規(guī)避可能出現(xiàn)的障礙物提出新要求。本文以某型遠(yuǎn)程魚雷為背景，基于模糊控制方法，提出一種基于避碰聲吶的遠(yuǎn)程魚雷自主避障算法，提出規(guī)避障礙物的規(guī)劃，并通過計(jì)算模擬避碰聲吶探測信息，在仿真平臺(tái)上對該算法進(jìn)驗(yàn)證，結(jié)果表明運(yùn)用該算法遠(yuǎn)程魚雷能利用自身攜帶的前視避碰聲吶探測到障礙物，并通過障礙規(guī)避算法控制魚雷航行規(guī)避障礙物時(shí)，本文提出的障礙規(guī)避算法可以引導(dǎo)魚雷規(guī)避障礙物，到達(dá)設(shè)定的目標(biāo)點(diǎn)。

遠(yuǎn)程魚雷；障礙規(guī)避；算法仿真

0　引　言

遠(yuǎn)程魚雷是一種由魚雷發(fā)射平臺(tái)在防區(qū)外發(fā)射，發(fā)射后由魚雷自主航渡到指定作戰(zhàn)區(qū)域，執(zhí)行待機(jī)、巡航、搜索、攻擊、誘騙等任務(wù)的新型水中兵器［1］。這種魚雷的航程可達(dá)幾百甚至上千千米，在水下作業(yè)時(shí)間也大大超過了傳統(tǒng)魚雷，在水下看不見、摸不著的環(huán)境中，也沒有電子海圖的情況下［2］，遠(yuǎn)程魚雷的自主完成任務(wù)能力顯得十分重要，本文以某型遠(yuǎn)程魚雷為研究背景，提出一種基于避碰聲吶信息的遠(yuǎn)程魚雷障礙自主規(guī)避算法，通過利用自身攜帶的前視避碰聲吶探測障礙物，并通過解算控制其繞開障礙物并回到原航線，保證自身航行安全，順利完成預(yù)定任務(wù)。

1　系統(tǒng)組成及避障模擬

1.1系統(tǒng)組成

遠(yuǎn)程魚雷與傳統(tǒng)魚雷主要區(qū)別是在傳統(tǒng)魚雷的雷頂自導(dǎo)基陣的基礎(chǔ)上需要加裝一套用于探測、發(fā)現(xiàn)障礙物目標(biāo)的前視避碰聲吶系統(tǒng)，與規(guī)劃與決策系統(tǒng)、控制系統(tǒng)、動(dòng)力系統(tǒng)共同引導(dǎo)遠(yuǎn)程魚雷規(guī)避障礙物。系統(tǒng)組成結(jié)構(gòu)圖如圖1所示。

遠(yuǎn)程魚雷在自主避障過程中，前視避碰聲吶通過發(fā)射聲波并檢測接收到的回波判斷魚雷航行前方是否存在障礙物［3］，并上報(bào)規(guī)劃決策系統(tǒng)；規(guī)劃決策系統(tǒng)根據(jù)障礙物信息和自主避障算法形成避碰方案下發(fā)控制系統(tǒng)和動(dòng)力系統(tǒng)，并由控制和動(dòng)力系統(tǒng)分別通過操舵及調(diào)整航速操縱遠(yuǎn)程魚雷運(yùn)動(dòng)，避開障礙物。各系統(tǒng)信息傳遞流向及內(nèi)容如圖2 所示。

圖1　系統(tǒng)組成結(jié)構(gòu)圖Fig. 1　System composition structure

圖2　系統(tǒng)間信息流向及通信內(nèi)容Fig. 2　Information flow and communication between systems

1.2障礙物模擬

障礙物模擬的主要功能是通過人工設(shè)定障礙物形狀、位置、大小提供障礙物信息，供避碰聲吶判斷位置關(guān)系，產(chǎn)生避碰信息。

在實(shí)際應(yīng)用中障礙物應(yīng)該是任意形狀，為簡化研究本文提供了圓形和三角形 2 種基本形狀障礙物的避碰模擬，對其他形狀的模擬主要是通過這 2 種基本形狀的組合實(shí)現(xiàn)，其中圓形提供圓弧段，三角形提供直線段和突出角，組合方案如圖3 所示。

圖3　障礙物組合示意圖Fig. 3　Obstacles combination schemes

對障礙物的模擬主要在仿真平臺(tái)上完成，對圓形基本障礙物由圓心 X 坐標(biāo)、Y 坐標(biāo)、圓半徑 R 三個(gè)信息來描述；對三角形基本障礙物由頂點(diǎn) A，B，C 的 X坐標(biāo)、Y 坐標(biāo) 6 個(gè)信息來描述。仿真平臺(tái)提供 10 個(gè)圓形基本障礙物和 10 個(gè)三角形基本障礙物共 20 個(gè)障礙物的描述信息數(shù)組。

1.3避碰聲吶信息模擬

采用數(shù)學(xué)模擬方法，不考慮探測精度的影響，在開角范圍、波束設(shè)置、探測距離等方面采用如下設(shè)置［4］：

1）前視避碰聲吶為單平面聲吶，可以在遠(yuǎn)程魚雷水平方向探測，暫不考慮遠(yuǎn)程魚雷橫滾、俯仰對前視避碰聲吶的影響；

2）前視避碰聲吶探測波束設(shè)置在以遠(yuǎn)程魚雷為對稱軸左右各 30 °范圍內(nèi)，間隔 4 °，共 16 個(gè)波束，每個(gè)波束探測距離為 300 m；

3）前視避碰聲吶各波束之間無干擾，固定周期上傳各波束探測信息，包括有無障礙物及障礙物距離。

2　模糊控制避障規(guī)則

2.1系統(tǒng)輸入輸出

依上文假設(shè)，避障聲吶的波束開角為左右 30 °，探測距離為 300 m，以此為模糊控制方法的基本輸入，過多的輸入變量會(huì)造成模糊控制方法非常復(fù)雜，因此選擇探測到的障礙物距離作為避障算法的核心輸入變量，輸出量為魚雷航向控制角度。

2.2隸屬度函數(shù)的建立

隸屬度函數(shù)是對模糊概念的定量描述。隸屬度函數(shù)的確定過程，本質(zhì)上說應(yīng)該是客觀的，但每個(gè)人對于同一個(gè)模糊概念的認(rèn)識理解又有差異，因此，隸屬度函數(shù)的確定又帶有主觀性［5］。一般根據(jù)經(jīng)驗(yàn)或統(tǒng)計(jì)進(jìn)行確定，也可由專家給出。隸屬度函數(shù)形式有多種，根據(jù)實(shí)際問題而具體確定或選用，常見的有梯形、三角形、高斯型等隸屬度函數(shù)類型。

結(jié)合魚雷航行速度，障礙物距離設(shè)定變量論域?yàn)閐 = ［0， 300］，變量描述為 ｛D，N，M，F(xiàn)｝；輸出魚雷航向轉(zhuǎn)向角增量 θ 的控制量為 ［-30°，30°］。其中障礙物距離是指 16 個(gè)波束中，障礙物距離最小的那個(gè)值。變量描述 D，N，M，F(xiàn) 分別代表距離危險(xiǎn)、距離較近、距離適中、距離較遠(yuǎn)，距離區(qū)間分別是［0， 50］、［50，100］、［100， 200］、［200， 300］。本文采用三角類型與梯形類型結(jié)合的的隸屬度函數(shù)［6］，隸屬度函數(shù)模型見公式（1），隸屬度函數(shù)圖形如圖4 所示。

圖4　隸屬度函數(shù)示意圖Fig. 4　Membership functions

2.3模糊控制規(guī)則

模糊控制規(guī)則是由若干個(gè)語言變量構(gòu)成的模糊條件語句，表達(dá)了對被控制對象執(zhí)行控制規(guī)則時(shí)的模糊思維和判斷過程。模糊控制規(guī)則是對專家和理論知識與實(shí)踐的經(jīng)驗(yàn)總結(jié)，它是通過語言條件語句來模擬人類的控制行為，語句與專家的控制特性直接有關(guān)［7］。

本文針對障礙距離的不同，制定的模糊控制規(guī)則如下：

1）危險(xiǎn)區(qū)域。在危險(xiǎn)區(qū)域內(nèi)探測到障礙物時(shí)，留給魚雷機(jī)動(dòng)規(guī)避的時(shí)間、空間有限，所以在這一區(qū)域內(nèi)，一旦探測到障礙物，魚雷先停車上報(bào)避障故障，再做處理。

2）距離較近。在這個(gè)區(qū)域內(nèi)發(fā)現(xiàn)障礙物時(shí)，判斷障礙物距離最小值對應(yīng)的探測波束，然后根據(jù)該波束的安裝角度，確定魚雷的旋轉(zhuǎn)方向和角度，確定原則是魚雷轉(zhuǎn)過相應(yīng)的角度后，可以使該波束在魚雷的正前方位置。如左邊 10 ° 波束發(fā)現(xiàn)障礙物的距離最小，則魚雷右轉(zhuǎn) 10 °。

3）距離適中。這種情況下魚雷有足夠的時(shí)間和空間進(jìn)行避障機(jī)動(dòng)，避障控制方法有 2 種，A 方法是從外向內(nèi)，B 方法是從內(nèi)向外。

用 A 方法控制時(shí)，控制器首先檢查左 30 ° 波束和右 30 ° 波束，控制規(guī)則如表1 所示。

表1　控制規(guī)則表ATab. 1　Control rules of table A

當(dāng)魚雷左 30 ° 波束和右 30 ° 波束都探測到障礙物時(shí)，比較哪個(gè)波束探測到的障礙物距離更近，控制器控制魚雷向另一邊轉(zhuǎn)向；只有一個(gè)波束探測到障礙物時(shí)，控制器控制魚雷向另一邊轉(zhuǎn)向；兩邊都沒有探測到障礙物時(shí)，控制器首先檢查左 26 ° 波束和右 26 ° 波束是否有障礙物，如有障礙物按表規(guī)則類比執(zhí)行，如沒有障礙物繼續(xù)向內(nèi)檢查，直到所有波束都被檢查。

用 B 方法控制時(shí)，控制器從內(nèi)向外判斷，只要保證一定范圍內(nèi)沒有障礙物即認(rèn)為可以安全航行，如圖5所示，假設(shè)距離 200 m，安全寬度為 40 m，三角形的角度為 5.73 °，選 6 ° 波束即可滿足要求。

圖5　安全角度示意圖Fig. 5　Security angle

表2　控制規(guī)則 B 表Tab. 2　Control rules of table B

所以需要判斷第 7，8，9，10 四個(gè)波束是否有障礙物來確定魚雷航向，共有 16 種情況，其模糊控制規(guī)則見表2（表中數(shù)字 0 代表沒有探測到障礙物，數(shù)字 1代表探測到障礙物）。

當(dāng)出現(xiàn) 7，8，9，10 號波束都有障礙的情況時(shí)，控制器向外檢查，檢查 10 °，即 6，11 號波束是否探測到障礙物，以此類推。

4）距離較遠(yuǎn)。這時(shí)發(fā)現(xiàn)障礙物只做記錄，暫不規(guī)避。

3　仿真驗(yàn)證

3.1仿真平臺(tái)介紹

依據(jù)上述算法，在 Visual Studio 2008 環(huán)境下開發(fā)了基于對話框的“新型水中兵器自主避障算法”仿真驗(yàn)證平臺(tái)（見圖6）［8］。

圖6　魚雷避碰仿真平臺(tái)界面Fig. 6　Collision avoidance simulation platform

仿真平臺(tái)具備魚雷運(yùn)動(dòng)模擬、障礙物設(shè)定、避碰聲吶信息模擬、避碰規(guī)則驗(yàn)證等功能。

平臺(tái)界面左側(cè)可選擇障礙物的設(shè)置類型、避障規(guī)則及魚雷基本運(yùn)動(dòng)設(shè)定；平臺(tái)界面右側(cè)部分可通過TeeChart 控件顯示障礙物及魚雷運(yùn)動(dòng)軌跡及魚雷航行狀態(tài)參數(shù)。

3.2通過功能驗(yàn)證

設(shè)定魚雷從（0，0）點(diǎn)開始向（1 400，700）點(diǎn)運(yùn)動(dòng)，并設(shè)置一個(gè)圓形障礙物，圓心（750，450）半徑200。仿真開始分別用控制規(guī)則 A 和控制規(guī)則 B 控制魚雷避障機(jī)動(dòng)，觀察魚雷是否能規(guī)避障礙物航行，到達(dá)目標(biāo)點(diǎn)。

圖7　魚雷規(guī)避圓形障礙物Fig. 7　Avoid circular obstacle

仿真結(jié)果如圖7 所示，圖中圓形即為障礙物，顏色較淺的直線為魚雷預(yù)選規(guī)劃的航路，顏色較深的 2條曲線是魚雷在按航路規(guī)劃運(yùn)動(dòng)過程中，避碰聲吶探測到障礙物后，魚雷按 A、B 兩種不同的避碰規(guī)則進(jìn)行避障機(jī)動(dòng)的規(guī)避軌跡。從圖中可以直觀地看出：2種避碰規(guī)則都可以控制魚雷規(guī)避障礙物，到達(dá)目標(biāo)點(diǎn)。但是避碰規(guī)則 B 中，由于優(yōu)先考慮中間波束的障礙物信息，當(dāng)中間沒有障礙物時(shí)不考慮較外邊的探測波束，這樣相對于規(guī)則 A ，航程較小，可以減小一定的航程損失。

3.3穿越功能驗(yàn)證

設(shè)定魚雷從（600，-600）點(diǎn)開始向（600，1 800）點(diǎn)運(yùn)動(dòng)，并設(shè)置 2 個(gè)圓形障礙物，障礙物一圓心（400，1 400）半徑 100，障礙物二圓心（800，1 400）半徑 110。仿真開始分別用控制規(guī)則 A 和控制規(guī)則 B控制魚雷避障機(jī)動(dòng)，觀察魚雷是否能規(guī)避障礙物航行，到達(dá)目標(biāo)點(diǎn)。

仿真結(jié)果如圖所示，魚雷按 A、B 兩種不同的避碰規(guī)則進(jìn)行避障機(jī)動(dòng)時(shí)，避碰規(guī)則 B 可以使魚雷從障礙物的中間穿過，而避碰規(guī)則 A 只能控制魚雷從障礙群的邊緣繞過，航程損失較大（見圖8）。

圖8　魚雷規(guī)避 2 個(gè)圓形障礙物Fig. 8　Avoid two circular obstacles

由于避碰算法只對當(dāng)前周期內(nèi)的障礙信息進(jìn)行處理，即不記錄曾經(jīng)發(fā)現(xiàn)過的障礙物，更無法預(yù)料未來發(fā)現(xiàn)的障礙物，所以避碰規(guī)則 B 雖然可以從 2 個(gè)障礙物中穿過，但有可能陷入“U 型”陷阱中，即從障礙物中穿過后發(fā)現(xiàn)前方還有障礙物，走到頭發(fā)現(xiàn)是死胡同，還需要從陷阱中繞出來，再繞過障礙群，到達(dá)目標(biāo)點(diǎn)（見圖9），這樣其航程明顯比避碰規(guī)則 A 所用的航程更多，并且還存在繞不出來，撞向障礙物的風(fēng)險(xiǎn)。

3.4多種障礙物通過功能驗(yàn)證

圖10 所示分別是魚雷通過 1 個(gè)三角形障礙物、1個(gè)復(fù)雜障礙物、多個(gè)障礙物和多個(gè)復(fù)雜障礙物的驗(yàn)證示意圖，按照上述控制規(guī)則，魚雷都可以繞過或者穿越障礙物，到達(dá)目標(biāo)點(diǎn)，證明本文提出的障礙規(guī)避算法可行。

從圖中可看出，雖然控制規(guī)則 A 和控制規(guī)則 B 都可以控制魚雷繞過或者穿越障礙物，但還是存在以下問題：

1）是魚雷在避障后期會(huì)出現(xiàn)航向抖動(dòng)，在某一周期沒有障礙物時(shí)向目標(biāo)點(diǎn)運(yùn)動(dòng)；在下一周期由于魚雷位置、航向變化又探測到障礙物，需要按規(guī)則左轉(zhuǎn)或右轉(zhuǎn)；轉(zhuǎn)過之后沒有障礙物魚雷又改變航向向目標(biāo)點(diǎn)運(yùn)動(dòng)，這種現(xiàn)象會(huì)對魚雷舵機(jī)系統(tǒng)造成影響，如果魚雷在避障和向目標(biāo)點(diǎn)運(yùn)動(dòng)時(shí)的速度不同，又會(huì)造成魚雷電機(jī)頻繁改變轉(zhuǎn)速，對電機(jī)造成影響。

2）是在控制規(guī)則 A 的控制下，由于避障聲吶存在探測盲區(qū)，魚雷在繞過三角形障礙物時(shí)，特別是繞過三角形頂點(diǎn)時(shí)，會(huì)離障礙物非常近，有時(shí)甚至?xí)∮?0 m，雖然在控制規(guī)則 B 中加入了在小于 50 m 時(shí)緊急停車的規(guī)則，但沒有解決魚雷在障礙規(guī)避時(shí)距離太近的問題，反而增加了魚雷避障失敗停車的概率，導(dǎo)致魚雷無法正常到達(dá)目標(biāo)點(diǎn)。

3.5控制規(guī)則改進(jìn)

針對上節(jié)中提到的 2 個(gè)問題，對控制規(guī)則做了一些改進(jìn)，主要方案是在魚雷避障完成后，在某個(gè)周期沒有探測到障礙物時(shí)，不是馬上向目標(biāo)點(diǎn)運(yùn)動(dòng)，而是在魚雷當(dāng)前航向的正前方 200 m 處增加一個(gè)路徑調(diào)整點(diǎn)，魚雷向路徑調(diào)整點(diǎn)運(yùn)動(dòng)，到達(dá)后再向目標(biāo)點(diǎn)運(yùn)動(dòng)。

由于路徑調(diào)整點(diǎn)是在魚雷的探測范圍內(nèi)，所以認(rèn)為魚雷到路徑調(diào)整點(diǎn)無障礙，此外魚雷向路徑調(diào)整點(diǎn)運(yùn)動(dòng)的同時(shí)，依然由避碰聲吶探測障礙物，若發(fā)現(xiàn)障礙物則繼續(xù)避碰并隨時(shí)調(diào)整路徑調(diào)整點(diǎn)，這樣可以保證魚雷在向路徑調(diào)整點(diǎn)的運(yùn)動(dòng)過程是安全的。

仿真驗(yàn)證結(jié)果如圖11 所示?？梢钥吹?，魚雷采用路徑調(diào)整點(diǎn)對算法進(jìn)行改進(jìn)后，在避障機(jī)動(dòng)與障礙物的距離小了很多，說明改進(jìn)方法起到了一定的效果。

圖9　魚雷規(guī)避陷阱障礙物Fig. 9　Avoid trap obstacles

圖10　魚雷規(guī)避障礙物Fig. 10　Avoid different obstacles

圖11　加入路徑調(diào)整點(diǎn)后魚雷規(guī)避障礙物Fig. 11　Avoid obstacles after joining path adjustment points

4　結(jié)　語

通過仿真驗(yàn)證，證明本文提出的基于避碰聲吶信息的規(guī)避障礙物算法是可以引導(dǎo)魚雷航行中成功規(guī)避障礙物。該算法作為魚雷局部航路規(guī)劃算法與魚雷全局航路規(guī)劃算法相結(jié)合，可以形成完整的遠(yuǎn)程魚雷航路規(guī)劃方法，滿足遠(yuǎn)程魚雷的工程化使用要求，提高遠(yuǎn)程魚雷的自主性能力。

［1］李本昌， 梁濤. 遠(yuǎn)程魚雷的作戰(zhàn)樣式及其技術(shù)需求［J］. 魚雷技術(shù)， 2008， 16（4）： 54-57. LI Ben-chang， LIANG Tao. Operational concept of long-range torpedo and its technical requirements［J］. Torpedo Technology，2008， 16（4）： 54-57.

［2］胡泊， 劉鋒， 陳彥勇. 基于電子海圖的遠(yuǎn)程巡航魚雷路徑規(guī)劃技術(shù)研究［J］. 艦船科學(xué)技術(shù)， 2015， 37（7）： 116-119. HU Bo， LIU Feng， CHEN Yan-yong. Research on route planning technology based on electronic charts for long-distance torpedo［J］. Ship Science and Technology， 2015， 37（7）： 116-119.

［3］秦政， 邊信黔， 嚴(yán)浙平. 多波束前視聲吶在潛器避障中的應(yīng)用研究［J］. 電子器件， 2007， 30（5）： 1909-1913. QIN Zheng， BIAN Xin-qian， YAN Zhe-ping. Research on application of multibeam forward looking sonar in obstacle avoidance for underwater vehicle［J］. Chinese Journal of Electron Devices， 2007， 30（5）： 1909-1913.

［4］劉和祥， 邊信黔， 秦政， 等. 基于前視聲吶信息的AUV避碰規(guī)劃研究［J］. 系統(tǒng)仿真學(xué)報(bào)， 2007， 19（24）： 5672-5674， 5679. LIU He-xiang， BIAN Xin-qian， QIN Zheng， et al. Research on obstacle avoidance planning for AUV based on forward looking Sonar［J］. Journal of System Simulation， 2007， 19（24）：5672-5674， 5679.

［5］曾光奇. 模糊控制理論與工程應(yīng)用［M］. 武漢： 華中科技大學(xué)出版社， 2006. ZENG Guang-qi. Fuzzy control theory and engineering application［M］. Wuhan： Huazhong University of Science and Technology Press， 2006.

［6］徐昌文. 模糊數(shù)學(xué)在船舶工程中的應(yīng)用［M］. 北京： 國防工業(yè)出版社， 1992. XU Chang-wen. Application of fuzzy mathematics in ship engineering［M］. Beijing： National Defence Industry Press， 1992.

［7］姜沛然. 基于模糊理論和強(qiáng)化學(xué)習(xí)的自主式水下機(jī)器人運(yùn)動(dòng)規(guī)劃技術(shù)［D］. 哈爾濱： 哈爾濱工程大學(xué)， 2005. JIANG Pei-ran. Motion planning for autonomous underwater vehicle using the fuzzy theory and reinforcement learning［D］. Harbin： Harbin Engineering University， 2005.

［8］李策. 水下避碰聲納系統(tǒng)軟件設(shè)計(jì)［D］. 哈爾濱： 哈爾濱工程大學(xué)， 2012. LI Ce. Software design of underwater obstacle avoidance sonar system［D］. Harbin： Harbin Engineering University， 2012.

Study on the algorithm of remote torpedo underwater autonomous obstacle avoiding using the collision avoidance sonar

HU Niu， DONG Chun-kai， WANG Zhi-qiang
（Kunming Branch of the 705 Research Institute of CSIC， China Shipbuilding Corporation， Kunming 650118， China）

Accompanied by the appearance of the remote torpedo whose voyage can be thousands of kilometers， we need the torpedo autonomous navigations to the designed area and autonomous avoids the obstacles in the process of the voyage. This paper is under the background of a certain type of remote torpedo， and uses the fuzzy control method， and puts forward an algorithm of remote torpedo underwater autonomous obstacle avoiding using the collision avoidance sonar and the planning of avoiding obstacles， and then through numerical simulations the information of the collision avoidance sonar，and last validation the algorithm in the simulation platform. The result shows that using this algorithm the remote torpedo can detect the obstacles and avoid the obstacles and arrive at the end of the set.

remote torpedo；obstacle avoiding；algorithm simulation

U666.7

A

1672-7619（2016）05-0116-05

10.3404/j.issn.1672-7619.2016.05.025

2015-12-14；

2016-01-06

胡紐（1989-），男，碩士研究生，從事水聲工程技術(shù)研究。