CN114924527A - 适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法及系统 - Google Patents

Abstract

本发明提供了一种适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法及系统，包括：步骤S1：根据待磨曲面三维点云计算各点法向、主曲率、主方向以及理想工具轴向，并根据计算得到的各点法向、主曲率、主方向以及理想工具轴向将工件待磨表面分割成若干子曲面；步骤S2：根据磨抛工具及待磨曲面相关工艺参数构建磨抛力学模型；步骤S3：基于磨抛力学模型对子曲面分别进行局部路径规划；步骤S4：基于局部路径规划生成各个子曲面的局部路径，对局部路径进行子曲面间移动规划生成完整磨抛轨迹；本发明更适用于侧壁内腔等复杂曲面的分割，可以解决传统解析接触力学模型无法适用于非连续曲面的问题，生成的路径能较好的覆盖待磨曲面并使磨抛量达到预期。

Description

适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法及系统

技术领域

本发明涉及机器人磨抛路径规划领域，具体地，涉及适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，可根据待磨曲面三维点云以及相关工艺参数生成机器人完整磨抛轨迹。

背景技术

打磨抛光是各种工业零件生产加工的最后制造步骤。对于工件侧壁、内腔等复杂曲面，目前工业生产线上常用的方式依然是手工磨抛。为减轻工人负担，提高工作效率，机器人磨抛越来越成为磨抛工序的首选。在该工序中，机器人运动规划是重要一环，主要包括对工件表面进行曲面分割，建立工件与工具间的磨抛力学模型，进而基于模型对磨抛路径进行规划三个部分。

目前主流的曲面分割方法，包括经典梯形分解、BCD分解、基于莫尔斯函数的分解等，大多适用于二维平面，通过轮廓等信息将曲面分成若干个连续无障碍的单元，但未考虑工件表面的法方向、曲率等三维特征，不适合用于类似于侧壁、内腔等区域的分割。

磨抛路径规划的基础是对磨抛机理的研究，即描述工具与工件之间接触压强分布以及材料去除率的磨抛力学模型研究。赫兹接触模型适用于圆柱磨抛工具，但不适用于具有非连续曲率特征的工件，如工具穿过孔或者磨抛工件表面边缘的情况。数值力学模型可有效解决上述问题，但目前缺少针对圆柱磨抛工具的数值模型。

专利文献CN111468991B(申请号：202010370120.6)公开了一种基于曲面修补的打磨抛光机器人的路径规划方法，包括：对复杂曲面进行重建，以获得复杂曲面的几何模型文件；对该复杂曲面进行修补，以获得能够包含该复杂曲面的最小四边形曲面；根据表面粗糙度要求，选择路径间距值；在四边形曲面上生成四边形映射网格，取各网格节点作为打磨路径点，以生成z字形的打磨路径；对网格节点进行删减操作，删除属于四边形曲面但不属于修补前的复杂曲面上的路径点，以得到最终的打磨路径示意图。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种。

根据本发明提供的一种适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，包括：

步骤S1：根据待磨曲面三维点云计算各点法向、主曲率、主方向以及理想工具轴向，并根据计算得到的各点法向、主曲率、主方向以及理想工具轴向将工件待磨表面分割成若干子曲面；

步骤S2：根据磨抛工具及待磨曲面相关工艺参数构建磨抛力学模型；

步骤S3：基于磨抛力学模型对子曲面分别进行局部路径规划；

步骤S4：基于局部路径规划生成各个子曲面的局部路径，对局部路径进行子曲面间移动规划生成完整磨抛轨迹。

优选地，所述步骤S1采用：

步骤S1.1：根据工件待磨曲面三维点云数据使用最小二乘法对点云各点进行二次曲面拟合，并计算各点的法向、主曲率与主方向；

步骤S1.2：计算工件表面各点曲率信息，并将工件表面各点曲率最小方向作为理想工具轴向；

步骤S1.3：根据计算的各点法方向、主曲率、主方向以及理想工具轴向，使用K-Means聚类算法将待磨曲面三维点云进行曲面分割。

优选地，所述磨抛力学模型采用：对圆柱磨抛工具相关工艺参数建立数值接触力学模型与材料去除模型；

所述数值接触力学模型是建立法向磨抛力和磨抛工具与工件表面接触深度的关系，在已知恒定法向磨抛力设定值的情况下，通过割线法计算最大接触深度，进而计算其余各点接触深度以及接触压强；

所述材料去除模型是根据工件点所受接触压强以及工件点与磨抛工具相对滑动速度预测工件表面该点单位时间的材料去除深度；

其中，k_p表示材料去除系数；P表示该点所受接触压强；v_s表示相对滑动速度。

优选地，所述步骤S3采用：

步骤S3.1：对分割得到的各个子曲面分别建立局部坐标系并规划工具进给方向；

步骤S3.2：基于所述磨抛力学模型对材料去除量的预测，以期望磨抛量与实际去除量差值最小为目标，规划子曲面规划工具进给速度以及相邻路径间距，得到局部路径。

优选地，所述步骤S3.1采用：

将分割得到的若干子曲面的中心点分别作为子曲面的局部坐标系原点，中心点的法向量为子曲面局部坐标系z轴；计算子曲面内所有点的理想工具轴向的平均值作为整个子曲面的中心轴向T；中心点的路径方向为法方向e_z与中心轴向T的向量积e_z×T；并将当前反向作为局部坐标系y轴；对于同一子曲面采用平行路径的磨抛方式保持同一子曲面内的磨抛路径在局部坐标系Oxy平面的投影互相平行；{x_c,y_c,z_c}为子曲面局部坐标系，工具进给方向在局部坐标系Oxy平面的投影始终与y轴平行；磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向为工具轴向与曲面法向的向量积。

优选地，所述步骤S3.2采用：将点云投影至局部坐标系Oxy平面中，各条路径与y轴平行，表示为x＝x_i,i＝1,2,...，以间隔d对每条路径进行离散取样，得到

个采样点，分别记为y＝y_i,j,j＝1,2,...,n_i，下标i用以指示不同路径，其中，

分别为路径x＝x_i沿y轴的上界与下界，n_i表示路径x＝x_i上的采样点数量；

对于任意一个采样点(x_i,y_i,j)，已知该点曲面法向以及工具的进给方向，由此确定工具位姿；通过所述数值接触力学模型计算给定法向磨抛力下的最大接触深度，进而通过计算在该位姿下子曲面内各点相应的材料去除率；设工具在该采样点的驻留时间为T_i,j，子区域上一工件点P对应的材料去除率记为

则该点的磨抛量为

该点的期望去除深度设为Re^p，则需使

满足预设要求；因此磨抛工具在各采样点驻留时间通过最小二乘问题进行优化求解，同时在优化问题中增加时间变化惩罚项以及速度约束，以避免速度振荡；最终求解的问题表示为：

其中，w_t表示时间变化惩罚项系数；

表示nabla算子；

为计算T_i的散度；V_max表示磨抛工具的最大进给速度；V_min表示最小进给速度；d表示对路径离散化时的采样间距；ò表示设定的时间变化约束值；

求解出T_i后，工具在路径上各采样点的进给速度近似为v_i,j＝d/T_i,j；

相邻路径间距通过贪心算法计算；在确定前i条路径x＝x₁,x＝x₂,...,x＝x_i的情况下，规划第i+1条路径x＝x_i+1，使得x＝x_i与x＝x_i+1之间的平均残余待磨量最小，该优化问题表示为

s.t.x_i＜x_i+1·x_i+H(3)

其中，

为路径x＝x_i与x＝x_i+1之间工件曲面的面积，

表示这两条路径之间的工件表面的残余待磨量的平方和，H为圆柱磨抛工具高度。

优选地，所述步骤S4采用：

步骤S4.1：使用贪心算法规划局部路径串联顺序；

步骤S4.2：标定工具坐标系及工件坐标系，同时对工作空间进行环境标定，获得障碍物与机器人的相对位置关系，通过快速随机搜索树方法，求得局部路径串联顺序下，从前一条路径终点至后一条路径起点的满足无碰撞约束的路径，最终生成完整的磨抛轨迹。

优选地，所述步骤S4.1采用：在圆柱磨抛工具当前位置下，在所有可能的局部路径起始点中寻找离当前工具位置最近的点，该点对应的子曲面则为下一个磨抛区域，磨抛工具以相应的路径连接方式对当前区域进行加工，当前区域的路径终点为下一个循环的起始点。

根据本发明提供的一种适用于圆柱磨抛工具的机器人力控磨抛轨迹规划系统，包括：

模块M1：根据待磨曲面三维点云计算各点法向、主曲率、主方向以及理想工具轴向，并根据计算得到的各点法向、主曲率、主方向以及理想工具轴向将工件待磨表面分割成若干子曲面；

模块M2：根据磨抛工具及待磨曲面相关工艺参数构建磨抛力学模型；

模块M3：基于磨抛力学模型对子曲面分别进行局部路径规划；

模块M4：基于局部路径规划生成各个子曲面的局部路径，对局部路径进行子曲面间移动规划生成完整磨抛轨迹。

优选地，所述模块M1采用：

模块M1.1：根据工件待磨曲面三维点云数据使用最小二乘法对点云各点进行二次曲面拟合，并计算各点的法向、主曲率与主方向；

模块M1.2：计算工件表面各点曲率信息，并将工件表面各点曲率最小方向作为理想工具轴向；

模块M1.3：根据计算的各点法方向、主曲率、主方向以及理想工具轴向，使用K-Means聚类算法将待磨曲面三维点云进行曲面分割；

所述磨抛力学模型采用：对圆柱磨抛工具相关工艺参数建立数值接触力学模型与材料去除模型；

所述数值接触力学模型是建立法向磨抛力和磨抛工具与工件表面接触深度的关系，在已知恒定法向磨抛力设定值的情况下，通过割线法计算最大接触深度，进而计算其余各点接触深度以及接触压强；

所述材料去除模型是根据工件点所受接触压强以及工件点与磨抛工具相对滑动速度预测工件表面该点单位时间的材料去除深度；

其中，k_p表示材料去除系数；P表示该点所受接触压强；v_s表示相对滑动速度；

所述模块M3采用：

模块M3.1：对分割得到的各个子曲面分别建立局部坐标系并规划工具进给方向；

模块M3.2：基于所述磨抛力学模型对材料去除量的预测，以期望磨抛量与实际去除量差值最小为目标，规划子曲面规划工具进给速度以及相邻路径间距，得到局部路径；

所述模块M3.1采用：

将分割得到的若干子曲面的中心点分别作为子曲面的局部坐标系原点，中心点的法向量为子曲面局部坐标系z轴；计算子曲面内所有点的理想工具轴向的平均值作为整个子曲面的中心轴向T；中心点的路径方向为法方向e_z与中心轴向T的向量积e_z×T；并将当前反向作为局部坐标系y轴；对于同一子曲面采用平行路径的磨抛方式保持同一子曲面内的磨抛路径在局部坐标系Oxy平面的投影互相平行；{x_c,y_c,z_c}为子曲面局部坐标系，工具进给方向在局部坐标系Oxy平面的投影始终与y轴平行；磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向为工具轴向与曲面法向的向量积；

所述模块M3.2采用：将点云投影至局部坐标系Oxy平面中，各条路径与y轴平行，表示为x＝x_i,i＝1,2,...，以间隔d对每条路径进行离散取样，得到

个采样点，分别记为y＝y_i,j,j＝1,2,...,n_i，下标i用以指示不同路径，其中，

分别为路径x＝x_i沿y轴的上界与下界，n_i表示路径x＝x_i上的采样点数量；

对于任意一个采样点(x_i,y_i,j)，已知该点曲面法向以及工具的进给方向，由此确定工具位姿；通过所述数值接触力学模型计算给定法向磨抛力下的最大接触深度，进而通过计算在该位姿下子曲面内各点相应的材料去除率；设工具在该采样点的驻留时间为T_i,j，子区域上一工件点P对应的材料去除率记为

则该点的磨抛量为

该点的期望去除深度设为Re^p，则需使

满足预设要求；因此磨抛工具在各采样点驻留时间通过最小二乘问题进行优化求解，同时在优化问题中增加时间变化惩罚项以及速度约束，以避免速度振荡；最终求解的问题表示为：

其中，w_t表示时间变化惩罚项系数；

表示nabla算子；▽·T_i为计算T_i的散度；V_max表示磨抛工具的最大进给速度；V_min表示最小进给速度；d表示对路径离散化时的采样间距；ò表示设定的时间变化约束值；

求解出T_i后，工具在路径上各采样点的进给速度近似为v_i,j＝d/T_i,j；

相邻路径间距通过贪心算法计算；在确定前i条路径x＝x₁,x＝x₂,...,x＝x_i的情况下，规划第i+1条路径x＝x_i+1，使得x＝x_i与x＝x_i+1之间的平均残余待磨量最小，该优化问题表示为

s.t.x_i＜x_i+1·x_i+H(3)

其中，

为路径x＝x_i与x＝x_i+1之间工件曲面的面积，

表示这两条路径之间的工件表面的残余待磨量的平方和，H为圆柱磨抛工具高度；

所述模块M4采用：

模块M4.1：使用贪心算法规划局部路径串联顺序；

模块M4.2：标定工具坐标系及工件坐标系，同时对工作空间进行环境标定，获得障碍物与机器人的相对位置关系，通过快速随机搜索树方法，求得局部路径串联顺序下，从前一条路径终点至后一条路径起点的满足无碰撞约束的路径，最终生成完整的磨抛轨迹；

所述模块M4.1采用：在圆柱磨抛工具当前位置下，在所有可能的局部路径起始点中寻找离当前工具位置最近的点，该点对应的子曲面则为下一个磨抛区域，磨抛工具以相应的路径连接方式对当前区域进行加工，当前区域的路径终点为下一个循环的起始点。

与现有技术相比，本发明具有如下的有益效果：

1、此圆柱磨抛工具的机器人力控磨抛路径规划方法中的曲面分割考虑了待磨曲面的三维特征信息并加入工具轴向至考虑因素中，使结果更符合加工需求，相对于单纯使用曲面轮廓灯二维信息的分割方式，更适用于侧壁、内腔等区域的分割；

2、此圆柱磨抛工具的机器人力控磨抛路径规划方法中的数值磨抛力学模型解决了赫兹接触模型等解析接触力学模型不适用于非连续曲面的缺点，能够处理工具经过工件空隙或边缘的情况，灵活性更强；

3、此圆柱磨抛工具的机器人力控磨抛路径规划方法中的局部路径规划方法以残余待磨量最优为目标，提高了磨抛量的均一性；

4、本发明更适用于侧壁内腔等复杂曲面的分割，可以解决传统解析接触力学模型无法适用于非连续曲面的问题，生成的路径能较好的覆盖待磨曲面并使磨抛量达到预期。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明流程。

图2圆柱工具侧面磨抛定向模型。

图3圆柱工具磨抛曲面模型。

图4圆柱工具接触深度示意图。

图5局部路径示意图。

图6局部路径规划流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1

根据本发明提供的一种适用于圆柱磨抛工具的机器人力控磨抛路径规划方法，如图1所示，包括如下步骤：

步骤1：根据待磨曲面三维点云，计算各点法向、主曲率、主方向以及理想工具轴向，将工件待磨表面分割成若干子曲面；

步骤2：根据磨抛工具及待磨曲面相关工艺参数，构建磨抛力学模型；

步骤3：基于所述磨抛力学模型，对所述子曲面分别进行局部路径规划；

步骤4：所述局部路径规划中生成有各个子曲面的局部路径，对所述局部路径进行子曲面间移动规划，生成完整磨抛轨迹。

所述步骤1，包括：

步骤1.1：根据工件待磨曲面三维点云数据，使用最小二乘法对点云各点进行二次曲面拟合，并计算各点的法向、主曲率与主方向。

对于点云中任意一点P(x,y,z)，以点P为中心取距离最近的k个点，通过这k个点利用最小二乘法拟合二次曲面

z(x,y)＝ax²+bxy+cy²+dx+ey+f (1)

其中，(a,b,c,d,e,f)为需要拟合的系数；x,y分别表示点P的x坐标与y坐标；

即对点云中任一数据点的邻域点(x_i,y_i.z_i)_i＝1,...,k，使下式取最小值进行拟合

其中，k表示曲面拟合所使用的邻域点数量，(x_i,y_i,z_i)_i＝1,...,k分别表示这k个点的坐标。

根据微分几何相关定理，可由拟合得到的二次曲面公式计算得到该点P的法向、主曲率与主方向。

步骤1.2：根据工件点云各点的相关曲率信息，计算各点的理想工具轴向。

如图2所示，

与

为工具曲面在P点处的主方向，

与

为相应的主曲率。

与

为工件曲面在P点处的主方向，

与

为相应主曲率。δ为

与

夹角，通过求解以下最优化问题求解δ以确定曲率最小方向与曲面主方向的相对关系，将此曲率最小方向作为理想工具轴向，即圆柱轴线方向。

其中

计算可得，若

同号，则δ＝0或δ＝π；若

异号，则

或

其中

步骤1.3：根据曲面上各点的坐标、法方向及理想工具轴向，使用K-Means聚类算法对曲面进行分割。

将工件点云作为样本集S＝{x₁,x₂,...,x_N}，每个样本为9维特征向量，表示为x_i＝[x_i,y_i,z_i,αn_ix,αn_iy,αn_iz,εe_ix,εe_iy,εe_iz]^T，其中，n_i＝[n_ix,n_iy,n_iz]^T为该点的单位法向量，α＞0为其权重参数；e_i＝[e_ix,e_iy,e_iz]^T为该点的理想工具轴向，ε＞0为其权重参数。上标T表示转置符号，(x_i,y_i,z_i)表示该点的位置坐标，K-Means算法将样本S划分为K个不相交的簇，每个簇相当于一个子曲面。

所述步骤2，包括：

步骤2.1：根据圆柱磨抛工具相关工艺参数建立数值接触力学模型。

如图3所示,{O,x,y,z}为世界坐标系，{O_T,x_T,y_T,z_T}为工具坐标系，O_T为圆柱轴的中点，x轴与工具轴线方向一致，z轴与工件曲面在与磨具的中心接触点O_w上的法向一致。

将磨抛工具考虑为半径为R，高度为H的圆柱体，其中心为刚体，外层包裹厚度为D的弹性材料。根据接触力学及圆柱几何特征，法向磨抛力F_N通过下式计算

其中，E是杨氏模量，S为磨抛工具与工件的接触区域，h(s)是磨抛工具在接触区域的面积小量s上沿半径方向没入工件的深度，θ(s)是该小量所在半径与曲面法向夹角。β是可以通过材料负载试验确定的应力应变功率指数。

将圆柱侧面离散为M个小面片后，法向磨抛力F_N可以表示为

其中，Δs_i为接触区域离散面片面积，通过Δs_i＝dH_i·dθ_i·R计算，dH_i与dθ_i分别为离散面片高度与弧度角。法向磨抛力为接触区域所有离散小量与工件表面压强所做的贡献之和，当某个区域小量接触深度h_i为负时，它对法向磨抛力没有贡献，因此需要将其设置成零。

本发明使用具有力控功能的末端执行器使法向磨抛力F_N始终保持在恒定值。如图4所示，所在半径与曲面法向垂直的区域接触深度最大，记为h₀。对于工具表面各离散点，以图4中点B为例，在已知最大接触深度h₀的情况下，通过θ_i与工件点云数据可找到其所在半径上的工件点(图4点A)，进而计算两点间的距离即接触深度为h_i＝R-O_TA，若h_i为负，则设为零。当h₀增大，各工具点的h_i亦增大，因此法向磨抛力是关于最大接触深度的单调递增函数。在给定法向磨抛力F_N的情况下，通过割线法计算最大接触深度h₀，进而计算其余各点接触深度h_i以及接触压强P_i。

步骤2.2：建立材料去除模型。使用Preston方程计算工件表面各点单位时间的材料去除深度

其中，k_p是材料去除系数，P是该点所受接触压强，v_s是相对滑动速度，在步骤3中将对时间dt进行规划使得工件表面最终的材料去除深度达到期望值。

所述步骤3，对所述步骤1分割得到的各个子曲面分别进行局部路径规划，包括：

步骤3.1：对于每个子曲面，分别建立局部坐标系，并确定各个子曲面的工具进给方向。

对于所述步骤1分割得到的各个子曲面，将各个子曲面的中心点分别作为子曲面的局部坐标系原点，中心点的法向量为子曲面局部坐标系z轴。计算子曲面内所有点的理想工具轴向的平均值作为整个子曲面的“中心轴向”T，中心点路径方向为法方向e_z与“中心轴向”T的向量积，即e_z×T，并将该方向作为局部坐标系y轴。对于同一子曲面，采用平行路径的磨抛方式，即保持同一子曲面内的磨抛路径在局部坐标系Oxy平面的投影互相平行。如图5所示，{x_c,y_c,z_c}为子曲面局部坐标系，工具进给方向在局部坐标系Oxy平面的投影始终与y轴平行。磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向则为工具轴向与曲面法向的向量积。

步骤3.2：规划磨抛工具在各点的进给速度以及相邻路径间距。

如图5所示，将点云投影至局部坐标系Oxy平面中，各条路径与y轴平行，可表示为x＝x_i，i＝1,2,...以间隔d对每条路径进行离散取样，得到

个采样点，分别记为y＝y_i,j，j＝1,2,...,n_i，下标i用以指示不同路径，，其中y_i ^U，y_i ^L分别为路径x＝x_i沿y轴的上界与下界，n_i表示路径x＝x_i上的采样点数量。

对于任意一个采样点(x_i,y_i,j)，已知该点曲面法向以及工具的进给方向，由此确定工具位姿。通过步骤2.1所述数值接触力学模型计算给定法向磨抛力下的最大接触深度，进而通过步骤2.2计算在该位姿下子曲面内各点相应的材料去除率。设工具在该采样点的驻留时间为T_i,j，子区域上一工件点P对应的材料去除率记为

则该点的磨抛量为

该点的期望去除深度设为Re^p，则需使

尽可能小。因此磨抛工具在各采样点驻留时间可通过最小二乘问题进行优化求解，同时在优化问题中增加时间变化惩罚项以及速度约束，以避免速度振荡。最终求解的问题表示为：

其中，w_t表示时间变化惩罚项系数，

表示nabla算子，

即为计算T_i的散度，V_max表示磨抛工具的最大进给速度，V_min表示最小进给速度，d表示对路径离散化时的采样间距，ò表示设定的时间变化约束值。

求解出T_i后，工具在路径上各采样点的进给速度可以近似为v_i,j＝d/T_i,j。

相邻路径间距通过贪心算法计算。在确定前i条路径x＝x₁,x＝x₂,...,x＝x_i的情况下，规划第i+1条路径，即x＝x_i+1，使得x＝x_i与x＝x_i+1之间的平均残余待磨量最小，该优化问题可表示为

s.t.x_i＜x_i+1·x_i+H

其中，

为路径x＝x_i与x＝x_i+1之间工件曲面的面积，

表示这两条路径之间的工件表面的残余待磨量的平方和，H为圆柱磨抛工具高度。

局部路径规划流程如图6所示，基于子曲面三维点云数据，首先根据所述步骤3.1对子曲面建立局部坐标系，磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向则为工具轴向与曲面法向的向量积；选择一条起始路径，根据所述步骤3.2中工具进给速度规划方法对路径进行采样并计算工具在各个采样点的进给速度，该起始路径通常为该子曲面上存在的在局部坐标系下x坐标最小的路径；根据规划得到的工具在各路径采样点的驻留时间来更新子曲面上各点的残余待磨量；随后，根据所述步骤3.2中的相邻路径间距规划方法计算下一条路径的x坐标；若新路径位于子曲面内，则计算工具在该新路径上的进给速度，并对残余待磨量进行更新，若新路径超过子曲面边界，则结束循环。

所述步骤4，包括：

步骤4.1：使用贪心算法规划所述局部路径串联顺序，具体的，在圆柱磨抛工具当前位置下，在所有可能的局部路径起始点中寻找离当前工具位置最近的点，该点对应的子曲面则为下一个磨抛区域，磨抛工具以相应的路径连接方式对该区域进行加工，该区域的路径终点为下一个循环的起始点；

步骤4.2：标定工具坐标系及工件坐标系，同时对工作空间进行环境标定，即获得障碍物与机器人的相对位置关系，通过快速随机搜索树方法，求出所述局部路径串联顺序下，从前一条路径终点至后一条路径起点的满足无碰撞约束的路径，最终生成完整的磨抛轨迹。

根据本发明提供的一种适用于圆柱磨抛工具的机器人力控磨抛路径规划系统，包括：

模块1：根据待磨曲面三维点云，计算各点法向、主曲率、主方向以及理想工具轴向，将工件待磨表面分割成若干子曲面；

模块2：根据磨抛工具及待磨曲面相关工艺参数，构建磨抛力学模型；

模块3：基于所述磨抛力学模型，对所述子曲面分别进行局部路径规划；

模块4：所述局部路径规划中生成有各个子曲面的局部路径，对所述局部路径进行子曲面间移动规划，生成完整磨抛轨迹。

所述模块1，包括：

模块1.1：根据工件待磨曲面三维点云数据，使用最小二乘法对点云各点进行二次曲面拟合，并计算各点的法向、主曲率与主方向。

对于点云中任意一点P(x,y,z)，以点P为中心取距离最近的k个点，通过这k个点利用最小二乘法拟合二次曲面

z(x,y)＝ax²+bxy+cy²+dx+ey+f (1)

其中，(a,b,c,d,e,f)为需要拟合的系数；x,y分别表示点P的x坐标与y坐标；

即对点云中任一数据点的邻域点(x_i,y_i.z_i)_i＝1,...,k，使下式取最小值进行拟合

其中，k表示曲面拟合所使用的邻域点数量，(x_i,y_i,z_i)_i＝1,...,k分别表示这k个点的坐标。

根据微分几何相关定理，可由拟合得到的二次曲面公式计算得到该点P的法向、主曲率与主方向。

模块1.2：根据工件点云各点的相关曲率信息，计算各点的理想工具轴向。

如图2所示，

与

为工具曲面在P点处的主方向，

与

为相应的主曲率。

与

为工件曲面在P点处的主方向，

与

为相应主曲率。δ为

与

夹角，通过求解以下最优化问题求解δ以确定曲率最小方向与曲面主方向的相对关系，将此曲率最小方向作为理想工具轴向，即圆柱轴线方向。

其中

计算可得，若

同号，则δ＝0或δ＝π；若

异号，则

或

其中

模块1.3：根据曲面上各点的坐标、法方向及理想工具轴向，使用K-Means聚类算法对曲面进行分割。

将工件点云作为样本集S＝{x₁,x₂,...,x_N}，每个样本为9维特征向量，表示为x_i＝[x_i,y_i,z_i,αn_ix,αn_iy,αn_iz,εe_ix,εe_iy,εe_iz]^T，其中，n_i＝[n_ix,n_iy,n_iz]^T为该点的单位法向量，α＞0为其权重参数；e_i＝[e_ix,e_iy,e_iz]^T为该点的理想工具轴向，ε＞0为其权重参数。上标T表示转置符号，(x_i,y_i,z_i)表示该点的位置坐标，K-Means算法将样本S划分为K个不相交的簇，每个簇相当于一个子曲面。

所述模块2，包括：

模块2.1：根据圆柱磨抛工具相关工艺参数建立数值接触力学模型。

如图3所示,{O,x,y,z}为世界坐标系，{O_T,x_T,y_T,z_T}为工具坐标系，O_T为圆柱轴的中点，x轴与工具轴线方向一致，z轴与工件曲面在与磨具的中心接触点O_w上的法向一致。

将磨抛工具考虑为半径为R，高度为H的圆柱体，其中心为刚体，外层包裹厚度为D的弹性材料。根据接触力学及圆柱几何特征，法向磨抛力F_N通过下式计算

其中，E是杨氏模量，S为磨抛工具与工件的接触区域，h(s)是磨抛工具在接触区域的面积小量s上沿半径方向没入工件的深度，θ(s)是该小量所在半径与曲面法向夹角。β是可以通过材料负载试验确定的应力应变功率指数。

将圆柱侧面离散为M个小面片后，法向磨抛力F_N可以表示为

其中，Δs_i为接触区域离散面片面积，通过Δs_i＝dH_i·dθ_i·R计算，dH_i与dθ_i分别为离散面片高度与弧度角。法向磨抛力为接触区域所有离散小量与工件表面压强所做的贡献之和，当某个区域小量接触深度h_i为负时，它对法向磨抛力没有贡献，因此需要将其设置成零。

本发明使用具有力控功能的末端执行器使法向磨抛力F_N始终保持在恒定值。如图4所示，所在半径与曲面法向垂直的区域接触深度最大，记为h₀。对于工具表面各离散点，以图4中点B为例，在已知最大接触深度h₀的情况下，通过θ_i与工件点云数据可找到其所在半径上的工件点(图4点A)，进而计算两点间的距离即接触深度为h_i＝R-O_TA，若h_i为负，则设为零。当h₀增大，各工具点的h_i亦增大，因此法向磨抛力是关于最大接触深度的单调递增函数。在给定法向磨抛力F_N的情况下，通过割线法计算最大接触深度h₀，进而计算其余各点接触深度h_i以及接触压强P_i。

模块2.2：建立材料去除模型。使用Preston方程计算工件表面各点单位时间的材料去除深度

其中，k_p是材料去除系数，P是该点所受接触压强，v_s是相对滑动速度，在模块3中将对时间dt进行规划使得工件表面最终的材料去除深度达到期望值。

所述模块3，对所述模块1分割得到的各个子曲面分别进行局部路径规划，包括：

模块3.1：对于每个子曲面，分别建立局部坐标系，并确定各个子曲面的工具进给方向。

对于所述模块1分割得到的各个子曲面，将各个子曲面的中心点分别作为子曲面的局部坐标系原点，中心点的法向量为子曲面局部坐标系z轴。计算子曲面内所有点的理想工具轴向的平均值作为整个子曲面的“中心轴向”T，中心点路径方向为法方向e_z与“中心轴向”T的向量积，即e_z×T，并将该方向作为局部坐标系y轴。对于同一子曲面，采用平行路径的磨抛方式，即保持同一子曲面内的磨抛路径在局部坐标系Oxy平面的投影互相平行。如图5所示，{x_c,y_c,z_c}为子曲面局部坐标系，工具进给方向在局部坐标系Oxy平面的投影始终与y轴平行。磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向则为工具轴向与曲面法向的向量积。

模块3.2：规划磨抛工具在各点的进给速度以及相邻路径间距。

如图5所示，将点云投影至局部坐标系Oxy平面中，各条路径与y轴平行，可表示为x＝x_i，i＝1,2,...以间隔d对每条路径进行离散取样，得到

个采样点，分别记为y＝y_i,j，j＝1,2,...,n_i，下标i用以指示不同路径，，其中

分别为路径x＝x_i沿y轴的上界与下界，n_i表示路径x＝x_i上的采样点数量。

对于任意一个采样点(x_i,y_i,j)，已知该点曲面法向以及工具的进给方向，由此确定工具位姿。通过模块2.1所述数值接触力学模型计算给定法向磨抛力下的最大接触深度，进而通过模块2.2计算在该位姿下子曲面内各点相应的材料去除率。设工具在该采样点的驻留时间为T_i,j，子区域上一工件点P对应的材料去除率记为

则该点的磨抛量为

该点的期望去除深度设为Re^p，则需使

尽可能小。因此磨抛工具在各采样点驻留时间可通过最小二乘问题进行优化求解，同时在优化问题中增加时间变化惩罚项以及速度约束，以避免速度振荡。最终求解的问题表示为：

其中，w_t表示时间变化惩罚项系数，

表示nabla算子，

即为计算T_i的散度，V_max表示磨抛工具的最大进给速度，V_min表示最小进给速度，d表示对路径离散化时的采样间距，ò表示设定的时间变化约束值。

求解出T_i后，工具在路径上各采样点的进给速度可以近似为v_i,j＝d/T_i,j。

相邻路径间距通过贪心算法计算。在确定前i条路径x＝x₁,x＝x₂,...,x＝x_i的情况下，规划第i+1条路径，即x＝x_i+1，使得x＝x_i与x＝x_i+1之间的平均残余待磨量最小，该优化问题可表示为

s.t.x_i＜x_i+1·x_i+H

其中，

为路径x＝x_i与x＝x_i+1之间工件曲面的面积，

表示这两条路径之间的工件表面的残余待磨量的平方和，H为圆柱磨抛工具高度。

局部路径规划流程如图6所示，基于子曲面三维点云数据，首先根据所述模块3.1对子曲面建立局部坐标系，磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向则为工具轴向与曲面法向的向量积；选择一条起始路径，根据所述模块3.2中工具进给速度规划方法对路径进行采样并计算工具在各个采样点的进给速度，该起始路径通常为该子曲面上存在的在局部坐标系下x坐标最小的路径；根据规划得到的工具在各路径采样点的驻留时间来更新子曲面上各点的残余待磨量；随后，根据所述模块3.2中的相邻路径间距规划方法计算下一条路径的x坐标；若新路径位于子曲面内，则计算工具在该新路径上的进给速度，并对残余待磨量进行更新，若新路径超过子曲面边界，则结束循环。

所述模块4，包括：

模块4.1：使用贪心算法规划所述局部路径串联顺序，具体的，在圆柱磨抛工具当前位置下，在所有可能的局部路径起始点中寻找离当前工具位置最近的点，该点对应的子曲面则为下一个磨抛区域，磨抛工具以相应的路径连接方式对该区域进行加工，该区域的路径终点为下一个循环的起始点；

模块4.2：标定工具坐标系及工件坐标系，同时对工作空间进行环境标定，即获得障碍物与机器人的相对位置关系，通过快速随机搜索树方法，求出所述局部路径串联顺序下，从前一条路径终点至后一条路径起点的满足无碰撞约束的路径，最终生成完整的磨抛轨迹。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，包括：

步骤S1：根据待磨曲面三维点云计算各点法向、主曲率、主方向以及理想工具轴向，并根据计算得到的各点法向、主曲率、主方向以及理想工具轴向将工件待磨表面分割成若干子曲面；

步骤S2：根据磨抛工具及待磨曲面相关工艺参数构建磨抛力学模型；

步骤S3：基于磨抛力学模型对子曲面分别进行局部路径规划；

步骤S4：基于局部路径规划生成各个子曲面的局部路径，对局部路径进行子曲面间移动规划生成完整磨抛轨迹。

2.根据权利要求1所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述步骤S1采用：

步骤S1.1：根据工件待磨曲面三维点云数据使用最小二乘法对点云各点进行二次曲面拟合，并计算各点的法向、主曲率与主方向；

步骤S1.2：计算工件表面各点曲率信息，并将工件表面各点曲率最小方向作为理想工具轴向；

步骤S1.3：根据计算的各点法方向、主曲率、主方向以及理想工具轴向，使用K-Means聚类算法将待磨曲面三维点云进行曲面分割。

3.根据权利要求1所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述磨抛力学模型采用：对圆柱磨抛工具相关工艺参数建立数值接触力学模型与材料去除模型；

所述数值接触力学模型是建立法向磨抛力和磨抛工具与工件表面接触深度的关系，在已知恒定法向磨抛力设定值的情况下，通过割线法计算最大接触深度，进而计算其余各点接触深度以及接触压强；

所述材料去除模型是根据工件点所受接触压强以及工件点与磨抛工具相对滑动速度预测工件表面该点单位时间的材料去除深度；

其中，k_p表示材料去除系数；P表示该点所受接触压强；v_s表示相对滑动速度。

4.根据权利要求1所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述步骤S3采用：

步骤S3.1：对分割得到的各个子曲面分别建立局部坐标系并规划工具进给方向；

步骤S3.2：基于所述磨抛力学模型对材料去除量的预测，以期望磨抛量与实际去除量差值最小为目标，规划子曲面规划工具进给速度以及相邻路径间距，得到局部路径。

5.根据权利要求4所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述步骤S3.1采用：

将分割得到的若干子曲面的中心点分别作为子曲面的局部坐标系原点，中心点的法向量为子曲面局部坐标系z轴；计算子曲面内所有点的理想工具轴向的平均值作为整个子曲面的中心轴向T；中心点的路径方向为法方向e_z与中心轴向T的向量积e_z×T；并将当前反向作为局部坐标系y轴；对于同一子曲面采用平行路径的磨抛方式保持同一子曲面内的磨抛路径在局部坐标系Oxy平面的投影互相平行；{x_c,y_c,z_c}为子曲面局部坐标系，工具进给方向在局部坐标系Oxy平面的投影始终与y轴平行；磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向为工具轴向与曲面法向的向量积。

6.根据权利要求4所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述步骤S3.2采用：将点云投影至局部坐标系Oxy平面中，各条路径与y轴平行，表示为x＝x_i,i＝1,2,...，以间隔d对每条路径进行离散取样，得到

个采样点，分别记为y＝y_i,j,j＝1,2,...,n_i，下标i用以指示不同路径，其中，

分别为路径x＝x_i沿y轴的上界与下界，n_i表示路径x＝x_i上的采样点数量；

对于任意一个采样点(x_i,y_i,j)，已知该点曲面法向以及工具的进给方向，由此确定工具位姿；通过所述数值接触力学模型计算给定法向磨抛力下的最大接触深度，进而通过计算在该位姿下子曲面内各点相应的材料去除率；设工具在该采样点的驻留时间为T_i,j，子区域上一工件点P对应的材料去除率记为

则该点的磨抛量为

该点的期望去除深度设为Re^p，则需使

满足预设要求；因此磨抛工具在各采样点驻留时间通过最小二乘问题进行优化求解，同时在优化问题中增加时间变化惩罚项以及速度约束，以避免速度振荡；最终求解的问题表示为：

其中，w_t表示时间变化惩罚项系数；

表示nabla算子；

为计算T_i的散度；V_max表示磨抛工具的最大进给速度；V_min表示最小进给速度；d表示对路径离散化时的采样间距；ò表示设定的时间变化约束值；

求解出T_i后，工具在路径上各采样点的进给速度近似为v_i,j＝d/T_i,j；

相邻路径间距通过贪心算法计算；在确定前i条路径x＝x₁,x＝x₂,...,x＝x_i的情况下，规划第i+1条路径x＝x_i+1，使得x＝x_i与x＝x_i+1之间的平均残余待磨量最小，该优化问题表示为

s.t.x_i＜x_i+1·x_i+H (3)

其中，

为路径x＝x_i与x＝x_i+1之间工件曲面的面积，

表示这两条路径之间的工件表面的残余待磨量的平方和，H为圆柱磨抛工具高度。

7.根据权利要求1所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述步骤S4采用：

步骤S4.1：使用贪心算法规划局部路径串联顺序；

步骤S4.2：标定工具坐标系及工件坐标系，同时对工作空间进行环境标定，获得障碍物与机器人的相对位置关系，通过快速随机搜索树方法，求得局部路径串联顺序下，从前一条路径终点至后一条路径起点的满足无碰撞约束的路径，最终生成完整的磨抛轨迹。

8.根据权利要求7所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划方法，其特征在于，所述步骤S4.1采用：在圆柱磨抛工具当前位置下，在所有可能的局部路径起始点中寻找离当前工具位置最近的点，该点对应的子曲面则为下一个磨抛区域，磨抛工具以相应的路径连接方式对当前区域进行加工，当前区域的路径终点为下一个循环的起始点。

9.一种适用于圆柱磨抛工具的机器人力控磨抛轨迹规划系统，其特征在于，包括：

模块M1：根据待磨曲面三维点云计算各点法向、主曲率、主方向以及理想工具轴向，并根据计算得到的各点法向、主曲率、主方向以及理想工具轴向将工件待磨表面分割成若干子曲面；

模块M2：根据磨抛工具及待磨曲面相关工艺参数构建磨抛力学模型；

模块M3：基于磨抛力学模型对子曲面分别进行局部路径规划；

模块M4：基于局部路径规划生成各个子曲面的局部路径，对局部路径进行子曲面间移动规划生成完整磨抛轨迹。

10.根据权利要求9所述的适用于圆柱磨抛工具的机器人力控磨抛轨迹规划系统，其特征在于，所述模块M1采用：

模块M1.1：根据工件待磨曲面三维点云数据使用最小二乘法对点云各点进行二次曲面拟合，并计算各点的法向、主曲率与主方向；

模块M1.2：计算工件表面各点曲率信息，并将工件表面各点曲率最小方向作为理想工具轴向；

模块M1.3：根据计算的各点法方向、主曲率、主方向以及理想工具轴向，使用K-Means聚类算法将待磨曲面三维点云进行曲面分割；

所述磨抛力学模型采用：对圆柱磨抛工具相关工艺参数建立数值接触力学模型与材料去除模型；

所述数值接触力学模型是建立法向磨抛力和磨抛工具与工件表面接触深度的关系，在已知恒定法向磨抛力设定值的情况下，通过割线法计算最大接触深度，进而计算其余各点接触深度以及接触压强；

所述材料去除模型是根据工件点所受接触压强以及工件点与磨抛工具相对滑动速度预测工件表面该点单位时间的材料去除深度；

其中，k_p表示材料去除系数；P表示该点所受接触压强；v_s表示相对滑动速度；

所述模块M3采用：

模块M3.1：对分割得到的各个子曲面分别建立局部坐标系并规划工具进给方向；

模块M3.2：基于所述磨抛力学模型对材料去除量的预测，以期望磨抛量与实际去除量差值最小为目标，规划子曲面规划工具进给速度以及相邻路径间距，得到局部路径；

所述模块M3.1采用：

将分割得到的若干子曲面的中心点分别作为子曲面的局部坐标系原点，中心点的法向量为子曲面局部坐标系z轴；计算子曲面内所有点的理想工具轴向的平均值作为整个子曲面的中心轴向T；中心点的路径方向为法方向e_z与中心轴向T的向量积e_z×T；并将当前反向作为局部坐标系y轴；对于同一子曲面采用平行路径的磨抛方式保持同一子曲面内的磨抛路径在局部坐标系Oxy平面的投影互相平行；{x_c,y_c,z_c}为子曲面局部坐标系，工具进给方向在局部坐标系Oxy平面的投影始终与y轴平行；磨抛工具在子曲面内各点的轴向为各点法方向与局部坐标系y轴的向量积，工具进给方向为工具轴向与曲面法向的向量积；

所述模块M3.2采用：将点云投影至局部坐标系Oxy平面中，各条路径与y轴平行，表示为x＝x_i,i＝1,2,...，以间隔d对每条路径进行离散取样，得到

个采样点，分别记为y＝y_i,j,j＝1,2,...,n_i，下标i用以指示不同路径，其中，

分别为路径x＝x_i沿y轴的上界与下界，n_i表示路径x＝x_i上的采样点数量；

对于任意一个采样点(x_i,y_i,j)，已知该点曲面法向以及工具的进给方向，由此确定工具位姿；通过所述数值接触力学模型计算给定法向磨抛力下的最大接触深度，进而通过计算在该位姿下子曲面内各点相应的材料去除率；设工具在该采样点的驻留时间为T_i,j，子区域上一工件点P对应的材料去除率记为

则该点的磨抛量为

该点的期望去除深度设为Re^p，则需使

满足预设要求；因此磨抛工具在各采样点驻留时间通过最小二乘问题进行优化求解，同时在优化问题中增加时间变化惩罚项以及速度约束，以避免速度振荡；最终求解的问题表示为：

其中，w_t表示时间变化惩罚项系数；

表示nabla算子；

为计算T_i的散度；V_max表示磨抛工具的最大进给速度；V_min表示最小进给速度；d表示对路径离散化时的采样间距；ò表示设定的时间变化约束值；

求解出T_i后，工具在路径上各采样点的进给速度近似为v_i,j＝d/T_i,j；

相邻路径间距通过贪心算法计算；在确定前i条路径x＝x₁,x＝x₂,...,x＝x_i的情况下，规划第i+1条路径x＝x_i+1，使得x＝x_i与x＝x_i+1之间的平均残余待磨量最小，该优化问题表示为

s.t.x_i＜x_i+1·x_i+H (3)

其中，

为路径x＝x_i与x＝x_i+1之间工件曲面的面积，

表示这两条路径之间的工件表面的残余待磨量的平方和，H为圆柱磨抛工具高度；

所述模块M4采用：

模块M4.1：使用贪心算法规划局部路径串联顺序；

模块M4.2：标定工具坐标系及工件坐标系，同时对工作空间进行环境标定，获得障碍物与机器人的相对位置关系，通过快速随机搜索树方法，求得局部路径串联顺序下，从前一条路径终点至后一条路径起点的满足无碰撞约束的路径，最终生成完整的磨抛轨迹；

所述模块M4.1采用：在圆柱磨抛工具当前位置下，在所有可能的局部路径起始点中寻找离当前工具位置最近的点，该点对应的子曲面则为下一个磨抛区域，磨抛工具以相应的路径连接方式对当前区域进行加工，当前区域的路径终点为下一个循环的起始点。

Images