Gough-Stewart平台逆运动学算法详解与MATLAB仿真实现

前阵子做并联运动平台的概念验证，第一道坎就是把Gough-Stewart平台的逆运动学在MATLAB里完整跑通。Gough-Stewart平台在国内通常叫斯图尔特平台，是六自由度并联机器人的经典构型——一个固定底座、一个动平台、六根可伸缩支腿，末端可以同时控制X、Y、Z三个方向的平移和绕三个轴的旋转。这个仿真最吸引人的地方在于，它的逆运动学推导非常干净，没有串联机械臂那种三角函数耦合爆炸，只要你把几何模型摆正，剩下的就是坐标变换和两点间距离公式。

这篇文章我准备按自己实际做项目的过程来写：先解释为什么并联机构的逆解这么特殊，再把建模时那些容易被忽略的几何细节讲透，接着给出完整的MATLAB逆解函数与动画实现，最后分享我怎么验证结果对不对，以及从逆解往工作空间分析和雅可比矩阵延伸的思路。无论你是做毕业设计还是工程预研，按照这套流程走一遍，基本能把六自由度并联平台的逆解从原理到代码都吃透。

1. 为什么并联机构是"逆解容易、正解难"：先把这个绕清楚

1.1 Gough-Stewart平台到底是什么

这个平台的结构不复杂：底座和动平台各自有六个铰点，下方六个铰点通过万向铰或虎克铰与支腿底部相连，上方六个铰点通过球铰与支腿顶部相连。六根支腿自身可以伸缩，通常由电动缸、液压缸或丝杠驱动。动平台相对底座有六个自由度，这六个自由度分别对应笛卡尔空间里的三个平移和三个旋转。

很多人第一次接触并联机器人会有一个直观疑问：六根腿同时伸缩，平台到底怎么动？其实每根腿的长度一旦确定，动平台的位姿在理论上就是唯一确定的（忽略装配误差和奇异位形），因为六根腿就是对动平台的六个约束方程。这也是并联机构刚度大的根本原因——负载分散在六根腿上，而且每根腿沿轴向受力，几乎没有悬臂弯曲。

1.2 串并联逆解难度相反的原因

我当年先从六轴串联机械臂入门，被逆解折磨得不轻：6自由度串联臂的逆解要面对多解、无解、奇异位形，还经常需要解析法加数值法混用。原因在于串联机器人的关节角通过一大堆三角函数和连杆长度层层叠加，最终映射到末端位姿，逆过来就是解非线性方程组。

并联机器人和它正好相反。给定位姿求腿长，思路是：把动平台期望位置和姿态给定，动平台上的六个铰点在基座坐标系下的坐标就全部确定了，基座铰点是固定不变的，于是每根腿的长度就是两个铰点之间的直线距离。六根腿的求解过程完全独立，不存在联立方程。

串联是"正解容易逆解难"，并联是"逆解容易正解难"。正解——给定六根腿长求平台位姿——才是真正棘手的方向，因为腿长通过六个非线性约束同时决定位姿，需要数值迭代求解。理解了这一点，你就能明白为什么绝大多数并联平台运动控制器都是基于逆解做实时轨迹规划的，正解反而留给标定和位姿估计用。

1.3 这个仿真到底能做什么

如果你刚接触并联机构，我的建议是先别急着上Adams或者Simulink的多体工具箱，先用MATLAB把逆解、可视化、工作空间这三件事跑通。原因很简单：逆解是整个并联控制算法的最小闭环核心，动画能让你直观验证运动学是否合理，工作空间扫描则能告诉你哪些位姿能到、哪些不能到。一旦这三块做熟了，后面接动力学、接控制算法都有清晰的地基。

这篇文章适合三类人：准备做并联机器人毕业设计的学生、想评估Gough-Stewart平台构型的工程师、以及单纯想把MATLAB可视化与机构运动学结合练习的爱好者。我会尽量把每一步的"为什么"也讲清楚，不只是丢代码给你抄。

2. 建模前的几何地基：铰点坐标、坐标系与姿态角约定

2.1 结构参数定义

在推公式之前，先把几何参数确定下来。为了让仿真结果有对称性、便于验证，我选择了一组比较典型的尺寸参数，单位统一用米，角度统一用弧度。

这里上下铰点的布置角都按6个铰点均匀分布处理，也就是相邻铰点间隔60°。上下平台之间偏转30度错开，是为了让六根腿形成交叉支撑的几何关系。如果你把上下平台完全正对，支腿会变成竖直或近似竖直的组合，平台的抗扭转刚度会差很多。这个偏转角在建模阶段很容易被忽略，但直接影响后面逆解结果是否合理。

2.2 铰点坐标的计算方式

基座铰点在基座坐标系{B}下表达，动平台铰点先在动平台坐标系{P}下表达，后续再变换到基座坐标系。因为铰点都分布在一个圆上，用极坐标转直角坐标就行：

基座第i个铰点坐标（基座系）：
b_ix = Rb * cos(theta_b0 + (i-1)*pi/3)
b_iy = Rb * sin(theta_b0 + (i-1)*pi/3)
b_iz = 0

动平台第i个铰点坐标（动平台系）：
p_ix = Rp * cos(theta_p0 + (i-1)*pi/3)
p_iy = Rp * sin(theta_p0 + (i-1)*pi/3)
p_iz = 0

注意这个"动平台系下坐标"是跟随着动平台一起运动的。当动平台平移旋转后，这些点要经过旋转矩阵和平移向量变换到基座坐标系，才算拿到真实位置。我在代码里把上下铰点都预先算成3×6矩阵，后续逆解函数只做坐标变换，不反复算三角函数。

2.3 姿态描述：ZYX欧拉角与旋转矩阵

动平台的姿态描述方式有很多种，四元数、旋转矩阵、欧拉角都行。做逆解时我推荐用ZYX欧拉角表示，也就是先绕Z轴旋转偏航角yaw，再绕新的Y轴旋转俯仰角pitch，最后绕新的X轴旋转横滚角roll。对应到MATLAB的rotx、roty、rotz函数，旋转矩阵写成：

R = rotz(yaw) * roty(pitch) * rotx(roll)

这里有一个必须说清楚的坑：MATLAB的rotx/roty/rotz默认是绕固定坐标系的轴旋转，还是绕自身坐标系的轴旋转？实际做下来你会发现，按照上面这个乘法顺序，R = Rz * Ry * Rx，等价于先绕固定坐标系X轴转roll、再绕固定坐标系Y轴转pitch、最后绕固定坐标系Z轴转yaw。也就是说，MATLAB这三个函数相乘得到的整体旋转，与"ZYX欧拉角（先z后y再x，绕动轴）"是等价的。很多人在这一步搞混，导致后续逆解平台姿态完全错乱。

我建议你在写代码之前先做一个最小验证：给roll=pi/2、pitch=yaw=0，看看动坐标系Z轴转到了哪个方向。如果与你的姿态定义一致，再继续往下写。

2.4 铰点配对与相位偏转：容易翻车的细节

上下各六个铰点，还需要确定第i根腿连接上铰点i和下铰点i，一一对应。这种1号连1号、2号连2号的配对方式最直观，也最容易检查。但如果你看到别家的图纸，铰点下标不一定按角度顺序排，这时候就需要仔细核对配对关系。

另一个容易翻车的地方是相位偏转。我上面取的θp0=30°，意味着动平台铰点相对基座铰点逆时针转了30度。如果你把上平台铰点设成0度和下平台对齐，逆解计算本身不会报错，但结构就退化成另一种构型，运动学和受力特性都不一样。所以仿真前先画一张俯视图，确认六个上铰点和六个下铰点在圆周上的交错关系，这一步能帮你省下大量调试时间。

3. 位置反解公式推导：六条支腿六个独立的长度方程

3.1 支腿向量方程的核心推导

整个逆解推导用一张纸就能写完。设动平台期望位姿为：位置向量t=[x,y,z]^T，姿态旋转矩阵为R（从动平台系到基座系的变换）。基座铰点坐标b_i是已知常数，动平台铰点在动平台系下的坐标p_i也是常数。

第一步，把动平台铰点变换到基座坐标系：

P_i = t + R * p_i

第二步，支腿向量等于动平台上铰点减去基座下铰点：

L_vec_i = P_i - b_i

第三步，腿长就是支腿向量的模：

L_i = ||L_vec_i||

如果你想顺便拿到驱动轴方向，支腿单位向量可以直接算：

u_i = L_vec_i / L_i

这一步就是完整的逆运动学。每根腿的计算只依赖自己的两个铰点，与其他腿无关，所以六根腿的求解天然可以并行。这也是为什么并联平台控制器能做到非常高的刷新率——逆解的计算量其实很小。

3.2 用MATLAB实现通用逆解函数

我把逆解封装成函数，输入是6×1的位姿向量（x,y,z,roll,pitch,yaw），输出是六根腿长。结构参数放在结构体params中，这样换平台只需要改参数表，不需要动函数本体。

function [L, u, P] = stewart_inverse_kinematics(pose, params)
    % 输入: pose = [x; y; z; roll; pitch; yaw]
    % 输出: L    6x1 腿长
    %       u    3x6 支腿单位向量
    %       P    3x6 动平台铰点在基座系下的坐标

    x = pose(1); y = pose(2); z = pose(3);
    roll = pose(4); pitch = pose(5); yaw = pose(6);

    % 姿态旋转矩阵，ZYX顺序
    R = rotz(yaw) * roty(pitch) * rotx(roll);
    t = [x; y; z];

    P = zeros(3, 6);
    L = zeros(6, 1);
    u = zeros(3, 6);

    for i = 1:6
        P(:, i) = t + R * params.Po_i(:, i);
        leg_vec = P(:, i) - params.Bo_i(:, i);
        L(i) = norm(leg_vec);
        u(:, i) = leg_vec / L(i);
    end
end

调用前需要先构建params结构体，把上下铰点矩阵算好：

% 结构参数
params.Rb = 0.45;
params.Rp = 0.30;
params.h0 = 0.75;

% 基座铰点（基座系）
theta_b = 0:60:300;
params.Bo_i = [params.Rb * cosd(theta_b); ...
               params.Rb * sind(theta_b); ...
               zeros(1, 6)];

% 动平台铰点（动平台系）
theta_p = 30:60:390;
params.Po_i = [params.Rp * cosd(theta_p); ...
               params.Rp * sind(theta_p); ...
               zeros(1, 6)];

3.3 为什么逆解函数要返回单位方向向量

很多逆解资料只输出腿长就结束了，但我习惯把单位方向向量u也一并返回。原因有两个：第一，单位方向向量就是腿的轴向，后续做雅可比矩阵时每个元素都要用到u；第二，做碰撞检查或者画图时，需要知道支腿的方向和长度，不只有长度这一个标量。

从向量层面理解逆解还有一个好处：你能直观地看到每根腿的"拉伸方向"。比如平台做Z轴平移时，六根腿的u向量变化基本上是同步的，说明各腿受力比较均匀；而当平台绕某个轴大角度旋转时，某些腿的u向量会倾斜得很厉害，这就是铰链转角很快逼近极限的前兆。逆解算出来容易，但看懂支腿方向的变化，才能用好这个结果。

4. 从单点逆解到动画仿真：MATLAB实现全过程

4.1 零位单点逆解测试

先跑最基础的测试：让动平台处于零位，即x=0、y=0、z=0.75、roll=pitch=yaw=0。此时动平台和底座平行，理论上六根腿长度应该完全一样。

pose0 = [0; 0; params.h0; 0; 0; 0];
L0 = stewart_inverse_kinematics(pose0, params);
disp(L0');

由于上下铰点相对偏转30°，且半径不同，零位腿长不是简单的高度差0.75，而是要加上水平方向的差值。按我给的参数算出来六根腿都是0.7882m左右，数值一致性可以验证几何建模是否正确。如果六根腿长度不一致，别急着改代码，先检查铰点角度是否写错，尤其是deg与rad混用非常常见。

4.2 连续轨迹规划与批量逆解

单点逆解只能说明函数能跑，真正体现价值的是连续轨迹。我设计了一个螺旋上升加姿态摆动的复合轨迹：动平台中心沿螺旋线运动，同时姿态做小幅正弦摆动。时间10秒，采样步长0.01秒，总共1001个采样点。

dt = 0.01;
t = 0:dt:10;
N = length(t);

% 轨迹定义
r_traj = 0.12;          % 螺旋半径
z_traj = params.h0 + 0.1 * sin(0.5 * t);
x_traj = r_traj * cos(0.8 * t);
y_traj = r_traj * sin(0.8 * t);
roll_traj = 0.15 * sin(0.6 * t);
pitch_traj = 0.10 * cos(0.4 * t);
yaw_traj = 0.20 * sin(0.3 * t);

Legs = zeros(6, N);
for k = 1:N
    pose_k = [x_traj(k); y_traj(k); z_traj(k); ...
              roll_traj(k); pitch_traj(k); yaw_traj(k)];
    Legs(:, k) = stewart_inverse_kinematics(pose_k, params);
end

得到Legs矩阵后，我会先画出六条腿长随时间的变化曲线。曲线平滑连续，说明轨迹设计合理没有突变；如果某条腿长度剧烈跳变，往往意味着轨迹穿过了奇异位形或者铰点极限。这一步在仿真里非常便宜，但能提前暴露问题。

4.3 三维可视化与动画录制

数值求解只是第一步，没有三维动画的逆解仿真总觉得缺一口气。可视化我分成三层：底盘、动平台、六根支腿。底盘和动平台用六边形patch绘制，支腿用line绘制，并让图形随着位姿逐帧更新。

function draw_stewart(pose, params, ax)
    % 逆解拿到铰点位置和腿长
    [~, ~, P] = stewart_inverse_kinematics(pose, params);
    B = params.Bo_i;

    cla(ax);
    hold(ax, 'on');

    % 基座六边形
    patch(ax, B(1, :)', B(2, :)', B(3, :)', [0.8 0.8 0.8]);
    % 动平台六边形
    patch(ax, P(1, :)', P(2, :)', P(3, :)', [0.2 0.6 1.0]);
    % 六根支腿
    for i = 1:6
        line(ax, [B(1, i), P(1, i)], ...
                 [B(2, i), P(2, i)], ...
                 [B(3, i), P(3, i)], 'LineWidth', 2, 'Color', [0.85 0.33 0.1]);
    end
    % 铰点标记
    plot3(ax, B(1, :)', B(2, :)', B(3, :)', 'ko', 'MarkerSize', 6);
    plot3(ax, P(1, :)', P(2, :)', P(3, :)', 'bo', 'MarkerSize', 6);

    axis(ax, 'equal');
    xlabel(ax, 'X'); ylabel(ax, 'Y'); zlabel(ax, 'Z');
    grid(ax, 'on');
    view(ax, 35, 25);
    hold(ax, 'off');
end

动画主循环只需要把轨迹位姿一帧帧传入绘图函数，并加上drawnow刷新。实测下来，1000帧左右、每帧画6条线和2个多边形，MATLAB实时渲染完全跑得动，不需要额外用moviein录制。如果你想导出视频，用VideoWriter逐帧写入即可。

有一点要提醒：MATLAB的绘图默认坐标轴比例不是等比例的，画三维机构时一定要axis equal，否则你会看到平台被"压扁"或者"拉长"，严重影响对运动轨迹的判断。

5. 逆解结果对不对？三种交叉验证方法

5.1 零位对称性检查

第一种验证最简单，也是最容易发现低级错误的手段。在零位姿态下，如果结构参数是对称的，六根腿长必须严格一致。我甚至可以手工推导零位腿长公式来校验：

水平方向两铰点距离：

d = sqrt(Rb^2 + Rp^2 - 2 * Rb * Rp * cos(30°))

零位腿长：

L0 = sqrt(h0^2 + d^2)

代入参数：d≈0.2423m，L0≈0.7882m，与代码输出一致。如果你的参数不是对称布置，这个测试就不适用，但对称构型用这个检查百试百灵。

5.2 平移与旋转的物理直观验证

第二种方法是利用对称构型的特性做物理直觉检验。比如让平台只沿X轴正向平移0.1m，保持姿态不变，观察六根腿长变化。因为平台前后对称，按我的铰点编号，1号腿和4号腿的变化应该方向相反、数值接近相等；2号腿和5号腿变化也应该符号相反。

再比如平台绕Z轴转一个很小的角度，由于上铰点偏转30°，腿长变化也会出现特定的对称规律。这一步不是精确计算，而是快速排除"旋转方向反了""铰点配对错了"这类低级错误。我每次改完结构参数，都会先跑这两种检验，感觉对了再继续。

5.3 用正解做闭环互校

零位验证和直观验证都通过后，更严谨的手段是用正解闭环互校。思路很直接：先用逆解求某位姿对应的六根腿长，然后以六根腿长为输入，用数值方法反求位姿，再对比两个位姿是否一致。

正解可以用fsolve实现，未知量就是6个位姿参数，目标函数是逆解算出来的腿长与目标腿长的差：

options = optimoptions('fsolve', 'Display', 'off', ...
                       'Algorithm', 'levenberg-marquardt');
q_est = fsolve(@(q) stewart_inverse_kinematics(q, params) - L_target, ...
              q0, options);

q0是迭代初值，建议用零位位姿。由于并联平台正解存在多解，初值选择很关键。闭环互校能同时验证逆解函数和正解思路，我在做传感器数据融合仿真时也复用了这套代码，工程价值很高。

5.4 常见错误对照表

这张表是我从多次踩坑里总结的。其中欧拉角顺序错误最具迷惑性，因为平台做纯平移时完全正常，一旦加入旋转就乱套。所以记得在验证环节刻意设计一个带姿态变化的轨迹，别只在纯平移下测试。

6. 顺着逆解往前走：工作空间、雅可比与实物落差

6.1 用逆解做工作空间扫描

逆解最大的工程应用之一是分析工作空间。给定腿长范围，遍历动平台可能的位置和姿态，判断哪些位姿满足所有腿长约束。

二维扫描的代码核心并不复杂：

x_range = -0.35:0.02:0.35;
y_range = -0.35:0.02:0.35;
reachable = zeros(length(y_range), length(x_range));

for ix = 1:length(x_range)
    for iy = 1:length(y_range)
        pose = [x_range(ix); y_range(iy); params.h0; 0; 0; 0];
        L = stewart_inverse_kinematics(pose, params);
        if all(L > params.Lmin) && all(L < params.Lmax)
            reachable(iy, ix) = 1;
        end
    end
end
imagesc(x_range, y_range, reachable);

实际做工作空间分析时，维度灾难会很快出现。全姿态工作空间是6维的，无法直接可视化，常规做法是固定3到5个维度，扫剩下的1到3个维度。比如固定姿态，扫描X-Y平面；或者固定位置，扫描roll-pitch两个角度。这类分析依赖逆解函数的高效性——因为每个点都要调用多次，函数写得越精简，扫描越快。

6.2 从逆解到雅可比矩阵

逆解输出的腿长和单位方向向量，是推导雅可比矩阵的现成素材。对第i根腿，考虑动平台速度v和角速度ω，上铰点速度可以写为：

v_Pi = v + omega × (R * p_i)

支腿伸长速度等于支腿方向与端点速度的点积：

L_dot_i = u_i' * v_Pi

把六根腿写成一个线性方程组，就能得到从平台速度到腿速的映射，这个映射矩阵就是逆雅可比。有了它，你可以分析奇异位形、做力雅可比变换，还可以评估各方向上的刚度特性。雅可比矩阵的每一行由支腿单位向量和铰点位置叉积项构成，而这些量逆解函数里都已经算好，不需要额外求导，非常方便。

6.3 仿真到实物的几个落差

如果只是停在仿真层面，这篇内容其实已经功德圆满了。但如果你打算把逆解结果拿去搭实物样机，有几条实际落差需要提前有心理准备。

第一，铰链转角限制。逆解只保证腿长在范围内，但球铰和虎克铰都有最大摆动角。腿长没问题、铰链先撞限位的情况非常常见。所以实物设计时，仿真里还要加上"支腿单位方向向量与各铰点法向的夹角"这个约束条件。

第二，驱动映射。逆解输出的是腿长，而伺服电动缸的输入量是电机角度，中间还隔着减速器和丝杠导程。实际控制时要把腿长换算成电机位置指令，这步虽然只是乘一个比例系数，但传动间隙、丝杠反向间隙都会影响最终精度。

第三，标定与正解。实物肯定有加工装配误差，零位位置、铰点半径、高度都比图纸值有偏差。这时候正解就派上用场了——通过测量平台实际位姿和腿长数据，反求结构参数，再做参数标定。这也是为什么我建议你不要把正解丢在一边，它在实物阶段是你唯一能依赖的校准工具。

在我自己做完这套仿真后，最大的感受是：并联机构的逆解是一个"窗口很小、地基很深"的方向——公式推导只要半天，但几何建模、旋转矩阵约定、验证思路这些准备工作才真正决定项目成败。如果你在仿真的某一环节卡住了，不要急着怀疑公式，先回头检查铰点坐标和姿态约定，绝大多数问题都出在那里。沿着逆解这条主线，你后面再做工作空间分析、雅可比计算、动力学建模，甚至搭建样机控制，都会觉得轻车熟路。