CN115958597A - 一种工业机器人连续姿态路径光顺方法及*** - Google Patents

Abstract

本发明属于机器人路径规划领域，为工业机器人连续姿态路径光顺方法及***，包括步骤：读取工业机器人的姿态路径代码，得到一系列姿态向量；把姿态向量转化为单位四元数；依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；采用五次多项式对余留姿态路径段的姿态进行插补；对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。本发明大幅降低了姿态转接时的曲率峰值，减小了姿态路径拐角光顺所需要的角位移，提高了工业机器人的运动平稳性。

Description

一种工业机器人连续姿态路径光顺方法及***

技术领域

本发明属于机器人路径规划领域，具体涉及一种工业机器人连续姿态路径光顺方法及***。

背景技术

在工业机器人路径规划领域，用于控制机器人终端实际转动的命令是通过对机器人终端给定的运动姿态信息插补得到。由于给定的运动姿态信息为离散数据点，对其直接线性插补会导致在每一个姿态处的曲率不连续，尤其是在姿态变化特别频繁时，造成机器人在每一个姿态处频繁加减速。为了解决机器人在姿态转接时的频繁加减速问题，提高机器人终端的转动速度，需要对姿态路径进行转接过渡处理。采用高阶连续曲线光顺转接处的姿态可以保证光顺后的姿态路径满足角加速度连续。然而，对于姿态变化比较频繁的工况，如何减小转接处所需要的角位移成为了姿态路径光顺亟需解决的问题。

文献1“Pu Y,Shi Y,Lin X,et al.C2-Continuous Orientation Planning forRobot End-Effector with B-Spline Curve Based on Logarithmic Quaternion[J].Mathematical Problems in Engineering,2020.”公开了一种基于对数四元数的高效姿态四元数插值方法。该方法把旋转空间S³中的姿态四元数转化为三维空间R³中的点，维度降阶后采用B样条曲线进行姿态光顺，B样条曲线的C2连续性保证了光顺后姿态路径的C2连续，但是无法保证姿态的加速度连续且姿态路径的曲率未知。文献2“王效杰.基于四元数样条曲线的姿态轨迹规划研究[D].西南科技大学.2015”公开了一种基于四元数样条曲线的姿态路径过渡方法。该方法通过对给定四元数姿态、速度和加速度条件建立了四元数平滑过渡曲线模型，但是该方法只保证了姿态转接的C2连续。文献3“Huang J,Du X,Zhu LM.Real-time local smoothing for five-axis linear toolpath consideringsmoothing error constraints[J].International Journal of Machine Tools andManufacture,2018,124:67-79”公开了一种基于三次B样条曲线的姿态光顺方法，该方法分析了姿态转接处的曲率特点，满足姿态路径的G2连续。文献4“Herzog R,Blanc P.OptimalG2 Hermite interpolation for 3D curves[J].Computer-Aided Design,2019,117:102752.”公开了一种G2连续的三维路径过渡算法，但是没有研究姿态路径光顺情况。于2021年1月15日公开的专利文献CN112223290A“一种关节空间连续点轨迹过渡方法及装置”，采用轨迹允许的最大速度约束和贝塞尔曲线实现了位置的过渡，通过修正过渡起始姿态和过渡结束姿态完成关节空间连续点轨迹过渡，但是该方法不能保证过渡姿态的高阶连续。

如上所述，通过分析已有的离散姿态路径光顺方法可知，现有技术光顺后的姿态路径实现了一定的姿态连续性和速度连续性，但是并没有对曲率进行优化，对过渡姿态路径比较短的姿态光顺情况不适用。

发明内容

为克服现有技术中存在的缺陷，本发明提供一种工业机器人连续姿态路径光顺方法及***，结合单位四元数和七次埃尔米特样条曲线，大幅降低了姿态转接时的曲率峰值，减小了姿态路径拐角光顺所需要的角位移，提高了工业机器人的运动平稳性。

本发明方法所采用的技术方案为：一种工业机器人连续姿态路径光顺方法，包括以下步骤：

读取工业机器人的姿态路径代码，得到一系列姿态向量；

把姿态向量转化为单位四元数；

依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；

采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；

采用五次多项式对余留姿态路径段的姿态进行插补；

对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。

本发明***所采用的技术方案为：一种工业机器人G3连续姿态路径光顺***，包括以下模块：

姿态向量获取模块，用于读取工业机器人的姿态路径代码，得到一系列姿态向量；

姿态向量转化模块，用于把姿态向量转化为单位四元数；

路径段获取模块，用于依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；

插补模块，用于采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；以及采用五次多项式对余留姿态路径段的姿态进行插补；

拼接模块，用于对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。

与现有技术相比，本发明取得的技术效果包括：

本发明结合单位四元数和七次埃尔米特样条曲线，大幅降低了姿态转接时的曲率峰值，减小了姿态路径拐角光顺所需要的角位移，适用于过渡姿态路径比较短的姿态光顺情况，提高了工业机器人的运动平稳性。

附图说明

图1是本发明实施例中工业机器人连续姿态路径光顺方法的流程图。

图2是本发明实施例中未光顺的姿态路径图。

图3是本发明实施例中光顺后的姿态路径图。

图4是采用本发明和文献3中的方法1、文献4中的方法2进行姿态路径光顺后的曲率对比图。

图5是采用本发明方法进行姿态路径光顺后插补得到的姿态欧拉角。

具体实施方式

下面结合实施例和附图对本发明的技术方案做进一步详细的描述，但本发明的实施方式并不限于此。

实施例

本实施例提供一种工业机器人连续姿态路径光顺方法，如图1所示，以工业机器人G3为例，对路径光顺方法所包括的主要步骤，详细描述如下：

步骤1、读取工业机器人笛卡尔空间下的姿态路径代码，得到一系列姿态向量P_i(x_i,y_i,z_i)，其中i＝1,2,3…,N，x_i、y_i、z_i分别是第i个姿态在x轴、y轴和z轴上的分量，N是姿态路径代码中给定的姿态个数，本实施例中N＝5；未光顺的姿态路径如图2所示。

步骤2、把姿态向量P_i转化为单位四元数Q_i，具体步骤包括：

步骤2-1、采用如下公示计算单位四元数Q_i中的转角：

步骤2-2、采用如下公示计算单位四元数Q_i的转轴向量：

n_i＝(x_i/θ_i,y_i/θ_i,z_i/θ_i)

步骤2-3、根据单位四元数的转角和转轴向量建立姿态的单位四元数，如下公式：

Q_i＝[cos(θ_i/2)sin(θ_i/2)·n_i]

步骤3、依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段，具体步骤包括：

步骤3-1、根据机器人过渡精度需求确定转接比例系数ε，其中0<ε<0.5，本实施例取ε＝0.3；

步骤3-2、对从第一个姿态开始的三个相邻姿态四元数Q_i(i＝1，2，3)，即Q₁、Q₂和Q₃，计算四元数Q₁和Q₂之间的角位移θ₁、Q₂和Q₃之间的角位移θ₂，如下式所示：

θ₁＝2·acos[Q₁(1)·Q₂(1)+Q₁(2)·Q₂(2)+Q₁(3)·Q₂(3)+Q₁(4)·Q₂(4)]

θ₂＝2·acos[Q₂(1)·Q₃(1)+Q₂(2)·Q₃(2)+Q₂(3)·Q₃(3)+Q₂(4)·Q₃(4)]

取θ₁和θ₂中的较小值乘以转接比例系数ε，得到姿态四元数Q₂处的转接路径段Θ_s1，如下式所示：

Θ_s1＝min{θ₁,θ₂}·ε

式中，min表示取最小值函数。

根据姿态四元数Q₁和转接姿态路径段的起点，构成第一段余留姿态路径段Θ_r1；并对第一段余留姿态路径段Θ_r1进行插补，如下式所示：

其中，f(Θ_r1/θ₁)是余留姿态路径段的插补函数，见步骤5。

步骤3-3、对于从第二个姿态至倒数第二个姿态结束的三个相邻姿态四元数Q_i-1、Q_i和Q_i+1(i＝3,4…,N-2)，即Q₂、Q₃和Q₄，采用类似于步骤3-2的方法，计算姿态四元数Q_i-1和Q_i之间的角位移θ_i-1、姿态四元数Q_i和Q_i+1之间的角位移θ_i，取角位移θ_i-1和θ_i两者之间较小值乘以转接比例系数得到转接路径段的角位移Θ_s,i-1，取角位移Θ_s,i-1和Θ_s,i-2中的较小值作为姿态四元数Q_i处的转接路径段Θ_s,i-1，根据姿态四元数Q_i-1和转接姿态路径段起点构成第i-1段余留姿态路径段Θ_r,i-1；并对第i-1段余留姿态路径段Θ_r,i-1进行插补，如下式所示：

步骤3-4、对从倒数第三个姿态开始的三个相邻姿态四元数Q_N-2、Q_N-1和Q_N，，计算姿态四元数Q_N-2和Q_N-1之间的角位移θ_N-2、姿态四元数Q_N-1和Q_N之间的角位移θ_N-1，取角位移θ_N-2和θ_N-1两者之间较小值乘以转接比例系数得到姿态四元数Q_N-1处的转接路径段Θ_s,N-2，根据姿态四元数Q_N-2和转接姿态路径段起点构成第N-2段余留姿态路径段Θ_r,N-2，根据姿态四元数Q_N和转接姿态路径段起点构成第N-1段余留姿态路径段Θ_r,N-1；并对Θ_r,N-2和Θ_r,N-1进行插补，如下式所示：

步骤4、采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补，具体步骤包括：

步骤4-1、由转接路径段的角位移Θ_s,i-2得到姿态四元数Q_i转接时的姿态起点Q_i,s和姿态终点Q_i,e；

步骤4-2、对姿态起点Q_i,s、姿态四元数Q_i和姿态终点Q_i,e采用七次埃尔米特样条曲线光顺，具体步骤包括：

步骤4-2-1、计算七次埃尔米特样条曲线的控制点，如下式所示：

式中，T₀＝(Q_i-Q_i,s)/||Q_i-Q_i,s||，T₁＝(Q_i,e-Q_i)/||Q_i,e-Q_i||，a₀、a₁、r₀、r₁、s₀和s₁为用于控制曲线形状的大于0的常数，其中：

s₀＝0；

s₁＝0。

步骤4-2-2、建立伯恩斯坦多项式，如下式所示：

其中，x为七次埃尔米特样条曲线的归一化参数变量，由机器人姿态路径的插补周期和运动时间确定，x∈[0,1]。

步骤4-2-3、建立七次埃尔米特样条曲线方程，如下式所示：

H(x)＝b₀B₀(x)+b₁B₁(x)+b₂B₂(x)+b₃B₃(x)+b₄B₄(x)+b₅B₅(x)+b₆B₆(x)+b₇B₇(x)。

步骤4-3、通过单位化七次埃尔米特样条曲线方程H(x)得到转接姿态路径段的插补姿态Q_tH。

步骤5、采用五次多项式对余留姿态路径段的姿态进行插补，具体步骤如下：

步骤5-1、计算姿态四元数Q_i-1和姿态起点Q_i,s的转轴向量Q_m，如下式所示：

Q_m＝Q_i-1 ^-1Q_i,s

步骤5-2、采用五次多项式对转轴向量Q_m进行插补，如下式所示：

Q_tm＝Q_i-1·(Q_m)^f(Θ)

其中f(Θ)＝6Θ⁵-15Θ⁴+10Θ³，Θ为步骤3中插补函数f里面的自变量参数。

本实施例中，对转轴向量Q_m进行插补的具体公式为：

其中，x为[0,1]之间等分的参数。

步骤6、对3个转接姿态路径段插补后的姿态和4个余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人G3连续的姿态光顺路径；通过把四元数转化为三维单位向量，可以把光顺后的姿态路径表示在单位球上，如图3所示。

图4展示了本发明光顺姿态是的曲率变化情况，和现有的两种方法对比可以看出，本发明方法光顺姿态路径时，曲率峰值更小，从而可以使机器人以更高的角速度通过姿态转接处。另外，图4表明采用本发明方法光顺后的姿态路径更短，因此机器人的整体运动时间会进一步缩短。最后，根据姿态四元数转化得到的姿态欧拉角如图5所示。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可对上述实施例进行变化、修改、替换和变形。

Claims (10)

1.一种工业机器人G3连续姿态路径光顺方法，其特征在于，包括以下步骤：

读取工业机器人的姿态路径代码，得到一系列姿态向量；

把姿态向量转化为单位四元数；

依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；

采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；

采用五次多项式对余留姿态路径段的姿态进行插补；

对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。

2.根据权利要求1所述的方法，其特征在于，所述姿态向量为P_i(x_i,y_i,z_i)，其中i＝1,2,3…,N，x_i、y_i、z_i分别是第i个姿态在x轴、y轴和z轴上的分量，N是姿态路径代码中给定的姿态个数；

所述把姿态向量转化为单位四元数，包括：

计算单位四元数中的转角：

计算单位四元数的转轴向量：n_i＝(x_i/θ_i,y_i/θ_i,z_i/θ_i)；

根据单位四元数的转角和转轴向量建立姿态向量的单位四元数Q_i：

Q_i＝[cos(θ_i/2)sin(θ_i/2)·n_i]。

3.根据权利要求1所述的方法，其特征在于，所述姿态向量为P_i(x_i,yi,z_i)，其中i＝1,2,3…,N，x_i、y_i、z_i分别是第i个姿态在x轴、y轴和z轴上的分量，N是姿态路径代码中给定的姿态个数；

所述依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段，包括：

根据机器人过渡精度需求确定转接比例系数ε；

对从第一个姿态开始的三个相邻姿态四元数Q₁、Q₂和Q₃，计算四元数Q₁和Q₂之间的角位移θ₁、Q₂和Q₃之间的角位移θ₂；取角位移θ₁和θ₂中的较小值乘以转接比例系数ε，得到姿态四元数Q₂处的转接路径段Θ_s1；根据姿态四元数Q₁和转接姿态路径段的起点，构成第一段余留姿态路径段Θ_r1；并对第一段余留姿态路径段Θ_r1进行插补；

对从第二个姿态至倒数第二个姿态结束的三个相邻姿态四元数Q_i-1、Q_i和Q_i+1，i＝3,4…,N-2，计算姿态四元数Q_i-1和Q_i之间的角位移θ_i-1、姿态四元数Q_i和Q_i+1之间的角位移θ_i，取角位移θ_i-1和θ_i两者之间较小值乘以转接比例系数得到转接路径段的角位移Θ_s,i-1；取角位移Θ_s,i-1和Θ_s,i-2中的较小值作为姿态四元数Q_i处的转接路径段Θ_s,i-1，根据姿态四元数Q_i-1和转接姿态路径段起点构成第i-1段余留姿态路径段Θ_r,i-1；并对第i-1段余留姿态路径段Θ_r,i-1进行插补；

对从倒数第三个姿态开始的三个相邻姿态四元数Q_N-2、Q_N-1和Q_N，计算姿态四元数Q_N-2和Q_N-1之间的角位移θ_N-2、姿态四元数Q_N-1和Q_N之间的角位移θ_N-1，取角位移θ_N-2和θ_N-1两者之间较小值乘以转接比例系数得到姿态四元数Q_N-1处的转接路径段Θ_s,N-2，根据姿态四元数Q_N-2和转接姿态路径段起点构成第N-2段余留姿态路径段Θ_r,N-2，根据姿态四元数Q_N和转接姿态路径段起点构成第N-1段余留姿态路径段Θ_r,N-1；并对Θ_r,N-2和Θ_r,N-1进行插补。

4.根据权利要求3所述的方法，其特征在于，所述采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补，包括：

由转接路径段的角位移Θ_s,i-2得到姿态四元数Q_i转接时的姿态起点Q_i,s和终点Q_i,e；

对姿态起点Q_i,s、姿态四元数Q_i和姿态终点Q_i,e采用七次埃尔米特样条曲线光顺，包括：计算七次埃尔米特样条曲线的控制点，建立伯恩斯坦多项式，根据控制点和伯恩斯坦多项式得到七次埃尔米特样条曲线方程；

通过单位化七次埃尔米特样条曲线方程得到转接姿态路径段的插补姿态。

5.根据权利要求4所述的方法，其特征在于，所述采用五次多项式对余留姿态路径段的姿态进行插补，包括：

计算姿态四元数Q_i-1和姿态起点Q_i,s的转轴向量Q_m：Q_m＝Q_i-1 ^-1Q_i,s；

采用五次多项式对转轴向量Q_m进行插补：Q_tm＝Q_i-1·(Q_m)^f(Θ)；

其中f(Θ)＝6Θ⁵-15Θ⁴+10Θ³，Θ为插补函数的自变量参数。

6.根据权利要求4所述的方法，其特征在于，所述计算七次埃尔米特样条曲线的控制点，公式为：

式中，T₀＝(Q_i-Q_i,s)/||Q_i-Q_i,s||，T₁＝(Q_i,e-Q_i)/||Q_i,e-Q_i||，a₀、a₁、r₀、r₁、s₀和s₁为用于控制曲线形状的大于0的常数；

所述伯恩斯坦多项式为：

其中，x为七次埃尔米特样条曲线的归一化参数变量；

所述七次埃尔米特样条曲线方程为：

H(x)＝b₀B₀(x)+b₁B₁(x)+b₂B₂(x)+b₃B₃(x)+b₄B₄(x)+b₅B₅(x)+b₆B₆(x)+b₇B₇(x)。

7.一种工业机器人G3连续姿态路径光顺***，其特征在于，包括以下模块：

姿态向量获取模块，用于读取工业机器人的姿态路径代码，得到一系列姿态向量；

姿态向量转化模块，用于把姿态向量转化为单位四元数；

路径段获取模块，用于依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；

插补模块，用于采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；以及采用五次多项式对余留姿态路径段的姿态进行插补；

拼接模块，用于对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。

8.根据权利要求7所述的***，其特征在于，所述姿态向量为P_i(x_i,y_i,z_i)，其中i＝1,2,3…,N，x_i、y_i、z_i分别是第i个姿态在x轴、y轴和z轴上的分量，N是姿态路径代码中给定的姿态个数；

所述路径段获取模块依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段，包括：

根据实际需求确定转接比例系数ε；

对从第一个姿态开始的三个相邻姿态四元数Q₁、Q₂和Q₃，计算四元数Q₁和Q₂之间的角位移θ₁、Q₂和Q₃之间的角位移θ₂；取角位移θ₁和θ₂中的较小值乘以转接比例系数ε，得到姿态四元数Q₂处的转接路径段Θ_s1；根据姿态四元数Q₁和转接姿态路径段的起点，构成第一段余留姿态路径段Θ_r1；并对第一段余留姿态路径段Θ_r1进行插补；

对从第二个姿态至倒数第二个姿态结束的三个相邻姿态四元数Q_i-1、Q_i和Q_i+1，i＝3,4…,N-2，计算姿态四元数Q_i-1和Q_i之间的角位移θ_i-1、姿态四元数Q_i和Q_i+1之间的角位移θ_i，取角位移θ_i-1和θ_i两者之间较小值乘以转接比例系数得到转接路径段的角位移Θ_s,i-1；取角位移Θ_s,i-1和Θ_s,i-2中的较小值作为姿态四元数Q_i处的转接路径段Θ_s,i-1，根据姿态四元数Q_i-1和转接姿态路径段起点构成第i-1段余留姿态路径段Θ_r,i-1；并对第i-1段余留姿态路径段Θ_r,i-1进行插补；

对从倒数第三个姿态开始的三个相邻姿态四元数Q_N-2、Q_N-1和Q_N，计算姿态四元数Q_N-2和Q_N-1之间的角位移θ_N-2、姿态四元数Q_N-1和Q_N之间的角位移θ_N-1，取角位移θ_N-2和θ_N-1两者之间较小值乘以转接比例系数得到姿态四元数Q_N-1处的转接路径段Θ_s,N-2，根据姿态四元数Q_N-2和转接姿态路径段起点构成第N-2段余留姿态路径段Θ_r,N-2，根据姿态四元数Q_N和转接姿态路径段起点构成第N-1段余留姿态路径段Θ_r,N-1；并对Θ_r,N-2和Θ_r,N-1进行插补。

9.根据权利要求8所述的***，其特征在于，所述插补模块采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补，包括：

由转接路径段的角位移Θ_s,i-2得到姿态四元数Q_i转接时的姿态起点Q_i,s和终点Q_i,e；

对姿态起点Q_i,s、姿态四元数Q_i和姿态终点Q_i,e采用七次埃尔米特样条曲线光顺，包括：计算七次埃尔米特样条曲线的控制点，建立伯恩斯坦多项式，建立七次埃尔米特样条曲线方程；

通过单位化七次埃尔米特样条曲线方程得到转接姿态路径段的插补姿态。

10.根据权利要求9所述的***，其特征在于，所述插补模块采用五次多项式对余留姿态路径段的姿态进行插补，包括：

计算姿态四元数Q_i-1和姿态起点Q_i,s的转轴向量Q_m：Q_m＝Q_i-1 ^-1Q_i,s；

采用五次多项式对转轴向量Q_m进行插补：Q_tm＝Q_i-1·(Q_m)^f(Θ)；

其中f(Θ)＝6Θ⁵-15Θ⁴+10Θ³，Θ为插补函数的自变量参数。

Images