[發明專利]利用半徑搜索優化激光雷達定位的方法

有效

• 申請號: 201911015902.1
• 申請日: 2019-10-24
• 公開（公告）號: CN110515055B
• 公開（公告）日: 2020-12-04
• 發明（設計）人: 楊歡;陳誠;張旸
• 申請（專利權）人: 奧特酷智能科技（南京）有限公司
• 主分類號: G01S7/48
• 分類號: G01S7/48;G01S17/02;G01S17/06
• 代理公司: 南京行高知識產權代理有限公司 32404
• 代理人: 李曉

本發明屬于道路網絡導航技術領域，提供一種利用半徑搜索優化激光雷達定位的方法，利用多線程編程方案，在另外的線程中，以輸入GPS坐標為中心，在半徑為R，高度為H的范圍內，利用方法F生成N個位姿參數，不停的嘗試匹配，直到滿足設定的匹配精度S后結束。本發明針對車輛所需要定位的位置，通過GPS粗略坐標，在數據庫中查詢當地的道路分布和車輛可達區域信息，在生成預測位姿的時候，過濾掉車輛不可達的位姿，可大大減少預測位姿的數量，從而提高匹配精度和速度。

技術領域

本發明涉及自動駕駛領域的無線電導航，具體涉及一種利用半徑搜索優化激光雷達定位的方法。

背景技術

車輛定位是自動駕駛系統中非常重要的一個環節，用于確定汽車在高精度地圖中的朝向和位置。車輛定位有多種方式，其中使用激光雷達點云進行匹配是比較常見的方式，NDT-正態分布變換（Normal Distributions Transform，NDT），以下稱ndt，是一種常見的激光雷達點云匹配算法，該算法原理是將激光雷達產生的目標點云和高精度點云地圖信息轉換為多維變量的正態分布，通過計算分布概率，若兩者匹配度較高，則認為掃描點云和地圖位姿相匹配，ndt雖然有效率高，可離線計算的優勢，但由于算法所限，對于同樣的激光雷達點云，會在高精度地圖中存在多個可能的匹配位姿點，為了提高定位的準確度，需要將激光雷達點云的粗略位姿作為輸入傳遞給ndt，目前通用的做法中，此姿態由GPS定位給出，但由于GPS精度和漂移問題，會導致每次給出的預估姿態不盡相同，從而導致有可能長時間無法達到所需要的匹配精度，甚至出現車輛定位出現丟失的情況。

發明內容

發明目的：本發明的目的在于提供一種優化的方法，提高ndt預測位姿的準確度，從而保證ndt定位的準確性和穩定性。

技術方案：為解決上述技術問題，本發明提供的利用半徑搜索優化激光雷達定位的方法，包括以下步驟：

步驟1，初始化，建立多個搜索線程。

步驟2，通過GPS獲取的粗略坐標，在可達區域和道路數據庫中查詢當地的道路分布和車輛可達區域信息；

步驟3，結合車輛狀態，過濾掉車輛不可達的位姿，在數據庫中搜索最優的位姿點生成函數F，在線程運行時，F產生位姿點P，生成預測位姿；

步驟4，利用P作為初始位姿對激光雷達點云與既有的點云地圖進行ndt匹配；

步驟5，判斷達到設定精度S后搜索線程暫停，若未達到設定進度S則返回步驟3繼續運行。

具體地，在步驟2中，根據車載GPS獲取的車身基本位置，利用地圖提供商，百度高德等或者自建地圖服務查詢得到車身周圍的可能道路信息和可能的行駛區域。獲取道路信息后，根據車載IMU或者其他傳感器獲取車身朝向信息，篩選出當前狀態下車輛最可能行駛/停靠的道路和區域。

所述步驟3中查詢結果產生后的自適應算法函數F的偽代碼如下：

{

…

Results = QueryByGPS() //根據車輛GPS查詢當前可能的道路列表

foreach (lane in Result) //循環當前的道路列表

{

A = Euler(lane) //獲取道路朝向歐拉角

B = Euler(vehicle)//獲取車輛朝向歐拉角

if (abs(A-B) N) //如果道路朝向角和車輛朝向角偏差小于N

List.Add(lane) //則將當前道路加入可選列表

}