基于模糊算法的AUV避障與姿態(tài)控制

劉 用， 楊曉飛， 夏金銘

(江蘇科技大學(xué) 電子信息學(xué)院， 江蘇 鎮(zhèn)江 212003)

從古至今，人類對海洋的探索就從未停歇，廣闊的海洋擁有豐富的可再生資源和礦產(chǎn)資源.隨著科技的發(fā)展，人們對海洋的探索得到了進一步的發(fā)展，水下機器人(autonomous underwater vehicle，AUV)應(yīng)運而生，水下機器人是用來對海洋開發(fā)、深海探測和水下作業(yè)的一種水下無人平臺.然而，AUV控制器面臨高維度、強耦合、非線性以及復(fù)雜的海水環(huán)境等重要因素導(dǎo)致的數(shù)學(xué)模型參數(shù)不穩(wěn)定，會對AUV的避障與姿態(tài)控制變得困難.從國內(nèi)外的發(fā)展現(xiàn)狀來看，AUV的自主控制仍是一大難題.

路徑規(guī)劃問題作為水下機器人的基本環(huán)節(jié)，很多學(xué)者對其進行了研究.黃朝熙等[1]提出了一種利用單波束聲納的探測波束依次旋轉(zhuǎn)的方法獲取AUV附近的障礙物信息，成功實現(xiàn)了AUV躲避開角的障礙物.張汝波等[2]提出了一種運用蟻群間信息素調(diào)整準(zhǔn)則的方法，成功應(yīng)用到了AUV航路多重避障的模型中.龐師坤等[3]針對AUV在海底可能遇到的靜動態(tài)障礙物問題，設(shè)計了基于零空間行為法的避障策略，在復(fù)雜障礙物環(huán)境中達到了預(yù)期的避障效果.姚鵬等[4]提出了一種高效的修正導(dǎo)航向量場的方法，成功應(yīng)用在復(fù)雜海洋環(huán)境下的AUV避障任務(wù).孫兵等[5]提出了一種基于粒子群優(yōu)化的模糊路徑規(guī)劃算法，有效地對動靜態(tài)障礙物進行了避障路徑規(guī)劃.張楠楠等[6]針對自主式水下機器人的三維避障最優(yōu)路徑問題，提出了一種適用于全局路徑規(guī)劃的改進蟻群算法，在大尺度海底環(huán)境下仿真驗證了該算法的有效性.王超等[7]將速度合成的方法與人工勢場相結(jié)合，克服了海流對AUV水下航行的影響.時常鳴等[8]研究了一種基于混合整數(shù)線性規(guī)劃的AUV與水下移動平臺對接的實時路徑規(guī)劃方法，在仿真試驗下，驗證了方法的有效性.徐言民等[9]針對水下復(fù)雜環(huán)境、障礙物不規(guī)則、規(guī)劃困難等問題，提出了適用于三維空間路徑規(guī)劃的空間分層路徑規(guī)劃方法并驗證了方法的有效性.付振秋等[10]提出一種粒子群融合算法，充分利用粒子群算法較強的全局搜索能力，通過仿真驗證了該方法的有效性.朱大齊等[11]提出了一種基于改進模糊的全覆蓋路徑規(guī)劃算法，使AUV能夠自動避開各類靜態(tài)和動態(tài)障礙物，自動逃離路徑的死鎖區(qū).LIANG X.等[12]研究了欠驅(qū)動自主水下航行器在模型不確定性和時變干擾等復(fù)雜未知條件下的三維軌跡跟蹤問題，在時變參考軌跡的基礎(chǔ)上，進行了運動學(xué)上的俯仰角和偏航角的設(shè)計,通過仿真驗證了所提方案的有效性和優(yōu)越性.

為了實現(xiàn)對AUV的避障與姿態(tài)控制，筆者結(jié)合模糊算法的優(yōu)點，分別設(shè)計AUV在水平面和豎直面的模糊控制器，實現(xiàn)AUV的避障與姿態(tài)控制.

1 AUV系統(tǒng)結(jié)構(gòu)

AUV三維模型結(jié)構(gòu)如圖1所示，在AUV機體上，配置了5個聲納(圖1中1-5)，分別用來探測AUV在水下航行時，上方、右方、前方、左方和下方的障礙物；配置在AUV機體上的推進器(圖1中a-f)通過模糊算法，輸出向左、向右和下潛加速度.

圖1 AUV三維模型結(jié)構(gòu)圖

將AUV所在的三維平面分為水平二維平面和豎直二維平面，在水平面上，左右2個聲納與正前方的聲納的夾角均為60°，3個聲納分別用來探測AUV左、前和右方的障礙物；向左推進器和向右推進器用來控制AUV的水平方向上的前進速度和搖艏角速度；在豎直面上，上下2個聲納與正前方的聲納的夾角均為90°，3個聲納分別用來探測AUV上、前和下方的障礙物；推進器用來控制AUV的豎直方向下潛加速度.

2 基于模糊算法的三維避障決策設(shè)計

2.1 水平面避障決策的構(gòu)建

采用if-then構(gòu)建的水平面模糊規(guī)則如表1所示.

表1 水平面模糊規(guī)則

使用重心法(center of gravity，COG)去模糊化，可得

(1)

對于模糊控制器輸出的AUV的左、右加速度進行積分，可得

(2)

(3)

式中：L為向左和向右推進器之間的距離.

(4)

(5)

式中：kr為搖艏角速度的阻尼系數(shù).

2.2 豎直面避障決策的構(gòu)建

綜上所述，采用if-then構(gòu)建模糊規(guī)則，得到的豎直面模糊規(guī)則如表2所示.

表2 豎直面模糊規(guī)則

對于w′，系統(tǒng)同樣不穩(wěn)定，通過引入阻尼系數(shù)，可將下潛速度期望指令設(shè)計為

(6)

式中：kw為下潛速度的阻尼系數(shù).

3 AUV姿態(tài)控制

由拉格朗日方程可得，AUV的運動數(shù)學(xué)模型為

(7)

式中：η=(x,y,z,φ,θ,ψ)T,x、y、z、φ、θ、ψ分別為慣性坐標(biāo)系中的前進位移、橫移位移、豎直位移、橫傾角度、縱傾角度和搖艏角度；J(η)∈R6×6為轉(zhuǎn)換矩陣；τ為推進器產(chǎn)生的力和力矩，τ∈R6；v=(u,v,w,p,q,ω)T，u、v、w、p、q、ω分別為機體坐標(biāo)系中的前進速度、橫移速度、下潛速度、橫傾角速度、縱傾角速度和搖艏角速度；MRB為剛體慣性矩陣；MA為附加質(zhì)量矩陣；CRB為科里奧利力矩陣；CA為附加科里奧利力和向心力矩陣；D(v)為阻尼矩陣；G(η)為恢復(fù)力矩陣[13].

令M=MRB+MA，C(v)=CRB+CA，可得動力學(xué)方程為

(8)

AUV在復(fù)雜海水環(huán)境中工作時遇到的干擾以及不確定性等通常體現(xiàn)在動力學(xué)方程不確定的參數(shù)上，因此，考慮到動力學(xué)參數(shù)的不確定性，得到新的動力學(xué)方程為

(9)

式中：ΔC和ΔD為動力學(xué)不確定參數(shù).

將AUV的期望速度指令定義為

vd=(ud,0,wd,0,0,ωd)T.

(10)

則速度跟蹤誤差為

ξ=v-vd.

(11)

對速度跟蹤誤差求導(dǎo),并結(jié)合式(9),可得含不確定參數(shù)的非線性系統(tǒng)為

M-l[τ-(C(v)+ΔC)v-(D(v)+ΔD)v-

(12)

令

(13)

式中： Δf(v)為未知不確定項.

由式(13)可得

(14)

式中：u=M-1τ.

選擇模糊邏輯[14]逼近未知不確定項Δf(v)=(Δf1(v),Δf2(v),…,Δfk(v)).模糊推理機使用模糊if-then規(guī)則，執(zhí)行從輸入語言向量v到輸出變量w∈R的映射.第l條模糊規(guī)則可表示為

具有單變量模糊器、乘積推理機和中心平均解模糊器的模糊系統(tǒng)的輸出可以寫成：

(15)

因此，Δf(v)的逼近可以表示為

(16)

通過模糊算法對不確定項進行逼近后，可據(jù)此選擇合適的李雅普諾夫函數(shù)：

(17)

對式(17)求時間導(dǎo)數(shù)，可得

(18)

(19)

式中：k=diag(k1,k2,k3,k4,k5,k6)，k1、k2、k3、k4、k5、k6為正常數(shù).

將式(19)代入式(18)得

(20)

因此，可以得到自適應(yīng)律如下：

(21)

至此，對AUV速度的控制律和自適應(yīng)律設(shè)計已完成，分別為式(19)、(21).

4 仿真與分析

設(shè)置目標(biāo)在(200,200,200)m，障礙物中心分別在(0,10,110)和(140,140,190)m，半徑均為25 m時，經(jīng)過640 s仿真后，AUV的軌跡曲線如圖2所示,AUV在遇到第1個障礙物時，從下方繞過了障礙物，之后繼續(xù)向目標(biāo)前進；當(dāng)遇到第2個障礙物時，AUV從右方繞過障礙物，最終到達目標(biāo)位置.

圖2 AUV的軌跡曲線

AUV速度指令如圖3所示.

圖3 AUV速度指令

從圖3可以看出：AUV一直保持較穩(wěn)定的前進速度；遇到第1個障礙物時，通過調(diào)整下潛速度，成功繞開了第1個障礙物，之后重新調(diào)整下潛速度；當(dāng)遇到第2個障礙物時，通過調(diào)整搖艏角速度繞過第2個障礙物，最終到達目標(biāo)位置.

5 結(jié) 論

針對AUV的避障與姿態(tài)控制，設(shè)計了模糊控制器，對相關(guān)公式的推導(dǎo)得到了控制律和自適應(yīng)律，通過仿真驗證了設(shè)計算法的有效性.