[发明专利]一种6自由度空间位置和姿态的检测系统和方法

权利要求书

1.一种6自由度空间位置和姿态的检测系统，其特征在于，包括传感器模块、图像采集模块和处理器，所述传感器模块和所述图像采集模块均设于被测目标上；所述传感器模块和所述图像采集模块分别与所述处理器连接；其中：

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

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

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

2.根据权利要求1所述的检测系统，其特征在于，

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

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

3.根据权利要求2所述的检测系统，其特征在于，

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

4.根据权利要求2所述的检测系统，其特征在于，

所述加速度计为三轴加速度计；

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

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

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

5.根据权利要求1-4任一项所述的检测系统，其特征在于，还包括：

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

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

6.一种6自由度空间位置和姿态的检测方法，其特征在于，

该方法由权利要求1-5任一项所述的6自由度空间位置和姿态的检测系统的处理器来执行，包括：

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

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

7.根据权利要求6所述的检测方法，其特征在于，所述传感数据包括加速度、角速度和磁场的分布量。

8.根据权利要求7所述的检测方法，其特征在于，对所述原始数据进行数据标定、融合或相互补偿处理，包括：

对每个所述摄像头采集到的所述图像数据进行标定及坐标系转换；

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

将多个所述初始空间位置数据和所述姿态数据进行融合，得到融合后的所述空间位置数据和所述姿态数据；

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

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

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

9.根据权利要求8所述的检测方法，其特征在于，

在将多个所述初始空间位置数据和所述姿态数据进行融合前，还包括：为多个所述初始空间位置和所述姿态数据分别设定一个权值。

10.根据权利要求8所述的检测方法，其特征在于，

利用所述加速度、所述磁场的分布量和所述融合后的姿态数据对所述原始姿态数据进行校正，得到所述3自由度姿态数据，包括：

利用所述加速度，对所述俯仰角和所述滚转角进行补偿校正，利用所述磁场的分布量和所述融合后的姿态数据对所述偏航角进行补偿校正，得到校正后的所述3自由度姿态数据；

利用所述加速度、所述原始姿态数据和所述空间位置信息得到所述3自由度空间位置数据，包括：

利用所述加速度和所述原始姿态数据，计算线性加速度，并对所述线性加速度进行积分计算，得到第一速度值，对所述空间位置信息进行微分计算，得到第二速度值，利用所述第二速度值对所述第一速度值进行补偿校正，并对补偿校正后的速度值进行积分计算，得到所述3自由度空间位置数据。