[發明專利]基于多目視覺測量與激光點云地圖匹配的位姿估計方法

有效

• 申請號: 202011212309.9
• 申請日: 2020-11-03
• 公開（公告）號: CN112288813B
• 公開（公告）日: 2022-06-21
• 發明（設計）人: 張宇;萬澤宇
• 申請（專利權）人: 浙江大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G01C21/00;G01C21/20;G01C11/00
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 劉靜

本發明公開了一種基于多目視覺測量與激光點云地圖匹配的位姿估計方法，簡稱多目激光位姿估計方法，該方法包括：組成多目視覺測量系統，多目視覺測量系統中相機數量至少三個，每個相機至少與其它兩個相機共視，當三個及以上相機的擺放位置在同一直線上時，該直線必須與其中某兩個相機恢復的環境空間的近似平面相交。獲取有共視區域的圖片，重建近似平面，恢復環境空間在近似平面前后的誤差分布信息，計算近似平面到相機的真實距離，得到多目視覺測量模型。針對激光點云地圖，邊緣化過擬合模型，對邊緣化模型的點進行重新分配，生成點云的連續函數模型。基于表面掃描原理，對多目視覺測量模型和點云的連續函數模型進行對齊配準，實現位姿估計。

技術領域

本發明涉及機器人導航領域，尤其涉及一種基于多目視覺測量與激光點云地圖匹配的位姿估計方法。

背景技術

傳統單目視覺方法恢復的點云存在尺度未知的問題，這極大的制約了該方法的實用性。傳統雙目相機恢復點云的方法依賴于兩個相機的基線進行三角測量，然而使用廣角相機時只能恢復中近距離的點云，遠處的點沒有視差，無法恢復；使用長焦相機時，可以恢復遠距離的點，但恢復的精度及其依賴于這兩個相機之間標定的外參數，外參數的微小差異都可能導致恢復的遠處點離它的真實值相去甚遠。因此為克服恢復點云依賴于標定外參數的缺點，本發明提出了三目及其以上的多目視覺測量恢復點云的方法。激光點云地圖精度高但較為稀疏，視覺測量所恢復點云精度低但較為稠密，針對對齊配準這兩種性質特殊點云的問題，本發明提出了邊緣化過擬合模型的算法，對兩種點云提取模型擬合，邊緣化過擬合的模型后再對齊配準，從而提高精度。

發明內容

鑒于此，本發明提供一種基于多目視覺測量與激光點云地圖匹配的位姿估計方法，通過特殊的多目系統配置與測量，得到較為稠密、精度略低的環境點云數據，而激光掃描得到是較為稀疏、精度較高的環境點云地圖，因此使用邊緣化過擬合模型的期望最大化算法，將視覺點云和激光點云表達為連續函數模型，使用表面掃描原理進行模型與模型之間的對齊配準，對齊時優化算法只保留表面法向量方向的誤差項。

本發明的目的是通過以下技術方案來實現的：一種基于多目視覺測量與激光點云地圖匹配的位姿估計方法，該方法包括以下步驟：

(1)組成多目視覺測量系統，所述多目視覺測量系統中相機數量至少三個，每個相機至少與其它兩個相機共視，當三個及以上相機的擺放位置在同一直線上時，該直線必須與其中某兩個相機恢復的環境空間的近似平面相交；

(2)獲取有共視區域的圖片，重建近似平面，恢復環境空間在近似平面前后的誤差分布信息，計算近似平面到相機的真實距離，得到多目視覺測量模型；

(3)針對激光點云地圖，邊緣化過擬合模型，對邊緣化模型的點進行重新分配，生成點云的連續函數模型；

(4)基于表面掃描原理，對多目視覺測量模型和點云的連續函數模型進行對齊配準，實現位姿估計。

進一步地，所述步驟(2)具體為：獲取有共視區域的圖片，根據多視圖幾何原理，恢復所觀測環境空間的同一近似平面，恢復環境空間在近似平面前后的誤差分布信息，根據空間中任意兩點在不同距離相機觀測，產生透視投影比例關系，恢復近似平面到相機的真實距離，恢復環境空間的真實三維點云結構。

進一步地，通過計算有共視區域圖片的單應性矩陣，恢復近似平面的方程表達式，單應性矩陣的計算可用如下兩種方式：

(a)提取特征點并匹配的特征點法：對有共視區域的A張圖片均提取特征點，其中第a張圖片提取N對特征點對A張圖片兩兩之間進行特征點匹配，構建以單應性矩陣H為變量的代價函數并進行最小化，如下：

其中特征點q_ia是特征點的匹配對應點；

(b)采用圖片光度誤差信息的直接法：初始時，將單應性矩陣H設置為滿足單應性矩陣條件的隨機矩陣，構建以單應性矩陣H為變量的代價函數并進行最小化，如下：