CN116277024A

CN116277024A - 一种初加速度不为0的s型加减速规划方法

Info

Publication number: CN116277024A
Application number: CN202310419797.8A
Authority: CN
Inventors: 周昕芸; 胡家勇; 魏旭东; 陈利敏
Original assignee: Wuxi Xinje Electric Co Ltd
Current assignee: Wuxi Xinje Electric Co Ltd
Priority date: 2023-04-19
Filing date: 2023-04-19
Publication date: 2023-06-23

Abstract

本发明涉及工业机器人运动控制技术领域，具体是一种初加速度不为0的S型加减速规划方法，在速度规划层中通过给定加速度、速度、位置等参数计算变速过程中，加速度的分段时间、以及各段端点的加速度、速度；在步进规划层中根据速度规划层的计算结果，计算得到任意时刻的位置、速度、加速度；在位置规划层中根据速度规划层的计算结果，得到该种情况下位置运动情况，对比给定位置总长度数据进行反馈是否需要重新进行速度规划。本方法能克服现有技术不能处理初速度不为0的情况，能根据初末速度情况自行选取最短时间解；同时，相比较一般的S型曲线规划，适用性范围广，计算不复杂同时兼容性强。

Description

一种初加速度不为0的S型加减速规划方法

技术领域

本发明涉及工业机器人运动控制技术领域，尤其涉及一种初加速度不为0的S型加减速规划方法。

背景技术

对于工业机器人而言，轨迹规划方法是在有地图条件或是没有地图的条件下，移动机器人按什么样的路径轨迹来行走，主要是机械臂末端行走的曲线轨迹，或是操作臂在运动过程中的位移、速度和加速度的曲线轮廓。

目前轨迹规划中对于S型加减速规划主要为给定初末速度、最大速度、加速度、最大加减速度且初末加速度为0的情况。

但是传统轨迹规划方法通常不能处理初速度不为0的情况，也不能根据初末速度情况自行选取最短时间解，进而降低了S型加减速规划的适应范围。

因此，急需一种新的技术方案来解决上述技术问题。

发明内容

本发明的目的在于克服上述现有技术的问题，提供了一种初加速度不为0的S型加减速规划方法，可有效解决传统技术不能处理初速度不为0的情况，以及不能根据初末速度情况自行选取最短时间解的技术问题。

上述目的是通过以下技术方案来实现：

一种初加速度不为0的S型加减速规划方法，包括如下步骤：

步骤(1)根据初加速度A_s大小方向、初速度V_s、稳定运行速度V_f，判断是否需要将初加速度A_s降为0，并在此基础上生成新初速度V_s'和新初加速度A_s'；

步骤(2)根据稳定运行速度V_f、新初速度V_s'和末速度V_e的关系分成4种模型并进行选择，所述模型包括加速加速模型、减速减速模型、加速减速模型和减速加速模型；

步骤(3)若为加速加速模型和减速减速模型时，则不采用7段加减速算法计算速度从新初速度V_s'变化到末速度V_e的总位移L_s；若为加速减速模型和减速加速模型时，则采用7段加减速算法计算速度从新初速度V_s'变化到末速度V_e的总位移L_s；

步骤(4)在达到稳定运行速度V_f时，判断总位移L_s是否小于给定总距离L；包括：

步骤(4-1)总位移L_s小于给定总距离L，则计算时间；

步骤(4-2)总位移L_s不小于给定总距离L，则通过二分法迭代收缩得到新稳定运行速度V_f'；

步骤(4-3)判断在新稳定运行速度V_f'下总位移L_s是否等于给定总距离L；若是，则计算时间；若否，则返回步骤(4-2)。

进一步地，步骤(1)中所述判断是否需要将初加速度A_s降为0，包括：

若是，则将初加速度A_s降为0，计算此时的时间，并计算此时的新初速度V_s'；

若否，则将初始加速度为0加速至加速度为A_s时，并根据初速度V_s和位移S₀计算新初速度V_s'。

进一步地，所述将初始加速度为0加速至加速度为A_s时，并根据初速度V_s和位移S₀计算新初速度V_s'，具体为：从当前初加速度A_s以及初速度V_s开始，直接加速到末速度V_e，计算加速度从0到A_s所用时间t₀，并用于计算新初速度V_s'以及从新初速度V_s'加速到初速度V_s所用路程S₀用；

其中，t₀＝A_s/J_max，J_max为最大加加速度；S₀为虚拟情况，即计算加速度从0到A_s，实际未发生，仅用于方便后续计算，所以总路程为负号。

进一步地，所述将初加速度A_s降为0，计算此时的时间，并计算此时的新初速度V_s'，具体为：先将加速度降到0并记录此时速度为新的7段加减速规划的新初速度V_s'，新初加速度A_s'＝0，再加速到末速度V_e，得到加减速规划的第1段时间t₀，以及加速度方向，并计算所用路程S₀。；

其中，t₀＝A_s/J_max，J_max为最大加加速度；S₀为实际情况，总路程为正。

进一步地，步骤(3)中所述若为加速加速模型和减速减速模型时，则不采用7段加减速算法计算速度从新初速度V_s'变化到末速度V_e的总位移L_s，具体为：计算从新初速度V_s'到末速度V_e的最短时间下位移小于给定总距离L能否满足：若满足条件，则按单调情况计算并补充匀速段；若不满足条件，则报错。

有益效果

本发明所提供的一种初加速度不为0的S型加减速规划方法，主要针对存在不为0的初加速度时，进行S型加减速度规划，并保持末加速度为0。本方法能克服现有技术不能处理初速度不为0的情况，能根据初末速度情况自行选取最短时间解；同时，相比较一般的S型曲线规划，适用性范围广，计算不复杂同时兼容性强。

附图说明

图1为本发明所述一种初加速度不为0的S型加减速规划方法的流程图。

具体实施方式

下面根据附图和实施例对本发明作进一步详细说明。所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

如图1所示，一种初加速度不为0的S型加减速规划方法，主要解决实际生产中存在初加速度不为0，直接进行S型加减速规划的情况。

本方法分解为三个层面：

在速度规划层中通过给定加速度、速度、位置等参数计算变速过程中，加速度的分段时间、以及各段端点的加速度、速度；

在步进规划层中根据速度规划层的计算结果，计算得到任意时刻的位置、速度、加速度；

在位置规划层中根据速度规划层的计算结果，得到该种情况下位置运动情况，对比给定位置总长度数据进行反馈是否需要重新进行速度规划。

本方法包括如下步骤：

步骤(4-1)总位移L_s小于给定总距离L，则计算时间；

其中，步骤(1)中所述判断是否需要将初加速度A_s降为0，包括：

具体的，根据初加速度A_s、初速度V_s，末速度V_e，给定总距离L进行计算，存在两种情况:

情况一

从当前初加速度A_s以及初速度V_s开始，直接加速到末速度V_e，计算加速度从0到A_s所用时间t₀＝A_s/J_max(J_max为最大加加速度)，并用于计算新初速度V_s'(计算条件为：新初速度为V_s'，初加速度从0开始增加到A_s时，速度为V_s),以及从V_s'加速到V_s所用路程S₀(该段路程为虚拟情况，即计算加速度从0到A_s，实际未发生，仅用于方便后续计算，所以总路程为负号)；

情况二

先将加速度降到0并记录此时速度为新的7段加减速规划的新初速度V_s'，新初加速度A_s'＝0，再加速到末速度V_e。

得到加减速规划的第1段时间t₀＝A_s/J_max，以及加速度方向，并计算所用路程S₀(该段路程为实际情况，符号为正)。

根据步骤(2)计算得到的新初加速度A_s'、新初速度V_s'，进行7段S型加减速曲线规划；各段时间记为t₁～t₇，则有：

步骤(3-1)按照给定的匀速运动速度条件V_f(即稳定运行速度)，根据计算得到的新初速度V_s'、末速度V_e、最大加速度A_max、最大加加速度J_max等约束条件，计算各段时间；

当V_f-V_s'≥A_max ²/J_max，则t₁＝t₃＝A_max/J_max，t₂＝(V_f-V_s'-A_max ²/J_max)/A_max

否则

t₂＝0；

t₅，t₆，t₇计算方法相同；

计算到达给定速度时总运动距离：

若该距离小于总给定路程，则计算按V_f匀速运动时间t₄，否则进入步骤(3-2)；

此时，t₄＝(L-S-S₀)/V_f。

步骤(3-2)达不到V_f情况时(此时不存在匀速段t₄＝0)，根据V_s'、V_e、V_f的大小关系分成4种模型：括加速加速模型、减速减速模型、加速减速模型和减速加速模型；

其中加速加速(V_s'≤V_f≤V_e)和减速减速(V_s'≥V_f≥V_e)模型不采用7段加减速算法，计算从V_s'到V_e的最短时间下位移小于给定总距离L能否满足：

若满足条件，则按单调情况计算并补充匀速段；

若不满足条件，则报错；

当|V_e-V_s'|≥A_max ²/J_max时，则t₁＝t₃＝A_max/J_max，t₂＝(|V_e-V_s'|-A_max ²/J_max)/A_max

否则

t₂＝0；

匀速段时间为：t＝(L-S-S₀)/max{V_e,V_s'}

针对先加速后减速(V_s'＜V_f＞V_e)和先减速后加速(V_s'＞V_f＜V_e)模型，通过二分法迭代收缩得到V_f'(V_f'为实际情况前后段临界速度)，求当前位移条件下各段时间。

其中，二分法迭代时，V_f'迭代初始范围为(max(V_e,V_s)＜V_f'＜V_f)；

计算临界速度V＝(max(V_e,V_s)+V_f)/2条件下总位移L_s；

本实施例以加速减速模型为例:

若L_s＜L,则下一段迭代范围((max(V_e,V_s)+V_f)/2＜V_f'＜V_f)；

若L_s＝L,则V_f'＝(max(V_e,V_s)+V_f)/2；

若L_s＞L,则下一段迭代范围(max(V_e,V_s)＜V_f'＜(max(V_e,V_s)+V_f)/2)；

迭代得到V_f'时停止，并根据V_f'计算出7段加减速时间(因无匀速段，t₄＝0)。

求当前位移条件下各段时间方法与步骤(3-1)中相同，仍然以加速减速模型为例:

当V_f'-V_s'≥A_max ²/J_max，则t₁＝t₃＝A_max/J_max，t₂＝(V_f'-V_s'-A_max ²/J_max)/A_max

否则

t₂＝0；

t₅，t₆，t₇计算方法相同。

再根据上述位置、速度、加速度关于时间的公式计算得到任意时刻的位置、速度、加速度；通过对加速度积分，得到加速度公式；再对速度积分得到任意时刻位置。

作为本方案的一个具体实施例，如下：

给定初加速度A_s、初速度V_s、末速度V_e、最大加速度A_max、最大减速度D_max、加速度J_max、稳定运行速度V_f。

若初加速度A_s超限时，则存在如下两种情况：

1.加速度超最大加速度，将加速度降低至最大加速度，并求出此时位移、速度、时间，为最终规划情况的第一段；

2.加速度小于最大减速度，将加速度升高至最大减速度，并求出此时位移、速度、时间，为最终规划情况的第一段；

若初加速度未超限，则记时间、位移为0，速度加速度不变；

此时根据条件计算是否需要将加速度先变化到0：

通过将加速度降至0，计算此时速度，记为V_s1；

通过将加速度反向降至0，计算此时速度，记为V_s2；

初加速度大于等于0时

V_s1≥V_f，加速度降低到加速度为0再减速，计算时间时v_s＝V_s1；

否则直接加速，计算加减速时间时，中间变量计算用初速度v_s＝V_s2；

初加速度小于0时

V_s1≤V_f，加速度增加到加速度为0再加速，计算时间时v_s＝V_s1；

否则直接减速，计算加减速时间时，中间变量计算用初速度v_s＝V_s2；

再根据V_f和V_e关系决定S型加减速第二段速度变化方向，从而决定S型加减速类型为先增后减，单调增加，先减后增，单调减速四种中的哪一种。

根据，S型加减速类型以及各参数，采用计算用初速度v_s，末速度V_e，最大加减速度A_max、D_max，加速度J_max，给定匀速V_f进行计算7段加减速时间的计算，最终得到总位移。

根据总位移和给定路程计算是否存在匀速运动t₄，是则计算第四段时间，否则返回重新规划7段加减速时间，最终得到符合条件的结论，规划完成。

以上所述仅为说明本发明的实施方式，并不用于限制本发明，对于本领域的技术人员来说，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种初加速度不为0的S型加减速规划方法，其特征在于，包括：

步骤(4-1)总位移L_s小于给定总距离L，则计算时间；

2.根据权利要求1所述的一种初加速度不为0的S型加减速规划方法，其特征在于，步骤(1)中所述判断是否需要将初加速度A_s降为0，包括：

3.根据权利要求2所述的一种初加速度不为0的S型加减速规划方法，其特征在于，所述将初始加速度为0加速至加速度为A_s时，并根据初速度V_s和位移S₀计算新初速度V_s'，具体为：从当前初加速度A_s以及初速度V_s开始，直接加速到末速度V_e，计算加速度从0到A_s所用时间t₀，并用于计算新初速度V_s'以及从新初速度V_s'加速到初速度V_s所用路程S₀用；

4.根据权利要求2所述的一种初加速度不为0的S型加减速规划方法，其特征在于，所述将初加速度A_s降为0，计算此时的时间，并计算此时的新初速度V_s'，具体为：先将加速度降到0并记录此时速度为新的7段加减速规划的新初速度V_s'，新初加速度A_s'＝0，再加速到末速度V_e，得到加减速规划的第1段时间t₀，以及加速度方向，并计算所用路程S₀。；

5.根据权利要求1所述的一种初加速度不为0的S型加减速规划方法，其特征在于，步骤(3)中所述若为加速加速模型和减速减速模型时，则不采用7段加减速算法计算速度从新初速度V_s'变化到末速度V_e的总位移L_s，具体为：计算从新初速度V_s'到末速度V_e的最短时间下位移小于给定总距离L能否满足：若满足条件，则按单调情况计算并补充匀速段；若不满足条件，则报错。