[发明专利]基于双目视觉的目标姿态解算方法

权利要求书

1.一种基于双目视觉的目标姿态解算方法，其特征在于，包括如下步骤：

步骤1：通过左右两个相机采集多组图像，并通过所述多组图像完成左右两个相机的标定；

步骤2：对左右两个相机采集到的图像进行校正、增强和滤波处理；

步骤3：从步骤2中的图像中提取目标，处理后得到目标轮廓以及目标在图像中的位置信息；

步骤4：从步骤2的图像中分割出包含目标的图片，从分割出的图片中提取特征点，通过步骤3中得到的目标位置信息对特征点进行修正，获得匹配成功的特征点在图像像素坐标系下的坐标；

步骤5：获得下一时刻匹配成功的特征点；匹配相邻时刻左图的特征点，根据特征点的分散程度选择最优特征点；

步骤6：利用最优特征点解算目标姿态；

所述步骤5包括：

步骤5.1：通过执行步骤2至4获得下一相邻时刻的能够成功匹配的特征点，将上一时刻T₁的特征点坐标记为：(x_1i,y_1i,z_1i)，其中i＝1,2,...n1，n1为T₁时刻得到的特征点数；将下一时刻T₂的特征点坐标记为：(x_2i,y_2i,z_2i)，i＝1,2,...n2，n2为T₂时刻匹配成功的特征点数；

步骤5.2：将T₁时刻和T₂时刻获得的左图特征点重新匹配，删除误匹配特征点，得到匹配成功的特征点坐标，分别记为(x_3i,y_3i,z_3i)，(x′_3i,y′_3i,z′_3i)，i＝1,2,...n3，n3为T₁和T₂时刻左图中目标所在区域匹配成功的特征点个数；

步骤5.3：在步骤5.2已获得的相邻间隔两帧图像中，提取相同特征点在左相机坐标系下的坐标值，根据特征点的分散程度选择最优的三个特征点，分别记为P_i(X_i,Y_i,Z_i)，P′_i(Z′_i,Y′_i,Z′_i)，其中i＝1,2,3；

所述步骤6包括：

步骤6.1：根据T₁时刻图像获得的三个特征点建立新的坐标系，定义P_i(X_i,Y_i,Z_i)和P′_i(Z′_i,Y′_i,Z′_i)在新坐标系下的坐标分别为p_i(x_i,y_i,z_i),p′_i(x′_i,y′_i,z′_i)，且分别定义P₁(X₁,Y₁,Z₁)和P′₁(Z′₁,Y′₁,Z′₁)为坐标原点，在新坐标系下坐标原点的坐标值为p₁(0，0，0)p′₁(0，0，0)；所述新坐标系是指：目标坐标系；

步骤6.2：确定最优特征点在新坐标系下的坐标；定义向量指向的方向为新坐标下X轴的方向，与X轴垂直的方向为Y轴方向，Z轴的方向由右手定则确定，则p₂的坐标为根据确定p₃的坐标，即p₃(x₃，y₃，0)；

确定p′₁，p′₂和p′₃的坐标值，即p′₁(0，0，0)，p′₂(0，y′₂，0)，p′₃(x′₃，y′₃，0)；

步骤6.3：计算旋转矩阵R₁，

定义得到r₁，r₂的值，且_r3＝r₁×r₂；

同理计算R₂，

通过上式可以求出r′₁和r′₂，且r′₃＝r′₁×r′₂；

步骤6.4：计算旋转矩阵C，

式中：为R₁的逆矩阵；

步骤6.5：根据步骤6.4求出的C矩阵确定四元数向量q＝[a b c d]；具体如下：