[發(fā)明專利]一種用于雙關(guān)節(jié)機(jī)器魚的復(fù)合避障方法和裝置

在審

• 申請(qǐng)?zhí)枺?/td>: 202110245581.5
• 申請(qǐng)日: 2021-03-05
• 公開（公告）號(hào): CN113009923A
• 公開（公告）日: 2021-06-22
• 發(fā)明（設(shè)計(jì)）人: 張川;文小玲;喻盈;黃晨華
• 申請(qǐng)（專利權(quán)）人: 武漢工程大學(xué)
• 主分類號(hào): G05D1/06
• 分類號(hào): G05D1/06;B63C11/52
• 代理公司: 湖北武漢永嘉專利代理有限公司 42102
• 代理人: 崔友明

1.一種用于雙關(guān)節(jié)機(jī)器魚的復(fù)合避障方法，其特征在于：包括以下步驟：

S0：搭建用于雙關(guān)節(jié)機(jī)器魚的復(fù)合避障裝置，包括擺動(dòng)單元、浮潛單元、控制單元、傳感器網(wǎng)絡(luò)；控制單元的信號(hào)接收端連接傳感器網(wǎng)絡(luò)的信號(hào)發(fā)送端，控制單元的信號(hào)發(fā)送端分別連接擺動(dòng)單元和浮潛單元的信號(hào)接收端；

擺動(dòng)單元包括連接桿、第一關(guān)節(jié)舵機(jī)、第二關(guān)節(jié)舵機(jī)和魚尾，第一關(guān)節(jié)舵機(jī)、第二關(guān)節(jié)舵機(jī)和魚尾通過連接桿依次連接；

浮潛單元包括依次連接的抽水水泵、水箱和排水水泵；抽水水泵通過抽水水泵進(jìn)水口連接水源，抽水水泵通過抽水水泵出水口連接水箱的水箱進(jìn)水口；水箱通過排氣口放氣調(diào)節(jié)水箱內(nèi)外氣壓平衡；排水水泵通過排水水泵進(jìn)水口連接水箱的水箱排水口，排水水泵通過排水水泵出水口排水；

控制單元包括MCU、PWM調(diào)節(jié)模塊、速度系數(shù)模糊控制器、轉(zhuǎn)向角度模糊控制器和上浮下潛控制器；

傳感器網(wǎng)絡(luò)包括正前方測(cè)距傳感器F_IRS、左側(cè)測(cè)距傳感器L_IRS、右側(cè)測(cè)距傳感器R_IRS、上側(cè)測(cè)距傳感器T_IRS和下側(cè)測(cè)距傳感器B_IRS，分別位于雙關(guān)節(jié)機(jī)器魚魚頭位置的正前方、左側(cè)、右側(cè)、上側(cè)和下側(cè)，且上側(cè)測(cè)距傳感器T_IRS和下側(cè)測(cè)距傳感器B_IRS向前傾斜一定的角度，左側(cè)測(cè)距傳感器L_IRS和右側(cè)測(cè)距傳感器R_IRS向內(nèi)傾斜一定的角度；

S1：傳感器網(wǎng)絡(luò)采集雙關(guān)節(jié)機(jī)器魚與環(huán)境障礙物的距離信息并分別發(fā)送給速度系數(shù)模糊控制器、轉(zhuǎn)向角度模糊控制器和上浮下潛控制器；

S2：根據(jù)雙關(guān)節(jié)機(jī)器魚的關(guān)節(jié)擺動(dòng)角度模型在速度系數(shù)模糊控制器和轉(zhuǎn)向角度模糊控制器中建立模糊規(guī)則庫，并根據(jù)模糊規(guī)則庫分別輸出速度系數(shù)V和轉(zhuǎn)向角度α；

S3：在上浮下潛控制器中建立上浮下潛規(guī)則庫，并根據(jù)上浮下潛規(guī)則庫輸出上浮距離或下潛距離；

S4：MCU接收步驟S2得到的速度系數(shù)V和轉(zhuǎn)向角度α、步驟S3得到的上浮距離或下潛距離，并通過PWM調(diào)節(jié)模塊將控制信號(hào)分別發(fā)送給擺動(dòng)單元和浮潛單元執(zhí)行避障動(dòng)作。

2.根據(jù)權(quán)利要求1所述的一種用于雙關(guān)節(jié)機(jī)器魚的復(fù)合避障方法，其特征在于：所述的步驟S2中，建立關(guān)節(jié)擺動(dòng)角度模型的具體步驟為：

在雙關(guān)節(jié)機(jī)器魚的游動(dòng)過程中，第二關(guān)節(jié)舵機(jī)擺動(dòng)角度的幅值的正弦函數(shù)相位滯后于第一關(guān)節(jié)舵機(jī)擺動(dòng)角度的幅值的正弦函數(shù)的相位180°；設(shè)第一關(guān)節(jié)舵機(jī)擺動(dòng)角度為第一關(guān)節(jié)舵機(jī)與軸線的夾角α，第二關(guān)節(jié)舵機(jī)擺動(dòng)角度為第二關(guān)節(jié)舵機(jī)與軸線的夾角β，第一關(guān)節(jié)舵機(jī)擺動(dòng)振幅為A₁，第一關(guān)節(jié)舵機(jī)擺動(dòng)周期為T₁，第二關(guān)節(jié)舵機(jī)擺動(dòng)振幅為A₂，第二關(guān)節(jié)舵機(jī)擺動(dòng)周期為T₂，對(duì)雙關(guān)節(jié)機(jī)器魚進(jìn)行運(yùn)動(dòng)分析得到關(guān)節(jié)擺動(dòng)角度模型為：

3.根據(jù)權(quán)利要求2所述的一種用于雙關(guān)節(jié)機(jī)器魚的復(fù)合避障方法，其特征在于：所述的步驟S2中，雙關(guān)節(jié)機(jī)器魚以正常速度直線游動(dòng)時(shí)，第一關(guān)節(jié)舵機(jī)與第二關(guān)節(jié)舵機(jī)在滿足擺動(dòng)幅值條件的同時(shí)保持周期一致為0.35s的關(guān)節(jié)擺動(dòng)角度的數(shù)學(xué)模型為：

由上式得到，擺動(dòng)幅值決定游動(dòng)速度；因此雙關(guān)節(jié)機(jī)器魚減速時(shí)，減小α和β的擺動(dòng)幅值，并在一個(gè)擺動(dòng)周期中通過PWM調(diào)節(jié)模塊的控制信號(hào)增加延時(shí)以保證擺動(dòng)周期不變；

設(shè)k的范圍為[-45°，45°]，則雙關(guān)節(jié)機(jī)器魚轉(zhuǎn)向時(shí)關(guān)節(jié)擺動(dòng)角度的數(shù)學(xué)模型為：