
此算法为加速度连续的7段s型加减速规划实现,理论参考:https://blog.csdn.net/Septembernine/article/details/53125828
(1)加加速段
加加速度为 jmaxjmax,加速度线性增加至设定值或最大值amaxamax。
(2)匀加速段
加加速度为0,加速度恒定。
(3)减加速段
当速度接近设定的值或最大值vmaxvmax时,加加速度突变为反向的jmaxjmax,进入加速度线性减小的变减速运动阶段。
(4)匀速段
当速度增至vmaxvmax后,加加速和加速度均变为0,进入匀速运动阶段。
(5)加减速段
加加速度突变为反向的jmaxjmax,加速度反向线性增加至−amax−amax。
(6)匀减速段
加加速度和为0,减加速度恒定。
(7)减减速段
加速度突变为jmaxjmax, 加速度由负向的amaxamax线性减小至0。
程序实现如下:
int sign(float q0, float q1);
// S曲线参数计算
void STrajectoryPara(float q0, float q1, float v0, float v1, float vmax, float amax, float jmax);
// 计算位置
float S_position(float);
// 计算速度
float S_velocity(float t);
// 计算加速度
float S_acceleration(float t);
// 计算加加速度
float S_jerk(float t);
int sSpeed::sign(float q0, float q1)
{
int a = 0;
if (q1 - q0 > 0)
a = 1;
else
a = -1;
return a;
}
void sSpeed::STrajectoryPara(float q0, float q1, float v0, float v1, float vmax, float amax, float jmax)
{
float vmin, amin, jmin;
int sigma = 0;
float Tj = 0, delta = 0;
vmin = -vmax;
amin = -amax;
jmin = -jmax;
// 計算实际的q_0、q_1、v_max、a_max
sigma = sign(q0, q1);
Input_data.q0 = sigma * q0;
Input_data.q1 = sigma * q1;
Input_data.v0 = sigma * v0;
vmax = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
vmin = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
Input_data.amax = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
Input_data.amin = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
Input_data.jmax = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
Input_data.jmin = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;
// 判断是否达到最大速度
if ((vmax - Input_data.v0)*Input_data.jmax < pow(Input_data.amax, 2)){
// 达不到amax
Input_data.Tj1 = sqrt((vmax - Input_data.v0)/Input_data.jmax);
Input_data.Ta = 2*Input_data.Tj1;
Input_data.alima = Input_data.jmax * Input_data.Tj1;
}else{
// 能够达到amax
Input_data.Tj1 = Input_data.amax/Input_data.jmax;
Input_data.Ta = Input_data.Tj1 + (vmax - Input_data.v0 )/Input_data.amax;
Input_data.alima = Input_data.amax;
}
if ((vmax - Input_data.v1)*Input_data.jmax < pow(Input_data.amax, 2)){
// 达不到amin
Input_data.Tj2 = sqrt((vmax - Input_data.v1)/Input_data.jmax);
Input_data.Td = 2 * Input_data.Tj2;
Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
}else{
// 能够达到amin
Input_data.Tj2 = Input_data.amax/Input_data.jmax;
Input_data.Td = Input_data.Tj2 + (vmax - Input_data.v1)/Input_data.amax;
Input_data.alimd = -Input_data.amax;
}
// 计算匀速段时间
Input_data.Tv = (Input_data.q1 - Input_data.q0)/vmax - (Input_data.Ta/2)*(1 + Input_data.v0/vmax)
- (Input_data.Td/2)*(1 + Input_data.v1/vmax);
// 对Tv进行讨论
if (Input_data.Tv > 0)
// 能够达到给定的最大速度vmax, 即存在匀速阶段
Input_data.vlim = vmax;
else{
// 达不到最大速度,即匀速阶段Tv=0
// 假设最大加速度和最小加速度均能达到
Input_data.Tv = 0;
Tj = Input_data.amax / Input_data.jmax;
Input_data.Tj1 = Tj;
Input_data.Tj2 = Tj;
delta = (pow(Input_data.amax, 4)/pow(Input_data.jmax, 2)) + 2*(pow(Input_data.v0, 2) + pow(Input_data.v1, 2))
+ Input_data.amax*(4*(Input_data.q1 - Input_data.q0) - 2*(Input_data.amax/Input_data.jmax)*(Input_data.v0 + Input_data.v1));
Input_data.Ta = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v0 + sqrt(delta)) / (2*Input_data.amax);
Input_data.Td = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v1 + sqrt(delta)) / (2*Input_data.amax);
// 对Ta和Td进行讨论
if (Input_data.Ta < 0 || Input_data.Td < 0){
if (Input_data.Ta < 0){
// 没有加速段,只有减速段
Input_data.Ta = 0;
Input_data.Tj1 = 0;
Input_data.Td = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
Input_data.Tj2 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
+ pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
Input_data.alima = 0;
Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
Input_data.vlim = v0;
}else if(Input_data.Td < 0){
// 没有减速段,只有加速段
Input_data.Td = 0;
Input_data.Tj2 = 0;
Input_data.Ta = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
Input_data.Tj1 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
- pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
Input_data.alima = Input_data.jmax * Input_data.Tj1;
Input_data.alimd = 0;
Input_data.vlim = Input_data.jmax * Input_data.Tj1;
}
}else if(Input_data.Ta >= 2*Tj && Input_data.Td >= 2*Tj){
// 加速段和减速段都能达到最大速度
Input_data.alima = Input_data.amax;
Input_data.alimd = -Input_data.amax;
Input_data.vlim = v0 + Input_data.alima*(Input_data.Ta - Tj);
}else{
// 加速段和减速阶段至少有一段不能达到最大加速度
float lambda = 0.99; // 系统取0<lambda<1
while (Input_data.Ta < 2*Tj || Input_data.Td < 2*Tj){
Input_data.amax = lambda * Input_data.amax;
Input_data.Tv = 0;
Tj = Input_data.amax / Input_data.jmax;
Input_data.Tj1 = Tj;
Input_data.Tj2 = Tj;
delta = (pow(Input_data.amax, 4)/pow(Input_data.jmax, 2)) + 2*(pow(Input_data.v0, 2) + pow(Input_data.v1, 2))
+ Input_data.amax*(4*(Input_data.q1 - Input_data.q0) - 2*(Input_data.amax/Input_data.jmax)*(Input_data.v0 + Input_data.v1));
Input_data.Ta = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v0 + sqrt(delta)) / (2*Input_data.amax);
Input_data.Td = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v1 + sqrt(delta)) / (2*Input_data.amax);
if (Input_data.Ta < 0 || Input_data.Td < 0){
if (Input_data.Ta < 0){
// 没有加速段,只有减速段
Input_data.Ta = 0;
Input_data.Tj1 = 0;
Input_data.Td = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
Input_data.Tj2 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
+ pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
Input_data.alima = 0;
Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
Input_data.vlim = v0;
}else if(Input_data.Td < 0){
// 没有减速段,只有加速段
Input_data.Td = 0;
Input_data.Tj2 = 0;
Input_data.Ta = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
Input_data.Tj1 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
- pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
Input_data.alima = Input_data.jmax * Input_data.Tj1;
Input_data.alimd = 0;
Input_data.vlim = Input_data.jmax * Input_data.Tj1;
}
}else if(Input_data.Ta >= 2*Tj && Input_data.Td >= 2*Tj){
// 加速段和减速段都能达到最大速度
Input_data.alima = Input_data.amax;
Input_data.alimd = -Input_data.amax;
Input_data.vlim = v0 + Input_data.alima*(Input_data.Ta - Tj);
}
}
}
}
qDebug()<<Input_data.Ta;
}
float sSpeed::S_position(float t)
{
float T = 0, q = 0;
float Ta = Input_data.Ta;
float Tv = Input_data.Tv;
float Td = Input_data.Td;
float Tj1 = Input_data.Tj1;
float Tj2 = Input_data.Tj2;
float q0 = Input_data.q0;
float q1 = Input_data.q1;
float v0 = Input_data.v0;
float v1 = Input_data.v1;
float vlim = Input_data.vlim;
float alima = Input_data.alima;
float alimd = Input_data.alimd;
float jmax = Input_data.jmax;
float jmin = Input_data.jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
q = q0 + v0*t + jmax*pow(t, 3)/6;
else if (t >= Tj1 && t < Ta - Tj1)
q = q0 + v0*t +(alima/6)*(3*pow(t, 2) - 3*Tj1*t + pow(Tj1, 2));
else if (t >= Ta - Tj1 && t < Ta)
q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*(pow(Ta - t, 3)/6);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(pow(t - T + Td, 3)/6);
else if (t >= T - Td + Tj2 && t < T - Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*pow(t - T + Td, 2) - 3*Tj2*(t - T + Td) + pow(Tj2, 2));
else if (t >= T - Tj2 && t <= T)
q = q1 - v1*(T - t) - jmax*(pow(T - t, 3)/6);
return q;
}
float sSpeed::S_velocity(float t )
{
float T = 0, qd = 0;
float Ta = Input_data.Ta;
float Tv = Input_data.Tv;
float Td = Input_data.Td;
float Tj1 = Input_data.Tj1;
float Tj2 = Input_data.Tj2;
float v0 = Input_data.v0;
float v1 = Input_data.v1;
float vlim = Input_data.vlim;
float alima = Input_data.alima;
float alimd = Input_data.alimd;
float jmax = Input_data.jmax;
float jmin = Input_data.jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qd = v0 + jmax*(pow(t, 2)/2);
else if (t >= Tj1 && t < Ta - Tj1)
qd = v0 + alima*(t - Tj1/2);
else if (t >= Ta - Tj1 && t < Ta)
qd = vlim + jmin*(pow(Ta - t, 2)/2);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qd = vlim;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qd = vlim - jmax*(pow(t - T + Td, 2)/2);
else if (t >= T - Td + Tj2 && t < T - Tj2)
qd = vlim + alimd*(t - T + Td - Tj2/2);
else if (t >= T - Tj2 && t <= T)
qd = v1 + jmax*(pow(t - T, 2)/2);
return qd;
}
float sSpeed::S_acceleration(float t)
{
float T = 0, qdd = 0;
float Ta = Input_data.Ta;
float Tv = Input_data.Tv;
float Td = Input_data.Td;
float Tj1 = Input_data.Tj1;
float Tj2 = Input_data.Tj2;
float alima = Input_data.alima;
float alimd = Input_data.alimd;
float jmax = Input_data.jmax;
float jmin = Input_data.jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qdd = jmax * t;
else if (t >= Tj1 && t < Ta - Tj1)
qdd = alima;
else if (t >= Ta - Tj1 && t < Ta)
qdd = -jmin * (Ta - t);
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qdd = 0;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qdd = -jmax * (t - T - Td);
else if (t >= T - Td + Tj2 && t < T - Tj2)
qdd = alimd;
else if (t >= T - Tj2 && t <= T)
qdd = -jmax * (T - t);
return qdd;
}
float sSpeed::S_jerk(float t)
{
float T = 0, qddd = 0;
float Ta = Input_data.Ta;
float Tv = Input_data.Tv;
float Td = Input_data.Td;
float Tj1 = Input_data.Tj1;
float Tj2 = Input_data.Tj2;
float jmax = Input_data.jmax;
float jmin = Input_data.jmin;
T = Ta + Tv + Td;
// 加速段
if (t >= 0 && t < Tj1)
qddd = jmax;
else if (t >= Tj1 && t < Ta - Tj1)
qddd = 0;
else if (t >= Ta - Tj1 && t < Ta)
qddd = -jmin;
// 匀速段
else if (t >= Ta && t < Ta + Tv)
qddd = 0;
// 减速段
else if (t >= Ta + Tv && t < T - Td + Tj2)
qddd = -jmax;
else if (t >= T - Td + Tj2 && t < T - Tj2)
qddd = 0;
else if (t >= T - Tj2 && t <= T)
qddd = jmax;
return qddd;
}
仿真结果如下:

完整工程Git下载链接:https://gitee.com/luck126/sspeed
工具:Qt Creater5.9
该博客详细介绍了加速度连续的7段S型加减速规划算法,包括加加速度、匀加速、减加速、匀速、加减速、匀减速和减减速等阶段的数学模型和程序实现。算法考虑了速度限制、加速度限制和加加速度限制,并通过Qt Creator进行仿真验证。
4万+

被折叠的 条评论
为什么被折叠?



