电机运动S曲线位置速度规划学习并使用Matlab实现

最新推荐文章于 2026-07-06 15:07:15 发布

原创最新推荐文章于 2026-07-06 15:07:15 发布 · 2.6k 阅读

34 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

无刷电机位置S曲线拟合轨迹

S曲线的作用

平滑位置突变，避免因为电机位置变化过大造成速度波动大导致电机启动抖动，可以提高电机的运行速度和运行效率，让电机运动起来更加平滑

S曲线的原理

S型曲线加减速是指在加减速时，加速度的导数 $J$ 为常数的加减速过程通过控制加速度导数来避免加速度突变，减
小运动过程中由于加速度突变而引起的机械系统振动．采用S型曲线加减速算法可以获得平滑的速度和加速度图。
对于电机位置控制来说，因为初始速度和末端速度都一样的（都是0），因此加速度变化波形曲线是对称的。因此波形实际只有以下几种情况
1.（七段式S曲线）速度变化过程包括：加加速阶段，匀加速阶段，减加速阶段，匀速阶段，加减速阶段，匀减速阶段，减减速阶段
2.（六段式S曲线）速度变化过程包括：加加速阶段，匀加速阶段，减加速阶段，加减速阶段，匀减速阶段，减减速阶段
3.（五段式S曲线）速度变化过程包括：加加速阶段，减加速阶段，匀速阶段，加减速阶段，减减速阶段
3.（四段式S曲线）速度变化过程包括：加加速阶段，减加速阶段，加减速阶段，减减速阶段
以第一种情况为例：加速度，速度，位移曲线如下：

S曲线曲线图

分别为：位置变化曲线，速度变化曲线，加速度变化曲线
在这里插入图片描述

S曲线数学方程

定义一些参数变量如下：

参数名	参数含义
$J$	加速度变化率
$a_{max}$	最大加速度
$V_{max}$	给定最大速度
$T_n$	每个阶段的运行时间 $n$ 从1到7
$t$	时间
$S$	位移（如果是转动则是角位移）

第一步

首先要判断S曲线是什么类型（七段，六段还是四段S曲线）
假设S曲线加速阶段不存在匀加速阶段（因为前后速度不变，加速和减速阶段对称，所以相应也不存在匀减速阶段）,此时加速度和速度满足以下关系：
$amax=JTv=∫0tamaxdt=amax2/Ja_{max}=JT\\ \\ v=\int_0^ta_{max}dt=a_{max}^2/J$
可以根据计算加加速度阶段和减加速阶段所求得的速度 $v$ 和我们给定速度进行比较，判断S曲线应该是什么类型的。
1.如果加加速阶段和减加速阶段所求速度 $v$ 小于给定速度 $V_{max}$ ,说明此时应该存在匀加速阶段使得速度可以加速到达给定速度。通过前面可以知道变加速的时间，
下一步计算匀加速阶段的时间。因为速度需要加速到给定速度，所以有
$T_2*a_{max}+a_{max}^2/J=V_{max}\\ \\ T_2=\frac{V_{max}}{a_{max}}-a_{max}/J$
如果大于给定速度，这个时候说明我们给的 $a_{max}$ 太大了，需要重新求解 $a_{max}$ 使得加速阶段得到的速度能够满足要求。因为我们前面得到速度关系 $v=a_{max}^2/J$ 因此只要让 $v=V_{max}$ 带入前面方程就可以解得所需的加速度，并解出对应阶段的时间了
因此可以得到以下关系：
$\left\{ \begin{matrix} T_1=T_3=T_5=T_7=a_{max}/J & V_{max}>=a_{max}^2/J\\ \\ T_1=T_3=T_5=T_7\sqrt{\frac{V_{max}}{J}}&V_{max}<a_{max}^2/J \end{matrix} \right.$

$\left\{ \begin{matrix} T_2=T_6\frac{V_{max}}{a_{max}}-a_{max}/J & V_{max}>=a_{max}^2/J\\ \\ T_2=T_6=0&V_{max}<a_{max}^2/J \end{matrix} \right.$
因为匀减速阶段和匀加速阶段对称，因此只要求得匀加速阶段的曲线就能得到匀减速阶段的方程。

通过速度和加速度变化率得到了加速阶段和减速阶段的时间，接下来判断是否存在匀速时间 $T_4$
上面计算得到了加速阶段和减速阶段的时间，因此加速阶段和减速阶段总位移如下：
$S'=\frac{V_{max}}{2}(2T_1+T_2)+\frac{V_{max}}{2}(2T_3+T_5)$

判断是否存在匀速时间的方法是，通过计算加速阶段和减速阶段的总位移 $S^{'}$ 和给定位移S比较来判断，分为以下三种情况：
1. $S^{'} < S$ 说明加速段和减速段的总位移小于给定位移，因此还需要给定一段时间进行匀速运动使得达到目标位置,此时 $T4=S−S′VmaxT_4=\frac{S-S'}{V_{max}}$ （七段S曲线）
2. $S^{'} = S$ 说明此时不存在匀速运动， $T_4=0$ 加速减速阶段总位移刚好等于给定目标（六段S曲线）
3. $S^{'} > S$ 说明此时不存在匀速运动， $T_4=0$ 并且说明我们给定速度大于实际应该给的速度，这个时候需要重新求解给定速度以满足位移要求

假设这个时候不存在匀加速段，匀减速段（令 $T_2=T_6=0$ ）和匀速段。给定速度 $V_{max}=a_{max}^2/J$ ,将求得的给定速度代入位移方程得到 $S^{''}$ ,通过 $S^{''}$ 和 $S$ 的比较有以下三种情况：

第一种

如果 $S^{''} < S$ 说明我们实际的速度应该大于 $a_{max}^2/J$ (只有加加速和减加速阶段得到的速度)而小于我们一开始给定的速度，因此这个时候应该存在匀加速段，所以实际S曲线应该有6段（加加速，匀加速，减加速，加减速，匀减速，减加速）
此时S曲线各阶段时间如下：
$\left\{ \begin{matrix} T_1=T_3=T_5=T_7=a_{max}/J\\ \\ T_2=T_6=\frac{V_{max}}{a_{max}}-a_{max}/J \end{matrix} \right.$
将其带入位移方程得到速度 $V_{max}$ 的一元二次方程：

$2JV_{max}^2+2a_{max}^2V_{max}-2Ja_{max}S=0\\ \\ V_{max}=-\frac{a_{max}^2}{2J}+\frac{\sqrt{a_{max}^4+4a_{max}J^2S}}{2J}$
将求得的 $V_{max}$ 带入前面方程就可以解得对应的时间和加速度值

第二种

如果 $S^{''} == S$ 说明我们实际速度刚好等于 $a_{max}^2/J$ ，此时只有S曲线为四段S曲线(加加速，减加速，加减速，减减速)

第三种

如果 $S^{''} > S$ 说明我们实际速度小于 $a_{max}^2/J$ ，此时S曲线为四段S曲线(加加速，减加速，加减速，减减速)，并且需要不断减小 $a_{max}$ 使得 $S^{''}$ 能够达到目标值

Matlab程序如下


syms target Start V_s V_e T_1 T_2 T_3 T_4 T_5 T_6 T_7 a_max J v_max V
syms S_1 s s_current v S_3 S_4
 
V_s=0;%初始速度
V_e=0;%末端速度
J=10; %加速度变化率
V=20; %给定速度
a_max=10; %给定加速度
s=10      %目标位置
S_3=[];
count=0;

if(V>=V_s+a_max^2/J)%速度判断，判断是否有匀加速阶段
    T_1=a_max/J;
    T_2=(V-V_s)/a_max-a_max/J;
    T_3=T_1;
else
    T_1=sqrt((V-V_s)/J);
    T_2=0;
    T_3=T_1;
    a_max=sqrt((V-V_s)*J)
end

if(V>=V_e+a_max^2/J)
    T_5=a_max/J;
    T_6=(V-V_e)/a_max-a_max/J;
    T_7=T_5;
else
    T_5=sqrt((V-V_e)/J);
    T_6=0;
    T_7=T_5;
end

S_1=(V+V_s)/2*(T_1+T_2+T_3)+(V+V_e)/2*(T_5+T_6+T_7)


if(s>S_1)
    T_4=(s-S_1)/V;
else
    T_4=0;
    V=a_max^2/J;%假设匀减速加速都不存在
    S_2=(V+V_s)/2*(T_1+T_3)+(V+V_e)/2*(T_5+T_7)
    if S_2<s%表示存在匀加速减速
        V=-a_max^2/(2*J)+sqrt((a_max^4-2*J*(a_max^2*(V_s+V_e)-J*(V_s^2+V_e^2)-2*a_max*J*s)))/(2*J);
        T_1=a_max/J;
        T_2=(V-V_s)/a_max-a_max/J;
        T_3=T_1;
        T_5=a_max/J;
        T_6=(V-V_e)/a_max-a_max/J;
        T_7=T_5;
    elseif S_2==s
        T_1=a_max/J;
        T_2=(V-V_s)/a_max-a_max/J;
        T_3=T_1;
        T_5=a_max/J;
        T_6=(V-V_e)/a_max-a_max/J;
        T_7=T_5;
    else
        a_0=0;
        a_1=a_max;
        while 1 %二分法求取合适加速度a_max
            count=count+1;
            a_max=(a_0+a_1)/2;
            % a_max=a_max-0.001;
            S_3(:,count)=(a_max^2/J+V_s)*a_max/J+(a_max^2/J+V_e)*a_max/J;
            if abs(S_3(:,count)-s)<=0.1
                break
            end
            if(S_3(:,count)>s)
                a_1=a_max;
            else
                a_0=a_max;
            end
            
        end
        T_1=a_max/J;
        T_2=0;
        T_3=T_1;
        T_5=a_max/J;
        T_6=0;
        T_7=T_5;
    end

end
a_max
V
T_1
T_2
T_3
T_4
T_5
T_6
T_7
T=T_1+T_2+T_3+T_4+T_5+T_6+T_7 %总的运行时间

s_current=[];
v_current=[];
a=[]
n=1;
v_1=V_s+J*T_1^2/2;
s_1=V_s*T_1+J*T_1^3/6;
v_2=v_1+a_max*T_2;
s_2=s_1+v_1*T_2+a_max*T_2^2/2;
v_3=v_2+a_max*T_3-J*T_3^2/2;
s_3=s_2+v_2*T_3+a_max*T_3^2/2-J*T_3^3/6;
v_4=v_3;
s_4=s_3+v_4*T_4;
v_5=v_4-0.5*J*T_5^2;
s_5=s_4+v_4*T_5-J*T_5^3/6;
v_6=v_5-a_max*T_6;
s_6=s_5+v_5*T_6-a_max*T_6^2/2;
%绘制位移，速度，加速度图像
for t_0= 0:0.001:T
    if t_0>=0&&t_0<=T_1
        t=t_0;
        s_current(n,:)=V_s*t+J*t^3/6;
        v_current(n,:)=V_s+J*t^2/2;   
        a(n,:)=J*t;
    elseif t_0>T_1&&t_0<=(T_2+T_1)
        t=t_0-T_1;
        s_current(n,:)=s_1+v_1*t+0.5*a_max*t^2;
        v_current(n,:)=v_1+a_max*t;
        a(n,:)=a_max;
    elseif t_0>T_2+T_1&&t_0<=T_3+T_2+T_1
        t=t_0-T_2-T_1;
        s_current(n,:)=s_2+v_2*t+0.5*a_max*t^2-1/6*J*t^3;
        v_current(n,:)=v_2+a_max*t-J*t^2/2;
        a(n,:)=a_max-J*t;
    elseif t_0>T_3+T_2+T_1&&t_0<=T_4+T_3+T_2+T_1
         t=t_0-T_3-T_2-T_1;
        s_current(n,:)=s_3+v_3*t;
        v_current(n,:)=v_3;
        a(n,:)=0;
    elseif t_0>T_4+T_3+T_2+T_1&&t_0<=T_5+T_4+T_3+T_2+T_1
        t=t_0-T_4-T_3-T_2-T_1;
        s_current(n,:)=s_4+v_4*t-J*t^3/6;
        v_current(n,:)=v_4-J*t^2/2;
        a(n,:)=-J*t;
    elseif t_0>T_5+T_4+T_3+T_2+T_1&&t_0<=T_6+T_5+T_4+T_3+T_2+T_1
        t=t_0-T_5-T_4-T_3-T_2-T_1;
        s_current(n,:)=s_5+v_5*t-a_max*t^2/2;
        v_current(n,:)=v_5-a_max*t;
        a(n,:)=-a_max;
    elseif t_0>T_6+T_5+T_4+T_3+T_2+T_1&&t_0<=T
        t=t_0-T_6-T_5-T_4-T_3-T_2-T_1;
        s_current(n,:)=s_6+v_6*t-a_max*t^2/2+J*t^3/6;
        v_current(n,:)=v_6-a_max*t+J*t^2/2;
        a(n,:)=-a_max+J*t;
    end
    n=n+1;
end
t= 0:0.001:T;
plot(t,s_current')
plot(t,v_current')
plot(t,a')
t=1:1:count
plot(t,S_3)