CN113741426A - 一种基于局部点云曲线拟合的机器人加工路径规划方法 - Google Patents

Abstract

本发明公开了一种基于局部点云曲线拟合的机器人加工路径规划方法，首先通过激光扫描仪获取原始点云数据，对原始点云数据进行标定和预处理后，选定截平面信息，确定选定的点云数据集；然后对点云数据进行子区域划分，对各子区域点云进行降维处理；之后根据每个子区域的线性特点进行直线拟合或曲线拟合；计算各子区域拟合的曲线与截平面的交点，得到控制点位置，通过获得的控制点信息，拟合出路径截面曲线；之后进行步长计算，并提取到路径点的位置信息，提取到路径点框架；最后基于路径点框架，通过逆运动学，进行机器人轴配置信息计算，即获得完整规划路径。本发明提高了曲面重构精度，降低重构时间，提高了所规划的加工路径的平滑性。

Description

一种基于局部点云曲线拟合的机器人加工路径规划方法

技术领域

本发明属于逆向工程和机器人加工技术领域，涉及一种机器人加工路径规划技术，具体涉及一种基于局部点云曲线拟合的机器人加工路径规划方法。

背景技术

目前，大部分机器人的自适应路径规划方法，都是在已拥有物体模型上进行路径规划，再利用视觉标定技术，将三维点云与物体曲面模型进行匹配，这种方法严重依赖模型与实物的准确度，特别对于那种生产时易变形、大型工件(局部与模型误差会比较大)，该方法不适用。

针对上述情况，目前常用的方法就是采用逆向工程的方法，将扫描的实物点云进行逆向重构模型，再进行路径规划，这种方法对于密集的点云数据，高精度的曲面重构时间长，效率低下，并且局部重构精度无法保证。此外，还有局部区域曲面拟合，再在局部拟合曲面上进行路径提取，但存在高精度下低效率问题，并且加工路径生成不好直接控制。

发明内容

本发明的目的在于针对没有实体模型或者实体于模型不匹配的对象，本文提供了一种高效率的基于局部点云曲线拟合的机器人加工路径规划方法，提高曲面重构精度，降低曲面重构时间，从而提高路径规划效率和精度。

为了解决上述问题，本发明提供了一种基于局部点云曲线拟合的机器人加工路径规划系统，其特征在于：包含点云处理模块、人机交互模块、路径截面曲线拟合模块和加工路径生成模块；

所述机器人加工路径规划系统是基于C++语言、VS开发平台实现；其运行效率快；

所述点云处理模块，用于对点云数据采集、视觉设备标定和点云预处理流程；

所述人机交互模块，为基于QT开发的交互界面，主要提供截平面信息输入功能；

所述路径截面曲线拟合模块，主要用于点云子区域划分、分段曲线拟合和路径截面曲线拟合；

所述加工路径生成模块，主要用于步长计算、路径点信息提取、机器人轴配置计算和加工路径生成。

所述的点云处理模块，结合本方法的特点，因为只是局部点云的读取，点云数据量不大，故其中点云预处理流程仅仅需要进行滤波去噪处理，不需考虑点云密集度。

所述的人机交互模块，其截平面信息主要包含：截平面的平面方程，加工路径的判定向量方向，点云截取区域宽度。

本发明还提供一种基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于，包括如下步骤：

步骤1：通过激光扫描仪获取原始点云数据，通过标定视觉设备，对点云数据进行标定矩阵旋转平移处理，确保点云相对机器人位置的精度；

步骤2：通过人机交互模块输入的截平面信息，确定选定的点云数据集；

步骤3：对选定的点云数据进行子区域划分；

步骤4：通过主成分分析法提取各个子区域的法向量并计算得到法平面，再基于各子区域的法平面，对各子区域点云进行降维处理；

步骤5：利用主成分分析法计算每个子区域的二维点云数据的线性特点，满足线性特点的区域点云进行直线拟合，否则就是进行曲线拟合；

步骤6：计算各子区域拟合的曲线与截平面的交点，得到控制点位置，通过获得的控制点信息，利用B样条曲线逼近，拟合出路径截面曲线；

步骤7：基于路径截面曲线，进行步长计算，并提取到路径点的位置信息，再结合路径点的邻域信息，提取到路径点框架；

步骤8:基于路径点框架，通过逆运动学，进行机器人轴配置信息计算，结合加工工艺参数，即获得完整规划路径。

进一步地，步骤1中，对原始点云数据标定后进行预处理，所述预处理为采用滤波去噪处理，剔除噪声点和离群点。

进一步地，步骤2中，所述截平面信息包括截平面的平面方程，加工路径的判定向量方向，点云截取区域宽度Ω。

进一步地，步骤2中，截平面确定的方法如下：

通过人机交互界面在点云数据中选取两个目标点P₁与P₂，两点确定直线L，L为截平面旋转轴，在给定旋转角下，即确定截平面信息；直线L的方向向量作为加工路径方向的参考向量L_X；点云截取宽度的方向为截平面的法向向量方向。

进一步地，步骤2中，确定截平面后建立截平面坐标系O_jie，坐标原点位于大地坐标(0，0，0)，X轴方向由L_X确认，Y轴方向由截平面法线方向向量L_Y确认，坐标系Z轴方向由左手定则确定，即

通过点云截取区域宽度确定截平面两侧的边界截面Plane1和Plane2；边界截面Plane1、Plane2、起始边界以及终止边界所包含区域内的点云为所截取点云集S_jie。

进一步地，步骤3中，子区域划分方法如下：

步骤3.1、沿着X轴方向将选定的点云数据均匀划分为若干个子区域，每个子区域的点云数量为N(最后1个区域的点云数量范围为

N与点云截取宽度以及密度有关(一般默认N＝10Ω))，相邻子区域之间点云的重复度为40-60％，最优为50％；完成初步子区域划分；

步骤3.2、对边界区域的子区域进行斜边判定，符合斜边条件的边界区域则舍去，当边界两端的子区域点云都不符合斜边条件时，完成子区域划分，此时所有子区域集合为所选取的点云数据集。

进一步地，步骤3中，斜边判定方法如下：

计算各子区域点云的质心、以及点云在截平面两边分布的情况，当子区域内大部分点云集中在截平面一侧并且质心与截平面距离大于阈值时，该边界区域就被认定为斜边，舍弃该子区域。

进一步地，步骤4中，降维处理是将子区域点云投影到该子区域的法平面上，其法平面计算：基于PCA的特性，估算第i个子区域的点云法线方向Li_n；基于截平面法特点，要保证子区域的投影平面都垂直于截平面，故该法平面的法线向量Pni_n＝Li_n×L_Y，法平面的位置由子区域的质心确定。

进一步地，步骤4中，线性特点采取二维的PCA分析来判断，将计算出的最大特征值与最小特征值进行比较，当两者相差过大时，即比值大于设定阈值时，则判断为线性，最优比例阈值为100，一般为取值范围为60-200。

进一步地，步骤5中，线性特点判断方法如下：

采取二维的PCA分析，将计算出每个子区域的最大特征值与最小特征值进行比较，当两者差值大于阈值时，则判断为线性。

进一步地，步骤5中，曲线拟合加入了线性特点判断，弥补了样条拟合直线的不足，空间直线拟合采用最小二乘法拟合直线参数方程，并且考虑了直线参数方程拟合时的特殊情况；曲线拟合采用的是B样条逼近拟合，因为投影后的点云比较密集，直接拟合效果差，所以需要进行点云简化来获得控制点，简化的方法采用k邻域聚类

每个类的质心就是局部曲线拟合所需的控制点，再利用这些控制点拟合子区域的曲线。

进一步地，步骤6中，由于生成机理是通过区域拟合的曲线与截平面的交点，故控制点在保留大部分三维特点情况下，都位于一个平面，并且不存在控制点分布杂乱的问题，这让路径截面曲线拟合效果和效率得到保证。此外为了保证曲线拟合的平滑性，减少误差点的影响，对于与相邻控制点连线夹角小于150°的控制点进行剔除。

截面路径曲线拟合采用采用B样条准均匀逼近拟合，次数选择为三次，保证曲线的C2连续性。

进一步地，步骤7中，步长计算结合拟合曲线的曲率特点采用等弦高误差法来计算；

进一步地，步骤7中，路径点框架计算方法如下：

因为截面曲线为二维特征，其反映不了一个点的三维法线信息，故通过PCA提取路径点的邻域(通过R半径邻域来确定，半径

)法线方向，该方向确定为路径点框架的Z轴方向

为了保证加工路径的平顺性，路径点的加工方向，即X轴方向需要在截平面内，该方向向量计算：

Y轴方向由左手定则确定，即

本发明的优势与效果：

本发明采用局部点云的曲线拟合的方法，通过截平面截取的局部点云来获得实物表面信息，曲线拟合效率比曲面拟合高，加工效率得到很大提高；拟合的曲线就是加工路径参考曲线，可以直接控制加工路径生成；本文采用的截平面选取局部点云的方法比较灵活，可以适应大部分的曲面特点的点云；本文采用子区域的划分、二维投影、曲线拟合，考虑了局部的三维点云分布特点，精度得到基本保障；本文结合截面曲线拟合信息以及路径点邻域信息，保证生成路径点框架在符合实物对应位置的三维信息下，加工路径平滑性。

附图说明

图1是本发明公开的一种基于局部点云曲线拟合的机器人加工路径规划方法流程图；

图2是点云截平面法局部选取点云示意图，其中图2(a)是路径截平面生成示意图，图2(b)是点云截取示意图；

图3，其中图3(a)是点云子区域划分示意图，图3(b)是子区域曲线生成示意图，图3(c)是路径截面曲线控制点生成示意图；

图4是子区域曲线拟合以及生成控制点效果图；

图5是边界子区域有斜边情况时的路径规划效果图；

图6是整体路径规划效果图；

图7是规划路径局部效果展示图；

图8是点云大部分子区域为非线性时的路径生成效果图；

图9是初始点云以及截面选择效果图；

本文附图中所有出现的坐标系都是截平面坐标系。

附图标记：1-截平面，2、17-边界截面，3-截取点云，4-截平面旋转轴生成点，5-截平面旋转轴，6-理想的路径曲线，7-子区域曲线控制点，8-子区域拟合曲线，9-子区域投影点，10-路径截面曲线控制点，11-某一子区域点云，12-整体点云，13-路径截面曲线，14-加工路径点及其框架，15-某一子集的投影平面，16-边界子区域。

具体实施方式

结合附图和实施例对本发明进一步详细说明，但本方法应用只不限于该例子。

本发明提供了一种基于局部点云曲线拟合的机器人加工路径规划系统，包含点云处理模块、人机交互模块、路径截面曲线拟合模块和加工路径生成模块；

所述机器人加工路径规划系统是基于C++语言、VS开发平台实现；

所述点云处理模块，用于对点云数据采集、视觉设备标定和点云预处理流程；

所述人机交互模块，为基于QT开发的交互界面，主要提供截平面信息输入功能；

所述路径截面曲线拟合模块，主要用于点云子区域划分、分段曲线拟合和路径截面曲线拟合；

所述加工路径生成模块，主要用于步长计算、路径点信息提取、机器人轴配置计算和加工路径生成。

本发明还提供一种基于局部点云曲线拟合的机器人加工路径规划方法，包括如下步骤：

步骤1、通过激光扫描仪获取原始点云数据，通过标定视觉设备，对点云数据进行标定矩阵旋转平移处理，确保点云相对机器人位置的精度；

步骤2：对点云数据采用滤波去噪进行预处理，剔除噪声点和离群点；

将扫描的原始点云进行预处理与位置标定后，计算各点与邻近点距离X_i，总和为

计算点云密度估值：

(N_In为点云总数)。

步骤3：通过人机交互模块输入的截平面信息，确定选定的点云数据集；

如图2(a)所示，实心点为处理后的点云数据，通过交互界面选择两个截平面旋转轴生成点4(图中空心点)，将两个截平面旋转轴生成点4连接的黑色直线轴为截平面旋转轴5，其方向向量为L_X；

通过交互界面的旋转操作，确定截平面1相对旋转轴的旋转角度，从而获得截平面1的几何信息，得到截平面1，并得到截平面1的法线方向向量L_Y。

建立截平面坐标系O_jie，坐标系原点取(0，0，0)，其X轴方向由L_X确认，Y轴方向由截平面法线方向向量L_Y确认，坐标系Z轴方向由左手定则确定，即

点云区域宽度默认Ω＝10ρ，一般可以取5-15ρ，可通过交互界面进行修改，本次实例使用默认值。从而确定点云的边界截面2、17(边界截面2和边界截面17均与截平面1平行，距离为0.5Ω)

两个边界截面2、17所包围区域的点云为截取点云3，即所截取点云集S_jie，如图2右图所示，从O_jie的XOY方向看，蓝色矩形内点云为所截取的点云。

步骤4：对选定的点云数据进行子区域划分；

步骤4.1、沿着X轴方向将选定的点云数据均匀划分为若干个子区域；完成初步子区域划分；具体方法如下

在O_jie坐标系下，按其X轴方向对截取点云集S_jie中的点云进行排序，获得排序点云集

再对排序点云集

进行均匀划分，分割为M个子区域，第i个子区域对应的点云子集

点云子集包含点数为N，每个子区域与相邻区域点云的重复度为50％。一般默认N＝10Ω，取值范围可以为5-15Ω，考虑到截取点云集S_jie一般无法均分，所以将均分后的余数放在最后一个子区域中，即第M个子区域的点云数量范围为

步骤4.2、对边界区域的子区域进行斜边判定，符合斜边条件的边界区域则舍去，当边界两端的子区域点云都不符合斜边条件时，完成子区域划分，此时所有子区域集合为所选取的点云数据集。

斜边判定方法为：计算各子区域点云的质心、以及点云在截平面1两边分布的情况，当子区域内大部分点云集中在截平面1一侧并且质心与截平面1距离大于阈值时，该边界区域就被认定为斜边，舍弃该子区域。

具体方法如下：

再边界的子区域进行斜边判断，计算排序点云集

的边界子集，即第一个子区域

与第M个子区域

的质心位置

计算质心

与截平面1的距离

与

当

或

与截平面1距离大于阈值

(取值范围可以为

)时，则对应的子区域存在质心偏离情况，按照上述方法计算所有的点云子区域的质心偏离情况。

计算每个子区域点云的偏置情况，计算

子集中各点与边界截面2、17的距离，更邻近边界截面2划为1区，否则划分为2区；统计所有点后，当1区或2区的统计点数大于

时，该区域则被判断为存在点云偏置，对于

的判断同理。

当一个边界的点云子区域同时存在质心偏离与点云偏置情况，则判定为边界斜边子集，舍去该子区域，再进行新一轮斜边判断，直到

两端的子区域不再存在斜边情况，即停止判断，最终得到F个子区域组成的点云数据集

效果如图5中所示，符合斜边条件的边界子区域16，会被舍去，不会生成路径点。

步骤5：通过主成分分析法提取各个子区域的法向量并计算得到法平面，再基于各子区域的法平面，对各子区域点云进行降维处理；具体步骤如下：

对点云数据集

的每个点云子集

进行二维投影。

的投影平面确定，先利用PCA提取该点云子集的法向量，对

去中心化后的子集表示为D＝{x₁,x₂,…,x_N}，计算协方差矩阵

计算协方差矩阵的特征值

最小特征值

对应的特征向量即为点云子集

的法向量

再计算

的投影平面法向量

结合

的质心

即可以得到各子集的投影平面。然后对各点云子集

进行二维投影获得投影子集

步骤6：利用主成分分析法计算每个子区域的二维点云数据的线性特点，满足线性特点的区域点云进行直线拟合，否则就是进行曲线拟合；具体步骤如下：

先通过PCA分析每个二维点云数据(投影子集

)线性特点，计算点云协方差矩阵的两个特征值λ₁、λ₂(λ₁＞λ₂)来表现点云数据的最大与最小变化趋势，当

时，则为满足线性特性进行直线拟合，如果不满足则为曲线特征，进行曲线拟合。

对满足线性的子区域进行最小二乘法直线拟合，由于空间曲线参数方程的特性，无法拟合平行于XY平面的直线，故加入二维最小二乘法拟合补充该情况；获得线集合curveL。

对曲线特征的子区域进行曲线拟合，由于投影后的子区域投影点9的分布存在密集点，故需要进行点云简化来获得子区域曲线拟合所需的子区域曲线控制点7。

对每个投影子集

采用k邻域聚类

将

分为

个类

floor为取整)，依次提取的

第j类的质心

这些质心点就是局部曲线拟合所需的控制点，再利用这些控制点，进行B样条逼近拟合子区域的曲线，得到子区域拟合曲线8，如图3所示，所有的子区域拟合曲线8一起组成线集合curveBS。

步骤7：计算各子区域拟合的曲线与截平面1的交点，得到控制点位置，通过获得的控制点信息，利用NURBS曲线逼近，拟合出路径截面曲线13；具体步骤如下：

步骤7.1、截平面1路径曲线控制点生成，先获得各个子区域的曲线集curve＝curveBS+curveL，效果图见图4的子区域拟合曲线8，从而获得截平面1路径曲线的初始控制点集

其示意图见图3(c)所示，图中圆点为路径截面曲线控制点10，，效果图见图4，四角星为路径截面曲线控制点10。

对初始控制点集

进行处理；将每个路径截面曲线控制点10与相邻的两个控制点连线，并计算夹角，当夹角小于150°时，则判定为误差噪声点，舍去。得到最优的控制点集P^path＝{p₁,p₂,…p_Pnum}，控制点数量为Pnum。

步骤7.2、拟合出路径截面曲线13，这里采用B样条准均匀逼近拟合，次数选择为三次(degree＝3)，节点数量Knum＝Pnum-degree-1(两端节点重复度为4)，节点矢量t_n采用等距计算；节点向量为{0,0,0,0,t₀,t₁,…,t_n,1,1,1,1},n＝Knum，从而进行B样条准均匀逼近拟合，保证曲线的两个端点都是控制点，这里面的权因子w_i都设置为1，故曲线拟合公式为：

其中N_i,degree(t)为基函数，根据deBoor-cox推导公式可以得到。

效果：如图6-9所示，图6-7的对象其点云截取宽度为3mm,图8-9的对象点云截取宽度为4mm，可以看到，对于不同曲面特征下的点云，拟合的曲线效果符合加工精度需求。

步骤8：基于路径截面曲线13进行步长计算，并提取到路径点的位置信息，再结合路径点的邻域信息，提取到路径点框架(是一个坐标框架，包含X轴，对应加工方向；Z轴表示加工表面的法线方向；Y轴方向通过左手定则确定)；具体方法如下：

在获得截面路径曲线后，将曲线两端点作为路径的起始与结束点，采用等弦高误差法，此处等弦高误差δ设置为0.05mm，计算曲线步长step，

此处R为曲线的曲率半径。

基于曲线参数，结合step的值，从曲线上提取到笛卡尔坐标系下的路径点位置信息。

通过PCA提取路径点的邻域(通过R半径邻域来确定，半径

)法线方向，该方向确定为路径点框架的Z轴方向

为了保证加工路径的平顺性，路径点的加工方向，即X轴方向需要在截平面1内，该方向向量计算：

Y轴方向由左手定则确定，即

至此获得所有路径点的位置与框架信息，效果见图6-9。

并计算各路径点与对应子区域点云的最近距离以及点云的密度(通过点云平均间隔来体现)，如下表1所示(单位默认mm)，对比发现，点云平均间隔为0.379595mm，而路径点与最近点间隔全部小于点云平均间隔，路径点与最近点间隔平均值为0.190194mm,可以看到路径点的位置尽可能靠近了点云表面，位置偏差不大。

表1路径点与最近点间隔计算值

步骤9:基于路径点框架，结合加工参数、逆运动学，进行机器人轴配置信息计算，结合加工工艺参数，即获得完整规划路径。

以上实施方式仅用于说明本发明，而非对本发明的限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行各种组合、修改或者等同替换，都不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于，包括如下步骤：

步骤1：通过激光扫描仪获取原始点云数据，通过标定视觉设备，对原始点云数据进行标定矩阵旋转平移处理，确保点云相对机器人位置的精度；

步骤2：通过人机交互模块输入的截平面信息，确定选定的点云数据集；

步骤3：对选定的点云数据进行子区域划分；

步骤4：通过主成分分析法提取各个子区域的法向量并计算得到法平面，再基于各子区域的法平面，对各子区域点云进行降维处理；

步骤5：利用主成分分析法计算每个子区域的二维点云数据的线性特点，满足线性特点的区域点云进行直线拟合，否则就是进行曲线拟合；

步骤6：计算各子区域拟合的曲线与截平面的交点，得到控制点位置，通过获得的控制点信息，利用B样条曲线逼近，拟合出路径截面曲线；

步骤7：基于路径截面曲线，进行步长计算，并提取到路径点的位置信息，再结合路径点的邻域信息，提取到路径点框架；

步骤8:基于路径点框架，通过逆运动学，进行机器人轴配置信息计算，结合加工工艺参数，即获得完整规划路径。

2.根据权利要求1所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤1中，对原始点云数据标定后进行预处理，所述预处理为采用滤波去噪处理，剔除噪声点和离群点。

3.根据权利要求1所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤2中，所述截平面信息包括截平面的平面方程，加工路径的判定向量方向，点云截取区域宽度Ω。

4.根据权利要求3所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤2中，截平面确定的方法如下：

通过人机交互界面在点云数据中选取两个目标点P₁与P₂，两点确定直线L，L为截平面旋转轴，在给定旋转角下，即确定截平面信息；直线L的方向向量作为加工路径方向的参考向量L_X；点云截取宽度的方向为截平面的法向向量方向。

5.根据权利要求4所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤2中，确定截平面后建立截平面坐标系O_jie，X轴方向由L_X确认，Y轴方向由截平面法线方向向量L_Y确认，坐标系Z轴方向由左手定则确定，即

通过点云截取区域宽度确定截平面两侧的边界截面Plane1和Plane2；边界截面Plane1、Plane2、起始边界以及终止边界所包含区域内的点云为所截取点云集S_jie。

6.根据权利要求5所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤3中，子区域划分方法如下：

步骤3.1、沿着X轴方向将选定的点云数据均匀划分为若干个子区域，相邻子区域之间点云的重复度为40-60％；完成初步子区域划分；

步骤3.2、对边界区域的子区域进行斜边判定，符合斜边条件的边界区域则舍去，当边界两端的子区域点云都不符合斜边条件时，完成子区域划分，此时所有子区域集合为所选取的点云数据集。

7.根据权利要求6所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤3中，斜边判定方法如下：

计算各子区域点云的质心、以及点云在截平面两边分布的情况，当子区域内大部分点云集中在截平面一侧并且质心与截平面距离大于阈值时，该边界区域就被认定为斜边，舍弃该子区域。

8.根据权利要求6所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤4中，降维处理是将子区域点云投影到该子区域的法平面上，其法平面计算：基于PCA的特性，估算第i个子区域的点云法线方向

基于截平面法特点，要保证子区域的投影平面都垂直于截平面，故该法平面的法线向量

法平面的位置由子区域的质心确定。

9.根据权利要求6所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤5中，线性特点判断方法如下：

采取二维的PCA分析，将计算出每个子区域的最大特征值与最小特征值进行比较，当两者比值大于阈值时，则判断为线性。

10.根据权利要求6所述基于局部点云曲线拟合的机器人加工路径规划方法，其特征在于：步骤6中，拟合前对于与相邻控制点连线夹角小于150°的控制点进行剔除，截面路径曲线拟合采用采用B样条准均匀逼近拟合。

Images