張 川，文小玲，喻 盈，黃晨華

武漢工程大學(xué)電氣信息學(xué)院，湖北 武漢430205

隨著科學(xué)技術(shù)的不斷發(fā)展，機器人技術(shù)在工業(yè)、農(nóng)業(yè)、國防及人們的日常生活中得到越來越廣泛的應(yīng)用［1-2］。仿生機器魚因其靈活、游動速度和效率高等特點在水下探測、水產(chǎn)養(yǎng)殖、水環(huán)境監(jiān)測等領(lǐng)域有廣闊的應(yīng)用前景。1994 年，麻省理工學(xué)院研制出第一條仿生機器魚“Robo Tuna”［3］，尾部推動結(jié)構(gòu)達到8 個關(guān)節(jié)，推進效率91%，速度1.5 m/s，但是結(jié)構(gòu)復(fù)雜，總共有近3 000 個零件，體長達到了1.25 m。2005 年，英國埃塞克斯大學(xué)研制的“fish-G9”機器魚［4］，使用3 個伺服電機構(gòu)成驅(qū)動結(jié)構(gòu)，游動速度達到0.9 m/s，體長縮短至0.52 m。2012 年，新加坡國立大學(xué)電氣與計算機工程系徐建新等人采用了兩關(guān)節(jié)的魚尾結(jié)構(gòu)［5］，不具備上浮下潛功能，長度為0.25 m。 2019 年，印度的薩斯特拉大學(xué)采用了多關(guān)節(jié)的魚尾結(jié)構(gòu)［6］，讓機器魚的游動姿態(tài)有極高的擬真度，但是也增加了控制的復(fù)雜程度。在機器魚的尾部結(jié)構(gòu)方面，關(guān)節(jié)數(shù)量多具有靈活、速度快、姿態(tài)擬真度高等優(yōu)點，同時也會帶來長度和控制難度增加的弊端，而關(guān)節(jié)數(shù)量少的機器魚具有體積小、控制容易、結(jié)構(gòu)簡單的特點，缺點則是速度會相對降低。Sfakiotakis 等［7］的研究表明機器魚的機動性與參與推進的身體部分占比有關(guān)，占比越高、機動性越高，但占比過高會引起游動姿態(tài)的混亂，最后提出了合理的推進部分占比。HUANG 等［8］提出了基于神經(jīng)網(wǎng)絡(luò)的機器魚避障控制方法，并通過仿真驗證了其可行性，但因神經(jīng)網(wǎng)絡(luò)需要大量的訓(xùn)練集數(shù)據(jù)和漫長的訓(xùn)練過程而無法實現(xiàn)機器魚的實時避障控制。Wang 等［9］采用了基于迭代學(xué)習(xí)的控制方法，通過不斷迭代讓機器魚關(guān)節(jié)運動滿足預(yù)設(shè)的軌跡，仿真結(jié)果表明迭代到一定次數(shù)之后與預(yù)設(shè)軌跡誤差較小，但是計算量較大，因此實時性不強。王耀威等［10］采用仿真對比展現(xiàn)了 中央模式發(fā)生器（central pattern generator，CPG）控制方法的穩(wěn)定性，但是目前該控制方法仍處于起步階段，且含有高度非線性的環(huán)節(jié)，給CPG 的應(yīng)用研究帶來了許多困難。張倩［11］采用了基于模糊規(guī)則的控制策略，模糊控制的計算量不大，并且具有較強的實時性和魯棒性，對于處理未知環(huán)境的避障問題有很大的優(yōu)勢，缺點在于缺乏系統(tǒng)性，隸屬函數(shù)的確立需要憑借經(jīng)驗。

鑒于尾部結(jié)構(gòu)和控制方法的優(yōu)缺點分析，本文提出一種雙關(guān)節(jié)仿生機器魚，主要創(chuàng)新點如下：

①推進結(jié)構(gòu)方面，采用雙關(guān)節(jié)尾鰭推進方式，在能完成基本運動要求的同時可以最大程度簡化機械結(jié)構(gòu)和控制難度，同時選擇了尾部推進部分合適的占比以彌補關(guān)節(jié)數(shù)量少導(dǎo)致的機動性不足。

②控制算法方面，在速度和轉(zhuǎn)角兩個方面采用模糊控制，具有較強的實時性和魯棒性，上浮下潛則直接制定規(guī)則庫進行控制，可以簡化計算量，對該復(fù)合避障算法進行了仿真和實際測試，證明該算法的可行性和避障率高以及機器魚控制的低擾動性。

1 機器魚外形結(jié)構(gòu)

根據(jù)魚類的體型體態(tài)，仿生機器魚的推進方式可以分為兩大類：利用魚體和尾鰭推進（body and/or caudal fin，BCF）的推進方式，利用中間鰭或?qū)捦七M（median and/or paired fin，MPF）的推進方式［12］。BCF 與MPF 相比，游動效率更高，且加速和啟動性能良好，機動性也更高［13］，因此本文采用BCF 推進方式，并采用雙關(guān)節(jié)的推動結(jié)構(gòu)。

機器魚的上浮下潛方式可以分為兩種：改變重心法和改變重力法。改變重力法是通過改變魚體內(nèi)儲水器的儲水量來控制重力與浮力之間的平衡關(guān)系［14］，具有更易于實現(xiàn)和對環(huán)境的干擾更小的優(yōu)點，因此選擇采用該方法。通過改變水箱儲水量實現(xiàn)機器魚上浮下潛?？紤]到水箱內(nèi)部氣壓對水泵進水和排水的影響，在水箱上加裝了排氣孔和氣囊，用于平衡水箱內(nèi)外氣壓。上浮下潛結(jié)構(gòu)位于機器魚的中部，實物圖如圖1（a）所示。

借鑒鳙魚的體態(tài)，采用雙關(guān)節(jié)舵機驅(qū)動的機器魚3D 模型結(jié)構(gòu)如圖1（b）所示。

其中1 為紅外測距傳感器，2 為水質(zhì)測量傳感器，3 為舵機。整個機器魚長450 mm、寬140 mm、高180 mm。Sfakiotakis 等［7］的研究表明，當機器魚的尾部推進部分為整個身體長度的1/3 時，機器魚可以在機動性和姿態(tài)的連貫性之間取得平衡，因此機器魚推進部分的長度為150 mm。

由圖1（b）制作出仿生機器魚的實物如圖1（c）所示。

圖1 機器魚相關(guān)圖像：（a）水箱，（b）機器魚3D 模型，（c）機器魚實物Fig. 1 Robotic fish related images：（a）water tank，（b）3D model of robotic fish，（c）physical robotic fish

2 運動控制和避障方法

2.1 動力學(xué)模型

基于魚體波運動曲線方程，忽略環(huán)境因素的影響，雙關(guān)節(jié)機器魚的運動可抽象為以下數(shù)學(xué)模型：

式中：α、β分別為第一、二關(guān)節(jié)與機器魚軸線的夾角；Kα為振幅系數(shù)，表示關(guān)節(jié)振幅與最大振幅的比值，當Kα=0 時，兩個關(guān)節(jié)不擺動，當Kα=1 時，兩個關(guān)節(jié)以最大振幅擺動；A1m表示第一個關(guān)節(jié)和魚體軸之間的擺動振幅，A2m表示第二個關(guān)節(jié)和第一個關(guān)節(jié)之間的擺動振幅；K為偏斜系數(shù)，表示各關(guān)節(jié)擺動對稱軸與魚體軸的偏斜程度，其范圍為-1～1，當K為0 時，表示魚體軸與對稱軸重合；T為擺動周期；φ表示第二個關(guān)節(jié)和第一個關(guān)節(jié)之間的相位差。

2.2 游動控制

機器魚游動時，雙關(guān)節(jié)擺動模型如圖2（a）所示。圖2（a）中，α1、β1分別是第一、二關(guān)節(jié)與機器魚軸線的夾角。忽略外界環(huán)境的影響，根據(jù)機器魚在直線游動時，K= 0，φ= -π［15］，則A1m、A2m均轉(zhuǎn)換為關(guān)節(jié)與魚體軸之間的擺動振幅，用速度系數(shù)kv代替振幅系數(shù)Kα，代入式（1），可以得到機器魚直線游動時的數(shù)學(xué)模型如下：

圖2 機器魚雙關(guān)節(jié)擺動模型：（a）直線游動，（b）轉(zhuǎn)向Fig. 2 Double joint swing models of robotic fish：（a）linear swimming，（b）turning

式中，A1、A2分別為第一、二關(guān)節(jié)擺動的振幅，T1為第一、二關(guān)節(jié)擺動的周期；kv為速度系數(shù)（范圍0.1～1），其值越小、角度的幅值就越小、前進的速度也就越慢，通過改變kv的值就可以改變機器魚的游動速度。

2.3 轉(zhuǎn)向控制

機器魚轉(zhuǎn)向時尾部的運動狀態(tài)與游動時不同，以機器魚右轉(zhuǎn)為例，雙關(guān)節(jié)擺動模型如圖2（b）所示。

轉(zhuǎn)向時，以機器魚軸線作為兩個關(guān)節(jié)的擺動軸線，則K= 0；且兩個關(guān)節(jié)均在機器魚軸線一側(cè)擺動，用轉(zhuǎn)向系數(shù)kf代替振幅系數(shù)Kα，在式（1）的基礎(chǔ)上，得到轉(zhuǎn)向時的數(shù)學(xué)模型如下：

式中，A3、A4分別為第一、二關(guān)節(jié)擺動的振幅，T3、T4分別為第一、二關(guān)節(jié)擺動的周期；kf為轉(zhuǎn)向系數(shù)（范圍-1～1），正值表示右轉(zhuǎn)、負值表示左轉(zhuǎn)，kf的絕對值越大、轉(zhuǎn)向半徑越小，反之亦然；k是常數(shù)，其值為1 或者-1，與kf同號。

2.4 避障算法

5 個紅外測距傳感器用于實時避障。具體位置和朝向如圖3 所示。

圖3 傳感器位置和朝向Fig. 3 Sensor position and orientation

圖3 中，1 為正前方（FIRS）、2 為上側(cè)（TIRS）、3 為下側(cè)（BIRS）、4 為左側(cè)（LIRS）、5 為右側(cè)（RIRS）。

基于游動、轉(zhuǎn)向、上浮、下潛控制原理，以及傳感器的布局，提出一種基于模糊控制的復(fù)合三維避障方法。對于速度系數(shù)和轉(zhuǎn)向角度，采用模糊控制，對于上浮下潛，采用建立規(guī)則庫來實現(xiàn)避障。整個復(fù)合避障控制器的結(jié)構(gòu)如圖4 所示。

圖4 中，l是速度系數(shù)模糊控制器的輸入量，其值為

圖4 復(fù)合避障控制框圖Fig. 4 Block diagram of compound obstacle avoidance control

式中，F(xiàn)IRS、TIRS、BIRS、LIRS、RIRS分別表示其對應(yīng)傳感器測量到的距離。其論域為［10，80］，模糊子集為｛很近（NT）、近（N）、中（M）、遠（F）｝，輸出速度系數(shù)V的模糊子集為｛VSM（最慢速）、VS（慢速）、VM（中速）、VN（正常速度）｝，速度系數(shù)kv的論域為［0.2，1］。速度系數(shù)模糊規(guī)則如表1 所示。

表1 速度系數(shù)模糊規(guī)則Tab. 1 Fuzzy rules of speed coefficient

ε是轉(zhuǎn)向系數(shù)模糊控制器的輸入量，其值為

根據(jù)左右兩側(cè)紅外測距傳感器的檢測范圍為10～80 cm，再加上一定的裕度，可得其論域為［-80，80］。ε的模糊子集為｛右遠（RF）、右中（RM）、右近（RN）、零（ZO）、左近（LN）、左中（LM）、左遠（LF）｝。當ε為正值時，其值越大則表明障礙物離機器魚右側(cè)越近；當ε為負值時，其值越小則表明障礙物離機器魚左側(cè)越近，鑒于紅外測距傳感器的精度，左右側(cè)兩個紅外測距傳感器的值基本不存在相等的情況，因此當ε等于0 時，此時可認為前方?jīng)]有障礙物。輸出轉(zhuǎn)向系數(shù)kf的模糊子集為｛PB（正大）、PM（正中）、PS（正?。?、Z（零）、NS（負?。M（負中）、NB（負大）｝，轉(zhuǎn)向系數(shù)kf的論域為［-1，1］。轉(zhuǎn)向系數(shù)模糊規(guī)則如表2 所示。

表2 轉(zhuǎn)向角度模糊規(guī)則Tab.2 Fuzzy rules of steering angle

對于上述速度系數(shù)和轉(zhuǎn)向角度模糊控制器，輸入、輸出都采用三角隸屬度函數(shù)，反模糊化采用最大隸屬度法。

上浮下潛控制的輸入信號來自上側(cè)（TIRS）和下側(cè)（BIRS）紅外測距傳感器，與速度和轉(zhuǎn)向角度的控制方式不同，上浮下潛控制的輸出為上浮或者下潛的二值離散變量，而不是連續(xù)變量，因此直接采用制定規(guī)則進行控制可以進一步減少算法的計算量。上浮下潛控制規(guī)則如表3 所示。

表3 上浮下潛控制規(guī)則Tab. 3 Control rules for floating and diving

表3 中，考慮到機器魚的高度以及上側(cè)和下側(cè)傳感器傾斜的角度，設(shè)定40 cm 作為臨界距離。若TIRS和BIRS的測量值都小于40 cm，則表明機器魚不能通過向前游動避開障礙物，只能通過轉(zhuǎn)向來避障；若TIRS和BIRS的測量值都大于40 cm，則表明機器魚可以直接前游，不用采取避障措施。在其余兩種情況下，則往數(shù)值較大的一側(cè)上浮或者下潛即可完成避障。

綜合速度系數(shù)、轉(zhuǎn)向系數(shù)模糊控制和上浮下潛的規(guī)則控制方法得到最終的復(fù)合避障方法。

利用MATLAB 進行仿真，設(shè)定一個長寬高均為10 cm 的三維空間，并在其中設(shè)置了6 組障礙物，每一組障礙物由若干個邊長為0.3 cm 的正方體構(gòu)成，仿真結(jié)果如圖5（a）所示。

其中1 組障礙物的坐標為（2，1，a），其中整數(shù)a 的范圍為8～10，2 組為（2，3，b），其中整數(shù)b的范圍為8～10，3組為（3，5，c），其中整數(shù)c的范圍為5～10，4 組為（2，8，d），其中整數(shù)d的范圍為5～10，5 組為（6，e，f），其中整數(shù)e的范圍為7～10，整數(shù)f的范圍為5～10，6組為（g，6，h），其中整數(shù)g的范圍為6～10，整數(shù)h的范圍為1～10，起點坐標為（0.5，0.5，9.5），終點坐標為（9.5，9.5，0.5），得到仿真結(jié)果的立體圖。

從圖5（a）的避障仿真結(jié)果可以看出，利用本文中的復(fù)合避障算法，機器魚可以成功避障并抵達目的地。選取圖5（a）的俯視視角對避障效果和過程進行分析，俯視圖如圖5（b）所示。從俯視圖可以看出，機器魚出發(fā)時，右側(cè)有障礙物1、左側(cè)是邊界（視為障礙物）、因此直行，一直到經(jīng)過障礙物2，由于此時障礙物3 還較遠，左側(cè)是邊界，因此右轉(zhuǎn)，遇到障礙物3 時左轉(zhuǎn)，又遇到障礙物4 時右轉(zhuǎn)，由于障礙物5 下方有足夠空間，因此下潛避開障礙物，最后到達終點。

圖5 避障仿真結(jié)果：（a）立體圖，（b）俯視圖Fig. 5 Obstacle avoidance simulation results：（a）stereograph，（b）top view

3 控制系統(tǒng)設(shè)計

仿生機器魚的控制系統(tǒng)硬件架構(gòu)如圖6 所示，以STM32F103ZET6 為控制核心，通過對5 個型號為GP2Y0A21YK0F 的紅外測距傳感器的信息進行處理和分析實現(xiàn)機器魚的避障，水泵和舵機分別實現(xiàn)機器魚的上浮下潛和前進轉(zhuǎn)向，采用NEO-6M GPS 定位模塊，無線通信采用傳輸距離較遠的Lora 模塊，使用雙輸出鋰電池為STM32 和舵機提供5 V 電源、為水泵提供12 V 電源。

圖6 控制系統(tǒng)總體框架圖Fig. 6 Overall framework of control system

控制系統(tǒng)的軟件由主程序、運動控制與避障程序、電源管理程序、PWM控制信號產(chǎn)生程序等組成。

根據(jù)機器魚的控制系統(tǒng)總體框架，機器魚控制系統(tǒng)電路原理圖如圖7 所示。紅外測距傳感器輸出的是模擬信號，利用控制器的模擬數(shù)字轉(zhuǎn)換器（analog to digital converter，ADC）采集數(shù)據(jù)；兩個舵機由控制器輸出兩路PWM 信號控制轉(zhuǎn)動角度；兩個水泵由控制器的兩個普通I/O 控制開通和關(guān)斷，分別由兩路脈沖寬度調(diào)制（pulse width modulation，PWM）信號控制水泵抽水和排水的速度；GPS 和Lora 無線模塊都采用串口通信協(xié)議，通過串口與控制器相連；電源及電量檢測的輸出信號也是利用控制器的ADC 進行采集；STM32F103ZET6 控制器原本有144 個引腳，I/O 引腳112 個，圖中只畫出了實際用到的引腳，其余引腳可以作為后續(xù)擴展使用。

圖7 控制系統(tǒng)電路原理圖Fig. 7 Schematic diagram of control system circuit

3.1 電源管理程序

為了保證電量充沛，需要對鋰電池電量進行監(jiān)測和預(yù)警，確保仿生機器魚有足夠的電量返航。

耗電量測量程序流程如圖8（a）所示。由于機器魚到達目的地和返回起始點的耗電量可以近似看作相等，因此采用以下方法進行電池電量的檢測。在機器魚啟動時、測得電池電量U1，機器魚到達目的地時、測得電池電量U2，機器魚執(zhí)行任務(wù)完畢時測得的電量為U3，可以得到返航所需的最小電量為：

圖8 流程圖：（a）耗電量測量，（b）電量預(yù)警，（c）PWM 產(chǎn)生，（d）運動控制與避障Fig. 8 Flow diagram：（a）power consumption measurement，（b）electricity warning，（c）PWM output，（d）motion control and obstacle avoidance

機器魚執(zhí)行一次任務(wù)所需的最小電量為：

如圖8（b）所示，機器魚實際運動的過程中，每30 s 檢測一次電池電量，考慮電池工作時的損耗，因此在原來的最低返航閾值的基礎(chǔ)上乘以1.1，確保機器魚電量報警時還有足夠電量返航，如果U小于1.1Uc，說明系統(tǒng)電量只足夠機器魚返航，不能繼續(xù)運動，如果U小于1.1Ue，說明系統(tǒng)電量不足以維持機器魚執(zhí)行任務(wù)，同樣應(yīng)該迅速返航。

3.2 PWM 信號產(chǎn)生程序

PWM 信號用于控制舵機，PWM 輸出程序流程如圖8（c）所示。STM32 控制器輸出PWM 是基于高級定時器，首先需要初始化用于輸出PWM 的I/O，包括使能I/O 的時鐘和配置輸出模式為復(fù)用輸出，初始化定時/計數(shù)器，選取時鐘頻率為72 MHz；PWM 有兩種輸出模式：PWM1 和PWM2，兩種模式在輸出上沒有區(qū)別；然后配置PWM 的周期和占空比，兩者是舵機控制的決定性因素；最后使能定時器和PWM 輸出。

3.3 運動控制與避障程序

機器魚的運動控制與避障程序流程如圖8（d）所示。機器魚在運動時，若有1 個紅外測距傳感器檢測到有障礙物，則將5 個紅外測距傳感器的測量值都進行數(shù)據(jù)處理，得到l、ε、TIRS和BIRS并將其輸入到復(fù)合模糊避障控制器，分別作為速度系數(shù)、轉(zhuǎn)向系數(shù)和上浮下潛控制器的輸入，經(jīng)過模糊控制器的處理，得到對應(yīng)的速度系數(shù)、轉(zhuǎn)向系數(shù)以及上浮下潛的距離，最后產(chǎn)生PWM 信號去控制舵機和水泵實現(xiàn)機器魚的避障，成功避障后再向預(yù)定點游動，若沒有檢測到障礙物，則向預(yù)定點游動。

檢測機器魚是否到達預(yù)定點，若到達預(yù)定點，則執(zhí)行水質(zhì)采集任務(wù)，否則繼續(xù)判斷是否存在障礙物，以此循環(huán)，直到到達預(yù)定點。

4 樣機測試及結(jié)果分析

制作樣機進行測試，樣機內(nèi)部構(gòu)成如圖9所示。

圖9 樣機內(nèi)部圖Fig. 9 Prototype internal diagram

圖9 中，1 為紅外測距傳感器（另外有2 個被遮擋未標出），2 為抽水水泵，3 為排水水泵，4 為繼電器，5 為Lora 無線通信模塊，6 為GPS，7 為STM32控制器，8 為蓄電池，9 為數(shù)據(jù)采集板（對水質(zhì)測量傳感器的數(shù)據(jù)進行處理，以RS-485 協(xié)議輸出數(shù)據(jù)），10 為485 轉(zhuǎn)串口模塊（將數(shù)據(jù)采集板輸出的數(shù)據(jù)以串口協(xié)議發(fā)到STM32 控制器），11 為溶解氧測量傳感器，12 為pH 值測量傳感器，13 為第一關(guān)節(jié)舵機，14 為第二關(guān)節(jié)舵機。

4.1 避障測試

在學(xué)校的靜心湖對機器魚進行了運動控制及避障測試，以湖泊堤壩作為障礙物，每隔4 s 拍攝一次圖片，避障結(jié)果如圖10 所示。從圖10 可以看出，機器魚可以成功避開障礙物。

圖10 實際避障測試Fig. 10 Actual obstacle avoidance test

為了測試避障的成功率，進行了20 次避障測試，其中避障成功18 次，避障失敗2 次，有觸碰湖堤壩的情況，避障成功率達到90%。

4.2 巡游測試

為了測試機器魚的低擾動性，在學(xué)校靜心湖進行了巡游測試，在機器魚巡游過程中，每隔2 s拍攝1 張圖片，如圖11 所示。

圖11 機器魚巡游情況Fig. 11 Robot fish cruise situation

從圖11 可見，機器魚在游動過程中，并未對水中的魚類造成干擾，由此可以看出整個系統(tǒng)具有低擾動性。

4.3 實際應(yīng)用測試

為了驗證機器魚的實際應(yīng)用效果，在機器魚上搭載溶解氧和pH 水質(zhì)檢測傳感器（兩種傳感器均有溫度補償，因此也可以測出水溫）采集水質(zhì)參數(shù)并利用Lora 上傳到遠端服務(wù)器。

將采集到的溶解氧數(shù)據(jù)與臺灣衡欣公司的高精度溶氧儀AZ8403 的測試數(shù)據(jù)進行對比?？偣灿涗? 次數(shù)據(jù)，測量結(jié)果如表4 所示。

表4 溶解氧測量數(shù)據(jù)對比Tab. 4 Comparison of dissolved oxygen measurement data

從表4 可以看出，機器魚搭載水質(zhì)參數(shù)傳感器可以完成水質(zhì)參數(shù)采集工作，并且采集到的數(shù)據(jù)與溶氧儀測量的數(shù)據(jù)誤差較小。同時，在長時間的應(yīng)用測試中，機器魚的故障表現(xiàn)為：在電池的電量不足時必須返航而中斷數(shù)據(jù)采集工作，在電量充足的前提下整個系統(tǒng)可以處在正常的工作狀態(tài)。

綜合上所述：機器魚采用本文的復(fù)合避障算法可以成功實現(xiàn)避障并且成功率較高；機器魚在巡游過程中擾動低；機器魚的水質(zhì)參數(shù)采集測試說明機器魚有實際的應(yīng)用價值。

5 結(jié) 論

研制的仿生機器魚以STM32 控制器為核心，搭載GPS 可以進行實時定位，搭載無線通信模塊Lora 可以實現(xiàn)信息的交互，電源管理模塊可以對電量進行實時監(jiān)測和預(yù)警，利用雙關(guān)節(jié)舵機實現(xiàn)前游和轉(zhuǎn)向，利用水泵和水箱實現(xiàn)上浮和下潛。對機器魚進行測試的結(jié)果表明，將紅外測距傳感器網(wǎng)絡(luò)與模糊控制和規(guī)則控制相結(jié)合的復(fù)合避障方法可以有效實現(xiàn)機器魚的三維運動和避障，并且避障成功率較高，在游動過程中擾動性低，同時機器魚還能搭載水質(zhì)傳感器進行水質(zhì)監(jiān)測。