[发明专利]单目相机-IMU-机械臂的空间联合标定方法

权利要求书

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非线性优化计算得到。