原始速度函数
设初始速度为v0,最大加速度为am,加速时间为t1,滤波时间为t2,于是有
v(t)=v0+amt,f(t)=t21,t∈[0, t1]t∈[0, t2]
构造滤波速度函数
令vm=v0+amt1,
v(t)=⎩⎪⎨⎪⎧v0,v0+am(t−t2),vm,t∈[0, t2]t∈[t2, t1+t2]t∈[t1+t2, t1+2t2]f(t)=t21, t∈[0, t2]
令V(t)=v(x)∗f(x)=∫−∞∞v(x)⋅f(t−x)dx,则被积函数的非零定义域为
{0<x<t1+2t20<t−x<t2⟹{0<x<t1+2t2t−t2<x<t
推导滤波速度公式
若 t1⩾t2
V(t)的被积函数的非零定义域如下图所示
当t<0时
V(t)≡0.
当0⩽t⩽t2时
V(t)=∫0tv0⋅t21dx=t2v0t.
当t2⩽t⩽2t2时
V(t)=∫t−t2t2v0⋅t21dx+∫t2t[v0+am(x−t2)]⋅t21dx=t2v0(2t2−t)+t2v0−amt2(t−t2)+2t2am(t2−t22)=2t2amt2−amt+v0+21amt2.
当2t2⩽t⩽t1+t2时
V(t)=∫t−t2t[v0+am(x−t2)]⋅t21dx=t2v0−amt2[t−(t−t2)]+2t21am[t2−(t−t2)2]=v0−amt2+amt−21amt2=amt+v0−23amt2.
当t1+t2⩽t⩽t1+2t2时
V(t)=∫t−t2t1+t2[v0+am(x−t2)]⋅t21dx+∫t1+t2tvm⋅t21dx=t2v0−amt2(t1+2t2−t)+2t2am[(t1+t2)2−(t−t2)2]+t2vm(t−t1−t2)=t2v0−amt2(t1+2t2−t)+2t2am(t12+2t1t2−t2+2t2t)+t2vm(t−t1−t2)=−2t2amt2+t2am(t1+2t2)t+v0−2t2am(t12+2t1t2+4t22).
当t1+2t2⩽t⩽t1+3t2时
V(t)=∫t−t2t1+2t2vm⋅t21dx=t2vm(t1+3t2−t).
当t>t1+3t2时
V(t)≡0.
若 t1>t2
V(t)的被积函数的非零定义域如下图所示
当t<0时
V(t)≡0.
当0⩽t⩽t2时
V(t)=∫0tv0⋅t21dx=t2v0t.
当t2⩽t⩽t1+t2时
V(t)=∫t−t2t2v0⋅t21dx+∫t2t[v0+am(x−t2)]⋅t21dx=t2v0(2t2−t)+t2v0−amt2(t−t2)+2t2am(t2−t22)=2t2amt2−amt+v0+21amt2.
当t1+t2⩽t⩽2t2时
V(t)=∫t−t2t2v0⋅t21dx+∫t2t1+t2[v0+am(x−t2)]⋅t21dx+∫t1+t2tvm⋅t21dx=t2v0(2t2−t)+t2v0−amt2t1+2t2am[(t1+t2)2−t22]+t2vm(t−t1−t2)=t2amt1t+v0−2t2amt12+2amt1t2=t2amt1t+v0−2t2amt1(t1+2t2).
当2t2⩽t⩽t1+2t2时
V(t)=∫t−t2t1+t2[v0+am(x−t2)]⋅t21dx+∫t1+t2tvm⋅t21dx=t2v0−amt2(t1+2t2−t)+2t2am[(t1+t2)2−(t−t2)2]+t2vm(t−t1−t2)=t2v0−amt2(t1+2t2−t)+2t2am(t12+2t1t2−t2+2t2t)+t2vm(t−t1−t2)=−2t2amt2+t2am(t1+2t2)t+v0−2t2am(t12+2t1t2+4t22).
当t1+2t2⩽t⩽t1+3t2时
V(t)=∫t−t2t1+2t2vm⋅t21dx=t2vm(t1+3t2−t).
当t>t1+3t2时
V(t)≡0.
滤波速度公式
取t2⩽t⩽t1+2t2时的滤波速度函数作为滤波速度公式,则有
当t1⩾t2时
V(t)=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧2t2amt2−amt+v0+21amt2,amt+v0−23amt2,−2t2amt2+t2am(t1+2t2)t+v0−2t2am(t12+2t1t2+4t22),若 t2⩽t⩽2t2若 2t2⩽t⩽t1+t2若 t1+t2⩽t⩽t1+2t2
当t1<t2时
V(t)=⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧2t2amt2−amt+v0+21amt2,t2amt1t+v0−2t2amt1(t1+2t2),−2t2amt2+t2am(t1+2t2)t+v0−2t2am(t12+2t1t2+4t22),若 t2⩽t⩽t1+t2若 t1+t2⩽t⩽2t2若 2t2⩽t⩽t1+2t2
滤波速度公式的导数及积分
加速度公式
滤波速度公式的一阶导数,如下
当t1⩾t2时
V′(t)=⎩⎪⎪⎪⎨⎪⎪⎪⎧t2amt−am,am,−t2amt+t2am(t1+2t2),若 t2⩽t⩽2t2若 2t2⩽t⩽t1+t2若 t1+t2⩽t⩽t1+2t2
当t1<t2时
V′(t)=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧t2amt−am,t2amt1,−t2amt+t2am(t1+2t2),若 t2⩽t⩽t1+t2若 t1+t2⩽t⩽2t2若 2t2⩽t⩽t1+2t2
加加速度公式
滤波速度公式的二阶导数,如下
当t1⩾t2时
V′′(t)=⎩⎪⎪⎪⎨⎪⎪⎪⎧t2am,0,−t2am,若 t2⩽t⩽2t2若 2t2<t<t1+t2若 t1+t2⩽t⩽t1+2t2
当t1<t2时
V′′(t)=⎩⎪⎪⎪⎨⎪⎪⎪⎧t2am,0,−t2am,若 t2⩽t⩽t1+t2若 t1+t2<t<2t2若 2t2⩽t⩽t1+2t2
此即为加速度梯形算法的加加速度公式,若已知系统最大加加速度为Jm,则可以令t2am=Jm,于是有t2=Jmam。
距离公式
根据加速度公式的对称性,按照几何意义积分,可以得到运动距离的公式为
s=21(v0+vm)(t1+t2)
其中vm=v0+amt1 ⇔ t1=amvm−v0,t2=Jmam。
以上就是滤波方式下加速度梯形算法的连续表达式形式。