[发明专利]刚体运动捕捉方法及装置、AGV位姿捕捉系统

权利要求书

1.一种刚体运动捕捉方法，其特征在于，所述方法包括：

基于多个图像采集设备在当前时刻对刚体上的光学标志进行图像采集得到的各个图像，获取所述光学标志在所述各个图像中成像的二维位置信息，每个所述刚体上设置有多个光学标志，所述多个光学标志唯一标识所述刚体，且所述多个光学标志能够指示所述刚体的不同运动方向；

根据所述刚体中已知刚体在上一时刻的位姿信息进行预测，得到所述已知刚体上的光学标志在所述当前时刻的预测三维位置信息，所述已知刚体为记录了上一时刻的位姿信息的刚体，所述已知刚体在上一时刻的位姿信息包括所述已知刚体上的光学标志在所述上一时刻的三维位置信息和速度信息；

基于所述多个图像采集设备的参数信息，将所述已知刚体上的光学标志在所述当前时刻的预测三维位置信息转换为预测二维位置信息，所述参数信息包括内参和外参；

将所述已知刚体上的光学标志的预测二维位置信息与所述各个图像中成像的二维位置信息进行匹配，确定所述已知刚体上的光学标志在所述各个图像中成像的二维位置信息，同一个已知刚体上的光学标志在所述各个图像中成像的二维位置信息与预测二维位置信息之间的距离符合预设条件；

将所述已知刚体上的光学标志在所述各个图像中成像的二维位置信息，转换为所述已知刚体上的光学标志在所述当前时刻的三维位置信息；

根据预先存储的已知刚体上的光学标志之间的位置关系及所述已知刚体上的光学标志在所述当前时刻的三维位置信息，确定所述已知刚体在所述当前时刻的位姿信息，所述已知刚体在所述当前时刻的位姿信息包括所述已知刚体上的光学标志在所述当前时刻的三维位置信息和速度信息。

2.根据权利要求1所述的方法，其特征在于，所述基于所述多个图像采集设备的参数信息，将所述已知刚体上的光学标志在所述当前时刻的预测三维位置信息转换为预测二维位置信息，包括：

基于所述多个图像采集设备的参数信息和所述已知刚体上的光学标志在所述当前时刻的预测三维位置信息，将所述已知刚体上的光学标志反投影到所述多个图像采集设备的成像平面上，得到所述已知刚体上的光学标志的反投影点在所述多个图像采集设备的成像平面上的多个二维位置信息；

将所述反投影点在所述多个图像采集设备的成像平面上的多个二维位置信息作为所述预测二维位置信息。

3.根据权利要求1所述的方法，其特征在于，所述将所述已知刚体上的光学标志的预测二维位置信息与所述各个图像中成像的二维位置信息进行匹配，确定所述已知刚体上的光学标志在所述各个图像中成像的二维位置信息，包括：

获取所述已知刚体上的光学标志在目标图像上的预测二维位置信息以及所述目标图像中成像的二维位置信息，所述目标图像为任一图像采集设备在所述当前时刻采集的图像；

采用匈牙利算法对所述已知刚体上的光学标志在目标图像上的预测二维位置信息以及所述目标图像中成像的二维位置信息进行匹配，以得到至少一个匹配组合，每个所述匹配组合中包括至少一个匹配对，每个所述匹配对包括一个预测二维位置信息和一个成像的二维位置信息；

计算每个所述匹配组合对应的代价函数的值，所述代价函数用于指示所述匹配组合中所有匹配对的距离之和；

确定目标匹配组合中每个匹配对包括的预测二维位置信息和一个成像的二维位置信息指示所述已知刚体上的同一光学标志，所述目标匹配组合为所述至少一个匹配组合中所述代价函数的值最小的匹配组合；

从所述各个图像对应的目标匹配组合中获取所述已知刚体上的光学标志在所述各个图像中成像的二维位置信息。

4.根据权利要求1所述的方法，其特征在于，所述将所述已知刚体上的光学标志的预测二维位置信息与所述各个图像中成像的二维位置信息进行匹配，确定所述已知刚体上的光学标志在所述各个图像中成像的二维位置信息，包括：

从所述各个图像中成像的二维位置信息中，选择与所述已知刚体上的目标光学标志的多个预测二维位置信息的距离小于预设阈值的多个二维位置信息，所述目标光学标志为所述已知刚体上的任一光学标志；

当选择的多个二维位置信息能够重构出三维位置信息时，确定选择的多个二维位置信息为所述目标光学标志在所述各个图像中成像的二维位置信息。