[發(fā)明專利]一種軌跡融合方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 202010988161.1
• 申請(qǐng)日: 2020-09-18
• 公開（公告）號(hào): CN112084289B
• 公開（公告）日: 2021-08-20
• 發(fā)明（設(shè)計(jì)）人: 魯榮榮;傅文標(biāo);楊再甫;林寶尉;王彥哲
• 申請(qǐng)（專利權(quán)）人: 湖北億咖通科技有限公司
• 主分類號(hào): G06F16/29
• 分類號(hào): G06F16/29;G06T7/33;G01S19/42
• 代理公司: 北京柏杉松知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11413
• 代理人: 項(xiàng)京;高鶯然

1.一種軌跡融合方法，其特征在于，所述方法包括：

獲取待融合軌跡和基礎(chǔ)軌跡；

確定所述基礎(chǔ)軌跡的第一節(jié)點(diǎn)的節(jié)點(diǎn)數(shù)據(jù)和所述待融合軌跡的第二節(jié)點(diǎn)的節(jié)點(diǎn)數(shù)據(jù)，所述節(jié)點(diǎn)數(shù)據(jù)包括：節(jié)點(diǎn)的三維點(diǎn)云數(shù)據(jù)、節(jié)點(diǎn)的GPS數(shù)據(jù)和節(jié)點(diǎn)的位姿數(shù)據(jù)；

針對(duì)每一第二節(jié)點(diǎn)，基于該第二節(jié)點(diǎn)的GPS數(shù)據(jù)和各所述第一節(jié)點(diǎn)的GPS數(shù)據(jù)，確定該第二節(jié)點(diǎn)的鄰近第一節(jié)點(diǎn)，對(duì)該第二節(jié)點(diǎn)和所述鄰近第一節(jié)點(diǎn)進(jìn)行三維點(diǎn)云特征匹配，得到匹配節(jié)點(diǎn)對(duì)；

對(duì)所有匹配節(jié)點(diǎn)對(duì)的第二節(jié)點(diǎn)和鄰近第一節(jié)點(diǎn)進(jìn)行三維點(diǎn)云配準(zhǔn)，得到每一匹配節(jié)點(diǎn)對(duì)中第二節(jié)點(diǎn)和鄰近第一節(jié)點(diǎn)的三維點(diǎn)云之間的剛體變換矩陣，作為所述待融合軌跡和所述基礎(chǔ)軌跡之間的關(guān)聯(lián)約束；

基于每一所述匹配節(jié)點(diǎn)對(duì)中第二節(jié)點(diǎn)和鄰近第一節(jié)點(diǎn)的三維點(diǎn)云之間的剛體變換矩陣、所述基礎(chǔ)軌跡中各第一節(jié)點(diǎn)的位姿以及所述待融合軌跡中各第二節(jié)點(diǎn)的位姿，對(duì)所述待融合軌跡進(jìn)行位姿優(yōu)化，并將位姿優(yōu)化后的待融合軌跡和所述基礎(chǔ)軌跡進(jìn)行組合，得到融合軌跡。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述針對(duì)每一第二節(jié)點(diǎn)，基于該第二節(jié)點(diǎn)的GPS數(shù)據(jù)和各所述第一節(jié)點(diǎn)的GPS數(shù)據(jù)，確定該第二節(jié)點(diǎn)的鄰近第一節(jié)點(diǎn)，包括：

針對(duì)每一第二節(jié)點(diǎn)，基于哈希編碼規(guī)則將該第二節(jié)點(diǎn)的GPS數(shù)據(jù)轉(zhuǎn)換為GPS哈希碼；

基于預(yù)先構(gòu)建的第一節(jié)點(diǎn)集合與GPS哈希碼的對(duì)應(yīng)關(guān)系，從各所述第一節(jié)點(diǎn)集合中確定目標(biāo)第一節(jié)點(diǎn)集合，所述目標(biāo)第一節(jié)點(diǎn)集合的GPS哈希碼與所述第二節(jié)點(diǎn)的GPS哈希碼相同；

基于所述第二節(jié)點(diǎn)的GPS數(shù)據(jù)和所述目標(biāo)第一節(jié)點(diǎn)集合中各第一節(jié)點(diǎn)的GPS數(shù)據(jù)，從所述目標(biāo)第一節(jié)點(diǎn)集合中查找與所述第二節(jié)點(diǎn)距離最近的第一節(jié)點(diǎn)，作為該第二節(jié)點(diǎn)的鄰近第一節(jié)點(diǎn)。

3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述對(duì)該第二節(jié)點(diǎn)和所述鄰近第一節(jié)點(diǎn)進(jìn)行三維點(diǎn)云特征匹配得到匹配節(jié)點(diǎn)對(duì)的步驟，包括：

確定所述第二節(jié)點(diǎn)的三維點(diǎn)云數(shù)據(jù)，作為源點(diǎn)云數(shù)據(jù)；

確定所述鄰近第一節(jié)點(diǎn)的三維點(diǎn)云數(shù)據(jù)，作為目標(biāo)點(diǎn)云數(shù)據(jù)；

分別對(duì)所述源點(diǎn)云數(shù)據(jù)和所述目標(biāo)點(diǎn)云數(shù)據(jù)進(jìn)行關(guān)鍵點(diǎn)提取，得到源點(diǎn)云關(guān)鍵點(diǎn)和目標(biāo)點(diǎn)云關(guān)鍵點(diǎn)；

分別對(duì)所述源點(diǎn)云關(guān)鍵點(diǎn)和所述目標(biāo)點(diǎn)云關(guān)鍵點(diǎn)進(jìn)行局部特征描述，得到源點(diǎn)云關(guān)鍵點(diǎn)的局部特征信息和目標(biāo)點(diǎn)云關(guān)鍵點(diǎn)的局部特征信息；所述局部特征信息表示關(guān)鍵點(diǎn)與鄰近的其他數(shù)據(jù)點(diǎn)之間的相對(duì)位置關(guān)系；

基于所述源點(diǎn)云關(guān)鍵點(diǎn)的局部特征信息和所述目標(biāo)點(diǎn)云關(guān)鍵點(diǎn)的局部特征信息進(jìn)行同名點(diǎn)匹配，并確定同名點(diǎn)匹配對(duì)的數(shù)目；

判斷所述同名點(diǎn)匹配對(duì)的數(shù)目是否大于預(yù)設(shè)閾值，若大于，則所述第二節(jié)點(diǎn)和所述鄰近第一節(jié)點(diǎn)構(gòu)成匹配節(jié)點(diǎn)對(duì)。

4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述基于每一匹配節(jié)點(diǎn)對(duì)中第二節(jié)點(diǎn)和鄰近第一節(jié)點(diǎn)的三維點(diǎn)云之間的剛體變換矩陣、所述基礎(chǔ)軌跡中各第一節(jié)點(diǎn)的位姿以及所述待融合軌跡中各第二節(jié)點(diǎn)的位姿，對(duì)所述待融合軌跡進(jìn)行位姿優(yōu)化的步驟，包括：

從所述基礎(chǔ)軌跡的第一節(jié)點(diǎn)中確定一個(gè)標(biāo)準(zhǔn)節(jié)點(diǎn)，并將該標(biāo)準(zhǔn)節(jié)點(diǎn)的位姿確定為標(biāo)準(zhǔn)位姿；

以所述標(biāo)準(zhǔn)位姿為基準(zhǔn)，對(duì)所述基礎(chǔ)軌跡中其他第一節(jié)點(diǎn)的位姿進(jìn)行轉(zhuǎn)換，得到各其他第一節(jié)點(diǎn)相對(duì)于所述標(biāo)準(zhǔn)節(jié)點(diǎn)的第一相對(duì)位姿；

以所述標(biāo)準(zhǔn)位姿為基準(zhǔn)，對(duì)所述待融合軌跡中各第二節(jié)點(diǎn)的位姿進(jìn)行轉(zhuǎn)換，得到各第二節(jié)點(diǎn)相對(duì)于所述標(biāo)準(zhǔn)節(jié)點(diǎn)的第二相對(duì)位姿；

根據(jù)所述剛體變換矩陣、所述第一相對(duì)位姿和所述第二相對(duì)位姿，對(duì)所述待融合軌跡中各第二節(jié)點(diǎn)的所述第二相對(duì)位姿進(jìn)行優(yōu)化。

5.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述根據(jù)所述剛體變換矩陣、所述第一相對(duì)位姿和所述第二相對(duì)位姿，對(duì)所述待融合軌跡中各第二節(jié)點(diǎn)的所述第二相對(duì)位姿進(jìn)行優(yōu)化的步驟，包括：

以所述待融合軌跡中各第二節(jié)點(diǎn)的第二相對(duì)位姿作為優(yōu)化變量，構(gòu)建最小優(yōu)化誤差函數(shù)，所述最小優(yōu)化誤差函數(shù)包括第一優(yōu)化誤差、第二優(yōu)化誤差和第三優(yōu)化誤差，所述第一優(yōu)化誤差包含所述待融合軌跡中各個(gè)相鄰節(jié)點(diǎn)間的相對(duì)位姿誤差，所述第二優(yōu)化誤差包含所述待融合軌跡中預(yù)先確定的回環(huán)節(jié)點(diǎn)間的相對(duì)位姿誤差，所述第三優(yōu)化誤差包含各匹配節(jié)點(diǎn)對(duì)中第二節(jié)點(diǎn)和鄰近第一節(jié)點(diǎn)之間的相對(duì)位姿誤差；

求解所述最小優(yōu)化誤差函數(shù)，確定所述待融合軌跡中各節(jié)點(diǎn)的優(yōu)化后位姿。