[发明专利]单相机环境中主动式刚体的位姿定位方法及相关设备

权利要求书

1.一种单相机环境中主动式刚体的位姿定位方法，其特征在于，所述方法包括以下步骤：

获取单目相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和所述相机的相机参数，根据所述二维空间点编码，将相邻两帧的所述二维空间点坐标进行匹配，得到多组二维空间特征对，将多组所述二维空间特征对和所述相机参数构造线性方程组，求解出本质矩阵；

通过奇异值分解算法分解所述本质矩阵，得到多组旋转矩阵和平移矩阵；

根据多个相机的内参和畸变参数，将同帧中捕捉到的多个像素坐标转化为相机坐标；构造最小二乘法矩阵X和Y，根据等式和所述矩阵X、矩阵Y，利用奇异值分解求得一个三维空间点坐标；根据多个不同的旋转矩阵和平移矩阵，得到多个不同的三维空间点坐标；

检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿。

2.根据权利要求1所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿，包括：

将所述三维空间点坐标内的所有三维空间点之间的距离求和后取平均值，得到三维平均距离；

获取刚体坐标，将所述刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离；

通过优化公式将所述目标平移矩阵进行优化，得到优化后的目标平移矩阵，根据所述目标旋转矩阵和优化后的所述目标平移矩阵确定刚体位姿；

所述优化公式为：

其中，L1为所述三维平均距离，L2为所述刚体平均距离，T为优化前的所述目标平移矩阵，T′为优化后的所述目标平移矩阵。

3.根据权利要求2所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述获取刚体坐标，将所述刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离前，包括：

获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；

将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点坐标；

将同帧的所有三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体坐标。

4.根据权利要求3所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点坐标，包括：

将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，通过奇异值分解求解最小二乘法方法，解算得到一组三维空间点坐标；

判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；

计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标。

5.根据权利要求3所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述将同帧的所有三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体坐标，包括：

计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；

分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标。

6.根据权利要求1所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述等式为X*C＝Y，其中，所述X为4*3的矩阵，Y为4*1的矩阵，C为一个三维空间点坐标。