加速度梯形算法：滤波方式下的速度规划
已知线段长度 s s s，起点速度 v 0 v_0 v0，利用加速度梯形算法（滤波方式）计算能达到的最大终点速度和最小终点速度。其中，系统最大速度为 v m v_m vm，系统最大加速度为 a m a_m am，系统最大加加速度为 J m J_m Jm，插补周期为 T T T。
计算能达到的最大终点速度 v m ′ v'_m vm′
（1）假设实际运动的最大加速度为 a m ′ a'_m am′，则
n = v m ′ − v 0 a m ′ T , L = a m ′ J m T . \begin{aligned} &n=\dfrac{v'_m-v_0}{a'_mT}, \\ &L=\dfrac{a'_m}{J_mT}. \end{aligned} n=am′Tvm′−v0,L=JmTam′.
其中， n n n为直线加速度加速的整周期数， L L L为速度滤波器的长度。
实际运动的距离 s s s： s = 1 2 ( v 0 + v m ′ ) ( n + L ) T = 1 2 ( v 0 + v m ′ ) ( v m ′ − v 0 a m ′ + a m ′ J m ) s=\dfrac{1}{2}(v_0+v'_m)(n+L)T=\dfrac{1}{2}(v_0+v'_m) \left(\dfrac{v'_m-v_0}{a'_m}+\dfrac{a'_m}{J_m}\right) s=21(v0+vm′)(n+L)T=21(v0+vm′)(am′vm′−v0+Jmam′)。
若使运动时间最短，令 v m ′ − v 0 a m ′ = a m ′ J m \dfrac{v'_m-v_0}{a'_m}=\dfrac{a'_m}{J_m} am′vm′−v0=Jmam′， a m ′ = J m ( v m ′ − v 0 ) a'_m=\sqrt{J_m(v'_m-v_0)} am′=Jm(vm′−v0)
于是有
s = 1 2 ( v 0 + v m ′ ) ⋅ 2 ⋅ v m ′ − v 0 a m ′ = ( v m ′ + v 0 ) ( v m ′ − v 0 ) a m ′ , ( v m ′ + v 0 ) ( v m ′ − v 0 ) = a m ′ s = J m ( v m ′ − v 0 ) ⋅ s , ( v m ′ + v 0 ) ⋅ v m ′ − v 0 = J m ⋅ s , ( v m ′ + v 0 ) 2 ( v m ′ − v 0 ) = J m s 2 , v m ′ 3 + v 0 v m ′ 2 − v 0 2 v m ′ − v 0 3 − J m s 2 = 0. \begin{aligned} &s=\dfrac{1}{2}(v_0+v'_m)\cdot 2\cdot\dfrac{v'_m-v_0}{a'_m}=\dfrac{(v'_m+v_0)(v'_m-v_0)}{a'_m}, \\ &(v'_m+v_0)(v'_m-v_0)=a'_ms=\sqrt{J_m(v'_m-v_0)}\cdot s, \\ &(v'_m+v_0)\cdot \sqrt{v'_m-v_0}=\sqrt{J_m}\cdot s, \\ &(v'_m+v_0)^2(v'_m-v_0)=J_ms^2, \\ &v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3-J_ms^2=0. \end{aligned} s=21(v0+vm′)⋅2⋅am′vm′−v0=am′(vm′+v0)(vm′−v0),(vm′+v0)(vm′−v0)=am′s=Jm(vm′−v0)⋅s,(vm′+v0)⋅vm′−v0=Jm⋅s,(vm′+v0)2(vm′−v0)=Jms2,vm′3+v0vm′2−v02vm′−v03−Jms2=0.
（2）求解一元三次方程： v m ′ 3 + v 0 v m ′ 2 − v 0 2 v m ′ − v 0 3 − J m s 2 = 0 v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3-J_ms^2=0 vm′3+v0vm′2−v02vm′−v03−Jms2=0
得到实际的最大终点速度 v m ′ ( v m ′ > v 0 ) v'_m(v'_m>v_0) vm′(vm′>v0)。
（3）检验 a m ′ = v m ′ 2 − v 0 2 s a'_m=\dfrac{v^{'2}_m-v_0^2}{s} am′=svm′2−v02是否满足系统最大加速度的要求。
若 a m ′ a'_m am′不大于系统最大加速度，则 m i n ( v m ′ , v m ) min(v'_m,\ v_m) min(vm′, vm)即为能达到的最大终点速度。
（4）若 a m ′ a'_m am′大于系统最大加速度，则取 a m a_m am为实际运动的最大加速度，重新计算能达到的最大终点速度。
L = a m J m T , n = v m ′ − v 0 a m T , s = 1 2 ( v 0 + v m ′ ) ( L + n ) T = 1 2 ( v 0 + v m ′ ) ( a m J m + v m ′ − v 0 a m ) , J m v m ′ 2 + a m 2 v m ′ + a m 2 v 0 − J m v 0 2 − 2 a m J m s = 0. \begin{aligned} &L=\dfrac{a_m}{J_mT},\quad n=\dfrac{v'_m-v_0}{a_mT}, \\ &s=\dfrac{1}{2}(v_0+v'_m)(L+n)T=\dfrac{1}{2}(v_0+v'_m)\left(\dfrac{a_m}{J_m} +\dfrac{v'_m-v_0}{a_m}\right), \\ &J_mv^{'2}_m+a_m^2v'_m+a_m^2v_0-J_mv_0^2-2a_mJ_ms=0. \end{aligned} L=JmTam,n=amTvm′−v0,s=21(v0+vm′)(L+n)T=21(v0+vm′)(Jmam+amvm′−v0),Jmvm′2+am2vm′+am2v0−Jmv02−2amJms=0.
由此求得最大终点速度 v m ′ v'_m vm′，并且要求 v m ′ > v 0 v'_m>v_0 vm′>v0。若不满足要求，则令 v m ′ = v 0 v'_m=v_0 vm′=v0。
计算能达到的最小终点速度 v m ′ v'_m vm′
（1）首先判断能否减速到零，如果能减速到零，则 v m ′ = 0 v'_m=0 vm′=0。
（2）如果不能减速到零，则假设实际运动的最大加速度为 a m ′ a'_m am′，于是有
n = v 0 − v m ′ a m ′ T , L = a m ′ J m T . \begin{aligned} &n=\dfrac{v_0-v'_m}{a'_mT}, \\ &L=\dfrac{a'_m}{J_mT}. \end{aligned} n=am′Tv0−vm′,L=JmTam′.
其中， n n n为直线加速度减速的整周期数， L L L为速度滤波器的长度。
实际运动的距离 s s s： s = 1 2 ( v 0 + v m ′ ) ( n + L ) T = 1 2 ( v 0 + v m ′ ) ( v 0 − v m ′ a m ′ + a m ′ J m ) s=\dfrac{1}{2}(v_0+v'_m)(n+L)T=\dfrac{1}{2}(v_0+v'_m)\left(\dfrac{v_0-v'_m}{a'_m} +\dfrac{a'_m}{J_m}\right) s=21(v0+vm′)(n+L)T=21(v0+vm′)(am′v0−vm′+Jmam′)。
若使运动时间最短，令 v 0 − v m ′ a m ′ = a m ′ J m \dfrac{v_0-v'_m}{a'_m}=\dfrac{a'_m}{J_m} am′v0−vm′=Jmam′， a m ′ = J m ( v 0 − v m ′ ) a'_m=\sqrt{J_m(v_0-v'_m)} am′=Jm(v0−vm′)
于是有
s = 1 2 ( v 0 + v m ′ ) ⋅ 2 ⋅ v 0 − v m ′ a m ′ = ( v 0 + v m ′ ) ( v 0 − v m ′ ) a m ′ , ( v 0 + v m ′ ) ( v 0 − v m ′ ) = a m ′ s = J m ( v 0 − v m ′ ) ⋅ s , ( v 0 + v m ′ ) ⋅ v 0 − v m ′ = J m ⋅ s , ( v 0 + v m ′ ) 2 ( v 0 − v m ′ ) = J m s 2 , v m ′ 3 + v 0 v m ′ 2 − v 0 2 v m ′ − v 0 3 + J m s 2 = 0. \begin{aligned} &s=\dfrac{1}{2}(v_0+v'_m)\cdot 2\cdot\dfrac{v_0-v'_m}{a'_m}=\dfrac{(v_0+v'_m)(v_0-v'_m)}{a'_m}, \\ &(v_0+v'_m)(v_0-v'_m)=a'_ms=\sqrt{J_m(v_0-v'_m)}\cdot s, \\ &(v_0+v'_m)\cdot \sqrt{v_0-v'_m}=\sqrt{J_m}\cdot s, \\ &(v_0+v'_m)^2(v_0-v'_m)=J_ms^2, \\ &v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3+J_ms^2=0. \end{aligned} s=21(v0+vm′)⋅2⋅am′v0−vm′=am′(v0+vm′)(v0−vm′),(v0+vm′)(v0−vm′)=am′s=Jm(v0−vm′)⋅s,(v0+vm′)⋅v0−vm′=Jm⋅s,(v0+vm′)2(v0−vm′)=Jms2,vm′3+v0vm′2−v02vm′−v03+Jms2=0.
（3）求解一元三次方程： v m ′ 3 + v 0 v m ′ 2 − v 0 2 v m ′ − v 0 3 + J m s 2 = 0 v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3+J_ms^2=0 vm′3+v0vm′2−v02vm′−v03+Jms2=0
得到实际的最小终点速度 v m ′ ( v m ′ > 0 v'_m(v'_m>0 vm′(vm′>0并且 v m ′ < v 0 ) v'_m
（4）检验 a m ′ = v 0 2 − v m ′ 2 s a'_m=\dfrac{v_0^2-v^{'2}_m}{s} am′=sv02−vm′2是否满足系统最大加速度的要求。
若 a m ′ a'_m am′不大于系统最大加速度，则 v m ′ v'_m vm′即为能达到的最小终点速度。
（5）若 a m ′ a'_m am′大于系统最大加速度，则取 a m a_m am为实际运动的最大加速度，重新计算能达到的最小终点速度。
L = a m J m T , n = v 0 − v m ′ a m T , s = 1 2 ( v 0 + v m ′ ) ( L + n ) T = 1 2 ( v 0 + v m ′ ) ( a m J m + v 0 − v m ′ a m ) , J m v m ′ 2 − a m 2 v m ′ − a m 2 v 0 − J m v 0 2 + 2 a m J m s = 0. \begin{aligned} &L=\dfrac{a_m}{J_mT},\quad n=\dfrac{v_0-v'_m}{a_mT}, \\ &s=\dfrac{1}{2}(v_0+v'_m)(L+n)T=\dfrac{1}{2}(v_0+v'_m)\left(\dfrac{a_m}{J_m} +\dfrac{v_0-v'_m}{a_m}\right), \\ &J_mv^{'2}_m-a_m^2v'_m-a_m^2v_0-J_mv_0^2+2a_mJ_ms=0. \end{aligned} L=JmTam,n=amTv0−vm′,s=21(v0+vm′)(L+n)T=21(v0+vm′)(Jmam+amv0−vm′),Jmvm′2−am2vm′−am2v0−Jmv02+2amJms=0.
由此求得最小终点速度 v m ′ v'_m vm′，并且要求 v m ′ > 0 v'_m>0 vm′>0并且 v m ′ < v 0 v'_m
插补
已知起始速度 v s v_s vs，终止速度 v e v_e ve，系统最大速度为 v m v_m vm，系统最大加速度为 a m a_m am，系统最大加加速度为 J m J_m Jm。
粗插补计算能加速到的最大速度 v m ′ ( v m ′ ⩽ v m ) v_m'(v_m'\leqslant v_m) vm′(vm′⩽vm)，正向最大加速度 a m ′ a_m' am′，反向最大加速度 a m ′ ′ a_m'' am′′。
a m ′ = J m ( v m ′ − v s ) , a m ′ ′ = J m ( v m ′ − v e ) , a m ′ = m i n ( a m ′ , a m ) , a m ′ ′ = m i n ( a m ′ ′ , a m ) . \begin{aligned} &a_m'=\sqrt{J_m(v_m'-v_s)},\quad a_m''=\sqrt{J_m(v_m'-v_e)}, \\ &a_m'=min(a_m',\ a_m),\quad a_m''=min(a_m'',\ a_m). \\ \end{aligned} am′=Jm(vm′−vs),am′′=Jm(vm′−ve),am′=min(am′, am),am′′=min(am′′, am).
正向运动距离： s ′ = 1 2 ( v s + v m ′ ) ( L ′ + n ′ ) T s'=\dfrac{1}{2}(v_s+v_m')(L'+n')T s′=21(vs+vm′)(L′+n′)T，其中 L ′ = a m ′ J m T L'=\dfrac{a_m'}{J_mT} L′=JmTam′， n ′ = v m ′ − v s a m ′ T n'=\dfrac{v_m'-v_s}{a_m'T} n′=am′Tvm′−vs。
反向运动距离： s ′ ′ = 1 2 ( v e + v m ′ ) ( L ′ ′ + n ′ ′ ) T s''=\dfrac{1}{2}(v_e+v_m')(L''+n'')T s′′=21(ve+vm′)(L′′+n′′)T，其中 L ′ ′ = a m ′ ′ J m T L''=\dfrac{a_m''}{J_mT} L′′=JmTam′′， n ′ ′ = v m ′ − v e a m ′ ′ T n''=\dfrac{v_m'-v_e}{a_m''T} n′′=am′′Tvm′−ve。