步进电机的Sin曲线加减速生成

felwell 2019-05-23

展开全文

最近做毕设，魔方机器人的项目，有步进电机的控制，步进电机是86框式12Nm步进电机，带1000线编码器反馈。一开始我是直接给个固定频率脉冲（程序中未考虑方向问题）：

//@breaf 步进电机驱动程序
//@param distance  移动距离
//@param speed     移动速度，此值越大速度越小
void MotorMove(int distance，int speed)
{
    int i;
    for(i=0;i<distance;i++)
    {
        MotorPluseUp;
        DelayMs(speed);
        MotorPluseDown;
        DelayMs(speed);
}

在实际使用时发现，由于电机加速和减速都太猛，在带动魔方转动的时候很容易让魔方因为加减速太快而产生非预期的转动。上网查资料后发现，网上普遍使用S曲线对步进电机进行加减速，但是为什么使用S曲线并没有人说明白。由于S曲线不太好积分，所以本文使用曲线上相近的Sin函数曲线对步进电机做加减速。

如下图，假设要用Sin函数曲线对步进电机加速，目标速度为A，加速距离（即脉冲数目）为S，v-t图像如下：

函数公式为：

由于已经指定加速距离为S（单位为脉冲，即单位为1），所以曲线下面积为S，即：

根据高中数学知识，不难求解得到k的值：

在步进电机控制程序中，我们是通过控制每个脉冲的延时时间来控制步进电机在此时的速度的，由于每两个脉冲之间的时间间隔非常短，一般不适合让单片机进行实时运算，所以提前建立好加减速脉冲数组，每次调用即可。现在的问题是对于加速过程的每个脉冲，其延时时间如何求。

设脉冲编号为i=1,2,3...S，由于每个脉冲所表示的步进电机移动距离是相同的，因此有：

当求出上式中的xi之后，就可以求得每个脉冲的延时时间:

利用不定积分计算器，可以求解xi的方程：

即：

这是一个超越方程，不过可以求出数值解：

        double GetXi(double k,double A,double i)
        {
            double preXi = 0;
            double Xi = k / 2;
            while(Math.Abs(Xi-preXi)>1e-6)
            {
                double a = 2 * k * Math.Cos(Math.PI / 2 * (Xi - k) / k);
                preXi = Xi;
                Xi = a / Math.PI + i/ A;
            }
            return Xi;
        }

这样，xi就求出来了，通过xi可以求解Delayi：

        private void button2_Click(object sender, EventArgs e)
        {
            this.richTextBox1.Text = "";
            Graphics g = this.pictureBox1.CreateGraphics();
            g.Clear(Color.White);

            double S = System.Convert.ToDouble(this.S.Text);
            double v = System.Convert.ToDouble(this.v.Text);
            double A = 1 / v;
            double k = S / ((Math.PI / 2 - 1) * A);

            this.richTextBox1.Text += "#define " + this.PathName.Text + "AcNum  " + this.S.Text + "\r\n";
            this.richTextBox1.Text +=  "int "+this.PathName.Text + "[" + this.S.Text+"]={" + "\r\n";
            double Xi_1 = 0;
            double Xi = 0;
            for(int i=0;i<S;i++)
            {
                Point p = new Point();

                Xi_1 = Xi;
                Xi = GetXi(k, A, i);

                p.X = i;
                p.Y = (int)(Xi - Xi_1);

                g.DrawEllipse(Pens.Red, p.X, 400 - p.Y, 2, 2);

                if ((i % 25 == 0)&&(i>0)) this.richTextBox1.Text += "\r\n";
                this.richTextBox1.Text += ((int)(Xi - Xi_1 + 0.5)).ToString();
                if (i != S - 1) this.richTextBox1.Text += ",";
            }
            this.richTextBox1.Text += "\r\n};";
        }

运行效果如下：

将richTextBox1中的内容拷贝到keil中，

然后将生成的数组在运动函数中调用，下面的函数为步进电机转动90度的程序：

void MoveL1(int Delay[],u16 AcNum)
{
	  int i;
	  int totalPulse=MotorSub/4;   //转动90度，为细分数/4

		L_DIR_P;                     //方向
	  delay_5us(100);

		for(i=0;i<AcNum;i++)                     //加速阶段
		{
			L_PUL_UP;
			delay_5us((int)(Delay[i]));
			L_PUL_DOWN;
			delay_5us((int)(Delay[i]));
		}

		for(i=AcNum;i<totalPulse-AcNum;i++)      //匀速阶段
		{
			L_PUL_UP;
			delay_5us(Delay[AcNum-1]);
			L_PUL_DOWN;
			delay_5us(Delay[AcNum-1]);
		}

		for(i=totalPulse-AcNum;i<totalPulse;i++)  //减速阶段
		{
			L_PUL_UP;
			delay_5us((int)(Delay[totalPulse-i-1]));
			L_PUL_DOWN;
			delay_5us((int)(Delay[totalPulse-i-1]));
		}
}