简介围绕 Stewart 并联平台的运动学逆解与动力学仿真这份 PDF 文档面向从事机器人控制、并联机构研究的科研人员与技术爱好者着力解决理论推导到 MATLAB 仿真落地之间的衔接问题。压缩包共 1 个 PDF 文件约 116KB集中讲解坐标变换与支腿长度计算、雅可比矩阵的应用、基于牛顿-欧拉法建立动力学方程以及借助 Simscape Multibody 完成三维仿真并实时绘制平台运动的代码实现示例代码可直接在 MATLAB 中运行验证。文中还就奇异位形检测、轨迹规划不合理等常见问题给出排查与优化思路便于读者避开仿真陷阱、提升建模效率。目前已有 192 人学习下载适合希望系统掌握逆解流程与动力学仿真验证的中高级读者查阅。1. 六条腿各算各的Stewart平台逆解到底难在哪写过并联机构逆解的人基本有个共识Stewart平台的正解难逆解不难但逆解里写错的那几行最难查。你在 MATLAB 里给一个位姿函数老老实实返回六个长度曲线平滑、动画正常等接上 Simulink 闭环跑起来平台开始低频抖动、某条腿速度突然翻倍回头才发现是旋转顺序和铰点相位对不上。这份围绕 MATLAB 的 Stewart 平台资源把运动学逆解、雅可比矩阵、动力学仿真和三维可视化串成一条能跑通的链路适合手上有一台电缸平台、或者正在做并联机构课题想在仿真里先把参数边界和奇异区摸清的人。下面按建模、逆解、雅可比、动力学、避坑、可视化的顺序拆代码都能直接粘进脚本跑。2. 铰点坐标与位姿定义先把六条腿的几何关系钉死逆解出问题十次里有六次不是公式错是几何参数没定义清楚。上下平台各六个铰点怎么排、动平台坐标原点放在哪、哪一列对应哪条腿这些事在写第一行代码之前就得定死后面雅可比、动力学、可视化全都依赖这套约定。2.1 上下平台铰点半径、相位与交错角常见的 6-6 构型是上下平台各六个铰点、分三组、每组两个铰点靠得很近基座组和动平台组在圆周上错开一个角度。错开的目的是让支腿在初始位形下不共线、不接近竖直避开一开始就贴着奇异面走。同一组两个铰点的半角 beta 一般在 8° 到 15° 之间太大支腿互相干涉太小加工和装配都难受。参数定完用极坐标生成坐标矩阵每一列对应一条支腿这个约定要贯穿全篇。function [base_pts, top_pts] build_joint_points(Rb, Rt, beta) % 生成 6-6 Stewart 平台上下铰点坐标 % 输入 % Rb - 基座铰点分布圆半径m % Rt - 动平台铰点分布圆半径m % beta - 同组两铰点相对分布圆基准相位的半角rad % 输出 % base_pts - 3x6静平台铰点在基坐标系下的坐标 % top_pts - 3x6动平台铰点在动平台坐标系下的坐标原点在动平台几何中心 group (0:2) * 2*pi/3; % 三组铰点的基准相位 base_ang [group - beta; group beta]; % 2x3每组两个铰点 base_ang base_ang(:); % 展成 1x6顺序为 1-,1,2-,2,3- base_pts [Rb*cos(base_ang); Rb*sin(base_ang); zeros(1,6)]; % 动平台铰点整体错开 pi/3避免支腿在初始位形共线 top_ang base_ang pi/3; top_pts [Rt*cos(top_ang); Rt*sin(top_ang); zeros(1,6)]; % top_pts 的 z 分量为 0是因为它定义在动平台自身坐标系下 % 实际高度由位姿向量的 z 分量决定别在这里提前加 H0 end这里有个容易忽略的点base_ang(:)的展平顺序决定了第 i 条腿对应哪两个铰点。MATLAB 的列优先展开会得到 1-,1,2-,2,3-,3-如果你后面手工改过顺序而雅可比没同步就会出现左右腿互换的诡异现象——平台往左倾动画却往右歪。常用几何参数先记在表里后面所有函数都从这里取参数含义参考取值Rb基座铰点分布半径0.50 mRt动平台铰点分布半径0.35 mbeta同组铰点半角12°H0动平台中位高度0.60 m行程范围电缸最小/最大安装长度0.40 / 0.85 m2.2 位姿六元组与 ZYX 旋转顺序位姿统一用六元组pose [x; y; z; roll; pitch; yaw]前三个是动平台参考点在基坐标系下的位置后三个是姿态角单位弧度。旋转用eul2rotm转成 3x3 矩阵输入必须是 1x3 行向量而且顺序要显式写死。pose [0.05; -0.02; 0.62; deg2rad(8); deg2rad(-5); deg2rad(12)]; R_zyx eul2rotm(pose(4:6), ZYX); % 旋转顺序先绕 Z再绕 Y最后绕 X R_xyz eul2rotm(pose(4:6), XYZ); % 同一组角换个顺序 disp(norm(R_zyx - R_xyz, fro)); % 差值量级能到 0.3 以上不是舍入误差同一组角度在两种顺序下差出一个可观的旋转矩阵落到支腿长度上就是毫米到厘米级偏差行程校验直接失准。所以约定一次就不再改全篇统一 ZYX逆解、雅可比、动力学、可视化都用同一个 R别在某个函数里图省事换成 XYZ。如果你的 MATLAB 没装 Robotics System Toolboxeul2rotm会报未定义自己写三行Rz*Ry*Rx顶上就行顺序一致就没问题。2.3 参数初始化脚本单位制、行程与质量属性把几何、行程、质量、惯量全部塞进一个结构体脚本所有函数从它取参数。单位制混用是并联机构仿真里最隐蔽的翻车点之一长度一会儿米一会儿毫米、惯量一会儿 kg·m² 一会儿 kg·mm²仿真结果能飞上天。% params.m —— 全部使用 SI 单位长度 m角度 rad质量 kg P.Rb 0.50; P.Rt 0.35; P.beta deg2rad(12); P.H0 0.62; P.stroke_min 0.40; % 电缸最小安装长度 P.stroke_max 0.85; % 电缸最大安装长度 P.mass 18.5; % 动平台含负载质量 P.I diag([0.42 0.42 0.78]); % 主惯量kg·m^2 P.g [0; 0; -9.81]; [P.base_pts, P.top_pts] build_joint_points(P.Rb, P.Rt, P.beta);P.I用主惯量对角阵是简化处理真实动平台带偏置负载时惯量积不为零做控制器增益设计前要换成完整 3x3 矩阵。行程上下限不是装饰逆解每次返回长度都拿它比一次超行程直接报错停住比让它继续算下去、后面支腿长度曲线出现一段水平平台要好查得多。3. 逆解与雅可比从六条腿长度到速度映射逆解本身的数学只有两步把动平台铰点通过旋转和平移变到基坐标系再算它和基座铰点的欧氏距离。真正花时间的是把它写对、写快、写出能报警的边界。3.1 逆解主函数一次齐次变换加一次范数最直观的实现是逐条腿循环可读性好但慢向量化写法把六条腿一次算完代码更短也更容易和后面的批量位姿对齐。两种写法结果必须完全一致我会在开发时用循环版做基准向量化版做对照跑一百组随机位姿比最大误差。function [L, s_hat, p_world] ik_stewart(pose, base_pts, top_pts) % 位姿 - 六条支腿长度 % pose [x;y;z;roll;pitch;yaw]角度单位 rad % L 6x1 支腿长度 % s_hat 3x6 每条支腿的单位方向向量基座指向动平台 % p_world 3x6 动平台铰点在基坐标系下的位置 R eul2rotm(pose(4:6), ZYX); % 与全篇约定一致 t pose(1:3); p_world R * top_pts t; % 3x6MATLAB 自动按列广播 t leg_vec p_world - base_pts; % 3x6逐列相减 L vecnorm(leg_vec, 2, 1); % 沿第 1 维列方向求 2-范数得 1x6 s_hat leg_vec ./ L; % 逐列归一化L 是 1x6 行向量 end三个输出里s_hat和p_world是给雅可比准备的一次算完不重复劳动。R * top_pts t这行依赖 MATLAB 的隐式扩展比R*top_pts repmat(t,1,6)干净但要求t是 3x1 列向量——如果哪次传进来的是 1x3 行向量会得到一个 3x3 的错误结果而不报错属于典型静默失败。3.2 vecnorm 的维度参数最容易翻车的一行vecnorm(A, 2, dim)的第三个参数是沿哪一维求范数不是第几维是向量。矩阵按列存支腿时用dim1按行存时用dim2写反了不报错只是每条腿的长度变成另外几条腿的组合曲线看起来还挺平滑。% 一次算一批位姿验证维度参数是否用对 N 200; traj [0.02*sin(linspace(0,2*pi,N)), zeros(N,2), ... 0.6 0.05*cos(linspace(0,2*pi,N)), zeros(N,3)]; L_all zeros(N,6); for k 1:N L_all(k,:) ik_stewart(traj(k,:), P.base_pts, P.top_pts); end % 自查任何一条腿的长度都应落在行程内 bad any(L_all P.stroke_min | L_all P.stroke_max, all); assert(~bad, 存在超行程位姿检查 vecnorm 维度或几何参数);批量循环这里用 for 是合理的因为每个位姿要做一次矩阵乘向量化收益有限真要提速把R预先算出来缓存别在循环里重复调eul2rotm。这段自查的意义在于vecnorm维度写错时算出来的长度往往偏大或偏小得离谱行程校验能第一时间抓到比盯着动画看半天靠谱。3.3 雅可比矩阵从支腿方向到速度映射雅可比描述的是平台怎么动到腿怎么伸缩的线性映射。对第 i 条腿长度变化率等于支腿单位方向向量点乘动平台铰点的速度dl_i s_i · (v ω × p_i) s_i · v (p_i × s_i) · ω其中 p_i 是动平台铰点相对动平台参考点的位置向量s_i 是支腿单位方向。按 [vx vy vz wx wy wz] 的列顺序拼出来就是一个 6x6 矩阵。function J jacobian_stewart(pose, base_pts, top_pts) % 6x6 雅可比列顺序 [vx vy vz wx wy wz] [~, s_hat, p_world] ik_stewart(pose, base_pts, top_pts); J zeros(6,6); for i 1:6 p_rel p_world(:,i) - pose(1:3); % 铰点相对动平台参考点 J(i,:) [s_hat(:,i), cross(p_rel, s_hat(:,i))]; end endcross(p_rel, s_hat)的叉乘顺序不能反反了之后角速度项的符号整体翻转平台绕 X 轴正转时六条腿的速度方向全错闭环一上来就发散。想快速验证符号给一个只有绕 Z 轴正角速度的位姿手动推一条腿伸长、另一条缩短看J*[0;0;0;0;0;1]的符号对不对得上。数值验证解析雅可比有个笨办法但很好用用逆解做中心差分(L(poseeps*dq) - L(pose-eps*dq))/(2*eps)和J*dq比误差应在 1e-6 量级。差得多就是叉乘顺序或者 p_rel 定义错了。3.4 奇异位形条件数阈值与阻尼最小二乘平台进入奇异位形时雅可比降秩逆解求速度会炸。判断奇异最直接的是看cond(J)但这个量受量纲影响——前三列无量纲、后三列有长度量纲长度单位从米换成毫米条件数会跟着变。所以阈值必须绑定单位制别在不同脚本里混着用。c cond(J); if c 1e4 warning(接近奇异位形cond(J) %.3e位姿 z %.3f, c, pose(3)); end % 速度级逆解加阻尼最小二乘防止在奇异附近数值爆炸 lambda 1e-3; dx [0.01; 0; 0; 0; 0; 0]; % 期望末端速度 dq (J*J lambda^2*eye(6)) \ (J*dx); % dq 是六条腿的速度lambda 从 1e-6 往上试太小起不到阻尼作用太大跟踪精度掉得厉害1e-3 附近是常见起点。更稳的做法是把条件数判断和阻尼系数联动c小于 1e3 时 lambda 取 01e3 到 1e5 之间按对数插值放大超过 1e5 直接停轨迹。这套逻辑放在实时控制循环里比事后再看曲线强得多。4. 动力学与联合仿真牛顿-欧拉和 Simscape 怎么分工动力学这层最容易陷入两种极端一是纯符号推导推到一半算不动二是直接拖 Simscape 模型但不知道里面在解什么。实际做法是按用途分工——控制器前馈量用数值化的牛顿-欧拉快速算机构细节和接触、摩擦用 Simscape Multibody 兜底。4.1 牛顿-欧拉数值实现把质量和惯量压成矩阵完整推导涉及每条支腿的质量、质心位置和转动惯量写出来几百行调试成本极高。工程上先做动平台单刚体简化把动平台加负载的质量和惯量拼成任务空间矩阵通过雅可比转置映射到关节空间得到各条腿的驱动力。function tau inverse_dynamics_simple(dq, ddq, J, M_task, g, b_v, b_c) % 简化的逆动力学动平台单刚体 黏性摩擦 库伦摩擦 % dq, ddq 6x1 支腿速度、加速度 % J 6x6 雅可比 % M_task 6x6 任务空间质量阵 diag([m m m Ixx Iyy Izz]) % g 3x1 重力向量如 [0;0;-9.81] % b_v 黏性摩擦系数b_c 库伦摩擦幅值 a_task J * ddq; % 忽略雅可比导数项 Jd*dq低速时影响小 G_task [0; 0; M_task(3,3)*(-g(3)); 0; 0; 0]; tau J * (M_task * a_task G_task) ... % 惯性项 重力项映射回关节空间 b_v * dq b_c * sign(dq); % 摩擦项按实测标定别照抄 % 行程外的支腿不应该出力直接钳位更接近真实驱动器行为 end这段的价值在于量级估算和控制器前馈不是精确的机构动力学。Jd*dq项在速度较高时不能忽略忽略后前馈力会偏小b_v和b_c必须按实际电缸标定随手填 0.05 和 0.02 只能保证仿真跑起来不难看。真要精确建模就把每条腿的质量和惯量加进来用递归牛顿-欧拉逐杆算代码量翻几倍但换来的精度对控制器设计是值得的。4.2 Simscape MultibodyCAD 导入与闭环约束Simscape 这条路的优势是把几何、约束、接触交给物理引擎缺点是并联机构的闭环结构容易让求解器崩。常见流程是步骤操作注意点1CAD 导出各连杆为独立刚体上下平台、六条支腿分开别合成一个零件2smimport导入 XML 装配文件导入后检查每个关节的自由度是否和设计一致3每条支腿两端配约束一端球铰、一端万向节或按实际结构配4闭环检查六条腿同时约束会产生过约束需要放开冗余自由度5配置驱动输入电缸用 Prismatic Joint运动由外部输入驱动smimport(stewart_assembly.xml); % 导入后先看模型树确认刚体数量为 8 % 六个支腿的移动副改成外部输入驱动 set_param(stewart_model/Leg1_Prismatic, Motion, Provided by Input);导入之后第一件事不是急着跑是先打开 Mechanics Explorer 手动拖一下动平台看六个支腿是不是同步跟着动。如果某条腿不动或者动平台被锁死多半是闭环约束过定义需要把某个球铰的自由度放开或者改用 Bearing Joint 代替刚性连接。这类问题在纯数值代码里根本看不到只有多体模型会暴露。4.3 求解器与步长ode15s 什么时候该上并联机构的动力学方程刚性明显尤其是加了摩擦和驱动器的快速动态之后ode45会把步长压到极小、仿真时间拉得没法看。ode15s是变阶刚性求解器适合这类场景。opts odeset(RelTol, 1e-5, AbsTol, 1e-7, MaxStep, 1e-3); [t, x] ode15s((t,x) platform_ode(t, x, P), [0 10], x0, opts);MaxStep限制最大步长很关键不设的话求解器在平滑段会跳大步遇到铰点换向或者摩擦符号翻转时容易漏掉事件表现为支腿长度曲线出现台阶。RelTol和AbsTol也别放太松1e-4 以上在闭环里会累积成可见的相位偏差。Simulink 里对应的是变步长 solver 配置把 Solver 换成 ode15s、最大步长设成 1e-3效果和纯代码一致。5. 避坑清单仿真里最容易翻车的五个地方下面这几条基本都是我自己或者周围人真实踩过的现象、原因、解决按顺序写对着排查能省不少时间。5.1 逆解与几何建模的两个坑现象一平台绕 X 轴转 10°动画里却像是绕 Y 轴在转支腿长度曲线和手算对不上。原因基本锁定在旋转顺序。eul2rotm不写顺序时用默认值而不同工具箱版本默认值不完全一致加上有的脚本里传的是列向量导致维度对不上被隐式转置最终结果就是姿态矩阵和预期差一个置换。解决办法是全篇显式写ZYX并且在初始化时打印一次R和手算的三行三列对照确认前两行符号正确再往下走。现象二vecnorm算出来的六条腿长度很接近但平台明明已经倾斜了。原因是维度参数写成了dim2而数据是按列存的。按行求范数等于对每条腿的三个分量分别做了一次跨腿运算结果自然看起来对称又合理。解决办法固定数据布局所有点坐标统一按 3x6 列存vecnorm一律用dim1并在逆解外层加一次行程校验超限直接报错。5.2 动力学与求解器的两个坑现象三Simscape 模型一跑就报过约束或者动平台直接飞出去。并联机构的六条腿同时对动平台施加约束自由度数量刚好卡在临界任何一个铰链类型配错都会过定义。常见错误是把支腿两端都设成刚性连接或者球铰的三个转动自由度没锁对。解决办法是从三条腿开始逐步加约束每加一条跑一次 Mechanics Explorer确认机构还能动出现飞出去的情况先查质量属性惯量填成 0 或者单位写成 g·mm² 都会让求解器直接崩。现象四闭环仿真跑了半秒就卡住步长被压到 1e-9。摩擦项用了sign(dq)在速度过零时符号瞬间翻转求解器被迫无限缩步长去过零。解决办法是把库伦摩擦换成连续近似比如tanh(dq/eps_v)eps_v取 1e-3 到 1e-2或者保留sign但把求解器换成 ode15s 并设最大步长让事件检测接管过零处理。5.3 可视化与实时刷新的一条坑现象五动画卡成幻灯片drawnow一加帧率就掉到个位数。原因是每个时间步都在重建图形对象。plot3每次调用都新建 line 对象几百帧之后句柄堆积渲染线程扛不住。解决办法是初始化时把六条腿的 line 对象和动平台的 patch 一次性建好循环里只改XData、YData、ZData和变换矩阵刷新用drawnow limitrate实测能稳定在 50 到 100 Hz 的视觉刷新。另外别在循环里调axis equal它每次都会触发一次完整重排挪到循环外面。6. 可视化与轨迹验证用变换矩阵而不是重绘可视化这块的目的不只是好看而是把逆解和雅可比的结果变成肉眼可判的东西。我习惯用hgtransform管动平台的刚体变换六条腿各用一个 line 对象循环里只更新数据和矩阵。6.1 hgtransform 的正确用法hgtransform的Matrix属性接受完整的 4x4 齐次变换矩阵直接由旋转矩阵和平移向量拼出来。这里有个常见的写法错误makehgtform只支持translate、scale、xrotate这类关键字没有euler选项照着某些示例写成makehgtform(translate,p,euler,eul)会直接报参数错误。稳妥做法是自己拼矩阵。function animate_stewart(ax, hg_top, hg_legs, base_pts, top_pts, traj, fps) % traj: N x 6每行一个位姿 for k 1:size(traj,1) pose traj(k,:); R eul2rotm(pose(4:6), ZYX); T [R, pose(1:3); 0 0 0 1]; % 完整齐次变换矩阵 set(hg_top, Matrix, T); % 动平台整体跟着变换 p_world R * top_pts pose(1:3); for i 1:6 set(hg_legs(i), ... XData, [base_pts(1,i) p_world(1,i)], ... YData, [base_pts(2,i) p_world(2,i)], ... ZData, [base_pts(3,i) p_world(3,i)]); end drawnow limitrate % 限速刷新避免渲染拖垮仿真 pause(1/fps) end endhg_legs要在循环外一次性创建hg_top的父对象设成同一个坐标轴。drawnow limitrate会把刷新频率限制在渲染能力内不会阻塞计算循环太久比裸drawnow稳定得多。6.2 螺旋轨迹验证看支腿长度曲线而不是看动画轨迹验证我一般用一条螺旋上升加俯仰的组合既让六条腿都动起来又能逼出奇异区附近的异常。t 0:0.01:10; z 0.2*sin(2*pi*t) 0.1*t; pitch 0.1*sin(pi*t); traj [zeros(size(t)); zeros(size(t)); z; ... zeros(size(t)); pitch; zeros(size(t))]; L zeros(numel(t), 6); for k 1:numel(t) L(k,:) ik_stewart(traj(k,:), P.base_pts, P.top_pts); end figure; plot(t, L); grid on xlabel(时间 / s); ylabel(支腿长度 / m);判断标准不是动画顺不顺是这六条曲线有没有出现尖峰、平台段或者某条腿变化率明显高于其他腿。尖峰通常意味着轨迹穿过了奇异面附近雅可比条件数暴涨一段水平平台意味着该腿到了行程极限被钳位变化率异常则多半是雅可比符号错了。我现在的习惯是任何新轨迹在进 Simulink 闭环之前先在纯代码里跑一遍逆解、画长度曲线、看最大条件数、看行程余量四项都过了才往下接。这套流程强制走一遍比事后从发散曲线里倒推省事得多。资源包里的脚本按这个顺序跑基本能避开大部分几何和数值上的坑希望帮到你。本文还有配套的精品资源点击获取
简介围绕 Stewart 并联平台的运动学逆解与动力学仿真这份 PDF 文档面向从事机器人控制、并联机构研究的科研人员与技术爱好者着力解决理论推导到 MATLAB 仿真落地之间的衔接问题。压缩包共 1 个 PDF 文件约 116KB集中讲解坐标变换与支腿长度计算、雅可比矩阵的应用、基于牛顿-欧拉法建立动力学方程以及借助 Simscape Multibody 完成三维仿真并实时绘制平台运动的代码实现示例代码可直接在 MATLAB 中运行验证。文中还就奇异位形检测、轨迹规划不合理等常见问题给出排查与优化思路便于读者避开仿真陷阱、提升建模效率。目前已有 192 人学习下载适合希望系统掌握逆解流程与动力学仿真验证的中高级读者查阅。1. 六条腿各算各的Stewart平台逆解到底难在哪写过并联机构逆解的人基本有个共识Stewart平台的正解难逆解不难但逆解里写错的那几行最难查。你在 MATLAB 里给一个位姿函数老老实实返回六个长度曲线平滑、动画正常等接上 Simulink 闭环跑起来平台开始低频抖动、某条腿速度突然翻倍回头才发现是旋转顺序和铰点相位对不上。这份围绕 MATLAB 的 Stewart 平台资源把运动学逆解、雅可比矩阵、动力学仿真和三维可视化串成一条能跑通的链路适合手上有一台电缸平台、或者正在做并联机构课题想在仿真里先把参数边界和奇异区摸清的人。下面按建模、逆解、雅可比、动力学、避坑、可视化的顺序拆代码都能直接粘进脚本跑。2. 铰点坐标与位姿定义先把六条腿的几何关系钉死逆解出问题十次里有六次不是公式错是几何参数没定义清楚。上下平台各六个铰点怎么排、动平台坐标原点放在哪、哪一列对应哪条腿这些事在写第一行代码之前就得定死后面雅可比、动力学、可视化全都依赖这套约定。2.1 上下平台铰点半径、相位与交错角常见的 6-6 构型是上下平台各六个铰点、分三组、每组两个铰点靠得很近基座组和动平台组在圆周上错开一个角度。错开的目的是让支腿在初始位形下不共线、不接近竖直避开一开始就贴着奇异面走。同一组两个铰点的半角 beta 一般在 8° 到 15° 之间太大支腿互相干涉太小加工和装配都难受。参数定完用极坐标生成坐标矩阵每一列对应一条支腿这个约定要贯穿全篇。function [base_pts, top_pts] build_joint_points(Rb, Rt, beta) % 生成 6-6 Stewart 平台上下铰点坐标 % 输入 % Rb - 基座铰点分布圆半径m % Rt - 动平台铰点分布圆半径m % beta - 同组两铰点相对分布圆基准相位的半角rad % 输出 % base_pts - 3x6静平台铰点在基坐标系下的坐标 % top_pts - 3x6动平台铰点在动平台坐标系下的坐标原点在动平台几何中心 group (0:2) * 2*pi/3; % 三组铰点的基准相位 base_ang [group - beta; group beta]; % 2x3每组两个铰点 base_ang base_ang(:); % 展成 1x6顺序为 1-,1,2-,2,3- base_pts [Rb*cos(base_ang); Rb*sin(base_ang); zeros(1,6)]; % 动平台铰点整体错开 pi/3避免支腿在初始位形共线 top_ang base_ang pi/3; top_pts [Rt*cos(top_ang); Rt*sin(top_ang); zeros(1,6)]; % top_pts 的 z 分量为 0是因为它定义在动平台自身坐标系下 % 实际高度由位姿向量的 z 分量决定别在这里提前加 H0 end这里有个容易忽略的点base_ang(:)的展平顺序决定了第 i 条腿对应哪两个铰点。MATLAB 的列优先展开会得到 1-,1,2-,2,3-,3-如果你后面手工改过顺序而雅可比没同步就会出现左右腿互换的诡异现象——平台往左倾动画却往右歪。常用几何参数先记在表里后面所有函数都从这里取参数含义参考取值Rb基座铰点分布半径0.50 mRt动平台铰点分布半径0.35 mbeta同组铰点半角12°H0动平台中位高度0.60 m行程范围电缸最小/最大安装长度0.40 / 0.85 m2.2 位姿六元组与 ZYX 旋转顺序位姿统一用六元组pose [x; y; z; roll; pitch; yaw]前三个是动平台参考点在基坐标系下的位置后三个是姿态角单位弧度。旋转用eul2rotm转成 3x3 矩阵输入必须是 1x3 行向量而且顺序要显式写死。pose [0.05; -0.02; 0.62; deg2rad(8); deg2rad(-5); deg2rad(12)]; R_zyx eul2rotm(pose(4:6), ZYX); % 旋转顺序先绕 Z再绕 Y最后绕 X R_xyz eul2rotm(pose(4:6), XYZ); % 同一组角换个顺序 disp(norm(R_zyx - R_xyz, fro)); % 差值量级能到 0.3 以上不是舍入误差同一组角度在两种顺序下差出一个可观的旋转矩阵落到支腿长度上就是毫米到厘米级偏差行程校验直接失准。所以约定一次就不再改全篇统一 ZYX逆解、雅可比、动力学、可视化都用同一个 R别在某个函数里图省事换成 XYZ。如果你的 MATLAB 没装 Robotics System Toolboxeul2rotm会报未定义自己写三行Rz*Ry*Rx顶上就行顺序一致就没问题。2.3 参数初始化脚本单位制、行程与质量属性把几何、行程、质量、惯量全部塞进一个结构体脚本所有函数从它取参数。单位制混用是并联机构仿真里最隐蔽的翻车点之一长度一会儿米一会儿毫米、惯量一会儿 kg·m² 一会儿 kg·mm²仿真结果能飞上天。% params.m —— 全部使用 SI 单位长度 m角度 rad质量 kg P.Rb 0.50; P.Rt 0.35; P.beta deg2rad(12); P.H0 0.62; P.stroke_min 0.40; % 电缸最小安装长度 P.stroke_max 0.85; % 电缸最大安装长度 P.mass 18.5; % 动平台含负载质量 P.I diag([0.42 0.42 0.78]); % 主惯量kg·m^2 P.g [0; 0; -9.81]; [P.base_pts, P.top_pts] build_joint_points(P.Rb, P.Rt, P.beta);P.I用主惯量对角阵是简化处理真实动平台带偏置负载时惯量积不为零做控制器增益设计前要换成完整 3x3 矩阵。行程上下限不是装饰逆解每次返回长度都拿它比一次超行程直接报错停住比让它继续算下去、后面支腿长度曲线出现一段水平平台要好查得多。3. 逆解与雅可比从六条腿长度到速度映射逆解本身的数学只有两步把动平台铰点通过旋转和平移变到基坐标系再算它和基座铰点的欧氏距离。真正花时间的是把它写对、写快、写出能报警的边界。3.1 逆解主函数一次齐次变换加一次范数最直观的实现是逐条腿循环可读性好但慢向量化写法把六条腿一次算完代码更短也更容易和后面的批量位姿对齐。两种写法结果必须完全一致我会在开发时用循环版做基准向量化版做对照跑一百组随机位姿比最大误差。function [L, s_hat, p_world] ik_stewart(pose, base_pts, top_pts) % 位姿 - 六条支腿长度 % pose [x;y;z;roll;pitch;yaw]角度单位 rad % L 6x1 支腿长度 % s_hat 3x6 每条支腿的单位方向向量基座指向动平台 % p_world 3x6 动平台铰点在基坐标系下的位置 R eul2rotm(pose(4:6), ZYX); % 与全篇约定一致 t pose(1:3); p_world R * top_pts t; % 3x6MATLAB 自动按列广播 t leg_vec p_world - base_pts; % 3x6逐列相减 L vecnorm(leg_vec, 2, 1); % 沿第 1 维列方向求 2-范数得 1x6 s_hat leg_vec ./ L; % 逐列归一化L 是 1x6 行向量 end三个输出里s_hat和p_world是给雅可比准备的一次算完不重复劳动。R * top_pts t这行依赖 MATLAB 的隐式扩展比R*top_pts repmat(t,1,6)干净但要求t是 3x1 列向量——如果哪次传进来的是 1x3 行向量会得到一个 3x3 的错误结果而不报错属于典型静默失败。3.2 vecnorm 的维度参数最容易翻车的一行vecnorm(A, 2, dim)的第三个参数是沿哪一维求范数不是第几维是向量。矩阵按列存支腿时用dim1按行存时用dim2写反了不报错只是每条腿的长度变成另外几条腿的组合曲线看起来还挺平滑。% 一次算一批位姿验证维度参数是否用对 N 200; traj [0.02*sin(linspace(0,2*pi,N)), zeros(N,2), ... 0.6 0.05*cos(linspace(0,2*pi,N)), zeros(N,3)]; L_all zeros(N,6); for k 1:N L_all(k,:) ik_stewart(traj(k,:), P.base_pts, P.top_pts); end % 自查任何一条腿的长度都应落在行程内 bad any(L_all P.stroke_min | L_all P.stroke_max, all); assert(~bad, 存在超行程位姿检查 vecnorm 维度或几何参数);批量循环这里用 for 是合理的因为每个位姿要做一次矩阵乘向量化收益有限真要提速把R预先算出来缓存别在循环里重复调eul2rotm。这段自查的意义在于vecnorm维度写错时算出来的长度往往偏大或偏小得离谱行程校验能第一时间抓到比盯着动画看半天靠谱。3.3 雅可比矩阵从支腿方向到速度映射雅可比描述的是平台怎么动到腿怎么伸缩的线性映射。对第 i 条腿长度变化率等于支腿单位方向向量点乘动平台铰点的速度dl_i s_i · (v ω × p_i) s_i · v (p_i × s_i) · ω其中 p_i 是动平台铰点相对动平台参考点的位置向量s_i 是支腿单位方向。按 [vx vy vz wx wy wz] 的列顺序拼出来就是一个 6x6 矩阵。function J jacobian_stewart(pose, base_pts, top_pts) % 6x6 雅可比列顺序 [vx vy vz wx wy wz] [~, s_hat, p_world] ik_stewart(pose, base_pts, top_pts); J zeros(6,6); for i 1:6 p_rel p_world(:,i) - pose(1:3); % 铰点相对动平台参考点 J(i,:) [s_hat(:,i), cross(p_rel, s_hat(:,i))]; end endcross(p_rel, s_hat)的叉乘顺序不能反反了之后角速度项的符号整体翻转平台绕 X 轴正转时六条腿的速度方向全错闭环一上来就发散。想快速验证符号给一个只有绕 Z 轴正角速度的位姿手动推一条腿伸长、另一条缩短看J*[0;0;0;0;0;1]的符号对不对得上。数值验证解析雅可比有个笨办法但很好用用逆解做中心差分(L(poseeps*dq) - L(pose-eps*dq))/(2*eps)和J*dq比误差应在 1e-6 量级。差得多就是叉乘顺序或者 p_rel 定义错了。3.4 奇异位形条件数阈值与阻尼最小二乘平台进入奇异位形时雅可比降秩逆解求速度会炸。判断奇异最直接的是看cond(J)但这个量受量纲影响——前三列无量纲、后三列有长度量纲长度单位从米换成毫米条件数会跟着变。所以阈值必须绑定单位制别在不同脚本里混着用。c cond(J); if c 1e4 warning(接近奇异位形cond(J) %.3e位姿 z %.3f, c, pose(3)); end % 速度级逆解加阻尼最小二乘防止在奇异附近数值爆炸 lambda 1e-3; dx [0.01; 0; 0; 0; 0; 0]; % 期望末端速度 dq (J*J lambda^2*eye(6)) \ (J*dx); % dq 是六条腿的速度lambda 从 1e-6 往上试太小起不到阻尼作用太大跟踪精度掉得厉害1e-3 附近是常见起点。更稳的做法是把条件数判断和阻尼系数联动c小于 1e3 时 lambda 取 01e3 到 1e5 之间按对数插值放大超过 1e5 直接停轨迹。这套逻辑放在实时控制循环里比事后再看曲线强得多。4. 动力学与联合仿真牛顿-欧拉和 Simscape 怎么分工动力学这层最容易陷入两种极端一是纯符号推导推到一半算不动二是直接拖 Simscape 模型但不知道里面在解什么。实际做法是按用途分工——控制器前馈量用数值化的牛顿-欧拉快速算机构细节和接触、摩擦用 Simscape Multibody 兜底。4.1 牛顿-欧拉数值实现把质量和惯量压成矩阵完整推导涉及每条支腿的质量、质心位置和转动惯量写出来几百行调试成本极高。工程上先做动平台单刚体简化把动平台加负载的质量和惯量拼成任务空间矩阵通过雅可比转置映射到关节空间得到各条腿的驱动力。function tau inverse_dynamics_simple(dq, ddq, J, M_task, g, b_v, b_c) % 简化的逆动力学动平台单刚体 黏性摩擦 库伦摩擦 % dq, ddq 6x1 支腿速度、加速度 % J 6x6 雅可比 % M_task 6x6 任务空间质量阵 diag([m m m Ixx Iyy Izz]) % g 3x1 重力向量如 [0;0;-9.81] % b_v 黏性摩擦系数b_c 库伦摩擦幅值 a_task J * ddq; % 忽略雅可比导数项 Jd*dq低速时影响小 G_task [0; 0; M_task(3,3)*(-g(3)); 0; 0; 0]; tau J * (M_task * a_task G_task) ... % 惯性项 重力项映射回关节空间 b_v * dq b_c * sign(dq); % 摩擦项按实测标定别照抄 % 行程外的支腿不应该出力直接钳位更接近真实驱动器行为 end这段的价值在于量级估算和控制器前馈不是精确的机构动力学。Jd*dq项在速度较高时不能忽略忽略后前馈力会偏小b_v和b_c必须按实际电缸标定随手填 0.05 和 0.02 只能保证仿真跑起来不难看。真要精确建模就把每条腿的质量和惯量加进来用递归牛顿-欧拉逐杆算代码量翻几倍但换来的精度对控制器设计是值得的。4.2 Simscape MultibodyCAD 导入与闭环约束Simscape 这条路的优势是把几何、约束、接触交给物理引擎缺点是并联机构的闭环结构容易让求解器崩。常见流程是步骤操作注意点1CAD 导出各连杆为独立刚体上下平台、六条支腿分开别合成一个零件2smimport导入 XML 装配文件导入后检查每个关节的自由度是否和设计一致3每条支腿两端配约束一端球铰、一端万向节或按实际结构配4闭环检查六条腿同时约束会产生过约束需要放开冗余自由度5配置驱动输入电缸用 Prismatic Joint运动由外部输入驱动smimport(stewart_assembly.xml); % 导入后先看模型树确认刚体数量为 8 % 六个支腿的移动副改成外部输入驱动 set_param(stewart_model/Leg1_Prismatic, Motion, Provided by Input);导入之后第一件事不是急着跑是先打开 Mechanics Explorer 手动拖一下动平台看六个支腿是不是同步跟着动。如果某条腿不动或者动平台被锁死多半是闭环约束过定义需要把某个球铰的自由度放开或者改用 Bearing Joint 代替刚性连接。这类问题在纯数值代码里根本看不到只有多体模型会暴露。4.3 求解器与步长ode15s 什么时候该上并联机构的动力学方程刚性明显尤其是加了摩擦和驱动器的快速动态之后ode45会把步长压到极小、仿真时间拉得没法看。ode15s是变阶刚性求解器适合这类场景。opts odeset(RelTol, 1e-5, AbsTol, 1e-7, MaxStep, 1e-3); [t, x] ode15s((t,x) platform_ode(t, x, P), [0 10], x0, opts);MaxStep限制最大步长很关键不设的话求解器在平滑段会跳大步遇到铰点换向或者摩擦符号翻转时容易漏掉事件表现为支腿长度曲线出现台阶。RelTol和AbsTol也别放太松1e-4 以上在闭环里会累积成可见的相位偏差。Simulink 里对应的是变步长 solver 配置把 Solver 换成 ode15s、最大步长设成 1e-3效果和纯代码一致。5. 避坑清单仿真里最容易翻车的五个地方下面这几条基本都是我自己或者周围人真实踩过的现象、原因、解决按顺序写对着排查能省不少时间。5.1 逆解与几何建模的两个坑现象一平台绕 X 轴转 10°动画里却像是绕 Y 轴在转支腿长度曲线和手算对不上。原因基本锁定在旋转顺序。eul2rotm不写顺序时用默认值而不同工具箱版本默认值不完全一致加上有的脚本里传的是列向量导致维度对不上被隐式转置最终结果就是姿态矩阵和预期差一个置换。解决办法是全篇显式写ZYX并且在初始化时打印一次R和手算的三行三列对照确认前两行符号正确再往下走。现象二vecnorm算出来的六条腿长度很接近但平台明明已经倾斜了。原因是维度参数写成了dim2而数据是按列存的。按行求范数等于对每条腿的三个分量分别做了一次跨腿运算结果自然看起来对称又合理。解决办法固定数据布局所有点坐标统一按 3x6 列存vecnorm一律用dim1并在逆解外层加一次行程校验超限直接报错。5.2 动力学与求解器的两个坑现象三Simscape 模型一跑就报过约束或者动平台直接飞出去。并联机构的六条腿同时对动平台施加约束自由度数量刚好卡在临界任何一个铰链类型配错都会过定义。常见错误是把支腿两端都设成刚性连接或者球铰的三个转动自由度没锁对。解决办法是从三条腿开始逐步加约束每加一条跑一次 Mechanics Explorer确认机构还能动出现飞出去的情况先查质量属性惯量填成 0 或者单位写成 g·mm² 都会让求解器直接崩。现象四闭环仿真跑了半秒就卡住步长被压到 1e-9。摩擦项用了sign(dq)在速度过零时符号瞬间翻转求解器被迫无限缩步长去过零。解决办法是把库伦摩擦换成连续近似比如tanh(dq/eps_v)eps_v取 1e-3 到 1e-2或者保留sign但把求解器换成 ode15s 并设最大步长让事件检测接管过零处理。5.3 可视化与实时刷新的一条坑现象五动画卡成幻灯片drawnow一加帧率就掉到个位数。原因是每个时间步都在重建图形对象。plot3每次调用都新建 line 对象几百帧之后句柄堆积渲染线程扛不住。解决办法是初始化时把六条腿的 line 对象和动平台的 patch 一次性建好循环里只改XData、YData、ZData和变换矩阵刷新用drawnow limitrate实测能稳定在 50 到 100 Hz 的视觉刷新。另外别在循环里调axis equal它每次都会触发一次完整重排挪到循环外面。6. 可视化与轨迹验证用变换矩阵而不是重绘可视化这块的目的不只是好看而是把逆解和雅可比的结果变成肉眼可判的东西。我习惯用hgtransform管动平台的刚体变换六条腿各用一个 line 对象循环里只更新数据和矩阵。6.1 hgtransform 的正确用法hgtransform的Matrix属性接受完整的 4x4 齐次变换矩阵直接由旋转矩阵和平移向量拼出来。这里有个常见的写法错误makehgtform只支持translate、scale、xrotate这类关键字没有euler选项照着某些示例写成makehgtform(translate,p,euler,eul)会直接报参数错误。稳妥做法是自己拼矩阵。function animate_stewart(ax, hg_top, hg_legs, base_pts, top_pts, traj, fps) % traj: N x 6每行一个位姿 for k 1:size(traj,1) pose traj(k,:); R eul2rotm(pose(4:6), ZYX); T [R, pose(1:3); 0 0 0 1]; % 完整齐次变换矩阵 set(hg_top, Matrix, T); % 动平台整体跟着变换 p_world R * top_pts pose(1:3); for i 1:6 set(hg_legs(i), ... XData, [base_pts(1,i) p_world(1,i)], ... YData, [base_pts(2,i) p_world(2,i)], ... ZData, [base_pts(3,i) p_world(3,i)]); end drawnow limitrate % 限速刷新避免渲染拖垮仿真 pause(1/fps) end endhg_legs要在循环外一次性创建hg_top的父对象设成同一个坐标轴。drawnow limitrate会把刷新频率限制在渲染能力内不会阻塞计算循环太久比裸drawnow稳定得多。6.2 螺旋轨迹验证看支腿长度曲线而不是看动画轨迹验证我一般用一条螺旋上升加俯仰的组合既让六条腿都动起来又能逼出奇异区附近的异常。t 0:0.01:10; z 0.2*sin(2*pi*t) 0.1*t; pitch 0.1*sin(pi*t); traj [zeros(size(t)); zeros(size(t)); z; ... zeros(size(t)); pitch; zeros(size(t))]; L zeros(numel(t), 6); for k 1:numel(t) L(k,:) ik_stewart(traj(k,:), P.base_pts, P.top_pts); end figure; plot(t, L); grid on xlabel(时间 / s); ylabel(支腿长度 / m);判断标准不是动画顺不顺是这六条曲线有没有出现尖峰、平台段或者某条腿变化率明显高于其他腿。尖峰通常意味着轨迹穿过了奇异面附近雅可比条件数暴涨一段水平平台意味着该腿到了行程极限被钳位变化率异常则多半是雅可比符号错了。我现在的习惯是任何新轨迹在进 Simulink 闭环之前先在纯代码里跑一遍逆解、画长度曲线、看最大条件数、看行程余量四项都过了才往下接。这套流程强制走一遍比事后从发散曲线里倒推省事得多。资源包里的脚本按这个顺序跑基本能避开大部分几何和数值上的坑希望帮到你。本文还有配套的精品资源点击获取
