本公開的實施例公開了用于占據柵格子圖的閉環檢測方法和裝置，涉及自動駕駛技術領域。該方法的一具體實施方式包括：獲取當前掃描的激光點云數據和已建立的占據柵格子圖；將已建立的占據柵格子圖轉換成至少一種分辨率的占據柵格子圖；根據至少一種分辨率的占據柵格子圖構建樹形結構，其中，樹形結構中的每層節點表征一種分辨率下的位姿搜索空間，分辨率從根節點向下遞增，每個節點表征一種位姿；從樹形結構的根節點開始，逐層計算各節點表征的位姿與激光點云數據的匹配度，匹配度高的節點優先進入下一層節點繼續計算匹配度，直到在葉子節點匹配得到最佳位姿。該實施方式能夠有效減少閉環檢測中搜索時間，進而減小累計誤差。

技術領域

本公開的實施例涉及自動駕駛技術領域，具體涉及用于占據柵格子圖的閉環檢測的方法和裝置。

背景技術

在傳統的激光SLAM(simultaneous?localization?and?mapping，同步定位與建圖)系統中，當前幀的位姿估計是依賴于之前幀的位姿估計，整個定位過程中誤差會不斷積累。這些誤差在小尺度的運動過程中并不明顯，但是隨著運動距離的加長，累積的誤差很快會達到一個較大的量級。因此通過閉合回環約束定位軌跡，有效消除累計誤差。

發明內容

在激光SLAM系統中，每傳入一幀激光雷達掃描的數據，與最近建立的子圖（Submap）去進行匹配，匹配方法是采用高斯牛頓解最小二乘優化問題，使這一幀的激光雷達掃描數據插入到當前子圖上最優的位置，這個過程叫做掃描匹配。隨著持續插入新的激光雷達掃描數據，?子圖得到了更新，當子圖達到一定數量的激光雷達掃描數據后，不再有新的激光雷達掃描數據插入，則此子圖已經建立完成。接著去創建下一個子圖。通過創建大量的子圖來實現大規模場景地圖建立。?通過掃描匹配得到子圖的位姿估計在短時間內是準確可靠的，但長時間會存在累積誤差，需要通過閉環檢測（也稱回環檢測）來優化所有子圖的位姿，減小累計誤差。閉環檢測每隔一段時間進行，所有創建完成的子圖以及當前幀激光雷達掃描數據都會用作閉環檢測的掃描匹配。一般來說，閉環檢測的掃描匹配策略是根據當前幀激光雷達掃描數據，設定其附近子圖鄰域作為搜索空間。在搜索空間內尋找最優的匹配位姿，使得當前幀激光雷達掃描數據在該位姿下的得分最高（即與子圖環境最相似），此時找到閉環，這個匹配結果就會被用來作為位姿優化問題的閉環檢測約束條件。然而，此閉環檢測的掃描匹配策略是一種暴力的解法，即在當前激光掃描與其附近搜索空間范圍內中的每一幀進行匹配計算得分值找出其中最大值作為匹配結果。這種匹配搜索效率低，耗時和計算量大。

發明內容

本公開的實施例提出了用于占據柵格子圖的閉環檢測的方法和裝置。

第一方面，本公開的實施例提供了一種用于占據柵格子圖的閉環檢測的方法，包括：獲取當前掃描的激光點云數據和已建立的占據柵格子圖；將已建立的占據柵格子圖轉換成至少一種分辨率的占據柵格子圖；根據至少一種分辨率的占據柵格子圖構建樹形結構，其中，樹形結構中的每層節點表征一種分辨率下的位姿搜索空間，分辨率從根節點向下遞增，每個節點表征一種位姿；從樹形結構的根節點開始，逐層計算各節點表征的位姿與激光點云數據的匹配度，匹配度高的節點優先進入下一層節點繼續計算匹配度，直到在葉子節點匹配得到最佳位姿。

在一些實施例中，該方法還包括：基于最佳位姿進行激光點云數據和已建立的占據柵格子圖的拼接，得到優化后的位姿。

在一些實施例中，該方法還包括：獲取與優化后的位姿在位姿搜索空間中相鄰的至少一個候選位姿；將至少一個候選位姿分別與激光點云數據進行一致性校驗。

在一些實施例中，該方法還包括：根據校驗成功的位姿對已建立的占據柵格子圖加入圖優化。

在一些實施例中，該方法還包括：獲取當前車輛的GPS信息；根據GPS信息對已建立的占據柵格子圖進行裁剪，以縮小位姿搜索空間。