一種雙關節機器魚的設計與實現

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

武漢工程大學電氣信息學院，湖北 武漢430205

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

鑒于尾部結構和控制方法的優缺點分析，本文提出一種雙關節仿生機器魚，主要創新點如下：

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

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

1 機器魚外形結構

根據魚類的體型體態，仿生機器魚的推進方式可以分為兩大類：利用魚體和尾鰭推進（body and/or caudal fin，BCF）的推進方式，利用中間鰭或對鰭推進（median and/or paired fin，MPF）的推進方式［12］。BCF 與MPF 相比，游動效率更高，且加速和啟動性能良好，機動性也更高［13］，因此本文采用BCF 推進方式，并采用雙關節的推動結構。

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

借鑒鳙魚的體態，采用雙關節舵機驅動的機器魚3D 模型結構如圖1（b）所示。

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

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

圖1 機器魚相關圖像：（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 動力學模型

基于魚體波運動曲線方程，忽略環境因素的影響，雙關節機器魚的運動可抽象為以下數學模型：

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

2.2 游動控制

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

圖2 機器魚雙關節擺動模型：（a）直線游動，（b）轉向Fig. 2 Double joint swing models of robotic fish：（a）linear swimming，（b）turning

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

2.3 轉向控制

機器魚轉向時尾部的運動狀態與游動時不同，以機器魚右轉為例，雙關節擺動模型如圖2（b）所示。

轉向時，以機器魚軸線作為兩個關節的擺動軸線，則K= 0；且兩個關節均在機器魚軸線一側擺動，用轉向系數kf代替振幅系數Kα，在式（1）的基礎上，得到轉向時的數學模型如下：

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

2.4 避障算法

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

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

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

基于游動、轉向、上浮、下潛控制原理，以及傳感器的布局，提出一種基于模糊控制的復合三維避障方法。對于速度系數和轉向角度，采用模糊控制，對于上浮下潛，采用建立規則庫來實現避障。整個復合避障控制器的結構如圖4 所示。

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

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

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

表1 速度系數模糊規則Tab. 1 Fuzzy rules of speed coefficient

ε是轉向系數模糊控制器的輸入量，其值為

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

表2 轉向角度模糊規則Tab.2 Fuzzy rules of steering angle

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

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

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

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

綜合速度系數、轉向系數模糊控制和上浮下潛的規則控制方法得到最終的復合避障方法。

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

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

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

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

3 控制系統設計

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

圖6 控制系統總體框架圖Fig. 6 Overall framework of control system

控制系統的軟件由主程序、運動控制與避障程序、電源管理程序、PWM控制信號產生程序等組成。

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

圖7 控制系統電路原理圖Fig. 7 Schematic diagram of control system circuit

3.1 電源管理程序

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

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

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

機器魚執行一次任務所需的最小電量為：

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

3.2 PWM 信號產生程序

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

3.3 運動控制與避障程序

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

檢測機器魚是否到達預定點，若到達預定點，則執行水質采集任務，否則繼續判斷是否存在障礙物，以此循環，直到到達預定點。

4 樣機測試及結果分析

制作樣機進行測試，樣機內部構成如圖9所示。

圖9 樣機內部圖Fig. 9 Prototype internal diagram

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

4.1 避障測試

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

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

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

4.2 巡游測試

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

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

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

4.3 實際應用測試

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

將采集到的溶解氧數據與臺灣衡欣公司的高精度溶氧儀AZ8403 的測試數據進行對比。總共記錄5 次數據，測量結果如表4 所示。

表4 溶解氧測量數據對比Tab. 4 Comparison of dissolved oxygen measurement data

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

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

5 結 論

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