[發(fā)明專利]一種水下自主航行機(jī)器人自動返航方法及裝置

在審

• 申請?zhí)枺?/td>: 202110398938.3
• 申請日: 2021-04-14
• 公開（公告）號: CN113110528A
• 公開（公告）日: 2021-07-13
• 發(fā)明（設(shè)計(jì)）人: 肖志偉;翟慶林;張金;王柏文;陳卓
• 申請（專利權(quán)）人: 湖南國天電子科技有限公司
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 長沙大珂知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 43236
• 代理人: 伍志祥

1.一種水下自主航行機(jī)器人自動返航方法，其特征在于，包括以下步驟：

S1：在所述水下自主航行機(jī)器人完成任務(wù)執(zhí)行后，所述水下自主航行機(jī)器人裝置中的位置傳感器(9)對周圍環(huán)境進(jìn)行地圖創(chuàng)建與定位；

S2：建立與所述步驟S1得到的地圖相對應(yīng)的柵格地圖；

S3：利用數(shù)學(xué)形態(tài)學(xué)中的膨脹算子對所述步驟S2得到的柵格地圖所描述的多個(gè)障礙物進(jìn)行膨脹運(yùn)算，生成多個(gè)平行的邊界構(gòu)成的柵格Dirichlet圖的路徑，所述多個(gè)平行邊界構(gòu)成的柵格Dirichlet圖的路徑組成返航路徑；

S4：對所述S3步驟得到的返航路徑進(jìn)行路徑矢量化，得到矢量路徑網(wǎng)；

S5：為減少數(shù)據(jù)存儲空間，提高路徑規(guī)劃效率，將所述S4步驟得到的矢量路徑網(wǎng)進(jìn)行拓?fù)浠玫酵負(fù)渎窂骄W(wǎng)；

S6：采用Dijkstra算法對所述拓?fù)渎窂骄W(wǎng)進(jìn)行路徑規(guī)劃，選擇出一條最優(yōu)返航路徑；

S7：采用人工路徑規(guī)劃算法對所述S6步驟得到的所述最優(yōu)返航路徑進(jìn)行實(shí)時(shí)海洋環(huán)境基準(zhǔn)測試，若有效則采用所述最優(yōu)返航路徑，若無效則重復(fù)所述步驟S2～S6。

2.根據(jù)權(quán)利要求1所述的一種水下自主航行機(jī)器人自動返航方法，其特征在于，所述位置傳感器(9)采用差分定位系統(tǒng)、航速計(jì)程儀與捷聯(lián)慣導(dǎo)組合對周圍環(huán)境進(jìn)行地圖創(chuàng)建與定位。

3.根據(jù)權(quán)利要求1所述的一種水下自主航行機(jī)器人自動返航方法，其特征在于，所述S3步驟中利用數(shù)學(xué)形態(tài)學(xué)中的膨脹算子對所述步驟S2得到的柵格地圖所描述的多個(gè)障礙物進(jìn)行膨脹運(yùn)算時(shí)，為了明確所述水下自主航行機(jī)器人的膨脹邊界與所述路徑終點(diǎn)站所在原點(diǎn)之間的距離,使其更容易進(jìn)行邊界提取,采用一種基于正方形形式的測距結(jié)構(gòu)元素?cái)?shù)學(xué)模型：

D(m,j)＝min[D(m-1,n-1)＝b,

D(m-1,n)+a，D(m-1,n+1)+b，

D(m,n-1)+a，D(m,n)+0，

D(m,n+1)+a，D(m+1,n-1)+b，

D(m+1,n)+a，D(m+1,n+1)+b]；

其中，所述m、所述n為柵格編號，所述a、所述b為柵格距離參數(shù)。

4.根據(jù)權(quán)利要求3所述的一種水下自主航行機(jī)器人自動返航方法，其特征在于，所述S3步驟中基于膨脹運(yùn)算使用測距結(jié)構(gòu)元素對所述2步驟得到的柵格地圖進(jìn)行處理，通過重復(fù)循環(huán)查找所述地圖上的每一個(gè)柵格，當(dāng)柵格為空白時(shí)，使用所述測距結(jié)構(gòu)元素?cái)?shù)學(xué)模型，對所述水下自主航行機(jī)器人所在柵格的周圍8個(gè)柵格進(jìn)行掃描；如果周圍有障礙物，則根據(jù)所述測距結(jié)構(gòu)元素?cái)?shù)學(xué)模型計(jì)算值對該空白柵格進(jìn)行賦值，否則不對該所述空白柵格進(jìn)行操作，而進(jìn)行下一個(gè)柵格操作運(yùn)算；如該所述空白柵格下一個(gè)柵格已經(jīng)有初始值，則不做處理，繼續(xù)掃描下一個(gè)柵格，完成一次柵格地圖內(nèi)障礙物查找；

每完成一次所述障礙物查找，所涉及的障礙物體及所述終點(diǎn)站原點(diǎn)都均勻膨脹一圈；經(jīng)過多次所述障礙物查找后整張所述柵格地圖無空閑柵格時(shí)，所述膨脹運(yùn)算結(jié)束。

5.根據(jù)權(quán)利要求1所述的一種水下自主航行機(jī)器人自動返航方法，其特征在于，所述人工路徑規(guī)劃算法包括所述路徑終點(diǎn)站所在原點(diǎn)的對所述水下自主航行機(jī)器人的吸引勢、吸引能，所述路徑中的障礙物對所述水下自主航行機(jī)器人的排斥勢、排斥能，所述路徑終點(diǎn)站所在原點(diǎn)和所述障礙物對所述水下自主航行機(jī)器人的合成勢、合成能的計(jì)算。

6.根據(jù)權(quán)利要求5所述的一種水下自主航行機(jī)器人自動返航方法，其特征在于，所述路徑終點(diǎn)站所在原點(diǎn)對所述水下自主航行機(jī)器人的吸引勢U_att(x)計(jì)算公式如下：

其中，所述x為所述水下自主航行機(jī)器人所在位置，所述G為所述路徑終點(diǎn)站所在原點(diǎn)位置；所述‖·‖為歐幾里德距離計(jì)算函數(shù)；所述k_att為吸引勢計(jì)算比例常數(shù)；

所述路徑終點(diǎn)站所在原點(diǎn)對所述水下自主航行機(jī)器人的吸引能F_att(x)的計(jì)算公式如下：

其中，所述u(·)是方向單位向量。