[發明專利]一種基于協方差交叉融合SLAM的機器人定位方法及裝置

在審

• 申請號: 201910294760.0
• 申請日: 2019-04-12
• 公開（公告）號: CN110118556A
• 公開（公告）日: 2019-08-13
• 發明（設計）人: 董輝;周伯男;楊旭升;何青;俞立;陳子豪;張文安
• 申請（專利權）人: 浙江工業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 杭州君度專利代理事務所(特殊普通合伙) 33240
• 代理人: 楊天嬌

本發明公開了一種基于協方差交叉融合SLAM的機器人定位方法及裝置，通過機器人的雙目相機采集周邊環境的圖像并進行校正處理，得到校正后的圖像，對圖像進行處理，進而構建相應的地圖點，得到機器人的狀態空間模型以及機器人對路標的觀測模型，并根據前一時刻機器人位姿的優化估計值預測當前時刻機器人位姿的預測值，根據機器人對路標的觀測模型得到當前時刻的觀測量，結合當前時刻的觀測量和預測值，采用基于CI融合的濾波方法進行修正更新，得到當前時刻機器人位姿的優化估計值。本發明將協方差交叉融合算法應用于SLAM中，增強了在線性誤差方面的魯棒性，提高了濾波的精度，提升了機器人在不同實際應用環境中定位的準確性。

技術領域

本發明屬于機器人定位技術領域，尤其涉及一種基于協方差交叉融合SLAM的機器人定位方法及裝置。

背景技術

機器人的定位是指機器人在未知環境的情況下，可以得到自身在環境中所處位置。目前，機器人的定位方法可以分為非自主定位與自主定位。非自主定位是在定位的過程中機器人需要借助機器人本身以外的裝置如全球定位系統(GPS)、全局視覺系統等進行定位；而自主定位是機器人僅僅依靠機器人本身攜帶的傳感器進行定位，一般采用同時定位和地圖構建(simultaneous localization and mapping，SLAM)。在SLAM技術中，機器人從一個未知位置開始運動，利用自身攜帶的傳感器，例如攝像頭、激光雷達等來感知周圍環境并提取有用的信息，創建增量式的環境地圖，同時利用已經建立的地圖來同步刷新自身的位置。

按照初始位姿是否未知，機器人的自主定位分為初始位姿已知的位姿跟蹤(Posetracking)和初始位姿未知的全局定位(Global localization)。位姿跟蹤是在移植機器人的初始位姿的條件下，在機器人的運動過程中通過將觀測到的特征與地圖中的特征進行匹配，求取他們之間的差別，進而更新機器人的位姿的定位方法。位姿跟蹤通常采用擴展卡爾曼濾波器(Extended Kalman Filter，EKF)來實現，該方法采用高斯分布來近似地表示機器人位姿的后驗概率分布。但是在真實應用環境中，SLAM算法的應用有一個很大的困難是需要保持機器人和所有路標估計之間有完整的關聯結構，這需要花費大量的成本并且在線性誤差方面缺乏魯棒性。

發明內容

本發明的目的是提供一種基于協方差交叉融合SLAM的機器人定位方法及裝置，將協方差交叉(Covariance Intersection，CI)應用于SLAM。CI算法是分布式信息融合中不需要計算局部估計誤差之間的相關性、通過優化一定的目標函數得到的一種保守的分布式融合估計方法。其不需要尋求和計算位置的互協方差，可避免卡爾曼融合濾波的發散。將協方差交叉和SLAM算法結合，可在估計精確度和計算效率之間提供一種平衡，將給大多數實用性的系統提供最好可實現的結果。

為了實現上述目的，本發明技術方案如下：

一種基于協方差交叉融合SLAM的機器人定位方法，包括：

通過機器人的雙目相機采集周邊環境的圖像并進行校正處理，得到校正后的圖像；

對圖像進行圖像特征提取及匹配計算獲得相機的位姿，并通過雙目立體計算得到圖像匹配特征點的深度信息，進而構建相應的地圖點，得到機器人的狀態空間模型以及機器人對路標的觀測模型；

根據前一時刻機器人位姿的優化估計值預測當前時刻機器人位姿的預測值，根據機器人對路標的觀測模型得到當前時刻的觀測量，結合當前時刻的觀測量和預測值，采用基于CI融合的濾波方法進行修正更新，得到當前時刻機器人位姿的優化估計值；

不斷進行迭代，對機器人的位姿進行修正更新。

本發明的一種實現方式，所述根據前一時刻機器人位姿的優化估計值預測當前時刻機器人位姿的預測值，根據機器人對路標的觀測模型得到當前時刻的觀測量，結合當前時刻的觀測量和預測值，采用基于CI融合的濾波方法進行修正更新，得到當前時刻機器人位姿的優化估計值，包括：