一種雙關節(jié)舵機驅動的水質監(jiān)測仿生機器魚三維避障方法

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

（武漢工程大學 電氣信息學院，武漢430205）

水質監(jiān)測仿生機器魚作為一種靈活全方位的監(jiān)測方法，成為近幾年國內外研究的熱點。仿生機器魚具備仿生學外形，不僅成本低、便攜性好，而且可以有效地減少對環(huán)境的破壞和影響，提高監(jiān)測結果的準確性，因此將仿生機器魚應用于水質監(jiān)測有很大的優(yōu)勢。但是，水下環(huán)境未知且復雜，需要設計合理、高效的避障算法讓機器魚快速、安全完成水質監(jiān)測任務。在仿生機器魚避障傳感器的選用及避障算法方面，國內外專家和學者開展了很多的研究。

在避障傳感器方面，文獻[1]采用了一個水下聲吶系統(tǒng)作為避障傳感器；文獻[2]采用了紅外傳感器作為避障傳感器。聲吶傳感器和紅外傳感器的本質相同，都是利用是否產生反射聲波或者光波來判斷前方是否有障礙物，雖然可以完成避障，但是無法檢測障礙物的距離，也就沒有辦法做到對機器魚進行更加精準的運動控制。

韓國中南國立大學（Chonnam National University）[3]將紅外傳感器和掃描紅外傳感器相結合，利用掃描紅外傳感器識別障礙物的形狀，在一定程度上提高了運動控制的精確度，但是對于圖像形狀的分析較為復雜，并且會占用較多處理器的資源。在避障算法方面，文獻[4]提出了一種基于快速擴展隨機樹（RRT）的避障算法，其在傳統(tǒng)RRT 算法的基礎上減少了節(jié)點的數(shù)量，并且增加了一個偏置函數(shù)，加快了搜索的速度，但是隨機樹有可能無法找到終止條件；文獻[5]針對于三維水下環(huán)境，提出了一種基于生物激勵神經網絡的避障算法，將神經元與水下網格地圖位置一一對應，利用神經元的活動值完成水下三維避障，并通過仿真結果驗證了該算法的有效性，但是神經網絡參數(shù)的調試以及訓練過程具有不可控性，且過程較為漫長。

本文對于機器魚的水下三維避障采用了計算量較小、魯棒性強、實時性好的模糊控制算法。首先從通過分析水質監(jiān)測機器魚在水下的上浮、下潛、前進和轉向運動方式，充分考慮水質監(jiān)測所需要的穩(wěn)定條件，得出機器魚的運動控制方法，并進行仿真建模與分析。

1 水質監(jiān)測仿生機器魚的機械結構及工作流程

1.1 水質監(jiān)測仿生機器魚的機械結構

水質監(jiān)測仿生機器魚的整體裝配實物圖如圖1所示，工程圖具體尺寸如圖2所示。

圖1 仿生機器魚實物圖Fig.1 Bionic robotic fish in kind

圖2 仿生機器魚的結構Fig.2 Struture of bionic robotic fish

1.2 水質監(jiān)測仿生機器魚的工作流程

水質監(jiān)測仿生機器魚的工作流程如圖3所示。首先將機器魚放置在水面，由終端（手機或者電腦）向機器魚的無線模塊發(fā)送目標位置，機器魚進行目標位置與自身位置比對后游向目標點，到達目標地點通過傳感器采集水質參數(shù)。由于水下通信受限，因此先將水質參數(shù)存儲在內存卡中，采集結束后上浮至水面，最后通過無線模塊將數(shù)據(jù)發(fā)送到終端，這樣就完成了一次采集。由于水質數(shù)據(jù)短期內不會發(fā)生急劇的變化，因此每隔一段時間多次采集就可以實現(xiàn)對目標位置水域水質參數(shù)的監(jiān)測。

圖3 水質監(jiān)測仿生機器魚的工作示意圖Fig.3 Water quality monitoring work diagram of bionic robotic fish

通過工作示意圖可以看出，避障貫穿了整個監(jiān)測過程，成功避障是完成水質監(jiān)測的關鍵前提條件。

2 水質監(jiān)測仿生機器魚的運動模式

仿生機器魚在進行水質監(jiān)測的過程中，其運動方式分為前游、轉向、上浮和下潛4 種。

2.1 機器魚的前進與轉向

2.1.1 前游

機器魚采用尾鰭雙關節(jié)推進模式，用舵機作為動力源。相比于伺服電機、步進電機等，舵機具有穩(wěn)定性好、激活和響應迅速、結構和控制簡單的優(yōu)點。采用微控制器產生PWM 信號，并通過改變PWM 脈沖的寬度就可使舵機實現(xiàn)0°～180°旋轉。

采用雙關節(jié)的結構，既可保證機器魚實現(xiàn)二維平面運動又能最大程度地降低機器魚結構和控制的復雜程度。機器魚尾部擺動示意圖如圖4所示。在機器魚前進過程中，2 個關節(jié)擺動角度的幅值為正弦函數(shù)，幅值不同，周期不同，第2 關節(jié)擺動角度幅值的正弦函數(shù)相位滯后于第1 關節(jié)正弦函數(shù)的相位180°。

圖4 機器魚前進時尾部擺動示意圖Fig.4 A diagram of tail swing of the robotic fish when going forward

忽略外界環(huán)境的影響，對雙關節(jié)機器魚進行運動分析可以得到關節(jié)擺動角度的數(shù)學模型為

式中：A1為第1 關節(jié)擺動的振幅；T1為第1 關節(jié)擺動的周期；A2為第2 關節(jié)擺動的振幅；T2為第2 關節(jié)擺動的周期。

根據(jù)對鰉科魚類鯉魚的實際觀察，雙關節(jié)機器魚在無障礙物正常游動時，α 的幅值為20°，β 的幅值為40°，周期均為0.25 s[6]。由此可以計算出第1 關節(jié)舵機的最小轉動速度為5.5851 r/s；第2 關節(jié)舵機的最小轉動速度為11.1701 r/s。但是，根據(jù)舵機具體參數(shù)，其在4.8 V 電壓下最大空載轉動速度為7.5000 r/s；6.0 V 電壓下最大空載轉動速度為8.7266 r/s。由此可知，實際的舵機無法滿足第2 關節(jié)舵機的轉動速度要求，為此選擇延長舵機的轉動周期，使2 個舵機能夠在實際參數(shù)條件下滿足擺動幅值條件的同時保持周期一致，并考慮環(huán)境阻力，得出周期為0.35 s，采用6.0 V 電壓供電，在舵機速度無法改變的情況下，可通過增加小的延時來使周期為0.35 s，最后可得機器魚以正常速度直線游動時各關節(jié)的實際數(shù)學模型為

關節(jié)角度的波形如圖5所示。

圖5 正常速度關節(jié)角度波形Fig.5 Normal velocity joint angle waveform

2.1.2 轉向

機器魚的轉向與前游的原理類似，據(jù)觀察鰉科魚類鯉魚在實際轉彎時，機器魚以很低的角速度和恒定線速度轉彎，其中α 的最大幅度約為45°，β 的最大幅度約為60°，周期均為0.3 s[1]。綜合實際轉彎情況和機器魚前游計算得出的參數(shù)，將周期設定為0.35 s，在轉向時，α 根據(jù)障礙物的位置固定為對應的角度，數(shù)學模型為

式中：k根據(jù)轉向情況固定為一定角度，其范圍為［-45°，45°］。

2.2 上浮與下潛

目前控制機器魚上浮與下潛的方法主要分為改變重心法和改變重力法兩類?？紤]到機器魚在采集水質參數(shù)時，整個裝置應該保持靜止，因改變重心法需要機器魚一直產生動力來克服浮力或者改變重力來完成下潛和上浮，而改變重力法可以通過改變自身重力來與浮力相平衡使機器魚處于靜止狀態(tài)，以提高水質參數(shù)的準確性，因此采用改變重力法。

通過控制2 個水泵（進水泵和出水泵）向水箱抽水和排水來改變機器魚本身的重力實現(xiàn)機器魚的上浮下潛，其控制流程如圖6所示。

圖6 機器魚上浮下潛控制流程Fig.6 Robot fish snorkeling and diving control flow chart

為了實現(xiàn)機器魚上浮下潛的精準控制，采用了PWM 作為控制信號的水泵，可以最大限度保持整個裝置的穩(wěn)定性，從而成功避障。根據(jù)模糊控制算法，當需要上浮或者下潛時，單片機根據(jù)需要上浮或下潛的距離產生PWM 信號去控制排水水泵或者抽水水泵，并且PWM 信號在機器魚快要上浮或者下潛到所需距離時，占空比逐漸減小以減慢上浮或者下潛的速度，水泵PWM 信號控制仿真如圖7所示。

圖7 水泵PWM 信號控制仿真波形Fig.7 Pump PWM control simulation waveform

3 仿生機器魚的模糊控制策略

機器魚搭載5 個紅外測距傳感器，分別位于機器魚魚頭位置的正前方（FIRS）、左側（LIRS）、右側（RIRS）、上側（TIRS）以及下側（BIRS），其中上、下側的紅外測距傳感器都向前傾斜一定的角度；左側和右側紅外測距傳感器向內傾斜一定的角度，安裝位置如圖8所示。

圖8 紅外測距傳感器安裝示意圖Fig.8 Infrared ranging sensor installation diagram

根據(jù)紅外測距傳感器的配置，以及水質監(jiān)測的實際要求，設計了3 個模糊規(guī)則庫，分別為速度模糊規(guī)則庫、轉角模糊規(guī)則庫以及上浮下潛模糊規(guī)則庫。

3.1 速度模糊規(guī)則

將5 個紅外測距傳感器測得的最小值表示為

1=min｛FIRS，LIRS，RIRS，TIRS，BIRS｝

其中：論域為［10，80］；模糊子集為｛遠（F），中（M），近（N），很近（NT）｝；輸出速度系數(shù)V的模糊子集為｛VN（正常速度），VM（中速），VS（慢速），VSM（最慢速）｝；速度系數(shù)V的論域為［1，0.8，0.5，0.2］。根據(jù)機器魚的尺寸規(guī)定一個安全距離為60 cm，實際的最大檢測距離設定為80 cm，當>60 cm（對應模糊子集為：遠（F））時，機器魚按照式（2）對應的擺動數(shù)學模型所對應的速度運動（VN（正常速度）），其它速度以式（2）為基礎做相應改變即可，速度系數(shù)模糊規(guī)則如表1所示。

表1 速度模糊規(guī)則Tab.1 Speed fuzzy rule

3.2 轉角模糊規(guī)則

將左側（LIRS）紅外測距傳感器與右側（RIRS）紅外測距傳感器所測距離的差值表示為

ε＝LIRS-RIRS

根據(jù)左右兩側紅外測距傳感器的檢測范圍為［10，80］，再加上一定的裕度，可得其論域為［-80，80］。當ε 為正值時，其值越大則表明障礙物離機器魚右側越近；當ε 為負值時，其值越小則表明障礙物離機器魚左側越近。當出現(xiàn)左側（LIRS）與右側（RIRS）紅外測距傳感器所測距離相等的情況時，有兩種情況：一是障礙物位于兩傳感器中間；二是前方沒有障礙物，這時需要借助于前側（FIRS）傳感器輔助判斷。

ε 的模糊子集為｛左遠（LF），左中（LM），左近（LN），相等（EQ），右近（RN），右中（RM），右遠（RF）｝；輸出轉向角度Φ 的模糊子集為｛NB（負大），NM（負中），NS（負小），Z（零），PS（正?。?，PM（正中），PB（正大）｝，根據(jù)第2 節(jié)的轉向運動模式，轉向角度α 的論域為［-45°，-30°，-15°，0°，15°，30°，45°］；轉向角度模糊規(guī)則庫如表2所示。

表2 轉向角度模糊規(guī)則Tab.2 Steering angle fuzzy rule

3.3 上浮下潛模糊規(guī)則

上浮下潛避障與前游避障不同，上浮下潛避障時，應該讓機器魚加速游動，這樣才能更有利于避開障礙物；而前游避障時，則需要減速游動，避免速度過快來不及轉向而撞到障礙物；當兩者同時發(fā)生時，應該減速[7]。在只需要上浮下潛避障時，考慮到水質監(jiān)測的穩(wěn)定性要求，機器魚選用正常速度游動，不采用加速措施；考慮到機器魚的高度以及上側和下側傳感器傾斜的角度，設定上側和下側的安全距離為40 cm，將上側（TIRS）和下側（BIRS）紅外測距傳感器的實際最大測量距離設置為80 cm，這樣設置的目的是當上側與下側紅外傳感器的測量值中一個小于安全距離40 cm，而另一個在40～80 cm 之間時，可以計算出上浮或者下潛的距離，其論域為［10，80］，模糊子集為｛上遠（TF，>40 cm），上近（TN，<40 cm），下遠（BF，>40 cm），下近（BN，<40 cm）｝；上浮下潛對應模糊規(guī)則如表3所示。

表3 上浮下潛模糊規(guī)則Tab.3 Snorkeling and diving fuzzy rule

4 仿真結果

利用Matlab 進行仿真，設定一個10×10×10 的三維空間，在其中設置有密集障礙物以及單個障礙物，圖中紅色邊框的方框為單個障礙物，共有4 個，其余方塊是密集障礙物，設定一個起點（圖中紅色圓點），一個檢測點以及一個位于檢測點上方的回傳數(shù)據(jù)點（圖中均為綠色圓點），紅色實線是機器魚運動的軌跡，機器魚首先從起點游動到檢測點，檢測完成后上浮至水面回傳數(shù)據(jù)，數(shù)據(jù)回傳完畢后返回起點，這樣就完成了一次水質檢測的仿真，仿真結果如圖9所示，三視圖分別如圖10、圖11 以及圖12所示。

圖9 三維避障仿真圖Fig.9 3D obstacle avoidance simulation

圖10 三維避障仿真正視圖Fig.10 Positive view of 3D obstacle avoidance

圖11 三維避障仿真俯視圖Fig.11 Top view of 3D obstacle avoidance

圖12 三維避障仿真右視圖Fig.12 Right view of the 3D obstacle avoidance

從仿真實驗結果可以看出，在機器魚完成一次水質檢測仿真過程中，機器魚可以通過轉向、上浮以及下潛運動方式避開障礙物。仿真結果表明，采用本文的模糊控制避障算法，無論對于密集還是單個障礙物，機器魚都可以有效地避開障礙物并到達目的地。

5 結語

通過對機器魚運動模式的研究，結合雙關節(jié)舵機的驅動原理，得出機器魚運動的數(shù)學模型；通過對5 個位置的紅外測距傳感器數(shù)據(jù)進行綜合分析，得出速度、轉角以及上浮下潛的模糊控制策略；最后在Matlab 中搭建三維空間，設置單個和密集的障礙物，實現(xiàn)了機器魚在三維空間中的避障算法，并且模擬了機器魚檢測一次水質數(shù)據(jù)的路徑。仿真結果驗證了所提出的基于模糊控制的三維避障方法的可行性。