[發明專利]一種移動機器人同步定位與地圖構建方法

在審

• 申請號: 201910996670.6
• 申請日: 2019-10-19
• 公開（公告）號: CN110895146A
• 公開（公告）日: 2020-03-20
• 發明（設計）人: 傅桂霞;李濤濤;鄒國鋒;劉麗娜;杜欽君;呂傳浩;李金杰
• 申請（專利權）人: 山東理工大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20;G01S17/02;G01S17/89;G06T17/05
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明提供了一種移動機器人同步定位與地圖構建方法，包括如下步驟：（1）建立系統模型，包括機器人運動模型，觀測模型，環境特征模型；并將有色過程噪聲模型轉化為高斯白噪聲模型；（2）設定移動機器人同步定位與地圖構建算法的初始參數，包括航位推算系統參數，激光雷達參數；（3）狀態預測；（4）觀測；（5）數據關聯；（6）更新；（7）狀態增廣。其特征在于步驟（6）中，在標準容積卡爾曼濾波的量測更新過程中，選取自適應因子，自適應調整自相關協方差矩陣和互相關協方差矩陣，完成更新過程，同時能夠平衡系統模型預測信息與量測信息之間的權比，從而提高定位精度與地圖構建精度。

技術領域

本發明涉及的是一種移動機器人同步定位與地圖構建方法，特別是涉及一種基于自適應容積卡爾曼濾波的移動機器人同步定位與地圖構建方法。

背景技術

同步定位與地圖構建(simultaneous localization and mapping，簡稱：SLAM)是指移動機器人從一個未知的位置出發，在不斷運動的過程中利用自身傳感器對環境的動態感知信息遞增式構建環境地圖，并且利用環境中目標的位置計算自身的位姿。同步定位與地圖構建，最早由Smith和Cheeseman于1988年提出，用于解決移動機器人的定位與地圖構建問題，他們運用擴展卡爾曼濾波(extended Kalman filter，EKF)對狀態空間中的機器人位姿和地圖特征同時進行估計。

目前SLAM問題的解決方法以概率估計為主。最早的概率估計方法是基于擴展卡爾曼濾波的SLAM算法，此類算法存在難以解決的數據關聯問題，且計算量過大、精度不高。文獻《Unscented filtering and nonlinear estimation》(Julier S J,Uhlmann JK.Proceeding of the IEEE,2004,92(3):401-422)基于無跡卡爾曼濾波(unscentedKalman filter，簡稱：UKF)的SLAM算法，但利用UKF處理高維數(維數n≥4)系統時，為了避免非正定協方差矩陣的傳播，需要仔細“調整”UKF中的參數。為了克服UKF在高維系統中出現的數值不穩定及精度降低問題，英國萊斯特大學的研究人員在文獻《Cubature KalmanFilter based Localization and Mapping》(Kumar Pakki,Bharani Chandra,Da-Wei Gu,et al.2011, 18(part 1):2121-2125.)提出了基于容積卡爾曼濾波(Cubature KalmanFilter，簡稱：CKF)的SLAM算法，與基于無跡卡爾曼濾波的SLAM算法(簡稱：UKF-SLAM算法)相比，基于容積卡爾曼濾波的SLAM算法(簡稱：CKF-SLAM算法)對狀態的最大估計誤差降低了40％。

CKF是一種新型的非線性濾波方法，由加拿大學者Arasaratnam在文獻《CubatureKalman Filters》(IEEE Transactions on Automatic Control,2009,54(6):1254-1269.)中提出。 CKF根據容積準則，通過一組具有相同權重的點經過非線性系統方程轉換后產生新的點來給出下一時刻系統狀態的預測，避免了對非線性模型的線性化處理，其精度達3階。然而標準 CKF用于SLAM時，在遞推過程中，存在計算量大、數值不穩定等缺點。CKF-SLAM算法在精度方面仍然有提高的余地。

本發明結合容積卡爾曼濾波，提出了一種基于自適應容積卡爾曼濾波的SLAM算法(簡稱：自適應CKF-SLAM算法)。該算法通過在容積卡爾曼濾波中引入自適應因子實時調整互相關協方差矩陣和自相關協方差矩陣，自適應地平衡系統模型預測信息與量測信息之間的權比，提高濾波精度，進而提高地圖構建精度。通過實驗驗證了自適應CKF-SLAM算法，其導航定位性能優于CKF-SLAM算法和UKF-SLAM算法，證明了該算法的有效性。

發明內容