【龙印】步进电机使用七段式抛物线型S曲线加减速和路径规划的理论分析和实现

本文为在用龙芯1c做3D打印机过程中的笔记。龙芯1c做的3d打印机简称“龙印”，Git地址“http://git.oschina.NET/caogos/marlin_ls1c”

理论分析

S曲线算法的核心思想是让加速度不产生突变，从而使被控对象的速度控制具有快速、平稳的特性。这就回答了为什么要使用S曲线的原因。

常见的S曲线有抛物线型和三角函数型。这里主要探讨抛物线型。这里说的抛物线指的是速度曲线为抛物线，加速度的曲线却为梯形曲线。同样marlin中使用的梯形曲线也指的是速度曲线。

注意观察与后面的抛物线型S曲线的异同。

本文参考以下论文

七段式抛物线型S曲线

常见的需求是从一点（一个坐标）运动到灵一点（另一个坐标），要求用最短的时间，但是最大速度不能超过某个值。

在实际应用中，如果需要运动的距离太短，可能还来不及加速度最大速度，甚至加速度都还没达到最大值。这时可能就没有加加速阶段或者匀速阶段等。那么实际的速度曲线可能就是六段式，五段式，四段式抛物线型S曲线。

这就要求在路径规划时，需要根据每条路径的长短情况，选择不同的抛物线型S曲线。

其它网友有一种方案——将S曲线做成一张表，然后查表，实现S曲线加减速。假设每个路径的起点和终点速度都是v0，即从v0加速后再减速到v0。现在有两个不同长度的路径，但用的却是同一个表，同一个抛物线曲线，这会造成速度是连续的。加速度不连续，这点不太好理解，慢慢理解吧，呵呵！

六段式抛物线型S曲线

五段式抛物线型S曲线

四段式抛物线型S曲线

下面以四段式抛物线型S曲线为例详细讲解

从0到t1

[0,t1]

加速度的加速度=h_max

a(t)=h_max*t

对a(t)求积分

v(t)=h_max/2*t*t+c

将(0,v0)代入，求得c=v0

所以，v(t)=h_max/2*t*t+v0

对v(t)求积分

s(t)=h_max/2/3*t*t*t+v0*t+c

将(0,0)代入，求得c=0

所以，s(t)=h_max/6*t*t*t+v0*t

[0,t1]之间的距离S = s(t1)-S(0) = s(t1) = h_max/6*t1*t1*t1 + v0*t1

 

在t=t1时，

a(t1)=h_max*t1

v(t1)=h_max/2*t1*t1+v0

s(t1)=h_max/6*t1*t1*t1+v0*t1

从t1到t2

[t1,t2]

a(t)=k*t+b

将(t1,h_max*t1)和(2*t1,0)代入，求得

k=-h_max

b=2*h_max*t1

那么，a(t)=-h_max*t+2*h_max*t1

对a(t)积分

v(t)=-h_max/2*t*t+2*h_max*t1*t+c

将t=t1时的速度代入,(t1,h_max/2*t1*t1+v0)

h_max/2*t1*t1+v0=h_max/2*t1*t1+2*h_max*t1*t1+c

求得

c = v0 - h_max*t1*t1

所以，v(t)=-h_max/2*t*t + 2*h_max*t1*t + v0-h_max*t1*t1

对v(t)积分

s(t)=-h_max/2/3*t*t*t + 2*h_max*t1/2*t*t + (v0-h_max*t1*t1)*t + c

=-h_max/6*t*t*t + h_max*t1*t*t + (v0-h_max*t1*t1)*t + c

将t=t1时的距离代入,(t1,h_max/6*t1*t1*t1+v0*t1)

h_max/6*t1*t1*t1 + v0*t1 = -h_max/6*t1*t1*t1 + h_max*t1*t1*t1 + (v0-2*h_max*t1*t1)*t1 + c

求得

c = 1/3*h_max*t1*t1*t1

所以，s(t)=-h_max/6*t*t*t + h_max*t1*t*t + (v0-h_max*t1*t1)*t + 1/3*h_max*t1*t1*t1

[t1,t2]之间的距离

S = s(t2) - s(t1)

  = s(2*t1) - s(t1)

  =[-h_max/6*(2*t1)*(2*t1)*(2*t1) + h_max*t1*(2*t1)*(2*t1) + (v0-h_max*t1*t1)*(2*t1)]

 - [-h_max/6*t1*t1*t1 + h_max*t1*t1*t1 + (v0-h_max*t1*t1)*t]

  =5/6*h_max*t1*t1*t1 + v0*t1

[0,t2]的距离

S[0,t1] + S[t1,t2]

=[h_max/6*t1*t1*t1 + v0*t1] + [5/6*h_max*t1*t1*t1 + v0*t1]

=h_max*t1*t1*t1 + 2*v0*t1

再回顾下v(t1)=h_max/2*t1*t1+v0，仔细对比你会发现[0,t2]的距离S=2*v(t1)*t1，其中t2=2*t1。

用一个实际例子验证前面的公式

假设

v0=6

h_max=5

t1=7

t2=14

 

在区间[0,7]

加速度公式

a(t)=5t

v(t)=5/2*t*t+c

代入(0,6)

6=c

速度公式

v(t)=2.5*t*t+6

s(t)=2.5/3*t*t*t+6t+c

代入(0,7)

s(t)=5/6*t*t*t+6t+7

[0,7]的距离

S=s(7)-s(0)

 = (5/6*7*7*7 + 6*7 + 7) - (7)

 = 5/6*7*7*7 + 6*7

 = 327.83

 

t=7时

a(7)=35

v(7)=128.5

s(7)=2009/6=334.83

 

在区间[7,14]

a(t)=kx+b

代入(7,35)和(14,0)

35=7k+b

0=14k+b

k=-5

b=70

加速度公式

a(t)=-5t+70

v(t)=-5/2*t*t+70*t+c

代入(7,128.5)

128.5=-5/2*7*7+70*7+c

c=-239

速度公式

v(t)=-5/2*t*t+70*t-239

s(t)=-5/2/3*t*t*t+70/2*t*t-239*t+c

    =-5/6*t*t*t + 35*t*t -239*t + c

代入(7,2009/6)

2009/6=-5/6*7*7*7 + 35*7*7 - 239*7 + c

      =-1715/6 + 1715 - 1673 + c

      =-1715/6 + 42 + c

c = 2009/6 + 1715/6 + 42

  = 3724/6 + 42

  = 662.67

面积公式

s(t)=-5/6*t*t*t + 35*t*t - 239*t + c

[7,14]的面积

S=s(14)-s(7)

 = (-5/6*14*14*14 + 35*14*14 - 239*14 + c) - (-5/6*7*7*7 + 35*7*7 - 239*7 + c)

 = -13720/6 + 3514 - (-1715/6 + 42)

 = -13720/6 + 3514 + 1715/6 - 42

 =-12005/6 + 3472

 =-2000.83 + 3472

 =1471.17

 

[0,14]的距离

[0,7]的距离+[7,14]的距离

S = 327.83 + 1471.17

  = 1799

应用前面推论得到的公式

S = h_max*t1*t1*t1 + 2*v0*t1

 = 5*7*7*7+2*6*7

 = 1799

根据推论：[0,t2]的距离S=2*v(t1)*t1，其中t2=2*t1

S=2*v(7)*7=2*128.5*7=1799

实际计算结果和以上推论是相符合的，说明公式是正确的。

实现

用定点运算代替浮点运算