关于运动控制中S型速度曲线的简单演示（C++实现）_速度曲线算位置曲线-CSDN博客
步进电机有很多种算法，主要集中在速度的控制，其中S比较常见。上边是其他作者的作品， 这里截取一段代码 作为记录。
#include <stdio.h>
#include <math.h>
#define FP64 double
int main(void)
{
/**************************** 定义相关变量(需要映射到PLC寄存器才能进行监控) ***********************/
FP64 v_start; //初速度
FP64 v_end; //末速度
FP64 v_max = 0; //最大速度(有匀速段时,v_max = v_target)
/*根据需要修改*/
v_start = 100;
v_end = 150;
FP64 s_target; //目标位移量
FP64 v_target; //目标速度
FP64 a_target; //目标加速度
FP64 d_target; //目标减速度
FP64 j_max; //目标加加速度
FP64 t_sum;
FP64 L; //算出的位移量
FP64 v_real; //实际的最大速度
FP64 s_max; //L>s_target时候规划的位移
/*可以根据需要修改*/
j_max = 2000; //目标加加速度
a_target = 2000; //目标加速度
d_target = 3000; //目标减速度
v_target = 5000; //目标速度
s_target = 3000; //目标位移
int t; //运行时间
FP64 t_aver; //匀速区时间
FP64 t1; //加加速时间
FP64 t2; //匀加速时间
FP64 t3; //减加速时间
FP64 t4; //匀速运行时间
FP64 t5;
FP64 t6;
FP64 t7;
FP64 v1;
FP64 v2;
FP64 v3;
FP64 v4;
FP64 v5;
FP64 v6;
FP64 v7;
FP64 a_max; //加速度最大值(拥有匀加速段时 a_max = a_target)
FP64 d_max;
FP64 s1; //加加速位移 s1
FP64 s2; //匀加速位移 s2
FP64 s3; //减加速位移 s3
FP64 s4; //匀速位移 s4
FP64 s5; //加减速位移 s5
FP64 s6; //匀减速位移 s6
FP64 s7; //减减速位移 s7
FP64 s_acc; //加速区位移
FP64 s_dec; //减速区位移
FP64 j_cur; //当前加加速度
FP64 a_cur; //当前加速度
FP64 v_cur; //当前速度
FP64 s_cur; //当前位移量
FP64 v_down;
FP64 v_low;
FP64 v_high;
short int end_count=0;
short int flag = 0;
short int flag1 = 0;
short int count=0;
for (t = 0; t < 10000000000; t++)
{
if (v_max == 0) //只有第一遍
{
/***********************************根据目标速度,加速度,加加速度等规划出加速区和减速区******************/
if ((v_target - v_start) > a_target * a_target / j_max) //匀加速段
{
a_max = a_target;
t1 = a_max / j_max;
v1 = v_start + 0.5 * j_max * t1 * t1;
s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
t2 = (v_target - v_start) / a_max - t1;
v2 = v1 + a_target * t2;
s2 = v1 * t2 + 0.5 * a_max * t2 * t2;
t3 = t1;
v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
}
else
{
a_max = sqrt((v_target - v_start) * j_max);
t1 = sqrt((v_target - v_start) / j_max);
s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
v1 = v_start + 0.5 * j_max * t1 * t1;
t2 = 0;
v2 = v1;
s2 = 0;
t3 = t1;
s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
}
if (v_target - v_end > (d_target * d_target) / j_max)
{
d_max = d_target;
t7 = d_max / j_max;
s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
v6 = v_end + 0.5 * j_max * t7 * t7;
t6 = (v_target - v_end) / d_max - t7;
s6 = v6 * t6 + 0.5 * d_max * t6 * t6;
v5 = v6 + d_target * t6;
t5 = t7;
s5 = v5 * t5 + j_max * t5 * t5 * t5 / 3;
}
else
{
d_max = sqrt((v_target - v_end) * j_max);
t7 = sqrt((v_target - v_end) / j_max);
s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
v6 = v_end + 0.5 * j_max * t7 * t7;
t6 = 0;
s6 = 0;
t5 = t7;
v5 = v6;
s5 = v3 * t5 - j_max * t5 * t5 * t5 / 6;
}
L = 0.5 * ((v_start + v_target) * (t1 + t2 + t3) + (v_end + v_target) * (t5 + t6 + t7));
}
if (L >= s_target && v_max == 0) //如果 L >= s_target,那么达不到目标速度,需要规划出实际最大速度,只有第一遍循环会进这个程序
{
t4 = 0;
s4 = 0;
v4 = v3;
v_high = v_target;
/*初速度较大*/
if (v_start > v_end)
{
v_low = v_start;
}
/*末速度较大*/
else if (v_end > v_start)
{
v_low = v_end;
end_count += 1;
}
v_max = 0.5 * (v_low + v_high);
}
/*如果 L < s_target 那么可以正常进行规划(有匀速段),标志位flag1=1*/
else if (L < s_target)
{
v_max = v_target;
flag1 = 1;
}
/*********************************** 根据二分处理后的速度再算位移 ******************/
if ((v_max - v_start) > a_target * a_target / j_max) //匀加速段
{
a_max = a_target;
t1 = a_max / j_max;
v1 = v_start + 0.5 * j_max * t1 * t1;
s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
t2 = (v_max - v_start) / a_max - t1;
v2 = v1 + a_target * t2;
s2 = v1 * t2 + 0.5 * a_max * t2 * t2;
t3 = t1;
v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
}
else
{
a_max = sqrt((v_max - v_start) * j_max);
t1 = sqrt((v_max - v_start) / j_max);
s1 = v_start * t1 + (j_max * t1 * t1 * t1) / 6;
v1 = v_start + 0.5 * j_max * t1 * t1;
t2 = 0;
v2 = v1;
s2 = 0;
t3 = t1;
s3 = v2 * t3 + j_max * t3 * t3 * t3 / 3;
v3 = v2 + a_max * t3 - 0.5 * j_max * t3 * t3;
}
if (v_max - v_end > (d_target * d_target) / j_max) //匀减速段
{
d_max = d_target;
t7 = d_max / j_max;
s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
v6 = v_end + 0.5 * j_max * t7 * t7;
t6 = (v_max - v_end) / d_max - t7;
s6 = v6 * t6 + 0.5 * d_max * t6 * t6;
v5 = v6 + d_target * t6;
t5 = t7;
s5 = v5 * t5 + j_max * t5 * t5 * t5 / 3;
}
else
{
d_max = sqrt((v_max - v_end) * j_max);
t7 = sqrt((v_max - v_end) / j_max);
s7 = v_end * t7 + (j_max * t7 * t7 * t7) / 6;
v6 = v_end + 0.5 * j_max * t7 * t7;
t6 = 0;
s6 = 0;
t5 = t7;
v5 = v6;
s5 = v3 * t5 - j_max * t5 * t5 * t5 / 6;
}
s_max = s1 + s2 + s3 + s5 + s6 + s7; //s_max是二分后的位移
/*二分后的位移无限接近目标位移,则标志位flag=1,开始规划*/
if (s_max - s_target < 0.001 && s_max - s_target > 0)
{
flag = 1;
}
/*否则继续二分*/
else
{
//二分法思想：改变上下限
if (s_max < s_target) //如果规划出来的位移偏小
{
v_low = v_max;
}
else { //如果规划出来的位移偏大
v_high = v_max;
}
v_max = 0.5 * (v_low + v_high);
}
/*********************************** 只有在标志位置位时才规划(画图) ******************/
if (flag == 1 || flag1 == 1)
{
if (flag1 == 1 )
{
/*正常进行规划*/
t4 = (s_target - L) / v_max;
v4 = v3;
s4 = v4 * t4;
}
else if (flag == 1)
{
t4 = 0;
v4 = v3;
s4 = 0;
}
t_sum = t1 + t2 + t3 + t4 + t5 + t6 + t7;
if (t >= 0 && t < t1)
{
j_cur = j_max;
a_cur = j_cur * t;
v_cur = v_start + 0.5 * j_cur * t * t;
s_cur = v_start * t + j_cur * t * t * t / 6;
}
else if (t >= t1 && t < t1 + t2 && t2 != 0)
{
j_cur = 0;
a_cur = a_max;
v_cur = v1 + a_cur * (t - t1);
s_cur = s1 + v1 * (t - t1) + 0.5 * a_cur * (t - t1) * (t - t1);
}
else if (t >= t1 + t2 && t < t1 + t2 + t3)
{
j_cur = -j_max;
a_cur = a_max - j_max * (t - t1 - t2);
v_cur = v2 + a_max * (t - t1 - t2) - 0.5 * j_max * (t - t1 - t2) * (t - t1 - t2);
s_cur = s1 + s2 + v2 * (t - t1 - t2) + 0.5 * a_max * (t - t1 - t2) * (t - t1 - t2) - j_max * (t - t1 - t2) * (t - t1 - t2) * (t - t1 - t2) / 6;
}
else if (t >= t1 + t2 + t3 && t < t1 + t2 + t3 + t4 && t4 != 0)
{
j_cur = 0;
a_cur = 0;
v_cur = v3;
s_cur = s1 + s2 + s3 + v3 * (t - t1 - t2 - t3);
}
else if (t >= t1 + t2 + t3 + t4 && t < t1 + t2 + t3 + t4 + t5)
{
j_cur = -j_max;
a_cur = -j_max * (t - t1 - t2 - t3 - t4);
v_cur = v4 - 0.5 * j_max * (t - t1 - t2 - t3 - t4) * (t - t1 - t2 - t3 - t4);
s_cur = s1 + s2 + s3 + s4 + v4 * (t - t1 - t2 - t3 - t4) - j_max * (t - t1 - t2 - t3 - t4) * (t - t1 - t2 - t3 - t4) * (t - t1 - t2 - t3 - t4) / 6;
}
else if (t >= t1 + t2 + t3 + t4 + t5 && t < t1 + t2 + t3 + t4 + t5 + t6 && t6 != 0)
{
j_cur = 0;
a_cur = -d_max;
v_cur = v5 - d_target * (t - t1 - t2 - t3 - t4 - t5);
s_cur = s1 + s2 + s3 + s4 + s5 + v5 * (t - t1 - t2 - t3 - t4 - t5) - 0.5 * d_target * (t - t1 - t2 - t3 - t4 - t5) * (t - t1 - t2 - t3 - t4 - t5);
}
else if (t >= t1 + t2 + t3 + t4 + t5 + t6 && t < t1 + t2 + t3 + t4 + t5 + t6 + t7)
{
j_cur = j_max;
a_cur = -d_max + j_max * (t - t1 - t2 - t3 - t4 - t5 - t6);
v_cur = v6 - d_max * (t - t1 - t2 - t3 - t4 - t5 - t6) + 0.5 * j_max * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6);
s_cur = s1 + s2 + s3 + s4 + s5 + s6 + v6 * (t - t1 - t2 - t3 - t4 - t5 - t6) - 0.5 * d_max * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6) + j_max * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6) * (t - t1 - t2 - t3 - t4 - t5 - t6) / 6;
}
else if (t > t1 + t2 + t3 + t4 + t5 + t6 + t7)
{
a_cur = 0;
j_cur = 0;
v_cur = v_end;
return;
}
}
}
}
2073
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
