基于視覺信息丟失的機(jī)器魚運(yùn)動控制算法

謝景鵬，李宗剛，杜亞江，葛立明

(1.蘭州交通大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州，730070；2.蘭州交通大學(xué) 機(jī)器人研究所，甘肅 蘭州，730070)

0 引言

隨著海洋經(jīng)濟(jì)在世界經(jīng)濟(jì)的占比不斷提高，水下仿生機(jī)器人成為研究的熱點(diǎn)之一。在游動機(jī)理、仿生機(jī)構(gòu)設(shè)計，運(yùn)動控制方法以及與環(huán)境的交互技術(shù)等方面取得了顯著進(jìn)展[1]。為實現(xiàn)水下仿生機(jī)器人的自主運(yùn)動，水下運(yùn)動目標(biāo)識別也成為重要研究內(nèi)容之一[2-3]。

目前，水下機(jī)器人自主作業(yè)面臨著巨大挑戰(zhàn)，相較陸地，水下環(huán)境光線較弱，因此，水下環(huán)境中機(jī)器人目標(biāo)識別問題就顯得尤為重要。目標(biāo)識別主要包含以下的幾個階段： 一是數(shù)據(jù)預(yù)處理[4]，二是圖像特征提取[5]，三是分類特征選擇[6]。現(xiàn)階段的目標(biāo)檢測和圖像識別主要使用卷積神經(jīng)網(wǎng)絡(luò)來進(jìn)行[7]。

為了實現(xiàn)水下機(jī)器人的自主控制[8]，需要解決水中目標(biāo)的識別與跟蹤問題，很多學(xué)者針對這一問題做了大量研究。文獻(xiàn)[9]實現(xiàn)主動視覺跟蹤，使得機(jī)器魚能夠在二維平面上自由游動。文獻(xiàn)[10]主要研究面向機(jī)器魚的多目標(biāo)跟蹤系統(tǒng)，可以即時跟蹤多個目標(biāo)，是多機(jī)器魚實現(xiàn)群體協(xié)作、隊形控制的基礎(chǔ)。

文中針對一個領(lǐng)導(dǎo)者機(jī)器魚引導(dǎo)多個跟隨者機(jī)器魚向期望位置移動過程中存在的視覺信息丟失問題進(jìn)行研究。通過對機(jī)器魚圖像狀態(tài)識別采用二次濾波法進(jìn)行判定，使用基于半閉環(huán)控制的直線追蹤法和圓弧切線法實現(xiàn)對機(jī)器魚的運(yùn)動控制。

1 問題描述

設(shè)系統(tǒng)由N(N≥ 2)個機(jī)器魚組成，其中1 個機(jī)器魚為領(lǐng)航者，其余機(jī)器魚作為跟隨者。領(lǐng)航者通過圖像識別判斷跟隨者機(jī)器魚的位置和運(yùn)動狀態(tài)從而對跟隨者進(jìn)行運(yùn)動控制。文中主要研究當(dāng)領(lǐng)航者機(jī)器魚由于外在因素丟失圖像信息時，跟隨者根據(jù)之前的控制信息自主預(yù)測完成領(lǐng)航者機(jī)器魚發(fā)布的任務(wù)。文中所使用的機(jī)器魚為三關(guān)節(jié)仿生機(jī)器魚，如圖1 所示。由于實驗條件限制，攝像頭看作領(lǐng)航者，三關(guān)節(jié)仿生機(jī)器魚看作跟隨者。

圖1 機(jī)器魚Fig.1 Robotic fish

2 視覺信息丟失時目標(biāo)機(jī)器魚狀態(tài)判別

2.1 基于二次濾波法的機(jī)器魚狀態(tài)判定

領(lǐng)航者機(jī)器魚識別到執(zhí)行者機(jī)器魚有正反2種狀態(tài)。機(jī)器魚狀態(tài)識別正反判斷如圖2 所示。

圖2 機(jī)器魚狀態(tài)識別判定Fig.2 Judgment of robotic fish state recognition

圖中：φ1為上一通信模塊發(fā)送周期機(jī)器魚中心位置到當(dāng)前周期機(jī)器魚中心位置向量的角度；φ2為當(dāng)前周期機(jī)器魚的方向角度，σ=φ2-φ1為角度差。通信模塊發(fā)送周期ΔT取50 ms。在正常的游動中，機(jī)器魚處于反選狀態(tài)時，。然而，實驗中對實時數(shù)據(jù)分析后，發(fā)現(xiàn)在正常的游動中，存在數(shù)據(jù)的突變情況，故需將這種情況的信息濾掉。文中采用基于事件頻率的二次濾波法對數(shù)據(jù)進(jìn)行實時濾波，圖3 為數(shù)據(jù)處理流程。

圖3 數(shù)據(jù)處理流程圖Fig.3 Flow chart of data processing

2.2 視覺信息丟失時機(jī)器魚的運(yùn)動模型

視覺信息丟失時機(jī)器魚的軌跡模型見圖4。

圖4 機(jī)器魚軌跡模型Fig.4 Trajectory model of robotic fish

將曲線軌跡微分為多邊形，由圖4 可知，存在

式中：α′為微分后的其中一段軌跡對應(yīng)的圓心角；α′′為機(jī)器魚當(dāng)前周期方向與上一周期的偏差。設(shè)當(dāng)前周期數(shù)為i，θi表示當(dāng)前周期機(jī)器魚預(yù)測方向與目標(biāo)軌跡方向的理論夾角，ωk和vk分別表示當(dāng)前周期內(nèi)機(jī)器魚的角速度和線速度，其中k表示檔位，k=1，2，3，…，15；Yi為機(jī)器魚與目標(biāo)軌跡徑向偏差；Xi表示機(jī)器魚在目標(biāo)軌跡上的投影位置。則

a，b，c，d，e，f分別表示1 個動作周期內(nèi)，機(jī)器魚方向檔位4，5，7，9，10，7 對應(yīng)的執(zhí)行周期個數(shù)。也就是說1 個動作周期是由多個方向檔位共同作用完成，每個檔位的作用時間除以ΔT就是執(zhí)行周期個數(shù)。則動作指令矩陣可表示為

2.2.1 基于半閉環(huán)控制的直線追蹤法

當(dāng)平臺狀態(tài)識別丟失瞬間，魚頭方向與追蹤軌跡方向夾角-20°＜β0＜20°時，采用半閉環(huán)控制的直線追蹤法實現(xiàn)對機(jī)器魚的運(yùn)動控制。控制框圖如圖5 所示。

圖5 直線追蹤法控制框圖Fig.5 The control block diagram of straight-line tracking method

采用該方法的機(jī)器魚軌跡如圖6 所示。該控制方法實質(zhì)上是控制每個動作周期βj_1 和βj_2在15°左右，且抑制軌跡振幅在一定范圍內(nèi)。在第j個動作周期內(nèi)，機(jī)器魚半周期出射角用βj_1 表示，全周期出射角用βj_2 表示。Ⅰ、Ⅲ階段結(jié)束時，機(jī)器魚與追蹤軌跡的徑向偏差用Yj_chu1、Yj_chu2 表示。Ⅱ、Ⅳ階段結(jié)束時，機(jī)器魚與追蹤軌跡的徑向偏差用Yj_ru1、Yj_ru2 表示。βj_1′、βj_2′、Yj_ru1′、Yj_ru2′分別表示各量的期望值。上述變量均為理論計算值。以第j個動作周期為例，動作指令矩陣為

針對前半周期，設(shè)ZG1 為Ⅰ階段10 檔位作用時間的理論值。Fix()為向下取整函數(shù)，Mod()為取余函數(shù)，round(fj,0)表示將fj的值四舍五入到整數(shù)位。d j，e j，fj的計算過程如式(12)，具體取值如表1 所示。

圖6 直線追蹤法路徑規(guī)劃Fig.6 Path planning of straight-line tracking method

表1 dj，ej，fj 取值Table 1 The value of dj,ej,fj

計算Ⅱ階段結(jié)束時的機(jī)器魚位姿狀態(tài)

針對后半周期，設(shè)ZG2 為Ⅲ階段5 檔位作用時間的理論值。aj，b j，cj的計算如下，具體取值如表2 所示。

表2 aj, bj, cj 取值Table 2 The value of aj,bj,cj

根據(jù)上述模型，可以推導(dǎo)出對于n個動作周期，相應(yīng)的每半個周期的角度變化量矩陣為

對于第j個動作周期，結(jié)合式(15)，則半周期入射角βj_1，全周期出射角βj_2 為

2.2.2 圓弧切線法

當(dāng)平臺狀態(tài)識別丟失瞬間，魚頭方向與追蹤軌跡的方向夾角β0＜-20°或β0＞20°時，采用圓弧切線法實現(xiàn)對機(jī)器魚的轉(zhuǎn)彎控制。機(jī)器魚路徑規(guī)劃如圖7 所示。其中，F(xiàn)(x f，yf)和φ分別為機(jī)器魚識別狀態(tài)丟失瞬間魚頭點(diǎn)坐標(biāo)和魚頭方向。B(xb，yb)為目標(biāo)點(diǎn)坐標(biāo)，O(x0，y0)為單一檔位作用下機(jī)器魚所繞圓心，D(x d，yd)表示直線BD與圓軌跡的切點(diǎn)，R表示圓軌跡的半徑，由β0的大小決定，α為機(jī)器魚實走圓弧所對應(yīng)的圓心角。

O(x0，y0)坐標(biāo)如下，其中φ＜0時，h=1；φ＞ 0時，h=2。

圖7 圓弧切線法路徑規(guī)劃Fig.7 Path planning of circle tangent method

設(shè)過點(diǎn)B(xb，yb)的直線BD表達(dá)式如下，根據(jù)點(diǎn)到直線距離公式，存在以下關(guān)系

式中：φ＜0 時，k取最大值；φ＞0 時，k取最小值。設(shè)Tk表示單一檔位作用下，機(jī)器魚游動一周的時間，則圓心角α和時間tn可表示為

機(jī)器魚到達(dá)D點(diǎn)后，采用上述直線追蹤法實現(xiàn)對軌跡2 的追蹤，從而實現(xiàn)機(jī)器魚的直游控制。

3 仿真驗證

3.1 二次濾波法仿真

現(xiàn)將實驗中采集到的σ數(shù)據(jù)作為模型輸入。其中，σ為φ2與φ1的角度差，φ1為上一通信模塊發(fā)送周期的機(jī)器魚中心位置到當(dāng)前周期的機(jī)器魚中心位置的向量角度，φ2為當(dāng)前周期機(jī)器魚的方向角度。利用MATLAB 進(jìn)行輸入輸出波形仿真。如圖8 所示，圖中0 為反選，1 為正選。其中藍(lán)色波形表示輸入的原數(shù)據(jù)，紅色波形表示采用二次濾波法濾波后的輸出數(shù)據(jù)。由圖可知，機(jī)器魚在第3 s 之前處于正選狀態(tài)，在3 s 之后處于反選狀態(tài)。

3.2 基于半閉環(huán)控制的直線追蹤法仿真

圖8 二次濾波法仿真Fig.8 Simulation of secondary filtering method

當(dāng)視覺信息丟失發(fā)生，且魚頭方向與追蹤軌跡方向夾角-20°＜β0＜20°時，機(jī)器魚的速度取v4=7.35 px/ms，v5=7.53 px/ms，v7=9.09 px/ms，v9=8.43px/ms，v10=9.06 px/ms；角速度取ω4=0.094 rad/ms，ω5=0.066 rad/ms，ω9=0.039 rad/ms，ω10=0.058 rad/ΔT。則隨著β0的變化，機(jī)器魚運(yùn)動過程中各曲線如圖9 所示。圖9(a)和(b)分別表示β0=-5°、-15°、-20°、5°、15°和20°時，機(jī)器魚預(yù)測實時方向與追蹤軌跡的角度偏差變化曲線。由圖可知，曲線振幅維持在-15°～15°之間，曲線周期維持在1 s 左右。由于輸入偏角β0的不同，導(dǎo)致各曲線初值不同，且存在一定相位差，但曲線最終都趨于穩(wěn)態(tài)。圖9(c)、(d)分別為β0=-5 °、-15°、-2 0°、5°、15°和20°時，機(jī)器魚預(yù)測實時位置與追蹤軌跡的徑向偏差曲線。由圖可知，曲線振幅維持在-8～8 px 之間，曲線周期維持在1 s 左右。由于輸入偏角β0的不同，導(dǎo)致各曲線初值不同，且存在一定相位差，但曲線均能在1 s 左右都趨于穩(wěn)態(tài)。

3.3 圓弧切線法仿真

當(dāng)視覺信息丟失發(fā)生且魚頭方向與追蹤軌跡方向夾角β0＜-2 0°或β0＞-20°時，機(jī)器魚的速度v4，v5，v7，v9，v10取值以及機(jī)器魚角速度ω4，ω5，ω7，ω9，ω10取值與同3.2 節(jié)。目標(biāo)點(diǎn)B取xb=650 px，yb=300 px，視覺信息丟失時，圖10(a)中的魚頭點(diǎn)F取xf=200 px，yf=350 px，圖10(b)中的魚頭點(diǎn)F取xf=200 px，yf=100 px，則仿生機(jī)器魚隨著β0的變化，運(yùn)動過程中各變化曲線如圖 10(a)和(b)所示。圖 10(a)表示β0=-5 0°、-100°、-150°時，機(jī)器魚預(yù)測實時方向與追蹤軌跡2 的角度偏差變化曲線，圖10(b)表示β0=50 °、100°、150°時，機(jī)器魚預(yù)測實時方向與追蹤軌跡2 的角度偏差變化曲線。

由圖可知，不同的輸入偏角β0，會導(dǎo)致各曲線初值不同，每條曲線前期均有一段斜率穩(wěn)定的直線，表示機(jī)器魚正在以恒角速度進(jìn)行轉(zhuǎn)彎，轉(zhuǎn)彎時間隨著的增加依次增加。轉(zhuǎn)彎完成后，機(jī)器魚進(jìn)入直線運(yùn)動狀態(tài)，且該部分曲線最終趨于穩(wěn)態(tài)，曲線振幅維持在-15°～15°之間，曲線周期維持在1 s 左右。

圖9 直線追蹤法仿真曲線Fig.9 Simulation curves of straight-line tracking method

圖10 圓弧切線法仿真曲線Fig.10 Simulation curves of circle tangent method

4 實物實驗

實驗環(huán)境為3 m×2 m×0.3 m 的室內(nèi)水池，水池左上角為坐標(biāo)原點(diǎn)，仿生機(jī)器魚根據(jù)不同實驗需要布置在相應(yīng)位置。紅色立柱表示每個實驗中設(shè)立的目標(biāo)點(diǎn)位置。實驗環(huán)境如圖11 所示。

4.1 正反狀態(tài)判定策略實驗

圖11 實驗環(huán)境Fig.11 Experiment environment

實驗中，仿生機(jī)器魚初始位置在水池左上角隨機(jī)布置。實驗中機(jī)器魚的目標(biāo)點(diǎn)B為xb=645 px，yb=380 px 。圖12(a)中，機(jī)器魚剛開始處于正選狀態(tài)，向目標(biāo)點(diǎn)運(yùn)動，6 s 后被人為標(biāo)定為反選狀態(tài)。圖12(b)中，機(jī)器魚0 s 后一直標(biāo)定為反選狀態(tài)。圖12(c)給出了2 次實驗中，機(jī)器魚各自的軌跡圖像。由圖可知，相對于未采用判定策略前，采用判定策略的機(jī)器魚能夠識別自身反選狀態(tài)并做出補(bǔ)償，進(jìn)而不影響其向目標(biāo)點(diǎn)的運(yùn)動。

圖12 判定策略驗證實驗Fig.12 Verification experiment of judgment strategy

4.2 基于半閉環(huán)控制的直線追蹤法實驗

實驗中，仿生機(jī)器魚初始位置在水池左側(cè)隨機(jī)布置。設(shè)定機(jī)器魚的目標(biāo)點(diǎn)B為xb=645 px，yb=235 px。實體環(huán)境中機(jī)器魚的各變化曲線如圖13(b)和(c)所示。實驗程序設(shè)定第2 s 后機(jī)器魚處于視覺信息丟失階段，測得β0=-7.65°。

圖13(b)為機(jī)器魚實時方向與追蹤軌跡的角度偏差變化，曲線的振幅為 -1 5°～15 °，曲線周期為1 s 左右，且曲線圍繞著縱坐標(biāo)為0 的直線上下振蕩，趨于穩(wěn)態(tài)，與圖9(a)仿真結(jié)果一致。圖13(c)為機(jī)器魚實時位置與追蹤軌跡的徑向偏差變化，曲線的振幅基本為-8～8 px，曲線周期為1 s 左右，且曲線圍繞著縱坐標(biāo)為0 的直線上下振蕩，趨于穩(wěn)態(tài)，與圖9(c)仿真結(jié)果一致。

4.3 圓弧切線法實驗

圖13 直線追蹤法實驗Fig.13 Experiment of straight-line tracking method

實驗中，仿生機(jī)器魚初始位置在水池右側(cè)上部隨機(jī)布置。設(shè)定機(jī)器魚的目標(biāo)點(diǎn)B為xb=420 px，yb=315 px。第0 s 開始，機(jī)器魚處于視覺信息丟失階段，測得β0=149.8°。實驗截圖與曲線如圖14 所示。圖14(b)為機(jī)器魚實時方向與追蹤軌跡2 的角度偏差變化，機(jī)器魚在0～3.5 s 階段，處于恒角速度轉(zhuǎn)彎階段，從圖中可以看出機(jī)器魚在3.5 s 以后進(jìn)入直線運(yùn)動狀態(tài)，該階段曲線振幅為-1 5 °～ 15°，曲線周期為1 s 左右，且趨于穩(wěn)態(tài)，與圖10(b)仿真結(jié)果一致。

圖14 圓弧切線法實驗Fig.14 Experiment of circle tangent method

5 結(jié)束語

文中提出了機(jī)器魚在視覺信息丟失情況下的運(yùn)動控制問題，針對這個問題，提出了一種運(yùn)動補(bǔ)償控制算法。該算法由3 個部分組成，分別是基于二次濾波法的狀態(tài)判定，基于半閉環(huán)控制的直線追蹤法和圓弧切線法。所提算法經(jīng)過仿真、實驗的論證，均表明可以解決機(jī)器魚在視覺丟失的情況下的運(yùn)動控制問題。需要說明的是，該算法在實體實驗中還存在一定的不足。在后續(xù)工作中會對這3 種算法合成一種算法并且在戶外環(huán)境中對所提算法進(jìn)行試驗驗證和改進(jìn)。

相關(guān)文章導(dǎo)航

1.朱紅秀，杜闖，褚彥斌，等.一種電磁驅(qū)動機(jī)器魚避障方法[J].水下無人系統(tǒng)學(xué)報，2019，27(6).

2.胡橋，劉鈺，趙振軼，等.水下無人集群仿生人工側(cè)線探測技術(shù)研究進(jìn)展[J].水下無人系統(tǒng)學(xué)報，2019，27(2).

3.張晗，王偉，謝廣明.面向水下機(jī)器人的水下電場通信協(xié)議設(shè)計[J].水下無人系統(tǒng)學(xué)報，2019，27(2).

4.卞長生，白萬發(fā)，朱子才，等.基于IPMC 仿生機(jī)器魚驅(qū)動技術(shù)研究[J].水下無人系統(tǒng)學(xué)報，2019，27(2).

5.常龍飛，李超群，牛清正，等.IPMC 驅(qū)動柔性機(jī)器魚速度控制系統(tǒng)[J].水下無人系統(tǒng)學(xué)報，2019，27(2).

6.劉鈺，胡橋，趙振軼，等.基于EMD 與SVM 的仿生機(jī)器魚人工側(cè)線智能探測方法[J].水下無人系統(tǒng)學(xué)報，2019，27(2).

7.王延杰，郝牧宇，張霖，等.基于智能驅(qū)動材料的水下仿生機(jī)器人發(fā)展綜述[J].水下無人系統(tǒng)學(xué)報，2019，27(2).

8.范增，王揚(yáng)威，劉凱，等.仿生機(jī)器魚胸鰭波動與擺動融合推進(jìn)機(jī)制建模及實驗研究[J].水下無人系統(tǒng)學(xué)報，2019，27(2).