企业官网建设流程全解析

简介：基于MATLAB的Stewart并联平台运动学逆解与动力学仿真研究PDF文档，面向机器人控制与并联机构研究方向的科研人员和技术爱好者。内容以运动学逆解为基础，详解坐标变换、支腿长度计算和雅可比矩阵推导，并结合牛顿-欧拉法建立动力学方程；在仿真层面引入Simscape Multibody与Simulink混合仿真，提供实时可视化绘图代码，便于从理论推导平滑过渡到三维动态仿真。资源包共1个文件，为约116KB的PDF文档，适合在MATLAB环境中对照学习，快速掌握从平台模型搭建、轨迹规划到奇异位形检测的完整流程。已有189人学习下载，可作为Stewart平台从入门到进阶的实用参考资料。

1. 先理解这个标题在解决什么问题：六自由度平台的控制算法没地方验证

手头有一个Stewart并联平台要做运动控制验证，硬件还在装配，但运动学逆解和动力学仿真得先跑通。标题里“基于MATLAB”不是简单换个计算工具，而是把整条验证链路放在一个能快速改参数、能画曲线、能定位数值问题的环境里。运动学逆解解决的是“告诉平台去哪，六条腿各伸长多少”，动力学仿真解决的是“按这条轨迹跑，每条腿要出多大力”。这条链路跑通了，电机选型、控制器增益、奇异位形规避才有依据。适合刚接触并联机构的学生，也适合做方案预研的工程师。下面按我常用的工程套路展开：建模、写函数、跑轨迹、算力、避坑，代码都可以直接复制到本地运行。

2. 运动学逆解建模：从结构参数到支腿长度的解析化简

Stewart平台的逆解看着唬人，拆开之后就是“坐标变换加矢量求模”。难的是把坐标系、铰点布局、姿态约定这些前提一次性定对，后面全是机械操作。这一章先把结构参数定下来，再推导逆解公式，最后说明为什么控制里只用逆解不用正解。

2.1 上下铰点怎么布置：结构参数与坐标系

我一般先把几何参数集中放在一个结构体里，单位全部用米、千克、秒。常见的六铰点布局不是每个间隔60度，而是分成三组、每组两个铰点，组内夹角小，组间差120度。这种布局能让平台在三个方向上都有较好的力传递性能，也降低了绕竖直轴旋转的奇异风险。上平台和下平台的半角可以不同，常见取8到12度。

坐标系定义是这里最容易埋雷的地方。固定坐标系{B}建在下平台中心，动坐标系{P}建在上平台中心。上平台六个铰点在{P}系里的坐标是固定的，逆解时先把它们变换到{B}系，再和固定铰点做矢量差。生成铰点坐标的代码长这样：

function geom = setup_geometry() % 结构参数集中管理，后续所有函数共用 geom.R_b = 0.50; % 下平台铰点分布半径，单位 m geom.R_p = 0.35; % 上平台铰点分布半径，单位 m geom.H0 = 0.60; % 动平台初始高度，单位 m alpha_b = 8; % 下平台每组铰点半角，单位 deg alpha_p = 10; % 上平台每组铰点半角，单位 deg % 三组铰点，每组两个，组间隔 120 度 th_b = zeros(1, 6); th_p = zeros(1, 6); for k = 0 : 2 th_b(k*2+1 : k*2+2) = [k*120 - alpha_b, k*120 + alpha_b]; th_p(k*2+1 : k*2+2) = [k*120 - alpha_p, k*120 + alpha_p]; end % 固定铰点 B 和上平台本地铰点 P_local，都是 3x6 矩阵 geom.base_pivot = geom.R_b * [cosd(th_b); sind(th_b); zeros(1, 6)]; geom.platform_pivot_local = geom.R_p * [cosd(th_p); sind(th_p); zeros(1, 6)]; end

这里用cosd/sind是因为角度单位是度，避免在生成几何时反复换算。th_b和th_p生成后各含六个角度，按“组内对称、组间间隔120度”分布。上平台和下平台的半角不同，会让六条腿在初始位姿下的长度略有差异，这是正常现象，不是代码错误。

坐标系和结构参数定完之后，唯一还需要统一的是姿态约定。我固定用 Z-Y-X 内旋欧拉角，也就是先绕 z 轴转 yaw，再绕 y 轴转 pitch，最后绕 x 轴转 roll。这个约定会贯穿后面所有函数，包括逆解、轨迹生成和动力学计算，中途更换顺序是后期最隐蔽的返工源头。

2.2 用矢量闭环法推导逆解公式

Stewart平台的每条支腿可以看成一条独立的矢量链。固定铰点 B_i 在{B}系下的坐标是已知的，上平台铰点在{P}系下的坐标 P_local_i 也是已知的。给定动平台位置t = [x; y; z]和旋转矩阵R之后，上铰点在固定系下的坐标为：

p_i = t + R * P_local_i

支腿矢量就是：

L_vec_i = p_i - B_i

支腿长度为norm(L_vec_i)。整个过程对每个 i 完全独立，没有耦合方程，所以逆解是解析解，计算量极小。

旋转矩阵R是整个逆解的核心，也是最容易翻车的地方。按 Z-Y-X 内旋顺序，R = Rz(yaw) * Ry(pitch) * Rx(roll)。注意矩阵乘法顺序不能反，先转的和后转的在矩阵表达上是从右往左排列的。如果用列向量左乘旋转矩阵，这个顺序意味着“先绕 x 轴转，再绕 y 轴转，最后绕 z 轴转”。

这个推导看起来简单，但坐标正负号、旋转顺序、欧拉角定义这三件事只要错一个，逆解在大部分位姿下都没问题，只有在大角度姿态时才会露出马脚。所以后面第5章会专门讲怎么排查这类问题。逆解公式本身不需要再往深处推，控制里每一拍都要调用它，计算延迟越低越好。

2.3 为什么选逆解不选正解

并联平台的正解是给六条腿长求动平台位姿，这是一个六元非线性方程组，通常要迭代求解，存在多解和初值敏感的问题，实时性很差。逆解则正好相反，给位姿直接解析算出腿长，适合控制环路里高频调用。

在并联机器人领域有一个基本共识：控制靠逆解，标定靠正解。标定是离线做的，慢一点没关系；控制是实时的，每毫秒都要出结果。所以标题里强调“运动学逆解”而不是正解，正是因为它落地的位置在控制器内部。动力学仿真也一样，最终输出是六条支腿的驱动力，供电机选型和控制器设计使用。

3. 用MATLAB把逆解写成函数：单点验证与轨迹连续性

理论推导完成之后，下一步是把它变成可以反复调用的函数。这里有一个工程习惯：逆解函数只做“位姿到腿长”的转换，不掺入轨迹生成、绘图等逻辑，保证它随时可以被控制和仿真模块复用。

3.1 先把位姿约定写成独立工具函数

旋转矩阵函数会被逆解和动力学的力映射反复调用，我一般把它单独保存成一个rpy2rotm.m，避免在每个函数里重复写一遍匿名函数。这看起来是小事，但在多人协作的仿真项目里，姿态约定不统一造成的返工远大于这会花掉的五分钟。

function R = rpy2rotm(roll, pitch, yaw) % 按 Z-Y-X 内旋顺序构造旋转矩阵 % roll/pitch/yaw 单位均为 rad Rz = @(c) [cos(c) -sin(c) 0; sin(c) cos(c) 0; 0 0 1]; Ry = @(b) [cos(b) 0 sin(b); 0 1 0; -sin(b) 0 cos(b)]; Rx = @(a) [1 0 0; 0 cos(a) -sin(a); 0 sin(a) cos(a)]; R = Rz(yaw) * Ry(pitch) * Rx(roll); end

函数里三个匿名函数分别对应绕 z、y、x 轴的旋转矩阵，最后按 Z-Y-X 顺序相乘。这里的关键点是Rx的定义，[1 0 0; 0 cos -sin; 0 sin cos]是绕 x 轴转动的正方向约定，正负号如果写反，姿态和真实方向会差一个镜像。

3.2 逆解函数：输入位姿输出支腿长度

把rpy2rotm和几何参数组合起来，就是完整的逆解函数。保存为stewart_ik.m，配合第2章的setup_geometry.m即可独立运行。调用接口设计为L = stewart_ik(pose, geom)，位姿向量固定为[x; y; z; roll; pitch; yaw]六维列向量。

function L = stewart_ik(pose, geom) % 输入：pose = [x; y; z; roll; pitch; yaw]，长度单位 m，角度单位 rad % 输出：L 为 6x1 列向量，六条支腿长度，单位与几何参数一致 x = pose(1); y = pose(2); z = pose(3); roll = pose(4); pitch = pose(5); yaw = pose(6); t = [x; y; z]; R = rpy2rotm(roll, pitch, yaw); L = zeros(6, 1); for i = 1 : 6 p_i = t + R * geom.platform_pivot_local(:, i); L_vec = p_i - geom.base_pivot(:, i); L(i) = norm(L_vec); end end

这段代码里最容易被忽略的是geometry.platform_pivot_local已经在 {P} 系下定义好了，而R * p_local就是把本地坐标转到固定系。旋转矩阵左乘列向量完成了从 {P} 到 {B} 的变换。很多初学者在这里会写成p_local * R，结果维度都对不上。

写完之后先做单点验证：把位姿设为零位姿，也就是[0; 0; geom.H0; 0; 0; 0]，此时六条腿长度应该非常接近，且数值大致等于上下铰点水平投影差和高度差构成的斜边长度。如果输出出现 NaN 或者数量级明显不对，先查几何参数和单位，再查旋转矩阵。

geom = setup_geometry(); L_init = stewart_ik([0; 0; geom.H0; 0; 0; 0], geom); disp(L_init);

注意disp输出的六个数应当分布在一个很窄的区间内，比如几个毫米的差异来自上下平台半角不相同。如果差异达到十几厘米，说明alpha_b和alpha_p的符号方向安排有问题，需要回到 2.1 的布局方式检查。

3.3 用一条螺旋轨迹验证逆解的连续性

单点验证只能说明函数没有基本错误，不能证明整个位姿空间内都没问题。我习惯生成一条小幅螺旋轨迹，把每个采样点的位姿都送进逆解，然后看六条支腿长度曲线是否平滑连续。曲线出现阶跃或折断，几乎可以肯定是姿态约定或铰点坐标出了问题。

t_vec = linspace(0, 10, 1001); z_traj = geom.H0 + 0.03 * sin(2 * pi * 0.5 * t_vec); roll_traj = 4 * pi/180 * sin(2 * pi * 0.3 * t_vec); pitch_traj = 3 * pi/180 * cos(2 * pi * 0.4 * t_vec); L_hist = zeros(6, length(t_vec)); for k = 1 : length(t_vec) pose = [0; 0; z_traj(k); roll_traj(k); pitch_traj(k); 0]; L_hist(:, k) = stewart_ik(pose, geom); end plot(t_vec, L_hist'); xlabel('时间 (s)'); ylabel('支腿长度 (m)');

轨迹里同时给了 z 方向的平移以及 roll、pitch 的摆动，这样六条腿的长度都会有明显变化，便于观察。选择小幅值是为了避免触发工作空间边界。曲线正常的话应该是六条类似正弦但相位不同的平滑线，不会出现突变的毛刺。如果某条腿曲线在大角度附近有明显折角，优先检查欧拉角绕 z 轴的那一项是不是被意外带入了轨迹。

这个“连续验证”步骤就是逆解最早期的自检，比直接接进控制环路后再排查要省力得多。控制算法跑飞了原因可能有一百种，而支腿长度跳变这种问题，在轨迹验证阶段就能过滤掉一大部分。

4. 动力学仿真：把期望运动变成六条支腿的驱动力

运动学逆解解决“腿该多长”，动力学仿真解决“腿该出多大力”。高速高精度的并联平台动力学建模需要处理科氏力和离心力，工程预研阶段我常用操作空间力映射法，先算“平台需要多少广义力”，再映射到“六条支腿各自的轴向力”。这一章按这个思路给出可运行的代码。

4.1 动力学建模的取舍：解析拉格朗日还是操作空间力映射

Stewart平台的完整拉格朗日方程包含六自由度惯性矩阵、科氏力项、重力项，推导过程长且容易出错，而且模型参数稍微一变就要重新推导。工程上更常用的做法是多体动力学软件直接建模，但在方案阶段，参数还没定形，我用操作空间力映射法做第一版仿真。

核心思路很简单：已知动平台质量和转动惯量，从期望轨迹差分得到加速度，计算动平台需要的合力和合力矩，再通过每条支腿的单位方向向量建立 6x6 的力映射矩阵，解出六条腿的轴向力。这个模型忽略了支腿自身的惯量，低速和中低速场景下足够反映力的趋势，高速场景再把科氏项补回来。

这样做的好处是代码短、可解释性强。每条支腿的轴向力方向就是支腿矢量方向，所以力映射矩阵每一列都由当前位形决定，位形一变矩阵就变，这也天然暴露了奇异位形的问题——矩阵接近奇异时解出来的力会异常大。

4.2 期望轨迹与速度加速度的数值微分

动力学仿真需要速度和加速度，但给定轨迹往往只有位置。我用中心差分法从位置序列求速度和加速度，相比前向差分，中心差分在相同步长下噪声更小。

dt = 0.01; t_vec = 0 : dt : 8; % 期望轨迹：z 方向正弦 + 俯仰摆动 z_traj = geom.H0 + 0.04 * sin(2 * pi * 0.5 * t_vec); pitch_traj = 5 * pi/180 * sin(2 * pi * 0.3 * t_vec); N = length(t_vec); pos = zeros(6, N); pos(3, :) = z_traj; pos(5, :) = pitch_traj; % 中心差分：内部点用前后差分，边界点直接置零 vel = zeros(6, N); acc = zeros(6, N); vel(:, 2:end-1) = (pos(:, 3:end) - pos(:, 1:end-2)) / (2 * dt); acc(:, 2:end-1) = (pos(:, 3:end) - 2 * pos(:, 2:end-1) + pos(:, 1:end-2)) / dt^2;

代码里pos是 6xN 矩阵，第3行存 z，第5行存 pitch。中心差分的分母是2*dt，因为pos(:, 3:end)和pos(:, 1:end-2)之间隔了两个采样周期。二阶差分模板系数是[1, -2, 1]，对应三阶精度，代价是放大了高频噪声。

边界点直接置零会引入初始阶段的加速度突变，所以下一节会提到让轨迹从零速度起步。这里还有一个容易被忽略的点：pos矩阵的行顺序必须和逆解函数的位姿顺序完全一致，第1到第6行分别是 x、y、z、roll、pitch、yaw。顺序错位会让力和运动方向对不上。

4.3 由力映射矩阵求解六条支腿的驱动力

每条支腿只传递轴向力，设第 i 条腿的单位方向向量为e_i，上铰点相对动平台中心的位置矢量为r_i，则平台受到的合力为sum(F_i * e_i)，合力矩为sum(r_i × (F_i * e_i))。写成矩阵形式就是W * F_leg = [F_des; M_des]。

% 动平台质量与转动惯量，按预研阶段粗略值设置 m_plat = 2.0; % kg I_plat = diag([0.010, 0.015, 0.012]); % kg*m^2 g_vec = [0; 0; -9.81]; F_leg = zeros(6, N); for k = 1 : N pose_k = pos(:, k); R_k = rpy2rotm(pose_k(4), pose_k(5), pose_k(6)); t_k = pose_k(1:3); % 构造 6x6 力映射矩阵 W = zeros(6, 6); for i = 1 : 6 p_i = t_k + R_k * geom.platform_pivot_local(:, i); L_vec = p_i - geom.base_pivot(:, i); e_i = L_vec / norm(L_vec); r_i = p_i - t_k; W(:, i) = [e_i; cross(r_i, e_i)]; end % 期望广义力：平动用 F = m*a 并补偿重力，转动用 M = I*alpha F_des = m_plat * acc(1:3, k) - m_plat * g_vec; M_des = I_plat * acc(4:6, k); % 解六条腿轴向力，使用 pinv 避免奇异位形附近直接崩溃 F_leg(:, k) = pinv(W) * [F_des; M_des]; end % 必要时检查力映射矩阵条件数 cond_W = cond(W);

这段代码里pinv(W)替代W \是故意为之。轨迹推进过程中某些中间位形会让 W 接近奇异，直接左除会给出数量级离谱的力，pinv至少能让仿真继续跑完，方便你把奇异位形的位置定位出来。cond(W)的输出如果突然变大几个量级，就说明平台正经过奇异区域。

仿真结束后，把F_leg的六条曲线画出来，先看数量级是否和负载质量匹配，再看有没有间歇性拉压交替。对于负载 2 kg、轨迹幅度几厘米的平台，支腿力一般在十几牛到几十牛的范围内。如果某条腿的力出现上百牛的毛刺，大概率是轨迹设计问题或单位错误，而不是动力学方程本身有问题。

5. 仿真避坑清单：欧拉角跳变、力尖峰与数值噪声的排查

这个章节是反复跑仿真踩出来的经验。现象、原因、解决三步走，每一条都能直接在MATLAB里复现并验证。

5.1 欧拉角顺序不一致导致支腿长度跳变

现象：逆解函数在大多数位姿下结果正常，但一旦平台同时有 roll 和 pitch 运动，某条腿的长度曲线出现肉眼可见的阶跃，甚至六条腿长度在同一时刻全部突变。

原因：旋转矩阵的构造顺序和轨迹里姿态角定义不一致。比如轨迹里先 roll 后 pitch，而旋转矩阵用的是R = Rx * Ry * Rz，或者反过来。欧拉角顺序错位在零位附近几乎无感，因为小角度下矩阵乘法近似可交换，角度大了才暴露。

解决：全工程固定Z-Y-X内旋顺序，逆解函数、轨迹生成、动力学计算三处统一。把rpy2rotm函数单独放一个文件，禁止在别处重新定义旋转矩阵。改完之后重新跑 3.3 的轨迹验证，确认曲线平滑。

5.2 奇异位姿附近力求解尖峰

现象：动力学仿真结束后，某条腿的力曲线在某个时间点出现尖锐的大幅脉冲，幅值可能是正常值的几十倍，换了采样步长之后尖峰位置还有移动。

原因：平台处于接近奇异的位形，力映射矩阵W的某些列几乎线性相关，矩阵接近奇异，用左除或pinv解出来的力分量被放大。

解决：计算出cond(W)并画成曲线，尖峰位置会和条件数峰值重合。轨迹规划阶段刻意让平台远离这个位形，或者调整铰点布局参数。如果必须经过奇异区域，则要在控制策略上做关节空间的过渡，这已经属于另一个课题了。对于预研仿真，设计轨迹时限定 roll/pitch 不超过 15 度，大多数布局都能避开主要奇异区域。

5.3 数值微分放大噪声导致加速度曲线不可用

现象：位置轨迹平滑，求出来的加速度曲线却布满高频毛刺，力曲线跟着振荡，看起来像随机噪声。

原因：位置序列经过linspace生成时本身没有噪声，但一旦改成从仿真或传感器数据读取，位置数据里就混有量化噪声。二阶差分对噪声的放大系数是1/dt^2，步长越小噪声放大越严重。

解决：先做位置平滑再做差分，我常用smoothdata(pos, 'movmean', 5)或sgolayfilt。注意平滑窗口不要开太大，否则真实加速度会被削平。另一个稳妥做法是把采样周期放宽到 0.01 秒以上，不要为了“看起来精细”把步长压到 0.001 秒。

5.4 初始阶段力和加速度脉冲

现象：仿真的第一拍或前几拍，加速度序列出现一个明显的大脉冲，后续曲线恢复正常。

原因：中心差分在边界点无法计算，代码里直接置零，导致第一个采样周期内加速度从零突变到正常值，宏观上表现为初始冲击。

解决：轨迹设计成零初始速度起步，比如给位置轨迹乘一个斜坡包络(1 - cos(pi * t / T)) / 2，让位置、速度、加速度在起点都连续。边界点保持置零即可，但要在代码注释里写明“前三个采样点不建议参与数据分析”。

5.5 单位混用导致力数量级偏离

现象：仿真能跑通，但计算出的支腿力要么小到 1e-6，要么大到 1e8，物理上完全不合理。

原因：几何参数用了毫米，质量用了克，转动惯量用了 g*cm^2，而代码里重力加速度用了 9.81。只要有一处单位没统一，数量级直接偏出好几个量级。

解决：在setup_geometry里集中定义单位，并在注释里写明“全部国际单位”。质量、转动惯量、几何长度分开管理，每次改动只动结构体对应字段。做仿真之前先算一次静态重力补偿力，检查数值是否和m * g的六分之一到三分之一这个量级吻合。

6. 逆解自洽性验证：让正解反解互相校验

逆解算出来的腿长对不对，最好的办法不是看曲线，而是把它送回正解里去，看能不能恢复原来的位姿。这个闭环验证是我每次改动几何参数之后必跑的检查项。

MATLAB的fsolve可以直接用来做这个工作。把逆解函数本身作为残差函数，目标是找到一组位姿，让它的逆解结果等于已知的腿长。由于逆解是解析函数，fsolve收敛非常快。代码如下：

L_ref = stewart_ik(pose_true, geom); pose_init = pose_true + [0; 0; 0.01; 0.005; 0.005; 0]; options = optimoptions('fsolve', 'Display', 'off', 'OptimalityTolerance', 1e-12); pose_est = fsolve(@(pose) stewart_ik(pose, geom) - L_ref, pose_init, options); err = max(abs(pose_est - pose_true)); disp(['最大位姿误差: ', num2str(err)]);

把pose_true换成上一章轨迹里的任意一个点，err如果小于 1e-10，说明逆解函数内部自洽。如果误差在 1e-3 量级，基本可以断定旋转矩阵的某个符号写反了。另有一种情况是铰点坐标成对交换，逆解曲线也会看起来正常，但正解恢复出的位姿和你给的目标始终差一个固定偏移。这类问题只有闭环验证才能暴露。

我后来养成的习惯是：每次改完setup_geometry里的任何一个参数，都跑一次这个脚本。有一次项目里就是上平台铰点角度方向标反了，单点验证和轨迹连续性验证都没查出来，因为曲线形状完全正常，只是整体偏了一个角度，最后靠 fsolve 发现恢复位姿和给定位姿差了固定几度。从那以后，我对“曲线正常”这种结论都保持警惕，数值自洽才是真正可靠的判据。

如果你把逆解函数也用于正解迭代，记得给fsolve一个好初值。并联平台正解有多解，初值离真解太远可能收敛到另一个解，误差一看就很大。逐帧验证时可以用上一帧的位姿做初值，连续性会让迭代路径更稳定。希望这个验证习惯帮你在并联平台的调试里少走一段弯路。

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