CN110176041B

CN110176041B - 一种基于双目视觉算法的新型列车辅助装配方法

Info

Publication number: CN110176041B
Application number: CN201910454792.2A
Authority: CN
Inventors: 吴小平; 王艺宸; 张江泉; 陈诗伟; 漆令飞; 潘宏烨; 韩磊; 张祖涛
Original assignee: Southwest Jiaotong University
Current assignee: Southwest Jiaotong University
Priority date: 2019-05-29
Filing date: 2019-05-29
Publication date: 2021-05-11
Anticipated expiration: 2039-05-29
Also published as: CN110176041A

Abstract

一种基于双目视觉算法的新型列车辅助装配方法。本发明属于摄像机标定技术领域，尤其涉及一种基于双目视觉算法的新型列车辅助装配方法。针对现有技术中大型装配体的装配过程中需要人为地识别、抓取和装配零件，耗时长、效率低和出错率高的问题。本发明的方案是：通过双目视觉测出零件的距离，通过标志物识别零件的型号，最后通过机械臂抓取。实现精确测定零件的三维信息，实现零件的自动识别、自动抓取。

Description

一种基于双目视觉算法的新型列车辅助装配方法

技术领域

本发明属于摄像机标定技术领域，尤其涉及一种基于双目视觉算法的新型列车辅助装配方法。

背景技术

双目立体视觉融合两只眼睛获得的图像并观察它们之间的差别，使我们可以获得明显的深度感，建立特征间的对应关系，将同一空间物理点在不同图像中的映像点对应起来，这个差别，我们称作视差(Disparity)图像。

目前，我国是制造业大国，生产规模庞大，流水线众多，各种工序纷繁复杂，尤其是列车的装配过程。一辆列车上的零件成千上万，装配过程极其复杂，传统的装配，需要装配工人对照图纸确定零件的类型、型号。

对于现有技术，大型装配体装配过程中，对于零件的识别、抓取和装配需要工人对比图纸的情况下进行，即工人需要知道零件的位置和型号，应该知道用什么尺寸的紧固件，这样使得耗时长、效率低和出错率高。而若要通过机械臂自动抓取和装配零件，则存在机械臂难以从各种型号中的零件挑出正确型号的零件的困难。

发明内容

针对现有技术中大型装配体的装配过程中需要人为地识别、抓取和装配零件，耗时长、效率低和出错率高的问题，本发明提供一种一种基于双目视觉算法的新型列车辅助装配方法，其目的在于：通过双目立体视觉，精确测定零件的三维信息，实现零件的自动识别、自动抓取和自动装配。

本发明采用的技术方案如下：

一种基于双目视觉算法的新型列车辅助装配方法，包括以下步骤：

步骤1:分别将标志物贴在每个零件上；

步骤2:分别对两个摄像机通过张正友标定法进行单目标定，确定两个摄像机分别的内在参数Λ₁和Λ₂，确定摄像机的畸变系数；

步骤3：将两个摄像机的位置关系和相对方向固定，对摄像机进行双目标定，确定两个摄像机之间的相对位姿信息τ和Ω；

步骤4：利用步骤3中相对固定的两个摄像机同时拍摄一个零件，得到两个视图；

步骤5：从步骤4得到的两个视图中取出数对相互对应的2D点的坐标，利用步骤1得到的畸变系数对上述2D点的坐标进行校正，利用校正后的2D点的坐标和步骤2确定的相对位姿信息τ、Ω进行3D点求解，获取零件的距离；

步骤6:对摄像机拍摄的标志物进行预处理，进行颜色空间转换、颜色分割和中值滤波后得到一幅去噪的、含有标志信息的二值图，并对二值图进行轮廓检测；

步骤7：通过步骤6得到了标志物的轮廓及四个点，结合步骤5中零件的距离可以得到四个角点世界坐标中的三维信息；

步骤8:对机械臂进行建模，在每个连杆上建立一个坐标系，通过a、α、d和θ建立坐标系之间的关系，按照连杆连接顺序第i个坐标系相对于第连杆顺序第i-1个坐标系的齐次变换矩阵：

其中，a：连杆长度，α：连杆扭转，d：连杆偏移，θ：关节转角；

步骤9：通过齐次变换矩阵计算末端执行器相对于世界坐标系的变换矩阵：

其中

分别为齐次变换矩阵；

步骤10:对机械臂末端位姿的确定，确定转换矩阵，计算出机械臂的逆解：

摄像机和机械臂摆放位置涉及平移变化，可表示为：

(x′ y′ z′)＝(x+Δ_x y+Δ_y z+Δ_z)

由于坐标旋转变换围绕坐标系的X、Y和Z轴的旋转变换，根据步骤7中得到的标志物的四个点的三维信息可以得到末端执行器的三个角度，通过3个角度求解出末端执行器坐标系的转换坐标系：

步骤11：通过已知末端执行器的位姿，求取六个舵机的角度：

其中，c_i：cosθ_i，s_i：sinθ_i；

采用了此方案，通过在零件上贴标志物可以同时得到零件的型号信息和机械臂与零件之间的位置关系，而且通过标志物四个点的信息可以得到机械臂末端执行器抓取的角度，从而将机械臂抓取与双目识别技术结合达到了利用双目视觉算法来辅助机械臂自动抓取零件并自动装配的目的。

所述步骤2中利用SVD法求得摄像机的内在参数Λ为：

其中δ_x、δ_y、

γ的计算公式为：

所述步骤2中所述的畸变包括径向畸变和切向畸变：

校正所述径向畸变的公式为：

x′＝x×(1+k₁r²+k₂r⁴)

y′＝y×(1+k₁r²+k₂r⁴)

校正所述切向畸变的公式为：

x′＝x+[2p₁xy+p₂(r²+2x²)]

y′＝y+[2p₂xy+p₁(r²+2y²)]

所述步骤3中，摄像机之间相对位姿信息τ、Ω可以根据公式

求得，其中F为3x3的矩阵，

定义F的矢量形式f＝[f₁₁,f₁₂,f₁₃,f₂₁,f₂₂,f₂₃,f₃₁,f₃₂,f₃₃]，

根据

|f|＝1，

其中x_ij和y_ij为左右摄像机拍摄的同一图像中的匹配点，至少取8对点，获得这些点的坐标x_ij和y_ij对点，即可求得f和F，也就找到了两个相机位姿对应信息τ、Ω。

采用了此方案，通过单目标定，确定了摄像机的内在参数，并消除了相机的畸变，再通过双目标定求得同一场景两幅图像对应点间的几何关系，确定两个摄像机之间的相对平移和旋转关系。

其中，所述步骤4为:

综上所述，由于采用了上述技术方案，本发明的有益效果是：

1.单双目标定后的参数，均属相机固有属性，一旦摄像机的型号以及安装位置确定，均不会改变，即使出现意外，某一个摄像头出现问题进行了更换，导致相机的参数发生改变，仍可以人工进行摄像机单目、双目标定，重新确定参数，在扫描装配件时只需进行立体匹配即可得到零件的距离，结合标志物四个点的信息实现机械臂的抓取。

2.由于在摄像头生产的过程中，由于透镜的制造工艺，会使成像产生多种形式的畸变，主要有桶形畸变或枕形畸变，于是为了去除畸变，使成像后的图像与真实世界的景象保持一致，需要计算并利用畸变系数来矫正这种像差。

3.用两颗摄像头代替了人的眼睛，使得在列车装配的过程中，本应该由人对比图纸，查找装配件型号的任务由机器代为完成，装置结构简单，操作方便和安全，不仅缩短了查找耗时、减少了差错，而且大大减少了工人师傅的工作量，简化了装配流程，提高了装配效率。

附图说明

本发明将通过例子并参照附图的方式说明，其中：

图1是本发明流程示意图。

图2是本发明标定板示意图。

图3是图像再VW平面的投影。

图4是本发明扫描局部零件的点云图。

图5是本发明标志物示意图。

图6是本发明标志物4顶点相互距离示意图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。

下面结合图1-图6对本发明作详细说明。

步骤1:分别将标志物贴在每个零件上；

步骤4：利用步骤2中相对固定的两个摄像机同时拍摄一个标志物，得到两个视图；

步骤5：从步骤3得到的两个视图中取出数对相互对应的2D点的坐标，利用步骤1得到的畸变系数对上述2D点的坐标进行校正，利用校正后的2D点的坐标和步骤2确定的相对位姿信息τ、Ω进行3D点求解，获取零件的距离；

步骤6:对摄像机拍摄的图像进行预处理，进行颜色空间转换、颜色分割和中值滤波后得到一幅去噪的、含有标志信息的二值图，并对二值图进行轮廓检测；

(1)首先对RGB颜色空间向HSV颜色空间转换；

(2)通过进行颜色分割以在图像中提取标志物，

OpenCV中颜色分割的函数为：

void cv::inRange(

InputArray src,//输入图像

InputArray lowerb,//最低阈值

InputArray upperb,//最高阈值

OutputArray dst//输出图像

)

该函数在进行颜色分割时的判断依据是：

对于单通道数组：

dst(I)＝lowerb(I)₀≤src(I)₀≤upperb(I)₀

即对于灰度图像，如果在最低阈值和最高阈值的范围之内，则令其等于255，否则等于0。

对于双通道数组：

dst(I)＝lowerb(I)₀≤src(I)₀≤upperb(I)₀∧lowerb(I)₁≤src(I)₁≤upperb(I)₁

即只有当两个数组同时在阈值范围内时，该像素点的值为255，否则为0。

本文中，只需选取纯红色对应的阈值，即可将图像中的标志物分离出来。即使有其他区域的红色，也不占主要面积，可用其他方法进行分割。在OpenCV中，H、S、V值范围分别是[0,180)，[0,255)，[0,255),至于对应颜色的值的可以通过下表确定，因此，此处的阈值取为：(0，43，46)～(10，255，255)，以防止噪声。

(3)在进行颜色分割之后，还需要进行中值滤波处理；

中值滤波的算法流程如下：

Step1：初始化一个(2n+1)×(2n+1)的窗口；

Step2：从图像左上角开始，提取该窗口内的所有像素；

Step3：该像素从小到大排列并提取其中值，用中值代替图像中心点位的像素值；

Step4：依次遍历整个图像，即完成中值滤波。

OpenCV中中值滤波的函数为：

void medianBlur(

InputArray src,//输入图像

OutputArray dst,//输出图像

int ksize//窗口大小

)

(4)在进行颜色空间转换、颜色分割、中值滤波后，得到一幅去噪的和含有标志信息的二值图，此时需要对二值图进行形态学梯度处理；

图像膨胀:

其中：A为待处理图像；

S为用于处理A的结构元素(类似于滤波处理中的窗口)；

x、y表示像素坐标；

图像腐蚀:

形态学梯度处理:

形态学梯度处理就是用膨胀图像减去腐蚀图像，以突出其轮廓,在OpenCV中，形态学梯度处理函数为：

void cv::morphologyEx(

InputArray src,//输入图像

OutputArray dst,//输出图像

int op,//形态操作的类型

InputArray kernel,//结构元素

Point anchor＝Point(-1,-1),//用内核锚定位置

int iterations＝1,//侵蚀和扩张的次数

int borderType＝BORDER_CONSTANT,//像素外推方法

const Scalar&borderValue＝morphologyDefaultBorderValue()//边界不变的边界值

)

(5)对物体进行轮廓检测：

Step1：选定一个像素点(i,j)，并判断：

(1)如果f_ij＝1，且f_ij-1＝0，则(i,j)为轮廓边界的起始跟踪点，NBD(边界序列号，初值为1)加1，且令(i₂,j₂)←(i,j-1)；

(2)如果f_ij≥1，且f_ij+1＝0，则(i,j)为孔边界的起始跟踪点，NBD加1，且令(i₂,j₂)←(i,j+1)，当f_ij>1的情况下，LNBD←f_ij，LBND是上一个遇到的(内/外)边界的顺序数，每当开始扫描新的一行，重置LNBD＝1。

(3)否则直接跳转Step4。

Step2：根据最新边界的类型以及LNBD的边界的类型，以决定当前边界的母边界，判断规则如下表：

Step3：以(i,j)为起始点，跟踪检测到的边界，具体跟踪方法为：

(1)从(i₂,j₂)开始，顺时针查找点(i,j)的邻域，寻找非零点，直到找到第一个非零点记为(i₁,j₁)。否则：f_ij＝-NBD，直接跳转到步骤4。

(2)令(i₂,j₂)←(i₁,j₁)，且(i₃,j₃)←(i,j)。

(3)在点(i₃,j₃)的邻域中，以(i₂,j₂)为起始点，逆时针查找非零点，直到找到第一个非零点记为(i₄,j₄)。

(4)修改点(i₃,j₃)的f_i3，j3值，修改规则为：

(a)如果(3)中查找到点(i₃,j₃+1)为0-像素点，

则f_i3，j3←-NBD；

(b)如果(3)中查找到点(i₃,j₃+1)不是0-像素点；

(c)且f_i3，j3＝1，则f_i3，j3←NBD；

(d)否则，不修改f_i3，j3的值。

(5)如果(i₄,j₄)＝(i,j)且(i₃,j₃)＝(i₁,j₁)，则直接跳转到步骤4，否则，(i₂,j₂)←(i₃,j₃)，(i₃,j₃)←(i₄,j₄)，且返回步骤(3)。

Step4：如果f_ij≠1，则LNBD←|f_ij|，且从点(i,j+1)重新开始扫描。直到扫

描到达右下角点，则算法终止。

在OpenCV中，运用该算法进行轮廓检测的函数为：

void cv::findContours(

InputOutputArray image,//输入图像，必须为二值图

OutputArrayOfArrays contours,//检测到轮廓，每个轮廓都存储为点向量

OutputArray hierarchy,//可选项，给定参数则会输出轮廓树结构

int mode,//轮廓提取方式

int method,//轮廓近似方法

Point offset＝Point()//每个轮廓点移动的可选偏移量

)

对于mode的说明，共有五种可选模式：

(1)：RETR_EXTERNAL:仅检索极端外轮廓。

(2)：RETR_LIST：检索所有轮廓而不建立任何层次关系。

(3)：RETR_CCOMP:检索所有轮廓并将它们组织成两级层次结构。在顶层，组件有外部边界。在第二层，有孔的边界。如果连接组件的孔内有另一个轮廓，它仍然位于顶层。

(4)：RETR_TREE:检索所有轮廓并重建嵌套轮廓的完整层次结构

(5)：RETR_FLOODFILL：洪水填充法

另外，还设有绘制轮廓的函数：

void cv::drawContours(

InputOutputArray image,//输入输出图像

InputArrayOfArrays contours,//所有输入轮廓。每个轮廓都存储为点矢量。

int contourIdx,//要绘制的轮廓的参数。如果是负数，则绘制所有轮廓

const Scalar&color,//轮廓的颜色

int thickness＝1,//绘制轮廓的线条粗细

int lineType＝LINE_8,//线条类型，有默认值LINE_8

InputArray hierarchy＝noArray(),//可选的层次结构信息

int maxLevel＝INT_MAX,//绘制轮廓的最大级别。如果为0，则仅绘制指定的轮廓。如果为1，则该函数绘制轮廓和所有嵌套轮廓。如果为2，则该函数绘制轮廓，所有嵌套轮廓，所有嵌套到嵌套轮廓，等等。仅当有可用的层次结构时才考虑此参数。

Point offset＝Point()//可选的轮廓移位参数。按指定的移动所有绘制的轮廓。

)

本文程序开发IDE为：Visio Studio2015，采用OpenCV4.0库，轮廓检测函数命名为：int*findcontours()

如图5中，从最右点开始，逆时针标记ABCD四个角点，不难看出B、D两点的距离信息是一样的，而A、C两点则会有差距，从光心O出发，与A、C两点构成一个三角形，如图6所示；

可以看出偏移角度与光心O的位置无关，根据三角形关系，可得直立偏移角度为：

机械臂抓取的角度为：

机械臂抓取的位置为标志物的中心坐标为：

步骤8:对机械臂进行建模，在每个连杆上建立一个坐标系，通过a、α、d和θ建立坐标系之间的关系，从而建立首连杆和末连杆坐标系的关系，

a：两个关节的轴(旋转关节的旋转轴，平移关节的平移轴)之间的公共法线长度；

α：一个关节的轴相对于另一个关节的轴绕它们的公共法线旋转的角度；

d：一个关节与下一个关节的公共法线和它与上一个关节的公共法线沿这个关节轴的距离；

θ：一个关节与下一个关节的公共法线和它与上一个关节的公共法线绕这个关节轴的转角。

利用SolidWorks软件测量计算得到a、α、d和θ表：

步骤9：通过a、α、d和θ计算标系i相对于连杆坐标系i-1的齐次变换矩阵：

将a、α、d和θ带入上式可以得到相邻坐标系的齐次变换矩阵：

公式中c₁＝cosθ₁,s₁＝sinθ₁，将

的矩阵相乘可以得到末端执行器相对于世界坐标系的变换矩阵：

由此可得：

步骤10:对机械臂末端位姿的确定，确定转换矩阵：

(1)三维坐标系从世界坐标系向机械臂坐标系的转换；

摄像机和机械臂摆放位置的关系，因此这部分只涉及到平移变换，而没有旋转变换：

(x′ y′ z′)＝(x+Δ_x y+Δ_y z+Δ_z)

(2)确定机械臂坐标系与末端执行器的转换矩阵；

绕X轴的旋转矩阵可表示为：

绕Y轴的旋转矩阵可表示为：

绕Z轴的旋转矩阵可表示为：

已知末端执行器的三个角度的情况下，可以求解出末端执行器坐标系的转换坐标系：

由于

所以末端执行器的转换矩阵可表示为：

步骤11：通过已知末端执行器的位姿，求取六个舵机的角度，根据步骤n中的正解利用三角形积化和差、和差化积公式化简：

左乘上面两个矩阵得：

根据等式两边第三行第四列相等可得：

p_xs₁-p_yc₁＝0，

即:

根据矩阵两边(1,4)和(2,4)分别相等可得:

将两个公式两边分别平方相加可得:

p_z ²+(p_xc₁+p_ys₁)²＝

c₂₃₄ ²a₄ ²+c₂₃ ²a₃ ²+c₂ ²a₂ ²+2c₂₃₄a₄c₂₃a₃+2c₂₃a₃c₂a₂+2c₂₃₄a₄c₂a₂+s₂₃₄ ²a₄ ²+s₂₃ ²a₃ ²+s₂ ²a₂ ²+2s₂₃₄a₄s₂₃a₃+2s₂₃a₃s₂a₂+2s₂₃₄a₄s₂a₂＝a₄ ²+a₃ ²+a₂ ²+2a₃a₄c₄+2a₂a₃c₃+2a₂a₄c₂₃

通过

左乘转换公式可得：

根据等式两边的第三行第四列相等可得:

-s₂₃₄(c₁a_x+s₁a_y)+c₂₃₄a_z＝0，

同理，

即：

可以求得c₃，进而求解出θ₃，

对上述平方相加的公式做出另一种变形：

可得，θ₄＝θ₂₃₄-θ₂-θ₃，

根据矩阵两边(1,3)和(2,3)分别相等可得:

s₅＝c₂₃₄(c₁a_x+s₁a_y)+s₂₃₄a_z，

c₅＝-c₁a_y+s₁a_x，

根据矩阵两边(3,1)和(3,2)分别相等可得，

s₆＝-s₂₃₄(c₁n_x+s₁n_y)+c₂₃₄n_z，

c₆＝-s₂₃₄(c₁o_x+s₁o_y)+c₂₃₄o_z，

综上，六自由度的机械臂逆解为：

步骤12:将六个舵机的转动角度传输到控制器，通过串口通信实现抓取。

机械臂是可以通过串口通信进行控制，这里采用C++语言，直接调用Windows的API函数进行串口通信，串口通信分为四个部分：打开串口、配置串口、读写串口、关闭串口：

(1)打开串口

打开串口的函数声明为:

HANDLE CreateFile(

LPCTSTR lpFileName,//需要打开的串口逻辑名称，本文中为"COM2"

DWORD dwDesiredAccess,//指定串口的访问模式，有读取、写入或二者并列三种情况

DWORD dwShareMode,//指定共享模式，该参数一般设置为0

LPSECURITY_ATTRIBUTES lpSecurityAttributes,//引用安全性属性结构，通常为NULL

DWORD dwCreationDistribution,//创建方式，一般置为OPEN_EXISTING

DWORD dwFlagsAndAttributes,//文件属性和标志

HANDLE hTemplateFile//临时文件的句柄，通常为NULL

)；

(2)配置串口

在打开通讯设备句柄后，常常需要对串口进行一些初始化配置工作。这需要通过一个DCB结构来进行。DCB结构包含了诸如波特率、数据位数、奇偶校验和停止位数等信息。在查询或配置串口的属性时，都要用DCB结构来作为缓冲区。

typedef struct_DCB{

DWORD BaudRate；//波特率，设置为9600

DWORD fParity；//指定奇偶校验使能

BYTE ByteSize；//通信字节位数，设置为8字节

BYTE Parity；//指定奇偶校验方法

BYTE StopBits；//指定停止位的位数

}

(3)读写串口

读取串口的函数为：

BOOL ReadFile(

HANDLE hFile,//文件的句柄

LPVOID lpBuffer,//用于保存读入数据的一个缓冲区

DWORD nNumberOfBytesToRead,//要读入的字符数

LPDWORD lpNumberOfBytesRead,//指向实际读取字节数的指针

LPOVERLAPPED lpOverlapped

)；

写入串口的函数为：

BOOL WriteFile(

HANDLE hFile,//文件的句柄

LPCVOID lpBuffer,//用于保存读入数据的一个缓冲区

DWORD nNumberOfBytesToWrite,//要写入的字符数

LPDWORD lpNumberOfBytesWritten,//指向实际读取字节数的指针

LPOVERLAPPED

)；

(4)关闭串口

关闭串口的函数很简单：

BOOL CloseHandle(

HANDLE hObject；//关闭串口

)；

根据上述四步即可打开串口，并将需要的指令写入串口，进而控制机械臂动作，或读取串口信息，获取机械臂状态。

而控制机械臂的指令的“字典”是机械臂的二次开发串口通信协议，需要说明的是：如果用户发送正确数据到控制板，那么控制板上的蓝色LED2灯会闪一下，表明已经接收到正确数据。如果发送的是错误数据，那么蓝色LED2灯不会有任何反应，保持常亮，蜂鸣器会“滴，滴”响两声，提示用户发送数据错误。可以据此判断串口是否打开，以及指令是否正确。

在协议的指令集中，最需要的是两条指令：

1.指令名：CMD_SERVO_MOVE

功能：控制任意个舵机的转动

指令形式:

帧头：连续收到两个0x55,表示有数据包到达；

数据长度Length＝控制舵机的个数×3+5；

指令：控制代码，即需要进行何种操作；

参数：1、要控制舵机的个数

2、时间低八位

3、时间高八位

4、舵机ID号

5、角度位置低八位

6、角度位置高八位

例如：控制1号舵机在1000ms内转到800的位置，指令为：

因此，向串口发送的数据包为：

0x55 0x55 0x08 0x03 0x01 0xE8 0x03 0x01 0x20 0x03

2.指令名：CMD_MULT_SERVO_POS_READ

功能:读取多个舵机的角度位置值

数据长度：N＝控制舵机的个数+3；

指令名称:0x15；

参数:1、要读取舵机的个数

2：舵机a的ID号

3：舵机b的ID号

例如：读取1,2,3,4,5,6号舵机的角度位置值，指令为：

优选的，所述步骤2中利用SVD法求得摄像机的内在参数Λ为：

其中δ_x、δ_y、

γ的计算公式为：

优选的，所述步骤2中所述的畸变包括径向畸变和切向畸变：

校正所述径向畸变的公式为：

x′＝x×(1+k₁r²+k₂r⁴)

y′＝y×(1+k₁r²+k₂r⁴)

校正所述切向畸变的公式为：

x′＝x+[2p₁xy+p₂(r²+2x²)]

y′＝y+[2p₂xy+p₁(r²+2y²)]

所述畸变系数包括k₁、k₂、p₁和p₂。

优选的，所述步骤3中，摄像机之间相对位姿信息τ、Ω可以根据公式

求得，其中F为3x3的矩阵，

根据

|f|＝1，

其中xij和yij为左右摄像机拍摄的同一图像中的匹配点，至少取8对点，获得这些点的坐标xij和yij对点，即可求得f和F，也就找到了两个相机位姿对应信息τ、Ω。

单目标定主要分为两大部分，一是确定摄像机的内在参数，二是确定摄像头畸变系数，消除畸变，其中，为了方便说明，需要首先定义4个坐标系：

(1)世界坐标系：真实世界的三维空间坐标系，其坐标原点可以任意指定；

(2)相机坐标系：以相机光心为坐标原点建立的三维空间坐标系；

(3)图像坐标系：相机所成图像上建立的坐标系，是以图像中心为坐标原点的二维笛卡尔坐标系；

(4)像素坐标系：相机所成图像上建立的坐标系，是以图像左上角为坐标原点的二维坐标系，与图像坐标系不同的是，其度量尺寸为像素值；

在进行理论计算时，默认透镜没有畸变，但在实际中，不存在完美的透镜，摄像机在生产的过程中，透镜的制造精度存在限制，同时在装配的时候，很难将痛经与成像装置完全对齐，因此，会使成像产生多种形式的畸变。

1、径向畸变

径向畸变是一种沿着透镜半径方向分布的畸变，对于透镜来说，远离透镜中心的光线比靠近透镜中心的光线更容易弯曲。这种现象在廉价相机上体现的尤为明显。径向畸变一般分为桶形畸变和枕形畸变两种情况，通常情况下,成像过程中短焦距镜头产生桶形畸变,长焦距镜头产生枕形畸变。

对于径向畸变，成像仪光轴中心的畸变为0，随着向边缘移动，畸变逐渐增大，这种畸变可以用r＝0附近的泰勒数级展开式的前两项来描述，即：k1、k2，对于畸变较大的相机，可以引入第三个径向畸变系数k3来矫正，成像装置上某点的径向位置可以根据下面的公式调整：

x′＝x×(1+k₁r²+k₂r⁴)

y′＝y×(1+k₁r²+k₂r⁴)

2、切向畸变

切向畸变通常是由于在机械装配的过程中，透镜与成像平面不平行导致的。切线畸变可以用额外的两个参数表示：p1、p2。可根据以下公式调整：

x′＝x+[2p₁xy+p₂(r²+2x²)]

y′＝y+[2p₂xy+p₁(r²+2y²)]

综上，摄像头的畸变可以用k1、k2、(k3)、p1、p2，共四(五)个参数表示，摄像头的主要畸变为径向畸变，切向畸变一般很小，尤其是对于工业相机，其装配过程更为严格，切向畸变可以忽略不计，因此，对于本相机只需要计算k1、k2两个畸变系数即可。

引入归一化相机的模型，在该模型中，相机的焦距是1，且摄像机图像平面上2D坐标系(x,y)的原点位于投影点的中心，如图3是图像在vw平面的投影，由相似三角形得出

即3D世界向2D平面的映射，但归一化相机在实际情况下是不存在的，它存在两点差异：

(1)焦距不为1，且由于图像的最终位置是利用像素进行测量的，因此模型必须将感光体的间距考虑在内，考虑到感光体在x方向和y方向上的间距是不同的，因此引入两个比例因子：

称为焦距参数，原本的映射关系就变成了：

(2)在像素坐标系中，坐标原点位于左上角，而非图像中心，这就需要对x、y的位置进行转移，因此增加偏移量参数：δ_x，δ_y，同时引入偏移参数γ用于控制投影位置x作为真实世界中高度v的函数，原本的映射关系就变成了：

校正后，首先确定单目相机，从外部三维坐标点向二维平面映射关系，考虑摄像机外部因素，摄像机的位置并非总是位于全局坐标系的原点，尤其是考虑两台及以上摄像机的时候，为此，需要通过坐标变换，在真实世界通过投影模型前，得出摄像机坐标系中真实世界点w

简写为：w′＝Ωw+τ，其中：w′为变换点，Ω为3*3的旋转矩阵，τ为3*1的平移向量。综上所述，可以得到从3D世界中的点在2D图像上的映射：

该映射有两套参数，其一是摄像机的内在参数：

反映的是摄像机自身的性质，其二是摄像机的外在参数：{Ω,τ}，反映的是摄像机在现实世界中的位置与方向。

其中，内在参数可以用矩阵Λ表示，

张氏标定法的具体思路是，用于标定的棋盘是三维世界的一个平面Π，它在成像平面所成的像为另一个平面π，由于标定棋盘已知其角点坐标，图像的角点可以通过角点提取算法求得，通过两个平面的对应点的坐标，就可以求解出两个平面的单应矩阵H，从而求出摄像机的内在参数完成标定，设棋盘所在平面为世界坐标系中的z＝0的平面，即xy平面。这样，棋盘上的任意角点在世界坐标系中可表示为(X,Y,0)。待入上述映射方程可得:

用单应矩阵可以表示为：

联立两式可得(由于求得的H与真实的H之间可能存在误差，这里加上一个比例因子s)：

H＝sΛ(w₁ w₂ τ)

通过平面间的单应可得：

H＝[h₁ h₂ h₃]＝sΛ(w₁ w₂ τ)

将旋转矩阵Ω中的列向量用单应矩阵的列向量表示，可得:

w₁＝sΛ^-1h₁

w₂＝sΛ^-1h₂

τ＝sΛ^-1h₃

矩阵Ω是旋转矩阵，由于旋转矩阵也是正交矩阵，根据正交矩阵的性质：正交矩阵的列向量的模为1，且任意两个列向量的向量内积均为0。则有:

‖w₁‖＝‖w₂‖＝1

将上述等式用单应矩阵的列向量表示，可以得到在一幅棋盘标定板图像(即：一个单应矩阵)中的两个约束条件。

观察上式，注意到两个等式中都有Λ^-TΛ^-1。于是，令B＝Λ^-TΛ^-1，则有：

由于B是一个对称矩阵，其未知量只有六个。因此，将六个未知量携程向量形式：

b＝[B₁₁ B₁₂ B₂₂ B₁₃ B₂₃ B₃₃]

用h_ij表示单应矩阵的第i行第j列，则约束方程中的

为：

因此：

其中：

v_ij＝[h_i1h_j1 h_i1h_j2+h_i2h_j1 h_i2h_j2 h_i3h_j1+h_i1h_j3 h_i3h_j2+h_i2h_j3 h_i3h_j3]^T

现在再来看之前的两个约束公式，可转换为：

写为矩阵形式则有：

或：

对于上述方程，V是一个2n×6的矩阵，是一个六维向量，因此对于一幅标定板图像，可以得到两个方程组，而待求解的变量共有

δ_x、δ_y、γ五个变量，因此，至少需要三幅标定图像才能求解出全部参数，即：使得方程有解的充要条件是n≥3，张正友标定法，就是通过对三幅以上的标定图像，利用Vb＝0这一约束条件对摄像机的内参数进行求解。

对于m个线性无关的方程求解n个未知数，共有三种情况：

1、m>n，约束的个数大于未知数的个数，称为超定问题。

2、m＝n，约数的个数等于未知数的个数，方程有唯一解。

3、m<n，约数的个数小于未知数的个数，称为欠定问题。

对于求解Vb＝0的问题，当n≥3时，共有六个方程，需要求解的未知数共五个，因此m>n，该求解问题为超定问题。因此，该方程的解是不存在的，此时该问题就转换为求解最小二乘解的问题，即：min‖Vb-0‖。

求解最小二乘解需要用到奇异值(SVD)分解法。

SVD的定义：对于任意一个m×n维的矩阵A，可以分解为：A＝USV^T,其中U是一个m×m的正交阵，S(可写为Σ_r)是r个沿对角线从大到小排列的奇异值(非负)组成的方阵，V是一个n×n的正交阵。

V的列向量组成一套对A的正交“输入”或“分析”的基向量，这些向量是A^TA的特征向量。

U的列向量组成一套对A的正交“输出”的基向量，这些向量是AA^T的特征向量。

S对角线上的元素可视为在输入与输出间进行的纯量的"膨胀控制"，这些是A^TA和AA^T的特征值的非零平方根。

A^TA和AA^T用SVD分解可表示为:

A^TA＝(VS^TU^T)USV^T＝VS^T(U^TU)SV^T＝V(S^TS)V^T

AA^T＝USV^T(VS^TU^T)＝US^T(V^TV)SU^T＝U(SS^T)U^T

AA^T为实对称阵的特征值为λ，则S中的奇异值为：

V为A^TA的特征向量。对于奇异值，它在奇异值矩阵中也是按照从大到小排列，而且σ的减少特别的快，在很多情况下，前10％甚至1％的奇异值的和就占了全部的奇异值之和的99％以上了。这也是求得最小二乘解的本质思路，因此，我们可以用前r大的奇异值来近似描述矩阵A：

下面，利用奇异值(SVD)分解法求解一开始的问题。

设A∈R^m×n列满秩，A＝UΣV^T是A的奇异值分解。令U_n为U的前n列矩阵，即：

则：

当且仅当

时等号成立，所以：

即为最小二乘问题的解。

对于Vb＝0，对V^TV进行SVD分解，其最小特征值对应的特征向量就是Vb＝0的最小二乘解，从而求得矩阵B。利用SVD法，最终求得摄像机的内在参数为：

随后对两个摄像机进行双目标定，以求得两个摄像机的实矩阵(即求得两个摄像机的相对位置)。为了计算方便，我们对2D图像点和3D世界点的表达形式做出修改，使代投影方程变为线性的：

将2D图像点的坐标转化为3D其次坐标

将3D世界点的坐标转化为4D其次坐标

这样在不考虑相机位姿的情况下，原本的投影方程就转化为：

可写为：

λ＝w

这样原本的非线性映射关系就变成了线性的。继续加入相机的位姿信息，可以得到其次化的映射关系表达式：

现在考虑双目摄像机的投影，假定第一个摄像机为世界坐标的坐标中心，第二个摄像机处于任意位置。这样，在齐次坐标中，一个3D点投影到两个摄像机上，可表示为：

将上面两个公式合并，可得：

使用平移向量τ乘以两边求它们的向量外积，可将Λ₂τ这一项约去：

使用

乘以两边求它们的向量内积，可将等式左边约去：

令

则：

可以看作是摄像机2中的点向摄像机1中点的对应。

令E＝τ×Ω，则：

其中Λ₁，Λ₂表示摄像机1、2的内部参数，那么E就表示储存两个摄像机之间相对旋转和平移信息的转换矩阵。将F称为基础矩阵，E称为实矩阵。因此，分别对两部摄像机进行简单的标定，得到Λ₁，Λ₂后，只要求解出基础矩阵，就能知道实矩阵，也就找到了两个相机位姿对应信息，解决了问题。

由双摄像机模型，得到了摄像机2中的点向摄像机1中点的对应关系：

由于

为1x3的矩阵，

为3x1的矩阵，因此F为3x3的矩阵。将表达式展开写，在齐次坐标中，图像1中第i个点与图像2中第i个点的对应关系为：

继续展开，可写为：

将上式用向量内积的形式表达：

[x_i2x_i1,x_i2y_i1,x_i2,y_i2x_i1,y_i2y_i1,y_i2,x_i1,y_i1,1]f＝0

其中：f＝[f₁₁,f₁₂,f₁₃,f₂₁,f₂₂,f₂₃,f₃₁,f₃₂f₃₃]即为基础矩阵F的矢量形式。这样就为F中的元素提供了一个线性约束，因此，对于I个已知的匹配点，可以得到：

同时：|f|＝1。

优选的，

优选的，两个摄像机的型号相同，两个摄像机通过螺栓连接摄像头底部的螺栓孔与导轨上的导槽实现摄像头定位，摄像头通过USB线与上位机连接，导轨连接有握把在使用时方便移动。

对多个点进行3D点坐标求解，即可形成3D局部点云。

优选的，采用深度学习方法，预先处理好几个部分的点云图和已知位置，通过神经网络建立对应方法，最后根据输入的点云特征判断是哪个零件部分的。

以上所述实施例仅表达了本申请的具体实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请保护范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请技术方案构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。

Claims

1.一种基于双目视觉算法的新型列车辅助装配方法，其特征在于：包括以下步骤：

步骤1：分别将标志物贴在每个零件上；

步骤2：分别对两个摄像机通过张正友标定法进行单目标定，确定两个摄像机分别的内在参数Λ₁和Λ₂，确定摄像机的畸变系数；

步骤6：对摄像机拍摄的标志物进行预处理，进行颜色空间转换、颜色分割和中值滤波后得到一幅去噪的、含有标志信息的二值图，并对二值图进行轮廓检测；

步骤7：通过步骤6得到了标志物的轮廓及四个角点，结合步骤5中零件的距离可以得到四个角点世界坐标中的三维信息；

步骤8：对机械臂进行建模，在每个连杆上建立一个坐标系，通过a、α、d和θ建立坐标系之间的关系，按照连杆连接顺序第i个坐标系相对于第连杆顺序第i-1个坐标系的齐次变换矩阵：

其中

分别为齐次变换矩阵；

步骤10：对机械臂末端位姿的确定，确定转换矩阵，计算出机械臂的逆解：

摄像机和机械臂摆放位置涉及平移变化，可表示为：

(x′ y′ z′)＝(x+Δ_x y+Δ_y z+Δ_z)

其中，c_i：cosθ_i，s_i：sinθ_i；

步骤12：将六个舵机的转动角度传输到控制器，通过串口通信实现抓取。

2.根据权利要求1所述一种基于双目视觉算法的新型列车辅助装配方法，其特征在于：所述步骤2中利用SVD法求得摄像机的内在参数Λ为：

其中δ_x、δ_y、

γ的计算公式为：

其中，Λ为Λ₁和Λ₂的通用表达式，

分别为2D点的坐标x和y的焦距参数，δ_x、δ_y分别为2D点的坐标x和y的偏移量参数，γ为像素偏移参数，

3.根据权利要求1所述一种基于双目视觉算法的新型列车辅助装配方法，其特征在于：所述步骤2中所述的畸变包括径向畸变和切向畸变：

校正所述径向畸变的公式为：

x′＝x×(1+k₁r²+k₂r⁴)

y′＝y×(1+k₁r²+k₂r⁴)

校正所述切向畸变的公式为：

x′＝x+[2p₁xy+p₂(r²+2x²)]，

y′＝y+[2p₂xy+p₁(r²+2y²)]，

其中，k1、k2是径向畸变系数；p1、p2是切向畸变系数，r是点(x，y)到图像中心的距离。

4.根据权利要求1所述一种基于双目视觉算法的新型列车辅助装配方法，其特征在于：所述步骤3中，摄像机之间相对位姿信息τ、Ω可以根据公式

求得，其中F为3x3的矩阵，

定义F的矢量形式f＝[f₁₁，f₁₂，f₁₃，f₂₁，f₂₂，f₂₃，f₃₁，f₃₂，f₃₃]，

根据

其中x_ij和y_ij为左右摄像机拍摄的同一图像中的匹配点，至少取8对点，获得这些点的坐标x_ij和y_ij对点，即可求得f和F。

5.根据权利要求1所述一种基于双目视觉算法的新型列车辅助装配方法，其特征在于：所述步骤5中利用2D点的坐标计算3D点的坐标的公式为：

其中

是2D图像点的坐标转化为的3D齐次坐标，

是3D世界点的坐标转化为的4D齐次坐标。