[发明专利]基于单目视频的实时三维刚体目标姿态估计与测距方法

权利要求书

1.基于单目视频的实时三维刚体目标姿态估计与测距方法，其特征在于：包括以下步骤：

步骤1，通过光学观测设备采集目标的观测视频，光学观测设备为单目观测镜头；

步骤2，将采集得到的图像序列送入目标分割模块以获得目标的二值分割图像和轮廓图像，采用的方法是基于水平集的改进型区域竞争方法，分割模型方程如下所示：

E(C)=α·Length(C)+c·Area(inside(C))]]>

+λ1∫inside(C)|I-c1|dxdy]]>

+λ2∫outside(C)|I-c2|dxdy---(1)]]>

其中，I表示待分割图像，C表示零水平集轮廓，c₁表示目标区域的平均灰度，c₂表示背景区域的平均灰度；α，c，λ₁，λ₂均为加权系数；

步骤3，根据目标的二值图像和轮廓图像，提取目标轮廓点上的特征向量，由轮廓特征提取模块对目标轮廓上的点进行特征矢量提取，轮廓特征提取模块对目标轮廓上的点，提取如下的三维特征矢量：

fV=(x,y,curv)T---(2)]]>

x、y表示轮廓点在像平面的横纵坐标，curv表示目标轮廓在该点的曲率，采用如下步骤来计算目标轮廓在给定点的曲率：令Mask表示一个半径为r的圆形掩模，对于给定点p，记其灰度值为I(p)；以该p点为掩模中心，计算掩模Mask范围内的图像像素点与掩模中心点的灰度差异，则p点对应的曲率计算公式为：

curv=Σq∈MaskΦ(I(q)-I(p))---(3)]]>

其中，Φ(·)为隶属度模糊函数，方程如下：

Φ(x)=1if|x|≤t111-t1(t2-|x|t2)ift1<|x|≤t20if|x|>t2---(4)]]>

其中，x为自变量，t₁、t₂为阈值，t₁＜t₂；

步骤4，根据目标的轮廓图像和目标轮廓点上的特征向量，生成多特征驱动距离图，多特征驱动距离图是指使用加权多特征距离作为距离度量范数的距离图，其数学描述如下：

令G表示N维特征空间中的均匀采样网格，表示目标采样点集合，多特征驱动距离图描述的是网格G中的每一点到目标点集合C的最短距离，对采样点x∈G，其距离值定义为：

MFDM(x)=miny||x-y||,∀y∈C---(5)]]>

其中，距离度量||·||采用的是加权多特征距离范数，

“加权多特征距离范数”是指，设N维特征空间中的两点则该两点之间的加权多特征驱动距离定义为

MFD(pV,qV)=w1(x1-x1′)2+w2(x2-x2′)2+LwN(xN-xN′)2---(6)]]>

其中w₁L w_N为各特征维的加权系数，公式(6)所定义的加权多特征驱动距离提供了一种可以容纳任意个数的特征及其组合方式的算法框架。在加权多特征驱动距离框架下，可以添加任何有效的特征，并通过调整各特征维的加权系数来提高同名点的匹配精度，这在特征异质的情况下是非常必要的，

多特征驱动距离图是采用如下的加权快速距离变换来生成多特征驱动距离图：

1)一维情况下的加权快速距离变换

在一维情况下，加权距离变换定义式为：

Df(x)=minq∈C(w·||x-q||+f(q))---(7)]]>

其中f(·)为初始距离函数，C为其定义域，w为标量加权系数。当度量范数采用2范数时，上式可以重写为

Df(x)=minq∈C(w·(x-q)2+f(q))---(8)]]>

(8)式可以看成是以x为自变量的二次函数，其形状为抛物线。从(8)式可以得出如下结论：距离变换D_f(x)等于所有不同q值的抛物线y＝w·(x-q)²+f(q)的下包迹的值，

由于抛物线方程已知，要求取其下包迹只需要再知道各抛物线之间的交点坐标。对于q＝q₁和q＝q₂的两条抛物线，其交点坐标为

s=f(q1)-f(q2)2w(q1-q2)+q1+q2---(9)]]>

2)N维情况下的加权快速距离变换

在N维情况下，加权距离变换定义式为

Df(x1,x2LxN)=min(x1′,x2′LxN′)∈Cw1(x1-x1′)2+w2(x2-x2′)2+LwN(xN-xN′)2+f(x1′,x2′LxN′)---(10)]]>

其中w₁L w_N为各维的加权系数。上式可重写为

Df(x1,x2LxN)=minx1′w1(x1-x1′)2+minx2′w2(x2-x2′)2+LminxN′(wN(xN-xN′)2+f(x1′,x2′LxN′))---(11)]]>

(11)式的重要含义在于：对于N维加权距离变换，可以通过逐维进行一维加权距离变换求得。该加权快速距离变换算法的复杂度为O(L)，其中L为采样点的总数目；

步骤5，根据多特征驱动距离图以及从模型库中获得的目标的三维模型数据，建立输入的二维图像序列与目标的三维模型之间的试探性同名点特征对应关系，采用如下的两步方法建立二维图像与目标三维模型之间的试探性同名点特征：

1)2D-to-2D点对应关系的确定

首先将目标的三维模型进行投影得到相应的投影图像并从中提取目标的轮廓，然后利用多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，使用步骤4“多特征驱动距离图生成模块”所给出的多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，根据多特征驱动距离图的定义，给定采样点x∈G，其多特征驱动距离图值描述的是从该点到目标点集C的最小距离，因此具有相同距离图值的采样点形成一个个封闭和连续的曲面或称为等高面，为了寻找给定采样点x在目标点集C中的最邻近点，只需要从采样点x出发沿着距离图的梯度下降方向向下搜寻，在搜寻路径上遇到的第一个点x′∈C即为采样点x在目标点集C中的最邻近点，可以将寻找两轮廓上同名点的计算复杂度从O(MN)显著降低到O(1)，其中M、N分别为两轮廓上点的数目，

2)2D-to-3D点对应关系的确定

把建立的2D-to-2D点对应关系反向投影到目标的三维模型上去，以建立二维输入图像与目标三维模型间的2D-to-3D点对应关系，采用如下的面片染色方案，即在使用目标的三维模型进行投影时，赋予模型上每个三角面片不同的颜色，然后以此颜色为索引，可以从模型的几千个三角面片中找出对应于投影图像轮廓的三角面片，在找到对应于投影图像轮廓的三角面片后，即可求取投影图像轮廓同名点在模型表面上所对应点的准确三维坐标，设相机坐标系与目标自坐标系之间不存在旋转和平移，若一个三角面片的三个顶点坐标为则由该面片所定义的平面方程可表示为：

P=(xVv1-xVv3)×(xVv2-xVv3)-xVv3·(xVv1×xVv2)---(12)]]>

令表示投影轮廓同名点对应的模型上的点的三维坐标，则可按下式计算其值：

xVvg=(-P(4)/xVg·P(1:3))xVg---(13)]]>

步骤6，根据试探性同名特征对应关系对图像中目标的三维姿态和距离参数进行反演，令表示目标轮廓图像上某点在像平面坐标系的坐标，表示该点在相机坐标系中的对应坐标，有

vVi=(xVp,iT,f)T---(14)]]>

定义投影矩阵

Vi=vVi·vViTvViT·vVi---(15)]]>

目标姿态/距离参数反演模块的特征之一，是使用如下的目标代价函数来对目标的三维姿态和距离参数进行反演：

e=Σi=1N||(I-Vi)(RxVv,i+tV)||2---(16)]]>

步骤7，将步骤6反演得到的目标的三维姿态和距离参数反馈到步骤5，对二维图像序列与目标的三维模型之间的试探性同名特征对应关系进行修正更新；

步骤8，重复步骤5～步骤7的过程直至满足迭代停止条件。