[發明專利]位姿誤差修正方法及裝置、機器人及存儲介質

有效

• 申請號: 201710354828.0
• 申請日: 2017-05-18
• 公開（公告）號: CN107030699B
• 公開（公告）日: 2020-03-10
• 發明（設計）人: 陽方平
• 申請（專利權）人: 廣州視源電子科技股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京品源專利代理有限公司 11332
• 代理人: 孟金喆

1.一種位姿誤差修正方法，其特征在于，包括：

在每個采樣時刻觸發拍攝功能，并獲取機械臂目標位置的位姿狀態預測值以及位姿狀態最小均方誤差，其中，拍攝的目標對象與所述機械臂目標位置的位姿相對固定，所述目標對象設置在所述機械臂目標位置；

對所述位姿狀態預測值以及位姿狀態最小均方誤差進行修正，以得到修正預測值和修正最小均方誤差；

根據所述修正預測值和修正最小均方誤差確定對應采樣時刻拍攝的圖像中所述目標對象的拍攝區域位姿；

所述獲取機械臂目標位置的位姿狀態預測值以及位姿狀態最小均方誤差包括：

獲取對應采樣時刻機械臂目標位置的測量參數和狀態參數；

根據所述狀態參數確定位姿狀態預測值以及位姿狀態最小均方誤差；

相應的，所述對所述位姿狀態預測值以及位姿狀態最小均方誤差進行修正，以得到修正預測值和修正最小均方誤差包括：

根據所述位姿狀態最小均方誤差和所述測量參數確定誤差增益；

根據所述誤差增益、所述測量參數和所述狀態參數對所述位姿狀態預測值以及所述位姿狀態最小均方誤差進行修正。

2.根據權利要求1所述的位姿誤差修正方法，其特征在于，所述在每個采樣時刻觸發拍攝功能，并獲取機械臂目標位置的位姿狀態預測值以及位姿狀態最小均方誤差之前，還包括：

對機械臂目標位置進行運動規劃，以在每個采樣時刻時，根據運動規劃結果確定所述目標位置位姿的運動參數，所述運動參數屬于所述狀態參數。

3.根據權利要求1所述的位姿誤差修正方法，其特征在于，所述狀態參數包括：上一采樣時刻的修正最小均方誤差、上一采樣時刻的修正預測值、采樣周期、當前采樣時刻目標位置位姿的運動參數以及預設第一協方差矩陣。

4.根據權利要求3所述的位姿誤差修正方法，其特征在于，所述根據所述狀態參數確定所述位姿狀態預測值以及位姿狀態最小均方誤差包括：

根據所述采樣周期、所述上一采樣時刻的修正預測值和所述當前采樣時刻目標位置位姿的運動參數確定所述位姿狀態預測值；

根據所述采樣周期、所述上一采樣時刻的修正最小均方誤差和所述預設第一協方差矩陣確定所述位姿狀態最小均方誤差。

5.根據權利要求3所述的位姿誤差修正方法，其特征在于，所述測量參數包括：預設觀測矢量和預設第二協方差矩陣。

6.根據權利要求5所述的位姿誤差修正方法，其特征在于，所述根據所述誤差增益、所述測量參數和所述狀態參數對所述位姿狀態預測值以及所述位姿狀態最小均方誤差進行修正包括：

根據所述位姿狀態預測值、所述預設觀測矢量和所述預設第二協方差矩陣確定位姿測量值；

根據所述采樣周期、所述誤差增益、所述預設觀測矢量和所述位姿測量值對所述位姿狀態預測值進行修正，以得到修正預測值；

根據所述誤差增益和所述預設觀測矢量對所述位姿狀態最小均方誤差進行修正，以得到修正最小均方誤差。

7.一種位姿誤差修正裝置，其特征在于，包括：

參數獲取模塊，用于在每個采樣時刻觸發拍攝功能，并獲取機械臂目標位置的位姿狀態預測值以及位姿狀態最小均方誤差，其中，拍攝的目標對象與所述機械臂目標位置的位姿相對固定，所述目標對象設置在所述機械臂目標位置；

修正模塊，用于對所述位姿狀態預測值以及位姿狀態最小均方誤差進行修正，以得到修正預測值和修正最小均方誤差；

位姿確定模塊，用于根據所述修正預測值和修正最小均方誤差確定對應采樣時刻拍攝的圖像中所述目標對象的拍攝區域位姿；

參數獲取模塊具體包括：位置參數獲取單元，用于獲取對應采樣時刻機械臂目標位置的測量參數和狀態參數；參數確定單元，用于根據狀態參數確定位姿狀態預測值以及位姿狀態最小均方誤差；

相應的，修正模塊具體包括：誤差增益確定單元，用于根據位姿狀態最小均方誤差和測量參數確定誤差增益；參數修正單元，用于根據誤差增益、測量參數和狀態參數對位姿狀態預測值以及位姿狀態最小均方誤差進行修正。