[发明专利]基于目标旋转对称特征的位姿测量方法

权利要求书

1.基于目标旋转对称特征的位姿测量方法，采用视觉传感器采集测量目标的图像，通过图像处理系统对采集的图像进行分析处理来完成目标在空间的位置、姿态信息的测量，其特征在于，步骤如下：

S1获取目标上的旋转对称图案的灰度图片，选择图片中具有旋转对称图案投影的矩形区域，该矩形区域构成一个矩阵，记作I；获取相机的焦距f和像元尺寸d_x，则f_d＝f/d_x；获取目标上的旋转对称图案的直径d_real；

S2将参数始初化，角度参数初始值θ_x＝0,θ_y＝0,θ_z＝0，归一化位移参数的初始值参数用向量的形式表示为初始化的参数用向量的形式表示为τ_init＝[0,0,0,0,0,1]；

S3将步骤S1中选择的矩形区域I和初化的τ_init作为条件，求解式(11)的优化问题：

其中E是约束方程，Iοτ表示根据向量τ中的参数对矩阵I进行变换，设矩阵I上的一点为I(u₂,v₂)，经过变换后该点的坐标为(u₁,v₁)，则(u₁,v₁)与(u₂,v₂)的关系用式(6)来表示：

在式(6)中R₁₁＝C_yC_z，R₂₁＝S_xS_yC_z-C_xS_z，R₃₁＝C_xS_yC_z+S_xS_z，R₁₂＝C_yS_z，R₂₂＝S_xS_yS_z+C_xC_z，R₃₂＝C_xS_yS_z-S_xC_z，其中，C_x＝cosθ_x，S_x＝sinθ_x，C_y＝cosθ_y，S_y＝sinθ_y，C_z＝cosθ_z，S_z＝sinθ_z；F{Iοτ}表示 对Iοτ进行径向展开变换，设变换后得到图像I_p，I_p上任意一点为I_p(i,j)，在进行FE变换时，并不是对整幅图片进行变换，而是根据需要选取一对同心圆，对内圆和外圆之间的环面做FE变换；根据式(7)写出环面的FE变换的关系如下：

其中，m,n分别表示图像I_p的行数和列数，(i,j)表示图像I_p中任意一点，i＝1,2,3,…,m，j＝1,2,3,…,n，r₁、r₂分别是内外环的半径；对Iοτ而言，m表示径向采样点的个数，则径向采样间隔为n为旋转方向的采样个数，则角度采样间隔为

式(11)中的是目标函数,表示求矩阵的秩，λ是一个常数，E为误差矩阵，||E||₀表示求误差矩阵E的零范数；为I_p减去误差矩阵E之后得到的矩阵；

式(11)求解的具体步骤如下：

S3.1对式(11)约束方程中的F{Iοτ}做归一化处理：

表示求F{Iоτ}的F范数，即

S3.2对式(11)的目标函数进行凸松弛并对其约束方程进行线性化，得到式(13)如下：

其中表示求矩阵的核范数，||E||₁表示求矩阵E的1范数，J表示||F{Iοτ}||_norm的雅克比矩阵，即Δτ表示τ附近的增量， 

S3.3利用增广拉格朗日乘子法来求解式(13)的优化问题 其对应的拉格朗日函数是：其中， Y是一个拉格朗日乘子阵，μ＞0，用来折中约束部分与目标函数部分，<·,·>表示矩阵内积算子，||·||_F表示矩阵的F范数；则式(13)中的问题转化为同解的无约束的凸优化问题：

利用交替方向迭代法求解式(14)，分别迭代求解各个变量的最优解，步骤如下：

S3.3.1参数的初始值设置为：

Y₀＝0,E₀＝0,Δτ₀＝0,μ₀＞0,ρ＞1,k＝0,t_inner＞0,t_outer＞0,f_pre＝0；

S3.3.2

其中表示对做SVD分解；

S3.3.3

其中表示对Σ_k进行奇异值收缩，收缩阈值为收缩算子D_ξ(·)也称作软阈值操作算子，若定义在标量上，其表示形式写作 D_ξ(x)＝max(sign(x)·(|x|-ξ),0)；

S3.3.4

其中表示对矩阵的每个元素做收缩，收缩阈值为

S3.3.5

S3.3.6

S3.3.7μ_k+1＝ρμ_k；

S3.3.8判断s＜t_inner是否为真，若为假，返回步骤S3.3.2继续执行，若为真，继续执行下一步；

S3.3.9更新变换参数τ＝τ+Δτ_k+1，判断 |f_p-f_pre|＜t_outer是否为真，若为假，f_pre＝f_p并返回步骤S3.1继续执行，若为真，继续执行下一步；

S3.3.10输出τ，E＝E_k+1作为最终解；

S4根据步骤S3的计算结果求解目标的位置和姿态参数；其中姿态参数θ_x,θ_y,θ_z直接从τ中得到；位置参数是经过归一化的，用霍夫变换获取对正图片Iοτ中旋转对称图形的直径d_image，则d＝f·d_real/(d_image·d_x)，其中f为焦距，d_x为像元尺寸；根据求出的d得到：