[发明专利]基于机械臂末端单目视觉的动态目标位置和姿态测量方法

权利要求书

1.一种基于机械臂末端单目视觉的动态目标位置和姿态测量方法，其特征在于该方法包括以下步骤： 

（1）设摄像机拍摄图像的平面坐标系为（u，v），采用张正友棋盘格标定法，得到摄像机的内参数矩阵M，以及摄像机的径向畸变参数k₁和k₂以及切向畸变参数k₃和k₄，其中，f为摄像机焦距，为摄像机在图像平面坐标系的u轴上的归一化焦距，为摄像机在图像平面坐标系的v轴上的归一化焦距，（u₀，v₀）为摄相机光轴与摄像机拍摄图像平面交点的像素坐标； 

（2）利用手眼标定法，得到机械臂末端和摄像机之间的旋转变换矩阵，记为以及机械臂末端和摄像机之间的位移变换矩阵，记为为3×3矩阵，为3×1矩阵； 

（3）在目标上设置两个标记点，利用机械臂末端摄像机，在两个不同位置拍摄到该目标的两幅图像，从两幅图像中区分出目标区域和背景区域，利用尺度不变特征提取方法，分别从两幅图像中提取出目标区域的空间特征点，该空间特征点包括目标上的两个标记点； 

（4）根据上述空间特征点，对两幅图像中目标区域的空间特征点进行匹配，得到初始匹配结果，并根据初始匹配结果，对步骤（3）的尺度不变特征提取方法中的参数进行调整，以获得两幅图像之间有8对以上匹配特征点，匹配特征点中包括目标上的两个标记点； 

（5）根据步骤（4）的两幅图像之间8对以上匹配特征点，利用对极几何约束方法，求解两幅图像之间的基础矩阵F； 

（6）根据上述步骤（1）的摄像机内参数矩阵M和步骤（5）的基础矩阵F，求解摄 像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵E，利用本质矩阵E，求解摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转变换矩阵和位移变换矩阵 得到四组候选解，具体过程如下： 

（6-1）利用如下公式，根据步骤（5）的基础矩阵F和步骤（1）的摄像机内参数矩阵M，计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵E： 

E＝M^TFM，其中M^T为矩阵M的转置矩阵； 

（6-2）利用特征值分解方法，对上述本质矩阵E进行分解，E＝Udiag(s,s,0)V^Τ，得到四组候选解： 

其中

（7）根据机械臂的控制参数和上述步骤（2）得到的机械臂末端和摄像机之间的旋转变换矩阵和位移变换矩阵计算得到摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值和位移初值根据旋转初值和位移初值得到步骤（6）中的四组候选解中的正解和具体过程如下： 

（7-1）根据机械臂的控制参数得到机械臂末端的旋转变换矩阵和位移变换矩阵根据上述步骤（2）得到的机械臂末端和摄像机之间的旋转变换矩阵和位移变换矩阵 通过下式计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值和位移初值

（7-2）分别计算上述步骤（6）得到的旋转矩阵候选解与旋转初值之间的旋转矩阵， 得到该旋转矩阵的范数，将与该旋转矩阵范数中最小范数值相对应的候选解作为摄像机旋转变换的正解

（7-3）分别计算上述步骤（6）得到的位移矩阵候选解与位移初值之间的夹角，将与该夹角中最小夹角相对应的候选解作为摄像机位移变换的正解

（8）根据上述步骤（7）中得到的摄像机旋转变换矩阵和位移变换矩阵对上述步骤（4）得到的两幅图像之间的匹配特征点进行特征点三维重构，根据目标上的两个标记点之间的距离，对摄像机的位移变换和三维重构的特征点进行尺度校正，包括以下步骤： 

（8-1）利用三角测量方法，得到步骤（4）的两幅图像之间所有匹配特征点在摄像机坐标系中的三维坐标P′； 

（8-2）从上述所有匹配特征点的三维坐标中，得到目标上两个标记点之间的计算距离d； 

（8-3）根据目标上的两个标记点之间的物理距离D，得到该物理距离与上述计算距离之间的比值

（8-4）根据上述比值k，对上述摄像机的位移变换矩阵和特征点在摄像机坐标系中的三维坐标，根据下式进行尺度校正，得到尺度校正后的摄像机在拍摄目标的两幅图像时所在的两个不同位置之间的物理位移以及三维重构特征点的物理坐标P： 

P＝kP′ 

（9）构建目标坐标系，求解目标的相对摄像机的位置和姿态，具体过程如下： 

（9-1）根据上述步骤（8）得到的三维重构特征点的物理坐标，构建目标坐标系， 

以目标上3个特征点为例构建目标坐标系，i，j，k分别为目标坐标系XYZ坐标轴的单位向量，坐标原点为点A，AB为X轴，以ABC平面中垂直AB的方向为Y轴，然后依据 右手法则得到Z轴，设P_a，P_b，P_c为3特征点在摄像机坐标系下的坐标，各坐标轴的单位向量计算如下： 

（9-2）依据如下公式计算任意目标特征点在目标坐标系中的坐标_oP_i

_oP_i＝[i j k](P_i-P_a) 

其中，P_i为任意特征点的重构坐标； 

（9-3）利用坐标系转换方法或基于点特征定位的方法，根据任意目标特征点在目标坐标系中的坐标_oP_i和重构坐标P_i，计算目标的相对摄像机的位置和相对旋转矩阵

（9-4）根据上述相对旋转矩阵计算目标的相对姿态，以欧拉角XYZ顺序表示目标姿态（α,β,γ)，则旋转矩阵表示如下： 

从上述旋转矩阵中，得到目标的相对摄像机的姿态为： 

β=asin(-R_oc(1,3)) 

。