[發明專利]一種機器人定位方法、機器人和存儲介質

有效

• 申請號: 202010042753.4
• 申請日: 2020-01-15
• 公開（公告）號: CN111256689B
• 公開（公告）日: 2022-01-21
• 發明（設計）人: 胡仁志;胡滿;杜楠枰
• 申請（專利權）人: 北京智華機器人科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18
• 代理公司: 深圳市六加知識產權代理有限公司 44372
• 代理人: 孟麗平

1.一種機器人定位方法，所述方法用于機器人，所述機器人包括位移測量裝置、慣性測量裝置和攝像裝置，其特征在于，所述方法包括：

獲得位移測量裝置測量的第一測量值，基于所述第一測量值獲得所述機器人的第一機器人位姿；

獲得慣性測量裝置測量的第二測量值，基于所述第二測量值獲得所述機器人的第二機器人位姿；

根據所述第一機器人位姿和所述第二機器人位姿獲得所述機器人的第三機器人位姿；

獲得攝像裝置拍攝的包含標志碼的圖像，基于所述標志碼的位姿和所述第三機器人位姿獲得所述標志碼的圖像觀測值；

獲得標志碼坐標系下至少一個角點的角點坐標；

基于所述標志碼的圖像觀測值獲得所述標志碼至少一個角點的角點像素坐標；

基于圖優化算法獲得所述標志碼的最優初始位姿；

獲得所述攝像裝置的最優初始位姿；

根據所述角點坐標、所述角點像素坐標、所述標志碼的最優初始位姿和所述攝像裝置的最優初始位姿、基于所述標志碼的重投影誤差最小關系獲得所述機器人的估計位姿和所述標志碼的估計位姿；

其中，所述基于圖優化算法獲得所述標志碼的最優初始位姿，包括：

建立第一圖，所述第一圖中節點為所述標志碼的位姿在圖像坐標系下的像素位姿；

根據所述像素位姿、所述角點坐標和所述角點像素坐標、基于重投影誤差最小關系獲得所述標志碼之間的最佳相對像素位姿；

建立第二圖，所述第二圖中各節點的連接邊為所述最佳相對像素位姿，并為所述連接邊設置權重；

分別以所述第二圖中各個節點作為起點，獲得所述起點的最小生成樹；

確定各節點間最佳相對像素位姿加權之和最小的最小生成樹對應的起點為初始節點；

基于佛洛伊德算法計算所述初始節點的像素位姿，對所述初始節點的像素位姿進行坐標轉換，獲得所述標志碼的最優初始位姿；

或者，所述基于圖優化算法獲得所述標志碼的最優初始位姿，包括：

建立第一圖，所述第一圖中節點為所述標志碼的位姿在圖像坐標系下的像素位姿；

根據所述像素位姿、所述角點坐標和所述角點像素坐標、基于所述標志碼的重投影誤差最小關系獲得所述標志碼之間的最佳相對像素位姿；

建立第二圖，所述第二圖中各節點的連接邊為所述最佳相對像素位姿，并為所述連接邊設置權重；

分別以所述第二圖中各個節點作為起點，獲得所述起點的最小生成樹；

確定各節點間最佳相對像素位姿加權之和最小的最小生成樹對應的起點為第一初始節點；

去除所述第二圖中異常的邊，獲得第三圖；

將所述第一初始節點作為起點和終點，獲得至少一個包含多個節點的環路；

基于所述環路中節點間相對位姿的約束關系、以及所述節點間相對位姿和所述最佳相對像素位姿滿足的加權平方差最小關系，計算節點間的相對位姿獲得節點間的最優相對像素位姿；

分別以所述第三圖中各個節點作為起點，獲得所述起點的最小生成樹；

確定各節點間最優相對像素位姿加權之和最小的最小生成樹對應的起點為第二初始節點；

基于佛洛伊德算法計算所述第二初始節點的像素位姿，對所述第二初始節點的像素位姿進行坐標轉換，獲得所述標志碼的最優初始位姿。

2.根據權利要求1所述的方法，其特征在于，所述方法還包括：

根據所述機器人的估計位姿和所述第一測量值更新所述機器人的第一機器人位姿。

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

基于所述包含標志碼的圖像獲得所述標志碼的初始位姿。

4.根據權利要求1所述的方法，其特征在于，所述獲得所述攝像裝置的最優初始位姿，包括：

根據所述角點坐標、所述角點像素坐標和所述標志碼的最優初始位姿、基于所述標志碼的重投影誤差最小關系獲得所述攝像裝置的最優初始位姿。

5.根據權利要求1所述的方法，其特征在于，所述位移測量裝置包括編碼器，所述慣性測量裝置包括陀螺儀。