基于结构光三维测量系统的工件装配方法.pdf

本发明提供一种基于结构光三维测量系统的工件装配方法，在根据本发明的基于结构光三维测量系统的工件装配方法中，待装配工件上的连接孔的分布点云P1为连接件在待装配工件上的理论装配位置，锪窝的分布点云P3为连接件在待装配工件上的实际装配位置，根据连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H将连接孔的位置与锪窝的位置对应，即是将理论装配位置和实际装配位置对应，从而将待装配工件上的理论装配位置的连接件类型投影到实际装配位置上，提高了连接件装配到待装配工件上的连接孔的装配效率并减少发生错误的可能性。

1.一种基于结构光三维测量系统的工件装配方法，包括步骤：
S1：搭建包括相机(1)、投影仪(2)、支架(3)以及计算机(4)的结构光三维测量系统，相
机(1)与投影仪(2)均固定于支架(3)上，使得相机(1)与投影仪(2)的相对位置固定；
S2：利用计算机(4)导入待装配工件的三维模型，根据待装配工件的三维模型得到待装
配工件上的所有连接孔的连接孔信息，待装配工件上共有Nh个连接孔，将各连接孔的连接
孔信息转化为各连接孔的分布点云P1，其中各连接孔的连接孔信息包括各连接孔的三维坐
标和各连接孔对应的连接件型号，则各连接孔的分布点云P1包括各连接孔的三维坐标和各
连接孔对应的连接件型号；
S3：利用结构光三维测量系统基于NP步相移法测量原理得到与步骤S2中的待装配工件
上的连接孔对应的在结构光三维测量系统中的所有锪窝的三维点云P2，其中所有锪窝的三
维点云P2包括所有锪窝的三维坐标，共有Ng个锪窝，其中，Ng≤Nh；
S4：根据步骤S3中得到的待装配工件上的所有锪窝的三维点云P2，利用点云聚类提取
算法，对所有锪窝的三维点云P2，将属于同一锪窝的点归为一组，并计算各锪窝的点云的重
心坐标，从而得到待装配工件上的Ng个锪窝的分布点云P3，Ng个锪窝的分布点云P3包括Ng个
锪窝的重心坐标；
S5：对待装配工件上的各连接孔的分布点云P1，配准对应位置的锪窝的分布点云P3，获
得Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H；
S6：根据步骤S2中得到的Nh个连接孔的分布点云P1、步骤S4中得到的Ng个锪窝的分布点
云P3以及步骤S5中得到的Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间
的位置转换矩阵H，得到Nh个连接孔的分布点云P1在结构光三维测量系统中的位置坐标；
S7：根据步骤S6中得到的Nh个连接孔的分布点云P1在结构光三维测量系统中的位置坐
标，将各连接孔的连接件型号信息投影给对应位置的锪窝。
2.根据权利要求1所述的基于结构光三维测量系统的工件装配方法，其特征在于，步骤
S3中利用结构光三维测量系统基于NP步相移法测量原理得到与步骤S2中的待装配工件上
的连接孔对应的在结构光三维测量系统中的所有锪窝的三维点云P2包括步骤：
S31：对结构光三维测量系统进行标定，得到投影仪(2)的内参矩阵KP、相机(1)的内参矩
阵KC以及相机(1)与投影仪(2)之间的转换矩阵[RCP|tCP]，其中，RCP和tCP分别为旋转矩阵和
平移向量；
S32：将待装配工件设置在投影仪(2)与相机(1)的公共视场区域内，使投影仪(2)投影
的图像能投射到待装配工件的表面上，且相机(1)能拍摄到待装配工件，用计算机(4)生成
高频正弦条纹模板图像组和低频正弦条纹模板图像组，高频正弦条纹模板图像组和低频正
弦条纹模板图像组均有Np张图像，高频正弦条纹模板图像组中的图像的条纹频率为fh，低频
正弦条纹模板图像组中的图像的条纹频率为fl，fh＞fl，利用投影仪(2)分别依次将高频正
弦条纹模板图像组和低频正弦条纹模板图像组中的图像投射到待装配工件的表面上并用
相机(1)拍摄待装配工件，分别得到第一图像组和第二图像组；
S33：根据步骤S32中得到的第一图像组和第二图像组，得到待装配工件表面上的各像
素点在相机(1)的图像坐标系下的坐标(uc,vc)；
S34：根据步骤S32中得到的第一图像组和第二图像组，计算得到各像素点在相机(1)的
图像坐标系下的坐标(uc,vc)处的投影仪高频相对相位φh和低频相对相位φu，表达为：
${\mathrm{\φ}}_h=arctan\frac{-\underset{k=1}{\overset{N_p}{\Σ}}{c}_{kh}sin(2\mathrm{\π}\·(k-1)/N_p)}{\underset{k=1}{\overset{N_p}{\Σ}}{c}_{kh}\cos (2\mathrm{\π}\·(k-1)/N_p)}$
${\mathrm{\φ}}_u=arctan\frac{-\underset{k=1}{\overset{N_p}{\Σ}}{c}_{ku}sin(2\mathrm{\π}\·(k-1)/N_p)}{\underset{k=1}{\overset{N}{\Σ}}{c}_{ku}\cos (2\mathrm{\π}\·(k-1)/N_p)}$
其中，ckh为第一图像组中的第k张图像在坐标(uc,vc)处的灰度值，cku为第二张图像组
中的第k张图像在坐标(uc,vc)处的灰度值；
S35：根据步骤S34中得到的各像素点在相机(1)的图像坐标系下的坐标(uc,vc)处的投
影仪高频相对相位φh和低频相对相位φu，计算得到各像素点在相机(1)的图像坐标系下的
坐标(uc,vc)处的投影仪绝对相位φabs表达为：
${\mathrm{\φ}}_{abs}=\[\frac{(T_u/T_h)\·{\mathrm{\φ}}_u-{\mathrm{\φ}}_h}{2\mathrm{\π}}\]\·2\mathrm{\π}+{\mathrm{\φ}}_h$
其中，[*]为取整符号，Tu为低频正弦条纹模板图像组中的图像的条纹周期；Th为高频正
弦条纹模板图像组中的图像的条纹周期；
S36，根据步骤S35中得到的各像素点在相机(1)的图像坐标系下的坐标(uc,vc)处的投
影仪绝对相位φabs，得到各像素点在投影仪(2)的图像坐标系下的横坐标值up；
S37：各像素点在相机(1)坐标系下的坐标记为(XC,YC,ZC)，各像素点在相机(1)坐标系
下的坐标(XC,YC,ZC)和其对应的在相机(1)的图像坐标系下的坐标(uc,vc)满足第一等式：
$\left[\begin{array}{c}{u}_c\\ v_c\\ 1\end{array}\right]=\frac{1}{Z_C}{K}_C\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\end{array}\right]$
各像素点在投影仪坐标系下的坐标记为(XP,YP,ZP)，各像素点在投影仪坐标系下的坐
标(XP,YP,ZP)和其对应的在投影仪(2)的图像坐标系下的坐标(up,vp)满足第二等式：
$\left[\begin{array}{c}{u}_p\\ v_p\\ 1\end{array}\right]=\frac{1}{Z_P}{K}_P\left[\begin{array}{c}{X}_P\\ Y_P\\ Z_P\end{array}\right]$
各像素点在投影仪坐标系下的坐标(XP,YP,ZP)和其对应的在相机(1)坐标系下的坐标
(XC,YC,ZC)满足第三等式：
$\left[\begin{array}{c}{X}_P\\ Y_P\\ Z_P\end{array}\right]=R_{CP}\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\end{array}\right]+t_{CP}$
根据第一等式、第二等式和第三等式计算得到各像素点在相机(1)投影仪坐标系下的
三维坐标(XP,YP,ZP)，即得到所有锪窝的三维点云P2。
3.根据权利要求1所述的基于结构光三维测量系统的工件装配方法，其特征在于，步骤
S5包括步骤：
S51：分别计算Nh个连接孔的分布点云P1中的各点Q与Ng个锪窝的分布点云P3中的各点Q
的特征向量
LQ＝[t(1),t(2),…t(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]，
其中，t(j)为各连接孔的分布点云P1或各锪窝的分布点云P3中的第j个特征点集合
的距离平均值；θ(j)为各连接孔的分布点云P1或各锪窝的分布点云P3中的第j个特征
点集合内的所有特征点与点Q连线的夹角的平均值；τ(j)为各连接孔的分布点云P1或
各锪窝的分布点云P3中的各特征点集合内的重心位置G(j)所构成的集合{G(j)}内相邻
两个重心位置G(j)和G(j+1)与点Q所成向量的夹角，j＝1,2,...,N，i＝1,2,...,Nh；
S52：在各连接孔的分布点云P1中随机选择NC个点得到点集合{C}，在Ng个锪窝的分布点
云P3中，寻找与点集合{C}对应的点集合{D}，根据各连接孔的分布点云P1中的点集合{C}和
Ng个锪窝的分布点云P3中的与点集合{C}对应的点集合{D}，计算得到各连接孔的分布点云
P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H，从而得到Nh个连接孔的分布点
云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H。
4.根据权利要求3所述的基于结构光三维测量系统的工件装配方法，其特征在于，步骤
S51中的各连接孔的分布点云P1中的各点Q与各锪窝的分布点云P3中的各点Q的特征向量
LQ＝[t(1),t(2),…t(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]的计算过程包括步骤：
S511：对第i个连接孔的分布点云P1或第i个锪窝的分布点云P3中的一点Q，在分布点云
内搜索与点Q距离在(0,Nr]内的所有特征点，建立特征点集合r为
设定的距离分辨率，为正实数；N为设定的范围系数，为正整数，为满足下式的第j个特
征点集合：
j＝1,2,...,N，dist()为计算两个三维空间点的几何距离
的函数；
S512：对第j个特征点集合通过下式计算距离平均值t(j)：
$t^{\left(j\right)}=\frac{1}{n^{\left(j\right)}}\underset{i=1}{\overset{n^{\left(j\right)}}{\Σ}}dist(Q_i^{\left(j\right)},Q)$
其中n(j)为第j个特征点集合中的特征点总数；
S513：对第j个特征点集合通过下式计算集合内n(j)个特征点与点Q连线的夹角
的平均值θ(j)：
${\mathrm{\θ}}^{\left(j\right)}=\frac{1}{n^{\left(j\right)}\left(n^{\left(j\right)}-1\right)/2}\underset{l=1}{\overset{n^{\left(j\right)}}{\Σ}}\underset{m=l+1}{\overset{n^{\left(j\right)}}{\Σ}}angle\left(\stackrel{\‾}{{\mathrm{QQ}}_l^{\left(j\right)}},\stackrel{\‾}{{\mathrm{QQ}}_m^{\left(j\right)}}\right)$
其中，angle()为计算两个三维空间向量的夹角的函数，为点Q与集合内第l个特
征点所成向量；
S514：对第j个特征点集合通过下式计算重心位置G(j)：
$\stackrel{\‾}{{\mathrm{QG}}^{\left(j\right)}}=\frac{1}{n^{\left(j\right)}}\underset{l=1}{\overset{n^{\left(j\right)}}{\Σ}}\stackrel{\‾}{{\mathrm{QQ}}_l^{\left(j\right)}}$
其中，为点Q与重心位置G(j)所成向量；
S515：对各特征点集合的重心位置G(j)所构成的集合{G(j)}，计算集合{G(j)}内相
邻两个重心位置G(j)和G(j+1)与点Q所成向量的夹角τ(j)：
其中，当j＝N时，τ(N)＝0；
S516：对N个特征点集合进行步骤S512至S515，建立第i个连接
孔的分布点云P1或第i个锪窝的分布点云P3中的点Q的特征向量LQ：LQ＝[t(1),t(2),…t(N),
θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]，从而得到Nh个连接孔的分布点云P1中的各点Q与Ng个锪
窝的分布点云P3中的各点Q的特征向量
LQ＝[t(1),t(2),…t(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]。
5.根据权利要求3所述的基于结构光三维测量系统的工件装配方法，其特征在于，步骤
S52具体包括步骤：
S521：在第i个连接孔的分布点云P1中随机选择NC个点得到点集合{C}，对于点集合{C}
中的每一点cq，q＝1,...,NC，点cq的特征向量为在
Ng个锪窝的分布点云P3中，搜寻与每一点cq的特征向量距离σcd最小的点dq，组成点集合{D}，
点集合{D}作为点集合{C}的对应点集合；
S522：根据步骤S521中得到的点集合{C}和点集合{D}，计算得第i个连接孔的分布点云
P1和对应位置的锪窝的分布点云P3的转换矩阵HCD；
S523：根据步骤S522中得到的第i个连接孔的分布点云P1和对应位置的锪窝的分布点
云P3的转换矩阵HCD，计算第i个连接孔的分布点云P1与对应位置的锪窝的分布点云P3的配
准误差
${\mathrm{err}}_{H_{CD}}=ERROR(H_{cd}\·P1,P3)$
式中ERROR()为计算两点云相差程度的函数；
S524：重复步骤S521至S523M次，选择M次中配准误差最小的HCD，作为第i个连接孔
的分布点云P1与对应位置的锪窝的分布点云P3之间的粗略转换矩阵HCOR；
S525：根据步骤S524中得到的粗略转换矩阵HCOR，对第i个连接孔的分布点云P1进行转
换，获得估计点云PCOR：PCOR＝HCOR·P1
S526：利用迭代最近点ICP算法计算估计点云PCOR与对应位置的锪窝的分布点云P3之间
的转换矩阵HICP，进而计算第i个连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间
的转换矩阵H：H＝HICP·HCOR，从而得到Nh个连接孔的分布点云P1与对应位置的锪窝的分布
点云P3之间的位置转换矩阵H。
6.根据权利要求5所述的基于结构光三维测量系统的工件装配方法，其特征在于，在Ng
个锪窝的分布点云P3中，搜寻与点cq的特征向量距离σcd最小的点dq的计算过程为：
Ng个锪窝的分布点云P3中的每一点e的特征向量为
计算Ng个锪窝的分布点云P3中的每一点e与点cq的特征向量距离σcd，表达为：
${\mathrm{\σ}}_{cd}=\sqrt{\frac{1}{N}\left(\underset{j=1}{\overset{N}{\Σ}}{a}_t{(t_{c_q}^{\left(j\right)}-t_e^{\left(j\right)})}^2+a_{\mathrm{\θ}}{({\mathrm{\θ}}_{c_q}^{\left(j\right)}-{\mathrm{\θ}}_e^{\left(j\right)})}^2+a_{\mathrm{\τ}}{({\mathrm{\τ}}_{c_q}^{\left(j\right)}-{\mathrm{\τ}}_e^{\left(j\right)})}^2\right)}$
其中，at,aθ,aτ为权重系数；
比较Ng个锪窝的分布点云P3中的每一点e的特征向量距离σcd，特征向量距离σcd最小的
点e即为与点cq对应的点dq。

基于结构光三维测量系统的工件装配方法

技术领域

本发明涉及结构光三维测量系统，尤其涉及一种基于结构光三维测量系统的工件
装配方法。

背景技术

目前大型复杂结构的组装仍旧主要依赖工人查阅设计图纸与装配手册进行安装。
因此工人们在装配过程中，需要反复查看装配手册，寻找待装配工件上的每个连接孔对应
的连接件型号，因此导致现有的装配过程十分繁琐枯燥，时间成本高。而且在工人查阅手册
过程中，可能发生错误，将错误的连接件装配在连接孔里，造成待装配工件不可恢复的损
伤，使得装配质量下降。

为解决装配过程中工人反复查阅设计图纸导致的装配过程繁琐、耗时长、易发生
错误的问题，需要提出一种可以直接将连接件型号投影到待装配工件上的所需装配的位置
的工件装配方法。

发明内容

鉴于现有技术存在的缺陷，本发明的目的在于提供一种基于结构光三维测量系统
的工件装配方法，该方法能提高装配效率并减少发生错误的可能性。

为了实现上述目的，本发明提供了一种基于结构光三维测量系统的工件装配方
法，包括步骤：

S1：搭建包括相机、投影仪、支架以及计算机的结构光三维测量系统，相机与投影
仪均固定于支架上，使得相机与投影仪的相对位置固定；

S2：利用计算机导入待装配工件的三维模型，根据待装配工件的三维模型得到待
装配工件上的所有连接孔的连接孔信息，待装配工件上共有Nh个连接孔，将各连接孔的连
接孔信息转化为各连接孔的分布点云P1，其中各连接孔的连接孔信息包括各连接孔的三维
坐标和各连接孔对应的连接件型号，则各连接孔的分布点云P1包括各连接孔的三维坐标和
各连接孔对应的连接件型号；

S3：利用结构光三维测量系统基于NP步相移法测量原理得到与步骤S2中的待装配
工件上的连接孔对应的在结构光三维测量系统中的所有锪窝的三维点云P2，其中所有锪窝
的三维点云P2包括所有锪窝的三维坐标，共有Ng个锪窝，其中，Ng≤Nh；

S4：根据步骤S3中得到的待装配工件上的所有锪窝的三维点云P2，利用点云聚类
提取算法，对所有锪窝的三维点云P2，将属于同一锪窝的点归为一组，并计算各锪窝的点云
的重心坐标，从而得到待装配工件上的Ng个锪窝的分布点云P3，Ng个锪窝的分布点云P3包括
Ng个锪窝的重心坐标；

S5：对待装配工件上的各连接孔的分布点云P1，配准对应位置的锪窝的分布点云
P3，获得Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H；

S6：根据步骤S2中得到的Nh个连接孔的分布点云P1、步骤S4中得到的Ng个锪窝的分
布点云P3以及步骤S5中得到的Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3
之间的位置转换矩阵H，得到Nh个连接孔的分布点云P1在结构光三维测量系统中的位置坐
标；

S7：根据步骤S6中得到的Nh个连接孔的分布点云P1在结构光三维测量系统中的位
置坐标，将各连接孔的连接件型号信息投影给对应位置的锪窝。

本发明的有益效果如下：

在根据本发明的基于结构光三维测量系统的工件装配方法中，待装配工件上的连
接孔的分布点云P1为连接件在待装配工件上的理论装配位置，锪窝的分布点云P3为连接件
在待装配工件上的实际装配位置，根据连接孔的分布点云P1与对应位置的锪窝的分布点云
P3之间的位置转换矩阵H将连接孔的位置与锪窝的位置对应，即是将理论装配位置和实际
装配位置对应，从而将待装配工件上的理论装配位置的连接件类型投影到实际装配位置
上，提高了连接件装配到待装配工件上的连接孔的装配效率并减少发生错误的可能性。

附图说明

图1是根据本发明的基于结构光三维测量系统的工件装配方法中的结构光三维测
量系统的示意图；

图2是根据本发明的基于结构光三维测量系统的工件装配方法中的正弦条纹模板
图像组中的一张图像。

其中，附图标记说明如下：

1 相机

2 投影仪

3 支架

4 计算机

具体实施方式

下面参照附图来详细说明根据本发明的基于结构光三维测量系统的工件装配方
法。

参照图1和图2，本发明的基于结构光三维测量系统的工件装配方法包括步骤：

S1：搭建包括相机1、投影仪2、支架3以及计算机4的结构光三维测量系统，相机1与
投影仪2均固定于支架3上，使得相机1与投影仪2的相对位置固定；

S2：利用计算机4导入待装配工件的三维模型，根据待装配工件的三维模型得到待
装配工件上的所有连接孔的连接孔信息，待装配工件上共有Nh个连接孔，将各连接孔的连
接孔信息转化为各连接孔的分布点云P1，其中各连接孔的连接孔信息包括各连接孔的三维
坐标和各连接孔对应的连接件型号，则各连接孔的分布点云P1包括各连接孔的三维坐标和
各连接孔对应的连接件型号；

S3：利用结构光三维测量系统基于NP步相移法测量原理得到与步骤S2中的待装配
工件上的连接孔对应的在结构光三维测量系统中的所有锪窝的三维点云P2，其中所有锪窝
的三维点云P2包括所有锪窝的三维坐标，共有Ng个锪窝，其中，Ng≤Nh；

S4：根据步骤S3中得到的待装配工件上的所有锪窝的三维点云P2，利用点云聚类
提取算法，对所有锪窝的三维点云P2，将属于同一锪窝的点归为一组，并计算各锪窝的点云
的重心坐标，从而得到待装配工件上的Ng个锪窝的分布点云P3，Ng个锪窝的分布点云P3包括
Ng个锪窝的重心坐标；

S5：对待装配工件上的各连接孔的分布点云P1，配准对应位置的锪窝的分布点云
P3，获得Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H；

S6：根据步骤S2中得到的Nh个连接孔的分布点云P1、步骤S4中得到的Ng个锪窝的分
布点云P3以及步骤S5中得到的Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3
之间的位置转换矩阵H，得到Nh个连接孔的分布点云P1在结构光三维测量系统中的位置坐
标；

S7：根据步骤S6中得到的Nh个连接孔的分布点云P1在结构光三维测量系统中的位
置坐标，将各连接孔的连接件型号信息投影给对应位置的锪窝。

在根据本发明的基于结构光三维测量系统的工件装配方法中，待装配工件上的连
接孔的分布点云P1为连接件在待装配工件上的理论装配位置，锪窝的分布点云P3为连接件
在待装配工件上的实际装配位置，根据连接孔的分布点云P1与对应位置的锪窝的分布点云
P3之间的位置转换矩阵H将连接孔的位置与锪窝的位置对应，即是将理论装配位置和实际
装配位置对应，从而将待装配工件上的理论装配位置的连接件类型投影到实际装配位置
上，提高了连接件装配到待装配工件上的连接孔的装配效率并减少发生错误的可能性。

在这里补充说明的是，步骤S2中根据待装配工件的三维模型得到待装配工件上的
所有连接孔的连接孔信息，从而转化为连接孔的分布点云P1，得到的连接孔的分布点云P1
是连接件在待装配工件上的理论装配位置，步骤S3中利用结构光三维测量系统基于NP步相
移法测量原理得到的在结构光三维测量系统中的所有锪窝的三维点云P2是连接件在待装
配工件上的实际装配位置，将连接孔的连接件类型投影到对应锪窝的位置则是将待装配工
件上的理论装配位置对应到实际装配位置上。

步骤S5中获得Nh个连接孔的分布点云P1与对应位置的锪窝的分布点云P3之间的
位置转换矩阵H时，因为Ng≤Nh，可能得到的是Nh个连接孔中的一部分连接孔与对应位置的
锪窝的分布点云P3之间的位置转换矩阵H，从而先得到Nh个连接孔中的一部分连接孔的分
布点云P1在结构光三维测量系统中的位置坐标，因为待装配工件上的Nh个连接孔的相对位
置关系是确定的，则通过Nh个连接孔中的一部分连接孔的分布点云P1在结构光三维测量系
统中的位置坐标可得到Nh个连接孔的分布点云P1在结构光三维测量系统中的位置坐标。

步骤S4中根据步骤S3中得到的待装配工件上的所有锪窝的三维点云P2，利用点云
聚类提取算法，对所有锪窝的三维点云P2，将属于同一锪窝的点归为一组，并计算各锪窝的
点云的重心坐标，从而得到待装配工件上的Ng个锪窝的分布点云P3，这种方法为常规方法，
根据该算法，可以将锪窝的三维点云P2分成不同的组，其中将点之间的距离小于设定阈值
的点划分为一组，识别为一个锪窝，分组完成后，对于每一组的锪窝点云，计算点云的重心
坐标，加入到锪窝的分布点云中，从而形成锪窝的分布点云P3，实现过程为：

1.所有锪窝的三维点云P2共有NP2个点，对所有锪窝的三维点云P2，建立聚类组集
合A和待处理点序列B；

2.对所有锪窝的三维点云P2中每个点ai2∈P2，i2＝1,...,NP2，进行如下操作：

2.1将所有锪窝的三维点云P2中每个点ai2加入到待处理点序列B中；

2.2对待处理点序列B中的每个点ai2，在所有锪窝的三维点云P2中搜索与ai2距离
小于所设阈值rd的所有点，构成点集合B′i2，点集合B′i2中共有NB个点；

2.3对点集合B′i2中的每个点k1＝1,...,NB，判断是否被处理过，如果没有，
则将加入到待处理点序列B中；

2.4当待处理点序列B中的每个点都被处理过后，将待处理点序列B加入到聚类组
集合A中，得到一个锪窝点云，并将待处理点序列B设置为空集；

3.当所有锪窝的三维点云P2中的每个点ai2均被处理并被加入到聚类组集合C中，
算法结束，每处理一次得到的一个聚类组集合C即为一组锪窝点云，从而构成Ng个锪窝的分
布点云P3。

具体可参见Radu Bogdan Rusu的论文“Semantic 3D Object Maps for Everyday
Manipulation in Human Living Environments[D].TechnischeMünchen,
2009.”。

在根据本发明的基于结构光三维测量系统的工件装配方法中，步骤S3中利用结构
光三维测量系统基于NP步相移法测量原理得到与步骤S2中的待装配工件上的连接孔对应
的在结构光三维测量系统中的所有锪窝的三维点云P2包括步骤：

S31：对结构光三维测量系统进行标定，得到投影仪2的内参矩阵KP、相机1的内参
矩阵KC以及相机1与投影仪2之间的转换矩阵[RCP|tCP]，其中，RCP和tCP分别为旋转矩阵和平
移向量；

S32：将待装配工件设置在投影仪2与相机1的公共视场区域内，使投影仪2投影的
图像能投射到待装配工件的表面上，且相机1能拍摄到待装配工件，用计算机4生成高频正
弦条纹模板图像组和低频正弦条纹模板图像组，高频正弦条纹模板图像组和低频正弦条纹
模板图像组均有Np张图像，高频正弦条纹模板图像组中的图像的条纹频率为fh，低频正弦条
纹模板图像组中的图像的条纹频率为fl，fh＞fl，利用投影仪2分别依次将高频正弦条纹模
板图像组和低频正弦条纹模板图像组中的图像投射到待装配工件的表面上并用相机1拍摄
待装配工件，分别得到第一图像组和第二图像组；

S33：根据步骤S32中得到的第一图像组和第二图像组，得到待装配工件表面上的
各像素点在相机1的图像坐标系下的坐标(uc,vc)；

S34：根据步骤S32中得到的第一图像组和第二图像组，计算得到各像素点在相机1
的图像坐标系下的坐标(uc,vc)处的投影仪高频相对相位φh和低频相对相位φu，表达为：

其中，ckh为第一图像组中的第k张图像在坐标(uc,vc)处的灰度值，cku为第二张图
像组中的第k张图像在坐标(uc,vc)处的灰度值；

S35：根据步骤S34中得到的各像素点在相机1的图像坐标系下的坐标(uc,vc)处的
投影仪高频相对相位φh和低频相对相位φu，计算得到各像素点在相机1的图像坐标系下的
坐标(uc,vc)处的投影仪绝对相位φabs表达为：

其中，[*]为取整符号，Tu为低频正弦条纹模板图像组中的图像的条纹周期；Th为高
频正弦条纹模板图像组中的图像的条纹周期；

S36，根据步骤S35中得到的各像素点在相机1的图像坐标系下的坐标(uc,vc)处的
投影仪绝对相位φabs，得到各像素点在投影仪2的图像坐标系下的横坐标值up；

S37：各像素点在相机1坐标系下的坐标记为(XC,YC,ZC)，各像素点在相机1坐标系
下的坐标(XC,YC,ZC)和其对应的在相机1的图像坐标系下的坐标(uc,vc)满足第一等式：

各像素点在投影仪坐标系下的坐标记为(XP,YP,ZP)，各像素点在投影仪坐标系下
的坐标(XP,YP,ZP)和其对应的在投影仪2的图像坐标系下的坐标(up,vp)满足第二等式：

各像素点在投影仪坐标系下的坐标(XP,YP,ZP)和其对应的在相机1坐标系下的坐
标(XC,YC,ZC)满足第三等式：

根据第一等式、第二等式和第三等式计算得到各像素点在相机1投影仪坐标系下
的三维坐标(XP,YP,ZP)，即得到所有锪窝的三维点云P2。

在这里补充说明的是，步骤S31中对结构光三维测量系统进行标定是常规方法，将
标定用的标定板(未示出)放置于投影仪2和相机1的视场范围内，标定板(未示出)上设置有
多个标志点，多次改变标定板(未示出)的位姿，可得到不同位姿下的标定板(未示出)上的
多个标志点在世界坐标系下的坐标，利用投影仪2将测量用的模板图像投射到不同位姿下
的标定板(未示出)上并用相机1拍摄标定板(未示出)，利用计算机4处理用相机1拍摄得到
的图像，根据从相机1拍摄得到的图像得到标定板上的多个标志点在相机的图像坐标系下
的坐标和强度值，根据标定板上的多个标志点的强度值解码得到标定板上的多个标志点的
相位，进而计算得到标定板上的多个标志点在投影仪的图像坐标系下的坐标，根据得到的
标定板上的多个标志点在世界坐标系下的坐标、标定板上的多个标志点在相机的图像坐标
系下的坐标以及标定板上的多个标志点在投影仪的图像坐标系下的坐标利用OpenCV或者
Matlab相机双目标定工具箱即可得到投影仪2的内参矩阵KP、相机1的内参矩阵KC、投影仪2
和相机1之间的转换矩阵[RCP|tCP]。其中，世界坐标系是指原点在标定板(未示出)的左上角
的坐标系，坐标单位是毫米(mm)；相机坐标系是指原点在相机光心的坐标系，坐标单位是毫
米(mm)；相机的图像坐标系是指原点在相机成像平面上的图像的左上角的坐标系，坐标单
位是像素(pixel)；投影仪坐标系是指原点在投影仪光心的坐标系，坐标单位是毫米(mm)；
投影仪的图像坐标系是指原点在投影仪成像平面上的图像的左上角的坐标系坐标单位是
像素(pixel)；投影仪2的内参矩阵KP和相机1的内参矩阵KC分别包括投影仪2和相机1的水平
方向和竖直方向的焦距、图像的主点，表达了相机坐标系和投影仪坐标系分别与相机的图
像坐标系和投影仪的图像坐标系之间的变换。投影仪2和相机1之间的转换矩阵[RCP|tCP]包
括旋转矩阵RCP和平移向量tCP，表达了投影仪坐标系和相机坐标系之间的变换。对结构光三
维测量系统进行标定具体可参见Chen Rui的论文“Rui Chen,Jing Xu,Heping Chen,
Jianhua Su,Zonghua Zhang,Ken Chen.Accurate calibration method for camera and
projector in fringe patterns measurement system[J].Applied Optics,2016,55
(16):4293-4300.”。

在根据本发明的基于结构光三维测量系统的工件装配方法中，步骤S5包括步骤：

S51：分别计算Nh个连接孔的分布点云P1中的各点Q与Ng个锪窝的分布点云P3中的
各点Q的特征向量

LQ＝[t(1),t(2),…t(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]，

其中，t(j)为各连接孔的分布点云P1或各锪窝的分布点云P3中的第j个特征点集合
的距离平均值；θ(j)为各连接孔的分布点云P1或各锪窝的分布点云P3中的第j个特征
点集合内的所有特征点与点Q连线的夹角的平均值；τ(j)为各连接孔的分布点云P1或
各锪窝的分布点云P3中的各特征点集合内的重心位置G(j)所构成的集合{G(j)}内相邻
两个重心位置G(j)和G(j+1)与点Q所成向量的夹角，j＝1,2,...,N，i＝1,2,...,Nh；

S52：在各连接孔的分布点云P1中随机选择NC个点得到点集合{C}，在Ng个锪窝的分
布点云P3中，寻找与点集合{C}对应的点集合{D}，根据各连接孔的分布点云P1中的点集合
{C}和Ng个锪窝的分布点云P3中的与点集合{C}对应的点集合{D}，计算得到各连接孔的分
布点云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H，从而得到Nh个连接孔的
分布点云P1与对应位置的锪窝的分布点云P3之间的位置转换矩阵H。

在根据本发明的基于结构光三维测量系统的工件装配方法中，步骤S51中的各连
接孔的分布点云P1中的各点Q与各锪窝的分布点云P3中的各点Q的特征向量LQ＝[t(1),t
(2),…t(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]的计算过程包括步骤：

S511：对第i个连接孔的分布点云P1或第i个锪窝的分布点云P3中的一点Q，在分布
点云内搜索与点Q距离在(0,Nr]内的所有特征点，建立特征点集合r
为设定的距离分辨率，为正实数；N为设定的范围系数，为正整数，为满足下式的第j个
特征点集合：

j＝1,2,...,N，dist()为计算两个三维空间点的几何
距离的函数；

S512：对第j个特征点集合通过下式计算距离平均值t(j)：

其中n(j)为第j个特征点集合中的特征点总数；

S513：对第j个特征点集合通过下式计算集合内n(j)个特征点与点Q连线的
夹角的平均值θ(j)：

其中，angle()为计算两个三维空间向量的夹角的函数，为点Q与集合内第l
个特征点所成向量；

S514：对第j个特征点集合通过下式计算重心位置G(j)：

其中，为点Q与重心位置G(j)所成向量；

S515：对各特征点集合的重心位置G(j)所构成的集合{G(j)}，计算集合{G(j)}
内相邻两个重心位置G(j)和G(j+1)与点Q所成向量的夹角τ(j)：

其中，当j＝N时，τ(N)＝0；

S516：对N个特征点集合进行步骤S512至S515，建立第i个
连接孔的分布点云P1或第i个锪窝的分布点云P3中的点Q的特征向量LQ：LQ＝[t(1),t(2),…t
(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]，从而得到Nh个连接孔的分布点云P1中的各点Q与Ng
个锪窝的分布点云P3中的各点Q的特征向量

LQ＝[t(1),t(2),…t(N),θ(1),θ(2),…,θ(N),τ(1),τ(2),…,τ(N)]。

在根据本发明的基于结构光三维测量系统的工件装配方法中，步骤S52具体包括
步骤：

S521：在第i个连接孔的分布点云P1中随机选择NC个点得到点集合{C}，对于点集
合{C}中的每一点cq，q＝1,...,NC，点cq的特征向量为
在Ng个锪窝的分布点云P3中，搜寻与每一点cq的特征向量距离σcd最小的点dq，组成点集合
{D}，点集合{D}作为点集合{C}的对应点集合；

S522：根据步骤S521中得到的点集合C和点集合D，计算得第i个连接孔的分布点云
P1和对应位置的锪窝的分布点云P3的转换矩阵HCD；

S523：根据步骤S522中得到的第i个连接孔的分布点云P1和对应位置的锪窝的分
布点云P3的转换矩阵HCD，计算第i个连接孔的分布点云P1与对应位置的锪窝的分布点云P3
的配准误差

式中ERROR()为计算两点云相差程度的函数；

S524：重复步骤S521至S523M次，选择M次中配准误差最小的HCD，作为第i个连
接孔的分布点云P1与对应位置的锪窝的分布点云P3之间的粗略转换矩阵HCOR；

S525：根据步骤S524中得到的粗略转换矩阵HCOR，对第i个连接孔的分布点云P1进
行转换，获得估计点云PCOR：PCOR＝HCOR·P1

S526：利用迭代最近点ICP算法计算估计点云PCOR与对应位置的锪窝的分布点云P3
之间的转换矩阵HICP，进而计算第i个连接孔的分布点云P1与对应位置的锪窝的分布点云P3
之间的转换矩阵H：H＝HICP·HCOR，从而得到Nh个连接孔的分布点云P1与对应位置的锪窝的
分布点云P3之间的位置转换矩阵H。

在这里补充说明的是，步骤522中根据步骤S521中得到的点集合C和点集合D，计算
得第i个连接孔的分布点云P1和对应位置的锪窝的分布点云P3的转换矩阵HCD的方法的实现
过程为：

对点集合C＝{cq}与点集合D＝{dq},其中点集合C＝{cq}中的点的数量NC与点集合
D＝{dq}中的点的数量ND相同，并且序号相同的点相互对应，即：cq与dq对应；点集合C＝{cq}
与点集合D＝{dq}之间的转换矩阵HCD＝[RCD|tCD]即为第i个连接孔的分布点云P1和对应位
置的锪窝的分布点云P3的转换矩阵HCD＝[RCD|tCD]，其中RCD为旋转矩阵，tCD为平移向量；转
换矩阵HCD＝[RCD|tCD]的计算过程如下：

1.利用下式计算点集合C＝{cq}与点集合D＝{dq}的重心位置cg和dg：

2.将点集合C＝{cq}与点集合D＝{dq}中的所有点减去重心位置如下式：

cq＝cq-cg，dq＝dq-dg

3.点集合C＝{cq}与点集合D＝{dq}转换后的误差函数E为：

4.令J的奇异值分解(Sigular Value Decomposition，简称SVD)矩阵
分解为U和V，则当RCD＝VUT时，误差函数E最小，求出旋转矩阵RCD；

5.则平移向量tCD计算为：tCD＝qg-RCDpg

以及步骤S526中利用迭代最近点ICP算法计算估计点云PCOR与对应位置的锪窝的
分布点云P3之间的转换矩阵HICP的方法的实现过程为：

(1)对估计点云PCOR中的每一个点，在锪窝的分布点云P3中寻找与其距离最近的
点，作为对应点，构成点集合

(2)利用奇异值分解矩阵方法计算估计点云PCOR与点集合之间的转换矩阵HK；

(3)利用转换矩阵HK对估计点云PCOR进行转换，即：PCOR＝HKPCOR；

(4)重复步骤(1)至(3)，直到估计点云PCOR与点集合之间转换后的误差小于设定
的阈值，则最终的转换矩阵H为之前进行n1次转换得到的所有转换矩阵HK1,...,HKn1-1,HKn1
的乘积，即为：

H＝HKn1×HKn1-1×…×HK1

计算得第i个连接孔的分布点云P1和对应位置的锪窝的分布点云P3的转换矩阵HCD
的方法以及步骤S526中利用迭代最近点ICP算法计算点云PCOR与对应位置的锪窝的分布点
云P3之间的转换矩阵HICP的方法具体可参见Paul J的论文“Method for registration of
3-D shapes.Robotics-DL tentative.International Society for Optics and
Photonics,1992.”。

在根据本发明的基于结构光三维测量系统的工件装配方法中，在Ng个锪窝的分布
点云P3中，搜寻与点cq的特征向量距离σcd最小的点dq的计算过程为：

Ng个锪窝的分布点云P3中的每一点e的特征向量为
计算Ng个锪窝的分布点云P3中的每一点e与点cq的特征向量距离σcd，表达为：

其中，at,aθ,aτ为权重系数；

比较Ng个锪窝的分布点云P3中的每一点e的特征向量距离σcd，特征向量距离σcd最
小的点e即为与点cq对应的点dq。