劉錦豪，宋曉茹

（西安工業(yè)大學電子信息工程學院，西安 720021）

0 引言

隨著各國對海洋探索程度的提高和海洋防御重視程度的加深，高機動、高隱蔽、高效率、低噪音的水下仿生機器人越來越受到各國學者的關(guān)注。仿生機器魚是水下仿生機器人的代表之一，由于其所處環(huán)境的不確定：強干擾、非線性以及自身復(fù)雜的運動機理，使得其運動控制尤為困難。Ren等［1］以數(shù)據(jù)驅(qū)動為手段研究機器魚的運動控制方式，避免了復(fù)雜的動力學研究問題和機器魚的復(fù)雜數(shù)學模型的構(gòu)建問題，但并未考慮運動過程的非線性和擾動影響。Xie等［2］提出一種基于中樞模式發(fā)生器（CPG）的機器魚控制方法，只使用一個電機進行波幅震蕩，可以很容易地實現(xiàn)CPG控制，使得機器魚的轉(zhuǎn)動更加自然有效，但該方法較為簡單，沒有考慮到魚體運動時胸鰭與尾鰭之間的作用。Suebsaiprom等［3］提出基于滑膜控制的方法來滿足運動模型不穩(wěn)定、非線性的狀況，雖然模擬實驗取得了較好的控制效果，但滑?？刂埔桩a(chǎn)生抖震問題。Moe等［4］給出了一種基于視線法的航線路徑追蹤控制器，并充分考慮海流環(huán)境的影響，通過設(shè)計視線制導(dǎo)律和自適應(yīng)反饋線性控制器進行無人船自主航線路徑跟蹤控制實驗，取得了不錯的跟蹤效果，但仍無法克服抖震問題。

綜上所述，為使仿生機器魚的路徑跟蹤控制達到良好效果，本文給出了基于自抗擾控制器的機器魚路徑追蹤控制方法。首先建立多關(guān)節(jié)機器魚數(shù)學模型；其次通過機器魚路徑跟蹤誤差模型推導(dǎo)跟蹤誤差方程和期望角函數(shù)，將機器魚的路徑誤差收斂問題轉(zhuǎn)化為航速差和航向差的收斂問題；最后設(shè)計機器魚路徑跟蹤導(dǎo)引律并基于ADRC控制器實現(xiàn)參數(shù)整定。仿真結(jié)果證明了基于ADRC的機器魚路徑跟蹤控制器有良好的控制效果。

1 多關(guān)節(jié)機器魚數(shù)學模型

機器魚以身體-尾鰭模式游動且由于水環(huán)境具有強擾動的特點，因此對其進行受力分析極具困難。本文首先分析機器魚在魚體坐標系下的平移和旋轉(zhuǎn)。根據(jù)圖1所示的基于直游和轉(zhuǎn)彎的兩種基本數(shù)學模型，研究機器魚的二維運動。

圖1 多關(guān)節(jié)機器魚的游動與轉(zhuǎn)動模型

針對魚尾的游動和轉(zhuǎn)彎行為，采用拉格朗日方程建立了魚尾的動力學模型。因此，多關(guān)節(jié)機器魚的動態(tài)模型可以寫成如下二階矩陣形式［5-6］：

M(γ)是具有水動力學效應(yīng)的慣性矩陣，C(γ,?)是科里奧利矩陣，B是阻尼系數(shù)矩陣，K是彈簧系數(shù)矩陣，F(xiàn)是包含水動力的力矢量，γ=[γ0γ1γ2γ3]T是機器魚的鏈接角度。

圖2 機器魚魚尾等效模型

2 機器魚路徑跟蹤誤差模型

機器魚運行于復(fù)雜多變的水下環(huán)境，為了得到易于觀測的控制效果，本文只研究其水平面的二維運動。如圖3所示，機器魚的路徑跟蹤示意圖可以使用以下三個坐標系來表示，地球坐標系U，魚體坐標系B以及Serret-Frenet坐標系S F。其中，在SF坐標系上的點P為參考路線上任意一個點，表示虛擬機器人。假設(shè)參照點P（虛擬機器人）以速度VP按參考路線運動，P軸表示運動的切線方向，Pη軸表示運動的法線方向。Q為機器魚的重心，其速度向量在坐標系B上可表示為[u,v,r]T，(u,v,r)分別代表機器魚的前向速度、橫向速度以及轉(zhuǎn)向速度。ψ為Bu與zx的夾角，β=arctan(v/u)是機器魚的側(cè)滑角，ψw=ψ+β。τe,ye分別表示P點與Q點間距離在Pη軸和Pξ軸上的投影長度，且偏航角ψe為合速度Vt與Pη軸的夾角。因此，機器魚運動學方程可表示為

圖3 機器魚路徑跟蹤示意圖

將魚體坐標系{B}繞z軸旋轉(zhuǎn)角度β得到新的坐標系{W}，則在{W}坐標系下，機器魚的水平運動學方程可以表示為式（3）：

期望路徑由曲線Γ(s)描述，曲線上參考點P由Γ(s)唯一確定，為了完成路徑跟蹤的目標，需要實現(xiàn)機器魚質(zhì)心Q與參考點P的距離為零(τe,ye=0)，合速度Vt與Pη軸的夾角為零(ψe=0)，將上述條件統(tǒng)一在SF坐標系下有：

上式中CC(s)?是期望路徑的曲率，s是期望路徑的參數(shù)變量。

3 基于ADRC的路徑追蹤控制器

根據(jù)路徑跟蹤控制器的任務(wù)和控制目標的設(shè)計要求，該控制器必須使機器魚到期望曲線的相對距離為零，機器魚的速度矢量對準期望路徑的切向方向，機器魚的速度逐漸趨于分配的指定速度。

為了控制機器魚的前向速度u在整個路徑跟蹤過程中都以穩(wěn)定的期望速度Ud運動，采用Vt合速度對其進行調(diào)控。且為了保持整個系統(tǒng)的穩(wěn)定性，本文通過設(shè)計虛擬目標的速度，使機器魚動態(tài)特性曲線與期望路徑的特性曲線相擬合。同時，為防止因位置誤差而產(chǎn)生不連續(xù)點，對虛擬目標的前向位置依據(jù)切向偏差程度做出了適當?shù)恼{(diào)整。其設(shè)計公式見式（5）：

其中，kτ為增益函數(shù)?；谶\動學的理論設(shè)計滿足光滑路徑控制條件的非奇異路徑參考式（6）：

法向誤差需要利用ψ角進行控制。本文將引用期望角的計算方法調(diào)節(jié)機器魚在跟蹤路徑過程中的瞬態(tài)行為，趨近角實際上是方位誤差的期望角度。本文所選取期望角的表達式為

且滿足

由趨近角函數(shù)可知，當期望路徑上的參考目標點與機器魚的實際位置誤差存在較大的ye時，會產(chǎn)生一個較大的趨近角ψe，假若機器魚在期望路徑前向的右邊，機器魚就會向左偏轉(zhuǎn)；相反，趨近角將導(dǎo)引機器魚向右運動并趨向期望路徑。從誤差方程可以得到，如果姿態(tài)跟蹤的誤差角ψe收斂到ψd，并且τe收斂到零，即ye收斂到零，趨近角ψd也自然收斂到零。

綜上所述，本文選取如下導(dǎo)引律:

本文主要通過ADRC控制技術(shù)，使得機器魚可以更好地跟蹤上式的控制率?；谏鲜街性O(shè)計的導(dǎo)航算法，依此構(gòu)建了二階自抗擾路徑的追蹤控制器，其控制結(jié)構(gòu)框圖如圖4所示。

圖4 基于ADRC路徑跟蹤控制系統(tǒng)

4 仿真分析與實驗驗證

為了驗證本文所設(shè)計控制器的有效性和魯棒性，對機器魚進行了路徑跟蹤控制實驗，并分別進行了基于自抗擾控制器和常規(guī)PID路徑跟蹤控制器的曲線路徑跟蹤控制實驗。利用MATLAB/SIMULINK建立機器魚的路徑跟蹤控制器。

設(shè)定仿真參考直線路徑，其參數(shù)方程為

上式中，θ=1/4 rad為x軸與直線路徑的夾角。參考路徑的起始坐標為（0,0），期望速度Ud=1.5 m/s，機器魚起始坐標為（0,0），θ=0 rad，初始速度為0 m/s，其中加入的外界擾動為如下式：

仿真步長設(shè)置為自動，其中部分參數(shù)設(shè)置為Ke=1.2,Kr=1.2,K1=0.8，其仿真結(jié)果如下圖所示。

圖5—圖7給出了機器魚的直線路徑跟蹤運動曲線和速度變化曲線。直線路徑跟蹤圖表明本研究所設(shè)計的控制器能穩(wěn)定跟蹤預(yù)定路徑，快速收斂，并得到較光滑的跟蹤曲線。通過與基于PID控制的路徑追蹤控制方法相比，本文的ADRC控制器的機器魚具有更小震蕩，收斂情況更平滑快速。其速度變化曲線同樣證明了本文方法的超調(diào)更小，收斂速度更快，具有更好的控制效果。

圖5 直線路徑跟蹤圖

圖6 前向速度

圖7 轉(zhuǎn)向速度

5 結(jié)語

本文提出了一種基于ADRC的機器魚路徑跟蹤控制方法，首先建立機器魚數(shù)學模型，在SF坐標系中引入跟蹤誤差方程和期望角度函數(shù)，設(shè)計了機器魚的前向和轉(zhuǎn)向制導(dǎo)函數(shù)，并采用自抗擾控制器對參數(shù)進行調(diào)整。仿真結(jié)果表明，基于自抗擾控制器的路徑跟蹤控制器在控制超調(diào)量、快速性和振蕩時間方面優(yōu)于傳統(tǒng)PID控制，大大提高了機器魚在路徑跟蹤過程中的精度和魯棒性。