CN113478481A - 一种工业机器人位置和姿态运动同步方法 - Google Patents

Abstract

本发明公开了一种工业机器人位置和姿态运动同步方法。首先，通过局部拟合得到位置曲线和姿态曲线，并求得每个运动单元的位置总弧长L_p,i和姿态总弧长L_Q,i。接着，根据原有线性路径的位置直线和姿态直线的长度比例，初步确定每个运动单元的初末姿态速度比例QVs_i和QVe_i。然后，求解各个运动单元的中值姿态速度比例QVm_i，调整QVs_i和QVe_i，以降低姿态速度比例QV_i的峰值。再根据运动整体的QVs_i、QVe_i、L_p,i和L_Q,i，设计位置和姿态同步曲线。最后，通过位置移动量S_P，计算姿态点在运动单元上位移量S_Q，实现位姿同步。本发明可应用于工业机器人位姿同步技术领域中。

Description

一种工业机器人位置和姿态运动同步方法

技术领域

本发明涉及位姿同步技术领域，特别涉及一种工业机器人位置和姿态运动同步方法。

背景技术

工业机器人以其灵活性高、操作空间大、适应性好、成本低等优点而广泛应用。为了解决机器人运动的线性路径连续性较低的问题，通常是采用样条拟合来对路径点形成的拐角进行平滑处理。样条拟合是在一定误差范围内用光滑曲线来拟合原有线性路径。现有技术中，针对局部式样条拟合，常用的位姿同步方法有：等效弧长等比例法和直线段拟合法。

等效弧长等比例方法使得位置曲线和姿态曲线耦合在一起，无法单独设计姿态曲线，在特殊运动路径下姿态曲线平滑性较差。直线段拟合法虽然可以实现位置曲线和姿态曲线的独立设计，但是当直线段较短时，姿态速度的振动会增大。

因此，对于自由设计的位置曲线和姿态曲线，若不进行位姿同步优化，姿态速度的变化幅值大，往往会破坏整个运动过程的平稳性，会导致机器人运动速度频繁变化，加工效率大幅降低，损害电机寿命。

发明内容

本发明目的在于提供一种工业机器人位置和姿态运动同步方法，以解决现有技术中所存在的一个或多个技术问题，至少提供一种有益的选择或创造条件。

为解决上述技术问题所采用的技术方案：

一种工业机器人位置和姿态运动同步方法，包括以下步骤：

步骤S1：通过局部拟合得到位置曲线和姿态曲线，以每个拐点处局部拟合生成的拼接曲线作为一个运动单元，对拟合得到的曲线进行积分计算，求得每个运动单元的位置总弧长L_p,i和姿态总弧长L_Q,i；

步骤S2：根据原有线性路径的位置直线和姿态直线的长度比例，初步确定每个运动单元的初末姿态速度比例QVs_i和QVe_i；

步骤S3：求解各个运动单元的中值姿态速度比例QVm_i，调整QVs_i和QVe_i，降低姿态速度比例QV_i的峰值；

步骤S4：根据运动整体的QVs_i、QVe_i、L_p,i和L_Q,i，来设计位置和姿态同步曲线；

步骤S5：通过位置移动量S_P，计算姿态点在运动单元上位移量S_Q，实现位姿同步。

本发明所提供的工业机器人位置和姿态运动同步方法，至少具有如下的有益效果：本发明可以对任意通过局部拟合得到的位置曲线和姿态曲线进行位姿同步，为拐点平滑过程中的姿态曲线提供了极大的设计空间，使得姿态曲线的形状能够得到优化，运动精度也得到保证；通过对整个运动过程中的所有运动单元的姿态速度比例进行优化，避免局部姿态速度比例设置不合理的问题，提高了整个运动过程中姿态变化的平稳性。本发明的工业机器人位置和姿态运动同步方法，通过设计位姿同步曲线来实现每个运动单元的位姿同步运动，并根据整个运动过程来优化调整姿态速度比例，实现整个运动过程的位姿同步。

作为上述技术方案的进一步改进，所述步骤S1具体地：所述位置曲线和姿态曲线的对应路径均具有G2以上的连续性，相邻的三个路径点能够形成所述拐点，所述运动单元的拼接曲线为“直线-曲线-直线”型。

作为上述技术方案的进一步改进，所述步骤S2具体地：

设三维空间中路径点的位置坐标为P_i，姿态坐标为Q_i；所述拐点的三个路径点为：(P_i-1，Q_i-1)、(P_i，Q_i)和(P_i+1，Q_i+1)；

设||P_iP_i-1||的值为L₁，||P_iP_i+1||的值为L₂，||Q_iQ_i-1||的值为L₃，||Q_iQ_i+1||的值为L₄；

运动单元的初末姿态速度比例QVs_i和QVe_i有：

相邻两个运动单元由于共享同一段线性路径，因此在连接处的姿态速度比例相同，即：

通过上述技术方案，可以初步确定每个运动单元的初末姿态速度比例，为调整姿态速度比例做准备。

作为上述技术方案的进一步改进，所述步骤S3具体地：

当位置速度为1，通过运动单元的总时间在数值上等于L_p,i；以L_p,i/2时刻的姿态速度比例为QVm_i，假设前后两段运动的加速度曲线均为对称式，得到QVm_i的计算公式：

将上式变形可得到QVm_i、QVs_i和QVe_i之间的关系：

通过上述技术方案，得出QVm_i、QVs_i和QVe_i之间的关系表达式，从表达式可以看出：QVs_i、QVe_i与QVm_i的变化关系相反，通过该关系可以调整QVs_i和QVe_i，使得整个运动路径上QVs_i、QVe_i与QVm_i的峰值减小。

作为上述技术方案的进一步改进，所述QVs_i和QVe_i的调整方法具体地：设路径点个数为n，共形成n-2个运动单元和n-1个姿态速度比例QV_i，通过以下步骤调整所述QVs_i和QVe_i：

步骤S3.1：通过QVm_i、QVs_i和QVe_i之间的关系式：

计算每个运动单元的中值姿态速度比例QVm_i；

步骤S3.2：除第一个和最后一个外，顺序遍历姿态比例速度QV_i，并比较与QV_i有关的两个中值姿态速度比例QVm_i-1和QVm_i：

若QVm_i-1小于QVm_i，QV_i的变化量：

若QVm_i-1大于或等于QVm_i，QV_i的变化量：

步骤S3.3：重复执行步骤3.2两次。

通过上述技术方案，姿态比例速度经过三次迭代后与两个中值姿态速度比例的数值相近，能够有效地降低整个运动路径上的姿态比例速度的峰值。

作为上述技术方案的进一步改进，所述步骤S4具体地：

设每个运动单元的位置曲线为B_i，姿态曲线为C_i，位姿同步要求姿态曲线在端点处对位置曲线的弧长s的一阶导数为姿态速度比例值，二阶导数为0，即：

上式中，下标left表示在左端点处，下标right表示在右端点处；

设两条直线型的同步曲线E(u)和F(u)，所述两条同步曲线E(u)和F(u)分别与位置曲线和姿态曲线长度相等，即在同步曲线上的虚拟运动点代表了在实际运动单元的位置曲线和姿态曲线上的移动距离；

E(u)依次设有六个控制点E_i(i＝0,1,…,5)，其中E₀和E₅为端点；E₀、E₁、E₂的间距为c₁并设于中点的一侧，E₃、E₄、E₅的间距为c₂并设于中点的另一侧；E₂和E₃与中点的距离分别为d₁和d₂。

同样地，F(u)上存在F_i(i＝0,1,…,5)和相应的距离参数c₃、c₄、d₃、d₄；

姿态同步曲线F(u)在端点处对弧长s的导数为：

又因为运动点在同步曲线和实际曲线上运动距离相同，因此姿态同步曲线和实际姿态曲线对弧长s的导数相同，即：

联合上述两式，可得：

设控制点距离参数c不长于整个曲线的长度的1/6，得出控制点参数c的约束方程：

即：

随之可以确定控制点参数d：

根据上述两式可求得同步曲线E(u)和F(u)的所有控制点。

通过上述技术方案，能够得出同步曲线E(u)和F(u)的控制点参数，从而确认两条所述同步曲线，为姿态曲线和位置曲线提供同步过渡。

作为上述技术方案的进一步改进，所述步骤S5具体地：

当位置移动距离为S_P时，机器人位置移动到运动单元的P_move处；

从位置同步曲线E(u)上找到与起点距离为S_P的点E(u₀)和对应的参数值u₀；

在姿态同步曲线F(u)找到对应参数值的点F(u₀)，并计算出F(u₀)与起点的距离S_Q；

在运动单元上截取移动距离为S_Q的姿态点Q_move。

通过上述技术方案，不同的姿态点能够与位置运动点一一对应，位姿运动实现优化同步，保持运动过程中姿态变化的平稳性。

附图说明

下面结合附图和实施例对本发明做进一步的说明；

图1是本发明所提供的工业机器人位置和姿态运动同步方法，其一实施例的算法流程图；

图2是本发明所提供的工业机器人位置和姿态运动同步方法，其一实施例的局部拟合得到的位姿路径示意图；

图3是本发明所提供的工业机器人位置和姿态运动同步方法，其一实施例的两条同步曲线示意图；

图4是本发明所提供的工业机器人位置和姿态运动同步方法，其一实施例的同步过程示意图；

图5是本发明所提供的工业机器人位置和姿态运动同步方法，其一实施例的位置曲线以及同步前后的姿态曲线示意图。

具体实施方式

本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案，但其不能理解为对本发明保护范围的限制。

在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，如果具有“若干”之类的词汇描述，其含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。

本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。

参照图1至图5，本发明的工业机器人位置和姿态运动同步方法作出如下实施例：

一种工业机器人位置和姿态运动同步方法，包括以下步骤：

步骤S1：

通过局部拟合得到位置曲线和姿态曲线，以每个拐点处局部拟合生成的拼接曲线作为一个运动单元。

所述位置曲线和姿态曲线的对应路径均具有G2以上的连续性，相邻的三个路径点能够形成所述拐点，所述运动单元的拼接曲线为“直线-曲线-直线”型。

以利用5次Bezier曲线进行局部拟合为例。参照图2，局部拟合所得的位置曲线和姿态曲线，在每个拐点处都可以得到一条5次Bezier曲线、以及分别与曲线两端连接的两条直线；以此“直线-曲线-直线”型的拼接曲线为一个运动单元来进行同步处理。

对拟合得到的曲线进行积分计算，求得每个运动单元的位置总弧长L_p,i和姿态总弧长L_Q,i。

步骤S2：

初步确定每个运动单元的初末姿态速度比例QVs_i和QVe_i：

设三维空间中路径点的位置坐标为P_i，姿态坐标为Q_i；所述拐点的三个路径点为：(P_i-1，Q_i-1)、(P_i，Q_i)和(P_i+1，Q_i+1)。

原有线性路径的位置直线和姿态直线的长度可如下设定：||P_iP_i-1||的值为L₁，||P_iP_i+1||的值为L₂，||Q_iQ_i-1||的值为L₃，||Q_iQ_i+1||的值为L₄。

根据线性路径上位置直线和姿态直线的长度比例，初步确定运动单元的初末姿态速度比例QVs_i和QVe_i：

由于相邻两个运动单元由于共享同一段线性路径，因此在连接处的姿态速度比例相同，即：

步骤S3：

求解各个运动单元的中值姿态速度比例QVm_i：

当位置速度为1，通过运动单元的总时间在数值上等于L_p,i；以L_p,i/2时刻的姿态速度比例为QVm_i，假设前后两段运动的加速度曲线均为对称式，得到QVm_i的计算公式：

将上式变形可得到QVm_i、QVs_i和QVe_i之间的关系：

从上式可以看出：QVs_i、QVe_i与QVm_i的变化关系为负相关，因此可以调整QVs_i和QVe_i，使得整个运动路径上QVs_i、QVe_i与QVm_i的峰值减小。

设路径点个数为n，共形成n-2个运动单元和n-1个姿态速度比例QV_i；QVs_i和QVe_i的具体调整方法包括以下步骤：

步骤S3.1：通过QVm_i、QVs_i和QVe_i之间的关系式：

计算每个运动单元的中值姿态速度比例QVm_i；

步骤S3.2：除第一个和最后一个外，顺序遍历姿态比例速度QV_i，并比较与QV_i有关的两个中值姿态速度比例QVm_i-1和QVm_i：

若QVm_i-1小于QVm_i，QV_i的变化量：

若QVm_i-1大于或等于QVm_i，QV_i的变化量：

步骤S3.3：有限次地重复执行步骤3.2。

由经验所得，QV_i的变化量经过三次迭代之后，与两个中值姿态速度比例QVm_i-1和QVm_i的数值相近，继续重复迭代的意义不大，因此在本实施例中，步骤3.2的重复次数以两次为佳。

步骤S4：

设计位置和姿态同步曲线：

设每个运动单元的位置曲线为B_i，姿态曲线为C_i，位姿同步要求姿态曲线在端点处对位置曲线的弧长s的一阶导数为姿态速度比例值，二阶导数为0，即：

上式中，下标left表示在左端点处，下标right表示在右端点处。

本实施例中，利用5次Bezier曲线设计两条直线型的同步曲线E(u)和F(u)。在其他一些实施例中，还可以利用其他曲线进行设计，如双4次Bezier曲线、5次B样条曲线、多项式等，只需要满足位姿同步曲线在端点处G2及以上连续即可。如图3所示，所述两条同步曲线E(u)和F(u)分别与位置曲线和姿态曲线长度相等，即在同步曲线上的虚拟运动点代表了在实际运动单元在位置曲线和姿态曲线上的移动距离。因此，两条同步曲线E(u)和F(u)的中点分别为曲线长度L_p,i和L_Q,i的中值对应的点。

E(u)依次设有六个控制点E_i(i＝0,1,…,5)，其中E₀和E₅为端点。E₀、E₁、E₂的间距为c₁并设于中点的一侧，E₃、E₄、E₅的间距为c₂并设于中点的另一侧；E₂和E₃与中点的距离分别为d₁和d₂。

如图3所示，同理可得，F(u)上存在的六个控制点F_i(i＝0,1,…,5)和相应的距离参数c₃、c₄、d₃、d₄。

姿态同步曲线F(u)在端点处对弧长s的导数为：

又因为运动点在同步曲线和实际曲线上运动距离相同，因此姿态同步曲线和实际姿态曲线对弧长s的导数相同，即：

联合上述两式，可得：

出于均匀设置的目的，设控制点距离参数c不长于整个曲线的长度的1/6，从而得出控制点参数c的约束方程：

即：

随之可以确定控制点参数d：

根据上述两式可求得同步曲线E(u)和F(u)的所有控制点。

步骤S5：

根据获得的位姿同步曲线E(u)和F(u)，可以实现运动单元的同步运动：

如图4所示，当位置移动距离为S_P时，机器人位置移动到运动单元的P_move处。

从位置同步曲线E(u)上找到与起点距离为S_P的点E(u₀)和对应的参数值u₀；在姿态同步曲线F(u)找到对应参数值的点F(u₀)，并计算出F(u₀)与起点的距离S_Q；

在运动单元上截取移动距离为S_Q的姿态点Q_move。

根据不同的姿态点Q_move与位置运动点P_move一一对应，使得位姿实现同步；经过位姿同步优化后，姿态曲线的内部姿态速度、加速度是连续的，姿态速度比例根据整个运动过程来优化调整，大大增强了姿态曲线的平滑程度，保证了运动过程中姿态变化的平稳性(如图5所示，图中a为位置曲线图，b为同步前的姿态曲线图，c为同步后的姿态曲线图)。

以上对本发明的较佳实施方式进行了具体说明，但本发明并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变型或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。

Claims (8)

1.一种工业机器人位置和姿态运动同步方法，其特征在于：包括以下步骤：

步骤S1：通过局部拟合得到位置曲线和姿态曲线，以每个拐点处局部拟合生成的拼接曲线作为一个运动单元，并求得每个运动单元的位置总弧长L_p,i和姿态总弧长L_Q,i；

步骤S2：根据原有线性路径的位置直线和姿态直线的长度比例，初步确定每个运动单元的初末姿态速度比例QVs_i和QVe_i；

步骤S3：求解各个运动单元的中值姿态速度比例QVm_i，调整QVs_i和QVe_i，以降低姿态速度比例QV_i的峰值；

步骤S4：根据运动整体的QVs_i、QVe_i、L_p,i和L_Q,i，来设计位置和姿态同步曲线；

步骤S5：通过位置移动量S_P，计算姿态点在运动单元上位移量S_Q，实现位姿同步。

2.根据权利要求1所述的工业机器人位置和姿态运动同步方法，其特征在于：所述步骤S1具体地：所述位置曲线和姿态曲线的对应路径均具有G2以上的连续性，相邻的三个路径点能够形成所述拐点，所述运动单元的拼接曲线为“直线-曲线-直线”型。

3.根据权利要求1所述的工业机器人位置和姿态运动同步方法，其特征在于：所述步骤S2具体地：

设三维空间中相邻路径点在位置直线上的间距为L₁和L₂，在姿态直线上的间距为L₃和L₄；

运动单元的初末姿态速度比例QVs_i和QVe_i有：

相邻两个运动单元由于共享同一段线性路径，因此在连接处的姿态速度比例相同，即：

4.根据权利要求1所述的工业机器人位置和姿态运动同步方法，其特征在于：所述步骤S3具体地：

以L_p,i/2时刻的姿态速度比例为QVm_i，假设前后两段运动的加速度曲线均为对称式，得到QVm_i的计算公式：

将上式变形可得到QVm_i、QVs_i和QVe_i之间的关系：

5.根据权利要求4所述的工业机器人位置和姿态运动同步方法，其特征在于：

设路径点个数为n，共形成n-2个运动单元和n-1个姿态速度比例QV_i，所述QVs_i和QVe_i的调整方法具体地：

步骤S3.1：通过QVm_i、QVs_i和QVe_i之间的关系式计算每个运动单元的中值姿态速度比例QVm_i；

步骤S3.2：除第一个和最后一个外，顺序遍历姿态比例速度QV_i，并比较与QV_i有关的两个中值姿态速度比例QVm_i-1和QVm_i：

若QVm_i-1小于QVm_i，QV_i的变化量：

若QVm_i-1大于或等于QVm_i，QV_i的变化量：

步骤S3.3：有限次地重复执行步骤S3.2。

6.根据权利要求5所述的工业机器人位置和姿态运动同步方法，其特征在于：所述步骤S3.3中，重复执行步骤S3.2两次。

7.根据权利要求1所述的工业机器人位置和姿态运动同步方法，其特征在于：所述步骤S4具体地：

设每个运动单元的位置曲线为B_i，姿态曲线为C_i，位姿同步要求姿态曲线在端点处对位置曲线的弧长s的一阶导数为姿态速度比例值，二阶导数为0，即：

上式中，下标left表示在左端点处，下标right表示在右端点处；

设两条直线型的同步曲线E(u)和F(u)，所述两条同步曲线E(u)和F(u)分别与位置曲线和姿态曲线长度相等，即在同步曲线上的虚拟运动点代表了在实际运动单元的位置曲线和姿态曲线上的移动距离；

E(u)依次设有六个控制点E_i(i＝0,1,…,5)，其中E₀和E₅为端点，

同样地，F(u)上存在Fi(i＝0,1,…,5)和相应的距离参数；

姿态同步曲线F(u)在端点处对弧长s的导数为：

又因为运动点在同步曲线和实际曲线上运动距离相同，因此姿态同步曲线和实际姿态曲线对弧长s的导数相同，即：

联合上述两式，可得：

设控制点距离参数c不长于整个曲线的长度的1/6，得出控制点参数c的约束方程：

即：

随之可以确定控制点参数d：

根据上述两式可求得同步曲线E(u)和F(u)的所有控制点。

8.根据权利要求7所述的工业机器人位置和姿态运动同步方法，其特征在于：所述步骤S5具体地：

当位置移动距离为S_P时，从位置同步曲线E(u)上找到与起点距离为S_P的点E(u₀)和对应的参数值u₀，在姿态同步曲线F(u)找到对应参数值的点F(u₀)，并计算出F(u₀)与起点的距离S_Q。

Images