[發明專利]基于粒子濾波器的車輛自定位方法、系統、設備和介質

在審

• 申請號: 202110941721.2
• 申請日: 2021-08-17
• 公開（公告）號: CN113483769A
• 公開（公告）日: 2021-10-08
• 發明（設計）人: 江昆;楊殿閣;馮潤澤;于偉光
• 申請（專利權）人: 清華大學
• 主分類號: G01C21/28
• 分類號: G01C21/28;G01C21/16;G01S19/45;G01S19/47;G06T7/277;G06T7/73;G06N3/08;G06N3/04
• 代理公司: 北京紀凱知識產權代理有限公司 11245
• 代理人: 冀志華

本發明涉及一種基于粒子濾波器的車輛自定位方法、系統、設備和介質，包括以下步驟：設計粒子濾波器的量測模型，該量測模型包括直接量測模型和設計量測模型；基于采集的實車數據對預先構建的車輛運動系統模型進行訓練，得到訓練好的車輛運行系統模型；基于組合慣導和視覺傳感器對車輛運動狀態進行采集，并基于車輛運動系統模型和量測模型對自車位姿進行估計，實現對車輛的定位。本發明引入視覺模塊觀測的自車到左、右車道線的橫向截距作為額外觀測量，同時可以對自車的位置及朝向進行估計。本發明可以廣泛應用于車輛自定位領域。

技術領域

本發明涉及車輛自定位領域，具體涉及一種基于量產車中傳感器(全球導航衛星系統、慣性測量單元以及視覺傳感器)使用粒子濾波器對車輛進行自定位的方法、系統、設備和存儲介質。

背景技術

車輛的精確自定位是實現L3(Level 3)級和更高級別的自動駕駛的前提，當前較為成熟的車輛高精度自定位方案主要有以下四種：1.GNSS(Global Navigation SatelliteSystem，全球導航衛星系統)結合IMU(Inertial Measurement Unit，慣性測量單元)組成的組合慣性導航定位系統；2.組合慣性導航融合RTK(Real-Time Kinematic)的融合定位系統；3.激光雷達結合點云高精度地圖的匹配定位系統；4.視覺相機結合高精度地圖的匹配定位系統。然而，由于前三種方案中要使用到的高精度的IMU、結合RTK技術的組合慣性導航以及激光雷達的成本高昂，許多汽車制造商都盡量避免在量產車上使用，所以這些方案大多數只停留于研究階段，沒有在量產車型上的實際應用。而方案4中使用的視覺傳感器價格低廉而且可以獲得豐富的語義信息和視覺信息，與高精度地圖匹配后可以進行高精度定位，同時高精度地圖的使用成本低廉，因此方案4非常適用于量產車型。

當前視覺相機結合高精度地圖的匹配定位方法可以分為基于優化的方法和基于濾波的方法，下面分別進行介紹：

基于優化的方法是尋找一個視覺檢測特征與地圖特征之間差異最小的自車位姿，該方法首先需要從視覺信息中提取到大量視覺特征點，然后將提取到的視覺特征點與點云地圖進行匹配，不斷迭代計算兩幅點云之間的位姿關系進而求解車輛的位置和姿態。然而提取視覺特征點大多使用到深度學習的方法，而對大數目點云進行迭代求解，這兩個步驟都有較大的算力需求。

基于濾波的方法是根據車輛運動模型得到自車位姿的先驗概率分布，得到觀測結果后計算得到自車位姿的后驗概率分布，進而對自車位姿進行估計。基于濾波的方法計算量小，然而當前流行的卡爾曼濾波算法只適用于線性觀測模型的情況，但是基于視覺傳感器構建的觀測模型往往是非線性的，蔡豪等人使用卡爾曼濾波算法，基于視覺傳感器構建出的線性觀測模型，只能對車輛的位置進行估計，不能對車輛的姿態進行估計。另外，基于濾波的方法需要構建車輛的運動模型，當前普遍使用勻速勻角速度(CVCT)模型或勻加速度模型等基于運動學的運動模型，但是當采樣間隔增大時存在運動模型失效的問題，例如JaeKyu Suhr提出的視覺匹配地圖定位方法將濾波頻率設置為100Hz，然而很多視覺傳感器的采樣頻率約為30Hz，因此車輛運動模型失效問題也是算法實際應用的一個障礙。

發明內容

針對上述問題，本發明的目的是提供一種基于粒子濾波器的車輛自定位方法、系統、設備和介質，利用量產車中常見傳感器(GNSS、IMU和相機)結合矢量高精度地圖進行車輛的高精度定位。

為實現上述目的，本發明采取以下技術方案：

本發明的第一個方面，是提供一種基于粒子濾波器的車輛自定位方法，其包括以下步驟：

設計粒子濾波器的量測模型，該量測模型包括直接量測模型和設計量測模型；

基于采集的實車數據對預先構建的車輛運動系統模型進行訓練，得到訓練好的車輛運行系統模型；

基于組合慣導和視覺傳感器對車輛運動狀態進行采集，并基于車輛運動系統模型和量測模型對自車位姿進行估計，實現對車輛的定位。