CN110695991B - 基于变异粒子群优化的自动手眼标定方法 - Google Patents

Abstract

本发明公开了一种基于变异粒子群优化的自动手眼标定方法，初始化机械臂位姿，并从图像中提取特征物体和特征物体的特征点；采集机械臂关节角度数据以及特征点在相机坐标系下的坐标，进而确定机械臂末端坐标系到机械臂基座坐标系的转换矩阵，以及特征点在相机坐标系下的三维坐标的集合矩阵；采用在线变异粒子群优化算法，估计相机坐标系与机械臂末端坐标系的转换关系参数；通过能量最优函数计算各关节最优旋转角度，对机械臂关节角度进行补偿，同时考虑特征点质心与相机坐标系原点的距离限制，确定下一步的移动方向；重复在线变异粒子群优化算法和能量最优函数，直至机械臂走完固定路径。本发明减少了标定时间，降低了参数估计误差。

Description

基于变异粒子群优化的自动手眼标定方法

技术领域

本发明涉及机械臂手眼标定领域，具体涉及一种基于变异粒子群优化的自动手眼标定方法。

背景技术

随着计算机技术的不断发展，机械臂已广泛应用于生产生活之中。针对非结构化环境的应用需求，将视觉系统加入机械臂控制系统构成视觉伺服系统，可使机械臂具有环境和目标感知能力，极大地提高了机械臂控制的智能化水平。

手眼标定通过标定机械臂坐标系与视觉坐标系的关系矩阵，为机械臂高精度视觉伺服控制奠定基础。传统的手眼标定方法以离线手眼标定方法为主，其中，Tsai两步法是这类方法的代表，该方法首先利用通用旋转变换求得旋转矩阵，然后利用最小二乘法计算平移向量。由于离线手眼标定方法对采集到的数据是否含有异常值或是否发生退化现象无法得到实时的反馈，因此一些学者提出了在线手眼标定方法。例如王君臣等提出一种基于无迹卡尔曼滤波的手眼标定方法，通过无迹卡尔曼滤波在线估计标定参数，其标定精度高于最小二乘标定算法。然而，这类方法对于标定位姿的生成没有一种规范定量的计算方法，位姿选取仍然耗时较多。

发明内容

本发明的目的在于提供一种基于变异粒子群优化的自动手眼标定方法。

实现本发明目的的技术解决方案为：一种基于变异粒子群优化的自动手眼标定方法，包括以下步骤：

步骤1，搭建机械臂手眼标定实验系统，初始化机械臂位姿，并从图像中提取特征物体和特征物体的特征点；

步骤2，控制机械臂进行固定路径的平移和旋转运动，采集机械臂关节角度数据以及特征点在相机坐标系下的坐标，进而确定机械臂末端坐标系到机械臂基座坐标系的转换矩阵，以及特征点在相机坐标系下的三维坐标的集合矩阵；

步骤3，采用在线变异粒子群优化算法，估计相机坐标系与机械臂末端坐标系的转换关系参数；

步骤4，通过能量最优函数计算各关节最优旋转角度，对机械臂关节角度进行补偿，同时考虑特征点质心与相机坐标系原点的距离限制，确定下一步的移动方向；

步骤5，循环步骤3-4，直至机械臂走完固定路径,即完成自动手眼标定。

本发明与现有技术相比，其显著优点为：通过变异粒子群算法优化得到的手眼参数，同时通过能量最优法在线补偿机械臂位姿，无需人工给定机械臂标定路径，就能保证标定过程中特征点始终保持在相机视野范围内，大大减少了标定时间，降低了机械臂关节运动量较小时的参数估计误差。

附图说明

图1为本发明基于变异粒子群优化的自动手眼标定方法流程图。

图2为基于深度相机的手眼关系示意图。

图3为相机坐标系补偿前后示意图。

具体实施方式

下面结合附图和具体实施例,进一步说明本发明方案。

如图1所示，基于变异粒子群优化的自动手眼标定方法，包括以下步骤：

步骤1，搭建机械臂手眼标定实验系统，初始化机械臂位姿，并从图像中提取特征物体和特征物体的特征点，具体步骤如下：

步骤1-1，搭建基于深度相机的Eye-in-hand手眼系统，该系统中相机安装于机械臂末端关节，所需标定的是相机坐标系与机械臂末端坐标系的转换关系。

基于深度相机的Eye-in-hand手眼系统的坐标系关系如图2所示。图中，

为机械臂末端坐标系到机械臂基座坐标系的转换矩阵，由机械臂各关节角度确定；

为相机坐标系到机械臂末端坐标系的转换矩阵，由手眼标定参数构成；P_c为特征点在相机坐标系下的三维坐标的集合矩阵，其定义如下：设p_i为一个四维向量，由第i个特征点在相机坐标系下的三维坐标向量和一个元素1组成，则P_c定义为P_c＝[p₁,p₂,…,p_n] (1)

其中，n为特征点个数；P_r表示特征物体的特征点在机械臂基座坐标系下的三维坐标的集合矩阵，其定义如下：设第i次采集的机械臂末端坐标系到机械臂基座坐标系的转换矩阵为

特征点在相机坐标系下的三维坐标的集合矩阵为P_c(i)，当特征物体在机械臂基座坐标系下固定不动时，由图1中的坐标系关系，可得

其中，Constant表示常量，s为标定过程中采集的机械臂位姿数。

步骤1-2，将机械臂调整至初始位姿，保证特征物体在相机中心，从图像中提取特征物体，检测特征物体的角点作为特征点。

步骤2，控制机械臂进行固定路径的平移和旋转运动，采集机械臂关节角度数据以及特征点在相机坐标系下的坐标，进而确定机械臂末端坐标系到机械臂基座坐标系的转换矩阵，以及特征点在相机坐标系下的三维坐标的集合矩阵，具体步骤如下：

步骤2-1，随机生成一组增量式机械臂路径，控制机械臂按照生成路径进行平移和旋转运动；

增量式机械臂路径定义如下：设第i次机械臂运动前机械臂t个关节角度为

第i次机械臂运动增量为

则第i次机械臂运动后机械臂t个关节角度为

步骤2-2，采用角点跟踪方法跟踪特征点，计算特征点在相机坐标系下的坐标；

设特征角点在图像坐标系下的三维坐标为(u,v)，由式(3)计算得到特征角点在相机坐标系下的三维坐标：

其中，f_x，f_y，u₀，v₀为相机内参，通过深度相机标定得出，x_c、y_c、z_c为特征点在相机坐标系下的三维坐标，z_c可由深度相机直接读取出。记录每个角点的三维数据，并转化为式(1)的形式。

步骤2-3，采用socket通信读取机械臂各关节角度，由机械臂正运动学计算机械臂末端坐标系到机械臂基座坐标系的转换矩阵

并记录。

步骤3，采用在线变异粒子群优化算法，估计相机坐标系与机械臂末端坐标系的转换关系参数，具体步骤如下：

本发明中，需要估计的对象是相机坐标系到机械臂末端坐标系的转换矩阵

由六个参数确定，分别为绕相机三个坐标轴旋转角度α、β、γ以及坐标原点的平移量t_x、t_y、t_z。在线变异粒子群优化算法中，定义第i个粒子的位置为x_i＝[α_i,β_i,γ_i,t_xi,t_yi,t_zi]，速度为v_i＝[v_αi,v_βi,v_γi,v_txi,v_tyi,v_tzi]，粒子的更新过程为：

其中，k、k+1分别表示迭代次数，b_i为第i个粒子搜索过程中的最优位置，g为所有粒子搜索过程中的最优位置，c₁、c₂为加速度因子，在初始化粒子群框架时设置；r₁、r₂为(0,1)间的随机数；w为惯性因子，由式(5)确定：

其中，k_max表示粒子群算法最大迭代次数，用于控制收敛速度，w₀、w₁分别为设置的惯性因子初值与终值。

为避免粒子过早收敛，引入遗传算法变异思想，对粒子施加扰动量

其中，k表示迭代次数，s表示观测数据组数，λ₁、λ₂为预设系数，G(·)表示正态分布的随机数向量。

变异后的粒子表示为：

x_i′＝x_i+dist_i (7)

定义适应度函数为：

其中，Var(P_r(j)|x＝xi)表示第j个矩阵P_r(j)在粒子为x_i下的方差，矩阵的方差可由矩阵每个元素的方差和表示。

步骤4，通过能量最优函数计算各关节最优旋转角度，对机械臂关节角度进行补偿，同时考虑特征点质心与相机坐标系原点的距离限制，确定下一步的移动方向，具体实现步骤如下：

步骤4-1，定义补偿动作的能量函数为：

其中，t为机械臂关节数，θ_i、

分别为补偿前后各关节角度，a_i为角度能量比例系数，设下标i从小到大分别表示机械臂基座关节至末端关节的标号，为避免机械臂补偿过程中主体运动幅度过大，有：

步骤4-2，以补偿动作的能量函数最小为目标，计算各关节最优旋转角度；

相机坐标系补偿前后示意图如图3所示，图中，O为相机坐标原点，po、

分别为补偿前后的图像中心，p_c为点p在补偿前图像中的成像位置。记补偿前特征点质心坐标为p＝[x,y,z]^T，补偿后特征点质心坐标为p^*＝[x^*,y^*,z^*]^T，在相机坐标原点不平移的情况下，可得：

由坐标轴旋转变换关系可得：

p^*＝Rp (12)

其中，R＝R_zR_yR_x，由几何关系得：

式中，相机坐标系绕x轴旋转角度

绕y轴旋转角度

绕z轴旋转角度θ_z为可调整量。

设补偿前后机械臂末端坐标系到机械臂基座坐标系的转换矩阵分别为

则：

其中，P与P^*的定义与式(1)中的特征点集矩阵定义相同。该式与式(12)联立，可得：

由于矩阵

由机械臂关节角度唯一确定，式(15)可确定补偿后机械臂各关节角度与相机坐标系z轴旋转角度θ_z的关系；

对能量值J求关于θ_z的偏导；

当已知机械臂末端坐标系到机械臂基座坐标系的转换矩阵

时，可通过解析逆解的方式求得机械臂每个关节角度，即存在关系式F(·)使得

另外，由式(15)可得

式中，

为

矩阵关于θ^*的雅可比矩阵，

为

矩阵关于θ_z的雅可比矩阵。联立式(16)、式(17)与式(18)可求得J关于θ_z的偏导，从而求得能量最优情况下的θ_z角度，并由式(15)求得机械臂补偿后各关节角度，进行角度补偿；

步骤4-3，设图像边界阈值为d，则将限制特征点在图像坐标系下可表示为：

联立公式(3)和(19)，可得特征点与相机坐标系原点的距离满足如下关系：

式中，x_c、y_c的上界可由特征点与特征点质心的三维距离确定，因此可以确定z_c的下界，在控制机械臂末端朝一个固定方向进行位移时，若z_c小于下界，则调整机械臂末端运动方向，防止特征点移出相机视野。

步骤5，循环步骤3和步骤4，直至机械臂走完固定路径，具体步骤如下：

判断机械臂是否走完步骤2-1所生成的增量式路径，若是，结束标定，若否，则继续采集数据并优化标定参数。

Claims (3)

1.基于变异粒子群优化的自动手眼标定方法，其特征在于，包括以下步骤：

步骤1，搭建基于深度相机的Eye-in-hand手眼系统，初始化机械臂位姿，并从图像中提取特征物体和特征物体的特征点；

步骤2，控制机械臂进行固定路径的平移和旋转运动，采集机械臂关节角度数据以及特征点在相机坐标系下的坐标，进而确定机械臂末端坐标系到机械臂基座坐标系的转换矩阵，以及特征点在相机坐标系下的三维坐标的集合矩阵；

步骤3，采用在线变异粒子群优化算法，估计相机坐标系与机械臂末端坐标系的转换关系参数；

步骤4，通过能量最优函数计算各关节最优旋转角度，对机械臂关节角度进行补偿，同时考虑特征点质心与相机坐标系原点的距离限制，确定下一步的移动方向；

步骤5，循环步骤3-4，直至机械臂走完固定路径,即完成自动手眼标定；

步骤3中，在线变异粒子群优化算法框架为：

定义第i个粒子的位置为x_i＝[α_i,β_i,γ_i,t_xi,t_yi,t_zi]，速度为v_i＝[v_αi,v_βi,v_γi,v_txi,v_tyi,v_tzi]，粒子的更新过程为：

其中，α_i、β_i、γ_i分别表示第i个粒子绕相机坐标系三个坐标轴的旋转角度，t_xi、t_yi、t_zi分别表示第i个粒子的坐标原点沿相机坐标系三个坐标轴的平移量，k、k+1分别表示迭代次数，b_i为第i个粒子搜索过程中的最优位置，g为所有粒子搜索过程中的最优位置，c₁、c₂为加速度因子，c₁、c₂在初始化粒子群优化算法框架时设置，r₁、r₂为(0,1)间的随机数，w为惯性因子，由式(5)确定：

其中，k_max表示粒子群优化算法最大迭代次数，用于控制收敛速度，w₀、w₁分别为设置的惯性因子初值与终值；

定义对粒子施加扰动量为：

其中，k表示迭代次数，s表示观测数据组数，λ₁、λ₂为预设系数，G(·)表示正态分布的随机数向量，则变异后的粒子表示为：

x_i′＝x_i+dist_i (7)

定义适应度函数为：

其中，Var(P_r(j)|x＝x_i)表示第j个矩阵P_r(j)在粒子为x_i下的方差，矩阵的方差可由矩阵每个元素的方差和表示；

步骤4的具体方法为：

步骤4-1，定义补偿动作的能量最优函数为：

其中，t为机械臂关节数，θ_i、

分别为补偿前后各关节角度，a_i为角度能量比例系数，下标i从小到大分别表示机械臂基座关节至末端关节的标号，有：

步骤4-2，以补偿动作的能量最优函数最小为目标，计算各关节最优旋转角度；

设补偿前特征点质心坐标为p＝[x,y,z]^T，补偿后特征点质心坐标为p^*＝[x^*,y^*,z^*]^T，在相机坐标系原点不平移的情况下，可得：

由坐标轴旋转变换关系可得：

p^*＝Rp (12)

其中，R＝R_zR_yR_x，由几何关系得：

式中，相机坐标系绕x轴旋转角度

绕y轴旋转角度

绕z轴旋转角度θ_z为可调整量；

设补偿前后机械臂末端坐标系到机械臂基座坐标系的转换矩阵分别为

则有：

其中，

为相机坐标系到机械臂末端坐标系的转换矩阵，式(14)与式(12)联立，可得：

由于矩阵

由机械臂关节角度唯一确定，式(15)可确定补偿后机械臂各关节角度与相机坐标系绕z轴旋转角度θ_z的关系，对能量值J求关于θ_z的偏导；

当已知补偿后机械臂末端坐标系到机械臂基座坐标系的转换矩阵

时，可通过解析逆解的方式求得机械臂每个关节角度，即存在关系式F(·)使得

另外，由式(15)可得

式中，

为

关于θ^*的雅可比矩阵，

为

关于θ_z的雅可比矩阵，联立式(16)、式(17)与式(18)可求得能量最优情况下的θ_z，并由式(15)求得机械臂补偿后各关节角度，进行角度补偿；

步骤4-3，设图像边界阈值为d，则将限制特征点在图像坐标系下可表示为：

联立公式(3)和(19)，可得特征点与相机坐标系原点的距离满足如下关系：

式中，x_c、y_c、z_c为特征点在相机坐标系下的三维坐标，f_x，f_y，u₀，v₀为相机内参，x_c、y_c的上界能够由特征点与特征点质心的三维距离确定，因此能够确定z_c的下界，在控制机械臂末端朝一个固定方向进行位移时，若z_c小于下界，则调整机械臂末端运动方向，防止特征点移出相机视野，(u,v)为特征点在图像坐标系下的三维坐标。

2.根据权利要求1所述的基于变异粒子群优化的自动手眼标定方法，其特征在于，步骤1的具体方法为：

步骤1-1，该系统中相机安装于机械臂末端关节；

机械臂末端坐标系到机械臂基座坐标系的转换矩阵

由机械臂各关节角度确定；相机坐标系到机械臂末端坐标系的转换矩阵

为标定所要确定的结果，手眼标定参数由绕相机坐标系三个坐标轴的旋转角度α、β、γ以及相机坐标系原点延相机坐标系三个坐标轴的平移量t_x、t_y、t_z构成；特征点在相机坐标系下的三维坐标的集合矩阵P_c定义如下：设p_i为一个四维向量，由第i个特征点在相机坐标系下的三维坐标向量和一个元素1组成，则P_c定义为

P_c＝[p₁,p₂,…,p_n] (1)

其中，n为特征点个数；特征点在机械臂基座坐标系下的三维坐标的集合矩阵P_r，其定义如下：设第i次采集的机械臂末端坐标系到机械臂基座坐标系的转换矩阵为

特征点在相机坐标系下的三维坐标的集合矩阵为P_c(i)，当特征物体在机械臂基座坐标系下固定不动时，得到：

其中，Constant表示常量，s为标定过程中采集的机械臂位姿数，i＝1,2,…,s；

步骤1-2，将机械臂调整至初始位姿，保证特征物体在相机中心，从图像中提取特征物体，检测特征物体的角点作为特征点。

3.根据权利要求1所述的基于变异粒子群优化的自动手眼标定方法，其特征在于，步骤2的具体方法为：

步骤2-1，随机生成一组增量式机械臂路径，控制机械臂按照生成路径进行平移和旋转运动，其中：

增量式机械臂路径定义如下：设第i次机械臂运动前机械臂t个关节角度为

第i次机械臂运动增量为

则第i次机械臂运动后机械臂t个关节角度为

步骤2-2，采用角点跟踪方法跟踪特征点，计算特征点在相机坐标系下的三维坐标的集合矩阵P_c；

设特征点在图像坐标系下的三维坐标为(u,v)，则计算特征点在相机坐标系下三维坐标的公式为：

其中，f_x，f_y，u₀，v₀为相机内参，通过深度相机标定得出，x_c、y_c、z_c为特征点在相机坐标系下的三维坐标，z_c由深度相机直接读取出；

步骤2-3，采用socket通信读取机械臂各关节角度，由机械臂正运动学计算机械臂末端坐标系到机械臂基座坐标系的转换矩阵

Images