Matlab四次轨迹采样异常：加速度曲线畸变问题求助

问题背景 #

我在Matlab中生成带有五次收尾（quintic finish）的四次轨迹（quartic trajectory），已知路径点且设置了加速度限制。计算得到轨迹常数后，尝试在t=0s到t=0.04s之间按0.002s的间隔插入采样点，却得到异常畸变的速度和加速度曲线。

现象说明 #

• 蓝色曲线：未采样的原始轨迹加速度曲线
• 橙色曲线：采样后生成的加速度曲线
  

相关代码片段 #

% 轨迹常数计算示例
a0 = ...;  % 四次轨迹常数
a1 = ...;
a2 = ...;
a3 = ...;
a4 = ...;
b0 = ...;  % 五次收尾段轨迹常数
b1 = ...;
b2 = ...;
b3 = ...;
b4 = ...;
b5 = ...;

% 采样逻辑
t_switch = ...;  % 四次段与五次段的切换时间
t_sample = 0:0.002:0.04;
pos_sample = [];

for t = t_sample
    if t < t_switch
        pos = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4;
    else
        t_rel = t - t_switch;
        pos = b0 + b1*t_rel + b2*t_rel^2 + b3*t_rel^3 + b4*t_rel^4 + b5*t_rel^5;
    end
    pos_sample = [pos_sample, pos];
end

% 速度、加速度计算
vel_sample = diff(pos_sample)./0.002;
acc_sample = diff(vel_sample)./0.002;

疑问 #

目前怀疑问题出在采样环节，但无法准确定位错误点，希望能得到排查思路或解决方案。

内容的提问来源于stack exchange，提问作者Mehmet Emre Kılıç