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

在審

• 申請號: 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
• 代理人: 范晴;郭小紅

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

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

將所述單目相機、IMU傳感器和輪速計傳感器的處理值加入滑動窗口；

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

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

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

3.根據權利要求1所述的方法，其特征在于，得到所述移動機器人的位姿和地圖后，對所述滑動窗口進行更新，對更新后的滑動窗口的處理值和待估計狀態值再次進行非線性優化，得到所述更新后的移動機器人的位姿和地圖。

4.根據權利要求2或3所述的方法，其特征在于，所述更新的方法包括：

讀取滑動窗口內的最新兩幀，判斷倒數第二幀是否為關鍵幀，若是關鍵幀，則刪除滑動窗口內的最老關鍵幀的單目相機、IMU傳感器和輪速計傳感器的處理值，并保留當前幀的單目相機、IMU傳感器和輪速計傳感器的處理值；

若不是關鍵幀，則刪除當前幀的單目相機的處理值，保留IMU傳感器和輪速計傳感器的處理值。

5.根據權利要求1所述的方法，其特征在于，所述輪速計傳感器測量值進行預積分方法包括：構建輪速計傳感器的運動測量模型、測量預積分模型、誤差傳播模型以及協方差傳播模型。

6.根據權利要求1所述的方法，其特征在于，初始化IMU陀螺儀偏差方法包括：

獲取滑動窗口內的相鄰的兩個關鍵幀，讀取關鍵幀中的單目相機坐標系、IMU傳感器坐標系、輪速計傳感器坐標系在每個關鍵幀時刻的旋轉量，將所述單目相機坐標系、IMU傳感器坐標系的旋轉量和所述單目相機坐標系、輪速計傳感器坐標的旋轉量分別構建最小二乘方程，將求出的值進行求和與平均，初始化所述IMU陀螺儀偏差。

7.根據權利要求1所述的方法，其特征在于，初始化線速度和尺度因子的方法包括：

獲取滑動窗口內的相鄰的兩個關鍵幀，讀取關鍵幀中的IMU傳感器和輪速計傳感器的預積分平移量、速度變化量；以及單目相機的平移量、速度變化量和重力分量；

將IMU傳感器和輪速計傳感器的預積分平移量、速度變化量，以及單目相機的平移量、速度變化量和重力分量構建最小二乘方程，初始化所述線速度和尺度因子。

8.根據權利要求1所述的方法，其特征在于，所述構建因子圖模型獲得待估計狀態值的方法包括：

所述殘差項包括：IMU傳感器殘差、輪速計傳感器殘差、單目相機殘差、平面殘差；所述每個關鍵幀滑動窗口的待估計狀態值包括：在世界坐標系下的平移量、速度、旋轉量、IMU加速度計偏差、IMU陀螺儀偏差、特征點逆深度；

所述IMU傳感器殘差約束所述世界坐標系下的平移量、旋轉量、速度和IMU加速度計偏差、IMU陀螺儀偏差；所述輪速計傳感器約束所述世界坐標系下的平移量、旋轉量和IMU傳感器到輪速計的外參；所述單目相機殘差約束所述世界坐標系下的特征點逆深度、平移量、旋轉量、速度和IMU加速度計偏差、IMU陀螺儀偏差、IMU到單目相機的外參；所述平面殘差約束所述世界坐標系下的平移量和旋轉量；完成所述構建因子圖模型獲得待估計狀態值。