CN111152260A - 一种串联旋转关节机械臂的关节转角辅助测量系统及方法 - Google Patents

Abstract

本发明提供了本发明提供了一种串联旋转关节机械臂的关节转角辅助测量系统，包括：姿态测量单元和计算机，所述姿态测量单元包括微处理器，和与微处理器电信号连接的三轴加速度计、三轴陀螺仪和三轴磁力计；姿态测量单元还具有固定端，姿态测量单元通过固定端固定设置在机械臂的每个关节的任一平面的任一位置上；姿态测量单元与计算机通讯连接；计算机用于根据姿态测量单元的姿态信息解算机械臂的关节转角；实现灵活方便、高效准确、不受环境影响的关节转角测量；使得测量不受环境限制，经过求解，可以获得串联型机械臂在运动过程中的关节转角信息，极大简化了机械臂关节转角测量过程。

Description

一种串联旋转关节机械臂的关节转角辅助测量系统及方法

技术领域

本发明属于机械臂关节转角测量技术领域，涉及一种利用包含有工业级三轴陀螺仪、工业级三轴加速度计以及工业级三轴磁力计的姿态检测单元，对串联旋转关节机械臂的关节转角进行辅助测量的测量方法。

背景技术

机械臂中包含的高精度编码器系统是用户获得机械臂在运动过程中的各个关节角度信息的主要手段。通常为了更加准确的监测机械臂在运动过程中的位置和姿态等信息，用户通常会引入一套关节转角辅助测量系统。

这套关节转角辅助测量系统可以作为辅助手段，监测机械臂的关节转角信息，为实现对机械臂进行故障诊断和参数修正等目的提供必要的关节角度辅助测量信息。但目前测量系统对串联旋转关节机械臂进行辅助关节角度测量时繁琐复杂，如何灵活方便的进行测量是需要解决的技术问题。

发明内容

为实现对串联旋转关节机械臂进行灵活方便的辅助关节角度测量，本发明提出一种串联旋转关节机械臂的关节转角辅助测量方法。该方法完全独立于机械臂本体中所包含的高精度编码器系统，适用于多自由度机械臂，利用包含有工业级三轴陀螺仪、工业级三轴加速度计以及工业级三轴磁力计的姿态检测单元，实现对串联旋转关节机械臂的关节转角测量。

本发明针对串联旋转关节机械臂的关节转角辅助测量的问题，提出了一种串联旋转关节机械臂的关节转角辅助测量方法，通过将包含有工业级三轴陀螺仪、工业级三轴加速度计以及工业级三轴磁力计的姿态检测单元以任意姿态固定于串联旋转关节机械臂的每个关节上，利用机械臂运动过程中各个姿态测量单元输出的四元数信息，实时求解机械臂各个关节的旋转角度，该测量方法的目的是为串联旋转关节机械臂提供一种易于部署、成本低、灵活性高的关节转角辅助测量方法。

本发明所采用的技术方案是：

第一方面，本发明提供了一种串联旋转关节机械臂的关节转角辅助测量系统，包括：姿态测量单元和计算机，所述姿态测量单元包括微处理器，和与微处理器电信号连接的三轴加速度计、三轴陀螺仪和三轴磁力计；姿态测量单元还具有固定端，姿态测量单元通过固定端固定设置在机械臂的每个关节的任一平面的任一位置上；

姿态测量单元与计算机通讯连接；

计算机用于根据姿态测量单元的姿态信息解算机械臂的关节转角。

进一步的，所述惯性测量设备还包括通信组件和电源组件。

进一步的，电源组件为微处理器、三轴加速度计、三轴陀螺仪、三轴磁力计和通信组件供电；通信组件用于将惯性测量设备解算得到的姿态信息通过串口、网线、蓝牙和WiFi方式传输到计算机。

本发明还提供了一种如第一方面的机械臂的关节转角辅助测量系统的使用方法，包括以下步骤：

搭建测量环境，在串联旋转关节机械臂的每个关节上固定一个姿态测量单元；

统一坐标系，将串联旋转关节机械臂的关节转角辅助测量系统中的各坐标系进行统一；

分解机械臂的复合运动，将机械臂的每个关节的复合运动分解为一系列各个关节的独立旋转运动；

分解姿态测量单元的复合姿态，将姿态测量单元所输出的复合姿态分解为一系列各个关节独立旋转运动时产生的姿态变化；

计算机械臂的关节转角，根据得到的机械臂各关节独立旋转运动的姿态变化，计算相应的关节转角。

进一步的，所述统一的坐标系包括：世界坐标系、固连在机械臂的基座上的基坐标系、姿态测量单元所处的导航坐标系、固连在姿态测量单元上的传感器坐标系和固连在机械臂各个关节上的关节坐标系；其中，导航坐标系取为北-东-地坐标系或北-西-天坐标系。

进一步的，所述分解机械臂的复合运动，是按照关节序号从高到低的顺序，将机械臂的复合运动分解为当前关节的独立旋转运动和与当前关节前向串联的各个关节的独立旋转运动，其中与机械臂基座相连的关节为1，机械臂末端工具相连的关节序号最高。

进一步的，所述姿态测量单元的复合姿态分解，可以用以下公式表示：

其中，Π表示连乘符号，q_n表示固定在关节n上的姿态测量单元输出的四元数，描述了由于复合运动导致的关节n的姿态变化。q_n|init表示机械臂位于初始状态时，固定在关节n上的姿态测量单元输出的四元数。q_n(r_i,θ_i)表示固定在关节n上的姿态测量单元输出的四元数，描述了机械臂的关节n做独立旋转运动时的姿态变化，r_i表示关节n独立旋转运动的旋转轴，θ_i表示关节n独立旋转运动的旋转角度，是所要测量的机械臂关节n的关节转角。

进一步的，根据公式(1)，计算得到N自由度机械臂各个关节独立旋转运动时的姿态变化，具有以下形式：

其中，q*表示共轭四元数。

进一步的，计算所述的机械臂的关节转角时，假设机械臂的关节n独立旋转运动时，固定在关节n上的姿态测量单元输出的四元数q_n(r_n,θ_n)具有以下形式：

q_n(r_n,θ_n)＝[p x y z]^T(3)

根据四元数和轴角对的关系，可以得到以下公式：

将根据公式(4)计算得到的角度定义为：

α_n＝2arccos p，α_n∈(0,π)(8)

进一步的，将公式(8)代入公式(5)-(7)，可以计算得到机械臂的关节n独立旋转运动时的旋转轴各分量如下：

进一步的，在实际过程中应避免机械臂的各关节在旋转时的角度趋近于零；将公式(9)代入公式(5)-(7)，可以得到以下三个计算关节转角的公式，

最终的关节n的旋转角度由旋转轴的最大分量所对应的公式计算得到，当旋转轴的r_y分量最大时，使用公式(10)中的第二个子公式来计算关节n在运动过程中的旋转角度。

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

1、本发明提出了一种串联旋转关节机械臂关节转角辅助测量方法，适用于任意自由度串联旋转关节型机械臂。利用包含有工业级三轴加速度计、工业级三轴磁力计和工业级三轴陀螺仪的姿态测量单元输出的姿态，实现灵活方便、高效准确、不受环境影响的关节转角测量。

2、本发明允许将姿态测量单元以任意姿态固定于机械臂的每个关节上，简化了设备安装流程。各个姿态测量单元可以通过蓝牙、WiFi、2.4G等方式将所测得的四元数信息实时传输到计算机中，使得测量不受环境限制。计算机根据接收到的各个姿态测量单元实时输出的四元数信息，利用本发明中提出的关节转角测量方法，经过求解，可以获得串联型机械臂在运动过程中的关节转角信息。

3、作为一种关节转角辅助测量手段，本发明基于姿态测量单元的串联旋转关节机械臂关节转角测量方法，实施成本较低，极大简化了机械臂关节转角测量过程。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为按照本发明的优选实施例所构建的一种串联旋转关节机械臂关节转角辅助测量系统结构示意图。

图2为按照本发明的实施例1所构建的姿态测量单元的组成结构示意图。

图3为本发明所提出的一种串联旋转关节机械臂关节转角辅助测量方法流程图。

具体实施方式：

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在本发明中，术语如“上”、“下”、“左”、“右”、“前”、“后”、“竖直”、“水平”、“侧”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，只是为了便于叙述本发明各部件或元件结构关系而确定的关系词，并非特指本发明中任一部件或元件，不能理解为对本发明的限制。

本发明中，术语如“固接”、“相连”、“连接”等应做广义理解，表示可以是固定连接，也可以是一体的连接或可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的相关科研或技术人员，可以根据具体情况确定上述术语在本发明中的具体含义，不能理解为对本发明的限制。

实施例1

图1是按照本发明的优选实施例所构建的一种串联旋转关节机械臂关节转角辅助测量系统结构示意图，其中1为串联旋转关节机械臂，2为姿态测量单元，3为机械臂的控制柜，4为计算机，5为机械臂1与控制柜3之间的电缆，6为控制柜3连接至计算机4的网线，7为固定于机械臂1上的各个姿态测量单元2与计算机4之间的通信方式。

所述机械臂1在实际中可以是任意多自由度串联旋转关节机械臂，为方便起见，图1中所展示的是常见的六自由度串联旋转关节机械臂。

所述姿态测量单元2的个数应与机械臂的自由度相匹配，姿态测量单元2可以以任意姿态固定于机械臂1的每个关节上。

所述计算机4中存储有根据本发明提出的串联旋转关节机械臂关节转角辅助测量方法编写的一个程序，用于根据姿态测量单元2输出的姿态信息，计算得到机械臂在运动过程中的关节转角信息。

图2为按照本发明的优选实施例所构建的姿态测量单元的组成结构示意图。姿态测量单元2由微处理器、工业级三轴加速度计、工业级三轴陀螺仪、工业级三轴磁力计、通信组件和电源组件组成；电源组件为微处理器、工业级三轴加速度计、工业级三轴陀螺仪、工业级三轴磁力计和通信组件供电；通信组件用于将姿态测量单元2解算得到的姿态信息通过2.4G、蓝牙、Wi Fi等方式输出到计算机4。

图3为本发明所提出的一种串联旋转关节机械臂关节转角辅助测量方法流程图，包括三个步骤，分别为：搭建测量环境、统一坐标系、分解机械臂的复合运动、分解姿态测量单元的复合姿态，以及计算机械臂的关节转角。

下面将进一步说明本发明的具体实施步骤：

步骤1：搭建测量环境，在串联旋转关节机械臂的每个关节上固定一个姿态测量单元；

步骤2：统一坐标系，将串联旋转关节机械臂的关节转角辅助测量系统中的各坐标系进行统一；

步骤3：分解机械臂的复合运动，将机械臂的每个关节的复合运动分解为一系列各个关节的独立旋转运动；

步骤4：分解姿态测量单元的复合姿态，将姿态测量单元所输出的复合姿态分解为一系列各个关节独立旋转运动时产生的姿态变化；

步骤5：计算机械臂的关节转角，根据步骤4中得到的机械臂各关节独立旋转运动的姿态变化，计算相应的关节转角。

进一步的，所述的串联旋转关节机械臂可以为任意自由度机械臂。

进一步的，所述的姿态测量单元是以任意姿态固定在机械臂的每个关节的任意位置上，保证每个关节上都有一个姿态测量单元。每个姿态测量单元中包含有工业级三轴陀螺仪、工业级三轴加速度计以及工业级三轴磁力计，姿态测量单元的输出包含有四元数信息，姿态测量单元的信息输出方式可以为蓝牙、WiFi、2.4G等方式。

进一步的，所述统一的坐标系包括：真实的世界坐标系，固连在机械臂的基座上的基坐标系，姿态测量单元所处的导航坐标系，固连在姿态测量单元上的传感器坐标系，固连在机械臂各个关节上的关节坐标系，其中导航坐标系通常取为北-东-地坐标系或北-西-天坐标系。统一坐标系的目的是使用姿态测量单元的传感器坐标系在导航坐标系中的姿态改变来表示机械臂的关节坐标系在世界坐标系中的姿态改变，进而利用姿态测量单元输出的四元数求解机械臂在运动过程中的关节转角。

进一步的，所述分解机械臂的复合运动，是按照关节序号从高到低的顺序，将机械臂的复合运动分解为当前关节的独立旋转运动和与当前关节前向串联的各个关节的独立旋转运动，其中与机械臂基座相连的关节序号为1，机械臂末端工具相连的关节序号最高。

进一步的，所述姿态测量单元的复合姿态分解，可以用以下公式表示：

其中，Π表示连乘符号，q_n表示固定在关节n上的姿态测量单元输出的四元数，描述了由于复合运动导致的关节n的姿态变化。q_n|init表示机械臂位于初始状态时，固定在关节n上的姿态测量单元输出的四元数。q_n(r_i,θ_i)表示固定在关节n上的姿态测量单元输出的四元数，描述了机械臂的关节n做独立旋转运动时的姿态变化，r_i表示关节n独立旋转运动的旋转轴，θ_i表示关节n独立旋转运动的旋转角度，也就是本发明中所要测量的机械臂关节n的关节转角。

根据公式(1)，可以计算得到n自由度机械臂各个关节独立旋转运动时的姿态变化，具有以下形式：

其中，q*表示共轭四元数。

进一步的，计算所述的机械臂的关节转角时，假设机械臂的关节n独立旋转运动时，固定在关节n上的姿态测量单元输出的四元数q_n(r_n,θ_n)具有以下形式：

q_n(r_n,θ_n)＝[p x y z]^T(3)

根据四元数和轴角对的关系，可以得到以下公式：

进一步的，将根据公式(4)计算得到的角度定义为

α_n＝2arccos p，α_n∈(0,π)(8)

将公式(8)代入公式(5)-(7)，可以计算得到机械臂的关节n独立旋转运动时的旋转轴各分量如下：

为了避免出现奇异值，在实际过程中应避免机械臂的各关节在旋转时的角度趋近于零。将公式(9)代入公式(5)-(7)，可以得到以下三个计算关节转角的公式，

最终的关节n的旋转角度由旋转轴的最大分量所对应的公式计算得到，例如当旋转轴的r_y分量最大时，则使用公式(10)中的第二个子公式来计算关节n在运动过程中的旋转角度。

本发明实施例提出的一种串联旋转关节机械臂关节转角辅助测量方法，适用于多自由度串联旋转关节型机械臂。利用包含有工业级三轴加速度计、工业级三轴磁力计和工业级三轴陀螺仪的姿态测量单元输出的姿态，实现灵活方便、高效准确、不受环境影响的关节转角测量。该方法允许将姿态测量单元以任意姿态固定于机械臂的每个关节上，简化了设备安装流程。各个姿态测量单元可以通过蓝牙、WiFi、2.4G等方式将所测得的四元数信息实时传输到计算机中，测量不受环境限制。计算机根据接收到的各个姿态测量单元实时输出的四元数信息，利用本发明中提出的关节转角测量方法，经过求解，可以获得串联型机械臂在运动过程中的关节转角信息。作为一种关节转角辅助测量手段，这种基于姿态测量单元的串联旋转关节机械臂关节转角测量方法，实施成本较低，极大简化了机械臂关节转角测量过程。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种串联旋转关节机械臂的关节转角辅助测量系统，其特征在于，包括：姿态测量单元和计算机，所述姿态测量单元包括微处理器，和与微处理器电信号连接的三轴加速度计、三轴陀螺仪和三轴磁力计；姿态测量单元还具有固定端，姿态测量单元通过固定端固定设置在机械臂的每个关节的任一平面的任一位置上；

姿态测量单元与计算机通讯连接；

计算机用于根据姿态测量单元的姿态信息解算机械臂的关节转角。

2.如权利要求1所述的机械臂的关节转角辅助测量系统，其特征在于，所述惯性测量设备还包括通信组件和电源组件。

3.如权利要求2所述的机械臂的关节转角辅助测量系统，其特征在于，电源组件为微处理器、三轴加速度计、三轴陀螺仪、三轴磁力计和通信组件供电；通信组件用于将惯性测量设备解算得到的姿态信息通过串口、网线、蓝牙和WiFi方式传输到计算机。

4.一种如权利要求1-3所述的机械臂的关节转角辅助测量系统的使用方法，其特征在于，包括以下步骤：

搭建测量环境，在串联旋转关节机械臂的每个关节上固定一个姿态测量单元；

统一坐标系，将串联旋转关节机械臂的关节转角辅助测量系统中的各坐标系进行统一；

分解机械臂的复合运动，将机械臂的每个关节的复合运动分解为一系列各个关节的独立旋转运动；

分解姿态测量单元的复合姿态，将姿态测量单元所输出的复合姿态分解为一系列各个关节独立旋转运动时产生的姿态变化；

计算机械臂的关节转角，根据得到的机械臂各关节独立旋转运动的姿态变化，计算相应的关节转角。

5.如权利要求4所述的关节转角辅助测量方法，其特征在于，所述统一的坐标系包括：世界坐标系、固连在机械臂的基座上的基坐标系、姿态测量单元所处的导航坐标系、固连在姿态测量单元上的传感器坐标系和固连在机械臂各个关节上的关节坐标系；其中，导航坐标系取为北-东-地坐标系或北-西-天坐标系。

6.如权利要求4所述的关节转角辅助测量方法，其特征在于，所述分解机械臂的复合运动，是按照关节序号从高到低的顺序，将机械臂的复合运动分解为当前关节的独立旋转运动和与当前关节前向串联的各个关节的独立旋转运动，其中与机械臂基座相连的关节为1，机械臂末端工具相连的关节序号最高；

所述姿态测量单元的复合姿态分解，可以用以下公式表示：

其中，Π表示连乘符号，q_n表示固定在关节n上的姿态测量单元输出的四元数；q_n|init表示机械臂位于初始状态时，固定在关节n上的姿态测量单元输出的四元数；q_n(r_i,θ_i)表示固定在关节n上的姿态测量单元输出的四元数；r_i表示关节n独立旋转运动的旋转轴；θ_i表示关节n独立旋转运动的旋转角度。

7.如权利要求6所述的关节转角辅助测量方法，其特征在于，根据公式(1)，计算得到N自由度机械臂各个关节独立旋转运动时的姿态变化，具有以下形式：

其中，q*表示共轭四元数。

8.如权利要求7所述的关节转角辅助测量方法，其特征在于，计算所述的机械臂的关节转角时，假设机械臂的关节n独立旋转运动时，固定在关节n上的姿态测量单元输出的四元数q_n(r_n,θ_n)具有以下形式：

q_n(r_n,θ_n)＝[p x y z]^T (3)

根据四元数和轴角对的关系，可以得到以下公式：

将根据公式(4)计算得到的角度定义为：

α_n＝2arccosp，α_n∈(0,π) (8)。

9.如权利要求7所述的关节转角辅助测量方法，其特征在于，将公式(8)代入公式(5)-(7)，计算得到机械臂的关节n独立旋转运动时的旋转轴各分量如下：

10.如权利要求9所述的关节转角辅助测量方法，其特征在于，在实际过程中应避免机械臂的各关节在旋转时的角度趋近于零；将公式(9)代入公式(5)-(7)，可以得到以下三个计算关节转角的公式，

最终的关节n的旋转角度由旋转轴的最大分量所对应的公式计算得到，当旋转轴的r_y分量最大时，使用公式(10)中的第二个子公式来计算关节n在运动过程中的旋转角度。

Images