CN109262659A - 一种机械臂关节传感器的零位校准方法和设备 - Google Patents

Abstract

本发明公开了一种机械臂关节传感器的零位校准方法和设备，该方法包括：判断待测机械臂是否为冗余机械臂，若是则进行机械臂分段，将非冗余臂架作为设定末端臂架，执行如下单次校准法的步骤，直至完成该待测机械臂的所有关节的零位校准；将至少两块标靶平行安装在设定末端臂架上，各标靶的图案的匹配元素保持平行；将光源安装在机械臂搭载本体上；驱动机械臂各关节的运动，使光源的光束通过各标靶的轴心且光斑与各标靶的图案重叠；获取调整臂架位姿后的标靶的位姿，根据设定末端臂架的位姿与该光源的位姿之间的关系，求得设定末端臂架此时位姿下机械臂转动关节的旋转角和移动关节的移动量。相比现有技术，该方法进行零位校准时，更加简单高效。

Description

一种机械臂关节传感器的零位校准方法和设备

技术领域

本发明涉及工程机械领域，尤其涉及一种机械臂关节传感器的零位校准方法和设备。

背景技术

在工程机械应用中，在对例如矿用湿喷机等台机进行关节传感器零位校正时，常采用如下方法：通过旋转编码器或采用单个旋转变压器的解码装置来测量机械臂关节的角度，再通过计算该装置测得的角度和传感器反馈的数据进行比对来实现，但是这些装置成本较高，安装空间较大，有很大的局限性。

发明内容

本发明所要解决的技术问题之一是需要提供一种简单、高效的机械臂关节传感器的零位校准方法和设备。

为了解决上述技术问题，本申请的实施例首先提供了一种机械臂关节传感器的零位校准方法，该方法包括：判断步骤，判断待测机械臂是否为冗余机械臂，若是冗余机械臂，则进行机械臂分段，将非冗余臂架作为设定末端臂架，并执行如下单次校准法涉及的各个步骤直至完成该待测机械臂的所有关节的旋转角度/移动量的零位校准；

机械臂分段，其过程为以与机械臂搭载本体直接相连的臂架为起点进行分段，且分段后的每个机械臂区段的末端臂架均为非冗余。

单次校准法包括：标靶安装步骤，将至少两块标靶平行安装在待校准机械臂的设定末端臂架上，且各标靶的图案的匹配元素保持平行；光源安装步骤，将光源安装在机械臂搭载本体上；臂架调姿步骤，驱动机械臂各关节的运动调整臂架位姿，使得光源发出的光束通过各标靶的轴心且光斑与各标靶的图案重叠；单次校准步骤，获取调整臂架位姿后的标靶的位姿信息，并根据设定末端臂架的位姿信息与该光源的位姿信息之间的关系，求得该设定末端臂架此时位姿下机械臂转动关节的旋转角度和移动关节的移动量以完成零位校准。

对于已经校准完的关节传感器，其反馈值可作为实际值，应用到后续机械臂区段的其他关节传感器校准过程中。

在一个实施例中，在计算步骤中包括：臂架位姿矩阵建立子步骤，根据机械臂臂架结构，结合待求的机械臂各转动关节的旋转角，建立设定末端臂架的含关节旋转角的姿态矩阵和含关节移动量的位置矩阵；旋转角度计算子步骤，根据设定末端臂架的姿态与该光源的姿态之间的关系，利用光源的姿态信息和臂架姿态矩阵计算得到机械臂各转动关节的旋转角。

在一个实施例中，在所述计算步骤中，还包括：移动量计算子步骤，根据标靶的安装位置、标靶和光源的姿态关系以及标靶和设定末端臂架的姿态关系，利用设定末端臂架的位置矩阵，计算得到各移动关节的移动量。

在一个实施例中，在所述臂架位姿矩阵建立子步骤中，根据机械臂各臂架的长度，结合待求的机械臂各转动关节的未知旋转角，建立各关节坐标系之间的齐次变换矩阵，并依此确定设定末端臂架到机械臂载体之间的齐次变换矩阵，该齐次变换矩阵包括含关节旋转角的姿态矩阵和含关节移动量的位置矩阵。

在一个实施例中，在所述旋转角度计算子步骤中，通过下式计算机械臂各转动关节的旋转角：

其中，表示光源的姿态矩阵，α、β、γ表示姿态的欧拉角，表示包含关节旋转角的末端臂架的姿态矩阵。

在一个实施例中，在所述移动量计算子步骤中，执行如下操作：利用根据标靶的安装位置得到的标靶中心线与设定末端臂架之间的距离，以及标靶中心线上一点Q’与设定末端臂架关节中心点J构成的向量、与光源和Q’的向量之间的关系，得到包含未知移动量的Q’点坐标表达；将该Q’点坐标表达代入光源和标靶中心线上一点所成的关系表达，得到所述移动量。

在一个实施例中，在所述移动量计算子步骤中，在标靶中心线与设定末端臂架的中轴线重合时，执行如下操作：确定包含未知移动量的设定末端臂架关节中心点J坐标表达；将该J点坐标表达代入光源和该点所成的关系表达，得到所述移动量。

在一个实施例中，在所述计算步骤中，还包括：误差计算子步骤，对得到的该设定末端臂架此时位姿下机械臂各关节的旋转角和移动量和当前配置在各关节的传感器测量的角度值和移动量进行差值运算，完成零位校准。

在一个实施例中，在所述光源为光斑为十字形的激光发射器时，判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度，若具有，则判断该待测机械臂为冗余机械臂；或者，在所述光源为光斑为点圆形的激光发射器时，判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度或绕标靶中心线的转动自由度，若具有，则判断待测机械臂为冗余机械臂。

在一个实施例中，在所述判断步骤中，若判断待测机械臂为非冗余机械臂则直接执行所述单次校准法涉及的各个步骤。

根据本发明的另一方面，还提供一种机械臂关节传感器的零位校准设备，该设备包括：光源；至少两块标靶，每块标靶上具有图案，在校准的过程中，被平行安装在待校准机械臂的设定末端臂架上，且各图案的匹配元素保持平行；判断模块，其判断待测机械臂是否为冗余机械臂，若是冗余机械臂，则进行机械臂分段，将非冗余臂架作为设定末端臂架，并启动计算装置直至完成该待测机械臂的所有关节的旋转角度/移动量的零位校准；计算装置，其获取调整臂架位姿后的光源的位姿信息，并根据设定末端臂架的位姿信息与该光源的位姿信息之间的关系，求得该设定末端臂架此时位姿下机械臂转动关节的旋转角度和移动关节的移动量以完成零位校准，其中，通过调整臂架位姿使得光源发出的光束通过各标靶的轴心且光斑与各标靶的图案重叠。

在一个实施例中，所述计算装置，其包括：臂架位姿矩阵建立模块，其根据机械臂臂架结构，结合待求的机械臂各转动关节的旋转角，建立设定末端臂架的含关节旋转角的姿态矩阵和含关节移动量的位置矩阵；旋转角度计算模块，其根据设定末端臂架的姿态与该光源的姿态之间的关系，利用光源的姿态信息和臂架姿态矩阵计算得到机械臂各转动关节的旋转角。

在一个实施例中，所述计算装置，其还包括：移动量计算模块，其根据标靶的安装位置、标靶和光源的姿态关系以及标靶和设定末端臂架的姿态关系，利用设定末端臂架的位置矩阵，计算得到各移动关节的移动量。

在一个实施例中，所述臂架位姿矩阵建立模块，其根据机械臂各臂架的长度，结合待求的机械臂各转动关节的未知旋转角，建立各关节坐标系之间的齐次变换矩阵，并依此确定设定末端臂架到机械臂载体之间的齐次变换矩阵，该齐次变换矩阵包括含关节旋转角的姿态矩阵和含关节移动量的位置矩阵。

在一个实施例中，所述旋转角度计算模块，通过下式计算机械臂各转动关节的旋转角：

其中，表示光源的姿态矩阵，α、β、γ表示姿态的欧拉角，表示包含关节旋转角的末端臂架的姿态矩阵。

在一个实施例中，所述移动量计算模块，其执行如下操作：利用根据标靶的安装位置得到的标靶中心线与设定末端臂架之间的距离，以及标靶中心线上一点Q’与设定末端臂架关节中心点J构成的向量、与光源和Q’的向量之间的关系，得到包含未知移动量的Q’点坐标表达；将该Q’点坐标表达代入光源和标靶中心线上一点所成的关系表达，得到所述移动量。

在一个实施例中，所述移动量计算模块，在标靶中心线与设定末端臂架的中轴线重合时，其执行如下操作：确定包含未知移动量的设定末端臂架关节中心点J坐标表达；将该J点坐标表达代入光源和该点所成的关系表达，得到所述移动量。

在一个实施例中，所述计算装置，其还包括：误差计算模块，其对得到的该设定末端臂架此时位姿下机械臂各关节的旋转角和移动量和当前配置在各关节的传感器测量的角度值和移动量进行差值运算，完成零位校准。

在一个实施例中，所述光源为光斑为十字形或点圆形的激光发射器；

所述标靶为与所述激光发射器匹配的图案为十字形或点圆形的标靶。

在一个实施例中，在所述光源为光斑为十字形的激光发射器时，所述判断模块，其判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度，若具有，则判断该待测机械臂为冗余机械臂；或者，在所述光源为光斑为点圆形的激光发射器时，所述判断模块，其判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度或绕标靶中心线的转动自由度，若具有，则判断待测机械臂为冗余机械臂。

与现有技术相比，上述方案中的一个或多个实施例可以具有如下优点或有益效果：

根据本发明实施例，该方法采用激光发射器和标靶来进行位姿测量，不需要采用如背景技术那样的特定装置，且除了进行对这些测量装置的安装以外，后期的角度计算过程基本都是在计算装置中完成的，因此简化了测量设备和测量过程，提高了测量效率。而且，本发明实施例更多地利用机械臂本身运动机理计算获得结果，在降低了设备成本的同时减少主观测量产生的误差。另外，能够通过一次性计算获得各多个关节变量，进一步能够提高校准的效率。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明的技术方案而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构和/或流程来实现和获得。

附图说明

附图用来提供对本申请的技术方案或现有技术的进一步理解，并且构成说明书的一部分。其中，表达本申请实施例的附图与本申请的实施例一起用于解释本申请的技术方案，但并不构成对本申请技术方案的限制。

图1为本申请的机械臂关节传感器的零位校准设备的示例一的结构示意图。

图2为本申请的机械臂关节传感器的零位校准方法的示例一的流程示意图。

图3为本申请的机械臂关节传感器的零位校准设备的示例二的结构示意图。

图4为本申请的机械臂关节传感器的零位校准方法的示例二的流程示意图。

图5为本申请的一个具体示例的机械臂关节传感器的零位校准设备在应用时的装配示意图。

图6为本申请的图5所示的机械臂各关节的坐标系示意图。

图7（A）和图7（B）为本申请的各种类型标靶的示意图。

图8为本申请的机械臂关节传感器的零位校准设备的示例三的结构示意图。

图9为本申请的各研究对象的关系图。

图10为标靶与末端臂架安装示意图。

具体实施方式

以下将结合附图及实施例来详细说明本发明的实施方式，借此对本发明如何应用技术手段来解决技术问题，并达成相应技术效果的实现过程能充分理解并据以实施。本申请实施例以及实施例中的各个特征，在不相冲突前提下可以相互结合，所形成的技术方案均在本发明的保护范围之内。

为了更好地理解本申请，下面先简单介绍一下其中涉及到的一些技术内容。

本申请涉及针对冗余自由度机械臂和非冗余自由度机械臂的关节传感器零位校准。在本例中，机械臂是否为冗余自由度机械臂，是通过对比机械臂所具有的自由度和本发明的零位校准设备所能测量的机械臂自由度来确定的，即当机械臂所具有的自由度多于该设备可测量的机械臂自由度时，认为该机械臂为冗余自由度机械臂，反之，如果机械臂的自由度等于或少于设备可测量的机械臂自由度时，则称为非冗余机械臂。

这里的零位校准即各关节传感器的旋转角/移动量校准，在机械臂处于初始状态时，通过一系列操作使关节传感器的输出的旋转角/移动量与实测值之间的误差减到最小，二者达到无限接近。

机械臂运动学包含两个重要问题，一个由已知关节空间坐标系位姿，求取机械臂末端在工作空间坐标系位姿，另一问题是由已知末端在工作空间坐标系的位置，确定关节坐标系，前者称为前向运动学，即正运动学；后者称为反向运动学，即逆运动学。

本发明的技术原理简要概括如下：当机械臂各关节处于某一角度时，使得光束穿过两块标靶上的图案并与之重叠，此时获取标靶的多个自由度的位姿信息，并利用获取的信息、标靶安装位置和臂架的结构尺寸计算获取各关节的运动参数，记录下当前的关节参数，并以此信息对应校准各关节传感器的输出旋转角/移动量。下面先说明本发明涉及的技术原理性内容。

（一）本发明的零位校准方法校准目标包括移动关节的移动量l以及转动关节的转动角（又称旋转角）θ。研究对象包括四个（参见图9所示的各研究对象的关系图）：激光发射器（光源的一个例子）A，标靶B，末端臂架C，台车（机械臂载体的一个例子）D；其中，标靶B的实际研究对象为多个标靶的孔连线（或中心点连线）得到的中心线。研究内容为A、B、C、D的位置信息和姿态信息。

（二）根据测量条件可建立各研究对象之间的关系。

1、激光发射器A和标靶B之间的关系：

二者姿态相同；标靶中心线上任意一点T坐标为，则其与A点所构成的向量满足以下关系：

（1-1）

式中，为绕z轴旋转α角度的旋转变换矩阵；为绕y轴旋转β角度的旋转变换矩阵；k为常数，表示两个向量共线但大小不同。

2、标靶B与末端臂架C之间的关系：

参见图10，其为标靶与末端臂架安装示意图，其中1为末端臂架（截面），2为标靶，3为标靶上圆孔，h为标靶圆孔中心距臂架安装面的高度，a为末端臂架安装面与其理论中轴线之间的距离。

二者姿态相同；标靶中心线与末端臂架C之间距离d满足以下关系：

（1-2）

3、末端臂架C与台车D之间的关系：

在各关节中心点以及台车上建立坐标系，根据关节运动副情况，得到末端臂架C到台车D之间的齐次变换矩阵。

其中，为含关节转动角的姿态矩阵，表示末端臂架关节坐标系在台车坐标系下的姿态，其中m为机械臂中转动关节数量；为含关节移动量的位置矩阵，表示末端臂架关节坐标系，即末端臂架关节中心点，在台车坐标系下的位置坐标，其中p为机械臂中移动关节数量。

因此通过求解姿态矩阵可得转动关节的转动角；求解位置矩阵可得移动关节的移动量。

（三）求解

激光发射器在台车坐标系的位置坐标为（x，y，z），激光姿态用欧拉角α、β、γ表示，则激光发射器的姿态矩阵：

由于激光、标靶中心线以及末端臂架的姿态均相同，因此

（1-3）

根据式（1-3）可求得转动关节的转动角。

由第二节分析结果可知，假设J点为末端臂架关节中心点，则其在台车坐标系下的坐标为：

可以假设标靶中心线上一点台车坐标系下的坐标为，且点与末端臂架关节中心点J构成的向量与垂直，由以上可得以下关系式：

此外由于为标靶标靶中心线上一点，因此该点坐标满足式（1-1）。利用式（1-4），可将的坐标用J点坐标表示出来，并代入到式（1-5）中。

利用式（1-5），即可求解出其中移动关节的移动量，其中

上述求解过程为一般性的情况，而原文给定示例中，标靶中心线与末端臂架理论中轴线重合，因此d=0，J点坐标可以直接代入到式（1-5）进行求解中。

需要说明的是，此处之所以利用末端臂架上J的坐标来求标靶中心线上的一点，是因为考虑为假设的一点，采用上文的计算策略能更简便。实际根据上述关系同样可利用的坐标来推导出J点坐标，且方法不唯一。例如也可根据点在末端臂架的关节坐标系中的坐标，再结合齐次变换矩阵，求得在台车坐标系下的坐标。

，此处不做限定。

为了更好地了解本发明，下面说明本申请涉及到的几个实施例。

第一实施例

图1为本申请的机械臂关节传感器的零位校准设备的示例一的结构示意图，图2为本申请的机械臂关节传感器的零位校准方法的示例一的流程示意图。下面参考图1和图2来说明该实施例。

如图1所示，该零位校准设备1包括计算装置12、以及设置在台车上的激光发射器10和两块标靶11a和11b。其中，在每块标靶上设有图案，在本例中，该图案优选为大致“十”字形，例如实心十字形“┼”（参见图7（A）左图），当然，除了实心十字形以外还可以为空心十字形“╬”（参见图7（A）右图）。在校准的过程中，这两块标靶11a和11b被平行安装在待校准机械臂的设定末端臂架上，且各图案的匹配元素保持平行。具体来说，还以十字形标靶为例，横（图7（A）左图的标号“a”）和竖（图7（A）左图的标号“b”）形状作为图案的元素，在两个相同的十字标靶安装的时候，保证各标靶的横形状匹配元素彼此平行，竖形状匹配元素也彼此平行，换言之，保证各标靶的横形状在同一平面，竖形状在同一平面。

在本例中，激光发射器10优选为十字激光发射器，其发射出来的激光的光斑呈十字形，在校准的过程中被安装在台车上，保证其发射的光线不被其他物体遮挡。计算装置12，其获取调整臂架位姿后的激光发射器的N个自由度的位姿信息，并根据设定末端臂架位姿信息与该激光发射器的位姿信息之间的关系，进而求得该末端臂架此时位姿下机械臂转动关节的旋转角和移动关节的移动量以完成零位校准，其中，通过调整臂架位姿使得激光发射器的激光通过各标靶的轴心且光斑与各标靶的图案重叠。

具体地，如图1所示，计算装置12包括臂架位姿矩阵建立模块121、旋转角度计算模块122和移动量计算模块125，其中，臂架位姿矩阵建立模块121，其根据机械臂臂架结构，结合待求的机械臂各转动关节的旋转角，建立设定末端臂架的含关节旋转角的姿态矩阵和含关节移动量的位置矩阵；旋转角度计算模块122，其根据设定末端臂架的姿态与该光源的姿态之间的关系，利用光源的姿态信息和臂架姿态矩阵计算得到机械臂各转动关节的旋转角。移动量计算模块125，其根据标靶的安装位置、标靶和光源的姿态关系以及标靶和设定末端臂架的姿态关系，利用设定末端臂架的位置矩阵，计算得到各移动关节的移动量。

更具体地，臂架位姿矩阵建立模块121，其根据机械臂各臂架的长度，结合待求的机械臂各转动关节的未知旋转角，建立各关节坐标系之间的齐次变换矩阵，并依此确定设定末端臂架到机械臂载体之间的齐次变换矩阵，该齐次变换矩阵包括含关节旋转角的姿态矩阵和含关节移动量的位置矩阵。旋转角度计算模块122，通过上式（1-3）计算机械臂各转动关节的旋转角。

移动量计算模块125，其执行如下操作：利用根据标靶的安装位置得到的标靶中心线与设定末端臂架之间的距离，以及标靶中心线上一点Q’与设定末端臂架关节中心点J构成的向量、与光源和Q’的向量之间的关系（参见上式（1-4）），得到包含未知移动量的Q’点坐标表达；将该Q’点坐标表达代入光源和标靶中心线上一点所成的关系表达（参见上式（1-5）），得到所述移动量。

或者，移动量计算模块125，在标靶中心线与设定末端臂架的中轴线重合时，其执行如下操作：确定包含未知移动量的设定末端臂架关节中心点J坐标表达；将该J点坐标表达代入光源和该点所成的关系表达（参见后面式子（1-2’）和（1-3’）），得到所述移动量。

接下来，参考图2来说明如何采用上述的设备来完成对某一类型台车的机械臂关节传感器零位校准的过程。需要说明的是，在本实施例中，该方法是针对非冗余自由度机械臂的关节传感器校准法，也可以称为单次校准法。具体地采用如下测量工具：十字激光发射器（激光光斑为“十”字形）和中心带有“十”字形图案的标靶。由于是非冗余自由度机械臂，因此此处“设定的末端臂架”为机械臂实际的末端臂架，无需假定。

如图2所示，在步骤S210（标靶安装步骤）中，在机械臂末端臂架上正对着安装中心带有十字图案的两块相同标靶，在安装的过程中保证两个标靶十字的横、纵形状均在同一平面，且中轴线沿臂架延伸方向（可参见图5的标号B和C的中心点连线）。

在步骤S220（激光发射器安装步骤）中，将激光发射器安装在台车顶部，确保前方照射无障碍，测得其在台车坐标系中的位置坐标，以及激光发射方向的姿态。

具体来说，可以利用全站仪事先测量激光发射器在台车坐标系的位置坐标为（x，y，z），以及激光姿态为α、β、γ表示的欧拉角，因此获得有关激光发射器的6个自由度信息。利用全站仪来测量的方法为现有较为通用的技术，对此不再赘述。

在步骤S230（臂架调姿步骤）中，驱动机械臂各关节进行运动，调整臂架位姿，使得十字激光通过两块激光标靶的十字图形，此时可确定标靶位姿信息与激光发射器的位姿信息之间的关系。

具体地，在此状态下，标靶和激光发射器的姿态相同，且两标靶所处的位置连线与激光方向相同，参见上式（1-1）。

在步骤S240（臂架位姿计算步骤）中，臂架位姿矩阵建立模块121，其根据机械臂各臂架的长度，结合待求的机械臂各转动关节的旋转角，建立各关节坐标系之间的齐次变换矩阵，并依此确定设定末端臂架到机械臂载体之间的齐次变换矩阵（臂架位姿矩阵建立子步骤），该齐次变换矩阵包括含关节转动角的姿态矩阵和含关节移动量的位置矩阵，m为机械臂中转动关节数量，p为机械臂中移动关节数量。

在步骤S250（旋转角/移动量计算步骤）中，旋转角度计算模块122根据设定末端臂架的姿态与该光源的姿态之间的关系，利用光源的姿态信息和臂架姿态矩阵计算得到机械臂各转动关节的旋转角（旋转角度计算子步骤）。移动量计算模块125根据标靶的安装位置、标靶和光源的姿态关系以及标靶和设定末端臂架的姿态关系，利用设定末端臂架的位置矩阵，计算得到各移动关节的移动量（移动量计算子步骤）。

具体来说，在旋转角度计算子步骤中，通过式（1-3）可求得机械臂各转动关节的转动角（旋转角），在所述移动量计算子步骤中，利用根据标靶的安装位置得到的标靶中心线与设定末端臂架之间的距离，以及标靶中心线上一点Q’与设定末端臂架关节中心点J构成的向量、与光源和Q’的向量之间的关系，得到包含未知移动量的Q’点坐标表达；将该Q’点坐标表达代入光源和标靶中心线上一点所成的关系表达，得到所述移动量。在标靶中心线与设定末端臂架的中轴线重合时，执行如下操作：确定包含未知移动量的设定末端臂架关节中心点J坐标表达；将该J点坐标表达代入光源和该点所成的关系表达，得到所述移动量。

由此，本设备完成了移动关节的移动量l以及转动关节的转动角θ的计算，为后面零位校准做好准备。

下面以一个具体实施例，来说明如何计算台车上各机械臂关节的旋转角。

参见图5，其中，A为激光发射器，B、C为标靶，1~6为机械臂连接各关节运动的臂架（连杆），R₁、R₂、R₃、R₄为转动关节，其对应关节角为θ ₁ 、θ ₂ 、θ ₃ 、θ ₄ ，P₁为移动关节，其对应移动量l ₁ 。标靶十字中心与末端臂架6的中轴线重合，标靶十字的横、纵分别沿台车坐标系的y轴和z轴方向。本图仅作为校准工作的原理示意，需要说明的是，该例为理想状态下的示例，B、C标靶的中轴线与末端臂架的中心轴重合。

利用该图5来说明已知末端（在本例中为机械臂的臂架6）位姿求各关节运动量的计算推导过程。

当在E点建立台车坐标系O-x₀y₀z₀，各关节坐标系如图6所示。

在初始状态下，R₁转动轴线沿y₀轴方向；R₂转动轴线沿x₀轴方向；R₃转动轴线沿z₀轴方向；R₄转动轴线沿y₀轴方向；P₁为沿z₀轴方向的移动关节；激光发射器在台车坐标系的位置坐标为（x，y，z），激光姿态用欧拉角α、β、γ表示，臂架EF的长度为d₁；臂架FG的长度为d₂；臂架GH的初始长度为d₃，由于存在移动关节P₁，因此其实际长度为d₃+ l₁；臂架HI长度为d₄；臂架IJ的长度为d₅。

上述参数x、y、z、α、β、γ、d₁、d₂、d₃、d₄、d₅均可通过测量获得。

由此可得机械臂的各坐标系之间的齐次变换矩阵：

其中，表示坐标系j在坐标系i中的齐次变换矩阵，描述了坐标系j在坐标系i中的位置和姿态，其中描述坐标系j在坐标系i中的位置，描述坐标系j在坐标系i中的姿态；

则末端臂架上关节中心点J点在台车坐标系下坐标为

。

由于臂架6的姿态矩阵为。

而十字激光的姿态矩阵为：

由于B、C标靶的中轴线与末端臂架的中心轴重合（为一个理想状态），因此根据测量条件可知，

（1-1’）

此外，A、J所在直线沿着激光方向，因此向量与激光的姿态满足以下关系：

（1-2’）

（1-3’）

式中，为绕z轴旋转α角度的旋转变换矩阵、为绕y轴旋转β角度的旋转变换矩阵。

利用式（1-1’）以及式（1-3’），可求的各关节的运动量θ ₁、θ ₂、θ ₃、θ ₄、l ₁。

在步骤S260中（校准步骤）中，利用计算求得的关节的旋转角度/移动量数据与关节传感器反馈的数据进行对比校准。

在该步骤中将获得的各个关节的旋转角度/移动量与设置在各臂架处的关节传感器显示的数据进行比较，将二者之间的差值尽可能缩小到0，从而完成零位校准。

第二实施例

图3为本申请的机械臂关节传感器的零位校准设备的示例二的结构示意图，图4为本申请的机械臂关节传感器的零位校准方法的示例二的流程示意图。下面同时参考图3和图4来说明该实施例。

如图3所示，该零位校准设备100包括计算装置12’、以及设置在台车上的激光发射器10和两块标靶11a和11b。在本例中，采用的激光发射器10和标靶11a和11b与第一实施例大体一致，也是采用十字激光发射器（激光光斑为“十”字形）和中心带有“十”字形图案的标靶。计算装置12’除了具备与第一实施例的计算装置12大体一致的功能外，还包括判断模块123，该模块123判断待测机械臂是否为冗余机械臂，在非冗余机械臂的情况下，采用第一实施例的方法来进行零位校准，此处不再赘述。下面给出判断冗余机械臂的方法以及针对冗余机械臂如何进行零位校准。

在本例中，还以十字激光发射器为例，说明冗余机械臂的判断原理。

十字激光发射器可限定臂架五个自由度运动，只具有沿臂架方向的移动自由度，此时一次可测量的机械臂的五个自由度，也就是说，待测机械臂设定的末端臂架具有沿臂架末端方向（也可称为臂架延伸方向，或者说是激光照射方向、再或者是标靶中轴线方向，因为当激光穿过标靶后，臂架延伸方向与激光照射方向、标靶中轴线方向都是相同的。）的移动自由度时，认为该段机械臂关节自由度冗余。因此判断模块123判断待测机械臂是否存在激光发射器不能限定的自由度，即是否具有沿臂架末端方向的移动自由度，若存在，则判定该机械臂冗余，当机械臂自由度冗余时，则需要分段校准法，对机械臂进行分段，将非冗余臂架作为设定末端臂架，并执行单次校准法的步骤直至完成该待测机械臂的所有关节的旋转角度/移动量的零位校准；当机械臂自由度不冗余时，可使用如第一实施例所示的单次校准法。

判断模块123除了判断待测机械臂是否为冗余自由度机械臂以外，还用来判断是否所有关节的旋转角或移动量全部完成校准，容易理解，将已完成的校准关节数量与待测关节数量进行比较，若相同，则判断为全部校准，否则判断未全部校准，则返回步骤S410来执行。

接下来，参考图4来说明如何采用上述的设备来完成对某一类型台车的机械臂关节传感器零位校准的过程。需要说明的是，在本实施例中，与第一实施例的区别在于：该方法是针对冗余自由度机械臂的关节传感器校准法，也可以称为分段校准法。

如图4所示，在步骤S410中，利用计算装置12’的判断模块123来判断待测机械臂是否为冗余机械臂，即判断待测机械臂是否具有沿臂架末端方向的移动自由度，若有，则判断为冗余机械臂，否则，判断为非冗余机械臂。

在判断为待测机械臂为冗余机械臂的情况下，进行机械臂分段。

具体来说，由于机械臂是串联的，因此当整个机械臂是冗余时，则其中必然存在一个臂架是非冗余的，设定该臂架并以此臂架作为第一次校准的末端臂架，可称为末端臂架1。

执行第一实施例的单次校准法的步骤S210~S260，即进行标靶安装、激光发射器安装、臂架调姿、臂架位姿计算、关节旋转角/移动量计算、校准，此时完成末端臂架1与台车之间关节传感器的零位校准，此时传感器反馈信息真实可靠。

在步骤S420中，判断模块123还判断各关节角是否全部完成校准，若未完成，则返回步骤S410。

下面以还剩一个机械臂末端的关节角未校准为例，继续说明本实施例。

此时选取实际机械臂的末端作为二次校准的末端臂架，可称为末端臂架2。

重复S210~S260的过程：进行标靶安装、激光发射器安装、臂架调姿、臂架位姿计算、关节旋转角/移动量计算、校准，此时完成整个机械臂的校准。其中由于末端臂架1与台车之间的关节传感器已经经过零位校准，因此在关节旋转角/移动量计算时，这些关节角/移动量参数可直接读取传感器反馈信息，作为已知量，从而完成所有机械臂关节传感器的校准。

通过本发明实施例，能够采用简易的设备完成机械臂关节传感器的零位校准，而且，本实施例的方法比较简单，测量效率高、结果准确。

其他实施例

除了可以在第一实施例和第二实施例中采用十字激光发射器和图案为十字的标靶以外，还可以采用光斑为点圆形的点激光发射器和与该激光发射器匹配的图案为点圆形的标靶，此时，描述点激光姿态时，其具有确定欧拉角α、β，而欧拉角γ可为任意值，因此点激光可以限定臂架四个自由度的运动，机械臂设定的末端臂架具有沿臂架末端方向的移动自由度或绕标靶中轴线方向的转动自由度时，则认为待测机械臂冗余。在臂架调姿步骤中，通过驱动机械臂各关节进行运动，调整臂架位姿，使得点激光通过两块标靶的中心点，此时可以确定标靶四个自由度的位姿信息。

由于点激光可限定臂架四个自由度运动，仍具有沿臂架方向的移动自由度以及绕标靶中心线的转动自由度。当机械臂的设定末端臂架不具有沿臂架方向的移动自由度以及绕标靶中心线的转动自由度时，可使用单次校准法；当具有沿臂架方向的移动自由度或绕标靶中心线的转动自由度时，则需分段校准法，具体参考第二实施例。

当然，容易理解，标靶的图案不受上面实施例的限定，除了可以为点圆形和十字形的图案外，还可以根据实际情况采用其他图案，例如米字形标靶和匹配的光斑为米字形的激光发射器。

另外，除了设置两块标靶，还可以设置两块以上的标靶，只需在安装时保证各个标靶的图案的匹配元素平行，在臂架调姿时，保证激光发射器的光斑能同时穿越这些标靶的轴心且与这些标靶上的图案重叠。

在其他的实施例中，如图8所示，该零位校准设备的计算装置12还可以包括误差计算模块124，其对得到的该设定末端臂架此时位姿下机械臂各关节的旋转角和移动量和当前配置在各关节的传感器测量的角度值和移动量进行差值运算，完成零位校准（误差计算子步骤）。据此，能够协助用户在获得二者差值后，通过调节关节传感器的数值完成零位校准。

虽然本发明所揭露的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。

Claims (19)

1.一种机械臂关节传感器的零位校准方法，其特征在于，该方法包括：

判断步骤，判断待测机械臂是否为冗余机械臂，若是冗余机械臂，则进行机械臂分段，将非冗余臂架作为设定末端臂架，并执行如下单次校准法涉及的各个步骤直至完成该待测机械臂的所有关节的旋转角度/移动量的零位校准；

单次校准法包括：

标靶安装步骤，将至少两块标靶平行安装在待校准机械臂的设定末端臂架上，且各标靶的图案的匹配元素保持平行；

光源安装步骤，将光源安装在机械臂搭载本体上；

臂架调姿步骤，驱动机械臂各关节的运动调整臂架位姿，使得光源发出的光束通过各标靶的轴心且光斑与各标靶的图案重叠；

计算步骤，其获取调整臂架位姿后的光源的位姿信息，并根据设定末端臂架的位姿信息与该光源的位姿信息之间的关系，求得该设定末端臂架此时位姿下机械臂转动关节的旋转角度和移动关节的移动量以完成零位校准；

若判断待测机械臂为非冗余机械臂则直接执行所述单次校准法涉及的步骤。

2.根据权利要求1所述的零位校准方法，其特征在于，在计算步骤中包括：

臂架位姿矩阵建立子步骤，根据机械臂臂架结构，结合待求的机械臂各转动关节的旋转角，建立设定末端臂架的含关节旋转角的姿态矩阵和含关节移动量的位置矩阵；

旋转角度计算子步骤，根据设定末端臂架的姿态与该光源的姿态之间的关系，利用光源的姿态信息和臂架姿态矩阵计算得到机械臂各转动关节的旋转角。

3.根据权利要求2所述的零位校准方法，其特征在于，在所述计算步骤中，还包括：

移动量计算子步骤，根据标靶的安装位置、标靶和光源的姿态关系以及标靶和设定末端臂架的姿态关系，利用设定末端臂架的位置矩阵，计算得到各移动关节的移动量。

4.根据权利要求2所述的零位校准方法，其特征在于，在所述臂架位姿矩阵建立子步骤中，

根据机械臂各臂架的长度，结合待求的机械臂各转动关节的未知旋转角，建立各关节坐标系之间的齐次变换矩阵，并依此确定设定末端臂架到机械臂载体之间的齐次变换矩阵，该齐次变换矩阵包括含关节旋转角的姿态矩阵和含关节移动量的位置矩阵。

5.根据权利要求4所述的零位校准方法，其特征在于，在所述旋转角度计算子步骤中，通过下式计算机械臂各转动关节的旋转角：

其中，表示光源的姿态矩阵，α、β、γ表示姿态的欧拉角，表示包含关节旋转角的末端臂架的姿态矩阵。

6.根据权利要求3所述的零位校准方法，其特征在于，在所述移动量计算子步骤中，执行如下操作：

利用根据标靶的安装位置得到的标靶中心线与设定末端臂架之间的距离，以及标靶中心线上一点Q’与设定末端臂架关节中心点J构成的向量、与光源和Q’的向量之间的关系，得到包含未知移动量的Q’点坐标表达；

将该Q’点坐标表达代入光源和标靶中心线上一点所成的关系表达，得到所述移动量。

7.根据权利要求3所述的零位校准方法，其特征在于，在所述移动量计算子步骤中，在标靶中心线与设定末端臂架的中轴线重合时，执行如下操作：

确定包含未知移动量的设定末端臂架关节中心点J坐标表达；

将该J点坐标表达代入光源和该点所成的关系表达，得到所述移动量。

8.根据权利要求1~7任一项所述的零位校准方法，其特征在于，在所述计算步骤中，还包括：

误差计算子步骤，对得到的该设定末端臂架此时位姿下机械臂各关节的旋转角和移动量和当前配置在各关节的传感器测量的角度值和移动量进行差值运算，完成零位校准。

9.根据权利要求1所述的零位校准方法，其特征在于，

在所述光源为光斑为十字形的激光发射器时，判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度，若具有，则判断该待测机械臂为冗余机械臂；

或者，在所述光源为光斑为点圆形的激光发射器时，判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度或绕标靶中心线的转动自由度，若具有，则判断待测机械臂为冗余机械臂。

10.一种机械臂关节传感器的零位校准设备，其特征在于，该设备包括：

光源；

至少两块标靶，每块标靶上具有图案，在校准的过程中，被平行安装在待校准机械臂的设定末端臂架上，且各图案的匹配元素保持平行；

判断模块，其判断待测机械臂是否为冗余机械臂，若是冗余机械臂，则进行机械臂分段，将非冗余臂架作为设定末端臂架，并启动计算装置直至完成该待测机械臂的所有关节的旋转角度/移动量的零位校准；

计算装置，其获取调整臂架位姿后的光源的位姿信息，并根据设定末端臂架的位姿信息与该光源的位姿信息之间的关系，求得该设定末端臂架此时位姿下机械臂转动关节的旋转角度和移动关节的移动量以完成零位校准，其中，

通过调整臂架位姿使得光源发出的光束通过各标靶的轴心且光斑与各标靶的图案重叠。

11.根据权利要求10所述的零位校准设备，其特征在于，所述计算装置，其包括：

臂架位姿矩阵建立模块，其根据机械臂臂架结构，结合待求的机械臂各转动关节的旋转角，建立设定末端臂架的含关节旋转角的姿态矩阵和含关节移动量的位置矩阵；

旋转角度计算模块，其根据设定末端臂架的姿态与该光源的姿态之间的关系，利用光源的姿态信息和臂架姿态矩阵计算得到机械臂各转动关节的旋转角。

12.根据权利要求11所述的零位校准设备，其特征在于，所述计算装置，其还包括：

移动量计算模块，其根据标靶的安装位置、标靶和光源的姿态关系以及标靶和设定末端臂架的姿态关系，利用设定末端臂架的位置矩阵，计算得到各移动关节的移动量。

13.根据权利要求11所述的零位校准设备，其特征在于，

所述臂架位姿矩阵建立模块，其根据机械臂各臂架的长度，结合待求的机械臂各转动关节的未知旋转角，建立各关节坐标系之间的齐次变换矩阵，并依此确定设定末端臂架到机械臂载体之间的齐次变换矩阵，该齐次变换矩阵包括含关节旋转角的姿态矩阵和含关节移动量的位置矩阵。

14.根据权利要求13所述的零位校准设备，其特征在于，所述旋转角度计算模块，通过下式计算机械臂各转动关节的旋转角：

其中，表示光源的姿态矩阵，α、β、γ表示姿态的欧拉角，表示包含关节旋转角的末端臂架的姿态矩阵。

15.根据权利要求12所述的零位校准设备，其特征在于，所述移动量计算模块，其执行如下操作：

利用根据标靶的安装位置得到的标靶中心线与设定末端臂架之间的距离，以及标靶中心线上一点Q’与设定末端臂架关节中心点J构成的向量、与光源和Q’的向量之间的关系，得到包含未知移动量的Q’点坐标表达；

将该Q’点坐标表达代入光源和标靶中心线上一点所成的关系表达，得到所述移动量。

16.根据权利要求12所述的零位校准设备，其特征在于，所述移动量计算模块，在标靶中心线与设定末端臂架的中轴线重合时，其执行如下操作：

确定包含未知移动量的设定末端臂架关节中心点J坐标表达；

将该J点坐标表达代入光源和该点所成的关系表达，得到所述移动量。

17.根据权利要求10~16任一项所述的零位校准设备，其特征在于，所述计算装置，其还包括：

误差计算模块，其对得到的该设定末端臂架此时位姿下机械臂各关节的旋转角和移动量和当前配置在各关节的传感器测量的角度值和移动量进行差值运算，完成零位校准。

18.根据权利要求10所述的零位校准设备，其特征在于，

所述光源为光斑为十字形或点圆形的激光发射器；

所述标靶为与所述激光发射器匹配的图案为十字形或点圆形的标靶。

19.根据权利要求10所述的零位校准设备，其特征在于，

在所述光源为光斑为十字形的激光发射器时，所述判断模块，其判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度，若具有，则判断该待测机械臂为冗余机械臂；

或者，在所述光源为光斑为点圆形的激光发射器时，所述判断模块，其判断待测机械臂的设定末端臂架是否具有沿臂架末端方向的移动自由度或绕标靶中心线的转动自由度，若具有，则判断待测机械臂为冗余机械臂。