简介：面向机器人控制与并联机构研究方向的学习者与技术爱好者，这份PDF文档围绕Stewart并联平台的运动学逆解与动力学仿真展开，从坐标变换、支腿长度计算到雅可比矩阵应用，逐步搭建逆解流程，并以牛顿-欧拉法建立动力学方程，配合Simscape Multibody完成三维仿真与实时可视化。压缩包内共1个pdf文件，约116KB，内容集中紧凑，便于随查随用；目前已有192人学习下载。文档内给出可直接在MATLAB环境运行的代码片段，涵盖欧拉角转旋转矩阵、向量化支腿长度计算、动力学求解以及绘图刷新等关键环节，并提示旋转顺序选取、雅可比矩阵数值验证、奇异位形检测与轨迹规划优化等常见易错点，帮助读者少走弯路，理解从理论推导到仿真验证的完整链路。

1. 逆解比正解简单：六条腿的长度为什么能一次算完

第一次把六条腿的约束方程全列出来，多数人会本能觉得耦合这么重，逆解必然得迭代。事实正相反：给定动平台位姿，六条腿的长度就是一次坐标变换加一次求模的闭式结果；真正难解的是正解——已知六条腿长度反推平台位姿，要解六个耦合非线性方程，一般只能上牛顿迭代。这个不对称性直接决定了整套 MATLAB 仿真的架构：运动学逆解被塞进实时控制回路，每个周期跑一遍也不心疼；正解只在标定和轨迹校验时出场。搞懂这个差别，雅可比矩阵为什么能写成解析形式、奇异位形为什么能用条件数预判、动力学为什么敢把广义坐标降回六维，才都有落脚点。所谓六自由度并联机构的运动学逆解，本质上就是一道几何题，难点不在解方程，而在把坐标系定义对齐。

2. 坐标变换的账本：动平台铰点是如何被搬进世界系的

逆解函数的第一行永远是旋转矩阵。这一步做错，后面所有腿长都是错的，而且错得很隐蔽——平台看起来还在动，只是姿态跟你想的不一样。逆解的物理图像其实很朴素：上平台六个铰点在自己体坐标系里是常量，给定位姿后把它们旋转、平移，搬到基座坐标系里，再跟下平台六个固定铰点做差，得到六条腿的向量，取模就是腿长。全部计算量的 90% 花在这本坐标变换的账上。

2.1 齐次变换与旋转顺序：ZYX 到底差在哪

最直接的写法是用齐次变换矩阵把旋转和平移打包成 4×4，再对每个铰点做一次矩阵乘。但实际工程里更常见的是拆开写：先算 3×3 旋转矩阵，再整体平移，因为vecnorm这类函数能一次性处理所有点。拆开写就得面对一个绕不过去的问题——欧拉角顺序。

function L = inverse_kinematics(pose, base_pts, top_pts) % pose: 1x6 = [x y z roll pitch yaw]，角度单位 rad % base_pts, top_pts: 6x3，每行一个铰点在各自体坐标系下的坐标 R = eul2rotm(pose([6 5 4]), 'ZYX'); % 关键：输入重排成 [yaw pitch roll] T = pose(1:3); % 1x3 平移向量 top_w = (R * top_pts')' + T; % 6x3，动系铰点搬到世界系 leg_vec = top_w - base_pts; % 6x3，六条腿的向量 L = vecnorm(leg_vec, 2, 2); % 6x1，沿第 2 维求二范数 end

这段代码里最容易被略过的是pose([6 5 4])这个重排。MATLAB 的eul2rotm(eul, 'ZYX')要求输入向量按 sequence 的字母顺序排列，'ZYX' 对应[zAngle yAngle xAngle]，也就是[yaw pitch roll]；而工程里习惯把位姿存成[x y z roll pitch yaw]。两者不一致，直接把pose(4:6)塞进去，等于把 roll 当成了绕 z 轴的角，平台的俯仰会变成自转。这种错误在静态图上看不出来，一动起来就是「平台跟抽风似的乱转」。

选哪个顺序本身没有对错，关键是整条链路必须一致：轨迹生成端、逆解端、可视化端、Simulink 里的姿态输入，四处只要有一处用了别的顺序，仿真结果就会以一种很难归因的方式偏掉。我一般会在项目里写一个pose2rot的统一入口，所有模块只调它，避免各处各自eul2rotm。

2.2 vecnorm 的维度约定：一次算完六条腿

vecnorm(A, 2, 2)的第三个参数是运算维度，2 表示沿第二维（列方向）求范数。对 6×3 的leg_vec来说，就是每一行三个分量求模，输出 6×1。如果铰点存成了 3×6（每列一个点），整个表达式就得改成vecnorm(leg_vec, 2, 1)，输出也是 1×6，再转置。维度约定这件事在 MATLAB 里翻车率极高，因为R * top_pts在 3×6 和 6×3 两种存法下都能跑通，只是结果完全不同，而且不会报错。

向量化的收益在六条腿这个规模上并不夸张，for循环和vecnorm的耗时差异在微秒级。真正拉开差距的场景是参数扫描：调轨迹、扫工作空间、做蒙特卡洛奇异分析时，逆解可能被调用几万次，这时候循环开销就显出来了。更实际的理由是可读性——一行vecnorm表达的是「六条腿同时算」，跟数学公式一一对应，改起来不容易错。

提示：写完逆解先做一次自检，把动平台位姿设成零位，六条腿长度应该全部相等；如果有哪条腿明显偏长或偏短，八成是某个铰点的坐标符号或相位写错了。

2.3 铰点布置：极坐标生成六边形与相位角

上下平台的铰点坐标是整个模型的几何基础。最常见的做法是用极坐标生成正六边形，再给上下平台各加一个相位偏移。

Rb = 0.5; Rt = 0.3; % 下、上平台铰点分布半径 th_b = deg2rad([0 60 120 180 240 300]' + 30); % 基座铰点相位 th_t = deg2rad([0 60 120 180 240 300]' - 30); % 动平台铰点相位错开 base_pts = [Rb*cos(th_b), Rb*sin(th_b), zeros(6,1)]; top_pts = [Rt*cos(th_t), Rt*sin(th_t), zeros(6,1)];

相位错开不是装饰。常见的 6-6 构型里，上下铰点如果完全对齐，腿在初始位形下会接近竖直，工作空间被压得很扁，而且零位附近的雅可比条件数容易偏高；把动平台铰点相对基座转一个角度，腿会向外张开，工作空间更饱满，受力也更均匀。另一种做法是把每对相邻铰点靠得很近，形成「三对」的布局，等效于 3-3 构型的变体，目的是减少腿之间的干涉。

半径比Rt/Rb决定了初始腿长和机构的整体刚度。比值太接近 1，腿几乎水平，轴向刚度差；比值太小，腿接近竖直，抗侧向能力弱。实际设计里通常在 0.5 到 0.8 之间取，再根据电缸行程反推可用工作空间。这一步在仿真里也要落地：算完腿长必须跟行程做比对，超程就该报错而不是让平台继续飞。

Lmin = 0.4; Lmax = 0.7; % 电缸有效行程，单位 m if any(L < Lmin) || any(L > Lmax) error('腿长 %.3f~%.3f 超出行程 [%.2f, %.2f]', min(L), max(L), Lmin, Lmax); end

这个检查放在逆解外层，比放在动力学里更早暴露问题。曾经有次跑螺旋上升轨迹，平台在 z 方向冲到 0.6 m 时腿长还没超限，但雅可比已经接近奇异，腿长的数值开始抖，加上这个检查之后能第一时间定位到是轨迹的问题而不是求解器的问题。

3. 雅可比矩阵：运动学与动力学之间的那道转置桥

运动学回答「腿该多长」，雅可比回答「平台动一点，腿长变多少」。这个映射在运动学里是速度关系，在动力学里是力的转置关系——腿上的推力乘J^T就变成作用在平台上的广义力。很多仿真跑不通，根子就在这个矩阵的方向和量纲没对齐。

3.1 解析法求 J：一行一个腿方向

对 Stewart 平台，雅可比有一行对应一条腿，形式非常干净：第 i 行是[u^T, (b × u)^T]，其中u是第 i 条腿的单位方向向量，b是动平台铰点相对动平台中心的位置向量。

function J = jacobian_analytic(pose, base_pts, top_pts) R = eul2rotm(pose([6 5 4]), 'ZYX'); p = pose(1:3); J = zeros(6,6); for i = 1:6 b = (R * top_pts(i,:)')'; % 铰点相对动平台中心，已转到世界系 l = p + b - base_pts(i,:); % 从基座指向动平台的腿向量 u = l / norm(l); % 单位方向 J(i,:) = [u, cross(b, u)]; % 上 3 列平动，下 3 列转动 end end

推导只用到一次点积：腿长变化率等于腿方向单位向量与铰点速度的点积，而铰点速度是v + ω × b，展开后平动部分给出u，转动部分给出b × u。注意b用的是相对动平台中心的向量，不是加完平移后的绝对位置，因为角速度是绕平台中心定义的。

解析解写完必须用数值差分交叉验证，这是血泪经验。差分版本不需要任何推导，把位姿的每个分量加一个小扰动，重算腿长，再除以扰动量：

function J = jacobian_numeric(pose, base_pts, top_pts) h = [1e-6 1e-6 1e-6 1e-6 1e-6 1e-6]; % 平动 m，转动 rad L0 = inverse_kinematics(pose, base_pts, top_pts); J = zeros(6,6); for k = 1:6 p2 = pose; p2(k) = p2(k) + h(k); J(:,k) = (inverse_kinematics(p2, base_pts, top_pts) - L0) / h(k); end end

两个矩阵相减，元素量级应该在 1e-6 以下。差分步长要按量纲分开给：平动用 1e-6 m，转动用 1e-6 rad，都用同一个步长会让转动列的相对误差变大。这个验证过程五分钟能做完，但能省掉后面几小时的排查。

3.2 奇异位形判据：行列式不够，条件数才靠谱

奇异位形是并联机构的死穴。到了那个位形，平台在某些方向上失去约束，腿长的微小变化会引起位姿的巨大跳变，数值上表现为解算器给出的腿长突然断裂。教科书上的判据是det(J) = 0，但直接拿行列式做阈值判断是个坑。

原因在量纲。J 的前三列是无量纲的方向余弦，后三列是b × u，带长度量纲。整个 6×6 矩阵的行列式量纲是长度的三次方，0.5 m 尺度的机构和 2 m 尺度的机构，同一个物理奇异点对应的行列式数值差 64 倍。写死abs(det(J)) < 1e-6这种判断，换个机构尺度就失效。

更稳的做法是看奇异值。最小奇异值接近零说明矩阵接近降秩，而条件数（最大最小奇异值之比）是无量纲的，跨尺度可比：

s = svd(J); if s(end) / s(1) < 1e-4 warning('接近奇异，最小奇异值 %.3e，条件数 %.3e', s(end), s(1)/s(end)); end

阈值 1e-4 不是绝对的，取决于控制精度要求。奇异位形分三类：正向奇异是平台自己获得了一个不受腿约束的自由度，反向奇异是某条腿锁死但平台还能动，组合奇异是两者同时发生。工程上最关心正向奇异，因为它在工作空间内部就能碰到，通常出现在平台抬得过高、腿接近水平或者某几条腿共面的位置。

3.3 阻尼最小二乘：在奇异点附近不炸

即使加了检测，轨迹贴着奇异点走的时候，用J \ dx求速度指令仍然会把结果放大到不可接受。经典的补救是阻尼最小二乘，把求逆改成解一个带正则项的法方程：

lambda = 1e-3; % 阻尼系数，从 1e-6 起步试 dx = [v_desired; w_desired]; % 期望的平台速度 dq = (J'*J + lambda^2*eye(6)) \ (J'*dx); % 阻尼最小二乘解

lambda的作用是在奇异值方向上加一个下限，避免除以接近零的数。代价是解的精度下降：正常位形下它给出的腿速比精确解略小，跟踪会有稳态误差。实际调参一般从 1e-6 开始，观察奇异点附近的腿速是否还在爆，再往上加；加到 1e-2 量级时，跟踪误差已经肉眼可见，说明轨迹本身该重规划了，而不是继续靠阻尼兜底。

注意：阻尼项写lambda^2*eye(6)还是lambda*eye(6)取决于你把 lambda 定义成阻尼因子还是阻尼因子的平方，两种写法都有人用，混用时数值会差三个数量级，代码里统一注释清楚。

4. 动力学建模：闭环机构为什么要把广义坐标降回六维

运动学只关心几何，动力学要回答「给多大的力，平台怎么动」。Stewart 平台的特殊之处在于它是闭环：六条腿和两个平台构成多个闭链，如果用 12 个关节坐标去描述，就得多带 6 个约束方程。常见做法是把广义坐标直接取成动平台的六维位姿，让几何约束自动满足，代价是质量矩阵变成位形相关的稠密矩阵。

4.1 广义坐标的选择：6 维任务空间 vs 12 维关节空间

选 6 维位姿坐标的好处很直接：状态变量就是[x y z roll pitch yaw]加它们的时间导数，跟控制器的输入输出一一对应，不用处理约束力的拉格朗日乘子。腿长和腿速都能通过逆解和雅可比从位姿推出来，闭环约束变成了一层代数映射。动力学方程写成任务空间形式：

function [M, tau_g] = dynamics_terms(q, m, I, J) % q: 1x6 位姿，m: 动平台质量，I: 3x3 惯量张量，J: 6x6 雅可比 Jv = J(:,1:3); % 平动映射 Jw = J(:,4:6); % 转动映射 g = [0; 0; -9.81]; M = m * (Jv' * Jv) + Jw' * I * Jw; % 任务空间质量矩阵（暂忽略腿质量） tau_g = -Jv' * (m * g); % 重力对应的广义力 end

M是 6×6，由动平台的平动动能和转动动能经雅可比映射得到。这个形式的好处是它跟雅可比共用同一套几何，运动学改了几何参数，动力学自动跟着变，不用重推公式。缺点是腿的质量和惯量被忽略了——高速运动时腿的动能占比不可忽视，需要额外折合进来。

4.2 质量矩阵与力的映射：J 的转置在这里起作用

动力学方程组装起来是M(q) q̈ + C(q, q̇) q̇ + G(q) = J^T F，右边是六条腿的驱动力经J^T映射成的广义力。这里的方向必须跟运动学对齐：如果雅可比定义成L̇ = J q̇（腿速等于 J 乘平台速度），那么力映射就是J^T F；如果当初把 J 定义反了，力映射就得用伪逆，整套动力学都会歪。

求解加速度就是一次线性方程：

F = [f1; f2; f3; f4; f5; f6]; % 六条腿的驱动力 qdd = M \ (J' * F - tau_g - tau_c); % tau_c 为科氏与离心项

科氏项tau_c来自质量矩阵对时间的导数，数值上可以用M的差分或者解析地由雅可比的时间导数组装，前者实现简单但每个步长要多算几次M，后者推导麻烦但跑得快。仿真阶段用差分足够，实时控制里一般会换成解析式。

摩擦项是另一个容易被低估的部分。库伦摩擦在低速换向时会造成明显的定位偏差，仿真里如果不建模，控制器调出来的参数到了实物上会明显过冲。常见的处理是在每条腿的驱动力上加一项fc * sign(v_leg) + b * v_leg，fc和b靠实验辨识，仿真阶段先给个量级估计。

4.3 Simscape Multibody：闭环约束的坑与适用边界

自建方程可控性最好，但建模工作量大，尤其是腿的惯量和关节摩擦，手推一遍容易出错。Simscape Multibody 走的是另一条路：把上下平台和六条腿当作刚体，用关节块连接，闭环自动由求解器处理。

smimport('stewart_assembly.xml'); % 从 CAD 导入装配体 set_param('stewart_model/Revolute_Joint','Motion','Provided by Input');

导入之后最容易崩的地方是闭环约束。六条腿同时连接两个平台，构成了过约束或者接近过约束的拓扑，求解器在初始位形稍微不协调时就会报「约束冲突」。常见的处理是把其中几个关节改成柔性连接，或者先让平台在自由状态下沉降到平衡位置，再切到闭环。用 Mechanics Explorer 看机构的运动是最快的排查方式：哪个关节在抖或者飞出屏幕，问题就在那条腿上。

Simscape 的另一个隐形成本是求解器。闭环刚体系统是刚性的，显式求解器步长会被压到极小，跑十秒仿真要几分钟。换成ode15s或者ode23t之后速度能回来，但需要给雅可比矩阵的稀疏模式，否则每个步长都在做稠密分解。这也是为什么很多项目最终回到自建方程：状态维度只有 12，求解速度比通用多体求解器快一个量级。

5. 避坑与排查：五条让仿真跑飞的现场记录

仿真这件事，数学推导只占一小半，剩下的时间基本在跟各种不报错但结果不对的情况较劲。下面五条是这类项目里反复出现的翻车现场，每条都按现象、原因、处理三段说清楚。

5.1 单位制与铰点坐标系：仿真结果直接飞天

现象：模型参数看起来都对，一跑仿真平台瞬间飞到几十米外，或者腿长出现负值。

原因：CAD 导出模型时单位是英寸或者毫米，导入 Simscape 后跟 MATLAB 工作区里的米制参数混在一起。另一个高频原因是铰点坐标系的定义方向：动平台铰点定义在体坐标系里朝上，基座铰点定义在世界系里也朝上，两者做差时 z 分量符号反了，腿向量指向平台内部。

处理：在项目根目录放一个params.m，所有几何量在这里统一换算成米并注释来源，其他脚本只引用不重定义。铰点定义完之后跑一次零位检查，六条腿长度相等且方向朝外，这一步能挡掉大部分坐标系错误。

5.2 旋转顺序与镜像：平台姿态对不上

现象：给平台一个纯俯仰指令，实际动画里平台却在自转，或者俯仰方向跟预期相反。

原因：eul2rotm的输入顺序跟位姿向量的存储顺序不一致，或者轨迹生成端用了 'XYZ' 而逆解端用了 'ZYX'。镜像问题则来自铰点相位：上下平台的相位偏移符号写反，机构变成镜像对称，运动学上腿长仍然算得出来，但受力方向全反。

处理：固定一个位姿存储约定，在文档里写死，所有函数入口做一次断言检查。姿态验证用三个基元动作分别测试：纯滚转、纯俯仰、纯偏航，每个动作单独看动画，确认旋转轴和方向都正确再往下走。

5.3 奇异位形与积分步长：腿长曲线突然断裂

现象：轨迹跑到某个位置，腿长曲线出现锯齿或者直接跳到另一个分支，求解器步长被压到极小，仿真卡住。

原因：轨迹穿过了奇异位形附近，雅可比接近降秩，速度指令被放大；或者定步长积分器在刚性环节上失稳，误差累积后数值发散。两者经常同时出现，因为奇异点附近的动力学本身就变刚。

处理：先画雅可比最小奇异值随轨迹的变化曲线，确认是否贴到了奇异点。如果是轨迹问题，换插值方式或者调整工作空间；如果是积分问题，把求解器换成ode15s，并给相对容差设到 1e-6 量级。定步长仿真里把步长从 1 ms 降到 0.5 ms 通常能缓解，但治标不治本。

5.4 可视化性能：动画卡成幻灯片

现象：动力学算得很快，但一开动画整个仿真就慢十倍，刷新率掉到个位数。

原因：每一帧都用plot3重新绘制所有线条，图形对象反复创建销毁；或者用了drawnow而不是drawnow limitrate，强制每次刷新都走完整的渲染管线。

处理：图形对象只创建一次，后续用set改XData/YData/ZData，或者用hgtransform把整个平台挂到一个变换节点下，每帧只更新一个 4×4 矩阵。刷新用drawnow limitrate，它会把高频更新合并到显示器的刷新节奏，实测在 100 Hz 的仿真步长下动画仍然流畅。

h = hgtransform('Parent', ax); patch('Vertices', top_pts, 'Faces', 1:6, 'Parent', h, 'FaceAlpha', 0.5); % 每帧只改一个矩阵 set(h, 'Matrix', makehgtform('translate', p, 'euler', [roll pitch yaw])); drawnow limitrate;

5.5 符号推导与递归：内存爆掉和栈溢出

现象：用syms推动力学方程，推到一半 MATLAB 卡死或者报内存不足；或者逆解函数里不小心写成了自调用，运行直接栈溢出。

原因：六条腿的闭环动力学符号展开后项数爆炸，中间表达式没有及时化简，内存占用呈指数增长。递归则是复制粘贴时的疏忽，函数名没改。

处理：符号推导只用来验证小规模结论，比如单条腿的雅可比行向量，验证完立刻转成数值函数。完整动力学用数值组装，质量矩阵按支腿逐项累加，避免一次性展开。函数写完用mlint扫一遍，自调用这类问题它会直接标出来。

6. 仿真到实物：几处没法在 MATLAB 里验证的取舍

仿真跑通之后最容易产生的错觉，是以为实物也会一样听话。实际情况是，模型里被忽略的那些项，到了实物上会以另一种方式找回来。这里有几处取舍，我在项目里反复权衡过。

第一处是模型精度和实时性的取舍。把腿的质量、关节摩擦、液压阻尼全建进去，模型确实更准，但状态维度和刚性都上去了，实时控制回路里跑不动。常见的折中是分层：控制器内部用简化模型，只保留质量矩阵和重力项，摩擦当作前馈补偿；离线分析用完整模型，用来评估控制器在极端工况下的余量。两套模型的几何参数必须共享同一份定义，否则分析结果和实际行为对不上。

第二处是闭环约束的处理方式。Simscape 那种「拖拽建模、求解器管约束」的路线，上手快，但一旦出问题很难定位到具体方程；自建方程可控，但每加一个物理效应都要重推。我的经验是先用 Simscape 快速验证拓扑和运动是否合理，确认几何没问题之后，再把动力学换成自建方程，两边用同一组轨迹对比平台位姿，差异应该在小量级以内。

第三处是仿真里没法验证的东西。关节间隙、连杆柔性、传感器噪声、电缸的死区，这些在纯数值模型里要么没有，要么只能用一个粗糙的随机项代替。它们的共同特点是低频、小幅、难以解析建模，但在实物上会直接决定定位精度。可行的做法是留出辨识接口：仿真里给每个未建模效应留一个可调参数，实物调试时用实验数据反辨识，再把辨识结果填回仿真做闭环验证。

从那以后我每次开一个新机构项目，都会强制走一遍「零位自检 → 数值差分验证雅可比 → 奇异曲线扫描 → 单位制统一检查」这套流程，哪怕看起来浪费时间，也比后期在一堆互相矛盾的曲线里找原因强。这套东西没有捷径，能少踩一个坑就是一个。希望帮到你。

本文还有配套的精品资源，点击获取