CN117798937B - 机器人标定方法、系统及机器人 - Google Patents

Abstract

本发明涉及机器人标定技术领域，具体而言，涉及一种机器人标定方法、系统及机器人，该方法包括：若基于误差模型计算的最大位姿误差大于或等于设定误差，则修正运动学模型，以及基于修正后的运动学模型确定各点位的更新后位姿理论值；将点位的位姿实际值、更新后位姿理论值，输入误差模型得到各点位的更新后位姿误差；若各点位的更新后位姿误差呈现发散特征，则对多个点位进行择优筛选，以及基于择优筛选得到的点位对运动学模型进行修正。本发明实施例将初始点位加入误差模型，从而减小系统误差，提高收敛准确度；当计算误差不再趋于收敛时，择优点位，从而提高收敛精度，实现提高模型收敛稳定性。

Description

机器人标定方法、系统及机器人

技术领域

本发明涉及机器人标定技术领域，具体而言，涉及一种机器人标定方法、系统及机器人。

背景技术

机器人标定技术是提高机器人末端绝对定位精度的重要方法。以并联六轴机器人Hexapod为例，其机械结构由上下平台及六根驱动连杆构成，其下平台为固定平台，上平台为运动平台，通过改变六根驱动连杆的长度，实现上平台抵达不同的空间位姿。

然而实际生产的机器人，由于各种原因，其实际机械结构会存在不同程度的误差，若机器人控制仅依据理论模型进行控制，则会产生不同程度的误差，无法达到实际的控制效果。

传统标定方案，其构建完机器人模型后，辨识出误差参数，并代入测量点位进入迭代，以此寻求迭代出最优解。但传统模型算法迭代出的数据往往呈现出一定的发散特性，或陷入局部震荡，迭代效果往往无法达到目标预期。

发明内容

本发明的目的在于提供一种机器人标定方法，所述方法包括：建立机器人的三维坐标系；基于所述三维坐标系构建误差模型；获取机器人末端的初始点位的位姿实际值；基于预设的运动学模型确定所述初始点位、多个随机点位的位姿理论值，测量得到所述多个随机点位的位姿实际值；将所述初始点位的位姿实际值、位姿理论值，所述多个随机点位的位姿实际值、位姿理论值，输入所述误差模型得到各所述点位的位姿误差；若各所述点位的位姿误差中的最大位姿误差小于设定误差，则基于当前的误差模型确定最终的运动学模型；若所述最大位姿误差大于或等于所述设定误差，则修正所述运动学模型，以及基于修正后的运动学模型确定各所述点位的更新后位姿理论值；将所述初始点位的位姿实际值、更新后位姿理论值，所述多个随机点位的位姿实际值、更新后位姿理论值，输入所述误差模型得到各所述点位的更新后位姿误差；若各所述点位的更新后位姿误差呈现发散特征，则对多个所述点位进行择优筛选，以及基于择优筛选得到的点位对所述运动学模型进行修正。

可选地，所述若各所述点位的更新后位姿误差呈现发散特征，则对多个所述点位进行择优筛选，包括：若各所述点位的更新后位姿误差中的最大位姿误差大于上一阶段最大位姿误差，则确定为更新后位姿误差呈现发散特征；根据各所述点位的更新后位姿误差是否小于同一点位的所述上一阶段对应的位姿误差进行择优筛选；若所述点位的更新后位姿误差小于所述上一阶段对应的位姿误差，则保留对应的所述点位。

可选地，所述基于择优筛选得到的点位对所述运动学模型进行修正，包括：基于保留的所述点位对所述误差模型的误差矩阵元素进行更新，对未保留的所述点位则保留所述上一阶段对应的误差矩阵元素；基于更新后的所述误差模型对所述运动学模型进行修正。

可选地，所述方法还包括：若所述误差模型的误差矩阵元素在多次迭代后无变化，则基于当前的误差模型确定最终的运动学模型。

可选地，所述方法还包括：根据各所述点位的位姿误差确定对应的误差矩阵元素。

可选地，所述机器人为Hexapod并联机器人，所述Hexapod并联机器人包括6根驱动连杆，所述误差模型包括所述6根驱动连杆的12个铰点的三维坐标误差。

可选地，所述三维坐标系取所述Hexapod并联机器人的运动平台的中心点为坐标原点，Z轴竖直向上，X轴指向第一轴的连接点。

本发明实施例提供一种机器人标定系统，包括控制器，所述控制器用于执行上述方法。

本发明实施例提供一种机器人，包括控制器、运动平台及支撑臂；所述控制器包括上述机器人标定系统。

本发明实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现上述任一项所述的方法。

与现有技术相比，本发明的有益效果在于：

本发明实施例提供的机器人标定方法、系统及机器人，将初始点位加入误差模型，从而减小系统误差，提高收敛准确度；当计算误差不再趋于收敛时，择优点位，从而提高收敛精度，实现提高模型收敛稳定性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例提供的中心点位姿代入更新逻辑框图；

图2为本发明实施例提供的一种机器人标定方法的流程示意图；

图3为本发明实施例提供的择优筛选算法的流程示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面通过具体的实施例并结合附图对本发明做进一步的详细描述。

本发明实施例提出一种新的机器人标定算法，在传统算法模型的基础上，实现模型的快速收敛。

本发明实施例提供一种N+1点择优筛选标定法。

构建算法模型，如下：

（1）坐标系及点位建立

取固定平台理论圆心点为世界坐标系原点，轴竖直向上。

令为固定平台理论圆半径，/>为运动平台理论圆半径，/>为运动平台理论原点距离世界原点的高，/>为固定平台相邻靠近两铰点与固定平台中心的夹角，/>为运动平台相邻靠近两铰点与运动平台中心的夹角。

则基于世界坐标系，所有点位为：

A1=[LR;0;0];A2=Roz(-oA)*A1;A3=Roz(-2*pi/3)*A1;

A4=Roz(-2*pi/3-oA)*A1; A5=Roz(2*pi/3)*A1; A6=Roz(2*pi/3-oA)*A1;

AoB= (2*pi/3-oA-oB)/2;

B1=Roz(AoB)* [Lrr;0;h]; B6=Roz(oB)*B1;B2=Roz(-2*pi/3+oB)*B1;

B3=Roz(-2*pi/3)*B1;B5=Roz(2*pi/3)*B1; B4=Roz(2*pi/3+oB)*B1;

式中，为绕z轴旋转变换矩阵。

（2）机器人误差模型

以Hexapod机器人为例，其每个支链有7个误差参数，包括上下铰点的x, y, z定位误差及驱动杆轴向的长度误差。

默认支链的位移由高精编码器反馈，不考虑其产生的误差，则轴向长度误差由初始固定长度所产生。

由机器人逆解可知：

(1)

式中为支链长度，/>为支链的单位向量，/>为姿态变换矩阵，/>为位置偏移，/>为运动平台支链连接点位置，/>为固定平台支链连接点位置。

对公式两边微分可得：

(2)

已知，/>，/>，对(2)两侧乘/>：

(3)

其中，/>。

则机器人支链机构微分动量为：

(4)

则：

(5)

从式（5）可得出，机器人雅可比矩阵为6*42的行矩阵，故至少需要测量7点的坐标并组合矩阵，才能求解出其42个结构参数。

（3）传统标定算法

传统标定算法利用最小二乘法，代入计算点位值，计算理论点位误差，并将计算出的点位误差带入调整理论运动模型，并更新理论末端位姿，重新带入迭代。但传统算法在迭代中，最终的数据往往呈现发散特性，经过多次迭代，理论与实际的位姿误差往往呈现增大的效果，其相对最优常常出现在迭代的最初几次中，并且迭代效果往往无法达到目标预期。

在本发明实施例中提供新的机器人标定算法，具体如下。

（1）运动模型更新引入初始位姿

机器人所有点位计算均以末端中心位姿进行拟合，然而由于存在误差，初始末端点位并非处在理论零点位姿处，令为需要拟合的末端位姿，/>为理论初始中心点位姿，为实际初始中心点位姿。令：

(6)

(7)

修正运动学模型，关键在于构建修正运动学正解雅可比矩阵。基于误差模型所构建的误差函数，包含位姿偏移及误差所带来的驱动连杆增量，以及上下平台各铰点（x, y,z）位置误差。构建修正运动学正解雅可比矩阵，需要带入计算出的位置误差，并构建新的偏导方程。

(8)

为实际位置，/>为当前模型位置，/>为B点的位置偏差。

若采用原始点位，则拟合出的实际点位=。

而实际点位应该为：

(9)

可以看出，若不引入实际初始位姿，则在实际点位拟合中将缺乏误差，且很难补偿，故引入初始点位，则雅可比偏导方程为：

(10)

式中为代入误差的实际固定平台铰点位置，/>为连杆长度，常量偏导，仅需带入为0即可。

图1示出了中心点位姿代入更新逻辑框图，理论模型代入生成误差参数du，结合初始中心位姿代入生成新模型，基于新模型的误差更新初始中心位姿。

（2）计算点位择优筛选

原系统模型在生成误差参数du之后，更新模型，重新带入计算位姿误差后带入公式(5)计算新模型下的du，随后依次迭代循环。然而在实际计算中，多次迭代后结果往往呈现发散或震荡特性，在此情景下，直接套用公式(5)所生成的du并无法继续优化，为此本实施例中采用择优筛选的方式进行数据处理。

在实际程序中，误差模型经过多次迭代后，呈现出最优且未达到系统要求，则算法将陷入死循环，为此算法额外添加另一种程序出口，当/>元素无任何变化之后，算法陷入局部最优解，直接输出。

（3）仿真测试

在MATLAB仿真中，在计算点位择优筛选的逻辑基础上，设定实际误差参数，并随机生成n点实际点位（n>7），代入择优算法后，本实施例中算法比之原始算法能够有效呈现收敛特性，且收敛误差要远小于原始算法误差。在程序输出后，随机生成一末端点位，代入计算，校验误差，误差远小于原始程序输出的误差模型结果。

本实施例中采用n+1点测量方法，n为n个随机目标点，1为初始中心点，将中心点的误差模型代入，有利于提高模型精度；计算点位择优筛选，实现模型收敛，快速有效输出其有效范围内的相对最优解。

图2示出了本发明实施例提供的一种机器人标定方法的流程示意图，该方法包括：

S202，建立机器人的三维坐标系。

坐标系统的建立用于确定机器人初始理论位置，是计算的基础。坐标系统不同，初始位姿不同。机器人的坐标并不影响计算模型，仅是公式表达不同，故可根据情况，建立任意坐标系。本次算法取固定平台中心点为坐标原点，Z轴竖直向上，X轴指向第一轴的连接点。

以Hexapod并联机器人为例，三维坐标系取Hexapod并联机器人的运动平台的中心点为坐标原点，Z轴竖直向上，X轴指向第一轴的连接点。

S204，基于上述三维坐标系构建误差模型。

以Hexapod并联机器人为例，其现有理论误差模型共存在42个误差参数，即12个初始点位的XYZ误差，共36个误差参数，以及6个杆长误差，一共42个误差参数。

在上述理论算法中，杆长误差是运动分量和误差分量的叠加态，直接代入反而会增加计算误差。实际杆长即机器人连杆衔接点之间的距离长度，计算出实际点位之后，直接计算两点之间的距离，便可以求出实际杆长，因此最小误差参数集由42个误差缩减至36个误差。

S206，获取机器人末端的初始点位的位姿实际值。

测量机器人末端的初始点位的实际值，通过引入实际初始点位，能有效减小系统误差（理论证明如前文所示）。

S208，基于预设的运动学模型确定初始点位、多个随机点位的位姿理论值，测量得到多个随机点位的位姿实际值。

通过预设的运动学模型随机运行n个任意随机点位，以及上述初始点位，得到n+1个点位的位姿理论值，通过实际测量得到上述n个随机点位的位姿实际值。示例性地，上述n大于7。

S210，将初始点位的位姿实际值、位姿理论值，多个随机点位的位姿实际值、位姿理论值，输入误差模型得到各点位的位姿误差。

以上述n+1点位的实际值与理论值输入误差模型计算得到位姿误差。

S212，若上述各点位的位姿误差中的最大位姿误差小于设定误差，则基于当前的误差模型确定最终的运动学模型。

若计算得到的位姿误差中的最大位姿误差小于设定误差，可以认为运动学模型已满足条件，可以终止对运动学模型的迭代更新。

S214，若最大位姿误差大于或等于上述设定误差，则修正运动学模型，以及基于修正后的运动学模型确定各点位的更新后位姿理论值。

若最大位姿误差不满足条件，则需要继续修正运动学模型，以及进一步以迭代后的运行学模型计算上述各点位的更新后的位姿理论值。

S216，将初始点位的位姿实际值、更新后位姿理论值，多个随机点位的位姿实际值、更新后位姿理论值，输入误差模型得到各点位的更新后位姿误差。

以上述n+1点位的实际值与更新后位姿理论值再输入上述误差模型计算得到更新后的位姿误差。

S218，若各点位的更新后位姿误差呈现发散特征，则对多个点位进行择优筛选，以及基于择优筛选得到的点位对运动学模型进行修正。

在误差呈现发散或震荡特征，无法直接对其继续优化，在本实施例中采用择优筛选的方式进行数据处理，将上述各个点位中更新后误差小于上一阶段该点误差的点位保留（后续对该点位进行更新），将其他更新后误差大于上一阶段误差的点位不保留（后续不对该点位进行更新，继续采用上一阶段的参数）。通过择优筛选后的点位继续对运动学模型进行修正。

具体地，可以采用以下方式进行择优筛选：

首先，若上述各点位的更新后位姿误差中的最大位姿误差大于上一阶段最大位姿误差，则确定为更新后位姿误差呈现发散特征；

其次，根据上述各点位的更新后位姿误差是否小于同一点位的上一阶段对应的位姿误差进行择优筛选；若上述某点位的更新后位姿误差小于上一阶段对应的位姿误差，则保留对应的点位。

具体地，可以采用以下方式基于择优筛选得到的点位对运动学模型进行修正，包括：

首先，基于上述保留的点位对误差模型的误差矩阵元素进行更新，对上述未保留的点位则保留上一阶段对应的误差矩阵元素；其次，基于更新后的误差模型对运动学模型进行修正。

图3示出了本发明实施例中择优筛选算法的流程示意图，包括以下步骤：

S301，依据理论点位及测量点位，生成点位误差dS及其对应的误差矩阵矩阵元素。具体地，可以根据各点位的位姿误差确定对应的误差矩阵元素。

S302，择优选出7个点位，生成初始误差参数du。

S303，判断是否满足最大位姿误差＜设定误差。若是，则执行S304；若否，则执行S305。

S304，输出模型。

S305，更新模型，生成新的理论末端位姿。

S306，以此计算所有点位误差dS。

S307，判断是否满足最大位姿误差＜上一阶段最大位姿误差。若是，则执行S311；若否，则执行S308。

S308，判断是否满足单点误差＜上一阶段该点误差。若是，则执行S309；若否，则执行S310。

S309，更新该点的误差矩阵元素。

S310，保留该点的上一阶段误差矩阵元素。

S311，生成所有误差矩阵元素及其对应dS误差。此步骤后返回继续执行S302。

进一步，若上述误差模型的误差矩阵元素在多次迭代后无变化，则基于当前的误差模型确定最终的运动学模型。在实际程序中，误差模型经过多次迭代后，dS呈现出最优且未达到系统要求，则算法将陷入死循环，为此算法额外添加另一种程序出口，当dS元素无任何变化之后，算法陷入局部最优解，直接输出。

本发明实施例提供的机器人标定方法，基于理论误差模型，加入初始点位，可以减小系统误差，提高收敛准确度；当计算误差不再趋于收敛时，点位择优筛选，实现模型收敛，快速有效输出其有效范围内的相对最优解，实现提高模型收敛稳定性。

本发明实施例提供一种机器人标定系统，包括控制器，所述控制器用于执行上述方法。

本发明实施例提供一种机器人，包括控制器、运动平台及支撑臂；所述控制器包括上述机器人标定系统。

本发明实施例还提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述方法实施例的各个过程，且能达到相同的技术效果，为避免重复，这里不再赘述。其中，所述的计算机可读存储介质，如只读存储器（Read-Only Memory，简称ROM）、随机存取存储器（Random Access Memory，简称RAM）、磁碟或者光盘等。

当然，本领域技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程度来指令控制装置来完成，所述的程序可存储于一计算机可读取的存储介质中，所述程序在执行时可包括如上述各方法实施例的流程，其中所述的存储介质可为存储器、磁盘、光盘等。

虽然本发明披露如上，但本发明并非限定于此。任何本领域技术人员，在不脱离本发明的精神和范围内，均可作各种更动与修改，因此本发明的保护范围应当以权利要求所限定的范围为准。

最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术 语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而 使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且 还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或 者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……” 限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例， 而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种机器人标定方法，其特征在于，所述方法包括：

建立机器人的三维坐标系；

基于所述三维坐标系构建误差模型；

获取机器人末端的初始点位的位姿实际值；

基于预设的运动学模型确定所述初始点位、多个随机点位的位姿理论值，测量得到所述多个随机点位的位姿实际值；

将所述初始点位的位姿实际值、位姿理论值，所述多个随机点位的位姿实际值、位姿理论值，输入所述误差模型得到各所述点位的位姿误差；

若各所述点位的位姿误差中的最大位姿误差小于设定误差，则基于当前的误差模型确定最终的运动学模型；

若所述最大位姿误差大于或等于所述设定误差，则修正所述运动学模型，以及基于修正后的运动学模型确定各所述点位的更新后位姿理论值；

将所述初始点位的位姿实际值、更新后位姿理论值，所述多个随机点位的位姿实际值、更新后位姿理论值，输入所述误差模型得到各所述点位的更新后位姿误差；

若各所述点位的更新后位姿误差呈现发散特征，则对多个所述点位进行择优筛选，以及基于择优筛选得到的点位对所述运动学模型进行修正。

2.根据权利要求1所述的方法，其特征在于，所述若各所述点位的更新后位姿误差呈现发散特征，则对多个所述点位进行择优筛选，包括：

若各所述点位的更新后位姿误差中的最大位姿误差大于上一阶段最大位姿误差，则确定为更新后位姿误差呈现发散特征；

根据各所述点位的更新后位姿误差是否小于同一点位的所述上一阶段对应的位姿误差进行择优筛选；

若所述点位的更新后位姿误差小于所述上一阶段对应的位姿误差，则保留对应的所述点位。

3.根据权利要求2所述的方法，其特征在于，所述基于择优筛选得到的点位对所述运动学模型进行修正，包括：

基于保留的所述点位对所述误差模型的误差矩阵元素进行更新，对未保留的所述点位则保留所述上一阶段对应的误差矩阵元素；

基于更新后的所述误差模型对所述运动学模型进行修正。

4.根据权利要求3所述的方法，其特征在于，所述方法还包括：

若所述误差模型的误差矩阵元素在多次迭代后无变化，则基于当前的误差模型确定最终的运动学模型。

5.根据权利要求3所述的方法，其特征在于，所述方法还包括：

根据各所述点位的位姿误差确定对应的误差矩阵元素。

6.根据权利要求1所述的方法，其特征在于，所述机器人为Hexapod并联机器人，所述Hexapod并联机器人包括6根驱动连杆，所述误差模型包括所述6根驱动连杆的12个铰点的三维坐标误差。

7.根据权利要求6所述的方法，其特征在于，所述三维坐标系取所述Hexapod并联机器人的运动平台的中心点为坐标原点，Z轴竖直向上，X轴指向第一轴的连接点。

8.一种机器人标定系统，其特征在于，包括控制器，所述控制器用于执行权利要求1-7任一项所述的方法。

9.一种机器人，其特征在于，包括控制器、运动平台及支撑臂；所述控制器包括权利要求8所述的机器人标定系统。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器读取并运行时，实现权利要求1-7任一项所述的方法。