[發明專利]一種多傳感器的高精度即時定位與建圖方法

有效

• 申請號: 201710437709.1
• 申請日: 2017-06-12
• 公開（公告）號: CN107301654B
• 公開（公告）日: 2020-04-03
• 發明（設計）人: 王琦;李學龍;王丞澤
• 申請（專利權）人: 西北工業大學
• 主分類號: G06T7/246
• 分類號: G06T7/246;G06T7/73;G06T17/10
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 常威威

1.一種多傳感器的高精度即時定位與建圖方法，其特征在于步驟如下：

步驟1：輸入彩色圖像并預處理：輸入相機拍攝的彩色圖像序列，將每一幀彩色圖像轉換為灰度圖像；

步驟2：特征提取：利用ORB特征提取算法對每一幀灰度圖像進行特征提取，得到每一幀圖像的ORB特征點；

步驟3：特征匹配和運動估計：利用近似最近鄰庫對相鄰兩幀圖像的特征點進行匹配，對匹配好的點對利用基于隨機抽樣一致法的PnP求解進行運動估計，得到相機的運動估計序列其中，為相機在拍攝第i幀和第i-1幀圖像之間的運動估計，i＝2,…,N，N為圖像幀數，R_Cam表示相機旋轉矩陣，t_Cam表示相機位移矩陣；

步驟4：輸入對應激光雷達數據并預處理：輸入激光雷達數據，將距離大于激光雷達量程的0.8倍的點去掉，由剩余點構成激光雷達點云數據；這里，假定所述的激光雷達與步驟1中拍攝彩色圖像的相機的位置關系固定、已完成聯合標定，且二者視野相同、內部參數已知；

步驟5：激光雷達點云數據的特征匹配和運動估計：對激光雷達點云數據每隔x幀進行一次特征匹配和運動估計，即將每一幀激光雷達點云數據在水平方向上劃分為四個等大區域，每個區域根據光滑度提取4個邊緣點和4個平面點構成特征點，根據光滑度對第j幀和第j-x幀數據的特征點進行匹配，對匹配好的點對利用ICP算法進行運動估計，得到激光雷達的運動估計序列

其中，為激光雷達在掃描第j幀和第j-x幀數據之間的運動估計，j＝x+1,2x+1,…,M，M為激光雷達數據總幀數，x取值范圍為[5,20]，R_Lidar表示激光雷達旋轉矩陣，t_Lidar表示激光雷達位移矩陣；

所述的邊緣點為區域內光滑度最小的點，所述的平面點為區域內光滑度最大的點，光滑度為其中，S代表第k次掃描中所有的點構成的集合，L_k,p與L_k,q代表p與q點的空間坐標，||·||表示歐式距離；所述的根據光滑度進行匹配是指如果光滑度的差值小于當前點的光滑度的5％，則匹配，否則，不匹配；

步驟6：誤差修正并記錄軌跡：用步驟5得到的激光雷達的運動估計替換對應時間間隔內相機的運動估計之和，得到誤差修正后的相機運動估計序列S′，即相機的運動軌跡；

步驟7：建立三維點云地圖：按照計算得到三維點云地圖pcl_map，其中，為第n_j幀圖像變換到第一幀圖像視角坐標系下的三維點云地圖，設第m幀激光雷達點云掃描的同一時刻相機拍攝的圖像幀序號為n_j，由n_j構成的序列為N，m＝x+1,2x+1,…,M；為第n_j幀圖像的點云地圖，L為步驟2中提取的第n_j幀圖像的特征點的個數，(x_l,y_l,z_l)表示點的坐標，按以下公式計算：

x_l＝(u_l-c_x)·d_l/f_x

y_l＝(v_l -c_y)·d_l/f_y

z_l＝d_l

其中，l＝1,…,L，(u_l,v_l)為第l個特征點的圖像坐標，(f_x,f_y,c_x,c_y)為給定的相機參數，d_l為第l個特征點在圖像拍攝時刻對應的激光雷達點云中檢索到的深度值；

如果點云密度過大，則對點云地圖數據進行降采樣，即得到最終的三維點云地圖；所述的點云密度過大是指一立方米空間中點的個數超過10000。