基于模糊算法的水下機(jī)器人路徑規(guī)劃研究

王妹婷，陸柳延，齊永鋒，呂學(xué)智，蔣偉，李生權(quán)

(揚(yáng)州大學(xué)能源與動(dòng)力工程學(xué)院，江蘇揚(yáng)州225127)

水下機(jī)器人是一種工作于水下的極限作業(yè)機(jī)器人，由于水下工作環(huán)境具有復(fù)雜性和不確定性，提高自主式水下機(jī)器人智能化的關(guān)鍵環(huán)節(jié)——路徑規(guī)劃研究顯得尤為重要。常用的局部路徑規(guī)劃算法有勢(shì)場(chǎng)法、A*算法、柵格法及模糊算法［1－3］?？紤]到水下環(huán)境的復(fù)雜性，測(cè)距聲吶所得的環(huán)境信息都是近似、不完善的，并伴有一定的噪聲干擾［4］，而模糊控制器的最大優(yōu)點(diǎn)就是能夠采用輸入的模糊信息［5］進(jìn)行控制。此外，水下機(jī)器人的動(dòng)力學(xué)模型極其復(fù)雜，是一個(gè)典型的非線性系統(tǒng)［6］。針對(duì)這類特殊控制對(duì)象，提出了一種結(jié)合視線導(dǎo)航原理［7］和模糊算法的未知環(huán)境下水下機(jī)器人實(shí)時(shí)路徑規(guī)劃策略，應(yīng)用該策略，可于障礙物環(huán)境中，自動(dòng)尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑。

依據(jù)障礙物的不同形狀大小，采用不同方式求取機(jī)器人與障礙物之間距離以及目標(biāo)點(diǎn)、機(jī)器人與障礙物三者之間的夾角［8］。此外，在MATLAB 仿真中，根據(jù)障礙物特征，對(duì)模糊控制規(guī)則表進(jìn)行修改:障礙物較小時(shí)，以路徑最短最平滑為主;障礙物較大時(shí)，以嚴(yán)格快速避開(kāi)障礙物為主。

1 工作原理

假設(shè)水下機(jī)器人勻速運(yùn)動(dòng)，首先，通過(guò)短基線系統(tǒng)，實(shí)時(shí)得到機(jī)器人與目標(biāo)預(yù)定點(diǎn)的相對(duì)位置信息;利用測(cè)距聲吶對(duì)其運(yùn)動(dòng)前方的±90°范圍內(nèi)進(jìn)行實(shí)時(shí)探測(cè)，得到環(huán)境信息［9］。實(shí)時(shí)計(jì)算機(jī)器人與障礙物最近點(diǎn)的距離d，以及障礙物最近點(diǎn)、水下機(jī)器人、目標(biāo)點(diǎn)三者間的夾角α，如圖1所示。通過(guò)模糊控制器進(jìn)行實(shí)時(shí)模糊推理，得到避障狀態(tài)下水下機(jī)器人所需轉(zhuǎn)動(dòng)的偏轉(zhuǎn)角，從而使得水下機(jī)器人有效地避開(kāi)障礙物，安全到達(dá)設(shè)定目標(biāo)點(diǎn)。

圖1 視線導(dǎo)航原理圖

1.1 距離d 的求取

與工作環(huán)境尺寸相比，障礙物尺寸較小的情況下，障礙物大小、形狀均可忽略不計(jì)，視為一個(gè)點(diǎn)，距離d 為機(jī)器人與該障礙物兩點(diǎn)之間的距離，公式為:

其中，(xr，yr)為機(jī)器人當(dāng)前坐標(biāo)，(xo，yo)為障礙物坐標(biāo)。

反之，當(dāng)障礙物尺寸較大且形狀不規(guī)則時(shí)，根據(jù)障礙物的邊緣坐標(biāo)信息，近似變換為矩形障礙物，如圖2所示，得到矩形的4 個(gè)坐標(biāo)點(diǎn)，以方便求取水下機(jī)器人與障礙物的最小距離點(diǎn)。

圖2 不規(guī)則圖形矩形化

矩形障礙物上距離水下機(jī)器人最近點(diǎn)與兩者相對(duì)位置有關(guān)，為求取水下機(jī)器人與矩形障礙物最近點(diǎn)的距離，針對(duì)機(jī)器人位于矩形障礙物的8 個(gè)區(qū)域分別討論，如圖3所示。

矩形障礙物與水下機(jī)器人之間的最近距離點(diǎn)可表示為:水下機(jī)器人位于1 區(qū)域，即xr≤xmin，yr≤ymin時(shí)，最近距離點(diǎn)d_node 坐標(biāo)為(xmin，ymin);水下機(jī)器人位于2 區(qū)域，即xmin＜xr≤xmax，yr≤ymin時(shí)，d_node坐標(biāo)為(xr，ymin);水下機(jī)器人位于3 區(qū)域，即xr＞xmax，yr≤ymin時(shí)，d_node 坐標(biāo)為(xmax，ymin);水下機(jī)器人位于4 區(qū)域時(shí)，即xr≥xmax，ymin＜yr≤ymax時(shí)，d_node 坐標(biāo)為(xmax，yr);水下機(jī)器人位于5 區(qū)域，即xr≥xmax，yr＞ymax時(shí)，d_node 坐標(biāo)為(xmax，ymax);水下機(jī)器人位于6 區(qū)域，即xmin≤xr＜xmax，yr≥ymax時(shí)，d_node 坐標(biāo)為(xr，ymax);水下機(jī)器人位于7 區(qū)域，即xr＜xmin，yr≥ymax時(shí)，d_node 坐標(biāo)為(xmin，ymax);水下機(jī)器人位于8 區(qū)域，即xr≤xmin，ymin＜yr＜ymax時(shí)，d_node 坐標(biāo)為(xmin，yr)。

水下機(jī)器人與矩形障礙物間的距離轉(zhuǎn)化為水下機(jī)器人與實(shí)時(shí)得到的矩形障礙物上最近距離點(diǎn)兩點(diǎn)之間的距離:

圖3 水下機(jī)器人與矩形障礙物的相對(duì)位置示意圖

1.2 夾角α 的求取

通過(guò)機(jī)器人當(dāng)前位置、障礙物位置、目標(biāo)位置三點(diǎn)坐標(biāo)，能夠得到夾角α。因機(jī)器人的避障環(huán)境為其運(yùn)動(dòng)前方的±90°范圍內(nèi)，將連接機(jī)器人與目標(biāo)點(diǎn)的導(dǎo)航線作為分界線，當(dāng)障礙物在導(dǎo)航線左側(cè)時(shí)設(shè)α為正;反之，α 為負(fù)。故

式中:a 為機(jī)器人與實(shí)時(shí)障礙物最近點(diǎn)的距離;b 為機(jī)器人與目標(biāo)點(diǎn)的距離;c 為實(shí)時(shí)障礙物最近點(diǎn)與目標(biāo)點(diǎn)的距離。

2 模糊控制器研究

基于模糊算法的水下機(jī)器人避障控制系統(tǒng)如圖4所示，其中模糊控制器由模糊量化、模糊規(guī)則、模糊推理和反模糊化接口4 部分組成［10］。由短基線系統(tǒng)得到目標(biāo)的位置信息，由多個(gè)測(cè)距聲吶實(shí)時(shí)測(cè)得的與障礙物的距離信息，經(jīng)過(guò)信息融合計(jì)算后所得到的距離d，以及障礙物與機(jī)器人、目標(biāo)的夾角α，同時(shí)作為模糊控制器的輸入。輸入量經(jīng)過(guò)模糊控制器進(jìn)行模糊推理，最終得到水下機(jī)器人的偏轉(zhuǎn)角φ，然后水下機(jī)器人以新的方向進(jìn)行運(yùn)動(dòng)。

圖4 水下機(jī)器人避障控制系統(tǒng)示意圖

此處采用最簡(jiǎn)單的信息融合技術(shù)，即每個(gè)方向的距離信息中，取其中較小的一個(gè)數(shù)據(jù)作為該方向的輸入，然后在每個(gè)方向上提取的距離信息中取最小的作為這一時(shí)刻的距離信息。

2.1 模糊集、模糊論域及隸屬函數(shù)確立

模糊控制輸入為水下機(jī)器人與障礙物的最小距離d、兩視線導(dǎo)航角的夾角α，輸出為水下機(jī)器人在t時(shí)刻所需的偏轉(zhuǎn)角φ。

由于測(cè)距聲吶的測(cè)量距離達(dá)30 m，系統(tǒng)設(shè)定的障礙物影響距離d 為10 m，即d 的論域?yàn)椋?，100］，用模糊語(yǔ)言定義的模糊集為 D ={VS，S，M，B，VB }。其中，VS 表明距離極小，VB 表明距離極大。

為了判斷障礙物相對(duì)于目標(biāo)的位置，設(shè)定障礙物、機(jī)器人、目標(biāo)三者夾角α 的論域?yàn)椋郏?90°，90°］，用模糊語(yǔ)言定義的模糊集為α = {NL，NS，Z，PS，PL}。其中，Z 表明障礙物在導(dǎo)航線上，N表示負(fù)，即設(shè)定的障礙物在導(dǎo)航線右側(cè)，反之，P 表示障礙物在導(dǎo)航線左側(cè);L 表示角度大，S 則表示角度小。輸出偏轉(zhuǎn)角φ 設(shè)定為向左轉(zhuǎn)為負(fù)，向右轉(zhuǎn)為正，偏轉(zhuǎn)角φ 的論域?yàn)椋郏?5°，45°］，同樣用模糊語(yǔ)言定義的模糊集為α= {NL，NS，Z，PS，PL}。

根據(jù)輸入輸出的模糊論域設(shè)定合適的模糊語(yǔ)言變量，在此均采用三角形隸屬度函數(shù)來(lái)反應(yīng)模糊集合的模糊性。

2.2 模糊控制規(guī)則庫(kù)的建立與模糊推理

水下機(jī)器人在恒速運(yùn)動(dòng)過(guò)程中的避障原則為:在障礙物的影響距離范圍內(nèi)，當(dāng)障礙物在航線左側(cè)時(shí)，機(jī)器人向右側(cè)繞行，當(dāng)障礙物在航線上或者右側(cè)時(shí)，機(jī)器人向左側(cè)繞行;除此之外，機(jī)器人沿著導(dǎo)航線，向目標(biāo)方向前進(jìn)。

依據(jù)障礙物的不同尺寸大小，提出以下兩種控制器設(shè)計(jì)要求:當(dāng)障礙物大小在整個(gè)工作環(huán)境下可忽略不計(jì)時(shí)，以路徑能夠?qū)崿F(xiàn)平滑最短為主，防止水下機(jī)器人頻繁過(guò)大的角度偏轉(zhuǎn)導(dǎo)致的振蕩;當(dāng)障礙物較大的情況下，由于水下機(jī)器人運(yùn)動(dòng)速度比較快，所以在測(cè)出障礙物的同時(shí)就應(yīng)開(kāi)始偏轉(zhuǎn)，以免機(jī)器人無(wú)法安全繞行。因此，設(shè)定兩套控制規(guī)則，分別如表1、表2所示，并在仿真過(guò)程中，根據(jù)障礙物的大小，調(diào)節(jié)控制器中輸出偏轉(zhuǎn)角的大小及障礙物影響距離，可更好地完成機(jī)器人避障要求。

表1 障礙物較小時(shí)偏轉(zhuǎn)角φ 的模糊規(guī)則表

表2 障礙物較大時(shí)偏轉(zhuǎn)角φ 的模糊規(guī)則表

模糊推理是是模糊控制器的核心，在此采用Mamdani 推理，用中心面積法去模糊化［11］，最終得到水下機(jī)器人所需的偏轉(zhuǎn)角。

3 仿真研究

3.1 仿真實(shí)驗(yàn)流程

為了驗(yàn)證所提出的算法能否滿足水下機(jī)器人的避障要求，在MATLAB 中進(jìn)行了仿真實(shí)驗(yàn)。仿真針對(duì)的水下機(jī)器人以恒速運(yùn)動(dòng)，運(yùn)動(dòng)步長(zhǎng)l =1 mm，分別在障礙物近似為點(diǎn)的情況及障礙物較大且不規(guī)則的情況下進(jìn)行仿真，仿真步驟如圖5所示。在仿真過(guò)程中，設(shè)計(jì)的仿真實(shí)驗(yàn)平臺(tái)不僅能夠交互式地任意設(shè)置機(jī)器人起點(diǎn)和終點(diǎn)位置，還能交互式地任意設(shè)置障礙物的數(shù)量、大小、形狀和位置，可以更好地檢驗(yàn)算法的可行性和通用性。

圖5 基于模糊算法的機(jī)器人避障流程

3.2 仿真結(jié)果分析

通過(guò)交互式界面建立機(jī)器人起點(diǎn)坐標(biāo)、目標(biāo)點(diǎn)坐標(biāo)、路障信息后，當(dāng)該障礙物較小時(shí)，采用模糊控制器1，對(duì)應(yīng)的模糊控制規(guī)則如表1所示。當(dāng)障礙物對(duì)水下機(jī)器人的最小影響距離為d=100 mm，輸出偏轉(zhuǎn)角φ= ［－45°，45°］時(shí)，得到圖6 中的路徑1。由于在整個(gè)水下機(jī)器人避障設(shè)計(jì)過(guò)程中，提出在實(shí)現(xiàn)避障功能的同時(shí)，能夠盡可能的實(shí)現(xiàn)路徑最短且平滑。所以，在仿真過(guò)程中，設(shè)置障礙物對(duì)水下機(jī)器人的最小影響距離為d =50 mm，輸出偏轉(zhuǎn)角φ = ［－20°，20°］時(shí)，得出圖6 中的路徑2。經(jīng)過(guò)比較，得知路徑2 明顯優(yōu)于路徑1，所以根據(jù)障礙物的大小，改變模糊控制器的模糊論域，能夠取得更優(yōu)路徑。

圖6 多點(diǎn)障礙物環(huán)境中機(jī)器人路徑規(guī)劃仿真圖

當(dāng)障礙物較大時(shí)，采用模糊控制器2，對(duì)應(yīng)模糊控制表如表2所示，障礙物對(duì)水下機(jī)器人的最小影響距離為d=100 mm，輸入偏轉(zhuǎn)角φ = ［－45°，45°］。首先通過(guò)設(shè)置多邊形，如圖中* 號(hào)節(jié)點(diǎn)連線顯示，得到近似轉(zhuǎn)換的矩形，再通過(guò)控制器，得到水下機(jī)器人的路徑。通過(guò)設(shè)置不同路障的數(shù)量、位置、大小，機(jī)器人的初始位置、目標(biāo)點(diǎn)位置等信息，檢驗(yàn)了該算法的可行性，仿真結(jié)果如圖7所示。

圖7 多矩形障礙物環(huán)境中機(jī)器人路徑規(guī)劃仿真圖

4 結(jié)論

由于水下機(jī)器人運(yùn)動(dòng)環(huán)境的復(fù)雜性、不確定性，機(jī)器人難以完全依照預(yù)先規(guī)劃的路徑運(yùn)動(dòng)。采用測(cè)距聲吶、短基線系統(tǒng)等傳感器，實(shí)時(shí)探測(cè)水下未知環(huán)境?；谝暰€導(dǎo)航原理和模糊控制器，實(shí)現(xiàn)了水下機(jī)器人的避障路徑規(guī)劃。此外，根據(jù)障礙物的不同尺寸，給出了不同的模糊控制器，可獲得最優(yōu)化路徑。障礙物不規(guī)則時(shí)，提取邊緣信息后近似轉(zhuǎn)換成矩形障礙物，并討論了機(jī)器人與矩形障礙物處于不同相對(duì)位置時(shí)相對(duì)距離的求取方式。仿真結(jié)果表明:該算法對(duì)水下機(jī)器人在未知環(huán)境中對(duì)障礙物的數(shù)量、形狀、大小、位置都沒(méi)有限制，均可形成一條安全穩(wěn)定的可靠路徑，驗(yàn)證了算法的有效性。

【1】郝宗波，洪炳熔.未知環(huán)境下基于傳感器的移動(dòng)機(jī)器人路徑規(guī)劃［J］.電子學(xué)報(bào)，2006，34(5):953－956.

【2】高慶吉，雷亞莉，胡丹丹，等.基于自適應(yīng)感知復(fù)位算法的移動(dòng)機(jī)器人定位［J］.電子學(xué)報(bào)，2007，35(11):2166－2171.

【3】HUQ R，MANN G K I，GOSINE R G.Mobile Robot Uavigation Using Motor Schema and Fuzzy Context Dependent Behavior Modulation ［J］.Applied Soft Computing，2008，8(1):422－436.

【4】崔榮鑫，徐德民，嚴(yán)衛(wèi)生.一種自主水下航行器路徑規(guī)劃算法［J］.系統(tǒng)仿真學(xué)報(bào)，2006，18(12):3373－3376.

【5】陳衛(wèi)東，朱奇光.基于模糊算法的移動(dòng)機(jī)器人路徑規(guī)劃［J］.電子學(xué)報(bào)，2011(4):971－974.

【6】李曄，劉建成，徐玉如.帶翼水下機(jī)器人運(yùn)動(dòng)控制的動(dòng)力學(xué)建模［J］.機(jī)器人，2005，27(2):128－131.

【7】WU X P，F(xiàn)ANG Z P，ZHU J M.Line of Sight Guidance With Intelligent Obstacle Avoidance for Autonomous Underwater Vehicles［C］.OCEANS 2006，Boston，MA，2006:1－6.

【8】李慶春，高軍偉，謝廣明.基于模糊控制的仿生機(jī)器魚避障算法［J］.兵工自動(dòng)化，2011，30(12):65－69.

【9】PETILLOT Y，TEMA R I，LAME D M.Underwater Vehicle Obstacle Avoidance and Path Planning Using a Multi-Beam forward Looking Sonar［J］.IEEE Journal of Oceanic Engineering，2001，26(2):240－251.

【10】WANG L X.Adaptive Fuzzy System and Control:Design and Stability Analysis［M］.Englewood Cliffs:PTR Prentice Hall，1994.

【11】謝超平，孔峰，陶金.基于模糊控制的仿生機(jī)器魚轉(zhuǎn)向控制研究［J］.機(jī)器人技術(shù)與應(yīng)用，2009(4):26－27.