CN113190021A - 一种工业机器人小线段轨迹规划算法 - Google Patents

Abstract

本发明涉及机器人控制技术领域，尤其涉及一种工业机器人小线段轨迹规划算法。本发明采用如下技术方案：采用数值法求解几何路径特征点的关节限制速度，然后通过速度回溯和速度前瞻对小线段进行合并，得到实际关键点，再根据实际关键点进行速度规划。本发明的优点在于：1、通过建立关节空间限制速度到笛卡尔空间的映射关系，利用数值法求解路径特征点的限制速度，以此作为运动规划中对应特征点的限制速度，有效确保在该特征点的运动不会超过关节空间的运动限制；2、通过速度回溯和速度前瞻将频繁变化的小线段进行合并，从而减少运动过程中加速度的频繁变化，有效提高运动稳定性，提高加速运行时间，提高运行效率。

Description

一种工业机器人小线段轨迹规划算法

技术领域

本发明涉及机器人控制技术领域，尤其涉及一种工业机器人小线段轨迹规划算法。

背景技术

随着工业自动化程度的提高，机器人越来越多地被应用到复杂曲线、曲面的加工，复杂路径难以通过传统的示教方式得到，通常是采用离线编程的方式生成。通过离线编程得到复杂路径通常是用离散的小线段连接成线性路径；因此，小线段运动是机器人进行复杂曲线运动的基础。

由于在小线段的连接处切线方向发生突变，因此必须对小线段路径进行平滑处理，用光滑连续的样条曲线拟合原有路径，这一过程被称为路径光顺，然后需要确定机器人沿该路径运动的时间规律，这一过程称为轨迹规划。

在进行轨迹规划时需要设定最大速度、最大加速度等物理约束信息，但是路径点通常位于笛卡尔空间，而机器人的运动性能是由驱动各个关节转动的电机、减速器等物理设备决定的，换言之，机器人的性能约束在关节空间，通常表现为各个关节的最大转动角速度、角加速度等，而轨迹规划需要用到沿几何路径的最大速度、加速度等信息，因此需要建立机器人从关节空间到笛卡尔空间运动约束的映射关系，以此保证机器人在运动过程中不会超过各关节的运动约束。

在实际应用中，机器人运动往往是多段连续运动，因此，轨迹规划算法需要对整个运动路径进行整体考虑。目前常采用双向扫描算法。传统双向扫描算法主要分为两步：回溯和前瞻。首先选取几何路径曲率极值点作为特征点，然后根据特征点的曲率值求解弓高误差限制速度；然后进行速度回溯，从运动终点开始求解能加速到达前一个特征点的最大速度，该速度与弓高误差限制速度的较小值作为前述前一个特征点的限制速度值，然后再从该点继续求解能够到达再前一个特征点的最大速度，以此类推，直至回溯到运动起点，以此求得各个特征点的限制速度值，保证在限制速度内运动时可以在终点准确停止；之后再进行速度前瞻规划，依次求解各个特征点之间的速度曲线。传统的双向扫描算法只适用于简单多点的连续运动，当点与点之间的距离太短且点的数量过多，会造成频繁的加减速运动，导致运动速率降低、振动较大等问题。且在求解特征点的最大限制速度时，只考虑弓高误差对速度的限制，没有考虑到关节空间的运动约束。

发明内容

本发明的目的在于提供一种工业机器人小线段轨迹规划算法，具体在于提供一种适用于复杂小线段路径且能适应机器人关节空间约束的轨迹规划算法。

为达到上述目的，本发明采用如下技术方案：一种工业机器人小线段轨迹规划算法，其特征在于：包括如下步骤：

S01、输入路径，并对路径中的每个小线段路径进行平滑处理，同时确定各个进行平滑处理小线段路径的特征点。

S02、利用数值法求解步骤S01中所确定的各个特征点的关节限制速度。

S03、速度回溯，以路径结束点为回溯起点，反向向上一个特征点进行回溯，以此求解各个特征点的距离限制速度；若该特征点的距离限制速度小于关节限制速度，则去除该特征点，并继续向下一个特征点进行速度回溯，否则保留该特征点并以该特征点作为回溯起点向下一特征点进行速度回溯，最终剩下的特征点记为初始关键点。

S04、速度前瞻，从路径起始点开始正向向下一个初始关键点进行前瞻，以此求解各个初始关键点的距离限制速度；若该初始关键点的距离限制速度小于关节限制速度，则去除该初始关键点，并继续向下一个初始关键点进行速度前瞻，否则保留该初始关键点并以该初始关键点为起点向下一初始关键点进行速度前瞻，最终剩下的初始关键点记为实际关键点。

S05、根据步骤S04中获得实际关键点使用运动规划算法进行速度规划，求解各个实际关键点之间运动的时间规律。

具体的，步骤S03和S04中求解距离限制速度采用最大可达速度算法，所述最大可达速度算法，是在已知最大允许加速度、最大允许捷度的条件下，给定初始速度和运动距离，求解能达到的最大速度。

具体的，步骤S03和S04中采用的最大可达速度算法和步骤S05中采用的运动规划算法均采用S型速度规划方法。

优选的，S型速度规划方法采用7段式S型速度规划方法。

具体的，步骤S05中的运动规划算法，是在已知最大速度、最大加速度和最大捷度的前提，以相邻两个实际关键点的关节限制速度为初速度和末速度，对应两个实际关键点之间路径的长度为运动距离，求解7段S型速度曲线各段的时间和实际最大速度及加速段和减速段的实际最大加速度。

具体的，运动规划算法为，先利用最短运动距离算法分别计算从初速度加速到最大速度的最短位移和从最大速度减速到末速度的最短位移，以此得出恰好达到最大速度时的总位移量；若运动距离大于或等于恰好达到最大速度时的总位移量，则实际最大速度与最大速度一致，若运动距离小于恰好达到最大速度时的总位移量，则利用二分法在末速度和最大速度之间计算实际最大速度；最后根据求得的实际最大速度利用最短运动距离算法分别计算，从初速度加速到实际最大速度这一加速段的实际最大加速度和具体各段时间，以及从实际最大速度减速到末速度这一减速段的实际最大加速度和具体各段时间，并计算匀速段时长。

具体的，最短运动距离算法为已知最大加速度和最大捷度，根据初始速度和结束速度求解最短位移、实际最大加速度以及加加速度段、匀加速度段和减加速段的时间。

优选的，步骤S01中对路径中的每个小线段路径进行平滑处理具体利用5次Bezier曲线对每个小线段路径进行拟合，所确定的特征点为所拟合5次Bezier曲线的中点。

本发明的优点在于：1、通过建立关节空间限制速度到笛卡尔空间的映射关系，利用数值法求解路径特征点的限制速度，以此作为运动规划中对应特征点的限制速度，有效确保在该特征点的运动不会超过关节空间的运动限制；2、通过速度回溯和速度前瞻将频繁变化的小线段进行合并，从而减少运动过程中加速度的频繁变化，有效提高运动稳定性，提高加速运行时间，提高运行效率。

附图说明

附图1为实施例中小线段规划算法的流程示意图；

附图2为小线段路径图；

附图3为采用传统双向扫描算法对图1的小线段路径进行规划的速度曲线图；

附图4为采用实施例中小线段轨迹规划算法对图1的小线段路径进行规划的速度曲线图。

具体实施方式

一种工业机器人小线段轨迹规划算法，其特征在于：包括如下步骤：

S01、输入路径，并对路径中的每个小线段路径进行平滑处理，同时确定各个进行平滑处理小线段路径的特征点。

S02、利用数值法求解步骤S01中所确定的各个特征点的关节限制速度。

S03、速度回溯，以路径结束点为回溯起点，反向向上一个特征点进行回溯，以此求解各个特征点的距离限制速度；若该特征点的距离限制速度小于关节限制速度，则去除该特征点，并继续向下一个特征点进行速度回溯，否则保留该特征点并以该特征点作为回溯起点向下一特征点进行速度回溯，最终剩下的特征点记为初始关键点。

S04、速度前瞻，从路径起始点开始正向向下一个初始关键点进行前瞻，以此求解各个初始关键点的距离限制速度；若该初始关键点的距离限制速度小于关节限制速度，则去除该初始关键点，并继续向下一个初始关键点进行速度前瞻，否则保留该初始关键点并以该初始关键点为起点向下一初始关键点进行速度前瞻，最终剩下的初始关键点记为实际关键点。

S05、根据步骤S04中获得实际关键点使用运动规划算法进行速度规划，求解各个实际关键点之间运动的时间规律。

优选的，步骤S01中对路径中的每个小线段路径进行平滑处理具体利用5次Bezier曲线对每个小线段路径进行拟合，所确定的特征点为所拟合5次Bezier曲线的中点。

其中，步骤S02中利用数值法求解特征点的关节空间限制速度具体步骤为：确定空间几何路径后，路径上每个空间点均对应一个关节角矢量，该关节角矢量的维数与机器人的关节轴数一致，以六轴机器人为例，因此，该关节角矢量与路径上距离起点弧长为s的点可以建立对应关系，记为：

其中，q为关节角矢量，s为距离起点的弧长。

分别对时间求一阶导和二阶导，可得：

取与特征点距离为∆s的前后两个路径点，通过机器人逆运动学求解三个连续点的关节角矢量坐标q₁、q₂、q₃，当∆s足够小时，可利用数值差分法近似求解局部位置的映射关系Q。

设关节角速度为ω，角加速度为α，空间速度为v，空间加速度为a，根据上述两式可得

其中，ω、α、q_j（j=1，2，3）均为六维变量，v和a时一维变量，当空间加速度a=0时，可求v的最大值v_max，该速度值即为该特征点的关节限制速度：

其中，q_i，j（i=1，2，…6；j=1，2，3）表示第j个关节坐标中关节轴i的角度值。

另外，上述步骤S03和S04中求解距离限制速度采用最大可达速度算法，所述最大可达速度算法，是在已知最大允许加速度、最大允许捷度的条件下，给定初始速度和运动距离，求解能达到的最大速度。

具体的，上述步骤S03中的速度回溯，以下举例进行详细说明，计特征点P_i的关节限制速度为v_1，i，设结束点为P_n，利用最大可达速度算法求解从P_n到P_n-1的距离限制速度v_2，n-1，假设发现v_2，n-1＜v_1，n-1，则将P_n-1去除，依旧从P_n开始向P_n-2进行回溯，求解v_2，n-2，假设依旧发现v_2，n-2＜v_1，n-2，则依旧将P_n-2去除，依旧从P_n开始向P_n-3进行回溯，求解P_n-3的距离限制速度v_2，n-3，此时假设v_2，n-3＞v_1，n-3，则保留P_n-3，以该点的关节限制速度v_1，n-3为该点的限制速度，然后以P_n-3为起点继续进行回溯，直到路径的起始点为结束，剩下的特征点即为初始关键点。

与速度回溯相似，步骤S04中的速度前瞻，以下举例进行详细说明，设路径起始点为P₁，利用最大可达速度算法求解从P₁到下一个初始关键点P₂的距离限制速度v_2，2，若v_2，2小于该点的限制速度v_1，2，则去除该初始关键点，并继续以P₁为起点，向P₃求解距离限制速度v_2，3，若v_2，3仍然小于该点的限制速度v_1，3，则仍然去除该点，并继续以P₁为起点，向P₄求解距离限制速度v_2，4，假设此时v_2，4大于该点的限制速度v_1，4，则保留该点，并以该点为起点，向下一个初始关键点进行前瞻，直到路径的结束点为止，剩下的初始关键点即为实际关键点。

通过速度回溯和速度前瞻，可以对小线段进行合并，将运动过程中不会对速度规划产生约束的特征点进行隐藏，只保留关节限制速度较低的运动关键点，从而可以更好地发挥机器人的物理性能，提高运动规划的可靠性。

具体的，上述步骤S03和S04中采用的最大可达速度算法和步骤S05中采用的运动规划算法均采用S型速度规划方法，优选采用7段式S型速度规划方法。7段式S型速度规划方法是将一段运动轨迹分为加速段、匀速段和减速段三个阶段，其中加速段和减速段又分别再细分为三段，因此加起来总共7段。7段式S型速度规划方法包括最大可达速度算法、最短运动距离算法和运动规划算法。采用7段S型速度规划方法，可以保证运动过程中捷度可控、加速度连续，即每个特征点的加速度均为0。

其中，最大可达速度算法（简称V_VD）是指在最大允许加速度A_m、最大允许捷度J_m的条件下，给定初始速度v_s和运动距离d，求解能达到的最大速度v_e整个过程只有加速段，即7段S型速度曲线中的前3段，即加加速度段、匀加速段和减加速段，设三个阶段的时间分别为t₁、t₂、t₃。

计算过程中，先求解出恰好达到最大允许加速度A_m时的临界位移d₀。

1）若d≥d₀，则可以达到最大允许速度A_m且存在匀加速段，可以解得：

其中：

2）若d＜d₀，则无匀加速段，加速度无法达到A_m。此时t₂=0，t₁=t₃由运动过程的位移条件和速度条件可得如下方程：

求解上述方程的t₁后，可求得最大可达速度v_e。

其中，步骤S05中的运动规划算法，是在已知最大允许速度、最大允许加速度和最大允许捷度的前提，以相邻两个实际关键点的关节限制速度为初速度和末速度，对应两个实际关键点之间路径的长度为运动距离，求解7段S型速度曲线各段的时间和实际最大速度及加速段和减速段的实际最大加速度。

具体的，运动规划算法为，先利用最短运动距离算法分别计算从初速度加速到最大允许速度的最短位移和从最大允许速度减速到末速度的最短位移，以此得出恰好达到最大允许速度时的总位移量；若运动距离大于或等于恰好达到最大允许速度时的总位移量，则实际最大速度与最大允许速度一致，若运动距离小于恰好达到最大允许速度时的总位移量，则利用二分法在末速度和最大允许速度之间计算实际最大速度；最后根据求得的实际最大速度利用最短运动距离算法分别计算，从初速度加速到实际最大速度这一加速段的实际最大加速度和具体各段时间，以及从实际最大速度减速到末速度这一减速段的实际最大加速度和具体各段时间，并计算匀速段时长。

具体的，最短运动距离算法为已知最大允许加速度和最大允许捷度，根据初始速度和终点速度求解最短位移、实际最大加速度以及加加速度段、匀加速度段和减加速段的时间。

其中，最短运动距离算法（简称D_VV）是在最大允许加速度A_m、最大允许捷度J_m的条件下，给定初始速度v_s和终点速度v_e，求解从v_s加速或减速运动到v_e所需要的最短时间T_f、最短距离d、实际最大加速度A和整个运动过程中各个时间段的长度，为了使计算过程中不失一般性，假定v_s＜v_e，若实际情况相反，则交换v_s和v_e的数值进行计算后再将结果反向输出，具备一般性后，其同样仅包含7段S型速度曲线的加速段，具体包含加加速段、匀加速段和减加速段，同样设该三个阶段的时间为t₁、t₂、t₃。其计算过程具体为：

设实际速度变化量为∆v=v_e-v_s，并先求解恰好达到最大允许加速度A_m的速度变化量∆v₀：

1）若∆v≥∆v₀，则可达到最大允许加速度A_m。实际最大加速度A=A_m，根据速度变化关系可以求得整个加速过程的各个时间段：

2）若∆v＜∆v₀，则无法达到最大允许加速度A_m。此时t₂=0，通过速度变化关系可得：

因此，总体运动时间即最短时间T_f和总体运动位移即最短距离d可求得：

另外，运动规划算法（简称S_VVD）是在最大允许速度V_m、最大允许加速度A_m、最大允许捷度J_m的条件下，给定初速度v_s、末速度v_e和运动距离d，求解整个7段S型速度曲线的所有运动信息，包括实际最大速度V、加速段实际最大加速度A_加、减速段实际最大加速度A_减以及各个阶段的时间t₁、t₂、t₃、t₄、t₅、t₆、t₇。为了不失一般性，假定v_s＜v_e，若实际情况相反，则交换v_s和v_e的数值进行计算后再将结果反向输出。若运动距离d较小，则在整个运动过程中可能无法达到最大允许速度V_m，因此首先判断是否能达到最大允许速度。

先计算恰好达到最大允许速度的临界位移d₀。以v_s和V_m为初始速度和终点速度代入最短运动距离法中求解恰好加速到最大允许速度的最短位移d₁，同理可得恰好从最大允许速度V_m减速到v_e的最短位移d₂，d₀=d₁+d₂。

1）若d≥d₀，则可达到最大允许速度V_m，即，V=V_m。

此时加速段和减速段的所有信息已经通过最短运动距离算法得到，只需求解匀速段时间t₄，根据位移变化关系可得：

2）若d＜d₀，则无法达到最大允许速度V_m。

此时，实际最大速度在v_e和V_m之间，可以通过二分法计算得出，为了提高搜索速度，可以先设定一个时间阀值为3倍的插补周期3t₀，当实际匀速运行时间小于时间阀值时，二分搜索结束。具体的，设实际最大速度V=（v_e+V_m）/2，将V的数值和v_s的数值利用最短运动距离算法求解加速段最短距离d₁，以及V的数值和v_e的数值利用最短运动距离算法求解减速段最短距离d₂，则t₄=（d-d₁-d₂）/V，若t₄＜0，则向下求解，若t₄＞3t₀，则向上求解，直到0≤t₄≤3t₀，此时得到实际最大速度V和t₄，同时利用最短运动距离算法求得t₁~t₃、t₅~t₇、A_加、A_减。

下面以图2所示的曲线为例，图3为使用传统双向扫描算法规划的结果，图4为使用本发明的轨迹规划算法规划的结果，从图3和图4对比可以看出，图4中速度的变化比图3中的更为平滑，可见本发明所使用的规划算法能更好地发挥机器人的物理性能，有效提高运动规划的可靠性。

当然，以上仅为本发明较佳实施方式，并非以此限定本发明的使用范围，故，凡是在本发明原理上做等效改变均应包含在本发明的保护范围内。

Claims (8)

1.一种工业机器人小线段轨迹规划算法，其特征在于：包括如下步骤：

S01、输入路径，并对路径中的每个小线段路径进行平滑处理，同时确定各个进行平滑处理小线段路径的特征点；

S02、利用数值法求解步骤S01中所确定的各个特征点的关节限制速度；

S03、速度回溯，以路径结束点为回溯起点，反向向上一个特征点进行回溯，以此求解各个特征点的距离限制速度；若该特征点的距离限制速度小于关节限制速度，则去除该特征点，并继续向下一个特征点进行速度回溯，否则保留该特征点并以该特征点作为回溯起点向下一特征点进行速度回溯，最终剩下的特征点记为初始关键点；

S04、速度前瞻，从路径起始点开始正向向下一个初始关键点进行前瞻，以此求解各个初始关键点的距离限制速度；若该初始关键点的距离限制速度小于关节限制速度，则去除该初始关键点，并继续向下一个初始关键点进行速度前瞻，否则保留该初始关键点并以该初始关键点为起点向下一初始关键点进行速度前瞻，最终剩下的初始关键点记为实际关键点；

S05、根据步骤S04中获得实际关键点使用运动规划算法进行速度规划，求解各个实际关键点之间运动的时间规律。

2.根据权利要求1所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述步骤S03和S04中求解距离限制速度采用最大可达速度算法，所述最大可达速度算法，是在已知最大允许加速度、最大允许捷度的条件下，给定初始速度和运动距离，求解能达到的最大速度。

3.根据权利要求2所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述步骤S03和S04中采用的最大可达速度算法和步骤S05中采用的运动规划算法均采用S型速度规划方法。

4.根据权利要求3所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述S型速度规划方法优选采用7段式S型速度规划方法。

5.根据权利要求4所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述步骤S05中的运动规划算法，是在已知最大速度、最大加速度和最大捷度的前提，以相邻两个实际关键点的关节限制速度为初速度和末速度，对应两个实际关键点之间路径的长度为运动距离，求解7段S型速度曲线各段的时间和实际最大速度及加速段和减速段的实际最大加速度。

6.根据权利要求5所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述运动规划算法为，先利用最短运动距离算法分别计算从初速度加速到最大速度的最短位移和从最大速度减速到末速度的最短位移，以此得出恰好达到最大速度时的总位移量；若运动距离大于或等于恰好达到最大速度时的总位移量，则实际最大速度与最大速度一致，若运动距离小于恰好达到最大速度时的总位移量，则利用二分法在末速度和最大速度之间计算实际最大速度；最后根据求得的实际最大速度利用最短运动距离算法分别计算，从初速度加速到实际最大速度这一加速段的实际最大加速度和具体各段时间，以及从实际最大速度减速到末速度这一减速段的实际最大加速度和具体各段时间，并计算匀速段时长。

7.根据权利要求6所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述最短运动距离算法为已知最大加速度和最大捷度，根据初始速度和结束速度求解最短位移、实际最大加速度以及加加速度段、匀加速度段和减加速段的时间。

8.根据权利要求1-7任一项所述的一种工业机器人小线段轨迹规划算法，其特征在于：所述步骤S01中对路径中的每个小线段路径进行平滑处理具体利用5次Bezier曲线对每个小线段路径进行拟合，所确定的特征点为所拟合5次Bezier曲线的中点。

Images