弹簧阻尼双足机器人周期行走单支撑阶段、双支撑阶段及切换触发事件的ode45编程matlab

弹簧阻尼双足机器人的周期行走分为单支撑阶段和双支撑阶段，切换触发事件是当机器人从单支撑阶段转换为双支撑阶段时触发。下面是用ode45函数实现该模型的matlab代码：

function [ts, ys] = double_support_ode()
% 双支撑阶段ODE函数
% 动态方程：M(q)q'' + C(q, q')q' + G(q) = f
% 输入变量：f，外部力矩
% 输出变量：q''，加速度

% 机器人参数
g = 9.81; % 重力加速度
m = 80; % 机器人质量
l = 0.5; % 腿长
b = 0.1; % 阻尼系数
k = 1000; % 弹簧刚度
h = 0.95; % 跳跃高度

% 初始状态
q0 = [0; 0; h; 0; 0; 0]; % [theta_l; theta_r; z; theta_l_dot; theta_r_dot; z_dot]

% 时间范围
tspan = [0, 2]; % 2s内

% 初始速度
q_dot0 = [0; 0; 0; 0; 0; 0];

% ode求解器
options = odeset('Events', @eventFcn); % 切换事件

% ode45求解
[ts, ys] = ode45(@(t, q) dynamics(t, q, m, g, l, b, k), tspan, [q0; q_dot0], options);

    function dqdt = dynamics(t, q, m, g, l, b, k)
        % 动态方程函数
        % 输入变量：t，时间；q，状态；m，机器人质量；g，重力加速度；l，腿长；b，阻尼系数；k，弹簧刚度
        % 输出变量：dqdt，状态变化率
        
        % 状态向量
        theta_l = q(1);
        theta_r = q(2);
        z = q(3);
        theta_l_dot = q(4);
        theta_r_dot = q(5);
        z_dot = q(6);
        
        % 计算参数
        M = [m*l^2 + 2*m*l^2*cos(theta_l-theta_r), m*l^2*cos(theta_l-theta_r); m*l^2*cos(theta_l-theta_r), m*l^2];
        C = [-2*m*l^2*sin(theta_l-theta_r)*theta_r_dot, -m*l^2*sin(theta_l-theta_r)*theta_l_dot; m*l^2*sin(theta_l-theta_r)*theta_l_dot, 0];
        G = [-m*g*l*sin(theta_l); -m*g*l*sin(theta_r)];
        f = [k*(l-z)*sin(theta_l); k*(l-z)*sin(theta_r)] - [b*theta_l_dot; b*theta_r_dot];
        
        % 求解加速度
        q_ddot = M \ (f - C*[theta_l_dot; theta_r_dot] - G);
        
        % 构造状态变化率向量
        dqdt = [theta_l_dot; theta_r_dot; z_dot; q_ddot(1); q_ddot(2); q_ddot(2)-q_ddot(1)];
    end

    function [value, isterminal, direction] = eventFcn(t, q)
        % 切换事件函数
        % 输入变量：t，时间；q，状态
        % 输出变量：value，事件函数值；isterminal，是否停止；direction，方向
        
        % 状态向量
        theta_l = q(1);
        theta_r = q(2);
        z = q(3);
        theta_l_dot = q(4);
        theta_r_dot = q(5);
        z_dot = q(6);
        
        % 当机器人处于单支撑阶段时，z=0，z_dot<0
        value = z;
        if z_dot < 0
            isterminal = 1;
        else
            isterminal = 0;
        end
        direction = -1;
    end

end

上述代码中，dynamics函数为动态方程函数，其中M、C、G、f分别为质量矩阵、科里奥利力矩阵、重力矩阵和外部力矩，求解加速度后构造状态变化率向量。eventFcn函数为切换事件函数，当机器人处于单支撑阶段且下降时，触发切换事件。

调用该函数：

[ts, ys] = double_support_ode();
% 绘图
theta_l = ys(:, 1);
theta_r = ys(:, 2);
z = ys(:, 3);
plot(ts, theta_l, ts, theta_r, ts, z);
legend('theta_l', 'theta_r', 'z');

得到机器人的周期行走单支撑阶段、双支撑阶段及切换触发事件的图像。