[发明专利]一种快速规划6阶S型运动曲线的方法

权利要求书

1.一种快速规划6阶S型运动曲线的方法，其特征在于，包括如下步骤：

步骤1，输入待规划的运动曲线参数：轨迹长度L_d、启动速度V_s、结束速度V_e、最大加加速J_m、最大允许速度V_m；

步骤2，使用J_m与V_m作为约束条件，计算V_s、V_e、L_d之间的关系，将待规划的运动曲线分为加速阶段、匀速阶段和减速阶段，其中加速阶段记作H分段，匀速阶段记作B分段，减速阶段记作T分段，待规划的S型曲线上会出现的运动状态分段情况为H、T、B、BT、HB、HT、HBT几种分段中的一种，对待规划的S型曲线上的运动状态组合进行判断；

步骤3，对存在的H分段和T分段的运动曲线，使用5阶贝塞尔曲线规划速度曲线；

步骤4，对步骤三中规划出的速度曲线进行等间隔采样，将速度曲线等分为N份，每份时间间隔为h，采用向前差分的方式计算出速度曲线上的离散V值。

2.根据权利要求1所述的一种快速规划6阶S型运动曲线的方法，其特征在于，S型运动曲线上已知某段的初始速度V_i与末速度V_f可计算移动距离L_x，具体公式如下；

S型运动曲线上已知某段的初始速度V_i与移动距离L_x计算该段的末速度V_f，具体公式如下；

通过公式(1)和(2)计算，可对待规划的S型曲线上的运动状态分段组合进行判断，具体方法如下：

步骤2.1如果V_s＝V_e，则该段为匀速运动状态过程，为B分段，结束步骤二，否则进入步骤2.2；

步骤2.2将V_s与V_e代入式(1)，得到速度变化期间实际运动距离L_c，进入步骤2.2.1；

步骤2.2.1如果L_c≥L_d，且V_sV_e时，说明需要实际运行更远的距离，速度才能从V_s变化成V_e，而此时V_s又小于V_e，则在设定的距离L_d内一直做加速运动，为H分段模式，由公式(2)可计算得到该段实际的末速度V_c，进入步骤三，否则进入步骤2.2.2；

步骤2.2.2如果L_c≥L_d，且V_sV_e时，说明需要实际运行更远的距离，速度才能从V_s变化成V_e，而此时V_s又大于V_e，则在设定的距离L_d内一直做减速运动，为T分段，由公式(2)可计算得到该段实际的末速度V_c，进入步骤三，否则进入步骤2.2.3；

步骤2.2.3如果L_cL_d，且V_m＝V_s，将V_s与V_e代入式(1)，得到减速状态下运动的距离L_t，并且当L_tL_d时，为BT分段，即该段为先匀速后减速运动模式，进入步骤三，否则进入步骤2.2.4；

步骤2.2.4如果L_cL_d，将V_s与V_m代入式(1)，得到加速距离L_h，如果L_hL_d，且V_m＝V_e，为HB段模式，即该段为先加速后匀速运动模式，进入步骤三，否则进入步骤2.2.5；

步骤2.2.5如果L_cL_d，将V_s与V_m代入式(1)，得到加速状态下运动的距离L_h，将V_m与V_e代入式(1)，得到减速状态下运动的距离L_t，进入步骤2.2.5.1；

步骤2.2.5.1如果L_d(L_h+L_t)，为HT分段，即该段曲线为先加速，然后立即进入减速运动模式，进入步骤三，否则进入步骤2.2.5.2；

步骤2.2.5.2如果L_d(L_h+L_t)，为HBT分段，曲线先加速，然后进入匀速，最后减速，进入步骤三。