伺服系统使用S曲线

在之前文章《S形曲线规划方式汇总》 介绍过贝塞尔曲线方式，并且在Marlin开源工程中也有贝塞尔曲线步进系统的实现方式。本篇介绍伺服系统中基于时间分割法实现的贝塞尔S曲线。

1 贝塞尔曲线路程规划

上文中推导过贝塞尔曲线，本文直接用结论：

对于初速度为V0，末速度为Ve，总加速时间为T的加速过程，其在x*T(0<=x<=1)时刻的6点式贝塞尔曲线速度可以表示为:

v(t)=A*x^5 + B*x^4 + C*x^3 + F

其中：A=6*(Ve-V0)        B=15*(V0-Ve)         C=10*(Ve-V0)        F=V0

则从0试刻走到x*T时刻的总路程S为：

2 Python算法仿真

import matplotlib.pyplot as plt
import math

a = 200000 #加速度
d = 200000 #减速度

#T型速度规划函数
def move_line(len,speed):
    max_speed = speed
    acc_steps = max_speed*max_speed/(2*a)
    dec_steps = max_speed*max_speed/(2*d)
    p = len-acc_steps-dec_steps
    if p<0:
        acc_steps = len*d/(d+a)
        p = 0
        max_speed = math.sqrt(2*a*acc_steps)
    acc_time = float(max_speed)/a
    dec_time = float(max_speed)/d
    cru_time = float(p)/max_speed
    acc_until = acc_steps
    acc_after = acc_steps + p
    return acc_time,dec_time,cru_time,acc_until,acc_after,max_speed

acc_time,dec_time,cru_time,acc_until,acc_after,vm = move_line(8000,20000)

t = 0
Ta = acc_time
Tb = acc_time+cru_time
Tc = acc_time+dec_time+cru_time

#T型速度规划
timeArr = []
posArr = []
speedArr=[]
posPre = 0
while t

 运行结果：

 可以看到在加减速阶段Bezier规划的速度曲线相比T型曲线更柔和。