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

有效

• 申請(qǐng)?zhí)枺?/td>: 201710437709.1
• 申請(qǐng)日: 2017-06-12
• 公開（公告）號(hào): CN107301654B
• 公開（公告）日: 2020-04-03
• 發(fā)明（設(shè)計(jì)）人: 王琦;李學(xué)龍;王丞澤
• 申請(qǐng)（專利權(quán)）人: 西北工業(yè)大學(xué)
• 主分類號(hào): G06T7/246
• 分類號(hào): G06T7/246;G06T7/73;G06T17/10
• 代理公司: 西北工業(yè)大學(xué)專利中心 61204
• 代理人: 常威威

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

步驟1：輸入彩色圖像并預(yù)處理：輸入相機(jī)拍攝的彩色圖像序列，將每一幀彩色圖像轉(zhuǎn)換為灰度圖像；

步驟2：特征提?。豪肙RB特征提取算法對(duì)每一幀灰度圖像進(jìn)行特征提取，得到每一幀圖像的ORB特征點(diǎn)；

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

步驟4：輸入對(duì)應(yīng)激光雷達(dá)數(shù)據(jù)并預(yù)處理：輸入激光雷達(dá)數(shù)據(jù)，將距離大于激光雷達(dá)量程的0.8倍的點(diǎn)去掉，由剩余點(diǎn)構(gòu)成激光雷達(dá)點(diǎn)云數(shù)據(jù)；這里，假定所述的激光雷達(dá)與步驟1中拍攝彩色圖像的相機(jī)的位置關(guān)系固定、已完成聯(lián)合標(biāo)定，且二者視野相同、內(nèi)部參數(shù)已知；

步驟5：激光雷達(dá)點(diǎn)云數(shù)據(jù)的特征匹配和運(yùn)動(dòng)估計(jì)：對(duì)激光雷達(dá)點(diǎn)云數(shù)據(jù)每隔x幀進(jìn)行一次特征匹配和運(yùn)動(dòng)估計(jì)，即將每一幀激光雷達(dá)點(diǎn)云數(shù)據(jù)在水平方向上劃分為四個(gè)等大區(qū)域，每個(gè)區(qū)域根據(jù)光滑度提取4個(gè)邊緣點(diǎn)和4個(gè)平面點(diǎn)構(gòu)成特征點(diǎn)，根據(jù)光滑度對(duì)第j幀和第j-x幀數(shù)據(jù)的特征點(diǎn)進(jìn)行匹配，對(duì)匹配好的點(diǎn)對(duì)利用ICP算法進(jìn)行運(yùn)動(dòng)估計(jì)，得到激光雷達(dá)的運(yùn)動(dòng)估計(jì)序列

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

所述的邊緣點(diǎn)為區(qū)域內(nèi)光滑度最小的點(diǎn)，所述的平面點(diǎn)為區(qū)域內(nèi)光滑度最大的點(diǎn)，光滑度為其中，S代表第k次掃描中所有的點(diǎn)構(gòu)成的集合，L_k,p與L_k,q代表p與q點(diǎn)的空間坐標(biāo)，||·||表示歐式距離；所述的根據(jù)光滑度進(jìn)行匹配是指如果光滑度的差值小于當(dāng)前點(diǎn)的光滑度的5％，則匹配，否則，不匹配；

步驟6：誤差修正并記錄軌跡：用步驟5得到的激光雷達(dá)的運(yùn)動(dòng)估計(jì)替換對(duì)應(yīng)時(shí)間間隔內(nèi)相機(jī)的運(yùn)動(dòng)估計(jì)之和，得到誤差修正后的相機(jī)運(yùn)動(dòng)估計(jì)序列S′，即相機(jī)的運(yùn)動(dòng)軌跡；

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

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個(gè)特征點(diǎn)的圖像坐標(biāo)，(f_x,f_y,c_x,c_y)為給定的相機(jī)參數(shù)，d_l為第l個(gè)特征點(diǎn)在圖像拍攝時(shí)刻對(duì)應(yīng)的激光雷達(dá)點(diǎn)云中檢索到的深度值；

如果點(diǎn)云密度過大，則對(duì)點(diǎn)云地圖數(shù)據(jù)進(jìn)行降采樣，即得到最終的三維點(diǎn)云地圖；所述的點(diǎn)云密度過大是指一立方米空間中點(diǎn)的個(gè)數(shù)超過10000。