CN110757504A - 高精度可移动机器人的定位误差补偿方法 - Google Patents

Abstract

本发明公开了高精度可移动机器人的定位误差补偿方法，包括分别位于测量坐标系{M}的X、Y、Z轴上的交汇于原点的X杆、Y杆、Z杆，X杆、Y杆、Z杆上分别设置有高精度激光测距传感器用于测量坐标系原点距离底部测量参考面6、左侧测量参考面2、后侧测量参考面10的距离，其中，底部测量参考面6为水平面，左侧测量参考面2、后侧测量参考面10是相互垂直且与底部测量参考面垂直的两个垂面；本发明能有效减少移动定位误差给机器人带来的影响。

Description

高精度可移动机器人的定位误差补偿方法

技术领域

本发明涉及机器人领域，具体涉及高精度可移动机器人的定位误差补偿方法。

背景技术

工业机器人工作时的状态用其末端工具中心点(TCP)相对于基坐标系的位置和旋转角 度来描述，简称位姿。工业机器人执行程序作业的过程，实际上就是其末端位姿在不同状态 切换、完成规划动作的过程。编程的坐标系可分为基坐标系、工具坐标系和工件坐标系，工 件坐标系和工具坐标系中描述的位姿都可以通过该坐标系与基坐标系的变换关系转换为基坐 标系中的位姿。

移动机器人编程后，再次进入工位，机器人基座位置和方向有编程时的情况有一定误差， 该定位误差造成机器人基坐标系{B}发生位移和旋转，最终影响到工业机器人末端的工作精 度。为了提高移动工业机器人的工作精度，可以让机器人作业前先进行物理固定，该固定过 程成为二次定位。二次定位能够大幅减小机器人定位误差带来的影响，但是由于二次定位装 置的制造、装配、使用磨损等原因，二次定位同样存在微量误差，影响到机器人的精度。

为了提高移动工业机器人的工作精度，可以让机器人作业前先进行物理固定，该固定过 程成为二次定位。由于工业机器人在二次定位装置中已经进行物理定位，二次定位误差测量 为微量误差，客观上要求测量精度高、测量及时快速、测量仪器尺寸小。目前，机器人位姿 测量的方法使用激光跟踪仪测量、使用经纬仪测量、使用三坐标测量机测量和使用工业相机 摄像测量等方法，但是这些方法由于设备尺寸大、费用高昂、操作不便、系统复杂等原因， 不适合机器人入位误差的实时测量。

因此对于机器人在二次定位时产生的误差进行补偿对于机器人在定位时产生的误差进行 及时的修正，是迫切需要的。

发明内容

本发明所要解决的技术问题是现有的可移动的机器人在进入工位时，会产生定位误差， 在使用时不能及时的修正，影响机器人的工作精度；目的在于提供高精度可移动机器人的定 位误差补偿方法，解决机器人的定位误差的补偿的问题。

本发明通过下述技术方案实现：

高精度可移动机器人的定位误差测量补偿方法，包括分别位于测量坐标系{M}的x、y、z 坐标轴上交汇于原点的X杆、Y杆、Z杆，X杆、Y杆、Z杆上分别设置有X测距传感器、 Y测距传感器、Z测距传感器，Z测距传感器、Y测距传感器、X测距传感器分别用于测量 测量坐标系原点到底部测量参考面、左侧测量参考面、后侧测量参考面的距离，其中，底部 测量参考面、左侧测量参考面、后侧测量参考面分别代表世界坐标系{W}的xy平面、xz面 和zy平面；底部测量参考面、左侧测量参考面、后侧测量参考面分别位于定位装置的底面、 侧面、后侧面。

X杆、Y杆的下方分别连接有用于测量坐标系{M}的x、y坐标轴的水平倾斜角度的X轴 水平倾角传感器和Y轴水平倾角传感器；X杆上还连接有用于测量坐标系{M}中zx坐标平面 与铅垂面在垂直于x轴的平面内的夹角的坐标轴垂直倾角传感器；Y杆上连接有通过测量坐 标系{M}相对于地磁场在水平面上的角度位移的磁场方向传感器。

其中，坐标轴垂直倾角传感器包括连接轴和可绕连接轴旋转的平衡块以及角度传感器， 角度传感器包括活动端和固定端，活动端与平衡块连接，固定端与连接轴连接，连接轴与X 杆连接，并且连接轴与X杆轴线相互重合。

进一步的，本测量装置的底部测量参考面为水平面，左侧测量参考面、后侧测量参考面 是相互垂直且与底部测量参考面垂直的两个垂面。空间定位误差测量装置中构建了一个测量 坐标系{M}，X测距探头、Y测距探头、Z测距探头与x、y、z同轴设置，X轴水平倾角传感 器和Y轴水平倾角传感器位于x、y轴下方且分别与x、y轴平行设置，用于测量x、y坐标 轴的水平倾斜角度；坐标轴垂直倾角传感器的连接轴的轴线与x轴重合，用于测量zx坐标平面与铅垂面在垂直于x轴的平面内的夹角。

坐标轴垂直倾角传感器包括连接轴和可绕连接轴旋转的平衡块以及角度传感器，角度传 感器包括活动端和固定端，活动端与平衡块连接，固定端与连接轴连接，连接轴与X杆连接， 并且连接轴与X杆所在直线相互重合。该传感器主要由一根连接轴、可以绕轴旋转的平衡块 和一个角度传感器组成。角度传感器由活动端和固定端组成，活动端与平衡块相连，固定端 与轴相连。

测量中，将传感器的轴安装在与被测坐标轴同轴的位置。由于所测误差为微量误差，坐 标轴与水平面的倾角是很小的。当坐标平面与铅垂面不重合时，在重力作用下，平衡块重心 与轴心的连线始终保持在铅垂面内，从而使角度传感器的活动端与固定端产生角度位移，该 角度位移即为坐标平面与铅垂面的夹角。

本装置在测量时，没有繁杂的结构，尺寸较小，更便于使用，同时本装置的误差也更小， 进一步的提高了使用效率。

空间定位误差测量装置的测量方法，包括以下步骤：

(1)测量坐标系{M}相对于世界坐标系{W}的变换关系表示为：

其中，T_x(xM)、T_y(y_M)、T_z(z_M)分别为{W}到{M}移动变换的齐次矩阵，R_z(ψ)、

R_y″(θ) 分别为{M}先后绕z轴、x’轴和y”轴旋转变换的齐次矩阵；

(2)通过磁场方向传感器得到测量坐标系{M}绕z轴转过的角度ψ，即∠bO_Mb′；

(3)通过Y轴水平倾角传感器得到测量坐标系{M}绕x′轴转过的角度

即∠b″O_Mb′；

(4)通过坐标轴垂直倾角传感器得到zx坐标平面与经过x轴的铅垂面在垂直于x轴的 平面上的夹角α_x；通过X轴水平倾角传感器测出x轴与水平面的倾角为θ₀，通过α_x和θ₀得到 坐标系{M}绕y轴转过的角度θ为：

(5)通过Z测距传感器、Y测距传感器、X测距传感器分别测得底部测量参考面、左侧测量参考面、后侧测量参考面上光斑到测量坐标系{M}原点的距离分别为Lx、Ly、Lz，进而可以计算出世界坐标系{W}到测量坐标系{M}原点在x、y、z方向的移动距离分别为

(6)将步骤(2)～(5)中确定的参数带入步骤(1)中的计算公式，即可得到本装置相对于世界坐标系{W}的位置和方向，进而也可以确定空间定位误差测量装置所安装在其上的 机器人的空间位置和方向，以及所测得位置和方向与正确位置与和方向之间的误差。

具体的，机器人底座上连接有定位误差测量装置，定位误差测量装置中设置有一个测量 坐标系{M}，测量坐标系{M}与机器人基坐标系{B}方向平行，相对位置固定，在x、y、z三 个方向的位置差分别为Δx、Δy、Δz；补偿方法包括以下步骤：

(1)机器人编程时测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分别为x₀、y₀、z、

θ₀、ψ₀，本次测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分 别为x_i、y_i、z_i、

θ_i、ψ_i：

新的基坐标系{B’}中到旧的基坐标系{B}的变换：

式中

为{B’}到{M’}的齐次变换矩阵，

为{M’}到{M}的齐次变换矩阵，

为{M} 到{B}的齐次变换矩阵；

其中：

(2)机器人末端坐标系在新的基坐标中的位姿转换：

式中，

为工业机器人在旧的基坐标中的位姿。

(3)对于式(8)，如果是关节坐标位姿，可以用以下变换关系函数予以转换：

式中q_i各个关节变量。

(4)如果是工件坐标系和工具坐标系中直角坐标位姿数据，可按以下给出的计算方法 对工件坐标系或工具坐标系予以补偿，工件坐标系或工具坐标系被修正后，机器人根据修正 数据修正相关联的位姿数据。

式中，{T}为基坐标系{B}中的末端坐标系，{M’}为二次定位后测量坐标系，

为{B’} 到{M’}的齐次变换矩阵，

为{M’}到{M}的齐次变换矩阵，

为{M}到{B}的齐次变换矩阵；i为机器人关节数，

表示机器人末端执行器在基坐标中的姿态数据，f(q_i)为变换关系函数， 其中q_i各个关节变量；

分别为机器人坐标系{B}到工件坐标系{P}和工具坐标系{T}的 变换关系；

(5)前一次定位误差和本次定位误差值分别代入公式(5)中，得到齐次变换矩阵

再进一步按公式(6)进行计算，即可得出当前基坐标系{B’}到原始基坐标系{B}的齐次变换 矩阵

(6)根据程序中的位姿数据类型，分别将

代入公式(7)(8)(9)中更新位姿数据。

本发明与现有技术相比，具有如下的优点和有益效果：

1、本发明高精度可移动机器人的定位误差补偿方法，本发明能有效减少移动定位误差给 机器人带来的影响。误差软补偿的数学模型无近似算法，不存在原理误差，因此，误差修正 的效果依赖于定位误差测量精度；

2、本发明高精度可移动机器人的定位误差补偿方法，本发明的位移误差的测量精度为1 μm，角度测量精度为0.01°以内，使用效率更高；

3、本发明高精度可移动机器人的定位误差补偿方法，本发明操作简单，没有繁杂的结构 和操作，便于操作使用，使用效率更高。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不 构成对本发明实施例的限定。在附图中：

图1为本发明结构示意图；

图2为本发明坐标轴垂直倾角传感器结示意图；

图3为本发明世界坐标系经与测量坐标系的位移旋转变换图；

图4为本发明绕x轴旋转角度测量原理；

图5为本发明二次定位误差使机器人坐标系产生变换；

图6为本发明定位误差补偿程序算法流程。

附图中标记及对应的零部件名称：

1-磁场角度传感器，2-左侧测量参考面，3-Y测距探头，4-Y轴水平倾角传感器，5-Z测 距探头，6-底部测量参考面，7-X轴水平倾角传感器，8-坐标轴垂直倾角传感器，9-X测距探 头，10-后侧测量参考面，11-连接轴，12-角度传感器，13-平衡块。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明 作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本 发明的限定。

实施例1

本发明高精度可移动机器人的定位误差补偿方法，如图1-2所示，包括分别位于测量坐 标系{M}的X、Y、Z轴上的交汇于原点的X杆、Y杆、Z杆，X杆、Y杆、Z杆上分别设置 有高精度激光测距传感器用于测量坐标系原点距离底部测量参考面6、左侧测量参考面2、后 侧测量参考面10的距离，其中，底部测量参考面6为水平面，左侧测量参考面2、后侧测量 参考面10是相互垂直且与底部测量参考面垂直的两个垂面；

X杆、Y杆的下方分别连接有用于测量x、y坐标轴的水平倾斜角度的X轴水平倾角传感器7和Y轴水平倾角传感器4；X杆上还连接有用于测量zx坐标平面与铅垂面在垂直于x 轴的平面内的夹角的坐标轴垂直倾角传感器8；Y杆上连接有通过测量坐标系{M}相对于地磁场在水平面上的角度位移的磁场方向传感器1。

如图2所示，坐标轴垂直倾角传感器8包括连接轴11和可绕连接轴11旋转的平衡块13 以及角度传感器12，角度传感器12包括活动端和固定端，活动端与平衡块13连接，固定端 与连接轴11连接，连接轴11与X杆连接，并且连接轴11与X杆所在直线相互重合。

其中，X测距传感器9、Y测距传感器3、Z测距传感器5为高精度激光测距传感器。

在使用时，在二次定位装置的底面、侧面、后侧面安放水平和竖直的测量参考面，即底 部测量参考面6、左侧测量参考面2和后侧测量参考面10，同时，测量参考面与二次定位装 置上的定位夹紧装置无连接，避免二次定位装置震动影响到测量参考面。

并且，本装置放置坐标系{M}方向与机器人基坐标系平行，相对位置固定，在x、y、z三个方向的位置差分别为Δx、Δy、Δz。其中，Δx、Δy、Δz是与被测设备有关的可变机 械安装参数，具体数值需要在测量装置安装后测量取得。

因此在使用时，通过测量本装置的相对于世界坐标系{W}的位置和方向，可得到本装置 所安装的机器人的空间位置和方向，进而通过所测得位置和方向与正确位置与和方向相比较， 得到二次定位装置的误差。

实施例2

定位误差测量装置的测量方法，在实施例1的基础上，包括以下步骤：

(1)测量坐标系{M}相对于世界坐标系{W}的变换关系表示为：

其中，T_x(x_M)、T_y(y_M)、T_z(z_M)分别为{W}到{M}移动变换的齐次矩阵，R_z(ψ)、

R_y″(θ) 分别为{M}先后绕z轴、x’轴和y”轴旋转变换的齐次矩阵；

(2)通过磁场方向传感器(1)得到测量坐标系{M}绕z轴转过的角度ψ，即∠bO_Mb′；

(3)通过Y轴水平倾角传感器(4)得到测量坐标系{M}绕x′轴转过的角度

即∠b″O_Mb′；

(4)通过坐标轴垂直倾角传感器(8)得到zx坐标平面与经过x轴的铅垂面在垂直于x 轴的平面上的夹角α_x；通过X轴水平倾角传感器(7)测出x轴与水平面的倾角为θ₀，通过α_x和θ₀得到坐标系{M}绕y轴转过的角度θ为：

(5)通过X、Y、Z高精度激光测距传感器测出后侧测量参考面、左侧测量参考面、底部测量参考面上光斑到测量坐标系{M}原点的距离分别为Lx、Ly、Lz，进而可以计算出世界坐标系{W}到测量坐标系{M}原点在x、y、z方向的移动距离分别为

(6)将步骤(2)～(5)中确定的参数带入步骤(1)中的计算公式，即可确定权利要求1 所述的空间定位误差测量装置相对于世界坐标系{W}的位置和方向，也可以进一步确定空间 定位误差测量装置所安装与其上的设备的空间位置和方向，以及所测得位置和方向与正确位 置与和方向之间的误差。

实施例3

在实施例2的基础上，移动机器人前后两次进入工位，由于硬件定位会造成机器人基座 位置和方向前后有一定误差，实际上也就是造成机器人基坐标系{B}发生位移和旋转。对于任 意一个工作状态，机器人末端与机器人基体可以看作是刚体，因此，基坐标变化，基坐标系 中的末端坐标系{T}也就跟随发生相同的位移和旋转到{T’}，如图5。

要消除二次定位对机器人末端的位姿造成的影响，就需要在新的基坐标系{B’}中重新表 示出原来的末端的位姿末端坐标系{T}。

在新的基坐标系{B’}中到旧的基坐标系{B}的变换关系，{B’}到{B}的变换途径为： {B’}→{M’}→{M}→{B}。因此，变换关系：

式中

为{B’}到{M’}的齐次变换矩阵，

为{M’}到{M}的齐次变换矩阵，为{M} 到{B}的齐次变换矩阵。

机器人第一次测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分别为x₀、y₀、z、θ₀、ψ₀，本次测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分别为 x_i、y_i、z_i、

θ_i、ψ_i，则新的测量坐标系{M’}与旧的测量坐标系{M}的位置关系可表示为

则

工业机器人末端的直角坐标位姿数据转换描述的其实就是机器人基坐标系{B}到末端坐 标系{i}的变换关系，若该位姿用

表示，则应用定位误差补偿后，{B’}到{i}的关系为

式(9)即为机器人末端坐标系在新的基坐标中的位姿描述。

已知关节坐标位姿参数，可以根据机器人动力学模型求出对应的直角坐标位姿数据，计 算公式为

式中，i为机器人关节数，

表示机器人末端执行器在基坐标中的姿态数据，f(q_i)为变换 关系函数，该函数是因机器人而异的，其中q_i各个关节变量。

关节坐标位姿数据转换到直角坐标位姿数据后，即可按直角坐标位姿数据进行误差补偿 转换计算。

工件坐标系和工具坐标系中直角坐标位姿数据在机器人执行动作前首先被机器人内置程 序换算为基坐标中的直角坐标数据，进而根据反向动力学原理被换算出各个关节参数。因此， 对于工件坐标系和工具坐标系中直角坐标位姿数据的补偿，只需通过补偿改变工件坐标系和 工具坐标系的位姿，机器人程序运行时即会自动更新位姿数据对应的运行参数。若分 别为机器人坐标系{B}到工件坐标系{P}和工具坐标系{T}的变换关系，则{B’}到工件坐标系 {P}和工具坐标系{T}的变换关系可分别表示为

工件坐标系和工具坐标系被重新设置后，机器人内置程序会自动解算出新的工件坐标系 和工具坐标系中的位姿数据在新的基坐标中的表示和对应的关节坐标参数。

实施例4

对于示教编程机器人，机器人第一次测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分别为x₀、y₀、z、θ₀、ψ₀，以后每次测得的x、y、z方向的位移误差和绕旋转 x、y、z的角度误差分别为x_i、y_i、z_i、

θ_i、ψ_i，带入式(7)中，

再进一步按式

进行计算，即可得 出当前基坐标系{B’}到原始基坐标系{B}的齐次变换矩阵

根据程序中的位姿数据类型， 分别将

代入式(9)(10)(11)更新位姿数据，从而实现定位误差的补偿。

对于离线编程的机器人，默认原始定位误差为x₀＝C_x、y₀＝C_y、z₀＝C_z、

θ₀＝0°、 ψ₀＝0°(C_x、C_y、C_z为测定常数)。工业机器人在进入工位时，测出前定位误差为x′，y′，z′，

θ′，ψ′，即可按前述方法更新程序中的位姿数据，进行定位误差补偿。

定位误差补偿通过程序实现，程序原理流程如图6所示。机器人每次入位后，先将先前 编制的程序拷贝一个副本作为本次运行的程序，然后执行定位误差补偿计算，刷新程序中的 位姿数据。之后机器人根据反向动力学重新计算关节运动参数，执行修改后的程序。动作执 行完成后，本次生成的程序副本已经没有用处，即被删除。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说 明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护 范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本 发明的保护范围之内。

Claims (3)

1.高精度可移动机器人的定位误差补偿方法，其特征在于，机器人底座上连接有定位误差测量装置，定位误差测量装置中设置有一个测量坐标系{M}，测量坐标系{M}与机器人基坐标系{B}方向平行，相对位置固定，在x、y、z三个方向的位置差分别为Δx、Δy、Δz；补偿方法包括以下步骤：

(1)机器人第一次测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分别为x₀、y₀、z、

θ₀、ψ₀，本次测得的x、y、z方向的位移误差和绕旋转x、y、z的角度误差分别为x_i、y_i、z_i、

θ_i、ψ_i：

新的基坐标系{B’}中到旧的基坐标系{B}的变换：

式中

为{B’}到新的测量坐标系{M’}的齐次变换矩阵，

为{M’}到就的测量坐标系{M}的齐次变换矩阵，

为{M}到{B}的齐次变换矩阵；

其中：

(2)机器人末端坐标系在新的基坐标中的位姿转换：

式中，

为工业机器人在旧的基坐标中的位姿

(3)对于式(3)，如果是关节坐标位姿，可以用以下变换关系函数予以转换：

式中q_i各个关节变量，i为机器人关节顺序号；

(4)如果是工件坐标系和工具坐标系中位姿数据，可按以下给出的计算方法对工件坐标系或工具坐标系予以补偿，工件坐标系或工具坐标系被修正后，机器人修正位姿数据；

式中，

为{B’}到新测量坐标系{M’}的齐次变换矩阵，为{M’}到旧测量坐标系{M}的齐次变换矩阵，

为{M}到{B}的齐次变换矩阵；

分别为机器人坐标系{B}到工件坐标系{P}和工具坐标系{T}的变换关系；

(5)前一次定位误差和本次定位误差值分别代入公式(1)中，得到齐次变换矩阵

再进一步按公式(2)进行计算，即可得出当前基坐标系{B’}到原始基坐标系{B}的齐次变换矩阵

(6)根据程序中的位姿数据类型，分别将

代入公式(3)(4)(5)中更新位姿数据。

2.根据权利要求1所述的高精度可移动机器人的定位误差补偿方法，其特征在于，定位误差测量装置包括分别位于测量坐标系{M}的x、y、z坐标轴上交汇于原点的X杆、Y杆、Z杆，X杆、Y杆、Z杆上分别设置有X测距传感器(9)、Y测距传感器(3)、Z测距传感器(5)，Z测距传感器(5)、Y测距传感器(3)、X测距传感器(9)分别用于测量测量坐标系原点到底部测量参考面(6)、左侧测量参考面(2)、后侧测量参考面(10)的距离，其中，底部测量参考面(6)、左侧测量参考面(2)、后侧测量参考面(10)分别代表世界坐标系{W}的xy平面、xz面和zy平面；

X杆、Y杆的下方分别连接有用于测量坐标系{M}的x、y坐标轴的水平倾斜角度的X轴水平倾角传感器(7)和Y轴水平倾角传感器(4)；X杆上还连接有用于测量坐标系{M}中zx坐标平面与铅垂面在垂直于x轴的平面内的夹角的坐标轴垂直倾角传感器(8)；Y杆上连接有通过测量坐标系{M}相对于地磁场在水平面上的角度位移的磁场方向传感器(1)。

3.根据权利要求2所述的高精度可移动机器人的定位误差补偿方法，其特征在于，坐标轴垂直倾角传感器(8)包括连接轴(11)和可绕连接轴(11)旋转的平衡块(13)以及角度传感器(12)，角度传感器(12)包括活动端和固定端，活动端与平衡块(13)连接，固定端与连接轴(11)连接，连接轴(11)与X杆连接，并且连接轴(11)与X杆轴线相互重合。

Images