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

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

0 引言

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

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

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

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

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

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

圖1 三關(guān)節(jié)連桿系剛體機(jī)器魚(yú)模型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ī)器魚(yú)關(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ī)器魚(yú)相對(duì)靜止坐標(biāo)系的動(dòng)能;mi為每根連桿的質(zhì)量;V為機(jī)器魚(yú)勢(shì)能,文中只研究機(jī)器魚(yú)在一個(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ò)非保守力的做功來(lái)增加和消耗。非保守力包括關(guān)節(jié)的主動(dòng)力、摩擦力和魚(yú)體與流體間的粘滯阻力。其運(yùn)動(dòng)方式由魚(yú)體波動(dòng)方程(4)描述。方程中h(x,t)指在xoy平面內(nèi)魚(yú)體垂直于推進(jìn)方向即y方向的位移。需要指出的是,方程右邊的第1 個(gè)因子表示魚(yú)體以零幅值開(kāi)始擺動(dòng),逐漸增加幅值。根據(jù)魚(yú)體波動(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ī)器魚(yú)質(zhì)心的運(yùn)動(dòng)與其所受合力關(guān)系為

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

根據(jù)剛體轉(zhuǎn)動(dòng)的歐拉方程,構(gòu)成機(jī)器魚(yú)的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ī)器魚(yú)水動(dòng)力學(xué)建模

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

由于在實(shí)際控制機(jī)器魚(yú)的過(guò)程中,傳感器所采集的力學(xué)和運(yùn)動(dòng)學(xué)參數(shù)一般都是相對(duì)機(jī)器魚(yú)本身,因此有必要建立描述機(jī)器魚(yú)在運(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ú)線運(yùn)動(dòng)和角運(yùn)動(dòng)的動(dòng)力學(xué)方程

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

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

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

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

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

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

CPG 控制器輸出的周期信號(hào)具有穩(wěn)定性強(qiáng)、適應(yīng)性強(qiáng)和多模態(tài)等特性,因此被廣泛應(yīng)用于仿生機(jī)器人的控制中。穩(wěn)定性:無(wú)論輸出信號(hào)的初始狀態(tài)如何,CPG 振蕩器的輸出信號(hào)在平面相圖中呈現(xiàn)自持振蕩的孤立閉合軌跡,這在控制系統(tǒng)中體現(xiàn)為對(duì)擾動(dòng)的抑制。適應(yīng)性:引入反饋后,CPG 的輸出能夠隨著環(huán)境的變化而做出響應(yīng)來(lái)使受控對(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ī)器魚(yú)關(guān)節(jié)運(yùn)動(dòng)

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

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

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

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

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

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

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

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

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

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

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

機(jī)器魚(yú)在非靜止流場(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ī)器魚(yú)姿態(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ī)器魚(yú)的剛體模型有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ī)器魚(yú)目標(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ī)器魚(yú)控制系統(tǒng)設(shè)計(jì),使用MATLABSimulink 搭建仿真控制系統(tǒng)模型,如圖5 所示。

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

機(jī)器魚(yú)的基本參數(shù)見(jiàn)表1。文中將展示2 組仿真結(jié)果,形成參數(shù)優(yōu)化的前后對(duì)照。每組仿真根據(jù)機(jī)器魚(yú)的初始姿態(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ī)器魚(yú)參數(shù)列表Table 1 List of robotic fish parameters

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

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

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

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

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

設(shè)置穩(wěn)態(tài)跟蹤標(biāo)志為:機(jī)器魚(yú)目標(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ī)器魚(yú)目標(biāo)航向角與實(shí)際航向角之差收斂至2%所用時(shí)間 (以圖6(a)所示初始條件進(jìn)行優(yōu)化)。迭代過(guò)程適應(yīng)度函數(shù)變化曲線如圖8 所示。

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

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

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

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

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

圖10 優(yōu)化后機(jī)器魚(yú)航向角偏差變化曲線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%,振蕩頻率明顯變緩?？梢?jiàn)控制器的各項(xiàng)性能指標(biāo)都有所提升。

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

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

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

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