[发明专利]一种基于共面目标的单目视觉位姿估计方法

权利要求书

1.一种基于共面目标的单目视觉位姿估计方法，其特征在于，包括如下步骤：

S1：设计单目视觉成像模型对三维空间共面目标的特征点进行成像，得到特征点的像平面像素点，然后以单目视觉原点为圆心确定单位虚拟球面，将像平面像素点投影到单位虚拟球面上，将像平面像素点转换到单位虚拟球面上特征点；

S2：采用步骤S1中得到的单位虚拟球面上特征点，基于每两个单位虚拟球面上特征点之间距离平方并构建目标函数用于共面目标位姿估计，利用列文伯格-马夸尔特非线性优化方法得到目标函数中位姿耦合项第一个局部极小值；

S3：基于步骤S2中得到的位姿耦合项第一个局部极小值，将之转换至两个角度参数和一个长度参数，固定其中一个角度参数和一个长度参数，根据共面目标的共面特性确定位姿耦合项第二个局部极小值估计值，以第二个局部极小值估计值为初始值，利用列文伯格-马夸尔特非线性优化方法得到位姿耦合项第二个局部极小值；

S4：基于步骤S2、S3中得到的两个位姿耦合项局部极小值，比较两个位姿耦合项局部极小值对应目标函数大小，选择误差更小的局部极小值作为位姿耦合项全局最优值，利用奇异值分解对全局最优值进行解耦，分别得到共面目标姿态和位置。

2.根据权利要求1所述的方法，其特征在于：所述步骤1中，将像平面像素点转换到单位虚拟球面上特征点为：

其中u_i为像平面坐标点，R为目标姿态，t为目标位置，p_i为共面目标点在目标坐标系的坐标。

3.根据权利要求2所述的方法，其特征在于：所述步骤S2中，得到目标函数中位姿耦合项第一个局部极小值的具体过程为：

每两个单位虚拟球面上特征点之间距离平方为：

其中位姿耦合项t'＝R^Tt；

目标函数为：

其中然后利用列文伯格-马夸尔特非线性优化方法迭代求解位姿耦合项第一个局部极小值t′_1-local。

4.根据权利要求3所述的方法，其特征在于：所述步骤S3，得到位姿耦合项第二个局部极小值具体过程如下：

通过位姿耦合项第一个局部极小值t′_1-local确定对应两个角度参数γ₁,β₁和一个长度参数l₁为：

t′_1-local＝R_z(γ₁)R_y(β₁)[0 0 l₁]^T (4)

其中t_l1＝[0 0 l₁]^T，f_ij(t')的表达形式转换为：

特征点p_i经过角度γ₁旋转后的一组特征点并且第i个特征点求解第二个角度为：

位姿耦合项第二个局部极小值估计值为：

其中为n个β_i-2的平均值，并以t'₂为初始值，进行列文伯格-马夸尔特非线性优化方法，得到位姿耦合项第二个局部极小值t′_2-local。

5.根据权利要求4所述的方法，其特征在于：所述步骤S4中，求解共面目标姿态和位置的具体过程如下：

比较目标函数值E(t′_1-local)和E(t′_2-local)大小，选择E(t′_1-local)和E(t′_2-local)中较小值对应t'作为位姿耦合项全局最优值t′_global，并定义：

t”_global＝[t'_global(1),t'_global(2),-t'_global(3)]^T (8)

t′_global和t″_global都是目标函数的收敛值，两组单位虚拟球面上特征点：

结合s_i和奇异值分解方法得到共面目标两组姿态和位置为：

选择t'(3),t”(3)＞0的一组姿态和位置作为最终的共面目标姿态和位置。