[发明专利]一种初加速度不为零的S型曲线速度控制方法

摘要

一种初始加速度不为零的S型曲线速度控制方法，该方法分解为三个层面，其中速度规划层根据给定参数进行变速规划，得到变速过程的加加速截止时间、匀加速截止时间、减加速截止时间以及当前的加加速度；步进规划层根据速度规划层计算结果，计算任意时刻的位置、速度和加速度；位置规划层根据速度规划层和步进规划层的计算结果，计算当前起始点的下一步运动位置、速度和加速度，并保证最终运动到末点时速度不大于末点速度；当当前段链规划长度不够时，将当前段链链接下一条段链，保证段链长度足够规划。由于在每步速度规划过程中都按照运行到最大速度预测一步，并为保证达到末点速度而实时规划，能够快速实现算法，降低硬件要求和成本。

主权项

1.一种初加速度不为零的S型曲线速度控制方法，其特征在于：所述方法分解为三个层面，即速度规划层、步进规划层和位置规划层；其中速度规划层包含速度规划层函数，所述速度规划层函数根据给定的起始速度v0、末点速度ve、起始加速度a0、最大加加速度jmax和最大加速度amax进行变速规划，得到变速过程的加加速截止时间t1、匀加速截止时间t2、减加速截止时间t3以及当前的加加速度j；所述速度规划层函数得出变速过程的加加速截止时间t1、匀加速截止时间t2、减加速截止时间t3以及当前的加加速度j过程包括以下步骤：设v₀为起始速度，v_e为S型速度规划结束速度，首末速度差Δv＝v₀‑v_e，a₀为起始加速度，j_max为给定最大加加速度，a_max为给定最大加速度，标志速度常量v_x＝Δv×flag，加速度常量a_x＝a₀×flag；步骤A：当a_x＜0，且时，取步骤B：当a_x≥0或时，当时，取j＝‑j_max，当时，取j＝‑j_max，步骤C：更新当前加加速度，即取j＝j×flag；步进规划层包含步进规划层函数，所述步进规划层函数根据速度规划层计算结果，计算任意时刻t时的位置xr、速度vrx和加速度arx；所述步进规划层函数根据以下公式计算任意时刻t的位置xr、速度vrx和加速度arx：(1)当t≤t1时，arx＝a0+jt(2)t1＜t≤t2时，arx＝a0+jt1(3)当t2＜t≤t3时，arx＝a0+jt1‑j(t‑t2)(4)t≥t3时，arx＝a0+jt1‑j(t3‑t2)位置规划层根据速度规划层和步进规划层的计算结果、起始点位置x0、末点位置xe以及最大速度vmax，计算当前起始点下一步运动位置、速度和加速度，并保证最终运动到末点时速度不大于ve；当当前段链规划长度不够时，将当前段链链接下一条段链，并根据链接后段链总长度重新计算。