CN110288660B - 一种基于凸松弛全局优化算法的机器人手眼标定方法 - Google Patents

Abstract

本发明提供了一种基于四元数理论的凸松弛全局最优化手眼标定方法，该方法是针对机器人运动学正解及相机的外参数标定存在偏差时，基于非线性最优化的手眼标定算法无法确保目标函数收敛到全局极小值的问题而提出；考虑到机械手末端相对运动旋转轴之间的夹角对标定方程求解精度的影响，首先利用随机抽样一致性(Random Sample And Consensus，RANSAC)算法对标定数据中旋转轴之间的夹角进行预筛选，再利用四元数参数化旋转矩阵，建立多项式几何误差目标函数和约束，采用基于线性矩阵不等式(linear matrix inequality，LMI)凸松弛全局优化算法求解全局最优手眼变换矩阵。

Description

一种基于凸松弛全局优化算法的机器人手眼标定方法

本发明是专利号为201710095506.9，发明创造名称为一种基于凸松弛全局优化算法的机器人手眼标定方法的分案申请。

技术领域

本发明涉及一种基于凸松弛全局优化算法的机器人手眼标定方法，涉及工业机器人视觉标定技术领域，用于关节臂坐标测量机，机器人手术，服务机器人技术领域。

背景技术

近年来，随着科学技术的迅猛发展，机器人已经逐步进入我们的社会生活中，如在智能装配、自主导航、逆向工程、焊接工程等领域得到了广泛应用。机器人手眼标定作为机器人视觉的关键技术之一，一直以来都是机器视觉领域中的研究热点。在机器人手眼视觉系统中，往往需要将摄像机与机器人的手臂结合起来，才能获得空间中目标物体相对于机器人执行器的相对位置信息，然后精确控制机器人执行相应的动作。这一过程中的必要环节是获取摄像机与机器人末端执行器的坐标变换关系，即手眼标定问题。

1989年，Tsai在机器人视觉关键技术中，提出手眼标定问题并建立手眼标定误差模型。同年，Shiu提出了基于轴角变换的手眼标定闭环线性解法，分步求出手眼变换矩阵中的旋转矩阵和平移向量。1995年，Horaud提出了基于单位四元数的一步非线性法和两步线性法，该算法用单位四元数参数化旋转矩阵，简化了手眼标定方程求解步骤。1999年，Daniilidis采用对偶四元数和螺旋理论的方法得出了线性闭环解法，该算法简化了计算步骤，但对测量噪声较为敏感。同年，Andreff提出了基于矩阵直积的线性闭环解法，该算法适用于标定小角度移动的测量场合，但求解的结果通常情况下并不满足旋转矩阵的正交和单位特性，需要对其再加入正交约束。2001年，Andreff以机器人视觉伺服系统为背景，提出利用运动恢复结构(Structure from motion)的算法求解相机的方位信息，摆脱了手眼标定过程中对靶标的依赖，扩展了手眼标定的应用场景。2006年，Strobl利用欧氏运动群SE(3)描述机械手的刚体变换，提出一种自适应加权优化的手眼标定计算模型。

此外，考虑到标定数据的选择同样会影响手眼变换矩阵的求解精度，2008年，Schmidt在Tsai建立的标定误差数学模型基础上，提出利用矢量化编码技术构建手眼标定数据筛选机制，提高了手眼变换矩阵的求解精度。2010年，Malti提出一种改进的对偶四元数算法，为了避免标定数据中粗大误差对求解过程的影响，增加随机抽样一致性算法(RANSAC)对数据进行预筛选，增强了标定结果的精度和鲁棒性。同年，毛剑飞基于四元数理论，给出了手眼标定算法几何意义的对照，对标定方程各种解的情况进行了讨论。2011年，Zhao提出采用凸优化理论计算手眼变换矩阵的方法，提高了计算结果的鲁棒性，但求解过程较为复杂。同年，王君臣基于极大似然估计(Maximum Likelihood)的概念建立测度目标函数，提出一种手眼标定非线性优化算法。2015年，王金桥提出利用遗传算法解决关节臂视觉检测系统中的手眼标定问题。以上研究，有的只关注于手眼标定方程的参数化方法或者求解算法问题，还有的只是针对影响标定方程求解精度的某些因素进行分析，综合考虑标定方程的求解算法及误差影响因素的研究还比较少，而事实上，标定数据的筛选和标定方程的求解算法都会直接影响手眼变换矩阵的求解精度，因此有必要综合在一起进行研究。

因此，需要一种基于四元数理论的凸松弛全局最优化手眼标定算法，该方法不需要初值估计，且可以保证手眼标定结果收敛到全局最优解，从而提高手眼变换矩阵的求解精度和鲁棒性。

发明内容

本发明的目的在于提供一种基于四元数理论的凸松弛全局最优化手眼标定方法，采用RANSAC算法对标定数据中旋转轴之间的夹角进行预筛选，求解方程时提出一种基于四元数理论的凸松弛全局最优化手眼标定算法。该方法不需要初值估计，且可以保证手眼标定结果收敛到全局最优解，从而提高手眼变换矩阵的求解精度和鲁棒性。

为了实现上述目的，本发明采用的技术方案如下：

一种基于凸松弛全局优化算法的机器人手眼标定方法，包括以下步骤：

S1:在求解标定方程之前，设定角度阈值初值，利用自适应RANSAC算法对标定数据集Cⁿ中旋转轴之间的夹角进行预筛选。

S2:利用四元数法参数化手眼变换矩阵X，将S1中筛选出的满足角度阈值要求的靶标世界坐标系到摄像机坐标系的变换矩阵A和机械手末端执行器坐标系到机械手基坐标系的变换矩阵B代入手眼标定方程AX＝XB，建立多项式几何误差目标函数和约束条件。

S3:采用基于线性矩阵不等式(LMI)凸松弛全局优化算法对S2参数化的手眼标定方程进行求解，得到满足标定方程的全局最优参数值，然后利用该参数值得到全局最优的手眼变换矩阵。

本发明考虑到机械手末端相对运动旋转轴之间的夹角对标定方程求解精度的影响，首先利用随机抽样一致性(Random Sample And Consensus，RANSAC)算法对标定数据中旋转轴之间的夹角进行预筛选，再利用四元数参数化旋转矩阵，建立多项式几何误差目标函数和约束，采用基于线性矩阵不等式(linear matrix inequality，LMI)凸松弛全局优化算法求解全局最优手眼变换矩阵。

本发明采用RANSAC算法对标定数据中旋转轴之间的夹角进行预筛选，可减小标定数据的选择对标定方程求解精度的影响；提出一种基于四元数理论的凸松弛全局最优化手眼标定算法，有效解决了针对机器人运动学正解及相机的外参数标定存在误差时，基于非线性最优化的手眼标定算法无法确保目标函数收敛到全局最小值的问题，提高了标定算法的鲁棒性。

应当理解，前述大体的描述和后续详尽的描述均为示例性说明和解释，并不应当用作对本发明所要求保护内容的限制。

附图说明

参考随附的附图，本发明更多的目的、功能和优点将通过本发明实施方式的如下描述得以阐明，其中：

图1示意性示出手眼标定基本原理示意图。

图2示意性示出手眼标定实验装置图。

图3示意性示出本发明的整体方法流程图。

图4示意性示出两种不同优化算法的手眼标定几何误差比较图。

图5示意性示出两种不同优化算法的手眼标定几何误差棒比较图。

具体实施方式

通过参考示范性实施例，本发明的目的和功能以及用于实现这些目的和功能的方法将得以阐明。然而，本发明并不受限于以下所公开的示范性实施例；可以通过不同形式来对其加以实现。说明书的实质仅仅是帮助相关领域技术人员综合理解本发明的具体细节。

在下文中，将参考附图描述本发明的实施例。在附图中，相同的附图标记代表相同或类似的部件，或者相同或类似的步骤。

下面结合附图与实施例对本发明作进一步说明。

图3所示为本发明的整体方法流程图。如图3所示，一种基于凸松弛全局优化算法的机器人手眼标定方法，包括以下步骤：

S1:考虑到机械手末端相对运动旋转轴之间的夹角对标定方程求解精度的影响，在求解标定方程之前，设定角度阈值初值，利用自适应RANSAC算法对标定数据集Cⁿ中旋转轴之间的夹角进行预筛选。

S2:利用四元数法参数化手眼变换矩阵X，将S1中筛选出的满足角度阈值要求的靶标世界坐标系到摄像机坐标系的变换矩阵A和机械手末端执行器坐标系到机械手基坐标系的变换矩阵B代入手眼标定方程AX＝XB，建立多项式几何误差目标函数和约束条件。

S3:采用基于线性矩阵不等式(LMI)凸松弛全局优化算法对S2参数化的手眼标定方程进行求解，得到满足标定方程的全局最优参数值，然后利用该参数值得到全局最优的手眼变换矩阵。

为了进一步验证本发明提出的算法精度和鲁棒性，设计实测实验，所使用的实验装置如图2所示，包括机械手、平面靶标、工业相机和镜头，所述机械手采用日本电裝公司DENSO机械手VS-6577GM，X、Y、Z各方向的重复定位精度为±0.02mm；所述工业相机采用凯视佳公司UD274M/C型号的CCD工业相机，分辨率为1628×1236，像元尺寸为4.4μm；所述镜头选用COMPUTAR 12mm镜头；所述平面靶标采用11mm×11mm棋盘格平面靶标；所述镜头用于对11mm×11mm棋盘格平面靶标进行相机参数标定。

具体的，根据S1，首先将摄像机固定在机械手末端执行器法兰盘上，利用DENSO机械手带动摄像机每次选取N＝15个不同位姿对平面靶标拍照成像，得到靶标世界坐标系到不同姿态的摄像机坐标系的变换矩阵A_i和不同姿态下机械手末端执行器坐标系到机械手基坐标系的变换矩阵B_i，对其两两进行组合可以得到M＝N(N-1)/2组手眼标定数据集Cⁿ。在标定数据集Cⁿ中利用自适应RANSAC算法预先对标定数据进行角度筛选，得到满足阈值要求的标定数据集C_X。

具体地，根据S2，参数化手眼变换矩阵X，以最小化标定方程AX＝XB为几何误差目标函数，以单位四元数的性质为约束条件，建立关于手眼变换矩阵X的多元多项式优化问题。

具体地，根据S3，将筛选得到的标定数据集C_X中的变换矩阵A_j和B_j代入手眼标定多项式优化问题，利用凸松弛全局优化算法求解变换矩阵X，进行误差评定。

为了更清楚的描述本发明的技术方案，以下将列举一具体实施例描述步骤S1，需要说明的是，本具体实施例并不能用来限制本发明，在此不做详细赘述。

进一步地，所述S1中的标定数据集Cⁿ的筛选方法，筛选机械手末端执行器的相对运动数据步骤为：

S101：建立标定数据筛选模型。Tsai根据一系列引理得出的标定方程中旋转矩阵的误差模型表示为：

其中，

其中，∠(r_B12,r_B23)表示两次相对运动的单位旋转轴的夹角，θ_B12，θ_B23分别表示机械手末端执行器从位姿1到位姿2以及从位姿2到位姿3的旋转角，由旋转矩阵的误差公式可知：增大机械手末端执行器两次相对运动旋转轴之间的夹角或者增大末端执行器两次位姿变化的旋转角度可以改善手眼变换矩阵中旋转矩阵的求解精度，当机械手末端执行器两次相对运动旋转轴之间的夹角接近90°时旋转矩阵的均方根误差趋于最小值。因此，定义r_ij和r_kl分别表示机械手末端执行器从位姿i到位姿j及从位姿k到位姿l的单位旋转轴，θ_ij,kl表示两次相对运动的单位旋转轴的夹角，如下式所示，当θ_ij,kl接近90°或者θ_t接近0°时，旋转矩阵的误差最小：

θ_ij,kl＝∠(r_ij,r_kl)，θ_t＝||90-θ_ij,kl||

S102：将摄像机固定在机械手末端执行器上，采集标定数据。利用机械手带动摄像机每次选取N个不同位姿对平面靶标拍照成像，两两进行组合可以得到M＝N(N-1)/2组手眼标定数据集Cⁿ，利用罗德里格旋转公式将标定数据集Cⁿ中的变换矩阵A，B表示成轴角形式。

S103：利用自适应RANSAC算法对标定数据集Cⁿ中机械手末端执行器两次相对运动旋转轴之间的夹角进行预筛选。由于至少需要两组旋转轴非平行的标定数据就可以唯一确定手眼变换矩阵，所以设定最少数据点n＝2，设定满足角度阈值要求的内点比例初值w₀＝0.1，K次抽样中所有样本均为坏样本的概率z＝0.02，角度阈值初值θ₀＝5°，终止RANSAC抽样的条件为满足角度阈值的标定数据集C_X≥15，采用自适应算法抽样并更新w₀和θ₀，直到标定数据集C_X≥15，记下此时的角度阈值θ_t，终止抽样，将筛选得到的标定数据集C_X代入S2求解手眼变换矩阵X。

为了更清楚的描述本发明的技术方案，以下将列举一具体实施例描述步骤S2，需要说明的是，本具体实施例并不能用来限制本发明，在此不做详细赘述。

进一步地，所述S2四元数法参数化手眼标定方程的步骤为：

S201：图1是手眼标定基本原理示意图。如图1所示，定义A₁，A₂表示靶标世界坐标系到两个不同姿态的摄像机坐标系的变换矩阵，B₁，B₂表示为两次不同姿态的机械手末端执行器坐标系到机械手基坐标系的变换矩阵，X表示摄像机坐标系到机械手末端执行器坐标系的变换矩阵，则手眼关系可以表示为：

AX＝XB

其中，A、B、X都为4×4的矩阵，展开为旋转矩阵和平移向量的形式为：

用四元数法参数化变换矩阵X中的旋转矩阵R_x表示为：

其中，旋转矩阵R(q_x)可以表示为：

S202：以最小化标定方程AX＝XB为几何误差目标函数，以单位四元数的性质为约束条件，建立关于变量X(q_x,t_x)多元多项式优化问题：

S203：利用S3中LMI凸松弛全局优化算法把上述多项式函数优化问题松弛为半正定规划问题求解。经解算可得目标函数f为4次7元的多项式函数，由85个不同的单项式组成，由于求解过程中至少会得到2个全局最优解，因此需要增加约束条件q_x≥0，为了增加求解的数值稳定性，需先将A_i，B_i中平移向量归一化，再根据实际要求对变量t_x加入线性约束，将平移向量的模限制在有限的空间内(例如，可设||t_x||²≤1),从而确保半正定规划的内点法可以高效的进行解算。

为了更清楚的描述本发明的技术方案，以下将列举一具体实施例描述步骤S3，需要说明的是，本具体实施例并不能用来限制本发明，在此不做详细赘述。

进一步地，所述S3中，选用的基于线性矩阵不等式(LMI)凸松弛全局优化算法是由JanHeller研究的，在本发明中应用该算法仅针对对偶四元数的参数化方法解决机器人手眼标定问题，其步骤为：

设W_i(X)为X＝(x₁,x₂,…,x_m,)∈C^m上的标量多元多项式，则多元多项式的优化问题通常可以描述为：

min W₀(X)

s.t.W_i(X)≥0，i＝1,...,n

其中X＝(x₁,x₂,...,x_m)^T∈C^m

对于一个δ阶松弛多项式优化问题W(X)，定义v_δ(X)为t阶多项式的基，表示为{1,x₁,x₂,…,x_n,x₁ ²,x₁x₂,…,x₁x_n,x₂ ²,x₂x₃,…,x_n ²,…,x₁ ^t,…,x_n ^t}，即v_δ(X)表示x中的元素x_i互乘得到的次数不高于t的所有单项式和常量1构成的集合。

定义M_δ(y)，M_δ(Wy)为按照基v_δ(X)排列顺序组成的半正定矩阵(k<t)，例如，当n＝2，t＝2时，W(X)＝3-2x₁-x₁ ²-x²时，则半正定矩阵M₂(y)和M₁(Wy)可以表示为：

因而，多项式LMI优化方法一般可以归纳为如下3个步骤：

S301：使用提升变量线性化目标函数和约束项W(X)：使用

替代目标函数W₀(X)和约束项W_i(X)中的单项式

S302：添加半正定矩阵约束：按照δ阶多项式的基v_δ(X)的排列顺序，添加半正定矩阵约束M_δ(y)≥0，M_δ(Wy)≥0。

S303：将凸松弛多项式优化问题转化为半正定规划问题求解：修改松弛系数δ，每一阶松弛后，可以使用对偶内点算法求解由前两步组成的新半正定规划问题，直到求解速度和精度满足要求为止。实际上，一般当δ≤4时，LMI全局优化算法即可收敛到全局最优解。

为了证明算法的可靠性，结合图4、图5对2种算法进行比较分析。图4示意性示出两种不同优化算法的手眼标定几何误差比较图，随机抽取一组实测实验数据，两种手眼标定算法在M＝105种组合下的手眼标定几何误差的分散情况如图4所示，由图可知：本发明的全局优化算法在M＝105种组合下的手眼标定几何误差均小于非线性最优化算法。

图5示意性示出两种不同优化算法的手眼标定几何误差棒图。每次机械手变换Q＝15个不同的位置，进行10组完全独立的重复标定实验，每组实验中对应的变换矩阵A，B标定数据集有M＝105种组合，采用以目的为导向的精度评价方法评估各标定算法的求解精度，定义手眼标定几何误差ε_X如下所示，

其中i＝1,...,M；

由图5可知，从手眼标定几何误差的精度和稳定性上看，全局优化算法(GlobalOptimization,GO)的每组观测误差值均优于非线性迭代优化算法(Nonlinear,OL)。10组测量数据中，GO算法的手眼变换举证误差平均值最大为1.4mm，标准差小于0.16mm，而OL算法的手眼变换矩阵误差平均值最大已超过1.6mm，标准差接近0.2mm，即10组独立重复实测实验中本发明的全局优化算法的手眼标定几何数据的误差棒均小于非线性最优化算法的手眼标定几何误差棒。因此，本发明采用的全局优化算法有效提高了标定算法的鲁棒性。

本发明考虑到机械手末端相对运动旋转轴之间的夹角对标定方程求解精度的影响，首先利用随机抽样一致性(Random Sample And Consensus，RANSAC)算法对标定数据中旋转轴之间的夹角进行预筛选，再利用四元数参数化旋转矩阵，建立多项式几何误差目标函数和约束，采用基于线性矩阵不等式(linear matrix inequality，LMI)凸松弛全局优化算法求解全局最优手眼变换矩阵，从而提高了标定算法的鲁棒性。

结合这里披露的本发明的说明和实践，本发明的其他实施例对于本领域技术人员都是易于想到和理解的。说明和实施例仅被认为是示例性的，本发明的真正范围和主旨均由权利要求所限定。

Claims (4)

1.一种基于凸松弛全局优化算法的机器人手眼标定方法，其特征在于，包括以下步骤：

S1，设定角度阈值初值，利用自适应RANSAC算法对标定数据集Cⁿ中旋转轴之间的夹角进行预筛选，其中，

所述标定数据集Cⁿ的筛选方法，具体包括：

S101：建立标定数据筛选模型，标定方程中旋转矩阵的误差模型表示为：

其中，

其中，∠(r_B12,r_B23)表示两次相对运动的单位旋转轴的夹角，θ_B12，θ_B23分别表示机械手末端执行器从位姿1到位姿2以及从位姿2到位姿3的旋转角，

表示靶标世界坐标系到摄像机坐标系的变换矩阵A在位姿1的旋转误差，

表示靶标世界坐标系到摄像机坐标系的变换矩阵A在位姿2的旋转误差；

表示机械手末端执行器坐标系到机械手基坐标系的变换矩阵B在位姿1的旋转误差，

表示机械手末端执行器坐标系到机械手基坐标系的变换矩阵B在位姿2的旋转误差；

表示手眼标定方程中旋转矩阵的误差；

定义r_ij和r_kl分别表示机械手末端执行器从位姿i到位姿j及从位姿k到位姿l的单位旋转轴，θ_ij,kl表示两次相对运动的单位旋转轴的夹角，当θ_ij,kl接近90°或者θ_t接近0°时，旋转矩阵的误差最小，表示为：

θ_ij,kl＝∠(r_ij,r_kl)，θ_t＝||90-θ_ij,kl||；

S102：将摄像机固定在机械手末端执行器上，采集标定数据；利用机械手带动摄像机每次选取N个不同位姿对平面靶标拍照成像，两两进行组合可以得到M＝N(N-1)/2组手眼标定数据集Cⁿ，利用罗德里格旋转公式将标定数据集Cⁿ中的变换矩阵A，B表示成轴角形式；

S103：利用自适应RANSAC算法对标定数据集Cⁿ中机械手末端执行器两次相对运动旋转轴之间的夹角进行预筛选；

S2，利用四元数法参数化手眼变换矩阵X，将S1中筛选出的满足角度阈值要求的靶标世界坐标系到摄像机坐标系的变换矩阵A和机械手末端执行器坐标系到机械手基坐标系的变换矩阵B代入手眼标定方程AX＝XB，建立多项式几何误差目标函数和约束条件，

其中，所述四元数法参数化手眼标定方程具体包括：

S201：定义A₁，A₂表示靶标世界坐标系到两个不同姿态的摄像机坐标系的变换矩阵，B₁，B₂表示为两次不同姿态的机械手末端执行器坐标系到机械手基坐标系的变换矩阵，X表示摄像机坐标系到机械手末端执行器坐标系的变换矩阵，手眼关系表示为：AX＝XB

其中，A、B、X都为4×4的矩阵，展开为旋转矩阵和平移向量的形式为：

用四元数法参数化变换矩阵X中的旋转矩阵R_x表示为：

其中，旋转矩阵R(q_x)表示为：

S202：以最小化标定方程AX＝XB为几何误差目标函数，以单位四元数的性质为约束条件，建立关于变量X(q_x,t_x)多元多项式优化问题，

其中，R_x表示变换矩阵X中的旋转矩阵，R_A表示表示靶向世界坐标系到摄像机变换矩阵A中的旋转矩阵，R_B表示机械手末端执行器坐标系到机械手基坐标系的变换矩阵中的旋转矩阵，t_x、t_A、t_B分别表示平移向量，q_x、q₀、q₁、q₂、q₃分别表示旋转矩阵R(q_x)的元素，n＝2；

S203：利用LMI凸松弛全局优化算法把上述多项式函数优化问题松弛为半正定规划问题求解；

S3，采用基于线性矩阵不等式凸松弛全局优化算法对S2参数化的手眼标定方程进行求解，得到满足标定方程的全局最优参数值，然后利用该参数值得到全局最优的手眼变换矩阵，

其中，基于线性矩阵不等式(LMI)凸松弛全局优化算法具体步骤包括：

S301：使用提升变量线性化目标函数和约束项W(X)，使用

替代目标函数W₀(X)和约束项W_i(X)中的单项式

S302：添加半正定矩阵约束，即按照δ阶多项式的基v_δ(X)的排列顺序，添加半正定矩阵约束M_δ(y)≥0，M_δ(Wy)≥0；

其中，

表示目标函数W₀(X)和约束项W_i(X)中的单项式，M_δ(y)≥0，M_δ(Wy)≥0表示按照基v_δ(X)排列顺序组成的半正定矩阵，

表示半正定矩阵M_δ(y)≥0中的单项式；

S303：将凸松弛多项式优化问题转化为半正定规划问题求解。

2.根据权利要求1所述的机器人手眼标定方法，其特征在于，所述S103具体包括：设定最少数据点为2，设定满足角度阈值要求的内点比例初值w₀＝0.1，K次抽样中所有样本均为坏样本的概率z＝0.02，角度阈值初值θ₀＝5°，终止RANSAC抽样的条件为满足角度阈值的标定数据集C_X≥15，采用自适应算法抽样并更新w₀和θ₀，直到标定数据集C_X≥15，记下此时的角度阈值θ_t，终止抽样，将筛选得到的标定数据集C_X代入S2求解手眼变换矩阵X。

3.根据权利要求1所述的机器人手眼标定方法，其特征在于，所述S203求解过程中,至少会得到2个全局最优解，增加约束条件q_x≥0；先将A_i，B_i中平移向量归一化，再根据实际情况对变量t_x加入线性约束，将平移向量的模限制在有限的空间内，用于增加求解的数值稳定性。

4.根据权利要求1所述的机器人手眼标定方法，其特征在于，所述S303具体包括：修改多项式的阶数δ，每一阶松弛后，使用对偶内点算法求解由前两步组成的新半正定规划问题，直到求解速度和精度满足要求为止；当δ≤4时，LMI全局优化算法即可收敛到全局最优解。

Images