五杆机构运动学(6)

轨迹多项式

t=0时

 

 t=1时

带入原多项式，求解系数

 

解得

 

matlab实现

总时间T=0.5

对X轴轨迹参数设置

%X轴轨迹规划
x_T = 220; % x轴总位移（mm）(-70,150)

%%半程的运动规划%%%
x_a = 50; % x轴加速段位移（mm）
x_c = x_T/2 - x_a; % x轴匀速段位移（mm）
t_h = T/2; % 半程时间（加速+匀速）
dxt_max = 1/t_h*(35/16*x_a + x_c);% x轴最大速度即为匀速段的速度（mm/s），35/16是工程实践中通过调整多项式轨迹的积分系数和运动参数得到的经验值，目的是平衡加速段和匀速段的位移与时间分配
t_c = x_c/dxt_max; % 匀速段时间（秒）；匀速位移/匀速速度
t_a = t_h - t_c; % 加速段时间（秒）
St = 1/t_a/2; % 时间缩放因子（用于多项式归一化）

 生成X轴轨迹

% 生成x轴轨迹
i = 0; 
for t = 0 : 0.001 : T
    i = i + 1; 
    if t <= t_a        % 加速段：4-5-6-7多项式轨迹

        t1 = St*t; % 归一化时间
        pt = -20*t1^7 + 70*t1^6 - 84*t1^5 + 35*t1^4; % 轨迹多项式
        dpt = -140*t1^6 + 420*t1^5 - 420*t1^4 + 140*t1^3; % 一阶导数（速度）
        ddpt = -840*t1^5 + 2100*t1^4 - 1680*t1^3 + 420*t1^2; % 二阶导数（加速度）
        % 计算实际物理量,x的位移、速度、加速度
        xt(i) = 150 - 2*x_a*pt; % 初始位置150mm，双向对称运动
        dxt(i) = -x_a/t_a*dpt; % 速度计算
        d2xt(i) = -0.5*x_a/t_a^2*ddpt;% 加速度计算

    elseif t > t_a && t <= T - t_a% 匀速段

        xt(i) = 150 - x_a - dxt_max * (t - t_a); % 线性运动
        dxt(i) =  - dxt_max; % 恒定速度
        d2xt(i) = 0;% 加速度为零
        
    else% 减速段（对称加速段的多项式）
       
        t2 = St*(T - t); % 反向归一化时间
        pt = -20*t2^7 + 70*t2^6 - 84*t2^5 + 35*t2^4; 
        dpt = -140*t2^6 + 420*t2^5 - 420*t2^4 + 140*t2^3; 
        ddpt = -840*t2^5 + 2100*t2^4 - 1680*t2^3 + 420*t2^2;
        % 计算实际物理量（注意符号对称性）
        xt(i) = -70 + 2*x_a*pt; % 终点位置-70mm
        dxt(i) = -x_a/t_a*dpt; 
        d2xt(i) = 0.5*x_a/t_a^2*ddpt;

    end
end

 对Y轴轨迹参数设置

y_T = 100; % y轴总位移（mm）
y_a = 15; % y轴加速段位移（mm）
y_c = y_T/2 - y_a; % y轴匀速段位移（mm）
t_h = T/4; % 半程时间（仅前半段规划，后半段镜像）
dyt_max = 1/t_h*(35/16*y_a + y_c);% y轴最大速度（mm/s）
t_c = y_c/dyt_max; % 匀速段时间
t_a = t_h - t_c; % 加速段时间
St = 1/t_a/2; % 时间缩放因子

 生成Y轴轨迹

% 生成前半段轨迹（0到T/2）
i = 0; 
for t = 0 : 0.001 : T/2
    i = i + 1; 
    if t <= t_a% 加速段
        
        t1 = St*t; 
        pt = -20*t1^7 + 70*t1^6 - 84*t1^5 + 35*t1^4; 
        dpt = -140*t1^6 + 420*t1^5 - 420*t1^4 + 140*t1^3; 
        ddpt = -840*t1^5 + 2100*t1^4 - 1680*t1^3 + 420*t1^2; 
        
        yt(i) = 170 - 2*y_a*pt; % 初始位置170mm
        dyt(i) = -y_a/t_a*dpt; 
        d2yt(i) = -0.5*y_a/t_a^2*ddpt;
        
    elseif t > t_a && t <= T/2 - t_a% 匀速段
        
        yt(i) = 170 - y_a - dyt_max * (t - t_a); 
        dyt(i) =  - dyt_max; 
        d2yt(i) = 0;
        
    else% 减速段
        
        t2 = St*(T/2 - t); 
        pt = -20*t2^7 + 70*t2^6 - 84*t2^5 + 35*t2^4; 
        dpt = -140*t2^6 + 420*t2^5 - 420*t2^4 + 140*t2^3; 
        ddpt = -840*t2^5 + 2100*t2^4 - 1680*t2^3 + 420*t2^2;
        
        yt(i) = 70 + 2*y_a*pt; % 中间点70mm
        dyt(i) = -y_a/t_a*dpt; 
        d2yt(i) = 0.5*y_a/t_a^2*ddpt; 
        
    end
    
end

% 镜像生成后半段轨迹（T/2到T）
[~, temp] = size(yt); 

for n = 1 : temp - 1
    yt(i + n) = yt(temp - n); % 位置镜像
    dyt(i + n) = -dyt(temp - n); % 速度反向
    d2yt(i + n) = d2yt(temp - n); % 加速度保持符号
end