水產(chǎn)養(yǎng)殖機(jī)器魚設(shè)計(jì)與三維路徑跟蹤控制

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

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

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

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

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

1 材料與方法

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

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

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

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

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

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

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

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

1.3 機(jī)器魚數(shù)學(xué)模型

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

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

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

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

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

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

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

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

對(duì)（4）式求導(dǎo)得誤差動(dòng)力學(xué)方程[22]為：

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

為方便機(jī)器魚路徑跟蹤控制，作出以下假設(shè)：

假設(shè)1：機(jī)器魚期望路徑xd，yd和zd是有界的。

1.4 機(jī)器魚路徑跟蹤控制器設(shè)計(jì)

圖5為仿生機(jī)器魚路徑跟蹤控制器結(jié)構(gòu)圖，控制器設(shè)計(jì)分為兩步完成。第一步通過(guò)三維LOS制導(dǎo)法獲得機(jī)器魚期望角度，使虛擬目標(biāo)沿期望路徑運(yùn)動(dòng)，得到運(yùn)動(dòng)學(xué)控制器。第二步為解決外部環(huán)境不確定干擾，基于干擾觀測(cè)器的模糊滑?？刂苼?lái)解決機(jī)器魚動(dòng)力學(xué)問(wèn)題。

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

1）位置控制率的設(shè)計(jì)。為了實(shí)現(xiàn)機(jī)器魚制導(dǎo)與控制，利用以下LOS制導(dǎo)律[24]。

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

對(duì)公式（8）進(jìn)行時(shí)間求導(dǎo)并帶入公式（5）可得：

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

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

2）姿態(tài)子系統(tǒng)運(yùn)動(dòng)學(xué)控制率設(shè)計(jì)。為使機(jī)器魚達(dá)到路徑跟蹤的目的，設(shè)計(jì)所需跟蹤誤差角[25]為：

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

3）干擾觀測(cè)器設(shè)計(jì)。為了估計(jì)未知環(huán)境的擾動(dòng)，設(shè)計(jì)以下干擾觀測(cè)器：

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

4）航速控制律設(shè)計(jì)。通過(guò)控制輸入τu，當(dāng)t→∞時(shí)，使機(jī)器魚縱向速度u最終可以收斂到所需的期望航速ud，定義縱向速度跟蹤誤差為：

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

其中，cu＞0，對(duì)公式（18）求導(dǎo)可得：

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

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

5）縱傾控制律設(shè)計(jì)。定義艏向角速度跟蹤誤差為：

與本文“1.3 4）”類似，設(shè)計(jì)滑模面為：

其中，cq＞0?？v傾角控制輸入τq設(shè)計(jì)為：

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

6）艏向控制律設(shè)計(jì)。定義艏向角速度跟蹤誤差為：

并選擇滑模面為：

其中，cr＞0。艏向角控制輸入τr設(shè)計(jì)為：

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

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

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

綜合上述情況，本研究滑模趨近律被重新設(shè)計(jì)為：

式（31）中，α1j、α2j（j=1，2，3）分別為模糊控制的輸出值，選取運(yùn)動(dòng)點(diǎn)與滑模面之間的距離sl（l=u，q，r）為模糊控制的輸入值，在本研究中，艏向、縱傾與航速的趨近律設(shè)計(jì)相同，因此，都采用一種模糊規(guī)則進(jìn)行控制。如圖5所示，系統(tǒng)的輸入與輸出的模糊量均用ZO（零）、PS（正?。?、PM（正中）、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 無(wú)干擾仿真對(duì)比

為驗(yàn)證所提出控制算法的有效性，利用Matlab軟件對(duì)機(jī)器魚控制系統(tǒng)進(jìn)行仿真分析，并與常規(guī)PID、滑?？刂破餍阅苓M(jìn)行對(duì)比。仿生機(jī)器魚各參數(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；機(jī)器魚初始狀態(tài)為 ：η0=[0，10，0，0，0，0]T；初始速度為v0=[0.1，0，0，0.1，0.1]T；仿真選用普通工控機(jī)，CPU型號(hào)為Intel Core I5，仿真步長(zhǎng)為0.01 s，仿真時(shí)間長(zhǎng)度為100 s，計(jì)算方式選用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 機(jī)器魚期望路徑和實(shí)際路徑Fig.7 Robot fish expected path and actual path

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

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

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

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

2.2 對(duì)干擾的魯棒性能測(cè)試

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

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

圖11 不同擾動(dòng)下的跟蹤誤差Fig.11 Tracking errors under different disturbances

3 討論

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