动力学
行走动力学
平面被动行走机器人共有四个自由度，分别是两个夹角和两个绝对坐标，可以使用拉格朗日方程得到被动行走机器人的动力学方程。需要注意的是，在行走过程中，只有一只腿撑住地面，因此只需要求解一条腿的雅可比矩阵
碰撞动力学
当空中摆动腿和地面接触时，会产生一个顺势冲量，此时，行走动力学方程失效，可以将动力学方程对这个瞬间做积分，而后求解方程，得到碰撞后的解。
代码
动力学推导部分
本部分使用符号推导，得到被动行走机器人的行走动力学模型和碰撞动力学模型。
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_ss\b_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_hs\b_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_ss\b_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_hs\b_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
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
