基于中樞模式發(fā)生器的機(jī)器魚分層閉環(huán)控制方法

史曉虎，文小玲*，涂英杰

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

2. 潛江市吉冠生態(tài)農(nóng)業(yè)有限公司，湖北 潛江 433116

魚類在水中游動(dòng)時(shí)具備靈活性、高效率、低噪音等特點(diǎn)，因此，仿生機(jī)器魚在水質(zhì)監(jiān)測(cè)和水下探測(cè)等領(lǐng)域得到了越來(lái)越廣泛的應(yīng)用［1-5］。

中樞模式發(fā)生器［6-7］（central pattern generator，CPG）是一種基于動(dòng)物節(jié)律運(yùn)動(dòng)控制機(jī)理、仿神經(jīng)元網(wǎng)絡(luò)的控制方法。它以CPG 神經(jīng)元作為底層的運(yùn)動(dòng)控制器，通過(guò)神經(jīng)元之間的相互抑制實(shí)現(xiàn)自激振蕩，產(chǎn)生連續(xù)、穩(wěn)定的節(jié)律控制信號(hào)，從而實(shí)現(xiàn)對(duì)多個(gè)關(guān)節(jié)的運(yùn)動(dòng)控制［8-9］。機(jī)器魚常用的CPG 模型有3 種［10-12］：遞歸振蕩器，相位振蕩器和Hopf 振蕩器。Ikeda 采用基于遞歸振蕩器的CPG網(wǎng)絡(luò)控制多關(guān)節(jié)仿蝠鲼機(jī)器魚的游動(dòng)［13］。Cao等［14］利用相位振蕩器構(gòu)建CPG 網(wǎng)絡(luò)，實(shí)現(xiàn)了胸鰭波動(dòng)推進(jìn)模式的多關(guān)節(jié)機(jī)器魚的游動(dòng)控制。王揚(yáng)威等［15］基于20 個(gè)Hopf 振蕩器構(gòu)建了中心式CPG拓?fù)渚W(wǎng)絡(luò)模型，通過(guò)調(diào)節(jié)振蕩器的幅值和頻率來(lái)實(shí)現(xiàn)仿生機(jī)器魚的多種游動(dòng)模式及模式間的切換控制。王海龍等［16］采用Hopf 振蕩器構(gòu)建最近相鄰耦合的鏈?zhǔn)紺PG 網(wǎng)絡(luò)控制模型，并引入3 個(gè)頂層控制信號(hào)控制機(jī)器人模態(tài)的生成，再通過(guò)CPG網(wǎng)絡(luò)輸出控制信號(hào)，實(shí)現(xiàn)了仿海蟹機(jī)器人的三維游動(dòng)控制。李宗剛等［17］提出了一種將Hopf 振蕩器與模糊控制相結(jié)合的閉環(huán)運(yùn)動(dòng)控制方法，通過(guò)模糊控制器調(diào)節(jié)CPG 模型的控制參數(shù)，改善了機(jī)器魚定深控制的動(dòng)態(tài)與穩(wěn)態(tài)性能。

目前，CPG 網(wǎng)絡(luò)在實(shí)際工程應(yīng)用中多用于解決模態(tài)控制問(wèn)題，對(duì)于更復(fù)雜的避障及路徑規(guī)劃和軌跡跟蹤控制問(wèn)題，僅依靠CPG 網(wǎng)絡(luò)生成節(jié)律控制信號(hào)是不夠的，還需要根據(jù)環(huán)境反饋信息和上層的協(xié)調(diào)控制信息、利用CPG 網(wǎng)絡(luò)生成節(jié)律控制信號(hào)，才能實(shí)現(xiàn)機(jī)器魚的自主柔性游動(dòng)控制，但對(duì)于這方面的研究鮮有報(bào)道。

為實(shí)現(xiàn)仿生機(jī)器魚自主巡航及柔性游動(dòng)的同步控制，本文采用CPG 控制機(jī)器魚的各個(gè)關(guān)節(jié)，通過(guò)引入反饋控制，構(gòu)建包括模態(tài)調(diào)節(jié)層、模態(tài)生成層、運(yùn)動(dòng)控制層和反饋協(xié)調(diào)層的分層閉環(huán)控制器，實(shí)現(xiàn)了機(jī)器魚協(xié)調(diào)自然的游動(dòng)模態(tài)、模態(tài)之間的平滑切換以及自主巡航和避障。

1 機(jī)器魚控制系統(tǒng)總體架構(gòu)

尾鰭推進(jìn)模式下的魚類具有較大推進(jìn)力、游動(dòng)速度較快，胸鰭推進(jìn)模式下的魚類穩(wěn)定性更高。為此，采用三關(guān)節(jié)尾鰭和左右胸鰭協(xié)同推進(jìn)方式，實(shí)現(xiàn)機(jī)器魚的三維游動(dòng)。

仿生機(jī)器魚控制系統(tǒng)總體架構(gòu)如圖1 所示，以STM32F407 單片機(jī)作為主控制器，利用紅外測(cè)距傳感器獲取環(huán)境信息、GPS 和慣性傳感器獲取自身姿態(tài)及位置信息；主控制器對(duì)環(huán)境和自身姿態(tài)信息分析計(jì)算后，輸出5 路PWM 脈沖信號(hào)、控制舵機(jī)轉(zhuǎn)動(dòng)，實(shí)現(xiàn)機(jī)器魚的前進(jìn)、轉(zhuǎn)彎以及上浮下潛；利用水質(zhì)傳感器采集水質(zhì)參數(shù)，如溶解氧、pH值、溫度等；利用Lora 無(wú)線通信模塊，實(shí)現(xiàn)遠(yuǎn)距離無(wú)線傳輸，接收控制命令和將采集到的水質(zhì)參數(shù)數(shù)據(jù)、機(jī)器魚的狀態(tài)信息發(fā)送到上位機(jī)。

圖1 機(jī)器魚控制系統(tǒng)總體架構(gòu)Fig.1 Overall architecture of robot fish control system

2 CPG 建模

2.1 Hopf振蕩器

Hopf 振蕩器控制參數(shù)少，輸出信號(hào)的幅值、頻率相互獨(dú)立，可單獨(dú)控制；穩(wěn)定時(shí)生成的輸出信號(hào)收斂于正弦波，滿足仿魚形機(jī)器人運(yùn)動(dòng)所需控制信號(hào)的特點(diǎn)，并且易于耦合反饋；因此本文選擇Hopf振蕩器為CPG 模型。

Hopf振蕩器的微分方程為：

式中：x?，y?表示對(duì)x、y求導(dǎo)，x為振蕩器的膜電勢(shì)、y為振蕩器的調(diào)節(jié)電勢(shì)；ω、m分別表示振蕩器的頻率和振幅，h為收斂系數(shù)。

2.2 CPG 網(wǎng)絡(luò)模型

自然界中的魚類依靠胸鰭、身體、尾鰭的相互協(xié)調(diào)進(jìn)行自主游動(dòng)。因?yàn)閱蝹€(gè)CPG 只能控制1 個(gè)關(guān)節(jié)，而本機(jī)器魚有5 個(gè)關(guān)節(jié)，所以必須建立多個(gè)CPG 之間的耦合關(guān)系，對(duì)多個(gè)關(guān)節(jié)進(jìn)行協(xié)調(diào)控制，進(jìn)而實(shí)現(xiàn)機(jī)器魚的柔性控制。本文基于Hopf振蕩器，采用雙向鏈?zhǔn)阶罱噜徣躐詈戏绞?，建立的CPG 網(wǎng)絡(luò)模型如圖2 所示，其微分方程為：

圖2 CPG 網(wǎng)絡(luò)模型Fig.2 CPG network model

式中：i表示機(jī)器魚的第i個(gè)CPG，i=1、2、3 代表尾部關(guān)節(jié)的CPG，i=4、5 分別代表左右胸鰭關(guān)節(jié)的CPG；φi為神經(jīng)元間的相位差，bi為第i個(gè)CPG 的偏移；l為耦合系數(shù)，表示CPG 間的抑制關(guān)系。

為驗(yàn)證所構(gòu)建CPG 網(wǎng)絡(luò)模型的有效性，利用MATLAB 軟件對(duì)機(jī)器魚直游、轉(zhuǎn)彎、上浮下潛等模態(tài)以及彼此之間的切換進(jìn)行仿真分析。

收斂系數(shù)h和耦合系數(shù)l都取1，相鄰尾鰭之間的相位差為30°，胸鰭之間的相位差為0°。

直游模態(tài)下，5 個(gè)舵機(jī)的擺動(dòng)頻率均保持一致，3 個(gè)尾鰭舵機(jī)的擺動(dòng)幅值依次增加、相位依次滯后，2 個(gè)胸鰭舵機(jī)的擺動(dòng)幅值和相位都保持一致。通過(guò)改變舵機(jī)擺動(dòng)頻率和幅值實(shí)現(xiàn)機(jī)器魚游動(dòng)速度的切換。機(jī)器魚直游時(shí)的CPG 控制參數(shù)及其輸出波形分別如表1 和圖3 所示。

表1 直游控制參數(shù)Tab.1 Control parameters of straight-line swimming

圖3 直游輸出波形Fig.3 Output waveforms of straight-line swimming

給尾部舵機(jī)擺動(dòng)增加1 個(gè)偏移即可實(shí)現(xiàn)轉(zhuǎn)彎。3 個(gè)尾鰭舵機(jī)的偏移依次增加，并且偏移的正負(fù)決定了機(jī)器魚轉(zhuǎn)彎的方向，偏移為正表示向右轉(zhuǎn)彎。機(jī)器魚轉(zhuǎn)彎時(shí)的CPG 控制參數(shù)及其輸出波形分別如表2 和圖4 所示。

表2 轉(zhuǎn)彎控制參數(shù)Tab.2 Control parameters of turning

圖4 轉(zhuǎn)彎輸出波形Fig.4 Output waveforms of turning

給胸鰭舵機(jī)擺動(dòng)增加1 個(gè)偏移即可實(shí)現(xiàn)上浮下潛。2 個(gè)胸鰭舵機(jī)的偏移保持一致，并且偏移的正負(fù)決定了機(jī)器魚上浮下潛的方向，偏移為正表示上浮。機(jī)器魚上浮下潛時(shí)的CPG 控制參數(shù)及其輸出波形分別如表3 和圖5 所示。

表3 上浮下潛控制參數(shù)Tab.3 Control parameters of upstroke and downstroke in swimming

圖5 上浮下潛輸出波形Fig.5 Output waveforms of upstroke and downstroke in swimming

由上述仿真結(jié)果可以看出，通過(guò)改變控制參數(shù)ωi、mi、bi可以生成多種波形。當(dāng)參數(shù)突變時(shí)、CPG 的波形仍然比較平滑，并能很快達(dá)到新的穩(wěn)定狀態(tài)，因此可實(shí)現(xiàn)機(jī)器魚直游、轉(zhuǎn)彎及上浮下潛等多個(gè)模態(tài)以及彼此之間的平滑切換，進(jìn)而實(shí)現(xiàn)對(duì)機(jī)器魚的柔性控制，驗(yàn)證了所提CPG 網(wǎng)絡(luò)模型的有效性。

3 基于CPG 的分層閉環(huán)控制方法

利用CPG 控制能夠?qū)崿F(xiàn)各個(gè)模態(tài)間的平滑切換，但是缺乏靈活適應(yīng)環(huán)境的能力，為了在復(fù)雜水下環(huán)境中實(shí)現(xiàn)機(jī)器魚的自主避障，提出基于CPG的分層閉環(huán)控制方法。其總體架構(gòu)如圖6 所示，包括模態(tài)調(diào)節(jié)層、模態(tài)生成層、運(yùn)動(dòng)控制層、反饋協(xié)調(diào)層，進(jìn)行機(jī)器魚路徑規(guī)劃的同時(shí)、完成多個(gè)舵機(jī)的協(xié)調(diào)控制。

圖6 運(yùn)動(dòng)控制總體架構(gòu)Fig.6 Overall architecture of motion control

圖6 中的m、ω、b分別表示用于生成CPG 節(jié)律信號(hào)的模態(tài)控制參數(shù)：幅值、頻率、偏移，z表示用于生成PWM 波形的節(jié)律信號(hào)，d、θ0、e分別表示機(jī)器魚與障礙物間的距離、機(jī)器魚的實(shí)際航向以及實(shí)際航向與規(guī)劃航向間的偏差角。

模態(tài)調(diào)節(jié)層控制流程如圖7 所示，完成路徑規(guī)劃及生成模態(tài)控制信號(hào)。其中，采用將粒子群算法和人工勢(shì)場(chǎng)法相結(jié)合的路徑規(guī)劃方法，首先利用粒子群算法根據(jù)已知環(huán)境信息規(guī)劃出一條初始路徑，再利用人工勢(shì)場(chǎng)法根據(jù)反饋層獲取的環(huán)境信息進(jìn)行實(shí)時(shí)避障和路徑更新，然后根據(jù)規(guī)劃出的路徑以及反饋的信息采用模糊控制方法生成模態(tài)控制信號(hào)。

圖7 模態(tài)調(diào)節(jié)層控制流程Fig.7 Control flow of modal adjustment layer

3 個(gè)模糊控制器分別用于控制速度、偏航角和俯仰角。其中，速度控制器的輸入為機(jī)器魚與障礙物間的距離d，輸出為速度系數(shù)δv；偏航角和俯仰角控制器的輸入為規(guī)劃航向θ及機(jī)器魚的實(shí)際航向θ0和規(guī)劃航向θ間的偏差角e在水平和豎直方向的分量，分別為偏航角θY、偏航角偏差eY、俯仰角θP、俯仰角偏差eP，輸出分別為偏航角系數(shù)δY、俯仰角系數(shù)δP。偏航角控制器和俯仰角控制器的模糊控制規(guī)則完全一樣，只有輸入、輸出不同，速度控制器和偏航角控制器模糊控制規(guī)則如圖8 所示。

圖8 模糊控制規(guī)則：（a）速度控制器，（b）偏航角控制器Fig.8 Fuzzy control rules：（a）velocity controller，（b）yaw angle controller

經(jīng)過(guò)模糊推理后，利用重心法將輸出清晰化，便可求得速度系數(shù)δv、偏航角系數(shù)δY和俯仰角系數(shù)δP，進(jìn)而得到模態(tài)控制參數(shù)。模態(tài)控制參數(shù)與三個(gè)系數(shù)的關(guān)系如下：

式中，bY、bP分別為尾鰭、胸鰭偏置，km、kω、kY、kP分別為幅值因子、頻率因子、航向角因子、俯仰角因子，cm、cω分別為幅值和頻率的最小值。

模態(tài)生成層（即CPG 網(wǎng)絡(luò)模型）根據(jù)模態(tài)控制信號(hào)生成相應(yīng)的CPG 輸出信號(hào)，并將其放大后輸出節(jié)律信號(hào)z，此外還可接受反饋協(xié)調(diào)層的調(diào)節(jié)，建立機(jī)器魚實(shí)際航向與規(guī)劃航向間的偏差角e與CPG 模型間的耦合，具有反饋機(jī)制的CPG 網(wǎng)絡(luò)模型微分方程為：

式中，ri為第i個(gè)CPG 的反饋值、與e有關(guān)，μ為反饋信號(hào)的影響系數(shù)，λ為輸出信號(hào)的放大系數(shù)，yi為CPG 輸出，z為模態(tài)生成層輸出。

運(yùn)動(dòng)控制層根據(jù)節(jié)律信號(hào)生成5 路PWM 控制信號(hào)去控制各個(gè)關(guān)節(jié)的舵機(jī)，實(shí)現(xiàn)多關(guān)節(jié)機(jī)器魚的游動(dòng)控制。

反饋協(xié)調(diào)層將多個(gè)傳感器組成的傳感器網(wǎng)絡(luò)獲取的環(huán)境信息及自身狀態(tài)信息，如與障礙物的距離d、實(shí)際航向與規(guī)劃航向間的偏差角e反饋給其它層。

4 仿真與實(shí)驗(yàn)

4.1 仿真分析

首先，利用MATLAB 搭建一個(gè)單位為70×70×70 的三維空間，設(shè)置山峰障礙物表示已知環(huán)境、正方體障礙物表示未知環(huán)境，設(shè)置起點(diǎn)坐標(biāo)為（5，5，5）、設(shè)置終點(diǎn)坐標(biāo)為（49，41，30）。路徑規(guī)劃結(jié)果如圖9 所示，可以看出利用粒子群算法可以在已知環(huán)境下生成一條較短路徑、利用人工勢(shì)場(chǎng)法可以避開(kāi)這條路徑上新出現(xiàn)的未知障礙物，到達(dá)目標(biāo)點(diǎn)。

圖9 路徑規(guī)劃Fig.9 Route planning

然后，用7 組隨機(jī)離散值表示傳感器檢測(cè)信號(hào)，模擬機(jī)器魚周圍環(huán)境和自身姿態(tài)信息，根據(jù)傳感器信息進(jìn)行路徑規(guī)劃后、得到模糊控制器的輸入和輸出信號(hào)如圖10 所示。

圖10 模糊控制器：（a）輸入，（b）輸出Fig.10 Fuzzy controller：（a）inputs，（b）outputs

從輸入信號(hào)可以看出，4 s 和6 s 時(shí)的d一樣大、θY一樣大、并都小于0，eY都小于0，并且4 s 時(shí)要更小，表明4 s 和6 s 時(shí)距離障礙物一樣近、速度應(yīng)相同、并都需要左轉(zhuǎn)，但4 s 時(shí)航向更偏左，所以4 s 所需左轉(zhuǎn)的角度要比6 s 小。輸出結(jié)果中，δv在4 s 和6 s 時(shí)相等，δY在4 s 和6 s 都小于0、并且6 s時(shí)更小，符合預(yù)期。因此，對(duì)波形分析得出該分層閉環(huán)控制方法可以完成自動(dòng)調(diào)整控制參數(shù)、實(shí)現(xiàn)機(jī)器魚的避障及自動(dòng)巡航。

4.2 機(jī)器魚游動(dòng)實(shí)驗(yàn)

首先，在現(xiàn)場(chǎng)進(jìn)行機(jī)器魚的直游、轉(zhuǎn)彎模態(tài)的實(shí)現(xiàn)及切換測(cè)試。當(dāng)設(shè)置CPG 參數(shù)為表2 中第一組控制參數(shù)時(shí)，實(shí)驗(yàn)結(jié)果如圖11（a）所示，機(jī)器魚處于直游模態(tài)，機(jī)器魚的游動(dòng)速度約為0.35 m/s；當(dāng)設(shè)置CPG 參數(shù)為表2 中第四組控制參數(shù)時(shí)，實(shí)驗(yàn)結(jié)果如圖11（b）所示，機(jī)器魚處于轉(zhuǎn)彎模態(tài)，機(jī)器魚的轉(zhuǎn)彎半徑約為0.32 m；當(dāng)1~4 s 設(shè)置CPG 參數(shù)為表2 中第一組控制參數(shù)，5~8 s 設(shè)置CPG 參數(shù)為表2 中第四組控制參數(shù)時(shí)，機(jī)器魚模態(tài)切換實(shí)驗(yàn)結(jié)果如圖11（c）所示，由圖可以看出機(jī)器魚在前4 s為直游模態(tài)、后4 s 因偏移為負(fù)而進(jìn)入向左轉(zhuǎn)彎模態(tài)，機(jī)器魚可以做到模態(tài)的快速切換，并且過(guò)渡過(guò)程平滑自然。上述三組實(shí)驗(yàn)表明，機(jī)器魚可以根據(jù)控制參數(shù)實(shí)現(xiàn)相應(yīng)的模態(tài)以及模態(tài)間的切換，并且機(jī)器魚在游動(dòng)游動(dòng)過(guò)程中姿態(tài)協(xié)調(diào)自然，模態(tài)切換過(guò)程快速、過(guò)渡平滑。

圖11 機(jī)器魚游動(dòng)模態(tài)：（a）直線游動(dòng)，（b）轉(zhuǎn)彎游動(dòng)，（c）模態(tài)切換Fig.11 Swimming modal of robotic fish：（a）straight-line swimming，（b）turning，（c）modal switching

然后，進(jìn)行機(jī)器魚的避障及自動(dòng)巡航測(cè)試。

利用現(xiàn)場(chǎng)水域的左側(cè)臺(tái)階作為單側(cè)障礙物，目標(biāo)點(diǎn)在水域右上方，機(jī)器魚歷時(shí)4 s、可成功避開(kāi)障礙物，并向目標(biāo)方向游動(dòng)，如圖12（a）所示。利用現(xiàn)場(chǎng)水域的兩側(cè)臺(tái)階構(gòu)成L 型障礙物，目標(biāo)點(diǎn)在水域上方，機(jī)器魚歷時(shí)6 s、可成功避開(kāi)障礙物，并向目標(biāo)方向游動(dòng)，如圖12（b）所示。

圖12 避障實(shí)驗(yàn)結(jié)果：（a）單側(cè)障礙物，（b）L 型障礙物Fig.12 Obstacle avoidance experimental results：（a）unilateral obstacle，（b）L-shaped obstacle

在現(xiàn)場(chǎng)水域設(shè)置起點(diǎn)和終點(diǎn)，如圖13（a）所示，機(jī)器魚從起點(diǎn)出發(fā)，向終點(diǎn)游動(dòng)的圖像如圖13（b）所示，機(jī)器魚可以規(guī)劃出從起點(diǎn)到終點(diǎn)的路徑，并沿規(guī)劃路徑游動(dòng)，到達(dá)終點(diǎn)，驗(yàn)證了所提路徑規(guī)劃方法及軌跡跟蹤方法的有效性。

圖13 路徑規(guī)劃及軌跡跟蹤實(shí)驗(yàn)結(jié)果：（a）起點(diǎn)和終點(diǎn)，（b）游動(dòng)路線Fig.13 Path planning and trajectory tracking experimental results：（a）start and end points，（b）swimming route

3 結(jié) 論

針對(duì)機(jī)器魚的控制問(wèn)題，構(gòu)建了基于Hopf 振蕩器的CPG 網(wǎng)絡(luò)控制模型，并進(jìn)一步提出了基于CPG 的分層閉環(huán)控制方法。最后，對(duì)該方法進(jìn)行了仿真和現(xiàn)場(chǎng)測(cè)試，測(cè)試結(jié)果表明機(jī)器魚在基于CPG 的分層閉環(huán)控制方法下不僅可以實(shí)現(xiàn)多個(gè)協(xié)調(diào)自然的游動(dòng)模態(tài)及模態(tài)間的平滑切換、而且可在復(fù)雜環(huán)境下進(jìn)行自主巡游及避障，驗(yàn)證了所提控制方法的可行性和有效性。