[發(fā)明專利]一種基于相機和激光雷達融合的里程計估計方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201910793566.7
• 申請日: 2019-08-27
• 公開（公告）號: CN110501021B
• 公開（公告）日: 2021-01-29
• 發(fā)明（設計）人: 謝成鋼;施成浩;于清華;肖軍浩;盧惠民
• 申請（專利權）人: 中國人民解放軍國防科技大學
• 主分類號: G01C22/00
• 分類號: G01C22/00;G01S17/88;G06K9/00;G06K9/62
• 代理公司: 長沙國科天河知識產權代理有限公司 43225
• 代理人: 董惠文

1.一種基于相機和激光雷達融合的里程計估計方法，其特征在于，包括如下步驟：

步驟101，獲取相機視場圖像與激光雷達視場圖像，建立基于殘差的里程計算模型；

步驟102，在相機視場圖像與激光雷達視場圖像共視區(qū)選擇若干具有顯著特征的特征點作為路標點，將與所述特征點距離最近的激光落點的深度作為該特征點的深度坐標，從而獲取所有路標點的三維坐標，具體包括：

步驟201，獲取相機視場圖像中與激光雷達視場圖像共視區(qū)二維的特征點；

步驟202，在所有的特征點中篩選出滿足深度插值條件的特征點作為待插值點，基于強度梯度的梯度直方圖在特征點中選取待插值點，具體為滿足下式的特征點為待插值點：

式中，p(c_j,c_i)表示待插值點的置信度，c_i表示所有的圖像點，c_j表示激光與相機的共識點，(u,v)表示點的像素坐標，(r,g,b)表示像素點的RGB顏色信息，表示像素點的方差，表示像素顏色的方差；

步驟203，獲取與待插值點距離最短的激光落點，并根據(jù)所述激光落點在激光雷達視場圖像中的位置獲得該激光落點的深度，將該所述激光落點的深度插值于該激光落點對應的待插值點，即獲取三維的路標點；

步驟103，根據(jù)所述路標點的三維坐標與里程計算模型獲取里程計。

2.根據(jù)權利要求1所述基于相機和激光雷達融合的里程計估計方法，其特征在于，步驟101中，所述基于殘差的里程計算模型為：

式中，e_rep,i表示重投影殘差，e_pho,j表示光度測量殘差，和表示觀測噪聲協(xié)方差，a_on表示世界坐標系下已添加的路標點，a_wn表示圖像中對應的路標點，φ表示相對運動。

3.根據(jù)權利要求1所述基于相機和激光雷達融合的里程計估計方法，其特征在于，步驟201具體包括：

步驟301，基于光度信息在相機視場圖像內提取第一類特征點；

步驟302，基于ORB算法在相機視場圖像內提取第二類特征點；

步驟303，合并第一類特征點與第二類特征點，即為相機視場圖像中二維的特征點。

4.根據(jù)權利要求3所述基于相機和激光雷達融合的里程計估計方法，其特征在于，

步驟301中，第一類特征點具體為相機視場圖像中灰度梯度大于閾值的點。

5.一種基于相機和激光雷達融合的里程計估計系統(tǒng)，包括：存儲器和處理器，所述基于相機和激光雷達融合的里程計估計程序，所述處理器在運行所述程序時執(zhí)行所述權利要求1～4任一項方法所述的步驟。