[發(fā)明專利]一種基于UWB與IMU融合的多機(jī)器人協(xié)同定位方法

有效

• 申請?zhí)枺?/td>: 202210177993.4
• 申請日: 2022-02-25
• 公開（公告）號: CN114554392B
• 公開（公告）日: 2023-05-16
• 發(fā)明（設(shè)計(jì)）人: 黃政杰;吳元清;席星;江向民;王維鋼;鐘文鍵;李俊;王堯欣
• 申請（專利權(quán)）人: 新基線（江蘇）科技有限公司
• 主分類號: H04W4/02
• 分類號: H04W4/02;G01C21/20;G01C21/16
• 代理公司: 廣東省中源正拓專利代理事務(wù)所(普通合伙) 44748
• 代理人: 黨沖

1.一種基于UWB與IMU融合的多機(jī)器人協(xié)同定位方法，其特征在于，應(yīng)用于多機(jī)器人協(xié)同定位系統(tǒng)的多個(gè)機(jī)器人中的目標(biāo)機(jī)器人，所述目標(biāo)機(jī)器人為多個(gè)機(jī)器人中任意一個(gè)機(jī)器人，所述目標(biāo)機(jī)器人包括超寬帶技術(shù)UWB模塊和慣性測量單元IMU模塊，所述方法包括：

通過所述目標(biāo)機(jī)器人的IMU模塊和其它各個(gè)機(jī)器人的IMU模塊，獲取所述目標(biāo)機(jī)器人的預(yù)測坐標(biāo)和其它各個(gè)機(jī)器人的預(yù)測坐標(biāo)，根據(jù)預(yù)測坐標(biāo)計(jì)算所述目標(biāo)機(jī)器人與其它各個(gè)機(jī)器人之間的距離，作為第一距離；

將所述目標(biāo)機(jī)器人的UWB模塊作為定位標(biāo)簽，將其它各個(gè)機(jī)器人的UWB模塊作為定位基站，測量所述目標(biāo)機(jī)器人與其它各個(gè)機(jī)器人之間的距離，作為第二距離；

針對每一第二距離，計(jì)算該第二距離與該第二距離對應(yīng)的第一距離之間的誤差距離；

若所述誤差距離大于預(yù)設(shè)閾值，則將該第一距離作為該第二距離對應(yīng)的目標(biāo)距離；

若所述誤差距離不大于預(yù)設(shè)閾值，則將該第二距離作為該第二距離對應(yīng)的目標(biāo)距離；

根據(jù)多個(gè)目標(biāo)距離和預(yù)設(shè)定位算法確定所述目標(biāo)機(jī)器人的當(dāng)前的位置信息。

2.根據(jù)權(quán)利要求1所述的一種基于UWB與IMU融合的多機(jī)器人協(xié)同定位方法，其特征在于，在通過所述目標(biāo)機(jī)器人的IMU模塊和其它各個(gè)機(jī)器人的IMU模塊，獲取所述目標(biāo)機(jī)器人的預(yù)測坐標(biāo)和其它各個(gè)機(jī)器人的預(yù)測坐標(biāo)，根據(jù)預(yù)測坐標(biāo)計(jì)算所述目標(biāo)機(jī)器人與其它各個(gè)機(jī)器人之間的距離，作為第一距離之前，所述方法還包括：

通過所述目標(biāo)機(jī)器人的IMU模塊持續(xù)獲取所述目標(biāo)機(jī)器人當(dāng)前的運(yùn)動動態(tài)信息；所述運(yùn)動動態(tài)信息包括所述目標(biāo)機(jī)器人的線速度和角速度；

通過預(yù)設(shè)航位推演算法、所述運(yùn)動動態(tài)信息和所述目標(biāo)機(jī)器人的初始位置，獲得所述目標(biāo)機(jī)器人當(dāng)前的估計(jì)位置；所述估計(jì)位置包括所述目標(biāo)機(jī)器人當(dāng)前的預(yù)測坐標(biāo)和運(yùn)動航向。

3.根據(jù)權(quán)利要求2所述的一種基于UWB與IMU融合的多機(jī)器人協(xié)同定位方法，其特征在于，所述方法還包括：

根據(jù)所述目標(biāo)機(jī)器人當(dāng)前的位置信息，對所述目標(biāo)機(jī)器人的IMU模塊進(jìn)行累積誤差矯正。

4.根據(jù)權(quán)利要求3所述的一種基于UWB與IMU融合的多機(jī)器人協(xié)同定位方法，其特征在于，所述目標(biāo)機(jī)器人包括多個(gè)UWB模塊；

根據(jù)多個(gè)目標(biāo)距離和預(yù)設(shè)定位算法確定所述目標(biāo)機(jī)器人的當(dāng)前的位置信息，包括：

針對每一UWB模塊，根據(jù)該UWB模塊對應(yīng)的多個(gè)目標(biāo)距離和預(yù)設(shè)定位算法，確定該UWB模塊的當(dāng)前的目的坐標(biāo)；

根據(jù)多個(gè)UWB模塊的目的坐標(biāo)，確定所述目標(biāo)機(jī)器人的坐標(biāo)和目標(biāo)航向，作為當(dāng)前的位置信息。