[发明专利]一种基于点特征的单目视觉位姿测量方法

权利要求书

1.一种基于点特征的单目视觉位姿测量方法，其特征在于包括以下步骤：

步骤一：获取摄像机拍摄的图像，用张正友标定法对摄像机进行标定，获取摄像机内参数矩阵；

步骤二：提取图像中任意4个不共面的特征点P_i(i＝1,2,3,4)，计算特征点在成像平面坐标系中的坐标{q_i(x_i,y_i,f),i＝1,2,3,4}，根据两点间距离公式计算出任意两个特征点之间的距离：d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

步骤三：设P₁为世界坐标系的原点，O_c为光心点，分析给定的任意不共线的4个特征点之间的相对位置关系：当4个特征点所在的两条直线平行时，定义特征点相关距离因子{k_i,i＝1,2,3,4}；当4个特征点所在的两条直线相交时，分别计算两条直线交点在世界坐标系中的坐标及成像坐标系中的坐标；

步骤四：分别求解出世界坐标系中X_w、Y_w、Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)；

步骤五：根据世界坐标系的原点在摄像机中的坐标求解出物体相对于摄像机的初始位置，并通过LM算法对初始位姿进行优化，得到物体相对于摄像机的初始姿态。

2.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，分析给定的4个共面不共线特征点P_i(i＝1,2,3,4)的位置关系：P₁、P2两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄之间平行和相交。

3.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述步骤四中当P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄平行时：

世界坐标系中的X_w轴上的向量变换到摄像机坐标系中的单位向量为

世界坐标系中的Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₃,r₂₃,r₃₃)为

世界坐标系中的Y_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₂,r₂₂,r₃₂)为(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。