原始速度函数
设初始速度为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形算法的连续表达式形式。