[发明专利]兼容运动中目标位置更新的s型曲线规划方法及系统

权利要求书

1.一种兼容运动中目标位置更新的S型曲线规划方法，其特征在于，包括以下步骤：

将曲线规划分为7个阶段：加速段的加加速度段，加速段的匀加速段，加速段的减加速段，匀速段，减速段的加加速度段，减速段的匀加速段，减速段的减加速段；

设定并初始化曲线规划的运动参数；

根据运动参数计算曲线规划中直接减速的距离，并判断是否可以直接减速；

依据判断结果计算加速到最大速度的加速时间T_acc和加速距离S_acc；

根据所述加速时间T_acc和加速距离S_acc计算各阶段的时间点；

实时检测是否有新的目标位置，若否，则根据时间点更新曲线规划各阶段状态；若是，则返回至初始步骤重新规划。

2.根据权利要求1所述的方法，其特征在于，所述设定并初始化曲线规划的运动参数包括：

设定新的目标位置P_e1、当前位置P_s、移动距离S、最大速度V_m、平均加速度A和曲线规划各阶段的时间点如下：加速段的加加速度段T₀-T₁，加速段的匀加速段T₁-T₂，加速段的减加速段T₂-T₃，匀速段T₃-T₄，减速段的加加速度段T₄-T₅，减速段的匀加速段T₅-T₆，减速段的减加速段T₆-T₇；

初始化运动参数：

其中，t为当前时刻，v_t为t时刻实时速度，a_t为t时刻实时加速度，s_t为t时刻实时运动距离，P_t为t时刻实时位置，V₀为初始速度。

3.根据权利要求2所述的方法，其特征在于，所述根据运动参数计算曲线规划中直接减速的距离S_dec1，公式如下：

，其中，β为减速段与加速段的平均加速度的比值，且0β≤1。

4.根据权利要求3所述的方法，其特征在于，所述判断是否可以直接减速具体如下：若SS_dec1，则最大速度V_m和平均加速度A数值分别取其相反数，否则不变。

5.根据权利要求4所述的方法，其特征在于，所述依据判断结果计算加速到最大速度的加速时间T_acc和加速距离S_acc，公式如下：

。

6.根据权利要求5所述的方法，其特征在于，根据所述加速时间T_acc和加速距离S_acc计算各阶段的时间点包括：

判断是否存在匀速段，计算匀速时间T_avg和匀速距离S_avg：

若abs(S)abs(S_acc+S_dec)，则存在匀速段，公式如下：

，

若abs(S)≤abs(S_acc+S_dec)，则不存在匀速段，公式如下：S_avg= 0，T_avg= 0，并重新计算能够到达的最大速度V_m、加速时间T_acc和减速时间T_dec以及对应的加速距离S_acc、减速距离S_dec，公式如下：

；

根据加速时间T_acc、匀速时间T_avg和减速时间T_dec计算各阶段的时间点，公式如下：

，

其中，k为减速段或加速段内的最大加速度与平均加速度的比值且1k≤2。