CN106338990A - 基于激光跟踪仪的工业机器人dh参数标定与零位标定方法 - Google Patents

Abstract

本发明公开了基于激光跟踪仪的工业机器人DH参数标定与零位标定方法，包括机器人、机器人控制器、计算机和激光跟踪仪；计算机分别与机器人控制器和激光跟踪仪电连接；包括如下步骤：机器人运行到50个位置点；计算机读取关节角，激光跟踪仪测量靶球位置；计算机计算修正后的DH参数；设置机器人新的零位点；将修正后的DH参数写到机器人控制器中。本发明具有精度高、速度快的特点。

Description

基于激光跟踪仪的工业机器人DH参数标定与零位标定方法

技术领域

本发明涉及6R工业机器人技术领域，尤其是涉及一种精度高、速度快的基于激光跟踪仪的工业机器人DH参数标定与零位标定方法。

背景技术

机器人精度指标包含重复定位精度和绝对定位精度。其中，重复定位精度主要由减速器齿轮回差、电机控制的最小精度等硬件条件决定，一般机器人的重复定位精度较好；

绝对定位精度主要与机器人装配过程中引起的装配误差、机器人使用过程中的磨损或超载时造成的不可回复形变、机器人的实际DH参数和零位与设计的DH参数和零位不一致相关。

目前，企业对发展柔性生产的需求日益强烈，要求机器人能够适合不同生产线的生产任务，通过视觉识别与离线编程技术，要求机器人的末端能够到达笛卡尔系准确的位置，对绝对定位精度的要求越来越高。

DH参数的误差是影响绝对精度的主要因素，对DH参数的标定对提升机器人的性能起着至关重要的作用。机器人DH参数的标定是对机器人控制器中能够修改的部分DH参数进行辨识与补偿，以提升机器人性能。其中DH参数中的关节角θ₁-θ₆值标定，即为机器人的零位标定。目前，常用的零位标定方法，如冯亚磊的“一种工业机器人零位标定方法”，操作繁琐且准确度较差。而DH参数标定需要通过关节长度精确加工达到，对加工与装配的精度要求极高，机器人的生产制造成本也较高。

发明内容

本发明的发明目的是为了克服现有技术中的标定操作繁琐、准确度较差、制造成本高的不足，提供了一种精度高、速度快的基于激光跟踪仪的工业机器人DH参数标定与零位标定方法。

为了实现上述目的，本发明采用以下技术方案：

一种基于激光跟踪仪的工业机器人DH参数标定与零位标定方法，包括机器人、机器人控制器、计算机和激光跟踪仪；计算机分别与机器人控制器和激光跟踪仪电连接；包括如下步骤：

(1-1)选择机器人工作空间中的立方体，使机器人末端均匀的到达立方体中的50个位置点，安装于机器人夹具上的靶球在每个位置点处均朝向激光跟踪仪；

(1-2)计算机通过机器人控制器读取机器人在50个位置点的关节角θ₁-θ₆的值，激光跟踪仪测量50个位置点的靶球位置x_j、y_j、z_j；

(1-3)计算机利用50个位置点的关节角θ₁至θ₆，靶球位置x_j、y_j、z_j，名义DH参数值计算修正后的DH参数；

(1-4)将机器人复位到理论值的零位，使机器人运动到θ参数误差角度处，将θ₁-θ₆参数误差角度设为机器人新的零位点；

(1-5)计算机将修正后的DH参数写到机器人控制器中，完成对机器人参数的补偿。

本发明能够高精度、快速地进行机器人DH参数标定与零位标定，并且能够同时标定出工具末端TCP点到机器人末端法兰盘中心的工具坐标系转换。

作为优选，步骤(1-3)包括如下步骤：

将机器人通过底座安装到工作平台上，底座上设有机器人的六个关节；底座不可能保持绝对水平，标定过程中必须同时标定底座所在的基坐标系相对于激光跟踪仪坐标系的位置与姿态转换。

(2-1)设定基坐标转换矩阵为

其中，为激光跟踪仪测量坐标系meas到机器人基坐标系base的转换矩阵，ψ为欧拉角绕测量系X轴旋转的角度，θ为欧拉角绕测量系Y轴旋转的角度，为欧拉角绕测量系Z轴旋转的角度，dl为转换矩阵沿X轴方向的位置平移，dm为转换矩阵沿Y轴方向的位置平移，dn为转换矩阵沿Z轴方向的位置平移，c为三角函数符号cos的缩写，s为三角函数符号sin的缩写；

(2-2)使用修正的DH模型对机器人进行建模，得到六个关节的转换矩阵

$A_i^{i-1}=\left[\begin{array}{cccc}{\mathrm{c\θ}}_i{\mathrm{c\β}}_i-{\mathrm{s\θ}}_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& -{\mathrm{s\θ}}_i{\mathrm{c\α}}_i& {\mathrm{c\θ}}_i{\mathrm{s\β}}_i+{\mathrm{s\θ}}_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& a_i{\mathrm{c\θ}}_i\\ {\mathrm{s\θ}}_i{\mathrm{c\β}}_i+{\mathrm{cos\θ}}_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& {\mathrm{c\θ}}_i{\mathrm{c\α}}_i& {\mathrm{s\θ}}_i{\mathrm{s\β}}_i-{\mathrm{c\θ}}_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& a_i{\mathrm{s\θ}}_i\\ -{\mathrm{c\α}}_i{\mathrm{s\β}}_i& {\mathrm{s\α}}_i& {\mathrm{c\α}}_i{\mathrm{c\β}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

其中，为i-1系到i系的转换矩阵，i＝1，2...6，0系与base系一致；DH参数包括第i个关节的关节角θ_i，第i个关节的连杆扭角α_i，第i个关节的偏距d_i，第i个关节的连杆长度a_i和避免转换矩阵奇异引入的第i个关节的扭角β_i；

(2-3)设定6系到工具坐标系tool的转换矩阵为

$A_{tool}^6=\left[\begin{array}{cccc}1& 0& 0& do\\ 0& 1& 0& dp\\ 0& 0& 1& dq\\ 0& 0& 0& 1\end{array}\right]$

其中，do为转换矩阵沿6系X轴的位置平移，dp为转换矩阵沿6系Y轴的位置平移，dq为转换矩阵沿6系Z轴的位置平移；

(2-4)设定基坐标系转换误差为

其中，为的误差值，δθ为θ的误差值，δψ为ψ的误差值，δ(dl)为dl的误差值，δ(dm)为细的误差值，δ(dn)为dn的误差值；

(2-5)设定得到基坐标的位姿误差矢量：

其中，^basedx为base系下dx的误差值，^basedy为base系下dy的误差值，^basedz为base系下dz的误差值，^baseδx为base系下δx的误差值，^baseδy为base系下δy的误差值，^baseδz为base系下δz的误差值，dx为转换矩阵沿X轴方向的位置误差，dy为转换矩阵沿Y轴方向的位置误差，dz为转换矩阵沿Z轴方向的位置误差，δx为转换矩阵绕X轴方向的姿态误差，δy为转换矩阵绕Y轴方向的姿态误差，δz为转换矩阵绕Z轴方向的姿态误差；

(2-6)设定机器人的每个关节误差为

其中，δa_i为a_i的误差值，δd_i为d_i的误差值，δα_i为α_i的误差值，δθ_i为θ_i的误差值，δβ_i为β_i的误差值；

(2-7)设定得到关节坐标转换的位姿误差矢量为

$\left[\begin{array}{c}d_{i}x\\ d_{i}y\\ d_{i}z\\ \mathrm{\δ}_{i}x\\ \mathrm{\δ}_{i}y\\ \mathrm{\δ}_{i}z\end{array}\right]=\left[\begin{array}{ccccc}0& {\mathrm{c\β}}_i& 0& a_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& -{\mathrm{c\α}}_i{\mathrm{s\β}}_i\\ 0& 0& 0& a_i{\mathrm{c\α}}_i& {\mathrm{s\α}}_i\\ 0& {\mathrm{s\β}}_i& 0& -a_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& {\mathrm{c\α}}_i{\mathrm{c\β}}_i\\ {\mathrm{c\β}}_i& 0& 0& -{\mathrm{c\α}}_i{\mathrm{s\β}}_i& 0\\ 0& 0& 1& {\mathrm{s\α}}_i& 0\\ {\mathrm{s\β}}_i& 0& 0& {\mathrm{c\α}}_i{\mathrm{c\β}}_i& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ\α}}_i\\ {\mathrm{\δa}}_i\\ {\mathrm{\δ\β}}_i\\ {\mathrm{\δ\θ}}_i\\ {\mathrm{\δd}}_i\end{array}\right]=H_i{\mathrm{\Δq}}_i$

其中，ⁱdx为i系下dx的误差值，ⁱdy为i系下dy的误差值，ⁱdz为i系下dz的误差值，ⁱδx为i系下δx的误差值，ⁱδy为i系下δy的误差值，ⁱδz为i系下δz的误差值；

(2-8)设定误差传递关系为

$\left[\begin{array}{c}d_i{x}_{tool}\\ d_i{y}_{tool}\\ d_i{z}_{tool}\\ \mathrm{\δ}_i{x}_{tool}\\ \mathrm{\δ}_i{y}_{tool}\\ \mathrm{\δ}_i{z}_{tool}\end{array}\right]=\left[\begin{array}{cccccc}{n}_x& n_y& n_z& {(P\×n)}_x& {(P\×n)}_y& {(P\×n)}_z\\ o_x& o_y& o_z& {(P\×o)}_x& {(P\×o)}_y& {(P\×o)}_z\\ a_x& a_y& a_z& {(P\×a)}_x& {(P\×a)}_y& {(P\×a)}_z\\ 0& 0& 0& n_x& n_y& n_z\\ 0& 0& 0& o_x& o_y& o_z\\ 0& 0& 0& a_x& a_y& a_z\end{array}\right]\left[\begin{array}{c}d_{i}x\\ d_{i}y\\ d_{i}z\\ \mathrm{\δ}_{i}x\\ \mathrm{\δ}_{i}y\\ \mathrm{\δ}_{i}z\end{array}\right]={G_i}^i{e}_i$

其中， A为关节转换矩阵，P为误差传递矩阵的位置转换关系，n，o，a为误差传递矩阵的姿态转换关系，ⁱdx_tool为i系下的dx转换到tool系下的位置误差值，ⁱdy_tool为i系下的dy转换到tool系下的位置误差值，ⁱdz_tool为i系下的dz转换到tool系下的位置误差值，ⁱδx_tool为i系下的δx转换到tool系下的姿态误差值，ⁱδy_tool为i系下的δy转换到tool系下的姿态误差值，ⁱδz_tool为i系下的δz转换到tool系下的姿态误差值，(P×n)_x为向量P与向量n叉积结果的第一个元素，(P×n)_y为向量P与向量n叉积结果的第二个元素，(P×n)_z为向量P与向量n叉积结果的第三个元素，(P×o)_x为向量P与向量o叉积结果的第一个元素，(P×o)_y为向量P与向量o叉积结果的第二个元素，(P×o)_z为向量P与向量o叉积结果的第三个元素，(P×a)_x为向量P与向量a叉积结果的第一个元素，(P×a)_y为向量P与向量a叉积结果的第二个元素，(P×a)_z为向量P与向量a叉积结果的第三个元素；

(2-9)将基坐标转换的误差和每个关节的DH参数误差转换到工具坐标系下：

${\stackrel{j}{E}}_{(6\×1)}=\[G_0{H}_0,G_1{H}_1,G_2{H}_2,G_3{H}_3,G_4{H}_4,G_5{H}_5,G_6{H}_6,I\left(6\right)\]Q={\stackrel{j}{M}}_{(6\×42)}{Q}_{(42\×1)}$

其中，表示第j个位置点的理论值与名义值在工具坐标系下的位姿误差，为6行1列矩阵；表示第j个位置点的各关节及基坐标的误差由各自坐标系转换到工具坐标系的位姿误差转换矩阵；Q为基坐标系位姿误差值，关节误差值，工具坐标系位姿误差值组成的向量；

(2-10)则修正的基坐标转换到工具坐标系下的关系式为

其中，取自的前3行，表示第j个位置点激光跟踪仪的测量位置值x_j，y_j，z_j与机器人末端位置名义值在工具坐标系下的误差；取自的前3行，前39列，表示第j个位置点的各关节及基坐标的误差由各自坐标系转换到工具坐标系的位置误差转换矩阵；取自Q_(42×1)的前39列，为基坐标系位姿误差值，关节误差值，工具坐标系位置误差值组成的向量；

(2-11)将DH参数名义值代入基坐标转换坐标系的位姿dl，dm，dn，δθ，δψ初始值设为0，工具转换坐标系的位置do，dp，dq初始值设为0；

利用公式

计算选取的第4列前3行作为机器人末端位置名义值；

(2-12)将50个位置点的关节角数据θ₁至θ₆和50个位置点的测量位置数据x_j、y_j、z_j代入下式，即可以计算出误差向量Q^*；

根据最小二乘法有

Q^*＝(M^TM)^-1M^TE

将误差向量Q^*中的DH参数修正值加上原DH参数名义值作为新的DH参数，将误差向量Q^*中的基坐标位姿变量与工具坐标位置变量修正值加上原始值作为新的坐标变换；

当DH参数＞R时，转入步骤(2-1)，其中，R为修正阈值；

当DH参数≤R时，得到修正后的DH参数。

由于机器人控制器的原因，部分DH参数即使标定好后也无法写入机器人控制器，将无法写入机器人控制器的DH参数的误差值的系数全设为0，不对该参数进行标定。本发明实现了与实际情况相符的对部分DH参数的标定。

作为优选，R为10^-7至10^-13。

因此，本发明具有如下有益效果：标定精度高、速度快，可实现DH参数标定与零位标定。

附图说明

图1是本发明的机器人和激光跟踪仪的一种结构示意图；

图2为本发明的一种机器人坐标系建立过程示意图；

图3为本发明的一种流程图；

图4为本发明的一种标定前后绝对定位精度对比图。

图中：底座1、第1个关节2、第2个关节3、第3个关节4、第4个关节5、第5个关节6、第6个关节7、激光跟踪仪8、机器人9。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步的描述。

如图1所示的实施例是一种基于激光跟踪仪的工业机器人DH参数标定与零位标定方法，包括机器人9、机器人控制器、计算机和激光跟踪仪8；计算机分别与机器人控制器和激光跟踪仪电连接；图1中还包括底座1、第1个关节2、第2个关节3、第3个关节4、第4个关节5、第5个关节6和第6个关节7；图2为坐标系建立过程示意图；

如图3所示，包括如下步骤：

步骤100，机器人运行到50个位置点

选择机器人工作空间中的立方体，使机器人末端均匀的到达立方体中的50个位置点，安装于机器人夹具上的靶球在每个位置点处均朝向激光跟踪仪；

步骤200，计算机读取关节角，激光跟踪仪测量靶球位置

计算机通过机器人控制器读取机器人在50个位置点的关节角θ₁-θ₆的值，激光跟踪仪测量50个位置点的靶球位置x_j、y_j、z_j；

步骤300，计算机计算修正后的DH参数

计算机利用50个位置点的关节角θ₁至θ₆，靶球位置x_j、y_j、z_j，名义DH参数值计算修正后的DH参数；

将机器人通过底座安装到工作平台上，底座上设有机器人的六个关节；

步骤301，设定基坐标转换矩阵为

其中，为激光跟踪仪测量坐标系meas到机器人基坐标系base的转换矩阵，ψ为欧拉角绕测量系X轴旋转的角度，θ为欧拉角绕测量系Y轴旋转的角度，为欧拉角绕测量系Z轴旋转的角度，d1为转换矩阵沿X轴方向的位置平移，dm为转换矩阵沿Y轴方向的位置平移，dn为转换矩阵沿Z轴方向的位置平移，c为三角函数符号cos的缩写，s为三角函数符号sin的缩写；

步骤302，使用修正的DH模型对机器人进行建模，得到六个关节的转换矩阵

$A_i^{i-1}=\left[\begin{array}{cccc}{\mathrm{c\θ}}_i{\mathrm{c\β}}_i-{\mathrm{s\θ}}_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& -{\mathrm{s\θ}}_i{\mathrm{c\α}}_i& {\mathrm{c\θ}}_i{\mathrm{s\β}}_i+{\mathrm{s\θ}}_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& a_i{\mathrm{c\θ}}_i\\ {\mathrm{s\θ}}_i{\mathrm{c\β}}_i+{\mathrm{cos\θ}}_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& {\mathrm{c\θ}}_i{\mathrm{c\α}}_i& {\mathrm{s\θ}}_i{\mathrm{s\β}}_i-{\mathrm{c\θ}}_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& a_i{\mathrm{s\θ}}_i\\ -{\mathrm{c\α}}_i{\mathrm{s\β}}_i& {\mathrm{s\α}}_i& {\mathrm{c\α}}_i{\mathrm{c\β}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

其中，为i-1系到i系的转换矩阵，i＝1，2...6，0系与base系一致；DH参数包括第i个关节的关节角θ_i，第i个关节的连杆扭角α_i，第i个关节的偏距d_i，第i个关节的连杆长度a_i和避免转换矩阵奇异引入的第i个关节的扭角β_i；

步骤303，设定6系到工具坐标系too]的转换矩阵为

$A_{tool}^6=\left[\begin{array}{cccc}1& 0& 0& do\\ 0& 1& 0& dp\\ 0& 0& 1& dq\\ 0& 0& 0& 1\end{array}\right]$

其中，do为转换矩阵沿6系X轴的位置平移，dp为转换矩阵沿6系Y轴的位置平移，dq为转换矩阵沿6系Z轴的位置平移；

步骤304，设定基坐标系转换误差为

其中，为的误差值，δθ为θ的误差值，δψ为ψ的误差值，δ(dl)为dl的误差值，δ(dm)为dm的误差值，δ(dn)为dn的误差值；

步骤305，设定得到基坐标的位姿误差矢量：

其中，^basedx为base系下dx的误差值，^basedy为base系下dy的误差值，^basedz为base系下dz的误差值，^baseδx为base系下δx的误差值，^baseδy为base系下δy的误差值，^baseδz为base系下δz的误差值，dx为转换矩阵沿X轴方向的位置误差，dy为转换矩阵沿Y轴方向的位置误差，dz为转换矩阵沿Z轴方向的位置误差，δx为转换矩阵绕X轴方向的姿态误差，δy为转换矩阵绕Y轴方向的姿态误差，δz为转换矩阵绕Z轴方向的姿态误差；

步骤306，设定机器人的每个关节误差为

其中，δa_i为a_i的误差值，δd_i为d_i的误差值，δα_i为α_i的误差值，δθ_i为θ_i的误差值，δβ_i为β_i的误差值；

步骤307，设定得到关节坐标转换的位姿误差矢量为

$\left[\begin{array}{c}d_{i}x\\ d_{i}y\\ d_{i}z\\ \mathrm{\δ}_{i}x\\ \mathrm{\δ}_{i}y\\ \mathrm{\δ}_{i}z\end{array}\right]=\left[\begin{array}{ccccc}0& {\mathrm{c\β}}_i& 0& a_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& -{\mathrm{c\α}}_i{\mathrm{s\β}}_i\\ 0& 0& 0& a_i{\mathrm{c\α}}_i& {\mathrm{s\α}}_i\\ 0& {\mathrm{s\β}}_i& 0& -a_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& {\mathrm{c\α}}_i{\mathrm{c\β}}_i\\ {\mathrm{c\β}}_i& 0& 0& -{\mathrm{c\α}}_i{\mathrm{s\β}}_i& 0\\ 0& 0& 1& {\mathrm{s\α}}_i& 0\\ {\mathrm{s\β}}_i& 0& 0& {\mathrm{c\α}}_i{\mathrm{c\β}}_i& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ\α}}_i\\ {\mathrm{\δa}}_i\\ {\mathrm{\δ\β}}_i\\ {\mathrm{\δ\θ}}_i\\ {\mathrm{\δd}}_i\end{array}\right]=H_i{\mathrm{\Δq}}_i$

其中，ⁱdx为i系下dx的误差值，ⁱdy为i系下dy的误差值，ⁱdz为i系下dz的误差值，ⁱδx为i系下δx的误差值，ⁱδy为i系下δy的误差值，ⁱδz为i系下δz的误差值；

步骤308，设定误差传递关系为

$\left[\begin{array}{c}d_i{x}_{tool}\\ d_i{y}_{tool}\\ d_i{z}_{tool}\\ \mathrm{\δ}_i{x}_{tool}\\ \mathrm{\δ}_i{y}_{tool}\\ \mathrm{\δ}_i{z}_{tool}\end{array}\right]=\left[\begin{array}{cccccc}{n}_x& n_y& n_z& {(P\×n)}_x& {(P\×n)}_y& {(P\×n)}_z\\ o_x& o_y& o_z& {(P\×o)}_x& {(P\×o)}_y& {(P\×o)}_z\\ a_x& a_y& a_z& {(P\×a)}_x& {(P\×a)}_y& {(P\×a)}_z\\ 0& 0& 0& n_x& n_y& n_z\\ 0& 0& 0& o_x& o_y& o_z\\ 0& 0& 0& a_x& a_y& a_z\end{array}\right]\left[\begin{array}{c}d_{i}x\\ d_{i}y\\ d_{i}z\\ \mathrm{\δ}_{i}x\\ \mathrm{\δ}_{i}y\\ \mathrm{\δ}_{i}z\end{array}\right]={G_i}^i{e}_i$

其中， A为关节转换矩阵，P为误差传递矩阵的位置转换关系，n，o，a为误差传递矩阵的姿态转换关系，ⁱdx_tool为i系下的dx转换到tool系下的位置误差值，ⁱdy_tool为i系下的dy转换到tool系下的位置误差值，ⁱdz_tool为i系下的dz转换到tool系下的位置误差值，ⁱδx_tool为i系下的δx转换到tool系下的姿态误差值，ⁱδy_tool为i系下的δy转换到tool系下的姿态误差值，ⁱδz_tool为i系下的δz转换到tool系下的姿态误差值，(P×n)_x为向量P与向量n叉积结果的第一个元素，(P×n)_y为向量P与向量n叉积结果的第二个元素，(P×n)_z为向量P与向量n叉积结果的第三个元素，(P×o)_x为向量P与向量0叉积结果的第一个元素，(P×o)_y为向量P与向量o叉积结果的第二个元素，(P×o)_z为向量P与向量o叉积结果的第三个元素，(P×a)_x为向量P与向量a叉积结果的第一个元素，(P×a)_y为向量P与向量a叉积结果的第二个元素，(P×a)_z为向量P与向量a叉积结果的第三个元素；

步骤309，将基坐标转换的误差和每个关节的DH参数误差转换到工具坐标系下：

${\stackrel{j}{E}}_{(6\×1)}=\[G_0{H}_0,G_1{H}_1,G_2{H}_2,G_3{H}_3,G_4{H}_4,G_5{H}_5,G_6{H}_6,I\left(6\right)\]Q={\stackrel{j}{M}}_{(6\×42)}{Q}_{(42\×1)}$

其中，表示第j个位置点的理论值与名义值在工具坐标系下的位姿误差，为6行1列矩阵；表示第j个位置点的各关节及基坐标的误差由各自坐标系转换到工具坐标系的位姿误差转换矩阵；Q为基坐标系位姿误差值，关节误差值，工具坐标系位姿误差值组成的向量；

步骤310，则修正的基坐标转换到工具坐标系下的关系式为

其中，取自的前3行，表示第j个位置点激光跟踪仪的测量位置值x_j，y_j，z_j与机器人末端位置名义值在工具坐标系下的误差；取自的前3行，前39列，表示第j个位置点的各关节及基坐标的误差由各自坐标系转换到工具坐标系的位置误差转换矩阵；取自Q_(42×1)的前39列，为基坐标系位姿误差值，关节误差值，工具坐标系位置误差值组成的向量；

步骤311，将DH参数名义值代入基坐标转换坐标系的位姿初始值设为0，工具转换坐标系的位置do，dp，dq初始值设为0；

利用公式

计算选取的第4列前3行作为机器人末端位置名义值；

步骤312，将50个位置点的关节角数据θ₁至θ₆和50个位置点的测量位置数据x_j、y_j、z_j代入下式，即可以计算出误差向量Q^*；

根据最小二乘法有

Q*＝(M^TM)^-1M^TE

将误差向量Q^*中的DH参数修正值加上原DH参数名义值作为新的DH参数，将误差向量Q^*中的基坐标位姿变量与工具坐标位置变量修正值加上原始值作为新的坐标变换；

当DH参数＞10^-7时，转入步骤301；

当DH参数≤10^-7时，得到修正后的DH参数。

步骤400，设置机器人新的零位点

将机器人复位到理论值的零位，使机器人运动到θ参数误差角度处，将θ参数误差角度设为机器人新的零位点；

步骤500，将修正后的DH参数写到机器人控制器中

计算机将修正后的DH参数写到机器人控制器中，完成对机器人参数的补偿。

为验证本发明的可行性，使待标定的机器人末端运动至任意的50个点，记录下对应的关节θ值与激光跟踪仪的测量值，对DH参数进行标定，结果参见表1(a)和表1(b)。

校正前后的绝对定位误差见表2和图4。从见表2和图4的结果可以看出，机器人经过标定后，绝对定位精度提升了一个数量级，极大地提升了机器人的性能。

表1(a)DH参数名义值

a	d	theta	alpha	beta
					160	0	0	90	0
575	0	90	0	0
					130	0	0	90	0
0	644	0	-90	0
					0	0	0	90	0
0	109.5	90	0	0

表1(b)部分参数标定后DH参数修正值

表2参数标定前后绝对定位精度对比

应理解，本实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

Claims (3)

1.一种基于激光跟踪仪的工业机器人DH参数标定与零位标定方法，其特征是，包括机器人、机器人控制器、计算机和激光跟踪仪；计算机分别与机器人控制器和激光跟踪仪电连接；包括如下步骤：

(1-1)选择机器人工作空间中的立方体，使机器人末端均匀的到达立方体中的50个位置点，安装于机器人夹具上的靶球在每个位置点处均朝向激光跟踪仪；

(1-2)计算机通过机器人控制器读取机器人在50个位置点的关节角θ₁-θ₆的值，激光跟踪仪测量50个位置点的靶球位置x_j、y_j、z_j；

(1-3)计算机利用50个位置点的关节角θ₁至θ₆，靶球位置x_j、y_j、z_j，名义DH参数值计算修正后的DH参数；

(1-4)将机器人复位到理论值的零位，使机器人运动到θ参数误差角度处，将θ参数误差角度设为机器人新的零位点；

(1-5)计算机将修正后的DH参数写到机器人控制器中，完成对机器人参数的补偿。

2.根据权利要求1所述的基于激光跟踪仪的工业机器人DH参数标定与零位标定方法，其特征是，步骤(1-3)包括如下步骤：

将机器人通过底座安装到工作平台上，底座上设有机器人的六个关节；

(2-1)设定基坐标转换矩阵为

其中，为激光跟踪仪测量坐标系meas到机器人基坐标系base的转换矩阵，ψ为欧拉角绕测量系X轴旋转的角度，θ为欧拉角绕测量系Y轴旋转的角度，为欧拉角绕测量系Z轴旋转的角度，dl为转换矩阵沿X轴方向的位置平移，dm为转换矩阵沿Y轴方向的位置平移，dn为转换矩阵沿Z轴方向的位置平移，c为三角函数符号cos的缩写，s为三角函数符号sin的缩写；

(2-2)使用修正的DH模型对机器人进行建模，得到六个关节的转换矩阵

$A_i^{i-1}=\left[\begin{array}{cccc}{\mathrm{c\θ}}_i{\mathrm{c\β}}_i-{\mathrm{s\θ}}_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& -{\mathrm{s\θ}}_i{\mathrm{c\α}}_i& {\mathrm{c\θ}}_i{\mathrm{s\β}}_i+{\mathrm{s\θ}}_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& a_i{\mathrm{c\θ}}_i\\ {\mathrm{s\θ}}_i{\mathrm{c\β}}_i+{\mathrm{cos\θ}}_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& {\mathrm{c\θ}}_i{\mathrm{c\α}}_i& {\mathrm{s\θ}}_i{\mathrm{s\β}}_i-{\mathrm{c\θ}}_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& a_i{\mathrm{s\θ}}_i\\ -{\mathrm{c\α}}_i{\mathrm{s\β}}_i& {\mathrm{s\α}}_i& {\mathrm{c\α}}_i{\mathrm{c\β}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]$

其中，为i-1系到i系的转换矩阵，i＝1，2...6，0系与base系一致；DH参数包括第i个关节的关节角θ_i，第i个关节的连杆扭角α_i，第i个关节的偏距d_i，第i个关节的连杆长度a_i和避免转换矩阵奇异引入的第i个关节的扭角β_i；

(2-3)设定6系到工具坐标系tool的转换矩阵为

$A_{tool}^6=\left[\begin{array}{cccc}1& 0& 0& do\\ 0& 1& 0& dp\\ 0& 0& 1& dq\\ 0& 0& 0& 1\end{array}\right]$

其中，do为转换矩阵沿6系X轴的位置平移，dp为转换矩阵沿6系Y轴的位置平移，dq为转换矩阵沿6系Z轴的位置平移；

(2-4)设定基坐标系转换误差为

其中，为的误差值，δθ为θ的误差值，δψ为ψ的误差值，δ(dl)为dl的误差值，δ(dm)为dm的误差值，δ(dn)为dn的误差值；

(2-5)设定得到基坐标的位姿误差矢量：

其中，^basedx为base系下dx的误差值，^basedy为base系下dy的误差值，^basedz为base系下dz的误差值，^baseδx为base系下δx的误差值，^baseδy为base系下δy的误差值，^baseδz为base系下δz的误差值，dx为转换矩阵沿X轴方向的位置误差，dy为转换矩阵沿Y轴方向的位置误差，dz为转换矩阵沿Z轴方向的位置误差，δx为转换矩阵绕X轴方向的姿态误差，δy为转换矩阵绕Y轴方向的姿态误差，δz为转换矩阵绕Z轴方向的姿态误差；

(2-6)设定机器人的每个关节误差为

其中，δa_i为α_i的误差值，δd_i为d_i的误差值，δα_i为α_i的误差值，δθ_i为θ_i的误差值，δβ_i为β_i的误差值；

(2-7)设定得到关节坐标转换的位姿误差矢量为

$\left[\begin{array}{c}d_{i}x\\ d_{i}y\\ d_{i}z\\ \mathrm{\δ}_{i}x\\ \mathrm{\δ}_{i}y\\ \mathrm{\δ}_{i}z\end{array}\right]=\left[\begin{array}{ccccc}0& {\mathrm{c\β}}_i& 0& a_i{\mathrm{s\α}}_i{\mathrm{s\β}}_i& -{\mathrm{c\α}}_i{\mathrm{s\β}}_i\\ 0& 0& 0& a_i{\mathrm{c\α}}_i& {\mathrm{s\α}}_i\\ 0& {\mathrm{s\β}}_i& 0& -a_i{\mathrm{s\α}}_i{\mathrm{c\β}}_i& {\mathrm{c\α}}_i{\mathrm{c\β}}_i\\ {\mathrm{c\β}}_i& 0& 0& -{\mathrm{c\α}}_i{\mathrm{s\β}}_i& 0\\ 0& 0& 1& {\mathrm{s\α}}_i& 0\\ {\mathrm{s\β}}_i& 0& 0& {\mathrm{c\α}}_i{\mathrm{c\β}}_i& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ\α}}_i\\ {\mathrm{\δa}}_i\\ {\mathrm{\δ\β}}_i\\ {\mathrm{\δ\θ}}_i\\ {\mathrm{\δd}}_i\end{array}\right]=H_i{\mathrm{\Δq}}_i$

其中，ⁱdx为i系下dx的误差值，ⁱdy为i系下dy的误差值，ⁱdz为i系下dz的误差值，ⁱδx为i系下δx的误差值，ⁱδy为i系下δy的误差值，ⁱδz为i系下δz的误差值；

(2-8)设定误差传递关系为

$\left[\begin{array}{c}d_i{x}_{tool}\\ d_i{y}_{tool}\\ d_i{z}_{tool}\\ \mathrm{\δ}_i{x}_{tool}\\ \mathrm{\δ}_i{y}_{tool}\\ \mathrm{\δ}_i{z}_{tool}\end{array}\right]=\left[\begin{array}{cccccc}{n}_x& n_y& n_z& {(P\×n)}_x& {(P\×n)}_y& {(P\×n)}_z\\ o_x& o_y& o_z& {(P\×o)}_x& {(P\×o)}_y& {(P\×o)}_z\\ a_x& a_y& a_z& {(P\×a)}_x& {(P\×a)}_y& {(P\×a)}_z\\ 0& 0& 0& n_x& n_y& n_z\\ 0& 0& 0& o_x& o_y& o_z\\ 0& 0& 0& a_x& a_y& a_z\end{array}\right]\left[\begin{array}{c}d_{i}x\\ d_{i}y\\ d_{i}z\\ \mathrm{\δ}_{i}x\\ \mathrm{\δ}_{i}y\\ \mathrm{\δ}_{i}z\end{array}\right]={G_i}^i{e}_i$

其中， A为关节转换矩阵，P为误差传递矩阵的位置转换关系，n，o，a为误差传递矩阵的姿态转换关系，ⁱdx_tool为i系下的dx转换到tool系下的位置误差值，ⁱdy_tool为i系下的dy转换到tool系下的位置误差值，ⁱdz_tool为i系下的dz转换到tool系下的位置误差值，ⁱδx_tool为i系下的δx转换到tool系下的姿态误差值，ⁱδy_tool为i系下的δy转换到tool系下的姿态误差值，ⁱδz_tool为i系下的δz转换到tool系下的姿态误差值，(P×n)_x为向量P与向量n叉积结果的第一个元素，(P×n)_y为向量P与向量n叉积结果的第二个元素，(P×n)_z为向量P与向量n叉积结果的第三个元素，(P×o)_x为向量P与向量o叉积结果的第一个元素，(P×o)_y为向量P与向量o叉积结果的第二个元素，(P×o)_z为向量P与向量o叉积结果的第三个元素，(P×a)_x为向量P与向量a叉积结果的第一个元素，(P×a)_y为向量P与向量a叉积结果的第二个元素，(P×a)_z为向量P与向量a叉积结果的第三个元素；

(2-9)将基坐标转换的误差和每个关节的DH参数误差转换到工具坐标系下：

${\stackrel{j}{E}}_{(6\×1)}=\[G_0{H}_0,G_1{H}_1,G_2{H}_2,G_3{H}_3,G_4{H}_4,G_5{H}_5,G_6{H}_6,I\left(6\right)\]Q={\stackrel{j}{M}}_{(6\×42)}{Q}_{(42\×1)}$

其中，表示第j个位置点的理论值与名义值在工具坐标系下的位姿误差，为6行1列矩阵；表示第j个位置点的各关节及基坐标的误差由各自坐标系转换到工具坐标系的位姿误差转换矩阵；Q为基坐标系位姿误差值，关节误差值，工具坐标系位姿误差值组成的向量；

(2-10)则修正的基坐标转换到工具坐标系下的关系式为

其中，取自的前3行，表示第j个位置点激光跟踪仪的测量位置值x_j，y_j，z_j与机器人末端位置名义值在工具坐标系下的误差；取自的前3行，前39列，表示第j个位置点的各关节及基坐标的误差由各自坐标系转换到工具坐标系的位置误差转换矩阵；取自Q_(42×1)的前39列，为基坐标系位姿误差值，关节误差值，工具坐标系位置误差值组成的向量；

(2-11)将DH参数名义值代入基坐标转换坐标系的位姿dl，dm，dn，δθ，δψ初始值设为0，工具转换坐标系的位置do，dp，dq初始值设为0；

利用公式

计算选取的第4列前3行作为机器人末端位置名义值；

(2-12)将50个位置点的关节角数据θ₁至θ₆和50个位置点的测量位置数据x_j、y_j、z_j代入下式，即可以计算出误差向量Q^*；

根据最小二乘法有

Q^*＝(M^TM)^-1M^TE

将误差向量Q^*中的DH参数修正值加上原DH参数名义值作为新的DH参数，将误差向量Q^*中的基坐标位姿变量与工具坐标位置变量修正值加上原始值作为新的坐标变换；

当DH参数>R时，转入步骤(2-1)，其中，R为修正阈值；

当DH参数≤R时，得到修正后的DH参数。

3.根据权利要求2所述的基于激光跟踪仪的工业机器人DH参数标定与零位标定方法，其特征是，R为10^-7至10^-13。