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

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

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

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

1 工作原理

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

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

1.1 距離d 的求取

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

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

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

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

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

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

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

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

1.2 夾角α 的求取

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

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

2 模糊控制器研究

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

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

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

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

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

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

為了判斷障礙物相對于目標(biāo)的位置，設(shè)定障礙物、機(jī)器人、目標(biāo)三者夾角α 的論域?yàn)椋郏?90°，90°］，用模糊語言定義的模糊集為α = {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°］，同樣用模糊語言定義的模糊集為α= {NL，NS，Z，PS，PL}。

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

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

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

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

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

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

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

3 仿真研究

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

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

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

3.2 仿真結(jié)果分析

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

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

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

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

4 結(jié)論

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

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

【2】高慶吉，雷亞莉，胡丹丹，等.基于自適應(yīng)感知復(fù)位算法的移動機(jī)器人定位［J］.電子學(xué)報，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é)報，2006，18(12):3373－3376.

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

【6】李曄，劉建成，徐玉如.帶翼水下機(jī)器人運(yùn)動控制的動力學(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］.兵工自動化，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.