一种复杂动态环境孔特征的高精视觉定位方法与流程

技术特征：

1.一种复杂动态环境孔特征的高精视觉定位方法，其特征在于该方法步骤如下：

第一步，通过摄像头采集带有孔特征的图像，将图像进行二值化处理及边缘检测，获得孔特征轮廓，利用椭圆对孔特征轮廓进行拟合，得到图像坐标系下孔特征轮廓对应的椭圆参数方程；

第二步，以摄像头光心为原点，以焦距方向为Z轴正方向，以图像水平和竖直方向分别为x和y轴正方向，建立直角坐标系xyz；

第三步，以xyz坐标系原点为圆锥面的顶点，以椭圆参数方程为圆锥面的准线，构建获得椭圆锥面方程；

第四步，将椭圆锥面方程用二次型矩阵表示，并计算椭圆锥面方程二次型矩阵的特征值，由特征值获得交平面方程；

第五步，通过因式分解将交平面方程的二次型矩阵形式X^T(C-λ₂I)X＝0分解为两个平面方程，获得两个平面方程各自对应的平面法向量，作为孔特征可能的位姿方向；

第六步，计算因式分解得到的平面法向量与坐标系Z轴正方向单位向量的夹角，并转换为旋转矩阵；

第七步，通过旋转矩阵将直角坐标系xyz进行旋转获得新坐标系x′y′z′，并计算获得新坐标系下的椭圆锥面方程，然后计算在新坐标系下x′y′z′孔特征的圆心位置，再通过旋转矩阵求得孔特征在直角坐标系xyz下的初步圆心位置；

第八步，将摄像头移动距离，重复上述第一步到第七步的步骤，得到第二次拍摄图像处理后获得的孔特征在直角坐标系xyz下的初步圆心位置，再确定正确的孔特征圆心及其位姿。

2.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述第一步中的椭圆参数方程表示为：

ax²+bxy+cy²+dx+ey+f＝0

其中，a,b,c,d,e,f分别为椭圆的第一、第二、第三、第四、第五、第六参数，满足4ac-b²>0，x，y分别表示孔特征轮廓在图像坐标系下的水平和竖直坐标，x，y为变量，单位为像素。

3.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述第三步中的椭圆锥面方程表示为：

${\mathrm{ax}}^2+{\mathrm{cy}}^2+\frac{f}{{f_c}^2}{z}^2+bxy+\frac{d}{f_c}xz+\frac{e}{f_c}yz=0$

其中，f_c为摄像头焦距，f为椭圆的第六参数。

4.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述第四步，椭圆锥面方程转化为以下公式的二次型矩阵形式，获得特征值λ₁、λ₂和λ₃：

X^TCX＝0

$C=\left[\begin{array}{ccc}a& b/2& d/2\\ b/2& c& e/2\\ d/2& e/2& f\end{array}\right],X={\[xyz\]}^T$

其中，C表示二次型矩阵，λ₁、λ₂和λ₃分别表示二次型矩阵C的第一、第二、第三特征值，三个特征值满足λ₁≥λ₂≥λ₃；

然后获得采用以下公式的交平面方程表示的椭圆锥面与球面之间的交线平面，并取λ＝λ₂代入交平面方程：

${\mathrm{ax}}^2+{\mathrm{cy}}^2+\frac{f}{{f_c}^2}{z}^2+bxy+\frac{d}{f_c}xz+\frac{e}{f_c}yz-\mathrm{\λ}(x^2+y^2+z^2)=0$

其中，λ表示交平面方程的特征值，λ＝λ₂。

5.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述的第五步中，由交平面方程的二次型矩阵形式表示为X^T(C-λ₂I)X＝0，X表示椭圆锥面方程变量，将X^T(C-λ₂I)X＝0通过因式分解得到的两个平面方程表示为：

$\left\{\begin{array}{c}{l}_{1}x+m_{1}y+n_{1}z=0\\ l_{2}x+m_{2}y+n_{2}z=0\end{array}\right.$

其中，l₁,m₁,n₁表示第一平面方程的三个系数，l₂,m₂,n₂表示第二平面方程的三个系数，从而获得第一平面法向量N₁(l₁,m₁,n₁)和第二平面法向量N₂(l₂,m₂,n₂)。

6.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述第六步具体步骤如下：

第一步，采用以下公式求出两个平面法向量的夹角：

其中，N表示平面法向量，平面法向量N为第一平面法向量N₁或第二平面法向量N₂，为沿z轴正方向的单位向量Z_r(0,0,1)与平面法向量N的夹角，Z_r表示沿z轴正方向的单位向量；

第二步，采用以下公式求出两个平面法向量旋转的转轴u(u_x,u_y,u_z)：

u＝N×Z_r

其中，u_x,u_y,u_z分别表示转轴u沿x、y、z轴的分量；

然后将夹角和转轴u代入以下公式获得旋转矩阵R_c：

7.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述第七步获得新坐标系x′y′z′后具体步骤如下：

先采用以下公式计算获得新坐标系x′y′z′下的椭圆锥面方程，并以二次型矩阵形式表示：

C′＝R_c^-TCR_c^-1

$C^{\′}=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{12}& c_{22}& c_{23}\\ c_{13}& c_{23}& c_{33}\end{array}\right]$

其中，C′为新坐标系下椭圆锥面方程的二次型矩阵；

接着采用以下公式计算获得新坐标系下孔特征圆心位置p’的坐标为：

$\left\{\begin{array}{c}{x_0}^{\′}=-\frac{c_{13}{z_0}^{\′}}{c_{11}}\\ {y_0}^{\′}=-\frac{c_{23}{z_0}^{\′}}{c_{11}}\\ {z_0}^{\′}=\frac{c_{11}r}{\sqrt{c_{13}^2+c_{23}^2-c_{11}{c}_{33}}}\end{array}\right.$

然后通过旋转矩阵将坐标反变换计算获得孔特征在原直角坐标系下的初步圆心位置p的坐标：

p＝R_cp′

其中，R_c表示旋转矩阵。

8.根据权利要求1所述的一种开放场景下孔特征的视觉定位方法，其特征在于：所述的第八步具体步骤如下：首先将摄像头向不平行于z轴的任意方向移动距离T，确保孔特征仍然位于摄像头视野范围内；重复上述步骤进行图像处理并获得移动摄像头后的孔特征的初步圆心位置；将移动摄像头后的孔特征的初步圆心位置和移动摄像头后的孔特征的初步圆心位置进行比较，取其中两次初步圆心位置中重复的圆心点作为最终正确的孔特征圆心，并以该圆心对应的平面法向量作为孔特征的位姿方向。