S曲线公式到步进加速过程推导

zkmcu

最近在搞步进电机控制，加减速度控制是必须的。
我的要求是，给出起始速度，最大速度，加速时间，加速阶段脉冲数4个参数后能自动计算出每个脉冲点的速度
S曲线因为加速度不突变而普遍采用，我选用的S曲线公式为Y = K/(1+EXP(A-BX))，加速度连续的公式应该都可以。

把K定义为最大速度Vm,X定义为时间t，Y即为计算出的速度。
t=0, 起始速度V0 = Vm/(1+EXP(A))  计算出 A = ln((Vm-V0)/VO)   (1）

加速时间定义为Ta,加速时间的一半定义为Th，定义t= Th时，Y = Vm/2 则有
Vm/2 = Vm/(1+EXP(A-Bt))  计算出 B = A/t  (2)

根据公式(1),(2)就可以计算实际速度了。

具体计算每个脉冲点的速度时，就把公式离散化就行了。
比如 加速阶段的时间为100ms, 分100个脉冲完成。那就每隔1ms带入计算就行了。
每点的速度知道了，具体到计算定时器重装周期就根据自己的情况去算吧。

zkmcu

我定义的是加速过程一半时的速度为最大速度的一半来计算参数B的，这样加加速和减加速是对称的，也可以在定义在其它点来计算

fengyunyu

坐等大神继续讲课

bsz84

坐等大神继续讲课+

samo110

我也分享一个之前做的S加减速曲线计算方法

1. /*
   
2. 主计算函数
   
3. 本来想做成一个通用的函数，但是目前手上的项目需求很明确，时间也不多，所以临时做了个版本
   
4. 该函数并不具有通用性，几个参数考虑的重要程度不一样，会有不同的取舍，我是按下面这几条思想做的，供大家参考，有发现不合理的地方请大家指正
   
5. 计算中优先考虑的是运动距离必须跟用户设定的保持一致
   
6. 另外四个变加速过程的捷度被认为是受动力源和机械结构限制，所以也保持跟用户设定一致，不能修改
   
7. 其他参数在计算过程中发现逻辑冲突的，会优先考虑修改最大加减速
   
8. 加减速参数满足条件但用户设定行程比较短，会考虑使用一个比用户设定速度更小的参数计算，直到满足要求
   
9. */
   
10. void MainWindow::Update(bool isV, bool isSave, bool PorV)
    
11. {
    
12.     /*计算结果缓存，笨办法开两个比较大的数组*/
    
13.     QVector<double> x(1024*1024),v(1024*1024),s(1024*1024);
    
14. 
    
15.     ui->txLog->clear();
    
16. 
    
17.     /*读取用户设定参数*/
    
18.     TimeStep = ui->nTimeStep->text().toDouble() * 0.000001; //s
    
19.     Smax = ui->nS->text().toDouble();
    
20.     Vmax = ui->nVmax->text().toDouble();
    
21.     ACCmax = ui->nACCmax->text().toDouble();
    
22.     DCCmax = ui->nDCCmax->text().toDouble();
    
23.     J1 = ui->nJ1->text().toDouble();
    
24.     J2 = ui->nJ2->text().toDouble();
    
25.     J3 = ui->nJ3->text().toDouble();
    
26.     J4 = ui->nJ4->text().toDouble();
    
27. 
    
28.     int xcount = 0;
    
29. 
    
30.     ui->txLog->append("Calculate begin.");
    
31.     ui->txLog->append("----");
    
32.    
    
33.     double S1,S2,S3,S4,S5,S6,S7;
    
34.     double V1,V2,V3,V4;
    
35.     int PlotPointer = 0;
    
36.     bool fFirst = true;
    
37.     bool fS4Neg = false;
    
38.     do
    
39.     {
    
40.         /*判断用户设定的捷度加减速度和最大速度能不能使各段曲线衔接上*/
    
41.         V1 = (ACCmax*ACCmax)/(2*J1);
    
42.         V2 = Vmax - (ACCmax*ACCmax)/(2*J2);
    
43.         V3 = Vmax - (DCCmax*DCCmax)/(2*J3);
    
44.         V4 = (DCCmax*DCCmax)/(2*J4);
    
45.         if(fFirst)
    
46.         {
    
47.             ui->txLog->append("Velocity at end of S1 (V1): "+QString::number(V1, 10, 8));
    
48.             ui->txLog->append("Velocity at begin of S3 (V2): "+QString::number(V2, 10, 8));
    
49.             ui->txLog->append("Velocity at end of S5 (V3): :"+QString::number(V3, 10, 8));
    
50.             ui->txLog->append("Velocity at begin of S7 (V4): "+QString::number(V4, 10, 8));
    
51.         }
    
52.         if(V2<V1)//加速度出现曲线冲突，修改最大加速度
    
53.         {
    
54.             ACCmax = sqrt((2*J1*J2*Vmax)/(J1+J2));
    
55.             ui->nACCmax->setText(QString::number(ACCmax, 10, 8));
    
56.             V1 = (ACCmax*ACCmax)/(2*J1);
    
57.             V2 = Vmax - (ACCmax*ACCmax)/(2*J2);
    
58.             if(fFirst)
    
59.             {
    
60.                 ui->txLog->append("----");
    
61.                 ui->txLog->append("[-ERROR-]: V2 < V1, try to set a smaller ACCmax.");
    
62.                 ui->txLog->append("First try V2 = V1, ACCmax = sqrt((2*J1*J2*Vmax)/(J1+J2)) = "+QString::number(ACCmax, 10, 8)+".");
    
63.                 ui->txLog->append("Velocity at end of S1 (V1): "+QString::number(V1, 10, 8));
    
64.                 ui->txLog->append("Velocity at begin of S3 (V2): "+QString::number(V2, 10, 8));
    
65.             }
    
66.         }
    
67.         if(V3<V4)//减速段出现曲线冲突，修改最大减速度
    
68.         {
    
69.             DCCmax = sqrt((2*J3*J4*Vmax)/(J3+J4));
    
70.             ui->nDCCmax->setText(QString::number(DCCmax, 10, 8));
    
71.             V3 = Vmax - (DCCmax*DCCmax)/(2*J3);
    
72.             V4 = (DCCmax*DCCmax)/(2*J4);
    
73.             if(fFirst)
    
74.             {
    
75.                 ui->txLog->append("----");
    
76.                 ui->txLog->append("[-ERROR-]: V3 < V4, try to set a smaller DCCmax.");
    
77.                 ui->txLog->append("First try V3 = V4, DCCmax = sqrt((2*J3*J4*Vmax)/(J3+J4)) = "+QString::number(DCCmax, 10, 8)+".");
    
78.                 ui->txLog->append("Velocity at end of S5 (V3): "+QString::number(V3, 10, 8));
    
79.                 ui->txLog->append("Velocity at begin of S7 (V4): "+QString::number(V4, 10, 8));
    
80.             }
    
81.         }
    
82. 
    
83.         /*计算七段运动距离，判断是否符合用户设定的运动距离*/
    
84.         S1 = (ACCmax*ACCmax*ACCmax)/(6*J1*J1);
    
85.         S2 = (V2*V2 - V1*V1)/(2*ACCmax);
    
86.         S3 = Vmax*ACCmax/J2 - (ACCmax*ACCmax*ACCmax)/(6*J2*J2);
    
87. 
    
88.         S5 = Vmax*DCCmax/J3 - (DCCmax*DCCmax*DCCmax)/(6*J3*J3);
    
89.         S6 = (V3*V3 - V4*V4)/(2*DCCmax);
    
90.         S7 = (DCCmax*DCCmax*DCCmax)/(6*J4*J4);
    
91. 
    
92.         S4 = Smax - (S1+S2+S3+S5+S6+S7);
    
93.         /*第四段位移为负，说明加减速段位移太长，考虑减少最大速度使加减速段时间缩短，行程缩短*/
    
94.         if(S4 < 0)
    
95.         {
    
96.             fS4Neg = true;
    
97.             Vmax -=0.001;
    
98.         }
    
99.         fFirst = false;
    
100.     }while(S4 < 0);
     
101. 
     
102.     /*输出最终参与计算的运动参数*/
     
103.     ui->nVmax->setText(QString::number(Vmax, 10, 8));
     
104.     if(fS4Neg)
     
105.     {
     
106.         ui->txLog->append("----");
     
107.         ui->txLog->append("[-ERROR-]: Smax too small, try smaller Vmax = "+QString::number(Vmax, 10, 8));
     
108.         ui->txLog->append("Recalc with new Smax.");
     
109. 
     
110.         ui->txLog->append("ACCmax  = "+QString::number(ACCmax, 10, 8)+".");
     
111.         ui->txLog->append("Velocity at end of S1 (V1): "+QString::number(V1, 10, 8));
     
112.         ui->txLog->append("Velocity at begin of S3 (V2): "+QString::number(V2, 10, 8));
     
113.         ui->txLog->append("DCCmax  = "+QString::number(DCCmax, 10, 8)+".");
     
114.         ui->txLog->append("Velocity at end of S5 (V3): "+QString::number(V3, 10, 8));
     
115.         ui->txLog->append("Velocity at begin of S7 (V4): "+QString::number(V4, 10, 8));
     
116.     }
     
117.     ui->txLog->append("----");
     
118.     ui->txLog->append("Fianl S1~S7 : "+QString::number(S1, 10, 8)+", "+QString::number(S2, 10, 8)
     
119.               +", "+QString::number(S3, 10, 8)+", "+QString::number(S4, 10, 8)
     
120.               +", "+QString::number(S5, 10, 8)+", "+QString::number(S6, 10, 8)+", "+QString::number(S7, 10, 8));
     
121. 
     
122.     /*计算各段时间*/
     
123.     double T1,T2,T3,T4,T5,T6,T7;
     
124.     T1 = ACCmax/J1;
     
125.     T2 = (V2-V1)/ACCmax;
     
126.     T3 = ACCmax/J2;
     
127.     T4 = S4/Vmax;
     
128.     T5 = DCCmax/J3;
     
129.     T6 = (V3-V4)/DCCmax;
     
130.     T7 = DCCmax/J4;
     
131.     ui->txLog->append("Fianl T1~T7 : "+QString::number(T1, 10, 8)+", "+QString::number(T2, 10, 8)
     
132.               +", "+QString::number(T3, 10, 8)+", "+QString::number(T4, 10, 8)
     
133.               +", "+QString::number(T5, 10, 8)+", "+QString::number(T6, 10, 8)+", "+QString::number(T7, 10, 8));
     
134. 
     
135. 
     
136.     double AllCount = T1+T2+T3+T4+T5+T6+T7;
     
137.     if((AllCount/TimeStep)>1024*1024)
     
138.     {
     
139.         ui->txLog->append("----");
     
140.         ui->txLog->append("Data count exceed max buffer limit, please set a bigger TimeStep.");
     
141.         return;
     
142.     }
     
143. 
     
144.     int TempCount = 0;
     
145.     double TempTime = 0.0;
     
146. 
     
147.     double Astart = 0.0;
     
148.     double Vstart = 0.0;
     
149.     double Sstart = 0.0;
     
150. 
     
151.     /*分七段生成位移曲线和速度曲线*/
     
152.     while(Sstart < Smax)
     
153.     {
     
154.         //加加速 S=(J*T^3)/6  V=(J*T^2)/2
     
155.         if(TempTime <= T1)
     
156.         {
     
157.             Sstart = J1*TempTime*TempTime*TempTime/6.0;
     
158.             Vstart = 0.5*J1*TempTime*TempTime;
     
159.         }
     
160.         //匀加速   S=V0*T+(A*T^2)/2  V=V0+A*T
     
161.         else if(TempTime <= T1+T2)
     
162.         {
     
163.             Sstart = S1 + V1*(TempTime-T1) + 0.5*ACCmax*(TempTime-T1)*(TempTime-T1);
     
164.             Vstart = V1 + ACCmax*(TempTime-T1);
     
165.         }
     
166.         //减加速 曲线公式参考加加速段，需要做一点变换
     
167.         else if(TempTime <= T1+T2+T3)
     
168.         {
     
169.             Sstart = S1 + S2 + Vmax*(TempTime-T1-T2) - ((J2*T3*T3*T3/6)-(J2*(T1+T2+T3-TempTime)*(T1+T2+T3-TempTime)*(T1+T2+T3-TempTime)/6));
     
170.             Vstart = Vmax - (0.5*J2*(T1+T2+T3-TempTime)*(T1+T2+T3-TempTime));
     
171.         }
     
172.         //匀速
     
173.         else if(TempTime <= T1+T2+T3+T4)
     
174.         {
     
175.             Sstart = S1 + S2 + S3 + Vmax*(TempTime-T1-T2-T3);
     
176.             Vstart = Vmax;
     
177.         }
     
178.         //加减速 曲线公式参考加加速段，需要做一点变换
     
179.         else if(TempTime <= T1+T2+T3+T4+T5)
     
180.         {
     
181.             Sstart = S1 + S2 + S3 + S4 + Vmax*(TempTime-T1-T2-T3-T4) - J3*(TempTime-T1-T2-T3-T4)*(TempTime-T1-T2-T3-T4)*(TempTime-T1-T2-T3-T4)/6.0;
     
182.             Vstart = Vmax - 0.5*J3*(TempTime-T1-T2-T3-T4)*(TempTime-T1-T2-T3-T4);
     
183.         }
     
184.         //匀减速
     
185.         else if(TempTime <= T1+T2+T3+T4+T5+T6)
     
186.         {
     
187.             Sstart = S1 + S2 + S3 + S4 + S5 + V3*(TempTime-T1-T2-T3-T4-T5)-0.5*DCCmax*(TempTime-T1-T2-T3-T4-T5)*(TempTime-T1-T2-T3-T4-T5);
     
188.             Vstart = V3 - DCCmax*(TempTime-T1-T2-T3-T4-T5);
     
189.         }
     
190.         //减减速 曲线公式参考加加速段，需要做一点变换
     
191.         else
     
192.         {
     
193.             Sstart = Smax - J4*(T1+T2+T3+T4+T5+T6+T7-TempTime)*(T1+T2+T3+T4+T5+T6+T7-TempTime)*(T1+T2+T3+T4+T5+T6+T7-TempTime)/6.0;
     
194.             Vstart = 0.5*J4*(T1+T2+T3+T4+T5+T6+T7-TempTime)*(T1+T2+T3+T4+T5+T6+T7-TempTime);
     
195.         }
     
196. 
     
197.         x[TempCount] = TempCount;
     
198.         s[TempCount] = Sstart;
     
199.         v[TempCount] = Vstart;
     
200.         xcount = TempCount;
     
201.         TempTime += TimeStep;
     
202.         TempCount ++;
     
203.     }
     
204. 
     
205.     ui->txLog->append("----");
     
206.     ui->txLog->append("All data count : "+QString::number(xcount, 10, 0)+" data in "+QString::number(AllCount, 10, 8)+"s.");
     
207.    
     
208. 
     
209.     /*判断绘图区域绘制速度曲线还是位移曲线*/
     
210.     if(isV)
     
211.     {
     
212.         ui->customPlot->xAxis->setRange(0,xcount);
     
213.         ui->customPlot->yAxis->setRange(0,Vmax);
     
214.         ui->customPlot->graph(0)->setData(x,v);
     
215.     }
     
216.     else
     
217.     {
     
218.         ui->customPlot->xAxis->setRange(0,xcount);
     
219.         ui->customPlot->yAxis->setRange(0,Smax);
     
220.         ui->customPlot->graph(0)->setData(x,s);
     
221.     }
     
222. 
     
223.     ui->customPlot->replot();
     
224. }

复制代码

hkjabcd

顶起来，听课

wx-ta

自带小板凳了

cgbabc

请教大神，假如1ms计算一次速度，当前的速度和下一个速度的脉冲时间间隔如何衔接？

Excellence

谢谢分享。

mdjfish

假如1ms计算一次速度，

haishangfeiyin

坐等大神讲课,顺带问一句:S曲线加速过程的位移怎么计算呢?

关于以后

感谢分享。

lb0857

坐等老师讲课

wowangru

看着挺有用！！！！！！

zkmcu

cgbabc 发表于 2019-2-25 01:15
请教大神，假如1ms计算一次速度，当前的速度和下一个速度的脉冲时间间隔如何衔接？ ...

这里是举例说加速时间是100ms,分100个脉冲完成，计算周期表的时候就是按每增加1ms算一次速度值，并不是1ms去计算一次。 公式里的单位量纲 速度时 mm/s, 时间是s,  所以实际计算时量纲不要搞错

amxx

S曲线，这个得持续关注

nust-奔跑

围观，学习

meerlin

samo110 发表于 2019-2-24 22:18
我也分享一个之前做的S加减速曲线计算方法

图用qcustomplot绘制的？

samo110

meerlin 发表于 2019-2-25 13:32
图用qcustomplot绘制的？

恩，常用绘图任务简单方便，就是大量高速绘图时比较卡

meerlin

samo110 发表于 2019-2-25 14:54
恩，常用绘图任务简单方便，就是大量高速绘图时比较卡

楼主现在不做结构了么

samo110

meerlin 发表于 2019-2-25 14:56
楼主现在不做结构了么

你这接着我这一层回复，有口口声声喊着楼主，我很尴尬呀
我反正是机械专业出来，干了结构干硬件，干完硬件转软件，啥都打一耙，啥都挖不深

zkmcu

samo110 发表于 2019-2-25 19:10
你这接着我这一层回复，有口口声声喊着楼主，我很尴尬呀
我反正是机械专业出来，干了结构干硬件，干完硬 ...

我也尴尬啊

wulixian

感谢分享

yiyinzhudan

感谢楼主分享

advarx21ic

感谢楼主分享

Stm32Motor

haishangfeiyin 发表于 2019-2-25 08:48
坐等大神讲课,顺带问一句:S曲线加速过程的位移怎么计算呢?
(引用自11楼)

给速度曲线进行积分就好了．可以看下我发的一个贴子.

Stm32Motor

我开始也是想通过位置曲线来求取相临速度采样点之间应该发多少脉冲，仔细一想，把事情搞复杂了，把速度采样率提高就是对应的脉冲了．