[發(fā)明專利]基于點(diǎn)云匹配的位姿計(jì)算方法、裝置、計(jì)算機(jī)設(shè)備及存儲(chǔ)介質(zhì)

在審

• 申請?zhí)枺?/td>: 202010344161.8
• 申請日: 2020-04-27
• 公開（公告）號(hào): CN111540015A
• 公開（公告）日: 2020-08-14
• 發(fā)明（設(shè)計(jì)）人: 莫松文;高國清;任仲超;凌云志;王遠(yuǎn)
• 申請（專利權(quán)）人: 深圳南方德爾汽車電子有限公司
• 主分類號(hào): G06T7/73
• 分類號(hào): G06T7/73;G06T7/33;G06T7/521
• 代理公司: 深圳市精英專利事務(wù)所 44242
• 代理人: 曹祥波

本發(fā)明公開了一種基于點(diǎn)云匹配的位姿計(jì)算方法、裝置、計(jì)算機(jī)設(shè)備及存儲(chǔ)介質(zhì)，方法包括以下步驟：獲取探測設(shè)備檢測目標(biāo)物的當(dāng)前幀點(diǎn)云；將當(dāng)前幀點(diǎn)云設(shè)置為參考幀；再次獲取探測設(shè)備檢測的新幀點(diǎn)云；計(jì)算由當(dāng)前幀點(diǎn)云變換至新幀點(diǎn)云的變換矩陣；根據(jù)變換矩陣的計(jì)算結(jié)果，更新參考幀，以計(jì)算目標(biāo)物的下一個(gè)位姿。與現(xiàn)有技術(shù)相比，本發(fā)明的實(shí)施例提供了一種基于點(diǎn)云匹配的位姿計(jì)算、裝置、計(jì)算機(jī)設(shè)備及存儲(chǔ)介質(zhì)，其通過ICP或NDT算法獲得前后位姿的最優(yōu)變換矩陣，從而實(shí)現(xiàn)目標(biāo)物在隧道中等的準(zhǔn)確定位問題。

技術(shù)領(lǐng)域

本發(fā)明涉及位姿計(jì)算技術(shù)領(lǐng)域，尤其涉及一種基于點(diǎn)云匹配的位姿計(jì)算方法、裝置、計(jì)算機(jī)設(shè)備及存儲(chǔ)介質(zhì)。

背景技術(shù)

傳統(tǒng)的定位方法通常采用RTK算法，但是傳統(tǒng)的RTK算法，由于隧道等內(nèi)信號(hào)較差，因此不能解決隧道中定位問題，無法準(zhǔn)確計(jì)算目標(biāo)物的位姿。

發(fā)明內(nèi)容

本發(fā)明的實(shí)施例提供了一種基于點(diǎn)云匹配的位姿計(jì)算方法、裝置、計(jì)算機(jī)設(shè)備及存儲(chǔ)介質(zhì)，旨在解決現(xiàn)有位姿計(jì)算無法在隧道中定位的問題。

為達(dá)到上述目的，本發(fā)明所提出的技術(shù)方案為：

第一方面，本發(fā)明提供了一種基于點(diǎn)云匹配的位姿計(jì)算方法，其包括以下步驟：

獲取探測設(shè)備檢測目標(biāo)物的當(dāng)前幀點(diǎn)云；

將當(dāng)前幀點(diǎn)云設(shè)置為參考幀；

再次獲取探測設(shè)備檢測的新幀點(diǎn)云；

計(jì)算由當(dāng)前幀點(diǎn)云變換至新幀點(diǎn)云的變換矩陣；

根據(jù)變換矩陣的計(jì)算結(jié)果，更新參考幀，以計(jì)算目標(biāo)物的下一個(gè)位姿。

其中，所述步驟“計(jì)算由當(dāng)前幀點(diǎn)云變換至新幀點(diǎn)云的變換矩陣”中矩陣變換的算法采用ICP算法或NDT算法。

其中，所述IPC算法包括以下步驟：

點(diǎn)云預(yù)處理，以便濾波和清理數(shù)據(jù)；

進(jìn)行點(diǎn)云配準(zhǔn)，求解變換矩陣以尋找最近點(diǎn)；

加權(quán)處理，所述加權(quán)處理用于調(diào)整對(duì)應(yīng)點(diǎn)的權(quán)重；

剔除不合理的配準(zhǔn)點(diǎn)云，以降低噪聲影響；

計(jì)算loss函數(shù)，根據(jù)loss函數(shù)獲得最優(yōu)變化矩陣。

其中，所述NDT算法包括以下步驟：

將掃描占用的空間細(xì)分為單元網(wǎng)格，基于單元網(wǎng)格內(nèi)的點(diǎn)云分布計(jì)算每個(gè)單元的PDF；

根據(jù)每個(gè)單元的PDF掃描的點(diǎn)云的每個(gè)點(diǎn)進(jìn)行轉(zhuǎn)移矩陣變換；

根據(jù)矩陣變換結(jié)果，計(jì)算響應(yīng)的概率分布函數(shù)；

根據(jù)概率分布函數(shù)，獲得最優(yōu)變換矩陣。

第二方面，本發(fā)明還提供了一種基于點(diǎn)云匹配的位姿計(jì)算裝置，其包括：

當(dāng)前幀點(diǎn)云獲取單元，用于獲取探測設(shè)備檢測目標(biāo)物的當(dāng)前幀點(diǎn)云；

參考幀設(shè)置單元，用于將當(dāng)前幀點(diǎn)云設(shè)置為參考幀；

新幀點(diǎn)云獲取單元，用于再次獲取探測設(shè)備檢測的新幀點(diǎn)云；

變換矩陣計(jì)算單元，用于計(jì)算由當(dāng)前幀點(diǎn)云變換至新幀點(diǎn)云的變換矩陣；

位姿計(jì)算單元，用于根據(jù)變換矩陣的計(jì)算結(jié)果，更新參考幀，以計(jì)算目標(biāo)物的下一個(gè)位姿。

其中，所述變換矩陣計(jì)算單元運(yùn)算并執(zhí)行ICP算法或NDT算法。

其中，所述變換矩陣計(jì)算單元包括：

預(yù)處理單元，用于點(diǎn)云預(yù)處理，以便濾波和清理數(shù)據(jù)；