一种基于多源信息融合的高精度运动轨迹检测系统的制作方法
【技术领域】
[0001] 本发明涉及一种运动轨迹检测系统，具体的涉及一种采用惯性传感、机器视觉和 电磁定位子系统组成的高精度运动轨迹检测系统，应用领域包括机器人等运动部件的运动 轨迹检测。
【背景技术】
[0002] 工业机器人和移动机器人等运动部件日益广泛应用导致对其操作性能，尤其是对 运动执行器的动态定位精度提出很高的要求。例如工业机器人作为由减速器、伺服电机、增 量式编码器和负载反馈单元实现半闭环的运动控制方式，其机械手臂结构高度非线性，高 速末端动态变异（偏移、抖动）和高负载变异（末端工具置换）将影响路径定位精度。所 以一种高精度的运动轨迹检测系统来实现机器人的实时运动反馈与控制在装配定位、振动 分析以及性能指标的测量与评价等应用显得非常的必要。
[0003]目前国内外对运动跟踪和定位技术的研究相对集中在射频信号检测定位、惯性传 感、磁场定位、视觉定位和声源定位等。基于惯性传感技术的Xsens动作捕捉系统，将加速 度计、陀螺仪和磁力计进行信息融合，能获得精度较高的三维姿态信息，因为加速度值二次 积分后误差较大，获得的线性位移只能作为参考值。电磁定位系统通过磁传感器阵列对永 磁体或者电磁线圈在空间分布的三维磁场强度进行检测，再进行迭代求解得到永磁体或电 磁线圈的空间位置和姿态信息，NDI公司的电磁定位系统就采用两个垂直放置的3轴电磁 感应线圈实现完整6轴的运动检测，但是电磁定位系统容易受到环境电磁波及铁磁物质的 干扰，在工业环境下这种干扰难以避免；基于光学定位技术的VICON动作捕捉系统由红外 高速摄像机、一个数据处理器和配套的应用软件构成，红外高速摄像机捕捉被动发光标记 点，采用机器视觉原理和激光扫描技术，实现运动位置信息的测量，但光学定位系统只能测 量标记点的空间位置信息，且容易受到遮挡以及环境光和背景的影响。
[0004] 针对光学定位系统容易受遮挡的问题，专利申请号为201410661823.9的中国发 明申请，该发明提出一种基于惯性检测的激光跟踪仪靶球定位系统，可实现断光续接功能， 便于对难测点或遮档位置的测量，但主要不是提升目标定位跟踪的精度和维度。
[0005] 因此，在复杂多变的测试环境中，使用单个定位测量系统会存在以下不足：1、测量 获得的信息量单一，如光学定位系统只能测量到位置信息，惯性定位系统只能测量到姿态 信息；2、受到环境因素的干扰而导致定位精度不高，如电磁定位系统容易受电磁波的干扰， 光学定位系统容易受到遮挡以及环境光和背景的影响。
【发明内容】
[0006] 为克服单个定位系统存在的问题，本发明的目的在于提供一种基于多源信息融合 的运动轨迹检测系统，实现机器人等运动部件的六维位姿的高精度、高稳定和快速运动检 测系统。
[0007] 为解决上述技术问题，本发明采用以下技术方案：
[0008] -种基于多源信息融合的高精度运动轨迹检测系统，包括惯性传感定位、电磁定 位、机器视觉定位三个模块化的子系统以及数据处理平台，惯性传感定位子系统、电磁定位 子系统和机器视觉定位子系统采集的数据均传输到所述数据处理平台，以实现信息融合定 位和协作定位；其中：
[0009] 所述惯性传感定位子系统用于测量运动部件三维姿态角；
[0010] 所述电磁定位子系统用于测量运动部件三维位置和三维姿态角；
[0011] 所述机器视觉定位子系统用于测量运动部件三维位置信息；
[0012] 所述数据处理平台通过将惯性传感定位子系统、电磁定位子系统、机器视觉定位 子系统中获取的多源信息进行综合分析和协调处理，基于分布式状态融合结构模型，对数 据进行坐标转换和数据校正、数据关联和状态估计融合，综合考虑噪声干扰和环境因素的 影响，提升目标动态定位跟踪的精度和维度，增强系统的可靠性和鲁棒性。
[0013] 优选地，所述的惯性传感定位子系统包括MEMS传感器和第一子处理器，MEMS传感 器（包括三轴加速度计、三轴陀螺仪、三轴磁力计）贴附于运动部件来实现实时三维姿态信 息的获取，第一子处理器采用卡尔曼滤波或正交余弦矩阵融合算法对MEMS传感器采集的 数据进行融合，从而获得运动部件精确的三维姿态角信息。
[0014] 更优选地，所述的MEMS传感器包括三轴陀螺仪、三轴加速度计、三轴磁力计，所述 三轴陀螺仪、三轴加速度计、三轴磁力计采集的数据均传送到第一子处理器，第一子处理器 对三者数据进行处理，得到MEMS传感器当前的姿态角，即运动部件的姿态角；其中：
[0015] 三轴陀螺仪测得角速度、一次积分后得到运动部件偏转的姿态角，但误差会随着 时间而累积；
[0016] 三轴磁力计用于测量地磁强度，进而得到运动部件的航向角；
[0017] 三轴加速度计测量得到的信号中的三轴重力分量，用于测量运动部件的绝对俯仰 角和翻滚角（相对于地球坐标）；
[0018] 三轴磁力计和三轴加速度计输出的姿态角动态性能差，用于补偿三轴陀螺仪信号 一次积分后得到的姿态角，去除漂移。
[0019] 优选地，所述的电磁定位子系统包括三轴正交的激励线圈和三轴正交的感应线 圈、以及第二子处理器，其中：感应线圈固定在运动部件，激励线圈则作为固定点；在很短 时间内，激励线圈交替通过相同频率和幅度的交流电流，交变的电流信号通过激励线圈在 空间产生交变的电磁场，感应线圈在交变的电磁场中输出频率相同的信号；第二子处理器 根据感应线圈输出信号的幅值和相位信息，通过定位算法计算出感应线圈相对于激励线圈 的位置和方向信息。
[0020] 优选地，所述的机器视觉定位子系统由若干个相机和FPGA嵌入式处理器组成，其 中：若干个相机安装在特征点的周围，用于从不同的方位实时连续采集特征点的图像信号 并传递给FPGA嵌入式处理器；FPGA嵌入式处理器用于控制相机获取含有标记点的图像信 号，并将图像信号进行处理，实现特征点图像坐标的获取；所述特征点采用主动发光或被动 发光的标记点，且贴附于运动部件上。
[0021] 更优选地，所述的机器视觉定位子系统采用多个相机从不同的方位实时连续采集 特征点的图像信号，各路图像信号采用基于颜色空间模型进行目标识别，进而找出特征点 在各个相机中不同时刻的成像位置的二维坐标；将特征点在各个相机中不同时刻的成像位 置二维坐标通过优化算法进行2D坐标插值计算；在所有相机成像面中的多个二维平面坐 标所构成多条空间异面直线，通过三维坐标定位算法计算特征点的空间三维坐标。
[0022] 更优选地，所述的优化算法采用最小二乘法、平均法或中值法等。
[0023]优选地，所述的数据处理平台采用多源信息融合算法，该算法基于分布式状态融 合结构模型，该结构模型的特点是每个子系统的传感器数据在进入数据处理平台前，先由 各个子系统的数据处理器生成局部的定位跟踪轨迹，然后把处理过的信息送至数据处理平 台，数据处理平台根据各子系统的定位跟踪轨迹数据，进行坐标转换和数据较正、数据关联 以及状态估计融合，最终生成具有六维位姿的目标定位跟踪轨迹；此外，目标定位跟踪轨迹 数据还反馈信息到各个子系统中，为各子系统的定位跟踪提供参考和较准。
[0024] 更优选地，所述多源信息融合算法，包括以下3个处理过程：
[0025] 1)坐标转换和数据校正：统一各子系统的坐标系，并结合反馈的位姿信息对数据 进行较正；
[0026] 2)数据关联：对各个子系统测量的数据先进行关联；
[0027] 3)状态估计融合：通过测量结果估计目标当前的状态向量，利用上周期外推所得 的本周期目标的可能位姿信息，和本周期实时检测到的目标位姿信息，采用滤波算法实现 融合。
[0028] 更优选地，所述数据关联，采用以下任一种方法：最邻近数据关联、联合概率数据 关联、统计关联或模糊关联；
[0029] 更优选地，所述状态估计融合，采用以下任一种方法：卡尔曼加权融合、协方差加 权轨迹融合或模糊轨迹融合。
[0030] 与现有技术相比，本发明的有益效果：
[0031]本发明采用三个子系统的设计获取多源信息，并进一步在每个子系统中均设置相