T型速度曲线走圆弧插补的简单演示（c++实现）

最近一直在研究运动控制算法相关的知识。之所以要多研究下圆弧插补是因为
圆弧可以作为两条成夹角之间的过渡曲线，弄懂圆弧插补的速度与位置的关系，就能为速度规划中的速度过渡打好理解的基础。
以下示例简单的演示了圆弧插补过程中速度与位置的关系。使用的是T型速度曲线。

#include "stdafx.h"
#include <iostream>
#include <Windows.h>
#include <fstream>
#include <math.h>
using namespace std;
#define   PI  3.14159265
void  GetSpeed()
{  
      double startv=0;
 double endv=100;
 double  a=1000;
 double  x0=0,y0=0;
 double  R=10;
  double    m_pos;
 double  temp_v=0;
 float  temp_time;
 float  interval=0.001;
 double  temp_times=0;
 double  xpos=0,ypos=0;
 double temp_vx=0,temp_vy=0;

  ofstream  out("speed.txt",ios_base::ate);
 //根据T型运动曲线的设定，有匀速运动段
    temp_time=(endv-startv)/a;
 if((1/2)*temp_time*temp_time*a<2*PI*R)    //有匀速运动段
 {
    while(TRUE)
    {
        if((temp_times>=0)&&(temp_times<temp_time))
     {
     temp_v=a*temp_times;
           m_pos=a*temp_times*temp_times/2;
     }
          else if(temp_times>=temp_time&&temp_times<(PI/5))
     {
      temp_v=endv;
         m_pos=5+endv*(temp_times-0.1);
     }
          else if(temp_times>=(PI/5))    //减速段
     {
      temp_v=endv-a*(temp_times-PI/5);      //减速段的速度
      m_pos=20*PI-5+endv*(temp_times-PI/5)-a*(temp_times-PI/5)*(temp_times-PI/5)/2;    //实际位置
      if(temp_v<=(-1))
      { 
            xpos=R*cos(float(m_pos/R));
       ypos=-R*sin(float(m_pos/R));
       cout<<temp_v<<" "<<m_pos<<" "<<xpos<<" "<<ypos<<"\n";
       out<<temp_v<<" "<<m_pos<<" "<<xpos<<" "<<ypos<<"\n";
       break;
      }
      }
           xpos=R*cos(float(m_pos/R));
     ypos=-R*sin(float(m_pos/R));
          temp_vx=temp_v*cos(float(m_pos/R));
     temp_vy=temp_v*sin(float(m_pos/R));
          cout<<temp_v<<" "<<m_pos<<" "<<xpos<<" "<<ypos<<" "<<temp_vx<<" "<<temp_vy<<"\n";
     out<<temp_v<<" "<<m_pos<<" "<<xpos<<" "<<ypos<<" "<<temp_vx<<" "<<temp_vy<<"\n";
     
          temp_times=temp_times+interval;
    }
        out.close();      
 }
out.close();
}

int _tmain(int argc, _TCHAR* argv[])
{
 AllocConsole();
 GetSpeed();
 system("pause");
 return 0;
}

生成的数据的绘制结果如下：

图一：插补速度

图二：插补位置

图三：X方向位置分解

图四：Y方向位置分解

图五：X，Y方向位置合成圆
图六：X方向速度

图七：Y方向速度

由图五可见，根据T型速度曲线走的插补，在X、Y方向上的位置可以完美的合成一个圆。在插补的过程中，直接将圆视为一条直线来处理。

如有问题，欢迎支持。