2. Stewart平台动力学建模与仿真

任务目标

建立Stewart平台六自由度并联机构的动力学模型，采用牛顿-欧拉递推方法推导驱动力与运动状态（位置、速度、加速度）之间的映射关系，并在MATLAB/Simulink环境中通过逆动力学仿真验证模型准确性。

动力学建模方法

Stewart平台由上平台（动平台）、下平台（静平台）及六条可伸缩支腿构成。采用牛顿-欧拉法建立动力学模型：对每条支腿单独进行力/力矩平衡分析，再通过雅可比矩阵将支腿驱动力映射到上平台的广义力。核心关系为：

τ = J⁻ᵀ · F_platform

其中τ为六条支腿的驱动力，J为速度雅可比矩阵，F_platform为上平台所需的广义力（含惯性力、科氏力、重力及外力）。

仿真环境与依赖

  • MATLAB R2020b及以上版本（需Simulink与Simscape Multibody工具箱）
  • 本案例使用Simscape Multibody搭建可视化仿真模型，用于验证动力学方程的正确性

关键完整代码

以下代码实现牛顿-欧拉逆动力学求解器，输入上平台运动轨迹（位置、速度、加速度），输出六条支腿所需驱动力：


function tau = stewart_inverse_dynamics(q, qd, qdd, params)
% 输入: q(6x1) 上平台位姿[x,y,z,roll,pitch,yaw]
%       qd(6x1) 速度, qdd(6x1) 加速度
% 输出: tau(6x1) 六条支腿驱动力
% 参数: params 结构体(几何尺寸、质量、惯性张量)

% 1. 上平台运动学
R = eul2rotm(q(4:6)', 'ZYX');  % 姿态旋转矩阵
p = q(1:3)';                    % 上平台参考点位置
omega = qd(4:6)';               % 角速度(体坐标系)
alpha = qdd(4:6)';              % 角加速度(体坐标系)
v = qd(1:3)';                   % 线速度
a = qdd(1:3)';                  % 线加速度

% 2. 计算雅可比矩阵(几何法)
J = compute_jacobian(q, params);

% 3. 上平台牛顿-欧拉方程
m = params.m_platform;
Ig = params.I_platform;         % 质心惯性张量(体坐标系)
g = [0; 0; -9.81];

% 质心位置(假设质心在平台几何中心)
pc = p + R * params.r_cm;       % 质心全局位置
ac = a + cross(alpha, R*params.r_cm) + ...
    cross(omega, cross(omega, R*params.r_cm));  % 质心加速度

% 力平衡
F_total = m * (ac - g);         % 所需惯性力+重力补偿

% 力矩平衡(绕质心)
I_world = R * Ig * R';          % 惯性张量转换到世界系
M_total = I_world * alpha + cross(omega, I_world*omega);

% 转换到上平台参考点
F_ref = F_total;
M_ref = M_total + cross(pc - p, F_total);

% 4. 映射到支腿驱动力
F_platform = [F_ref; M_ref];
tau = J' \ F_platform;          % 注意: 实际使用J的转置逆
end

function J = compute_jacobian(q, params)
% 基于几何关系计算6x6雅可比矩阵
R = eul2rotm(q(4:6)', 'ZYX');
p = q(1:3)';
J = zeros(6,6);
for i = 1:6
    % 支腿i的上连接点(平台坐标系)
    b_i = params.B(:, i);       % 上平台铰点(体坐标)
    % 支腿i的下连接点(世界坐标, 固定)
    A_i = params.A(:, i);
    
    % 上铰点全局位置
    B_i = p + R * b_i;
    % 支腿方向向量
    l_i = B_i - A_i;
    L_i = norm(l_i);
    s_i = l_i / L_i;            % 单位方向向量
    
    % 雅可比行(对应支腿i)
    J(i, 1:3) = s_i';
    J(i, 4:6) = (cross(R*b_i, s_i))';
end
end

Simulink仿真验证模型

为验证上述动力学方程的正确性，搭建Simscape Multibody仿真环境：


% 创建Simscape模型并验证
mdl = 'stewart_validation';
open_system(mdl);  % 需预先建立模型(见下方说明)

% 仿真参数
T = 5;              % 仿真时长(秒)
freq = 0.5;         % 运动频率(Hz)
A = 0.05;           % 振幅(米)

% 输入轨迹: 上平台沿Z轴正弦运动
t = linspace(0, T, 1000);
z_des = A * sin(2*pi*freq*t);
zd_des = 2*pi*freq*A * cos(2*pi*freq*t);
zdd_des = -(2*pi*freq)^2 * A * sin(2*pi*freq*t);

% 运行仿真并采集驱动力
simOut = sim(mdl, 'StopTime', num2str(T));
tau_sim = simOut.tau_sim;   % Simscape测量的实际驱动力

% 使用逆动力学计算理论驱动力
tau_theory = zeros(6, length(t));
for k = 1:length(t)
    q = [0; 0; z_des(k); 0; 0; 0];
    qd = [0; 0; zd_des(k); 0; 0; 0];
    qdd = [0; 0; zdd_des(k); 0; 0; 0];
    tau_theory(:, k) = stewart_inverse_dynamics(q, qd, qdd, params);
end

% 对比验证
figure;
plot(t, tau_sim(1,:), 'b-', t, tau_theory(1,:), 'r--', 'LineWidth', 1.5);
legend('Simscape仿真', '牛顿-欧拉理论');
xlabel('时间(s)'); ylabel('支腿1驱动力(N)');
title('动力学模型验证');
grid on;

Simscape模型搭建要点

在Simulink中创建stewart_validation模型需包含：

  1. 上平台：使用Brick Solid模块，质量5kg，惯性张量[0.05 0 0; 0 0.05 0; 0 0 0.1] kg·m²
  2. 六条支腿：每条支腿由Cylindrical Joint连接上下铰点，作动器施加力
  3. 运动驱动：使用关节传感器测量实际运动，通过PID控制器跟踪期望Z轴轨迹
  4. 力测量：在每条支腿的作动器处添加力传感器，输出实际驱动力

验证方法与判定标准

仿真完成后，对比Simscape实测驱动力与牛顿-欧拉理论计算值：

  • 通过条件：六条支腿的驱动力曲线与理论曲线基本重合，最大相对误差小于5%
  • 误差来源：Simscape模型包含关节摩擦、数值积分误差，微小偏差属正常现象
  • 附加验证：可改变运动轨迹（如加入俯仰运动），重复上述对比，验证模型在耦合运动下的准确性

若误差超过5%，检查雅可比矩阵计算是否正确、惯性张量是否与Simscape模型一致、质心位置是否匹配。

← 上一章 📖 返回目录 下一章 →