超级会员免费看
本文介绍了机器人运动控制中的梯形加减速时间同步，包括位姿时间同步的重要性、梯形时间最优速度规划和时间同步方法。通过实例演示了不同参数下时间同步的解决方案，并总结了该算法的应用场景和潜在问题。
2.2 梯形加减速时间同步
什么是位姿时间同步
因为机器人运动涉及位置和姿态两个量 ， 所以在速度规划和前 瞻控制中还需要考虑二者的 同步 ，即在同一规划时间内机器人末端要满 足轨迹的位置 和姿态的联动 ， 这 就对控制算法提供了更高的要求 。关节型多轴机器人的任务一般是通过控制末端工具的位置和姿态来完成。由于驱动系统饱和限制或任务要求，对机器人的运动速度和加速度有一定的约束。为了分别满足位置和姿态的速度、加速度幅值约束，往往需要分别对位置和姿态进行时间同步的轨迹规划，保证机器人的位置和姿态同时到达期望值。
时间同步的方法有直接规划法，时间缩放法，广义速度法，双样条法。时间缩放法具体可以参考一下这篇文章《轨迹时间同步–时间缩放法》
梯形时间同步规划原理
(1)梯形时间最优速规划
梯形时间最优加减速规划时在给定参数约束条件下，求解时间最短的梯形速度曲线。具体原理和实现参考上一篇博客《梯形加减速》。通过梯形时间最优的速度规划，分别获得了位置加减速曲线和姿态加减速曲线，假设其运动时间分别为
t
p
t_p
tp和
t
o
t_o
to，位姿时间同步后位置和姿态加减速曲线的时间均为
t
s
t_s
ts。由于时间最优速度规划确定的时间是满足给定的加速度和速度边界条件的最小时间，因此，位姿时间同步后的运动时间是均不能小于各自时间最优规划的时间，否则位置或姿态必有其一超出它们的约束条件，因此，时间同步后的运动时间为
t
s
=
max
(
t
p
,
t
o
)
\begin{equation} t_s=\max(t_p,t_o) \end{equation}
ts=max(tp,to)
(2)梯形加减速时间同步
由前面的分析知道 t s t_s ts是不会小于位姿梯加减速的最优时间的，因此，运动时间为t_s时，梯形加减速的速度约束条件是必定满足的，因此梯形加减速时间同步就是在位姿时间最优速度规划后，再把运动时间较小者的运动时间拉长，使得多维度（比如位置和姿态）的运动时间相等。
例如，针对位置和姿态而言，梯形位姿时间同步的问题可描述为：给定位置和姿态的运动幅值 L p , L a L_p,L_a Lp,La、起步速度 v p s , v o s v_{ps},v_{os} vps,vos、最大速度 v p c , v o c v_{pc},v_{oc} vpc,voc 、 末速度 v p e , v o e v_{pe},v_{oe} vpe,voe、加速度 a p , a o a_{p},a_o ap,ao、减速度 d p , d o d_p,d_o dp,do，求解梯形加减速曲线，使得它们的运动时间相等。
有两种方式可以实现，
（1）一种方法是根据给定参数，直接求解位置和姿态的等时间梯形加减速曲线，该方法可以在约束条件下，保持给定的加速度，尽可能以最高速度运行，匀速时间较长。
（2）另一种方法是采用时间缩放法，使位置和姿态运动时间相等，该方法会降低加速度，延迟加速度和减速时间，匀速运行时间较短。
首先，分别对位置和姿态独立进行时间最优的梯形加减速，得到各自的运动时间，按照方程（1）取较大者作为同步时间 t s t_s ts 。然后，对运动时间较小的梯形曲线进行重新规划，使其运动时间为 t s t_s ts。那么就可以保证速度和加速度满足给定约束。那么问题转变为给定：给定运动幅值 L L L、起步速度 v s v_s vs 、末速度 v e v_e ve 、第一段加速度限制 a m a x a_{max} amax 、第三段加速度限制 d m a x d_{max} dmax ，运动间 t s t_s ts, 求解梯形加减速曲线。这里值得强调的是，运动时间 t s t_s ts不是随意给定的，而是在时间最优梯形规划后，按照方程（1）确定。
根据《梯形加减速》可知梯形速度曲线，位置和姿态梯形速度规划已经确定各自的起步速度，终止速度, 运行时间，根据方程（1）确定他们同步的运动时间 t s t_s ts，现在进行时间同步规划就是把其时间较小的梯形曲线，按照时间 t s t_s ts重新规划。同步规划的梯形曲线的起步速度为 v s v_s vs，终止速度为 v e v_e ve, 第一段加速度为 a a a，第三段减速度为 d d d。注意，为了近可能达到给定的速度边界，策略与时间最优梯形规划有一个显著的不同，第一段有可能是减速段，匀速速度可能小于起步速度或终止速度，第三段可能是加速度段。
如下图所示，指定同步时间重新规划，相当于时间拉长，其形状根据同步时间
t
s
t_s
ts变化，方便分类讨论的两个分界点分别为
T
1
T_1
T1,
T
2
T_2
T2，不妨设
v
s
<
v
e
v_s<v_e
vs<ve(若
v
s
≥
v
e
v_s\ge v_e
vs≥ve，公式是类似的，具体参考下面代码) 计算公式如下
T
1
=
{
v
e
−
v
s
a
m
a
x
+
L
−
(
v
e
2
−
v
s
2
)
/
(
2
a
m
a
x
)
v
e
,
v
s
≤
v
e
v
e
−
v
s
−
d
m
a
x
+
L
−
(
v
e
2
−
v
s
2
)
/
(
−
2
d
m
a
x
)
v
s
,
v
s
>
v
e
\begin{equation} T_1= \begin{cases} \frac{v_e-v_s}{a_{max}}+\frac{L-(v_e^2-v_s^2)/(2a_{max})}{v_e},v_s\leq v_e\\ \frac{v_e-v_s}{-d_{max}}+\frac{L-(v_e^2-v_s^2)/(-2d_{max})}{v_s},v_s> v_e\\ \end{cases} \end{equation}
T1={amaxve−vs+veL−(ve2−vs2)/(2amax),vs≤ve−dmaxve−vs+vsL−(ve2−vs2)/(−2dmax),vs>ve
T 2 = { v e − v s d m a x + L − ( v e 2 − v s 2 ) / ( 2 d m a x ) v s , v s ≤ v e v e − v s a m a x + L − ( v s 2 − v e 2 ) / ( 2 a m a x ) v e , v s > v e \begin{equation} T_2= \begin{cases} \frac{v_e-v_s}{d_{max}}+\frac{L-(v_e^2-v_s^2)/(2d_{max})}{v_s},v_s\leq v_e\\ \frac{v_e-v_s}{a_{max}}+\frac{L-(v_s^2-v_e^2)/(2a_{max})}{v_e},v_s> v_e\\ \end{cases} \end{equation} T2={dmaxve−vs+vsL−(ve2−vs2)/(2dmax),vs≤veamaxve−vs+veL−(vs2−ve2)/(2amax),vs>ve
T 1 T_1 T1代表在给定参数下，匀速的速度刚好等于 v e v_e ve时的梯形曲线运动时间。$T_2 代表在给定参数下，匀速的速度刚好等于 代表在给定参数下，匀速的速度刚好等于 代表在给定参数下，匀速的速度刚好等于v_s 的梯形曲线运动时间，对于 的梯形曲线运动时间，对于 的梯形曲线运动时间，对于v_s 或 或 或v_e 接近于 0 时，在数值计算时注意 ( 3 ) , ( 4 ) 分母为零的情形，需要特殊处理，可以把 接近于0时，在数值计算时注意(3),(4)分母为零的情形，需要特殊处理，可以把 接近于0时，在数值计算时注意(3),(4)分母为零的情形，需要特殊处理，可以把T_1,T_2$ 设置为很大的数值。
综上所述，可以按照如下步骤进行同步时间规划。首先计算 T 1 T_1 T1 ， T 2 T_2 T2，然后确定梯形曲线的形状，如图所示，可分为①，②， ③， ④这4种情形，当 t s < T 2 t_s<T_2 ts<T2时，时间同步必定有解，就是①，②这两种情形。当 t s > T 2 t_s>T_2 ts>T2时，不一定有解，分③， ④两种情形。其中④就是下面的case3-2, case3-3的情形，在给定参数下，时间同步无解，这时候通过修改末速度使方程有解。假设匀速速度为 v c v_c vc, 那么有
case1 : 若
t
s
≤
T
1
t_s\le T_1
ts≤T1，梯形三段曲线分别为加速、匀速、减速处理，满足如下方程 。注意某一段的时间可能为0，但如下方程仍然是满足的。
v
c
2
−
v
s
2
2
a
+
v
c
(
t
s
−
v
c
−
v
s
a
−
v
e
−
v
c
d
)
+
v
e
2
−
v
c
2
d
=
L
\begin{equation} \frac{v_c^2-v_s^2}{2a}+v_c(t_s-\frac{v_c-v_s}{a}-\frac{v_e-v_c}{d})+\frac{v_e^2-v_c^2}{d}=L \end{equation}
2avc2−vs2+vc(ts−avc−vs−dve−vc)+dve2−vc2=L
其中，
a
=
a
m
a
x
,
d
=
−
d
m
a
x
a=a_{max},d=-d_{max}
a=amax,d=−dmax, 上式可整理成如下标准的一元二次方程。
(
a
−
d
)
v
c
2
+
(
2
a
d
t
s
+
2
d
v
s
−
2
a
v
e
)
v
c
+
(
a
v
e
2
−
d
v
s
2
−
2
a
d
L
)
=
0
\begin{equation} (a-d)v_c^2+(2adt_s+2dv_s-2av_e)v_c+(av_e^2-dv_s^2-2adL)=0 \end{equation}
(a−d)vc2+(2adts+2dvs−2ave)vc+(ave2−dvs2−2adL)=0
令
A
=
a
−
d
A=a-d
A=a−d,
B
=
2
a
d
t
s
+
2
d
v
s
−
2
a
v
e
B=2adt_s+2dv_s-2av_e
B=2adts+2dvs−2ave,
C
=
a
v
e
2
−
d
v
s
2
−
2
a
d
L
C=av_e^2-dv_s^2-2adL
C=ave2−dvs2−2adL, 选择其中的可行解为
v
c
=
−
B
−
B
2
−
4
A
C
2
A
\begin{equation} v_c=\frac{-B-\sqrt{B^2-4AC}}{2A} \end{equation}
vc=2A−B−B2−4AC
这时各段时间为
{
t
1
=
v
c
−
v
s
a
t
3
=
v
e
−
v
c
d
t
2
=
t
s
−
t
1
−
t
2
\begin{equation} \begin{cases} t_1=\frac{v_c-v_s}{a}\\ t_3=\frac{v_e-v_c}{d}\\ t_2=t_s-t_1-t_2 \end{cases} \end{equation}
⎩
⎨
⎧t1=avc−vst3=dve−vct2=ts−t1−t2
case 2: 若
T
1
<
t
s
≤
T
2
T_1<t_s\le T_2
T1<ts≤T2，梯形三段曲线分别为加速、匀速、加速计算，也就是
a
=
a
m
a
x
,
d
=
d
m
a
x
a=a_{max},d=d_{max}
a=amax,d=dmax，代入（7）（8）式计算得到各段时间。
若
t
s
>
T
2
t_s>T2
ts>T2 ，匀速速度比起步速度和末速度都要低。这时梯形三段曲线分别为减速，匀速，加速，即
a
=
−
a
m
a
x
,
d
=
d
m
a
x
a=-a_{max},d=d_{max}
a=−amax,d=dmax，方程（5)有至少有一个大于等于零的解，即非负解，时间同步才有解。假设方程（5）的两个解为
x
1
,
x
2
x_1,x_2
x1,x2，考虑临界情况，至少有一个零解时，根据韦达定理有
x
1
∗
x
2
=
C
A
=
a
v
e
2
−
d
v
s
2
−
2
a
d
L
a
−
d
=
0
\begin{equation} x_1*x_2=\frac{C}{A}=\frac{av_e^2-dv_s^2-2adL}{a-d}=0 \end{equation}
x1∗x2=AC=a−dave2−dvs2−2adL=0
即
a
v
e
2
−
d
v
s
2
−
2
a
d
L
=
−
a
m
a
x
v
e
2
+
d
m
a
x
v
s
2
+
2
a
m
a
x
d
m
a
x
L
=
0
\begin{equation} av_e^2-dv_s^2-2adL=-a_{max}v_e^2+d_{max}v_s^2+2a_{max}d_{max}L=0 \end{equation}
ave2−dvs2−2adL=−amaxve2+dmaxvs2+2amaxdmaxL=0
进一步整理
L
=
v
s
2
2
a
m
a
x
+
v
e
2
2
d
m
a
x
\begin{equation} L=\frac{v_s^2}{2a_{max} }+ \frac{v_e^2}{2d_{max}} \end{equation}
L=2amaxvs2+2dmaxve2
显然，根据梯形曲线特点，当
L
L
L越大时，匀速速度越大。因此，满足下式有非负解。
L
≥
v
s
2
2
a
m
a
x
+
v
e
2
2
d
m
a
x
\begin{equation} L\ge \frac{v_s^2}{2a_{max} }+ \frac{v_e^2}{2d_{max}} \end{equation}
L≥2amaxvs2+2dmaxve2
case 3: 若
L
≥
v
s
2
2
a
m
a
x
+
v
e
2
2
d
m
a
x
L\ge \frac{v_s^2}{2a_{max} }+ \frac{v_e^2}{2d_{max}}
L≥2amaxvs2+2dmaxve2，方程(5)有非负解（
v
c
≥
0
v_c\ge 0
vc≥0）。 代入（6）（7）式可计算得到各段时间。
case 4: 若 L < v s 2 2 a m a x + v e 2 2 d m a x L<\frac{v_s^2}{2a_{max}}+\frac{v_e^2}{2d_{max}} L<2amaxvs2+2dmaxve2，方程(5)没有非负解，这时只能通过降低末速度使（11）成立，方程有非负解。分两种情况讨论。
case 4-1: L ≥ v s 2 2 a m a x L\ge\frac{v_s^2}{2a_{max}} L≥2amaxvs2，可按照下式修改末速度使方程有非负解。
v
e
=
2
d
m
a
x
(
L
−
v
s
2
2
a
m
a
x
)
\begin{equation} v_e=\sqrt {2d_{max}(L-\frac{v_s^2}{2a_{max}})} \end{equation}
ve=2dmax(L−2amaxvs2)
修正末速度后，按照 代入（6）（7）式可计算得到各段时间。
case 4-2:
L
<
v
s
2
2
a
m
a
x
L< \frac{v_s^2}{2a_{max}}
L<2amaxvs2，修改末速度也无法使方程（5）有非负解，这时降低起步速度，获得非负解。
{
v
s
=
2
a
m
a
x
L
v
c
=
v
e
=
0
t
1
=
v
s
/
a
m
a
x
,
t
2
=
t
s
−
t
1
,
t
3
=
0
\begin{equation} \begin{cases} v_s=\sqrt{2a_{max}L}\\ v_c=v_e=0\\ t_1=v_s/a_{max},\\ t_2=t_s-t_1,\\ t_3=0\\ \end{cases} \end{equation}
⎩
⎨
⎧vs=2amaxLvc=ve=0t1=vs/amax,t2=ts−t1,t3=0
示例演示
case1 : 给定参数vs=10;vmax=50;ve=20;amax=300;dmax=200;L=10;ts=0.4; 时间同步结果如下图
case2:vs=10;vmax=50;ve=20;amax=300;dmax=200;L=10;ts=0.65;时间同步结果如下图
case3 :vs=10;vmax=50;ve=20;amax=300;dmax=200;L=10;ts=1.5;有非负解，时间同步结果如下图
case4-1 :vs=10;vmax=50;ve=20;amax=300;dmax=200;L=0.5;ts=0.2;降低末速度得到非负解，时间同步结果如下图
case4-2 :vs=10;vmax=50;ve=20;amax=300;dmax=200;L=0.1;ts=0.1;降低起步速度得到非负解，时间同步结果如下图
小结
（1）梯形时间同步可以应用在五轴数控、机器人末端点到点，或者连续轨迹的位置和姿态的同步，也可以用于任意多个轴间同时启动同时到达的速度规划。
（2）梯形时间同步分两步进行，第一步是位置和姿态（或各轴）分别进行时间最优的时间规划，得到可行的速度边界和运动时间（用户给定的速度边界不一定能到达，有时需要修改），第二步取最大的时间进行时间同步规划，根据参数进行分类讨论处理。
（3）该算法的一个缺陷是，若在速度前瞻中，目标点在最后一轮的向后调整中（速度前瞻包含向前回溯和向后调整两个过程），出现case3-3的情形，改变了起步速度，直接送入插补器，会使的连续轨迹的速度出现跳变。若下一轮的向前回溯中处理了该点，那么就可以把速度跳变消除。
示例代码
matlab测试函数代码如下
clc
clear
close('all')
vs=10;
vmax=50;
ve=20;
amax=300;
dmax=200;
L=0.1;
ts=0.1;%在时间最优规划基础上，时间拉长
Ts=0.001;
%%
%时间最优梯形规划
tp=trapProfile(vs,vmax,ve,amax,dmax,L);
k=1;
ti=0;
for t=0:Ts:tp.t
ti(k,1)=(k-1)*Ts;
pt(k,1)= trapProfilePos(tp,t);
vt(k,1)=trapProfileVel(tp,t);
at(k,1)= trapProfileAcc(tp,t);
k=k+1;
end
figure
fig1=subplot(3,1,1);
ylabel('position')
grid on;
hold on;
fig2=subplot(3,1,2);
ylabel('velocity')
grid on;
hold on;
fig3=subplot(3,1,3);
ylabel('acceleration')
xlabel('time');
grid on;
hold on;
c11=plot(fig1,ti,pt,'-r.');
c21=plot(fig2,ti,vt,'-r.');
c31=plot(fig3,ti,at,'-r.');
%%
%梯形时间同步
global time;
time=tp.t;
tp2=calcSyncTrapProfile(tp.vs,tp.ve,amax,dmax,L,ts);
k=1;
ti=0;pt=0;vt=0;at=0;
for t=0:Ts:ts
ti(k,1)=(k-1)*Ts;
pt(k,1)= trapProfilePos(tp2,t);
vt(k,1)= trapProfileVel(tp2,t);
at(k,1)= trapProfileAcc(tp2,t);
k=k+1;
end
c12=plot(fig1,ti,pt,'-b.');
c22=plot(fig2,ti,vt,'-b.');
c32=plot(fig3,ti,at,'-b.');
legend([c11,c12],'时间最优','时间延长')
legend([c21,c22],'时间最优','时间延长')
legend([c31,c32],'时间最优','时间延长')
梯形时间最优速度规划函数如下
function tp = trapProfile(vs, vmax, ve, amax,dmax, L)
acc=amax;
dec=-dmax;
%起步速度和终止速度不能大于匀速速度
if (vs > vmax)
vs=vmax;
end
if (ve > vmax)
ve = vmax;
end
%4种情形
vf = sqrt((-2.0 * acc * dec * L - vs * vs * dec + ve * ve * acc) / (acc - dec));
if (vf > vmax)%有匀速段
vc = vmax;
t1 = (vc - vs) / acc;
t3 = (ve - vc) / dec;
L1 = vs * t1 + 0.5 * acc * t1 * t1;
L3 = vc * t3 + 0.5 * dec * t3 * t3;
L2 = L - L1 - L3;
t2 = L2 / vc;
t = t1 + t2 + t3;
else%没有匀速段
if (vs < vf && vf < ve)%只有加速段
ve = sqrt(vs * vs + 2 * acc * L);
vc = ve;
t1 = (ve - vs) / acc;
t2 = 0;
t3 = 0;
L1 = vs * t1 + 0.5 * acc * t1 * t1;
L2 = 0;
L3 = 0;
t = t1;
elseif (ve < vf && vf < vs)%只有减速段
ve = sqrt(vs * vs + 2 * dec * L);
vc = vs;
vc = vs;
t1 = 0;
t2 = 0;
t3 = (ve - vs) / dec;
L1 = 0;
L2 = 0;
L3 = vc * t3 + 0.5 * dec * t3 * t3;
t = t3;
else%存在加速段和减速段
vc = vf;
t1 = (vc - vs) / acc;
t2 = 0;
t3 = (ve - vc) / dec;
L1 = vs * t1 + 0.5 * acc * t1 * t1;
L2 = 0;
L3 = vc * t3 + 0.5 * dec * t3 * t3;
t = t1 + t3;
end
end
tp.vs=vs;
tp.vc=vc;
tp.ve=ve;
tp.acc=amax;
tp.dec=-dmax;
tp.t1=t1;
tp.t2=t2;
tp.t3=t3;
tp.L1=L1;
tp.L2=L2;
tp.L3=L3;
tp.L=L;
tp.t=t;
end
梯形时间同步规划算法函数如下
%梯形时间同步
function param=calcSyncTrapProfile(vs,ve,amax,dmax,L,ts)
t1=0;t2=0;t3=0;
L1=0;L2=0;L3=0;
a=0;d=0;vc=0;
%计算T1，T2
if(vs<ve)
if(ve<1.0e-5)
T1=1.0e10;
T2=1.0e10;
elseif(vs<1.0e-5)
T1=(ve-vs)/amax+(L-(ve*ve-vs*vs)/(2.0*amax))/ve;
T2=1.0e10;
else
T1=(ve-vs)/amax+(L-(ve*ve-vs*vs)/(2.0*amax))/ve;
T2=(ve-vs)/(dmax)+(L-(ve*ve-vs*vs)/(2.0*dmax))/vs;
end
else
%vs>=ve
if(vs<1.0e-5)
T1=1.0e10;
T2=1.0e10;
elseif(ve<1.0e-5)
T1=(ve-vs)/(-dmax)+(L-(ve*ve-vs*vs)/(-2.0*dmax))/vs;
T2=1.0e10;
else
T1=(ve-vs)/(-dmax)+(L-(ve*ve-vs*vs)/(-2.0*dmax))/vs;
T2=(ve-vs)/(-amax)+(L-(ve*ve-vs*vs)/(-2.0*amax))/ve;
end
end
%%
if(ts<T1)
%加速，匀速，减速
a=amax;
d=-dmax;
A=a-d;
B=2*a*d*ts+2*d*vs-2*a*ve;
C=a*ve*ve-d*vs*vs-2*a*d*L;
if(abs(A)>1.0e-3)
vc=(-B-sqrt(abs(B*B-4*A*C)))/(2*A);
else
vc=-C/B;
end
t1=(vc-vs)/a;
t3=(ve-vc)/d;
t2=ts-t1-t3;
L1=vs*t1+0.5*a*t1*t1;
L2=vc*t2;
L3=vc*t3+0.5*d*t3*t3;
elseif(ts<T2)
if(vs<ve)
%加速，匀速，加速
a=amax;
d=amax;
A=a-d;
B=2*a*d*ts+2*d*vs-2*a*ve;
C=a*ve*ve-d*vs*vs-2*a*d*L;
if(abs(A)>1.0e-3)
vc=(-B-sqrt(abs(B*B-4*A*C)))/(2*A);
else
vc=-C/B;
end
t1=(vc-vs)/a;
t3=(ve-vc)/d;
t2=ts-t1-t3;
L1=vs*t1+0.5*a*t1*t1;
L2=vc*t2;
L3=vc*t3+0.5*d*t3*t3;
else
%减速，匀速，减速
a=-amax;
d=-dmax;
A=a-d;
B=2*a*d*ts+2*d*vs-2*a*ve;
C=a*ve*ve-d*vs*vs-2*a*d*L;
if(abs(A)>1.0e-3)
vc=(-B-sqrt(abs(B*B-4*A*C)))/(2*A);
else
vc=-C/B;
end
t1=(vc-vs)/a;
t3=(ve-vc)/d;
t2=ts-t1-t3;
L1=vs*t1+0.5*a*t1*t1;
L2=vc*t2;
L3=vc*t3+0.5*d*t3*t3;
end
else
%ts>T2
%减速，匀速，加速
a=-amax;
d=dmax;
A=a-d;
B=2*a*d*ts+2*d*vs-2*a*ve;
C=a*ve*ve-d*vs*vs-2*a*d*L;
if(L>ve*ve/(2*dmax)+vs*vs/(2*amax))
%有非负解
if(abs(A)>1.0e-3)
vc=(-B-sqrt(abs(B*B-4*A*C)))/(2*A);
else
vc=-C/B;
end
t1=(vc-vs)/a;
t3=(ve-vc)/d;
t2=ts-t1-t3;
L1=vs*t1+0.5*a*t1*t1;
L2=vc*t2;
L3=vc*t3+0.5*d*t3*t3;
elseif(L>vs*vs/(2*amax))
%无非负解，修改末速度
ve=sqrt(abs(2*dmax*(L-vs*vs/(2*amax))));
B=2*a*d*ts+2*d*vs-2*a*ve;
C=a*ve*ve-d*vs*vs-2*a*d*L;
if(abs(A)>1.0e-3)
vc=(-B-sqrt(abs(B*B-4*A*C)))/(2*A);
else
vc=-C/B;
end
t1=(vc-vs)/a;
t3=(ve-vc)/d;
t2=ts-t1-t3;
L1=vs*t1+0.5*a*t1*t1;
L2=vc*t2;
L3=vc*t3+0.5*d*t3*t3;
else
%修改末速度也无解,降低起步速度
vs=sqrt(2*amax*L);
vc=0;
ve=0;
t1=vs/amax;
t2=ts-t1;
t3=0;
L1=L;
L2=0;
L3=0;
end
end
%%
param.t1=t1;
param.t2=t2;
param.t3=t3;
param.vs=vs;
param.vc=vc;
param.ve=ve;
param.L1=L1;
param.L2=L2;
param.L3=L3;
param.L=L;
param.acc=a;
param.dec=d;
param.t=ts;
end
位置，速度和加速度时间采样函数如下
function Lt= trapProfilePos(tp,t)
tmp = 0.0;
Lt = 0.0;
if (tp.L < 1.0E-5)
Lt = 0.0;
elseif (t < tp.t1)
Lt = tp.vs * t + 0.5 * tp.acc * t * t;
elseif (t < tp.t1 + tp.t2)
Lt = tp.L1 + tp.vc * (t - tp.t1);
else
tmp = t - tp.t1 - tp.t2;
Lt = tp.L1 + tp.L2 + tp.vc * tmp + 0.5 * tp.dec * tmp * tmp;
end
end
function vt= trapProfileVel(tp, t)
vt = 0.0;
if (tp.L < 1.0E-5)
vt = 0.0;
elseif (t < tp.t1)
vt = tp.vs + tp.acc * t;
elseif (t < tp.t1 + tp.t2)
vt = tp.vc;
else
vt = tp.vc + tp.dec * (t - tp.t1 - tp.t2);
end
end
function acc= trapProfileAcc(tp,t)
if (t < 0.0)
acc= 0.0;
elseif (t < tp.t1)
acc=tp.acc;
elseif (t < tp.t1 + tp.t2)
acc=0.0;
else
acc=tp.dec;
end
function vt= trapProfileVel(tp, t)
vt = 0.0;
if (tp.L < 1.0E-5)
vt = 0.0;
elseif (t < tp.t1)
vt = tp.vs + tp.acc * t;
elseif (t < tp.t1 + tp.t2)
vt = tp.vc;
else
vt = tp.vc + tp.dec * (t - tp.t1 - tp.t2);
end
end
function acc= trapProfileAcc(tp,t)
if (t < 0.0)
acc= 0.0;
elseif (t < tp.t1)
acc=tp.acc;
elseif (t < tp.t1 + tp.t2)
acc=0.0;
else
acc=tp.dec;
end
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
