1. Stewart平台运动学建模与仿真

任务目标

建立Stewart平台（6-SPS并联机构）的运动学模型，实现位置逆解（由动平台位姿求六支腿长度）与位置正解（由六支腿长度求动平台位姿），并在MATLAB中通过数值仿真验证模型正确性。

环境依赖

• MATLAB R2019b及以上（仅需基础模块，无需额外工具箱）
• 操作系统：Windows/Linux/macOS均可

运动学模型说明

采用6-SPS构型：静平台6个铰点均匀分布在半径\(R_b\)的圆上，动平台6个铰点均匀分布在半径\(R_p\)的圆上，上下铰点按角度错开布置。逆解：给定动平台位置\([x,y,z]\)与姿态（ZYX欧拉角\([\alpha,\beta,\gamma]\)），计算旋转矩阵\(\mathbf{R}\)，各支腿向量\(\mathbf{l}_i = \mathbf{R}\mathbf{p}_i + \mathbf{t} - \mathbf{b}_i\)，腿长\(L_i = \|\mathbf{l}_i\|\)。正解：采用牛顿-拉夫逊迭代，以逆解为初值，通过数值雅可比迭代逼近满足六条腿长约束的位姿。

关键完整代码（stewart_kinematics.m）

function stewart_kinematics()
% Stewart平台运动学建模与仿真（逆解+正解验证）
clear; clc;

% ---------- 几何参数 ----------
Rb = 1.0;   % 静平台铰点分布圆半径
Rp = 0.6;   % 动平台铰点分布圆半径
theta_b = 0; % 静平台第1铰点起始角(rad)
theta_p = pi/6; % 动平台第1铰点相对错位角(rad)

% 铰点角度（6个均匀分布，相邻间隔60°）
angles_b = theta_b + (0:5)*pi/3;
angles_p = theta_p + (0:5)*pi/3;

% 静平台铰点坐标 (6x3)
B = [Rb*cos(angles_b)', Rb*sin(angles_b)', zeros(6,1)];
% 动平台铰点在局部坐标系坐标 (6x3)
P_local = [Rp*cos(angles_p)', Rp*sin(angles_p)', zeros(6,1)];

% ---------- 目标位姿（用于验证） ----------
t_true = [0.1; 0.15; 0.8];          % 位置
euler_true = [5*pi/180; 8*pi/180; -3*pi/180]; % ZYX欧拉角(alpha,beta,gamma)
R_true = eul2rotm(euler_true, 'ZYX');

% ---------- 位置逆解 ----------
L = zeros(6,1);
for i = 1:6
    L(i) = norm(R_true*P_local(i,:)' + t_true - B(i,:)');
end
fprintf('逆解得到的六支腿长度:\n');
disp(L');

% ---------- 位置正解（牛顿-拉夫逊迭代） ----------
% 以逆解结果L为输入，初值取静平台上方某点
x0 = [0; 0; 0.7; 0; 0; 0]; % [x,y,z,alpha,beta,gamma]
x = x0;
tol = 1e-10;
max_iter = 100;
for iter = 1:max_iter
    % 当前位姿计算腿长
    R_cur = eul2rotm(x(4:6)', 'ZYX');
    L_cur = zeros(6,1);
    for i = 1:6
        L_cur(i) = norm(R_cur*P_local(i,:)' + x(1:3) - B(i,:)');
    end
    f = L_cur - L;  % 残差
    
    if norm(f) < tol
        break;
    end
    
    % 数值雅可比 (6x6)
    J = zeros(6,6);
    delta = 1e-6;
    for j = 1:6
        xp = x; xp(j) = xp(j) + delta;
        R_p = eul2rotm(xp(4:6)', 'ZYX');
        L_p = zeros(6,1);
        for i = 1:6
            L_p(i) = norm(R_p*P_local(i,:)' + xp(1:3) - B(i,:)');
        end
        xm = x; xm(j) = xm(j) - delta;
        R_m = eul2rotm(xm(4:6)', 'ZYX');
        L_m = zeros(6,1);
        for i = 1:6
            L_m(i) = norm(R_m*P_local(i,:)' + xm(1:3) - B(i,:)');
        end
        J(:,j) = (L_p - L_m)/(2*delta);
    end
    
    dx = -J\f;
    x = x + dx;
end

% ---------- 结果输出与验证 ----------
fprintf('\n正解迭代次数: %d\n', iter);
fprintf('正解位置误差: [%.2e, %.2e, %.2e] m\n', abs(x(1:3)-t_true'));
fprintf('正解姿态误差: [%.2e, %.2e, %.2e] rad\n', abs(x(4:6)-euler_true'));

% 验证：用正解结果重新计算腿长，与输入L对比
R_solved = eul2rotm(x(4:6)', 'ZYX');
L_check = zeros(6,1);
for i = 1:6
    L_check(i) = norm(R_solved*P_local(i,:)' + x(1:3) - B(i,:)');
end
fprintf('腿长最大反算误差: %.2e m\n', max(abs(L_check - L)));

% ---------- 可视化 ----------
figure('Color','w','Position',[100 100 800 600]);
hold on; axis equal; grid on; view(45,30);
% 绘制静平台
plot3(B(:,1), B(:,2), B(:,3), 'ko', 'MarkerSize', 8, 'LineWidth', 1.5);
for i = 1:6
    plot3([B(i,1) B(mod(i,6)+1,1)], [B(i,2) B(mod(i,6)+1,2)], ...
          [B(i,3) B(mod(i,6)+1,3)], 'k-', 'LineWidth', 1.2);
end
% 绘制动平台（正解结果）
P_world = (R_solved*P_local')';
plot3(P_world(:,1), P_world(:,2), P_world(:,3), 'ro', 'MarkerSize', 8, 'LineWidth', 1.5);
for i = 1:6
    plot3([P_world(i,1) P_world(mod(i,6)+1,1)], ...
          [P_world(i,2) P_world(mod(i,6)+1,2)], ...
          [P_world(i,3) P_world(mod(i,6)+1,3)], 'r-', 'LineWidth', 1.2);
end
% 绘制支腿
for i = 1:6
    plot3([B(i,1) P_world(i,1)], [B(i,2) P_world(i,2)], [B(i,3) P_world(i,3)], ...
          'b-', 'LineWidth', 1.5);
end
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('Stewart平台运动学模型验证');
legend('静平台铰点','静平台','动平台铰点','动平台','支腿', 'Location', 'best');
hold off;
end

运行方式

将上述代码保存为 stewart_kinematics.m，在MATLAB命令行直接运行：

>> stewart_kinematics

验证方法与判定标准

• 逆解正确性：程序输出的六支腿长度均为正实数，且数值在合理范围内（本参数下约0.5~1.5 m）。
• 正解收敛性：命令行显示迭代次数小于100（通常10次以内收敛）。
• 精度判定：正解位置误差各分量小于1e-6 m，姿态误差各分量小于1e-6 rad，腿长最大反算误差小于1e-8 m，即视为模型正确。
• 可视化确认：图形窗口显示上下两个六边形平台，六条支腿连接对应铰点，无交叉错连，动平台位于静平台上方。

常见问题

• 正解不收敛：调整初值x0为更接近真实位姿的值，或增大max_iter。
• 欧拉角奇异：当β接近±90°时ZYX欧拉角存在奇异，本案例验证位姿已避开奇异区域。
• 铰点角度错位：θ_p取π/6是常用布置，若取0会导致机构奇异，逆解矩阵病态。