[發(fā)明專(zhuān)利]一種位姿預(yù)測(cè)方法及裝置

在審

• 申請(qǐng)?zhí)枺?/td>: 202210093752.1
• 申請(qǐng)日: 2022-01-26
• 公開(kāi)（公告）號(hào): CN114593735A
• 公開(kāi)（公告）日: 2022-06-07
• 發(fā)明（設(shè)計(jì)）人: 陳星鑫;龐敏健;萬(wàn)培珮;劉賢焯
• 申請(qǐng)（專(zhuān)利權(quán)）人: 奧比中光科技集團(tuán)股份有限公司
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20;G01C21/34;G06Q10/04
• 代理公司: 深圳市恒申知識(shí)產(chǎn)權(quán)事務(wù)所(普通合伙) 44312
• 代理人: 廖厚琪

1.一種位姿預(yù)測(cè)方法，其特征在于，所述方法包括:

獲取目標(biāo)設(shè)備前一時(shí)刻的運(yùn)動(dòng)速度參數(shù)；其中，所述運(yùn)動(dòng)速度參數(shù)包括目標(biāo)設(shè)備的角速度和線加速度，所述前一時(shí)刻為所述目標(biāo)設(shè)備丟失視覺(jué)信息前的一歷史時(shí)刻；

利用所述運(yùn)動(dòng)速度參數(shù)和預(yù)設(shè)位移預(yù)測(cè)模型，估計(jì)所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的預(yù)測(cè)位移增量；其中，所述當(dāng)前時(shí)刻為所述目標(biāo)設(shè)備丟失視覺(jué)信息時(shí)的最新時(shí)刻及丟失視覺(jué)信息后的每個(gè)時(shí)刻；

根據(jù)所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的所述預(yù)測(cè)位移增量計(jì)算所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻對(duì)應(yīng)的預(yù)測(cè)位姿，并構(gòu)建所述目標(biāo)設(shè)備的預(yù)測(cè)運(yùn)動(dòng)軌跡；

對(duì)所述目標(biāo)設(shè)備的所述預(yù)測(cè)位姿及所述預(yù)測(cè)運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化，獲取所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的目標(biāo)位姿及對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)軌跡。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述利用所述運(yùn)動(dòng)速度參數(shù)和預(yù)設(shè)位移預(yù)測(cè)模型，估計(jì)所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的預(yù)測(cè)位移增量，包括：

根據(jù)所述目標(biāo)設(shè)備前一時(shí)刻的設(shè)備坐標(biāo)系下的角速度，確定目標(biāo)旋轉(zhuǎn)矩陣；

利用所述目標(biāo)旋轉(zhuǎn)矩陣，將所述設(shè)備坐標(biāo)系下的角速度和線加速度轉(zhuǎn)換為世界坐標(biāo)系下的角速度和線加速度；

將所述世界坐標(biāo)系下的角速度和線加速度輸入所述預(yù)設(shè)位移預(yù)測(cè)模型，得到所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的預(yù)測(cè)位移增量。

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述目標(biāo)設(shè)備的預(yù)測(cè)位移增量包括X軸上的預(yù)測(cè)位移增量、Y軸上的預(yù)測(cè)位移增量和Z軸上的預(yù)測(cè)位移增量；

所述根據(jù)所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的所述預(yù)測(cè)位移增量計(jì)算所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻對(duì)應(yīng)的預(yù)測(cè)位姿，包括：

根據(jù)所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻在X軸上的預(yù)測(cè)位移增量、Y軸上的預(yù)測(cè)位移增量、Z軸上的預(yù)測(cè)位移增量以及所述目標(biāo)設(shè)備在前一時(shí)刻的位置信息，確定所述目標(biāo)設(shè)備的在X軸上的預(yù)測(cè)位置、Y軸上的預(yù)測(cè)位置和Z軸上的預(yù)測(cè)位置；

根據(jù)所述目標(biāo)設(shè)備的在X軸上的預(yù)測(cè)位置、Y軸上的預(yù)測(cè)位置和Z軸上的預(yù)測(cè)位置，確定所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的預(yù)測(cè)位姿。

4.根據(jù)權(quán)利要求1至3中任一所述的方法，其特征在于，所述預(yù)設(shè)位移預(yù)測(cè)模型包括多個(gè)級(jí)聯(lián)的卷積層和一個(gè)輸出層；其中，所述輸出層與所述多個(gè)級(jí)聯(lián)的卷積層中最后一級(jí)卷積層相連接，并且，所述輸出層由全局平均池化層和卷積層串聯(lián)組成；所述位移預(yù)測(cè)模型的損失函數(shù)為均方誤差損失函數(shù)。

5.根據(jù)權(quán)利要求1至3中任一所述的方法，其特征在于，在所述獲取目標(biāo)設(shè)備在前一時(shí)刻的運(yùn)動(dòng)速度參數(shù)的步驟之前，所述方法還包括：

判斷是否檢測(cè)到所述目標(biāo)設(shè)備的視覺(jué)信息；

若未檢測(cè)到所述目標(biāo)設(shè)備的視覺(jué)信息，則執(zhí)行所述獲取目標(biāo)設(shè)備在前一時(shí)刻的運(yùn)動(dòng)速度參數(shù)的步驟。

6.根據(jù)權(quán)利要求5所述的方法，其特征在于，所述對(duì)所述目標(biāo)設(shè)備的所述預(yù)測(cè)位姿及所述預(yù)測(cè)運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化，獲取所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的目標(biāo)位姿及對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)軌跡，還包括：

判斷所述目標(biāo)設(shè)備的視覺(jué)信息是否滿(mǎn)足重定位條件，若滿(mǎn)足，則根據(jù)所述視覺(jué)信息中的圖像幀對(duì)所述目標(biāo)設(shè)備的預(yù)測(cè)位姿及預(yù)測(cè)運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化，得到目標(biāo)位姿及目標(biāo)運(yùn)動(dòng)軌跡；若不滿(mǎn)足，則繼續(xù)執(zhí)行所述獲取目標(biāo)設(shè)備在前一時(shí)刻的運(yùn)動(dòng)速度參數(shù)的步驟；其中，所述重定位條件為檢測(cè)到所述視覺(jué)信息后，判斷所述視覺(jué)信息中的圖像幀是否和預(yù)先建立的全局地圖或者局部地圖中的一幀圖像幀匹配成功。

7.根據(jù)權(quán)利要求6所述的方法，其特征在于，所述對(duì)所述目標(biāo)設(shè)備的所述預(yù)測(cè)位姿及所述預(yù)測(cè)運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化，獲取所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的目標(biāo)位姿及對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)軌跡，包括：

根據(jù)所述視覺(jué)信息中的圖像幀，確定目標(biāo)設(shè)備各當(dāng)前時(shí)刻對(duì)應(yīng)的理想位姿；

根據(jù)所述理想位姿與未檢測(cè)到視覺(jué)信息之前所估計(jì)的預(yù)測(cè)位姿對(duì)所述預(yù)測(cè)運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化，確定所述目標(biāo)運(yùn)動(dòng)軌跡；

根據(jù)所述目標(biāo)運(yùn)動(dòng)軌跡對(duì)所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的預(yù)測(cè)位姿進(jìn)行位姿優(yōu)化，得到所述目標(biāo)設(shè)備各當(dāng)前時(shí)刻的目標(biāo)位姿。