[发明专利]一种定位刚体的方法、装置和系统

权利要求书

1.一种定位刚体的方法，其特征在于，所述方法包括：

接收双目红外摄像头模组采集的红外发光体的红外图像；

接收惯性测量单元IMU采集的姿态信息；其中，所述红外发光体和所述IMU设置在待定位刚体上；

依据所述红外图像中的与所述红外发光体对应的红外发光点的位置，确定所述红外发光体在全局坐标系的三维位置坐标，将所述三维位置坐标作为所述待定位刚体的移动自由度；

依据所述姿态信息确定所述待定位刚体在所述全局坐标系的转动自由度；其中，依据所述移动自由度和所述转动自由度定位所述待定位刚体；

所述接收双目红外摄像头模组采集的红外发光体的红外图像包括：

接收多个双目红外摄像头模组采集的红外发光体的多个红外图像；

所述依据所述红外图像中的与所述红外发光体对应的红外发光点的位置，确定所述红外发光体在全局坐标系的三维位置坐标包括：

依据所述多个红外图像中的与所述红外发光体对应的红外发光点的位置，计算所述红外发光体相对于每个双目红外摄像头模组的三维位置坐标；

获取所述红外发光体相对于每个双目红外摄像头模组的历史轨迹；

依据所述三维位置坐标和所述历史轨迹，计算所述多个双目红外摄像头模组在全局坐标系的相对位置；

依据所述相对位置和所述红外发光体相对于每个双目红外摄像头模组的三维位置坐标，计算所述红外发光体在所述全局坐标系的三维位置坐标。

2.如权利要求1所述的方法，其特征在于，

所述接收双目红外摄像头模组采集的红外发光体的红外图像；接收惯性测量单元IMU采集的姿态信息包括：接收双目红外摄像头模组采集的多个红外发光体的红外图像；接收多个惯性测量单元IMU采集的姿态信息；其中，所述多个红外发光体和所述多个惯性测量单元IMU分别置于多个待定位刚体上；

在所述依据所述红外图像中的与所述红外发光体对应的红外发光点的位置，确定所述红外发光体在全局坐标系的三维位置坐标之前，所述方法还包括：

确定所述红外图像中的红外发光点与所述红外发光体的对应关系；

所述依据所述红外图像中的与所述红外发光体对应的红外发光点的位置，确定所述红外发光体在全局坐标系的三维位置坐标，将所述三维位置坐标作为所述待定位刚体的移动自由度包括：

依据所述红外图像中的与所述红外发光体对应的红外发光点的位置，确定与所述红外发光点具有对应关系的所述红外发光体的三维位置坐标，将所述三维位置坐标作为与所述红外发光体对应的所述待定位刚体的移动自由度。

3.如权利要求2所述的方法，其特征在于，所述确定所述红外图像中的红外发光点与所述红外发光体的对应关系包括：

依据每个所述红外发光点的历史轨迹计算每个所述红外发光点的线速度方向；

依据每个所述IMU的所述姿态信息确定每个所述IMU当前朝向的单位方向向量；并依据每个所述IMU的朝向的方向向量的历史轨迹和所述当前朝向的单位方向向量计算每个所述IMU的角度变化方向；

判断所述红外发光点的线速度方向与所述IMU的角度变化方向是否一致；若判断为是，则确定所述红外发光点与所述IMU具有对应关系；其中，所述IMU与所述红外发光体相对应。

4.如权利要求2所述的方法，其特征在于，所述确定所述红外图像中的红外发光点与所述红外发光体的对应关系包括：

依据所述红外图像中与所述多个红外发光体的对应的红外发光点的位置关系，向所述多个红外发光体发送闪烁指令，所述闪烁指令用于指示所述多个红外发光体依据所述闪烁指令进行闪烁；

接收所述双目红外摄像头模组采集所述多个红外发光体的红外图像；

依据所述红外图像中的与所述多个红外发光体对应的红外发光点的发光状态确定所述红外发光点与所述红外发光体的对应关系。