SLIP
前言
SLIP模型是Hopping的重要模型，可以使用raibert方法控制这个混合动力学模型
代码
main函数
robot.m = 1;
robot.g = 10;
robot.ground = 0;
robot.l = 1;
robot.control.k = 100;
robot.control.T = pi*sqrt(robot.m/robot.control.k);
robot.control.kp = 0.1;
robot.control.vdes = [0 0.5 0.6 0.9 1.0 1.1];
% [0 0.5 0.6 0.9 1.0 1.1 0.7 0.3 0];[0 0.1 0.2 0.3 0.4 0.5];
robot.control.theta = asin(robot.control.vdes(1)*robot.control.T/(2*robot.l)) + robot.control.kp * (robot.control.vdes(1) - robot.control.vdes(1));
robot.fps = 20;
%%%% 初始条件 %%%%
y0 = 1.2; xdot = robot.control.vdes(1); x0 = 0; y0dot = 0;
z0 = [x0 xdot y0 y0dot];
t0 = 0;
z1 = z0(2:3);
steps = length(robot.control.vdes); % 仿真步数
%%%% main %%%%
options = optimset('TolFun',1e-10,'TolX',1e-10,'Display','iter');
[zstart,fval,exitflag] = fsolve(@fixpt,z1,options,robot);
zstart = [0 zstart 0 robot.l*sin(robot.control.theta) ...
y0 - robot.l*cos(robot.control.theta)];
[t_all,z_all,v_apex]=bounce(zstart,t0,steps,robot);
figure(1)
animate(t_all,z_all,robot); % z_all = [x0 xdot y0 y0dot xfoot yfoot];
% figure(2)
% subplot(2,1,1)
% plot(t_all,z_all(:,1),'r',t_all,z_all(:,3),'b');
% % plot(t_all,t_all(:,3),'b');hold off;
% legend('x','y');
% xlabel('时间','FontSize',12);ylabel('位移','FontSize',12);
% subplot(2,1,2)
% plot(t_all,z_all(:,2),'r',t_all,z_all(:,4),'b');
% % plot(t_all,t_all(:,4),'b');hold off;
% legend('Vx','Vy');
% xlabel('时间','FontSize',12);ylabel('速度','FontSize',12);
figure(2)
subplot(2,1,1)
plot(t_all,z_all(:,1),'r');hold on;
plot(t_all,z_all(:,3),'b');
legend('x','y');
xlabel('时间','FontSize',12);ylabel('位移','FontSize',12);
subplot(2,1,2)
plot(t_all,z_all(:,2),'r');hold on;
plot(t_all,z_all(:,4),'b');
legend('Vx','Vy');
xlabel('时间','FontSize',12);ylabel('速度','FontSize',12);
figure(3)
plot(z_all(:,1),z_all(:,3),'r');
figure(4)
stairs(robot.control.vdes,'k--','LineWidth',2);hold on;
stairs(v_apex,'r','LineWidth',2);
legend('v_desired','v_actual');
其他函数
main函数通过调用bounce函数来实现SLIP模型的跳动，因此要定义bounce函数，同时也要定义一个动画仿真函数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%% 全部函数 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% 总运动函数 %%%%
function [t_all,z_all,v_apex]=bounce(z0,t0,steps,robot)% z0 = [x0 xdot y0 y0dot xfoot yfoot];
t_al = [0]; z_al = z0;v_tem_apex = [];
for i = 1:steps
robot.control.theta = asin(z0(2)*robot.control.T/(2*robot.l)) + robot.control.kp * (z0(2) - robot.control.vdes(i));
[t_temp1,z_temp1] = fall(z0,t0,robot); %下落积分
t0 = t_temp1(end); z0 = z_temp1(end,1:6);
[t_temp2,z_temp2] = stance(z0,t0,robot); %触地积分
t0 = t_temp2(end); z0 = z_temp2(end,1:6);
[t_temp3,z_temp3] = fly(z0,t0,robot);%上升积分
t0 = t_temp3(end); z0 = z_temp3(end,1:6);
v_tem_apex = [v_tem_apex z0(2)];
t_al = [t_al;t_temp1(2:end);t_temp2(2:end);t_temp3(2:end)];
z_al = [z_al;z_temp1(2:end,:);z_temp2(2:end,:);z_temp3(2:end,:)];
end
t_all = t_al;
z_all = z_al;
v_apex = v_tem_apex;
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% 分批运动函数 %%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% 空中下落 %%%%
function [t_temp1,z_temp1] = fall(z0,t0,robot) % z0 = z0 = [x0 xdot y0 y0dot xfoot yfoot];
dt = 10;
tspan = linspace(t0,t0+dt,dt*1000);
options = odeset('RelTol',1e-9,'AbsTol',1e-9,'Events',@contact);
[t,z] = ode45(@flight,tspan,z0(1:4),options,robot);
t_temp1 = t;
z_temp1 = [z ...
z(:,1) + robot.l*sin(robot.control.theta) ...
z(:,3) - robot.l*cos(robot.control.theta)];
%%%% 支撑相 %%%%
function [t_temp2,z_temp2] = stance(z0,t0,robot)% z0 = z0 = [x0 xdot y0 y0dot xfoot yfoot];
x_foot = z0(1) + robot.l*sin(robot.control.theta);
z0(1) = -robot.l*sin(robot.control.theta); % 改成相对位置
dt = 10;
tspan = linspace(t0,t0+dt,dt*1000);
options = odeset('RelTol',1e-9,'AbsTol',1e-9,'Events',@release);
z1 = z0(1:4);
[t,z] = ode45(@stancefun,tspan,z1,options,robot);
z(:,1) = z(:,1) + x_foot; % 改回绝对位置
t_temp2 = t;
z_temp2 = [z ...
x_foot*ones(length(z(:,1)),1)...
zeros(length(z(:,1)),1)];
%%%% 返回顶点 %%%%
function [t_temp3,z_temp3] = fly(z0,t0,robot)
dt = 10;
tspan = linspace(t0,t0+dt,dt*1000);
options = odeset('RelTol',1e-9,'AbsTol',1e-9,'Events',@apex);
[t,z] = ode45(@flight,tspan,z0(1:4),options,robot);
t_temp3 = t;
z_temp3 = [z ...
z(:,1) - robot.l*sin(robot.control.theta) ...
z(:,3) - robot.l*cos(robot.control.theta)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% 动力学方程 %%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
function zdot = flight(t,z,robot)
zdot = [z(2) 0 z(4) -robot.g]';
function zdot = stancefun(t,z,robot)
x = z(1); y = z(3); %x & y position of com wrt ground
l = sqrt(x^2+y^2);
F_spring = robot.control.k*(robot.l-l);
Fx_spring = F_spring*(x/l);
Fy_spring = F_spring*(y/l);
Fy_gravity = robot.m*robot.g;
xddot = (1/robot.m)*(Fx_spring);
yddot = (1/robot.m)*(-Fy_gravity+Fy_spring);
zdot = [z(2) xddot z(4) yddot]';
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% 检测函数 %%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [gstop,isterminal,direction] = contact(t,z0,robot)
gstop = z0(3) - robot.l*cos(robot.control.theta);
isterminal = 1;
direction = -1;
function [gstop,isterminal,direction] = release(t,z0,robot)
gstop = z0(3) - robot.l*cos(robot.control.theta);
isterminal = 1;
direction = 1;
function [gstop,isterminal,direction] = apex(t,z0,robot)% z0 = z0 = [x0 xdot y0 y0dot xfoot yfoot];
gstop = z0(4);
isterminal = 1;
direction = -1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% 可视化函数 %%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
function animate(t_all,z_all,robot)% z_all = [x0 xdot y0 y0dot xfoot yfoot];
t_inter = linspace(t_all(1),t_all(end),robot.fps*(t_all(end)-t_all(1)));
[m,n] = size(z_all);
for i = 1:n
z_inter(:,i) = interp1(t_all,z_all(:,i),t_inter);
end
camera_rate = (max(z_all(:,1))-min(z_all(:,1)))/length(t_inter);
window_xmin = -1.0*robot.l; window_xmax = robot.l;
window_ymin = -0.1; window_ymax = 1.9*robot.l;
axis('equal');
set(gcf,'Color',[1,1,1])
% axis([window_xmin window_xmax window_ymin window_ymax])
for i = 1:length(t_inter)
z_temp = z_inter(i,:);
x0 = z_temp(1); y0 = z_temp(3); xfoot = z_temp(5); yfoot = z_temp(6);
plot(x0,y0,'ko','MarkerEdgeColor','black','MarkerSize',20,'MarkerFaceColor','k');
line([xfoot x0],[yfoot y0],'LineWidth',4,'Color','green'); % 腿
line([-10 10], [0 0],'LineWidth',2,'Color','black'); % 地面
window_xmin = window_xmin + camera_rate;
window_xmax = window_xmax + camera_rate;
axis('equal')
axis off
axis([window_xmin window_xmax window_ymin window_ymax])
pause(0.05);
end
