双足机器人周期行走双支撑阶段当满足一定条件转换成单支撑阶段的ode45 MATLAB程序

以下是一个双足机器人周期行走的ODE45 MATLAB程序，实现了双支撑阶段当双腿角度之和 $q_2+q_3$ 等于 $0$ 时转换成单支撑阶段：

function [dx] = biped_ode(t, x, params)

% 参数
m1 = params.m1; % 身体质量
m2 = params.m2; % 腿的质量
l = params.l; % 腿的长度
g = params.g; % 重力加速度

% 状态变量
q1 = x(1); % 身体的倾斜角
q2 = x(2); % 右腿的角度
q3 = x(3); % 左腿的角度
dq1 = x(4); % 身体的角速度
dq2 = x(5); % 右腿的角速度
dq3 = x(6); % 左腿的角速度

% 右腿和左腿的位置和速度
rfoot_x = l*sin(q2);
rfoot_y = -l*cos(q2);
lfoot_x = l*sin(q3);
lfoot_y = -l*cos(q3);
rfoot_dx = l*cos(q2)*dq2;
rfoot_dy = l*sin(q2)*dq2;
lfoot_dx = l*cos(q3)*dq3;
lfoot_dy = l*sin(q3)*dq3;

% 双腿支撑期
if q2 > 0 && q3 > 0
    % 右腿和左腿都在地面上，双支撑期
    dx(1,1) = dq1;
    dx(2,1) = dq2;
    dx(3,1) = dq3;
    dx(4,1) = (m2*rfoot_dx + m2*lfoot_dx)/(m1 + 2*m2*l^2);
    dx(5,1) = (m1*lfoot_y*dq1^2 + m2*lfoot_y*dq1^2 + (m1+m2)*g*lfoot_y)/(m1 + m2);
    dx(6,1) = (m1*rfoot_y*dq1^2 + m2*rfoot_y*dq1^2 + (m1+m2)*g*rfoot_y)/(m1 + m2);
    
    % 双腿支撑期到单腿支撑期的转换
    if q2 + q3 == 0
        dx(5,1) = 0; % 右腿速度为零
        dx(6,1) = 0; % 左腿速度为零
    end
    
% 单腿支撑期
elseif q2 <= 0 && q3 <= 0
    % 左腿支撑地面，单支撑期
    dx(1,1) = dq1;
    dx(2,1) = dq2;
    dx(3,1) = dq3;
    dx(4,1) = (m2*rfoot_dx + m2*lfoot_dx)/(m1 + 2*m2*l^2);
    dx(5,1) = 0; % 右腿速度为零
    dx(6,1) = (m1*rfoot_y*dq1^2 + m2*rfoot_y*dq1^2 + (m1+m2)*g*rfoot_y)/(m1 + m2);
    
    % 单腿支撑期到双腿支撑期的转换
    if q2 + q3 == 0
        dx(5,1) = 0; % 右腿速度为零
        dx(6,1) = 0; % 左腿速度为零
    end
    
% 右腿离地，左腿支撑
elseif q2 <= 0 && q3 > 0
    dx(1,1) = dq1;
    dx(2,1) = dq2;
    dx(3,1) = dq3;
    dx(4,1) = (m2*rfoot_dx + m2*lfoot_dx)/(m1 + 2*m2*l^2);
    dx(5,1) = 0; % 右腿速度为零
    dx(6,1) = (m1*rfoot_y*dq1^2 + m2*rfoot_y*dq1^2 + (m1+m2)*g*rfoot_y)/(m1 + m2);
    
% 左腿离地，右腿支撑
elseif q2 > 0 && q3 <= 0
    dx(1,1) = dq1;
    dx(2,1) = dq2;
    dx(3,1) = dq3;
    dx(4,1) = (m2*rfoot_dx + m2*lfoot_dx)/(m1 + 2*m2*l^2);
    dx(5,1) = (m1*lfoot_y*dq1^2 + m2*lfoot_y*dq1^2 + (m1+m2)*g*lfoot_y)/(m1 + m2);
    dx(6,1) = 0; % 左腿速度为零
end

end

这个程序跟上面的程序很相似，唯一的区别是在双支撑期的转换条件。当双腿角度之和 $q_2+q_3$ 等于 $0$ 时，双支撑期转换成单支撑期，程序将右腿和左腿的速度都设置为零。这个条件可以保证双足机器人在转换时保持平衡，避免摔倒。