MATLAB仿真并联平台并联机器人Stewart 舵机

最近在折腾Stewart并联平台的仿真，发现用MATLAB搞这个特别有意思。这种六自由度平台看着复杂，其实拆开来看就是六个直线驱动器协调运动的事儿。咱们今天重点聊聊怎么用MATLAB实现运动学仿真，顺便把舵机控制逻辑揉进去。

先来点基础设定。假设平台上下两个圆盘半径分别是0.5m和0.3m，六个支腿均匀分布。在MATLAB里先把几何参数定死：

R_base = 0.5; % 基座半径
R_plat = 0.3; % 动平台半径
theta_base = linspace(0, 2*pi, 7); % 基座安装点角度
theta_plat = linspace(pi/6, 2*pi+pi/6, 7); % 动平台安装点偏移

接下来得算逆运动学——也就是给定平台位姿，求六个舵机的伸缩量。这里有个向量旋转的技巧：

function leg_lengths = inverse_kinematics(pose, R_base, R_plat)
    Tx = pose(1); Ty = pose(2); Tz = pose(3);
    alpha = pose(4); beta = pose(5); gamma = pose(6);
    
    R = eul2rotm([gamma, beta, alpha], 'ZYX');
    base_points = R_base * [cos(theta_base(1:6))' sin(theta_base(1:6))' zeros(6,1)];
    plat_points = R_plat * [cos(theta_plat(1:6))' sin(theta_plat(1:6))' zeros(6,1)];
    
    leg_vectors = (R * plat_points' + [Tx; Ty; Tz])' - base_points;
    leg_lengths = vecnorm(leg_vectors, 2, 2);
end

这段代码的核心思想是把动平台坐标系下的安装点先旋转再平移，和基座安装点做向量差。注意eul2rotm这个函数，它把欧拉角转成旋转矩阵的顺序很重要，这里用的是Z-Y-X顺序，和大多数机器人标准一致。

动力学仿真部分得考虑质量惯性。假设平台质量20kg，转动惯量按均质圆盘算：

mass = 20;
inertia = diag([0.5*mass*R_plat^2, 0.5*mass*R_plat^2, mass*R_plat^2]);

舵机控制用个简单的PID，但得注意六个舵机要独立控制。这里展示其中一个通道的控制器：

Kp = 150; Ki = 2; Kd = 30;
prev_error = 0;
integral = 0;

function force = pid_control(target_len, actual_len, dt)
    error = target_len - actual_len;
    integral = integral + error * dt;
    derivative = (error - prev_error) / dt;
    force = Kp*error + Ki*integral + Kd*derivative;
    prev_error = error;
end

PID参数调试是个玄学，建议先用Ziegler-Nichols法粗调。注意这里每个执行器都要维护自己的积分项和上次误差，别用全局变量搞串了。

最后整个仿真循环大概长这样：

dt = 0.001; % 时间步长
current_pose = [0 0 1 0 0 0]; % 初始高度1米
target_pose = [0.1 0 1.05 pi/12 0 0]; % 目标位姿

for t = 0:dt:2
    target_lengths = inverse_kinematics(target_pose, R_base, R_plat);
    current_lengths = inverse_kinematics(current_pose, R_base, R_plat);
    
    % 计算每个执行器所需力
    forces = arrayfun(@(i) pid_control(target_lengths(i), current_lengths(i), dt), 1:6);
    
    % 动力学计算更新位姿（这里需要实现牛顿-欧拉方程）
    [acc, angular_acc] = dynamics_model(forces, current_pose);
    current_pose(1:3) = current_pose(1:3) + current_pose(1:3)*dt + 0.5*acc*dt^2;
    current_pose(4:6) = current_pose(4:6) + current_pose(4:6)*dt + 0.5*angular_acc*dt^2;
end

实际应用中动力学模型那部分得展开写，涉及雅可比矩阵和广义力转换。不过初步仿真可以先忽略复杂动力学，用一阶惯性环节近似。

仿真时容易遇到的坑：1) 奇异位形导致雅可比矩阵不可逆 2) 舵机推力饱和 3) 欧拉角万向锁。建议用四元数表示旋转，不过MATLAB的机器人工具箱处理这些还算方便。

可视化部分用plot3搞个动态显示挺直观：

h = plot3(base_points(:,1), base_points(:,2), base_points(:,3), 'ro');
hold on
plat_plot = plot3(plat_points(:,1), plat_points(:,2), plat_points(:,3), 'bo');
leg_plots = line([base_points(:,1) plat_points(:,1)]',...
                 [base_points(:,2) plat_points(:,2)]',...
                 [base_points(:,3) plat_points(:,3)]');
axis equal

跑起来能看到六个支腿像机械蜘蛛的腿一样协调运动，强迫症患者表示极度舒适。仿真做到这个程度，基本上可以拿去和实际平台参数做对比验证了，不过记得真实舵机有响应延迟，仿真时最好加个一阶滞后环节。