一种工业机器人工具坐标系及零点自标定方法与流程

1.本发明涉及机器人定位系统技术领域，特别涉及一种工业机器人工具坐标系及零点自标定方法。


背景技术：

2.经过了长时间的技术更新、产品升级，工业机器人成为了集机电一体化、精密传感器、计算机、人工智能等多学科技术于一体的高度集成自动化设备，是现代重要的加工运输装备。
3.工业机器人绝对定位精度决定了离线编程技术应用的效果与范围。在现场应用过程中，由于不同加工任务的需求不同，需要更换加工工具，或由于加工的作用力造成工具的变形或偏移，使得工业机器人控制器中的工具坐标系参数发生了变化，其中最主要的是tcp(工具中心点)相对于末端法兰中心点的相对位置发生变化，使得无法准确控制tcp的位置。
4.为了提升机器人的绝对定位精度，需要通过tcp标定准确计算得到tcp参数的高精确值。现有技术中，tcp参数的标定方法可分为两类：一是基于外部基准，通过高精密的测量仪器测量标定；二是机器人的自标定方法。外部基准法需要安装高精度的测量装置于特定位置，例如借助球杆仪、坐标测量机、自动经纬仪、激光跟踪仪或机器视觉等，成本较高，无法广泛使用。自标定法降低了对测量设备的要求，标定过程效率高，结果一般能满足工业精度要求，因此广泛应用于加工现场。传统的自标定方法虽然计算简单且效率高，但是应用条件较为苛刻，在许多情况下无法使用，尤其是机器人关节零点丢失时，传统的自标定方法所得的结果都会失效而无法使用。


技术实现要素：

5.本发明目的在于提供一种工业机器人工具坐标系及零点自标定方法，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。
6.为解决上述技术问题所采用的技术方案：
7.一种工业机器人工具坐标系及零点自标定方法，包括以下步骤：
8.步骤s1：建立机器人基坐标系{b}、工具坐标系{t}和空间对齐点世界坐标系{u}，从而建立工业机器人的dh运动学模型；
9.步骤s2：调整工业机器人各个关节角的角度使得机器人工具末端与空间同一个点对齐n次，n大于或等于4，并记录每一次对齐时的各关节角值；
10.步骤s3：建立{b}参数的误差模型；
11.步骤s4：分析误差模型系数矩阵的病态程度，去除矩阵中线性相关列并对该矩阵进行优化；
12.步骤s5：求解{b}参数误差模型，并对相应参数初步辨识并补偿；
13.步骤s6：建立{b}和tcp参数的误差模型并执行步骤s4，求解 {b}和tcp参数的误差模型，并对相应参数补偿；
14.步骤s7：建立{b}、tcp和零点的参数误差模型并执行步骤s4，进行参数误差辨识并补偿，对{b}、tcp和零点参数进行精确标定。
15.特别地，所述的dh模型是一套建立关节坐标系的规范，依靠这个规范和关节的参数，能够推导出基坐标系到末端坐标系的变换矩阵。在各个关节的变量已知的情况下，可以得到末端坐标系的位置和姿态，即正解；反之，知道末端坐标系的位置和姿态，也可求得所需的关节变量的值，即逆解。
16.本发明所提供的工业机器人工具坐标系及零点自标定方法，至少具有如下的有益效果：实际操作中，本自标定方法操作简单，工程人员上手快，只需要使用控制器控制机器人作姿态变换，进行尖点对齐即可；而且成本低，不需要实用昂贵的设备配合标定；整个标定流程较其他标定算法耗时更短；在对tcp进行标定的同时，还能够寻回加工现场因各种原因丢失的机器人零点，不需要返厂重新标定。本发明的工业机器人工具坐标系及零点自标定方法，分阶段进行机器人基坐标系、tcp参数以及零点的标定。
17.作为上述技术方案的进一步改进，完成步骤s2后，可直接执行步骤s6，即直接对{b}和tcp参数进行初步标定。直接对{b}和tcp 参数进行初步标定，效率更高；而{b}和tcp参数分阶段进行参数标定，可以将机器人的其他微小的参数偏差通过标定过程进行补偿修正，进一步地提高工业机器人的加工精度。
18.作为上述技术方案的进一步改进，所述步骤s1具体地：
19.在机器人工具末端与空间同一个点对齐的情况下，{u}和{t}是重合的；并且{b}在一次标定过程中相对{u}的位置姿态都是不变的，因此可以设定{b}与{t}姿态相同，{u}变换到{t}的欧拉角参数固定为 [0,0,0]
t
，初始化待辨识参数关节角偏移量δθ
i
(i＝1,2,3,4,5,6)为0，待辨识参数{u}到{b}坐标位移变换参数[b
x
,b
y
,b
z
]
t
和tcp参数[t
x
,t
y
,t
z
]
t
为 [0,0,0]
t
；
[0020]
按照d
‑
h方法建立连杆坐标系，以各连杆的结构参数和运动参数得出d
‑
h参数表，根据d
‑
h参数表和连杆坐标系，可将机器人运动学方程描述为{b}到{t}的齐次变换矩阵：
[0021][0022]
上式中表示第{i
‑
1}个连杆坐标系相对于第{i}个连杆的齐次变换矩阵，即：
[0023][0024]
上式中的cθ
i
是cos(θ
i
)的缩写，sθ
i
是sin(θ
i
)的缩写，以此类推；
[0025]
将各个连杆的变换矩阵相乘，便可得到关于关节变量θ1、θ2、θ3、θ4、θ5、θ6的函数，即机器人的手臂变换矩阵。
[0026]
作为上述技术方案的进一步改进，所述步骤s3具体地：用表示表示和分别表示连杆坐标系{i}向连杆坐标系{i 1}的实际变换与理论变换，表示连杆坐标系{i}的微分变换，可以表示为：
[0027][0028]
连杆i的预辨识模型为：
[0029][0030]
d(a
i
)可以由的泰勒一阶展开近似求得：
[0031][0032]
综上可得：
[0033][0034]
又：
[0035][0036]
上式中，dx,dy,dz分别表示x、y和z轴方向的位置微分变量，δ
x
,δ
y
,δ
z
分别为绕x、y和z轴的微分旋转变量；
[0037]
微分运动矢量e
i
构成了e
i
的前3个元素为位置误差d，后3 个元素为姿态误差δ，矢量e
i
为：
[0038]
e
i
＝[
i
dx,
i
dy,
i
dz,
i
δ
x
,
i
δ
y
,
i
δ
z
]
t
[0039]
所以dh模型的e
i
为：
[0040][0041]
用几何实际模型误差δx
i
＝[δθ
i
]
t
的形式表示为：
[0042][0043]
上式中，g
i
为误差的系数矩阵，e
i
代表连杆i的参数偏差于这根连杆末端造成的误差；
[0044]
将a用一般形式的齐次变换矩阵表示，并分别对参数b
x
，b
y
，b
z
和t
x
，t
y
，t
z
进行泰勒展开，列出线性方程：
[0045][0046][0047]
由连杆i的e
i
而引起的末端执行器的误差，用微分变换矩阵表示为：
[0048][0049]
又：
[0050][0051]
即：
[0052][0053]
上式中，微分运动矢量e
i
表示{i}参数偏差造成{i}末端的位姿误差；代表从{i}到{n}的微分变换矩阵；微分运动矢量表示{i}参数偏差造成机器人工具末端点的位姿误
差；
[0054]
机器人末端执行器的总误差e是由每根连杆i的误差e
i
传递到末端坐标系下的误差之和，即：
[0055][0056]
取e的前三行即位置微分变量，进行标定计算。
[0057]
作为上述技术方案的进一步改进，所述步骤s4具体地：通过奇异值分解对矩阵进行线性相关变量剔除处理：
[0058]
令j＝h
t
h并使用奇异值分解，得：
[0059][0060]
其中u，v是正交矩阵，∑＝diag(σ1，σ2...σ
r
，)(r<＝12)，r是矩阵h的秩。因此，冗余参数的数量为12
‑
r；
[0061][0062]
由于矩阵j的对称性，使得x中的线性相关参数可以确定，并且可以通过对矩阵的基本行变换进行数学运算来删除最后12
‑
r行。因此，该系数矩阵的r等于11，这意味着只有1个冗余参数。该冗余参数与其他参数的相关性如下所示：
[0063]
δθ6＝k1·
δt
x
k2·
δt
y
(k1，k2∈r)
[0064]
因此剔除待辨识参数δθ6。
[0065]
由于标定实验的特殊性，{t}与{b}位移固定不变，因此可设{t} 与{b}位移的半径长度为r，在δθ1较小时存在：
[0066][0067]
将上式于(a，b)处线性化得：
[0068][0069]
可知δθ1与其他待辨识参数存在局部高线性相关关系，因此剔除待辨识参数δθ1。
[0070]
通过系数矩阵的奇异值分解以及几何关系分析去除冗余参数，能够有效识别参数，剔除线性相关变量，以减小矩阵维数并获得稳定的特征向量。优化后的矩阵病态程度大大降低，而且提高了扩展卡尔曼滤波的稳定性、加快了收敛速度。
[0071]
作为上述技术方案的进一步改进，对奇异值分解后的矩阵，采取对列向量单位化后再辨识的方法再进行优化：
[0072]
记h＝(a
ij
)
m
×
n
，α
j
＝[a
1j
a
2j
…
a
nj
]
t
，将列向量进行单位化：
[0073]
[0074]
可以得到新的矩阵并且可以表示为：
[0075][0076]
其中：
[0077][0078]
上式中，
[0079][0080]
令式一更新为：
[0081][0082]
用最小二乘法解出后，再计算并将其补偿给相应待辨识参数。
[0083]
再次优化后的矩阵的条件数大大降低，提高了最小二乘法的稳定性和扩展卡尔曼滤波算法的收敛性。
[0084]
作为上述技术方案的进一步改进，所述步骤s5、步骤s6和步骤 s7具体地，采用迭代最小二乘法对误差模型的进行求解，根据最小二乘原理，方程存在唯一最小二乘解为：x＝(h
t
h)
‑1h
t
e。
[0085]
在上述技术方案的基础上进一步地，所述自标定方法还包括步骤 s8：对标定后的参数进行噪声消除降低过拟合程度。在对应的n组数据的基础上，对补偿后的参数用扩展卡尔曼滤波消除对其和其他过程中的高斯噪声的影响，具体地包括如下步骤：
[0086]
步骤s8.1：以k|k
‑
1代表预测估计，以k|k代表在测量点k的后验估计，建立用于参数识别的ekf算法的方程式：
[0087]
x
k|k
‑1＝x
k
‑
1|k
‑1[0088]
p
k|k
‑1＝p
k
‑
1|k
‑1 q
k
‑1[0089]
所述方程式中，x
k|k
‑1代表参数在测量点k的偏差，x
k
‑
1|k
‑1是x
k|k
‑1的预估值；p
k|k
‑1是参数在测量点k的预测估计误差的协方差矩阵， p
k
‑
1|k
‑1是后验估计误差的协方差矩阵，q
k
‑1是第(k
‑
1)次迭代时系统噪声的协方差矩阵；
[0090]
点k处的位置误差y
k
可以表达为：
[0091]
y
k
＝h
k
x
k
e
k
[0092]
上式中，第k次迭代时x
k
和矩阵h
k
的大小分别为10
×
1和3
×
10； e
k
表示第k次迭代时的测量误差的协方差矩阵；
[0093]
步骤s8.2：根据步骤s8.1中的方程式，最佳卡尔曼增益可以表示为：
[0094][0095]
上式中，r
k
表示第k次迭代时的测量噪声的协方差矩阵；
[0096]
步骤s8.3：计算第k次迭代中x的递归识别方程：
[0097][0098]
x
k|k
＝x
k|k
‑1 k
k
e
k
[0099]
更新协方差矩阵：
[0100]
p
k|k
＝(i
‑
k
k
h
k
)p
k|k
‑1[0101]
上述各式中，i是单位矩阵，矩阵q和p都由10
‑4i
10
初始化，r 则由10
‑4i3初始化；
[0102]
实际参数x
r
的表达式：
[0103]
x
r
＝x
n
x
[0104]
即实际参数x
r
通过将名义参数x
n
的值与辨识得到的参数偏差x 相加得出。
[0105]
对尖过程通过人手操作，对尖结果难免会受到各种噪声的干扰，在这种情况下，使用最小二乘估计可能过度拟合，即用于计算的采样点的补偿精度提高了，而其余采样点的精度却降低了。用扩展卡尔曼滤波消除补偿后的参数以及其他过程中的高斯噪声的影响，降低过拟合程度。
附图说明
[0106]
下面结合附图和实施例对本发明做进一步的说明；
[0107]
图1是本发明所提供的工业机器人工具坐标系及零点自标定方法，其一实施例的算法流程图；
[0108]
图2是本发明所提供的工业机器人工具坐标系及零点自标定方法，其一实施例的运动模型坐标系示意图；
[0109]
图3是本发明所提供的工业机器人工具坐标系及零点自标定方法，其一实施例的连杆坐标系示意图；
[0110]
图4是本发明所提供的工业机器人工具坐标系及零点自标定方法，其一实施例的连杆误差传递示意图；
[0111]
图5是本发明所提供的工业机器人工具坐标系及零点自标定方法，其一实施例的{t}与{b}参数的几何关系示意图。
具体实施方式
[0112]
本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案，但其不能理解为对本发明保护范围的限制。
[0113]
在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0114]
在本发明的描述中，如果具有“若干”之类的词汇描述，其含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。
[0115]
本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所
属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。
[0116]
参照图1至图5，本发明的工业机器人工具坐标系及零点自标定方法作出如下实施例：
[0117]
一种工业机器人工具坐标系及零点自标定方法：
[0118]
首先，建立机器人基坐标系{b}、工具坐标系{t}和空间对齐点世界坐标系{u}，从而建立工业机器人的dh运动学模型。
[0119]
参照图2，在机器人工具末端与空间同一个点对齐的情况下，{u} 和{t}是重合的，并且{b}在一次标定过程中相对{u}的位置姿态都是不变的。因此可以设定{b}与{t}姿态相同，{u}变换到{t}的欧拉角参数固定为[0,0,0]
t
。初始化待辨识参数关节角偏移量δθ
i
(i＝1,2,3,4,5,6)为0，待辨识参数{u}到{b}坐标位移变换参数[b
x
,b
y
,b
z
]
t
和tcp参数 [t
x
,t
y
,t
z
]
t
为[0,0,0]
t
。
[0120]
以hsr_br606型工业机器人为例，按照d
‑
h方法建立连杆坐标系如图3所示，各连杆的结构参数和运动参数得出dh参数表如下所示：
[0121][0122]
结合上表以及连杆坐标系，可得出机器人运动学方程可描述为{b} 到{t}的齐次变换矩阵：
[0123][0124]
上式中表示第{i
‑
1}个连杆坐标系相对于第{i}个连杆的齐次变换矩阵，即：
[0125][0126]
上式中的cθ
i
是cos(θ
i
)的缩写，sθ
i
是sin(θ
i
)的缩写，以此类推。
[0127]
将各个连杆的变换矩阵相乘，便可得到关于关节变量θ1、θ2、θ3、θ4、θ5、θ6的函数，即
机器人的手臂变换矩阵。
[0128]
然后，调整工业机器人各个关节角的角度使得机器人工具末端与空间同一个点对齐n次，并记录每一次对齐时的各关节角值。特别地，为满足所有设定参数的求解，对尖次数n大于或等于4次。考虑到对尖操作时间和运算时间，为提高标定精度，实际应用中对尖次数n以15至25次为佳。在本实施例中，对尖次数n为20次。
[0129]
对相应的待标定参数建立预辨识模型，亦即是对应参数的误差模型。由于机器人dh参数的a
i
，α
i
，d
i
是准确的，只是零点丢失造成θ
i
存在误差。用表示表示和分别表示连杆坐标系{i}向连杆坐标系{i 1}的实际变换与理论变换，表示连杆坐标系{i}的微分变换，可以表示为：
[0130][0131]
连杆i的预辨识模型为：
[0132][0133]
d(a
i
)可以由的泰勒一阶展开近似求得：
[0134][0135]
综上可得：
[0136][0137]
又：
[0138][0139]
上式中，dx,dy,dz分别表示x、y和z轴方向的位置微分变量，δ
x
,δ
y
,δ
z
分别为绕x、y和z轴的微分旋转变量。
[0140]
微分运动矢量e
i
构成了e
i
的前3个元素为位置误差d，后3 个元素为姿态误差δ，矢量e
i
为：
[0141]
e
i
＝[
i
dx,
i
dy,
i
dz,
i
δ
x
,
i
δ
y
,
i
δ
z
]
t
[0142]
所以dh模型的e
i
为：
[0143][0144]
用几何实际模型误差δx
i
＝[δθ
i
]
t
的形式表示为：
[0145][0146]
上式中，g
i
为误差的系数矩阵，e
i
代表连杆i的参数偏差于这根连杆末端造成的误差。
[0147]
将a用一般形式的齐次变换矩阵表示，并分别对参数b
x
，b
y
，b
z
和t
x
，t
y
，t
z
进行泰勒展开，列出线性方程：
[0148][0149][0150]
一般标定中测量的对象不是每根连杆的末端，而是工业机器人的末端执行器坐标系，测出的误差数据是所有连杆参数偏差于末端执行器综合而表示出来的误差。所以，必须将每根连杆的误差变换到工业机器人的末端执行器上，这个过程被称为误差传递。
[0151]
如图4所示，假设从连杆i的坐标系到末端执行器要经历a
i 1
、 a
i 2
至an
‑
1、an的变换，机器人任一连杆的参数偏差而引起末端位姿的误差是由其后面的连杆进行该误差的传递。机器人动力学中的雅克比矩阵建立了从关节速度向操作速度的映射关系，又称为微分变换矩阵，这里可以表示误差传递。由连杆i的e
i
而引起的末端执行器的误差，用微分变换矩阵表示为：
[0152][0153]
又：
[0154][0155]
即：
[0156][0157]
上式中，微分运动矢量e
i
表示{i}参数偏差造成{i}末端的位姿误差，代表从{i}到{n}的微分变换矩阵；微分运动矢量表示{i}参数偏差造成机器人工具末端点的位姿误差；
[0158]
机器人末端执行器的总误差e是由每根连杆i的误差e
i
传递到末端坐标系下的误差之和，即：
[0159][0160]
上式下称为“式一”；
[0161]
取e的前三行即位置微分变量，进行标定计算。
[0162]
tcp参数误差模型的建立和{u}转换到{b}的位移误差模型的建立，与e
i
的误差模型建立方法相同。
[0163]
各参数的误差模型均为线性模型，需要标定{b}三个参数、{t}三个参数、θ六个参数共12个参数。对各参数进行标定的时候需取点较多从而构建超定方程组，再进行求解。本实施例中，采用迭代最小二乘法求解，根据最小二乘原理，方程存在唯一最小二乘解为：
[0164]
x＝(h
t
h)
‑1h
t
e
[0165]
在进行求解之前要对矩阵进行线性相关变量剔除处理。系数矩阵 h的冗余分析，可以避免求解方程式(如式一等)的奇异性问题；svd，即奇异值分解，是一种非常有效的参数识别方法，可以减小矩阵维数并获得稳定的特征向量。
[0166]
令j＝h
t
h并使用奇异值分解，得：
[0167][0168]
其中u，v是正交矩阵，∑＝diag(σ1，σ2...σ
r
，)(r<＝12)，r是矩阵h的秩。因此，冗余参数的数量为12
‑
r。
[0169][0170]
由于矩阵j的对称性，使得x中的线性相关参数可以确定，并且可以通过对矩阵的基本行变换进行一些数学运算来删除最后12
‑
r行。因此，该系数矩阵的r等于11，这意味着只有1个冗余参数。该冗余参数与其他参数的相关性如下所示：
[0171]
δθ6＝k1·
δt
x
k2·
δt
y
(k1，k2∈r)
[0172]
因此剔除待辨识参数δθ6，由于标定实验的特殊性，使得{t}与{b} 位移固定不变，因此存在如图5所示的几何关系，设{t}与{b}位移即图中的实线圆的半径长度为r，在δθ1较小时存在：
[0173][0174]
将上式于(a，b)处线性化得：
[0175][0176]
可知δθ1与其他待辨识参数存在局部高线性相关关系，因此剔除待辨识参数δθ1。
[0177]
图1为标定流程图。如图1所示，case的值为1或2时，均没有标定δθ
i
参数，不存在冗余参数。case的值为1时，标定{b}三个参数。 case的值为2时，标定{b}和{t}共六个参数。case的值为3时，需要去除线性相关参数δθ1和δθ6，标定{b}和{t}，以及θ的四个参数，共十个参数。
[0178]
此外，由于角度和位移的单位以及数量级不同，因此对奇异值分解后的h矩阵，可以采取了对列向量单位化后再辨识的方法进行优化，提高最小二乘法的稳定性和扩展卡尔曼滤波算法的收敛性。
[0179]
记h＝(a
ij
)
m
×
n
，α
j
＝[a
1j
a
2j
…
a
nj
]
t
，将列向量进行单位化：
[0180][0181]
可以得到新的矩阵并且可以表示为：
[0182][0183]
其中：
[0184][0185]
上式中，
[0186]
[0187]
令式一更新为：
[0188][0189]
用最小二乘法解出后，再计算并将其补偿给相应待辨识参数。
[0190]
在对应的20组数据的基础上，对补偿后的参数用扩展卡尔曼滤波消除对其和其他过程中的高斯噪声的影响，降低过拟合程度：
[0191]
以k|k
‑
1代表预测估计，以k|k代表在测量点k的后验估计，建立用于参数识别的ekf算法的方程式：
[0192]
x
k|k
‑1＝x
k
‑
1|k
‑1[0193]
p
k|k
‑1＝p
k
‑
1|k
‑1 q
k
‑1[0194]
所述方程式中，x
k|k
‑1代表参数在测量点k的偏差，x
k
‑
1|k
‑1是x
k|k
‑1的预估值；p
k|k
‑1是参数在测量点k的预测估计误差的协方差矩阵， p
k
‑
1|k
‑1是后验估计误差的协方差矩阵，q
k
‑1是第(k
‑
1)次迭代时系统噪声的协方差矩阵；
[0195]
点k处的位置误差y
k
可以表达为：
[0196]
y
k
＝h
k
x
k
e
k
[0197]
上式中，第k次迭代时x
k
和矩阵h
k
的大小分别为10
×
1和3
×
10； e
k
表示第k次迭代时的测量误差的协方差矩阵；
[0198]
最佳卡尔曼增益可以表示为：
[0199][0200]
上式中，r
k
表示第k次迭代时的测量噪声的协方差矩阵；
[0201]
计算第k次迭代中x的递归识别方程：
[0202][0203]
x
k|k
＝x
k|k
‑1 k
k
e
k
[0204]
更新协方差矩阵：
[0205]
p
k|k
＝(i
‑
k
k
h
k
)p
k|k
‑1[0206]
上述各式中，i是单位矩阵，矩阵q和p都由10
‑4i
10
初始化，r 则由10
‑4i3初始化；
[0207]
实际参数x
r
的表达式：
[0208]
x
r
＝x
n
x
[0209]
即实际参数x
r
通过将名义参数x
n
的值与辨识得到的参数偏差x 相加得出。
[0210]
此外，本发明还可以使用如：最大似然估计，levenberg
‑
marquardf 算法，模拟退火算法，粒子滤波等的其他辨识方法。本发明使用的是迭代最小二乘法和去除高斯噪声的扩展卡尔曼滤波法，能够兼顾效率和稳定性，具有很强的实用性和代表性。
[0211]
以上对本发明的较佳实施方式进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变型或替换，这些等同的变型或替换均包含在本技术权利要求所限定的范围内。