一种基于光束平差的机器人方位与手眼关系同时标定方法与流程

本发明涉及工业机器人视觉标定技术领域，特别是涉及一种基于光束平差的机器人方位与手眼关系同时标定方法。

背景技术：

模拟人的视觉功能，从客观世界的图像中提取有效信息进行处理并加以理解是仿生视觉一个主要研究目的，其促使智能机器人可以通过视觉传感器感知三维环境信息的能力，并根据自身需要将这些信息传递到最终执行机构完成手眼协调任务。自从1966年世界上第一台具有视觉传感器的智能机器人shakey诞生以来，视觉传感器广泛应用于机器人自主避障和导航、医疗手术、汽车制造等领域。在利用机器人视觉系统进行自主作业时，需要将摄像机与机器人手臂连接一起构成手眼系统，即通过视觉传感器给机器人运动控制系统提供眼的定位功能。评估上述机器人视觉系统测量精确度的关键步骤之一是准确地测算出机器人坐标系与视觉传感器坐标系间的相对方位关系，即智能机器人手眼标定问题。该问题当前主流数学模型主要有以下3种：

1)ax＝xb标定模型，该模型通过平面靶标来确定摄像机运动位姿矩阵a，利用机器人运动学模型得到工具坐标系到基坐标系的变换矩阵b，从而得到摄像机坐标系与机器人工具坐标系的手眼变换矩阵x；2)ax＝zb标定模型，该模型在利用平面靶标求解手眼关系x时，也可以同时求解标定方程中机器人基坐标系和世界坐标系方位关系z；3)a(α)x＝xb或a(α)x＝zb标定模型，该模型在缺失靶标的情况下可以利用运动恢复结构算法(sfm)得到缺失尺度因子α摄像机运动位姿矩阵a(α)，然后利用非线性优化方法对标定方程中尺度因子α和手眼关系x，由于在标定模型求解过程多引入一个尺度因子α为待求参数，造成方程求解精度不稳定，因此第三种无靶标参照物手眼标定模型精度较前两种标定模型往往准确性较低，不能满足当前智能机器人视觉导航和定位日益增长的准确性需求。

因此，在没有靶标作为参照物的情况下，如何消除尺度因子对手眼标定模型的影响是当前扩展手眼标定方法应用范围的难点。此外，考虑到手眼标定的目的是让机器人准确感知周围三维环境信息，赋予机器人眼的定位功能，因此还有必要在求解机器人和摄像机手眼关系的同时对目标在世界坐标系下进行精确三维定位。

技术实现要素：

本发明的目的是提供一种基于光束平差的机器人方位与手眼关系同时标定方法，基于光束平差算法，能够在求解机器人和摄像机手眼关系的同时对目标进行精确三维定位，在摆脱了对靶标的依赖的同时也实现了检测目标在世界坐标系下定位，真正赋予机器人眼的定位功能。

为实现上述目的，本发明提供了如下方案：

一种基于光束平差的机器人方位与手眼关系同时标定方法，该方法包括以下步骤：

s1，通过单目立体视觉sfm算法进行三维重建，利用ransac数据筛选算法对相机外参数标定数据进行阈值自适应筛选；

s2，对手眼标定方程a(α)x＝zb做等价变换，消除标定方程中尺度因子α，利用矩阵直积法建立齐次线性方程组，代入筛选后的标定数据，进行奇异值分解，得到机器人方位与手眼关系初值；

s3，利用光束平差优化模型对机器人方位与手眼关系初值以及单目立体视觉算法得到的三维重建结果一同进行迭代优化；

s4，判断是否满足设定的迭代终止条件，如果满足，则终止迭代，同时输出机器人方位和手眼关系矩阵以及检测目标的三维空间点坐标；

s5，利用puma560运动学模型建立机器人和摄像机运动轨迹仿真系统，模拟检测目标世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的空间变换关系，验证标定方法的精度和鲁棒性。

可选的，所述步骤s2，对手眼标定方程a(α)x＝xb做等价变换，消除标定方程中尺度因子α，利用矩阵直积法建立齐次线性方程组，代入筛选后的标定数据，进行奇异值分解，得到机器人方位和手眼关系初值，具体包括：

s201，设a表示参照物世界坐标系到不同姿态的摄像机坐标系的变换矩阵，b表示为不同姿态的机器人工具坐标系到机器人基坐标系的变换矩阵，机器人手眼关系x以及机器人坐标系与世界坐标系方位关系z，则传统手眼标定方程表示为：ax＝zb；考虑到尺度因子α，将传统手眼标定方程转化为：

a(α)x＝zb(1)

其中a、x、z、b都为4×4的矩阵；

s202，对a(α)x＝zb解耦展开成旋转和平移分开的形式为：

rarx＝rzrb

ratx+αta＝rztb+tz(3)

式中，ra,rx,rz,rb分别表示a、x、z、b变换矩阵中的旋转部分，ta,tx,tz,tb分别表示a、x、z、b变换矩阵中的平移部分；

s203，由式(3)可知，标定方程中只有平移向量部分受尺度因子α的影响，将平移向量部分等式两边都乘以平移向量ta的反对称矩阵消去尺度因子α可得：

其中，平移向量ta的反对称矩阵为

s204，利用矩阵直积表示方法，将上式(4)用矩阵直积表示为：

对上式齐次线性方程组进行奇异值分解，最小特征值对应的特征向量v即为对应方程的解。

可选的，所述步骤s3，利用光束平差优化模型对机器人方位与手眼关系初值以及单目立体视觉算法得到的三维重建结果一同进行迭代优化，具体包括：

s301，建立光束平差优化模型的目标函数：

式中，定义n帧图像，每帧图像含m个特征点，设xij表示第i个三维特征点在第j张像平面对应的特征点坐标，λij用来判别三维特征点i在第j张像平面上是否有投影，如果有λij＝1，否则λij＝0，aj表示图像坐标系到世界坐标系的刚体变换矩阵，即相机外参数，gi表示世界坐标系下空间特征点三维坐标，pj(·)表示透视投影矩阵，即相机内参数，表示使用中心投影法将摄像机坐标系下三维特征点投影到第j张像平面坐标系下；

s302，由传统手眼标定方程ax＝zb可知，相机的外参数由已知机器人的运动位姿表示为aj＝zbjx^-1，则公式(10)目标函数重新表示为：

s303，利用公式(11)作为最小化目标函数，在整个迭代优化过程中，设置相机的内参数和机器人的运动位姿为不变量，只对手眼关系x，机器人坐标系与世界坐标系方位关系z以及空间特征点三维坐标g进行迭代优化，手眼关系x和机器人坐标系与世界坐标系方位关系z包含6个旋转变量和6个平移变量，同时每个特征点三维坐标gi包含3个位置变量，因此公式(11)总共有3m+12个参数需要迭代优化。

可选的，所述步骤s4中，设定的迭代终止条件，具体为：

设置前后两次迭代计算得到的手眼关系平移向量或重建三维点坐标向量的变化值不大于10^-3mm为迭代终止条件。

可选的，所述步骤s5中，利用puma560运动学模型建立机器人和摄像机运动轨迹仿真系统，模拟检测目标世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的空间变换关系，验证标定方法的精度和鲁棒性，具体包括：

仿真系统中给定手眼关系x以及机器人方位关系z真值，利用petercorkeroboticstoolbox和cameracalibrationtoolbox工具箱模拟生成了50个空间点坐标g，这些空间点随机分布在0.5m×0.5m×0.5m方格内作为检测目标，同时模拟生成了摄像机的8个不同的位姿正对50个空间点进行成像，分别利用puma560机器人运动学模型和摄像机成像得到机器人和摄像机的运动位姿a和b，从而建立了检测目标世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的联系，验证标定方法的精度和鲁棒性。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供的基于光束平差的机器人方位与手眼关系同时标定方法，基于矩阵直积的a(α)x＝zb手眼标定扩展模型快速求解，并通过引入反对称矩阵消除手眼标定扩展模型中尺度因子对方程求解稳定性的影响；利用光束平差优化模型对机器人方位与手眼关系以及目标特征点三维坐标一同进行迭代优化，从而建立机器人坐标系、摄像机坐标系以及目标点世界坐标系精确的空间变换关系，可以同时得到检测目标点的三维坐标、手眼关系x及机器人坐标系与世界坐标系方位关系z的最优解，在摆脱了对靶标的依赖的同时也实现了检测目标的定位，真正赋予机器人眼的定位功能。此外，本发明还利用puma560机器人运动学模型和摄像机成像模型建立机器人和摄像机运动轨迹仿真系统，通过建立检测目标世界坐标系，机器人基坐标系，机器人工具坐标系以及摄像机坐标系四者之间的对应关系，提供了一种方便可行的手眼标定精度评价方法。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为ax＝zb手眼标定模型示意图；

图2为基于光束平差的机器人方位与手眼关系同时标定方法流程图；

图3为机器人和摄像机运动轨迹仿真系统模型示意图；

图4为不同噪声水平下x和z旋转矩阵和平移向量误差比较图。

图5为不同迭代次数下三维重建和手眼标定的计算误差比较图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于光束平差的机器人方位与手眼关系同时标定方法，基于光束平差算法，能够在求解机器人和摄像机手眼关系的同时对目标进行精确三维定位，在摆脱了对靶标的依赖的同时也实现了检测目标在世界坐标系下定位，真正赋予机器人眼的定位功能。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1至图2所示，本发明提供的基于光束平差的机器人方位与手眼关系同时标定方法，包括以下步骤：

s1，通过单目立体视觉sfm算法进行三维重建，利用ransac数据筛选算法对相机外参数标定数据进行阈值自适应筛选；

s2，对手眼标定方程a(α)x＝zb做等价变换，消除标定方程中尺度因子α，利用矩阵直积法建立齐次线性方程组，代入筛选后的标定数据，进行奇异值分解，得到机器人方位与手眼关系初值；

s3，利用光束平差优化模型对机器人方位与手眼关系初值以及单目立体视觉算法得到的三维重建结果一同进行迭代优化；

s4，判断是否满足设定的迭代终止条件，如果满足，则终止迭代，同时输出机器人方位和手眼关系矩阵以及检测目标的三维空间点坐标；

s5，利用puma560运动学模型建立机器人和摄像机运动轨迹仿真系统，模拟检测目标世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的空间变换关系，验证标定方法的精度和鲁棒性。

其中，在所述步骤s1之前，利用摄像机在多个位姿下正对检测目标采集图像，利用sift(尺度不变特征变换)方法进行图像特征点提取及匹配，利用hartly五点相对定向方法计算相机在各个位姿下的外参数。

所述步骤s2，对手眼标定方程a(α)x＝xb做等价变换，消除标定方程中尺度因子α，利用矩阵直积法建立齐次线性方程组，代入筛选后的标定数据，进行奇异值分解，得到机器人方位和手眼关系初值，具体包括：

s201，设a表示参照物世界坐标系到不同姿态的摄像机坐标系的变换矩阵，b表示为不同姿态的机器人工具坐标系到机器人基坐标系的变换矩阵，机器人手眼关系x以及机器人坐标系与世界坐标系方位关系z，则传统手眼标定方程表示为：ax＝zb；考虑到尺度因子α，将传统手眼标定方程转化为：

a(α)x＝zb(1)

其中a、x、z、b都为4×4的矩阵；

传统传统手眼标定方程ax＝zb可以解耦展开成旋转和平移分开的形式为：

rarx＝rzrb

ratx+ta＝rztb+tz(2)

其中，rx，rz和tx，tz为待求参数，当没有精确的靶标作为尺度基准，直接拍摄任意具有丰富纹理信息的检测目标时，因缺乏拍摄场景的实际尺度信息α，方程(2)中的旋转矩阵方程不变，而平移向量与真实值之间存在一个未知的尺度因子α；

s202，对a(α)x＝zb解耦展开成旋转和平移分开的形式为：

rarx＝rzrb

ratx+αta＝rztb+tz(3)

式中，ra,rx,rz,rb分别表示a、x、z、b变换矩阵中的旋转部分，ta,tx,tz,tb分别表示a、x、z、b变换矩阵中的平移部分；

s203，由式(3)可知，标定方程中只有平移向量部分受尺度因子α的影响，将平移向量部分等式两边都乘以平移向量ta的反对称矩阵消去尺度因子α可得：

其中，平移向量ta的反对称矩阵为

s204，利用矩阵直积表示方法，将上式(4)用矩阵直积表示为：

对上式齐次线性方程组进行奇异值分解，最小特征值对应的特征向量v即为对应方程的解。

所述步骤s2还包括：

步骤s205，考虑到旋转矩阵rx和rz的行列式值都1，设旋转矩阵rx和rz对应的特征向量v组成的矩阵vx＝vec^-1(v1：9)，vz＝vec^-1(v10：18)，vec^-1表示vec列向量符号的逆操作，即将mn维向量表示为m×n的矩阵，则旋转矩阵表示为：

其中，矩阵的系数ω和通过下式确定：

同理，平移向量tx和tz用特征向量v表示为：

由于测量误差的存在，实际情况下旋转矩阵rx和rz并不严格满足正交性，因此还需对矩阵直积计算出的旋转矩阵进行二次正交化处理。

所述步骤s3，利用光束平差优化算法对矩阵直积法得到的机器人方位与手眼关系初值以及单目立体视觉算法得到的三维重建结果进行迭代优化，具体包括：

s301，建立光束平差优化模型的目标函数：

式中，定义n帧图像，每帧图像含m个特征点，设xij表示第i个三维特征点在第j张像平面对应的特征点坐标，λij用来判别三维特征点i在第j张像平面上是否有投影，如果有λij＝1，否则λij＝0，aj表示图像坐标系到世界坐标系的刚体变换矩阵，即相机外参数，gi表示世界坐标系下空间特征点三维坐标，pj(·)表示透视投影矩阵，即相机内参数，表示使用中心投影法将摄像机坐标系下三维特征点投影到第j张像平面坐标系下；

为了进一步提高手眼关系x和机器人坐标系与世界坐标系方位关系z标定精确度，本发明使用(10)式光束平差模型对(6)式矩阵直积计算的初值进行迭代优化；

s302，由传统手眼标定方程ax＝zb可知，相机的外参数由已知机器人的运动位姿表示为aj＝zbjx^-1，则公式(10)目标函数重新表示为：

s303，利用公式(11)作为最小化目标函数，在整个迭代优化过程中，设置相机的内参数和机器人的运动位姿为不变量，只对手眼关系x，机器人坐标系与世界坐标系方位关系z以及空间特征点三维坐标g进行迭代优化，手眼关系x和机器人坐标系与世界坐标系方位关系z包含6个旋转变量和6个平移变量，同时每个特征点三维坐标gi包含3个位置变量，因此公式(11)总共有3m+12个参数需要迭代优化。最终得到机器人基坐标系、摄像机坐标系以及检测目标世界坐标系三者之间精确的空间位置关系，从而实现智能机器人感知与定位的功能。

所述步骤s4中，设定的迭代终止条件，具体为：

设置前后两次迭代计算得到的手眼关系平移向量或重建三维点坐标向量的变化值不大于10^-3mm为迭代终止条件。尽管通过(6)式矩阵直积计算手眼标定值得到的三维重建结果初始误差比较大，但是通过有限次的迭代(一般5-10次)，三维重建和手眼标定的误差可以迅速收敛到全局最优解。

所述步骤s5中，利用puma560运动学模型建立机器人和摄像机运动轨迹仿真系统，模拟检测目标世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的空间变换关系，验证标定方法的精度和鲁棒性，具体包括：

仿真系统中给定手眼关系x以及机器人方位关系z真值，利用petercorkeroboticstoolbox和cameracalibrationtoolbox工具箱模拟生成了50个空间点坐标g，这些空间点随机分布在0.5m×0.5m×0.5m方格内作为检测目标，同时模拟生成了摄像机的8个不同的位姿正对50个空间点进行成像，分别利用puma560机器人运动学模型和摄像机成像得到机器人和摄像机的运动位姿a和b，从而建立了检测目标世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的联系，验证标定方法的精度和鲁棒性。

考虑到在真实测量场景中无法直接测量得到手眼关系x和机器人方位z以及检测目标三维坐标g绝对真值，为验证本发明提出的光束平差的机器人方位与手眼关系同时标定方法实际测量精度和鲁棒性，利用的puma560运动学模型和摄像机成像模型建立机器人和摄像机运动轨迹仿真系统，模拟真实测量场景下的手眼标定过程，如图3所示。该仿真系统建立了世界坐标系、机器人基坐标系、机器人工具坐标系以及摄像机坐标系四者之间的联系，在0.5m×0.5m空间内随机模拟生成50个空间特征点，摄像机从8个不同的位姿对特征点进行透视投影变换。虚拟相机的分辨率为4288×2848pixels，主距为20mm，主点坐标为(0.1,0.1)mm，6自由度puma560机器人的d-h(denavit-hartenberg)参数为q,d,a,α，见下表1所示：

表1

为了定量分析仿真实验结果，每次分别给摄像机的姿态变换矩阵ai(i＝1,…,9)和机器人工具坐标系的姿态变换矩阵bi加入不同的测量噪声，进行500次完全独立的重复标定实验，统计不同方法计算得到的平均标定结果r和t与给定真值和进行比较分析。定义变换矩阵中旋转矩阵er和平移向量et误差为：

其中，表示旋转矩阵真值，而r表示旋转矩阵估计值，表示平移向量真值，而t表示平移向量估计值，具体地，我们在仿真实验中设置手眼关系x矩阵中平移向量为固定值机器人坐标系与世界坐标系方位关系z中平移向量为固定值整个仿真实验主要分为以下3个步骤：

(1)考虑到特征点在像面的提取精度可达1pixel，给虚拟相机像面坐标添加白噪声，噪声系数η为(0,1)间5等分值，根据图3手眼标定仿真模型中的虚拟相机参数和空间后方交会模型计算8个不同的位姿下的摄像机变换矩阵ai(i＝1,…,8)。

(2)根据给定的变换矩阵真值和以及摄像机变换矩阵ai，利用传统手眼标定方程反求出摄像机的8个姿态变换矩阵考虑到目前市场上用于精密测量的小型多关节工业机器人重复定位精度在0.025mm左右，给puma560机器人姿态变换矩阵bi加入σ＝0.025mm的关节变量偏移高斯噪声(offsets)。

(3)分别使用shah四元数松耦合法(“shah”)、dornaika四元数紧耦合法(“dornaika”)以及公式(6)中矩阵直积法(“kpherwc”)3种ax＝zb方程求解方法与本发明提出的光束平差的机器人方位与手眼关系同时标定方法(“baherwc”)进行比较分析，分别比较这四种方法在公式(12)中旋转矩阵和平差向量误差值，实验结果如图4所示。

从图4中箱线图可以看出，在给摄像机的像面坐标添加白噪声及机器人关节变量添加偏移噪声的情况下，四种方法中本文提出的“baherwc”方法计算得到关于手眼关系x和机器人方位z的旋转矩阵er和平移向量et误差最小。而“kpherwc”方法计算得到关于手眼关系x和机器人方位z的旋转矩阵er和平移向量et误差最大，说明在噪声存在实际情况下，利用光束平差方法对公式(6)中矩阵直积法得到的初值进行迭代优化是非常有必要的。

此外，为了验证本文baherwc方法的迭代收敛特性，图5给出了不同迭代次数下三维重建和手眼标定的计算误差变化趋势。由图5可知，尽管通过(6)式矩阵直积计算手眼标定值得到的三维重建和手眼标定结果初始误差比较大，但是通过有限次的迭代(5-10次)，三维重建的误差可以迅速收敛到全局最小值，进一步证明了利用光束平差迭代优化方法的有效性。

本发明提供的基于光束平差的机器人方位与手眼关系同时标定方法，基于矩阵直积的a(α)x＝zb手眼标定扩展模型快速求解，并通过引入反对称矩阵消除手眼标定扩展模型中尺度因子对方程求解稳定性的影响；利用光束平差优化模型对机器人方位与手眼关系初值以及单目立体视觉算法得到的三维重建结果一同进行迭代优化，从而建立机器人坐标系、摄像机坐标系以及目标点世界坐标系精确的空间变换关系，可以同时得到检测目标点的三维坐标、手眼关系x及机器人坐标系与世界坐标系方位关系z的最优解，在摆脱了对靶标的依赖的同时也实现了检测目标的精确定位，真正赋予机器人眼的定位功能。此外，本发明还利用puma560运动学模型和摄像机成像模型建立机器人和摄像机运动轨迹仿真系统，为手眼标定方法的精度评定提供了一个方便可行的评价思路。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。