[發(fā)明專利]一種水下機(jī)器人的位置估計(jì)方法及裝置

有效

• 申請?zhí)枺?/td>: 202010978540.2
• 申請日: 2020-09-17
• 公開（公告）號: CN112083377B
• 公開（公告）日: 2023-03-17
• 發(fā)明（設(shè)計(jì)）人: 姜言清;崔林濤;李曄;馬騰;李柯垚;張蔚欣;李岳明;曹建;武皓微
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G01S5/18
• 分類號: G01S5/18
• 代理公司: 北京隆源天恒知識產(chǎn)權(quán)代理有限公司 11473
• 代理人: 吳航

1.一種水下機(jī)器人的位置估計(jì)方法，其特征在于，包括：

獲取水下機(jī)器人當(dāng)前時刻的傳感位置信息；

若所述當(dāng)前時刻的傳感位置信息為有效傳感信息，則確定跟隨設(shè)備在當(dāng)前時刻下連續(xù)接收到所述有效傳感信息的連續(xù)通信次數(shù)；

根據(jù)所述連續(xù)通信次數(shù)，確定初始狀態(tài)粒子集，進(jìn)而確定所述水下機(jī)器人當(dāng)前時刻的預(yù)估位置，以使所述跟隨設(shè)備對所述水下機(jī)器人進(jìn)行跟蹤定位，其中，所述初始狀態(tài)粒子集包括第二初始粒子集；

所述根據(jù)所述連續(xù)通信次數(shù)，確定初始狀態(tài)粒子集包括：當(dāng)所述連續(xù)通信次數(shù)等于預(yù)設(shè)次數(shù)時，根據(jù)所述水下機(jī)器人的歷史航跡對所述跟隨設(shè)備在當(dāng)前時刻下連續(xù)N個時刻接收到的所述有效傳感信息進(jìn)行初始化修正，確定所述第二初始粒子集；

所述根據(jù)所述水下機(jī)器人的歷史航跡對所述跟隨設(shè)備連續(xù)N個時刻接收到的所述有效傳感信息進(jìn)行初始化修正，確定所述第二初始粒子集包括，根據(jù)所述跟隨設(shè)備在當(dāng)前時刻下連續(xù)N個時刻接收到的所述有效傳感信息，確定所述連續(xù)N個時刻中每個時刻對應(yīng)的一組運(yùn)動狀態(tài)值；根據(jù)所述連續(xù)N個時刻中每個時刻對應(yīng)的所述一組運(yùn)動狀態(tài)值，確定所述連續(xù)N個時刻中每個時刻對應(yīng)的第一狀態(tài)粒子，所述連續(xù)N個時刻的所述第一狀態(tài)粒子構(gòu)成候選粒子集，其中，所述初始狀態(tài)粒子集包括所述候選粒子集；根據(jù)所述歷史航跡對所述候選粒子集中的所述第一狀態(tài)粒子進(jìn)行權(quán)重修正，確定所述第二初始粒子集，其中，N等于所述預(yù)設(shè)次數(shù)；

所述根據(jù)所述歷史航跡對所述候選粒子集中的所述第一狀態(tài)粒子進(jìn)行權(quán)重修正，確定所述第二初始粒子集包括：

根據(jù)所述歷史航跡，確定所述連續(xù)N個時刻中每個時刻對應(yīng)的一組預(yù)測狀態(tài)值；

根據(jù)所述連續(xù)N個時刻中每個時刻對應(yīng)的所述一組預(yù)測狀態(tài)值，確定所述連續(xù)N個時刻中每個時刻對應(yīng)的預(yù)測粒子，所述連續(xù)N個時刻的所述預(yù)測粒子構(gòu)成預(yù)測狀態(tài)集；

根據(jù)所述候選粒子集和所述預(yù)測狀態(tài)集之間的粒子轉(zhuǎn)移概率，確定所述候選粒子集中每個所述第一狀態(tài)粒子對應(yīng)的修正權(quán)值；

根據(jù)每個所述第一狀態(tài)粒子和對應(yīng)的所述修正權(quán)值，確定修正后的第二狀態(tài)粒子，所述連續(xù)N個時刻的所述第二狀態(tài)粒子構(gòu)成所述第二初始粒子集，所述修正權(quán)值表示為其中，表示第i個第一狀態(tài)粒子和預(yù)測狀態(tài)集之間粒子轉(zhuǎn)移概率，表示N個第一狀態(tài)粒子的轉(zhuǎn)移概率轉(zhuǎn)移之和。

2.如權(quán)利要求1所述的水下機(jī)器人的位置估計(jì)方法，其特征在于，還包括：

若所述當(dāng)前時刻的傳感位置信息為無效傳感信息，則根據(jù)所述水下機(jī)器人的歷史航跡，確定所述水下機(jī)器人當(dāng)前時刻的預(yù)估位置。

3.如權(quán)利要求1所述的水下機(jī)器人的位置估計(jì)方法，其特征在于，所述根據(jù)所述連續(xù)通信次數(shù)，確定初始狀態(tài)粒子集，進(jìn)而確定所述水下機(jī)器人當(dāng)前時刻的預(yù)估位置包括：

根據(jù)所述初始狀態(tài)粒子集建立對應(yīng)的交互式多模型，將所述初始狀態(tài)粒子集作為所述交互式多模型的初始輸入，確定所述水下機(jī)器人當(dāng)前時刻的預(yù)估位置。

4.如權(quán)利要求3所述的水下機(jī)器人的位置估計(jì)方法，其特征在于，所述根據(jù)所述連續(xù)通信次數(shù)，確定初始狀態(tài)粒子集包括：

當(dāng)所述連續(xù)通信次數(shù)大于預(yù)設(shè)次數(shù)時，根據(jù)所述跟隨設(shè)備在當(dāng)前時刻下連續(xù)N個時刻接收到的所述有效傳感信息，確定所述連續(xù)N個時刻中每個時刻對應(yīng)的一組運(yùn)動狀態(tài)值，其中，N等于所述預(yù)設(shè)次數(shù)；

根據(jù)所述連續(xù)N個時刻中每個時刻對應(yīng)的所述一組運(yùn)動狀態(tài)值，確定所述連續(xù)N個時刻中每個時刻對應(yīng)的第一狀態(tài)粒子，所述連續(xù)N個時刻的所述第一狀態(tài)粒子構(gòu)成所述候選粒子集。

5.如權(quán)利要求3所述的水下機(jī)器人的位置估計(jì)方法，其特征在于，所述根據(jù)所述初始狀態(tài)粒子集建立對應(yīng)的交互式多模型，將所述初始狀態(tài)粒子集作為所述交互式多模型的初始輸入，確定所述水下機(jī)器人當(dāng)前時刻的預(yù)估位置包括：

將所述初始狀態(tài)粒子集中的每個狀態(tài)粒子輸入至設(shè)置的多個濾波模型中，以進(jìn)行模型交互；

根據(jù)所述有效傳感信息確定狀態(tài)觀測值，并根據(jù)所述狀態(tài)觀測值更新每個所述濾波模型的模型權(quán)重；

根據(jù)每個所述濾波模型的所述模型權(quán)重進(jìn)行數(shù)據(jù)融合，輸出所述水下機(jī)器人當(dāng)前時刻的預(yù)估位置。