一种6自由度空间位置和姿态的检测系统和方法与流程

本发明实施例涉及空间位置与姿态检测技术领域，尤其涉及一种6自由度空间位置和姿态的检测系统和方法。

背景技术：

近年来，随着空间位置和姿态检测技术的发展，该技术越来越广泛的应用于诸如辅助体育训练、增强现实、医疗康复辅助、工业设备检测以及人机交互等技术领域。

现有的空间位置和姿态检测技术研究，多采用单一传感器的检测方式，如惯性传感器式、电磁传感器式、光学摄像头式或激光传感器式，由于只存在一种检测方式，因此该类研究往往存在精确度低、检测空间范围有限、容易受到遮挡影响或检测数据有限等问题。此外，也有部分研究采用结合光学摄像头和其他传感器的方案，该类研究利用光学摄像头的低频信息和其他传感器的高频信息，保证最终获取到的空间位置和姿态数据的准确性，但是，由于该类研究中的光学摄像头需要安装在目标物体的周围，因此，仍然存在易受遮挡影响的问题，并且由于摄像头固定，极大限制了检测的范围，并且移动性和便携性都比较差。

因此，需要研究一种既能满足准确检测空间位置和姿态数据，又能够避免由于遮挡问题造成的影响，同时还能提高移动性和便携性的空间位置和姿态检测系统与方法。

技术实现要素：

本发明提供一种6自由度空间位置和姿态的检测系统和方法，在提高准确检测空间位置和姿态数据的同时，避免了由于遮挡问题造成的影响。

第一方面，本发明实施例提供了一种6自由度空间位置和姿态的检测系统，包括传感器模块、图像采集模块和处理器，所述传感器模块和所述图像采集模块均设于被测目标上；所述传感器模块和所述图像采集模块分别与所述处理器连接；其中：

所述传感器模块，用于测量与所述被测目标的位置和姿态相关的传感数据；

所述图像采集模块，用于采集所述被测目标四周的图像数据；

所述处理器，用于处理所述传感数据和所述图像数据获得6自由度空间位置和姿态数据。

进一步的，所述传感器模块，包括加速度计、陀螺仪和磁力计；其中：所述加速度计用于测量所述被测目标的加速度，所述陀螺仪用于测量所述被测目标的角速度，所述磁力计用于测量所述被测目标四周的磁场的分布量；

所述图像采集模块，包括至少四个摄像头。

进一步的，所述检测系统的四个侧面分别包括至少一个所述摄像头。

进一步的，所述加速度计为三轴加速度计；

所述陀螺仪为三轴陀螺仪；

所述磁力计为三轴磁力计；

所述摄像头为广角摄像头。

进一步的，还包括：

无线传输模块，与所述处理器连接，用于输出所述6自由度空间位置和姿态数据；和

电源管理模块，与所述传感器模块、所述图像采集模块、所述处理器和所述无线传输模块连接，用于对各部分进行电力供应和电力管理。

第二方面，本发明还提供了一种6自由度空间位置和姿态的检测方法，该方法由第一方面所述的6自由度空间位置和姿态的检测系统的处理器来执行，包括：

接收原始数据，所述原始数据包括与被测目标的位置和姿态相关的传感数据和图像数据；

对所述原始数据进行数据标定、融合或相互补偿处理获得6自由度空间位置和姿态数据。

本发明实施例提供的6自由度空间位置和姿态的检测系统和方法，通过将传感器模块与图像采集模块均安装于被测目标上，随被测目标的运动而运动，这样提高了系统的移动性和便携性；图像采集模块位置不固定，增加了其数据采集的范围；图像采集模块采集周围环境图像数据而不是采集被测目标本身运动图像数据，避免了由于被测目标被遮挡而造成图像数据无法利用的问题；同时，对传感器模块采集到的高频数据和图像采集模块采集到的低频数据进行处理，提高了目标数据的精确度和稳定性，实现了对被测目标的6自由度空间位置和姿态的高精度检测。

附图说明

图1是本发明实施例一中的一种6自由度空间位置和姿态的检测系统的结构示意图；

图2a是本发明实施例二中的一种6自由度空间位置和姿态的检测系统的结构俯视图；

图2b是本发明实施例二中的一种6自由度空间位置和姿态的检测系统的结构前视图；

图3是本发明实施例三中的一种6自由度空间位置和姿态的检测方法的流程图；

图4是本发明实施例四中的一种6自由度空间位置和姿态的检测方法的流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

实施例一

图1是本发明实施例一提供的一种6自由度空间位置和姿态的检测系统的结构示意图。如图1所示，该检测系统包括：

传感器模块110、图像采集模块120和处理器130，传感器模块110和图像采集模块120均设于被测目标上；传感器模块110和图像采集模块120分别与处理器130连接。

不同于传统的安装方式，本实施例中，传感器模块110和图像采集模块120都安装于被测目标上，当被测目标运动时，传感器模块110和图像采集模块120也随之运动，以实时监测被测目标的运动情况。

本实施例中，传感器模块110优选可以通过spi或i2c接口与处理器130进行连接；图像采集模块120优选可以通过usb接口电路与处理器130进行连接。

其中，传感器模块110，用于测量与被测目标的位置和姿态相关的传感数据；图像采集模块120，用于采集被测目标四周的图像数据；处理器130，用于处理传感数据和图像数据获得6自由度空间位置和姿态数据。

本实施例中，传感器模块110采样频率高，实时性好，其采集到的传感数据能够实时反映被测目标的位置和姿态的变化。

本实施例中，与传统的图像采集模块采集被测目标自身的位置和姿态的图像数据不同，图像采集模块120采集的是被测目标周围环境的图像数据，根据周围环境的位置和姿态的图像变化，得到被测目标的位置和姿态的变化数据，图像采集模块120的采样频率低，但其采样精度高，能够与传感器模块110采集到的传感数据相互补充，以保证检测系统的实时性和稳定性。

本实施例中，处理器130的运行频率优选可以是1.2ghz以上，内存优选可以是4g以上。

本实施例中，通过将传感器模块与图像采集模块均安装于被测目标上，随被测目标的运动而运动，提高了系统的移动性和便携性；图像采集模块位置不固定，增加了其数据采集的范围；图像采集模块采集周围环境图像数据而不是采集被测目标本身运动的图像数据，避免了由于遮挡问题而造成图像数据无法利用的问题；同时，对传感器模块采集到的高频数据和图像采集模块采集到的低频数据进行处理，提高了目标数据的精确度，实现了对被测目标的6自由度空间位置和姿态的高精度检测。

在上述实施例的基础上，进一步的，传感器模块110可以包括加速度计、陀螺仪和磁力计，其中，加速度计用于测量被测目标的加速度，陀螺仪用于测量被测目标的角速度，磁力计用于测量被测目标四周的磁场的分布量。

本实施例中，加速度计优选可以是三轴加速度计；陀螺仪优选可以是三轴陀螺仪；磁力计优选可以是三轴磁力计；三轴加速度计、三轴陀螺仪和三轴磁力计均可测量x、y、z三个轴向的数据，以保证所测数据全面准确。

图像采集模块120，包括至少四个摄像头。

本实施例中，图像采集模块120由多个摄像头构成，其中，每个摄像头的参数相同，摄像头优选可以是广角摄像头，其分辨率优选可以是480p以上，采样频率优选可以是30fps以上。摄像头的个数优选可以是4个，以保证图像采集模块可以全方位、无死角采集到被测目标周围环境的图像，提高图像数据的全面性和准确性。

实施例二

图2a是本发明实施例二提供的一种6自由度空间位置和姿态的检测系统的结构俯视图，图2b是本发明实施例二提供的一种6自由度空间位置和姿态的检测系统的结构前视图，图2a和图2b中所示出的坐标系均为传感器模块的坐标系。本实施例在实施例一的基础上进行了优化，如图2a和2b所示，

检测系统的四个侧面分别包括至少一个摄像头。

本实施例中，传感器模块、图像采集模块和处理器优选可以集成为一个整体。其中，检测系统壳体250优选可以封装成立方体或长方体形状，优选可以将传感器模块和处理器封装于立方体或长方体形状的检测系统壳体250内部，其中，传感器模块可以包括加速度计、陀螺仪和磁力计。检测系统壳体250的四个侧面中，相对的两个面相互平行，相邻的两个面相互垂直。每个侧面优选可以包括一个摄像头，每个摄像头朝向外部设置，其中，摄像头210和摄像头220位于相对的两个侧面且朝向相反，摄像头230和摄像头240位于相对的另外两个侧面且朝向相反，摄像头210和摄像头220之间的连线与摄像头230和摄像头240之间的连线相互垂直，四个摄像头两两之间的连线在同一个水平面上。

在上述各实施例的基础上，进一步的，6自由度空间位置和姿态的检测系统还包括：无线传输模块(图未示出)和电源管理模块(图未示出)，其中，无线传输模块与所述处理器连接，用于输出所述6自由度空间位置和姿态数据；电源管理模块与所述传感器模块、所述图像采集模块、所述处理器和所述无线传输模块连接，用于对各部分进行电力供应和电力管理。优选的，还可以将无线传输模块和电源管理模块封装于检测系统壳体250内部。

本实施例中，处理器优选可以通过uart或spi接口与无线传输模块连接；无线传输模块优选可以通过无线传输方式将6自由度空间位置和姿态数据发送给上位主机。

本实施例中，通过将传感器模块、图像采集模块和处理器集成为一个整体，提高了系统的移动性和便携性；将图像采集模块中的摄像头均匀设置于检测系统壳体的四个侧面中心，保证了摄像头视场充分被利用，增加了数据采集的范围；图像采集模块采集周围环境图像数据而不是采集被测目标本身运动的图像数据，避免了由于被测目标被遮挡而造成图像数据无法利用的问题；同时，对传感器模块采集到的高频数据和图像采集模块采集到的低频数据进行处理，提高了目标数据的精确度，实现了对被测目标的6自由度空间位置和姿态的高精度检测。

示例性的，6自由度空间位置和姿态的检测系统中的传感器模块包含三轴加速度计、三轴陀螺仪和三轴磁力计，将其封装到立方体形状的检测系统壳体中；图像采集模块包含四个广角摄像头，其中，四个广角摄像头分别安装于检测系统壳体四个侧面的中心位置。将该结构安装于被测目标上，传感器模块中的三轴加速度计、三轴陀螺仪和三轴磁力计通过spi接口将测量得到的被测目标的运动加速度、角速度和四周磁场的分布量发送给该系统的处理器；图像采集模块中的摄像头通过usb接口电路将采集到的被测目标周围环境的图像数据发送给该系统的处理器。处理器接收到传感数据和图像数据后，对其进行处理，得到6自由度空间位置和姿态数据，并通过uart接口将该数据发送给无线传输模块，无线传输模块利用wifi将该数据发送给上位主机。

实施例三

图3是本发明实施例三提供的一种6自由度空间位置和姿态的检测方法的流程图，该方法可由上述各实施例中的6自由度空间位置和姿态的检测系统的处理器来执行，包括：

步骤310、接收原始数据，原始数据包括与被测目标的位置和姿态相关的传感数据和图像数据。

本实施例中，处理器接收原始数据，该原始数据包括加速度、角速度、磁场的分布量以及图像数据，由上述各实施例中的传感器模块中的加速度计、陀螺仪、磁力计以及图像采集模块中的摄像头分别获取并发送。

步骤320、对原始数据进行数据标定、融合或相互补偿处理获得6自由度空间位置和姿态数据。

本实施例中，通过处理器接收由多种传感器和摄像头发送的全面且多方位的、与被测目标当前6自由度空间位置和姿态数据等相关的数据，克服了传统使用单一传感器数据的不足；同时，传感器模块采集到的高频数据和图像采集模块采集到的低频数据进行数据标定、融合或相互补偿处理，能够更加精确地得到被测目标6自由度空间位置和姿态等信息，实现了对目标物体6自由度空间位置和姿态的高精度检测。

实施例四

本实施例在实施例三的基础上，提供了步骤320的优选实施方式，图4是本发明实施例四提供的一种6自由度空间位置和姿态的检测方法的流程图，参考图4，该6自由度空间位置和姿态的检测方法包括：

步骤410、接收原始数据，原始数据包括与被测目标的位置和姿态相关的传感数据和图像数据。

步骤420、对每个摄像头采集到的图像数据进行标定及坐标系转换。

本实施例中，在摄像头使用之前，需要对其参数进行标定。对于摄像头参数的标定方法，优选可以是平面方格标定法，具体标定步骤如下：(1)选择一个尺寸为324(mm)*252(mm)，由7行9列正方形方格组成标定板；(2)利用水平平行的左右两个上述摄像头同时采集标定板的不同位姿图像(位姿越多，标定的结果越精确，优选可以是15组位姿)；(3)提取每幅图像中的特征点，进行单目标定，得到左右相机的内外参数以及畸变系数，至此完成摄像头参数的标定。利用上述定标过程得到的内外参数以及畸变系数，即可确定图像数据中每个像素点所对应的外界环境的实际距离。

又由于每个摄像头以及传感器模块都存在自身的坐标系，因此，在对图像数据进行标定前，需要对每个摄像头以及传感器模块之间的坐标关系进行标定。对于每个摄像头以及传感器模块之间的坐标关系的标定方法，优选可以利用摄像头与摄像头之间，摄像头与传感器模块之间的位置关系，建立对应的坐标转换矩阵，通过该坐标转换矩阵，将每个摄像头测得的图像数据和传感器模块采集到的传感数据置于一个坐标系下进行处理。

步骤430、对每个经标定及坐标系转换后的图像数据进行处理，得到与传感器模块的坐标系相对应的初始空间位置数据和姿态数据。

本实施例中，优选可以利用基于视觉的同步定位与地图构建方法，对每个摄像头采集到的，经过标定及坐标系转换(将摄像头坐标系转换为传感器模块坐标系)的图像数据进行处理，得到与传感器模块的坐标系相对应的初始3自由度空间位置数据和3自由度姿态数据。其中，基于视觉的同步定位与地图构建方法，优选可以是orb-slam算法、ptam算法、lsd-slam算法、dtam算法、dpptam算法等。

步骤440、将多个初始空间位置数据和姿态数据进行融合，得到融合后的空间位置数据和姿态数据。

本实施例中，在得到与传感器模块的坐标系相对应的初始空间位置数据和姿态数据后，以及将多个初始空间位置数据和姿态数据进行融合前，进一步的，还包括：为初始空间位置数据和姿态数据分别设定一个权值。

本实施例中，利用基于视觉的同步定位与地图构建方法处理图像的过程中，还可以获得每个摄像头所采集图像数据的特征清晰度和显著度，利用该特征清晰度和显著度，可以确定该图像数据是否可信。当特征清晰度和显著度较高时，图像数据可信，当特征清晰度和显著度较低时，图像数据不可信。因此，优选可以根据特征清晰度和显著度的高低，对初始空间位置数据和姿态数据进行权值补偿，以避免出现由于使用模糊图像而导致的数据处理结果精确度较低的问题。当特征清晰度和显著度较低时，权值优选可以设为0.00-0.10之间的数值，当特征清晰度和显著度较高时，权值优选可以设为0.95-1.00之间的数值。

之后，对每个与传感器模块的坐标系相对应的，且经过权值补偿后的初始3自由度空间位置数据和3自由度姿态数据进行融合，得到基于多摄像头的，且经过相互补偿后的3自由度空间位置数据和3自由度姿态数据。其中，融合算法优选可以是互补滤波器算法。

本实施例中，由于摄像头采集到的图像数据为低频信号，因此，由上述处理得到的基于多摄像头的，且经过相互补偿后的3自由度空间位置数据和3自由度姿态数据可定期对传感数据进行融合与补偿处理。

步骤450、利用角速度计算被测目标的原始姿态数据，所述姿态数据包括俯仰角、偏航角和滚转角。

本实施例中，利用传感数据中的角速度，初步计算被测目标对应的原始姿态数据，其中，姿态数据包括三个姿态角，分别是俯仰角、偏航角和滚转角。

步骤460、利用加速度、磁场的分布量和融合后的姿态数据对原始姿态数据进行校正，得到3自由度姿态数据。

本实施例中，利用加速度对俯仰角和滚转角进行补偿校正，利用磁场的分布量和融合后的姿态数据对航向角进行补偿校正，得到经过补偿校正后的3自由度姿态数据。

步骤470、利用加速度、原始姿态数据和空间位置信息得到3自由度空间位置数据。

为了得到3自由度空间位置数据，需要计算线性加速度。本实施例中，优选可以利用加速度和原始姿态数据中的俯仰角、偏航角和滚转角，计算线性加速度。具体的，由于加速度中包含重力加速度，因此，可以结合由角速度计算得到的三个姿态角数据，得到重力加速度在传感器模块的三个坐标轴上的分量，并将上述分量从加速度中去除，从而得到线性加速度。对该线性加速度进行积分，得到传感器模块对应的速度数据。为了使输出的3自由度空间位置数据更加精确，优选可以利用由图像采集模块得到的3自由度空间位置，对传感器模块对应的空间位置数据进行补偿校正。具体的，对图像采集模块对应的空间位置数据进行微分计算，得到图像采集模块对应的速度数据。利用图像采集模块对应的速度数据，对传感器模块对应的速度数据进行补偿校正后，对补偿校正后的速度数据进行积分计算，得到经过补偿校正后的3自由度空间位置数据。

本实施例中，通过对图像采集模块的低频图像数据与传感器模块测得的高频传感数据进性数据标定、融合或相互补偿校正处理，提高了目标数据的精确度，实现了对被测目标的6自由度空间位置和姿态的高精度检测。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。