[發明專利]一種機器人地圖的構建方法、機器人的定位方法及裝置

在審

• 申請號: 202010427965.4
• 申請日: 2020-05-20
• 公開（公告）號: CN113701766A
• 公開（公告）日: 2021-11-26
• 發明（設計）人: 韓松杉;王世漢;劉星;胡孟宇;張彌;趙家陽
• 申請（專利權）人: 浙江欣奕華智能科技有限公司
• 主分類號: G01C21/32
• 分類號: G01C21/32;G01C21/30;G06K9/00;G06K9/46;G06K9/62;G06T7/70
• 代理公司: 北京同達信恒知識產權代理有限公司 11291
• 代理人: 劉彩紅

1.一種機器人地圖的構建方法，其特征在于，包括：

根據所述機器人實時同步采集到的圖像數據和慣性測量單元IMU數據，按照預設的關鍵幀數據確定規則，確定關鍵幀數據；其中所述關鍵幀數據包括：關鍵幀的位姿信息、以及關鍵幀對應圖像中的特征點數據，所述特征點為所在圖像中的角點，且與相鄰一幀圖像中的角點相匹配的角點；

根據所述關鍵幀數據，確定所述關鍵幀對應的閉環關鍵幀、以及閉環特征點對；其中所述閉環特征點對包括：位于該所述關鍵幀對應圖像中的第一特征點、以及位于對應所述閉環關鍵幀對應圖像中的第二特征點，所述第一特征點與所述第二特征點所描述的圖像信息相匹配；

在確定所述關鍵幀與對應所述閉環關鍵幀滿足預設的閉環條件時，對所述所有所述關鍵幀對應的特征點數據、所述閉環特征點對、以及所述IMU數據對進行第一集束調整BA優化處理后，得到用于表示所述機器人所在環境的地圖；

其中，所述關鍵幀數據確定規則包括：在確定出采集到的每一幀圖像包括的左目圖像和右目圖像中角點的匹配情況、以及采集到的連續兩幀圖像之間的狀態增量信息時，且對每一幀圖像進行滑窗優化處理后，根據所述滑窗優化處理的次數、以及所述特征點的重投影誤差，確定出所述關鍵幀，并確定出所述關鍵幀的位姿信息、以及所述關鍵幀對應圖像中所述特征點數據；所述狀態增量信息包括：位置增量信息、角度增量信息和速度增量信息。

2.如權利要求1所述的構建方法，其特征在于，所述閉環條件包括：

在根據所述閉環特征點對、以及預設的PNP算法，確定出所述關鍵幀與對應所述閉環關鍵幀之間的前向PNP相對姿態和后向PNP相對姿態時，所述前向PNP相對姿態與所述后向PNP相對姿態的差值小于預設的第一閾值；

其中，所述前向PNP相對姿態包括：

所述閉環關鍵幀對應圖像中的所述第二特征點的三維位置信息與對應所述關鍵幀對應圖像中對應的所述第一特征點的二維位置信息之間的相對姿態；

所述后向PNP相對姿態包括：

任一所述關鍵幀對應圖像中所述第一特征點的三維位置信息與對應所述閉環關鍵幀對應圖像中對應的所述第二特征點的二維位置信息之間的相對姿態。

3.如權利要求1所述的構建方法，其特征在于，在確定所述關鍵幀與對應所述閉環關鍵幀不滿足所述閉環條件時，還包括：

對所述關鍵幀對應的特征點數據、以及所述IMU數據對進行所述第一BA優化處理后，得到所述地圖。

4.如權利要求3所述的構建方法，其特征在于，對所述關鍵幀對應的特征點數據、以及所述IMU數據對進行所述第一BA優化處理，具體包括：

根據如下公式，對所述關鍵幀對應的特征點數據、以及所述IMU數據對進行所述第一BA優化處理：

其中，k表示所述第k幀關鍵幀，B表示在確定所述狀態增量信息時執行過預積分的關鍵幀的列表，表示采集到的左目圖像上的特征點的觀測值，表示采集到的右目圖像上的特征點的觀測值，j表示第j幀關鍵幀，P_l表示特征點l在世界坐標系中的坐標，表示在確定所述狀態增量信息時IMU數據的預積分結果，χ表示所述滑窗需要優化的狀態，表示所述狀態增量信息對應的雅克比矩陣，表示左目圖像的方差矩陣，表示右目圖像的方差矩陣，表示所述IMU數據的殘差，表示所述關鍵幀對應圖像中的左目圖像的殘差，表示所述關鍵幀對應圖像中的右目圖像的殘差。