CN102825602B - 一种基于psd的工业机器人自标定方法及装置 - Google Patents

Abstract

本发明公开了一种基于PSD的工业机器人自标定装置，其将两个PSD分别安装在V形夹具的两个板上，且V形夹具放置在机器人可达范围内，根据机器人在不同位置时，使固定在机器人末端的激光器发出的激光束的投射光斑和反射光斑分别定位在两个PSD的中心点位置，并通过两条虚拟约束线，进行机器人零位偏差自标定和空间位姿的自标定。本发明的自标定装置结构简单，安装与操作简单，定位精度高，可同时实现机器人空间位姿与零位偏差的自标定；本发明还提供一种基于PSD的工业机器人自标定方法。

Description

一种基于PSD的工业机器人自标定方法及装置

技术领域

本发明属于工业机器人的标定技术领域，尤其涉及一种基于PSD和激光虚拟约束线的6自由度工业机器人零位偏差及空间位姿自标定方法及装置。

技术背景

随着工业机器人在工业生产中应用范围以及所完成任务复杂程度的不断扩大，对工业机器人位姿精度的要求也越来越高。目前，大多数商品化的机器人重复精度很高，能达到0.1mm数量级，但其定位精度却很差，只能达到cm数量级，这样的定位精度远远不能满足现代工业发展的要求。导致工业机器人定位精度不高的原因有很多，例如机器人运动学模型的参数偏差、机器人杆件的弹性变形、齿轮传动误差、关节间隙以及环境因素等，对于不同结构、不同用途的机器人，以上各因素的影响也不尽相同。但一般来讲，机器人运动学模型中几何结构参数偏差所引起的定位误差占机器人总误差的80%左右。因此，对机器人运动学模型进行偏差标定补偿进而提高其定位精度是目前机器人技术领域内急需解决的问题。所谓标定就是应用先进的测量手段和基于模型的参数识别方法辨识出机器人模型的准确参数，从而提高机器人定位精度的过程。标定的结果是一组被识别的机器人参数，这些参数可以供机器人生产厂家作为产品质量检验指标，也可以帮助机器人用户提高机器人的定位精度。

近年来，为了提高工业机器人的定位精度，国内外学者相继提出了一些标定方法，这些方法大致可以分为以下三种：

第一种方法是利用一些高精度的测量设备来获取机器人末端精准的位置与姿态，例如三坐标测量机、电子经纬仪、球杆仪等。上述这些测量仪器在精度、成本及是否容易使用等方面有很大差异，但均具有需要专业的技术人员来操作这些仪器、数据采集费时费力、难以实现自动化、成本高、测量方法繁琐等缺点；

第二种方法是增加外部冗余传感器，通过传感器提供的信息来实现机器人运动学参数的自标定，例如利用安装于机器人末端的摄像头对机器人运动学参数进行了自标定，但是使用视觉传感器的问题在于摄像机本身需要标定，而且由于视野和分辨率之间的矛盾导致很难获得较高的测量精度。

第三种方法通过在机器人工作空间施加位置已知或形状已知的物理约束，利用机器人末端与这些约束接触，形成运动学闭合链并建立约束方程来求解运动学参数，例如中国发明专利号为CN101968341A所公开的一种工业机器人零位自标定方法及装置，其首先将一个PSD器件任意放置在机器人可达工作空间，然后仅依赖于通过机器人末端的一束激光以不同位置自动定位投射在该PSD的中心点即可实现机器人的标定任务，但采用此种方法只能进行零位偏差自标定，不能进行机器人空间位姿的自标定，并且进行零位偏差自标定时，需要机器人末端或激光器从至少7个不同的方位将光斑投射到PSD的中心，因此标定过程复杂。

由上述可见，研发适合机器人工作现场、便于携带并且成本低廉的机器人自标定方法及装置，已经成为机器人应用企业迫切需要解决的难题。

发明内容

本发明的目的就是为了解决上述现有技术存在的问题，提供一种基于PSD的工业机器人自标定装置，其结构简单，价格便宜，安装与操作简单，定位精度高，可同时实现机器人空间位姿与零位偏差的自标定；本发明还提供一种基于PSD的工业机器人自标定方法。

为实现上述目的，本发明的基于PSD的工业机器人自标定装置包括：机器人；安装在机器人末端的激光器；设置于机器人可达工作空间内的V形夹具；分别安装在V形夹具的两个板上的两个位置敏感器件PSD，即第一PSD和第二PSD，其中，机器人在不同位置时，激光器发出的激光束投射在第一PSD或第二PSD上，以便形成投射光斑和反射光束，且反射光束投射到第二PSD或第一PSD上，形成反射光斑；与第一PSD和第二PSD分别连接的机器人闭环控制单元，其将所述投射光斑和反射光斑分别定位在第一PSD或第二PSD的中心点位置，并确定对应的机器人末端的位置和机器人的6个关节角度值，以便通过点线关系，建立两条虚拟约束线的几何方程组，从而实现机器人零位偏差自标定和空间位姿自标定；其中，所述两条虚拟约束线为第一PSD和第二PSD中心点连线分别在第一PSD和第二PSD表面反射所形成的反射线。

其中，所述机器人闭环控制单元包括：与所述第一PSD和第二PSD分别连接的信号处理电路，其对两个PSD的输出信号进行处理，并反馈投射光斑和反射光斑在对应PSD上的二维坐标位置；通过数据采集卡与信号处理电路连接的工业控制计算机，其基于所述投射光斑和反射光斑在PSD上的二维坐标位置的反馈，发送指令给机器人控制器，使机器人控制器控制机器人带动激光器将所述投射光斑和反射光斑分别定位在对应PSD的中心点位置，同时传送对应位置时机器人的关节角度值。

特别是，所述数据采集卡通过无线通讯或机器人通讯接口方式与所述工业控制计算机连接。

优选的，所述激光器通过连接装置安装在机器人末端。

本发明还提供一种基于PSD的工业机器人自标定方法，该方法包括：

A）安装在机器人末端的激光器将发出的激光束投射在第一PSD或第二PSD上，形成投射光斑和反射光束，通过将反射光束投射到第二PSD或第一PSD上，形成反射光斑；

B）通过闭环控制，使所述投射光斑定位在第一PSD或第二PSD的中心点位置，使所述反射光斑定位在第二PSD或第一PSD的中心点位置，然后将此时机器人末端所处位置标记为第一位置P1；

C）根据机器人末端所处位置，确定机器人在当前位置的6个关节角度值；

D）改变机器人末端所处位置，按照步骤A)—步骤C）的处理方式，分别得到基于第一位置的第二位置P2、第三位置P3和第四位置P4，以及与第二位置P2、第三位置P3和第四位置P4分别对应的机器人6个关节角度值；

E）通过机器人末端所处的第一位置P1和第二位置P2及机器人末端所处的第三位置P3和第四位置P4，以及确定的与上述各位置所对应的机器人6个关节角度值，分别通过点线关系，建立由两个PSD的中心点连线分别在两个PSD表面反射所形成的两条虚拟约束线的几何方程组，从而实现机器人零位偏差自标定和空间位姿自标定。

特别是，所述B）步骤包括如下进行步骤：

B1）两个所述PSD的输出信号经所述信号处理电路处理后，给出所述投射光斑和反射光斑在两个所述PSD上的二维坐标位置，并将两个所述坐标位置通过数据采集卡反馈给工业控制计算机；

B2）工业控制计算机基于所述坐标位置的反馈，发送指令给机器人控制器，并控制机器人带动激光器将所述投射光斑和反射光斑分别定位在相应PSD的中心点位置，同时标记机器人末端所处位置。。

特别是，所述D）步骤包括如下进行步骤：

D1）机器人末端基于第一位置P1向上或向下移动一定距离，按照步骤A)—步骤C）的处理方式，将机器人末端所处位置标记为第二位置P2，确定机器人在对应位置处的6个关节角度值；

D2）机器人末端基于第二位置向右偏转一定角度，按照步骤A)—步骤C）的处理方式，将机器人末端所处位置标记为第三位置P3，确定机器人在对应位置处的6个关节角度值；

D3）机器人末端基于第三位置向上或向下移动一定距离，按照步骤A)—步骤C）的处理方式，将机器人末端所处位置标记为第四位置P4，确定机器人在对应位置处的6个关节角度值。

特别是，所述E）步骤包括如下步骤：

E1）通过机器人末端所处的第一位置和第二位置及机器人末端所处的第三位置和第四位置，及所确定的各对应位置处的机器人关节角度值，分别通过点线关系，建立由两个PSD的中心点连线分别在两个PSD表面反射所形成的两条虚拟约束线的几何方程组，并以虚拟约束线中的激光束虚拟线约束建立目标函数，经过迭代运算，进行机器人零位偏差的标定；

E2）根据上述步骤所求解出的机器人零位偏差，计算两条所述虚拟约束线分别在V形夹具上的工件空间坐标系{D}和机器人基座坐标系{B}中的单位向量，以便进行机器人空间位姿自标定。

进一步的，所述E1）步骤包括如下进行步骤：

E11）通过机器人末端所处的第一至第四位置，以及所确定的四个位置对应处机器人的6个关节角度值，基于机器人运动学误差模型，分别获得第一至第四位置上的机器人末端位置和姿态表达式；

E12）通过机器人末端所处的第一位置和第二位置，建立一组由两个PSD的中心点连线在一个PSD表面反射所形成的虚拟约束线的几何方程组，通过机器人末端所处的第三位置和第四位置，建立另一组由两个PSD的中心点连线在另一个PSD表面反射所形成的虚拟约束线的几何方程组；

E13）以两条虚拟约束线建立目标函数，通过迭代算法搜索机器人标定参数，完成机器人零位偏差的标定。

优选的，所述E2）步骤包括如下进行步骤：

E21）根据求解出的机器人零位偏差，计算工件空间坐标系{D}相对于机器人基座坐标系{B}的旋转变换矩阵R；

E22）计算工件空间坐标系{D}相对于机器人基座坐标系{B}的平移向量t；

E23）根据步骤E21）和步骤E22）所分别得到的旋转变换矩阵R和平移向量t，通过计算得到工件空间坐标系{D}相对于机器人基座坐标系{B}的齐次变换矩阵^BT_D，从而完成机器人空间位姿的自标定。

与现有技术相比，本发明的基于PSD的工业机器人自标定装置具有如下显著优点：

1）本发明的自标定装置只需要一个激光器和两个PSD器件及信号处理、采集电路，因此结构简单，价格便宜；

2）本发明的自标定装置中采用两个PSD建立机器人末端的虚拟约束线，因此标定过程中不需知道PSD与机器人之间的相对空间位姿及进行物理接触，因此测量精度高；

3）本发明的自标定装置通过PSD、信号处理电路及数据采集卡将激光光斑位姿反馈给机器人控制器，实现机器人的自动定位，机器人在不同位姿下均可自动完成定位任务，不需要人工干预，自动化程度高，安装与操作简单；

4）采用本发明的自标定装置进行标定时，只需要机器人完成4次自动定位即可完成机器人空间位姿与零位偏差的自标定，标定过程简单，耗时短；

5）本发明的自标定装置将机器人的空间位姿自标定过程和零位偏差自标定过程集成与一体，因此使用更加方便。

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

附图说明

图1是本发明的工业机器人自标定装置结构示意图。

附图标记说明：1-机器人；2-机器人末端；3-连接装置；4-激光器；5、6-反射线；7-第一PSD；8-第二PSD；9-V形夹具；10-信号处理电路；11-数据采集卡；12-工业控制计算机；13-机器人控制器。

具体实施方式

如图1所示，为本发明的基于PSD的工业机器人自标定装置的结构示意图，其包括：机器人1；安装在机器人末端2的激光器4；设置于机器人1可达工作空间内的V形夹具9；分别安装在V形夹具9的两个板上的两个位置敏感器件PSD，即第一PSD7和第二PSD8，其中，机器人在不同位置时，激光器发出的激光束投射在第一PSD或第二PSD上，以便形成投射光斑和反射光束，且反射光束投射到第二PSD或第一PSD上，形成反射光斑；与第一PSD7和第二PSD8分别连接的机器人闭环控制单元，其将投射光斑和反射光斑分别定位在第一PSD7或第二PSD8的中心点位置，并确定对应的机器人末端2的位置和机器人的关节角度值，以便通过点线关系，建立两条虚拟约束线的几何方程组，从而实现机器人零位偏差自标定和空间位姿自标定；其中，两条虚拟约束线为第一PSD和第二PSD中心点连线分别在第一PSD和第二PSD表面反射所形成的反射线5、6，并且这两条反射线5、6与激光器所发出的分别投射在第一PSD或第二PSD中心点位置上的激光束重合。

其中，机器人闭环控制单元包括：与第一PSD7和第二PSD8分别连接的信号处理电路10，其对两个PSD的输出信号进行处理，并反馈投射光斑和反射光斑在对应PSD上的二维坐标位置；通过数据采集卡11与信号处理电路10连接的工业控制计算机12，其基于投射光斑和反射光斑在PSD上的二维坐标位置的反馈，发送指令给机器人控制器13，使机器人控制器13控制机器人1带动激光器4将投射光斑和反射光斑分别定位在对应PSD的中心点位置，同时传送对应位置时机器人1的关节角度值。

本发明中，数据采集卡通过无线通讯或机器人通讯接口方式与工业控制计算机连接，并且激光器采用可调焦距的精密半导体激光器，其功率为1mW，波长为670nm，光斑直径为2.5mm，将激光器通过连接装置3安装在机器人末端2后，相对关系确定且保证安装精度，可以实现激光器与机器人末端的坐标系{E}的X轴方向一致。

本发明中，第一PSD7和第二PSD8选用分段式高精度光电元件，分辨率达0.1μm，有效表面直径为10mm，其输出信号经信号处理电路11可给出激光光斑在两个PSD表面的二维位置坐标，从而作为反馈信号精确控制机器人的姿态。

本发明还提供了一种基于PSD的工业机器人自标定方法，其标定过程如下：

A）安装在机器人末端的激光器将发出的激光束投射在第一PSD或第二PSD上，形成投射光斑和反射光束，通过将反射光束投射到第二PSD或第一PSD上，形成反射光斑；

B）通过闭环控制，使所述投射光斑定位在第一PSD或第二PSD的中心点位置，使所述反射光斑定位在第二PSD或第一PSD的中心点位置，然后将此时机器人末端所处位置标记为第一位置P1；

C）根据机器人末端所处位置，确定机器人在当前位置的6个关节角度值；

D）改变机器人末端所处位置，按照步骤A)—步骤C）的处理方式，分别得到基于第一位置的第二位置P2、第三位置P3和第四位置P4，以及与第二位置P2、第三位置P3和第四位置P4分别对应的机器人6个关节角度值；

E）通过机器人末端所处的第一位置P1和第二位置P2及机器人末端所处的第三位置P3和第四位置P4，以及确定的与上述各位置所对应的机器人6个关节角度值，分别通过点线关系，建立由两个PSD的中心点连线分别在两个PSD表面反射所形成的两条虚拟约束线的几何方程组，从而实现机器人零位偏差自标定和空间位姿自标定。

具体的，以ABB的IRB120机器人为例，对本发明的自标定装置进行空间位姿和零位偏差的自标定方法进行详细说明：

将安装在机器人末端的激光器所发出的激光束投射在第一PSD表面上，形成投射光斑和反射光束，当该反射光束投射到第二PSD表面上时，形成反射光斑。

两个PSD的输出信号经机器人闭环控制单元中的信号处理电路处理后，给出投射光斑和反射光斑在两个PSD上的二维坐标位置，并将两个坐标位置通过数据采集卡反馈给工业控制计算机；而工业控制计算机基于上述两个坐标位置的反馈，发送指令给机器人控制器，并控制机器人带动激光器将投射光斑和反射光斑分别定位在第一PSD和第二PSD的中心点位置，同时将机器人末端所处位置标记为第一位置。

精确定位后，工业控制计算机通过无线通讯或机器人通讯接口从机器人控制器读取机器人在第一位置处的6个关节角度值，并且每个关节的角度值可读取两次以上，再取其平均值作为第一位置的机器人的6个关节角度值，从而确定了机器人在当前位置的6个关节角度值。

机器人末端载着激光器基于第一位置沿着反射线5向上或向下移动一定距离，重复上述第一、第二和第三步骤，使投射光斑和反射光斑分别定位在第一PSD和第二PSD的中心点位置，同时将机器人末端所处位置标记为第二位置，从机器人控制器中读取当前位置所对应的机器人6个关节角度值；然后，机器人末端载着激光器基于第二位置向右偏转一定角度至反射线6上的某处，按照上述三个步骤的处理方式，使激光器所发出的激光束投射在第二PSD表面上，以形成投射光斑和反射光束，当反射光束投射到第一PSD表面上时，形成反射光斑；再通过闭环控制，使投射光斑和反射光斑分别定位在第二PSD和第一PSD的中心点位置，同时将机器人末端所处位置标记为第三位置，从机器人控制器中读取当前位置所对应的机器人6个关节角度值；最后，机器人末端载着激光器基于第三位置沿反射线6向上或向下移动一定距离，按照上述三个步骤的处理方式，使激光器所发出的激光束投射在第二PSD表面上，以形成投射光斑和反射光束，当反射光束投射到第一PSD表面上时，形成反射光斑；再通过闭环控制，使投射光斑和反射光斑分别定位在第二PSD和第一PSD的中心点位置，将机器人末端所处位置标记为第四位置，并从机器人控制器中读取当前位置所对应的机器人6个关节角度值。

通过机器人末端所处的第一至第四位置，以及所确定的四个位置对应处机器人的6个关节角度值，并考虑到机器人存在的关节零位偏差δ_i，基于机器人运动学误差模型，获得第一至第四位置上的机器人末端位置和姿态表达式。

依据Denavit-Hartenberg（D-H）模型建立的机器人运动学误差模型如下：

(i＝1,2,…,6)

其中，ai、αi、di、θi分别表示杆件长度、杆件扭角、杆件距离和关节转角。cαi和sαi分别表示cosαi和sinαi。和分别表示sin(θ_i+δ_i)和cos(θ_i+δ_i)，δ_i表示第i个关节的零位偏差。同时该6自由度工业机器人的正运动学方程亦可表达为：

定义机器人在第j(j=1,2,3,4)个位置下，机器人末端2在机器人基座坐标系{B}中的坐标为P_j，则有：

$P_j=\left[\begin{array}{c}{P}_{\mathrm{xj}}\\ P_{\mathrm{yj}}\\ P_{\mathrm{zj}}\\ 1\end{array}\right]={\left(T_E^B\right)}_j\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]={\left[\begin{array}{c}{T}_{14}\\ T_{24}\\ T_{34}\\ 1\end{array}\right]}_j$ (j=1,2,3,4)

基于两点确定一条直线的原理，通过机器人末端所处的第一位置和第二位置，建立一组由两个PSD的中心点连线在第一PSD表面反射所形成的虚拟约束线（即反射线5）的几何方程组，通过机器人末端所处的第三位置和第四位置，建立另一组由两个PSD的中心点连线在第二PSD表面反射所形成的虚拟约束线（即反射线6）的几何方程组，这两组方程组如下：

$\left\{\begin{array}{c}{m}_1=m_2\\ n_1=n_2\\ p_1=p_2\\ \frac{P_{x1}-P_{x2}}{m_1}=\frac{P_{y1}-P_{y2}}{n_1}=\frac{P_{z1}-P_{z2}}{p_1}\end{array}\right.$ 和 $\left\{\begin{array}{c}{m}_3=m_4\\ n_3=n_4\\ p_3=p_4\\ \frac{P_{x3}-P_{x4}}{m_3}=\frac{P_{y3}-P_{y4}}{n_3}=\frac{P_{z3}-P_{z4}}{p_3}\end{array}\right.$

当基于两个PSD位置反馈的机器人定位误差忽略时，如果不存在机器人杆件参数误差和零点位置偏差，则机器人的四个位置分别位于两条反射线5、6上，即位于连接机器人末端和两个PSD表面中心点的两条直线上。但实际上，由于机器人杆件参数误差的存在，机器人的四个位置在反射线5和反射线6附近某个区域内分布，此时，需通过迭代算法搜索机器人杆件误差参数，使机器人的四个位置无限收敛于两条反射线，即实际的激光束5和激光束6。以两条虚拟约束线建立目标函数，并通过最小化以下目标函数来优化求解机器人杆件参数误差和机器人零点位置偏差，完成机器人零位偏差的标定：

$\mathrm{\Ψ}=\arg \min \underset{k=1}{\overset{2}{\mathrm{\Σ}}}\underset{m=1}{\overset{4}{\mathrm{\Σ}}}{\mathrm{\Ψ}}_{\mathrm{km}}^2=\arg \min \underset{k=1}{\overset{2}{\mathrm{\Σ}}}({\mathrm{\Ψ}}_{k1}^2+{\mathrm{\Ψ}}_{k2}^2+{\mathrm{\Ψ}}_{k3}^2+{\mathrm{\Ψ}}_{k4}^2)$

其中

$\left\{\begin{array}{c}{\mathrm{\Ψ}}_{k1}={\left(T_{11}\right)}_{2k}-{\left(T_{11}\right)}_{2k-1}\\ {\mathrm{\Ψ}}_{k2}={\left(T_{21}\right)}_{2k}-{\left(T_{21}\right)}_{2k-1}\\ {\mathrm{\Ψ}}_{k3}=\frac{{\left(T_{11}\right)}_{2k-1}}{{\left(T_{21}\right)}_{2k-1}}-\frac{{\left(T_{14}\right)}_{2k}-{\left(T_{14}\right)}_{2k-1}}{{\left(T_{24}\right)}_{2k}-{\left(T_{24}\right)}_{2k-1}}\\ {\mathrm{\Ψ}}_{k4}=\frac{{\left(T_{21}\right)}_{2k-1}}{{\left(T_{31}\right)}_{2k-1}}-\frac{{\left(T_{24}\right)}_{2k}-{\left(T_{24}\right)}_{2k-1}}{{\left(T_{34}\right)}_{2k}-{\left(T_{34}\right)}_{2k-1}}\end{array}\right.$

在迭代优化求解得到机器人关节零位偏差δ_i(i=2,3,…,6)后，完成了机器人零位偏差的自标定。

根据求解出的机器人零位偏差，将其代入机器人运动学误差模型并修改机器人控制器零位参数，得到机器人四个位置上分别对应的正运动学齐次变换矩阵^BT_E，并通过机器人末端的第一位置、第二位置（或第三位置、第四位置）相对于机器人基座坐标系{B}的空间坐标，或机器人末端的第一位置（或第三位置）相对于机器人基座坐标系{B}的空间姿态，求出激光束（即反射线）相对于机器人基座坐标系{B}的单位方向向量，可以通过第一PSD7和第二PSD8在工件空间坐标系{D}中的已知位置，得到激光束相对于工件空间坐标系{D}的单位方向向量。

激光束在机器人基座坐标系{B}中的单位向量v_j为：

$v_j=\left[\begin{array}{c}{m}_j\\ n_j\\ p_j\\ 0\end{array}\right]=={\left(T_E^B\right)}_j\left[\begin{array}{c}1\\ 0\\ 0\\ 0\end{array}\right]={\left[\begin{array}{c}{T}_{11}\\ T_{21}\\ T_{31}\\ 0\end{array}\right]}_j$ (j=1,2,3,4)

由于2条虚拟约束线在V形夹具9上的工件空间坐标系{D}中的单位向量和在机器人基座坐标系{B}中的单位向量通过同一个旋转变换矩阵R完成转换，所以采用四元数法可以求解得出该旋转变换矩阵R。

根据四元数法（the Quaternion-based Algorithm），有：

L_B=RL_D=qL_Dq^*

其中，以L_D和L_B分别表示激光束相对于工件空间坐标系{D}和机器人基座坐标系{B}的单位方向向量，为旋转矩阵R的四元数形式，表示一个角度为θ的转动，转轴为虚部的方向而为一个三维单位向量，即q₀和分别为单位四元数q的实部和虚部；q^*表示单位四元数q的共轭四元数。

因此可以将上式改写为：

$L_D-L_B=\tan (\mathrm{\θ}/2)(L_D+L_B)\×\stackrel{\→}{e}$

定义一斜对称矩阵Ω(v)，即

$\mathrm{\Ω}\left(v\right)\≡\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right]$

并且令L_D+L_B=a，L_D-L_B=b，由于整个标定过程形成了两条虚拟约束线，所以存在以下关系：

$\mathrm{\Ω}\left(a_k\right)\stackrel{\→}{g}=b_k$ k=1,2

定义 $C=\left[\begin{array}{c}\mathrm{\Ω}\left(a_1\right)\\ \mathrm{\Ω}\left(a_2\right)\end{array}\right]$ 和 $D=\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right],$ 则上式可以改写为：

其中可以通过最小二乘法计算得出，而和θ可以通过以下两个公式求出：

$\stackrel{\→}{e}=\stackrel{\→}{g}/\left|\right|\stackrel{\→}{g}\left|\right|,$ $\mathrm{\θ}=2\·a\tan ({\left(\stackrel{\→}{g}\right)}_{\max }/{\left(\stackrel{\→}{e}\right)}_{\max })$

因此，齐次变换矩阵^BT_D中的旋转变换矩阵R可以通过下式求得：

$R=\left[\begin{array}{ccc}{e}_x^2(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& e_x{e}_y(1-\cos \mathrm{\θ})-e_z\sin \mathrm{\θ}& e_z{e}_x(1-\cos \mathrm{\θ})+e_y\sin \mathrm{\θ}\\ e_x{e}_y(1-\cos \mathrm{\θ})+e_z\sin \mathrm{\θ}& e_y^2(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& e_y{e}_z(1-\cos \mathrm{\θ})-e_x\sin \mathrm{\θ}\\ e_z{e}_x(1-\cos \mathrm{\θ})-e_y\sin \mathrm{\θ}& e_y{e}_z(1-\cos \mathrm{\θ})+e_x\sin \mathrm{\θ}& e_z^2(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}\end{array}\right]$

在求出旋转矩阵R之后，对每一条虚拟约束线建立虚拟线约束几何方程，对该几何方程进行求解可以得到工件空间坐标系{D}相对于机器人基座坐标系{B}的平移向量t。

对于每一条虚拟线约束建立以下关系：

$\left[\begin{array}{c}{x}_{1\mathrm{Bk}}\\ y_{1\mathrm{Bk}}\\ z_{1\mathrm{Bk}}\end{array}\right]=\left[\begin{array}{c}{x}_{0\mathrm{Bk}}\\ y_{0\mathrm{Bk}}\\ z_{0\mathrm{Bk}}\end{array}\right]+{\mathrm{\λ}}_k\left[\begin{array}{c}{M}_{\mathrm{Bk}}\\ N_{\mathrm{Bk}}\\ P_{\mathrm{Bk}}\end{array}\right]=R\left[\begin{array}{c}{x}_{0\mathrm{Dk}}\\ y_{0\mathrm{Dk}}\\ z_{0\mathrm{Dk}}\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]+{\mathrm{\λ}}_k\left[\begin{array}{c}{M}_{\mathrm{Bk}}\\ N_{\mathrm{Bk}}\\ P_{\mathrm{Bk}}\end{array}\right]$ k=1,2

其中，[x_0Dk y_0Dk z_0Dk]^T表示PSD的中心点在工件空间坐标系{D}中的位置坐标；[x_0Bk y_0Bk z_0Bk]^T表示该中心点在机器人基座坐标系{B}中的位置坐标；[x_1Bk y_1Bk z_1Bk]^T表示激光器（或机器人末端）第一位置或第二位置k=1时）及第三位置或第四位置（k=2时）在机器人基座坐标系{B}中的位置坐标，该坐标通过(^BT_E)_j(j=1,2,3,4)分别求出。

令[a_xk a_yk a_zk]^T=R[x_0Dk y_0Dk z_0Dk]^T，则有：

$\frac{x_{1\mathrm{Bk}}-a_{\mathrm{xk}}-t_x}{M_{\mathrm{Bk}}}=\frac{y_{1\mathrm{Bk}}-a_{\mathrm{yk}}-t_y}{N_{\mathrm{Bk}}}=\frac{z_{1\mathrm{Bk}}-a_{\mathrm{zk}}-t_z}{P_{\mathrm{Bk}}}$ k=1,2

通过该式，可以求得[t_x t_y t_z]^T：

$\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]=\left[\begin{array}{c}\frac{(z_{1B2}-z_{1B1})-(a_{z2}-a_{z1})-\frac{P_{B2}}{M_{B2}}(x_{1B2}-a_{x2})+\frac{P_{B1}}{M_{B1}}(x_{1B1}-a_{x1})}{\frac{P_{B1}}{M_{B1}}-\frac{P_{B2}}{M_{B2}}}\\ \frac{(x_{1B2}-x_{1B1})-(a_{x2}-a_{x1})-\frac{M_{B2}}{N_{B2}}(y_{1B2}-a_{y2})+\frac{M_{B1}}{N_{B1}}(y_{1B1}-a_{y1})}{\frac{M_{B1}}{N_{B1}}-\frac{M_{B2}}{N_{B2}}}\\ \frac{(y_{1B2}-y_{1B1})-(a_{y2}-a_{y1})-\frac{N_{B2}}{P_{B2}}(z_{1B2}-a_{z2})+\frac{N_{B1}}{P_{B1}}(z_{1B1}-a_{z1})}{\frac{N_{B1}}{P_{B1}}-\frac{N_{B2}}{P_{B2}}}\end{array}\right]$

由此得到工件空间坐标系{D}相对于机器人基座坐标系{B}的平移向量t。

根据所求出的旋转变换矩阵R和平移向量t，得到工件空间坐标系{D}相对于机器人基座坐标系{B}的齐次变换矩阵^BT_D：

$T_D^B=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$

从而完成机器人空间位姿的自标定。其中，^BT_D表示V形夹具上的工件空间坐标系{D}相对于机器人基座坐标系{B}的齐次变换矩阵。

通过上述方法可得到在ABB IRB120机器人上完成的标定实验结果，该结果与三坐标测量机标定结果进行对比，如下表所示：

表1 IRB120工业机器人零位偏差标定结果（单位：度）

从实验结果可以看出，采用本发明的标定方法得到的标定结果与采用三坐标测量机标定的结果基本相等，误差小于0.2°，根据实验结果可知，本发明的标定方法的有效性和标定结果的准确性。

尽管上文对本发明作了详细说明，但本发明不限于此，本技术领域的技术人员可以根据本发明的原理进行修改，因此，凡按照本发明的原理进行的各种修改都应当理解为落入本发明的保护范围。

Claims (10)

1.一种基于PSD的工业机器人自标定装置，其特征在于，包括：

机器人(1)；

安装在机器人末端(2)的激光器(4)；

设置于机器人(1)可达工作空间内的V形夹具(9)；

分别安装在V形夹具(9)的两个板上的两个位置敏感器件PSD，即第一PSD(7)和第二PSD(8)，其中，机器人在不同位置时，激光器发出的激光束投射在第一PSD或第二PSD上，以便形成投射光斑和反射光束，且反射光束投射到第二PSD或第一PSD上，形成反射光斑；

与第一PSD(7)和第二PSD(8)分别连接的机器人闭环控制单元，其将所述投射光斑和反射光斑分别定位在第一PSD或第二PSD的中心点位置，并确定对应的机器人末端的位置和机器人的6个关节角度值，以便通过点线关系，建立两条虚拟约束线的几何方程组，从而实现机器人零位偏差自标定和空间位姿自标定；

其中，所述的确定对应的机器人末端的位置和机器人的6个关节角度值包括：

将所述反射光斑定位在第二PSD或第一PSD的中心点位置时机器人末端所在位置标记为第一位置P1；

根据机器人末端所处的第一位置P1，确定机器人在第一位置P1的6个关节角度值；

将此后移动机器人并使所述反射光斑定位在第二PSD或第一PSD的中心点位置时机器人末端所在的三个不同位置分别标记为基于第一位置P1的第二位置P2、第三位置P3和第四位置P4；

根据机器人末端所处的基于第一位置的第二位置P2、第三位置P3和第四位置P4，分别确定与第二位置P2、第三位置P3和第四位置P4分别对应的机器人6个关节角度值；

其中，所述的通过点线关系建立两条虚拟约束线的几何方程组包括：

通过机器人末端所处的第一位置P1和第二位置P2及机器人末端所处的第三位置P3和第四位置P4，以及确定的与上述各位置所对应的机器人6个关节角度值，分别通过点线关系，建立由两个PSD的中心点连线分别在两个PSD表面反射所形成的两条虚拟约束线的几何方程组。

2.根据权利要求1所述的基于PSD的工业机器人自标定装置，其特征在于，所述机器人闭环控制单元包括：

与所述第一PSD(7)和第二PSD(8)分别连接的信号处理电路(10)，其对两个PSD的输出信号进行处理，并反馈投射光斑和反射光斑在对应PSD上的二维坐标位置；

通过数据采集卡(11)与信号处理电路(10)连接的工业控制计算机(12)，其基于所述投射光斑和反射光斑在PSD上的二维坐标位置的反馈，发送指令给机器人控制器(13)，使机器人控制器控制机器人带动激光器(4)将所述投射光斑和反射光斑分别定位在对应PSD的中心点位置，同时传送对应位置时机器人的关节角度值。

3.根据权利要求2所述的基于PSD的工业机器人自标定装置，其特征在于，所述数据采集卡(11)通过无线通讯方式与所述工业控制计算机(12)连接。

4.根据权利要求3所述的基于PSD的工业机器人自标定装置，其特征在于，所述激光器(4)通过连接装置(3)安装在机器人末端(2)。

5.一种使用工业机器人自标定装置的自标定方法，其中，所述工业机器人自标定装置包括：机器人(1)；安装在机器人末端(2)的激光器(4)；设置于机器人(1)可达工作空间内的V形夹具(9)；分别安装在V形夹具(9)的两个板上的两个位置敏感器件PSD，即第一PSD(7)和第二PSD(8)；

所述方法包括：

A)安装在机器人末端的激光器将发出的激光束投射在第一PSD或第二PSD上，形成投射光斑和反射光束，通过将反射光束投射到第二PSD或第一PSD上，形成反射光斑；

B)通过闭环控制，使所述投射光斑定位在第一PSD或第二PSD的中心点位置，使所述反射光斑定位在第二PSD或第一PSD的中心点位置，然后将此时机器人末端所处位置标记为第一位置P1；

C)根据机器人末端所处位置，确定机器人在当前位置的6个关节角度值；

D)改变机器人末端所处位置，按照步骤A)—步骤C)的处理方式，分别得到基于第一位置的第二位置P2、第三位置P3和第四位置P4，以及与第二位置P2、第三位置P3和第四位置P4分别对应的机器人6个关节角度值；

E)通过机器人末端所处的第一位置P1和第二位置P2及机器人末端所处的第三位置P3和第四位置P4，以及确定的与上述各位置所对应的机器人6个关节角度值，分别通过点线关系，建立由两个PSD的中心点连线分别在两个PSD表面反射所形成的两条虚拟约束线的几何方程组，从而实现机器人零位偏差自标定和空间位姿自标定。

6.根据权利要求5所述的使用工业机器人自标定装置的自标定方法，其特征在于，所述B)步骤包括如下进行步骤：

B1)两个所述PSD的输出信号经所述信号处理电路处理后，给出所述投射光斑和反射光斑在两个所述PSD上的二维坐标位置，并将两个所述坐标位置通过数据采集卡反馈给工业控制计算机；

B2)工业控制计算机基于所述坐标位置的反馈，发送指令给机器人控制器，并控制机器人带动激光器将所述投射光斑和反射光斑分别定位在相应PSD的中心点位置，同时标记机器人末端所处位置。

7.根据权利要求5或6所述的使用工业机器人自标定装置的自标定方法，其特征在于，所述D)步骤包括如下进行步骤：

D1)机器人末端基于第一位置P1向上或向下移动一定距离，按照步骤A)—步骤C)的处理方式，将机器人末端所处位置标记为第二位置P2，确定机器人在对应位置处的6个关节角度值；

D2)机器人末端基于第二位置向右偏转一定角度，按照步骤A)—步骤C)的处理方式，将机器人末端所处位置标记为第三位置P3，确定机器人在对应位置处的6个关节角度值；

D3)机器人末端基于第三位置向上或向下移动一定距离，按照步骤A)—步骤C)的处理方式，将机器人末端所处位置标记为第四位置P4，确定机器人在对应位置处的6个关节角度值。

8.根据权利要求7所述的使用工业机器人自标定装置的自标定方法，其特征在于，所述E)步骤包括如下步骤：

E1)通过机器人末端所处的第一位置和第二位置及机器人末端所处的第三位置和第四位置，及所确定的各对应位置处的机器人关节角度值，分别通过点线关系，建立由两个PSD的中心点连线分别在两个PSD表面反射所形成的两条虚拟约束线的几何方程组，并以虚拟约束线中的激光束虚拟线约束建立目标函数，经过迭代运算，进行机器人零位偏差的标定；

E2)根据上述步骤所求解出的机器人零位偏差，计算两条所述虚拟约束线分别在V形夹具上的工件空间坐标系{D}和机器人基座坐标系{B}中的单位向量，以便进行机器人空间位姿自标定。

9.根据权利要求8所述的使用工业机器人自标定装置的自标定方法，其特征在于，所述E1)步骤包括如下进行步骤：

E11)通过机器人末端所处的第一至第四位置，以及所确定的四个位置对应处机器人的6个关节角度值，基于机器人运动学误差模型，分别获得第一至第四位置上的机器人末端位置和姿态表达式；

E12)通过机器人末端所处的第一位置和第二位置，建立一组由两个PSD的中心点连线在一个PSD表面反射所形成的虚拟约束线的几何方程组，通过机器人末端所处的第三位置和第四位置，建立另一组由两个PSD的中心点连线在另一个PSD表面反射所形成的虚拟约束线的几何方程组；

E13)以两条虚拟约束线建立目标函数，通过迭代算法搜索机器人标定参数，完成机器人零位偏差的标定。

10.根据权利要求8所述的使用工业机器人自标定装置的自标定方法，其特征在于，所述E2)步骤包括如下进行步骤：

E21)根据求解出的机器人零位偏差，计算工件空间坐标系{D}相对于机器人基座坐标系{B}的旋转变换矩阵R；

E22)计算工件空间坐标系{D}相对于机器人基座坐标系{B}的平移向量t；

E23)根据步骤E21)和步骤E22)所分别得到的旋转变换矩阵R和平移向量t，通过计算得到工件空间坐标系{D}相对于机器人基座坐标系{B}的齐次变换矩阵^BT_D，从而完成机器人空间位姿的自标定。