CN106092009B - 一种基于拉线编码器的站点可动式测量机构的初始位置校准方法 - Google Patents

Abstract

一种基于拉线编码器的站点可动式测量机构的初始位置校准方法，为形象化四站式布局，对总体的测量结构进行初步建模；将表示机器人各个关节之间坐标系的D‑H模型推广到测量模块四站式布局站点之间的坐标变换，并且引进α、d1、d2、β四个参数来表示四个站点之间的坐标变换；建立四站坐标系数学模型；对上述测量结构建立的D‑H模型进行求解，得出初始位置校准几何模型中的四个未知量，从而确定各个站点之间的相互位置关系，确保测量机构初始位置的精度。该算法效率高，成本较低并且环境适应性强，应用范围广，易于实现，操作方便。

Description

一种基于拉线编码器的站点可动式测量机构的初始位置校准 方法

技术领域

本发明属于工业机器人技术领域,涉及一种基于拉线编码器的站点可动式初始位置校准算法。

背景技术

目前，随着机器人技术的快速发展，工业机器人已经被广泛引用于各个行业，在航空业，传统制造业，电子信息业等行业，需要工业机器人能够按照精确的轨迹来完成所要完成的任务。

经过标定的工业机器人能够在允许的误差范围内到达指定点，但是在离线编程，电弧焊等任务中，经常需要工业机器人能够精确的按照一定的轨迹来完成任务。

高精度，响应速度快的测量系统对机器人运动轨迹的测量具有重要的意义.但是，现有的三维测量设备价格比较昂贵，同时很多测量设备的可移动性差。

多站法测量原理是采用只需测量长度数据的布局方案，可以全部使用长度数据来求解坐标值，很好的避免了大范围距离角度误差引起的精度退化问题。

在专利公开号为CN103486989A的拉线式空间位置测量机构中，由于测量机构的四个站点是固定式，即在测量过程中不能移动该测量机构的四个站点，否则将导致之前测量的数据失效。所以，该测量机构的测量方式的最大的缺点就是站点固定，使其大大受限于测量时的环境，提高了测量时的难度，所以为保证测量精度，并且提高该测量机构的环境适应性，使该测量机构能更好的适用于不用行业，不同环境的测量当中。本发明在测量机构的基础上进行了改进，发明了一种站点可动式的测量机构，并且对该可动式的测量机构进行初始位置校准的算法。只需要在测量前对测量的基站进行初始位置校准即可保证其测量机构的测量精度。

基于拉线编码器的空间位置测量机构属于四站式布局。采用多站法的测量具有高效、方便、成本低等优点，为分析机器人性能而进行初始位置的校准是保证系统测量精度的关键。

发明内容

本发明的目的是针对初始位置的校准问题，提出一种基于拉线编码器的站点可动式测量机构的初始位置校准方法。

本发明的技术方案是：

一种基于拉线编码器的站点可动式测量机构的初始位置校准方法，它采用站点可动式测量机构，它包括两个固定装置，各固定装置上安装有两个拉线编码器，即测量机构的两个站点，两个固定装置上的两个站点之间的位置固定；该校准方法包括以下步骤：

S1、对可动式测量机构的四个站点进行初步建模：

设下左、下右、上左、上右四个站点编号分别为站点1—站点4，设站点1为参考坐标系T1，站点2的坐标系为T2，站点3的坐标系为T3，站点4的坐标系为T4，站点1—站点4的基准点分别表示各站点滑轮的下切点，记为O1-O4，以站点1的基准点O1指向站点2的基准点O2作为参考坐标系的X1轴，以站点1的基准点O1指向站点3的基准点O3作为参考坐标系的Y1轴，将以基准点O1为原点，垂直于X1轴、Y1轴所在平面的直线作为Z1轴；

S2、对四站式布局的站点1-4建立D-H模型，站点2与站点1固连，站点2、3、4的坐标系与站点1之间的关系通过齐次变换矩阵来表示，具体的变换顺序如下：

站点2：站点2的坐标系方向与参考坐标系T1一致，原点为站点2的基准点，因此通过参考坐标系沿着X轴平移固定距离a得到；

站点3：由于Y1轴与Y3轴共面，将参考坐标系绕Z1旋转α角使得Y1轴、Y3轴平行，然后，将前述坐标系沿着旋转后的X1轴平移距离d1使得Y1轴、Y3轴共线；再将变换后的坐标系沿着Y1(Y3)轴平移距离d2使得前述坐标系与站点3坐标系T3的原点O3重合；最后，再将前述坐标系绕Y1(Y3)轴旋转β角使得X1轴，X3轴共线即可得到坐标系T3；

站点4：站点4的坐标系方向与坐标系T3一致，原点为站点4的基准点，因此通过坐标系T3沿着X轴平移固定距离a得到；具体变换公式如下：

T₂＝Trans(a,0,0)

T₃＝R(Z₁,α)·Trans(d₁,0,0)·Trans(0,d₂,0)·R(Y₁,β)

T₄＝R(Z₁,α)·Trans(d₁,0,0)·Trans(0,d₂,0)·R(Y₁,β)·Trans(a,0,0)

由于因为T1是参考坐标系(0,0,0,1),所以乘不乘的结果都是一样的)我们用一个4X1的矩阵来表示T1的坐标。

其中：a表示站点1基准点O1与站点2基准点O2的距离即站点3基准点O3与站点4基准点O4的距离；α,d₁,d₂,β根据第二个固定装置的放置位置获取；

S3、设置校准点M1、M2：在参考坐标系T1选择任意一点作为校准点M1，在参考坐标系T4选择任意一点作为校准点M2，具体如下；

采用一组拉线编码器获取站点3、站点4与校准点M1的拉线长度q₃,q₄；采用另一组拉线编码器获取站点1、站点2与校准点M2的拉线长度q₁,q₂；

站点3与站点4拉线末端要相交于站点1的校准点M1，在参考坐标系1下M1的坐标为M₁，

站点1与站点2拉线末端要相交于站点4的校准点M2，在参考坐标系1下校准点M2的坐标M₂，采用下述公式表达：

M₂＝T_AM₂₃＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)M₂₃

其中：T_A表示将站点3相对于参考坐标系即Т₁的变换矩阵，M₂₃表示校准点2在坐标系3上的坐标位置，由于T3到T4的变换矩阵是已知的—Trans(a,0,0),所以，M2的坐标应该包括Trans(a,0,0)﹒M2在站点4坐标系下的坐标，我们把这两项的乘积合成一个坐标M₂₃；

S4、获取四个站点滑轮上切点的坐标P_i,i＝1,2,3,4；

S4-1、获取四个站点的基准点O1-O4即滑轮下切点相对于参考坐标系T1的坐标，将站点4基准点投影到前三个站点基准点构成的平面上，引入的误差符合工程应用的需求，将站点3投影在前述平面上相当于忽略β角对站点4坐标系Z坐标的影响；

O₁(0,0),

O₂(a,0),

O₃＝T_AO₁＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)O₁,

O₄＝T_AT_BO₁＝T_AO₂＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)O₂

其中，a表示站点1基准点O1与站点2基准点O2的距离即站点3基准点O3与站点4基准点O4的距离；T_A表示将站点3相对于参考坐标系即Т₁的变换矩阵，T_B表示将站点2相对于参考坐标系即Т₁的变换矩阵；

S4-2、采用下述公式获取四个站点滑轮的圆心坐标Q₁-Q₄；

Q₁(0,r),

Q₂(a,r),

Q₃＝T_AQ₃₃,

Q₄＝T_AT_BQ₃₃＝T_AQ₄₃

其中，r表示滑轮半径，Q₃₃表示Q₃在坐标系3下的坐标，Q₄₃表示Q₄在坐标系3下的坐标；

S4-3、采用下述公式获取四个站点滑轮的上切点坐标P_i,i＝1,2,3,4，

其中：i表示站点编号，T_i表示站点i所在的坐标系，Tran(x,y)表示将站点坐标系平移到以滑轮的圆心为坐标原点的坐标系的平移矩阵，E_i表示滑轮的圆外任意一点在以滑轮圆心为坐标系原点的坐标系下的坐标，Q点表示每个滑轮的圆心，QE表示校准点与站点坐标系的距离，表示旋转角度的旋转矩阵，表滑轮圆心与圆外一点E连接与滑轮的交点F在以滑轮为圆心坐标系下的坐标；

S5、根据步骤S4获取的上切点坐标，计算四个拉线弧长在滑轮上的包络角度α_i,i＝1,2,3,4，α_i表示滑轮上切点P、滑轮外一点E与滑轮圆心Q的夹角；

S6、将前述计算结果带入下述公式获取α,d₁,d₂；

其中，q₁,q₂表示拉线编码器获取的站点1、站点2与校准点M2的拉线长度，q₃,q₄表示拉线编码器获取的站点3、站点4与校准点M1的拉线长度；表示在参考坐标系下各点之间的长度，α₁,α₂,α₃,α₄表示拉线编码器的四个拉线弧长在滑轮上的包络角度；

S7、采用拉线编码器对任一点进行检测，获取该检测点到四个站点的四条拉线长度L₁、L₂、L₃、L₄，采用下述公式获取β，完成四个站点坐标系的建立，从而完成校准；

其中：x₄,y₄,z₄分别表示站点4的坐标，其只跟β有关，x,y,z分别表示被测点的坐标，l₄表示拉线编码器采集到的被测点与站点4的距离。

本发明的有益效果：

本发明可以提供机器人厂家对于机器人测量的初始位置进行校准，以提高机器人的测量精度。同时在中小企业中可得到广泛的运用，弥补了现在很多三维测量设备的移动性差，成本较高等缺点。涉及到工业机器人在各个行业的运用，如航空业，传统制造业，电子信息业等行业，该算法效率高，成本较低并且环境适应性强，应用范围广，易于实现，操作方便。

附图说明

图1是本发明的测量模块四站式布局。

图2是本发明的四站坐标系数学模型。

图3为车间平整度模型。

图4是测量结构坐标系示意图。

图5是上切点求解数学模型。

图6为初始位置校准几何模型。

图7是基于拉线编码器的站点可动式初始位置校准算法流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

如图1所示，一种基于拉线编码器的站点可动式测量机构的初始位置校准方法，它采用站点可动式测量机构，它包括两个固定装置，各固定装置上安装有两个拉线编码器，即测量机构的两个站点，两个固定装置上的两个站点之间的位置固定；该校准方法包括以下步骤：

S1、对可动式测量机构的四个站点进行初步建模：

设下左、下右、上左、上右四个站点编号分别为站点1—站点4，设站点1为参考坐标系T1，站点2的坐标系为T2，站点3的坐标系为T3，站点4的坐标系为T4，站点1—站点4的基准点分别表示各站点滑轮的下切点，记为O1-O4，以站点1的基准点O1指向站点2的基准点O2作为参考坐标系的X1轴，以站点1的基准点O1指向站点3的基准点O3作为参考坐标系的Y1轴，将以基准点O1为原点，垂直于X1轴、Y1轴所在平面的直线作为Z1轴；

S2、对四站式布局的站点1-4建立D-H模型，站点2与站点1固连，站点2、3、4的坐标系与站点1之间的关系通过齐次变换矩阵来表示，具体的变换顺序如下：

站点2：站点2的坐标系方向与参考坐标系T1一致，原点为站点2的基准点，因此通过参考坐标系沿着X轴平移固定距离a得到；

站点3：由于Y1轴与Y3轴共面，将参考坐标系绕Z1旋转α角使得Y1轴、Y3轴平行，然后，将前述坐标系沿着旋转后的X1轴平移距离d1使得Y1轴、Y3轴共线；再将变换后的坐标系沿着Y1(Y3)轴平移距离d2使得前述坐标系与站点3坐标系T3的原点O3重合；最后，再将前述坐标系绕Y1(Y3)轴旋转β角使得X1轴，X3轴共线即可得到坐标系T3；

站点4：站点4的坐标系方向与坐标系T3一致，原点为站点4的基准点，因此通过坐标系T3沿着X轴平移固定距离a得到；具体变换公式如下：

T₂＝Trans(a,0,0)

T₃＝R(Z₁,α)·Trans(d₁,0,0)·Trans(0,d₂,0)·R(Y₁,β)

T₄＝R(Z₁,α)·Trans(d₁,0,0)·Trans(0,d₂,0)·R(Y₁,β)·Trans(a,0,0)

由于因为T1是参考坐标系(0,0,0,1),所以乘不乘的结果都是一样的)我们用一个4X1的矩阵来表示T1的坐标。

其中：a表示站点1基准点O1与站点2基准点O2的距离即站点3基准点O3与站点4基准点O4的距离；α,d₁,d₂,β根据第二个固定装置的放置位置获取；

S3、设置校准点M1、M2：在参考坐标系T1选择任意一点作为校准点M1，在参考坐标系T4选择任意一点作为校准点M2，具体如下；

采用一组拉线编码器获取站点3、站点4与校准点M1的拉线长度q₃,q₄；采用另一组拉线编码器获取站点1、站点2与校准点M2的拉线长度q₁,q₂；

站点3与站点4拉线末端要相交于站点1的校准点M1，在参考坐标系1下M1的坐标为M₁，

站点1与站点2拉线末端要相交于站点4的校准点M2，在参考坐标系1下校准点M2的坐标M₂，采用下述公式表达：

M₂＝T_AM₂₃＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)M₂₃

其中：T_A表示将站点3相对于参考坐标系即Т₁的变换矩阵，M₂₃表示校准点2在坐标系3上的坐标位置，由于T3到T4的变换矩阵是已知的—Trans(a,0,0),所以，M2的坐标应该包括Trans(a,0,0)﹒M2在站点4坐标系下的坐标，我们把这两项的乘积合成一个坐标M₂₃；

S4、获取四个站点滑轮上切点的坐标P_i,i＝1,2,3,4；

S4-1、获取四个站点的基准点O1-O4即滑轮下切点相对于参考坐标系T1的坐标，将站点4基准点投影到前三个站点基准点构成的平面上，引入的误差符合工程应用的需求，将站点3投影在前述平面上相当于忽略β角对站点4坐标系Z坐标的影响；

O₁(0,0),

O₂(a,0),

O₃＝T_AO₁＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)O₁,

O₄＝T_AT_BO₁＝T_AO₂＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)O₂

其中，a表示站点1基准点O1与站点2基准点O2的距离即站点3基准点O3与站点4基准点O4的距离；T_A表示将站点3相对于参考坐标系即Т₁的变换矩阵，T_B表示将站点2相对于参考坐标系即Т₁的变换矩阵；

S4-2、采用下述公式获取四个站点滑轮的圆心坐标Q₁-Q₄；

Q₁(0,r),

Q₂(a,r),

Q₃＝T_AQ₃₃,

Q₄＝T_AT_BQ₃₃＝T_AQ₄₃

其中，r表示滑轮半径，Q₃₃表示Q₃在坐标系3下的坐标，Q₄₃表示Q₄在坐标系3下的坐标；

S4-3、采用下述公式获取四个站点滑轮的上切点坐标P_i,i＝1,2,3,4，

其中：i表示站点编号，T_i表示站点i所在的坐标系，Tran(x,y)表示将站点坐标系平移到以滑轮的圆心为坐标原点的坐标系的平移矩阵，E_i表示滑轮的圆外任意一点在以滑轮圆心为坐标系原点的坐标系下的坐标，Q点表示每个滑轮的圆心，QE表示校准点与站点坐标系的距离，表示旋转角度的旋转矩阵，表滑轮圆心与圆外一点E连接与滑轮的交点F在以滑轮为圆心坐标系下的坐标；

S5、根据步骤S4获取的上切点坐标，计算四个拉线弧长在滑轮上的包络角度α_i,i＝1,2,3,4，α_i表示滑轮上切点P、滑轮外一点E与滑轮圆心Q的夹角；

S6、将前述计算结果带入下述公式获取α,d₁,d₂；

其中，q₁,q₂表示拉线编码器获取的站点1、站点2与校准点M2的拉线长度，q₃,q₄表示拉线编码器获取的站点3、站点4与校准点M1的拉线长度；表示在参考坐标系下各点之间的长度，α₁,α₂,α₃,α₄表示拉线编码器的四个拉线弧长在滑轮上的包络角度；

S7、采用拉线编码器对任一点进行检测，获取该检测点到四个站点的四条拉线长度L₁、L₂、L₃、L₄，采用下述公式获取β，完成四个站点坐标系的建立，从而完成校准；

其中：x₄,y₄,z₄分别表示站点4的坐标，其只跟β有关，x,y,z分别表示被测点的坐标，l₄表示拉线编码器采集到的被测点与站点4的距离。

步骤S7中，检测点坐标的坐标获取方式采用专利号为201310421491.2的拉线式空间位置测量机构及测量方法获取。

本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。

Claims (1)

1.一种基于拉线编码器的站点可动式测量机构的初始位置校准方法，其特征是它采用站点可动式测量机构，它包括两个固定装置(1)，各固定装置(1)上安装有两个拉线编码器，即测量机构的两个站点，两个固定装置(1)上的两个站点之间的位置固定；该校准方法包括以下步骤：

S1、对可动式测量机构的四个站点进行初步建模：

设下左、下右、上左、上右四个站点编号分别为站点1—站点4，设站点1为参考坐标系T1，站点2的坐标系为T2，站点3的坐标系为T3，站点4的坐标系为T4，站点1—站点4的基准点分别表示各站点滑轮的下切点，记为O₁-O₄，以站点1的基准点O₁指向站点2的基准点O₂作为参考坐标系的X1轴，以站点1的基准点O₁指向站点3的基准点O₃作为参考坐标系的Y1轴，将以基准点O₁为原点，垂直于X1轴、Y1轴所在平面的直线作为Z1轴；

S2、对四站式布局的站点1-4建立D-H模型，站点2与站点1固连，站点2、3、4的坐标系与站点1之间的关系通过齐次变换矩阵来表示，具体的变换顺序如下：

站点2：站点2的坐标系方向与参考坐标系T1一致，原点为站点2的基准点，因此通过参考坐标系沿着X轴平移固定距离a得到；

站点3：由于Y1轴与Y3轴共面，将参考坐标系绕Z1旋转α角使得Y1轴、Y3轴平行，然后，将旋转α角后的坐标系沿着旋转后的X1轴平移距离d1使得Y1轴、Y3轴共线；再将变换后的坐标系沿着Y1(Y3)轴平移距离d2使得平移d1后的坐标系与站点3坐标系T3的基准点O₃重合；最后，再将平移d2后的坐标系绕Y1(Y3)轴旋转β角使得X1轴，X3轴共线即可得到坐标系T3；

站点4：站点4的坐标系方向与坐标系T3一致，原点为站点4的基准点，因此通过坐标系T3沿着X轴平移固定距离a得到；具体变换公式如下：

T₂＝Trans(a,0,0)

T₃＝R(Z₁,α)·Trans(d₁,0,0)·Trans(0,d₂,0)·R(Y₁,β)

T₄＝R(Z₁,α)·Trans(d₁,0,0)·Trans(0,d₂,0)·R(Y₁,β)·Trans(a,0,0)

其中：a表示站点1基准点O₁与站点2基准点O₂的距离即站点3基准点O₃与站点4基准点O₄的距离；α,d₁,d₂,β根据第二个固定装置的放置位置获取；

S3、设置校准点M1、M2：在参考坐标系T1选择任意一点作为校准点M1，在参考坐标系T4选择任意一点作为校准点M2，具体如下；

采用一组拉线编码器获取站点3、站点4与校准点M1的拉线长度q₃,q₄；采用另一组拉线编码器获取站点1、站点2与校准点M2的拉线长度q₁,q₂；

站点3与站点4拉线末端要相交于站点1的校准点M1，在参考坐标系T1下M1的坐标为M₁，

站点1与站点2拉线末端要相交于站点4的校准点M2，在参考坐标系T1下校准点M2的坐标M₂，采用下述公式表达：

M₂＝T_AM₂₃＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)M₂₃

其中：T_A表示将站点3相对于参考坐标系即Т₁的变换矩阵，M₂₃表示校准点M 2在坐标系T3上的坐标位置，由于T3到T4的变换矩阵是已知的—Trans(a,0,0),所以，M2的坐标应该包括Trans(a,0,0)﹒M2在站点4坐标系下的坐标，我们把这两项的乘积合成一个坐标M₂₃；

S4、获取四个站点滑轮上切点的坐标P_i,i＝1,2,3,4；

S4-1、获取四个站点的基准点O₁-O₄即滑轮下切点相对于参考坐标系T1的坐标，将站点4基准点投影到前三个站点基准点构成的平面上，引入的误差符合工程应用的需求，将站点3投影在前述平面上相当于忽略β角对站点4坐标系Z坐标的影响；

O₁(0,0),

O₂(a,0),

O₃＝T_AO₁＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)O₁,

O₄＝T_AT_BO₁＝T_AO₂＝R(Z₁,α)Trans(d₁,0,0)Trans(0,d₂,0)O₂

其中，a表示站点1基准点O₁与站点2基准点O₂的距离即站点3基准点O₃与站点4基准点O₄的距离；T_A表示将站点3相对于参考坐标系即Т1 的变换矩阵，T_B表示将站点2相对于参考坐标系即Т1 的变换矩阵；

S4-2、采用下述公式获取四个站点滑轮的圆心坐标Q₁-Q₄；

Q₁(0,r),

Q₂(a,r),

Q₃＝T_AQ₃₃,

Q₄＝T_AT_BQ₃₃＝T_AQ₄₃

其中，r表示滑轮半径，Q₃₃表示Q₃在坐标系T 3下的坐标，Q₄₃表示Q₄在坐标系T 3下的坐标；

S4-3、采用下述公式获取四个站点滑轮的上切点坐标P_i,i＝1,2,3,4，

其中：i表示站点编号，T_i表示站点i所在的坐标系，Tran(x,y)表示将站点坐标系平移到以滑轮的圆心为坐标原点的坐标系的平移矩阵，E_i表示滑轮的圆外任意一点在以滑轮圆心为坐标系原点的坐标系下的坐标，Q点表示每个滑轮的圆心，QE表示校准点与站点i所在的坐标系的距离，站点3、站点4选取校准点M1，站点1、站点2选取校准点M2，表示旋转角度的旋转矩阵，表示滑轮圆心与圆外一点E连接与滑轮的交点F在以滑轮为圆心坐标系下的坐标；

S5、根据步骤S4获取的上切点坐标，计算四个拉线弧长在滑轮上的包络角度α_i,i＝1,2,3,4，α_i表示滑轮上切点P、滑轮外一点E与滑轮圆心Q的夹角；

S6、将前述计算结果带入下述公式获取α,d₁,d₂；

其中，q₁,q₂表示拉线编码器获取的站点1、站点2与校准点M2的拉线长度，q₃,q₄表示拉线编码器获取的站点3、站点4与校准点M1的拉线长度；表示在参考坐标系下各点之间的长度，α₁,α₂,α₃,α₄表示拉线编码器的四个拉线弧长在滑轮上的包络角度；

S7、采用拉线编码器对任一点进行检测，获取该检测点到四个站点的四条拉线长度L₁、L₂、L₃、L₄，采用下述公式获取β，完成四个站点坐标系的建立，从而完成校准；

其中：x₄,y₄,z₄分别表示站点4的坐标，其只跟β有关，x,y,z分别表示被测点的坐标，l₄表示拉线编码器采集到的被测点与站点4的距离。