[發明專利]基于粒子漂移的粒子濾波點云定位方法及其裝置和系統

有效

• 申請號: 202010555426.9
• 申請日: 2020-06-17
• 公開（公告）號: CN111915675B
• 公開（公告）日: 2023-06-23
• 發明（設計）人: 李曉歡;陳倩;姚榮彬;李春海;王對武;唐欣
• 申請（專利權）人: 廣西綜合交通大數據研究院
• 主分類號: G06T7/73
• 分類號: G06T7/73;G01S19/42;G01C21/16;G01S17/02;G01C21/00
• 代理公司: 廣州嘉權專利商標事務所有限公司 44205
• 代理人: 熊思遠

本發明公開了一種基于粒子漂移的粒子濾波點云定位方法，基于粒子濾波定位算法，提升定位的精度，包括將初始粒子集根據當前時刻的里程計數據轉換為預測粒子集，并根據漂移算法，獲取漂移粒子集，并根據所述漂移粒子集中權重大的漂移粒子和坐標信息，輸出定位結果。同時，還提供一種基于粒子漂移的粒子濾波定位系統和基于粒子漂移的粒子濾波定位裝置；所述基于粒子漂移的粒子濾波定位系統和基于粒子漂移的粒子濾波定位裝置應用所述基于粒子漂移的粒子濾波點云定位方法提升定位的精度。

技術領域

本發明涉及定位技術領域，特別是涉及一種基于粒子漂移的粒子濾波點云定位方法及其裝置和系統。

背景技術

基于高精度點云地圖的點云定位過程中，由于地圖的非實時性與傳感器噪聲，實時采集的數據與地圖之間存在不一致的問題。這一問題通常會給基于地圖匹配的定位算法的精度帶來影響，甚至造成局部最優或者定位無法收斂的情況。針對這一問題，現有算法通常從如道路兩旁的墻壁、房屋、電線桿等固定物體上提取點、線、面等特征進行定位。理想情況下，上述算法能夠在一定程度上確保實時采集數據與地圖數據的一致性。但是，大量的特征提取給定位算法帶來計算開銷，場景的變化也可能讓基于固定特征的定位算法失效。現有技術中，基于貝葉斯概率理論，常采用粒子濾波算法，充分將不一致性通過概率的形式表達，通過將定位問題轉換為預測和預測更新兩個步驟，從而對定位的不確定性有了更好的預測估計。然而，由于地圖與實時采集的數據間存在不一致且粒子具有隨機性，粒子權重的計算值并不能準確地反映其所在定位的正確性，基于粒子濾波算法的自動駕駛點云定位技術通常面臨收斂程度不高，粒子退化等問題。

發明內容

本發明旨在至少解決現有技術中存在的技術問題之一。為此，第一方面，本發明提供了一種基于粒子漂移的粒子濾波點云定位方法，提升定位的精度；第二方面，本發明提供一種基于粒子漂移的粒子濾波定位系統，采用第一方面所述基于粒子漂移的粒子濾波點云定位方法以提升定位的精度。第三方面，基于粒子漂移的粒子濾波定位裝置，采用第一方面所述的基于粒子漂移的粒子濾波點云定位方法以提升定位的精度。

第一方面，本發明提出一種基于粒子漂移的粒子濾波點云定位方法，包括如下步驟：

獲取位置數據，所述位置數據包括前一時刻的位置粒子集、當前時刻的里程計數據。

將所述位置粒子集與所述里程計數據進行狀態疊加，得到預測粒子集。

根據粒子漂移算法對所述預測粒子集中各預測粒子進行漂移，獲得漂移粒子集。

將所述漂移粒子集中各粒子進行權重處理，獲得當前時刻的位置信息。

其中，根據粒子漂移算法對所述預測粒子集中各預測粒子進行漂移，獲得漂移粒子集包括如下步驟：

獲取與所述預測粒子對應的第一觀測點云集合，以及獲取與所述第一觀測點云集合中各點最近的點的第一地圖點云集合。

設定漂移矩陣變量，根據漂移矩陣變量對所述第一觀測點云集合進行多次漂移模擬，將漂移后的所述第一觀測點云集合與所述第一地圖點云集合相似度最高的一次漂移對應的漂移矩陣變量的值設為最優漂移矩陣。

根據最優漂移矩陣對所述預測粒子進行漂移，獲得漂移粒子。

獲取所述預測粒子的第一相似度以及所述漂移粒子的第二相似度。

根據所述第一相似度和所述第二相似度的比較結果，判斷是否將所述預測粒子替換成所述漂移粒子，并形成所述漂移粒子集。

根據本發明的上述實施例，至少具有如下有益效果：由于基于粒子濾波的點云定位算法中，每個預測粒子表示一個可能的定位點。而第一地圖點云集合為圍繞該定位點真實值的周圍的點的集合。因此可以基于漂移后的第一觀測點云集合與第一地圖點云集合相似度最高得到的最優漂移矩陣，以及第一觀測點云集合和預測粒子具有相同的收斂特征，從而得到收斂度更高的漂移粒子，從而可以根據漂移粒子集進行真實位置判定，提升定位的精度。