原始速度函數
設初始速度爲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。
以上就是濾波方式下加速度梯形算法的連續表達式形式。