CN105222772B - 一种基于多源信息融合的高精度运动轨迹检测系统 - Google Patents

Abstract

本发明提供一种基于多源信息融合的高精度运动轨迹检测系统，包括惯性传感定位、机器视觉定位和电磁定位三个子系统及数据处理平台，其中：惯性传感定位子系统用于三维姿态角的测量；电磁定位子系统用于三维位置和三维姿态信息的测量；机器视觉定位子系统用于三维位置信息的测量；数据处理平台通过将各个子系统中获取的多源信息进行综合分析和协调处理，基于分布式状态融合结构模型，对数据进行坐标转换和数据校正、数据关联和状态估计融合，实现机器人等运动部件六维位姿的动态检测。本发明克服单个系统获得的定位信息量单一以及容易受环境因素干扰的问题。

Description

一种基于多源信息融合的高精度运动轨迹检测系统

技术领域

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

背景技术

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

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

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

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

发明内容

为克服单个定位系统存在的问题，本发明的目的在于提供一种基于多源信息融合的运动轨迹检测系统，实现机器人等运动部件的六维位姿的高精度、高稳定和快速运动检测系统。

为解决上述技术问题，本发明采用以下技术方案：

一种基于多源信息融合的高精度运动轨迹检测系统，包括惯性传感定位、电磁定位、机器视觉定位三个模块化的子系统以及数据处理平台，惯性传感定位子系统、电磁定位子系统和机器视觉定位子系统采集的数据均传输到所述数据处理平台，以实现信息融合定位和协作定位；其中：

所述惯性传感定位子系统用于测量运动部件三维姿态角；

所述电磁定位子系统用于测量运动部件三维位置和三维姿态角；

所述机器视觉定位子系统用于测量运动部件三维位置信息；

所述数据处理平台通过将惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统中获取的多源信息进行综合分析和协调处理，基于分布式状态融合结构模型，对数据进行坐标转换和数据校正、数据关联和状态估计融合，综合考虑噪声干扰和环境因素的影响，提升目标动态定位跟踪的精度和维度，增强系统的可靠性和鲁棒性。

优选地，所述的惯性传感定位子系统包括MEMS传感器和第一子处理器，MEMS传感器(包括三轴加速度计、三轴陀螺仪、三轴磁力计)贴附于运动部件来实现实时三维姿态信息的获取，第一子处理器采用卡尔曼滤波或正交余弦矩阵融合算法对MEMS传感器采集的数据进行融合，从而获得运动部件精确的三维姿态角信息。

更优选地，所述的MEMS传感器包括三轴陀螺仪、三轴加速度计、三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计采集的数据均传送到第一子处理器，第一子处理器对三者数据进行处理，得到MEMS传感器当前的姿态角，即运动部件的姿态角；其中：

三轴陀螺仪测得角速度、一次积分后得到运动部件偏转的姿态角，但误差会随着时间而累积；

三轴磁力计用于测量地磁强度，进而得到运动部件的航向角；

三轴加速度计测量得到的信号中的三轴重力分量，用于测量运动部件的绝对俯仰角和翻滚角(相对于地球坐标)；

三轴磁力计和三轴加速度计输出的姿态角动态性能差，用于补偿三轴陀螺仪信号一次积分后得到的姿态角，去除漂移。

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

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

更优选地，所述的机器视觉定位子系统采用多个相机从不同的方位实时连续采集特征点的图像信号，各路图像信号采用基于颜色空间模型进行目标识别，进而找出特征点在各个相机中不同时刻的成像位置的二维坐标；将特征点在各个相机中不同时刻的成像位置二维坐标通过优化算法进行2D坐标插值计算；在所有相机成像面中的多个二维平面坐标所构成多条空间异面直线，通过三维坐标定位算法计算特征点的空间三维坐标。

更优选地，所述的优化算法采用最小二乘法、平均法或中值法等。

优选地，所述的数据处理平台采用多源信息融合算法，该算法基于分布式状态融合结构模型，该结构模型的特点是每个子系统的传感器数据在进入数据处理平台前，先由各个子系统的数据处理器生成局部的定位跟踪轨迹，然后把处理过的信息送至数据处理平台，数据处理平台根据各子系统的定位跟踪轨迹数据，进行坐标转换和数据较正、数据关联以及状态估计融合，最终生成具有六维位姿的目标定位跟踪轨迹；此外，目标定位跟踪轨迹数据还反馈信息到各个子系统中，为各子系统的定位跟踪提供参考和较准。

更优选地，所述多源信息融合算法，包括以下3个处理过程：

1)坐标转换和数据校正：统一各子系统的坐标系，并结合反馈的位姿信息对数据进行较正；

2)数据关联：对各个子系统测量的数据先进行关联；

3)状态估计融合：通过测量结果估计目标当前的状态向量，利用上周期外推所得的本周期目标的可能位姿信息，和本周期实时检测到的目标位姿信息，采用滤波算法实现融合。

更优选地，所述数据关联，采用以下任一种方法：最邻近数据关联、联合概率数据关联、统计关联或模糊关联；

更优选地，所述状态估计融合，采用以下任一种方法：卡尔曼加权融合、协方差加权轨迹融合或模糊轨迹融合。

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

本发明采用三个子系统的设计获取多源信息，并进一步在每个子系统中均设置相应的处理器，从而生成局部的定位跟踪轨迹通过串行通信总线传送至数据处理平台，并采用多源信息融合算法，该算法基于分布式状态融合结构模型，对数据进行坐标转换和数据校正、数据关联和状态估计融合。本发明借助多源信息融合算法提升检测的精度和维度，克服单个系统获得的定位信息量单一以及容易受环境因素干扰的问题。该系统中，不仅具有局部独立的定位跟踪能力，而且还有全局监视和评估特性。通过将各个定位子系统中获取的多源信息进行综合分析和协调处理，基于分布式状态融合结构模型，实现机器人等运动部件的六维位姿(三维位置和三维姿态角)的高精度、高稳定和快速运动检测系统。

附图说明

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

图1为本发明一实施例的运动轨迹检测系统的结构框图；

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

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

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

图5为本发明一实施例的多源信息融合框图；

图中：机械手臂法兰盘1，惯性传感定位子系统2，电磁定位子系统3，感应线圈4，激励线圈5，相机6，特征点7，机器视觉定位子系统8。

具体实施方式

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

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

如图1所示，一种基于多源信息融合的高精度运动轨迹检测系统，该系统由惯性传感定位子系统2、电磁定位子系统3和机器视觉定位子系统8三个模块化的子系统，以及数据处理平台组成；其中：所述惯性传感定位子系统用于三维姿态角的测量，所述电磁定位子系统用于三维位置和三维姿态信息的测量，所述机器视觉定位子系统8用于三维位置信息的测量；各个子系统的数据通过串行通信总线(SPI或者CAN)传输到所述数据处理平台，实现信息融合定位和协作定位。

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

如图2所示，在一优选实施例中，所述惯性传感定位子系统2固定在机械手臂法兰盘1末端，其采用卡尔曼滤波或正交余弦矩阵融合算法对MEMS传感器数据进行融合，以获得机械手臂法兰盘1末端精确的三维姿态角信息。

所述MEMS传感器包括一个三轴陀螺仪、一个三轴加速度计和一个三轴磁力计，第一子处理器对三者数据进行处理，得到MEMS传感器当前的姿态角，由于MEMS传感器贴附于机械手臂法兰盘1，因此也是机械手臂法兰盘1(运动部件)的姿态角。其中：三轴陀螺仪测得的角加速度一次积分后得到运动部件偏转的姿态角，但误差会随着时间而累积；三轴磁力计用于测量地磁强度进而得到动动部件的航向角；三轴加速度计信号中的三轴重力分量用于测量绝对俯仰角和翻滚角(相对于地球坐标)；三轴磁力计和三轴加速度计输出的姿态角动态性能差，用于补偿三轴陀螺仪信号一次积分后得到的姿态角，去除漂移。

物体线性位移通过对线性加速度进行二次积分，由于二次积分会引入误差，因此线性位移的跟踪精度较差，只供参考。

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

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

当然，在其他实施例中也可以采用其他能够实现上述目的的计算方法，这并不影响本发明的实质。

如图4所示，在另一优选实施例中，所述机器视觉定位子系统包括若干个相机6和FPGA嵌入式处理器，其中：若干个相机6安装在特征点7的周围，相机6采集的图像数据传到FPGA嵌入式处理器中去处理；特征点7可采用主动发光或被动发光的标记点，且贴附于运动部件上。

FPGA嵌入式处理器用于控制相机获取含有标记点的图像信号，并将图像信号传入内嵌DSP builder模块进行处理，实现特征点图像坐标的获取。首先多个相机6从不同的方位实时连续采集特征点7的图像信号；各路图像信号采用基于颜色空间模型进行目标识别，即先多次拍摄特征点取平均值，提取出特征点7的颜色分量模型[R,G,B]，再和含有该特征点7的待识别图像的每个颜色分量进行对比，进而找出特征点7在该幅图像中的二维坐标；将特征点7在各个相机6中不同时刻的成像位置二维坐标通过优化算法进行2D坐标插值计算，优化算法可以采用最小二乘法、平均法或中值法等；在所有相机6成像面中的多个二维平面坐标所构成的多条空间异面直线，通过三维坐标定位算法计算特征点7的空间三维坐标。

如图5所示，上述的各子系统采集的数据传到数据处理平台，所述数据处理平台采用分布式状态融合结构模型，该结构模型的特点是每个子系统的传感器数据在进入数据处理平台前，先由各个子系统的数据处理器(即上述第一子处理器、第二子处理器、FPGA嵌入式处理器)生成局部的定位跟踪轨迹，然后把处理过的信息送至数据处理平台，数据处理平台根据各子系统的定位跟踪轨迹数据，进行坐标转换和数据较正、数据关联以及状态估计融合，最终生成具有六维位姿的目标定位跟踪轨迹。此外，目标定位跟踪轨迹数据还反馈信息到各个子系统中，为各子系统的定位跟踪提供参考和较准。数据处理平台能够有效利用多重定位系统的定位冗余信息和互补信息，综合考虑噪声干扰和环境因素的影响，能提升目标定位跟踪的精度和维度，还能增强系统的可靠性和鲁棒性。多定位子系统提供同一运动目标的独立测量数据时，需要融合多组测量数据以导出目标的位姿信息，具体包括以下3个处理：

1)坐标转换和数据校正。不同子系统测量的数据会涉及不同的坐标系，因此需要统一各子系统的坐标系，并结合反馈的位姿信息对数据进行较正。

2)数据关联。各子系统有可能会在不同的时间观察不同的源，并且具有不同的空间准确度和分辨率。因此必须要对各个子系统测量的数据先进行关联。

常见的数据关联算法有：最邻近数据关联、联合概率数据关联、统计关联以及模糊关联等。

在一优选实施例中，采用统计关联算法，具体如下：

假设来自两个子系统的同一目标的两条轨迹，子系统i的状态估计为子系统j的状态估计为它们的误差协方差分别为P_i和P_j；两个目标状态估计的互协方差表示为P_ij和P_ji，两者之间采用关联距阵作为度量标准。

当轨迹状态估计误差是相关的时候，必须考虑互相关，这时的关联距阵为：

用关联距阵度量一条轨迹对另一条轨迹的靠近程度，便于作出关联决策指示。

当然，在其他实施例中也可以采用其他能够实现上述目的的方法，这并不影响本发明的实质。

3)状态估计融合。通过测量结果估计目标当前的状态向量，一般利用上周期外推所得的本周期目标的可能位姿信息，和本周期实时检测到的目标位姿信息，按一定的滤波算法实现融合。

常见的融合算法有：卡尔曼加权融合、协方差加权轨迹融合以及模糊轨迹融合等。

在一优选实施例中，采用卡尔曼加权融合算法，具体如下：

该系统中，各子系统假设表示子系统i的状态估计,表示子系统j的状态估计，且和相互独立，估计误差协方差分别为P_i和P_j，根据最小均方估计算法，传感器i,j的融合估计为

上式中融合估计是各传感器状态的线性组合，加权系数就是各传感器估计误差的协方差阵。此时融合估计的协方差可表示为

M_ij＝P_i(P_i+P_j)^-1P_j。

当然，在其他实施例中也可以采用其他能够实现上述目的的方法，这并不影响本发明的实质。

本发明通过将各个定位子系统中获取的多源信息进行综合分析和协调处理，基于分布式状态融合结构模型，实现机器人等运动部件三维位置和三维姿态角的高精度、高稳定和快速运动检测。

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

Claims (10)

1.一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，包括惯性传感定位、电磁定位、机器视觉定位三个模块化的子系统以及数据处理平台，惯性传感定位子系统、电磁定位子系统和机器视觉定位子系统采集的数据均传输到所述数据处理平台，以实现信息融合定位和协作定位；其中：

所述惯性传感定位子系统用于测量运动部件三维姿态角；

所述电磁定位子系统用于测量运动部件三维位置和三维姿态角；

所述机器视觉定位子系统用于测量运动部件三维位置信息；

所述数据处理平台通过将惯性传感定位子系统、电磁定位子系统、机器视觉定位子系统中获取的多源信息进行综合分析和协调处理，基于分布式状态融合结构模型，对数据进行坐标转换和数据校正、数据关联和状态估计融合。

2.根据权利要求1所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述的惯性传感定位子系统包括MEMS传感器和第一子处理器，MEMS传感器贴附于运动部件来实时获取三维姿态信息，第一子处理器采用卡尔曼滤波或正交余弦矩阵融合算法对MEMS传感器采集的数据进行融合，从而获得运动部件精确的三维姿态角信息。

3.根据权利要求2所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述的MEMS传感器包括三轴陀螺仪、三轴加速度计、三轴磁力计，所述三轴陀螺仪、三轴加速度计、三轴磁力计采集的数据均传送到第一子处理器，第一子处理器对三者数据进行处理，得到MEMS传感器当前的姿态角，即运动部件的姿态角；其中：

三轴陀螺仪测得角速度一次积分后得到运动部件偏转的姿态角；

三轴磁力计用于测量地磁强度，进而得到运动部件的航向角；

三轴加速度计测量到的信号中的三轴重力分量，用于测量运动部件的绝对俯仰角和翻滚角；

三轴磁力计和三轴加速度计输出的姿态角动态性能差，用于补偿三轴陀螺仪信号一次积分后得到的姿态角，去除漂移。

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

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

6.根据权利要求5所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述的机器视觉定位子系统采用多个相机从不同的方位实时连续采集特征点的图像信号，各路图像信号采用基于颜色空间模型进行目标识别，进而找出特征点在各个相机中不同时刻的成像位置的二维坐标；将特征点在各个相机中不同时刻的成像位置的二维坐标通过优化算法进行2D坐标插值计算；在所有相机成像面中的多个二维平面坐标所构成多条空间异面直线，通过三维坐标定位算法计算特征点的空间三维坐标。

7.根据权利要求6所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述的优化算法采用最小二乘法、平均法或中值法。

8.根据权利要求1所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述的数据处理平台采用多源信息融合算法，该算法基于分布式状态融合结构模型，该结构模型的特点是每个子系统的传感器数据在进入数据处理平台前，先由每个子系统自己的数据处理器生成局部的定位跟踪轨迹，然后把处理过的信息送至数据处理平台，数据处理平台根据各子系统的定位跟踪轨迹数据，进行坐标转换和数据较正、数据关联以及状态估计融合，最终生成具有六维位姿的目标定位跟踪轨迹；此外，目标定位跟踪轨迹数据还反馈信息到各个子系统中，为各子系统的定位跟踪提供参考和较准。

9.根据权利要求8所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述多源信息融合算法，包括以下3个处理过程：

1)坐标转换和数据校正：统一各子系统的坐标系，并结合反馈的位姿信息对数据进行较正；

2)数据关联：对各个子系统测量的数据进行关联；

3)状态估计融合：通过测量结果估计目标当前的状态向量，利用上周期外推所得的本周期目标的可能位姿信息，和本周期实时检测到的目标位姿信息，采用滤波算法实现融合。

10.根据权利要求9所述的一种基于多源信息融合的高精度运动轨迹检测系统，其特征在于，所述数据关联，采用以下任一种方法：最邻近数据关联、联合概率数据关联、统计关联或模糊关联；

所述状态估计融合，采用以下任一种方法：卡尔曼加权融合、协方差加权轨迹融合或模糊轨迹融合。