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

在審

• 申請號: 202010511607.1
• 申請日: 2020-06-08
• 公開（公告）號: CN111795686A
• 公開（公告）日: 2020-10-20
• 發明（設計）人: 陳力軍;劉佳;牛鈺茜;邵波;沈奎林
• 申請（專利權）人: 南京大學;江蘇圖客機器人有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/18;G01C21/20;G06K9/46;G06K9/62;G06T7/70;G06K9/00
• 代理公司: 蘇州創元專利商標事務所有限公司 32103
• 代理人: 范晴;郭小紅

一種移動機器人定位與建圖的方法，包括移動機器人主體、單目相機、IMU傳感器和輪速計傳感器，單目相機鏡頭朝向所述移動機器人主體的正前方；處理單目相機獲取的每幀圖像，獲得單目相機的處理值，對每兩相鄰幀圖像間的IMU傳感器測量值進行預積分，獲得IMU傳感器的處理值，對每兩相鄰幀圖像間的輪速計傳感器測量值進行預積分，獲得輪速計傳感器的處理值；將單目相機、IMU傳感器和輪速計傳感器的處理值加入滑動窗口；獲取滑動窗口的處理值，對所述移動機器人的位姿、特征點的坐標、IMU陀螺儀偏差、線速度和尺度因子進行初始化；初始化成功后，構建因子圖模型獲得待估計狀態值，對滑動窗口的處理值和待估計狀態值進行非線性優化，得到所述移動機器人的位姿和地圖。

技術領域

本發明涉及移動機器人領域，具體涉及一種移動機器人定位與建圖的方法。

背景技術

同步定位與建圖(SLAM，Simultaneous Localization and Mapping)技術是移動機器人的關鍵技術。該技術可以讓機器人利用自身攜帶的傳感器提取到環境信息和已有地圖，從而進行定位和地圖更新。目前SLAM技術大致可以分為兩大類：激光SLAM和視覺SLAM。

激光SLAM使用的核心傳感器之一是激光雷達，視覺SLAM使用的核心傳感器之一是視覺傳感器，但是同等級激光雷達的價格普遍高于同等級的視覺傳感器。現有技術當中，視覺SLAM采用的單目相機和IMU傳感器信息融合是主流方案之一，被稱為單目視覺慣性里程計。因為該方案采用的單目相機和IMU傳感器價格較為低廉，而且技術也較為成熟，所以應用較為廣泛。該方案通過獲取預積分后的IMU測量值和單目相機對環境中特征點的觀測值，采用非線性優化方法對機器人的位姿、線速度、IMU線加速度偏差、角速度偏差以及特征點的三維坐標進行實時狀態估計。

但是，該方案在某些情況下存在額外的不可觀性。例如當機器人在平面上以固定的線加速度運動時，存在尺度因子不可觀性；當機器人在平面上沿直線運動時，存在航向角、俯仰角和橫滾角的不可觀性。某個值存在不可觀性，意味著該值可能有多個解。因此，該方案會造成移動機器人位姿估計不準確，進而無法構建高準確度的地圖。

發明內容

為了克服單目視覺慣性里程計方案在某些情況下存在額外的不可觀性造成移動機器人位姿估計不準確，進而無法構建高準確度地圖的問題，本發明在單目視覺慣性里程計方案的基礎上加入輪速計傳感器和平面約束信息，提出一種移動機器人的定位與建圖方法，可以提高移動機器人的定位精度和建圖的準確性。

基于上述目的，本發明的技術方案如下：

提供一種移動機器人定位與建圖的方法，所述移動機器人包括移動機器人主體、單目相機、IMU傳感器和輪速計傳感器，所述單目相機鏡頭朝向所述移動機器人主體的正前方，其中IMU傳感器包括IMU加速度計和IMU陀螺儀；

處理單目相機獲取的每幀圖像，獲得單目相機的處理值，對每兩相鄰幀圖像間的IMU傳感器測量值進行預積分，獲得IMU傳感器的處理值，對每兩相鄰幀圖像間的輪速計傳感器測量值進行預積分，獲得輪速計傳感器的處理值；

將所述單目相機、IMU傳感器和輪速計傳感器的處理值加入滑動窗口；獲取滑動窗口的處理值，對所述移動機器人的位姿、特征點的坐標、IMU陀螺儀偏差、線速度和尺度因子進行初始化；

初始化成功后，構建因子圖模型獲得待估計狀態值，對滑動窗口的處理值和待估計狀態值進行非線性優化最小化殘差項，得到所述移動機器人的位姿和地圖。

其中，成功完成對所述移動機器人的位姿、特征點的坐標、IMU陀螺儀偏差、線速度和尺度因子的初始化的方法包括：對所述移動機器人的位姿、特征點的坐標、IMU陀螺儀偏差、線速度和尺度因子進行初始化，若初始化失敗，對所述滑動窗口進行更新，之后再次獲取所述滑動窗口的處理值，直至成功初始化。