写这篇东西的起因是最近连续被几个做毕业设计和预研项目的朋友问到同一个问题一个六自由度的Stewart平台到底怎么在MATLAB里从零开始仿真问的人有搞飞控的、有做车辆运动模拟器的、也有想拿并联机构做精密定位的。Stewart平台这东西结构上看着就是六根腿撑着上下两个平台但真要把它跑起来难点藏在运动学解算、关节约束和驱动策略里。这篇文章就基于我在MATLAB环境下做Stewart平台并联机器人仿真的一套完整流程从运动学分析讲到Simulink/Simscape Multibody建模尽量把每一步为什么这么做、坑在哪都说清楚。适合正在做课程设计、机械臂或运动平台预研的读者也算是一个拿来就能上手的参考路线。1. 项目概述与建模思路1.1 这个仿真到底在仿真什么Stewart平台本质上是一个6-SPS机构也就是六个可伸缩的支腿连接固定基座和上方动平台每条腿的伸缩长度决定平台在三维空间里的位置和姿态。因为六个自由度互相耦合它不像串联机械臂那样“一个关节对应一个方向”随便动一条腿平台的位置和姿态会同时改变。这也是为什么做Stewart平台仿真时运动学解算永远排在第一位。在MATLAB里做这个仿真目标通常不是把实体模型画得多漂亮而是要在造零件之前回答几个关键问题逆运动学公式对不对、六条腿的行程够不够、给定平台运动轨迹时关节长度变化是否平顺、控制算法能不能跑得通。把这些在仿真环境里验证过一遍再去做硬件选型或者CAD装配心里才会真正有底。从工程角度看这类平台的应用范围很广飞行模拟器和汽车驾驶模拟器里给驾驶员提供六自由度运动感光学设备里用来做次镜精密调姿还有工业自动化里的对接、装配、振动隔离。不管用在哪个场景底层的运动学和动力学建模逻辑基本都是相通的只需要改参数和驱动方式。1.2 为什么用Simulink/Simscape而不是自己写微分方程有人会问运动学公式我都推出来了直接写个MATLAB脚本把六个腿长按公式算出来画成曲线不就行了对于只看运动学曲线确实够用。但一旦涉及平台受到负载、关节摩擦、惯性力这些动力学因素或者要加PID控制看闭环响应纯脚本就力不从心了。Simscape Multibody以前叫SimMechanics的价值在于你不需要手推并联机器人的动力学方程。你只需要把质量、转动惯量、关节类型和约束关系搭好软件会基于多体动力学自动建立微分代数方程并求解。这等于把“正向动力学”这块极其容易出错的活外包了让你能把精力集中在运动学、轨迹规划和控制策略上。另外Simscape Multibody自带3D可视化运行时可以直接看到平台如何运动、腿如何倾斜、关节是否干涉。这对排查构型设计问题非常直观。相比Adams这类专业多体软件Simulink/Simscape的优势是和控制环境天然打通不用把模型导出导入好几次。2. 运动学建模所有仿真的“地基”2.1 构型参数与坐标系定义做仿真之前先要把机械构型参数定清楚。我这里用的是最常见的6-6构型基座和动平台各有六个铰点分成三对每对之间隔120度每对内部两个铰点张开一定角度。这种构型姿态工作空间相对均匀也是很多论文和参考设计里能直接抄作业的布局。我这次仿真用的参数如下表所示你可以直接拿去做初始设计参数含义符号数值单位基座铰点分布圆半径Rb0.50m动平台铰点分布圆半径Rp0.35m基座铰点对间隔角-120deg基座铰点对内部半角αb15deg动平台铰点对内部半角αp15deg动平台相对基座的初始偏转-60deg初始平台高度h00.55m基座六个铰点的方位角我以平台正前方为0度三对铰点中心分别放在0度、120度、240度每个中心左右各扩15度所以基座铰点角度依次是-15、15、105、135、225、255度。动平台三对铰点中心放在60度、180度、300度也就是相对基座整体偏转60度这样布局能让六条腿在工作空间内分布得更对称避免出现大片奇异区域。坐标系定义方面基座坐标系原点放在基座中心Z轴竖直向上。动平台坐标系原点放在平台中心初始状态与基座坐标系方向一致。每个铰点在各自坐标系里的位置直接用“半径乘方向向量”算出来即可。2.2 逆运动学推导过程逆运动学解决的是已知平台要到达的位置和姿态求六条腿各自的长度。这是Stewart平台仿真里最核心的公式没有之一。假设平台的位置为向量p [x, y, z]^T平台姿态用欧拉角表示我这里统一按ZYX顺序先绕Z轴转偏航角ψ再绕新Y轴转俯仰角θ最后绕新X轴转滚转角φ。旋转矩阵写成R Rz(ψ) * Ry(θ) * Rx(φ)其中Rz、Ry、Rx分别是绕Z、Y、X轴的基本旋转矩阵。这个顺序必须固定下来因为同一个姿态用不同欧拉角顺序得到的矩阵是不一样的后面控制模块和Simscape里的姿态观测环节都要保持同一套约定。第i条腿的基座铰点位置记为b_i动平台铰点在平台本地坐标系里的位置记为a_i。平台运动后动平台铰点在世界坐标系里的位置是P_i p R * a_i那么从基座铰点指向动平台铰点的腿矢量就是l_i P_i - b_i p R * a_i - b_i第i条腿的长度是L_i ||l_i|| sqrt(l_i(1)^2 l_i(2)^2 l_i(3)^2)这个公式看着简单实际编程时要格外注意两点。第一a_i必须乘旋转矩阵R后再加平动位移p不能先加位移再旋转那会直接导致坐标关系错乱。第二b_i是在基座坐标系里的值默认基座不动如果基座本身相对世界坐标有平移或旋转还需要额外做一次坐标变换。在MATLAB里逆运动学函数可以写成下面这样function legs ik_stewart(pose, basePts, topPts) % pose [x, y, z, roll, pitch, yaw]单位米、弧度 x pose(1); y pose(2); z pose(3); r pose(4); p pose(5); yw pose(6); R rotz(yw) * roty(p) * rotx(r); legs zeros(6, 1); for i 1:6 legVec [x; y; z] R * topPts(i, :) - basePts(i, :); legs(i) norm(legVec); end endrotz、roty、rotx需要Robotics System Toolbox如果你没有这个工具箱也可以自己手写基本的旋转矩阵代码量也不大。2.3 正运动学与雅可比矩阵正运动学是逆运动学的反面已知六条腿长度求平台位姿。并联机器人正运动学比串联难很多因为一组腿长可能对应多个平台位姿而且没有闭合解只能用数值方法迭代求解。实际仿真里如果你用的是Simscape Multibody做动力学软件会自动帮你在后台解算正运动学不需要你手动做。但正运动学仍然值得实现一遍因为它是验证模型正确性和做奇异分析的重要工具。我最常用的方法是用Optimization Toolbox里的fsolve。把逆运动学函数当作误差函数找到一组位姿参数使该位姿下计算出的腿长等于目标腿长function pose fk_stewart(targetLegs, basePts, topPts, pose0) options optimoptions(fsolve, ... Algorithm, levenberg-marquardt, ... Display, off); pose fsolve((q) ik_stewart(q, basePts, topPts) - targetLegs, ... pose0, options); end为什么选Levenberg-Marquardt而不是默认的Trust-Region因为并联机构正运动学的误差函数在多数区域接近线性但接近奇异位形时会出现病态LM算法带阻尼项对初值的要求宽松一些实测收敛率更高。当然初值依然很重要最好用上一时刻的位姿作为下一时刻的初值这样连续轨迹求解时基本都能稳定收敛。雅可比矩阵描述关节速度与平台速度之间的映射关系对并联机器人来说它是6x6矩阵。第i行的构造方法是取当前时刻沿腿方向的单位向量e_i以及基座坐标系下从平台中心指向动平台铰点的向量r_i那么该行的两个3元素块分别是e_i和r_i × e_i。写成公式J_i [e_i^T, (r_i × e_i)^T]这个矩阵不是常数它随平台位形变化。雅可比行列式的绝对值接近零时平台处于奇异位形此时可能出现“腿锁死但平台仍有自由度”或者“平台在一个方向上失去刚度”的异常情况。做轨迹规划时可以把这个行列式值当作一个实时监控量一旦接近零就提示调整路径。2.4 运动学模型怎么在MATLAB里验证运动学代码写完之后千万别直接连仿真先做几个简单测试能省下无数排查时间。第一个测试是零点测试把姿态设为roll0、pitch0、yaw0位置设为[0, 0, h0]。此时平台与基座平行且同向逆运动学算出的六条腿长应该完全相等并且等于理论上初始腿长sqrt((Rb-Rp)^2 h0^2)。如果六个值有差异说明动平台铰点角度或者基座铰点角度编号有错位。第二个测试是纯平移测试只改变z比如把z从0.5改到0.6六条腿长应该一起增大或一起减小且变化量接近均匀。如果出现某些腿变长、某些腿变短就要检查是不是平台偏转角度定义错了。第三个测试是纯偏航测试把yaw设成5度左右其他保持初始位姿。此时六条腿长应该两两成组变化而且曲线关于前后方向对称。这个测试能快速暴露旋转矩阵连乘顺序的问题。我在实际项目里发现80%的“仿真飞了”并不是Simscape模型的问题而是运动学公式在根上就是错的。把这几个测试写成一个脚本以后改任何参数都能一键回归验证。3. Simscape Multibody仿真模型搭建3.1 整体架构与关键模块Simscape Multibody模型的基本思路是用Body表示刚体用Joint表示关节约束用Rigid Transform表示坐标系之间的相对位姿用World Frame和Solver Configuration搭出仿真环境。Stewart平台在Simscape里的装配方式我在经过几次尝试后觉得下面这个方案最稳基座用一根固定的圆柱体通过Rigid Transform固定到World Frame上代表地面安装座。动平台用一块圆形平板悬在基座上方。六条腿每一条都由三段组成下铰点球铰、中间可伸缩支腿、上铰点球铰。中间支腿的伸缩用Prismatic Joint实现这个平移关节就是六自由度平台的“驱动关节”。这里有一个容易被忽视的细节每条腿两端都用球铰Spherical Joint会引入绕支腿自身轴线的多余旋转自由度导致仿真时支腿绕自身乱转虽然不影响平台运动但看起来很别扭偶尔也会造成数值问题。更稳妥的做法是一端用球铰、另一端用万向节Universal Joint这样既保留了实现运动所需的自由度又消除了绕着支腿轴线自转的自由度。每个Prismatic Joint的Prismatic Axis默认沿关节局部坐标系的Z轴。为了让支腿方向与从基座铰点到动平台铰点的连线一致需要在装配每个支腿时用Rigid Transform把关节局部坐标系的Z轴转到腿的初始方向上去。这一步最容易出错尤其是六个腿方向各不相同最容易把符号搞反。3.2 参数设置与装配细节在Simscape里每个Body的“质量中心坐标系”决定了这个刚体的空间位置。给动平台Body设置Mass、Inertia等参数之前先确认Body坐标系原点在平台中心。动平台上的六个铰点在Rigid Transform里分别设置平移量就是2.1节算出来的平台本地坐标。基座上的六个铰点也一样先用脚本把坐标算好保存为baseAnchor6x3矩阵和topAnchor6x3矩阵。接下来在Simscape模型里每个铰接位置都用Rigid Transform的Translation字段引用这些参数。这样后续想改半径、改角度只需要在MATLAB工作区里改一次参数再重新初始化模型不用进Simscape界面手工一个个改。Prismatic Joint的初始状态特别重要。因为平台一开始放在h0高度每个平移关节的初始位置必须是当前几何关系下的腿长也就是逆运动学算出的一组值。很多第一次做Simscape并联机构的人遇到“Initial condition ... not satisfied”这类报错八成就是Prismatic Joint的初始位移没对上。具体操作时可以在模型初始化回调里用assignin把六个初始腿长写到基本工作区然后在Prismatic Joint的State Target选项卡里分别填Initial Position。注意Joint的State Target不是默认显示的需要把Block Parameters窗口右下角的“Show state target parameters”勾选上。3.3 用逆运动学生成驱动信号运动学模型搭好后接下来就是让平台动起来。最直接的驱动方式是Motion驱动也就是把逆运动学算出来的腿长作为六个Prismatic Joint的位置输入。这种方式适合验证运动学是否正确以及观测工作空间范围。在Simulink侧我一般用MATLAB Function块输入期望平台轨迹。比如让平台在x方向做幅值0.05m、频率0.5Hz的正弦运动同时yaw方向做一个±5度的慢摆function legCmds traj2legs(time, basePts, topPts) x 0.05 * sin(2 * pi * 0.5 * time); yaw 5 * pi / 180 * sin(2 * pi * 0.2 * time); roll 0; pitch 0; z 0.55; pose [x, 0, z, roll, pitch, yaw]; legCmds ik_stewart(pose, basePts, topPts); end这里用时间time作为唯一输入是因为我们这次做的是开环运动学验证平台轨迹由公式给定不需要反馈闭环。这个函数块输出的六个腿长是Simulink信号接到Simscape侧之前每个信号都要通过Simulink-PS Converter转成物理信号再送到对应的Prismatic Joint Actuator。如果你的控制策略是力矩控制比如要用力/位混合控制或者阻抗控制那就把Prismatic Joint的Actuation改成Force/Torque输入量不再是腿长而是六条腿的驱动力。这时候Simscape会自动根据动力学模型计算出平台的实际运动响应对应的腿长你再通过Joint Sensor读出来做反馈。两者的区别是Motion驱动是“规定它走这个轨迹”Force驱动是“给这么大的力看它怎么走”。我建议初学者先用Motion驱动跑通一遍确认机构的装配关系无误再切换到Force驱动做动力学控制研究。否则一上来就是力控制一旦模型发散你很难判断到底是装配错了、动力学参数错了还是控制器参数调得不对。3.4 仿真调试与数据观测Simscape Multibody自带的Mechanics Explorer能在仿真过程中实时显示三维动画这是排查机构干涉和关节方向问题的最好工具。运行时留意每个支腿是否乱转、铰点是否穿模、动平台是否出现明显不对的翻转。要定量验证运动学仿真结果需要用Transform Sensor测量动平台坐标系相对基座坐标系的位姿同时用Joint Sensor测出六个Prismatic Joint的实际位移。把它们和输入的期望轨迹放在同一个Scope里对比。如果Motion驱动模式下实际位姿与期望轨迹存在偏差说明关节初始位置或方向配置有问题如果完全重合那至少说明运动学实现是对的接下来的动力学和控制研究就有一个可靠的基础平台。仿真求解器设置方面Simscape Multibody模型本质上是刚性的微分代数方程使用变步长求解器时推荐ode15s或ode23t。不要图省事用ode45很多并联机构模型跑到中途会因步长太小报错换成ode15s后明显稳定。仿真时长设成10秒到20秒比较合适既能看清周期运动规律又不会让调试循环太慢。4. 常见问题与避坑记录4.1 初始化失败的三个典型原因Simscape模型最常见的报错是初始化失败。根据我的经验九成情况都可以归到下面三类第一类是腿长没有按当前几何关系预设。平台放在初始高度但Prismatic Joint初始位置还是默认的0模型内部必然出现约束冲突。解决办法前面说了用逆运动学算出初始腿长填到State Target里。第二类是Rigid Transform的方向装反了。比如基座侧球铰的安装方向用了负数导致腿向量指向平台内部。这类问题在Mechanics Explorer里最容易发现几何上看起来是腿“穿”过了平台。第三类是欧拉角顺序不一致。Simscape侧如果用了XYZ顺序而你的逆运动学函数用ZYX顺序那么即使公式代码没错驱动出来的姿态也会“拧”得不对。建议在Simulink模型里把所有姿态参数都显式标注顺序或者干脆用四元数做接口绕开欧拉角顺序的坑。4.2 仿真发散与代数环处理Motion驱动的开环仿真一般不容易发散真正容易中招的是闭环控制。当你把Transform Sensor测到的实际位姿反馈给控制器控制器算出新的腿长再驱动模型时如果信号链路是纯直通的Simulink可能报出代数环。处理代数环最实用的办法是在反馈回路里加一个Unit Delay或者把控制器的采样时间设成离散模式。并联机构的机械惯性通常不小一个仿真步长的延迟对控制效果影响很小但能立刻把代数环消掉让仿真顺畅跑起来。如果模型本身发散得毫无规律先检查是不是某个关节的初始速度、初始加速度没有设置。Simscape Multibody默认初始速度为零如果你在Motion驱动模式下给了一个阶跃形式的腿长命令初始瞬间会产生很大的冲量可能导致平台剧烈震荡甚至发散。解决办法是把输入轨迹统一用正弦或smoothstep形式的平滑信号避免阶跃突变。4.3 正运动学不收敛的排查思路用fsolve做正运动学时最常见的现象是报“求解器提前终止”或者收敛到错误的位形。第一个排查项是初值把上一时刻的位姿作为当前求解的初值能大幅提高连续性轨迹的求解成功率。第二个排查项是误差函数的量纲。位置误差是米姿态误差是弧度两者数值范围差别很大。如果同时出现在一个误差向量里LM算法可能会优先优化数值更大的位置分量导致姿态正确但位置差一点、或者反过来。我习惯把位置误差除以0.1米、姿态误差除以0.1弧度做一下无量纲化让两者权重相当实测收敛效果明显好转。第三个排查项是奇异位形。当雅可比矩阵接近奇异时正运动学方程组本身就病态此时不要继续硬迭代应该修改期望轨迹绕开奇异点。这也是为什么我建议把雅可比行列式监控放在轨迹规划模块里而不是等到仿真发散后才去查。4.4 参数设定小技巧最后分享几个我实际用下来很省事的技巧。第一所有机械参数一律从MATLAB脚本里定义然后在Simscape模型的参数初始化回调中通过assignin注入。不要直接在Simscape块界面上填数字否则改一次参数要改十几个模块极易漏改出错。第二把逆运动学、正运动学、雅可比矩阵三个函数封装成独立脚本Simulink里的MATLAB Function只负责调用。这样既能单独测试运动学也能在模型运行时用同一个函数做在线计算避免逻辑重复。第三做参数扫描时用Simulink.SimulationInput配合parfor批量跑可以把不同半径、不同高度下的工作空间数据一口气算出来。比如我想看平台在一定腿长行程内能扫过的姿态范围每组参数设置好之后并行仿真最后把结果汇总画成工作空间云图效率比在Simscape界面里手工改参数高出一个量级。我在这个项目里踩过的坑不算少但最值得说的一条是运动学代码的验证永远要放在Simscape建模之前至少要把2.4节那三个测试跑过再做下一步。现在每次拿到新的并联机构参数我都会先把运动学脚本跑一遍确认数值合理才允许自己打开Simscape模型。看起来多花十分钟实际上能省掉后面好几个小时的查错时间。这个习惯比我用过的任何仿真技巧都管用。