动力学

行走动力学

平面被动行走机器人共有四个自由度，分别是两个夹角和两个绝对坐标，可以使用拉格朗日方程得到被动行走机器人的动力学方程。需要注意的是，在行走过程中，只有一只腿撑住地面，因此只需要求解一条腿的雅可比矩阵

碰撞动力学

当空中摆动腿和地面接触时，会产生一个顺势冲量，此时，行走动力学方程失效，可以将动力学方程对这个瞬间做积分，而后求解方程，得到碰撞后的解。

代码

动力学推导部分

本部分使用符号推导，得到被动行走机器人的行走动力学模型和碰撞动力学模型。

function my_drive
clc 
clear
close all

syms M m I real; % M：Hip的质量，m：腿的质量,I:绕质心转动惯量
syms c l real;
syms gam g real;
syms theta1 theta2 real;
syms theta1dot theta2dot real;
syms theta1ddot theta2ddot real;
syms theta1_n theta2_n real;
syms theta1dot_n theta2dot_n real;
syms x y real;
syms xdot ydot real;
syms xddot yddot real;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%            坐标转换            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G11 = [(l-c);0;1];
G22 = [c;0;1];
C22 = [l;0;1];
H11 = [l;0;1];
O01 = [x;y];
O12 = [l;0];
% 变换矩阵
R01 = simplify([cos(pi/2+theta1-gam) -sin(pi/2+theta1-gam); ...
               sin(pi/2+theta1-gam) cos(pi/2+theta1-gam)]);
R12 = simplify([cos(pi+theta2) -sin(pi+theta2); ...
               sin(pi+theta2) cos(pi+theta2)]);

H01 = [R01 O01;0 0 1];
H12 = [R12 O12;0 0 1];
% 坐标变换
R_G1 = H01*G11;
r_G1 = R_G1(1:2);
x_G1 = r_G1(1); y_G1 = r_G1(2);

R_H = H01*H11;
r_H = R_H(1:2);
x_H = r_H(1); y_H = r_H(2);

R_G2 = H01*H12*G22;
r_G2 = R_G2(1:2);
x_G2 = r_G2(1); y_G2 = r_G2(2);

R_C2 = H01*H12*C22;
r_C2 = R_C2(1:2);
x_C2 = r_C2(1); y_C2 = r_C2(2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%            速度变换            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

v_H = jacobian(r_H,[x y theta1 theta2])*[xdot ydot theta1dot theta2dot]';
v_G1 = jacobian(r_G1,[x y theta1 theta2])*[xdot ydot theta1dot theta2dot]';
v_G2 = jacobian(r_G2,[x y theta1 theta2])*[xdot ydot theta1dot theta2dot]';


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%          拉格朗日方程           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T = 0.5*(simplify((M*dot(v_H,v_H)+m*dot(v_G1,v_G1)+m*dot(v_G2,v_G2) + ...
         I*dot(theta1dot,theta1dot)+I*dot(theta1dot + theta2dot,theta1dot + theta2dot))));
V = simplify(m*g*(y_G1 + y_G2) + M*g*y_H);
L = T - V;

q = [x y theta1 theta2];
qdot = [xdot ydot theta1dot theta2dot];
qddot = [xddot yddot theta1ddot theta2ddot];


for i = 1:4
    dLdqdot(i) = diff(L,qdot(i));
    % 链式法则求解对时间的导数
    ddt_dLdqdot(i) = diff(dLdqdot(i),q(1))*qdot(1) + diff(dLdqdot(i),qdot(1)) * qddot(1) + ...
                     diff(dLdqdot(i),q(2))*qdot(2) + diff(dLdqdot(i),qdot(2)) * qddot(2) + ...
                     diff(dLdqdot(i),q(3))*qdot(3) + diff(dLdqdot(i),qdot(3)) * qddot(3) + ...
                     diff(dLdqdot(i),q(4))*qdot(4) + diff(dLdqdot(i),qdot(4)) * qddot(4);
    dLdq(i) = diff(L,q(i));
    Eom(i) = simplify(ddt_dLdqdot(i) - dLdq(i));
end

%%%% 分别计算矩阵 %%%%
% M(q)qddot = B(q,qdot) +JPc
M_ss = jacobian(Eom,[xddot yddot theta1ddot theta2ddot]); % 4*4 矩阵
b_ss(1,1) = -subs(Eom(1),qddot,[0 0 0 0]);
b_ss(2,1) = -subs(Eom(2),qddot,[0 0 0 0]);
b_ss(3,1) = -subs(Eom(3),qddot,[0 0 0 0]);
b_ss(4,1) = -subs(Eom(4),qddot,[0 0 0 0]);

disp('以下是本程序使用符号推导得到的内容');
disp('1，摆动相');

% 摆动相过程中，x,y并不运动，不参与动力学方程，只要M_ss右下角的2*2方阵和B的(3:4)
disp(['A11 = ',char(simplify(M_ss(3,3))),';']);
disp(['A12 = ',char(simplify(M_ss(3,4))),';']);
disp(['A21 = ',char(simplify(M_ss(4,3))),';']);
disp(['A22 = ',char(simplify(M_ss(4,4))),';']);
disp('A_ss = [A11 A12;A21 A22]');
disp(' ');
disp(['b1 = ',char(simplify(b_ss(3,1))),';']);
disp(['b2 = ',char(simplify(b_ss(4,1))),';']);
disp(['b_ss = [b1;b2]',';']);
disp(' ');
disp(['qddot = M_ssb_ss',';']);
disp(' ');
disp(' ');

% 两脚同时触地
J_newleg = jacobian(r_C2,q);% 2*4的矩阵
J_num_newleg = subs(J_newleg,[theta1 theta2],[theta1_n theta2_n]);
A_num_newleg = subs(M_ss,[theta1 theta2],[theta1_n theta2_n]);
disp(' ');
disp('两腿触地瞬间的方程如下：');
disp(' ');
disp(['J11 = ',char(simplify(J_num_newleg(1,1))),';']);
disp(['J12 = ',char(simplify(J_num_newleg(1,2))),';']);
disp(['J13 = ',char(simplify(J_num_newleg(1,3))),';']);
disp(['J14 = ',char(simplify(J_num_newleg(1,4))),';']);
disp(['J21 = ',char(simplify(J_num_newleg(2,1))),';']);
disp(['J22 = ',char(simplify(J_num_newleg(2,2))),';']);
disp(['J23 = ',char(simplify(J_num_newleg(2,3))),';']);
disp(['J24 = ',char(simplify(J_num_newleg(2,4))),';']);
disp('J = [J11 J12 J13 J14;J21 J22 J23 J24];');
disp(' ')

disp(['A11 = ',char(simplify(A_num_newleg(1,1))),';']);
disp(['A12 = ',char(simplify(A_num_newleg(1,2))),';']);
disp(['A13 = ',char(simplify(A_num_newleg(1,3))),';']);
disp(['A14 = ',char(simplify(A_num_newleg(1,4))),';']);
disp(['A21 = ',char(simplify(A_num_newleg(2,1))),';']);
disp(['A22 = ',char(simplify(A_num_newleg(2,2))),';']);
disp(['A23 = ',char(simplify(A_num_newleg(2,3))),';']);
disp(['A24 = ',char(simplify(A_num_newleg(2,4))),';']);
disp(['A31 = ',char(simplify(A_num_newleg(3,1))),';']);
disp(['A32 = ',char(simplify(A_num_newleg(3,2))),';']);
disp(['A33 = ',char(simplify(A_num_newleg(3,3))),';']);
disp(['A34 = ',char(simplify(A_num_newleg(3,4))),';']);
disp(['A41 = ',char(simplify(A_num_newleg(4,1))),';']);
disp(['A42 = ',char(simplify(A_num_newleg(4,2))),';']);
disp(['A43 = ',char(simplify(A_num_newleg(4,3))),';']);
disp(['A44 = ',char(simplify(A_num_newleg(4,4))),';']);
disp('A_n_hs = [A11 A12 A13 A14; A21 A22 A23 A24; A31 A32 A33 A34; A41 A42 A43 A44];');
disp(' ');

disp('X_num_newleg = [0 0 theta1dot_n theta2dot_n]'';'); %[vx_stance vy_stance omega1 omega2]
disp('b_hs = [A_n_hs*X_n_hs; 0; 0];'); %[momentum before footstrike = A_n_hs*X_n_hs; v_swing_foot_after_foot_strike = 0 0 
disp('A_hs = [A_n_hs -J'' ; J zeros(2,2)];');
disp('X_hs = A_hsb_hs;');
disp('thetadot(1) = X_hs(3)+X_hs(4); thetadot(2) = -X_hs(4);');
disp(' ');
disp(' ');

仿真部分

本部分带入动力学模型，带入数据，得到仿真结果。

function passivewalker_sim

clc
close all
format long
 
%%%% Dimensions %%
%%% c = COM on the leg from hip M = hip mass, m = leg mass, I = leg inertia, l = leg length
walker.M = 1.0; walker.m = 0.5; walker.I = 0.02; walker.l = 1.0; 
walker.c = 0.5; walker.g = 1.0; walker.gam = 0.01; 

%%%% Initial State %%%%%
q1 = 0.2; u1 = -0.25; %q1 = theta1; u1 = omega1
q2 = -0.4; u2 = 0.2; %Try different u2 to get second root

z0 = [q1 u1 q2 u2];

steps = 3; %number of steps to animate
fps = 20; %Use low frames per second for low gravity
flag_analyze = 1;

if (flag_analyze==0)
%%% forward simulation  %%%
[z,t] = onestep(z0,walker,steps);
figure(1)
animate(t,z,walker,steps,fps);
else
% % %%% Root finding will give this stable root 
% zstar = [0.162597833780041  -0.231869638058930  -0.325195667560083   0.037978468073743]
% onestep(zstar,walker)
% %%%%%%%%%%%%%%%%%%%%%%%%%

%%%% Root finding, Period one gait %%%%
options = optimset('TolFun',1e-12,'TolX',1e-12,'Display','off');
[zstar,fval,exitflag] = fsolve(@fixedpt,z0,options,walker);
if exitflag == 1
    disp('Fixed point:');
    disp(zstar);
else
    error('Root finder not converged, change guess or change system parameters')
end

%%% Stability, using eigenvalues of Poincare map %%%
J=partialder(@onestep,zstar,walker);
disp('EigenValues for linearized map are');
eigJ = eig(J);
for i=1:4
    disp(norm(eigJ(i)));
end

%%% forward simulation  %%%
[z,t] = onestep(zstar,walker,steps);

%%%% Animate result %%%
disp('Animating...');
figure(1)
animate(t,z,walker,steps,fps);

end

%%% Plot data %%%
disp('Some plots...')
figure(2)
subplot(2,1,1);
title('passive walker position and velocity as a function of time');
plot(t,z(:,1),'r--','Linewidth',3); hold on
plot(t,z(:,3),'b','Linewidth',2);
ylabel('position','Fontsize',12);
legend('theta_1','theta_2','Location','best','Fontsize',12);
subplot(2,1,2)
plot(t,z(:,2),'r--','Linewidth',3); hold on
plot(t,z(:,4),'b','Linewidth',2);
ylabel('velocity','Fontsize',12);
xlabel('time','Fontsize',12);
legend('theta_1','theta_2','Location','best','Fontsize',12);

% %%% forward simulation  %%%
% z_pert = zstar + [0 0.01 0 0.05];
% [z,t] = onestep(z_pert,walker,steps);
% 

% figure(4)
% subplot(2,1,1);
% title('passive walker absolute leg angle rate versus absolute leg angle');
% plot(z(:,1),z(:,2),'r','Linewidth',3); hold on;
% plot(z(1,1),z(1,2),'ko','Markersize',10,'MarkerFaceColor','k');
% text(z(1,1)+0.01,z(1,2)+0.01,'start','Fontsize',12)
% ylabel('$dot{theta}_1$','Fontsize',12,'Interpreter','Latex');
% xlabel('theta_1','Fontsize',12);
% subplot(2,1,2);
% plot(z(:,1)+z(:,3),z(:,2)+z(:,4),'b','Linewidth',2); hold on;
% plot(z(1,1)+z(1,3),z(1,2)+z(1,4),'ko','Markersize',10,'MarkerFaceColor','k');
% text(z(1,1)+z(1,3)+0.01,z(1,2)+z(1,4)+0.01,'start','Fontsize',12)
% ylabel('$dot{theta}_1+dot{theta}_2$','Fontsize',12,'Interpreter','Latex');
% xlabel('theta_1+theta_2','Fontsize',12);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% FUNCTIONS START HERE %%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%===================================================================
function zdiff=fixedpt(z0,walker)
%===================================================================
zdiff=onestep(z0,walker)-z0; 

%===================================================================
function J=partialder(FUN,z,walker)
%===================================================================
pert=1e-5;
n = length(z);
J = zeros(n,n);

%%%% Using forward difference, accuracy linear %%%
% y0=feval(FUN,z,walker); 
% for i=1:n
%     ztemp=z;
%     ztemp(i)=ztemp(i)+pert; 
%     J(:,i)=(feval(FUN,ztemp,walker)-y0) ;
% end
% J=(J/pert);

%%% Using central difference, accuracy quadratic %%%
for i=1:n
    ztemp1=z; ztemp2=z;
    ztemp1(i)=ztemp1(i)+pert; 
    ztemp2(i)=ztemp2(i)-pert; 
    J(:,i)=(feval(FUN,ztemp1,walker)-feval(FUN,ztemp2,walker)) ;
end
J=J/(2*pert);

%===================================================================
function [z,t]=onestep(z0,walker,steps)
%===================================================================

l = walker.l;

flag = 1;
if nargin<2
    error('need more inputs to onestep');
elseif nargin<3
    flag = 0; %send only last state, for root finder and jacobian
    steps = 1;
end

theta1 = z0(1);
xh = 0;
yh = l*cos(theta1);
xh_start = xh;

t0 = 0; 
dt = 4; %might need to be changed based on estimate of time taken for one step
time_stamps = 100;
t_ode = t0;
z_ode = [z0 xh yh];

for i=1:steps
    options=odeset('abstol',1e-13,'reltol',1e-13,'events',@collision);
    tspan = linspace(t0,t0+dt,time_stamps);
    [t_temp, z_temp] = ode113(@single_stance,tspan,z0,options,walker);

    zplus=footstrike(t_temp(end),z_temp(end,:),walker);    
    
    z0 = zplus;
    t0 = t_temp(end);
    
    xh_temp = xh_start + l*sin(z_temp(1,1))-l*sin(z_temp(:,1)); 
    yh_temp =  l*cos(z_temp(:,1));
    
    t_ode = [t_ode; t_temp(2:end)];
    z_ode = [z_ode; [z_temp(2:(end-1),:); zplus] xh_temp(2:end) yh_temp(2:end)];
    xh_start = xh_temp(end);
end

z = zplus(1:4);

if flag==1
   z=z_ode;
   t=t_ode;
end

%===================================================================
function zdot=single_stance(t,z,walker)  
%===================================================================
theta1 = z(1);   omega1 = z(2);                         
theta2 = z(3);   omega2 = z(4);                         
                    
M = walker.M;  m = walker.m; I = walker.I;   
l = walker.l;  c = walker.c; 
g = walker.g; gam = walker.gam;

%%%%%%%%% copy pasted from passivewalker_derive.m %%%%%%
A11 = 2*I + M*l^2 + 2*c^2*m + 2*l^2*m - 2*c*l*m - 2*c*l*m*cos(theta2);
A12 = I + c^2*m - c*l*m*cos(theta2);
A21 = I + c^2*m - c*l*m*cos(theta2);
A22 = I + c^2*m;
A_ss = [A11 A12; A21 A22];
 
b1 = c*g*m*sin(gam - theta1) - M*g*l*sin(gam - theta1) - c*g*m*sin(theta1 - gam + theta2) - 2*g*l*m*sin(gam - theta1) - c*l*m*omega2^2*sin(theta2) - 2*c*l*m*omega1*omega2*sin(theta2);
b2 = -c*m*(g*sin(theta1 - gam + theta2) - l*omega1^2*sin(theta2));
b_ss = [b1; b2];
 
alpha = A_ssb_ss;

%%%%%%%%%%% ends %%%%%%%%%%%%%
 
zdot = [omega1 alpha(1) omega2 alpha(2)]';  

%===================================================================
function [gstop, isterminal,direction]=collision(t,z,walker)
%===================================================================

theta1 = z(1); theta2 = z(3); 

gstop = theta2 + 2*theta1;
if (theta1>-0.05) %allow legs to pass through for small hip angles (taken care in real walker using stepping stones)
    isterminal = 0;
else
    isterminal=1; %ode should terminate is conveyed by 1, if you put 0 it goes till the final time u specify
end
direction=[]; % The t_final can be approached by any direction is indicated by the direction

%===================================================================
function zplus=footstrike(t,z,walker)      
%===================================================================

theta1_n = z(1);   omega1_n = z(2);                         
theta2_n = z(3);   omega2_n = z(4);                         
                      
theta1 = theta1_n + theta2_n;                         
theta2 = -theta2_n;                                       

M = walker.M;  m = walker.m; I = walker.I;   
l = walker.l;  c = walker.c;  

%%%%%%%%% copy pasted from passivewalker_derive.m %%%%%%
J11 = 1;
J12 = 0;
J13 = l*(cos(theta1_n + theta2_n) - cos(theta1_n));
J14 = l*cos(theta1_n + theta2_n);
J21 = 0;
J22 = 1;
J23 = l*(sin(theta1_n + theta2_n) - sin(theta1_n));
J24 = l*sin(theta1_n + theta2_n);
J = [J11 J12 J13 J14; J21 J22 J23 J24];
 
A11 = M + 2*m;
A12 = 0;
A13 = (m*(2*c*cos(theta1_n + theta2_n) - 2*l*cos(theta1_n)))/2 + m*cos(theta1_n)*(c - l) - M*l*cos(theta1_n);
A14 = c*m*cos(theta1_n + theta2_n);
A21 = 0;
A22 = M + 2*m;
A23 = (m*(2*c*sin(theta1_n + theta2_n) - 2*l*sin(theta1_n)))/2 - M*l*sin(theta1_n) + m*sin(theta1_n)*(c - l);
A24 = c*m*sin(theta1_n + theta2_n);
A31 = (m*(2*c*cos(theta1_n + theta2_n) - 2*l*cos(theta1_n)))/2 + m*cos(theta1_n)*(c - l) - M*l*cos(theta1_n);
A32 = (m*(2*c*sin(theta1_n + theta2_n) - 2*l*sin(theta1_n)))/2 - M*l*sin(theta1_n) + m*sin(theta1_n)*(c - l);
A33 = 2*I + M*l^2 + 2*c^2*m + 2*l^2*m - 2*c*l*m - 2*c*l*m*cos(theta2_n);
A34 = I + c^2*m - c*l*m*cos(theta2_n);
A41 = c*m*cos(theta1_n + theta2_n);
A42 = c*m*sin(theta1_n + theta2_n);
A43 = I + c^2*m - c*l*m*cos(theta2_n);
A44 = I + c^2*m;
A_n_hs = [A11 A12 A13 A14; A21 A22 A23 A24; A31 A32 A33 A34; A41 A42 A43 A44];
 
X_n_hs = [0 0 omega1_n omega2_n]';
b_hs = [A_n_hs*X_n_hs; 0; 0];
A_hs = [A_n_hs -J' ; J zeros(2,2)];
X_hs = A_hsb_hs;
omega(1) = X_hs(3)+X_hs(4); omega(2) = -X_hs(4);
 

%%%%%%%%%%%%% ends %%%%%%%%%%%%%

zplus = [theta1 omega(1) theta2 omega(2)];                     


%===================================================================
function animate(t_all,z_all,walker,steps,fps)
%===================================================================

%%%% Interpolate linearly using fps %%%%%
z_all_plot = [z_all(:,1) z_all(:,3) z_all(:,5) z_all(:,6)];
nn = size(z_all_plot,2);
total_frames = round(t_all(end)*fps);
t = linspace(0,t_all(end),total_frames);
z = zeros(total_frames,nn);
for i=1:nn
    z(:,i) = interp1(t_all,z_all_plot(:,i),t);
end

%%%%% Now animate the results %%%%%%%  
l = walker.l;    
c = walker.c;

mm = size(z,1);

min_xh = min(z(:,3)); max_xh = max(z(:,3)); 
dist_travelled = max_xh - min_xh;
camera_rate = dist_travelled/mm;

window_xmin = -1*l; window_xmax = 1*l;
window_ymin = -0.1; window_ymax = 1.1*l;

axis('equal')
axis([window_xmin window_xmax window_ymin window_ymax])
axis off
set(gcf,'Color',[1,1,1])

%%%% create ramp %%%%
rampref=[min_xh-1 max_xh+l ; 0 0];

%%%% Draw ramp %%%%%%%%%%
line('xdata',rampref(1,:),'ydata',rampref(2,:),'linewidth', 1,'color','black'); hold on;
 
for i=1:mm
   %moving window %%%%%%%
   window_xmin = window_xmin + camera_rate;
   window_xmax = window_xmax + camera_rate;
   axis('equal')
   axis([window_xmin window_xmax window_ymin window_ymax])

   %%% get angles and hip position 
   theta1 = z(i,1); theta2 = z(i,2); 
   xh = z(i,3); yh = z(i,4);
   
   %%% coordinates of points of interest %
   hinge=[xh; yh];  
   stance_foot = [xh+l*sin(theta1);         yh-l*cos(theta1)];
   stance_leg_com = [xh+c*sin(theta1);         yh-c*cos(theta1)];
   swing_foot =  [xh+l*sin(theta1 + theta2),yh-l*cos(theta1 + theta2)];
   swing_leg_com =  [xh+c*sin(theta1 + theta2),yh-c*cos(theta1 + theta2)];
   
   %animate 
   h0 = plot(hinge(1),hinge(2),'ko','MarkerFaceColor','k','Markersize',15);
   h1 = plot(stance_leg_com(1),stance_leg_com(2),'ko','MarkerFaceColor','k','Markersize',10);
   h2 = plot(swing_leg_com(1),swing_leg_com(2),'ko','MarkerFaceColor','k','Markersize',10);
   h3 = line([hinge(1) stance_foot(1)],[hinge(2) stance_foot(2)],'Color','red','Linewidth',2);
   h4 = line([hinge(1) swing_foot(1)], [hinge(2) swing_foot(2)],'Color','red','Linewidth',2);
   
   %delay to create animation
   pause(0.01);
   
   if (i~=mm)     %delete all but not the last entry
       delete(h0);
       delete(h1);
       delete(h2);
       delete(h3);
       delete(h4);
   end
   
end

最后

以上就是老实身影为你收集整理的5,Matlab仿真passive walker动力学代码的全部内容，希望文章能够帮你解决5,Matlab仿真passive walker动力学代码所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。