Stewart并联平台逆解、雅可比与Simscape动力学仿真

简介面向机器人控制与并联机构研究的科研人员及技术爱好者这份文档以Stewart并联平台为对象系统梳理从运动学逆解到动力学仿真的完整实现流程。内容围绕坐标变换与欧拉角转旋转矩阵、六条支腿长度计算、雅可比矩阵的微分求解展开并以牛顿-欧拉法建立动力学方程配合Simscape Multibody搭建三维仿真模型三维可视化部分给出实时更新平台位姿的绘制方法并对刷新效率与刚体变换的实现方式作了说明。压缩包内为1个PDF文件约116KB篇幅紧凑适合作为理论推导与MATLAB实践之间的衔接材料。已有192人学习。读者可从中获得逆解与动力学求解的代码组织思路、仿真模型搭建要点以及奇异位形检测、轨迹规划优化、刚性系统求解器选择等常见问题的排查方向便于在MATLAB环境中复现并对照调试。1. Stewart并联平台仿真先别急着搭模型逆解错一毫米动力学力就全歪了六自由度Stewart并联平台做运动学逆解和动力学仿真很多人第一反应是直接拖Simscape Multibody模块把六条腿连起来看动画。真实情况是动画能动不代表逆解对逆解能对上位置也不代表雅可比符号对而雅可比符号一错动力学仿真里的执行器力会整条曲线反过来。MATLAB在这里的价值不是画漂亮曲线而是把铰点坐标、旋转矩阵、腿长零位、速度映射和力映射串成一条可对拍的链。适合做并联机构控制、实验台标定、半实物仿真的人。先把几何和约定钉死再谈动力学后面每一步都能复现。2. 先把坐标系和铰点顺序钉死Stewart并联平台逆解的几何建模与参数表逆解公式本身并不长真正让人返工的是约定基座坐标系原点放在哪、动平台参考点取哪个点、六个铰点从哪开始编号、旋转矩阵按什么顺序乘、腿长零位是总长还是执行器位移。我见过同一套几何参数两个人算出来的腿长差十几毫米最后发现不是公式错而是一个人把上平台第3个铰点对到了下平台第4个铰点。Stewart平台是闭链机构六条腿互相约束单个腿长对不上后面动力学力曲线就会变成黑匣子看着有数但不知道数从哪来。先把几何模型写成可检查的矩阵再写逆解函数比在Simscape里反复试装配要省时间。2.1 位姿向量q[x y z roll pitch yaw]旋转顺序写错后面全白算我一般把动平台位姿写成行向量q [x y z roll pitch yaw]位置单位用米角度单位用弧度。位置t q(1:3)是动平台参考点在基座坐标系下的坐标姿态用绕固定轴X-Y-Z的ZYX顺序也就是先滚转、再俯仰、最后偏航旋转矩阵写成R Rz(yaw)*Ry(pitch)*Rx(roll)。这个约定必须在函数注释里写死因为从三维模型导出的姿态可能是XYZ也可能是ZYX甚至有的工具给的是四元数。约定不写后面数值对拍就会变成玄学。function R rotZYX(roll, pitch, yaw) % 输入为弧度旋转顺序先绕X滚转再绕Y俯仰最后绕Z偏航 Rx [1 0 0; 0 cos(roll) -sin(roll); 0 sin(roll) cos(roll)]; Ry [ cos(pitch) 0 sin(pitch); 0 1 0; -sin(pitch) 0 cos(pitch)]; Rz [cos(yaw) -sin(yaw) 0; sin(yaw) cos(yaw) 0; 0 0 1]; R Rz * Ry * Rx; end这段代码只做一件事把三个角度按固定顺序乘成旋转矩阵。roll、pitch、yaw都必须是弧度如果上游给的是角度先在入口处deg2rad不要在函数内部偷偷转换。R的列是动平台坐标轴在基座坐标系下的方向后面算铰点位置时统一用R * platformPoint。如果发现仿真里动平台绕某个轴转反了先检查这里是不是把Rz*Ry*Rx写成了Rx*Ry*Rz不要急着调控制器。2.2 六条支腿的闭式逆解从铰点坐标到腿长和雅可比行设基座铰点在基座坐标系下为b_i动平台铰点在动平台坐标系下为p_i。给定位姿后动平台铰点在基座系下的位置是t R*p_i第i条腿的向量是L_i t R*p_i - b_i腿长就是norm(L_i)单位方向是u_i L_i / norm(L_i)。速度层面腿长速度与动平台速度的关系可以写成legDot J * [v; omega]其中v是动平台参考点线速度omega是基座系下的角速度J的第i行是[u_i, (R*p_i × u_i)]。这个叉乘项用R*p_i不是p_i - b_i也不是p_i符号错一位执行器力就会反向。function [legLen, u, J, pWorld] stewartIK(q, geom) % q [x y z roll pitch yaw]行向量长度单位m角度单位rad t q(1:3); R rotZYX(q(4), q(5), q(6)); n size(geom.base, 2); % 支腿数Stewart平台通常n6 legLen zeros(n,1); u zeros(3,n); J zeros(n,6); pWorld zeros(3,n); for i 1:n pWorld(:,i) t R * geom.platform(:,i); L pWorld(:,i) - geom.base(:,i); legLen(i) norm(L); u(:,i) L / legLen(i); % J行对应第i条腿[线速度系数, 角速度系数] J(i,:) [u(:,i), cross(R * geom.platform(:,i), u(:,i))]; end endgeom.base是3×6矩阵每列一个基座铰点geom.platform是3×6矩阵每列一个动平台铰点两边的列号必须人为约定为同一条腿。legLen是两铰点之间的总距离不是执行器行程。如果Simscape里的Prismatic Joint读数是相对零位的位移需要另外减legLen0这一步放在外层做不要混在逆解函数里。J是6×6矩阵行对应腿列对应[vx vy vz wx wy wz]后面做速度验证和静力学映射都用它。2.3 参数表与单位统一腿长零位、铰点偏置和安装角建模前先把参数表填完整比在代码里到处写常数可靠。下面是一组示例参数只用于跑通流程实际项目要换成自己的几何数据。单位不统一是血泪经验有人在CAD里用毫米在MATLAB里按米算腿长差1000倍Simscape一开就飞。参数符号示例值单位说明基座铰点半径Rb0.35m六个下铰点分布圆半径动平台铰点半径Rp0.18m六个上铰点分布圆半径初始高度h00.45m动平台参考点在基座系下的z值铰点分布角theta30deg相邻铰点夹角按实际布置填腿长零位len0由逆解算mq0对应的六条腿总长执行器行程stroke0.12m只用于检查腿长是否超限铰点顺序建议在MATLAB里画一次三维散点连线把geom.base和geom.platform的每一列用线连起来看六条线是否交叉成合理的腿而不是在空中打结。零位legLen0 stewartIK(q0, geom)要保存下来后面做Prismatic Joint初始位置、执行器位移、力曲线零偏都用它。参数表里还要写清楚铰点是否有安装偏置比如球铰中心不在平台表面上这种偏置如果漏掉逆解残差可能只有几毫米但动力学力曲线在高负载下会明显偏。3. 用MATLAB把逆解跑成可复用函数数值验证、雅可比与工作空间检查逆解函数写完不代表能用至少要做三件事批量位姿能不能算、雅可比与数值微分能不能对上、工作空间里有没有腿长超限和接近奇异的区域。很多人只检查初始位姿的六条腿长看和CAD量出来差不多就往下走结果轨迹一给大范围某条腿先到行程末端仿真直接报约束错误。逆解是后续所有仿真的入口入口函数要能批量调用输出维度要固定错误要能定位到第几条腿而不是只返回一个“失败”。3.1 逆解函数怎么定输入输出批量位姿、行向量和列向量别混我一般让stewartIK只接受单条位姿行向量批量轨迹在外面循环。这样函数简单调试时容易在第k个点停下来看六条腿。批量计算时先预分配leg zeros(N,6)不要每轮扩数组。下面这段代码给一条小幅螺旋轨迹检查六条腿长是否连续、有没有某条腿突变。q0 [0 0 0.45 0 0 0]; geom.base makeCirclePoints(0.35, 30, 6); % 自定义函数按实际铰点布置生成 geom.platform makeCirclePoints(0.18, 30, 6); N 2001; tt linspace(0, 4, N); q repmat(q0, N, 1); q(:,1) 0.02 * sin(2*pi*0.5*tt); q(:,2) 0.015 * sin(2*pi*0.5*tt pi/3); q(:,6) deg2rad(3) * sin(2*pi*0.25*tt); leg zeros(N,6); for k 1:N leg(k,:) stewartIK(q(k,:), geom); end plot(tt, leg, LineWidth, 1.0); xlabel(时间 t / s); ylabel(腿长 / m); grid on;q的每一行是一个位姿循环里取q(k,:)传给逆解返回的legLen是6×1列向量转置后放进leg(k,:)。makeCirclePoints需要按实际铰点分布实现示例里只表达“半径、起始角、数量”这三个输入。画图不是为了好看而是看六条腿长是否在同一个量级、变化是否连续。如果某条腿长在某个时刻突然跳变优先检查铰点顺序和姿态角是否过零不要先怀疑求解器。3.2 雅可比矩阵与数值微分对拍世界系角速度和机体系角速度要分清雅可比矩阵最容易错在角速度参考系。上面的J推导用的是基座系下的角速度omega数值验证时也要用基座系角速度构造旋转增量R1 expm(skew(dt*omega)) * R0。如果写成R0 * expm(skew(dt*omega))那是机体系角速度和J的列定义不一致误差会随着姿态角变大而变大。下面这段对拍代码直接用旋转矩阵算腿长绕开欧拉角速度转换避免把q_dot(4:6)误当成角速度。function err checkJacobian(geom, q, v, omega) % v和omega均为基座系3×1向量omega单位rad/s dt 1e-6; t0 q(1:3); R0 rotZYX(q(4), q(5), q(6)); [~,~,J] stewartIK(q, geom); legDotJ J * [v(:); omega(:)]; R1 expm(skew(dt*omega)) * R0; R2 expm(skew(-dt*omega)) * R0; len1 legLenFromTR(t0 dt*v, R1, geom); len2 legLenFromTR(t0 - dt*v, R2, geom); legDotNum (len1 - len2) / (2*dt); err norm(legDotJ - legDotNum) / max(norm(legDotNum), eps); end function S skew(w) S [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; end function len legLenFromTR(t, R, geom) n size(geom.base, 2); len zeros(n,1); for i 1:n L t R * geom.platform(:,i) - geom.base(:,i); len(i) norm(L); end endcheckJacobian返回相对误差正常情况下应该在1e-6到1e-8量级。如果误差在1e-2以上先检查叉乘项用的是R*p_i还是p_i - b_i再检查omega是世界系还是机体系。dt不要取得太大中心差分会引入截断误差也不要取得太小浮点相减会放大舍入误差。我一般取1e-6秒配合双精度够用。这个对拍脚本建议和逆解函数放在同一个文件夹改一次铰点就跑一次比事后看力曲线猜原因省时间。3.3 工作空间和奇异位形扫描腿长超限只是第一道门槛工作空间扫描不能只看腿长是否在[lenMin, lenMax]内还要看雅可比矩阵的条件数。Stewart平台接近奇异时腿长可能仍然在行程内但速度映射会变得很差动力学仿真里的执行器力会出现尖峰。下面这段代码在固定姿态下扫xy平面把腿长超限的点设为NaN其余点画条件数。注意J的前三列是长度量纲后三列包含长度平方量纲直接cond(J)会受单位影响更稳妥的做法是做尺度归一化或者分别看线速度和角速度子块的最小奇异值。xs linspace(-0.08, 0.08, 41); ys linspace(-0.08, 0.08, 41); z0 0.45; condMap nan(numel(xs), numel(ys)); for i 1:numel(xs) for j 1:numel(ys) q [xs(i) ys(j) z0 0 0 0]; [len,~,J] stewartIK(q, geom); if any(len 0.30) || any(len 0.55) condMap(i,j) NaN; else condMap(i,j) cond(J); end end end imagesc(xs, ys, condMap); axis xy; colorbar; xlabel(x / m); ylabel(y / m); title(雅可比条件数扫描);len 0.30和len 0.55只是示例行程边界实际要按执行器行程和铰点几何替换。condMap里NaN表示腿长超限颜色越亮表示条件数越大越接近奇异。这个图只能做初筛不能代替完整的六维工作空间因为姿态角也会显著改变条件数。我的习惯是把轨迹上的每个点都算一遍最小奇异值低于阈值就减速或改轨迹不要等Simscape报约束错误再回头找。4. 从逆解到动力学仿真Simscape Multibody里驱动、力和求解器怎么配逆解跑通之后动力学仿真要回答两个不同的问题如果给定六条腿的运动动平台需要多少驱动力如果给定六条腿的驱动力动平台会怎么运动。前者是运动学驱动下的逆动力学后者是力驱动下的正动力学。很多人把两者混在一个模型里Prismatic Joint同时给Motion和Force仿真一开始就报过约束或者动平台直接飞走。Simscape Multibody能处理闭链但闭链的初始装配、驱动模式和求解器设置必须一致否则曲线对不上不是算法问题而是模型配置问题。4.1 刚体树怎么连基座、六条腿、动平台和闭链初始装配常见建法是世界坐标系下固定基座刚体六个下球铰装在基座上每条腿由下球铰、下腿刚体、Prismatic Joint、上腿刚体、上球铰组成六个上球铰连接到动平台刚体。六条腿和上下平台形成闭链动平台不再额外加6-DOF Joint否则会过约束。闭链模型最怕初始装配不满足逆解六个Prismatic Joint的初始位置如果不是q0对应的腿长求解器会在第一个步长强行拉回力曲线出现巨大尖峰。我一般先在MATLAB里算legLen0 stewartIK(q0, geom)把每条腿的初始位置按这个值设置再开仿真。q0 [0 0 0.45 0 0 0]; [legLen0, ~, ~] stewartIK(q0, geom); % 写入基础工作区供Simscape模型参数引用 assignin(base, legLen0, legLen0); assignin(base, geom_base, geom.base); assignin(base, geom_platform, geom.platform); % 轨迹先小幅正弦确认装配和驱动模式 N 4001; tt linspace(0, 4, N); q repmat(q0, N, 1); q(:,3) q0(3) 0.02 * sin(2*pi*0.5*tt); legCmd zeros(N,6); for k 1:N legCmd(k,:) stewartIK(q(k,:), geom); end legDisp legCmd - legLen0; % Prismatic Joint若读相对位移用这个 assignin(base, tt, tt); assignin(base, legDisp, legDisp);legLen0是六条腿的总长legCmd是轨迹上每条腿的总长legDisp是相对零位的执行器位移。Simscape里Prismatic Joint的初始位置要设成0还是legLen0取决于关节的零位定义如果关节零位对应两铰点重合那初始位置是legLen0如果零位对应装配位置那初始位置是0输入用legDisp。这个区别不写清楚轨迹会整体偏一个常数肉眼看动画不明显但力曲线会有预载。4.2 运动学驱动还是动力学驱动Prismatic Joint的Motion与Force不能同时开运动学驱动适合验证逆解、轨迹、干涉和关节行程把六个Prismatic Joint的Actuation设为Motion输入legDisp时间序列仿真输出关节约束力和动平台位姿。动力学驱动适合求执行器力、负载响应和能量把Actuation设为Force输入六条腿的驱动力仿真输出动平台运动。两种模式不能同时开。如果既给Motion又给Force模型要么报过约束要么求解器把力当成外部扰动曲线没有物理意义。我一般分两轮跑第一轮Motion驱动确认仿真腿长与逆解逐点对上动平台位姿和输入q一致第二轮切Force驱动先用重力平衡力做静态保持再加轨迹力。切Force之前要把Motion输入断开检查Prismatic Joint的Actuation选项、初始位置和阻尼。如果第一轮Motion驱动下腿长就对不上不要切Force因为动力学只会把误差放大。4.3 求解器、步长和执行器力输出让仿真腿长与逆解逐点对上闭链机构对求解器比较敏感。固定步长ode4配1e-3秒适合快速看趋势但约束刚性大时力曲线会有高频抖动变步长ode23t或ode15s更适合闭链微分代数方程代价是仿真速度慢。我一般先用变步长跑一遍确认没有装配错误再用固定步长做实时性评估。仿真输出至少记三组六条腿的实际位移、六条腿的执行器力、动平台位姿。把实际腿长和逆解legCmd逐点相减最大残差应该接近0如果残差随时间漂移检查零位和单位如果残差在换向时尖峰检查球铰约束和步长。set_param(stewart_dyn, StopTime, 4); set_param(stewart_dyn, Solver, ode23t); set_param(stewart_dyn, RelTol, 1e-5); sim(stewart_dyn); legSim logsout.get(legDisp).Values.Data; tau logsout.get(actuatorForce).Values.Data; residual legSim - legDisp; fprintf(腿长最大残差 %.3e m\n, max(abs(residual(:))));stewart_dyn是Simscape模型文件名logsout里要提前把关节位移和执行器力勾选为记录信号。RelTol从1e-5开始试太小会拖慢仿真太大则力曲线毛刺明显。residual是仿真腿长减逆解腿长正常情况下应在数值误差量级如果达到毫米级先查Prismatic Joint零位再查输入时间序列是否和求解器步长对齐。执行器力tau的符号约定也要和J对拍用静力学tau J \ F或tau J^{-T} F验证一个静态位姿符号反了就改J的行顺序或力输出符号不要靠调增益掩盖。5. Stewart平台仿真最常见的4个翻车点从腿长对不上到力曲线发散Stewart平台的坑大多不在高级算法而在约定和装配。下面四条是我在模拟项目X和几个实验台模型里反复见到的翻车点每条按现象、原因、解决写。新手可以当成检查清单熟手可以对照看看自己的模型有没有类似隐患。出现问题时先别改控制器参数先把逆解、零位、驱动模式和求解器四件事排一遍很多“玄学抖动”其实有明确的几何或配置来源。5.1 腿长对不上铰点顺序、旋转方向和零位偏置踩坑1逆解腿长和CAD测量差几毫米到几十毫米。现象是初始位姿六条腿长两两不等或者某一条明显偏离。原因通常是上下铰点编号顺序不一致比如上平台按顺时针编号、下平台按逆时针编号逆解把第3条腿连到了第4个下铰点也可能是旋转矩阵顺序和CAD姿态约定相反。解决方法是先在q00的对称位姿下检查六条腿长是否相等再把geom.base和geom.platform的每一列画三维连线看六条线是否交叉旋转方向用右手系核对必要时把roll或yaw取反做一次对比确认哪一组和CAD一致。踩坑2Simscape初始就报装配失败。现象是仿真还没开始就提示约束不满足或者动平台被拉到奇怪位置。原因多半是Prismatic Joint初始位置没有设成逆解腿长或者模型里关节零位是装配位置而输入却用了总长。解决方法是先算legLen0确认关节读数是总长还是相对位移如果是相对位移初始位置设0输入用legCmd - legLen0如果是总长初始位置设legLen0输入不要再减。这个符号错一次后面力曲线会整体带预载很容易被误判成摩擦。5.2 一给力就飞驱动模式冲突与质量单位踩坑3切到力驱动后动平台瞬间飞走或穿模。现象是Motion驱动时一切正常切Force后第一步就发散发散。原因常见有三种Prismatic Joint同时开了Motion和Force刚体质量单位从kg写成了g或者重力方向反了初始位置没有做重力平衡执行器力从0开始动平台直接下落。解决方法是检查每个关节的Actuation只有一个来源统一用SI单位先在重力下给一个静态保持力确认动平台能停住再加轨迹力。如果静态保持力符号反了动平台会加速下落这时候不要加大阻尼先把力符号改对。踩坑4执行器力曲线在换向时出现很大尖峰。现象是腿长轨迹平滑但力曲线在正弦换向点出现窄脉冲。原因可能是球铰初始穿透、闭链约束刚性大、固定步长太大或者质量比悬殊导致求解器频繁切换。解决方法是先对齐初始装配把步长减半试一次再用变步长求解器对比检查上下腿质量和动平台质量是否在一个合理量级必要时给关节加小阻尼。尖峰如果只出现在仿真第一步通常是初始条件不连续如果每个换向都出现优先查零位预载和约束间隙。5.3 力曲线高频抖动步长、约束和零位预载除了上面四条还有一种现象是力曲线持续高频抖动动画看不出异常。原因可能是固定步长相对闭链约束太粗也可能是Prismatic Joint零位偏置导致执行器一直带着预载还可能是球铰约束和移动副方向不完全共线。解决方法是先看腿长残差是否也抖如果残差抖问题在约束或步长如果残差平顺而力抖问题在质量、阻尼或力输出滤波。我的习惯是先把仿真步长减半、求解器换变步长各跑一次再决定要不要动模型参数。不要一上来就加低通滤波滤波会把真实的约束力尖峰藏起来后面做控制器设计时容易误判执行器带宽。6. 用逆解残差和能量守恒做验收三个不依赖商业软件的验证技巧逆解和动力学模型跑通后我一般不做“看起来差不多”的验收而是留三个可重复的检查脚本。第一个是逆解残差给定一组腿长用最小二乘找位姿使stewartIK算出的腿长与输入腿长一致残差应该接近0这相当于用数值正解反查逆解不需要额外商业软件。第二个是雅可比数值对拍前面已经写过改铰点后必须重跑。第三个是能量守恒在无驱动、无外力的自由摆动或重力下落工况下机械能动能势能的变化应该只来自阻尼在Motion驱动下输入功率与动能变化率、势能变化率、耗散功率之和应该对得上。下面给一个能量残差的短检查框架。% 假设已记录t, q, qdot, tau, legDot % q为动平台位姿qdot为[v; omega]tau为六条腿执行器力 m 12.0; g 9.81; I diag([0.08 0.08 0.12]); Ek 0.5*m*(qdot(1:3)*qdot(1:3)) 0.5*qdot(4:6)*I*qdot(4:6); Ep m*g*q(3); P_in tau * legDot; P_out gradient(Ek Ep, t); residualPower P_in - P_out; fprintf(功率残差RMS %.3e W\n, rms(residualPower));这段代码只表达验收思路Ek用平动加转动动能Ep用重力势能P_in是六条腿执行器输入功率P_out是机械能变化率。实际模型里还要加腿自身质量、转动惯量和关节阻尼I也要换成动平台真实惯量。残差RMS不用追求绝对0先看量级是否合理如果功率残差随时间单调增大通常是质量或惯量参数漏了或者力符号反了。我一般会把q0、legLen0、雅可比对拍脚本和这个能量残差脚本放在同一个文件夹里每次改铰点先跑这三项再开Simscape。希望帮到你。本文还有配套的精品资源点击获取