[发明专利]一种基于双目视觉的姿态测量方法

权利要求书

1.一种基于双目视觉的姿态测量方法，其特征在于，使用双目视觉系统通过计算被跟踪物体上的标记条来测量和定位，包括以下步骤：

步骤1、双目视觉系统的标定，得到相机的内部参数，以及相机之间的平移和旋转关系；

步骤2、姿态计算，利用三角测量对标记点进行三维坐标的测量；

步骤3、对测定的姿态旋转和平移数据进行卡尔曼滤波；

所述步骤2中，姿态计算的具体步骤如下：

(1)首先计算当前双目相机检测到的三个标记点的质心坐标Poc，然后将三个三维点分别减去质心坐标，得到质心坐标系下的新坐标，设为Pc；三个三维坐标分别减去质心坐标计算前一次双目相机检测到的三个标记点在其质心Pop坐标系下的新坐标Pp；

(2)利用点集Pc和Pp确定的平面的法向量nc＝Pc1 x Pc2和np＝Pp1 x Pp2，利用两个平面的法向量将两个平面旋转至同一个平面，使用四元数的理论计算出两个法向量之间的旋转，其中Pc＝{Pc1，Pc2，Pc3}为双目相机当前时刻检测到的三个标记点三维坐标，Pp＝{Pp1，Pp2，Pp3}为双目相机上一时刻检测到的三个标记点三维坐标；

(3)经过上述步骤，当前时刻三个标记点确定的平面与上一时刻由三个标记点确定的两个平面已经处在同一个平面，这时两个坐标系的z轴重合，但是x、y坐标系依然存在旋转，通过计算两组点组成的向量之间的夹角即可算出面内旋转；

(4)两组标记条的质心之间的坐标即为平移矢量，得到运动前后标记点之间的旋转和平移关系。

2.根据权利要求1所述的基于双目视觉的姿态测量方法，其特征在于，所述步骤1中的标定过程包括相机内参标定和双目相机立体标定。

3.根据权利要求2所述的基于双目视觉的姿态测量方法，其特征在于，相机内参标定包括相机的焦距、光心坐标以及相机成像时的畸变参数的标定。

4.根据权利要求2所述的基于双目视觉的姿态测量方法，其特征在于，在双目相机立体标定过程中，首先计算多个相机之刚体变换关系，从而建立真实的物理世界到虚拟的相机坐标系之间的对应关系，再利用相机的内部参数就可以构造物理世界到相机坐标系之间的对应关系。

5.根据权利要求1所述的基于双目视觉的姿态测量方法，其特征在于，选择三个点的坐标过程中，首先选择五个对应点进行检测，得到稳定的五个对应点之后取出其中的三个进行计算。