原始速度函數
設初始速度爲v0,最大加速度爲am,加速時間爲t1,濾波時間爲t2,於是有
v(t)=v0+amt,f(t)=2t2πsin(t2πt),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)=2t2πsin(t2πt),t∈[0,t2]
令V(t)=v(x)∗f(x)=∫−∞∞v(t−x)⋅f(x)dx,則被積函數的非零定義域爲
{0⩽t−x⩽t1+2t20⩽x⩽t2⟹{t−(t1+2t2)⩽x⩽t0⩽x⩽t2
推導濾波速度公式
根據t1和t2的大小,分別推導濾波速度公式V(t)。
若 t1⩾t2
當 t<0 時
V(t)≡0.
當 0⩽t⩽t2 時
V(t)=∫0tv0⋅2t2πsin(t2πx)dx=−2v0cos(t2πx)∣∣∣∣∣0t=2v0−2v0cos(t2πt).
當 t2⩽t⩽2t2 時
V(t)====∫t−t2t2v0⋅2t2πsin(t2πx)dx+∫0t−t2[v0+am(t−x−t2)]⋅2t2πsin(t2πx)dx−2v0cos(t2πx)∣∣∣∣∣t−t2t2+[−21(v0+amt−amt2)cos(t2πx)+2amxcos(t2πx)−2am⋅πt2sin(t2πx)]∣∣∣∣∣0t−t22v0+2v0cos(t2π(t−t2))+[−21(v0+amt−amt2)cos(t2π(t−t2))+2am(t−t2)cos(t2π(t−t2))−2am⋅πt2sin(t2π(t−t2))]−[−21(v0+amt−amt2)]v0+2am(t−t2)−2πamt2sin(t2π(t−t2)).
當 2t2⩽t⩽t1+t2 時
V(t)====∫0t2[v0+am(t−x−t2)]⋅2t2πsin(t2πx)dx[−21(v0+amt−amt2)cos(t2πx)+2amxcos(t2πx)−2am⋅πt2sin(t2πx)]∣∣∣∣∣0t221(v0+amt−amt2)−2amt2−[−21(v0+amt−amt2)]v0+amt−23amt2.
當 t1+t2⩽t⩽t1+2t2 時
V(t)====∫t−t1−t2t2[v0+am(t−x−t2)]⋅2t2πsin(t2πx)dx+∫0t−t1−t2vm⋅2t2πsin(t2πx)dx[−21(v0+amt−amt2)cos(t2πx)+2amxcos(t2πx)−2am⋅πt2sin(t2πx)]∣∣∣∣∣t−t1−t2t2−2vmcos(t2πx)∣∣∣∣∣0t−t1−t221(v0+amt−amt2)−2amt2−[−21(v0+amt−amt2)cos(t2π(t−t1−t2))+2am(t−t1−t2)cos(t2π(t−t1−t2))−2am⋅πt2sin(t2π(t−t1−t2))]−2vmcos(t2π(t−t1−t2))+2vm21(v0+vm)+21amt−amt2+2πamt2sin(t2π(t−t1−t2)).
當 t1+2t2⩽t⩽t1+3t2 時
V(t)===∫t−t1−2t2t2vm⋅2t2πsin(t2πx)dx−2vmcos(t2πx)∣∣∣∣∣t−t1−2t2t22vm+2vmcos(t2π(t−t1−2t2)).
當 t>t1+3t2 時
V(t)≡0.
若 t1<t2
當 t<0 時
V(t)≡0.
當 0⩽t⩽t2 時
V(t)=∫0tv0⋅2t2πsin(t2πx)dx=2v0−2v0cos(t2πt).
當 t2⩽t⩽t1+t2 時
V(t)==∫t−t2t2v0⋅2t2πsin(t2πx)dx+∫0t−t2[v0+am(t−x−t2)]⋅2t2πsin(t2πx)dxv0+2am(t−t2)−2πamt2sin(t2π(t−t2)).
當 t1+t2⩽t⩽2t2 時
V(t)====∫t−t2t2v0⋅2t2πsin(t2πx)dx+∫t−t1−t2t−t2[v0+am(t−x−t2)]⋅2t2πsin(t2πx)dx+∫0t−t1−t2vm⋅2t2πsin(t2πx)dx−2v0cos(t2πx)∣∣∣∣∣t−t2t2+[−21(v0+amt−amt2)cos(t2πx)+2amxcos(t2πx)−2am⋅πt2sin(t2πx)]∣∣∣∣∣t−t1−t2t−t2−2vmcos(t2πx)∣∣∣∣∣0t−t1−t22v0+2v0cos(t2π(t−t2))+[−21(v0+amt−amt2)cos(t2π(t−t2))+2am(t−t2)cos(t2π(t−t2))−2πamt2sin(t2π(t−t2))]−[−21(v0+amt−amt2)cos(t2π(t−t1−t2))+2am(t−t1−t2)cos(t2π(t−t1−t2))−2πamt2sin(t2π(t−t1−t2))]−2vmcos(t2π(t−t1−t2))+2vm21(v0+vm)−2πamt2sin(t2π(t−t2))+2πamt2sin(t2π(t−t1−t2)).
當 2t2⩽t⩽t1+2t2 時
V(t)==∫t−t1−t2t2[v0+am(t−x−t2)]⋅2t2πsin(t2πx)dx+∫0t−t1−t2vm⋅2t2πsin(t2πx)dx21(v0+vm)+21amt−amt2+2πamt2sin(t2π(t−t1−t2)).
當 t1+2t2⩽t⩽t1+3t2 時
V(t)=∫t−t1−2t2t2vm⋅2t2πsin(t2πx)dx=2vm+2vmcos(t2π(t−t1−2t2)).
當 t>t1+3t2 時
V(t)≡0.
濾波速度公式
取 t2⩽t⩽t1+2t2 時的濾波速度函數作爲濾波速度公式,則有
當 t1⩾t2 時
V(t)=⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧v0+2am(t−t2)−2πamt2sin(t2π(t−t2)),v0+amt−23amt2,21(v0+vm)+21amt−amt2+2πamt2sin(t2π(t−t1−t2)),若 t2⩽t⩽2t2若 2t2⩽t⩽t1+t2若 t1+t2⩽t⩽t1+2t2
當 t1<t2 時
V(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧v0+2am(t−t2)−2πamt2sin(t2π(t−t2)),21(v0+vm)−2πamt2sin(t2π(t−t2))+2πamt2sin(t2π(t−t1−t2)),21(v0+vm)+21amt−amt2+2πamt2sin(t2π(t−t1−t2)),若 t2⩽t⩽t1+t2若 t1+t2⩽t⩽2t2若 2t2⩽t⩽t1+2t2
濾波速度公式的導數及積分
加速度公式
濾波速度公式的一階導數,如下
當 t1⩾t2 時
V′(t)=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧2am−2amcos(t2π(t−t2)),am,2am+2amcos(t2π(t−t1−t2)),若 t2⩽t⩽2t2若 2t2⩽t⩽t1+t2若 t1+t2⩽t⩽t1+2t2
當 t1<t2 時
V′(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧2am−2amcos(t2π(t−t2)),−2amcos(t2π(t−t2))+2amcos(t2π(t−t1−t2)),2am+2amcos(t2π(t−t1−t2)),若 t2⩽t⩽t1+t2若 t1+t2⩽t⩽2t2若 2t2⩽t⩽t1+2t2
加加速度公式
濾波速度公式的二階導數,如下
當 t1⩾t2 時
V′′(t)=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧2t2amπsin(t2π(t−t2)),0,−2t2amπsin(t2π(t−t1−t2)),若 t2⩽t⩽2t2若 2t2<t<t1+t2若 t1+t2⩽t⩽t1+2t2
當 t1<t2 時
V′′(t)=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧2t2amπsin(t2π(t−t2)),2t2amπsin(t2π(t−t2))−2t2amπsin(t2π(t−t1−t2)),−2t2amπsin(t2π(t−t1−t2)),若 t2⩽t⩽t1+t2若 t1+t2<t<2t2若 2t2⩽t⩽t1+2t2
此即爲加速度S形算法的加加速度公式,若已知系統最大加加速度爲Jm,則可以令 2t2amπ=Jm,於是有 t2=2Jmamπ。
距離公式
根據加速度公式的對稱性,按照幾何意義積分,可以得到運動距離的公式爲
s=21(v0+vm)(t1+t2)
其中 vm=v0+amt1⇔t1=amvm−v0, t2=2Jmamπ。
以上就是濾波方式下加速度S形算法的連續表達式形式。