王懿偲，夏英凱，朱明，曾鑫，齊湘洪

華中農(nóng)業(yè)大學工學院/農(nóng)業(yè)農(nóng)村部長江中下游農(nóng)業(yè)裝備重點實驗室/農(nóng)業(yè)農(nóng)村部水產(chǎn)養(yǎng)殖設施工程重點實驗室，武漢 430070

水下機器人作為水下工程裝備中的一種，與傳統(tǒng)水下作業(yè)模式相比，在作業(yè)環(huán)境與模式上均有著明顯優(yōu)勢，對于提升水產(chǎn)養(yǎng)殖業(yè)的效率與品質(zhì)有著重要意義[1]。仿生機器魚作為水下機器人一種，相對于傳統(tǒng)自治水下機器人（autonomous underwater vehicle，AUV）而言，其游動通過尾部擺動提供動力，具有更高的游動效率，同時仿生機器魚游動時噪聲小，最大程度上減小魚群的應激反應，因此設計一款用于實時檢測水質(zhì)、監(jiān)測魚群狀態(tài)的仿生機器魚具有現(xiàn)實意義。為實現(xiàn)水產(chǎn)養(yǎng)殖作業(yè)精準化和智能化，機器魚需要具有自主巡航與良好的路徑跟蹤控制能力。近年來，國內(nèi)外學者針對仿生機器魚路徑跟蹤控制方面開展了廣泛的研究。宋曉茹等[2]基于自抗擾技術(shù)（active disturbance rejection control，ADRC）實現(xiàn)機器魚路徑跟蹤控制。李宗剛等[3]基于中樞模式發(fā)生器（central pattern generator，CPG）與模糊控制相結(jié)合以實現(xiàn)機器魚的定深控制。Yu等[4]基于滑模觀測器（SMO）與視線制導律（line-of-sight，LOS）控制機器魚運動，通過模糊控制調(diào)整機器魚胸鰭攻角實現(xiàn)定深控制。Zou等[5]基于神經(jīng)網(wǎng)絡的離散滑?？刂品椒?，實現(xiàn)多條仿生機器魚協(xié)同軌跡跟蹤任務。Castano等[6]基于平均動態(tài)模型，提出一種非線性模型預測控制方法使機器魚在水平面內(nèi)跟蹤期望路徑。需要指出的是，上述研究成果都局限在二維平面內(nèi)，若想實現(xiàn)全水域監(jiān)測，則需要機器魚能有效地跟蹤三維空間曲線，而三維空間曲線的路徑跟蹤相對二維平面其欠驅(qū)動的自由度增加2個，控制問題更加復雜。

針對三維路徑跟蹤控制問題，相關(guān)學者在常見的水下機器人方向已經(jīng)開展了大量的研究。Wang等[7]通過反推法建立欠驅(qū)動AUV的運動學制導律，使AUV能夠在外部擾動的情況下跟蹤上期望路徑。Wang等[8]通過Serret-Frenet坐標系以及LOS 法計算出所需偏航角，通過PID控制器對偏航角進行跟蹤控制。Xu等[9]針對模型不確定性和外部干擾下欠驅(qū)動AUV路徑跟蹤問題，通過適當?shù)剡x擇積分器的控制增益，使AUV能較為精確地跟蹤期望路徑。李澤宇等[10]建立AUV路徑跟蹤導引律，基于RBF網(wǎng)絡Q學習方法對滑模控制參數(shù)進行訓練優(yōu)化，提高AUV在不同航速及外擾動下的路徑跟蹤性能。然而，對于機器魚而言，其主要依靠魚尾擺動進行游動，相對于傳統(tǒng)水下機器人而言，其模型更為復雜，機器魚三維路徑跟蹤的研究成果相對較少。焦歐陽[11]針對機器魚目標跟蹤，利用反推控制使水下機器人跟蹤期望目標點，但反推控制依賴精確的數(shù)學模型[12]，同時過高的控制增益會導致系統(tǒng)產(chǎn)生嚴重的抖振現(xiàn)象[13]。Suebsaiprom等[14]考慮到機器魚在運動時類似于細長體AUV，基于李雅普諾夫穩(wěn)定性理論與反步法的控制器，使機器魚實現(xiàn)路徑跟蹤控制，但控制過程中容易產(chǎn)生抖振現(xiàn)象。Chen等[15]為方便深海網(wǎng)箱檢查，設計了一種二關(guān)節(jié)仿生機器魚，并基于模糊PID與CPG實現(xiàn)機器魚的三維螺旋路徑跟蹤控制，但模糊PID較為依賴經(jīng)驗進行調(diào)參。

針對上述問題，本研究設計了一款水產(chǎn)養(yǎng)殖機器魚，為監(jiān)測水質(zhì)與水下魚群狀態(tài)起輔助作用，該機器魚搭載了水質(zhì)傳感器與網(wǎng)絡攝像頭，具備水質(zhì)監(jiān)測與觀測魚群狀態(tài)的功能，并完成了機器魚控制系統(tǒng)的設計。針對存在不確定擾動和模型不確定性情況下機器魚的三維路徑跟蹤控制問題，本研究在Serret-Frenet坐標系下建立了機器魚三維空間路徑跟蹤誤差數(shù)學模型，基于LOS[16]與李雅普諾夫理論設計一種模糊滑?？刂破?，并對該控制算法進行對比仿真驗證，旨在為機器魚水下精準作業(yè)實際應用提供參考。

1 材料與方法

1.1 仿生機器魚總體結(jié)構(gòu)

水產(chǎn)養(yǎng)殖機器魚主要由浸水艙、耐壓艙、尾鰭擺動裝置構(gòu)成，其結(jié)構(gòu)如圖1所示，外殼使用尼龍材料通過3D打印技術(shù)成形，其總長約為1 000 mm，直徑185 mm，質(zhì)量約為13.1 kg，在水中為微正浮力狀態(tài)。機器魚的浸水艙與尾鰭完全浸入水中，搭載的攝像頭、傳感器和舵機都采用水密產(chǎn)品，其中位于艏部浸水艙中的水質(zhì)傳感器用于監(jiān)測水質(zhì)溶氧度、濁度與酸堿度參數(shù)，多普勒計程儀與深度傳感器用于采集機器魚位置信息，魚鰭處安裝WiFi模塊與定位天線，并用密封膠將其密封，位于浸水艙中的水下舵機通過控制胸鰭擺動實現(xiàn)機器魚上浮下潛?？刂圃骷挥谀蛪号撝?，通過水密接插件實現(xiàn)與傳感器、舵機之間數(shù)據(jù)傳輸，耐壓艙兩側(cè)連接處裝有密封圈以防止水進入艙內(nèi)。安裝在機器魚尾部的2個水下舵機作為動力裝置，輸出不同的相位差可以實現(xiàn)機器魚直線、轉(zhuǎn)彎游動。

圖1 水產(chǎn)養(yǎng)殖機器魚三維結(jié)構(gòu)圖Fig.1 Three-dimensional structure diagram of aquaculture robotic fish

1.2 仿生機器魚控制系統(tǒng)

為實現(xiàn)精準作業(yè)，本研究設計了如圖2所示的仿生機器魚控制系統(tǒng)硬件拓撲結(jié)構(gòu)，該系統(tǒng)包括水上系統(tǒng)與水下系統(tǒng)。水下系統(tǒng)控制器采用樹莓派，實現(xiàn)數(shù)據(jù)的整合與機器魚的控制，當機器魚位于水面游動時，UWB定位模塊實時監(jiān)測仿生機器魚所處位置并通過通信系統(tǒng)將信息傳輸至水上系統(tǒng)。當機器魚潛入水下游動時，UWB模塊由于信號原因無法獲取機器魚位置信息，而水下機器人通常采用慣性導航系統(tǒng)獲取位置信息，但由于其體積較大，不合適用于本研究的機器魚中。因此，本研究導航系統(tǒng)通過多普勒計程儀[17]、深度傳感器與陀螺儀采集機器魚水下的游動速度與深度信息，并對速度積分可求出機器魚位置信息。動力系統(tǒng)中采用2個水下舵機提供動力，位于浸水艙的水下舵機控制胸鰭角度調(diào)整。電源系統(tǒng)由24 V鋰電池為系統(tǒng)各元件供電，并通過電壓適配器為各元器件提供不同電壓。感知系統(tǒng)用于收集水質(zhì)信息與監(jiān)測水下狀態(tài)，并將搜集的水質(zhì)信息傳輸至控制系統(tǒng)。通訊系統(tǒng)通過WiFi模塊以及電力載波的形式實現(xiàn)信息的交互。上位機可實時顯示當前水域水質(zhì)信息以及水下環(huán)境，通過操縱按鈕發(fā)出控制參數(shù)，控制魚體運動。

圖2 水產(chǎn)養(yǎng)殖機器魚控制系統(tǒng)結(jié)構(gòu)框圖Fig.2 Block diagram of the control system of aquaculture robotic fish

本研究基于ROS開發(fā)平臺設計仿生機器魚軟件[18]，其系統(tǒng)結(jié)構(gòu)如圖3所示。仿生機器魚軟件由感知、通信、決策和控制等4層組成。在感知層中通過相關(guān)傳感器收集機器魚位置信息，濾波后將信息傳輸至控制層與通信層，在通信層中通過WiFi模塊和電力載波實現(xiàn)與上位機的數(shù)據(jù)交換。在決策層中，將確定仿生機器魚的工作模式，并將控制指令傳輸至控制層。執(zhí)行仿生機器魚的運動控制算法和驅(qū)動控制在控制層中實現(xiàn)。

圖3 水產(chǎn)養(yǎng)殖機器魚軟件系統(tǒng)結(jié)構(gòu)框圖Fig.3 The structure diagram of the aquaculture robotic fish software system

1.3 機器魚數(shù)學模型

1）仿生機器魚模型。為實現(xiàn)仿生機器魚路徑跟蹤，基于Serret-Frenet坐標系建立路徑跟蹤誤差方程，坐標系及參數(shù)定義如圖4所示，其中：{I}、{B}和{SF}分別為慣性參考坐標系、載體坐標系和Serret-Frene坐標系。{B}坐標系的原點與仿生機器魚質(zhì)心重合，其xB軸指向機器魚艏部方向，yB指向右舷，zB軸與xB軸和yB軸構(gòu)成右手坐標系；{SF}坐標系的原點為期望路徑上的任意一點P，其軸由切向量xSF、法向量ySF和副法向量zSF組成；s為P點的橫坐標，c1(s)和c2(s)分別為路徑的曲率和撓率。期望視線角(LOS)為ψlos、θlos；航向誤差角為ψe、θe；Δy、Δz為視線距離。

圖4 空間路徑跟蹤控制坐標系Fig.4 Space path tracking control coordinate system

圖4中，(x，y，z)為{I}坐標系下O點的坐標位置 ；(xe，ye，ze)為 {SF}坐 標 系 下P點 的 位 置 ；u、v、w、q和r分別為機器魚在{B}坐標系下的縱向速度、橫向速度、垂向速度、縱傾角速度及回轉(zhuǎn)角速度；機器魚的航向角與潛浮角分別為ψw=ψ+β，θw=θ+α； 其 中 ，分別為攻角與漂角；定義{SF}坐標系相對于{I}坐標系的姿態(tài)角為和，并 且 有。

本研究的仿生機器魚質(zhì)量均勻分布，且為零浮力，忽略高于2階非線性水動力阻尼項，不考慮橫搖運動的影響，因此，水下機器人的5自由度運動學模型[19]和動力學模型[20]如下：

其中，mii(i=1，2，3，5，6)為機器魚慣性質(zhì)量和流體附加質(zhì)量；dii(i=1，2，3，5，6)為機器魚線性流體動力參數(shù)；dk，k=u，v，w，q，r分別為機器魚在運動過程中受到的干擾；G為機器魚的浮力；h為重心和浮心之間的垂向距離。τu為機器魚沿xB軸方向控制輸入的力，τq和τr為機器魚繞yB軸與zB軸輸入的力矩。

將式（2）中子系統(tǒng)u、q和r簡化為：

在機器魚載體中，潛浮控制力矩τq由胸鰭舵板產(chǎn)生，航速控制力矩τu和航向控制力矩τr由艉部的2個舵機耦合產(chǎn)生。在實際試驗時，潛浮控制相對簡單，可通過τq直接推算出胸鰭舵板的控制角度；進行航速和航向控制時，可先構(gòu)建兩輸入-兩輸出的舵角-力矩模型，然后通過偽逆法或二次優(yōu)化的方法解算出實際控制舵角的大小。本研究僅針對機器魚的三維路徑跟蹤控制問題，進行控制方法和仿真研究，因此暫不討論舵角與力矩之間的控制解算問題，在后續(xù)研究中將進一步介紹。

2）路徑跟蹤誤差模型。將{I}坐標系下的機器魚和期望路徑上點P的位置誤差轉(zhuǎn)換至{SF}坐標系下，其誤差方程[21]如下：

對（4）式求導得誤差動力學方程[22]為：

3）控制問題描述。本研究的目標是設計一款控制器使機器魚在三維空間中跟蹤期望路徑，由于路徑跟蹤控制問題與時間無關(guān)，因此，假設機器魚前向運動的速度ud為常數(shù)，基于機器魚運動學模型，推導出有效的運動學控制律，以產(chǎn)生期望的縱傾角速度qd和回轉(zhuǎn)角速度rd，使路徑跟蹤誤差在t→∞時收斂到零。并建立有效的動力學控制律，以產(chǎn)生控制輸入τu，τr和τq，使誤差xe，ye，ze和u-ud在t→ ∞時收斂到零。

為方便機器魚路徑跟蹤控制，作出以下假設：

假設1：機器魚期望路徑xd，yd和zd是有界的。

1.4 機器魚路徑跟蹤控制器設計

圖5為仿生機器魚路徑跟蹤控制器結(jié)構(gòu)圖，控制器設計分為兩步完成。第一步通過三維LOS制導法獲得機器魚期望角度，使虛擬目標沿期望路徑運動，得到運動學控制器。第二步為解決外部環(huán)境不確定干擾，基于干擾觀測器的模糊滑?？刂苼斫鉀Q機器魚動力學問題。

圖5 機器魚路徑跟蹤控制圖Fig.5 Control block diagram of robot fish path tracking

1）位置控制率的設計。為了實現(xiàn)機器魚制導與控制，利用以下LOS制導律[24]。

構(gòu)造李雅普諾夫函數(shù)為：

對公式（8）進行時間求導并帶入公式（5）可得：

為使?1＜0，設計虛擬向?qū)俣瓤刂坡蔀椋?/p>

其中，k1＞0為設計的控制增益，并將公式（7）與（10）代入（9）中可得：

2）姿態(tài)子系統(tǒng)運動學控制率設計。為使機器魚達到路徑跟蹤的目的，設計所需跟蹤誤差角[25]為：

其中，k2＞0，k3＞0，為設計的控制增益，為得到期望回轉(zhuǎn)角速度與縱傾角速度，將公式（12）帶入（6）可得：

3）干擾觀測器設計。為了估計未知環(huán)境的擾動，設計以下干擾觀測器：

其中，pu、pq和pr為設計的輔助變量，k4、k5和k6為正數(shù)。

4）航速控制律設計。通過控制輸入τu，當t→∞時，使機器魚縱向速度u最終可以收斂到所需的期望航速ud，定義縱向速度跟蹤誤差為：

為實現(xiàn)縱向角速度的跟蹤，采用滑?？刂品椒?，并選擇滑模面為：

其中，cu＞0，對公式（18）求導可得：

為提高系統(tǒng)收斂效率，采用以下趨近定律：

其中，εu＞0，k7＞0，將（20）帶入（19），其縱傾角控制輸入τu設計為：

5）縱傾控制律設計。定義艏向角速度跟蹤誤差為：

與本文“1.3 4）”類似，設計滑模面為：

其中，cq＞0?？v傾角控制輸入τq設計為：

其中，εq＞0，k8＞0。

6）艏向控制律設計。定義艏向角速度跟蹤誤差為：

并選擇滑模面為：

其中，cr＞0。艏向角控制輸入τr設計為：

其中，εr＞0，k9＞0。

7）穩(wěn)定性證明。選取如下李雅普諾夫函數(shù)：

8）模糊滑?？刂破髟O計。根據(jù)式（30）可以看出，V?2收斂至零時，其收斂速度取決于k7、k8和k9，在指數(shù)趨近中，趨近速度從較大值逐步減小至零，使運動點到達切換面時的速度很小，當運動點接近滑模面s=0時，有s?=±ε，即運動點到達滑模面時的趨近速度是ε而不是零，可以保證運動點有限時間到達。當ε較大時，運動點穿過滑模面距離變大，引起系統(tǒng)劇烈抖振，但選取ε值較小時，雖然減小了抖振，但延長了系統(tǒng)的趨近時間。為保障系統(tǒng)能快速趨近的同時削弱抖振，應當在增大k的同時減小ε[26]。

綜合上述情況，本研究滑模趨近律被重新設計為：

式（31）中，α1j、α2j（j=1，2，3）分別為模糊控制的輸出值，選取運動點與滑模面之間的距離sl（l=u，q，r）為模糊控制的輸入值，在本研究中，艏向、縱傾與航速的趨近律設計相同，因此，都采用一種模糊規(guī)則進行控制。如圖5所示，系統(tǒng)的輸入與輸出的模糊量均用ZO（零）、PS（正?。M（正中）、PB（正大）為模糊子集，隸屬度函數(shù)均采用三角形隸屬度函數(shù)，并提出4條模糊規(guī)則：R1：if|sl|is ZO thenα1jis ZO，α2jis PM；R2：if|sl|is PS thenα1jis PS，α2jis PB；R3：if|sl|is PM thenα1jis PM，α2jis PS；R4：if|sl|is PB thenα1jis PB，α2jis ZO。

圖6 模糊系統(tǒng)的隸屬度函數(shù)Fig.6 Membership function of fuzzy systems

2 結(jié)果與分析

2.1 無干擾仿真對比

為驗證所提出控制算法的有效性，利用Matlab軟件對機器魚控制系統(tǒng)進行仿真分析，并與常規(guī)PID、滑?？刂破餍阅苓M行對比。仿生機器魚各參數(shù)選取為：m11=25 kg，m22=17 kg，m33=30 kg，m55=22.5 kg，m66=15 kg，d11=30，d22=30，d33=30，d55=20，d66=20，Gh=5。選擇期望路徑為Pd(μ)=[μ，20cos(0.05μ)，20sin(0.05μ)]T；選擇期望游速為ud=1 m/s；機器魚初始狀態(tài)為 ：η0=[0，10，0，0，0，0]T；初始速度為v0=[0.1，0，0，0.1，0.1]T；仿真選用普通工控機，CPU型號為Intel Core I5，仿真步長為0.01 s，仿真時間長度為100 s，計算方式選用ode3?？刂破髌渌嚓P(guān)參數(shù)選擇為：k1=0.1，k2=1，k3=20，k4=k5=k6=2.5，k7=20，k8=k9=1，εu=15，εq=εr=2，cu=cq=cr=1.5。其仿真結(jié)果如圖7所示。

圖7 機器魚期望路徑和實際路徑Fig.7 Robot fish expected path and actual path

從圖7可知，機器魚通過模糊滑?？刂聘欀疗谕窂綍r間為38.7 s，滑?？刂聘欀疗谕窂綍r間為69.5 s，常規(guī)PID跟蹤至期望路徑時間為77.5 s。從圖8可知，所提出的模糊滑?？刂葡鄬τ诔Ｒ?guī)PID與滑模控制，其收斂速度更快，超調(diào)量更小。從圖9可知，模糊滑模控制相對于常規(guī)滑模明顯減小了系統(tǒng)抖振，取得更好的控制效果。從圖10可知，滑?？刂破飨鄬τ诔Ｒ?guī)PID與滑?？刂破鳎斎肓εc力矩波動更小。

圖8 機器魚與期望軌跡直接的位置誤差Fig.8 The position error between the robot fish and the expected trajectory

圖9 機器魚線速度和角速度Fig.9 Line speed and angular velocity of robotic fishing

圖10 機器魚控制輸入力和輸入力矩Fig.10 Robot fish controls input force and input torque

2.2 對干擾的魯棒性能測試

本研究所設計的機器魚主要在養(yǎng)殖水塘中作業(yè)，受到的水流較小，在機器魚游動過程中，主要面對來自自身運動，如艏搖等方面的干擾。因此，為進一步驗證所提出的控制器對干擾的魯棒性能，進行了2種不同情況下擾動的仿真試驗，并與無擾動系統(tǒng)進行對比，在干擾1中，機器魚受到持續(xù)擾動，在干擾2中，機器魚受到的擾動與時間相關(guān)。

從圖11可知，在加入擾動下，系統(tǒng)出現(xiàn)了一些波動，但仍然較為精確地跟蹤期望路徑，由此證明所研究的干擾觀測器有效，三維路徑跟蹤算法在面對未知干擾時仍具備良好的精度及魯棒性，上述特質(zhì)為算法移植到真實樣機提供了良好的條件。當然，必須指出，在面臨真實環(huán)境中的復雜擾動時，相關(guān)控制參數(shù)需進一步優(yōu)化。

圖11 不同擾動下的跟蹤誤差Fig.11 Tracking errors under different disturbances

3 討論

本研究基于現(xiàn)代化水產(chǎn)養(yǎng)殖需求，設計了一款水產(chǎn)養(yǎng)殖機器魚以實現(xiàn)全水域?qū)崟r監(jiān)測，通過軟件系統(tǒng)可獲得水域環(huán)境信息并對機器魚進行控制，為水產(chǎn)養(yǎng)殖水質(zhì)與水下環(huán)境監(jiān)測起輔助作用。針對機器魚三維路徑跟蹤控制問題，本研究通過Serret-Frenet坐標系下建立了機器魚三維空間路徑跟蹤誤差模型，由于難以精確建立機器魚數(shù)學模型，因此在參數(shù)不確定的情況下，基于LOS制導法和李雅普諾夫理論設計一種模糊滑?？刂破鳎ㄟ^Matlab對所提控制器與常規(guī)PID和滑?？刂破鬟M行對比。仿真結(jié)果驗證模糊滑模控制器能夠?qū)崿F(xiàn)欠驅(qū)動機器魚在模型參數(shù)不確定性下的三維路徑跟蹤控制，并且控制精度與魯棒性明顯優(yōu)于常規(guī)PID與滑?？刂破?。限于論文篇幅及機器魚加工進度等問題，本研究僅開展了數(shù)字仿真工作，證實了本文提出的算法具有良好的控制精度和魯棒性，具備移植到實際樣機中的條件，但考慮到實際試驗環(huán)境中干擾會更加復雜，在進行樣機試驗時，還需進一步優(yōu)化算法，相關(guān)內(nèi)容將在后續(xù)研究中介紹。