CN107538493B - 一种避免线缆干涉约束的工业机器人轨迹规划方法 - Google Patents

Abstract

本发明提供了一种避免线缆干涉约束的工业机器人轨迹规划方法，属于轨迹规划领域；其中包括工业机器人运动学模型的建立、线缆模型的建立、求解最佳跳转点、轨迹插补。所述线缆数学模型的建立发明了一种线缆数学模型的建立方法；所述求解最佳跳转点发明了一种基于智能优化算法求解带线缆机器人轨迹规划中避免线缆干涉约束的跳转点求解方法；轨迹规划发明了一种经跳转点的轨迹插补方法；采用本发明的技术方案，可以得到运动时间较短且平稳，避免线缆干涉约束的工业机器人运动轨迹，降低了成本，提高了工业机器人在运动过程的安全性。

Description

一种避免线缆干涉约束的工业机器人轨迹规划方法

技术领域

本发明涉及机器人轨迹规划领域，更具体地说，涉及一种避免线缆干涉约束下工业机器人轨迹规划方法。

背景技术

工业机器人以其低成本，高效率，高柔性等特点在工业制造领域如焊接，装配，搬运，磨削等多个方面得到应用，成为智能制造系统，数字化工厂，计算机集成制造系统的工作单元。为满足不同工作任务类型需求，工业机器人末端通常连接内含气管，导线，水管等装置的柔性线缆用于安装各类执行器。

机器人轨迹规划是指根据任务需求，在满足运动学和动力学约束条件下，计算出机器人预期运动轨迹，获得运动过程中各关节的位置，速度，加速度，加加速度。机器人轨迹规划方法分为：示教编程和离线编程。示教编程包括示教，记录和轨迹再现，较为简单，但效率低下，无法满足复杂轨迹规划的要求。离线编程是指通过计算机图形学建立工作场景，利用软件自动生成运动轨迹，因其能够在不用访问机器人本体的情况下进行轨迹规划被广泛采用。目前，国内外离线编程软件提供机器人各关节间相互干涉和碰撞检测功能，未考虑执行器末端柔性线缆与机器人间碰撞，挤压以及自身扭转过大的问题，导致线缆损坏，对工业机器人作业过程造成严重影响。轨迹规划时常采用手动插入路径点，凭经验判断线缆干涉与否及轨迹的可行性，缺少有效的线缆移动端轨迹插补和优化策略，降低轨迹规划效率和机器人工作安全性。

综上所述，为提高轨迹的可行性和规划效率，增强带线缆工业机器人工作的安全性，寻找有效的移动端线缆的轨迹插补策略和进行多目标的轨迹优化具有重要的现实意义。

发明内容

针对上述问题，本发明目的在于公开一种避免线缆干涉约束的工业机器人轨迹规划方法，解决现有工业机器人轨迹规划中线缆干涉问题，并以轨迹长度和平滑性作为优化目标函数，建立多目标优化模型，采用智能优化算法寻求多目标工业机器人最优轨迹。

为了实现上述目的，本发明的技术方案主要包括以下步骤：

步骤1、通过机器人结构图纸获取D-H参数信息，根据修正的D-H参数法建立机器人运动学模型，求得正反解；

步骤2、建立线缆移动端和固定端坐标系，通过笛卡尔空间坐标变换法得到线缆移动端与固定端在基坐标系下的位姿变换矩阵；

步骤3、确定带线缆机器人轨迹的优化目标函数和约束条件，建立多目标优化数学模型，采用智能优化算法对多目标优化问题进行求解并得到最佳跳转点，并以萤火虫算法为例，对上述问题进行求解，其余智能优化算法可类同。

所述的多目标优化的数学模型为：

优化目标函数：F_G＝ω₁.f_l+ω₂.f_j+ω₃.f_ob

(5)

公式(5)-(8)中，f_l代表工业机器人线缆移动端的轨迹长度，作为避免运动突变的评价指标；f_j代表六个关节的平均加加速度，作为工业机器人平稳运行的评价指标；f_ob代表工业机器人线缆移动端在运动中缠绕，扭转和挤压的评价指标；ω₁、ω₂、ω₃分别为f₁、f₂、f₃三个评价指标的权值系数。

所述的线缆未发生干涉定义为：线缆移动端相对于固定端姿态偏差参数ED小于设定的安全线缆姿态偏差参数ED_safe，同时，线缆移动端未与连杆二和连杆四发生挤压，线缆挤压参数ES为1，即：

ES＝1 (10)

式中：α₀,β₀,γ₀分别代表线缆固定端姿态欧拉角的偏转角，俯仰角与回转角；

分别代表轨迹规划中的离散点线缆移动端姿态欧拉角的偏转角，俯仰角与回转角； f₁,f₂,f₃分别代表基坐标系下的X，Y，Z三个主轴，线缆移动端相对于线缆固定端的欧拉角差值的评价指标的权值系数。

所述的萤火虫算法的数学模型为：

假设任意一个萤火虫i的位置信息为X_i时与另一个体j的位置信息为X_j时，其距离r_ij如下：

式(11)中，d代表笛卡尔空间坐标的坐标维数，x_i,k代表萤火虫个体i在笛卡尔空间坐标X_i的第k维分量，x_j,k代表萤火虫个体在笛卡尔空间X_j的第k维分量。

在迭代时萤火虫个体i相对于萤火虫个体j的吸引力如下：

式(12)中，β₀表示萤火虫个体之间距离r＝0时的吸引力，其通常设置为1；γ表示光照吸收率，理论取值范围是0到正无穷大，其通常设置为常数；r_ij表示萤火虫个体i与萤火虫个体j之间的距离。

在迭代过程中亮度较低的萤火虫个体i向亮度较高的萤火虫个头j移动时的规则如下：

式(13)中，α表示随机步长因子，其取值范围为[0,1],通常设置为1；rand表示服从在 (0,1)上均匀分布的随机数。

步骤4、根据任务需求，通过示教获取工业机器人初始点和任务目标点位姿信息，结合上述求得的跳转点信息，采用圆弧插补和线性插补策略，对工业机器人初始点，跳转点，任务目标点进行位置和姿态插补，经逆解算法得到机器人各关节空间位置插值点，；

所述的圆弧插补策略是以初始点，跳转点，任务目标点建立空间圆弧，可参考图2，以圆心O_R为原点建立圆弧平面插补坐标系O_RX_RY_RZ_R，圆弧平面插补坐标系相对于基座标系OX₀Y₀Z₀的齐次变换矩阵如下：

式(14)中，a，b，c分别为圆心O_R在基座标系OX₀Y₀Z₀下的X，Y，Z坐标值，θ为圆弧平面插补坐标系X_R轴相对于基座标系X₀轴的角度，α为圆弧平面插补坐标系Z_R轴相对于基座标系Z₀轴的角度。

设轨迹起始点到任务目标点所扫过的圆弧角为φ，将圆弧等分成N段生成N+1个离散点，则起始点到第i个插补点所扫过的圆弧角为φ_i＝iφ/N,可得圆弧平面坐标系下插值点经矩阵变换转换到基座标系下的坐标值

如下：

所述的线性插补策略是对示教后初始点姿态欧拉角(α₀，β₀，γ₀)和任务目标点姿态欧拉角(α_e，β_e，γ_e)进行线性插补，对上述N+1个离散点，在基座标系下插值点

对应的姿态欧拉角的公式如下：

Euler_i＝(α₀+i(α_e-α₀)/N,β₀+i(β_e-β₀)/N,γ₀+i(γ_e-γ₀)/N)

步骤5、通过三次样条曲线对上述关节位置插值点进行拟合得到关节角度-时间序列，并检测是否满足各关节限制。

所述的关节限制为机器人在运动过程中速度，加速度和加加速度均小于设定的最大值，即：

式(16)中，

分别表示机器人关节角n的最大速度、加速度和加加速度，v_n(t)、a_n(t)、j_n(n)分别表示在任意时刻机器人关节角n的速度、加速度、加加速度。

带线缆机器人在运动过程中，线缆易受到弯曲与扭转，因此尽量使线缆移动端坐标系{moved}对应于线缆固定端坐标系{fixed}的X,Y,Z轴尽量重合，即坐标系{moved}相对于坐标系{fixed}的旋转矩阵^fixedR_moved对应的姿态欧拉角α，β，γ尽可能趋向于0。由于线缆移动端坐标系{moved}相对于线缆固定端坐标系{fixed}的Z轴都指向线缆的中心线，因此欧拉角α是描述线缆扭转的重要参数，而X,Y轴都处于线缆的横截面，β，γ是描述线缆弯曲的重要参数。

根据线缆在运动过程中来自弯曲与扭转的疲劳损伤，以及线缆与机械臂发生缠绕时对线缆拉扯造成的损伤，结合线缆移动端坐标系{moved}相对于线缆固定端坐标系{fixed}的姿态变化所对应的姿态欧拉角α，β，γ，定义线缆姿态偏差参数ED如下：

ED＝f₁|α|+f₂|β|+f₃|γ| (17)

根据线缆在运动过程中与机械臂存在相互挤压的情况，具体描述如下：在运动过程中时线缆移动端易与连杆二，连杆四发生挤压，定义线缆挤压参数ES如下：

式(17)-(20)中，

分别表示线缆移动端到连杆二和连杆四的距离，R₂、R₄分别表示连杆二的横截面半径大小，连杆四的横截面半径大小，e₂、e₄分别表示线缆移动端到连杆二和连杆四的安全距离。

步骤6、将所得的关节空间轨迹发送到机器人驱动器，控制机器人完成运动轨迹。

采用上述避免线缆干涉约束的工业机器人轨迹规划方法，可以得到一条速度，加速度，加加速度连续，轨迹平滑，防止线缆约束的工业机器人运动轨迹，避免机器人和线缆的损坏。

附图说明

图1是本发明提出的基于萤火虫算法求解带线缆工业机器人的多目标轨迹规划流程图；

图2是本发明提出的轨迹规划中空间圆弧插补坐标系的建立方式。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面详细说明本发明的具体实施方式：

所述的基于智能优化算法求解带线缆工业机器人的多目标轨迹规划方法流程，具体实施步骤如下：

步骤1、建立机器人运动学模型，求得工业机器人正反解；

所述的运动学模型是通过建立各连杆坐标系，获取D-H参数信息，并根据坐标系变换关系推导出运动学正反解。

步骤2、根据线缆安装位置，建立线缆移动端和固定端坐标系，根据笛卡尔空间坐标变换关系得到线缆移动端与固定端的齐次变换矩阵。

步骤3、确定带线缆机器人轨迹规划的优化目标函数和约束条件，采用智能优化算法算法对多目标优化问题进行求解。

所述结合智能优化算法进行求解涉及到算法，以萤火虫算法为例，其他智能优化算法可类同，参考图1，具体执行步骤如下：

S1、初始化优化目标函数，在约束条件决定的可行域范围内对由萤火虫算法对优化解进行编码得到的萤火虫位置进行随机赋值；

S2、判断是否达到所设迭代次数，若是，算法结束，若否，则进入S3；

S3、由跳转点结合初始点，任务目标点生成轨迹的离散点，即步骤4和步骤5；

S4、判断线缆干涉指标f_ob是否为0，若是，则令萤火虫算法适应度函数值无穷大，至S8，若否，则至S5；

S5、用公式

计算线缆移动端轨迹长度；

S6、用公式

计算六个关节的平均加加速度；

S7、用公式F_G＝w₁f₁+w₂f₂+w₃f_ob计算适应度函数值；

S8、用公式

更新萤火虫位置，至S2；

通过算法迭代，将会得到任务空间的最佳跳转点和一条避免线缆干涉，速度，加速度，加加速度连续，轨迹较短，运动平滑的关节运动轨迹。

步骤4、根据任务需求，结合机器人示教产生的初始点和任务目标点以及上述得到的最佳跳转点，采用圆弧插补和线性插补策略对初始点，任务目标点和跳转点位置和姿态进行插值，并将插值点位姿经逆解算法转化为关节空间。具体的插值步骤为：

S1、将关节空间初始点，任务目标点，跳转点经正解算法获得基坐标系下三点的空间位置P₁(x₁，y₁，z₁),P₁(x₂，y₂，z₂),P₃(x₃，y₃，z₃)；

S2、结合P₁，P₂，P₃点建立空间圆弧插补坐标系，建立方式如图2，并得到空间圆弧插补坐标系与基座标系的坐标变换关系T_R；

S3、将起始点到任务目标点所扫过的圆弧角φ等分成N段得到插值点，转换到基坐标系下的插值点:P_i＝T_R[RcosΦ_i，RsinΦ_i，0，1]^T，其中R为圆弧半径；

S4、对示教的初始点姿态欧拉角(α₀，β₀，γ₀)和进刀点姿态欧拉角(α_e，β_e，γ_e)进行线性插补，得到上述插值点姿态欧拉角：

Euler_i＝(α₀+i(α_e-α₀)/N，β₀+i(β_e-β₀)/N)，α₀+i(α_e-α₀)/N

S5、将上述插值点位置结合欧拉角代表的姿态经逆解算法转化到关节空间。

步骤5、采用三次样条曲线对满足约束条件的插值点进行拟合，判断关节空间关节角度-时间序列点是否满足关节约束，若不满足，选取另一组逆解算法重新对插值点进行求解并拟合。

步骤6、将多目标优化模块得到的运动轨迹信息发送给机器人关节驱动器，控制机器人完成运动轨迹。

Claims (2)

1.一种避免线缆干涉约束的工业机器人轨迹规划方法，带线缆工业机器人是指多自由度串联式机械臂，执行器末端连接柔性线缆，所述柔性线缆包含导线或气管，线缆移动端固连在执行器末端随执行器一起运动，线缆固定端与外部设备固连，其特征在于，包括以下几个步骤：

步骤1、根据带线缆工业机器人的结构，获取D-H参数信息并建立机器人运动学模型，求得机器人的运动学正反解；

步骤2、建立线缆移动端坐标系和线缆固定端坐标系，通过坐标变换得到线缆移动端与线缆固定端之间的位姿变换矩阵；

步骤3、确定带线缆工业机器人轨迹规划的优化目标函数和约束条件，建立多目标优化数学模型，采用智能优化算法求解任务空间中机器人轨迹的最佳跳转点和一条避免线缆干涉，速度、加速度、加加速度连续，运动平滑的关节运动轨迹；

步骤4、根据任务需求，结合机器人示教产生的初始点和任务目标点以及上述得到的最佳跳转点，采用圆弧插补与线性插补策略对初始点，任务目标点和最佳跳转点的位置和姿态进行插值，并将插值点的位姿经逆解算法转化到关节空间；

步骤5、采用关节空间三次样条曲线拟合方法，求得机器人关节空间轨迹，并检测是否满足几何限制和运动约束；

步骤6、将所述关节空间轨迹发送给机器人控制器，控制机器人完成任务所需的运动轨迹；

所述步骤3中，所述确定带线缆工业机器人轨迹规划的优化目标函数和约束条件，建立多目标优化数学模型，采用智能优化算法求解任务空间中机器人轨迹的最佳跳转点和一条避免线缆干涉，速度、加速度、加加速度连续，运动平滑的关节运动轨迹的具体步骤为：

步骤S1：设定迭代次数；

步骤S2：初始化优化目标函数，在约束条件决定的可行域范围内对由智能优化算法对优化解进行编码得到的粒子的位置并进行随机赋值；

步骤S3：由初始点，最佳跳转点和任务目标点生成运动轨迹，并判断机器人的运动轨迹中线缆是否发生干涉，即判断线缆移动端在运动中缠绕、扭转和挤压的评价指标f_ob的值是否为0，若f_ob的值为0，则线缆未发生干涉，执行步骤S4，若f_ob的值不为0，则线缆发生干涉，将适应度函数设为无穷大，执行步骤S5；

步骤S4：计算轨迹总长度及关节平均加加速度所对应的适应度函数值；

步骤S5：更新由智能优化算法对优化解进行编码得到的粒子的位置；

步骤S6：判断是否达到所设的迭代次数，若是，则算法结束，若否，则执行步骤S3；

所述步骤S3中，若出现以下两种情况则判定为线缆发生干涉：第一种情况是线缆姿态偏差大于设定的安全值，即线缆移动端相对于线缆固定端的姿态变化过大而超过设定的安全值，第二种情况是线缆与机械臂接触并发生挤压，根据这两种情况定义线缆移动端在运动中缠绕、扭转和挤压的评价指标f_ob如下：

所述的线缆姿态偏差定义如下：运动轨迹初始点的线缆固定端相对于基坐标系的姿态欧拉角为α₀，β₀，γ₀，运动轨迹各点的线缆移动端相对于基坐标系的姿态欧拉角为α₁ ⁿ，β₁ ⁿ，γ₁ ⁿ，n表示运动轨迹中点的个数，f₁,f₂,f₃分别代表基坐标系下的X，Y，Z三个主轴的线缆移动端相对于线缆固定端的欧拉角差值的评价指标的权值系数，线缆姿态偏差用参数ED表示，定义为线缆移动端和线缆固定端的关于X,Y,Z轴姿态欧拉角差值的权系数之和，数学描述如下：

2.根据权利要求1所述的避免线缆干涉约束的工业机器人轨迹规划方法，其特征在于：所述的线缆与机械臂接触并发生挤压的定义如下：从机械臂固定基座开始为连杆编号，固定基座为连杆零，第一个可动连杆为连杆一，以此类推，线缆易与连杆二和连杆四发生接触和挤压，轨迹规划过程中需考虑线缆挤压约束，设连杆二的横截面半径为R₂，线缆移动端到连杆二的距离为

线缆移动端到连杆二的安全距离为e₂，线缆与连杆二发生挤压定义为线缆移动端与连杆二的距离是否超出安全距离与连杆横截面半径之和，数学描述如下：

连杆四的横截面半径为R₄，线缆移动端到连杆四的距离

线缆移动端到连杆四的安全距离为e₄，线缆与连杆四发生挤压定义为线缆移动端与连杆四的距离是否超出安全距离与连杆横截面半径之和,数学描述如下：

定义线缆挤压参数ES为上述两种挤压的乘积，数学描述如下：

根据ES的值判断线缆与机械臂是否发生挤压，当ES的值为0时，线缆与机械臂接触并发生挤压；当ES的值为1时，线缆与机械臂未发生挤压。

Images