单目相机-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＝(a^ta)^-1a^tb(2.11)

其中，a和b表示如下：

步骤2.3：imu-机械臂联合标定具体为：

imu-机械臂的标定通过齐次变换矩阵xri来制定，机械臂以设计好的轨迹运动，即同时，imu也按照类似的轨迹(imu姿态序列)进行运动，即{i1→i2→i3→…}；

与公式(2.1)类似，imu和机械臂的位姿关系满足以下公式：

经转换可得：

以类似的方法，imu与机械臂之间的相对旋转矩阵rri和平移向量tri可以通过求解以下各式得到：

其中，和分别对应于机械臂末端执行器坐标系变换的旋转和平移；和分别对应于imu坐标系变换i2i1^-1的旋转和平移；同样，和分别对应于机械臂末端执行器坐标系变换的旋转和平移；和分别对应于imu坐标系变换i3i2^-1的旋转和平移。

所述步骤3获取优化的单目相机-imu-机械臂的空间联合标定模型具体为：

变换矩阵trc、tri、tic满足以下几何关系：

其中表示从{c}到{r}的空间变换关系；表示从{i}到{r}的空间变换关系；表示从{c}到{i}的空间变换关系；

该联合标定方法通过评估变换矩阵和单位矩阵i4×4之间的相似性来判断标定是否达到收敛标准；表示根据测量结果计算得到的转换矩阵，λn，n＝1,2,3,4表示变换矩阵在每个轴上的特征值：

|λne-a|＝0(4.2)

其中e是四阶的单位矩阵，通过以下的方式，将λn，n＝1,2,3,4与对应于单位矩阵i4×4的每个轴上的特征值1进行比较，从而判断标定结果是否收敛：

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

一种单目相机-imu-机械臂的空间联合标定系统，包括：单目相机和imu单元、控制器和机械臂驱动器、标定板；单目相机和imu单元固定在机械臂末端，通过单目相机拍摄标定板的目标位置进行标定后获取单目相机的像素坐标位置；机械臂驱动器用于按照设定的轨迹控制机械臂带动末端的imu单元移动；imu单元用于自动获取与机械臂运动轨迹相对应的imu实际移动的姿态序列；控制器用于读取程序执行如上所述的方法步骤，获取优化的单目相机-imu-机械臂的空间联合标定模型，用于实际的机械臂运动。

本发明具有以下优点：

1.本发明方法采用先估计旋转矩阵，再估计平移向量的方式来解决平移向量误差易耦合到旋转矩阵估计的问题。

2.本发明方法适用于对单目相机、imu和机械臂进行联合标定，具有稳定、准确的优势。

3.本发明方法能够使单目相机和imu实现稳定、准确的信息融合。并使机械臂在复杂、非结构化的环境下实现随动感知的功能。

附图说明

图1为总体流程图；

图2为单目相机-imu-机械臂空间联合标定的系统装置。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方法做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。

除非另有定义，本文所使用的所有技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

本发明方法是基于硬件系统实现的，硬件系统包括：单目相机、imu单元、控制器和kinovajaco2机械臂、机械臂操作杆、标定板。设定机械臂运动轨迹，机械臂操作杆用于控制机械臂带动末端的imu单元移动。imu单元用于自动获取与机械臂运动轨迹相对应的imu实际移动的姿态序列。

单目相机和imu单元固定在机械臂末端，通过单目相机拍摄标定板的目标位置进行标定后获取单目相机的像素坐标位置c1、c2等。

kinovajaco2机械臂操作杆用于控制机械臂带动单目相机和imu单元运动，并通过机械臂的sdk获得末端执行器坐标位置等。

本发明方法包括四部分：

(1)单目相机-imu联合标定；

(2)单目相机-机械臂联合标定；

(3)imu-机械臂联合标定；

(4)统一框架下的联合标定。

具体实现过程如下所述。

步骤1：单目相机-imu联合标定。

假设角速度偏差和加速度偏差始终保持不变，然后可以通过最小化imu旋转和单目相机旋转之间的差异值来计算偏差。在迭代过程中，估计的旋转矩阵用于更新角速度偏差。

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

步骤1.2：单目相机-imu旋转矩阵估计

此处，旋转矩阵可以通过将时刻t到t+1之间的单目相机旋转与预积分处理后的imu旋转对齐得到。假设在时间间隔[t,t+1]之间，单目相机与imu的姿态变化分别为和尽管在不同的参考坐标系中，和仍然表示相同的旋转。将单目相机坐标系转换为imu坐标系的四元数其满足以下关系：

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

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

步骤1.3：加速度偏差估计

在时间间隔[i,j]内，单目相机平移和imu预积分平移之间的关系为：

其中，是估算得到的单目相机与imu之间的旋转矩阵；表示单目相机在i时刻和j时刻之间的平移；表示时刻t相对于i帧的速度；δt是imu采样率；表示imu在i时刻和j时刻之间的旋转。

最佳的加速度偏差可以通过最小化得到：

其中表示时刻t相对于i帧的速度，表示第i帧和第t帧之间的旋转矩阵，它被表示为

步骤1.4：单目相机-imu平移向量估计

此处给定加速度偏差和角速度偏差单目相机-imu旋转矩阵得到单目相机坐标系{c}到imu坐标系{i}的平移向量为：

其中是时间间隔[i,j]的惯性平移量，它可以通过imu预积分得到；是时间间隔[i,j]的单目相机平移量，它可以通过标定板特征点跟踪得到。

步骤2：单目相机-机械臂联合标定。

假设机械臂从位置运动到单目相机也将随着机械臂从c1运动到c2，(其中，单目相机的位置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)代入到2.4式和2.5式其中i＝1,2，可以得到：

并可以得到平移向量trc：

trc＝(a^ta)^-1a^tb(2.11)

其中，a和b表示如下：

步骤3：imu-机械臂联合标定。

imu-机械臂的标定可以通过齐次变换矩阵xri来制定。机械臂以设计好的螺旋轨迹运动，即同时，imu也按照类似的轨迹(imu姿态序列)进行运动，即{i1→i2→i3→…}。

与公式(2.1)类似，imu和机械臂的位姿关系满足以下公式：

经转换可得：

以类似的方法，imu与机械臂之间的相对旋转矩阵rri和平移向量tri可以通过求解以下各式得到：

其中，和分别对应于机械臂末端执行器坐标系变换的旋转和平移；和分别对应于imu坐标系变换i2i1^-1的旋转和平移。

同样，和分别对应于机械臂末端执行器坐标系变换的旋转和平移；和分别对应于imu坐标系变换i3i2^-1的旋转和平移。

步骤4：统一框架下的联合标定。

在单目相机-imu-机械臂联合标定模型中，由于单目相机和imu牢牢固定在机械臂末端，所以机械臂末端执行器坐标系{r}、imu坐标系{i}和单目相机坐标系{c}之间存在固有的几何约束。理论上，变换矩阵trc、tri、tic满足以下几何关系：

其中表示从{c}到{r}的空间变换关系；表示从{i}到{r}的空间变换关系；表示从{c}到{i}的空间变换关系。

该联合标定方法通过评估变换矩阵和单位矩阵i4×4之间的相似性来判断标定是否达到收敛标准。表示根据测量结果计算得到的转换矩阵，λn，n＝1,2,3,4表示变换矩阵在每个轴上的特征值：

|λne-a|＝0(4.2)

其中e是四阶的单位矩阵，通过以下的方式，将λn，n＝1,2,3,4与对应于单位矩阵i4×4的每个轴上的特征值1进行比较，从而判断标定结果是否收敛：

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

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。