文家燕 ,溫林榮 ,謝廣明,3 ,羅文廣

(1.廣西科技大學(xué) 電氣電子與計(jì)算機(jī)科學(xué)學(xué)院,廣西 柳州,545006;2.廣西科技大學(xué) 廣西汽車零部件與整車技術(shù)重點(diǎn)實(shí)驗(yàn)室,廣西 柳州,545006;3.北京大學(xué) 工學(xué)院,北京,100871)

0 引言

仿生機(jī)器魚參考了自然界魚類的外形和運(yùn)動(dòng)模式,其具備自然魚類運(yùn)動(dòng)性能的部分優(yōu)點(diǎn),如靜音、節(jié)能、靈活等[1],是水下機(jī)器人研究的一個(gè)熱門研究領(lǐng)域。

機(jī)器魚在向目標(biāo)行進(jìn)的過程中會(huì)受到附近流場(chǎng)(即水流速度大小和方向)的影響,使其航向偏離目標(biāo)方向。如果機(jī)器魚不能正確修正自己的航向,它就會(huì)不斷偏離設(shè)定航向,最后丟失對(duì)目標(biāo)的跟蹤。對(duì)航向的修正可以通過轉(zhuǎn)向來進(jìn)行。自然界魚類以身體/尾鰭(body and/or caudal fin propulsion,BCF)模式推進(jìn)時(shí),轉(zhuǎn)向是通過使尾部做不對(duì)稱擺動(dòng)來實(shí)現(xiàn)的。受此啟發(fā),機(jī)器魚也可以通過做不對(duì)稱擺動(dòng)來抵消當(dāng)前水流干擾的影響[2]。實(shí)際上,這是一個(gè)機(jī)器魚的姿態(tài)控制問題。如何實(shí)現(xiàn)航向角快速收斂至目標(biāo)方位角是這個(gè)姿態(tài)控制問題的關(guān)鍵。

姿態(tài)控制主要涉及3 個(gè)自由度的控制,即滾轉(zhuǎn)、俯仰和偏航。大多數(shù)已有研究成果都考慮了仿生機(jī)器魚的單自由度控制問題[3-4]。喻俊志等[5]提出了一種在背腹運(yùn)動(dòng)中進(jìn)行俯仰轉(zhuǎn)彎的控制方法,首先在機(jī)器海豚原型上實(shí)現(xiàn)了翻筋斗。Su等[6]研究了多關(guān)節(jié)機(jī)器魚的快速轉(zhuǎn)彎方法,通過模擬魚類常見的C 形啟動(dòng)動(dòng)作,峰值轉(zhuǎn)動(dòng)角速度約為11.7 rad/s,轉(zhuǎn)動(dòng)精度小于10°。Gong 等[7]建立了一種基于中樞模式發(fā)生器(center pattern generator,CPG)的模糊方法來實(shí)現(xiàn)由胸鰭推進(jìn)的機(jī)器魚的精確橫搖控制。Cao 等[8]開發(fā)了一種用于多鏈路機(jī)器海豚航向控制的自校正模糊方法。Yuan 等[9]提出了一種將控制器和反推控制器相結(jié)合的框架,用于滑游機(jī)器海豚的偏航控制?？v觀以上研究,都沒有涉及機(jī)器魚受到非靜止流場(chǎng)作用時(shí),如何自適應(yīng)地調(diào)整姿態(tài)以保持航向穩(wěn)定。

文中首先對(duì)機(jī)器魚的擺動(dòng)以及與水流的相互作用建立模型,其次根據(jù)動(dòng)力學(xué)模型設(shè)計(jì)控制器,主要用于自身運(yùn)動(dòng)狀態(tài)的感知,實(shí)現(xiàn)航向角信息的反饋。但是并不包含能夠感知流場(chǎng)信息的傳感器,因此機(jī)器魚在航向調(diào)節(jié)時(shí)必須具有平動(dòng)速度,即邊游邊轉(zhuǎn)向?？刂破魇箼C(jī)器魚做相應(yīng)的轉(zhuǎn)向來消除水流的干擾,并使航向角盡快收斂至目標(biāo)方位角。更進(jìn)一步,采用粒子群優(yōu)化(particle swarm optimization,PSO)算法對(duì)CPG 控制器的參數(shù)進(jìn)行優(yōu)化,最終提升機(jī)器魚的快速轉(zhuǎn)向能力和靈活性。

1 機(jī)器魚動(dòng)力學(xué)建模

為了獲得機(jī)器魚各個(gè)關(guān)節(jié)的控制力矩,先對(duì)機(jī)器魚進(jìn)行動(dòng)力學(xué)建模。將機(jī)器魚簡(jiǎn)化為一個(gè)六自由度的平面串聯(lián)連桿系,如圖1 所示,機(jī)器魚的三關(guān)節(jié)剛體模型的每節(jié)連桿被各自的質(zhì)心分成前后2 段。

圖1 三關(guān)節(jié)連桿系剛體機(jī)器魚模型Fig.1 Robotic fish model of three joint-linkage system rigid body

圖中:(xi,yi)為第i根連桿的質(zhì)心;li1為連桿左端與質(zhì)心間的長(zhǎng)度;li2為連桿與質(zhì)心間的長(zhǎng)度;θi為關(guān)節(jié)轉(zhuǎn)角,即連桿與水平方向的夾角[10]。

1.1 機(jī)器魚關(guān)節(jié)動(dòng)力學(xué)模型

根據(jù)拉格朗日力學(xué)建立連桿系的拉格朗日方程。其廣義坐標(biāo)為各關(guān)節(jié)轉(zhuǎn)角θ和連桿質(zhì)心位置的橫縱坐標(biāo)x、y。此處的坐標(biāo)系固定在靜止的環(huán)境中,稱為靜止坐標(biāo)系。關(guān)節(jié)轉(zhuǎn)角θi的確定如圖1 所示,每個(gè)關(guān)節(jié)的中心都落在波動(dòng)曲線上。

拉格朗日函數(shù)

式中:T為機(jī)器魚相對(duì)靜止坐標(biāo)系的動(dòng)能;mi為每根連桿的質(zhì)量;V為機(jī)器魚勢(shì)能,文中只研究機(jī)器魚在一個(gè)水平面內(nèi)的運(yùn)動(dòng)情況,因此不考慮豎直方向的受力情況,勢(shì)能不包含重力勢(shì)能,只有關(guān)節(jié)處的轉(zhuǎn)動(dòng)彈性勢(shì)能;ki為相鄰2 根連桿之間的相對(duì)運(yùn)動(dòng)阻力系數(shù)。

系統(tǒng)的總機(jī)械能通過非保守力的做功來增加和消耗。非保守力包括關(guān)節(jié)的主動(dòng)力、摩擦力和魚體與流體間的粘滯阻力。其運(yùn)動(dòng)方式由魚體波動(dòng)方程(4)描述。方程中h(x,t)指在xoy平面內(nèi)魚體垂直于推進(jìn)方向即y方向的位移。需要指出的是,方程右邊的第1 個(gè)因子表示魚體以零幅值開始擺動(dòng),逐漸增加幅值。根據(jù)魚體波動(dòng)方程與連桿質(zhì)心位置與關(guān)節(jié)角的關(guān)系表達(dá)如方程(5)和(6)所示,其中Li為第i根連桿的總長(zhǎng)度。

從而求出每個(gè)關(guān)節(jié)轉(zhuǎn)角θi隨時(shí)間變化的函數(shù)

將關(guān)節(jié)轉(zhuǎn)角代入拉格朗日方程(8),其中Bi是關(guān)節(jié)之間的阻尼系數(shù)。解得關(guān)節(jié)力矩M1、M2和M3,繪制出3 個(gè)關(guān)節(jié)力矩的曲線如圖2 所示。

圖2 關(guān)節(jié)驅(qū)動(dòng)力矩變化曲線Fig.2 Curves of joint driving torque

以上的建模是正向動(dòng)力學(xué)建模,即已知運(yùn)動(dòng)狀態(tài)求驅(qū)動(dòng)力。要準(zhǔn)確地對(duì)控制系統(tǒng)進(jìn)行仿真還需要建立它的逆向模型,此模型要描述的就是已知關(guān)節(jié)動(dòng)力求關(guān)節(jié)運(yùn)動(dòng),即關(guān)節(jié)電機(jī)轉(zhuǎn)矩,輸出為各關(guān)節(jié)的轉(zhuǎn)角。

由于各連桿之間由關(guān)節(jié)相連,它們之間存在幾何約束關(guān)系為

根據(jù)剛體的質(zhì)心運(yùn)動(dòng)定理,4 根連桿構(gòu)成的機(jī)器魚質(zhì)心的運(yùn)動(dòng)與其所受合力關(guān)系為

式中:Fx為機(jī)器魚所受x方向的合力;Fy為機(jī)器魚所受y方向的合力。

根據(jù)剛體轉(zhuǎn)動(dòng)的歐拉方程,構(gòu)成機(jī)器魚的4 根連桿的轉(zhuǎn)角、質(zhì)心位置與關(guān)節(jié)電機(jī)輸出力矩

其中各等式左邊的第1 項(xiàng)是每根連桿的慣性力,等號(hào)左邊后面幾項(xiàng)是所受相鄰連桿提供的力矩。式中:Fx32為第3 根連桿對(duì)第2 根連桿x方向的作用力;Fy32為第3 根連桿對(duì)第2 根連桿y方向的作用力;Fx23與Fy23則為上述兩力的反作用力。

其中,Fx32、Fy32與Fx23、Fy23可以由第2、3 根連桿的質(zhì)心平動(dòng)表示為

1.2 機(jī)器魚水動(dòng)力學(xué)建模

機(jī)器魚關(guān)節(jié)轉(zhuǎn)動(dòng)使魚體做波狀擺動(dòng),周圍流場(chǎng)則對(duì)機(jī)器魚產(chǎn)生反作用力。為了獲取機(jī)器魚的推進(jìn)力和轉(zhuǎn)向力矩,需要建立機(jī)器魚的水動(dòng)力學(xué)模型。文中采用目前在BCF 模式下[11-13]應(yīng)用最為廣泛的魚類推進(jìn)理論——細(xì)長(zhǎng)體理論為機(jī)器魚的水動(dòng)力學(xué)建模。

1.2.1 機(jī)器魚受水流推進(jìn)力及轉(zhuǎn)向力矩

圖3 為機(jī)器魚的俯視圖。圖中,W為機(jī)器魚質(zhì)心速度,φ為實(shí)際航向角,T為目標(biāo)矢量,?為目標(biāo)航向角,V為水流速度。

圖3 機(jī)器魚及流場(chǎng)環(huán)境狀態(tài)示意圖Fig.3 Diagram of robotic fish and flow field environment state

將作用在魚體上的力L(x,t)分解為平行于前進(jìn)方向的推進(jìn)力和垂直于前進(jìn)方向的偏航力。垂直于前進(jìn)方向的力產(chǎn)生使魚體轉(zhuǎn)向的力矩。根據(jù)細(xì)長(zhǎng)體理論,魚體受到流場(chǎng)的反作用力可以由魚體周圍附加質(zhì)量的動(dòng)量變化率求得。所謂附加質(zhì)量就是魚體做波狀擺動(dòng)時(shí)環(huán)繞魚體的流速被顯著改變的流體質(zhì)量。

附加質(zhì)量沿中脊線的分布由下式確定

式中:ρ為水流密度;a為魚體橫截面的近似橢圓半長(zhǎng)軸。

魚體相對(duì)附加質(zhì)量的速度為v,魚體相對(duì)流場(chǎng)的平均速度為U,則

魚體受到流場(chǎng)的反作用力就是附加質(zhì)量的動(dòng)量變化率,即式(18)與(19)乘積對(duì)時(shí)間的1 階導(dǎo)數(shù)

根據(jù)附加質(zhì)量力計(jì)算公式,分別對(duì)機(jī)器魚魚體的x和y方向積分,即可得到魚體的推進(jìn)力FxL和側(cè)方向受力FyL,即

以質(zhì)心為旋轉(zhuǎn)中心,由附加質(zhì)量力產(chǎn)生的偏航力矩為

式中,h是機(jī)器魚y方向的形變。從上述力與力矩的表達(dá)式可知h須是在魚體長(zhǎng)度范圍內(nèi)的連續(xù)函數(shù)。根據(jù)上一節(jié)機(jī)器魚的剛體力學(xué)模型,在每一時(shí)刻對(duì)其關(guān)節(jié)點(diǎn)進(jìn)行擬合,得到魚體中脊線的波動(dòng)曲線,即h在每一時(shí)刻的函數(shù)值。機(jī)器魚的實(shí)際運(yùn)動(dòng)由自身游動(dòng)和環(huán)境流場(chǎng)的相互作用決定。由于流場(chǎng)速度的不確定性,通常將環(huán)境流場(chǎng)對(duì)魚運(yùn)動(dòng)的作用視為一種干擾。

1.2.2 水流速度變化對(duì)機(jī)器魚航向角的影響

流場(chǎng)速度與方向會(huì)影響機(jī)器魚的姿態(tài),即俯仰角、偏航角和滾轉(zhuǎn)角。其中滾轉(zhuǎn)角主要受魚體軸向外力矩影響,自然流場(chǎng)通常不會(huì)產(chǎn)生這種力矩,故不予考慮。俯仰角受到流場(chǎng)豎直方向分量影響而產(chǎn)生的偏移量可以通過機(jī)器魚的胸鰭來調(diào)節(jié),并且少量的俯仰偏移并不會(huì)使機(jī)器魚的航向受到影響,故流場(chǎng)的偏航力矩是文中的研究重點(diǎn)。

為了研究水流速度對(duì)機(jī)器魚航向角的影響,需要對(duì)機(jī)器魚的航向角進(jìn)行定義。如圖3 所示,為描述水流和機(jī)器魚運(yùn)動(dòng),建立一個(gè)以大地或海床為參考系的坐標(biāo)系,稱為靜止坐標(biāo)系。機(jī)器魚質(zhì)心的速度方向W與坐標(biāo)系x軸的夾角即實(shí)際航向角φ。而目標(biāo)矢量T與坐標(biāo)系x軸的夾角即目標(biāo)航向角?。用矢量V來表示機(jī)器魚外部小范圍流場(chǎng)的速度,則機(jī)器魚的絕對(duì)速度、相對(duì)水流的速度和小范圍流場(chǎng)的速度之間存在

水流提供機(jī)器魚前進(jìn)的推力,也產(chǎn)生阻礙其運(yùn)動(dòng)的阻力。影響機(jī)器魚阻力的因素有流場(chǎng)的密度、朝向來流方向的機(jī)器魚的面積和機(jī)器魚相對(duì)水流的速度。因此,機(jī)器魚所受x方向和y方向的阻力公式為

以及其所受水流產(chǎn)生的阻礙轉(zhuǎn)向的力矩

式中:Cd為阻力系數(shù);CL為阻力矩系數(shù);Sx和Sy為朝向來流方向的面積分量。

由于在實(shí)際控制機(jī)器魚的過程中,傳感器所采集的力學(xué)和運(yùn)動(dòng)學(xué)參數(shù)一般都是相對(duì)機(jī)器魚本身,因此有必要建立描述機(jī)器魚在運(yùn)動(dòng)坐標(biāo)系中的動(dòng)力學(xué)方程。將運(yùn)動(dòng)坐標(biāo)系固連在剛體模型中的第1 根連桿上,根據(jù)運(yùn)動(dòng)參考系中的牛頓-歐拉動(dòng)力學(xué)方程,且僅考慮二維運(yùn)動(dòng)的情況下,有以下描述機(jī)器魚線運(yùn)動(dòng)和角運(yùn)動(dòng)的動(dòng)力學(xué)方程

式中:M是機(jī)器魚質(zhì)量;I是機(jī)器魚相對(duì)運(yùn)動(dòng)坐標(biāo)系z(mì)軸的轉(zhuǎn)動(dòng)慣量;ux和uy是機(jī)器魚整體質(zhì)心相對(duì)運(yùn)動(dòng)坐標(biāo)系的速度。

機(jī)器魚相對(duì)水流的速度在運(yùn)動(dòng)坐標(biāo)系與靜止坐標(biāo)系之間的變換式為

將式(29)代入(24)得到機(jī)器魚質(zhì)心在靜止坐標(biāo)系中的速度,根據(jù)航向角的定義,得到其表達(dá)式為

2 機(jī)器魚姿態(tài)控制系統(tǒng)

2.1 CPG 運(yùn)動(dòng)控制簡(jiǎn)介

CPG 是一類模仿生物控制節(jié)律性運(yùn)動(dòng)的神經(jīng)信號(hào)發(fā)生器的數(shù)學(xué)模型[14]。一個(gè)CPG 單元能夠產(chǎn)生周期性信號(hào),由CPG 單元通過一定的拓?fù)浣Y(jié)構(gòu)連接而成的網(wǎng)絡(luò)能夠產(chǎn)生一系列相位互鎖的周期信號(hào)?？赏ㄟ^改變網(wǎng)絡(luò)連接參數(shù)來實(shí)現(xiàn)對(duì)周期信號(hào)的幅值、頻率和相位的改變。

CPG 控制器輸出的周期信號(hào)具有穩(wěn)定性強(qiáng)、適應(yīng)性強(qiáng)和多模態(tài)等特性,因此被廣泛應(yīng)用于仿生機(jī)器人的控制中。穩(wěn)定性:無論輸出信號(hào)的初始狀態(tài)如何,CPG 振蕩器的輸出信號(hào)在平面相圖中呈現(xiàn)自持振蕩的孤立閉合軌跡,這在控制系統(tǒng)中體現(xiàn)為對(duì)擾動(dòng)的抑制。適應(yīng)性:引入反饋后,CPG 的輸出能夠隨著環(huán)境的變化而做出響應(yīng)來使受控對(duì)象重新穩(wěn)定。多模態(tài):CPG 網(wǎng)絡(luò)里的各單元之間相互聯(lián)系、耦合,調(diào)整連接參數(shù)即可產(chǎn)生不同的周期信號(hào),進(jìn)而控制被控對(duì)象做出不同的運(yùn)動(dòng)形態(tài)。

2.2 CPG 控制機(jī)器魚關(guān)節(jié)運(yùn)動(dòng)

魚類波動(dòng)狀擺動(dòng)的運(yùn)動(dòng)方式可以用一個(gè)以二次函數(shù)為包絡(luò)線的波動(dòng)方程來描述。魚體關(guān)節(jié)的基本運(yùn)動(dòng)是周期性的,只要調(diào)節(jié)好各關(guān)節(jié)之間的相位差就可以生成魚體波。如果采用一系列無相互耦合的周期信號(hào)控制各關(guān)節(jié)的擺動(dòng),就無法自動(dòng)形成相位互鎖,即穩(wěn)定的相位差。需要定期動(dòng)態(tài)矯正偏差帶來的軸向不對(duì)稱,增加算法負(fù)擔(dān)。而CPG 則克服了以上問題,適合用來控制機(jī)器魚的基本運(yùn)動(dòng)[15-16]。

針對(duì)魚類波狀運(yùn)動(dòng)形式,采用具有明顯描述波參數(shù) (振幅和頻率) 的Hopf 振蕩器的CPG 單元。Hopf 振蕩器各參數(shù)的物理意義清晰,其數(shù)學(xué)表達(dá)形式為

式中:zi為第i個(gè)CPG 單元的輸出信號(hào)(以下類推),CPG 輸出信號(hào)控制對(duì)應(yīng)關(guān)節(jié)電機(jī)輸出扭矩;連接系數(shù)wi和常數(shù)ri主要影響的是所在CPG 單元的振蕩頻率和振幅,反映到機(jī)器魚上就是關(guān)節(jié)輸出力矩的大小和頻率;ri的值可以根據(jù)1.1 節(jié)中關(guān)節(jié)動(dòng)力學(xué)模型的計(jì)算結(jié)果確定;φi表示各個(gè)CPG輸出信號(hào)之間的相位差,該值表征了魚體波的形狀;hi1和hi2是耦合系數(shù),表示各個(gè)振蕩器單元之間的耦合強(qiáng)度[17-18],關(guān)系到相位差的收斂速度;偏置bi表征輸出信號(hào)在時(shí)間軸上下的偏移,這個(gè)值決定了機(jī)器魚拐彎動(dòng)作的幅度大小。

2.3 機(jī)器魚姿態(tài)控制系統(tǒng)設(shè)計(jì)

機(jī)器魚本身不能直接檢測(cè)所在流場(chǎng)的速度方向及大小,但是可以通過搭載的運(yùn)動(dòng)傳感器和定位模塊獲取自身的運(yùn)動(dòng)信息。因此機(jī)器魚的實(shí)際航行方向已知。由于魚體的運(yùn)動(dòng)始終是擺動(dòng)著,正常行進(jìn)時(shí)比較環(huán)節(jié)輸出的偏差是個(gè)小幅度波動(dòng)信號(hào),因此需要一個(gè)閾值非線性環(huán)節(jié)來過濾這個(gè)偏差信號(hào)。CPG 控制器的輸入是閾值非線性環(huán)節(jié)過濾后的偏差。

式中:ki為偏置系數(shù);a為 非線性環(huán)節(jié)的閾值[19];α為機(jī)器魚頭部與實(shí)際航向間夾角;λi為此夾角對(duì)偏置bi的修正系數(shù)。

確定了被控對(duì)象和控制器后即可設(shè)計(jì)出完整的控制系統(tǒng)。圖4 為機(jī)器魚姿態(tài)控制系統(tǒng)框圖。系統(tǒng)的輸入為目標(biāo)航向角,輸出為實(shí)際航向角。實(shí)際航向角的反饋,使系統(tǒng)控制形成閉環(huán)。

圖4 機(jī)器魚控制系統(tǒng)框圖Fig.4 Block diagram of robotic fish control system

根據(jù)2.1 節(jié)CPG 控制器特性,CPG 控制器能使閉環(huán)系統(tǒng)穩(wěn)定,最終使機(jī)器魚航向偏差收斂。

3 控制器參數(shù)優(yōu)化

3.1 優(yōu)化指標(biāo)與優(yōu)化對(duì)象

機(jī)器魚在非靜止流場(chǎng)中要有快速調(diào)整姿態(tài)跟蹤目標(biāo)航向的能力,即優(yōu)化指標(biāo)作系統(tǒng)的響應(yīng)時(shí)長(zhǎng),優(yōu)化方向是縮短響應(yīng)時(shí)間。而CPG 控制器各振蕩單元的偏置系數(shù)ki決定了機(jī)器魚姿態(tài)的調(diào)整,進(jìn)而影響航向角偏差的收斂速度。因此,各振蕩器單元的偏置系數(shù)ki就是優(yōu)化的對(duì)象。

3.2 PSO 算法簡(jiǎn)介

PSO算法是一種被廣泛應(yīng)用的高效全局尋優(yōu)算法[20]。PSO 算法的基本原理是將優(yōu)化對(duì)象的一組解看做是一群(群體容量用m標(biāo)記)體積和質(zhì)量為零的質(zhì)點(diǎn),那么解空間(解的維數(shù)標(biāo)記為n)就是粒子飛行的空間。所有粒子的運(yùn)動(dòng)規(guī)律都遵循方程

式中:vij為第i個(gè)粒子的j分量速度;xij為其相應(yīng)的位置;pij為第i個(gè)粒子所尋得的j分量的最優(yōu)解,pgj為粒子群整體的j分量的全局最優(yōu)解;c1和c2為加速常數(shù);r1和r2為[0,1]范圍內(nèi)的隨機(jī)數(shù)。式中,第1 項(xiàng)代表粒子的慣性,慣性權(quán)重因子w越大表示粒子的慣性越大。而慣性越大,搜索全局最優(yōu)解的速度越快,但是收斂速度越慢。第2 項(xiàng)代表粒子受到本身目前搜尋到的最優(yōu)解的影響,趨于靠近此局部最優(yōu)解。第3 項(xiàng)代表粒子受到目前搜尋到的全局最優(yōu)解的影響,趨于靠近全局最優(yōu)解。c1和c2分別表示目前的局部最優(yōu)解和全局最優(yōu)解對(duì)粒子吸引力的放大作用。而隨機(jī)數(shù)r1和r2則是避免搜索陷入局部最優(yōu)解的關(guān)鍵。

3.3 參數(shù)優(yōu)化算法設(shè)計(jì)

機(jī)器魚的剛體模型有3 個(gè)關(guān)節(jié),即包含3 個(gè)振蕩器單元CPG 控制器,因此由3 個(gè)偏差系數(shù)組成解空間。根據(jù)關(guān)節(jié)動(dòng)力學(xué)模型的計(jì)算結(jié)果,3 個(gè)偏置的范圍應(yīng)小于各自對(duì)應(yīng)的關(guān)節(jié)擺動(dòng)幅值。

算法流程如下。

1) 初始化粒子群,設(shè)置群體容量m,每個(gè)粒子的初始位置[x1,x2,x3]=[k10,k20,k30]和速度[v1,v2,v3]=[0,0,0]。

2) 計(jì)算每個(gè)粒子的適應(yīng)度值fit[i]。文中fit[i]的取值是機(jī)器魚目標(biāo)航向角與實(shí)際航向角之差收斂至小于百分之二并持續(xù)2 個(gè)求解周期 (求解周期指的是仿真程序中求解一次微分方程組設(shè)置的時(shí)間跨度) 時(shí)所用時(shí)間。

3) 對(duì)每個(gè)粒子,用其當(dāng)前的適應(yīng)度值fit[i]和自身目前尋得的最大適應(yīng)度值p[i]比較。若fit[i]

4) 對(duì)每個(gè)粒子,用其當(dāng)前的適應(yīng)度值fit[i]和全局極值g[i]比較。若fit[i]

5) 按照式(33)更新粒子的速度和位置。

6) 處理邊界條件。

7) 判斷算法終止條件是否滿足:若是,則結(jié)束算法并輸出優(yōu)化結(jié)果;否則返回步驟2) 。

4 仿真與結(jié)果分析

按照機(jī)器魚控制系統(tǒng)設(shè)計(jì),使用MATLABSimulink 搭建仿真控制系統(tǒng)模型,如圖5 所示。

圖5 Simulink 控制系統(tǒng)仿真模型Fig.5 The control system simulation model in Simulink

機(jī)器魚的基本參數(shù)見表1。文中將展示2 組仿真結(jié)果,形成參數(shù)優(yōu)化的前后對(duì)照。每組仿真根據(jù)機(jī)器魚的初始姿態(tài)不同分為(a)和(b)2 個(gè)仿真。按照?qǐng)D1 所示結(jié)構(gòu)分配各個(gè)連桿的參數(shù)。其中轉(zhuǎn)動(dòng)慣量以各自質(zhì)心為轉(zhuǎn)動(dòng)中心。

表1 機(jī)器魚參數(shù)列表Table 1 List of robotic fish parameters

如圖3 所示,設(shè)置機(jī)器魚航向角目標(biāo)角 φ為26.6°,水流V方向與固定坐標(biāo)系橫坐標(biāo)軸夾角為45°,流速為2 m/s,初始狀態(tài)下機(jī)器魚隨水流運(yùn)動(dòng),因此實(shí)際航向角φ也為45°,速度大小也與水流一致。機(jī)器魚初始姿態(tài)魚頭指向與固定坐標(biāo)系橫坐標(biāo)夾角為分別為?90°和90°。控制目標(biāo)是使機(jī)器魚的實(shí)際航向跟蹤目標(biāo)航向?？刂破鲄?shù)經(jīng)過手動(dòng)調(diào)試,能夠使機(jī)器魚的航向角跟蹤到目標(biāo)航向角。將其設(shè)置為初始參數(shù),見表2。

表2 控制器參數(shù)初始值列表Table 2 List of initial values for controller parameters

仿真得出的機(jī)器魚運(yùn)動(dòng)軌跡和實(shí)際航向變化曲線分別如圖6(其中直線僅表示方向,而不是目標(biāo)與出發(fā)點(diǎn)的連線)和圖7 所示。

圖6 機(jī)器魚航向調(diào)整過程運(yùn)動(dòng)軌跡Fig.6 The robotic fish movement track during the course adjustment process

圖7 機(jī)器魚航向角偏差變化曲線Fig.7 Curves of course angle deviation of robotic fish

設(shè)置穩(wěn)態(tài)跟蹤標(biāo)志為:機(jī)器魚目標(biāo)航向角與實(shí)際航向角之差收斂至小于2%,并持續(xù)2 個(gè)求解周期。根據(jù)此要求得出未優(yōu)化控制器參數(shù)時(shí)穩(wěn)態(tài)時(shí)間為分別為30 s 和25 s。

根據(jù)文中所提方法對(duì)控制器參數(shù)進(jìn)行優(yōu)化,將適應(yīng)度函數(shù)設(shè)置為機(jī)器魚目標(biāo)航向角與實(shí)際航向角之差收斂至2%所用時(shí)間 (以圖6(a)所示初始條件進(jìn)行優(yōu)化)。迭代過程適應(yīng)度函數(shù)變化曲線如圖8 所示。

圖8 適應(yīng)度函數(shù)變化曲線Fig.8 Fitness function changing curve

迭代計(jì)算至結(jié)果穩(wěn)定后輸出優(yōu)化參數(shù)見表3。

表3 優(yōu)化后控制器參數(shù)列表Table 3 Controller parameter list after optimization

使用優(yōu)化參數(shù)后的控制器再次仿真,結(jié)果見圖9 和圖10。

圖9 優(yōu)化后機(jī)器魚航向調(diào)整過程運(yùn)動(dòng)軌跡Fig.9 The trajectory of robotic fish in course adjustment process after optimization

圖10 優(yōu)化后機(jī)器魚航向角偏差變化曲線Fig.10 Curves of course angle deviation of robotic fish after optimization

優(yōu)化后航向角調(diào)節(jié)的穩(wěn)態(tài)時(shí)間分別為20 s 和16 s,較優(yōu)化前調(diào)節(jié)時(shí)間減少了33%和36%,超調(diào)量下降了12%和10%,振蕩頻率明顯變緩?？梢娍刂破鞯母黜?xiàng)性能指標(biāo)都有所提升。

從圖6 中可以看出,機(jī)器魚在水流方向與目標(biāo)航向不一致的情況下,以不同的初始姿態(tài)出發(fā),最終的航向能夠與目標(biāo)航向同向平行。驗(yàn)證了機(jī)器魚控制系統(tǒng)航向穩(wěn)定的有效性。對(duì)比圖8 和圖10,經(jīng)過PSO 算法優(yōu)化后機(jī)器魚航向跟蹤到目標(biāo)航向的時(shí)間縮短,超調(diào)量變小。

5 結(jié)束語(yǔ)

通過建立機(jī)器魚的動(dòng)力學(xué)模型,結(jié)合CPG 控制器,利用航向角反饋,能夠設(shè)計(jì)出解決機(jī)器魚在非靜止流場(chǎng)中的航向穩(wěn)定問題的控制系統(tǒng)。而CPG 控制器的偏置系數(shù)經(jīng)優(yōu)化后可以在一定程度改善機(jī)器魚航向調(diào)整的快速性和平穩(wěn)性,提升了機(jī)器魚的轉(zhuǎn)向靈活性。仿真結(jié)果驗(yàn)證了文中所建模型與優(yōu)化方案的有效性。

此外,由于文中所做仿真的假設(shè)條件是忽略流場(chǎng)短時(shí)間內(nèi)的變化,測(cè)試機(jī)器魚航向控制系統(tǒng)的環(huán)境流場(chǎng)設(shè)置的是確定的水流速度。此仿真無法模擬現(xiàn)實(shí)中有可能出現(xiàn)的水流劇烈變化的情形。要完全解決變流場(chǎng)航向穩(wěn)定,還需對(duì)機(jī)器魚附近流場(chǎng)的變化率范圍與機(jī)器魚所需響應(yīng)速度極限之間的關(guān)系進(jìn)行進(jìn)一步研究。