CN113843804B - 基于平面约束的机器人运动学标定方法及系统 - Google Patents

Abstract

本发明提供了一种基于平面约束的机器人运动学标定方法及系统，包括：建立基坐标系、法兰坐标系、工具坐标系；操作机器人移动，记录激光位移传感器度数、机器人关节角度；建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；在机器人工作空间内重复步骤，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量。本发明可以对机器人运动学参数进行校正，提高其工作的绝度定位精度。本发明非接触式测量，精度较高；使用激光位移传感器和标定平面，成本低，操作简便，可广泛应用于中小企业，在实际中具有良好的可行性。

Description

基于平面约束的机器人运动学标定方法及系统

技术领域

本发明涉及机器人标定领域，具体地，涉及一种基于平面约束的机器人运动学标定方法及系统。

背景技术

随着机器人技术在各行各业中的广泛应用，对机器人的重复定位精度和绝对定位精度要求也越来越高。目前机器人重复定位精度比较高，绝对定位精度却很低，并且每台机器人的差别较大，严重制约了机器人的应用范围。机器人运动学标定一般包括四个步骤，建模，测量，误差辨识，补偿。目前数据测量一般要借助激光跟踪仪、三坐标测量机、球杆仪等昂贵的精密测量仪器，而且需要专业人员操作。

本发明针对现有运动学标定设备昂贵、操作复杂等技术问题，而提供一种工业机器人的运动学标定方法，能够降低标定成本，提高标定效率。

专利文献CN104608129B(申请号：CN201410711022.9)公开了一种基于平面约束的机器人标定方法，具体包括以下步骤：利用DH与MDH结合的方法建立机器人运动学模型；基于微分变换原理建立机器人末端位置误差模型；基于平面约束建立机器人的位置标定模型；标定块位姿摆放；示教并记录机器人末端理论位姿；机器人运动学参数标定；利用标定结果进行对比，如不满足精度要求，重新标定。但该发明与本发明标定块不同：该发明的标定块有三个相互垂直的平面，分别在三个平面示教n个点，与本发明不同；该发明与本发明误差模型不同：该发明通过机器人末端任意两点在平面法线方向上的位置误差的差值等于这两点理论坐标值之差建立误差模型；该发明需要确保标定快三个相互垂直的法向量与机器人基坐标系平行，实施难度较大；该发明需要控制机器人与标定块接触示教，实施误差较大。

专利文献CN108406771B(申请号：CN201810196264.7)公开了一种机器人自标定方法，包括以下步骤：(1)建立机器人运动学模型；(2)建立机器人末端位置误差模型；(3)建立平面约束误差模型；(4)驱动机器人对约束平面分别进行测量；(5)机器人运动学参数辨识；(6)标定结果验证。该发明需要知道机器人基坐标系在标定块角点处坐标系OcXcYcZc的理论其次变换矩阵，该发明是通过对标定块测量，分别建立三个平面的坐标系，根据三个平面的位置误差建立误差模型。

专利文献CN107972071B(申请号：CN201711264644.1)公开了一种基于末端点平面约束的工业机器人连杆参数标定方法，包括如下步骤：1)建立工业机器人的连杆坐标系和工具坐标系，得到工业机器人末端位置坐标；2)对末端点进行平面约束建立工业机器人连杆参数误差辨识模型；3)变换工业机器人的位姿态，记录关节变量的关节值和激光束的长度，并根据前三个位姿的位置坐标计算平面方程的初始参数；4)对工业机器人连杆参数进行误差辨识；5)依次对待修正参数进行修正，并验证修正后工业机器人的精度。该发明需要计算平面方程的初始参数，存在测量误差；该发明机器人运动范围受平面摆放位置限制，不能在机器人较大工作空间内标定。

专利文献CN108731591B(申请号：CN201810374769.8)公开了一种基于平面约束的机器人工具坐标系标定方法，包括设置机器人的运动模式；建立基坐标系B、末端坐标系E和工具坐标系T；建立平面约束条件；保证照射点始终在大理石平台上，保持末端坐标系E的姿态不变，控制末端坐标系E的位置发生六次移动；保证照射点始终在大理石平台上，保持末端坐标系E的位置不变，控制末端坐标系E的姿态发生三次改变；求解照射点坐标方程。但该发明平面固定不动且使用的是标定工具坐标系。

专利文献CN110340881A(申请号：CN201810303734.5)公开了一种机器人工具的标定方法和标定系统。标定方法包括：提供校准工具和摄像机，校准工具上形成有具有规则的几何形状的校准特征，摄像机的光轴与竖直方向平行；将校准工具安装在机器人工具上，校准工具上的校准特征的几何中心点与机器人工具的中心点在竖直方向上对齐；用摄像机识别校准特征的几何中心点的初始位置；驱动机器人工具围绕竖直轴线旋转预定角度；用摄像机识别校准特征的几何中心点的终止位置；根据初始位置、终止位置和预定角度计算机器人工具的中心点相对于末端法兰盘的中心点的偏置距离；根据偏置距离标定机器人工具的中心点。但该发明平面固定不动，不能在机器人较大工作空间内标定。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于平面约束的机器人运动学标定方法及系统。

根据本发明提供的一种基于平面约束的机器人运动学标定方法，包括：

步骤S1：建立基坐标系、法兰坐标系、工具坐标系；

步骤S2：操作机器人移动，记录激光位移传感器度数、机器人关节角度；

步骤S3：建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；

步骤S4：根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；

在机器人工作空间内重复步骤S2-S4，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量。

优选地，在所述步骤S1中：

机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

机器人工作空间内固定一块平面，平面度等级为00级以上；

优选地，在所述步骤S2中：

操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数l和记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据，n取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程。

优选地，在所述步骤S3中：

采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

为机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵，n为机器人法兰中心建立法兰坐标系，

为N连杆相对于n-1连杆的齐次变换矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的旋转矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的位置矩阵，R为旋转矩阵，p为位置矩阵；

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数；

平面上激光点在机器人基坐标系下的位置为：

P为平面上激光点在机器人基坐标系下的位置矩阵，I为单位旋转矩阵，t为工具坐标系；

表示激光发射点在法兰坐标系下的位置偏置，与机器人运动学误差量一起作为标定的误差量，平面上激光点在机器人基坐标系的名义位置pⁿ为：

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置p为：

p＝pⁿ+J*σ

其中J为标定雅可比矩阵，σ为运动学参数和工具参数误差向量。

优选地，在所述步骤S4中：

对于测量的n组数据，得到第i个点的实际位置p_i为：

p_i＝p_i ⁿ+J_i*σ，i＝1，2...，n

p_i ⁿ为第i个点的名义位置；J_i为第i个位置点的雅可比矩阵；

取测量的平面上的两点与测量n个点中的第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，...，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略；M_i、N_i为简要表述代式；

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得：

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2...，n-3；

第一个法向量指测量平面前3个点计算的法向量；

展开得：

y_i＝b_i*σ，i＝1，2...，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

y_i、b_i为简要表述代式；

建立单个平面的标定方程：

Y＝B*σ

其中，

Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

Y、B、y_n、b_n均为简要表述对应的代式。

优选地，在机器人工作空间内移动平面，重复步骤S2-S4k次，k取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；

设第k个平面的标定方程用y_k，i表示，则k个平面的标定方程为：

其中：

为、

为、y_k，n-3为、b_k，n-3均为简要表述对应的代式；

根据最小二乘法计算运动学参数和工具参数误差向量σ：

根据本发明提供的一种基于平面约束的机器人运动学标定系统，包括：

模块M1：建立基坐标系、法兰坐标系、工具坐标系；

模块M2：操作机器人移动，记录激光位移传感器度数、机器人关节角度；

模块M3：建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；

模块M4：根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；

在机器人工作空间内重复模块M2-M4，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量。

优选地，在所述模块M1中：

机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

机器人工作空间内固定一块平面，平面度等级为00级以上；

优选地，在所述模块M2中：

操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数l和记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据，n取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程。

优选地，在所述模块M3中：

采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

为机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵，n为机器人法兰中心建立法兰坐标系，

为N连杆相对于n-1连杆的齐次变换矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的旋转矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的位置矩阵，R为旋转矩阵，p为位置矩阵；

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数；

平面上激光点在机器人基坐标系下的位置为：

P为平面上激光点在机器人基坐标系下的位置矩阵，I为单位旋转矩阵，t为工具坐标系；

表示激光发射点在法兰坐标系下的位置偏置，与机器人运动学误差量一起作为标定的误差量，平面上激光点在机器人基坐标系的名义位置pⁿ为：

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置p为：

p＝pⁿ+J*σ

其中J为标定雅可比矩阵，σ为运动学参数和工具参数误差向量。

优选地，在所述模块M4中：

对于测量的n组数据，得到第i个点的实际位置p_i为：

p_i＝p_i ⁿ+J_i*σ，i＝1，2...，n

p_i ⁿ为第i个点的名义位置；J_i为第i个位置点的雅可比矩阵；

取测量的平面上的两点与测量n个点中的第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，...，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略；M_i、N_i为简要表述代式；

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得：

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2...，n-3；

第一个法向量指测量平面前3个点计算的法向量；

展开得：

y_i＝b_i*σ，i＝1，2...，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

y_i、b_i为简要表述代式；

建立单个平面的标定方程：

Y＝B*σ

其中，Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

Y、B、y_n、b_n均为简要表述对应的代式。

优选地，在机器人工作空间内移动平面，重复模块M2-M4k次，k取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；

设第k个平面的标定方程用y_k，i表示，则k个平面的标定方程为：

其中：

为、

为、y_k，n-3为、b_k，n-3均为简要表述对应的代式；

根据最小二乘法计算运动学参数和工具参数误差向量σ：

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

1、本发明的用于机器人的运动学标定装置和方法，可以对机器人运动学参数进行校正，提高其工作的绝度定位精度，非接触式测量，精度较高；

2、本发明使用激光位移传感器和标定平面，成本很低，操作简便，可以广泛应用于中小企业；

3、本装置采用的组成元件或设备均可从现有成熟商业产品中选择，在实际中具有良好的可行性；

4、本发明运动范围不受平面摆放位置限制，能在机器人较大工作空间内标定。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为标定示意图；

图2为标定流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1：

根据本发明提供的一种基于平面约束的机器人运动学标定方法，如图1-图2所示，包括：

步骤S1：建立基坐标系、法兰坐标系、工具坐标系；

步骤S2：操作机器人移动，记录激光位移传感器度数、机器人关节角度；

步骤S3：建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；

步骤S4：根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；

在机器人工作空间内重复步骤S2-S4，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量。

具体地，在所述步骤S1中：

机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

机器人工作空间内固定一块平面，平面度等级为00级以上；

具体地，在所述步骤S2中：

操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数l和记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据，n取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程。

具体地，在所述步骤S3中：

采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

为机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵，n为机器人法兰中心建立法兰坐标系，

为N连杆相对于n-1连杆的齐次变换矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的旋转矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的位置矩阵，R为旋转矩阵，p为位置矩阵；

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数；

平面上激光点在机器人基坐标系下的位置为：

P为平面上激光点在机器人基坐标系下的位置矩阵，I为单位旋转矩阵，t为工具坐标系；

表示激光发射点在法兰坐标系下的位置偏置，与机器人运动学误差量一起作为标定的误差量，平面上激光点在机器人基坐标系的名义位置pⁿ为：

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置p为：

p＝pⁿ+J*σ

其中J为标定雅可比矩阵，σ为运动学参数和工具参数误差向量。

具体地，在所述步骤S4中：

对于测量的n组数据，得到第i个点的实际位置p_i为：

p_i＝p_i ⁿ+J_i*σ，i＝1，2...，n

p_i ⁿ为第i个点的名义位置；J_i为第i个位置点的雅可比矩阵；

取测量的平面上的两点与测量n个点中的第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，...，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略；M_i、N_i为简要表述代式；

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得：

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2...，n-3；

第一个法向量指测量平面前3个点计算的法向量；

展开得：

y_i＝b_i*σ，i＝1，2...，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

y_i、b_i为简要表述代式；

建立单个平面的标定方程：

Y＝B*σ

其中，

Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

Y、B、y_n、b_n均为简要表述对应的代式。

具体地，在机器人工作空间内移动平面，重复步骤S2-S4k次，k取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；

设第k个平面的标定方程用y_k，i表示，则k个平面的标定方程为：

其中：

为、

为、y_k，n-3为、b_k，n-3均为简要表述对应的代式；

根据最小二乘法计算运动学参数和工具参数误差向量σ：

实施例2：

实施例2为实施例1的优选例，以更为具体地对本发明进行说明。

本领域技术人员可以将本发明提供的一种基于平面约束的机器人运动学标定方法，理解为基于平面约束的机器人运动学标定系统的具体实施方式，即所述基于平面约束的机器人运动学标定系统可以通过执行所述基于平面约束的机器人运动学标定方法的步骤流程予以实现。

根据本发明提供的一种基于平面约束的机器人运动学标定系统，包括：

模块M1：建立基坐标系、法兰坐标系、工具坐标系；

模块M2：操作机器人移动，记录激光位移传感器度数、机器人关节角度；

模块M3：建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；

模块M4：根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；

在机器人工作空间内重复模块M2-M4，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量。

具体地，在所述模块M1中：

机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

机器人工作空间内固定一块平面，平面度等级为00级以上；

具体地，在所述模块M2中：

操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数1和记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据，n取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程。

具体地，在所述模块M3中：

采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

为机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵，n为机器人法兰中心建立法兰坐标系，

为N连杆相对于n-1连杆的齐次变换矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的旋转矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的位置矩阵，R为旋转矩阵，p为位置矩阵；

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数；

平面上激光点在机器人基坐标系下的位置为：

P为平面上激光点在机器人基坐标系下的位置矩阵，I为单位旋转矩阵，t为工具坐标系；

表示激光发射点在法兰坐标系下的位置偏置，与机器人运动学误差量一起作为标定的误差量，平面上激光点在机器人基坐标系的名义位置pⁿ为：

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置p为：

p＝pⁿ+J*σ

其中J为标定雅可比矩阵，σ为运动学参数和工具参数误差向量。

具体地，在所述模块M4中：

对于测量的n组数据，得到第i个点的实际位置p_i为：

p_i＝p_i ⁿ+J_i*σ，i＝1，2...，n

p_i ⁿ为第i个点的名义位置；J_i为第i个位置点的雅可比矩阵；

取测量的平面上的两点与测量n个点中的第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，...，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略；M_i、N_i为简要表述代式；

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得：

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2...，n-3；

第一个法向量指测量平面前3个点计算的法向量；

展开得：

y_i＝b_i*σ，i＝1，2...，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

y_i、b_i为简要表述代式；

建立单个平面的标定方程：

Y＝B*σ

其中，Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

Y、B、y_n、b_n均为简要表述对应的代式。

具体地，在机器人工作空间内移动平面，重复模块M2-M4k次，k取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；

设第k个平面的标定方程用y_k，i表示，则k个平面的标定方程为：

其中：

为、

为、y_k，n-3为、b_k，n-3均为简要表述对应的代式；

根据最小二乘法计算运动学参数和工具参数误差向量σ：

实施例3：

实施例3为实施例1的优选例，以更为具体地对本发明进行说明。

本发明的机器人运动学标定方法，利用激光位移传感器测量平面点坐标，根据平面内三点构成平面法向量平行为约束条件，得到标定方程，利用最小二乘法求出运动学参数。包含以下步骤：

(1)机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

(2)机器人工作空间内固定一块平面，可以为大理石，铁块等材质，平面度等级为00级或以上。

(3)操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数l，同时记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据。

(4)采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数。则平面上激光点在机器人基坐标系下的位置为

其中

表示激光发射点在法兰坐标系下的位置偏置，可以与机器人运动学误差量一起作为标定的误差量。平面上激光点在机器人基坐标系的名义位置为

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置为

p＝pⁿ+J*σ

其中J为标定雅可比矩阵，σ为运动学参数和工具参数误差向量。

(5)对于步骤3)测量的n组数据，由步骤4)可以得到第i个点的实际位置为

p_i＝p_i ⁿ+J_i*σ，i＝1，2...，n

取平面两点与第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，...，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略，

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2...，n-3；

展开得：

y_i＝b_i*σ，i＝1，2...，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

建立单个平面的标定方程：Y＝B*σ

，其中Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

(6)在机器人工作空间内移动平面，重复步骤3)，步骤4)，步骤5)k次，设第k个平面的标定方程y_k，i表示，则k个平面的标定方程为：

其中

根据最小二乘法计算运动学参数和工具参数误差向量

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (2)

1.一种基于平面约束的机器人运动学标定方法，其特征在于，包括：

步骤S1：建立基坐标系、法兰坐标系、工具坐标系；

步骤S2：操作机器人移动，记录激光位移传感器读数、机器人关节角度；

步骤S3：建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；

步骤S4：根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；

在机器人工作空间内重复步骤S2-S4，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量；

在所述步骤S4中：

对于测量的n组数据，得到第i个点的实际位置p_i为：

p_i＝p_i ⁿ+J_i*σ，i＝1，2…，n

p_i ⁿ为第i个点的名义位置；J_i为第i个位置点的雅可比矩阵；

取测量的平面上的两点与测量n个点中的第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，…，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略；M_i、N_i为简要表述代式；

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得：

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2…，n-3；

第一个法向量指测量平面前3个点计算的法向量；

展开得：

y_i＝b_i*σ，i＝1，2…，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

y_i、b_i为简要表述代式；

建立单个平面的标定方程：

Y＝B*σ

其中，

Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

Y、B、y_n、b_n均为简要表述对应的代式；

在所述步骤S1中：

机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光位移传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

机器人工作空间内固定一块平面，平面度等级为00级以上；

在所述步骤S2中：

操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数l和记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据，n取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；θ_i为机器人第i个关节角度；

在所述步骤S3中：

采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

为机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵，n为机器人法兰中心建立的法兰坐标系，

为n连杆相对于n-1连杆的齐次变换矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的旋转矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的位置矩阵，R为旋转矩阵，p为位置矩阵；

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数；

平面上激光点在机器人基坐标系下的位置为：

P为平面上激光点在机器人基坐标系下的位置矩阵，I为单位旋转矩阵，为工具坐标系；

表示激光发射点在法兰坐标系下的位置偏置，与机器人运动学误差量一起作为标定的误差量，平面上激光点在机器人基坐标系的名义位置pⁿ为：

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置p为：

p＝pⁿ+J*σ

其中J为雅可比矩阵，σ为运动学参数和工具参数误差向量；

在机器人工作空间内移动平面，重复步骤S2-S4k次，k取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；

设第k个平面的标定方程用y_k，i表示，则k个平面的标定方程为：

其中：

y_k，n-3、b_k，n-3均为简要表述对应的代式；

根据最小二乘法计算运动学参数和工具参数误差向量σ：

2.一种基于平面约束的机器人运动学标定系统，其特征在于，包括：

模块M1：建立基坐标系、法兰坐标系、工具坐标系；

模块M2：操作机器人移动，记录激光位移传感器读数、机器人关节角度；

模块M3：建立机器人的运动学模型，根据激光位移传感器读数，得到平面上激光点在坐标系的位置；

模块M4：根据平面上激光点的位置计算平面的法向量，建立单个平面的标定方程；

在机器人工作空间内重复模块M2-M4，直到运动学参数和工具参数误差向量是满秩的方程，计算运动学参数和工具参数误差向量；

在所述模块M4中：

对于测量的n组数据，得到第i个点的实际位置p_i为：

p_i＝p_i ⁿ+J_i*σ，i＝1，2…，n

p_i ⁿ为第i个点的名义位置；J_i为第i个位置点的雅可比矩阵；

取测量的平面上的两点与测量n个点中的第一个点计算平面的法向量

(p_i+2-p₁)×(p_i+1-p₁)

＝(p_i+2 ⁿ-p₁ ⁿ+J_i+2*σ-J₁*σ)×(p_i+1 ⁿ-p₁ ⁿ+J_i+1*σ-J₁*σ)，

其中，i＝1，2，...，n-2；

展开得

(p_i+2-p₁)×(p_i+1-p₁)＝M_i+N_i*σ，i＝1，2，...，n-2

其中

M_i＝(p_i+2 ⁿ-p₁ ⁿ)×(p_i+1 ⁿ-p₁ ⁿ)

N_i＝(p_i+2 ⁿ-p₁ ⁿ)×(J_i+1-J₁)+(J_i+2-J₁)(p_i+1 ⁿ-p₁ ⁿ)

σ的二次项忽略；M_i、N_i为简要表述代式；

实际平面的法向量之间平行，法向量之间的叉乘为0，取第一个法向量和任意法向量叉乘得：

M₁×M_i+1＝-(M₁×N_i+1+N₁×M_i+1)*σ，i＝1，2…，n-3；

第一个法向量指测量平面前3个点计算的法向量；

展开得：

y_i＝b_i*σ，i＝1，2...，n-3

y_i＝M₁×M_i+1

b_i＝-(M₁×N_i+1+N₁×M_i+1)

y_i、b_i为简要表述代式；

建立单个平面的标定方程：

Y＝B*σ

其中，Y＝[y₁，y₂，...，y_n-3]^T，B＝[b₁，b₂，...，b_n-3]^T

Y、B、y_n、b_n均为简要表述对应的代式；

在所述模块M1中：

机器人基座建立基坐标系0，机器人法兰中心建立法兰坐标系n，机器人法兰安装激光位移传感器，激光位移传感器建立工具坐标系T，工具坐标系原点与激光位移传感器激光发射点重合，工具坐标系Z方向与激光照射方向重合，工具坐标系的方向与法兰坐标系方向相同；

机器人工作空间内固定一块平面，平面度等级为00级以上；

在所述模块M2中：

操作机器人移动，使激光位移传感器的激光光束打在平面上，记录激光位移传感器的读数l和记录机器人关节角度θ_i，改变机器人位姿，重复测量n次数据，n取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；θ_i为机器人第i个关节角度；

在所述模块M3中：

采用D-H法建立机器人的运动学模型，将连杆坐标系i-1到连杆坐标系i的齐次变换矩阵表示为

则机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵为

为机器人末端法兰坐标系n相对于机器人基坐标系0的齐次变换矩阵，n为机器人法兰中心建立的法兰坐标系，

为n连杆相对于n-1连杆的齐次变换矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的旋转矩阵，

为机器人末端法兰坐标系n相对于机器人基坐标系0的位置矩阵，R为旋转矩阵，p为位置矩阵；

根据激光位移传感器读数，可以得到工具坐标系下平面上激光点的位置为[0，0，l，1]^T，其中l为激光位移传感器读数；

平面上激光点在机器人基坐标系下的位置为：

P为平面上激光点在机器人基坐标系下的位置矩阵，I为单位旋转矩阵，为工具坐标系；

表示激光发射点在法兰坐标系下的位置偏置，与机器人运动学误差量一起作为标定的误差量，平面上激光点在机器人基坐标系的名义位置pⁿ为：

pⁿ＝f(θ_i，l)

考虑机器人运动学误差和工具坐标系偏差，平面上激光点在机器人基坐标系的实际位置p为：

p＝pⁿ+J*σ

其中J为雅可比矩阵，σ为运动学参数和工具参数误差向量；

在机器人工作空间内移动平面，重复模块M2-M4k次，k取决于要标定误差的个数，保证运动学参数和工具参数误差向量是满秩的方程；

设第k个平面的标定方程用y_k，i表示，则k个平面的标定方程为：

其中：

y_k，n-3、b_k，n-3均为简要表述对应的代式；

根据最小二乘法计算运动学参数和工具参数误差向量σ：

Images