[發明專利]單目相機-IMU-機械臂的空間聯合標定方法

有效

• 申請號: 202110584637.X
• 申請日: 2021-05-27
• 公開（公告）號: CN113298881B
• 公開（公告）日: 2023-09-12
• 發明（設計）人: 張吟龍;梁煒;張思超;夏曄;劉帥;李世明;楊雨沱
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: G06T7/80
• 分類號: G06T7/80;G06T7/73
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 周宇

1.一種單目相機-IMU-機械臂的空間聯合標定方法，其特征在于，包括以下步驟：

步驟1：單目相機和IMU單元固定在機械臂末端，設定機械臂運動軌跡，即機械臂末端執行器坐標系{R}下的位置變化：控制機械臂帶動末端的IMU單元和單目相機移動；移動過程中，IMU單元自動獲取與機械臂運動軌跡相對應的IMU實際移動的姿態序列，即IMU坐標系{I}下的位置變化：{I₁→I₂→I₃→…}，單目相機拍攝標定板目標圖像，并實時定位單目相機坐標系{C}下的位置：{C₁→C₂→C₃→…}；

步驟2：對單目相機、IMU、機械臂兩兩之間進行分別標定，獲取單目相機坐標系{C}、IMU坐標系{I}、機械臂末端執行器坐標系{R}兩兩之間的空間變換關系；

步驟3：根據上述三個坐標系之間的變換關系獲取統一框架下的聯合標定模型，判斷模型是否收斂，若不收斂則返回步驟1按照設定的機械臂運動軌跡控制機械臂末端執行器再次運動執行建模步驟直到模型收斂結束；從而獲取優化的單目相機-IMU-機械臂的空間聯合標定模型；所述獲取優化的單目相機-IMU-機械臂的空間聯合標定模型具體為：

變換矩陣T_RC、T_RI、T_IC滿足以下幾何關系：

其中表示從{C}到{R}的空間變換關系；

表示從{I}到{R}的空間變換關系；表示從{C}到{I}的空間變換關系；

該聯合標定方法通過評估變換矩陣和單位矩陣I_4×4之間的相似性來判斷標定是否達到收斂標準；表示根據測量結果計算得到的轉換矩陣，λ_n，n＝1,2,3,4表示變換矩陣在每個軸上的特征值：

|λ_nE-A|＝0??????????????(4.2)

其中E是四階的單位矩陣，通過以下的方式，將λ_n，n＝1,2,3,4與對應于單位矩陣I_4×4的每個軸上的特征值1進行比較，從而判斷標定結果是否收斂：

λ_n≤1，n＝1,2,3,4??????????(4.3)。

2.根據權利要求1所述的一種單目相機-IMU-機械臂空間聯合標定方法，其特征在于，所述分別標定包括三個步驟：

步驟2.1：通過最小化IMU旋轉和單目相機旋轉之間的差異值來計算偏差，在迭代過程中計算角速度偏差估計用于估計單目相機-IMU旋轉矩陣計算加速度偏差估計用于估計單目相機-IMU平移向量，并迭代更新相關參數，獲取單目相機-IMU聯合標定；

步驟2.2：通過單目相機跟隨機械臂運動的位姿，獲取單目相機-機械臂聯合標定；

步驟2.3：通過IMU跟隨機械臂運動的位姿，獲取IMU-機械臂聯合標定。

3.根據權利要求2所述的一種單目相機-IMU-機械臂空間聯合標定方法，其特征在于，包括：步驟2.1.1：角速度偏差估計：

單目相機在時刻t到t+1之間的旋轉由下式給出：

其中與分別表示t和t+1時刻從世界坐標系{W}到IMU坐標系{I}的旋轉矩陣；同樣，和分別表示t和t+1時刻從世界坐標系{W}到單目相機坐標系{C}的旋轉矩陣；時刻t到t+1之間的單目相機旋轉與IMU預積分的關系表示為：

其中表示t和t+1時刻IMU預積分后的旋轉增量；是SO(3)的右雅克比行列式；是t時刻從單目相機坐標系{C}到世界坐標系{W}的旋轉矩陣；是t+1時刻從世界坐標系{W}到單目相機坐標系{C}的旋轉矩陣；和分別是估算得到的從IMU坐標系{I}到單目相機坐標系{C}的旋轉矩陣和從單目相機坐標系{C}到IMU坐標系{I}的旋轉矩陣；表示t和t+1時刻預積分后的旋轉增量；是估算得到的角速度偏差；通過使用對數運算，b_t^g被最小化為：

該式通過Levenberg-Marquart非線性優化計算得到。