四足机器人|机器狗|仿生机器人|多足机器人|MATLAB动画仿真|Simulink动画仿真

四足机器人|机器狗|仿生机器人|多足机器人|MATLAB动画仿真|Simulink动画仿真

四足机器人的连杆模型，利用机器人工具箱，行走规划是用的CPG，详情见
https://blog.csdn.net/Ezekiel_Mok/article/details/110914854

%% compiled by Ezekiel according to robot modeling and control
%% 20xx/xx/x
% clear;
% clc;
% clear L
Len_tool=0;
du=pi/180;
L1 = 0.08; L2 = 0.2; L3 = 0.2;L = 0.53; W = 0.35; C=0.05;
P=[
L/2 -L/2 L/2 -L/2;
W/2 W/2 -W/2 -W/2;
-0.30 -0.30 -0.30 -0.30];
F=[
L/2 -L/2 L/2 -L/2;
W/2 W/2 -W/2 -W/2];
Q=zeros(3,4);
plotopt = {'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes', 'delay', 0};
s = 'Rx(q1).Tz(L1).Ry(q2).Tx(L2).Ry(q3).Tx(L3)';
dh = DHFactor(s);
dh.command('leg') ;
leg = eval( dh.command('leg') );
leg.offset=[0;pi-pi/6;+pi/3];
legs(1) = SerialLink(leg, 'name', 'leg1','base', transl(L/2, W/2, 0)*trotx(pi)*troty(pi/2)); 
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', transl(-L/2, W/2, 0)*trotx(pi)*trotz(pi)*troty(pi/2));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', transl(L/2, -W/2, 0)*trotx(pi)*troty(pi/2));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', transl(-L/2, -W/2, 0)*trotx(pi)*trotz(pi)*troty(pi/2));
%建立四足机器人模型
%模型
hold on
% 身体
xdata=[L/2;
       -L/2;
      -L/2;
       L/2];
ydata=[W/2;
       W/2;
       -W/2;
       -W/2];
zdata=[0;
       0;
       0;
       0];
zdata1=zdata+[C,C,C,C]';
xdata2=[-L/2,-L/2,-L/2,-L/2]';
ydata2=[-W/2,-W/2,W/2,W/2]';
zdata2=[0,C,C,0]';
xdata4=[-L/2,-L/2,L/2,L/2]';
ydata4=[-W/2,-W/2,-W/2,-W/2]';
zdata4=[0,C,C,0]';
xdata3=-xdata2;
ydata5=-ydata4;
patch(xdata,ydata,zdata,'blue');
patch(xdata,ydata,zdata1,'blue');
patch(xdata2,ydata2,zdata2,'blue');
patch(xdata3,ydata2,zdata2,'blue');
patch(xdata4,ydata4,zdata4,'blue');
patch(xdata4,ydata5,zdata4,'blue');
% %%初始位置
legs(4).plot([0*du,(20)*du,0*du], plotopt{:});
legs(2).plot([0*du,(20)*du,0*du], plotopt{:});
legs(1).plot([0*du,(20)*du,0*du], plotopt{:});
legs(3).plot([0*du,(20)*du,0*du], plotopt{:});
% %%
% for A=90:-5:50
% pause(0.2);
% view(-A,0+A);
% end
% view(-30,60);
view(-0,20);
%设置髋关节与膝关节幅值，侧摆关节动作
Ahip=15*du;
Akne=10*du;
draw_LF=zeros(Pon_num,3);
draw_RF=zeros(Pon_num,3);
draw_RB=zeros(Pon_num,3);
draw_LB=zeros(Pon_num,3);
for i=1:1:Pon_num
 %LF关节映射函数
 draw_LF(i,2)=Ahip*leg_h_Point_x(i,1);
 draw_LF(i,3)=Akne*leg_k_Point_x(i,1);
 %RF关节映射函数
 draw_RF(i,2)=Ahip*leg_h_Point_x(i,2);
 draw_RF(i,3)=Akne*leg_k_Point_x(i,2);
 %RB关节映射函数
 draw_RB(i,2)=Ahip*leg_h_Point_x(i,3);
 draw_RB(i,3)=Akne*leg_k_Point_x(i,3);
 %LB关节映射函数
 draw_LB(i,2)=Ahip*leg_h_Point_x(i,4);
 draw_LB(i,3)=Akne*leg_k_Point_x(i,4);
end
%% 启动行走动画
        for i=1:1:Pon_num
            legs(4).animate( draw_LF(i,:));
            legs(2).animate( draw_RF(i,:));
            legs(1).animate( draw_RB(i,:));
            legs(3).animate( draw_LB(i,:));
            drawnow
        end