CN107414831B - 一种机械臂关节坐标计算系统及方法 - Google Patents

Abstract

本发明公开了一种机械臂关节坐标计算方法，包括：依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号；根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。本发明还公开了一种机械臂关节坐标计算系统。上述坐标计算方法，可以减少信号接收器的数量，缩小能量接收器阵列的面积，又可以降低测量和分析的难度，提高测量的效率。

Description

一种机械臂关节坐标计算系统及方法

技术领域

本发明涉及机械臂关节坐标计算技术领域，特别涉及一种机械臂关节坐标计算系统及方法。

背景技术

随着机械臂控制技术日益成熟和灵活性不断提高，机械臂代替人手在工业生产中扮演着越来越重要的角色，大大提高了生产效率。对机械臂的控制离不开对机械臂当前姿态的估计，只有获取了当前机械臂的姿态，才能进一步对机械臂发送控制信号。因此，在机械臂的控制过程中，通过获取机械臂关节的坐标来估计机械臂当前的姿态，是机械臂控制过程中的一个重要的环节。

当前主要的物体空间坐标测量方法是通过在空间物体上安装能量发射器(如激光发射器，电磁波发射器等)，并在待测量物体旁边安装能量接收器，通过在能量接收器处测量由于该能量信号的作用引起的属性值(如电阻、电流)的变化，结合接收器的位置和测量值的变化量来估算出物体的空间坐标。例如在待测量物体处向外发射一束激光，激光打到某光敏电阻上导致该光敏电阻的阻值变小，通过结合该电阻的坐标来估计出待测物体的坐标。

目前已有的实现机械臂关节坐标测量的方案是通过在平面上部署能量接收器阵列，并在机械臂关节上安装一个能量发射器，通过测量每个能量接收器的某特定属性值来确定接收器与发射器的位置关系，进而估计出当前发射器的位置，当前发射器的坐标就是机械臂关节的空间坐标。

发明内容

本发明的目的是提供一种机械臂关节坐标计算系统及方法，可以减少信号接收器的数量，缩小能量接收器阵列的面积，又可以降低测量和分析的难度，提高测量的效率。

为实现上述目的，本发明提供一种机械臂关节坐标计算方法，包括：

依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号；

根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。

相对于上述背景技术，本发明提供的机械臂关节坐标计算方法，摒弃了现有技术中直接在机械臂关节处设置能量发射器的方法，而是在非机械臂关节处设置多个能量发射器，按照在机械臂上的安装位置到机械臂关节的距离远近，将所有的发射器从小到大进行编号，所有的发射器安装的方向一致，每个发射器可以同时往水平和竖直两个垂直方向发射信号，并且所有的发射器由一个微型控制器控制，可以通过微型控制器控制发射器按照一定的频率依次发出能量信号；通过任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标；如此设置，避免了因机械臂的运动区域大所带来的现有技术中的测量方法需要的测量装置的测量范围要覆盖整个机械臂的运动区域、由此需要在两个平面上布置面积很大的信号接收器阵列、并且要处理大量测量出来的属性值的问题，进而提升了计算效率，而且降低能量损耗，节省资源。

优选地，所述依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号的步骤具体为：

依次获取由安装于非机械臂关节处的任意两个能量发射器所发射的能量信号；

根据两所述能量信号得到两个能量发射器的三维坐标A(x₁，y₁，z₁)和B(x₂，y₂，z₂)。

优选地，所述根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤具体为：

根据两个所述三维坐标A(x₁，y₁，z₁)和B(x₂，y₂，z₂)、点A相对于机械臂关节点Z的距离AZ、点B相对于机械臂关节点Z的距离BZ并通过如下三个公式计算得到机械臂关节点Z的三维坐标(x，y，z)；其中：

优选地，所述依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号的步骤具体为：

依次获取由安装于非机械臂关节处的任意三个能量发射器所发射的能量信号；

根据三所述能量信号得到三个均含有一个未知参数的能量发射器的三维坐标C(x₃，m，z₃)、D(x₄，n，z₄)和E(x₅，y₅，o)；其中，m、n和o为未知参数。

优选地，所述根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤具体为：

根据三个所述三维坐标C(x₃，m，z₃)、D(x₄，n，z₄)和E(x₅，y₅，o)，以及点C相对于点D之间的距离CD、点C相对于点E之间的距离CE以及点D相对于点E之间的距离DE并通过如下三个公式计算得到未知参数m、n和o；其中：

通过上述三个所述三维坐标C(x₃，m，z₃)、D(x₄，n，z₄)和E(x₅，y₅，o)中的任意两个点分别相对于机械臂关节点Z的距离以及任意两个点与机械臂关节点Z之间的直线的公式计算得到机械臂关节点Z的坐标。

优选地，所述依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号的步骤具体为：

依次获取由安装于非机械臂关节处的任意两个能量发射器所发射的能量信号；

判断获取的任意两个能量发射器所依次发射的能量信号所对应的能量发射器的坐标是否均为二维坐标；若是，则再次获取第三个能量发射器所发射的能量信号；并执行根据三个所述能量发射器之间的距离、三个所述能量发射器分别相对于所述机械臂关节的距离以及三个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤。

优选地，所述判断获取的任意两个能量发射器所依次发射的能量信号所对应的能量发射器的坐标是否均为二维坐标时，还包括：

当获取的任意两个能量发射器所依次发射的能量信号所对应的两个能量发射器的坐标具体为两个三维坐标或者一个三维坐标与一个二维坐标时，则执行根据两个所述能量发射器之间的距离、两个所述能量发射器分别相对于所述机械臂关节的距离以及两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤。

优选地，所述判断获取的任意两个能量发射器所依次发射的能量信号所对应的两个能量发射器的坐标是否均为二维坐标的具体步骤为：

根据两个相互垂直设置且每个能量接收器空间位置坐标已知的能量接收阵列分别接收由能量发射器朝向两个相互垂直方向发射的能量信号；

判断在两个能量接收器阵列中是否均有能量接收器接收到能量信号的强度能够达到强度阈值；若是，则执行下一步；

结合所述能量信号所对应于两所述能量接收器的空间位置坐标得到能量发射器的三维坐标；

若仅仅在一个方向上的能量信号的强度能够达到强度阈值，则通过结合达到强度阈值的能量信号所对应的能量接收器的空间位置坐标得到对应的能量发射器的二维坐标。

本发明提供一种机械臂关节坐标计算系统，包括：

能量信号接收模块：用于依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号；

关节坐标计算模块：用于根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。

附图说明

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

图1为本发明实施例所提供的一种机械关节臂的坐标计算方法的流程图；

图2为本发明实施例所提供的一种机械关节臂的坐标计算系统的结构框图；

图3为本发明实施例所提供的一种机械关节臂的坐标计算装置的结构简图；

图4为图1中的一种具体实施方式的算法流程图。

具体实施方式

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

为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。

请参考图1至图4，图1为本发明实施例所提供的一种机械关节臂的坐标计算方法的流程图；图2为本发明实施例所提供的一种机械关节臂的坐标计算系统的结构框图；图3为本发明实施例所提供的一种机械关节臂的坐标计算装置的结构简图；图4为图1中的一种具体实施方式的算法流程图。

本发明提供的一种机械臂关节坐标计算方法，如说明书附图1所示，主要包括：

S1、依次获取由安装于非机械臂关节处的任意相邻的至少两个能量发射器所发射的能量信号；

S2、根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。

在步骤S1中，在机械臂上安装多个能量发射器，多个能量发射器呈线状等间距分布在机械臂的每节手臂上，按照在机械臂上的安装位置到机械臂关节的距离远近，将所有的能量发射器从小到大进行编号，所有的发射器安装的方向一致，每个发射器可以同时往水平和竖直两个垂直方向发射信号，并且所有的能量发射器可由一个微型控制器控制，可以通过微型控制器控制发射器按照一定的频率依次发出能量信号，这些能量发射器组成了系统的能量发射装置。可以看出，每个能量发射器之间的距离已知，且每个能量发射器相对于机械臂关节11的距离也为已知。除此之外，能量接收装置可由水平和竖直平面上的两个能量接收器阵列组成，分别为水平能量接收器阵列31和竖直能量接收器阵列32；每个接收器阵列上均匀分布多个横纵交错设置的能量接收器，这些能量接收器具有一个特定的属性，该属性在接收能量发射器的信号后会产生相应的属性值变化，每个能量接收器的世界坐标是已知的，如说明书附图3所示。

信号测量装置连接能量接收装置，用于测量某能量信号落到能量接收阵列后能量接收器的特定属性值，从而提取出该接收器阵列中属性值变化最大的那个能量接收器，用于后面机械臂关节坐标的估计。

综上可知，可以利用能量接收装置依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号。

在步骤S2中，根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。其具体过程如下：

针对能量发射过程：

在微控制器的控制下，能量发射装置中的能量发射器按照编号顺序依次发射能量信号，且每个能量发射器每次同时发射水平和竖直两个方向上的信号，每两个相邻发射器发射信号的时间间隔为40ms，即每个能量发射器每次发射能量信号的时长也为40ms。

针对能量接收过程：

任意时刻能量信号落在能量接收器阵列中，能量接收器接收该能量信号，接收到该能量信号的能量接收器会产生相应的属性值变化，而其它没有接收到该能量信号的接收器属性值不变或者变化量很小。

针对属性测量过程：

接收到能量信号的能量接收器会产生属性值的改变，通过测量电路可以在能量接收器阵列中，提取出该信号引起的属性值变化量最大的那个能量接收器，这个接收器为发射器在接收器阵列所在平面上的投影，因此可以利用一个方向的信号估计出该能量发射器的二维坐标。假设判断出的属性值变化最大的信号接收器的空间坐标为(a₁，b₁，c₁),如果信号是来自垂直方向，则该信号发射器的坐标为(a₁，y，c₁)；如果信号是来自水平方向，则该信号发射器的坐标为(a₁，b₁，z)，其中y、z的值是未知的参数。

针对关节坐标估计过程：

如果能量发射器的一个方向上的能量信号落在一个能量接收器阵列上，就可以根据该信号引起的属性值变化，提取出属性值变化量最大的能量接收器，该接收器就是发射器在该能量接收器阵列上的投影，由该接收器的坐标可以得到发射器的二维坐标，通过获取三个这样的二维坐标，就可以估计出机械臂关节的三维坐标；如果一个能量发射器的两个方向上的能量信号均落在这两个能量接收器阵列上，则可以得到两个投影面上的二维坐标，进而确定出该发射器的三维坐标，获取两个这样的三维坐标，就可以估计出机械臂节点的三维坐标。

具体来说，倘若依次获取由安装于非机械臂关节处的任意两个能量发射器所发射的能量信号；且根据两所述能量信号能够得到两个三维坐标A(x₁，y₁，z₁)和B(x₂，y₂，z₂)，则根据两个所述三维坐标A(x₁，y₁，z₁)和B(x₂，y₂，z₂)、点A相对于机械臂关节点Z的距离AZ、点B相对于机械臂关节点Z的距离BZ并通过如下三个公式计算得到机械臂关节点Z的三维坐标(x，y，z)；其中：

倘若依次获取由安装于非机械臂关节处的任意三个能量发射器所发射的能量信号中，根据三所述能量信号得到三个均含有一个未知参数的能量接收器的三维坐标C(x₃，m，z₃)、D(x₄，n，z₄)和E(x₅，y₅，o)；其中，m、n和o为未知参数，则根据三个所述三维坐标C(x₃，m，z₃)、D(x₄，n，z₄)和E(x₅，y₅，o)，以及点C相对于点D之间的距离CD、点C相对于点E之间的距离CE以及点D相对于点E之间的距离DE并通过如下三个公式计算得到未知参数m、n和o；其中：

通过上述三个所述三维坐标C(x₃，m，z₃)、D(x₄，n，z₄)和E(x₅，y₅，o)中的任意两个点分别相对于机械臂关节点Z的距离以及任意两个点与机械臂关节点Z之间的直线的公式计算得到机械臂关节点Z的坐标。当然，倘若获取到三个发射器的三维坐标，则可以任意选取其中的两个计算直线方程，从而求解出机械臂关节点Z的坐标。

以说明书附图4为例：i为编号为i的能量发射器的编号，f为已确定二维坐标的能量发射器的个数，h为已确定三维坐标的能量发射器的个数，m为机械臂上的能量发射器的总个数；

步骤S10中，开始计算过程；

步骤S20中，设置属性值改变阈值，也即设置能量接收装置的灵敏度，以确定其准确坐标；

步骤S30中，将i设置为0，f设置为0，h设置为0；

步骤S40中，第i时刻编号为i的能量发射器发射能量信号，如上文所述的能量发射过程；

步骤S50中，判断是否有能量接收器的属性值改变量超过阈值，也即判断是否有能量信号落到能量接收器阵列中，从而判断是否能够准确得到当前能量发射器的位置坐标；

若是，则执行步骤S60，若否，则执行步骤S51；

步骤S51中，没有能量接收器的属性值改变量超过阈值，即当前能量发射器发射的能量信号并没有落到能量接收器阵列中，也即无法得到当前能量发射器的位置坐标，则将i＝i+1，也即下一个能量发射器发射信号，重复步骤S40；

步骤S60中，判断是否每一个信号接收器阵列均有能量接收器的属性值变化量超过阈值，即水平能量接收器阵列31和竖直能量接收器阵列32是否均能够接收到来自垂直方向的能量信号，也即是否能够准确获知能量发射器的位置(三维坐标)，若是，则执行步骤S61，若否，则执行步骤S62；

步骤S61中，确定在每个接收器阵列中属性值变化量最大的两个接收器并结合它们的坐标估计第i时刻发射器a的坐标；也即能够确定当前发射器a的三维坐标；此时h＝h+1(步骤S610)，表明已确定三维坐标的能量发射器的个数增加一个；步骤S611中，判断h是否为2，也即已确定三维坐标的能量发射器的个数是否为两个，若是，则可通过步骤S70，计算得到关节坐标，如上文所述的公式。

步骤S62中，仅仅水平能量接收器阵列31或者竖直能量接收器阵列32中有能量接收器的属性值变化超过阈值，也即无法得到当前能量发射器的三维坐标，由超出阈值的接收器的坐标得到第i时刻发射器i的二维平面坐标，此时f＝f+1(步骤S620)，表明已确定二维坐标的能量发射器的个数增加一个；步骤S621中，判断f是否为3，也即已确定二维坐标的能量发射器的个数是否为三个，若是，则可通过步骤S70，计算得到机械臂关节坐标，如上文所述的公式。也即，每个能量接收器阵列都有很多个能量接收器，并且每个能量接收器的坐标都是已知的，可以通过测量电路来测量属性值来确定属性值改变量达到阈值的能量接收器，该接收器的坐标用于关节坐标的计算。

步骤S70的具体计算方法如上文所述，即可求出当前状态下机械臂关节11的三维坐标；

步骤S80中，将h设置为0，f设置为0；步骤S90中，判断全部能量发射器是否均已发射能量，若是，则执行步骤S100；若否，则将i＝i+1，并重复步骤S40。

在步骤S611与步骤S621中，倘若已确定三维坐标的能量发射器的个数少于两个，已确定二维坐标的能量发射器的个数少于三个，则无法求得机械臂关节11的三维坐标，因此需执行步骤S90，在全部能量发射器尚未均已发射能量信号的前提下，令下一个能量发射器继续发射能量信号，以得到下一个能量信号的二维或三维坐标。

本发明提供的机械臂关节坐标计算方法，多个能量发射装置按预先设定好的编号依次发射能量信号，能量接收器可以根据接收到能量信号的时间来确定信号的来源；倘若能量发射器的一个方向上的能量信号落在一个能量接收器阵列上，可以得到发射器的二维坐标，通过动态获取三个这样的二维坐标，就可以计算出机械臂关节的三维坐标；如果一个能量发射器的两个方向上的能量信号均落在这两个能量接收器阵列上，则可以得到该发射器的三维坐标，通过动态获取两个这样的三维坐标，就可以计算得到机械臂节点的三维坐标。

本发明提供的一种机械关节臂的坐标计算系统，如说明书附图2所示，包括：

能量信号接收模块101：用于依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号；

关节坐标计算模块102：用于根据任意相邻的至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。

需要说明的是，在本说明书中，诸如第一和第二之类的关系术语仅仅用来将一个实体与另外几个实体区分开来，而不一定要求或者暗示这些实体之间存在任何这种实际的关系或者顺序。

以上对本发明所提供的机械臂关节坐标计算系统及方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (5)

1.一种机械臂关节坐标计算方法，其特征在于，包括：

依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号；

根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标；

所述依次获取由安装于非机械臂关节处的任意的至少两个能量发射器所发射的能量信号的步骤具体为：

依次获取由安装于非机械臂关节处的任意两个能量发射器所发射的能量信号；

判断获取的任意两个能量发射器依次发射的能量信号所对应的两个能量发射器的坐标是否均为二维坐标；若是，则再次获取第三个能量发射器所发射的能量信号；并执行根据三个所述能量发射器之间的距离、三个所述能量发射器分别相对于所述机械臂关节的距离以及三个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤；

所述判断获取的任意两个能量发射器所依次发射的能量信号所对应的两个能量发射器的坐标是否均为二维坐标时，还包括：

当获取的任意两个能量发射器所依次发射的能量信号所对应的两个能量发射器的坐标具体为两个三维坐标或者一个三维坐标与一个二维坐标时，则执行根据两个所述能量发射器之间的距离、两个所述能量发射器分别相对于所述机械臂关节的距离以及两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤。

2.根据权利要求1所述的坐标计算方法，其特征在于，所述依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号的步骤具体为：

依次获取由安装于非机械臂关节处的任意两个能量发射器所发射的能量信号；

根据两所述能量信号得到两个能量发射器的三维坐标A(x₁，y₁，z₁)和B(x₂，y₂，z₂)。

3.根据权利要求2所述的坐标计算方法，其特征在于，所述根据任意的至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标的步骤具体为：

根据两个所述能量发射器的三维坐标A(x₁，y₁，z₁)和B(x₂，y₂，z₂)、点A相对于机械臂关节点Z的距离AZ、点B相对于机械臂关节点Z的距离BZ并通过如下三个公式计算得到机械臂关节点Z的三维坐标(x，y，z)；其中：

4.根据权利要求1所述的坐标计算方法，其特征在于，所述判断获取的任意两个能量发射器所依次发射的能量信号所对应的两个能量发射器的坐标是否均为二维坐标的具体步骤为：

根据两个相互垂直设置且每个能量接收器空间位置坐标已知的能量接收阵列分别接收由能量发射器朝向两个相互垂直方向发射的能量信号；

判断在两个能量接收器阵列中是否均有能量接收器接收到能量信号的强度能够达到强度阈值；若是，则执行下一步；

结合所述能量信号所对应于两所述能量接收器的空间位置坐标得到对应的能量发射器的三维坐标；

若仅仅在一个方向上的能量信号的强度能够达到强度阈值，则结合达到强度阈值的能量信号所对应的能量接收器的空间位置坐标得到对应的能量发射器的二维坐标。

5.一种机械臂关节坐标计算系统，适用于上述权利要求1-4任一项所述的机械臂关节坐标计算方法，其特征在于，包括：

能量信号接收模块：用于依次获取由安装于非机械臂关节处的任意至少两个能量发射器所发射的能量信号；

关节坐标计算模块：用于根据任意至少两个所述能量发射器之间的距离、至少两个所述能量发射器分别相对于所述机械臂关节的距离以及至少两个所述能量发射器的空间坐标计算得到所述机械臂关节的三维空间坐标。

Images