[發(fā)明專(zhuān)利]基于3D NDT-ICP算法的掘進(jìn)巷道點(diǎn)云配準(zhǔn)方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202110467464.3
• 申請(qǐng)日: 2021-04-28
• 公開(kāi)（公告）號(hào): CN113077497A
• 公開(kāi)（公告）日: 2021-07-06
• 發(fā)明（設(shè)計(jì)）人: 楊健健;吳淼;常維亞
• 申請(qǐng)（專(zhuān)利權(quán)）人: 北京坤世拓智能科技有限公司
• 主分類(lèi)號(hào): G06T7/30
• 分類(lèi)號(hào): G06T7/30;G06T7/10
• 代理公司: 暫無(wú)信息
• 代理人: 暫無(wú)信息

1.基于3D NDT-ICP算法的掘進(jìn)巷道點(diǎn)云匹配方法，其特征在于，包括以下步驟：

S1、通過(guò)Voxel Grid濾波方法對(duì)掘進(jìn)巷道點(diǎn)云進(jìn)行預(yù)處理，保持點(diǎn)云整體結(jié)構(gòu)的同時(shí)降低點(diǎn)云數(shù)量；

S2、使用3D NDT算法對(duì)掘進(jìn)巷道點(diǎn)云進(jìn)行坐標(biāo)變換求解；

S3、結(jié)合掘進(jìn)巷道環(huán)境特點(diǎn)對(duì)算法單元格分辨率進(jìn)行參數(shù)優(yōu)化；

S4、將求解得到的坐標(biāo)變換參數(shù)傳遞到ICP算法，初始化ICP算法中點(diǎn)云七參數(shù)坐標(biāo)矩陣；

S5、在ICP算法中引進(jìn)KD-Tree進(jìn)行點(diǎn)對(duì)搜索；

S6、采用Gauss-Newton法對(duì)算法非線性目標(biāo)函數(shù)求解進(jìn)行優(yōu)化，完成掘進(jìn)巷道點(diǎn)云精確配準(zhǔn)。

2.根據(jù)權(quán)利要求1所述的基于3D NDT-ICP算法的掘進(jìn)巷道點(diǎn)云配準(zhǔn)方法，其特征在于，所述步驟S2具體為：

S21、首先使用3D NDT算法將基準(zhǔn)點(diǎn)云M(x,y,z)數(shù)據(jù)樣本均勻分割成若干個(gè)大小相同且規(guī)則的三維體素單元，然后通過(guò)正態(tài)分布表述三維體素單元中每個(gè)三維點(diǎn)云位置的概率分布，表達(dá)式如下所示：

式中C是每個(gè)體素單元中三維點(diǎn)云的協(xié)方差矩陣，q是每個(gè)體素單元中三維點(diǎn)云的均值；其中c為常量。q和C具體定義如下所示：

式中M_i(i＝1,......,n)表示體素單元中所有三維點(diǎn)云數(shù)據(jù)。

S22、對(duì)點(diǎn)云M和N進(jìn)行變換參數(shù)初始化：

S23、將待配準(zhǔn)點(diǎn)云N(x,y,z)的每一個(gè)點(diǎn)云樣本根據(jù)坐標(biāo)變換參數(shù)，映射到基準(zhǔn)點(diǎn)云M(x,y,z)數(shù)據(jù)樣本坐標(biāo)系中，得到映射后的點(diǎn)云記為N'，將N'中每個(gè)三維點(diǎn)映射的概率分布進(jìn)行求和，對(duì)坐標(biāo)變換的參數(shù)進(jìn)行評(píng)估：

3.根據(jù)權(quán)利要求2所述的基于3DNDT-ICP算法的掘進(jìn)巷道點(diǎn)云匹配方法，其特征在于，對(duì)于步驟S23，通過(guò)Hessian矩陣法優(yōu)化s(p)，使s(p)值達(dá)到最大。

4.根據(jù)權(quán)利要求1所述的基于3DNDT-ICP算法的掘進(jìn)巷道點(diǎn)云匹配方法，其特征在于，對(duì)于其步驟S3，3D NDT算法單元格分辨率取值通常在0.5m-2m之間，這對(duì)于激光掃描設(shè)備而言建模結(jié)果較為理想。本發(fā)明實(shí)驗(yàn)結(jié)果顯示，單元格分辨率設(shè)置為0.5m時(shí)，算法用時(shí)最短，算法s(p)值達(dá)到最大，匹配精度最高。

5.根據(jù)權(quán)利要求1所述的基于3DNDT-ICP算法的掘進(jìn)巷道點(diǎn)云匹配方法，其特征在于，其步驟S4具體為：

S41、設(shè)基準(zhǔn)點(diǎn)云M中任一點(diǎn)P(x_P,y_P,z_P)，待配準(zhǔn)點(diǎn)云N中任一點(diǎn)Q(x_P,y_P,z_P)，且P，Q滿足P∈(M∩O)，Q∈(N∩O)，通過(guò)ICP算法采用七參數(shù)空間相似變換模型進(jìn)一步描述兩個(gè)獨(dú)立3D坐標(biāo)系的外部轉(zhuǎn)換關(guān)系，即有以下公式：

其中，t_x，t_y，t_z為沿坐標(biāo)軸方向的三個(gè)分量，α，β，γ為繞坐標(biāo)軸旋轉(zhuǎn)的三個(gè)角度參數(shù)，w為坐標(biāo)系間尺度變換因子，一般默認(rèn)為1。

S42、將S3中得到的初始化后的變化參數(shù)的最終結(jié)果傳遞給ICP算法，初始化上式中的旋轉(zhuǎn)矩陣R(α,β,γ)＝R和平移矩陣[t_x t_y t_z]^T＝T；

S43、設(shè)掘進(jìn)巷道兩組點(diǎn)云共產(chǎn)生m組對(duì)應(yīng)點(diǎn)對(duì)，當(dāng)下式取得最優(yōu)解時(shí)，兩組點(diǎn)云便完成了空間位置轉(zhuǎn)換關(guān)系求解，公式如下：

6.根據(jù)權(quán)利要求1所述的基于3D NDT-ICP算法的掘進(jìn)巷道點(diǎn)云匹配方法，點(diǎn)云X軸、Y軸、Z軸旋轉(zhuǎn)誤差均控制在0.6°以內(nèi)，點(diǎn)云X軸、Y軸、Z軸平移誤差均控制在2.5cm左右。