弹簧阻尼双足机器人周期行走单支撑阶段、双支撑阶段ode45编程matlab

弹簧阻尼双足机器人周期行走模型的ODE方程如下：

$$\ddot{x} = \frac{1}{m}(F_{l}+F_{r}-k_{s}(x_{l}+x_{r}-2x_{0})-k_{d}(\dot{x}_{l}+\dot{x}_{r}-2\dot{x}_{0}))$$

其中，$m$ 是机器人质量，$F_l$ 与 $F_r$ 是左右脚所受外力，$k_s$ 是弹簧刚度，$k_d$ 是弹簧阻尼系数，$x_l$ 与 $x_r$ 是左右脚相对于机器人质心的位移，$x_0$ 是静态平衡时脚与机器人质心的距离，$\dot{x}_l$ 与 $\dot{x}_r$ 是左右脚相对于机器人质心的速度。

在单支撑阶段，机器人只有一个脚着地。设左脚着地，则ODE方程可以简化为：

$$\ddot{x} = \frac{1}{m}(F_{r}-k_{s}(x_{r}-x_{0})-k_{d}(\dot{x}_{r}-\dot{x}_{0}))$$

在双支撑阶段，机器人两只脚同时着地。设左右脚分别为 $l$ 和 $r$，则ODE方程可以简化为：

$$\begin{cases}\ddot{x}_{l} = \frac{1}{2m}(F_{l}-k_{s}x_{l}-k_{d}\dot{x}_{l}+F_{r}-k_{s}(x_{r}-x_{0})-k_{d}(\dot{x}_{r}-\dot{x}_{0}))\\ \ddot{x}_{r} = \frac{1}{2m}(F_{r}-k_{s}x_{r}-k_{d}\dot{x}_{r}+F_{l}-k_{s}(x_{l}-x_{0})-k_{d}(\dot{x}_{l}-\dot{x}_{0}))\end{cases}$$

其中，$x_0$ 为两只脚的平均位置。

ODE方程可以使用MATLAB中的ode45函数求解。下面是一个简单的MATLAB代码示例：

function [t, y] = bipedal_walking()
    % 参数设置
    m = 70; % 机器人质量
    ks = 1000; % 弹簧刚度
    kd = 100; % 弹簧阻尼系数
    x0 = 0.5; % 静态平衡时脚与机器人质心的距离
    T = 1; % 步态周期
    F_l = @(t) 0; % 左脚外力
    F_r = @(t) 0; % 右脚外力

    % 初始状态
    x_l0 = 0;
    x_r0 = 0.5;
    dx_l0 = -0.1;
    dx_r0 = 0;

    % ODE求解
    options = odeset('RelTol', 1e-6, 'AbsTol', 1e-6);
    [t, y] = ode45(@(t, y) bipedal_ode(t, y, m, ks, kd, x0, F_l, F_r), [0, T], [x_l0, x_r0, dx_l0, dx_r0], options);

    % 可视化结果
    figure;
    plot(t, y(:, 1), 'b-', t, y(:, 2), 'r-');
    xlabel('Time (s)');
    ylabel('Foot position (m)');
    legend('Left foot', 'Right foot');
end

function dydt = bipedal_ode(t, y, m, ks, kd, x0, F_l, F_r)
    x_l = y(1);
    x_r = y(2);
    dx_l = y(3);
    dx_r = y(4);

    if mod(t, 2 * pi) < pi % 单支撑阶段
        F = F_l(t);
        x = x_l;
        dx = dx_l;
    else % 双支撑阶段
        F = F_l(t) + F_r(t);
        x = (x_l + x_r) / 2;
        dx = (dx_l + dx_r) / 2;
    end

    dd = (F - ks * (x - x0) - kd * (dx - 0)) / m;

    if mod(t, 2 * pi) < pi % 单支撑阶段
        dydt = [dx_l; dx_r; dd; 0];
    else % 双支撑阶段
        dydt = [dx_l; dx_r; dd / 2; dd / 2];
    end
end

在代码中，我们先定义了一个 `bipedal_walking` 函数，其中设置了机器人的参数，初始状态和外力等。然后调用 `ode45` 函数求解ODE方程，并将结果可视化。

ODE方程的求解和ODE函数的编写放在了一个单独的函数 `bipedal_ode` 中，该函数根据当前时间和状态判断是单支撑阶段还是双支撑阶段，然后根据ODE方程求解下一时刻的状态。