CN104596486A - 基于目标旋转对称特征的位姿测量方法 - Google Patents

Abstract

基于目标旋转对称特征的位姿测量方法，属于计算机视觉测量技术领域。其特征是本发明所要测量的目标上的图案具有旋转对称性，拍摄一幅包含实际图案投影的目标图片，根据实际图案的旋转对称性通过径向展开变换和低秩特征恢复的方法由目标图片恢复出对正图片，根据实际图案上的点在目标图片和对正图片上投影点之间的关系解出目标的位姿参数。本发明的效果和益处是，利用一幅具有旋转对称特征的图案即可完成目标位姿的测量，将视觉测量问题与图像本身的对称性建立了联系，不需要提取图像的底层特征，减少了人工干预，易于实现自动化，能够保证测量精度。

Description

基于目标旋转对称特征的位姿测量方法

技术领域

本发明属于计算机视觉测量技术领域，涉及一种适用于旋转对称图案的空间位置和姿态测量方法。

背景技术

视觉测量是一门以图像运算为基础，综合运用电子学、光电探测、图像处理和计算机视觉等技术实现对目标的尺寸或位姿进行非接触测量的新兴测量技术。在视觉测量技术中，采用视觉传感器采集测量目标的图像，通过图像处理系统对采集的图像进行分析处理来完成对目标的几何尺寸或者目标在空间的位置、姿态等信息的测量。

根据采用的摄像机数量将视觉测量系统分为单目、双目和多目视觉测量系统。其中，单目视觉测量具有结构简单、标定步骤少、成本低等优点，同时还避免了双目或多目视觉中的视场小和匹配难等不足，所以近年来这方面的研究比较活跃。单个摄像机无法直接测出目标的深度信息，因此单目视觉测量需要提前知道目标上的特征信息。目前已有的单目视觉测量方法通常需要提取图像中的点、线或者几何图形等图像底层特征，从而建立图像与目标之间的特征点的对应关系，通过图像求解目标的位姿信息。由于这类方法的测量精度依赖于特征检测提取的精度，当图像中存在噪声干扰时，图像特征检测的精度往往达不到实际应用的要求，而且为了减少特征检测精度不高带来的影响，传统方法通常需要通过拍摄多幅图像进行测量或者通过多次迭代改善测量结果，有时还需要一定程度的人工干预，这使得此类单目视觉测量方法难于实现自动化。

发明内容

本发明提供了一种基于目标旋转对称特征的位姿测量方法，该方法将视觉测量问题直接与图像整体的旋转对称性建立了联系，不需要对特征点进行提取，避免了传统方法对特征检测与提取的依赖，解决了传统单目视觉测量方法测量精度严重依赖于特征检测精度以及难于实现自动化的不足。

本发明的技术方案是：

一、技术方案的原理

(1)技术方案的基本概念

实际图案：位于目标上的平面图案，具有旋转对称性，这样的图案在本发明中称为实际图案，由于实际图案在目标上，因此实际图案的位姿等同于目标的位姿。

场景图像：相机拍摄包含实际图案的场景得到的图像，也称为场景图片，在本发明中，图片和图像的概念是等价的。

投影：实际场景中物体在相机拍摄的图片中所成的像。

目标图像：场景图像中既包含实际图案的投影，也包括其它背景物体的投影，将场景图像中实际图案的投影用矩形框选出来就得到目标图像。

对正拍摄：相机CCD平面平行于实际图案拍摄。实际上，让相机CCD平面完全平行于实际图案拍摄几乎是不可能实现的，在本发明中，对正拍摄是为了描述方便引入的假设。

对正图片：对正拍摄得到的图片。在本发明中，对正拍摄是为了描述方便引入的假设，而对正图片是利用实际图案的旋转对称性，通过低秩特征恢复的方法由非对正图片恢复出来的。

倾斜拍摄：拍摄时相机CCD平面与实际图案不平行，实际上，让相机CCD平面完全平行于实际图案拍摄几乎是不可能实现的，因此在本发明中，为了测量目标位姿进行的任意一次拍摄均视为倾斜拍摄。

非对正图片：倾斜拍摄的到的图片。

(2)技术方案的具体原理：

目标的位姿不同，则目标图像也不同，即目标图像中包含了目标的位姿信息，本发明根据目标图像来确定目标的空间位姿参数θ_x，θ_y，θ_z，t_x，t_y，t_z。本发明的基本原理为：目标上的图案具有旋转对称性，拍摄一幅包含实际图案投影的目标图片，根据实际图案的旋转对称性通过径向展开变换和低秩特征恢复的方法由目标图片恢复出对正图片，根据实际图案上的点在目标图片和对正图片上投影点之间的关系解出目标的位姿参数。下面将对技术方案原理进行详细的介绍，首先介绍非对正图片和对正图片之间的关系，然后介绍径向展开变换，最后介绍低秩约束及目标位姿求解。

1)非对正图片和对正图片之间的关系

首先介绍本发明所用的坐标系，如图1所示。在针孔模型下，空间坐标点与像平面上对应点之间的关系描述为： $s\·\left[\begin{array}{c}u\\ v\\ l\end{array}\right]=N\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ l\end{array}\right].$

其中，s为一个任意非零的比例因子。[R t]是相机外部参数，由世界坐标系相对光心坐标系的旋转矩阵R和平移向量t组成。R是一个3×3的旋转矩阵，由3个欧拉角θ_x、θ_y和θ_z来描述。N为相机内部参数矩阵，一般表示为： $N=\left[\begin{array}{ccc}f/\mathrm{dx}& 0& u_0\\ 0& f/\mathrm{dy}& v_0\\ 0& 0& 1\end{array}\right].$ 其中f为摄像机的焦距；dx、dy分别表示X、Y方向相邻像素之间的距离，即像元尺寸，在本发明中认为像素是正方形的，因此有：dx＝dy，f/dx＝f/dy。记作f/dx＝f/dy＝f_d。主点，即摄像机光轴与成像平面的交点，在本发明中认为是图像的中心点，为了之后的推导方便，将图像坐标系的原点建立在图像中心，因此内参矩阵写为： $N=\left[\begin{array}{ccc}{f}_d& 0& 0\\ 0& f_d& 0\\ 0& 0& 1\end{array}\right].$

对于所拍摄的平面场景，不失一般性，在建立世界坐标系时，将平面场景作为世界坐标系的X_w-O_w-Y_w平面。则图像上一点(u,v)与其对应的空间平面(z＝0)上一点(x_w,y_w,0)的非齐次坐标的关系表达式如下：

$\begin{array}{c}u=\frac{f_d\·(R_{11}{x}_w+R_{12}{y}_w+t_x)}{R_{31}{x}_w+R_{32}{y}_w+t_z}\\ v=\frac{f_d\·(R_{21}{x}_w+R_{22}{y}_w+t_y)}{R_{31}{x}_w+R_{32}{y}_w+t_z}\end{array}---\left(1\right)$

如图2所示，假设第一次是对正拍摄，得到图片I₁，第二次是倾斜拍摄，得到图片I₂。则对正拍摄时，世界坐标系与光心坐标系的旋转矩阵为单位阵，平移矢量t只有z轴方向上的分量。分别记两次的旋转矩阵和平移矢量为：

$R_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],t_1={\left[\begin{array}{ccc}0& 0& d\end{array}\right]}^T---\left(2\right)$

$R_{22}=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right],t_2={\left[\begin{array}{ccc}{t}_x& t_y& t_z\end{array}\right]}^T---\left(3\right)$

假设空间平面(z＝0)上一点(x_w,y_w,0)，对应图片I₁上的投影点坐标为(u₁,v₁)，对应图片I₂上的投影点坐标为(u₂,v₂)。将式(2)、(3)分别代入式(3)，分别列出该点与两幅图片上投影点间的关系为：

$\begin{array}{c}{u}_1=\frac{f_d\·x_w}{d}\\ v_1=\frac{f_d\·y_w}{d}\end{array}---\left(4\right)$

$\begin{array}{c}{u}_2=\frac{f_d\·(R_{11}{x}_w+R_{12}{y}_w+t_x)}{R_{31}{x}_w+R_{32}{y}_w+t_z}\\ v_2=\frac{f_d\·(R_{21}{x}_w+R_{22}{y}_w+t_y)}{R_{31}{x}_w+R_{32}{y}_w+t_z}\end{array}---\left(5\right)$

结合式(4)、(5)，列出两幅图片上的投影点之间的关系：

$\begin{array}{c}{u}_2=\frac{f_d\·(R_{11}{u}_1+R_{12}{v}_1+\frac{f_d}{d}\·t_x)}{R_{31}{u}_1+R_{32}{v}_1+\frac{f_d}{d}\·t_z}=\frac{f_d\·(R_{11}{u}_1+R_{12}{v}_1+f_d\·{\tilde{t}}_1)}{R_{31}{u}_1+R_{32}{v}_1+f_d{\tilde{t}}_3}\\ v_2=\frac{f_d\·(R_{21}{u}_1+R_{22}{v}_1+\frac{f_d}{d}\·t_y)}{R_{31}{u}_1+R_{32}{v}_1+\frac{f_d}{d}\·t_z}=\frac{f_d\·(R_{21}{u}_1+R_{22}{v}_1+f_d\·{\tilde{t}}_2)}{R_{31}{u}_1+R_{32}{v}_1+f_d\·{\tilde{t}}_3}\end{array}---\left(6\right)$

其中， ${\tilde{t}}_1=t_x/d,{\tilde{t}}_2=t_y/d,{\tilde{t}}_3=t_z/d.$ 将 $\tilde{t}={\left[\begin{array}{ccc}{\tilde{t}}_1& {\tilde{t}}_2& {\tilde{t}}_3\end{array}\right]}^T$ 称为由距离d归一化的位移矢量。

式(6)给出了相机倾斜拍摄与对正拍摄一个平面场景的两幅图片的投影点之间的关系。反之，如果得到了这样两幅图片之间投影点的关系，就能够进一步解析出f_d，R，等参数。

2)径向展开变换(Frieze-Expansion Transform)

式(6)表示了非对正图片与对正图片之间的关系。对正图片保留了目标模板的旋转对称性，对正图片经过径向展开变换(FE变换)后，原有的旋转对称性转换成平移对称性，具有平移对称性的纹理是低秩的。

拍摄的实际图案是旋转对称的。而且，这种旋转对称性可通过FE变换转换成平移对称性。如图3所示，旋转对称纹理(a)，(b)，(c)，(d)分别经过FE变换转换为平移对称纹理(e)，(f)，(g)，(h)，这些平移对称纹理通常是低秩纹理。在进行FE变换时，并不是对整幅图片进行变换，而是根据需要选取一对同心圆，对内圆和外圆之间的环面做FE变换。写出环面的FE变换的关系如下：

$I_p(i,j)=I\left(\right[r_1+\frac{r_2-r_1}{m-1}(i-1)]\·\cos \frac{2\mathrm{\πj}}{n},[r_1+\frac{r_2-r_1}{m-1}(i-1)]\·\sin \frac{2\mathrm{\πj}}{n})---\left(7\right)$

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

$I_p^0(i,j)=I^0(R\left(i\right)\·\cos \frac{2\mathrm{\πj}}{n},R\left(i\right)\·\sin \frac{2\mathrm{\πj}}{n})---\left(8\right)$

3)低秩约束及目标位姿求解

如图4所示,相机在没有正对该平面场景的情况下，经过透视变换，拍摄平面场景所得到的像不再保留旋转对称的特性，而且经过FE变换之后得到的纹理不是低秩的。

当通过某种方式恢复出图5所示的对正拍摄的图片时，其视觉效果才具有旋转对称性，而且经过FE变换之后得到的纹理是低秩的。相对来讲，倾斜拍摄的图片FE变换之后得到的纹理的秩要高。因此，一个纹理的旋转对称性，能够通过将纹理做FE变换之后的秩来衡量。下面介绍利用低秩约束并结合FE变换，对正一幅具有旋转对称结构的图片，同时根据实际图案上的点在拍摄的图片与对正图片的投影点之间的关系，求出目标的位置和姿态参数。

式(6)描述了两幅图片之间对应点之间的关系，这种关系是非线性的，原图片I与对正后的图片I⁰存在这样的关系：

Iоτ＝I⁰    (9)

其中为图像单应性变换参数，包含目标的位置和姿态信息，о为图像单应性变换运算符。

对式(11)两边做FE变换，则有：

其中操作算子F{·}表示FE变换。对于应该是低秩的。式(10)描述了原图像I与对正图像的FE变换后的纹理之间的关系。引入误差矩阵E表示可能的光照、噪声等误差，而且认为是稀疏的。列出以下关系式：

E为表示图像中噪声的稀疏矩阵，λ为一个常量，rank()为求矩阵的秩，|| ||₀为求矩阵的零阶范数

求解式(11)，得到Ι⁰，τ和E的解，其中目标的姿态参数θ_x，θ_y，θ_z，能够直接从求解的τ中得到，为归一化的位置参数，如果实际图案的半径已知，那么就能够根据实际图案与对正图片的比例关系求出对正图片与实际图案的距离d，从而计算出位置参数t_x，t_y，t_z。

二、技术方案的步骤

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

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

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

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

$\begin{array}{c}{u}_2=\frac{f_d\·(R_{11}{u}_1+R_{12}{v}_1+\frac{f_d}{d}\·t_x)}{R_{31}{u}_1+R_{32}{v}_1+\frac{f_d}{d}\·t_z}=\frac{f_d\·(R_{11}{u}_1+R_{12}{v}_1+f_d\·{\tilde{t}}_1)}{R_{31}{u}_1+R_{32}{v}_1+f_d{\tilde{t}}_3}\\ v_2=\frac{f_d\·(R_{21}{u}_1+R_{22}{v}_1+\frac{f_d}{d}\·t_y)}{R_{31}{u}_1+R_{32}{v}_1+\frac{f_d}{d}\·t_z}=\frac{f_d\·(R_{21}{u}_1+R_{22}{v}_1+f_d\·{\tilde{t}}_2)}{R_{31}{u}_1+R_{32}{v}_1+f_d\·{\tilde{t}}_3}\end{array}---\left(6\right)$

在式(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，2，…，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的雅克比矩阵，即Δτ表示τ附近的增量， $\mathrm{\Δ\τ}=[\mathrm{\Δ}{\mathrm{\θ}}_x,{\mathrm{\Δ\θ}}_y,{\mathrm{\Δ\θ}}_z,{\mathrm{\Δ}\tilde{t}}_x,{\mathrm{\Δ}\tilde{t}}_y,{\mathrm{\Δ}\tilde{t}}_z];$

S3.3利用增广拉格朗日乘子法来求解式(13)的优化问题其对应的拉格朗日函数是： $L(I_p^0,E,\mathrm{\&Delta;\&tau;},Y,\mathrm{\&mu;})=f(I_p^0,E)+<Y,R(I_p^0,E,\mathrm{\&Delta;\&tau;})>+\frac{\mathrm{\&mu;}}{2}{\left|\right|R(I_p^0,E,\mathrm{\&Delta;\&tau;})\left|\right|}_F^2,$ 其中， $f(I_p^0,E)={\left|\right|I_p^0\left|\right|}_{*}+\mathrm{\&lambda;}{\left|\right|E\left|\right|}_1,$ Y是一个拉格朗日乘子阵，μ＞0，用来折中约束部分与目标函数部分，<·，·>表示矩阵内积算子，||·||_F表示矩阵的F范数；则式(13)中的问题转化为同解的无约束的凸优化问题：

$(I_p^0,E,\mathrm{\&Delta;\&tau;})=\underset{I_p^0,E,\mathrm{\&Delta;\&tau;}}{\arg \min }L(I_p^0,E,\mathrm{\&Delta;\&tau;},Y,\mathrm{\&mu;})---\left(14\right)$

利用交替方向迭代法求解式(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 ${\left(I_p^0\right)}_{k+1}=U_k{D}_{{{\mathrm{\&mu;}}_k}^{-1}}\left[{\mathrm{\&Sigma;}}_k\right]V_k^{*};$

其中表示对Σ_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_o是否为真，若为假，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得到： $t_x={\tilde{t}}_x\&CenterDot;d,t_y={\tilde{t}}_y\&CenterDot;d,t_z={\tilde{t}}_z\&CenterDot;d.$

本发明的效果和益处是，利用一幅具有旋转对称特征的图案即可完成目标位姿的测量，将视觉测量问题与图像本身的对称性建立了联系，不需要提取图像的底层特征，减少了人工干预，易于实现自动化，能够保证测量精度。

附图说明

图1是三个坐标系图。图中，1光心坐标系，2图像坐标系，3世界坐标系。图中，O_w-X_w-Y_w-Z_w为世界坐标系，用于描述现实世界中点的位置。O-U-V是以像素为单位的图像坐标系，以CCD成像平面的中心，即摄像机光轴与成像平面的交点为原点，U轴平行于CCD平面的水平方向、V轴平行于CCD平面的垂直方向。O_c-X_c-Y_c-Z_c为摄像机坐标系，其中O_c为摄像机的光心，X_c轴和Y_c轴与图像坐标系中的U轴和V轴分别平行，Z_c轴为摄像机的光轴，它与图像平面垂直，光轴与图像平面的交点即为图像坐标系的原点O，且O_cO为摄像机焦距。3D场景中一点M，其世界坐标系下的齐次坐标用(x_w,y_w,z_w,1)^T表示，经过相机拍摄，投影到像平面上一点m，其图像坐标系下的齐次坐标表示为(u,v,1)^T。

图2是对正拍摄与倾斜拍摄示意图。图中，4对正拍摄，即相机的CCD平面平行于实际图案拍摄一幅图像；5倾斜拍摄，即相机以任意位姿拍摄一幅实际图案的图像。

图3是FE变换效果图。图中(a)、(b)、(c)、(d)是不同类型的旋转对称纹理图；图中(e)、(f)、(g)、(h)分别是(a)、(b)、(c)、(d)经过FE变换之后的低秩纹理图。

图4是非对正图片的FE变换图。图中左侧是非对正图片，右侧是非对正图片的FE变换图，白色箭头指向的两点是FE变换前后的对应点。

图5是对正图片的FE变换图。图中左侧是对正图片，右侧是对正图片的FE变换图，白色箭头指向的两点是FE变换前后的对应点。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

实施例：

使用Basler pilot的高速面阵摄像机采集图像，相机型号为piA1600-35gc，分辨率为1606×1206，像元尺寸为7.4×7.4μm²即dx＝0.0074mm，接口为千兆网口。所采用相机镜头的有效焦距是25mm，因此f_d＝25/0.0074＝3378.38。所用模板半径为375mm即d_real＝375mm。模板到相机的距离在3.5m到4m之间。

步骤1、获取模板的灰度图片，选择图片中具有旋转对称图案投影的矩形区域I，像素灰度级为0到255。

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

步骤3、对式(11)约束方程中的F{Iоτ}做归一化处理： 表示求F{Iоτ}的F范数，即求出||F{Iоτ}||_norm的雅克比矩阵，即

步骤4、利用交替方向迭代法求解式(14)：

$(I_p^0,E,\mathrm{\&Delta;\&tau;})=\underset{I_p^0,E,\mathrm{\&Delta;\&tau;}}{\arg \min }L(I_p^0,E,\mathrm{\&Delta;\&tau;},Y,\mathrm{\&mu;})---\left(14\right)$

其中 $L(I_p^0,E,\mathrm{\&Delta;\&tau;},Y,\mathrm{\&mu;})=f(I_p^0,E)+<Y,R(I_p^0,E,\mathrm{\&Delta;\&tau;})>+\frac{\mathrm{\&mu;}}{2}{\left|\right|R(I_p^0,E,\mathrm{\&Delta;\&tau;})\left|\right|}_F^2,$ $f(I_p^0,E)={\left|\right|I_p^0\left|\right|}_{*}+\mathrm{\&lambda;}{\left|\right|E\left|\right|}_1,$ 分别迭代求解各个变量的最优解，步骤如下：

步骤4.1、初始化参数设置为：

步骤4.2、

步骤4.3、 ${\left(I_p^0\right)}_{k+1}=U_k{D}_{{{\mathrm{\&mu;}}_k}^{-1}}\left[{\mathrm{\&Sigma;}}_k\right]V_k^{*};$

步骤4.4、

步骤4.5、

步骤4.6、

步骤4.7、μ_k+1＝ρμ_k；

步骤4.8、判断s＜t_inner是否为真，若为假，返回步骤4.2继续执行，若为真，继续执行下一步。

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

步骤4.10、输出E＝E_k+1作为最终解；

步骤5、根据步骤S3的计算结果求解目标的位置和姿态参数。其中姿态参数θ_x,θ_y,θ_z直接从τ中得到。位置参数是经过归一化的， 用霍夫变换获取对正图案Iоτ中旋转对称图形的直径d_image，则d＝f·d_real/(d_image·d_x)，其中f为焦距，d_x为像元尺寸。根据求出的d得到： $t_x={\tilde{t}}_x\&CenterDot;d,t_y={\tilde{t}}_y\&CenterDot;d,t_z={\tilde{t}}_z\&CenterDot;d.$

Claims (1)

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得到：