CN205066775U - 一种高精度运动轨迹检测装置 - Google Patents

Abstract

本实用新型提供一种高精度运动轨迹检测装置，其中：惯性传感定位子系统，测量运动部件三维姿态角；电磁定位子系统，测量运动部件三维位置和三维姿态角；机器视觉定位子系统，测量运动部件三维位置信息；用于数据融合的处理器同时连接惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统的输出端，并将三个子系统的数据进行融合，得到运动部件的运动轨迹。本实用新型克服单个系统获得的定位信息量单一以及容易受环境因素干扰的问题。

Description

一种高精度运动轨迹检测装置

技术领域

本实用新型涉及一种运动轨迹检测装置，具体的涉及一种采用惯性传感、机器视觉和电磁定位子系统组成的高精度运动轨迹检测装置，应用领域包括工业机器人等运动部件的运动轨迹检测。

背景技术

工业机器人和移动机器人等运动部件日益广泛应用导致对其操作性能，尤其是对运动执行器的动态定位精度提出很高的要求。例如工业机器人作为由减速器、伺服电机、增量式编码器和负载反馈单元实现半闭环的运动控制方式，其机械手臂结构高度非线性，高速末端动态变异(偏移、抖动)和高负载变异(末端工具置换)将影响路径定位精度。所以一种高精度的运动轨迹检测系统来实现机器人的实时运动反馈与控制在装配定位、振动分析以及性能指标的测量与评价等应用显得非常的必要。

目前国内外对运动跟踪和定位技术的研究相对集中在射频信号检测定位、惯性传感、磁场定位、视觉定位和声源定位等。基于惯性传感技术的Xsens动作捕捉系统，将加速度计、陀螺仪和磁力计进行信息融合，能获得精度较高的三维姿态信息，因为加速度值二次积分后误差较大，获得的线性位移只能作为参考值。电磁定位系统通过磁传感器阵列对永磁体或者电磁线圈在空间分布的三维磁场强度进行检测，再进行迭代求解得到永磁体或电磁线圈的空间位置和姿态信息，NDI公司的电磁定位系统就采用两个垂直放置的3轴电磁感应线圈实现完整6轴的运动检测，但是电磁定位系统容易受到环境电磁波及铁磁物质的干扰，在工业环境下这种干扰难以避免；基于光学定位技术的VICON动作捕捉系统由红外高速摄像机、一个数据处理器构成，红外高速摄像机捕捉被动发光标记点，采用机器视觉原理和激光扫描技术，实现运动位置信息的测量，但光学定位系统只能测量标记点的空间位置信息，且容易受到遮挡以及环境光和背景的影响。

针对光学定位系统容易受遮挡的问题，申请号为201420695742.6的中国实用新型专利申请，该实用新型提出一种基于惯性检测的激光跟踪仪靶球定位装置，可实现断光续接功能，便于对难测点或遮档位置的测量，但主要不是提升目标定位跟踪的精度和维度。

因此，在复杂多变的测试环境中，使用单个定位测量系统会存在以下不足：1、测量获得的信息量单一，如光学定位系统只能测量到位置信息，惯性定位系统只能测量到姿态信息；2、受到环境因素的干扰而导致定位精度不高，如电磁定位系统容易受电磁波的干扰，光学定位系统容易受到遮挡以及环境光和背景的影响。

实用新型内容

为克服单个定位系统存在的问题，本实用新型的目的在于提供一种高精度运动轨迹检测装置，实现工业机器人等运动部件的6维位姿的高精度、高稳定和快速运动检测系统。

为解决上述技术问题，本实用新型采用以下技术方案：

一种高精度运动轨迹检测装置，包括惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统以及用于数据融合的处理器，其中：

所述惯性传感定位子系统，其输入端测量运动部件三维姿态角，输出端连接到所述用于数据融合的处理器；

所述电磁定位子系统，其输入端测量运动部件三维位置和三维姿态角，输出端连接到所述用于数据融合的处理器；

所述机器视觉定位子系统，其输入端测量运动部件三维位置信息，输出端连接到所述用于数据融合的处理器；

所述用于数据融合的处理器同时连接惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统的输出端，并将三个子系统的数据进行融合，得到运动部件的运动轨迹。

优选地，所述的惯性传感定位子系统包括MEMS传感器和第一子处理器，MEMS传感器贴附于运动部件并实现实时三维姿态信息的获取，所述MEMS传感器的输出端连接到第一子处理器，第一子处理器对MEMS传感器采集的数据进行融合，获得运动部件精确的三维姿态角信息。

优选地，所述的MEMS传感器包括三轴陀螺仪、三轴加速度计、三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计均连接到第一子处理器。

优选地，所述的电磁定位子系统包括三轴正交的激励线圈和三轴正交的感应线圈，以及第二子处理器，其中：感应线圈固定在运动部件上，激励线圈则作为固定点；激励线圈交替通过相同频率和幅度的交流电流，交变的电流信号通过激励线圈在空间产生交变的电磁场，感应线圈在交变的电磁场中输出频率相同的信号；第二子处理器根据感应线圈输出信号的幅值和相位信息，得到感应线圈相对于激励线圈的位置和方向信息。

优选地，所述的机器视觉定位子系统由若干个相机和FPGA嵌入式处理器组成，其中：若干个相机安装在特征点的周围，用于从不同的方位实时连续采集特征点的图像信号并输出给FPGA嵌入式处理器；所述特征点采用主动发光或被动发光的标记点，且贴附于运动部件上；FPGA嵌入式处理器用于控制相机获取含有标记点的图像信号，并将图像信号进行处理，实现特征点图像坐标的获取。

与现有技术相比，本实用新型的有益效果：

本实用新型装置包括三个子系统，并且每个子系统中均设置相应的处理器，从而生成局部的定位跟踪轨迹通过串行通信总线传送至用于数据融合的处理器，用于数据融合的处理器对子系统的数据进行融合，得到运动部件的运动轨迹。本实用新型采用三个子系统采集多源信息、借助多源信息提升检测的精度和维度，克服单个系统获得的定位信息量单一以及容易受环境因素干扰的问题。

进一步的，该装置中，不仅具有局部独立的定位跟踪能力(三个子系统的设计)，而且还有全局监视和评估特性。通过各个定位子系统中获取的多源信息，并进一步融合，实现工业机器人等运动部件的六维位姿(三维位置和三维姿态角)的高精度、高稳定和快速运动检测。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本实用新型的其它特征、目的和优点将会变得更明显：

图1为本实用新型一实施例的运动轨迹检测装置的结构框图；

图2为本实用新型一实施例的惯性传感定位子系统结构示意图；

图3为本实用新型一实施例的电磁定位子系统结构示意图；

图4为本实用新型一实施例的机器视觉定位子系统结构示意图；

图中：运动部件1，惯性传感定位子系统2，MEMS传感器21，电磁定位子系统3，感应线圈31，激励线圈32，机器视觉定位子系统4，相机41，特征点42，FPGA嵌入式处理器43，用于数据融合的处理器5。

具体实施方式

下面结合具体实施例对本实用新型进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本实用新型，但不以任何形式限制本实用新型。应当指出的是，对本领域的普通技术人员来说，在不脱离本实用新型构思的前提下，还可以做出若干变形和改进。这些都属于本实用新型的保护范围。

如图1所示，一种高精度运动轨迹检测装置，该装置由惯性传感定位子系统2、电磁定位子系统3和机器视觉定位子系统4，以及用于数据融合的处理器5组成；其中：

所述惯性传感定位子系统2，其输入端测量运动部件三维姿态角，输出端连接到所述用于数据融合的处理器；

所述电磁定位子系统3，其输入端测量运动部件三维位置和三维姿态角，输出端连接到所述用于数据融合的处理器；

所述机器视觉定位子系统4，其输入端测量运动部件三维位置信息，输出端连接到所述用于数据融合的处理器；

所述用于数据融合的处理器5，同时连接惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统的输出端，并将三个子系统的数据进行融合，得到运动部件的运动轨迹。

本实施例中，各个子系统的数据通过串行通信总线(SPI或者CAN)传输到所述用于数据融合的处理器5，实现数据融合。

所述用于数据融合的处理器5，其具体融合技术可以采用现有技术，在一优选实施例中，也可以采用基于分布式状态融合结构模型，对数据进行坐标转换和数据校正、数据关联和状态估计融合。用于数据融合的处理器5能够有效利用多重定位系统的定位冗余信息和互补信息，综合考虑噪声干扰和环境因素的影响，能提升目标定位跟踪的精度和维度，还能增强系统的可靠性和鲁棒性。

以机械手臂法兰盘末端作为测量的运动部件1，结合附图对本实用新型详细实施例进行说明。

如图2所示，在一优选实施例中，所述的惯性传感定位子系统2包括MEMS传感器21和第一子处理器，MEMS传感器21贴附于运动部件1并实现实时三维姿态信息的获取，所述MEMS传感器21的输出端连接到第一子处理器，第一子处理器对MEMS传感器采集的数据进行融合，获得运动部件精确的三维姿态角信息。

在本实施例中，所述惯性传感定位子系统2固定在机械手臂法兰盘(运动部件1)末端，第一子处理器对MEMS传感器21数据进行处理，以获得机械手臂法兰盘(运动部件1)末端精确的三维姿态角信息。第一子处理器的具体数据处理采用现有技术，比如卡尔曼滤波或正交余弦矩阵融合算法等，这些不属于本实用新型的创新，本实用新型创新在于提供了装置整体结构。

进一步的，所述MEMS传感器21包括一个三轴陀螺仪、一个三轴加速度计和一个三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计均连接到第一子处理器。三轴陀螺仪、三轴加速度计、三轴磁力计采集的数据均传送到第一子处理器，第一子处理器对三者数据进行处理，得到MEMS传感器21当前的姿态角，由于MEMS传感器21贴附于运动部件1，因此也是运动部件1的姿态角。

优选的，三轴陀螺仪测得角加速度、一次积分后得到物体偏转的姿态角；三轴磁力计测量地磁强度，得到物体的航向角；三轴加速度计测得三轴重力分量，用于测量绝对俯仰角和翻滚角(相对于地球坐标)；三轴磁力计和三轴加速度计输出的姿态角动态性能差，用于补偿三轴陀螺仪信号一次积分后得到的姿态角，去除漂移。

所述第一子处理器可以采用逻辑电路、集成电路、单片机等实现，其具体处理技术可以采用现有技术实现。

如图3所示，在另一优选实施例中，所述电磁定位子系统由三轴正交的激励线圈32和三轴正交的感应线圈31、以及第二子处理器组成，其中：感应线圈31固定在机械手臂法兰盘(运动部件1)末端，激励线圈32则作为固定点。在短时间内，激励线圈32交替通过相同频率和幅度的交流电流，使激励线圈32在空间中产生交变的电磁场，感应线圈31在交变的电磁场中输出频率相同的信号；以及第二子处理器根据感应线圈31输出信号的幅值和相位信息，处理得到感应线圈31相对于激励线圈32的位置和方向信息。

所述第二子处理器可以采用逻辑电路、集成电路、单片机等实现，其具体处理技术可以采用现有技术实现，当然在一优选实施例中也可以采用以下技术：

假设所述激励线圈32的中心位置为(a,b,c)，所述感应线圈31的中心位置为(x,y,z)且其相对激励线圈32的方向用三个旋转角(α,β,γ)表示，电势幅值EM是相对位置参数(x-a,y-b,z-c)和角度参数(α,β,γ)的函数，即EM＝f(x-a,y-b,z-c,α,β,γ)。因此采用六种或以上独立的激励线圈32和感应线圈31的组合关系，对不同激励线圈32下的感应线圈31电势信号进行采样，通过定位算法就能够计算出六个位置参数(x-a,y-b,z-c)和角度参数(α,β,γ)的值。

如图4所示，在另一优选实施例中，所述机器视觉定位子系统包括若干个相机41和FPGA嵌入式处理器43，其中：若干个相机41安装在特征点42的周围，相机41采集的图像数据传到FPGA嵌入式处理器中去处理；特征点42可采用主动发光或被动发光的标记点，且贴附于运动部件上。FPGA嵌入式处理器43用于控制相机获取含有标记点的图像信号，并将图像信号传入内嵌DSPbuilder模块进行处理，实现特征点图像坐标的获取。

所述FPGA嵌入式处理器43，其具体处理技术可以采用现有技术实现，当然在一优选实施例中也可以采用以下技术：首先多个相机41从不同的方位实时连续采集特征点42的图像信号；各路图像信号采用基于颜色空间模型进行目标识别，即先多次拍摄特征点取平均值，提取出特征点42的颜色分量模型[R,G,B]，再和含有该特征点42的待识别图像的每个颜色分量进行对比，进而找出特征点42在该幅图像中的二维坐标；将特征点42在各个相机41中不同时刻的成像位置二维坐标通过优化算法进行2D坐标插值计算，优化算法可以采用最小二乘法、平均法或中值法；在所有相机41成像面中的多个二维平面坐标所构成的多条空间异面直线，通过三维坐标定位算法计算特征点42的空间三维坐标。

上述的各子系统的数据传到用于数据融合的处理器5，所述第二子处理器可以采用现有产品实现，比如数据融合处理器，单片机等，其具体数据融合技术可以采用现有技术实现，当然在一优选实施例中也可以采用以下技术：用于数据融合的处理器5采用分布式状态融合结构模型，该结构模型的特点是每个子系统的传感器数据在进入用于数据融合的处理器5前，先由自己的数据处理器(即第一子处理器、第二子处理器、FPGA嵌入式处理器)生成局部的定位跟踪轨迹，然后把处理过的信息送至用于数据融合的处理器5，用于数据融合的处理器5根据各子系统的定位跟踪轨迹数据，进行坐标转换和数据较正、数据关联以及状态估计融合，最终生成具有6维位姿的目标定位跟踪轨迹。此外，目标定位跟踪轨迹数据还反馈信息到各个子系统中，为各子系统的定位跟踪提供参考和较准。用于数据融合的处理器5能够有效利用多重定位系统的定位冗余信息和互补信息，综合考虑噪声干扰和环境因素的影响，能提升目标定位跟踪的精度和维度，还能增强系统的可靠性和鲁棒性。

本实用新型装置设置三个定位子系统，通过这三个定位子系统获取的多源信息，后续再进行处理，能实现工业机器人等运动部件的三维位置和三维姿态角的高精度、高稳定和快速运动检测。本实用新型提供了一种结构合理的检测装置，其中各个部分所采用的数据处理技术不属于本实用新型要求保护的内容。

以上对本实用新型的具体实施例进行了描述。需要理解的是，本实用新型并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本实用新型的实质内容。

Claims (5)

1.一种高精度运动轨迹检测装置，其特征在于，包括惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统以及用于数据融合的处理器，其中：

所述惯性传感定位子系统，其输入端测量运动部件三维姿态角，输出端连接到所述用于数据融合的处理器；

所述电磁定位子系统，其输入端测量运动部件三维位置和三维姿态角，输出端连接到所述用于数据融合的处理器；

所述机器视觉定位子系统，其输入端测量运动部件三维位置信息，输出端连接到所述用于数据融合的处理器；

所述用于数据融合的处理器同时连接惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统的输出端，并将三个子系统的数据进行融合，得到运动部件的运动轨迹。

2.根据权利要求1所述的一种高精度运动轨迹检测装置，其特征在于，所述的惯性传感定位子系统包括MEMS传感器和第一子处理器，MEMS传感器贴附于运动部件并实现实时三维姿态信息的获取，所述MEMS传感器的输出端连接到第一子处理器，第一子处理器对MEMS传感器采集的数据进行融合，获得运动部件精确的三维姿态角信息。

3.根据权利要求2所述的一种高精度运动轨迹检测装置，其特征在于，所述的MEMS传感器包括三轴陀螺仪、三轴加速度计、三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计均连接到第一子处理器。

4.根据权利要求1-3任一项所述的一种高精度运动轨迹检测装置，其特征在于：所述的电磁定位子系统包括三轴正交的激励线圈和三轴正交的感应线圈、以及第二子处理器，其中：感应线圈固定在运动部件上，激励线圈则作为固定点；激励线圈交替通过相同频率和幅度的交流电流，交变的电流信号通过激励线圈在空间产生交变的电磁场，感应线圈在交变的电磁场中输出频率相同的信号；第二子处理器根据感应线圈输出信号的幅值和相位信息，得到感应线圈相对于激励线圈的位置和方向信息。

5.根据权利要求1-3任一项所述的一种高精度运动轨迹检测装置，其特征在于，所述的机器视觉定位子系统由若干个相机和FPGA嵌入式处理器组成，其中：若干个相机安装在特征点的周围，用于从不同的方位实时连续采集特征点的图像信号并输出给FPGA嵌入式处理器；所述特征点采用主动发光或被动发光的标记点，且贴附于运动部件上；FPGA嵌入式处理器用于控制相机获取含有标记点的图像信号，并将图像信号进行处理，实现特征点图像坐标的获取。