已知线段长度 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<v_0) vm′<v0)。
(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_0
vm′<v0。若不满足要求,则令
v
m
′
=
v
0
v'_m=v_0
vm′=v0。
插补
已知起始速度 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。