三维姿态角信息。
[0036]在本实施例中，所述惯性传感定位子系统2固定在机械手臂法兰盘(运动部件1)末端，第一子处理器对MEMS传感器21数据进行处理，以获得机械手臂法兰盘(运动部件1)末端精确的三维姿态角信息。第一子处理器的具体数据处理采用现有技术，比如卡尔曼滤波或正交余弦矩阵融合算法等，这些不属于本实用新型的创新，本实用新型创新在于提供了装置整体结构。
[0037]进一步的，所述MEMS传感器21包括一个三轴陀螺仪、一个三轴加速度计和一个三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计均连接到第一子处理器。三轴陀螺仪、三轴加速度计、三轴磁力计采集的数据均传送到第一子处理器，第一子处理器对三者数据进行处理，得到MEMS传感器21当前的姿态角，由于MEMS传感器21贴附于运动部件1，因此也是运动部件1的姿态角。
[0038]优选的，三轴陀螺仪测得角加速度、一次积分后得到物体偏转的姿态角；三轴磁力计测量地磁强度，得到物体的航向角；三轴加速度计测得三轴重力分量，用于测量绝对俯仰角和翻滚角(相对于地球坐标)；三轴磁力计和三轴加速度计输出的姿态角动态性能差，用于补偿三轴陀螺仪信号一次积分后得到的姿态角，去除漂移。
[0039]所述第一子处理器可以采用逻辑电路、集成电路、单片机等实现，其具体处理技术可以采用现有技术实现。
[0040]如图3所示，在另一优选实施例中，所述电磁定位子系统由三轴正交的激励线圈32和三轴正交的感应线圈31、以及第二子处理器组成，其中:感应线圈31固定在机械手臂法兰盘(运动部件1)末端，激励线圈32则作为固定点。在短时间内，激励线圈32交替通过相同频率和幅度的交流电流，使激励线圈32在空间中产生交变的电磁场，感应线圈31在交变的电磁场中输出频率相同的信号；以及第二子处理器根据感应线圈31输出信号的幅值和相位信息，处理得到感应线圈31相对于激励线圈32的位置和方向信息。
[0041 ] 所述第二子处理器可以采用逻辑电路、集成电路、单片机等实现，其具体处理技术可以采用现有技术实现，当然在一优选实施例中也可以采用以下技术:
[0042]假设所述激励线圈32的中心位置为(a，b，c)，所述感应线圈31的中心位置为(x，y，z)且其相对激励线圈32的方向用三个旋转角(α，β, γ)表示，电势幅值ΕΜ是相对位置参数(x-a, y-b, z_c)和角度参数(α，β，γ )的函数，即EM =f (x-a, y-b, z-c, α, β , γ)。因此采用六种或以上独立的激励线圈32和感应线圈31的组合关系，对不同激励线圈32下的感应线圈31电势信号进行采样，通过定位算法就能够计算出六个位置参数(x-a, y-b, z-c)和角度参数(α，β，γ )的值。
[0043]如图4所示，在另一优选实施例中，所述机器视觉定位子系统包括若干个相机41和FPGA嵌入式处理器43，其中:若干个相机41安装在特征点42的周围，相机41采集的图像数据传到FPGA嵌入式处理器中去处理；特征点42可采用主动发光或被动发光的标记点，且贴附于运动部件上。FPGA嵌入式处理器43用于控制相机获取含有标记点的图像信号，并将图像信号传入内嵌DSP builder模块进行处理，实现特征点图像坐标的获取。
[0044]所述FPGA嵌入式处理器43，其具体处理技术可以采用现有技术实现，当然在一优选实施例中也可以采用以下技术:首先多个相机41从不同的方位实时连续采集特征点42的图像信号；各路图像信号采用基于颜色空间模型进行目标识别，即先多次拍摄特征点取平均值，提取出特征点42的颜色分量模型[R，G, B]，再和含有该特征点42的待识别图像的每个颜色分量进行对比，进而找出特征点42在该幅图像中的二维坐标；将特征点42在各个相机41中不同时刻的成像位置二维坐标通过优化算法进行2D坐标插值计算，优化算法可以采用最小二乘法、平均法或中值法；在所有相机41成像面中的多个二维平面坐标所构成的多条空间异面直线，通过三维坐标定位算法计算特征点42的空间三维坐标。
[0045]上述的各子系统的数据传到用于数据融合的处理器5，所述第二子处理器可以采用现有产品实现，比如数据融合处理器，单片机等，其具体数据融合技术可以采用现有技术实现，当然在一优选实施例中也可以采用以下技术:用于数据融合的处理器5采用分布式状态融合结构模型，该结构模型的特点是每个子系统的传感器数据在进入用于数据融合的处理器5前，先由自己的数据处理器(即第一子处理器、第二子处理器、FPGA嵌入式处理器)生成局部的定位跟踪轨迹，然后把处理过的信息送至用于数据融合的处理器5，用于数据融合的处理器5根据各子系统的定位跟踪轨迹数据，进行坐标转换和数据较正、数据关联以及状态估计融合，最终生成具有6维位姿的目标定位跟踪轨迹。此外，目标定位跟踪轨迹数据还反馈信息到各个子系统中，为各子系统的定位跟踪提供参考和较准。用于数据融合的处理器5能够有效利用多重定位系统的定位冗余信息和互补信息，综合考虑噪声干扰和环境因素的影响，能提升目标定位跟踪的精度和维度，还能增强系统的可靠性和鲁棒性。
[0046]本实用新型装置设置三个定位子系统，通过这三个定位子系统获取的多源信息，后续再进行处理，能实现工业机器人等运动部件的三维位置和三维姿态角的高精度、高稳定和快速运动检测。本实用新型提供了一种结构合理的检测装置，其中各个部分所采用的数据处理技术不属于本实用新型要求保护的内容。
[0047]以上对本实用新型的具体实施例进行了描述。需要理解的是，本实用新型并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本实用新型的实质内容。
【主权项】
1.一种高精度运动轨迹检测装置，其特征在于，包括惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统以及用于数据融合的处理器，其中: 所述惯性传感定位子系统，其输入端测量运动部件三维姿态角，输出端连接到所述用于数据融合的处理器； 所述电磁定位子系统，其输入端测量运动部件三维位置和三维姿态角，输出端连接到所述用于数据融合的处理器； 所述机器视觉定位子系统，其输入端测量运动部件三维位置信息，输出端连接到所述用于数据融合的处理器； 所述用于数据融合的处理器同时连接惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统的输出端，并将三个子系统的数据进行融合，得到运动部件的运动轨迹。2.根据权利要求1所述的一种高精度运动轨迹检测装置，其特征在于，所述的惯性传感定位子系统包括MEMS传感器和第一子处理器，MEMS传感器贴附于运动部件并实现实时三维姿态信息的获取，所述MEMS传感器的输出端连接到第一子处理器，第一子处理器对MEMS传感器采集的数据进行融合，获得运动部件精确的三维姿态角信息。3.根据权利要求2所述的一种高精度运动轨迹检测装置，其特征在于，所述的MEMS传感器包括三轴陀螺仪、三轴加速度计、三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计均连接到第一子处理器。4.根据权利要求1-3任一项所述的一种高精度运动轨迹检测装置，其特征在于:所述的电磁定位子系统包括三轴正交的激励线圈和三轴正交的感应线圈、以及第二子处理器，其中:感应线圈固定在运动部件上，激励线圈则作为固定点；激励线圈交替通过相同频率和幅度的交流电流，交变的电流信号通过激励线圈在空间产生交变的电磁场，感应线圈在交变的电磁场中输出频率相同的信号；第二子处理器根据感应线圈输出信号的幅值和相位信息，得到感应线圈相对于激励线圈的位置和方向信息。5.根据权利要求1-3任一项所述的一种高精度运动轨迹检测装置，其特征在于，所述的机器视觉定位子系统由若干个相机和FPGA嵌入式处理器组成，其中:若干个相机安装在特征点的周围，用于从不同的方位实时连续采集特征点的图像信号并输出给FPGA嵌入式处理器；所述特征点采用主动发光或被动发光的标记点，且贴附于运动部件上；FPGA嵌入式处理器用于控制相机获取含有标记点的图像信号，并将图像信号进行处理，实现特征点图像坐标的获取。
【专利摘要】本实用新型提供一种高精度运动轨迹检测装置，其中：惯性传感定位子系统，测量运动部件三维姿态角；电磁定位子系统，测量运动部件三维位置和三维姿态角；机器视觉定位子系统，测量运动部件三维位置信息；用于数据融合的处理器同时连接惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统的输出端，并将三个子系统的数据进行融合，得到运动部件的运动轨迹。本实用新型克服单个系统获得的定位信息量单一以及容易受环境因素干扰的问题。
【IPC分类】G01C21/00, G01C21/16
【公开号】CN205066775
【申请号】CN201520721883
【发明人】戴厚德, 苏诗荐, 夏许可, 林志榕, 曾雅丹, 吴周新
【申请人】泉州装备制造研究所
【公开日】2016年3月2日
【申请日】2015年9月17日