弹簧阻尼双足机器人周期行走单支撑阶段、双支撑阶段ode45编程matlab
时间: 2024-06-09 15:09:55 浏览: 10
弹簧阻尼双足机器人的周期行走包括单支撑阶段和双支撑阶段。单支撑阶段指的是机器人只有一只脚着地,而另一只脚在空中摆动。双支撑阶段指的是机器人的两只脚同时着地。下面给出使用ode45函数进行编程的示例代码:
```matlab
function [t, y] = double_stance_phase()
% 参数初始化
m1 = 10; % 身体质量
m2 = 2; % 每条腿的质量
l1 = 0.5; % 身体长度
l2 = 0.5; % 腿的长度
g = 9.8; % 重力加速度
k = 200; % 弹簧刚度
c = 20; % 阻尼系数
x0 = [0; 0; 0; 0; 0; 0]; % 初始状态 [身体位置; 身体速度; 脚1位置; 脚1速度; 脚2位置; 脚2速度]
% 时间范围和初始状态
tspan = [0 10];
options = odeset('Events', @event_func);
[t, y, te, ye] = ode45(@ode_func, tspan, x0, options);
% 绘制图像
figure
subplot(3,1,1)
plot(t, y(:,1), t, y(:,3), t, y(:,5))
legend('Body', 'Foot 1', 'Foot 2')
title('Position')
subplot(3,1,2)
plot(t, y(:,2), t, y(:,4), t, y(:,6))
legend('Body', 'Foot 1', 'Foot 2')
title('Velocity')
subplot(3,1,3)
plot(t, y(:,1)-y(:,3), t, y(:,1)-y(:,5))
legend('Foot 1', 'Foot 2')
title('Distance')
% 定义ODE函数
function dxdt = ode_func(t, x)
% 分离状态量
q = x(1:2);
q_dot = x(3:4);
p1 = x(5:6);
p1_dot = x(7:8);
p2 = x(9:10);
p2_dot = x(11:12);
% 计算控制输入
if t < te % 单支撑阶段
u1 = - k * (q(1) - p1(1)) - c * (q_dot(1) - p1_dot(1)) - m1*g/l1 - m2*g*(l1/2-q(2))/l1;
u2 = 0;
else % 双支撑阶段
u1 = - k * (q(1) - p1(1)) - c * (q_dot(1) - p1_dot(1)) - m1*g/l1 - m2*g*(l1/2-q(2))/l1;
u2 = - k * (q(1) - p2(1)) - c * (q_dot(1) - p2_dot(1)) - m1*g/l1 - m2*g*(l1/2-q(2))/l1;
end
% 计算加速度
M = [m1+m2, -m2*l1/2*cos(q(2)); -m2*l1/2*cos(q(2)), m2*(l1/2)^2];
C = [0, -m2*l1/2*sin(q(2))*q_dot(2); 0, 0];
G = [0; -m2*g*l1/2*sin(q(2))];
F = [u1; u2];
q_ddot = inv(M) * (F - C*q_dot - G);
% 计算状态量导数
dxdt = [q_dot; q_ddot; p1_dot; foot_1_ddot(q, q_dot, p1, p1_dot); p2_dot; foot_2_ddot(q, q_dot, p2, p2_dot)];
end
% 定义脚的加速度函数
function p_ddot = foot_1_ddot(q, q_dot, p, p_dot)
if p(2) >= 0 % 判断是否接触地面
p_ddot = [0; 0];
else
M = m1 + m2;
K = k + M*g/l1;
C = c;
F = K * (q(1) - p(1)) + C * (q_dot(1) - p_dot(1)) + M * q_ddot(1) + M * g/l1;
p_ddot = inv(m2/l2*[cos(q(2)), sin(q(2))]'*[cos(q(2)), sin(q(2))])*[F; -m2*g/l2*sin(q(2))];
end
end
function p_ddot = foot_2_ddot(q, q_dot, p, p_dot)
if p(2) >= 0 % 判断是否接触地面
p_ddot = [0; 0];
else
M = m1 + m2;
K = k + M*g/l1;
C = c;
F = K * (q(1) - p(1)) + C * (q_dot(1) - p_dot(1)) + M * q_ddot(1) + M * g/l1;
p_ddot = inv(m2/l2*[cos(q(2)), -sin(q(2))]'*[cos(q(2)), -sin(q(2))])*[F; -m2*g/l2*sin(q(2))];
end
end
% 定义事件函数
function [value, isterminal, direction] = event_func(t, x)
value = x(5); % 当脚1离地时触发事件
isterminal = 1; % 终止积分
direction = -1; % 当脚1离地时触发事件,脚1的高度是下降的,所以方向为-1
end
end
```
在上面的代码中,我们使用了ode45函数来求解ODE。同时,我们还定义了一个事件函数,在脚1离地时触发事件,从而在单支撑阶段和双支撑阶段之间切换。最后,我们绘制了机器人的位置、速度和脚之间的距离随时间的变化情况。
相关推荐
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)