[發(fā)明專利]一種激光雷達(dá)的自動標(biāo)定方法

有效

• 申請?zhí)枺?/td>: 202110169963.4
• 申請日: 2021-02-05
• 公開（公告）號: CN112882000B
• 公開（公告）日: 2023-02-03
• 發(fā)明（設(shè)計(jì)）人: 徐冬;江敏;楊荃;王曉晨;劉洋;孫友昭;何海楠
• 申請（專利權(quán)）人: 北京科技大學(xué)
• 主分類號: G01S7/497
• 分類號: G01S7/497;G06T7/73
• 代理公司: 北京市廣友專利事務(wù)所有限責(zé)任公司 11237
• 代理人: 張仲波

1.一種激光雷達(dá)的自動標(biāo)定方法，其特征在于：包括步驟如下：

(1)完成原始數(shù)據(jù)的初始標(biāo)定與數(shù)據(jù)過濾；

(2)通過環(huán)境數(shù)據(jù)擬合平面進(jìn)行第一次標(biāo)定，并對過濾數(shù)據(jù)進(jìn)行一次位姿變換；

(3)添加約束條件，通過第一次標(biāo)定數(shù)據(jù)擬合平面進(jìn)行第二次標(biāo)定，并再次對完成一次位姿變換的數(shù)據(jù)進(jìn)行一次位姿變換，完成標(biāo)定；

(4)若雷達(dá)位置變化、標(biāo)定線位置變化，則重復(fù)步驟(1)-步驟(3)；

所述步驟(1)中具體為：以庫區(qū)地面與標(biāo)定塊垂直面交線左端點(diǎn)作為臨時(shí)坐標(biāo)系原點(diǎn)，將原始點(diǎn)云數(shù)據(jù)進(jìn)行位姿變換，使數(shù)據(jù)原點(diǎn)與臨時(shí)坐標(biāo)系原點(diǎn)重合，記為P^s，公式為：

其中,x_i,y_i,z_i分別為第i個(gè)點(diǎn)云的x,y,z坐標(biāo)，為點(diǎn)云數(shù)量；s_x,s_y,s_z分別為臨時(shí)坐標(biāo)系原點(diǎn)的x,y,z坐標(biāo)；分別為位姿變換后點(diǎn)云數(shù)據(jù)中第i個(gè)點(diǎn)云的x,y,z坐標(biāo)，n^s為位姿變換后的點(diǎn)云數(shù)量；

將位姿變換后點(diǎn)云數(shù)據(jù)P^s，分別繞y軸、x軸和z軸旋轉(zhuǎn)角度θ_y、θ_x和θ_z，記為P^z，公式為：

P^z＝T^zT^xT^yP^s

其中，θ_y、θ_x和θ_z為已知的量角器測量的兩坐標(biāo)系繞y軸、x軸和z軸的相對旋轉(zhuǎn)角度；分別為旋轉(zhuǎn)變換后點(diǎn)云數(shù)據(jù)中第i個(gè)點(diǎn)云的x,y,z坐標(biāo)，n^z為旋轉(zhuǎn)變換后的點(diǎn)云數(shù)量；

將旋轉(zhuǎn)后點(diǎn)云數(shù)據(jù)P^z遍歷并過濾，記為P^f，過濾公式為：

其中，X_min,X_max,Y_min,Y_max分別為臨時(shí)坐標(biāo)系原點(diǎn)擴(kuò)大±2000mm范圍的x及y閾值；Z_min為庫區(qū)地面以下z閾值，Z_max為取標(biāo)定塊高度的1.5倍的z閾值；分別為過濾之后點(diǎn)云數(shù)據(jù)中的第j個(gè)點(diǎn)云的x,y,z坐標(biāo)，n^f為過濾后的點(diǎn)云數(shù)量；

所述步驟(3)中具體為：遍歷步驟(2)中一次位姿變換后點(diǎn)云數(shù)據(jù)P^dt，根據(jù)標(biāo)定塊閾值h^b提取標(biāo)定數(shù)據(jù)，記為P^b，提取公式為：

其中，h^b為標(biāo)定塊閾值，取容許識別偏差大小0.5-1.5倍；為P^dt中第i個(gè)點(diǎn)的y坐標(biāo)；分別為提取之后點(diǎn)云數(shù)據(jù)中第j個(gè)點(diǎn)云的x,y,z坐標(biāo)，n^b為提取之后的標(biāo)定數(shù)據(jù)點(diǎn)云數(shù)量；

通過標(biāo)定數(shù)據(jù)P^b，計(jì)算擬合標(biāo)定塊系數(shù)b₀、b₁，平面擬合公式為：

其中，∑x_i、∑y_i為別為點(diǎn)云數(shù)據(jù)x,y坐標(biāo)之和；為點(diǎn)云數(shù)據(jù)x坐標(biāo)平方之和；∑x_iy_i為點(diǎn)云數(shù)據(jù)x,y坐標(biāo)相乘之和；n^b為提取之后的標(biāo)定數(shù)據(jù)點(diǎn)云數(shù)據(jù)數(shù)量；b₀、b₁分別為擬合平面的各項(xiàng)系數(shù)；

根據(jù)擬合標(biāo)定塊系數(shù)，計(jì)算標(biāo)定參數(shù)，標(biāo)定參數(shù)計(jì)算公式分別為：

其中，b₀、b₁分別為擬合平面的各項(xiàng)系數(shù)；y_t為y軸位移，θ_zt為z軸旋轉(zhuǎn)角度；

將步驟(2)中變換后的點(diǎn)云數(shù)據(jù)P^dt，通過計(jì)算后的標(biāo)定參數(shù)進(jìn)行位姿變換，完成標(biāo)定，記為P^bt，位姿變換公式為：

P^bt＝T^tzT^ts2P^dt

其中，y_t為y軸位移，θ_zt為z軸旋轉(zhuǎn)角度；分別為位姿變換后點(diǎn)云數(shù)據(jù)中第i個(gè)點(diǎn)云的x,y,z坐標(biāo)，n^bt為通過標(biāo)定參數(shù)位姿變換后的點(diǎn)云數(shù)量。