牛头刨床运动学动力学matlab程序

其机构运动简图如下

采用矩阵法分析机构的运动学，对每个构建单独分析求出未知力系数矩阵来分析动力学。其matlab代码放置文章最后，其可以做出 机构位移、速度、加速度、未知力的图像以及机构的运动简图。

clc
clear
% Author: Tong
% Date:2024/8/29

h = 360; H = 845.982;l1 = 127.66; l3 = 818.493; l4 = 95.963;    % 结构参数      动力学参数在96行
w1 = -2*pi*50/60;        % 杆1角速度    负为顺时针，正为逆时针

figure(2)  % 位移图
subplot(2, 2, 1);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('xc');    
subplot(2, 2, 2);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('sita3');
subplot(2, 2, 3);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('sita4'); 
subplot(2, 2, 4);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('s3'); 
figure(3)  %速度图
subplot(2, 2, 1);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Vc');      
subplot(2, 2, 2);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('w3'); 
subplot(2, 2, 3);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('w4');  
subplot(2, 2, 4);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Vs3');   
figure(4)  %加速度图
subplot(2, 2, 1);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Ac');  
subplot(2, 2, 2);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('alpha3');  
subplot(2, 2, 3);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('alpha4');  
subplot(2, 2, 4);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('As3') ;   
figure(5)   %受力图
subplot(3, 3, 1);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Md');  
subplot(3, 3, 2);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr16');  
subplot(3, 3, 3);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr12');  
subplot(3, 3, 4);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr23');  
subplot(3, 3, 5);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr36');  
subplot(3, 3, 6);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr34');  
subplot(3, 3, 7);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr45');  
subplot(3, 3, 8);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('Fr56'); 
subplot(3, 3, 9);hold on;xlim([0 360]);xlabel('sita1 (degrees)');ylabel('M5'); 

for sita = 0:-1:-360
    %运动学计算
    sita2 = -20.7697200734923 + sita;
    sita1 = sita2 * pi / 180;
    
    s3 = sqrt(l1^2+h^2+2*h*l1*sin(sita1));
    sita3 = acos(l1*cos(sita1)/s3);
    
    sita4 = pi - asin((H-l3*sin(sita3))/l4);
    xc = l3 * cos(sita3) + l4 * cos(sita4);     %计算位置正解
    
    O2 = [0,h];
    A1 = [O2(1)+l1*cos(sita1),O2(2)+l1*sin(sita1)];
    B1 = [l3*cos(sita3),l3*sin(sita3)];
    C1 = [B1(1)+l4*cos(sita4),B1(2)+l4*sin(sita4)];
    
    figure(1)
    hold on;axis equal;    
    plot([O2(1),A1(1)],[O2(2),A1(2)]);plot([0,A1(1)],[0,A1(2)]);
    plot([B1(1),C1(1)],[B1(2),C1(2)]);plot([B1(1),A1(1)],[B1(2),A1(2)]);
   
    
    A = [cos(sita3), -s3*sin(sita3), 0, 0;
         sin(sita3),  s3*cos(sita3), 0, 0;
         0, -l3*sin(sita3), -l4*sin(sita4),-1;
         0,  l3*cos(sita3),  l4*cos(sita4), 0];
    C = [-l1*sin(sita1), l1*cos(sita1), 0,0]';
    Vx = w1 * pinv(A) * C;                      % 四个速度
    Vs3 = Vx(1); w3 = Vx(2); w4 = Vx(3); Vc = Vx(4);
    B = [-w3*sin(sita3), -Vs3*sin(sita3)-s3*w3*cos(sita3), 0,                0;
          w3*cos(sita3),  Vs3*cos(sita3)-s3*w3*sin(sita3), 0,                0;
          0            , -l3*w3*cos(sita3)               ,-l4*w4*cos(sita4), 0;
          0            , -l3*w3*sin(sita3)               ,-l4*w4*sin(sita4), 0];
    
    D = [-l1*w1*cos(sita1), -l1*w1*sin(sita1), 0, 0]';
    Ax = pinv(A) * (w1*D - B*Vx);               % 四个加速度
    Ac = Ax(4);alpha3 = Ax(2);alpha4 = Ax(3);As3 = Ax(1);
    
    figure(2) 
    subplot(2, 2, 1);plot(-sita, xc, '.');    
    subplot(2, 2, 2);plot(-sita, sita3*180/pi, '.');
    subplot(2, 2, 3);plot(-sita, sita4*180/pi, '.');     
    subplot(2, 2, 4);plot(-sita, s3, '.'); 
    figure(3)  
    subplot(2, 2, 1);plot(-sita, Vc, '.');       
    subplot(2, 2, 2);plot(-sita, w3, '.');      
    subplot(2, 2, 3);plot(-sita, w4, '.');   
    subplot(2, 2, 4);plot(-sita, Vs3, '.');
    figure(4)  
    subplot(2, 2, 1);plot(-sita, Ac, '.');     
    subplot(2, 2, 2);plot(-sita, alpha3, '.');    
    subplot(2, 2, 3);plot(-sita, alpha4, '.');  
    subplot(2, 2, 4);plot(-sita, As3, '.'); 
    
    % 动力学计算
    xa = A1(1);ya = A1(2);xb = B1(1);yb = B1(2);    %计算质心坐标，以及各点坐标
    xc = C1(1);yc = C1(2);xo2 = O2(1);yo2 = O2(2);
    xo4 = 0;yo4 = 0;
    xs3 = B1(1)/2;ys3 = B1(2)/2;
    xs4 = (xb+xc)/2;ys4 = (yb+yc)/2;
    xs5 = xc; % 设c点为杆5质心

    Fpr = 9000; %刨刀阻力
    m3 = 220/9.8;m4 = 10;m5 = 800/9.8;J3 = 1.2;J4 = 1.2;    %动力学参数
    F3x = -m3*alpha3*l3/2*cos(sita3)/1000; F3y = -m3*alpha3*l3/2*sin(sita3)/1000; % 计算惯性力和惯性力矩
    F4x = -m4*alpha4*l4/2*cos(sita4)/1000; F4y = -m4*alpha4*l4/2*sin(sita4)/1000;
    F5x = -m5*Ac/100;
    Mf3 = -J3*alpha3;
    Mf4 = -J4*alpha4;
    G3 = m3 * 9.8;G4 = m4*9.8;G5 = m5*9.8;
   % C1为系数矩阵 Fr为未知力矩阵
    C1 = [0, 1,0,1     ,0     ,         0,  0       ,0,0,0,0,0,0,0,0;
          0, 0,1,0     ,1     ,         0,  0       ,0,0,0,0,0,0,0,0;
          -1,0,0,yo2-ya,xa-xo2,         0,  0       ,0,0,0,0,0,0,0,0;
          0, 0,0,1     , 0    ,        -1,  0       ,0,0,0,0,0,0,0,0;
          0, 0,0,0     , 1    ,         0,-1        ,0,0,0,0,0,0,0,0;
          0, 0,0,0     , 0    ,cos(sita3),sin(sita3),0,0,0,0,0,0,0,0;
          0,0,0,0,0,-1, 0,1,0,1,0,0,0,0,0;
          0,0,0,0,0, 0,-1,0,1,0,1,0,0,0,0;
          0,0,0,0,0,ya-ys3,xs3-xa,ys3-yo4,xo4-xs3,ys3-yb,xb-xs3,0,0,0,0;
          0,0,0,0,0,0,0,0,0,1,0,-1,0,0,0;
          0,0,0,0,0,0,0,0,0,0,1,0,-1,0,0;
          0,0,0,0,0,0,0,0,0,ys4-yb,xb-xs4,yc-ys4,xs4-xc,0,0;
          0,0,0,0,0,0,0,0,0,0,0,1,0,0,0;
          0,0,0,0,0,0,0,0,0,0,0,0,1,-1,0;
          0,0,0,0,0,0,0,0,0,0,0,0,xs5,0,1];

        F = [0,0,0,0,0,0,F3x,F3y-G3,Mf3,-F4x,-F4y-G4,-Mf4,-F5x-Fpr,G5,0]'; %已知的惯性力和惯性力矩       
        Fr = pinv(C1)*F;
        
        Md = Fr(1);
        Fr16 = sqrt( Fr(2)^2 + Fr(3)^2 );
        Fr12 = sqrt( Fr(4)^2 + Fr(5)^2 );
        Fr23 = sqrt( Fr(6)^2 + Fr(7)^2 );
        Fr36 = sqrt( Fr(8)^2 + Fr(9)^2 );
        Fr34 = sqrt( Fr(10)^2 + Fr(11)^2 );
        Fr45 = sqrt( Fr(12)^2 + Fr(13)^2 );
        Fr56 = Fr(14);
        M5 = Fr(15);
        
           
        Frr = [Md,Fr16,Fr12,Fr23,Fr36,Fr34,Fr45,Fr56,M5]';  % Frr是计算 未知力的合力
        figure(5)
        subplot(3, 3, 1);plot(-sita, Frr(1), '.'); 
        subplot(3, 3, 2);plot(-sita, Frr(2), '.'); 
        subplot(3, 3, 3);plot(-sita, Frr(3), '.'); 
        subplot(3, 3, 4);plot(-sita, Frr(4), '.'); 
        subplot(3, 3, 5);plot(-sita, Frr(5), '.'); 
        subplot(3, 3, 6);plot(-sita, Frr(6), '.'); 
        subplot(3, 3, 7);plot(-sita, Frr(7), '.'); 
        subplot(3, 3, 8);plot(-sita, Frr(8), '.'); 
        subplot(3, 3, 9);plot(-sita, Frr(9), '.'); 
end