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

在審

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

1.一種移動機器人同步定位與地圖構建方法，包括如下步驟：

(1)建立系統模型，包括機器人運動模型，觀測模型，環境特征模型；并將有色過程噪聲模型轉化為高斯白噪聲模型；

(2)設定移動機器人同步定位與地圖構建算法的初始參數，包括航位推算系統參數，激光雷達參數；

(3)狀態預測：

狀態預測過程是基于標準容積卡爾曼濾波的時間更新過程：

其中，P_k-1|k-1為k-1時刻的誤差協方差；S_k-1|k-1為P_k-1|k-1喬里斯基因子；X_i,k-1|k-1為容積點集；m＝2n，n為狀態向量的維數；記n維單位列向量e＝[1,0,…,0]^T，使用符號[1]表示對e的元素進行全排列和改變元素符號產生的點集，稱為完整全對稱點集，[1]_i表示點集[1]中的第i個點；為k-1時刻的狀態估計值；為通過狀態方程傳播的容積點集；f(·)為非線性狀態方程；為k時刻的狀態預測值；P_k|k-1為k時刻的誤差協方差預測值；Q_k-1為系統噪聲協方差；

(4)觀測：

通過第(1)步構建的機器人觀測模型獲取觀測數據，即獲取傳感器探測范圍內機器人探測到的路標特征，并計算出探測到的每個路標特征到機器人的距離r_i及其與機器人前進方向的夾角θ_i；

(5)數據關聯：

采用獨立兼容最近鄰算法完成數據關聯；

其特征在于，還包括如下步驟(6)，在標準容積卡爾曼濾波的量測更新的基礎上，選取自適應因子α_k，自適應調整自相關協方差矩陣和互相關協方差矩陣，完成更新過程，同時能夠平衡系統模型預測信息與量測信息之間的權比，從而提高定位精度與地圖構建精度；

(6)更新：

如果第(5)步數據關聯計算結果表明，新的測量對應著一個已經存在于地圖中的特征時，需要利用觀測信息，對已有特征的狀態進行更新，該步驟基于標準容積卡爾曼濾波的量測更新過程：

z_i,k|k-1＝h(X_i,k|k-1)

其中，S_k|k-1為P_k|k-1的喬里斯基因子；X_i,k|k-1為容積點集；z_i,k|k-1為通過量測方程傳播的容積點集；h(·)為非線性量測函數；為k時刻的量測預測值；

選取自適應因子α_k，并采用自適應因子α_k自適應調整自相關協方差矩陣和互相關協方差矩陣，自適應因子α_k選為：

其中，γ_k為預測殘差，且協方差矩陣P為：自適應因子α_k的取值范圍為：0≤α_k≤1，α_k初始值設為1；如果α_k取值合理，它能夠基于預測殘差γ_k，利用量測信息自適應地調節平衡系統模型預測信息與量測信息之間的權比；當CKF初值選擇存在偏差或者系統模型存在異常擾動時，α_k將小于1，即系統模型預測信息在最終濾波結果所占的權值將盡量??；當系統模型預測信息明顯異常時，α_k將趨于0，即系統模型預測信息被棄用；

采用自適應因子α_k自適應調整自相關協方差矩陣和互相關協方差矩陣：

其中，P_zz,k|k-1為自相關協方差矩陣；R為量測噪聲協方差；P_xz,k|k-1為互相關協方差矩陣；K_k為卡爾曼增益；為k時刻的狀態估計值；z_k為k時刻的量測值；P_k|k為k時刻的狀態誤差協方差估計值；

如果第(5)步數據關聯計算結果表明新的測量未對應地圖中已存在的特征，即觀測到一個新特征時，需要對此特征在地圖中進行初始化，進而進行狀態增廣，進入第(7)步；

(7)增量式構圖：

在遞推的每一步都進行系統模型噪聲的白化處理；利用數據關聯完成路標特征探測和地圖匹配；通過狀態增廣不斷擴大地圖，遞推地進行同步定位與地圖構建。