基于PSO 的非靜止流場中機器魚轉向控制參數優化

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

(1.廣西科技大學 電氣電子與計算機科學學院,廣西 柳州,545006;2.廣西科技大學 廣西汽車零部件與整車技術重點實驗室,廣西 柳州,545006;3.北京大學 工學院,北京,100871)

0 引言

仿生機器魚參考了自然界魚類的外形和運動模式,其具備自然魚類運動性能的部分優點,如靜音、節能、靈活等[1],是水下機器人研究的一個熱門研究領域。

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

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

文中首先對機器魚的擺動以及與水流的相互作用建立模型,其次根據動力學模型設計控制器,主要用于自身運動狀態的感知,實現航向角信息的反饋。但是并不包含能夠感知流場信息的傳感器,因此機器魚在航向調節時必須具有平動速度,即邊游邊轉向。控制器使機器魚做相應的轉向來消除水流的干擾,并使航向角盡快收斂至目標方位角。更進一步,采用粒子群優化(particle swarm optimization,PSO)算法對CPG 控制器的參數進行優化,最終提升機器魚的快速轉向能力和靈活性。

1 機器魚動力學建模

為了獲得機器魚各個關節的控制力矩,先對機器魚進行動力學建模。將機器魚簡化為一個六自由度的平面串聯連桿系,如圖1 所示,機器魚的三關節剛體模型的每節連桿被各自的質心分成前后2 段。

圖1 三關節連桿系剛體機器魚模型Fig.1 Robotic fish model of three joint-linkage system rigid body

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

1.1 機器魚關節動力學模型

根據拉格朗日力學建立連桿系的拉格朗日方程。其廣義坐標為各關節轉角θ和連桿質心位置的橫縱坐標x、y。此處的坐標系固定在靜止的環境中,稱為靜止坐標系。關節轉角θi的確定如圖1 所示,每個關節的中心都落在波動曲線上。

拉格朗日函數

式中:T為機器魚相對靜止坐標系的動能;mi為每根連桿的質量;V為機器魚勢能,文中只研究機器魚在一個水平面內的運動情況,因此不考慮豎直方向的受力情況,勢能不包含重力勢能,只有關節處的轉動彈性勢能;ki為相鄰2 根連桿之間的相對運動阻力系數。

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

從而求出每個關節轉角θi隨時間變化的函數

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

圖2 關節驅動力矩變化曲線Fig.2 Curves of joint driving torque

以上的建模是正向動力學建模,即已知運動狀態求驅動力。要準確地對控制系統進行仿真還需要建立它的逆向模型,此模型要描述的就是已知關節動力求關節運動,即關節電機轉矩,輸出為各關節的轉角。

由于各連桿之間由關節相連,它們之間存在幾何約束關系為

根據剛體的質心運動定理,4 根連桿構成的機器魚質心的運動與其所受合力關系為

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

根據剛體轉動的歐拉方程,構成機器魚的4 根連桿的轉角、質心位置與關節電機輸出力矩

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

其中,Fx32、Fy32與Fx23、Fy23可以由第2、3 根連桿的質心平動表示為

1.2 機器魚水動力學建模

機器魚關節轉動使魚體做波狀擺動,周圍流場則對機器魚產生反作用力。為了獲取機器魚的推進力和轉向力矩,需要建立機器魚的水動力學模型。文中采用目前在BCF 模式下[11-13]應用最為廣泛的魚類推進理論——細長體理論為機器魚的水動力學建模。

1.2.1 機器魚受水流推進力及轉向力矩

圖3 為機器魚的俯視圖。圖中,W為機器魚質心速度,φ為實際航向角,T為目標矢量,?為目標航向角,V為水流速度。

圖3 機器魚及流場環境狀態示意圖Fig.3 Diagram of robotic fish and flow field environment state

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

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

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

魚體相對附加質量的速度為v,魚體相對流場的平均速度為U,則

魚體受到流場的反作用力就是附加質量的動量變化率,即式(18)與(19)乘積對時間的1 階導數

根據附加質量力計算公式,分別對機器魚魚體的x和y方向積分,即可得到魚體的推進力FxL和側方向受力FyL,即

以質心為旋轉中心,由附加質量力產生的偏航力矩為

式中,h是機器魚y方向的形變。從上述力與力矩的表達式可知h須是在魚體長度范圍內的連續函數。根據上一節機器魚的剛體力學模型,在每一時刻對其關節點進行擬合,得到魚體中脊線的波動曲線,即h在每一時刻的函數值。機器魚的實際運動由自身游動和環境流場的相互作用決定。由于流場速度的不確定性,通常將環境流場對魚運動的作用視為一種干擾。

1.2.2 水流速度變化對機器魚航向角的影響

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

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

水流提供機器魚前進的推力,也產生阻礙其運動的阻力。影響機器魚阻力的因素有流場的密度、朝向來流方向的機器魚的面積和機器魚相對水流的速度。因此,機器魚所受x方向和y方向的阻力公式為

以及其所受水流產生的阻礙轉向的力矩

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

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

式中:M是機器魚質量;I是機器魚相對運動坐標系z軸的轉動慣量;ux和uy是機器魚整體質心相對運動坐標系的速度。

機器魚相對水流的速度在運動坐標系與靜止坐標系之間的變換式為

將式(29)代入(24)得到機器魚質心在靜止坐標系中的速度,根據航向角的定義,得到其表達式為

2 機器魚姿態控制系統

2.1 CPG 運動控制簡介

CPG 是一類模仿生物控制節律性運動的神經信號發生器的數學模型[14]。一個CPG 單元能夠產生周期性信號,由CPG 單元通過一定的拓撲結構連接而成的網絡能夠產生一系列相位互鎖的周期信號。可通過改變網絡連接參數來實現對周期信號的幅值、頻率和相位的改變。

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

2.2 CPG 控制機器魚關節運動

魚類波動狀擺動的運動方式可以用一個以二次函數為包絡線的波動方程來描述。魚體關節的基本運動是周期性的,只要調節好各關節之間的相位差就可以生成魚體波。如果采用一系列無相互耦合的周期信號控制各關節的擺動,就無法自動形成相位互鎖,即穩定的相位差。需要定期動態矯正偏差帶來的軸向不對稱,增加算法負擔。而CPG 則克服了以上問題,適合用來控制機器魚的基本運動[15-16]。

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

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

2.3 機器魚姿態控制系統設計

機器魚本身不能直接檢測所在流場的速度方向及大小,但是可以通過搭載的運動傳感器和定位模塊獲取自身的運動信息。因此機器魚的實際航行方向已知。由于魚體的運動始終是擺動著,正常行進時比較環節輸出的偏差是個小幅度波動信號,因此需要一個閾值非線性環節來過濾這個偏差信號。CPG 控制器的輸入是閾值非線性環節過濾后的偏差。

式中:ki為偏置系數;a為 非線性環節的閾值[19];α為機器魚頭部與實際航向間夾角;λi為此夾角對偏置bi的修正系數。

確定了被控對象和控制器后即可設計出完整的控制系統。圖4 為機器魚姿態控制系統框圖。系統的輸入為目標航向角,輸出為實際航向角。實際航向角的反饋,使系統控制形成閉環。

圖4 機器魚控制系統框圖Fig.4 Block diagram of robotic fish control system

根據2.1 節CPG 控制器特性,CPG 控制器能使閉環系統穩定,最終使機器魚航向偏差收斂。

3 控制器參數優化

3.1 優化指標與優化對象

機器魚在非靜止流場中要有快速調整姿態跟蹤目標航向的能力,即優化指標作系統的響應時長,優化方向是縮短響應時間。而CPG 控制器各振蕩單元的偏置系數ki決定了機器魚姿態的調整,進而影響航向角偏差的收斂速度。因此,各振蕩器單元的偏置系數ki就是優化的對象。

3.2 PSO 算法簡介

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

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

3.3 參數優化算法設計

機器魚的剛體模型有3 個關節,即包含3 個振蕩器單元CPG 控制器,因此由3 個偏差系數組成解空間。根據關節動力學模型的計算結果,3 個偏置的范圍應小于各自對應的關節擺動幅值。

算法流程如下。

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

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

3) 對每個粒子,用其當前的適應度值fit[i]和自身目前尋得的最大適應度值p[i]比較。若fit[i]

4) 對每個粒子,用其當前的適應度值fit[i]和全局極值g[i]比較。若fit[i]

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

6) 處理邊界條件。

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

4 仿真與結果分析

按照機器魚控制系統設計,使用MATLABSimulink 搭建仿真控制系統模型,如圖5 所示。

圖5 Simulink 控制系統仿真模型Fig.5 The control system simulation model in Simulink

機器魚的基本參數見表1。文中將展示2 組仿真結果,形成參數優化的前后對照。每組仿真根據機器魚的初始姿態不同分為(a)和(b)2 個仿真。按照圖1 所示結構分配各個連桿的參數。其中轉動慣量以各自質心為轉動中心。

表1 機器魚參數列表Table 1 List of robotic fish parameters

如圖3 所示,設置機器魚航向角目標角 φ為26.6°,水流V方向與固定坐標系橫坐標軸夾角為45°,流速為2 m/s,初始狀態下機器魚隨水流運動,因此實際航向角φ也為45°,速度大小也與水流一致。機器魚初始姿態魚頭指向與固定坐標系橫坐標夾角為分別為?90°和90°。控制目標是使機器魚的實際航向跟蹤目標航向。控制器參數經過手動調試,能夠使機器魚的航向角跟蹤到目標航向角。將其設置為初始參數,見表2。

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

仿真得出的機器魚運動軌跡和實際航向變化曲線分別如圖6(其中直線僅表示方向,而不是目標與出發點的連線)和圖7 所示。

圖6 機器魚航向調整過程運動軌跡Fig.6 The robotic fish movement track during the course adjustment process

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

設置穩態跟蹤標志為:機器魚目標航向角與實際航向角之差收斂至小于2%,并持續2 個求解周期。根據此要求得出未優化控制器參數時穩態時間為分別為30 s 和25 s。

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

圖8 適應度函數變化曲線Fig.8 Fitness function changing curve

迭代計算至結果穩定后輸出優化參數見表3。

表3 優化后控制器參數列表Table 3 Controller parameter list after optimization

使用優化參數后的控制器再次仿真,結果見圖9 和圖10。

圖9 優化后機器魚航向調整過程運動軌跡Fig.9 The trajectory of robotic fish in course adjustment process after optimization

圖10 優化后機器魚航向角偏差變化曲線Fig.10 Curves of course angle deviation of robotic fish after optimization

優化后航向角調節的穩態時間分別為20 s 和16 s,較優化前調節時間減少了33%和36%,超調量下降了12%和10%,振蕩頻率明顯變緩。可見控制器的各項性能指標都有所提升。

從圖6 中可以看出,機器魚在水流方向與目標航向不一致的情況下,以不同的初始姿態出發,最終的航向能夠與目標航向同向平行。驗證了機器魚控制系統航向穩定的有效性。對比圖8 和圖10,經過PSO 算法優化后機器魚航向跟蹤到目標航向的時間縮短,超調量變小。

5 結束語

通過建立機器魚的動力學模型,結合CPG 控制器,利用航向角反饋,能夠設計出解決機器魚在非靜止流場中的航向穩定問題的控制系統。而CPG 控制器的偏置系數經優化后可以在一定程度改善機器魚航向調整的快速性和平穩性,提升了機器魚的轉向靈活性。仿真結果驗證了文中所建模型與優化方案的有效性。

此外,由于文中所做仿真的假設條件是忽略流場短時間內的變化,測試機器魚航向控制系統的環境流場設置的是確定的水流速度。此仿真無法模擬現實中有可能出現的水流劇烈變化的情形。要完全解決變流場航向穩定,還需對機器魚附近流場的變化率范圍與機器魚所需響應速度極限之間的關系進行進一步研究。