CN114872025B - 多关节机器人及其零位标定方法、装置和可读介质 - Google Patents

Abstract

本发明提供了一种多关节机器人及其零位标定方法。该零位标定方法包括：控制小臂的小臂轴线与底座的底座安装平面平行；控制第一关节匀速旋转，通过小臂惯性传感器获取小臂的第一动态加速度信息，根据第一动态加速度信息、大臂长度和小臂惯性传感器的位置计算第一偏转角；控制第四关节从第四初始位置旋转至目标位置，通过小臂惯性传感器获取小臂的第一加速度信息，根据第一加速度信息计算第四关节的第四偏转角；以及控制小臂中的电磁线圈靠近底座，控制电磁线圈产生方向相反的第一磁场和第二磁场，通过底座中的磁力计分别获得第一矢量和第二矢量，根据第一矢量和第二矢量之差在底座安装平面上的投影计算第一关节的第一偏转角。

Description

多关节机器人及其零位标定方法、装置和可读介质

技术领域

本发明主要涉及机器人领域，尤其涉及一种多关节机器人及其零位标定方法、装置和计算机可读介质。

背景技术

串联多关节机器人在出厂时要进行零位标定。多关节机器人的各活动关节在初始位置时具有相对误差，零位标定的目的是对零位相对误差进行辨识和补偿。目前的零位标定技术包括：(1)采用专业标定设备，比如激光跟踪仪、拉线编码器测量仪等，可以通用于不同厂家的机器人；(2)各个机器人厂家自主开发的标定设备。例一，在机械本体上预留安装位置，先利用水平仪或惯性传感器测量机器人安装平面的水平度，再把装有水平仪或惯性传感器的装置安装在机械本体相应位置上，分别测出该安装位置与安装平面之间的角度，推算机器人各个关节的零位位置。例二，在机械本体上预留孔位，设计一种插销装置，只有当机器人关节旋转到特定位置时插销才能插到位，通过该特定装置来推算零点位置。例三，在机械本体上预留安装位置，设计一种碰撞装置，将碰撞装置安装在机械本体上的安装位置上，当机器人关节旋转到特定位置时发生碰撞，机器人控制器通过碰撞检测，根据发生碰撞的位置来推算零点位置。如果机器人经过维修，需要进行拆装或电机更换，会使零位信息不匹配，则需要重新标定。根据上述的这些零位标定技术都需要依赖额外的设备和专业的工程人员，并且通常需要在客户现场进行零位标定，耗费人力物力，具有诸多不便。

发明内容

本发明要解决的技术问题是提供方便、准确、自动实现零位标定的多关节机器人及其零位标定方法、装置和计算机可读介质。

为解决上述技术问题，本发明提供了一种多关节机器人的零位标定方法，所述多关节机器人包括底座、大臂和小臂，所述大臂位于第二关节和第三关节之间，所述小臂位于第四关节和机器人末端之间，包括：控制所述小臂的小臂轴线与所述底座的底座安装平面平行；控制第一关节匀速旋转，所述第一关节位于第一初始位置，通过小臂惯性传感器获取所述小臂的第一动态加速度信息，根据所述第一动态加速度信息、大臂长度和所述小臂惯性传感器的位置计算第一偏转角，控制所述第二关节和所述第三关节旋转所述第一偏转角，使所述第二关节和所述第三关节位于各自的零位；控制所述第四关节从第四初始位置旋转至目标位置，通过所述小臂惯性传感器获取所述小臂的第一加速度信息，根据所述第一加速度信息计算所述第四关节的第四偏转角，控制所述第四关节从所述第四初始位置旋转所述第四偏转角，使所述第四关节位于其零位；以及控制所述第二关节和所述第三关节移动以使所述小臂中的电磁线圈位于靠近所述底座的目标位置，控制所述电磁线圈两端的电压使所述电磁线圈分别产生方向相反的第一磁场和第二磁场，读取所述底座中的磁力计的磁力计数据，所述磁力计数据包括所述第一磁场对应的第一矢量和所述第二磁场对应的第二矢量，使所述第一矢量和所述第二矢量相减获得第三矢量，获得所述第三矢量投影在所述底座安装平面上的第三投影矢量，根据所述磁力计的安装方向和所述第三投影矢量的当前方向计算第二偏转角，控制所述第一关节旋转所述第二偏转角，使所述第一关节位于其零位，其中，所述磁力计的安装方向和所述第一关节的零位方向重合。

在本申请的一实施例中，控制所述小臂的小臂轴线与所述底座的底座安装平面平行的步骤包括：当所述第一关节静止在所述第一初始位置时，通过所述小臂惯性传感器获取所述小臂的初始加速度信息；控制所述第一关节匀速旋转，通过所述小臂惯性传感器获取所述小臂的初始动态加速度信息；根据所述初始加速度信息和所述初始动态加速度信息计算所述小臂与所述底座安装平面之间的第一夹角；以及控制所述第二关节或所述第三关节旋转所述第一夹角，使所述小臂轴线与所述底座的底座安装平面平行。

在本申请的一实施例中，采用下面的公式计算所述第一夹角：

其中，θ表示所述第一夹角，(α0_x,α0_y,α0_z)表示所述初始加速度信息，(β0_x,β0_y,β0_z)表示所述初始动态加速度信息，(x,y,z)表示所述小臂惯性传感器的小臂坐标系的每个坐标轴。

在本申请的一实施例中，还包括：调整所述第一关节的位置到除所述第一初始位置之外的至少一个其他位置，当所述第一关节位于所述其他位置时，重新计算所述第一夹角，将所述第一初始位置对应的第一夹角和所述至少一个其他位置对应的至少一个第一夹角的平均值作为所述第一夹角。

在本申请的一实施例中，所述至少一个其他位置包括所述第一初始位置沿第一方向转动90度或所述第一初始位置沿第二方向转动90度，所述第一方向和所述第二方向相反。

在本申请的一实施例中，所述大臂长度是第二关节旋转轴和第三关节旋转轴之间的垂直距离。

在本申请的一实施例中，根据所述第一动态加速度信息、大臂长度和所述小臂惯性传感器的位置计算第一偏转角的步骤包括，采用下面的公式计算：

其中，γ表示所述第一偏转角，L表示所述小臂惯性传感器的位置到所述第三关节的距离，y表示所述小臂惯性传感器的小臂坐标系中的y轴，β1_y表示所述第一动态加速度信息中的y轴加速度，

表示所述第一关节的角速度，D表示所述大臂长度。

在本申请的一实施例中，还包括：调整所述第一关节的位置到除所述第一初始位置之外的至少一个其他位置，当所述第一关节位于所述其他位置时，重新计算所述第一偏转角，将所述第一初始位置对应的第一偏转角和所述至少一个其他位置对应的至少一个第一偏转角的平均值作为所述第一偏转角。

在本申请的一实施例中，控制所述第二关节和所述第三关节旋转所述第一偏转角的步骤包括：控制所述第二关节沿第一方向旋转所述第一偏转角，控制所述第三关节沿第二方向旋转所述第一偏转角，所述第一方向与所述第二方向相反。

在本申请的一实施例中，控制所述第四关节从第四初始位置旋转至目标位置，通过所述小臂惯性传感器获取所述小臂的第一加速度信息，根据所述第一加速度信息计算所述第四关节的第四偏转角的步骤包括：所述目标位置包括第一目标位置和第二目标位置，所述第一加速度信息包括第一目标加速度信息和第二目标加速度信息，根据所述第一目标加速度信息和所述第二目标加速度信息计算所述第四偏转角。

在本申请的一实施例中，所述第一目标位置是所述第四初始位置沿第一方向转动90度的位置，所述第二目标位置沿第二方向转动90度的位置，所述第一方向和所述第二方向相反，根据所述第一目标加速度信息和所述第二目标加速度信息计算所述第四偏转角的步骤包括，采用下面的公式计算所述第四偏转角：

α1_x sin(q₄)-α1_z cos(q₄)＝-α2_x sin(q₄)+α2_z cos(q₄)

其中，q₄表示所述第四偏转角，x、z分别表示所述小臂惯性传感器的小臂坐标系的x轴和z轴，α1_x、α1_z分别表示所述第一目标加速度信息中的x轴加速度和z轴加速度，α2_x、α2_z分别表示所述第二目标加速度信息中的x轴加速度和z轴加速度。

在本申请的一实施例中，所述小臂还包括第五关节，所述小臂惯性传感器和所述电磁线圈设置在所述第四关节和所述第五关节之间。

本申请为解决上述技术问题还提出一种多关节机器人，包括依次设置的底座、大臂和小臂，其特征在于，所述底座中包括底座惯性传感器和磁力计，所述底座惯性传感器和磁力计的第一安装平面平行于所述底座的底座安装平面，所述底座惯性传感器用于获得所述底座安装平面的位置信息；所述小臂中包括小臂惯性传感器和电磁线圈。

在本申请的一实施例中，所述小臂惯性传感器和所述电磁线圈位于第四关节和第五关节之间。

在本申请的一实施例中，还包括控制器，所述控制器用于执行如上所述的零位标定方法。

本申请为解决上述技术问题还提出一种多关节机器人的零位标定装置，包括：存储器，用于存储可由处理器执行的指令；处理器，用于执行所述指令以实现如上所述的方法。

本申请为解决上述技术问题还提出一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如上所述的方法。

本申请的多关节机器人的零位标定方法通过控制各个关节的运动来逐步调节各个关节的位置，在这个过程中，利用小臂惯性传感器所获得的小臂加速度信息，底座惯性传感器所获得的底座传感器安装平面加速度信息，小臂中的电磁线圈所产生的磁场使底座中的磁力计所感测到的磁力计数据信息等计算各个关节相对于其零位的偏转角，实现了对多关节机器人的自动零位标定过程。该零位标定方法具有方便、准确，可以自动实现，无需依赖额外的标定设备和专业的工程人员，节省零位标定的成本等诸多有益效果。

附图说明

包括附图是为提供对本申请进一步的理解，它们被收录并构成本申请的一部分，附图示出了本申请的实施例，并与本说明书一起起到解释本发明原理的作用。附图中：

图1是本申请一实施例的多关节机器人的立体结构示意图；

图2是本申请一实施例的多关节机器人的侧视结构示意图；

图3是本申请一实施例的多关节机器人的零位标定方法的示例性流程图；

图4是本申请一实施例的多关节机器人的各个关节的坐标系示意图；

图5是本申请一实施例的多关节机器人的零位标定方法中多关节机器人的状态图；

图6是本发明一实施例的多关节机器人的零位标定装置的系统框图。

具体实施方式

为了更清楚地说明本申请的实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构或操作。

如本申请和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

在本申请的描述中，需要理解的是，方位词如“前、后、上、下、左、右”、“横向、竖向、垂直、水平”和“顶、底”等所指示的方位或位置关系通常是基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，在未作相反说明的情况下，这些方位词并不指示和暗示所指的装置或元件必须具有特定的方位或者以特定的方位构造和操作，因此不能理解为对本申请保护范围的限制；方位词“内、外”是指相对于各部件本身的轮廓的内外。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本申请保护范围的限制。此外，尽管本申请中所使用的术语是从公知公用的术语中选择的，但是本申请说明书中所提及的一些术语可能是申请人按他或她的判断来选择的，其详细含义在本文的描述的相关部分中说明。此外，要求不仅仅通过所使用的实际术语，而是还要通过每个术语所蕴含的意义来理解本申请。

本申请中使用了流程图用来说明根据本申请的实施例的系统所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，可以按照倒序或同时处理各种步骤。同时，或将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。

本申请的多关节机器人及其零位标定方法在多关节机器人中设置小臂惯性传感器、电磁线圈、底座惯性传感器和磁力计，通过对各个关节进行控制，实现该多关节机器人的自动零位标定，该零位标定方法方便、准确，无需使用额外的标定设备。本申请的多关节机器人包括但不限于串联多关节机器人，包括但不限于工业机器人、协作机器人等。本申请对多关节机器人的关节数量不做限制。本申请的多关节机器人的关节数量为至少4个。本说明书将以6关节机器人为例进行说明，不用于限制关节的具体数量。

图1是本申请一实施例的多关节机器人的立体结构示意图。参考图1所示，该实施例的多关节机器人包括6个关节，分别标示为第一关节J1、第二关节J2、第三关节J3、第四关节J4、第五关节J5和第六关节J6。其中，每个关节可以沿着图1中所示的各自弯曲箭头方向旋转，该方向包括正方向(+)和负方向(-)。该实施例的多关节机器人包括底座110、大臂120和小臂130，大臂120位于第二关节J2和第三关节J3之间，小臂130位于第四关节J4和机器人末端之间。在图1中，机器人末端指第六关节J6所处的位置。根据该实施例，小臂130的小臂轴线指第四关节J4的旋转轴线。

图1所示不用于限制本申请的多关节机器人的具体关节数量、形状和大小。在一些实施例中，小臂130还包括第五关节J5。在一些实施例中，小臂130包括第四关节J4、第五关节J5和位于第四关节J4和第五关节J5之间的第四连杆140。在图1中还示出了位于末端的第六关节J6。小臂130沿着第四关节J4的旋转轴旋转，同时带动第五关节J5、第六关节J6旋转。

图2是本申请一实施例的多关节机器人的侧视结构示意图。结合图1和图2所示，底座110具有底座安装平面111，该底座安装平面111可以是底座110的底面。理想情况下，底座安装平面111应该是平行于水平面的。但是，在实际情况下或者根据用户的需求，底座安装平面111并不平行于水平面。参考图2，在底座110中设置有底座惯性传感器210，底座惯性传感器210具有平行于底座安装平面111第一安装平面211。底座惯性传感器210可以测量机器人在移动时的底座传感器安装平面加速度信息，其中包括重力的方向信息，根据该底座传感器安装平面加速度信息可以获得底座安装平面111和水平面之间的关系，从而可以将重力的方向信息带入动力学计算算法中，在本申请的零位标定方法的各个步骤中，可以准确地计算出各个关节需要的输出力矩。

参考图2，底座110中还设置有磁力计230。该磁力计230和底座惯性传感器210具有相同的第一安装平面211，因此，底座惯性传感器210所获得的加速度信息可以用于作为磁力计230的加速度信息。

在一些实施例中，底座惯性传感器210和磁力计230集成在同一个设备，例如芯片中。

参考图2，小臂130中包括小臂惯性传感器220和电磁线圈240。

在一些实施例中，小臂惯性传感器220和电磁线圈240设置在第四关节J4和第五关节J5之间。结合图1所示，小臂惯性传感器220和电磁线圈240可以安装在第四关节J4和第五关节J5之间的第四连杆140内部。

在一些实施例中，小臂惯性传感器220和电磁线圈240集成在同一个设备，例如芯片中。

图2所示不用于限制底座惯性传感器210、小臂惯性传感器220、磁力计230和电磁线圈240的大小、形状、类型和具体位置。

图3是本申请一实施例的多关节机器人的零位标定方法的示例性流程图。参考图3所示，该实施例的零位标定方法包括以下步骤：

步骤S310：控制小臂的小臂轴线与底座的底座安装平面平行；

步骤S320：控制第一关节匀速旋转，第一关节位于第一初始位置，通过小臂惯性传感器获取小臂的第一动态加速度信息，根据第一动态加速度信息、大臂长度和小臂惯性传感器的位置计算第一偏转角，控制第二关节和第三关节旋转第一偏转角，使第二关节和第三关节位于各自的零位；

步骤S330：控制第四关节从第四初始位置旋转至目标位置，通过小臂惯性传感器获取小臂的第一加速度信息，根据第一加速度信息计算第四关节的第四偏转角，控制第四关节从第四初始位置旋转第四偏转角，使第四关节位于其零位；以及

步骤S340：控制第二关节和第三关节移动以使小臂中的电磁线圈位于靠近底座的目标位置，控制电磁线圈两端的电压使电磁线圈分别产生方向相反的第一磁场和第二磁场，读取底座中的磁力计的磁力计数据，磁力计数据包括第一磁场对应的第一矢量和第二磁场对应的第二矢量，使第一矢量和第二矢量相减获得第三矢量，获得第三矢量投影在底座安装平面的第三投影矢量，根据磁力计的安装方向和第三投影矢量的当前方向计算第二偏转角，控制第一关节旋转第二偏转角，使第一关节位于其零位，其中，磁力计的安装方向和第一关节的零位方向重合。

以下结合附图详细说明上述步骤S310-S340。

在一些实施例中，在步骤S310之前，各个关节处于任意位置。

在一些实施例中，在步骤S310之前，先控制各个关节大致处于其零位。可以通过人为方式观察各个关节，使各个关节处于其零位。可以理解，根据这样的方式所确定的零位具有一定的误差，相比于各个关节处于任意位置，在采用本申请的零位标定方法前将各个关节调整至大致零位，可以更加快速地将各个关节调整到其准确的零位。在图1所示的实施例中，每个关节都大致处于其零位。

在步骤S310，可以通过各种方式控制小臂130的小臂轴线与底座110的底座安装平面111平行。例如，通过人工控制或通过控制器控制第一关节J1、第二关节J2、第三关节J3和第四关节J4中的一个或多个，使小臂130的小臂轴线与底座110的底座安装平面111平行。

在一些实施例中，通过底座惯性传感器210所获得的底座安装平面111和水平面之间的关系，计算出各个关节需要的输出力矩，以使小臂130的小臂轴线与底座安装平面111平行。在一些实施例中，控制第二关节J2或第三关节J3移动，使小臂130的小臂轴线与底座110的底座安装平面111平行。该实施例中，仅控制第二关节J2或第三关节J3，其他关节都不移动。

在一些实施例中，步骤S310中的控制小臂130的小臂轴线与底座110的底座安装平面111平行的步骤包括：

步骤S311：当第一关节J1静止在第一初始位置P1时，通过小臂惯性传感器220获取小臂130的初始加速度信息。

本申请对第一初始位置P1的具体位置不做限制。如图1所示，假设第一关节J1在图1中所示的位置即为其第一初始位置P1，在该静止状态下，小臂惯性传感器220所测得的数据代表小臂130的初始加速度信息。根据该初始加速度信息可以获得重力方向。

步骤S312：控制第一关节J1匀速旋转，通过小臂惯性传感器220获取小臂130的初始动态加速度信息。本说明书用动态加速度信息表示惯性传感器在自身处于运动状态下所获得的加速度信息，区别于惯性传感器在自身处于静置状态下所获得的加速度信息。

本申请对第一关节J1的旋转速度不做限制。参考图1，当第一关节J1匀速旋转时，同时带动大臂120和小臂130匀速转动，此时小臂惯性传感器220具有向心加速度，则根据小臂惯性传感器220可以获得小臂130的初始动态加速度信息。

在步骤S312，除第一关节J1之外的其他关节都没有被控制而进行主动运动。

步骤S313：根据初始加速度信息和初始动态加速度信息计算小臂130与底座安装平面111之间的第一夹角。

在一些实施例中，采用下面的公式计算第一夹角：

其中，θ表示第一夹角，(α0_x,α0_y,α0_z)表示初始加速度信息，(β0_x,β0_y,β0_z)表示初始动态加速度信息，(x,y,z)表示小臂惯性传感器220的小臂坐标系的每个坐标轴。该小臂坐标系以及其中x轴、y轴、z轴可以具体地参考图4。

图4是本申请一实施例的多关节机器人的各个关节的坐标系示意图。如图4所示，以每个坐标系的原点O_i来区分不同的坐标系，其中，第0坐标系O₀表示底座110所在的坐标系，也用于表示第一关节J1对应的坐标系，Z₀轴表示第一关节J1的旋转轴；第1坐标系O₁和第2坐标系O₂用于表示第二关节J2对应的坐标系，当第一关节J1旋转时，第二关节J2可以绕Z₁轴旋转，Z₂轴表示第二关节J2自身的旋转轴；第3标系O₃用于表示第三关节对应的坐标系，Z₃轴表示第三关节J3的旋转轴；第4坐标系O₄用于表示第四关节J4对应的坐标系，Z₄轴表示第四关节J4的旋转轴；第5坐标系O₅用于表示第五关节J5对应的坐标系，Z₅轴表示第五关节J5的旋转轴；第6坐标系O₆用于表示第六关节J6对应的坐标系，Z₆轴表示第六关节J6的旋转轴；第7坐标系O₇用于表示当前多关节机器人末端工具对应的坐标系，当前该坐标系与第四关节J4的坐标系O₄相同。需要说明，图4所示每个坐标系的原点不用于限制其所对应关节的具体位置。其中，第4、5、6坐标系的原点O₄、O₅、O₆重合。

图4中还示出了小臂惯性传感器220的小臂坐标系O_k，在一些实施例中，小臂坐标系O_k中的Y_k方向与第3坐标系的Y₃方向一致，Z_k方向与第5坐标系的Z₅方向一致。

图4所示仅为示例，用于确定不同坐标系之间的相对位置关系。在其他的实施例中，各个坐标系的坐标轴指向可以不同。

在一些实施例中，步骤S313还包括：调整第一关节J1的位置到除第一初始位置P1之外的至少一个其他位置，当第一关节J1位于其他位置时，重新计算第一夹角，将第一初始位置P1对应的第一夹角和至少一个其他位置对应的至少一个第一夹角的平均值作为第一夹角。

本申请对上述的其他位置的具体位置和数量都不做限制。例如，如图1所示，第一关节J1可以沿正方向(+)或负方向(-)旋转某一预设角度，使第一关节J1处于一其他位置P2，此时通过小臂惯性传感器220获取小臂130的初始加速度信息，并执行步骤S312-S313，从而获得一第一夹角θ1。之后，再将第一关节J1沿正方向(+)或负方向(-)旋转某另一预设角度，使第一关节J1处于一其他位置P3，此时，执行步骤S312-S313，从而获得一第一夹角θ2。取θ、θ1、θ2的平均值作为最终的第一夹角θa。根据这些实施例，可以减小第一夹角θ的测量误差。

在一些实施例中，上述的至少一个其他位置包括第一初始位置P1沿第一方向转动90度或第一初始位置P1沿第二方向转动90度，第一方向和第二方向相反。如图1所示，第一方向可以是图1中第一关节J1的正方向(+)或负方向(-)，第二方向则相应地为第一关节J1的负方向(-)或正方向(+)。

在一些实施例中，将第一关节J1从第一初始位置P1沿第一方向转动90度，获得第一夹角θ1；使第一关节J1回到第一初始位置P1；再将第一关节J1从第一初始位置P1沿第二方向转动90度，获得第一夹角θ2，取θ、θ1、θ2的平均值作为最终的第一夹角θa。

步骤S314：控制第二关节J2或第三关节J3旋转第一夹角θ，使小臂轴线与底座110的底座安装平面111平行。

延续前例，根据经过步骤S311-S313计算出来的第一夹角θa，控制第二关节J2或第三关节J3旋转第一夹角θa，根据图4所示的坐标系关系，认为此时小臂轴线与底座安装平面111平行。第一夹角θa是第二关节J2的零位偏差ε2与第三关节J3的零位偏差ε3之和。

经过步骤S310，通过小臂惯性传感器220可以获得小臂的重力方向，此时，无论底座110是否处于水平位置，都可以计算出小臂130和底座安装平面111之间的第一夹角θ，并通过控制第二关节J2或第三关节J3旋转第一夹角θa，使小臂轴线与底座安装平面111平行。

在步骤S320，使第一关节J1匀速旋转，从而可以通过小臂惯性传感器220获得第一动态加速度信息。此时，第一关节J1处于第一初始位置P1。

在本步骤中，第一关节J1匀速旋转的速度与步骤S312中第一关节的旋转速度可以相同也可以不同。

在一些实施例中，步骤S320中根据第一动态加速度信息、大臂长度和小臂惯性传感器的位置计算第一偏转角的步骤包括，采用下面的公式计算：

其中，γ表示第一偏转角，L表示小臂惯性传感器的位置到第三关节的距离，y表示小臂惯性传感器的小臂坐标系中的y轴，β1_y表示第一动态加速度信息中的y轴加速度，

表示第一关节的角速度，D表示大臂长度。关于其中的各个坐标系及坐标轴可以具体参考图4。

参考图4所示，以小臂坐标系O_k的原点位置作为小臂传感器220的位置，L是从原点O_k到大臂的垂直距离；在小臂坐标系中，β1_y的y轴方向如图4中的Y_k；

为第一关节J1匀速旋转的角速度。

在一些实施例中，L表示O_k和第3坐标系的原点O₃之间的距离在Y₃方向上的投影。

在一些实施例中，L表示第三关节J3和第四关节J4的旋转轴线的公垂线与第四关节旋转轴线的交点与小臂传感器的位置之间的距离。当第三关节J3的旋转轴线和第四关节J4的旋转轴线共面时，就是第三关节J3和第四关节J4的旋转轴线的交点与小臂惯性传感器的位置的距离。

参考图4所示，在一些实施例中，大臂长度D是第二关节旋转轴Z₂和第三关节旋转轴Z₃之间的垂直距离。

在一些实施例中，步骤S320还包括：调整第一关节J1的位置到除第一初始位置P1之外的至少一个其他位置，当第一关节J1位于其他位置时，重新计算第一偏转角，将第一初始位置P1对应的第一偏转角和至少一个其他位置对应的至少一个第一偏转角的平均值作为第一偏转角。

本申请对上述的其他位置的具体位置和数量都不做限制。通过计算第一关节J1在不同位置时的不同第一偏转角γ获得第一偏转角的平均值γa，与前文所述的计算第一夹角的平均值θa的过程类似。根据这些实施例，可以减小第一偏转角γ的测量误差。

在一些实施例中，上述的至少一个其他位置包括第一初始位置P1沿第一方向转动90度或第一初始位置P1沿第二方向转动90度，第一方向和第二方向相反。如图1所示，第一方向可以是图1中第一关节J1的正方向(+)或负方向(-)，第二方向则相应地为第一关节J1的负方向(-)或正方向(+)。

在步骤S320中，计算出第一偏转角γ之后，控制第二关节J2和第三关节J3旋转该第一偏转角γ，从而使第二关节J2和第三关节J3都位于各自的零位。

在经过步骤S310之后，小臂轴线与底座安装平面111平行，此时的第二关节J2和第三关节J3不一定在零位，但由于小臂轴线与底座安装平面111平行，因此，第二关节J2当前的零位偏差和第三关节J3当前的零位偏差之和等于0。

根据步骤S320所计算的第一偏转角γ即为第二关节J2当前的零位偏差和第三关节J3当前的零位偏差，两个零位偏差的数值相等，方向相反。

在一些实施例中，步骤S320中的控制第二关节J2和第三关节J3旋转第一偏转角γ的步骤包括：控制第二关节J2沿第一方向旋转第一偏转角γ，控制第三关节J3沿第二方向旋转第一偏转角γ，第一方向与第二方向相反。如图1所示，第二关节J2的正方向和第三关节J3的正方向一致，第二关节J2的负方向和第三关节J3的负方向也一致。则第一方向可以是第二关节J2或第三关节J3的正方向(+)或负方向(-)，第二方向则相应地为其负方向(-)或正方向(+)。

根据步骤S320，使第二关节J2和第三关节J3分别位于各自的零位。

在本申请的实施例中，多关节机器人的各个关节的电机编码器信息是可以获得的，例如由控制器获得。本申请对该控制器具体如何设置不做限制，该控制器可以是集成在多关节机器人的本体中，也可以是独立的控制器，无论是哪一种设置，控制器可以与多关节机器人中的电机编码器、各种传感器通过总线建立通信连接，从而与电机编码器、各种传感器进行通信。该通信方式可以是有线或无线，本申请对此不做限制。

在步骤S320中控制第二关节J2旋转第一偏转角γ之后，第二关节J2位于其零位，控制器获得第二关节J2的当前电机编码器信息，并建立和存储该当前电机编码器信息和第二关节J2零位的对应关系。同理，也可以建立和存储第三关节J3的当前电机编码器信息和第三关节J3零位的对应关系。获得这种对应关系即实现了对该关节的零位标定。

在步骤S330中，第四关节J4的第四初始位置是经过了步骤S310-S320之后，第四关节J4的当前位置。该第四初始位置在步骤S330中是未知的。

本申请对步骤S330中的目标位置的具体位置和数量不做限制。目标位置至少包括一个除第四初始位置之外的位置。

在一些实施例中，步骤S330中控制第四关节J4从第四初始位置旋转至目标位置，通过小臂惯性传感器220获取小臂的第一加速度信息，根据第一加速度信息计算第四关节J4的第四偏转角的步骤包括：目标位置包括第一目标位置和第二目标位置，第一加速度信息包括第一目标加速度信息和第二目标加速度信息，根据第一目标加速度信息和第二目标加速度信息计算第四偏转角。

在一些实施例中，第一目标位置是第四初始位置沿第一方向转动90度的位置，第二目标位置沿第二方向转动90度的位置，第一方向和第二方向相反，根据第一目标加速度信息和第二目标加速度信息计算第四偏转角的步骤包括，采用下面的公式计算第四偏转角：

α1_x sin(q₄)-α1_z cos(q₄)＝-α2_x sin(q₄)+α2_z cos(q₄) (3)

其中，q₄表示第四偏转角，x、z分别表示小臂惯性传感器220的小臂坐标系的x轴和z轴，α1_x、α1_z分别表示第一目标加速度信息中的x轴加速度和z轴加速度，α2_x、α2_z分别表示第二目标加速度信息中的x轴加速度和z轴加速度。关于其中的各个坐标系及坐标轴可以具体参考图4。第四偏转角q₄表示第四关节J4当前的零位偏差。

在上述方程(3)中，只有第四偏转角q₄是未知数，通过代入已知量，可以求解第四偏转角q₄。本申请对具体的解方程方法不做限制。

结合图1所示，在步骤S330控制第四关节J4沿其旋转轴旋转，则第一方向可以是图1中的第四关节J4的正方向(+)或负方向(-)，第二方向则相应地为第四关节J4的负方向(-)或正方向(+)。

经过步骤S330之后，使第四关节J4位于其零位。在一些实施例中，控制器建立和存储第四关节J4的当前电机编码器信息和第四关节J4零位的对应关系。

图5是本申请一实施例的多关节机器人的零位标定方法中多关节机器人的状态图。步骤S340可以分为以下的步骤S341-S344，下面结合图5来说明步骤S340。

步骤S341：控制第二关节J2和第三关节J3移动以使小臂140中的电磁线圈240位于靠近底座110的目标位置。

参考图5所示，该目标位置即如图5中所示的电磁线圈240所处的位置，例如位于底座的前上方。不同机器人的机械臂的长度可以是不同的，在步骤S340，本申请对目标位置不做限制，只要使小臂140中的电磁线圈240尽量靠近底座110即可。

在一些实施例中，为使电磁线圈240靠近底座，将第二关节J2沿负方向旋转π/4，以及将第三关节J3沿负方向旋转π/4，使小臂140的小臂轴线尽量与底座安装平面111平行。

步骤S342：当电磁线圈240处于目标位置时，控制施加到电磁线圈两端的电压，使电磁线圈分别产生方向相反的第一磁场和第二磁场。磁力计230分别处于该第一磁场和第二磁场中，可以产生不同的磁力计数据，即第一磁场对应的第一矢量V1和第二磁场对应的第二矢量V2。需要说明，第一矢量V1和第二矢量V2都是在空间中具有其对应方向的矢量。

步骤S343：使第一矢量V1和第二矢量V2相减获得第三矢量V3，获得第三矢量V3投影在底座安装平面211上的第三投影矢量V3'，根据磁力计230的安装方向和第三投影矢量V3'的当前方向计算第二偏转角q₂。

需要说明，磁力计230的安装方向和第一关节J1的零位方向重合。在一些实施例中，磁力计230是三轴磁力计，在安装该磁力计230时，使其X方向和第一关节J1的零点方向重合。根据第三投影矢量V3'可以反映第一关节J1当前的方向，因此，通过计算第三投影矢量V3'的当前方向和磁力计230的安装方向之间的第二偏转角q₂，可以获知第一关节J1的当前方向与其零位之间的偏差。

步骤S344：控制第一关节旋转第二偏转角q₂，使第一关节位于其零位。可以理解，在此步骤中，假设第一关节与其零位之间的第二偏转角q₂为正值，则第一关节J1转动-q₂，以使第一关节J1回到零位。

经过步骤S340，根据所获得的第二偏转角q₂可获知第一关节J1的零位偏差，也就完成了对该多关节机器人第一关节J1到第四关节J4的零位标定。

在一些实施例中，控制器建立和存储第一关节J1的当前电机编码器信息和第一关节J1零位的对应关系。

需要说明，上述具体实施方式以图4所示坐标体系为参考，在其他的实施例中，根据坐标体系的不同设置，上述具体实施方式中的公式或方程也可以进行相应调整，在此不一一列举。

根据上述的步骤S310-S340，通过逐步使大臂中的第二关节、第三关节调零，使小臂中的第四关节调零，以及最后使第一关节调零，分别建立了第一关节J1到第四关节J4的电机编码器信息和其零位之间的对应关系，实现了对多关节机器人的自动零位标定过程。该零位标定方法具有方便、准确，可以自动实现，无需依赖额外的标定设备和专业的工程人员，节省零位标定的成本等诸多有益效果。

如图1和图2所示，本申请还提供一种多关节机器人，包括依次设置的底座110、大臂120和小臂130，底座110中包括底座惯性传感器210和磁力计230，底座惯性传感器210和磁力计230的第一安装平面211平行于底座110的底座安装平面111，底座惯性传感器210用于获得底座安装平面111的位置信息；小臂130中包括小臂惯性传感器220和电磁线圈240。该多关节机器人还包括控制器，控制器用于执行如前文所述的零位标定方法。前文的说明内容都可以用于说明本申请的多关节机器人。

本申请还包括一种多关节机器人的零位标定装置，包括存储器和处理器。其中，该存储器用于存储可由处理器执行的指令；处理器用于执行该指令以实现前文所述的多关节机器人的零位标定方法。

图6是本发明一实施例的多关节机器人的零位标定装置的系统框图。参考图6所示，该零位标定装置600可包括内部通信总线601、处理器602、只读存储器(ROM)603、随机存取存储器(RAM)604以及通信端口605。当应用在个人计算机上时，该零位标定装置600还可以包括硬盘606。内部通信总线601可以实现该零位标定装置600组件间的数据通信。处理器602可以进行判断和发出提示。在一些实施例中，处理器602可以由一个或多个处理器组成。通信端口605可以实现该零位标定装置600与外部的数据通信。在一些实施例中，该零位标定装置600可以通过通信端口605从网络发送和接受信息及数据。该零位标定装置600还可以包括不同形式的程序储存单元以及数据储存单元，例如硬盘606，只读存储器(ROM)603和随机存取存储器(RAM)604，能够存储计算机处理和/或通信使用的各种数据文件，以及处理器602所执行的可能的程序指令。处理器执行这些指令以实现方法的主要部分。处理器处理的结果通过通信端口传给用户设备，在用户界面上显示。

上述的零位标定方法可以实施为计算机程序，保存在硬盘606中，并可加载到处理器602中执行，以实施本申请的零位标定方法。

本发明还包括一种存储有计算机程序代码的计算机可读介质，该计算机程序代码在由处理器执行时实现前文所述的零位标定方法。

多关节机器人的零位标定方法实施为计算机程序时，也可以存储在计算机可读存储介质中作为制品。例如，计算机可读存储介质可以包括但不限于磁存储设备(例如，硬盘、软盘、磁条)、光盘(例如，压缩盘(CD)、数字多功能盘(DVD))、智能卡和闪存设备(例如，电可擦除可编程只读存储器(EPROM)、卡、棒、键驱动)。此外，本文描述的各种存储介质能代表用于存储信息的一个或多个设备和/或其它机器可读介质。术语“机器可读介质”可以包括但不限于能存储、包含和/或承载代码和/或指令和/或数据的无线信道和各种其它介质(和/或存储介质)。

应该理解，上文所描述的实施例仅是示意。本文描述的实施例可在硬件、软件、固件、中间件、微码或者其任意组合中实现。对于硬件实现，处理器可以在一个或者多个特定用途集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理设备(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器和/或设计为执行本文所述功能的其它电子单元或者其结合内实现。

本申请的一些方面可以完全由硬件执行、可以完全由软件(包括固件、常驻软件、微码等)执行、也可以由硬件和软件组合执行。以上硬件或软件均可被称为“数据块”、“模块”、“引擎”、“单元”、“组件”或“系统”。处理器可以是一个或多个专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理器件(DAPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器或者其组合。此外，本申请的各方面可能表现为位于一个或多个计算机可读介质中的计算机产品，该产品包括计算机可读程序编码。例如，计算机可读介质可包括，但不限于，磁性存储设备(例如，硬盘、软盘、磁带……)、光盘(例如，压缩盘CD、数字多功能盘DVD……)、智能卡以及闪存设备(例如，卡、棒、键驱动器……)。

计算机可读介质可能包含一个内含有计算机程序编码的传播数据信号，例如在基带上或作为载波的一部分。该传播信号可能有多种表现形式，包括电磁形式、光形式等等、或合适的组合形式。计算机可读介质可以是除计算机可读存储介质之外的任何计算机可读介质，该介质可以通过连接至一个指令执行系统、装置或设备以实现通讯、传播或传输供使用的程序。位于计算机可读介质上的程序编码可以通过任何合适的介质进行传播，包括无线电、电缆、光纤电缆、射频信号、或类似介质、或任何上述介质的组合。

上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述发明披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请示范实施例的精神和范围。

同时，本申请使用了特定词语来描述本申请的实施例。如“一个实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。

一些实施例中使用了描述成分、属性数量的数字，应当理解的是，此类用于实施例描述的数字，在一些示例中使用了修饰词“大约”、“近似”或“大体上”来修饰。除非另外说明，“大约”、“近似”或“大体上”表明所述数字允许有±20％的变化。相应地，在一些实施例中，说明书和权利要求中使用的数值参数均为近似值，该近似值根据个别实施例所需特点可以发生改变。在一些实施例中，数值参数应考虑规定的有效数位并采用一般位数保留的方法。尽管本申请一些实施例中用于确认其范围广度的数值域和参数为近似值，在具体实施例中，此类数值的设定在可行范围内尽可能精确。

Claims (17)

1.一种多关节机器人的零位标定方法，所述多关节机器人包括底座、大臂和小臂，所述大臂位于第二关节和第三关节之间，所述小臂位于第四关节和机器人末端之间，其特征在于，包括：

控制所述小臂的小臂轴线与所述底座的底座安装平面平行；

控制第一关节匀速旋转，所述第一关节位于第一初始位置，通过小臂惯性传感器获取所述小臂的第一动态加速度信息，根据所述第一动态加速度信息、大臂长度和所述小臂惯性传感器的位置计算第一偏转角，控制所述第二关节和所述第三关节旋转所述第一偏转角，使所述第二关节和所述第三关节位于各自的零位；

控制所述第四关节从第四初始位置旋转至目标位置，通过所述小臂惯性传感器获取所述小臂的第一加速度信息，根据所述第一加速度信息计算所述第四关节的第四偏转角，控制所述第四关节从所述第四初始位置旋转所述第四偏转角，使所述第四关节位于其零位；以及

控制所述第二关节和所述第三关节移动以使所述小臂中的电磁线圈位于靠近所述底座的目标位置，控制所述电磁线圈两端的电压使所述电磁线圈分别产生方向相反的第一磁场和第二磁场，读取所述底座中的磁力计的磁力计数据，所述磁力计数据包括所述第一磁场对应的第一矢量和所述第二磁场对应的第二矢量，使所述第一矢量和所述第二矢量相减获得第三矢量，获得所述第三矢量投影在所述底座安装平面上的第三投影矢量，根据所述磁力计的安装方向和所述第三投影矢量的当前方向计算第二偏转角，控制所述第一关节旋转所述第二偏转角，使所述第一关节位于其零位，其中，所述磁力计的安装方向和所述第一关节的零位方向重合。

2.如权利要求1所述的零位标定方法，其特征在于，控制所述小臂的小臂轴线与所述底座的底座安装平面平行的步骤包括：

当所述第一关节静止在所述第一初始位置时，通过所述小臂惯性传感器获取所述小臂的初始加速度信息；

控制所述第一关节匀速旋转，通过所述小臂惯性传感器获取所述小臂的初始动态加速度信息；

根据所述初始加速度信息和所述初始动态加速度信息计算所述小臂与所述底座安装平面之间的第一夹角；以及

控制所述第二关节或所述第三关节旋转所述第一夹角，使所述小臂轴线与所述底座的底座安装平面平行。

3.如权利要求2所述的零位标定方法，其特征在于，采用下面的公式计算所述第一夹角：

其中，θ表示所述第一夹角，(α0_x,α0_y,α0_z)表示所述初始加速度信息，(β0_x,β0_y,β0_z)表示所述初始动态加速度信息，(x,y,z)表示所述小臂惯性传感器的小臂坐标系的每个坐标轴。

4.如权利要求2所述的零位标定方法，其特征在于，还包括：调整所述第一关节的位置到除所述第一初始位置之外的至少一个其他位置，当所述第一关节位于所述其他位置时，重新计算所述第一夹角，将所述第一初始位置对应的第一夹角和所述至少一个其他位置对应的至少一个第一夹角的平均值作为所述第一夹角。

5.如权利要求4所述的零位标定方法，其特征在于，所述至少一个其他位置包括所述第一初始位置沿第一方向转动90度或所述第一初始位置沿第二方向转动90度，所述第一方向和所述第二方向相反。

6.如权利要求1所述的零位标定方法，其特征在于，所述大臂长度是第二关节旋转轴和第三关节旋转轴之间的垂直距离。

7.如权利要求6所述的零位标定方法，其特征在于，根据所述第一动态加速度信息、大臂长度和所述小臂惯性传感器的位置计算第一偏转角的步骤包括，采用下面的公式计算：

其中，γ表示所述第一偏转角，L表示所述小臂惯性传感器的位置到所述第三关节的距离，y表示所述小臂惯性传感器的小臂坐标系中的y轴，β1_y表示所述第一动态加速度信息中的y轴加速度，

表示所述第一关节的角速度，D表示所述大臂长度。

8.如权利要求7所述的零位标定方法，其特征在于，还包括：调整所述第一关节的位置到除所述第一初始位置之外的至少一个其他位置，当所述第一关节位于所述其他位置时，重新计算所述第一偏转角，将所述第一初始位置对应的第一偏转角和所述至少一个其他位置对应的至少一个第一偏转角的平均值作为所述第一偏转角。

9.如权利要求1所述的零位标定方法，其特征在于，控制所述第二关节和所述第三关节旋转所述第一偏转角的步骤包括：控制所述第二关节沿第一方向旋转所述第一偏转角，控制所述第三关节沿第二方向旋转所述第一偏转角，所述第一方向与所述第二方向相反。

10.如权利要求1所述的零位标定方法，其特征在于，控制所述第四关节从第四初始位置旋转至目标位置，通过所述小臂惯性传感器获取所述小臂的第一加速度信息，根据所述第一加速度信息计算所述第四关节的第四偏转角的步骤包括：所述目标位置包括第一目标位置和第二目标位置，所述第一加速度信息包括第一目标加速度信息和第二目标加速度信息，根据所述第一目标加速度信息和所述第二目标加速度信息计算所述第四偏转角。

11.如权利要求10所述的零位标定方法，其特征在于，所述第一目标位置是所述第四初始位置沿第一方向转动90度的位置，所述第二目标位置沿第二方向转动90度的位置，所述第一方向和所述第二方向相反，根据所述第一目标加速度信息和所述第二目标加速度信息计算所述第四偏转角的步骤包括，采用下面的公式计算所述第四偏转角：

α1_x sin(q₄)-α1_z cos(q₄)＝-α2_x sin(q₄)+α2_z cos(q₄)

其中，q₄表示所述第四偏转角，x、z分别表示所述小臂惯性传感器的小臂坐标系的x轴和z轴，α1_x、α1_z分别表示所述第一目标加速度信息中的x轴加速度和z轴加速度，α2_x、α2_z分别表示所述第二目标加速度信息中的x轴加速度和z轴加速度。

12.如权利要求1所述的零位标定方法，其特征在于，还包括：通过设置在所述底座中的底座惯性传感器获得所述底座安装平面的位置信息。

13.如权利要求1所述的零位标定方法，其特征在于，所述小臂还包括第五关节，所述小臂惯性传感器和所述电磁线圈设置在所述第四关节和所述第五关节之间。

14.一种多关节机器人，包括依次设置的底座、大臂和小臂，其特征在于，所述底座中包括底座惯性传感器和磁力计，所述底座惯性传感器和磁力计的第一安装平面平行于所述底座的底座安装平面，所述底座惯性传感器用于获得所述底座安装平面的位置信息；所述小臂中包括小臂惯性传感器和电磁线圈，还包括控制器，所述控制器用于执行如权利要求1-13任一项所述的零位标定方法。

15.如权利要求14所述的多关节机器人，其特征在于，所述小臂惯性传感器和所述电磁线圈位于第四关节和第五关节之间。

16.一种多关节机器人的零位标定装置，包括：

存储器，用于存储可由处理器执行的指令；

处理器，用于执行所述指令以实现如权利要求1-13任一项所述的方法。

17.一种存储有计算机程序代码的计算机可读介质，所述计算机程序代码在由处理器执行时实现如权利要求1-13任一项所述的方法。

Images