CN114217573A - 一种速度前瞻控制方法 - Google Patents

Abstract

本发明公开了一种基于double S的速度前瞻方法，根据设定的路点信息、机器人的运动学和动力学约束给出每个转折点的速度，并进一步采用了回溯法进行速度规划。该方法结构简单，且对于复杂轨迹也能够保证其实时性。

Description

一种速度前瞻控制方法

技术领域

本发明属于机器人控制技术领域，特别涉及一种速度前瞻控制方法。

背景技术

普通的加减速控制在连续线段转折点处通常会以极低的速度通过，不仅造成了电机的频繁起停，也无法充分发挥机器人的性能，因此需要进行速度前瞻规划。速度前瞻规划的目的是在约束条件允许的范围内，通过获取后续路径的信息，将转折点的速度尽可能地提高，从而提高机器人的整体速度。

发明内容

本发明针对上述现有技术的存在的问题，提供一种速度前瞻控制方法，速度前瞻规划的方式是采取回溯法对设定的路点信息进行扫描，在进行每个转折点的速度规划时需要满足运动学和动力学约束条件，获取信息后，将规划完成后的转折点速度传给加减速控制过程，以完成后面的实时粗插补过程。

本发明通过以下技术手段实现解决上述技术问题的：

一种速度前瞻控制方法，包括如下步骤：

步骤1)路径长度获取：根据用户设置的路点信息确定起点、终点以及生成于起点、终点之间的路径，获取路径长度、路径上转折点P_i及对应于转折点P_i的缓存数据；

步骤2)速度约束条件获取：确定转折点P_i的速度约束条件，以及转折点P_i前后端以P_s为起点，以P_e+1为终点的缓存数据，点P_s处速度为V_s，点P_e+1的速度设置为0，线段P_eP_e+1为新补进线段；

步骤3)向前回溯：从点P_e向前回溯，计算转折点P_i向前回溯时的速度V_prev，比较圆弧段P_sP_e的初始速度V_s与直线段P_i-1P_s的终止速度V_e是否相等，相等则停止向前回溯，标记点为P_m，P_m之前的运动信息保持不变；

步骤4)向后规划：以V_newe作为点P_m的新规划初始速度，从点P_m向后进行调整，计算转折点P_i处向后调整时的速度V_nexti，直至点P_e，停止向后调整；

步骤5)插补：将起始段P_sP_s+1的起始速度V_s，终止速度V_newe传送至插补模块，以P_s+1为起点，P_e+2为终点循环执行步骤2)～4)，点P_s+1的速度为V_newe，点P_e+2的速度为V_nexte＝V_max×k_r,k_r＝R/R_max，V_max为关节电机的极限速度，直至P_i为终点。

进一步的，步骤3)与步骤4)中各段轨迹的运动参数根据double S的规划方式进行计算。

进一步的，转折点P_i的速度约束条件获取包括：

运动方式为MOVEJ-MOVEJ时，转折点P_i的速度约束为V_lim＝min(V_Motorlim,V_user)，其中，V_Motorlim为当前上位机设置的关节速度极限，V_user为设置的转折点速度，V_user＝V_max×k，V_max为关节电机的极限速度，k为设置的转折点交融增益；

运动方式为MOVEL-MOVEL时，并采用圆弧进行过渡，转折点P_i的速度约束为V_lim＝min(R_maxθ/T,V_max)，R_max为最大的圆弧半径，T为圆弧段的运行时间，V_max为机器人末端的运动极限速度。

本发明的有益效果为：本发明通过速度前瞻规划，对转折点的包括速度、路径等转折转改进行优化，从而可以将转折点的速度尽可能地提高，并提高机器人的整体速度。

附图说明

图1为速度前瞻控制方法的步骤流程图；

图2为圆弧进行过渡的路径；

图3为未进行前瞻规划的路径；

图4为未进行前瞻规划的速度曲线；

图5为进行前瞻规划的路径；

图6为进行前瞻规划的速度曲线。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

如图1所示，本发明的速度前瞻控制方法包括如下步骤：

步骤1)路径长度获取：根据用户设置的路点信息确定起点、终点以及生成于起点、终点之间的路径，若路径为直线段则路径长度的获取采用的是空间点到点的距离公式(假设空间中的两点为P₁(x₁,y₁,z₁),P₂(x₂,y₂,z₂)，其间的距离为

若是圆弧段路径长度的获取则为空间内圆弧的弧长(l＝R*θ，其中R为半径，θ为圆心角)。

首先读取N个路点的信息，根据每个路点的运动形式和交融信息，确定需要速度前瞻的缓存数据(缓存数据的数量与用户设置的路点数和交融路点数有关)。

假设有T个缓存数据，起点为P_s，终点为P_e+1，点P_s处速度为V_s(已知前瞻完线段P_{s- 1}P_s的末点速度)，点P_e+1的速度设置为0，线段P_eP_e+1为新补进线段。

步骤2)速度约束条件获取：根据转折点前后的运动方式可以分为MOVEJ-MOVEJ和MOVEL-MOVEL两种。

A、若运动方式为MOVEJ-MOVEJ，则转折点的速度约束为V_lim＝min(V_Motorlim,V_user)。其中，V_Motorlim为当前上位机设置的关节速度极限，V_user为用户设置的转折点速度V_user＝V_max×k，V_max为关节电机的极限速度，k为用户设置的转折点交融增益。

B、若运动方式为MOVEL-MOVEL，并采用圆弧进行过渡，则如图2所示：

此时限制转折点速度的约束主要有：过渡圆弧的半径R和机器人末端的运动限制。当P_i-1，P_i，P_i+1确定之后则确定了最大的圆弧半径R_max和最大弧长d_max，此时V_lim＝min(R_maxθ/T,V_max)。其中，T为圆弧段的运行时间，V_max为机器人末端的运动极限速度。

步骤3)速度规划：包括向前回溯和向后调整规划两步。

A、向前回溯：从点P_e向前回溯，计算各点P_i向前回溯时的速度V_prev，比较此时圆弧段P_sP_e的初始速度V_s与直线段P_i-1P_s的终止速度V_e是否相等，若相等则停止向前回溯，此时点为P_m，该点之前的运动信息保持不变。向前回溯时，各段轨迹的运动参数根据double S的规划方式进行计算。

B、向后调整规划速度：在步骤3)中结束向前回溯之后，以V_newe作为点P_m的新规划初始速度，从点P_m向后进行调整，计算各点P_i处向后调整时的速度V_nexti，直至点P_e，停止向后调整，向后调整时，各段轨迹的运动参数根据double S的规划方式进行计算。

步骤4)循环：将起始段P_sP_s+1的起始速度V_s，终止速度V_newe传送至插补模块。补进新线段P_e+1P_e+2，点P_s+1的速度为V_newe，点P_e+2根据用户给定的圆弧半径进行计算可得V_nexte＝V_max×k_r,k_r＝R/R_max，以P_s+1为起点，P_e+2为终止点进行第二轮前瞻，即转至步骤2)。

基于上述的速度前瞻控制方法，本实施例同时提供了仿真实验，实验类型为MOVEL与MOVEL之间的交融，实验参数设置如下：机器人末端最大速度V_max＝1m/s，最大加速度a_max＝4m/s²，最大加加速J_max＝25m/s³，插补周期为Tt＝10ms，实验设置了6个点。仿真结果参见图3～6，图3为未进行前瞻规划的路径，图4为未进行前瞻规划的速度曲线，图5为进行转折点设置即前瞻的路径，图6为进行前瞻规划的速度曲线，其中圆圈中的点为设置的转折点。

要说明的是，在本文中，如若存在第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (3)

1.一种速度前瞻控制方法，其特征在于，包括如下步骤：

步骤1)路径长度获取：根据用户设置的路点信息确定起点、终点以及生成于起点、终点之间的路径，获取路径长度、路径上转折点P_i及对应于转折点P_i的缓存数据；

步骤2)速度约束条件获取：确定转折点P_i的速度约束条件，以及转折点P_i前后端以P_s为起点，以P_e+1为终点的缓存数据，点P_s处速度为V_s，点P_e+1的速度设置为0，线段P_eP_e+1为新补进线段；

步骤3)向前回溯：从点P_e向前回溯，计算转折点P_i向前回溯时的速度V_prev，比较圆弧段P_sP_e的初始速度V_s与直线段P_i-1P_s的终止速度V_e是否相等，相等则停止向前回溯，标记点为P_m，P_m之前的运动信息保持不变；

步骤4)向后规划：以V_newe作为点P_m的新规划初始速度，从点P_m向后进行调整，计算转折点P_i处向后调整时的速度V_nexti，直至点P_e，停止向后调整；

步骤5)插补：将起始段P_sP_s+1的起始速度V_s，终止速度V_newe传送至插补模块，以P_s+1为起点，P_e+2为终点循环执行步骤2)～4)，点P_s+1的速度为V_newe，点P_e+2的速度为V_nexte＝V_max×k_r,k_r＝R/R_max，V_max为关节电机的极限速度，直至P_i为终点。

2.根据权利要求1所述的一种速度前瞻控制方法，其特征在于，步骤3)与步骤4)中各段轨迹的运动参数根据double S的规划方式进行计算。

3.根据权利要求1所述的一种速度前瞻控制方法，其特征在于，转折点P_i的速度约束条件获取包括：

运动方式为MOVEJ-MOVEJ时，转折点P_i的速度约束为V_lim＝min(V_Motorlim,V_user)，其中，V_Motorlim为当前上位机设置的关节速度极限，V_user为设置的转折点速度，V_user＝V_max×k，V_max为关节电机的极限速度，k为设置的转折点交融增益；

运动方式为MOVEL-MOVEL时，并采用圆弧进行过渡，转折点P_i的速度约束为V_lim＝min(R_maxθ/T,V_max)，R_max为最大的圆弧半径，T为圆弧段的运行时间，V_max为机器人末端的运动极限速度。

Images