CN112067337B - 基于标准球双目机器人的快速手眼标定装置及标定方法 - Google Patents

Abstract

本发明公开了一种基于标准球双目机器人的快速手眼标定装置，包括基座和双目摄像机，基座通过机械臂安装有机器人本体，机器人本体通过传动装置连接有法兰，法兰连接有夹具，夹具末端装夹有标准球；双目摄像机、机械臂和机器人本体均与机器人的示教器相连接；本发明还公开了相应的标定方法，第一步骤是定位标准球；第二步骤是得到手眼标定的旋转矩阵和平移矩阵的封闭解；第三步骤是根据Adam优化算法，通过优化迭代运算计算机器人坐标系与摄像机坐标系的相对位置关系作为手眼标定的结果。本发明成本低，安装方便，通过直线拟合得到三条互不共面的标准球直线轨迹，将标定方程AX＝XB转化成AX＝b，使用Adam迭代优化快速精确的求出手眼标定结果，结果更稳定更精确。

Description

基于标准球双目机器人的快速手眼标定装置及标定方法

技术领域

本发明涉及机器人标定技术领域，尤其是机器人手眼标定技术。

背景技术

传统手眼标定是通过摄像机拍摄多组标定靶(如棋盘格或圆形阵列标定靶)，来获取标定靶上标志点的位置和姿态，然后采用某种方法来计算出摄像机坐标系与机器人坐标系的相对位置关系。

这种标定过程往往比较复杂，特别是对于标定靶上标志点的位姿的计算，同时对相机和标定靶的精度要求比较高。

现提出一种基于标准球的快速手眼标定方法，只需计算出标准球的三条直线轨迹，就可以得到摄像机坐标系与机器人坐标系的相对位置关系，标定方法快捷准确，而且相比于棋盘格和圆形阵列标定靶，标准球具有制作成本低，标定精度更加稳定，安装方便快速等优点。

发明内容

本发明的目的在于提供一种基于标准球双目机器人的快速手眼标定装置，使用标准球作为标定靶，为简化标定过程、降低对相机和标定靶的精度要求、实现本发明的标定方法提供基础。

为实现上述目的，本发明的基于标准球双目机器人的快速手眼标定装置包括基座和双目摄像机，基座通过机械臂安装有机器人本体，机器人本体通过传动装置连接有法兰，法兰连接有夹具，夹具末端装夹有标准球；双目摄像机的拍摄方向朝向标准球的预定运动区域；

双目摄像机、机械臂和机器人本体均与机器人的示教器相连接；示教器与计算机相连接；示教器内建立有以机器人本体为中心的机器人坐标系和以双目摄像机为中心的摄像机坐标系。

本发明的目的还在于提供一种使用上述基于标准球双目机器人的快速手眼标定装置的标定方法，对计算出的标准球球心点的位姿进行直线拟合，得到三条互不共面的标准球的直线轨迹，利用线性不变性，将手眼标定经典方程AX＝XB转化成AX＝b的形式，使用Adam迭代优化快速精确的求出手眼标定结果。

为实现上述目的，本发明的标定方法按以下步骤进行：

第一步骤是定位标准球；

第二步骤是电控装置利用摄像机坐标系和机器人坐标系的关系、线性不变性原理以及标准球姿态与法兰姿态一致的原理进行优化初值计算，得到手眼标定的旋转矩阵和平移矩阵的封闭解；

第三步骤是电控装置根据手眼标定的旋转矩阵和平移矩阵的封闭解以及Adam优化算法，通过优化迭代运算计算机器人坐标系与摄像机坐标系的相对位置关系作为手眼标定的结果。

第一步骤包括三个子步骤；

第一子步骤是通过机器人的示教器移动标准球，同时示教器控制双目摄像机拍摄标准球运动图片；

第二子步骤是通过电控装置和示教器记录标准球运动图片的对应的法兰位姿P^r _e；

第三子步骤是通过双目视觉定位标准球球心位置P^c _b；

第二步骤包括三个子步骤；

第一子步骤是通过电控装置在机器人坐标系和摄像机坐标系下分别对标准球的球心轨迹作直线拟合，确定

和

为第i条直线在摄像机坐标系下的单位方向向量，

为第i条直线任一点坐标；

第二子步骤是获得手眼矩阵的旋转矩阵

第三子步骤是得出摄像机坐标系相对于机器人基座坐标系的平移矩阵

所述第二步骤第一子步骤具体是：

设空间任一点在摄像机坐标系下表示为P^c，在机器人坐标系下为P^r，则有式(3)：

式(3)中，

为摄像机坐标系到机器人坐标系的旋转向量，

是摄像机坐标系到机器人基座坐标系的平移向量；

通过示教器使机器人法兰做i＝1，2，3条直线运动时，则有式(4)：

其中

为直线上的点在机器人坐标系下的位姿，

为第i条直线在机器人基座坐标系下的单位方向向量，

为第i条直线任一点坐标，

和

从机器人的示教器上获取；

机器人末端法兰在直线轨迹上的姿态

保持不变，标准球姿态与机器人末端法兰姿态保持一致，因此标准球球心直线轨迹的方向向量与机器人末端法兰直线方向向量相同，标准球球心直线为式(5)：

式(5)中

为标准球的球心在机器人坐标系下的位置坐标；

由线性不变性，在摄像机坐标系下，标准球球心的运动轨迹为式(6)表达的直线：

其中

为直线上点在摄像机坐标系下的位姿，

为第i条直线在摄像机坐标系下的单位方向向量，

为第i条直线任一点坐标，对于

和

由双目相机拍摄标准球经图像处理得到标准球球心，然后作空间直线拟合，确定

和

所述第二步骤第二子步骤具体是：

设标准球球心的两条直线轨迹在机器人坐标系下的距离为

在摄像机坐标系下的距离为

则有式(7)：

对于标准球球心的同一条直线轨迹，在机器人坐标系下的方向向量

与在摄像机坐标系下的方向向量

满足式(8)：

令v_x＝v₁，v_y＝v₁×v₂，v_z＝v_x×v_y，V＝[v_x,v_y,v_z]，则

当v₁、v₂不共线时，矩阵V满秩；因此摄像机坐标系到机器人坐标系的旋转矩阵的封闭解为式(9)：

所述第二步骤第三子步骤具体是：

对于标准球球心在机器人坐标系下的3条直线轨迹

计算任意两条直线的距离，共有3组，n＝1,2,3，计算式为式(10)：

同理，对于标准球球心在摄像机坐标系下的3条直线轨迹

其两两直线之间的距离的计算式为式(11)：

令式(12)：

合并式(7)、式(10)、式(11)和式(12)得式(13)：

对于标准球球心相对于法兰的位置

由于标准球相对于法兰是固定的，所以

是恒定值，在安装标准球的时候预估

值，表示为

由公式(13)得到精确的

值：

其中A＝[A₁ ^T，A₂ ^T，A₂ ^T]^T，

为A的共轭转置矩阵；

在机器人坐标系下，标准球球心的每两条直线轨迹的最近点由式(14)计算得到，式(14)为：

同理在摄像机坐标系下，对应的每两条直线的最近点为

根据线性不变性原理及式(3)得到式(15)：

则摄像机坐标系相对于机器人坐标系的平移矩阵为式(16)：

第三步骤包括第一子步骤和第二子步骤；

第三步骤的第一子步骤是建立迭代方程，具体是：

对于机器人坐标系和摄像机坐标系下的标准球球心直线轨迹上的任意一组对应点，其相互关系表达为式(17)：

对于旋转矩阵

存在ω∈R³，使得：

本式为式(18)；

式18中

是ω的反对称矩阵；

将由式(9)求得的

作为初始解

一阶泰勒展开为式(19)：

令旋转矩阵

的修正量为

则有式(20)：

令

式(17)等号两边同乘以

联立式(20)得式(21)：

因为

整理可得式(22)即迭代方程：

式(22)形如AX＝B；

第三步骤的第二子步骤是使用Adam优化算法迭代优化修正

设置步长α为0.001，矩估计指数衰减速率β₁和β₂分别为0.9和0.999，用于数值稳定的小常数δ为10^-8；优化步骤如下：

①设置初始参数：

X₀；

g(X₀)＝DX₀-C；

α＝0.001；

β₁＝0.9；

β₂＝0.999；

δ＝10^-8；

②初始化偏一阶矩和偏二阶矩：

s₀＝0；

r₀＝0；

③初始化次数：

t＝1；

while(t＜1000)；

④计算梯度：

⑤更新偏一阶矩估计：

s_t＝β₁s_t-1+(1-β₁)/f(X)_t；

⑥更新偏二阶矩估计：

r_t＝β₂r_t-1+(1-β₂)/f(x)_t ²；

⑦修正偏一阶矩估计偏差：

⑧修正偏二阶矩估计偏差

⑨计算更新：

⑩结果：

X_t＝X_t-1+ΔX_t。

本发明具有如下的优点：

本发明采用标准球作为标定靶，标定精度更加稳定，并且标准球制作成本低，安装方便快捷。在此基础上的标定方法，通过对计算出的标准球球心点的位姿进行直线拟合，得到三条互不共面的标准球的直线轨迹，利用线性不变性原理，将手眼标定经典方程AX＝XB转化成AX＝b的形式，使用Adam迭代优化快速精确的求出手眼标定结果，标定过程简单高效，标定结果相较以往更为稳定和精确。

本发明具有两项突出优点：

1、使用标准球代替传统的标定靶，如棋盘格或圆形阵列标定靶，成本低、便于制作和安装，标定精度较高。

2、标定方法简单快捷，无需拍摄大量图像，将经典求解方程AX＝XB转换成对AX＝b方程的求解，将Adam优化方法引入到标定过程中，简化了迭代优化过程，提高了求解精度。

附图说明

图1是本发明的标定方法的流程图；

图2是本发明的基于标准球双目机器人的快速手眼标定装置的结构示意图；

图3是双目视觉定位原理图。

具体实施方式

如图1至图3所示，本发明的基于标准球双目机器人的快速手眼标定装置包括基座6和双目摄像机1，基座6通过机械臂7安装有机器人本体5，机器人本体5通过传动装置8(如传动杆)连接有法兰4，法兰4连接有夹具3，夹具3末端装夹有标准球2；双目摄像机1的拍摄方向朝向标准球2的预定运动区域；

双目摄像机1、机械臂7和机器人本体5均与机器人的示教器相连接；示教器与计算机相连接；示教器内建立有以机器人本体5为中心的机器人坐标系和以双目摄像机1为中心的摄像机坐标系。示教器为现有技术，图未示。

计算机并连接有显示屏、键盘和鼠标，可以采购各种型号的现有工控计算机，图未示。

本发明还公开了使用上述基于标准球2双目机器人的快速手眼标定装置的标定方法，按以下步骤进行：

第一步骤是定位标准球2；

第二步骤是电控装置利用摄像机坐标系和机器人坐标系的关系(一个坐标系的原点在另一个坐标系中的坐标决定了两个坐标系的关系)、线性不变性原理以及标准球2姿态与法兰4姿态一致的原理进行优化初值计算，得到手眼标定的旋转矩阵和平移矩阵的封闭解；

第三步骤是电控装置根据手眼标定的旋转矩阵和平移矩阵的封闭解以及Adam优化算法，通过优化迭代运算计算机器人坐标系与摄像机坐标系的相对位置关系作为手眼标定的结果。

第一步骤包括三个子步骤；

第一子步骤是通过机器人的示教器移动标准球2，同时示教器控制双目摄像机1拍摄标准球2运动图片；

第二子步骤是通过电控装置和示教器记录标准球2运动图片的对应的法兰4位姿P^r _e；

第三子步骤是通过双目视觉定位标准球2球心位置P^c _b；

双目视觉定位是利用视差及相似三角形原理来计算被测物体的深度信息，其原理是：假设空间一点P(X_W Y_W Z_W)在左、右相机下的投影坐标分别为P_l′(x_l，y_l)、P_r′(x_r，y_r)，深度为Z，视差为d＝x_l-x_r，如图3所示，根据三角形相似定律，可得式(1)：

由式(1)解方程得到式(2)：

其中基线B，即左右相机在x方向上的平移，可由相机标定确定；相机焦距f，由相机标定确定；视差d＝x_l-x_r。即P′_L,P′_R的关系。由式(2)可以知道求取空间一点P在摄像机坐标系下的三维坐标。

第二步骤包括三个子步骤；

第一子步骤是通过电控装置在机器人坐标系和摄像机坐标系下分别对标准球2的球心轨迹作直线拟合，确定

和

为第i条直线在摄像机坐标系下的单位方向向量，

为第i条直线任一点坐标；

第二子步骤是获得手眼矩阵的旋转矩阵

(即摄像机坐标系到机器人坐标系的旋转矩阵的封闭解)；

第三子步骤是得出摄像机坐标系相对于机器人基座6坐标系的平移矩阵

所述第二步骤第一子步骤具体是：

设空间任一点在摄像机坐标系下表示为P^c，在机器人坐标系下为P^r，则有式(3)：

式(3)中，

为摄像机坐标系到机器人坐标系的旋转向量，

是摄像机坐标系到机器人基座6坐标系的平移向量；

根据齐次变换的性质，在机器人基座6坐标系下的轨迹为直线，转换到摄像机坐标系下轨迹仍然为直线，并且每条直线的最近距离和最近点不变。通过示教器使机器人法兰4做i＝1，2，3条直线运动时，则有式(4)：

其中

为直线上的点在机器人坐标系下的位姿，

为第i条直线在机器人基座6坐标系下的单位方向向量，

为第i条直线任一点坐标，

和

从机器人的示教器上获取；

机器人末端法兰4在直线轨迹上的姿态

保持不变，标准球2姿态与机器人末端法兰4姿态保持一致，因此标准球2球心直线轨迹的方向向量与机器人末端法兰4直线方向向量相同，标准球2球心直线为式(5)：

式(5)中

为标准球2的球心在机器人坐标系下的位置坐标；

由线性(微分或积分)不变性，在摄像机坐标系下，标准球2球心的运动轨迹为式(6)表达的直线：

其中

为直线上点在摄像机坐标系下的位姿，

为第i条直线在摄像机坐标系下的单位方向向量，

为第i条直线任一点坐标，对于

和

由双目相机拍摄标准球2经图像处理得到标准球2球心，然后作空间直线拟合，确定

和

这两个参数。

所述第二步骤第二子步骤具体是：

设标准球2球心的两条直线轨迹在机器人坐标系下的距离为

在摄像机坐标系下的距离为

则有式(7)：

对于标准球2球心的同一条直线轨迹，在机器人坐标系下的方向向量

与在摄像机坐标系下的方向向量

满足式(8)：

令v_x＝v₁，v_y＝v₁×v₂，v_z＝v_x×v_y，V＝[v_x,v_y,v_z]，则

当v₁、v₂不共线时，矩阵V满秩；因此摄像机坐标系到机器人坐标系的旋转矩阵的封闭解为式(9)：

所述第二步骤第三子步骤具体是：

对于标准球2球心在机器人坐标系下的3条直线轨迹

计算任意两条直线的距离，共有3组，n＝1,2,3，计算式为式(10)：

同理，对于标准球2球心在摄像机坐标系下的3条直线轨迹

其两两直线之间的距离的计算式为式(11)：

令式(12)：

合并式(7)、式(10)、式(11)和式(12)得式(13)：

对于标准球2球心相对于法兰4的位置

由于标准球2相对于法兰4是固定的，所以

是恒定值，在安装标准球2的时候预估

值，表示为

由公式(13)得到精确的

值：

其中A＝[A₁ ^T，A₂ ^T，A₂ ^T]^T，

为A的共轭转置矩阵；

在机器人坐标系下，标准球2球心的每两条直线轨迹的最近点由式(14)计算得到，式(14)为：

同理在摄像机坐标系下，对应的每两条直线的最近点为

根据线性不变性原理及式(3)得到式(15)：

则摄像机坐标系相对于机器人坐标系的平移矩阵为式(16)：

第三步骤包括第一子步骤和第二子步骤；

第三步骤的第一子步骤是建立迭代方程，具体是：

对于机器人坐标系和摄像机坐标系下的标准球2球心直线轨迹上的任意一组对应点，其相互关系表达为式(17)：

对于旋转矩阵

存在ω∈R³，使得：

本式为式(18)；

式18中

是ω的反对称矩阵；

将由式(9)求得的

作为初始解

一阶泰勒展开为式(19)：

令旋转矩阵

的修正量为

则有式(20)：

令

式(17)等号两边同乘以

联立式(20)得式(21)：

因为

整理可得式(22)即迭代方程：

第三步骤的第二子步骤是使用Adam优化算法迭代优化修正

设置步长α为0.001，矩估计指数衰减速率β₁和β₂分别为0.9和0.999，用于数值稳定的小常数δ为10^-8；优化步骤如下：

①设置初始参数：

X₀；

g(X₀)＝DX₀-C；

α＝0.001；

β₁＝0.9；

β₂＝0.999；

δ＝10^-8；

②初始化偏一阶矩和偏二阶矩：

s₀＝0；

r₀＝0；

③初始化次数：

t＝1；

while(t＜1000)；

④计算梯度：

⑤更新偏一阶矩估计：

s_t＝β₁s_t-1+(1-β₁)/f(X)_t；

⑥更新偏二阶矩估计：

r_t＝β₂r_t-1+(1-β₂)/f(x)_t ²；

⑦修正偏一阶矩估计偏差：

⑧修正偏二阶矩估计偏差

⑨计算更新：

⑩结果：

X_t＝X_t-1+ΔX_t。

以上实施例仅用以说明而非限制本发明的技术方案，尽管参照上述实施例对本发明进行了详细说明，本领域的普通技术人员应当理解：依然可以对本发明进行修改或者等同替换，而不脱离本发明的精神和范围的任何修改或局部替换，其均应涵盖在本发明的权利要求范围当中。

Claims (1)

1.使用基于标准球双目机器人的快速手眼标定装置的标定方法，基于标准球双目机器人的快速手眼标定装置，包括基座和双目摄像机，基座通过机械臂安装有机器人本体，机器人本体通过传动装置连接有法兰，法兰连接有夹具，夹具末端装夹有标准球；双目摄像机的拍摄方向朝向标准球的预定运动区域；

双目摄像机、机械臂和机器人本体均与机器人的示教器相连接；示教器与计算机相连接；示教器内建立有以机器人本体为中心的机器人坐标系和以双目摄像机为中心的摄像机坐标系；其特征在于按以下步骤进行：

第一步骤是定位标准球；

第二步骤是电控装置利用摄像机坐标系和机器人坐标系的关系、线性不变性原理以及标准球姿态与法兰姿态一致的原理进行优化初值计算，得到手眼标定的旋转矩阵和平移矩阵的封闭解；

第三步骤是电控装置根据手眼标定的旋转矩阵和平移矩阵的封闭解以及Adam优化算法，通过优化迭代运算计算机器人坐标系与摄像机坐标系的相对位置关系作为手眼标定的结果；

第一步骤包括三个子步骤；

第一子步骤是通过机器人的示教器移动标准球，同时示教器控制双目摄像机拍摄标准球运动图片；

第二子步骤是通过电控装置和示教器记录标准球运动图片的对应的法兰位姿P^r _e；

第三子步骤是通过双目视觉定位标准球球心位置P^c _b；

第二步骤包括三个子步骤；

第一子步骤是通过电控装置在机器人坐标系和摄像机坐标系下分别对标准球的球心轨迹作直线拟合，确定

和

为第i条直线在摄像机坐标系下的单位方向向量，

为第i条直线任一点坐标；

第二子步骤是获得手眼矩阵的旋转矩阵

第三子步骤是得出摄像机坐标系相对于机器人基座坐标系的平移矩阵

所述第二步骤第一子步骤具体是：

设空间任一点在摄像机坐标系下表示为P^c，在机器人坐标系下为P^r，则有式(3)：

式(3)中，

为摄像机坐标系到机器人坐标系的旋转向量，

是摄像机坐标系到机器人基座坐标系的平移向量；

通过示教器使机器人法兰做i＝1，2，3条直线运动时，则有式(4)：

其中

为直线上的点在机器人坐标系下的位姿，

为第i条直线在机器人基座坐标系下的单位方向向量，

为第i条直线任一点坐标，

和

从机器人的示教器上获取；

机器人末端法兰在直线轨迹上的姿态

保持不变，标准球姿态与机器人末端法兰姿态保持一致，因此标准球球心直线轨迹的方向向量与机器人末端法兰直线方向向量相同，标准球球心直线为式(5)：

式(5)中

为标准球的球心在机器人坐标系下的位置坐标；

由线性不变性，在摄像机坐标系下，标准球球心的运动轨迹为式(6)表达的直线：

其中

为直线上点在摄像机坐标系下的位姿，

为第i条直线在摄像机坐标系下的单位方向向量，

为第i条直线任一点坐标，对于

和

由双目相机拍摄标准球经图像处理得到标准球球心，然后作空间直线拟合，确定

和

所述第二步骤第二子步骤具体是：

设标准球球心的两条直线轨迹在机器人坐标系下的距离为

在摄像机坐标系下的距离为

则有式(7)：

对于标准球球心的同一条直线轨迹，在机器人坐标系下的方向向量

与在摄像机坐标系下的方向向量

满足式(8)：

令v_x＝v₁，v_y＝v₁×v₂，v_z＝v_x×v_y，V＝[v_x,v_y,v_z]，则

当v₁、v₂不共线时，矩阵V满秩；因此摄像机坐标系到机器人坐标系的旋转矩阵的封闭解为式(9)：

所述第二步骤第三子步骤具体是：

对于标准球球心在机器人坐标系下的3条直线轨迹

计算任意两条直线的距离，共有3组，n＝1,2,3，计算式为式(10)：

同理，对于标准球球心在摄像机坐标系下的3条直线轨迹

其两两直线之间的距离的计算式为式(11)：

令式(12)：

合并式(7)、式(10)、式(11)和式(12)得式(13)：

对于标准球球心相对于法兰的位置

由于标准球相对于法兰是固定的，所以

是恒定值，在安装标准球的时候预估

值，表示为

由公式(13)得到精确的

值：

其中A＝[A₁ ^T，A₂ ^T，A₂ ^T]^T，

为A的共轭转置矩阵；

在机器人坐标系下，标准球球心的每两条直线轨迹的最近点由式(14)计算得到，式(14)为：

同理在摄像机坐标系下，对应的每两条直线的最近点为

根据线性不变性原理及式(3)得到式(15)：

则摄像机坐标系相对于机器人坐标系的平移矩阵为式(16)：

第三步骤包括第一子步骤和第二子步骤；

第三步骤的第一子步骤是建立迭代方程，具体是：

对于机器人坐标系和摄像机坐标系下的标准球球心直线轨迹上的任意一组对应点，其相互关系表达为式(17)：

对于旋转矩阵

存在ω∈R³，使得：

本式为式(18)；

式18中

是ω的反对称矩阵；

将由式(9)求得的

作为初始解

一阶泰勒展开为式(19)：

令旋转矩阵

的修正量为

则有式(20)：

令

式(17)等号两边同乘以

联立式(20)得式(21)：

因为

整理可得式(22)即迭代方程：

式(22)形如AX＝B；

第三步骤的第二子步骤是使用Adam优化算法迭代优化修正

设置步长α为0.001，矩估计指数衰减速率β₁和β₂分别为0.9和0.999，用于数值稳定的小常数δ为10^-8；优化步骤如下：

①设置初始参数：

X₀；

g(X₀)＝DX₀-C；

α＝0.001；

β₁＝0.9；

β₂＝0.999；

δ＝10^-8；

②初始化偏一阶矩和偏二阶矩：

s₀＝0；

r₀＝0；

③初始化次数：

t＝1；

while(t＜1000)；

④计算梯度：

⑤更新偏一阶矩估计：

s_t＝β₁s_t-1+(1-β₁)/f(X)_t；

⑥更新偏二阶矩估计：

r_t＝β₂r_t-1+(1-β₂)/f(x)_t ²；

⑦修正偏一阶矩估计偏差：

⑧修正偏二阶矩估计偏差

⑨计算更新：

⑩结果：

X_t＝X_t-1+ΔX_t。

Images