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

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

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

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

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

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

0　引　言

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

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

1.1系統(tǒng)組成

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

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

圖1　系統(tǒng)組成結構圖Fig. 1　System composition structure

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

1.2障礙物模擬

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

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

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

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

1.3避碰聲吶信息模擬

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

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

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

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

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

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

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

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

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

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

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

2.3模糊控制規(guī)則

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

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

1）危險區(qū)域。在危險區(qū)域內探測到障礙物時，留給魚雷機動規(guī)避的時間、空間有限，所以在這一區(qū)域內，一旦探測到障礙物，魚雷先停車上報避障故障，再做處理。

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

3）距離適中。這種情況下魚雷有足夠的時間和空間進行避障機動，避障控制方法有 2 種，A 方法是從外向內，B 方法是從內向外。

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

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

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

用 B 方法控制時，控制器從內向外判斷，只要保證一定范圍內沒有障礙物即認為可以安全航行，如圖5所示，假設距離 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 四個波束是否有障礙物來確定魚雷航向，共有 16 種情況，其模糊控制規(guī)則見表2（表中數(shù)字 0 代表沒有探測到障礙物，數(shù)字 1代表探測到障礙物）。

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

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

3　仿真驗證

3.1仿真平臺介紹

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

圖6　魚雷避碰仿真平臺界面Fig. 6　Collision avoidance simulation platform

仿真平臺具備魚雷運動模擬、障礙物設定、避碰聲吶信息模擬、避碰規(guī)則驗證等功能。

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

3.2通過功能驗證

設定魚雷從（0，0）點開始向（1 400，700）點運動，并設置一個圓形障礙物，圓心（750，450）半徑200。仿真開始分別用控制規(guī)則 A 和控制規(guī)則 B 控制魚雷避障機動，觀察魚雷是否能規(guī)避障礙物航行，到達目標點。

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

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

3.3穿越功能驗證

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

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

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

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

3.4多種障礙物通過功能驗證

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

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

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

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

3.5控制規(guī)則改進

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

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

仿真驗證結果如圖11 所示。可以看到，魚雷采用路徑調整點對算法進行改進后，在避障機動與障礙物的距離小了很多，說明改進方法起到了一定的效果。

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

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

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

4　結　語

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

［1］李本昌， 梁濤. 遠程魚雷的作戰(zhàn)樣式及其技術需求［J］. 魚雷技術， 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］胡泊， 劉鋒， 陳彥勇. 基于電子海圖的遠程巡航魚雷路徑規(guī)劃技術研究［J］. 艦船科學技術， 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］秦政， 邊信黔， 嚴浙平. 多波束前視聲吶在潛器避障中的應用研究［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)仿真學報， 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］曾光奇. 模糊控制理論與工程應用［M］. 武漢： 華中科技大學出版社， 2006. ZENG Guang-qi. Fuzzy control theory and engineering application［M］. Wuhan： Huazhong University of Science and Technology Press， 2006.

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

［7］姜沛然. 基于模糊理論和強化學習的自主式水下機器人運動規(guī)劃技術［D］. 哈爾濱： 哈爾濱工程大學， 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)軟件設計［D］. 哈爾濱： 哈爾濱工程大學， 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-），男，碩士研究生，從事水聲工程技術研究。