关于运动控制中S型速度曲线的简单演示(C++实现)

疫情期间,居家隔离,时间不多,只能简单的就S速度曲线的来说,建模,写代码,然后编译,再用MATLAB来查看位置曲线、速度曲线和加速度曲线,以及加加速度曲线。
确当S型速度曲线的速度、加速度和位移公式应该从加加速度开始,这里设定在整个的运动过程中,加加速度aa都是固定值(在不同的时间段只有正负之分,但是绝对值是相同的)。关于运动控制中S型速度曲线的简单演示(C++实现)_第1张图片
(论文:CNC系统S型曲线加减速算法的设计与实现)
上图为S型加减速的各种曲线,第一行为位移图与时间的关系,第二行为速度与时间的关系,第三行为加速度与时间的关系,第四行为加加速度的关系,时间段分为7个:t1(加加速段),t2(匀加速段),t3(加减速段),t4(匀速段),t5(减加速度段),t6(匀减速段),t7(减减速段)。
要使用加加速度的时间积分来求加速度,使用加速度的时间积分来求速度,使用速度的时间积分来求位置,其公式在程序中实现,大家可以自行推演。
鉴于S型加速度的复杂性,先只设定以上七个 时间段的时间段均相等。(在程序中的设定时间为10s)
关于具体的公式可以参看我的实现来具体的进行推演,因为我的都是在草稿纸上进行的,所以不在本文中具体说明。
实现代码如下:

#include "stdafx.h"
#include 
#include 
#include 
#include 
int _tmain(int argc, _TCHAR* argv[])        
{
     
 AllocConsole();
    ofstream out(".\\test.txt",ios_base::ate);
 double temp_time=0; 
 int times=0;
 double temp_aa;    //临时加加速度
 double aa=100;     //加加速度
 double a=0;        //加速度
 double temp_vel=0;
 double temp_pos=0;
 //每段只有10S,每10s分1000段。
 
 while (TRUE)
    {
     
  if(temp_time>=0&&temp_time<=10)     //加加速度段
  {
       
     temp_aa=aa;
     a=aa*temp_time;
           temp_pos=aa*(temp_time)*(temp_time)*temp_time/6;                                                                                                                                                                                                                                                                                                                                                                                                                                  ;
     temp_vel=aa*(temp_time)*(temp_time)/2;
  }
  else if(temp_time>10&&temp_time<=20)     //匀加速段
  {
       
     temp_aa=0;
     a=10*aa;
           temp_vel=50*aa+10*aa*(temp_time-10);    //加减速段
     temp_pos=1000*aa/6+5*aa*temp_time*temp_time-50*aa*temp_time;
  }
  else if(temp_time>20&&temp_time<=30)
  {
        
   temp_aa=-100;
   a=30*aa-aa*temp_time;
   temp_vel=30*aa*temp_time-aa*temp_time*temp_time/2-250*aa;
   temp_pos=1500*aa+15*aa*temp_time*temp_time-(aa*temp_time*temp_time*temp_time)/6-250*aa*temp_time;
  }
  else if(temp_time>30&&temp_time<=40)       //匀速段
  {
       
   temp_aa=0;
   a=0;
   temp_vel=200*aa;
   temp_pos=3000*aa+temp_vel*(temp_time-30);
  }
  else if(temp_time>40&&temp_time<=50)        //减加速段
  {
        
   temp_aa=-100;
   a=40*aa-aa*temp_time;
   temp_vel=-aa*temp_time*temp_time/2-600*aa+40*aa*temp_time;
         temp_pos=20*aa*temp_time*temp_time-aa*temp_time*temp_time*temp_time/6-600*aa*temp_time-3000*aa+32000*aa/3;
  }
  else if(temp_time>50&&temp_time<=60)        //匀减速段
  {
     
   temp_aa=0;
   a=-10*aa;
   temp_vel=150*aa-10*aa*temp_time+500*aa;
   //temp_pos=-(650*aa*temp_time-5*aa*temp_time*temp_time-32500*aa-12500*aa)+17000*aa-30500*a/3;
      temp_pos=17000*aa-30500*aa/3-20000*aa+650*aa*temp_time-5*aa*temp_time*temp_time;
  }
  else if(temp_time>60&&temp_time<=70)        //减减速段
  {
        
   temp_aa=100;
   a=aa*temp_time-70*aa;
   temp_vel=50*aa+aa*temp_time*temp_time/2-70*aa*temp_time+2400*aa;
      temp_pos=(-30500/3)*aa-39000*aa+2450*aa*temp_time+aa*temp_time*temp_time*temp_time/6-35*aa*temp_time*temp_time;
  }
  out<<temp_pos<<" "<<temp_vel<<" "<<a<<" "<<temp_aa<<"\n";
  cout<<temp_pos<<" "<<temp_vel<<" "<<a<<" "<<temp_aa<<"\n";
  temp_time+=0.01*times;
  temp_time++;
  if(temp_time>70)
  {
     
   break;
  }
    }
        out.close();
 system("pause");
 return 0;
}

生成的数据会在控制台中显示,也会存储在txt文件中,方便使用matlab进行分析。
得到的数据绘图如下:

关于运动控制中S型速度曲线的简单演示(C++实现)_第2张图片
图二:加加速度
关于运动控制中S型速度曲线的简单演示(C++实现)_第3张图片
图三:加速度
关于运动控制中S型速度曲线的简单演示(C++实现)_第4张图片
图四:速度
关于运动控制中S型速度曲线的简单演示(C++实现)_第5张图片
图五:位置
较为复杂的根据位置来进行速度规划的算法还在进一步的计算过程中,如果有时间将会在后面写出来。
经验:建模与计算远比代码的实现复杂,数据最好要有输出,要有好的数据分析软件,便于查看实际的效果。
欢迎大家的指点。

参考论文:CNC系统S型曲线加减速算法的设计与实现

你可能感兴趣的:(算法实现,求知)