CN111633668B - 一种用于机器人加工三维自由曲面的运动控制方法 - Google Patents

Abstract

本发明涉及一种用于机器人加工三维自由曲面的运动控制方法，建立加工轨迹每个小线段的位置向量和姿态向量，并根据建立的位置向量和姿态向量获得位置权因子和姿态权因子；根据获得的位置权因子和姿态权因子得到位置姿态合成向量，根据位置姿态合成向量得到位置姿态合成向量模量；对小线段对应的合成位置姿态向量进行前瞻和插补得到小线段内多个插补点距离小线段起始点的位移；根据得到的位移对位置姿态合成向量进行分解，得到插补点位置插补向量和姿态插补向量，获得插补点的位置信息和姿态信息，本发明的方法能够使位置和姿态同步保持高精特性。

Description

一种用于机器人加工三维自由曲面的运动控制方法

技术领域

本发明涉及计算机数控系统运动控制技术领域，具体涉及一种用于机器人加工三维自由曲面的运动控制方法。

背景技术

这里的陈述仅提供与本发明相关的背景技术，而不必然地构成现有技术。

机器人作为一种数控设备，相比于数控机床而言，因其成本低、工作空间大、空间灵活性高、生产柔性的优点，特别适合于大尺寸、低切削力和低精度要求的复杂曲面加工。

然而，以往的机器人加工应用中，机器人多应用于搬运、焊接、喷涂、钻孔、打磨等简单直线、圆弧轨迹或可参数化描述运动轨迹的场景，目前的应用中很少见到针对三维自由曲面的加工的使用，其主要原因在于自由曲面加工轨迹由系列化小线段组成，必须使用小线段前瞻算法进行速度前瞻，才能避免小线段加工时速度一直处于交替加速减速波动状态，以实现小线段运动的速度要求。要实现带有前瞻控制的运动，其运动控制算法比较复杂、难度较大。除此之外，机器人加工轨迹不同于数控机床，机器人的某些特殊应用场景，例如，针对自由曲面加工的激光熔覆，要求机器人末端激光束时刻垂直于加工表面，此类运动轨迹不仅需要使用笛卡尔空间的位置参数x、y、z，还要使用姿态参数α、β、γ参数加以描述，因此，相比与数控机床，机器人自由曲面加工的运动规划更为复杂。

目前，发明人发现，机器人运动规划多使用现场示教编程或简单离线运动轨迹编程的方式完成机器人运动轨迹规划工作，三维自由曲线由大量小线段组成，使用传统的现场示教的方法操作极其繁琐，且示教的方式会产生较大的位置、姿态误差，极大影响了加工质量，无法满足三维自由曲面的加工要求。此外，针对三维复杂的自由曲线运动轨迹的离线编程方法不成熟，且多缺少小线段速度前瞻，无法实现机器人复杂三维自由曲线的高速、高精、平稳运动。市场上少有的带速度前瞻的机器人运动控制系统中，其速度前瞻算法为位置、姿态分别前瞻，然后根据位置、姿态分别前瞻的结果，采取协调变速，要么是姿态插补协调于位置规划结果，要么是位置插补协调于姿态规划结果，没有考虑位置姿态同步问题，其位置、姿态的同步性往往不理想，且控制算法复杂，稳定性不足，难以在机器人实时控制系统上得到应用。

现有的方法中，提出了一种基于位置和姿态约束的机器人小线段前瞻规划方法，采用融合速度向量v_a＝(v₁，v₂，v₃，σω₁，σω₂，σω₃)替代线性速度作为前瞻规划方法中的规划量。发明人发现，该方法仅考虑速度约束，没有考虑加速度、加加速度的约束，且小线段间没有采取有效的过渡，在节点处会产生速度方向的突变问题，产生机械冲击。

综上所述，面对三维自由曲线的运动轨迹高速、高精、平稳的要求，目前没有很好的机器人运动控制方法。

发明内容

本发明的目的是为克服现有技术的不足，提供一种用于机器人加工三维自由曲面的运动控制方法，实现了机器人位置和姿态的同步，保证了机器人在加工三维自由曲面时位置和姿态协同的高精特性，同时保证了运动的稳定性。

为实现上述目的，本发明采用如下技术方案：

第一方面，本发明的实施例提供了一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，包括：

建立加工轨迹每个小线段的位置向量和姿态向量，并根据建立的位置向量和姿态向量获得位置权因子和姿态权因子；

根据获得的位置权因子和姿态权因子得到位置姿态合成向量，根据位置姿态合成向量得到位置姿态合成向量模量；

对小线段对应的合成位置姿态向量进行前瞻和插补得到小线段内多个插补点距离小线段起始点的位移；根据得到的位移对位置姿态合成向量进行分解，得到插补点位置插补向量和姿态插补向量，获得插补点的位置信息和姿态信息。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，根据实际加工工艺参数获得位置运动约束参数和姿态运动约束参数，并建立位置坐标系和姿态坐标系，构建每个小线段的位置向量和姿态向量，根据每个小线段的位置向量和姿态向量得到位置权因子和姿态权因子。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，相邻段位置姿态合成向量之间采用圆弧过渡，根据各个小线段的位置姿态合成向量计算相邻小线段节点处夹角，利用度数最大的夹角得到圆弧过渡的最小半径。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，对小线段的前瞻包括以下步骤：

根据位置运动约束参数、姿态运动约束参数、位置权因子和姿态权因子确定合成位置姿态约束参数；

根据合成位置姿态约束参数及合成位置姿态向量模量分别对小线段进行正向加速计算和反向加速计算，分别得到小线段的合成正向末端速度和合成反向末端速度；

将得到的合成正向末端速度、合成反向末端速度、合成位置姿态约束参数比对，得到小线段的末端规划速度。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，正向加速计算中，根据小线段起始点的合成正向首端速度，利用S曲线加减速算法得到小线段末端点的合成正向末端速度，小线段合成正向末端速度等于下一个小线段的合成正向首端速度。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，根据小线段的合成正向首端速度及合成位置姿态约束参数计算临界合成位移，将临界合成位移与合成位置姿态向量模量进行比较，在不同的比较结果情况下利用S曲线加减速算法得到小线段合成正向末端速度。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，反向加速计算中，根据小线段末端点的合成反向末端速度，利用S曲线加减速算法得到小线段起始点的合成反向首端速度，加工端合成反向首端速度等于前一个小线段的合成反向末端速度。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，根据小线段合成反向末端速度及合成位置姿态约束参数计算临界合成位移，将临界合成位移与合成位置姿态向量模量进行比较，在不同比较结果情况下利用S曲线加减速算法得到小线段合成反向首端速度。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，选择合成正向末端速度、合成反向末端速度及合成位置姿态约束参数中的合成最大速度中的最小者为小线段的末端规划速度。

结合第一方面，本发明的实施例提供了第一方面的一种可能实施方式，根据小线段的末端规划速度及位置姿态合成向量模量，利用S曲线加减速算法得到小线段中多个插补点距离小线段首段的位移。

本发明的有益效果：

1.本发明的方法，通过在算法中引入位置权因子和姿态权因子，构造了合成位置姿态向量。利用合成位置姿态向量进行小线段的前瞻和插补，平衡了位置空间与姿态空间的量纲，实现了位置与姿态同步，保证机器人在加工三维自由曲面时位置和姿态协同的高精特性。

2.本发明的方法，相邻小线段使用圆弧过渡，解决了由于节点处位置空间和姿态空间速度方向突变引起的冲击，保证机器人在加工三维自由曲面时运动的稳定性。

3.本发明的方法，使用S曲线加减速算法对小线段进行前瞻，并通过与临界合成位移的对比，在不同情况下进行合成正向末端速度和合成反向末端速度的计算，保证了机器人在加工三维自由曲面时的高速特性。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的限定。

图1为本发明实施例1方法流程示意图；

图2为本发明实施例1小线段位置、姿态信息示意图；

图3为本发明实施例1小线段在位置坐标系示意图；

图4为本发明实施例1小线段在姿态坐标系示意图；

图5为本发明实施例1合成位置姿态向量示意图；

图6为本发明实施例1合成位置姿态向量间圆弧过渡示意图；

图7为本发明实施例1圆弧过渡效果示意图；

图8为本发明实施例1第一种情况加速S曲线示意图；

图9为本发明实施例1第二种情况加速S曲线示意图；

图10为本发明实施例1第三种情况加速S曲线示意图；

图11为本发明实施例1验证实验机器人示意图；

图12为本发明实施例1验证实验加工轨迹示意图；

图13为本发明实施例1验证实验Matlab中显示加工轨迹示意图；

图14为本发明实施例1验证实验合成位置姿态向量前瞻插补结果示意图；

图15为本发明实施例1验证实验反馈关节电机位置信息示意图；

图16为本发明实施例1验证实验反馈关节电机速度信息示意图；

图17为本发明实施例1验证实验反馈关节1电机速度信息局部放大图；

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

为了方便叙述，本发明中如果出现“上”、“下”、“左”“右”字样，仅表示与附图本身的上、下、左、右方向一致，并不对结构起限定作用,仅仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位，以特定的方位构造和操作，因此不能理解为对本发明的限制。

正如背景技术所介绍的，目前的带速度前瞻的机器人运动控制系统中位置和姿态分别前瞻，没有考虑位置和姿态同步问题，针对上述问题，本申请提出了一种用于机器人加工三维自由曲面的运动控制方法。

本申请的一种典型实施方式实施例1中，一种用于机器人加工三维自由曲面的运动控制方法，包括以下步骤：

步骤1.建立加工轨迹每个小线段的位置向量和姿态向量，并根据建立的位置向量和姿态向量获得位置权因子和姿态权因子；

步骤2.根据获得的位置权因子和姿态权因子得到位置姿态合成向量，根据位置姿态合成向量得到位置姿态合成向量模量；

步骤3.对小线段对应的合成位置姿态向量进行前瞻和插补得到小线段内多个插补点距离小线段起始点的位移；根据得到的位移对位置姿态合成向量进行分解，得到插补点位置插补向量和姿态插补向量，获得插补点的位置信息和姿态信息。

所述步骤1的具体步骤为：

利用CAM软件对待加工的三维自由曲面刀具加工轨迹进行设计，加工轨迹由多个节点划分成多个离散的小线段，如图2所示，其中包含了加工过程中末端刀具工作点P(i)的位置信息(xi、yi、zi)和姿态信息(oi、ni、ai)，分别建立x、y、z三维直角坐标系作为位置坐标系，建立α、β、γ三维直角坐标系为姿态坐标系，各个小线段在位置坐标系的分量如图3所示，位置空间点Pp(i)表达了机器人末端刀具的运动位置信息(xi,yi,zi)，各个小线段在姿态坐标系的分量如图4所示，姿态空间点Po(i)表达了机器人末端刀具姿态信息(αi、βi、γi)，其可根据欧拉角计算公式由图2中的姿态信息(oi、ni、ai)获得。

根据实际加工工艺参数的要求，分别获得小线段的位置运动约束参数和姿态运动约束参数，其中位置运动约束参数包括最大线速度V_{p_max}、最大线加速度A_{p_max}、最大线加加速度J_{p_max}，姿态运动约束参数包括最大角速度ω_{o_max}、最大角加速度α_{o_max}、最大角加加速度β_{o_max}。

将小线段分别在位置坐标系和姿态坐标系进行分解，构造出小线段的位置向量和姿态向量。

第i段小线段的位置向量

计算方法如下：

第i段小线段的位置向量的向量模量

为：

第i段小线段的位置向量对应的单位向量

为：

其中，x_i+1、x_i分别为第i段小线段两端节点在位置坐标系中对应的X轴坐标值，y_i+1、y_i分别为第i段小线段两端节点在位置坐标系中对应的Y轴坐标值，z_i+1、z_i分别为第i段小线段两端节点在位置坐标系中对应的Z轴坐标值。

为位置坐标系中X轴对应的方向向量，

为位置坐标系中Y轴对应的方向向量，

为位置坐标系中Z轴对应的方向向量。

第i段小线段的姿态向量

计算方法如下：

第i段小线段姿态向量的向量模量

为：

第i段小线段的姿态向量对应的单位向量

为：

其中，α_i+1、α_i分别为第i段小线段两端节点在姿态坐标系中α轴对应的坐标值，β_i+1、β_i分别为第i段小线段两端节点在姿态坐标系中β轴对应的坐标值，γ_i+1、γ_i分别为第i段小线段两端节点在姿态坐标系中γ轴对应的坐标值。

为姿态坐标系中α轴对应的方向向量，

为姿态坐标系中β轴对应的方向向量，

为姿态坐标系中γ轴对应的方向向量。

计算位置权因子W_p和姿态权因子W_o。

由于位置坐标系中的量纲为毫米，而姿态坐标系中的量纲为弧度，二者量纲不统一，如果直接正交合成构造合成位置姿态向量，会造成位置和姿态的控制效果不均衡，运动效果不好。

位置权因子和姿态权因子分别表征了位置和姿态在合成位置姿态向量中的控制能力，位置权因子越大，则说明位置向量对合成位置姿态向量的影响力越大，反之则说明姿态向量对合成位置姿态向量的影响力越大。

位置权因子W_p和姿态权因子W_o的大小由小线段的位置向量模量总和S_{P_sum}与所有小线段的姿态向量模量总和θ_{o_sum}决定。

位置向量模量总和S_{P_sum}计算方法为：

其中，x_i+1、x_i分别为第i段小线段两端节点在位置坐标系中对应的X轴坐标值，y_i+1、y_i分别为第i段小线段两端节点在位置坐标系中对应的Y轴坐标值，z_i+1、z_i分别为第i段小线段两端节点在位置坐标系中对应的Z轴坐标值，n为小线段的总个数。

姿态向量模量总和θ_{o_sum}计算方法为：

其中，α_i+1、α_i分别为第i段小线段两端节点在姿态坐标系中α轴对应的坐标值，β_i+1、β_i分别为第i段小线段两端节点在姿态坐标系中β轴对应的坐标值，γ_i+1、γ_i分别为第i段小线段两端节点在姿态坐标系中γ轴对应的坐标值，n为小线段的总个数。

位置权因子W_p和姿态权因子W_o的计算方法如下：

通过引入位置权因子W_p和姿态权因子W_o，考虑了整个运动加工轨迹的位置和姿态的变化幅度，适应性地调节加工过程中位置和姿态在构造合成位置姿态向量过程中的支配力量，能够保证位置和姿态对运动规划具有相当能力的控制，避免了在位置运动幅度大而姿态运动幅度小，或者位置运动幅度小而姿态运动幅度大这两种情况下，位置和姿态对规划支配能力不均衡的问题，保证机器人在加工三维自由曲面时位置和姿态协同的高精特性。能够从全局轨迹的角度，保证位置和姿态对后期合成位置姿态向量的影响力均衡，实现位置和姿态对后期规划的同等影响作用，保证了位置和姿态的同步性，其原理如图5所示。

所述步骤2的具体步骤为：

根据位置权因子和姿态权因子，构造第i段小线段的位置姿态合成向量

构造方法如下：

代表虚轴量。

同时可以计算出第i段小线段的位置姿态合成向量模量

和位置姿态合成向量对应的单位位置姿态合成向量模量

由于后期需要对合成位置姿态向量进行速度前瞻，前瞻算法中合成位置姿态向量在节点处两侧速度大小相同，但速度方向不同，如果不使用过渡的话，机器人在运动至此处时会有速度方向的突变，产生机械冲击，降低运动的平稳性和精度。因此，在合成位置姿态向量的节点处引入圆弧过渡，实现速度的平滑过渡，消除由于速度突变产生的机械冲击。

具体方法如下：

首先根据拱高误差ε要求，遍历所有的相邻合成位置姿态向量的夹角，找到最大节点夹角

根据圆弧过渡的几何特点，如图6所示，可以求得圆弧过渡的最小半径R_ε，其具体方法如下：

为第i段小线段和第i+1段小线段对应的合成位置姿态向量之间的夹角。

n为小线段的总个数。

通过圆弧过渡能够实现合成位置姿态向量间圆滑地实现过渡，如图7所示，避免了位置空间和姿态空间由于速度方向不同产生的冲击。

所述步骤3的具体步骤为：

对小线段对应的合成位置姿态向量间进行前瞻，根据构造的合成位置姿态向量，使用S曲线加减速算法进行速度前瞻，S曲线加减速因其算法成熟、加减速圆滑，在数控系统中应用广泛。使用S曲线加减速进行位置姿态向量段间速度前瞻，能够实现小线段快速加速、快速减速，有利于提高机器人三维自由曲线的运动速度和运动平稳性。

包括以下具体步骤：

步骤a：根据位置运动约束参数(最大线速度V_{p_max}、最大线加速度A_{p_max}、最大线加加速度J_{p_max})和姿态运动约束参数(最大角速度ω_{o_max}、最大角加速度α_{o_max}、最大角加加速度β_{o_max})确定合成位置姿态约束参数(合成最大速度V_{c_max}、合成最大加速度A_{c_max}、合成最大加加速度J_{c_max})

使用步骤1获得的位置权因子和姿态权因子得到合成位置姿态约束参数：

V_{c_max}＝W_p·V_{p_max}+W_o·ω_{o_max}

A_{c_max}＝W_p·A_{p_max}+W_oα_{o_max}

J_{c_max}＝W_p·J_{p_max}+W_o·β_{o_max}

步骤b：进行正向加速计算。

第i个小线段的时间间隔t_c计算方法为：

第i个小线段起始点和末端点的速度变化量Δv_c计算方法为：

因此，

v_{c_e}(i)为第i段小线段末端点的合成正向末端速度，v_{c_s}(i)为第i段小线段起始点的合成正向首端速度。

计算临界合成位移S_c(i)

比较当前第i段小线段对应的位置姿态合成向量模量

与临界合成位移S_c(i)的大小关系，划分为三种情况：

第一种情况：

如图8所示，若

则说明以合成最大加加速J_{c_max}加速时，实际合成加速度恰好可以实现完整的三角形加速(等于完整三角形)，即实际合成加速度恰好可以达到合成最大加速度A_{c_max}，此时没有匀加速阶段，根据小线段起始点的合成正向首端速度，通过S曲线加减速算法得到小线段末端点的合成正向末端速度,图8中，S_{c_s}代表小线段起始点位移，S_{c_e}代表末端点位移，V_{c_s}代表起始点速度，V_{c_e}代表末端点速度。

其中第i段小线段的合成正向末端速度等于第i+1段小线段的合成正向首端速度，其中第1段小线段的合成正向首端速度等于0，即v_{c_s}(0)等于0。

第二种情况：

如图9所示，若

则说明以最大合成加加速度J_{c_max}加速时，实际合成加速度达到最大合成加速度A_{c_max}后保持一段时间匀加速阶段，之后按照合成最大加加速度-J_{c_max}，加速度减小至0(大于完整三角形)，根据小线段起始点的合成正向首端速度，通过S曲线加减速算法得到小线段末端点的合成正向末端速度。图9中，S_{c_s}代表小线段起始点位移，S_{c_e}代表末端点位移，V_{c_s}代表起始点速度，V_{c_e}代表末端点速度。

其中第i段小线段的合成正向末端速度等于第i+1段小线段的合成正向首端速度，其中第1段小线段的合成正向首端速度等于0，即v_{c_s}(0)等于0。

第三种情况：

如图10所示，若

则说明以合成最大加加速度J_{c_max}加速时，无法实现完整的三角形加速度(小于完整三角形)，即无法到达合成最大加速度A_{c_max}，此时需要将

代替临界合成位移计算公式中的S_c(i)，重新计算得到A_{c_max}′，获得实际的加速曲线，根据小线段起始点的合成正向首端速度，通过S曲线加减速算法得到小线段末端点的合成正向末端速度，图8中，S_{c_s}代表小线段起始点位移，S_{c_e}代表末端点位移，V_{c_s}代表起始点速度，V_{c_e}代表末端点速度。

其中第i段小线段的合成正向末端速度等于第i+1段小线段的合成正向首端速度，其中第1段小线段的合成正向首端速度等于0，即v_{c_s}(0)等于0。

步骤c：进行反向加速计算。

反向加速计算的计算过程与整形加速的计算过程相同，用于保证机器人能够在小线段结束前将运动速度减小至0。

反向加速计算中，根据小线段末端点的合成反向末端速度，利用S曲线加减速算法得到小线段起始点的合成反向首端速度，加工端合成反向首端速度等于前一个小线段的合成反向末端速度，计算方法与正向加速计算的方法相同，在此不进行重复叙述。

步骤d：将得到的合成正向末端速度、合成反向末端速度、合成最大速度比对，取最小者即为小线段的末端规划速度。

步骤e：根据步骤d得到的第i段小线段的末端规划速度及第i-1段的末端规划速度、位置姿态合成向量模量

以及步骤3中步骤a所述方法获得的合成位置姿态约束参数(合成最大速度V_{c_max}、合成最大加速度A_{c_max}(第三种情况下，使用A_{c_max}′)、合成最大加加速度J_{c_max})，采用S曲线加减速算法得到第i段小线段内多个插补点距离小线段起始点的位移s_c(i，t)

s_o(i，t)为第i段小线段内，t时刻的插补点与起始点的距离。

步骤f：完成插补后，对得到的位移进行分解，得到插补点位置插补向量和姿态插补向量，获得插补点的位置信息和姿态信息。

具体方法为：

在获得到位置插补向量P_p(i，t)、姿态插补向量P_o(i，t)后，再次分别在位置坐标系和姿态坐标系中分解获得位置信息x(i，t)、y(i，t)、z(i，t)和姿态信息a(i，t)、β(i，t)、γ(i，t)，其计算过程如下：

至此，实现了对于空间三维自由曲线生成的位置姿态信息的前瞻和插补，获得了高速、高精、平稳的运动控制信息，接下来可以驱动机器人，实现目标三维自由曲线运动。

本实施例的方法的一个验证实验中，采用型号Elfin3串联6自由度协作机器人本体，连接机器人控制器，机器人控制器采用标准的PC，装载Intel Core i5-4460 302GhzCPU、4.00GB内存和windows 10操作系统。为保证实时性，控制器通信周期为1ms。

实验目标轨迹为汽轮机叶片激光熔覆的一道曲线，如图12所示，在Matlab中可见该自由曲线轨迹由一系列小线段组成，小线段包含了该加工轨迹的大量位置、姿态信息，如图13所示。

使用本实施例的方法，构造了合成位置姿态向量

使用S曲线实现速度前瞻和插补处理，获得整个运动加工的所有插补点，如图14所示。图示三条曲线分别为合成位置姿态加速度变化曲线、合成位置姿态速度变化曲线合成位置姿态位移变化曲线。可以看到初始阶段小线段不断进行不完整三角形S曲线加速，在达到合成最大位置姿态速度后，进行匀速运动，然后在结束前不断进行不完整三角形S曲线减速，直到减速到0。图示中可见速度、位移曲线圆滑、连续。

使用插补获得的数据信息，可以解析得到机器人运动笛卡尔空间中每个插补周期的目标位置x、y、z信息和目标姿态α、β、γ信息，通过实时机器人控制器下发控制信息，使用反馈工具可获得机器人实际运动过程中反馈的关节位置信息和速度信息，具体如图15、16所示。图15中所示依次为关节1、关节2、关节3、关节4、关节5、关节6电机的反馈位置信息。图16所示依次为各个关节的速度信息关节1、关节2、关节3、关节4、关节5、关节6电机的反馈速度信息，图17为关节1电机速度曲线的局部放大图，可以看到关节速度平滑连续，综合图像信息可表明本实施例的方法能够实现位置、姿态同步前瞻，实现三维自由曲线运动的高速、高精、平稳的运动要求。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，包括：

建立加工轨迹每个小线段的位置向量和姿态向量，并根据建立的位置向量和姿态向量获得位置权因子和姿态权因子；具体步骤为：

位置权因子W_p和姿态权因子W_o的大小由小线段的位置向量模量总和S_{P_sum}与所有小线段的姿态向量模量总和θ_{o_sum}决定；

位置向量模量总和S_{P_sum}计算方法为：

其中，x_i+1、x_i分别为第i段小线段两端节点在位置坐标系中对应的X轴坐标值，y_i+1、y_i分别为第i段小线段两端节点在位置坐标系中对应的Y轴坐标值，z_i+1、z_i分别为第i段小线段两端节点在位置坐标系中对应的Z轴坐标值，n为小线段的总个数；

姿态向量模量总和θ_{o_sum}计算方法为：

其中，α_i+1、α_i分别为第i段小线段两端节点在姿态坐标系中α轴对应的坐标值，β_i+1、β_i分别为第i段小线段两端节点在姿态坐标系中β轴对应的坐标值，γ_i+1、γ_i分别为第i段小线段两端节点在姿态坐标系中γ轴对应的坐标值，n为小线段的总个数；

位置权因子W_p和姿态权因子W_o的计算方法如下：

根据获得的位置权因子和姿态权因子得到位置姿态合成向量，根据位置姿态合成向量得到位置姿态合成向量模量；

对小线段对应的位置姿态合成向量进行前瞻和插补得到小线段内多个插补点距离小线段起始点的位移；根据得到的位移对位置姿态合成向量进行分解，得到插补点位置插补向量和姿态插补向量，获得插补点的位置信息和姿态信息。

2.如权利要求1所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，根据实际加工工艺参数获得位置运动约束参数和姿态运动约束参数，并建立位置坐标系和姿态坐标系，构建每个小线段的位置向量和姿态向量，根据每个小线段的位置向量和姿态向量得到位置权因子和姿态权因子。

3.如权利要求1所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，相邻的两个小线段对应的位置姿态合成向量通过圆弧过渡连接，根据各个小线段的位置姿态合成向量计算相邻位置姿态合成向量的夹角，利用度数最大的夹角得到圆弧过渡的最小半径。

4.如权利要求1所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，对小线段的前瞻包括以下步骤：

根据位置运动约束参数、姿态运动约束参数、位置权因子和姿态权因子确定合成位置姿态约束参数；

根据合成位置姿态约束参数及荷载位置姿态向量模量分别对小线段进行正向加速计算和反向加速计算，分别得到小线段的合成正向末端速度和合成反向末端速度；

将得到的合成正向末端速度、合成反向末端速度、合成位置姿态约束参数比对，得到小线段的末端规划速度。

5.如权利要求4所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，正向加速计算中，根据小线段起始点的合成正向首端速度，利用S曲线加减速算法得到小线段末端点的合成正向末端速度，小线段合成正向末端速度等于下一个小线段的合成正向首端速度。

6.如权利要求5所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，根据小线段的合成正向首端速度及合成位置姿态约束参数计算临界合成位移，将临界合成位移与位置姿态合成向量模量进行比较，在不同的比较结果情况下利用S曲线加减速算法得到小线段合成正向末端速度。

7.如权利要求4所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，反向加速计算中，根据小线段末端点的合成反向末端速度，利用S曲线加减速算法得到小线段起始点的合成反向首端速度，加工端合成反向首端速度等于前一个小线段的合成反向末端速度。

8.如权利要求7所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，根据小线段合成反向末端速度及合成位置姿态约束参数计算临界合成位移，将临界合成位移与位置姿态合成向量模量进行比较，在不同比较结果情况下利用S曲线加减速算法得到小线段合成反向首端速度。

9.如权利要求4所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，选择合成正向末端速度、合成反向末端速度及合成位置姿态约束参数中的合成最大速度中的最小者为小线段的末端规划速度。

10.如权利要求4所述的一种用于机器人加工三维自由曲面的运动控制方法，其特征在于，根据小线段的末端规划速度及位置姿态合成向量模量，利用S曲线加减速算法得到小线段中多个插补点距离小线段首端的位移。

Images