CN111791231A - 一种机器人标定系统及二维平面、三维空间运动标定方法 - Google Patents

Abstract

本发明涉及一种机器人标定系统及二维平面、三维空间运动标定方法，系统包括工业机器人和上位机，还包括：靶标，固装在所述工业机器人的末端；测量组件，设于所述工业机器人的周围，包括棱镜组件和相机，所述棱镜组件设置在靶标和相机之间，且相机的视轴和棱镜组件的光轴保持同轴；标定组件，设置在所述测量组件的视场内，且不与工业机器人干涉；上位机与相机和棱镜组件连接。与现有技术相比，本发明可由上位机控制棱镜组件并调整与其配套相机的视轴，实现对靶标中心位置的实时动态测量，通过构建机器人当前几何参数名义坐标与测量坐标的误差响应方程，寻求误差最小值时所对应的机器人当前几何参数，可实现机器人的在线标定。

Description

一种机器人标定系统及二维平面、三维空间运动标定方法

技术领域

本发明涉及机器人技术领域，尤其是涉及一种机器人标定系统及二维平面、三维空间运动标定方法。

背景技术

随着新一轮科技革命的提出，智能化工厂、智能化车间以及智能化生产线的不断发展，工业机器人具备灵活性、重复性以及负载能力强等特点已经广泛应用在焊接、汽车装配、宇航制造和搬运码垛等领域，而现阶段工业机器人的作业精度较难满足越来越高的加工制造精度要求。工业机器人误差主要包括杆件误差、关节转角误差等运动学误差以及环境温度变化和负载变化等非几何误差，其中运动学误差占总误差的90％，因此提高工业机器人精度的主要任务是消除工业机器人的运动学误差。

机器人精度主要包括重复定位精度和绝对定位精度，工业机器人重复精度可达亚毫米量级，而绝对定位精度仅在毫米乃至厘米量级，提高机器人绝对定位精度水平具有重要意义。工业机器人标定技术可识别误差与误差参数关系，给予最合理的误差补偿策略，以最大程度的提高工业机器人作业精度。

在先技术(申请号：CN 201910167416.5，申请日：2019年3月6日“一种工业机器人运动学标定系统及标定方法”)提出一种工业机器人运动学标定系统，该系统采用在工业机器人安装测量装置实时获得标定球球心坐标，并通过标定装置和数据处理装置实现工业机器人在线标定，但该系统将测量装置安装在机器人末端将影响工业机器人的作业，且系统标定空间较大，不利于应用在实际生产中。

在先技术(申请号：CN 201610460753.X，申请日：2016年6月21日“一种基于直线位移传感器的工业机器人运动学参数的标定方法”)提出一种基于直线位移传感器的工业机器人标定方法，通过直线位移传感器测量机器人末端与传感器的距离，构建机器人参数与传感器参数的标定方程，实现工业机器人标定，该方法标定复杂效率较低，且较难实现工业机器人的在线标定。

在先技术(申请号：CN 201810011480.X，申请日：2018年1月5日“工业机器人的标定方法和系统”)提出一种工业机器人的标定方法和系统，依次将工业机器人移动至标定板上的多个孔位，并在工业机器人移动到每个孔位时获取相应孔位的位置信息，构建工业机器人杆长与孔距间的多项式，实现工业机器人的标定。由于需将工业机器人移动至精确的孔位置，将大大降低标定效率，孔位间的加工误差也将影响标定精度，且该方法较难实现工业机器人的在线标定。

在先技术(申请号：CN 201810718695.5，申请日：2018年7月3日“一种基于激光跟踪仪的机器人标定方法”)提出一种基于激光跟踪仪的机器人标定方法，控制机器人的所有轴依次进行单轴转动，并通过激光跟踪仪采集转动轴转动至某一角度时的靶球球心坐标点组，计算出机器人的DH参数对机器人进行标定，该方法需结合激光跟踪仪进行标定，采用相对测距原理，需合作靶镜，系统复杂，成本高，不利于大范围工业推广。

在先技术(申请号：US6408252B1，申请日：2002年6月18日“Calibration Systemand Displacement Measurement Device”)提出一种标定系统和位移测量装置。该系统采用一个拉线测量装置测量机器人末端与测量装置的距离，并根据机器人此时自身运动参数和结构参数可获得机器人末端和测量装置的距离，通过比较机器人不同位姿下二者距离的差值，实现对机器人的标定，该方法仅能实现机器人离线标定，较难实现更为实用的机器人在线标定。

在先技术(申请号：EP08001833.6，申请日：2008年1月31日“Calibration Deviceand Method for Robot Mechanism”)提出一种用于机器人标定的装置和方法，在工业机器人上建立第一坐标系，在工业机器人外建立第二坐标系，构建两坐标系间的关系，工业机器人移动到不同的预备位置，通过标定模块调整工业机器人参数，使当前预备位置坐标满足两坐标系间的关系，最终实现工业机器人的标定，该方法较难实现工业机器人的在线标定，与此同时测量模块安装在工业机器人本体上，将给工业机器人的作业带来不便。

发明内容

本发明的目的就是为了克服上述现有技术存在的不能对机器人在线实时标定的缺陷而提供一种机器人标定系统及二维平面、三维空间运动标定方法。

本发明的目的可以通过以下技术方案来实现：

一种机器人标定系统，包括工业机器人和上位机，还包括：

靶标，固装在所述工业机器人的末端；

测量组件，设于所述工业机器人的周围，包括棱镜组件和相机，所述棱镜组件设置在靶标和相机之间，且相机的视轴和棱镜组件的光轴保持同轴；

标定组件，设置在所述测量组件的视场内，且不与工业机器人干涉；

所述上位机与相机和棱镜组件连接。

优选的，所述棱镜组件包括转镜组件、编码器组件和驱动组件，所述转镜组件分别与驱动组件和编码器组件连接。

优选的，所述转镜组件包括安装座和设置在安装座上的齿轮二、套筒、镜套、棱镜和轴承，所述棱镜置于镜套内部，所述镜套固连在套筒中，所述套筒通过轴承固装在安装座上并与齿轮二连接。

优选的，所述编码器组件包括设置在安装座上的胀套二、齿轮三和编码器，所述编码器的旋转轴通过胀套二与齿轮三连接，所述齿轮三与齿轮二啮合。

优选的，光线通过所述转镜组件所形成的光学扫描域不小于相机的相机成像视场域，所述相机与转镜组件共同形成的组合成像视场域不小于相机成像视场域。

一种采用机器人标定系统的二维平面运动机器人标定方法，包括：

A1、建立世界坐标系、机器人坐标系、测量组件坐标系、棱镜组件坐标系以及与棱镜组件相匹配的相机坐标系；

A2、上位机控制棱镜组件，使相机的视轴经过棱镜组件无偏转出射，标定相机的内参数和外参数，保持相机不动；

A3、上位机通过视觉跟踪算法调整棱镜组件与其配套的相机视轴，实现测量组件对靶标中心的实时动态测量，由坐标转换关系获得靶标中心在世界坐标系下的测量坐标；由工业机器人自身系统实时获得靶标中心在机器人坐标系下的名义坐标，再由坐标转换关系获得靶标中心在世界坐标系下的名义坐标；

A4、设置标定误差阈值ΔB_f，构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角；

A5、改变工业机器人的姿态，重复A3实时得到靶标中心在世界坐标系下的名义坐标和测量坐标，计算得到靶标中心在世界坐标系下的名义坐标与测量坐标的误差ΔB_c；

A6、比较误差ΔB_c与误差阈值ΔB_f，若ΔB_c小于等于ΔB_f，则输出工业机器人当前DH模型参数a、α、d和θ，结束工业机器人的标定，否则修正工业机器人当前DH模型参数a、α、d和θ，重复A5～A6，直到ΔB_c小于等于ΔB_f。

优选的，所述A3中靶标中心在世界坐标系下的测量坐标的获得过程具体包括：

1)上位机调整与棱镜组件配套相机的视轴，使靶标落入相机的视场范围内，相机采集靶标的图像，并将图像传输至上位机；

2)上位机通过视觉跟踪算法调整与棱镜组件配套相机的视轴，使靶标的中心像点处于相机的视场中心；

3)上位机根据编码器反馈的棱镜组件中第i个棱镜的转角，得到测量组件视轴光线在其棱镜组件坐标系下的直线方程，联立该直线方程与机器人运动平面方程，得到靶标中心在棱镜组件坐标系下的坐标，通过坐标转换关系得到靶标中心在世界坐标系下的坐标。

一种采用机器人标定系统的三维空间运动标定方法，所述测量组件设有多个，该方法包括：

B1、建立世界坐标系、机器人坐标系、各个测量组件坐标系、各个棱镜组件坐标系以及与各个棱镜组件相匹配的相机坐标系；

B2、选取任意一个测量组件为基准，将其他测量组件与其配对，通过标定技术获得所有非基准测量组件坐标系相对基准测量组件坐标系的坐标转换矩阵；

B3、上位机调整至少两个测量组件中与棱镜组件配套的相机的视轴，使靶标落入相机的视场范围内，通过相机采集靶标的图像，并将图像传输至上位机；

B4、上位机通过视觉跟踪算法控制至少两个测量组件中与棱镜组件配套的相机的视轴，使靶标处于相机的视场中心；

B5、上位机由编码器反馈的第i个测量组件的棱镜组件的第j个棱镜的转角，得到任意测量组件视轴光线在其棱镜组件坐标系下的直线方程，再根据坐标转换关系转换得到视轴光线在世界坐标系下的直线方程；

B6、联立至少两个测量组件视轴光线在世界坐标系下的直线方程，求解得到靶标中心的测量坐标；

B7、设定靶标中心名义坐标与测量坐标的误差阈值ΔB_f，构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角；

B8、改变工业机器人的姿态，重复B3至B6，通过工业机器人自身系统及坐标转换实时获得靶标中心在世界坐标系下的名义坐标，通过测量组件获得靶标中心在世界坐标系下的测量坐标，计算得到靶标中心在世界坐标系下的名义坐标与测量坐标的误差；

B9、比较误差ΔB_c与误差阈值ΔB_f，若ΔB_c小于等于ΔB_f，则输出工业机器人当前DH模型参数a、α、d和θ，结束工业机器人的标定，否则修正工业机器人当前DH模型参数a、α、d和θ，重复B8～B9，直到ΔB_c小于等于ΔB_f。

优选的，所述B2具体包括：

B21、选取任意一个测量组件为基准，将其他测量组件与其配对，上位机控制任意一对测量组件的棱镜组件，使各自相机视轴经过棱镜组件无偏转出射；

B22、将标定组件置于步骤B21中被上位机控制的一对测量组件的公共视场内，且不与机器人干涉；

B23、采用双目标定技术，以该对测量组件中基准测量组件为基准，求解该对测量组件中两相机之间的坐标转换矩阵

并求解该对测量组件的坐标转换矩阵

B24、重复B21～B23，直到获得所有非基准测量组件坐标系相对基准测量组件坐标系的坐标转换矩阵。

优选的，所述视觉跟踪算法采用查表法、粗精耦合跟踪算法或迭代法。

与现有技术相比，本发明具有以下优点：

1.该标定系统可由上位机控制棱镜组件并调整与其配套相机的视轴，可实现对靶标中心位置的实时动态测量，通过构建机器人当前几何参数名义坐标与测量坐标的误差响应方程，寻求误差最小值时所对应的机器人当前几何参数，可实现机器人的在线标定。

2.该标定系统采用视觉测量原理，为非接触式标定方法，可避免因外界测量装置对机器人标定的影响。

3.该标定系统结构简单，体积小，易于携带，系统成本低。

4.该标定系统可实现二维平面运动机器人和三维空间运动机器人标定，对不同运动类机器人的标定适应性较强。

5.该标定系统采用粗精相结合的方式实现对靶标的跟踪，具备高速、精准跟踪特性，可实现优于像素级定位及跟踪。

附图说明

图1为本发明标定系统的三维空间运动机器人标定原理图；

图2为本发明标定系统的二维平面运动机器人标定原理图；

图3为本发明测量组件的标定示意图；

图4为棱镜组件结构图；

图5为转镜组件结构图；

图6为视轴调整光路图；

图7为相机、转镜组件及相机转镜组件组合的视场域示意图；

图8为棱镜结构图，其中(1)为棱镜的主视图，(2)为棱镜的侧视图；

图9为棱镜组合示意图，其中(a)～(d)为四种不同的棱镜组合；

图10为二维平面运动工业机器人标定流程图；

图11为三维空间运动工业机器人标定流程图；

图12为本发明坐标测量原理图；

图13为粗精耦合跟踪算法流程图；

图14为迭代法原理图。

图中标注：1为工业机器人、2为靶标、3为测量组件、4为上位机、5为标定组件、31为棱镜组件、32为相机、31为棱镜组件、51为标定板、52为三角架、311为转镜组件、312为编码器组件、313为驱动组件、3111为安装座、3112为齿轮二、3113为套筒、3114为镜套、3115为棱镜、3116为轴承、3112为齿轮二、3113为套筒、3121为胀套二、3122为齿轮三、3123为编码器、3131为电机、3132为支座、3133为齿轮一、3134为胀套一、7为相机成像视场域、8为相机与转镜组件共同形成的组合成像视场域、9为光学扫描域。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示，本申请提出一种机器人标定系统，包括工业机器人1、靶标2、测量组件3、上位机4和标定组件5。

靶标2固装在工业机器人1的末端，可采用磁力吸附或负压吸附在工业机器人1末端。靶标2为球状发光二极管，可根据需求改变发光颜色。

测量组件3设于工业机器人1的周围，如图4、图5所示，包括棱镜组件31和相机32，棱镜组件31设置在靶标2和相机32之间，且相机32的视轴和棱镜组件31的光轴保持同轴。上位机4与相机32和棱镜组件31通信连接。

标定组件5设置在多个测量组件3包围的区域内，在测量组件3的视场内，且不与工业机器人1干涉。如图3所示，标定组件5包括标定板51和三角架52，标定板51固装在三角架52上。

棱镜组件31包括转镜组件311、编码器组件312和驱动组件313，转镜组件311分别与驱动组件313和编码器组件312连接。可同轴布置多个转镜组件311及与其相配套的编码器组件312和驱动组件313。

转镜组件311包括安装座3111和设置在安装座3111上的齿轮二3112、套筒3113、镜套3114、棱镜3115和轴承3116，棱镜3115置于镜套3114内部，镜套3114固连在套筒3113中，套筒3113通过轴承3116固装在安装座3111上并与齿轮二3112连接。

编码器组件312包括设置在安装座3111上的胀套二3121、齿轮三3122和编码器3123，编码器3123的旋转轴通过胀套二3121与齿轮三3122连接，齿轮三3122与齿轮二3112啮合。

驱动组件313包括设置在安装座3111上的电机3131、支座3132、齿轮一3133和胀套一3134，所述电机3131通过支座3132固装在安装座3111上，所述电机3131的输出轴通过胀套一3134与齿轮一3133连接，所述齿轮一3133与所述齿轮二啮合。电机3131为伺服电机、步进电机或力矩电机任一种。

驱动组件313与转镜组件311的传动方式，以及转镜组件311与编码器组件312的传动方式，采用齿轮传动、蜗轮蜗杆传动、同步带传动以及链传动中任一种或其组合形式。

如图6所示，光线依次入射多个棱镜3115，在棱镜3115间产生折射，通过旋转棱镜3115调整光线的折射程度。

如图7所示，光线通过转镜组件311所形成的光学扫描域9不小于相机32的相机成像视场域7，相机32与转镜组件311共同形成的组合成像视场域8不小于相机成像视场域7。

如图8所示，棱镜3115一端为平面另一端为斜面，剖面为楔形，材料为K9。

如图9所示为首末两个棱镜的四种组合，多个棱镜3115同轴依次排布，棱镜3115的平面为光线入射平面时为正面布置，棱镜3115的斜面为光线入射平面时为反面布置。多个棱镜3115中除首末两个棱镜外的其余棱镜3115可正面布置或反面布置，布置方式不要求一致。

本系统由上位机1通过视觉跟踪算法控制测量组件3的棱镜组件31调整与其配套相机32视轴，实现对靶标2的跟踪，进而实现测量组件3对靶标2中心的实时动态测量，由坐标转换关系获得靶标2中心在世界坐标系O_w-X_wY_wZ_w下的测量坐标。工业机器人1自身系统实时获得的靶标2中心在工业机器人1坐标系O_R-X_RY_RZ_R下的测量坐标，由坐标转换关系获得靶标2中心在世界坐标系O_w-X_wY_wZ_w下的名义坐标。构建以工业机器人1当前参数为自变量，以名义坐标与测量坐标的误差为响应值的方程。当名义坐标与测量坐标的误差大于误差阈值时，修正工业机器人1当前参数，改变工业机器人1姿态，再次进行靶标2中心的测量，直到名义坐标与测量坐标的误差小于等于误差阈值。

如图10所示，本申请提出一种采用上述机器人标定系统的二维平面运动机器人标定方法，包括：

a1、如图2所示，建立世界坐标系O_w-X_wY_wZ_w，建立机器人坐标系O_R-X_RY_RZ_R，建立测量组件坐标系O_M-X_MY_MZ_M，建立测量组件3中棱镜组件坐标系O_r-X_rY_rZ_r，建立与棱镜组件31相匹配的相机坐标系O_c-X_cY_cZ_c；

a2、上位机4控制测量组件3的棱镜组件31，使相机32视轴经过棱镜组件31无偏转出射，采用合适的标定方法标定相机32，例如直接线性法、TAai两步标定法、张正友标定法或神经网络标定法，记录相机32内参数和外参数，并保持相机32不动；

a3、上位机4控制测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2落入相机32的视场范围内；

a4、相机32采集靶标2的图像，并将图像传输至上位机4；

a5、上位机4通过视觉跟踪算法控制测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2中心像点处于相机32视场中心；

a6、上位机4由编码器3123反馈的测量组件3的棱镜组件31的第i个棱镜3115的转角θ_r,i，工业机器人1末端的靶标2在棱镜组件31坐标系O_r-X_rY_rZ_r下的坐标可由下式获得：

式中，A_i1表示第i个棱镜3115的入射光单位向量；A_if＝(X_d,Y_d,Z_d)表示第i个棱镜3115的出射光单位向量；n₁为入射介质的折射率；n₂为出射介质的折射率；θ_r,i为第i个棱镜3115的转角，i取1～n；α为棱镜3115楔角；N＝(X_N,Y_N,Z_N)为棱镜组件31中任意棱镜3115的入射平面、出射平面或工作平面的法向量；(X_b,Y_b,Z_b)为棱镜组件31入射平面、出射平面或工作平面上任意一点；(X_m,Y_m,Z_m)为棱镜组件31入射光线或出射光线上任意一点；t为参数；当棱镜组件31出射光线与机器人工作平面相交时，(x_r,y_r,z_r)为靶标2在棱镜组件31坐标系O_r-X_rY_rZ_r下的坐标；

a7、工业机器人1末端的靶标2在世界坐标系O_w-X_wY_wZ_w下的坐标可由下式获得：

式中(x_w,y_w,z_w)为靶标2在世界坐标系O_w-X_wY_wZ_w下的坐标，

为棱镜组件31坐标系O_r-X_rY_rZ_r相对与其配套相机32坐标系O_c-X_cY_cZ_c的坐标转换矩阵，

为相机32坐标系O_c-X_cY_cZ_c相对测量组件3坐标系O_M-X_MY_MZ_M的坐标转换矩阵，

为测量组件3坐标系O_M-X_MY_MZ_M相对世界坐标系O_w-X_wY_wZ_w的坐标转换矩阵；

a8、构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角，将机器人DH模型当前参数、工业机器人1自身系统获得的靶标2中心名义坐标以及由测量组件3获得的靶标2中心坐标储存；

a9、设定靶标2中心名义坐标与测量坐标的误差阈值ΔB_f；

a10、改变工业机器人1姿态，工业机器人1自身系统实时获得的靶标2中心在工业机器人1坐标系O_R-X_RY_RZ_R下的坐标，通过机器人坐标系O_R-X_RY_RZ_R相对世界坐标系O_w-X_wY_wZ_w的坐标转换关系，可获得世界坐标系O_w-X_wY_wZ_w下的名义坐标，重复步骤a3至a8，测量组件3对工业机器人1的末端靶标2中心坐标实时测量，获得靶标2中心在世界坐标系O_w-X_wY_wZ_w下的测量坐标，上位机4可实时计算获得靶标2中心在世界坐标系下的名义坐标与测量坐标的误差ΔB_c：

式中，x_m，y_m，z_m为靶标2中心在世界坐标系下的名义坐标，x_c，y_c，z_c为靶标2中心在世界坐标系下的测量坐标；

a11、比较该误差ΔB_c与误差阈值ΔB_f，若误差ΔB_c小于等于误差阈值ΔB_f，则输出工业机器人1当前DH模型参数a、α、d和θ，结束工业机器人1的标定，若大于误差阈值ΔB_f，则修正工业机器人(1)当前DH模型参数a、α、d和θ，重复a10至a11，直至ΔB_c小于等于ΔB_f。

如图11所示，本申请提出一种采用上述机器人标定系统的三维空间运动标定方法，系统设置如图1所示，测量组件设有多个，该方法包括：

b1、建立世界坐标系O_w-X_wY_wZ_w，建立机器人坐标系O_R-X_RY_RZ_R，建立各个测量组件坐标系O_Mi-X_MiY_MiZ_Mi，建立第i个测量组件3中棱镜组件坐标系O_ri-X_riY_riZ_ri，建立与各个棱镜组件31相匹配的相机坐标系O_ci-X_ciY_ciZ_ci，i取1～n；

b2、选取任意一个测量组件3为基准，坐标系为O_M1-X_M1Y_M1Z_M1，将其他测量组件3与其配对，上位机4控制任意一对测量组件3的棱镜组件31，使相机32视轴经过棱镜组件31无偏转出射；

b3、将标定组件5置于该对测量组件3的公共视场内，且不与工业机器人1干涉；

b4、采用双目标定技术，以该对测量组件3中基准测量组件3为基准，求解该对测量组件3中两相机32之间的坐标转换矩阵

并求解该对测量组件3的坐标转换矩阵

下式为

求解公式：

其中，

为非基准测量组件坐标系相对基准测量组件坐标系的坐标转换矩阵，

为非基准测量组件坐标系相对于其配套相机坐标系的坐标转换矩阵，

为基准测量组件坐标系相对于其配套相机坐标系的坐标转换矩阵；

b5、重复b2至b4，直到获得所有非基准测量组件3坐标系相对基准测量组件坐标系的坐标转换矩阵

b6.上位机4控制至少两个测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2落入相机32的视场范围内；

b7、相机32采集靶标2的图像，并将图像传输至上位机1；

b8、上位机4通过视觉跟踪算法控制至少两个测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2处于相机32视场中心；

b9、上位机4由编码器3123反馈的第i个测量组件3的棱镜组件31的第j个棱镜3115的转角θ_r,ij，根据下式可得任意测量组件3视轴光线在棱镜组件31坐标系O_r-X_rY_rZ_r下的所在直线方程：

式中，A_i1表示第i个棱镜3115的入射光单位向量；A_if＝(X_d,Y_d,Z_d)表示第i个棱镜3115的出射光单位向量；n₁为入射介质的折射率；n₂为出射介质的折射率；θ_r,ij为第i个测量组件3的第j个棱镜3115的转角，i取1～n；α为棱镜3115楔角；N＝(X_N,Y_N,Z_N)为棱镜组件31中任意棱镜3115的入射平面或出射平面的法向量；(X_b,Y_b,Z_b)为棱镜组件31中任意棱镜3115的入射平面或出射平面上任意一点；(X_m,Y_m,Z_m)为棱镜组件31入射光线或出射光线上任意一点；t为参数；(x_r,y_r,z_r)为直线方程变量；

b10、将棱镜组件坐标系O_ri-X_riY_riZ_ri下的所在直线方程，根据坐标转换关系转换到世界坐标系O_w-X_wY_wZ_w下，坐标转换关系为：

式中，

为棱镜组件坐标系O_ri-X_riY_riZ_ri相对非基准测量组件坐标系O_Mi-X_MiY_MiZ_Mi的坐标转换矩阵；

为非基准测量组件坐标系O_Mi-X_MiY_MiZ_Mi相对基准测量组件坐标系O_M1-X_M1Y_M1Z_M1的坐标转换矩阵；

为基准测量组件坐标系O_M1-X_M1Y_M1Z_M1相对世界坐标系O_w-X_wY_wZ_w的坐标转换矩阵；

b11、如图12，联立至少两个测量组件3视轴光线在世界坐标系O_w-X_wY_wZ_w下的直线方程，采用最小二乘法可求解靶标2中心测量坐标；

b12、构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角，将机器人DH模型当前参数、工业机器人1自身系统获得的靶标2中心名义坐标以及由测量组件3获得的靶标2中心测量坐标储存；

b13、设定靶标2中心名义坐标与测量坐标的误差阈值ΔB_f；

b14、改变工业机器人1姿态，工业机器人1自身系统实时获得的靶标2中心在工业机器人1坐标系O_R-X_RY_RZ_R下的坐标，通过工业机器人1坐标系O_R-X_RY_RZ_R相对世界坐标系O_w-X_wY_wZ_w的坐标转换关系，可获得世界坐标系O_w-X_wY_wZ_w下的名义坐标，重复b6至b12，测量组件3对工业机器人1的末端靶标2中心坐标实时测量，获得靶标2中心在世界坐标系O_w-X_wY_wZ_w下的测量坐标，上位机4可实时计算获得靶标2中心在世界坐标系下的名义坐标与测量坐标的误差ΔB_c：

式中，x_m，y_m，z_m为靶标2中心在世界坐标系下的名义坐标，x_c，y_c，z_c为靶标2中心在世界坐标系下的测量坐标；

b15.比较该误差ΔB_c与误差阈值ΔB_f，若误差ΔB_c小于等于误差阈值ΔB_f，则输出工业机器人1当前DH模型参数a、α、d和θ，结束工业机器人1的标定，若大于误差阈值ΔB_f，否则修正工业机器人1当前DH模型参数a、α、d和θ，重复b14至b15，直至ΔB_c小于等于ΔB_f。

视觉跟踪算法可采用查表法、粗精耦合跟踪算法或迭代法。

视觉跟踪算法为查表法时，具体骤如下：

S1.在当前棱镜组件31的转角条件下，相机32采集图像；

S2.以靶标2在像平面上的像点和相机32焦点O的连线为棱镜组件31的入射光线向量，通过矢量光线追迹法解算棱镜组件31的出射光线向量；

S3.确定棱镜3115旋转步长θ_step，将棱镜3115的转动角度分为360°/θ_step份；

S4.令棱镜3115转角为θ_r，设θ_r初始值为0，以式θ_r＝θ_r+θ_step为变化方程，建立转角关于棱镜组件31出射点和出射光单位向量间的查找表，或建立转角关于虚拟平面的投射点T_i间的查找表，虚拟平面为与棱镜组件31转轴垂直的平面，棱镜组件31出射点、出射光单位向量以及虚拟平面的投射点计算公式如下：

式中，A_i1表示第i个棱镜3115的入射光单位向量；A_if＝(X_d,Y_d,Z_d)表示第i个棱镜3115的出射光单位向量；θ_r,i为测量组件3的第i个棱镜3115的转角θ_r，i取1～n；α为棱镜3115楔角；N＝(X_N,Y_N,Z_N)为棱镜组件31中任意棱镜3115的入射平面、出射平面或虚拟平面的法向量；(x_b,y_b,z_b)为棱镜组件31中任意棱镜3115的入射平面、出射平面或虚拟平面上任意一点；(x_m,y_m,z_m)为棱镜组件31入射光线或出射光线上任意一点；t为参数；当棱镜组件31出射光线与棱镜组件31出射平面相交时，(x,y,z)为棱镜组件31出射点坐标；当棱镜组件31出射光线与虚拟平面相交时，(x,y,z)为虚拟平面的投射点坐标；

S5.根据当前棱镜组件31的出射点和出射光单位向量，在所建立的表中首先搜索获取使误差Δ₁₁达到最小值时所对应的棱镜组件31转角组，然后在转角组中搜索与当前转角最接近的转角，或在完成Δ₁₁搜索的此基础上，再搜索使误差Δ₁₂达到最小值时所对应的棱镜组件31转角；或根据当前棱镜组件31的虚拟平面的投射点，在所建立的表中搜索获取使得误差Δ₂达到最小值时所对应的棱镜组件31转角，Δ₁₁，Δ₁₂和Δ₂的表达式分别为：

式中，X_tf、Y_tf、Z_tf为表中记录的棱镜组件31的出射光单位向量坐标，x_rf、y_rf、z_rf为当前棱镜组件31出射光单位向量坐标，X_kf、Y_kf、Z_kf为表中记录的棱镜组件31的出射点坐标，x_kf、y_kf、z_kf为当前棱镜组件31出射点坐标；

式中，X_tp、Y_tp、Z_tp为表中记录的虚拟平面投射点坐标，x_vp、y_vp、z_vp为当前虚拟平面投射点坐标。

视觉跟踪算法采用粗精耦合跟踪算法时，如图13所示，主要包括粗跟踪策略和精跟踪策略，具体步骤如下：

粗跟踪策略步骤如下：

D1.设置偏差阈值ΔB_t；

D2.在当前棱镜组件31的转角条件下，相机32采集图像，获取靶标2中心像点；

D3.以靶标2中心像点和相机32焦点O的连线为棱镜组件31的入射光线向量，通过矢量光线追迹法解算棱镜组件31的出射光线向量；

D4.在当前出射光线向量条件下，采用两步法解算棱镜组件31的转角；

D5.在当前棱镜组件31的转角条件下，相机32采集图像，获取靶标2中心像点图像坐标(u,v)；

D6.计算当前偏差ΔB_c，ΔB_c的表达式为：

其中u_z和v_z是视轴中心图像坐标；

D7.若ΔB_c小于等于ΔB_t，则结束调整；若ΔB_c大于ΔB_t，则重复D2～D7，或进行精跟踪策略。

精跟踪策略步骤如下：

E1.设置方位角步长初值step0，俯仰角步长初值step_i，i取1～n，令step＝step0；

E2.方位角调整，令ΔB_f＝ΔB_c，θ_r,i＝θ_r,i+step，θ_r,i为棱镜组件31的第i个棱镜3115的转角，i取1～n；，采集图像，检测图像中靶标2，计算当前偏差ΔB_c；

E3.比较ΔB_c和ΔB_f，若ΔB_c小于ΔB_f，则step＝step，θ_r,i＝θ_r,i+step；若ΔB_c大于等于ΔB_f，则step＝-step，θ_r,i＝θ_r,i+step；若ΔB_c小于等于ΔB_t，则结束调整；若θ_r,i＝θ_r,i+step时，ΔB_c大于等于ΔB_f，且θ_r,i＝θ_r,i-step时，ΔB_c大于等于ΔB_f，则减小step；

E4.俯仰角调整，令ΔB_f＝ΔB_c，θ_r,i＝θ_r,i+step_i，i取1～n，采集图像，检测图像中靶标2，计算当前偏差ΔB_c；

E5.比较ΔB_c和ΔB_f，若ΔB_c小于ΔB_f，则step_i＝step_i，θ_r,i＝θ_r,i+step_i；若ΔB_c大于等于ΔB_f，则step_i＝-step_i，θ_r,i＝θ_r,i+step_i；若ΔB_c小于等于ΔB_t，则结束调整；若θ_r,i＝θ_r,i+step_i，ΔB_c大于等于ΔB_f，且θ_r,i＝θ_r,i-step_i，ΔB_c大于等于ΔB_f，则减小step_i；

E6.重复E2～E5。

视觉跟踪算法为迭代法时，如图14所示，具体步骤如下：

X1.设置偏差阈值ΔB_t，N₀为最外侧棱镜3115中心点；

X2.以出射点N_i和目标点T的连线为出射向量A_0f，采用两步法求解棱镜组件31的各棱镜3115转角，i取1～n；

X3.以A_r＝(0,0,1)为入射光线，在当前棱镜组件31转角下，求解棱镜组件31的出射点N_i和指向点T_i，i取1～n，其计算表达式为：

式中，A_i1表示第i个棱镜3115的入射光单位向量；A_if表示第i个棱镜3115的出射光单位向量；θ_r,i为测量组件3的第i个棱镜3115的转角，i取1～n；α为棱镜3115楔角；N＝(X_N,Y_N,Z_N)为棱镜组件31中任意棱镜3115的入射平面、出射平面或目标平面的法向量，目标平面为目标与视轴垂直的所在面；(x_b,y_b,z_b)为棱镜组件31中任意棱镜3115的入射平面、出射平面或目标平面上任意一点；(x_m,y_m,z_m)为棱镜组件31入射光线或出射光线上任意一点；t为参数；当棱镜组件31出射光线与棱镜组件31出射平面相交时，(x,y,z)为N_i坐标；当棱镜组件31出射光线与目标平面相交时，(x,y,z)为T_i坐标，即(x,y,z)＝(x_i,y_i,z_i)；

X4.计算当前偏差ΔB_c，ΔB_c的表达式为：

式中，(X_rp,Y_rp,Z_rp)为目标点T坐标，(x_i,y_i,z_i)为指向点T_i坐标，如果当前偏差ΔB_c小于偏差阈值ΔB_t，则结束调整；

X5.重复X2～X4。

其中，两步法具体步骤如下：

(1)已知出射光向量(X_p,Y_p,Z_p)，解算出射光俯仰角ρ₀和方位角

计算公式如下：

(2)假设初始条件下棱镜组件31的第一棱镜3115不动，令第二个棱镜3115旋转θ_s，即此时出射光线俯仰角为ρ₀，θ_s可由下式求解：

式中，n为入射介质与出射介质折射率的比值。

(3)计算此时出射光线方位角

(4)令第一棱镜转角为

第二棱镜转角为

其中，矢量光线追迹法为已知棱镜组件31的入射光线方向向量及各棱镜3115转角，求解出射光线方向向量，其解算公式为：

式中，A_i1为第i个棱镜3115的入射光单位向量，A_if为第i个棱镜3115的出射光单位向量；N为棱镜组件31中任意棱镜3115折射平面的法向量；

式中，θ_r,i为第i个棱镜3115的转角，i取1～n，α为棱镜3115楔角。

实施例

本实施例以棱镜组件31为双棱镜为例，系统的二维平面运动标定方法如下：

a1、建立世界坐标系O_w-X_wY_wZ_w，建立机器人坐标系O_R-X_RY_RZ_R，建立测量组件坐标系O_M-X_MY_MZ_M，建立测量组件3中棱镜组件坐标系O_r-X_rY_rZ_r，建立与棱镜组件31相配套的相机坐标系O_c-X_cY_cZ_c，将测量组件坐标系O_M-X_MY_MZ_M、世界坐标系O_w-X_wY_wZ_w以及与测量组件3相配套的相机坐标系O_c-X_cY_cZ_c；

a2、上位机4控制测量组件3的棱镜组件31，使相机32视轴经过棱镜组件31无偏转出射，采用合适的标定方法标定相机32，所述合适的标定方法为直接线性法、Tsai两步标定法、张正友标定法或神经网络标定法，记录相机32内参数和外参数，并保持相机32不动；

a3、上位机4控制测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2落入相机32的视场范围内；

a4、相机32采集靶标2的图像，并将图像传输至上位机4；

a5、上位机4通过视觉跟踪算法控制测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2中心像点处于相机32视场中心；

a6、上位机4由编码器3123反馈测量组件3的棱镜组件31的第1个棱镜3115的转角为180°，第2个棱镜3115的转角为225°，棱镜3115楔角为15°，机器人工作平面为z＝400，工业机器人1末端的靶标2在棱镜组件31坐标系O_r-X_rY_rZ_r下的坐标为(93.47，36.97，400)，可由下式获得：

a7、棱镜组件坐标系O_r-X_rY_rZ_r相对测量组件坐标系O_M-X_MY_MZ_M的齐次坐标转换矩阵

为

工业机器人1末端的靶标2在世界坐标系O_w-X_wY_wZ_w下的坐标为(-36.97，93.47，455)；

a8、构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角，将机器人DH模型当前参数、工业机器人1自身系统获得的靶标2中心名义坐标以及由测量组件3获得的靶标2中心坐标储存；

a9、设定靶标2中心名义坐标与测量坐标的误差阈值ΔB_f；

a10、改变工业机器人1姿态，工业机器人1自身系统实时获得的靶标2中心在工业机器人1坐标系O_R-X_RY_RZ_R下的坐标，通过工业机器人1坐标系O_R-X_RY_RZ_R相对世界坐标系O_w-X_wY_wZ_w的坐标转换关系，可获得世界坐标系O_w-X_wY_wZ_w下的名义坐标，重复a3至a8，测量组件3对工业机器人1的末端靶标2中心坐标实时测量，获得靶标2中心在世界坐标系O_w-X_wY_wZ_w下的测量坐标，上位机4可实时计算获得靶标2中心在世界坐标系下的名义坐标与测量坐标的误差ΔB_c：

式中，x_m，y_m，z_m为靶标2中心在世界坐标系下的名义坐标，x_c，y_c，z_c为靶标2中心在世界坐标系下的测量坐标；

a11、比较该误差ΔB_c与误差阈值ΔB_f，若误差小于等于误差阈值ΔB_f，则输出工业机器人1当前DH模型参数a、α、d和θ，结束工业机器人1的标定，若大于误差阈值ΔB_f，否则修正工业机器人1当前DH模型参数a、α、d和θ，重复a10至a11，直至ΔB_c小于等于ΔB_f。

采用两个测量组件3，以棱镜组件31为双棱镜为例，三维空间运动标定方法如下：

b1、建立世界坐标系O_w-X_wY_wZ_w，建立机器人坐标系O_R-X_RY_RZ_R，建立两个测量组件坐标系O_M1-X_M1Y_M1Z_M1和坐标系O_M2-X_M2Y_M2Z_M2，建立两个测量组件3中各自棱镜组件坐标系O_r1-X_r1Y_r1Z_r1和O_r2-X_r2Y_r2Z_r2，建立与两个棱镜组件31相配套的相机坐标系O_c1-X_c1Y_c1Z_c1和O_c2-X_c2Y_c2Z_c2，将世界坐标系O_w-X_wY_wZ_w、测量组件坐标系O_M1-X_M1Y_M1Z_M1以及与测量组件3相配套的相机坐标系O_c1-X_c1Y_c1Z_c1重合；

b2、选取任意一个测量组件3为基准，坐标系为O_M1-X_M1Y_M1Z_M1，将另一个测量组件3与其配对，上位机4控制该对测量组件3的棱镜组件31，使相机32视轴经过棱镜组件31无偏转出射；

b3、将标定组件5置于该对测量组件3的公共视场内，且不与工业机器人1干涉；

b4、采用双目标定技术，以该对测量组件3中基准测量组件3为基准，求解该对测量组件3中两相机32之间的齐次坐标转换矩阵，即为该对测量组件3的齐次坐标转换矩阵：

b5、重复b2至b4，直到获得所有非基准测量组件坐标系相对基准测量组件3坐标系的坐标转换矩阵

b6、上位机4控制两个测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2落入相机32的视场范围内；

b7、相机32采集靶标2的图像，并将图像传输至上位机1；

b8、上位机4通过视觉跟踪算法控制两个测量组件3的棱镜组件31调整与其配套相机32的视轴，使靶标2处于相机32视场中心；

b9、上位机4由编码器3123反馈第1个测量组件3的第1个棱镜3115的转角200°和第2个棱镜3115的转角200°，以及第2个测量组件3的第1个棱镜3115的转角161.5°和第2个棱镜3115的转角220°，棱镜3115楔角为15°，两个测量组件3视轴光线在各自棱镜组件31坐标系O_r1-X_r1Y_r1Z_r1和坐标系O_r2-X_r2Y_r2Z_r2下的所在直线方程分别为：

b10、将棱镜组件坐标系O_r1-X_r1Y_r1Z_r1和坐标系O_r2-X_r2Y_r2Z_r2下的所在直线方程，根据坐标转换关系转换到世界坐标系O_w-X_wY_wZ_w下，坐标转换关系分别为：

b11、联立两个测量组件3视轴光线在世界坐标系O_w-X_wY_wZ_w下的直线方程，采用最小二乘法可求解靶标2中心测量坐标(-37.2895,102.4522,455)；

b12、构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角，将机器人DH模型参数、工业机器人1自身系统获得的靶标2中心名义坐标以及由测量组件3获得的靶标2中心测量坐标储存；

b13、设定靶标2中心名义坐标与测量坐标的误差阈值ΔB_f；

b14、改变工业机器人1姿态，工业机器人1自身系统实时获得的靶标2中心在工业机器人1坐标系O_R-X_RY_RZ_R下的坐标，通过工业机器人1坐标系O_R-X_RY_RZ_R相对世界坐标系O_w-X_wY_wZ_w的坐标转换关系，可获得世界坐标系O_w-X_wY_wZ_w下的名义坐标，重复b6至b11，测量组件3对工业机器人1的末端靶标2中心坐标实时测量，获得靶标2中心在世界坐标系O_w-X_wY_wZ_w下的测量坐标，上位机4可实时计算获得靶标2中心在世界坐标系下的名义坐标与测量坐标的误差ΔB_c：

式中，x_m，y_m，z_m为靶标2中心在世界坐标系下的名义坐标，x_c，y_c，z_c为靶标2中心在世界坐标系下的测量坐标；

b15、比较该误差ΔB_c与误差阈值ΔB_f，若误差ΔB_c小于等于误差阈值ΔB_f，则输出工业机器人1当前DH模型参数a、α、d和θ，结束工业机器人1的标定，若大于误差阈值ΔB_f，否则修正工业机器人1当前DH模型参数a、α、d和θ，重复b14至b15，直至ΔB_c小于等于ΔB_f。

Claims (10)

1.一种机器人标定系统，包括工业机器人(1)和上位机(4)，其特征在于，还包括：

靶标(2)，固装在所述工业机器人(1)的末端；

测量组件(3)，设于所述工业机器人(1)的周围，包括棱镜组件(31)和相机(32)，所述棱镜组件(31)设置在靶标(2)和相机(32)之间，且相机(32)的视轴和棱镜组件(31)的光轴保持同轴；

标定组件(5)，设置在所述测量组件(3)的视场内，且不与工业机器人(1)干涉；

所述上位机(4)与相机(32)和棱镜组件(31)连接。

2.根据权利要求1所述的一种机器人标定系统，其特征在于，所述棱镜组件(31)包括转镜组件(311)、编码器组件(312)和驱动组件(313)，所述转镜组件(311)分别与驱动组件(313)和编码器组件(312)连接。

3.根据权利要求2所述的一种机器人标定系统，其特征在于，所述转镜组件(311)包括安装座(3111)和设置在安装座(3111)上的齿轮二(3112)、套筒(3113)、镜套(3114)、棱镜(3115)和轴承(3116)，所述棱镜(3115)置于镜套(3114)内部，所述镜套(3114)固连在套筒(3113)中，所述套筒(3113)通过轴承(3116)固装在安装座(3111)上并与齿轮二(3112)连接。

4.根据权利要求3所述的一种机器人标定系统，其特征在于，所述编码器组件(312)包括设置在安装座(3111)上的胀套二(3121)、齿轮三(3122)和编码器(3123)，所述编码器(3123)的旋转轴通过胀套二(3121)与齿轮三(3122)连接，所述齿轮三(3122)与齿轮二(3112)啮合。

5.根据权利要求1所述的一种机器人标定系统，其特征在于，光线通过所述转镜组件(311)所形成的光学扫描域(9)不小于相机(32)的相机成像视场域(7)，所述相机(32)与转镜组件(311)共同形成的组合成像视场域(8)不小于相机成像视场域(7)。

6.一种采用所述权利要求1～5任一所述的机器人标定系统的二维平面运动机器人标定方法，其特征在于，包括：

A1、建立世界坐标系、机器人坐标系、测量组件坐标系、棱镜组件坐标系以及与棱镜组件(31)相匹配的相机坐标系；

A2、上位机(4)控制棱镜组件(31)，使相机(32)的视轴经过棱镜组件(31)无偏转出射，标定相机(32)的内参数和外参数，保持相机(32)不动；

A3、上位机(1)通过视觉跟踪算法调整棱镜组件(31)与其配套的相机(32)视轴，实现测量组件(3)对靶标(2)中心的实时动态测量，由坐标转换关系获得靶标(2)中心在世界坐标系下的测量坐标；由工业机器人(1)自身系统实时获得靶标(2)中心在机器人坐标系下的名义坐标，再由坐标转换关系获得靶标(2)中心在世界坐标系下的名义坐标；

A4、设置标定误差阈值ΔB_f，构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角；

A5、改变工业机器人(1)的姿态，重复A3实时得到靶标(2)中心在世界坐标系下的名义坐标和测量坐标，计算得到靶标(2)中心在世界坐标系下的名义坐标与测量坐标的误差ΔB_c；

A6、比较误差ΔB_c与误差阈值ΔB_f，若ΔB_c小于等于ΔB_f，则输出工业机器人(1)当前DH模型参数a、α、d和θ，结束工业机器人(1)的标定，否则修正工业机器人(1)当前DH模型参数a、α、d和θ，重复A5～A6，直到ΔB_c小于等于ΔB_f。

7.根据权利要求6所述的一种机器人标定系统的二维平面运动标定方法，其特征在于，所述A3中靶标(2)中心在世界坐标系下的测量坐标的获得过程具体包括：

1)上位机(4)调整与棱镜组件(31)配套相机(32)的视轴，使靶标(2)落入相机(32)的视场范围内，相机(32)采集靶标(2)的图像，并将图像传输至上位机(4)；

2)上位机(4)通过视觉跟踪算法调整与棱镜组件(31)配套相机(32)的视轴，使靶标(2)的中心像点处于相机(32)的视场中心；

3)上位机(4)根据编码器(3123)反馈的棱镜组件(31)中第i个棱镜(3115)的转角，得到测量组件(3)视轴光线在其棱镜组件坐标系下的直线方程，联立该直线方程与机器人运动平面方程，得到靶标(2)中心在棱镜组件坐标系下的坐标，通过坐标转换关系得到靶标(2)中心在世界坐标系下的坐标。

8.一种采用所述权利要求1～5任一所述的机器人标定系统的三维空间运动标定方法，其特征在于，所述测量组件设有多个，该方法包括：

B1、建立世界坐标系、机器人坐标系、各个测量组件坐标系、各个棱镜组件坐标系以及与各个棱镜组件(31)相匹配的相机坐标系；

B2、选取任意一个测量组件(3)为基准，将其他测量组件(3)与其配对，通过标定技术获得所有非基准测量组件(3)坐标系相对基准测量组件(3)坐标系的坐标转换矩阵；

B3、上位机(4)调整至少两个测量组件(3)中与棱镜组件(31)配套的相机(32)的视轴，使靶标(2)落入相机(32)的视场范围内，通过相机(32)采集靶标(2)的图像，并将图像传输至上位机(1)；

B4、上位机(4)通过视觉跟踪算法控制至少两个测量组件(3)中与棱镜组件(31)配套的相机(32)的视轴，使靶标(2)处于相机(32)的视场中心；

B5、上位机(4)由编码器(3123)反馈的第i个测量组件(3)的棱镜组件(31)的第j个棱镜(3115)的转角，得到任意测量组件(3)视轴光线在其棱镜组件坐标系下的直线方程，再根据坐标转换关系转换得到视轴光线在世界坐标系下的直线方程；

B6、联立至少两个测量组件(3)视轴光线在世界坐标系下的直线方程，求解得到靶标(2)中心的测量坐标；

B7、设定靶标(2)中心名义坐标与测量坐标的误差阈值ΔB_f，构建机器人DH模型：

T＝f(a,α,d,θ)

式中，T为机器人末端位姿矩阵，a为连杆长度，α为扭角，d为偏置，θ为关节角；

B8、改变工业机器人(1)的姿态，重复B3至B6，通过工业机器人(1)自身系统及坐标转换实时获得靶标(2)中心在世界坐标系下的名义坐标，通过测量组件(3)获得靶标(2)中心在世界坐标系下的测量坐标，计算得到靶标(2)中心在世界坐标系下的名义坐标与测量坐标的误差；

B9、比较误差ΔB_c与误差阈值ΔB_f，若ΔB_c小于等于ΔB_f，则输出工业机器人(1)当前DH模型参数a、α、d和θ，结束工业机器人(1)的标定，否则修正工业机器人(1)当前DH模型参数a、α、d和θ，重复B8～B9，直到ΔB_c小于等于ΔB_f。

9.根据权利要求8所述的一种机器人标定系统的三维空间运动标定方法，其特征在于，所述B2具体包括：

B21、选取任意一个测量组件(3)为基准，将其他测量组件(3)与其配对，上位机(4)控制任意一对测量组件(3)的棱镜组件(31)，使各自相机(32)视轴经过棱镜组件(31)无偏转出射；

B22、将标定组件(5)置于步骤B21中被上位机(4)控制的一对测量组件(3)的公共视场内，且不与机器人(1)干涉；

B23、采用双目标定技术，以该对测量组件(3)中基准测量组件(3)为基准，求解该对测量组件(3)中两相机(32)之间的坐标转换矩阵

并求解该对测量组件(3)的坐标转换矩阵

B24、重复B21～B23，直到获得所有非基准测量组件(3)坐标系相对基准测量组件(3)坐标系的坐标转换矩阵。

10.根据权利要求8所述的一种机器人标定系统的三维空间运动标定方法，其特征在于，所述视觉跟踪算法采用查表法、粗精耦合跟踪算法或迭代法。

Images