CN106839979B - 激光线结构光传感器的手眼标定方法 - Google Patents

Abstract

本发明提供了一种激光线结构光传感器的手眼标定方法，包括：在传感器上建立传感器坐标系，传感器中激光器投射结构光光平面至靶标产生一道结构光条纹，调整机器人位姿使该条纹与标定模板上的直线相交，形成特征点，记录此时机器人位姿参数及传感器输出数据；由传感器的输出结合其几何参数，可获取激光条纹上每个点在传感器坐标系下的三维坐标；对条纹与标定模板上直线相交产生的特征点进行提取。重复上述过程得到N组不同的机器人位姿参数与每组参数下对应的特征点，在空间直线约束下采集的N组数据将标定问题转化为最小二乘问题，利用优化算法求解该问题。本发明的方法简便易行，位置和姿态控制精确，效率高，质量好，具有较高的实用价值。

Description

激光线结构光传感器的手眼标定方法

技术领域

本发明涉及传感器标定技术领域，具体地，涉及激光线结构光传感器的手眼标定方法。

背景技术

将非接触式传感器(激光位移传感器、激光扫描仪、照相机等)安装于机器人末端连杆上，在不接触工件的情况下构成对环境有一定感知功能的智能机器人是机器人的主要发展趋势。要实现这一功能，传感器相对于末端连杆的位置与方向需要事先标定，这一过程即手眼标定。为了更有效的感知环境，需要进行准确的标定。

目前，机器人手眼标定的方法与研究集中于工业照相机。对末端机器人末端安装激光测距传感器进行手眼标定的方法较少，具体地包括如下几种：

1)利用外置的高精度三维坐标测量仪；

2)先标定传感器外壳某一点然后利用传感器几何尺寸关系间接标定；

3)通过求解机器人在特定几何约束(点、面、球)下的闭环运动链方程。但方法1的成本高、操作不便；方法2对姿态难以标定，同时存在着传感器磨损后精度下降等诸多问题；方法3目前只针对激光位移传感器。

现有技术中公开了一种“定点变位姿手—眼标定”，该方法要求机器人在不同位置与姿态下保持激光射在物体同一点上，记录相应的机器人关节编码角度，通过最小二乘法计算出激光位移传感器与机器人末端连杆之间的变换关系。显然上述要求是苛刻的，不同位姿下激光保持在同一点耗时耗力，且难以保持精度。

另一种采用平面模板的标定方法，机器人在不同位置与姿态下对平面进行测量，记录相应的机器人位姿参数，利用最小二乘法拟合平面求解激光位移传感器标定问题，该方法操作容易，但参数分布对结果影响较大，拟合平面所需要的点也较多，比较费时。

综上所述，对激光线结构光传感器目前缺少标定结果精确、标定过程简单省时的方法。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种激光线结构光传感器的手眼标定方法。

根据本发明提供的激光线结构光传感器的手眼标定方法，包括如下步骤：

步骤1：将传感器通过工装夹具夹持于机器人末端连杆上，其中所述传感器为激光线结构光传感器；

步骤2：在传感器上建立坐标系O_lX_lY_lZ_l，传感器的中激光器投射结构光光平面至靶标产生一道结构光条纹；令i＝1；

步骤3：调整机器人位姿使所述结构光条纹与标定模板上的直线相交形成特征点，并记录当前时刻机器人位姿参数及传感器的输出数据；

步骤4：提取结构光条纹与标定模板直线相交所形成的特征点，得到特征点在传感器坐标系下的坐标；

具体地，由传感器的输出结合相应的几何参数，能够获取结构光条纹上每个特征点在传感器坐标系下的三维坐标，记为(x_l，y_l，z_l)；

步骤5：判断i是否小于N，若小于则返回执行步骤3，令i自增1；若i大于等于N，得到N组特征点的坐标数据，执行步骤6；

步骤6：利用在空间直线约束下采集的N组特征点的坐标数据将标定问题转化为最小二乘问题，利用优化算法求解该最小二乘问题。

优选地，所述步骤2中在传感器上建立坐标系O_lX_lY_lZ_l包括：定义传感器坐标系的原点位于激光出射点，定义Z轴沿着出射激光角角平分线方向，X轴位于光平面内垂直于Z轴，Y轴垂直于光平面，可以由右手定则确定Y轴的方向。

优选地，所述步骤4包括：求取传感器输出曲线的二阶导数，其中所述二阶导数的最大值及最小值为待考量值，根据标定模块与线结构光相对位置确定特征点所对应的最值，记为P。

优选地，所述步骤6中的最小二乘问题的求解步骤如下：

步骤6.1：根据机器人运动学公式、标定模板的直线方程将机器人末端坐标系之间手眼关系标定问题转化为最优化问题；所述机器人运动学公式如下：

[p 1]^T＝T_BE T_EL[x_p y_p z_p 1]^T

式中：(x_p y_p z_p)表示特征点P在传感器坐标系下的坐标，T_BE表示末端连杆坐标系相对于世界坐标系的齐次变换矩阵，T_EL表示传感器坐标系相对于末端连杆坐标系的齐次变换矩阵，[p 1]^T中p表示特征点在世界坐标系上的三维坐标；

假设标定模板直线方程为

式中：x、y、z分别表示传感器坐标系的坐标变量，(x₀，y₀，z₀)为直线上一点，

(m，l，n)为该直线方向向量；

则标定模板直线外一点(x_b，y_b，z_b)到标定模板直线的距离的平方，记为d²，计算公式如下：

传感器坐标系与机器人末端坐标系之间手眼关系标定问题转化为最优化问题，即求解

式中：f(y)表示适应度函数，min表示求取最小值，表示第i个点到标定模板直线的距离的平方，M为惩罚因子，为求和函数，n为约束条件数量，k_i表示第i个约束条件。

优选地，还包括步骤7：利用遗传算法得到传感器坐标系与机器人末端坐标系的旋转平移关系。

优选地，所述步骤7中的遗传算法包括如下步骤：

步骤S1：首先生成初始种群A，并计算初始种群A中每条染色体的适度函数；其中，所述初始种群A在约束条件下随机生成并进行编码；

步骤S2：针对每条染色体的适度值，采用轮盘赌的方式从初始种群A中选择N条染色体，生成新的种群A’；

步骤S3：根据输入的交叉算子0.8使种群A’中的父代染色体两两交叉，交叉后的染色体进行合法检验，保留合法染色体；

步骤S4：根据输入的变异算子0.01对父代染色体进行基因段重排，进行合法检验，保留合法染色体；

步骤S5：判断是否满足停止准则f(y)小于阈值，若不满足，则返回执行，步骤S1；若满足，则输出得到传感器坐标系与机器人末端坐标系的旋转平移关系，结束。

与现有技术相比，本发明具有如下的有益效果：

本发明中的方法应用于末端安装激光扫描仪，且自由度冗余的机器人手眼标定，将现有一维激光位移传感器的标定扩展到二维激光线结构光传感器，操作时使结构光平面与模板直线相交即可，利用相关算法获取特征点，从而简化特征点获取过程。同时利用直线约束转化标定问题为最小二乘问题，结合智能优化算法，无需大量获取标定所需数据，在精度保证的前提下缩短了标定时间。本发明中的方法简便易行，位置和姿态控制精确，效率高，质量好，具有较高的实用价值。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

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

图2为机器人末端夹持传感器示意图；

图3为激光扫描仪输出二阶导数曲线图；

图4为遗传算法流程图；

图5为共面各点恢复计算值到拟合平面的距离结果示意图；

图中：

1-机器人末端法兰盘；

2-工装；

3-激光线结构光传感器。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

根据本发明提供的激光线结构光传感器的手眼标定方法，包括如下步骤：

步骤1：将传感器通过工装夹具夹持于机器人末端连杆上，其中所述传感器为激光线结构光传感器；

步骤2：在传感器上建立坐标系O_lX_lY_lZ_l，传感器的中激光器投射结构光光平面至靶标产生一道结构光条纹；令i＝1；

步骤3：调整机器人位姿使所述结构光条纹与标定模板上的直线相交形成特征点，并记录当前时刻机器人位姿参数及传感器的输出数据；

步骤4：提取结构光条纹与标定模板直线相交所形成的特征点，得到特征点在传感器坐标系下的坐标；

具体地，由传感器的输出结合相应的几何参数，能够获取结构光条纹上每个特征点在传感器坐标系下的三维坐标，记为(x_l，y_l，z_l)；

步骤5：判断i是否小于N，若小于则返回执行步骤3，令i自增1；若i大于等于N，得到N组特征点的坐标数据，执行步骤6；

步骤6：利用在空间直线约束下采集的N组特征点的坐标数据将标定问题转化为最小二乘问题，利用优化算法求解该最小二乘问题。

所述步骤2中在传感器上建立坐标系OlXlYlZl包括：定义传感器坐标系的原点位于激光出射点，定义Z轴沿着出射激光角角平分线方向，X轴位于光平面内垂直于Z轴，Y轴垂直于光平面，可以由右手定则确定Y轴的方向。

所述步骤4包括：求取传感器输出曲线的二阶导数，其中所述二阶导数的最大值及最小值为待考量值，根据标定模块与线结构光相对位置确定特征点所对应的最值，，记为P。

所述步骤6中的最小二乘问题的求解步骤如下：

步骤6.1：根据机器人运动学公式、标定模板的直线方程将机器人末端坐标系之间手眼关系标定问题转化为最优化问题；所述机器人运动学公式如下：

[p 1]^T＝T_BE T_EL[x_p y_p z_p 1]^T

式中：(x_p y_p z_p)表示特征点P在传感器坐标系下的坐标，T_BE表示末端连杆坐标系相对于极坐标系的齐次变换矩阵，T_EL表示传感器坐标系相对于末端连杆坐标系的齐次变换矩阵，[p 1]^T中p表示特征点在世界坐标系上的三维坐标；

假设标定模板直线方程为

式中：x、y、z分别表示传感器坐标系的坐标变量，(x₀，y₀，z₀)为直线上一点，(m，l，n)为直线方向向量；

则标定模板直线外一点(x_b，y_b，z_b)到标定模板直线的距离的平方，记为d²，计算公式如下：

传感器坐标系与机器人末端坐标系之间手眼关系标定问题转化为最优化问题，即求解

式中:f(y)表示适应度函数，min表示求取最小值，表示第i个点到标定模板直线的距离的平方。M为惩罚因子，为求和函数，n为约束条件数量，k_i表示第i个约束条件。

本发明提供的激光线结构光传感器的手眼标定方法，还包括步骤7：利用遗传算法得到传感器坐标系与机器人末端坐标系的旋转平移关系；

所述步骤7包括：

步骤7.1：首先生成初始种群A，并计算初始种群A中每条染色体的适度函数；其中，所述初始种群A在约束条件下随机生成并进行编码组成；

步骤7.2：针对每条染色体的适度值，采用轮盘赌的方式从初始种群A中选择N条染色体，生成新的种群A’；

步骤7.3：根据输入的交叉算子0.8使种群A’中的父代染色体两两交叉，交叉后的染色体进行合法检验，保留合法染色体；

步骤7.4：根据输入的变异算子0.01对父代染色体进行基因段重排，进行合法检验，保留合法染色体；

步骤7.5：判断是否满足停止准则f(y)小于阈值，若不满足，则返回执行，步骤7.1；若满足，则输出得到传感器坐标系与机器人末端坐标系的旋转平移关系，结束。

下面结合具体实施例对本发明中的技术方案作更加详细的说明。

总的来说，本发明的标定方法为：在传感器上建立传感器坐标系，传感器中激光器投射结构光光平面至靶标产生一道结构光条纹，调整机器人位姿使该条纹与标定模板上的直线相交，形成特征点，记录此时机器人位姿参数及传感器输出数据。由传感器的输出结合其几何参数，可获取激光条纹上每个点在传感器坐标系下的三维坐标。对条纹与标定模板上直线相交产生的特征点进行提取。重复上述过程得到N组不同的机器人位姿参数与每组参数下对应的特征点。在空间直线约束下采集的N组数据将标定问题转化为最小二乘问题，利用优化算法求解该问题。其具体应用见以下实施例：

实施例

采用基于直线约束的方法对末端夹持激光线结构光传感器的机器人进行手眼标定，实验用机器人为安川公司生产的Motoman ssa2000型六自由度机器人，传感器为KEYENCE公司生产的LJ_G200型。

末端夹持传感器的机器人手眼标定模型示意图如图1，在机器人底座上建立基础坐标系{B}，末端连杆上建立末端坐标系{E}，传感器上建立传感器坐标系{L}，其中{L}的坐标系原点位于激光出射点，Z轴方向与出射激光角角平分线方向重合，X轴在光平面上垂直于Z轴，Y轴垂直于光平面。{E}相对于{B}的齐次变换矩阵记为T_BE，该矩阵可由机器人运动学方程求得；{L}相对于{E}的齐次变换矩阵记为T_EL，该矩阵为待标定参数矩阵。控制机器人运动，使传感器发出的激光平面打在模板直线上交于点P。

第一步、将传感器通过工装夹持于机器人末端法兰盘上，如图2所示。

第二步、在传感器上建立传感器坐标系{L}，传感器中激光器投射结构光光平面至靶标产生一道结构光条纹如图1所示。

第三步、调整机器人位姿使该条纹与标定模板上的直线相交，形成特征点，图1点P即为特征点。记录此时机器人位姿参数(例：X:1202.383Y：-372.369Z:-71.484RX:3.32RY:-6,19RZ:-176.59)及传感器输出数据。由传感器的输出结合其几何参数，可获取激光条纹上每个点在传感器坐标系下的三维坐标。所用传感器每道条纹由800个点组成，出射激光角为13.04°，每个点的z轴坐标由传感器输出，x轴坐标为其中i为激光点序号，由坐标系定义可知y轴坐标为0。

第四步、对条纹与标定模板上直线相交产生的特征点进行提取，二阶导数如图3所示，由二阶导数的含义，区域A、B、C分别表示三条直线，取二次函数的最值即可得到三条直线的交点，即点P。

步骤五、重复步骤三、四，变换机器人姿态参数RX、RY、RZ，然后调整位置参数使光平面与直线模板相交，得到N组数据(例：X:1202.383Y：-372.369Z:-71.484RX:3.32RY:-6,19RZ:-176.59，x_p：-7.7481y_p：0z_p＝249.7850)。

步骤六、在空间直线约束下采集的N组数据将标定问题转化为最小二乘问题，利用优化算法求解该问题。根据机器人正运动学公式有：

[p l]^T＝T_BET_EL[x_py_p z_p l]^T

其中p为P点在{B}坐标系下的位置，将上式展开，有：

其中R_BE与t_BE分别为T_BE的旋转与平移向量，由于传感器坐标系下y_p为零，T_EL旋转矩阵的第二列对测量点在{B}坐标系下的坐标无影响，因此待标定的参数为[r₁ r₂ r₃ r₄ r₅r₆ t₁ t₂ t₃]，其中存在单位向量约束：

理想状态下由于测量点均在一条空间直线上，因此{B}坐标系下测量点均满足直线方程：

直线外一点到直线的距离的平方为：

将等式(1)中xb，yb，zb带入(3)式，由于空间直线约束的引入，增加了6个待标定参数[x₀ y₀ z₀ m l n]，其中m,l,n受到单位向量的约束：

m²+l²+n²＝1 (4)

按照上文所述标定过程，得到机器人不同位姿下的N个形如式(3)的等式，故传感器坐标系与机器人末端坐标系之间手眼关系的标定转化为在(2)、(4)单位向量约束下的最优化问题：

采用遗传算法处理上述问题，用罚函数法处理(2)、(4)约束，对不符合约束的染色体进行惩罚，适应度函数为：

其中M为惩罚系数，k_i为约束条件，此处存在3个约束条件故n为3。首先为生成规模N＝400的初始种群A；计算种群A中每条染色体的适度函数，以min f(y)为适度值；针对每条染色体的适度值赋予不同的概率函数采用赌轮盘的方式从初始种群A中选择N条染色体形成新的种群A’，输入交叉算子0.8，变异算子0.01对父段染色体进行交叉变异，生成新的染色体，重复该过程1951代后满足停止准则，输出结果。算法流程如图4。

利用经过以上步骤所得到的标定结果，在94种不同位姿下对同一平面上各取5个点，共计470个恢复点恢复平面。图5为共面多个点的恢复计算值与它们的拟合平面之间的距离。最大误差小于0.2mm，平面上方及平面下方点到所拟合平面的平均距离分别为0.0678mm、-0.0592mm，标准差分别为0.0496、0.0408，可见该手眼标定方法误差很小。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (4)

1.一种激光线结构光传感器的手眼标定方法，其特征在于，包括如下步骤：

步骤1：将传感器通过工装夹具夹持于机器人末端连杆上，其中所述传感器为激光线结构光传感器；

步骤2：在传感器上建立坐标系O_lX_lY_lZ_l，传感器中激光器投射结构光光平面至靶标产生一道结构光条纹；令i＝1；

步骤3：调整机器人位姿使所述结构光条纹与标定模板上的直线相交形成特征点，并记录当前时刻机器人位姿参数及传感器的输出数据；

步骤4：提取结构光条纹与标定模板直线相交所形成的特征点，得到特征点在传感器坐标系下的坐标；

具体地，由传感器的输出结合相应的几何参数，能够获取结构光条纹上每个特征点在传感器坐标系下的三维坐标，记为(x_l，y_l，z_l)；

步骤5：判断i是否小于N，若小于则返回执行步骤3，令i自增1；若i大于等于N，得到N组特征点的坐标数据，执行步骤6；

步骤6：利用在空间直线约束下采集的N组特征点的坐标数据将标定问题转化为最小二乘问题，利用优化算法求解该最小二乘问题；

所述步骤6中的最小二乘问题的求解步骤如下：

步骤6.1：根据机器人运动学公式、标定模板的直线方程将机器人末端坐标系之间手眼关系标定问题转化为最优化问题；所述机器人运动学公式如下：

[p1]^T＝T_BE T_EL[x_p y_p z_p 1]^T

式中：(x_p y_p z_p)表示特征点P在传感器坐标系下的坐标，T_BE表示末端连杆坐标系相对于世界坐标系的齐次变换矩阵，T_EL表示传感器坐标系相对于末端连杆坐标系的齐次变换矩阵，[p1]^T中p表示特征点在世界坐标系上的三维坐标；

将上式展开，有：

其中R_BE与t_BE分别为T_BE的旋转与平移向量，由于传感器坐标系下y_p为零，T_EL旋转矩阵的第二列对测量点在机器人底座坐标系下的坐标无影响，因此待标定的参数为[r₁ r₂ r₃ r₄r₅ r₆ t₁ t₂ t₃]，其中存在单位向量约束：

假设标定模板直线方程为

式中：x、y、z分别表示传感器坐标系的坐标变量，(x₀，y₀，z₀)为直线上一点，(m,l,n)为该直线方向向量；其中m,l,n受到单位向量的约束：m²+l²+n²＝1；则标定模板直线外一点(x_b，y_b，z_b)到标定模板直线的距离的平方，记为d²，计算公式如下：

d²＝(x_b-x₀)²+(y_b-y₀)²+(z_b-z₀)²-(m*(x_b-x₀)+l*(y_b-y₀)+n*(z-z₀))²

传感器坐标系与机器人末端坐标系之间手眼关系标定问题转化为最优化问题，即求解

式中：f(y)表示适应度函数，min表示求取最小值，表示第i个点到标定模板直线的距离的平方，M为惩罚因子，为求和函数，n为约束条件数量，k_i表示第i个约束条件。

2.根据权利要求1所述的激光线结构光传感器的手眼标定方法，其特征在于，所述步骤2中在传感器上建立坐标系O_lX_lY_lZ_l包括：定义传感器坐标系的原点位于激光出射点，定义Z轴沿着出射激光角角平分线方向，X轴位于光平面内垂直于Z轴，Y轴垂直于光平面，由右手定则确定Y轴的方向。

3.根据权利要求1所述的激光线结构光传感器的手眼标定方法，其特征在于，还包括步骤7：利用遗传算法得到传感器坐标系与机器人末端坐标系的旋转平移关系。

4.根据权利要求3所述的激光线结构光传感器的手眼标定方法，其特征在于，所述步骤7中的遗传算法包括如下步骤：

步骤S1：首先生成初始种群A，并计算初始种群A中每条染色体的适度函数；其中，所述初始种群A在约束条件下随机生成并进行编码；

步骤S2：针对每条染色体的适度值，采用轮盘赌的方式从初始种群A中选择N条染色体，生成新的种群A’；

步骤S3：根据输入的交叉算子0.8使种群A’中的父代染色体两两交叉，交叉后的染色体进行合法检验，保留合法染色体；

步骤S4：根据输入的变异算子0.01对父代染色体进行基因段重排，进行合法检验，保留合法染色体；

步骤S5：判断是否满足停止准则f(y)小于阈值，若不满足，则返回执行，步骤S1；若满足，则输出得到传感器坐标系与机器人末端坐标系的旋转平移关系，结束。