CN108594757A - 一种基于位置和姿态约束的机器人小线段前瞻规划方法 - Google Patents

Abstract

本发明公开了一种基于位置和姿态约束的机器人小线段前瞻规划方法，采用小线段前瞻规划方法获得机器人每个关节角速度规划数据，其特征在于采用融合速度向量替代线性速度作为前瞻规划方法中的规划量，其中：v₁、v₂、v₃表示末端坐标系原点沿着基坐标X、Y、Z轴方向运动的线速度，ω₁，ω₂，ω₃表示欧拉角的角速度。本发明对姿态和位置同时起到约束作用，可以有效避免机器人末端坐标系的姿态或位置其中任意一个或同时产生突变，若姿态不变，线速度可以达到设定的最大值，效率高，同时将姿态速度与线速度融合为一个量，被控量减少，计算更加简单、运算速度更快。

Description

一种基于位置和姿态约束的机器人小线段前瞻规划方法

技术领域

本发明涉及工业机器人按照一定轨迹对曲面、曲线进行焊接、喷涂、装配和加工领域。

背景技术

在工业生产实践中，工业机器人广泛运用于焊接、喷涂、装配和加工领域。有时需要机器人沿着特定的曲线运行，通过传统的示教方式很难得到所需要的轨迹路径。这时需要借助离线编程软件将该曲线划分为很多段小的直线和圆弧。在常规的轨迹规划中，对每条小线段做加减速处理使线段两端速度为零，这种方法不仅需要电机频繁启停，而且导致末端运行速度始终较低，达不到要求。

在末端坐标系运动时，已有的前瞻算法只考虑了末端坐标系原点运动的线速度规划，未考到末端坐标系姿态的变化。在实际使用中，坐标系姿态可能会在瞬间产生较大的跃变，对电机冲击较大；采用对加权融合速度进行规划，对姿态和位置同时起到约束作用，可以有效避免机器人末端坐标系的姿态或位置其中任意一个或同时产生突变，若姿态不变，线速度可以达到设定的最大值，效率高，同时将姿态速度与线速度融合为一个量，被控量减少，计算更加简单、运算速度更快。采用分段平滑指数加减数做速度规划，不仅速度连续，而且加速度连续，柔性冲击小，与梯形加减速相比不仅保证速度连续，而且加速度连续，不存在加速度跃变，避免柔性冲击，保证较高的柔性；与S型曲线加减速相比，在保证加速度连续同时，加速度增长更快，效率更高。

发明内容

本发明针对背景技术中存在的问题，提出了一种基于位置和姿态约束的机器人小线段前瞻规划方法。

技术方案：该方法采用小线段前瞻规划方法获得机器人每个关节角速度规划数据，并采用融合速度向量替代线性速度作为前瞻规划方法中的规划量，其中：v₁、v₂、v₃表示末端坐标系原点沿着基坐标X、Y、Z轴方向运动的线速度，ω₁，ω₂，ω₃表示欧拉角的角速度。

在更优的实施方式中，融合速度经加权后获得加权融合速度向量替代线性速度作为前瞻规划方法中的规划量，其中σ为缩放系数由用户设定。

优选的前瞻规划方法中，包括确定前瞻段数、当前第i段段末融合速度以及当前段加权融合速度V(t)与时间t的关系规划的步骤，所述三个步骤均采用分段平滑指数加减速曲线对速度进行规划，加速度公式如下：

其中A_m为允许最大融合加速度，由用户设定；t_u为加速总时间，时间t单位为s；V₀表示在初始速度为0的前提下，按上述加减速函数加速0.5秒后的速度，V₀＝V(0.5)＝A_m(1-1.5e^-0.5)；

本发明的具体步骤为：

(1)利用离线编程软件根据机器人需要运动的轨迹路径，生成机器人末端坐标系离散位置和姿态数据，然后求取每一段的融合路径长度L_i

设定允许最大线速度v_max、允许最大线加速度a_max、插补周期T、缩放系数σ及允许关节最大运动角速度θ_max；系统允许最大加权融合速度V_m＝v_max，系统允许最大融合加速度A_m＝a_max；

(2)确定前瞻段数n

在速度前瞻时，比较V_m与2V₀关系：

当V_m＜2V₀时，最小前瞻路径融合总长其中：t_x通过求解方程A_me^(t-0.5)-A_me^-0.5-A_me^-0.5t＝0.5V_m的解t＝t_x获得；

当V_m≥2V₀时，最小前瞻路径融合总长

假设当前处于第i段，前瞻段数n满足n＝min(m)，m满足其中m为非零自然数；

(3)对当前任意i段，已知前瞻段数n，取第k＝i+n段段末加权融合速度然后通过反向递推和正向推理确定当前第i段段末加权融合速度

(3-1)反向递推获得第i段段末反向递推加权融合速度

首先，k＝i+n，第k段段末加权融合速度取满足下述3个约束条件的最小值为第k＝i+n段初始加权融合速度

约束1：若第k段上一直在减速，第k段初始加权融合速度则

其中，V_z求取过程：求出方程的解t_y

约束2：第k段初始加权融合速度

约束3：在两端线段衔接处，第k段初始加权融合速度满足其中，与模值相等，T为插补周期；

然后，k＝i+n-1，第k段段末加权融合速度按上述3个约束条件，求出第i+n-1段的初始加权融合速度

以此类推，直至k＝i+1求出第i+1段初始加权融合速度V_c ⁱ⁺¹，获得当前段段末反向推理加权融合速度

(3-2)正向推理获得第i段段末正向推理加权融合速度

上式中：第i段初始加权融合速度其中t_z通过求解方程的解t＝t_z获得；

(3-3)比较取小获得当前第i段段末加权融合速度V_e ⁱ：

(4)根据规划当前段加权融合速度V(t)与时间t的关系，分为以下6种情况：

1)L_i＝L_a时，表明加权融合速度V(t)一直在加速，加权融合速度V(t)对时间t的关系是：

当时，

当时，

2)L_i＝L_a时，表明加权融合速度V(t)一直在减速，加权融合速度V(t)对时间t的关系是：

当时，

当时

3)L_a＜L_i＜L_b时，表明加权融合速度V(t)先加速后匀速，但是速度没有达到最大值；

当时，

当时，

4)L_a＜L_i＜L_b时，表明加权融合速度V(t)先减速再匀速；

当时，

当时，

5)L_i≥L_b时，表明加权融合速度V(t)先加速到最大值再匀速；

当时，

当时

6)L_i≥L_b时，表明加权融合速度V(t)先加速到最大值再匀速最后减速；

t₁₁表示匀加速时间，t₁₂表示匀减速时间，t₁₃表示保持最大速度运行时间：

当时

当 当

上述6种情况中，L_a表示从第i段初始加权融合速度V_c ⁱ加速或减速到第i段段末加权融合速度V_e ⁱ所经过的融合路径长度，t₁表示在|V_e ⁱ-V_c ⁱ|＜2V₀的情况下，从第i段初始加权融合速度V_c ⁱ加速或减速到第i段段末加权融合速度V_e ⁱ所需时间；L_b表示从第i段初始加权融合速度V_c ⁱ加速到允许最大加权融合速度V_m所经过的融合路径长度与从允许最大加权融合速度V_m减速到第i段段末加权融合速度V_ei所经过的融合路径长度之和，t₂表示在|V_m-V_c ⁱ|＜2V₀的情况下，从第i段初始加权融合速度V_c ⁱ加速到允许最大加权融合速度V_m所需时间，t₃表示在|V_e ⁱ-V_m|＜2V₀的情况下，从允许最大加权融合速度V_m减速到第i段段末加权融合速度V_ei所需时间；

t₁通过求出方程的解t＝t₁获得；

t₂通过求出方程的解t＝t₂获得；

t₃通过求出方程的解t＝t₃获得；

(5)将加权融合速度V(t)加工处理获得融合速度向量第i段：

(6)基于融合速度向量进行逆运算，获得每个关节角速度，各关节角速度θ_j,1＜j≤6

[θ₁(t) θ₂(t) θ₃(t) θ₄(t) θ₅(t) θ₆(t)]^T＝J^-1[v₁(t) v₂(t) v₃(t) ω₁(t) ω₂(t) ω₃(t)]^T

J表示机器人正运算的雅可比矩阵；

(7)判断在当前段上，各关节角速度最大值是否小于允许最大关节角速度θ_max

max(θ_j(t))＜θ_max 1≤j≤6

若满足，表示所求关节速度满足要求，继续规划下一段；

否则，说明当前第i段段末加权融合速度过大，导致关节角速度过大；将乘以比例系数μ，然后跳转到步骤(4)，重新规划加权融合速度V(t)与时间t关系，直到满足条件为止。

优选的，步骤(7)中，μ＝e^-0.3m-e^-3，m为不满足此条件次数，m＝1，2，3......10。

本发明的有益效果：

1)采用速度前瞻算法后，每一段的始末加权融合速度不需要为0，不仅避免了机器人频繁启停，减少损耗和机械磨损，而且提高了机器人运行效率。

2)采用加权融合速度作为规划量，对姿态和位置同时起到约束作用，可以有效避免机器人末端坐标系的姿态或位置其中任意一个或同时产生突变，若姿态不变，线速度可以达到设定的最大值，效率高，同时将姿态速度与线速度融合为一个量，被控量减少，计算更加简单、运算速度更快。

3)采用分段平滑指数加减速，在分段连接处，加速度相等，实现平滑过渡，而且在整个加减速过程中，速度变化开始及结束时加速度都为0，减少了柔性冲击。与梯形加减速相比不仅保证速度连续，而且加速度连续，不存在加速度跃变，避免柔性冲击，保证较高的柔性；与S型曲线加减速相比，在保证加速度连续同时，加速度增长更快，效率更高。

4)不仅对线速度和姿态速度进行约束，还保证了各关节角速度不会超过设定的允许关节最大角速度。

附图说明

图1是本发明的基于速度和姿态约束的机器人小线段前瞻规划方法流程图

图2是本发明的分段平滑指数加减速速度与加速度曲线图

图3是本发明实施例中所需加工路径示意图

图4是本发明实施例中获得的加权融合速度对时间关系图

图5是本发明实施例中获得的每个关节角速度对时间关系图

具体实施方式

下面结合实施例对本发明作进一步的说明。

结合图1，本发明包括以下步骤：

(1)利用离线编程软件根据机器人需要运动的轨迹路径，生成机器人末端坐标系离散位置和姿态数据，然后求取每一段的融合路径长度L_i

(2)确定前瞻段数n

(3)对当前任意i段，已知前瞻段数n，则第k＝i+n段段末加权融合速度V_e ⁱ⁺ⁿ＝0，然后确定当前第i段段末加权融合速度V_e ⁱ

(4)根据规划当前段加权融合速度V(t)与时间t的关系，分为6种情况

(5)将加权融合速度V(t)加工处理获得加权融合速度向量需要说明的是：本申请中，将向量以“字母+→”进行标记，将向量的大小(模值)以“字母”进行标记，故表示加权融合速度向量，V(t)表示加权融合速度，二者有以下关系：

(6)基于加权融合速度向量进行逆运算，获得每个关节角速度，各关节角速度θ_j,1＜j≤6

(7)判断在当前段上，各关节角速度最大值是否小于允许最大关节角速度θ_max。

作为本发明的发明点之一，步骤(2)确定前瞻段数n、步骤(3)确定当前第i段段末加权融合速度V_e ⁱ、以及步骤(4)确定当前段加权融合速度V(t)与时间t的关系规划，均采用分段平滑指数加减速曲线对速度进行规划，速度和加速度曲线如图2所示，加速度公式如下：

其中A_m为允许最大融合加速度，由用户设定；t_u为加速总时间，时间t单位为s；V₀表示在初始速度为0的前提下，按上述加减速函数加速0.5秒后的速度，V₀＝V(0.5)＝A_m(1-1.5e^-0.5)。

下面以一个具体实施例对本发明的操作步骤进行详细说明：

步骤(1)：利用robotguide软件根据所需加工路径，生成机器人末端坐标系位置和姿态的离散数据，以图3为例，P1-P2，P2-P3，P3-P4，P4-P5为机器人运动的4段小线段，robotguide中生成P1、P2、P3、P4、P5的位置和姿态如表1：

	x	y	z	a	b	c
							P1	1352.03977	575.321452	551.193348	3.10306579	-0.269634467	1.76241727
P2	1347.78669	584.406136	553.464538	3.04505563	-0.258798399	1.83253839
							P3	1344.09322	592.017642	555.634302	2.99891346	-0.251230557	1.88121899
P4	1330.10088	618.862704	558.611767	2.81682587	-0.223474388	2.07913327
							P5	1250.14297	757.34069	583.618189	2.08239798	-0.085785136	2.7996919528

表1各点的位置和姿态信息

求取每一段的融合路径长度L_i：

P1-P2段融合路径长度L₁

P2-P3段融合路径长度L₂

P3-P4段融合路径长度L₃

P4-P5段融合路径长度L₄

设定：在起始P1处加权融合速度大小0，即最大容许线速度v_max为200m/s，最大容许线加速度a_max为200m/s²，缩放系数σ为200，插补周期T为12ms，允许最大关节角速度θ_max为2rad/s。则最大加权融合速度V_m＝v_max＝200，最大融合加速度A_m＝a_max＝200。

步骤(2)：确定前瞻段数n

由于本实施例中V_m≥2V₀，故：

最小前瞻融合路径总长根据最小前瞻段数n满足n＝min(m)，m满足其中m为非零自然数。故本实施例中计算获得n＝3。

步骤(3)：确定第一段加权融合末速度V_e ¹

(3-1)反向递推获得第i段段末反向递推加权融合速度

在P5处加权融合速度为0，即设在P4处容许最大加权融合速度为V_c ⁴，则V_c ⁴满足：

(表示由点i指向点j的单位向量)

V_c ⁴＝14.79

P3-P4段：

在P4处融合速度大小为14.79，即V_e ³＝14.79，设在P3处最大融合速度为则V_c ³满足

V_c ³＝23.44；

P2-P3段：

在P3处融合速度大小为23.44，即V_e ²＝23.44，设在P2处最大融合速度为V_c ²，则V_e ²满足

(23.44+200*e^t-0.5-200*e^-0.5(1+t))*t＝0.5L₂的解t＝t_y＝0.2802

V_c ²＝27.65；

所以

(3-2)正向推理获得第i段段末正向推理加权融合速度

根据下式求解：

上式中：第i段初始加权融合速度其中t_z通过求解方程(的解t＝t_z获得；

L₁＝21.02，满足条件L₁＞(V_c ¹+V₀)*1，故V_e ^1″＝40.03

(3-3)比较取小获得当前第i段段末加权融合速度V_e ⁱ：

步骤(4)：根据V_e ¹，规划当前段直线上加权融合速度V(t)对时间t的关系

在第1段上，段初加权融合速度V_c ¹为0，段末加权融合速度V_e ¹为27.65，融合路径长度L₁为21.02，显然满足技术方案中情况3)条件。加速时间t_u为方程的200e^(0.5t-0.5)-200e^-0.5(1+0.5t)＝27.65/2解。t_u＝0.884。

运行时间t为：t＝0.884+(L₁-0.884*27.65/4)/V_e ¹＝1.423

所以第1段上加权融合速度对时间的关系是：

加权融合速度对时间关系图如图4所示，图4中，横坐标为时间，纵坐标为加权融合速度。本实施例中仅对第1段直线上加权融合速度对时间的关系求解方法进行说明，第2/3/4段与之同理，此处不再赘述。

步骤(5)：将加权融合速度V(t)大小分解获得融合速度

步骤(6)：基于融合速度向量进行逆运算，获得每个关节角速度

通过逆运算，计算每个关节角速度对时间关系，如图5所示，式中，横坐标为时间，纵坐标为关节角速度。

步骤7：判断在当前段上，各关节角速度最大值是否小于允许最大关节角速度

由图5显然可以看出，满足条件max(θ_j(t))＜2 1≤j≤6。可以继续规划下一段。

本文中所描述的具体实施例仅仅是对本发明精神做举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (5)

1.一种基于位置和姿态约束的机器人小线段前瞻规划方法，采用小线段前瞻规划方法获得机器人每个关节角速度规划数据，其特征在于采用融合速度向量替代线性速度作为前瞻规划方法中的规划量，其中：v₁、v₂、v₃表示末端坐标系原点沿着基坐标X、Y、Z轴方向运动的线速度，ω₁，ω₂，ω₃表示欧拉角的角速度。

2.根据权利要求1所述的方法，其特征在于融合速度经加权后获得加权融合速度向量替代线性速度作为前瞻规划方法中的规划量，其中σ为缩放系数由用户设定。

3.根据权利要求1或2所述的方法，其特征在于前瞻规划方法中，包括确定前瞻段数、当前第i段段末融合速度V_e ⁱ、以及当前段加权融合速度V(t)与时间t的关系规划的步骤，所述三个步骤均采用分段平滑指数加减速曲线对速度进行规划，加速度公式如下：

其中A_m为允许最大融合加速度，由用户设定；t_u为加速总时间，时间t单位为s；V₀表示在初始速度为0的前提下，按上述加减速函数加速0.5秒后的速度，V₀＝V(0.5)＝A_m(1-1.5e^-0.5)。

4.根据权利要求3所述的方法，其特征在于具体步骤为：

(1)利用离线编程软件根据机器人需要运动的轨迹路径，生成机器人末端坐标系离散位置和姿态数据，然后求取每一段的融合路径长度L_i

设定允许最大线速度v_max、允许最大线加速度a_max、插补周期T、缩放系数σ及允许关节最大运动角速度θ_max；系统允许最大加权融合速度V_m＝v_max，系统允许最大融合加速度A_m＝a_max；

(2)确定前瞻段数n

在速度前瞻时，比较V_m与2V₀关系：

当V_m＜2V₀时，最小前瞻路径融合总长其中：t_x通过求解方程A_me^(t-0.5)-A_me^-0.5-A_me^-0.5t＝0.5V_m的解t＝t_x获得；

当V_m≥2V₀时，最小前瞻路径融合总长

在规划第i段时，前瞻段数n满足n＝min(m)，m满足其中m为非零自然数；

(3)对当前任意i段，已知前瞻段数n，取第k＝i+n段段末加权融合速度然后通过反向递推和正向推理确定当前第i段段末加权融合速度V_e ⁱ：

(3-1)反向递推获得第i段段末反向递推加权融合速度

首先，k＝i+n，第k段段末加权融合速度取满足下述3个约束条件的最小值为第k＝i+n段初始加权融合速度

约束1：若第k段上一直在减速，第k段初始加权融合速度则

其中，V_z求取过程：求出方程(V_c ^k+1+A_me^t-0.5-A_me^0.5(1+t))*t＝0.5L_k的解t_y

约束2：第k段初始加权融合速度

约束3：在两端线段衔接处，第k段初始加权融合速度满足其中，与模值相等，T为插补周期；

然后，k＝i+n-1，第k段段末加权融合速度按上述3个约束条件，求出第i+n-1段的初始加权融合速度

以此类推，直至k＝i+1求出第i+1段初始加权融合速度获得当前段段末反向推理加权融合速度

(3-2)正向推理获得第i段段末正向推理加权融合速度

上式中：第i段初始加权融合速度其中 为已知量，t_z通过求解方程的解t＝t_z获得；

(3-3)比较取小获得当前第i段段末加权融合速度V_e ⁱ：

(4)根据规划当前段加权融合速度V(t)与时间t的关系，分为以下6种情况：

1)L_i＝L_a时，表明加权融合速度V(t)一直在加速，加权融合速度V(t)对时间t的关系是：

当时，

当时，

2)L_i＝L_a时，表明加权融合速度V(t)一直在减速，加权融合速度V(t)对时间t的关系是：

当时，

当时

3)L_a＜L_i＜L_b时，表明加权融合速度V(t)先加速后匀速，但是速度没有达到最大值；

当V_c ⁱ≤V_e ⁱ-2V₀时，

当V_c ⁱ＞V_e ⁱ-2V₀时，

4)L_a＜L_i＜L_b时，表明加权融合速度V(t)先减速再匀速；

当V_c ⁱ≤V_e ⁱ-2V₀时，

当V_c ⁱ＞V_e ⁱ-2V₀时，

5)V_e ⁱ＝V_m，L_i≥L_b时，表明加权融合速度V(t)先加速到最大值再匀速；

当V_m≥V_c ⁱ+2V₀时，

当V_m＜V_c ⁱ+2V₀时

6)L_i≥L_b时，表明加权融合速度V(t)先加速到最大值再匀速最后减速；

t₁₁表示匀加速时间，t₁₂表示匀减速时间，t₁₃表示保持最大速度运行时间：

当V_m≥V_c ⁱ+2V₀,V_m≥V_e ⁱ+2V₀时

当V_m≥V_c ⁱ+2V₀，V_m＜V_e ⁱ+2V₀，

当V_m≤V_c ⁱ+2V₀，

上述6种情况中，L_a表示从第i段初始加权融合速度V_c ⁱ加速或减速到第i段段末加权融合速度V_e ⁱ所经过的融合路径长度，t₁表示在|V_e ⁱ-V_c ⁱ|＜2V₀的情况下，从第i段初始加权融合速度V_c ⁱ加速或减速到第i段段末加权融合速度V_e ⁱ所需时间；L_b表示从第i段初始加权融合速度V_c ⁱ加速到允许最大加权融合速度V_m所经过的融合路径长度与从允许最大加权融合速度V_m减速到第i段段末加权融合速度V_e ⁱ所经过的融合路径长度之和，t₂表示在|V_m-V_c ⁱ|＜2V₀的情况下，从第i段初始加权融合速度V_c ⁱ加速到允许最大加权融合速度V_m所需时间，t₃表示在|V_e ⁱ-V_m|＜2V₀的情况下，从允许最大加权融合速度V_m减速到第i段段末加权融合速度V_e ⁱ所需时间；

t₁通过求出方程的解t＝t₁获得；

t₂通过求出方程的解t＝t₂获得；

t₃通过求出方程的解t＝t₃获得；

(5)将加权融合速度V(t)加工处理获得融合速度向量第i段：

(6)基于融合速度向量进行逆运算，获得每个关节角速度，各关节角速度θ_j,1＜j≤6

[θ₁(t) θ₂(t) θ₃(t) θ₄(t) θ₅(t) θ₆(t)]^T＝J^-1[v₁(t) v₂(t) v₃(t) ω₁(t) ω₂(t)ω₃(t)]^T

J表示机器人正运算的雅可比矩阵；

(7)判断在当前段上，各关节角速度最大值是否小于允许最大关节角速度θ_max

max(θ_j(t))＜θ_max 1≤j≤6

若满足，表示所求关节速度满足要求，继续规划下一段；

否则，说明当前第i段段末加权融合速度过大，导致关节角速度过大；将乘以比例系数μ，然后跳转到步骤(4)，重新规划加权融合速度V(t)与时间t关系，直到满足条件为止。

5.根据权利要求4所述的方法，其特征在于步骤(7)中，μ＝e^-0.3m-e^-3，m为不满足此条件次数，m＝1，2，3......10。