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

技术领域

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

背景技术

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

文献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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附图说明

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

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

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

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

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

具体实施方式

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

实施例

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

步骤1、读取工业机器人笛卡尔空间下的姿态路径代码，得到一系列姿态向量P

步骤2、把姿态向量P

步骤2-1、采用如下公示计算单位四元数Q

步骤2-2、采用如下公示计算单位四元数Q

n

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

Q

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

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

步骤3-2、对从第一个姿态开始的三个相邻姿态四元数Q

θ

θ

取θ

Θ

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

根据姿态四元数Q

其中，f(Θ

步骤3-3、对于从第二个姿态至倒数第二个姿态结束的三个相邻姿态四元数Q

步骤3-4、对从倒数第三个姿态开始的三个相邻姿态四元数Q

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

步骤4-1、由转接路径段的角位移Θ

步骤4-2、对姿态起点Q

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

式中，T

s

s

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

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

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

H(x)＝b

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

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

步骤5-1、计算姿态四元数Q

Q

步骤5-2、采用五次多项式对转轴向量Q

Q

其中f(Θ)＝6Θ

本实施例中，对转轴向量Q

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

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

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

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