单目相机-IMU-机械臂的空间联合标定方法技术

本发明专利技术提出了一种单目相机‑IMU‑机械臂的空间联合标定方法。将单目相机和惯性测量单元(IMU)固定在机械臂末端执行器附近，可以有效地协助机械臂进行运动估计和场景感知。单目相机、IMU和机械臂之间可靠、准确的标定是实现随动感知的前提。该方法包括单目相机‑IMU联合标定、单目相机‑机械臂联合标定、IMU‑机械臂联合标定和统一框架下的联合标定四个步骤。这种方法能够可靠且准确地关联单目相机、IMU和机械臂之间的空间关系。该方法采用先估计旋转矩阵，再估计平移向量的方式来解决平移向量误差易耦合到旋转矩阵估计的问题。本发明专利技术方法适用于对单目相机、IMU和机械臂进行联合标定，具有稳定、准确的优势。

【技术实现步骤摘要】
单目相机-IMU-机械臂的空间联合标定方法
本专利技术涉及多源信息融合技术和图像处理技术，具体地说是一种单目相机-IMU-机械臂的空间联合标定方法。
技术介绍
智能化的机械臂通常配有多模态感知单元，比如视觉传感器、惯性传感器，触觉传感器、力觉传感器等，通过多模态信息采集与处理来实现机械臂对自身和周围环境的感知功能。虽然市场上已有的感知模块，比如深度相机、激光雷达和双目相机能够有效做出载体的运动估计和深度估计，但由于其感知空间受限、反射敏感、感知单元尺寸过大等原因而无法适用于机械臂随动感知系统。而单目相机、IMU在感知精度和响应速度方面具有互补的优势。单目相机能够采集丰富的场景信息，在定位和导航领域应用广泛。IMU是测量物体三轴角速度和加速度的装置，它提供了类似于人类内部前庭系统一样的自身惯性信息，在导航中有着重要的应用价值。所以将单目相机和IMU固定在机械臂末端，可以有效地协助机械臂进行运动估计和场景感知。单目相机、IMU和机械臂之间可靠、准确的标定是实现随动感知的前提，因此，本专利技术提出一种单目相机-IMU-机械臂的联合标定方法。
技术实现思路
本专利技术提出了一种单目相机-IMU-机械臂的空间联合标定方法，该方法能够实现三者的有效融合。所采用的技术方案是：一种单目相机-IMU-机械臂的空间联合标定方法，包括以下步骤：步骤1：单目相机和IMU单元固定在机械臂末端，设定机械臂运动轨迹，即机械臂末端执行器坐标系{R}下的位置变化：控制机械臂带动末端的IMU单元和单目相机移动；移动过程中，IMU单元自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列，即IMU坐标系{I}下的位置变化：{I1→I2→I3→…}，单目相机拍摄标定板目标图像，并实时定位单目相机坐标系{C}下的位置：{C1→C2→C3→…}；步骤2：对单目相机、IMU、机械臂两两之间进行分别标定，获取单目相机坐标系{C}、IMU坐标系{I}、机械臂末端执行器坐标系{R}两两之间的空间变换关系；步骤3：根据上述三个坐标系之间的变换关系获取统一框架下的联合标定模型，判断模型是否收敛，若不收敛则返回步骤1按照设定的机械臂运动轨迹控制机械臂末端执行器再次运动执行建模步骤直到模型收敛结束；从而获取优化的单目相机-IMU-机械臂的空间联合标定模型。所述分别标定包括三个步骤：步骤2.1：通过最小化IMU旋转和单目相机旋转之间的差异值来计算偏差，在迭代过程中计算角速度偏差估计用于估计单目相机-IMU旋转矩阵计算加速度偏差估计用于估计单目相机-IMU平移向量，并迭代更新相关参数，获取单目相机-IMU联合标定；步骤2.2：通过单目相机跟随机械臂运动的位姿，获取单目相机-机械臂联合标定；步骤2.3：通过IMU跟随机械臂运动的位姿，获取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时刻预积分后的旋转增量；是估算得到的角速度偏差；通过使用对数运算，可以被最小化为：该式通过Levenberg-Marquart非线性优化计算得到。包括：步骤2.1.2：单目相机-IMU旋转矩阵估计：旋转矩阵通过将时刻t到t+1之间的单目相机旋转与预积分处理后的IMU旋转对齐得到；假设在时间间隔[t,t+1]之间，单目相机与IMU的姿态变化分别为和尽管在不同的参考坐标系中，和仍然表示相同的旋转；将单目相机坐标系转换为IMU坐标系的四元数其满足以下关系：其中表示四元数的共轭，然后，通过将以下函数最大化来获得最佳四元数然后将公式(1.6)得到的最佳四元数转换为矩阵形式，得到最优估计的单目相机与IMU间的旋转矩阵包括：步骤2.1.3：加速度偏差估计：在时间间隔[i,j]内，单目相机平移和IMU预积分平移之间的关系为：其中，是估算得到的单目相机与IMU之间的旋转矩阵；表示单目相机在i时刻和j时刻之间的平移；表示时刻t相对于i帧的速度；Δt是IMU采样率；表示IMU在i时刻和j时刻之间的旋转；最佳的加速度偏差通过最小化得到：其中表示第i帧和第t帧之间的旋转矩阵，它被表示为包括：步骤2.1.4：单目相机-IMU平移向量估计：此处给定加速度偏差和角速度偏差单目相机-IMU旋转矩阵得到单目相机坐标系{C}到IMU坐标系{I}的平移向量为：其中是时间间隔[i,j]的惯性平移量，它可以通过IMU预积分得到；是时间间隔[i,j]的单目相机平移量，它可以通过标定板特征点跟踪得到。步骤2.2：单目相机-机械臂联合标定具体实现为：假设机械臂从位置运动到单目相机也将随着机械臂从C1运动到C2，然后获得以下公式：其中XRC表示从单目相机坐标系{C}到机械臂末端执行器坐标系{R}的变换矩阵，经过转换后得到：其中表示机械臂末端执行器的相对运动；同样，C2C1-1表示单目相机的相对运动，通过计算编码器测量值得到；C2C1-1通过计算标定板坐标系(也称为世界坐标系{W})相对于单目相机坐标系{C}来得到；同样地，将机械臂末端执行器旋转到另一个位置，得到：相对旋转矩阵RRC和平移向量tRC通过以下公式得到：其中和分别对应于的旋转和平移；和分别对应于C2C1-1的旋转和平移；同样，和分别对应于的旋转和平移；和分别代表C3C2-1的旋转和平移；由于旋转矩阵通过角轴的形式表示为RR＝RR(kR,θR)(kR是旋转轴矢量，θR是旋转角度的比例因子)，所以旋转矩阵RRC满足如下等式：转换得到：通过将式(2.9)代入到式和其中i＝1,2，得到：并得到平移向量tRC：tRC＝(ATA)-1ATB(2.11)其中，A和B表示如下：步骤2.3：IMU-机械臂联合标定具体为：IMU-机械臂的标定通过齐次变换矩阵XRI来制定，机械臂以设计好的轨迹运动，即同时，IMU也按照类似的轨迹(IMU姿态序列)进行运动，即{I1→I2→I3→…}；与公式(本文档来自技高网...

【技术保护点】
1.一种单目相机-IMU-机械臂的空间联合标定方法，其特征在于，包括以下步骤：/n步骤1：单目相机和IMU单元固定在机械臂末端，设定机械臂运动轨迹，即机械臂末端执行器坐标系{R}下的位置变化：

【技术特征摘要】
1.一种单目相机-IMU-机械臂的空间联合标定方法，其特征在于，包括以下步骤：
步骤1：单目相机和IMU单元固定在机械臂末端，设定机械臂运动轨迹，即机械臂末端执行器坐标系{R}下的位置变化：控制机械臂带动末端的IMU单元和单目相机移动；移动过程中，IMU单元自动获取与机械臂运动轨迹相对应的IMU实际移动的姿态序列，即IMU坐标系{I}下的位置变化：{I1→I2→I3→…}，单目相机拍摄标定板目标图像，并实时定位单目相机坐标系{C}下的位置：{C1→C2→C3→…}；
步骤2：对单目相机、IMU、机械臂两两之间进行分别标定，获取单目相机坐标系{C}、IMU坐标系{I}、机械臂末端执行器坐标系{R}两两之间的空间变换关系；
步骤3：根据上述三个坐标系之间的变换关系获取统一框架下的联合标定模型，判断模型是否收敛，若不收敛则返回步骤1按照设定的机械臂运动轨迹控制机械臂末端执行器再次运动执行建模步骤直到模型收敛结束；从而获取优化的单目相机-IMU-机械臂的空间联合标定模型。


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时刻预积分后的旋转增量；是估算得到的角速度偏差；通过使用对数运算，可以被最小化为：



该式通过Levenberg-Marquart非线性优化计算得到。


4.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.2：单目相机-IMU旋转矩阵估计：
旋转矩阵通过将时刻t到t+1之间的单目相机旋转与预积分处理后的IMU旋转对齐得到；
假设在时间间隔[t,t+1]之间，单目相机与IMU的姿态变化分别为和尽管在不同的参考坐标系中，和仍然表示相同的旋转；将单目相机坐标系转换为IMU坐标系的四元数其满足以下关系：



其中表示四元数的共轭，然后，通过将以下函数最大化来获得最佳四元数



然后将公式(1.6)得到的最佳四元数转换为矩阵形式，得到最优估计的单目相机与IMU间的旋转矩阵





5.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.3：加速度偏差估计：
在时间间隔[i,j]内，单目相机平移和IMU预积分平移之间的关系为：



其中，是估算得到的单目相机与IMU之间的旋转矩阵；表示单目相机在i时刻和j时刻之间的平移；表示时刻t相对于i帧的速度；Δt是IMU采样率；表示IMU在i时刻和j时刻之间的旋转；
最佳的加速度偏差通过最小化得到：



其中表示第i帧和第t帧之间的旋转矩阵，它被表示为


6.根据权利要求2所述的一种单目相机-IMU-机械臂空间联合标定方法，其特征在于，包括：步骤2.1.4：单目相机-IMU平移向量估计：
此处给定加速度偏差和角速度偏差单目相机-IMU旋转矩阵得到...

【专利技术属性】
技术研发人员：张吟龙，梁炜，张思超，夏晔，刘帅，李世明，杨雨沱，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：辽宁;21