一种三维点云的实时采集装置及机器人打磨轨迹规划方法与流程

本发明应用于三维点云的获取及机器人打磨，既涉及基于激光位移传感器和机器人之间实时通信获取三维点云的装置，也涉及机器人基于三维点云的打磨轨迹规划方法，提供了一套新的三维点云获取方法，为机器人运用于打磨领域提供了新的试验方法和装置。

背景技术：

当今，三维点云的获取设备主要有三维激光扫描仪，二维线激光位移传感器加辅助运动，上述两种方法能够获取点数巨大的密集点云。另外也可以通过三坐标测量仪获取点云，这种方法获取的点云位置精度高，为点数较少的稀疏点云。这些设备价格昂贵，使得点云获取成本较高，随着消费级产品的出现，比如kinect，使得点云技术得到较快的发展。但是上述方法获取的点云会包含目标物体所处的环境信息，需要对原始点云进行滤波、去噪、分割和配准等处理，才能获取目标物的点云，后期处理复杂。

点云主要运用于逆向工程，进行现有物体的三维重构，为其创建数字化信息库；在机器视觉领域，点云可用于目标识别、目标三维检测和地图构建等方向，为机器人提供视觉信息，辅助其决策和路径规划。

机器人技术已广泛运用于焊接、喷涂等领域，但是复杂形状的工件打磨主要还是依靠人工打磨和数控机床加工，前者因为打磨噪声、粉尘大，有损工人健康，且对工人的技术水平要求较高；后者加工精度高，但加工设备价格昂贵，柔性较差限制其广泛应用。

工业机器人具有多自由度，灵活性好，较适合运用于打磨领域。目前，采用机器人离线编程技术，已成功将机器人技术运用于复杂工件打磨，但是离线编程技术要求提前对工具和工件坐标系进行标定，且由于工件的装夹误差和加工不一致性，导致机器人打磨一批工件时，出现精度不一致情况，甚至出现废次品。

技术实现要素：

为解决三维点云的获取问题及将点云运用于机器人打磨领域，本发明提供了一种基于激光位移传感器的三维点云的实时采集装置及机器人打磨轨迹规划方法。设计不同形状的工件进行实验，规划机器人扫描轨迹，基于总线通信协议和实时操作系统，通过总线通信方式实现机器人和激光位移传感器的实时通信，获取工件三维点云，为将三维点云运用于机器人打磨轨迹规划提供了外在条件。

为实现本发明的上述目的，本发明一方面提供了一种三维点云的实时采集装置，包括机器人、激光位移传感器、基于实时操作系统的机器人实时控制系统，所述激光位移传感器通过夹具设置在所述机器人的末端，所述机器人为六自由度关节机器人，所述机器人实时控制系统通过实时工业以太网总线连接机器人和激光位移传感器，用于使激光位移传感器的读数和机器人的位姿得到同步，将一维测量扩展为三维测量，从而通过扫描获取工件的三维点云。

进一步地，所述的机器人实时控制系统与机器人和激光位移传感器之间为毫秒级通信。

本发明另一方面提供了一种基于所述装置的机器人打磨轨迹规划方法，包括步骤：

(1)通过多点标定法标定激光位移传感器的测量坐标系相对于机器人末端坐标系的转换矩阵；

(2)机器人驱动激光位移传感器扫描工件，实现将一维测量扩展为三维测量，获取工件的三维点云；

(3)对所述三维点云进行法向估计，同时基于三维点云包含的空间位置信息和法向信息规划机器人打磨时的位姿和走刀路径，并以扫描轨迹加以修正作为机器人的打磨轨迹。

进一步地，所述步骤(1)具体包括：

(11)首先在机器人末端安装一个标定顶尖，控制机器人到达空间某一固定点，采用机器人工具坐标系五点标定法，工具坐标系标定完成后从示教器读出该固定点相对于机器人基坐标系的空间位置^bp；

(12)然后在机器人末端装夹激光位移传感器，控制机器人将激光照射在所述固定点，读出机器人当前末端相对于基坐标系的位姿及激光位移传感器的示数^mp，所述激光位移传感器的示数^mp和所述空间位置^bp的转换关系为：

式中^bp——空间点p在机器人基坐标系的坐标；

^mp——空间点p在测量坐标系的坐标；

——机器人末端坐标系相对于基坐标系的齐次变换矩阵；

——测量坐标系相对于机器人末端坐标系的齐次变换矩阵，其中，

(13)将式(1)转换为设则(1)可以表示如下：

(14)基于多点标定，将式(3)转化成ax＝k的形式，构成一个超定方程组，并采用最小二乘来求解，形式如下：

使最小的解x^*即为超定方程组ax＝k的最小二乘解；

(15)求解出x后即得到矩阵得到矩阵后，即可以将扫描的工件三维点云转换到机器人基坐标系下。

进一步地，步骤(2)中，所述将一维测量扩展为三维测量的步骤是通过机器人实时控制系统，工件点云的x，y位置由机器人末端位姿可知，工件点云的z位置由激光位移传感器可知，通过实时总线通讯技术，保证x、y、z的同步，从而实现将一维测量扩展为三维测量。

进一步地，所述的步骤(3)具体包括步骤：

(31)基于三维点云的每个点的空间位置信息及优化调整后的法向信息规划机器人打磨位姿；

(32)定义点云的加工节点，并以扫描先后顺序依次将加工节点信息发送给机器人，机器人进行逆运动学求解和插补运动将加工节点串联生成一条连续的机器人打磨轨迹。

进一步地，所述的步骤(31)具体包括步骤：

(301)确定打磨时刀具的位置，即利用三维点云中各点所包含的基于机器人基坐标系的空间位置信息；通过机器人逆运动学控制机器人使得刀具到达打磨时的位置；

(302)确定打磨时刀具的姿态，即基于三维点云的每个点的预估法向信息及优化调整后的法向信息规划机器人打磨时刀具的姿态。

进一步地，所述的步骤(302)具体包括步骤：

(311)通过改进的k邻域搜索算法搜索k个距离最近的点，即将一个点作为体心，以固定步长构建自身的正方体，然后在正方体内进行k邻域搜索，若找到k个距离最近的点，即终止搜索，否则以固定步长扩大正方体的范围，重新搜索k个距离最近的点，直至在正方体内搜索到距离最近的k个点；

(312)采用改进的pca算法，通过对某点的k邻域内的点赋予不同的权重函数，采用最小二乘拟合法拟合出一个平面，将所拟合平面的法向作为该点的初始法向；接着基于所述初始法向自适应的确定点云中各点的权重函数，再对初始法向进行优化；

(313)点云重定向，即采用最小生成树法对优化后的法向进行调整，使得点云法向指向一致。

进一步地，所述的步骤(312)具体包括步骤：

(321)在pca算法中引入两个权重函数使得拟合方法对点云中的外点和噪声具有一定的鲁棒性，最小二乘拟合可表示为：

s.t.||n||＝1(5)

式中ωd(xi)——高斯权重函数，使得距离该点近的点对拟合平面的作用大一点，反之影响作用小，ωd(xi)表示为：σd为距离带宽，其初值根据工件的形状及扫描点云的分布情况预先设定；

ωr(ri)——与拟合残差有关的高斯权重函数，ωr(ri)表示为：σr为拟合残差带宽，其初值根据工件的形状及扫描点云的分布情况预先设定；

ri^m——第m次迭代，xi点的拟合残差，ri^m可表示为：ri^m＝d^m+(xi-x)^tn^m；

(322)求解式(5)构造协方差矩阵c，表示如下：

式中——表示邻域的中心，表示为：

k——表示邻域内的点数，

通过求解协方差矩阵c，其最小的特征值对应的特征向量即为拟合平面的法向n，即作为该点的初始法向，且

(323)为点云中各个点定义一个特征系数fc来量化该点是位于平滑区域、尖锐特征区域或者过渡区域，特征系数可表示为：

式中s＝2，表示当比例为0.5时，则判定该点位于尖锐特征区域；

δl——表示该点与其距离最近的6个点的平均距离；

(324)确定特征系数fc的取值先定义点云中某一点邻域内的点云簇，当某一点云簇内的点数大于邻域内的点数与点云簇的数量之商，则认为该点云簇为主点云簇；为了自动的获得点云簇的数量，设置一个角度阈值，当两点的法向夹角大于阈值时，即认为该两点位于不同的点云簇内；得到点云簇的数量，从而计算出各点对应的特征系数fc，a，b两点的法向夹角可表示为：

d(a,b)＝cos^-1(na,nb)；(8)

(325)确定fc的值后，自适应的计算出各个点的距离带宽σd、拟合残差带宽σr、法向差异带宽σn，表示如下：

σd＝(1+fc)·δl(9)

σr＝rmax/3fc(10)

σn＝||ni-n||max/3fc；(11)

(326)在pca算法中引入第三个权重函数，则最小二乘拟合表示为：

s.t.||n||＝1(12)

式中ωn(ni)——与法向差异有关的权重函数，表示为：σn为法向差异带宽，ni为某点初始法向，由式(6)求解所得；

(327)通过步骤(325)中自适应计算出的各个点的距离带宽σd、拟合残差带宽σr、法向差异带宽σn计算点云中每个点的权重函数，替换协方差矩阵c中的

然后通过替换后的协方差矩阵c对式(12)求解，得到每个点的优化法向。

进一步地，所述步骤(313)具体包括步骤：

(331)选取点云中z坐标最大的点作为根结点，调整其法向使之与矢量(0,0,1)的点积大于0，这将使调整后的法向方向指向工件外侧；

(332)设置权重c＝1-|ni·nj|，这样保证了该权值非负，且当c越小表示两邻近点的法向方向越平行；

(333)根据权重c的大小进行遍历，若父结点ni与子结点nj满足ni·nj<0，则将nj反向；

(334)遍历所有结点，将法向方向进行传播扩散，传播过程中设置一个阈值e，当c≤e时，一次扩散至邻域内多点，当c>e时，一次扩散一个点，完成后判断是否满足c≤e，再进行多点扩散，直至扩散到整个点云空间；

(335)若未能遍历所有的点，则点云不连通，部分数据形成局部孤岛，如果未遍历点数个数少于点云总数的千分之一，则将其作为噪声舍弃，否则查找未遍历的数据集中与已遍历点之间距离最近的点，将其作为继续遍历的起点并转至步骤(334)。

相比现有技术，本发明通过对扫描点云进行法向估计，基于工件三维点云的位置信息和法向信息来规划机器人打磨时的位姿，并以扫描轨迹加以修正作为机器人的打磨轨迹，稳定可靠、精度高、具有实时性和柔性特点，且结构简单成本低，能够满足不同工件的加工。

附图说明

图1是本发明的点云实时采集装置示意图；

图2是标定装置示意图。

图中：1-机器人，2-激光位移传感器；3-机器人实时控制系统；4-工件；5-标定顶尖。

具体实施方式

为进一步理解本发明，下面结合附图和实施例对本发明做进一步说明，但是需要说明的是，本发明要求保护的范围并不局限于实施例表述的范围。

实施例

如图1所示，一种三维点云的实时采集装置，包括机器人1、激光位移传感器2、基于实时操作系统的机器人实时控制系统3，所述激光位移传感器2通过夹具设置在所述机器人1的末端，所述机器人1为六自由度关节机器人，所述机器人实时控制系统3通过实时工业以太网总线连接机器人1和激光位移传感器2，用于使激光位移传感器2的读数和机器人1的位姿得到同步，将一维测量扩展为三维测量，从而通过扫描获取工件4的三维点云。

所述的机器人实时控制系统与机器人1和激光位移传感器2之间为毫秒级通信。

激光扫描时，以示教再现的方式控制六轴垂直机器人运动，带着激光位移传感器2扫描工件4，基于本发明装置实时的采集工件4上的点，最终得到基于机器人基坐标系的工件三维点云。点云实时采集装置如图1所示。

基于实时操作系统和工业以太网总线技术构建此装置的机器人实时控制系统3，工件点云的x，y位置由机器人末端位姿可知，工件点云的z位置由激光位移传感器2可知，通过实时总线通讯技术，保证x、y、z的同步，从而实现将一维测量扩展为三维测量，且该控制系统的通信周期可达1ms，满足实时性要求，能够使用该装置扫描生成工件的三维点云。

所示机器人1末端装夹激光位移传感器2，由模拟量输入输出模块读取激光位移传感器的模拟量输出信号并与机器人的六个控制电机的脉冲信号串联，再基于工业以太网总线技术的实时操作系统通过总线通信方式将所有信号采集回来进行数据处理，通信周期能达到毫秒级，从而实现机器人的位姿和激光位移传感器的读数同步，将机器人1位姿和激光位移传感器2距离通过机器人变换方程求解，得到机器人基坐标系下的工件三维点云。

本实施例利用solidworks软件设计不同的表面形状的工件，用于扫描测试和打磨实验。本实验设计s形曲面工件，采用铝合金，并加工出来，用于测试实验。

一种基于所述装置的机器人打磨轨迹规划方法，包括步骤：

(1)通过多点标定法标定激光位移传感器2的测量坐标系相对于机器人1末端坐标系的转换矩阵，为了将测量坐标系下的点云转换到机器人基坐标系下，须提前标定激光位移传感器2的测量坐标系相对于机器人末端坐标系的转换矩阵，简称手眼标定；

(2)机器人1驱动激光位移传感器2扫描工件，实现将一维测量扩展为三维测量，获取工件的三维点云；

(3)通过对三维点云中各点设置不同的自适应权重函数对所述三维点云进行法向估计和优化，同时基于三维点云包含的空间位置信息和法向信息规划机器人1打磨时的位姿和走刀路径，并以扫描轨迹加以修正作为机器人1的连续打磨轨迹。

具体而言，所述步骤(1)具体包括：

(11)首先在机器人1末端安装一个标定顶尖5，控制机器人1到达空间某一固定点，采用机器人1工具坐标系五点标定法，工具坐标系标定完成后从示教器读出该固定点相对于机器人1基坐标系的空间位置^bp；

(12)然后在机器人1末端装夹激光位移传感器2，控制机器人1将激光照射在所述固定点，读出机器人1当前末端相对于基坐标系的位姿及激光位移传感器2的示数^mp，所述激光位移传感器2的示数^mp和所述空间位置^bp的转换关系为：

式中^bp——空间点p在机器人1基坐标系的坐标；

^mp——空间点p在测量坐标系的坐标；

——机器人1末端坐标系相对于基坐标系的齐次变换矩阵；

——测量坐标系相对于机器人1末端坐标系的齐次变换矩阵，其中，

(13)将式(1)转换为设则(1)可以表示如下：

(14)为了减少标定误差，基于多点标定，将式(3)转化成ax＝k的形式，构成一个超定方程组，并采用最小二乘来求解，形式如下：

使最小的解x^*即为超定方程组ax＝k的最小二乘解；

(15)求解出x后即得到矩阵得到矩阵后，即可以将扫描的工件三维点云转换到机器人1基坐标系下。

具体而言，步骤(2)中，所述将一维测量扩展为三维测量的步骤是通过机器人1实时控制系统，工件点云的x，y位置由机器人1末端位姿可知，工件点云的z位置由激光位移传感器2可知，通过实时总线通讯技术，保证x、y、z的同步，从而实现将一维测量扩展为三维测量。

由上述装置获取的点云是否能够真实的反映工件表面形状特征，能否作为后续的机器人1打磨轨迹规划数据，可以通过下述的方法进行验证。

进行验证性实验时，所加工的工件4已知其三维cad模型，可以将三维cad模型离散成点云作为标准点云，由于标准点云是基于工件坐标系的，可以预先标定工件相对于机器人1基坐标系的齐次变换矩阵，标定时先在工件上标记若干点相对于工件坐标系的坐标，再控制机器人1到对应点，获得该点在机器人1基坐标系的坐标。获得同一个点在两个坐标系下的表达，再通过svd算法接算出两个坐标系的转换矩阵，即工件坐标系与机器人1基坐标系的转换矩阵。标定完成后就可以将标准点云转换到基坐标系下，再与扫描所得工件点云进行对比，即得到扫描点云与标准点云之间存在的误差，从而验证该装置的可行性。由于机器人1定位误差、激光位移传感器2的测量误差和标定误差的存在，应允许扫描点云与标准点云之间存在误差，但这个误差应控制在合理范围之内。

具体而言，所述的步骤(3)具体包括步骤：

(31)基于三维点云的每个点的空间位置信息及通过自适应权重函数优化调整后的法向信息规划机器人1打磨位姿，所述打磨位姿的规划依赖于三维点云采集的精确度及点云法向估计的准确度，并且最终决定了工件的打磨质量；

(32)定义点云的加工节点，并以扫描先后顺序依次将加工节点信息发送给机器人1，机器人1进行逆运动学求解和插补运动将加工节点串联生成一条连续的机器人1打磨轨迹。

具体而言，所述的步骤(31)具体包括步骤：

(301)确定打磨时刀具的位置，因为三维点云中包含了扫描工件表面每个点的空间位置信息，并且点云的位置信息是基于机器人1基坐标系的，从而利用三维点云中各点所包含的基于机器人1基坐标系的空间位置信息；通过机器人1逆运动学控制机器人1使得刀具到达打磨时的位置；

(302)确定打磨时刀具的姿态，即通过自适应权重函数对三维点云的每个点的预估法向信息及优化调整后的法向信息规划机器人1打磨时刀具的姿态，因此准确的点云法向估计对工件的打磨质量具有重要影响。

具体而言，所述的步骤(302)具体包括步骤：

(311)通过改进的k邻域搜索算法搜索k个距离最近的点，即将一个点作为体心，以固定步长构建自身的正方体，然后在正方体内进行k邻域搜索，若找到k个距离最近的点，即终止搜索，否则以固定步长扩大正方体的范围，重新搜索k个距离最近的点，直至在正方体内搜索到距离最近的k个点，这样避免了搜索每个点的邻域时都要遍历所有点，提高了搜索效率；而传统的k邻域搜索算法，是遍历点云中的所有点，算出对应的欧式距离，并进行升序排列，取出k个距离最近的点作为k邻域；这种算法在计算每个点的邻域时都需要遍历点云中的每个点，耗时较长，不适用大规模点云的情况；

(312)采用改进的pca算法，通过对某点的k邻域内的点赋予不同的权重函数，采用最小二乘拟合法拟合出一个平面，将所拟合平面的法向作为该点的初始法向；接着基于所述初始法向自适应的确定点云中各点的权重函数，再对初始法向进行优化；传统的pca算法是在点云中某点的k邻域内，采用最小二乘拟合法，拟合出一个平面，将拟合平面的法向作为该点的法向，由于点云中不可避免的存在噪声、外点和尖锐特征，所以本步骤对pca算法进行改进，对邻域内的点赋予不同的权重函数，从而提高拟合平面的质量，最终提高法向估计的精度。

(313)点云重定向，即采用最小生成树法对优化后的法向进行调整，使得点云法向指向一致，最小生成树法是：先规定一点的法向指向，然后根据各点的权重生成扩散路径，最终扩散到整个点云空间，完成点云法向的调整。

具体而言，所述的步骤(312)具体包括步骤：

(321)要使pca算法拟合的平面准确反映该点处的法向，应尽量保证拟合邻域内的点位于同一面片上，但是常规的测量手段会使带有测量偏差的空间外点和噪声点存在扫描点云中，故在pca算法中引入两个权重函数使得拟合方法对点云中的外点和噪声具有一定的鲁棒性，最小二乘拟合可表示为：

s.t.||n||＝1(5)

式中ωd(xi)——高斯权重函数，使得距离该点近的点对拟合平面的作用大一点，反之影响作用小，ωd(xi)表示为：σd为距离带宽，其初值根据工件的形状及扫描点云的分布情况预先设定；

ωr(ri)——与拟合残差有关的高斯权重函数，ωr(ri)表示为：σr为拟合残差带宽，其初值根据工件的形状及扫描点云的分布情况预先设定；

ri^m——第m次迭代，xi点的拟合残差，ri^m可表示为：ri^m＝d^m+(xi-x)^tn^m；

(322)求解式(5)构造协方差矩阵c，表示如下：

式中——表示邻域的中心，表示为：

k——表示邻域内的点数，

通过求解协方差矩阵c，其最小的特征值对应的特征向量即为拟合平面的法向n，即作为该点的初始法向，且

(323)为点云中各个点定义一个特征系数fc来量化该点是位于平滑区域、尖锐特征区域或者过渡区域，特征系数可表示为：

式中s＝2，表示当比例为0.5时，则判定该点位于尖锐特征区域；

δl——表示该点与其距离最近的6个点的平均距离；

(324)确定特征系数fc的取值，先定义点云中某一点邻域内的点云簇，当某一点云簇内的点数大于邻域内的点数与点云簇的数量之商，则认为该点云簇为主点云簇；为了自动的获得点云簇的数量，设置一个角度阈值，当两点的法向夹角大于阈值时，即认为该两点位于不同的点云簇内；得到点云簇的数量，从而计算出各点对应的特征系数fc，a，b两点的法向夹角可表示为：

d(a,b)＝cos^-1(na,nb)；(8)

(325)确定fc的值后，自适应的计算出各个点的距离带宽σd、拟合残差带宽σr、法向差异带宽σn，表示如下：

σd＝(1+fc)·δl(9)

σr＝rmax/3fc(10)

σn＝||ni-n||max/3fc；(11)

(326)因工件不可避免的存在边缘和拐角等尖锐特征，使得邻域内的点处在多个面片上，从而导致较大的法向估计误差，因此在pca算法中引入第三个权重函数，则最小二乘拟合表示为：

s.t.||n||＝1(12)

式中ωn(ni)——与法向差异有关的权重函数，表示为：σn为法向差异带宽，ni为某点初始法向，由式(6)求解所得；

(327)通过步骤(325)中自适应计算出的各个点的距离带宽σd、拟合残差带宽σr、法向差异带宽σn计算点云中每个点的权重函数，替换协方差矩阵c中的

然后通过替换后的协方差矩阵c对式(12)求解，得到每个点的优化法向。

利用上述步骤求解时，影响各点的权重函数的分别是距离带宽σd、拟合残差带宽σr和法向差异带宽σn，上述步骤先根据工件的形状及扫描点云的分布情况，预先设定σd和σr的初值，利用式(6)算出初始法向，并基于初始法向自适应的确定点云中各点的带宽参数，再利用式(7)对初始法向进行优化。

传统的算法是人为的设定全局的带宽参数，实验证明这个参数并不能适应所有的点。本发明根据所处区域的不同设置不同的带宽参数来完成法向估计。采用自适应的方式，根据点云中各点的邻域特征，来确定相应的带宽参数，从而完成点云的法向估计。

具体而言，由上述步骤估计的点云法向，具有二义性，即法向既可以指向工件内部，也可以指向工件外部，所以本发明采用最小生成树法对法向进行调整，使得点云法向指向一致，其原理是通过设点云中xi、xj是距离很近的两点，计算两者的点积，若ni·nj≈1，则表示两点的法向方向相同，否则xi或xj应当反向，因此，所述步骤(313)具体包括步骤：

(331)选取点云中z坐标最大的点作为根结点，调整其法向使之与矢量(0,0,1)的点积大于0，这将使调整后的法向方向指向工件外侧；

(332)设置权重c＝1-|ni·nj|，这样保证了该权值非负，且当c越小表示两邻近点的法向方向越平行；

(333)根据权重c的大小进行遍历，若父结点ni与子结点nj满足ni·nj<0，则将nj反向；

(334)遍历所有结点，将法向方向进行传播扩散，传播过程中设置一个阈值e，当c≤e时，一次扩散至邻域内多点，当c>e时，一次扩散一个点，完成后判断是否满足c≤e，再进行多点扩散，直至扩散到整个点云空间；

(335)若未能遍历所有的点，则点云不连通，部分数据形成局部孤岛，如果未遍历点数个数少于点云总数的千分之一，则将其作为噪声舍弃，否则查找未遍历的数据集中与已遍历点之间距离最近的点，将其作为继续遍历的起点并转至步骤(334)。

本发明通过对点云进行法向估计得到其法向信息，然后将包含法向信息和位置信息的三维点作为加工节点，再通过轨迹规划，生成加工节点的x、y、z、rx、ry、rz信息，并以扫描的先后顺序将加工节点依次同步地发送给机器人1，机器人1进行插补运动和逆运动学求解，生成打磨姿态，从而完成扫描工件的打磨。

完成上述步骤，即完成了基于本发明装置的三维点云采集，并且仅根据三维点云的信息生成了机器人1打磨轨迹。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。