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

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

【技术实现步骤摘要】
一种工业机器人连续姿态路径光顺方法及系统


[0001]本专利技术属于机器人路径规划领域，具体涉及一种工业机器人连续姿态路径光顺方法及系统。

技术介绍

[0002]在工业机器人路径规划领域，用于控制机器人终端实际转动的命令是通过对机器人终端给定的运动姿态信息插补得到。由于给定的运动姿态信息为离散数据点，对其直接线性插补会导致在每一个姿态处的曲率不连续，尤其是在姿态变化特别频繁时，造成机器人在每一个姿态处频繁加减速。为了解决机器人在姿态转接时的频繁加减速问题，提高机器人终端的转动速度，需要对姿态路径进行转接过渡处理。采用高阶连续曲线光顺转接处的姿态可以保证光顺后的姿态路径满足角加速度连续。然而，对于姿态变化比较频繁的工况，如何减小转接处所需要的角位移成为了姿态路径光顺亟需解决的问题。
[0003]文献1“Pu Y,Shi Y,Lin X,et al.C2
‑
Continuous Orientation Planning for Robot End
‑
Effector with B
‑
Spline Curve Based on Logarithmic Quaternion[J].Mathematical Problems in Engineering,2020.”公开了一种基于对数四元数的高效姿态四元数插值方法。该方法把旋转空间S3中的姿态四元数转化为三维空间R3中的点，维度降阶后采用B样条曲线进行姿态光顺，B样条曲线的C2连续性保证了光顺后姿态路径的C2连续，但是无法保证姿态的加速度连续且姿态路径的曲率未知。文献2“王效杰.基于四元数样条曲线的姿态轨迹规划研究[D].西南科技大学.2015”公开了一种基于四元数样条曲线的姿态路径过渡方法。该方法通过对给定四元数姿态、速度和加速度条件建立了四元数平滑过渡曲线模型，但是该方法只保证了姿态转接的C2连续。文献3“Huang J,Du X,Zhu L M.Real
‑
time local smoothing for five
‑
axis linear toolpath considering smoothing error constraints[J].International Journal of Machine Tools and Manufacture,2018,124:67
‑
79”公开了一种基于三次B样条曲线的姿态光顺方法，该方法分析了姿态转接处的曲率特点，满足姿态路径的G2连续。文献4“Herzog R,Blanc P.Optimal G2 Hermite interpolation for 3D curves[J].Computer
‑
Aided Design,2019,117:102752.”公开了一种G2连续的三维路径过渡算法，但是没有研究姿态路径光顺情况。于2021年1月15日公开的专利文献CN112223290A“一种关节空间连续点轨迹过渡方法及装置”，采用轨迹允许的最大速度约束和贝塞尔曲线实现了位置的过渡，通过修正过渡起始姿态和过渡结束姿态完成关节空间连续点轨迹过渡，但是该方法不能保证过渡姿态的高阶连续。
[0004]如上所述，通过分析已有的离散姿态路径光顺方法可知，现有技术光顺后的姿态路径实现了一定的姿态连续性和速度连续性，但是并没有对曲率进行优化，对过渡姿态路径比较短的姿态光顺情况不适用。

技术实现思路

[0005]为克服现有技术中存在的缺陷，本专利技术提供一种工业机器人连续姿态路径光顺方法及系统，结合单位四元数和七次埃尔米特样条曲线，大幅降低了姿态转接时的曲率峰值，减小了姿态路径拐角光顺所需要的角位移，提高了工业机器人的运动平稳性。
[0006]本专利技术方法所采用的技术方案为：一种工业机器人连续姿态路径光顺方法，包括以下步骤：
[0007]读取工业机器人的姿态路径代码，得到一系列姿态向量；
[0008]把姿态向量转化为单位四元数；
[0009]依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；
[0010]采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；
[0011]采用五次多项式对余留姿态路径段的姿态进行插补；
[0012]对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。
[0013]本专利技术系统所采用的技术方案为：一种工业机器人G3连续姿态路径光顺系统，包括以下模块：
[0014]姿态向量获取模块，用于读取工业机器人的姿态路径代码，得到一系列姿态向量；
[0015]姿态向量转化模块，用于把姿态向量转化为单位四元数；
[0016]路径段获取模块，用于依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段；
[0017]插补模块，用于采用七次埃尔米特样条曲线对转接姿态路径段的姿态进行光顺和插补；以及采用五次多项式对余留姿态路径段的姿态进行插补；
[0018]拼接模块，用于对转接姿态路径段插补后的姿态和余留姿态路径段插补后的姿态进行拼接，并把四元数形式的姿态转化为姿态欧拉角，得到工业机器人连续的姿态光顺路径。
[0019]与现有技术相比，本专利技术取得的技术效果包括：
[0020]本专利技术结合单位四元数和七次埃尔米特样条曲线，大幅降低了姿态转接时的曲率峰值，减小了姿态路径拐角光顺所需要的角位移，适用于过渡姿态路径比较短的姿态光顺情况，提高了工业机器人的运动平稳性。
附图说明
[0021]图1是本专利技术实施例中工业机器人连续姿态路径光顺方法的流程图。
[0022]图2是本专利技术实施例中未光顺的姿态路径图。
[0023]图3是本专利技术实施例中光顺后的姿态路径图。
[0024]图4是采用本专利技术和文献3中的方法1、文献4中的方法2进行姿态路径光顺后的曲率对比图。
[0025]图5是采用本专利技术方法进行姿态路径光顺后插补得到的姿态欧拉角。
具体实施方式
[0026]下面结合实施例和附图对本专利技术的技术方案做进一步详细的描述，但本专利技术的实
施方式并不限于此。
[0027]实施例
[0028]本实施例提供一种工业机器人连续姿态路径光顺方法，如图1所示，以工业机器人G3为例，对路径光顺方法所包括的主要步骤，详细描述如下：
[0029]步骤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；未光本文档来自技高网...

【技术保护点】

【技术特征摘要】
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是姿态路径代码中给定的姿态个数；所述依次选取工业机器人的三个相邻姿态，得到余留姿态路径段和转接姿态路径段，包括：根据机器人过渡精度需求确定转接比例系数ε；对从第一个姿态开始的三个相邻姿态四元数Q1、Q2和Q3，计算四元数Q1和Q2之间的角位移θ1、Q2和Q3之间的角位移θ2；取角位移θ1和θ2中的较小值乘以转接比例系数ε，得到姿态四元数Q2处的转接路径段Θ
s1
；根据姿态四元数Q1和转接姿态路径段的起点，构成第一段余留姿态路径段Θ
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Θ5‑

【专利技术属性】
技术研发人员：王立平，杨洪丽，何敏佳，李伟涛，关立文，王衎，
申请(专利权)人：广州数控设备有限公司，
类型：发明
国别省市：