一种基于相机光轴约束的双机械臂标定方法与流程

本发明涉及一种双机械臂标定方法，特别是一种基于相机光轴约束的双机械臂标定方法，属于机器人标定领域。

背景技术：

双臂机器人作为机器人产业发展的重大标志性产品之一，在工业、生活、医疗、航空航天等领域有着越来越广泛的应用。双机械臂可以合作完成单个机械臂难以完成的任务，如移动质量或体积大的物体、多部件复杂装配、柔性物体的处理等，具有节约成本、节省空间、提高生产效率等优点。在双臂协作完成各种任务时，双臂系统的精度直接影响了任务的完成程度和成功率，零件装配等任务对双臂系统的精度提出了更高的要求。为了提高双臂系统的定位精度，必须对其进行有效的标定。

在双臂系统完成协作任务时，两机械臂基座的相对关系需要提前已知，即为双机械臂的基坐标标定。对于双机械臂系统，一般只进行基坐标标定，而由于制造公差、环境变化和磨损等因素的影响，机械臂实际的运动学参数与其出厂设置的名义运动学参数存在误差，导致机械臂的末端绝对定位精度降低，因此需要进行运动学参数标定。目前，机械臂运动学参数标定和基坐标系标定通常使用高精度的测量仪器或精心制作的标定工具，这类方法价格昂贵且需要专业的人员操作，不适合大多数场景中对简单有效标定的要求，并且机械臂运动学参数标定和基坐标标定通常分开进行，使用不同的标定设备和标定方法，使标定过程更加复杂。

技术实现要素：

针对上述现有技术，本发明要解决的技术问题是提供一种不需要昂贵的标定设备，精度高且操作简单的基于相机光轴约束的双机械臂标定方法。

为解决上述技术问题，本发明提供一种基于相机光轴约束的双机械臂标定方法，包括以下步骤：

步骤一：构建双机械臂标定系统；

步骤二：建立基于误差模型的参数标定方程；

步骤三：基于视觉控制的特征点对齐及位置数据获取；

步骤四：求解标定方程。

本发明还包括：

1.步骤一中双机械臂标定系统具体为：包括两个机械臂，在其中一个机械臂的末端固定一个相机，此机械臂为被动机械臂；另一个机械臂末端固定一个棋盘格标定板，此机械臂为主动机械臂。

2.步骤二中建立基于误差模型的参数标定方程具体为：

步骤1：分别对每个机械臂建立运动学误差模型，得到机械臂的末端位置误差δpe与运动学参数误差向量之间的关系：其中jp为运动学位置误差雅可比矩阵；

步骤2：根据建立的运动学误差模型，推导基于直线约束的运动学误差模型，得到运动学参数标定方程：其中e为位置对齐误差，φ为运动学误差雅可比矩阵；

步骤3：建立两机械臂的基座姿态变换误差模型，得到双臂基坐标姿态变换矩阵的标定方程，具体为：其中，{a}为主动机械臂基坐标系，{p}和{h}分别为被动机械臂基坐标系和末端坐标系，{c}为相机坐标系，^azc和^prh分别为相机z轴向量相对于主动机械臂基坐标系的向量和被动机械臂末端相对于其基坐标的姿态矩阵；^pra和^hzc分别为主动机械臂基坐标系相对于被动机械臂基坐标系的姿态矩阵和相机z轴相对于被动机械臂末端的向量，和是^hzc和^pra的名义值，δ^hzc和δ^pra是^hzc和^pra的误差；

步骤4：建立两机械臂的基座位置变换误差模型，得到双臂基坐标位置变换矩阵的标定方程，具体为：jm[δ^app,a,δ^hpc,h]^t＝ρm，其中^app,a和^hpc,h分别为在主动机械臂基坐标系中描述的被动机械臂基坐标相对于主动机械臂基坐标的位置向量和在被动机械臂末端坐标系中描述的相机到被动机械臂末端的位置向量，上述两个向量的误差分别为δ^app,a和δ^hpc,h，jm为基坐标位置误差雅可比矩阵,其中μk为光轴向量,i为单位矩阵,ρm为位置误差矩阵,其中,i为特征点当前的位置数，i≤p，k为光轴当前的位置数，k≤n，·^(i,k)为特征点在第k条光轴上第i个位置处变量·的值，{a}和{e}分别为主动机械臂基坐标系和末端坐标系，{p}和{h}分别为被动机械臂基坐标系和末端坐标系，{c}和{f}分别为相机坐标系和工具中心坐标系，^arp为被动机械臂基坐标系相对于主动机械臂基坐标系的姿态矩阵，^prh和^are分别为被动机械臂末端相对于被动机械臂基座的姿态矩阵和主动机械臂末端相对于主动机械臂基座的姿态矩阵，^ape,a为在主动机械臂基坐标系中描述的主动机械臂末端相对于主动机械臂基座的位置向量，^epf,e为在主动机械臂末端坐标系中描述的工具坐标系相对于主动机械臂末端的位置向量，^pph,p为在被动机械臂基坐标系中描述的被动机械臂末端相对于被动机械臂基座的位置向量，和分别为^hpc,h和^app,a的名义值。

3.步骤三中基于视觉控制的特征点对齐及位置数据获取具体为：

步骤1：被动机械臂末端位姿固定，利用基于图像的视觉控制方法控制主动机械臂运动，使特征点自动运动到光轴上，记录此时两机械臂的关节角；

步骤2：改变主动机械臂末端位姿，重复步骤1，使特征点依次到达光轴上n个不同的位置，其中n≥3；

步骤3：改变被动机械臂末端位姿，重复步骤1–2；

步骤4：根据记录的特征点与光轴对齐时两机械臂的关节角，利用每个机械臂的正运动学计算每个位置点处机械臂末端相对于基坐标系的名义位姿；

步骤5：互换相机和棋盘格标定板的位置，重复步骤1–4。

4.步骤四中求解标定方程具体为：

步骤1：根据运动学参数标定方程使用迭代最小二乘法求解每个机械臂的运动学参数误差，得到两个机械臂真实的运动学参数；

步骤2：根据双臂基坐标姿态变换矩阵标定方程迭代估计基坐标姿态变换矩阵；

步骤3：根据双臂基坐标位置变换矩阵标定方程jm[δ^app,a,δ^hpc,h]^t＝ρm，估计基坐标位置变换矩阵。

本发明有益效果：针对现有技术的缺陷和改进需求，本发明提供了一种基于相机光轴约束的双机械臂标定方法，利用相机光轴构建虚拟约束，建立基于直线约束的标定方程，令两机械臂运动到满足约束的位姿，测量的机械臂关节角及位姿数据同时用于运动学参数标定方程和基坐标标定方程，通过对标定方程求解估计得到真实的参数值。本发明不需要昂贵的标定设备，仅需相机和棋盘格标定板即可同时完成运动学参数标定和基坐标标定，标定精度高且操作简单，可直接应用于各场景的双臂系统标定中。本发明可同时实现两个机械臂的运动学参数标定和基坐标位姿变换矩阵标定。

1.本发明仅利用相机和棋盘格标定板构建双机械臂标定系统，操作简便，不需要昂贵的高精度仪器和精心制作的标定工具，降低了标定成本；

2.本发明基于相机光轴约束同时完成两个机械臂的运动学参数标定和基坐标标定，与使用不同方法分别标定运动学参数和基坐标的方法相比，本发明标定精度更高，标定步骤更少，操作更加方便；

3.本发明使用基于图像的视觉控制方法控制主动机械臂运动，使特征点自动与相机光轴对齐，标定过程无需专业人员操作，仅需操作者进行简单的监督即可；

4.本发明的标定方法适用于各类双臂协作系统，标定精度高，标定结果可满足大部分双臂协作任务的需求。

附图说明

图1是本发明的双机械臂标定系统示意图；

图2是本发明的特征点位置与光轴的相对关系图；

图3是本发明的双机械臂标定系统坐标系分布图；

图4是本发明的基于图像的视觉控制框图；

图5是本发明的特征点在一条光轴上的位置图。

具体实施方式

下面结合附图对本发明进行详细说明。

本发明提供了一种基于相机光轴约束的双机械臂标定方法。目前机器人飞速发展，越来越多的领域需要双臂协作机器人，为了顺利完成操作任务，必须对双臂系统进行标定。针对目前双臂协作机器人的定位精度较低的问题，本发明同时完成两个机械臂的运动学参数标定和基坐标标定，为双臂系统完成高精度任务提供技术支持。本发明的基本思想是令安装在一个机械臂末端的相机的光轴作为虚拟直线约束，另一个机械臂的末端位姿满足光轴虚拟约束，基于直线约束建立机械臂的运动学误差模型和基坐标误差模型，根据误差模型建立标定方程，通过对标定方程求解估计得到真实的机械臂运动学参数和基坐标位姿变换矩阵。本发明包括：构建典型的双机械臂标定系统，其中一个机械臂末端安装相机，另一个机械臂末端安装棋盘格标定板，将标定板上的中心角点作为特征点；建立双机械臂系统的参数误差模型，得到参数标定方程；将相机光轴作为虚拟约束，通过视觉控制方法控制特征点依次到达光轴上的多个不同位置，获取标定方程所需的位置信息；对标定方程求解，得到两机械臂的运动学参数和两机械臂基坐标的位姿变换矩阵，以此完成双臂系统的参数标定。本发明成本低、易操作，不需要昂贵的高精度测量设备和特定的标定工具，仅利用相机和机器人的关节角数据即可完成标定，对双机械臂系统标定具有通用性，适用于各类双臂协作环境。

本发明采用了以下技术方案：

基于相机光轴约束的双机械臂标定方法，包括构建双机械臂标定系统、建立基于误差模型的参数标定方程、基于视觉控制的特征点对齐及数据获取、标定方程求解。其中：

(1)构建双机械臂标定系统，其中一个机械臂末端固定一个相机，另一个机械臂末端固定一个棋盘格标定板；

(2)将相机光轴作为虚拟直线约束，建立基于直线约束的双臂系统的参数误差模型，包括运动学误差模型、基坐标姿态误差模型和基坐标位置误差模型，分别推导得到运动学参数和双臂基坐标位姿变换矩阵的标定方程；

(3)选择标定板的中心角点作为特征点，使用基于图像的视觉控制方法控制特征点依次到达相机光轴的多个位置，记录并保存位置对齐时两机械臂的关节角数据，并获取标定方程所需的位置信息；

(4)求解标定方程，估计真实的运动学参数及双臂基坐标位姿变换矩阵，完成双臂系统的标定。

在一些实施方式中，构建双机械臂标定系统具体为：

典型的双机械臂系统包括两个机械臂，构建双机械臂标定系统，参见图1：在其中一个机械臂的末端固定一个相机2，此机械臂称为被动机械臂4；另一个机械臂末端固定一个棋盘格标定板1，此机械臂称为主动机械臂3。

在一些实施方式中，建立基于误差模型的参数标定方程具体为：

(1)基于机械臂的正运动学方程分别对每个机械臂建立运动学误差模型，得到末端位置误差δpe与运动学参数误差向量之间的关系：其中jp为运动学位置误差雅可比矩阵。

(2)根据建立的运动学误差模型，推导基于直线约束的运动学误差模型，得到运动学参数标定方程。

选择标定板上的中心角点作为特征点，将特征点看作主动机械臂3的工具中心点，特征点在多个位置处与相机光轴对齐，此时主动机械臂3真实的末端位姿也满足直线约束，由于存在运动学参数误差，根据当前主动机械臂3的关节角计算的名义末端位姿也存在误差，且不满足光轴直线约束。

参见图2，假设与第k条光轴对齐的第i个位置点处主动机械臂3的真实位姿为对应的名义值为它们之间的差为：

分别将与沿着光轴方向进行分解，得到：

其中，为相机坐标系原点，μk为光轴向量，s^(i,k)与分别为与沿光轴方向的分量，为与光轴方向垂直的分量，由于位于光轴上，所以它与光轴垂直的分量为0.

那么，的等式可以表示为：在其两边同乘[μk×]，得到：

同理，位置处有：

上面两式相减得到：

将上式表示为：其中

对于所有与光轴对齐的位置点有：此式即为机械臂运动学参数的标定方程。

(3)建立两机械臂的基座姿态变换误差模型，得到双臂基坐标姿态变换矩阵的标定方程。

参见图3，图中在双机械臂标定系统中表示了各坐标系的符号表示，{a}和{e}分别为主动机械臂3基坐标系和末端坐标系，{p}和{h}分别为被动机械臂4基坐标系和末端坐标系，{c}和{f}分别为相机坐标系和工具中心坐标系。

根据坐标系的变换关系，得到：

^pra^azc＝^prh^hzc

其中，^azc和^prh分别为相机z轴向量相对于主动机械臂3基坐标系的向量和被动机械臂4末端相对于其基坐标的姿态矩阵，它们在运动学参数标定中可获得；^pra和^hzc分别为主动机械臂基坐标系相对于被动机械臂基坐标系的姿态矩阵和相机z轴相对于被动机械臂4末端的向量，将其误差分别表示为δ^pra和δ^hzc。那么上式可写为：

进一步得到：此式为基坐标姿态变换矩阵的标定方程，通过求解可估计得到两机械臂基坐标的姿态矩阵误差δ^pra。

(4)建立两机械臂的基座位置变换误差模型，得到双臂基坐标位置变换的标定方程。

根据坐标系的变换关系：

^arp^prh^hpc,h+^arp^pph,p+^app,a＝^ape,a+^are^epf,e+^apc,f

其中，^are为主动机械臂3末端执行器相对于主动机械臂基座的姿态矩阵，可以通过机械臂运动学计算得到；^pph,p,^ape,a和^epf,e分别为被动机械臂4末端到其基座的位置向量，主动机械臂3末端到其基座的位置向量和标定板到主动机械臂3末端的位置向量，他们的真实值通过机械臂运动学参数标定均可计算得到；^apc,f为相机坐标系到标定板的位置矩阵，无法计算得到，下文中通过推导将其消掉；^app,a和^hpc,h分别为两机械臂基座的位置向量和相机到被动机械臂4末端的位置向量，他们的误差分别为δ^app,a和δ^hpc,h。那么上式可以表示为：

对于与第k条光轴对齐的位置i处的特征点，用上式可表示为：

对上式两边同乘[μk×]，可以得到：

将上式写为：其中，i为单位矩阵，

对于n条光轴上所有p个与光轴对齐的位置点有：jm[δ^app,a,δ^hpc,h]^t＝ρm，其中为基坐标位置误差雅可比矩阵，为位置误差矩阵。此式即为双机械臂基坐标位置变换矩阵的标定方程。

在一些实施方式中，基于视觉控制的特征点对齐及位置数据获取具体为：

(1)被动机械臂4末端位姿固定，标定板1始终在相机的视野范围内，利用基于图像的视觉控制方法控制主动机械臂3运动，视觉控制框图参见图4，包括位置控制内环和图像控制外环，图像控制外环实时监测当前特征点与光轴在图像中的位置差，并将其转化为主动机械臂3末端的位置差，机械臂位置控制内环根据位置差不断调整机械臂位姿，直到特征点与光轴对齐。当特征点与光轴对齐时，记录此时两个机械臂的关节角。

(2)参见图5，改变主动机械臂3末端位姿后重复步骤(1)，使特征点依次到达第k条光轴上n(n≥3)个不同的位置p^(1,k),p^(2,k),…,p^(n,k)。

(3)改变被动机械臂4末端位姿，即改变相机光轴的位置，重复步骤(1)–(2)。

(4)根据记录的特征点与光轴对齐时两机械臂的关节角，利用每个机械臂的正运动学计算每个位置点处机械臂末端与基座的相对名义位姿^eta、^pth。

(5)互换相机2和标定板1的位置，重复步骤(1)–(4)。

在一些实施方式中，标定方程求解具体为：

(1)根据运动学参数标定方程迭代求解每个机械臂的运动学参数误差，得到两个机械臂真实的运动学参数。

首先根据主动机械臂3的名义位姿估计相机光轴在主动机械臂基坐标系的向量值，假设第k条光轴上位置i处主动机械臂3的名义位置坐标为那么第k条光轴向量μk(xk,yk,zk)为：

其中，n为与第k条光轴对齐的特征点的所有位置数。

然后计算得到运动学标定方程中的对齐误差矩阵e和雅可比矩阵φ。

最后使用迭代lm(levenberg-marquardt)算法求解运动学标定方程，第t次迭代中，估计的运动学参数误差为：

其中λlm(t)为lm参数：

h为2～10之间的常数，ε(t)为迭代t次时运动学标定误差：

(2)根据双臂基坐标姿态变换矩阵标定方程：迭代估计基坐标姿态变换矩阵参数，包括以下步骤：

(2.1)初始化，δ^pra＝0。

(2.2)对于特征点的第i个位置，估计在第t次迭代的值：

(2.3)在特征点所有位置处估计进而估计真实的相机坐标系z轴在被动机械臂4末端坐标系下的向量

其中，q为与光轴对齐的特征点的所有位置数。

(2.4)利用重新估计δ^pra(t)：

δ^pra(t)＝s(v(t))，

其中，

n为光轴位置变化的次数。

(2.5)得到第t+1次迭代时主动机械臂3与被动机械臂4基座间的姿态变换矩阵并将其进行标准正交化。

(2.6)重复步骤(2.2)–(2.5)，直至δ^pra(t)收敛趋近于0。

(3)根据双臂基坐标位置变换标定方程jm[δ^app,a,δ^hpc,h]^t＝ρm，估计基坐标位置变换矩阵。

根据上文计算的所有参数的真实值，可以计算jm，因为jm是非满秩矩阵，将其写为：

jm＝vmσmum，

然后估计[δ^app,a,δ^hpc,h]^t：其中σm⁺为σm的伪逆矩阵。最后估计真实的基坐标位置变换矩阵^app,a：