搞并联机器人的应该都有这个印象——网上聊Stewart平台正解的资料一大堆但真轮到自己要写逆解代码的时候反而要翻半天。我第一次接触这个是在做六自由度运动模拟台的时候当时最急的还不是控制策略而是先把一条最基本的链路跑通给平台一个位姿六根电动缸各自该伸多长。这个问题的名字就叫运动学逆解也是Stewart平台所有后续工作的地基。先说结论Stewart平台的运动学逆解并不难核心就是坐标系变换加上两点距离公式甚至不需要解任何非线性方程。但为什么很多新手第一次写出来的代码在零位就错了问题往往出在姿态描述、铰点布置、初始高度这些细节上。这篇我会把物理模型、数学推导、MATLAB完整代码、验证方法和调试中踩过的坑一起讲清楚。代码可以直接抄但建议跟着思路过一遍后面换参数、改结构才不会一脸懵。1. Stewart平台的逆解到底在解什么抛开矩阵先看物理结构1.1 六个杆、十二个铰点和两套坐标系Stewart平台最经典的构型是6-SPS六个驱动杆并联连接固定基座和动平台每个杆上端通过球铰连到动平台下端通过万向铰连到基座。整台机器只有六个移动副是主动自由度平台的空间六自由度完全由六根杆的长度决定。这句话反过来就是逆解的全部意义给定期望的平台位姿反推每根杆应该伸缩到什么长度。要把这个反推过程写成程序第一件事是定义两套坐标系。一套叫固定坐标系B固连在基座上原点放在基座铰点分布圆的圆心Z轴竖直向上另一套叫动坐标系P固连在运动平台上原点放在平台铰点分布圆的圆心。平台位姿就是用动坐标系相对固定坐标系的变换来描述包含三个平移量x, y, z和三个旋转量横滚、俯仰、偏航我后面统一写成roll、pitch、yaw。物理上为什么必须搞两套坐标系因为每个平台铰点在动坐标系里的坐标是固定不变的我们只要把动坐标系搬到目标位姿就能算出每个平台铰点在固定坐标系里的全局坐标再和基座铰点坐标做差取模长就是杆长。整个过程里唯一的运动学计算就是坐标变换剩下的全是一次范数。1.2 为什么并联机构的逆解比串联机构友好得多我见过一些人用串联机械臂的思路去套并联机构结果把自己绕晕了。串联机械臂的逆解之所以难是因为从基座到末端只有一条运动链给定末端位姿后每个关节角度之间相互耦合常常要解多元非线性方程甚至多项式方程。Stewart平台则完全不同它的六条驱动链在结构上彼此独立从基座铰点到平台铰点每一条都是独立的开链平台位姿一经确定每条腿的长度就直接由该腿两端铰点的空间距离决定不需要把它们放在同一个方程组里联立求解。用个不严谨但很好记的类比想象你站在地面用六根绳子拉一个悬空的圆盘盘子的姿态动一点每根绳子要收放的长度是各算各的——你分别量每根绳子的两个固定点之间的距离就行并不会出现调绳子A之前必须先知道绳子B的最终长度这种耦合。当然实际控制里各腿确实有协调问题但那是控制层的事逆解层面它就是一个纯粹的几何计算。这一节的代码基础是B矩阵保存六个基座铰点坐标P_local矩阵保存六个平台铰点在动系中的坐标一次旋转加平移六个norm搞定逆解完全没有迭代。后面所有代码都围绕这个物理结构展开。2. 姿态描述与铰点布局MATLAB实现前必须先敲定的三个细节2.1 ZYX欧拉角旋转矩阵顺序错一个符号满盘皆输姿态矩阵是我见过最容易写错的部分。MATLAB自带的eul2rotm确实方便它默认的旋转顺序是ZYX也就是先绕固定轴Z转yaw再绕新轴Y转pitch最后绕最新轴X转roll。但很多教程里的示例用的是别的顺序有人直接抄了个XYZ的旋转矩阵过来一上平台模型整个拧成麻花还找不到原因。为了不依赖工具箱、也为了把顺序锁死我在下面代码里手写了一个ZYX旋转矩阵。推导也不复杂就是把三个基本旋转矩阵按照从右往左的顺序相乘R Rz(yaw) * Ry(pitch) * Rx(roll)展开之后得到cx cos(roll); sx sin(roll); cy cos(pitch); sy sin(pitch); cz cos(yaw); sz sin(yaw); R [ cz*cy, cz*sy*sx - sz*cx, cz*sy*cx sz*sx; sz*cy, sz*sy*sx cz*cx, sz*sy*cx - cz*sx; -sy, cy*sx, cy*cx ];我的建议是即使是经验比较多的工程师也不要凭记忆背这个6行矩阵每次用都从三个基旋转矩阵现乘一遍顺带检查一下R * R是否等于单位阵、det(R)是否等于1。运算量几乎可以忽略但能挡住绝大多数低级错误。这里还要特别提醒一个老生常谈的坑MATLAB的cos、sin输入单位是弧度而工程上习惯用角度。我自己的习惯是参数里一律用弧度存、用弧度算只在最后打印或者画图时乘180/pi换算成角度给人看。一旦在某个角落写了个sin(30)平台姿态就会莫名其妙地偏而且这种错不会报错只能靠验证环节抓出来。2.2 基座和平台铰点的成对偏置布局Stewart平台的铰点并不是均匀分布六个点那样会形成奇异位形实际绝大多数设计采用成对偏置布局基座六个铰点分成三对每对内部有一个角度间隔三对之间相隔120度平台也类似而且每对的间隔通常和基座不同。我用的这套参数是比较典型的三角形分置方案构件分布圆半径成对半夹角铰点方位角度基座0.8 m10度10, 50, 130, 170, 250, 290平台0.6 m20度20, 40, 140, 160, 260, 280表里成对半夹角的意思是每对铰点相对该对中心轴线的偏置角代码里我用theta_b、theta_p表示整个对内部的夹角铰点坐标生成时用半角。生成逻辑写成循环phi_b zeros(1, 6); phi_p zeros(1, 6); for i 0 : 2 phi_b(2*i1) i*120*pi/180 params.theta_b/2; phi_b(2*i2) i*120*pi/180 60*pi/180 - params.theta_b/2; phi_p(2*i1) i*120*pi/180 params.theta_p/2; phi_p(2*i2) i*120*pi/180 60*pi/180 - params.theta_p/2; end这段循环的实质是在60度、180度、300度三个对中心位置两侧分别放一个铰点基座和平台的对中心重合但每对内部的开口角度不一样。为什么基座和平台要用不同的成对夹角从运动学直观上看这样的布局让六个驱动链在空间上形成良好的力传递各向同性减小某个方向上出现机构奇异的风险从逆解结果上看它让零位时六根杆长呈现三组两两相等的对称关系这恰好成了后面验证代码是否正确的一个有力判据。如果你的项目只需要小角度运动可以适当缩小这里的角度差来获得更大的结构刚度如果追求大工作空间则要反过来加大布置差异同时留意杆长行程和铰点转角极限。2.3 初始位形高度h0到底放在哪里很多第一次写逆解的人会在初始高度上栽跟头。平台铰点在动坐标系里的坐标无论在哪个位形都是[Rpcos(phi_p); Rpsin(phi_p); 0]这样的形式这里的Z坐标是0因为动系原点就定义在平台铰点分布圆的圆心。而固定坐标系的原点在基座铰点分布圆圆心所以当平台处于零位姿态为单位阵时动系原点相对固定系原点的位置是[0;0;h0]h0就是初始平台高度。换句话说初始高度不是加在P_local里而是包含在位置向量t里。如果有人在初始化时把平台铰点坐标直接写成[Rpcos(phi_p); Rpsin(phi_p); h0]后面逆解又在t里加了一次h0那平台就会跑到2*h0的高度上去整个模型的杆长全部偏大。我建议把P_local的定义和位姿向量彻底分开P_local永远只用动系坐标位姿向量里的z单独控制平台的绝对高度两层各管各的物理意义才清晰。后面等你做正解或者动力学时这个习惯能帮你省很多定位问题的功夫。3. 完整代码逐段拆解从参数结构体到六个杆长一步到位3.1 平台参数定义与铰点坐标生成统一把平台参数放进一个结构体params里这样后续传给逆解函数、可视化函数都方便也不会出现一堆裸变量满天飞的情况。主程序开头长这样clear; clc; close all; params.Rb 0.8; % 基座铰点分布圆半径m params.Rp 0.6; % 平台铰点分布圆半径m params.theta_b 20 * pi/180; % 基座每对铰点的夹角rad params.theta_p 40 * pi/180; % 平台每对铰点的夹角rad params.h0 1.2; % 零位时平台参考点高度m phi_b zeros(1, 6); phi_p zeros(1, 6); for i 0 : 2 phi_b(2*i1) i*120*pi/180 params.theta_b/2; phi_b(2*i2) i*120*pi/180 60*pi/180 - params.theta_b/2; phi_p(2*i1) i*120*pi/180 params.theta_p/2; phi_p(2*i2) i*120*pi/180 60*pi/180 - params.theta_p/2; end params.B zeros(3, 6); for i 1 : 6 params.B(:, i) [params.Rb * cos(phi_b(i)); params.Rb * sin(phi_b(i)); 0]; end params.P_local zeros(3, 6); for i 1 : 6 params.P_local(:, i) [params.Rp * cos(phi_p(i)); params.Rp * sin(phi_p(i)); 0]; endparams.B的每一列就是一个基座铰点在固定系里的坐标params.P_local的每一列就是一个平台铰点在动系里的坐标。后面所有运算都基于这两个矩阵不会再出现任何角度量这一点我觉得很重要角度布局只在初始化阶段用一次转换成坐标之后程序里就全是坐标了单位也统一成米能少很多麻烦。之所以要把参数封装成结构体而不是写成全局变量是因为后面你大概率要接着做正解、雅可比矩阵、轨迹规划这几个模块都要共用同一套平台参数。用结构体传参函数之间的依赖关系一眼就能看懂换一组平台参数也只需要改一处不会出现明明改了半径程序还在用旧值这种低级事故。3.2 逆解核心函数stewart_ik逆解函数我尽量写得短方便你看清每一步在干什么function L stewart_ik(params, pose) % stewart_ik Stewart 平台运动学逆解 % 输入: % params : 平台参数结构体, 包含 B, P_local % pose : 6x1 位姿向量 [x; y; z; roll; pitch; yaw] % xyz 单位 m, 角度单位 rad, 旋转顺序 ZYX % 输出: % L : 6x1 杆长向量, 单位 m x pose(1); y pose(2); z pose(3); roll pose(4); pitch pose(5); yaw pose(6); cx cos(roll); sx sin(roll); cy cos(pitch); sy sin(pitch); cz cos(yaw); sz sin(yaw); R [ cz*cy, cz*sy*sx - sz*cx, cz*sy*cx sz*sx; sz*cy, sz*sy*sx cz*cx, sz*sy*cx - cz*sx; -sy, cy*sx, cy*cx ]; t [x; y; z]; L zeros(6, 1); for i 1 : 6 P_global R * params.P_local(:, i) t; vec P_global - params.B(:, i); L(i) norm(vec); end end整个函数的核心就是那个for循环。循环体做的事情可以拆成三步第一步把动系里的平台铰点坐标通过旋转矩阵R和位置向量t变换成固定系里的全局坐标第二步用全局坐标减去基座铰点坐标得到从基座铰点指向平台铰点的杆长向量第三步对杆长向量取二范数得到杆长。三步对应前面说的物理过程每根腿就是基座铰点到平台铰点的一条空间直线杆长就是该直线的长度。我把姿态矩阵放在循环外面算是因为R和t对于六个铰点来说是公共量没必要在循环里重复构造。这算是习惯问题但六条腿的循环体里少做几次正余弦运算在几千上万次逆解调用时能省下不少时间。实际项目里如果要把这个函数跑在实时控制回路里后面我还会给出进一步的向量化写法这里先用最直接的循环方式讲清楚逻辑。3.3 主程序测试与输出解读写好函数之后先用两个位姿验证一下。第一个用零位姿位置取[0;0;h0]角度全取0。此时平台没转没歪理论上六根杆应该处在初始长度附近而且因为铰点布局是成对对称的六根杆长会呈现三组两两相等的规律。第二个用一个带偏移和姿态的位姿看各杆长如何跟随变化。pose_zero [0; 0; params.h0; 0; 0; 0]; L_zero stewart_ik(params, pose_zero); pose_demo [0.1; -0.05; 1.4; 5*pi/180; 8*pi/180; 10*pi/180]; L_demo stewart_ik(params, pose_demo); fprintf(零位姿杆长: ); fprintf(%.4f , L_zero); fprintf(\n带姿态偏移杆长: ); fprintf(%.4f , L_demo); fprintf(\n);跑出来的零位杆长按我这组参数应该能看到1、2号腿数值接近3、4号腿数值接近5、6号腿数值接近并且三组之间略有差异这是布局造成的正常现象。如果跑出来六根杆全部严格相等反而要回头检查一下铰点角度布局是不是写成了均匀分布如果连两两相等都不满足那就是拓扑角度或者配对关系写错了。杆长的具体数值范围也可以提前心里有个数基座和平台半径差0.2米且上下铰点存在约10度的周向角差所以水平方向投影的分量会贡献十几厘米到二十几厘米的杆长再加上1.2米的垂直高度零位杆长应该在一米二附近不会差到离谱。凡是算出来零点几米或者两米开外的基本可以断定是单位或坐标系搞错了这类问题越早发现越省时间。4. 怎么验证逆解算对了对称性检查和3D可视化4.1 零位杆长检查认不出三组两两相等就是布局配错了逆解代码的典型特点就是错了也不报错程序照样把六个数字给你打出来看着还挺像回事。所以我做验证特别较真第一关就是零位杆长。零位时姿态是单位阵平台纯平放六个铰点都只做了一次平移从理论上讲基座铰点和平台铰点的对应关系被分成三对每对的两根杆应该长度相等。按上面的布局基座铰点是(10,50)、(130,170)、(250,290)平台铰点是(20,40)、(140,160)、(260,280)所以1号腿基座10度对平台20度和2号腿基座50度对平台40度在零位时杆长应该相等同理3号和4号、5号和6号也应该分别相等。我第一次跑这个检查时发现前两对都对最后一对差了0.002米查了半天发现是280度写成了290度。这种精度级别的错误肉眼看模型根本看不出来只有靠数值对称性才能揪出来。所以我的建议是把零位杆长分成三组每组两根相等这个条件写成一个断言放到测试脚本里每次改完参数自动检查一遍。哪怕只是微调了某个铰点角度跑了断言心里就踏实了。如果项目里用了完全不同的铰点布局那就改成对应布局的对称性条件总之用结构对称性反推参数正确性这个思路是通用的。4.2 可视化函数与位姿观察数值对了不代表空间想象跟得上。我习惯再写一个可视化函数把基座、平台、六条腿画出来摆一个带姿态的位姿肉眼确认一下平台朝向和腿的伸缩方向是否合理。function visualize_platform(params, pose) x pose(1); y pose(2); z pose(3); roll pose(4); pitch pose(5); yaw pose(6); cx cos(roll); sx sin(roll); cy cos(pitch); sy sin(pitch); cz cos(yaw); sz sin(yaw); R [ cz*cy, cz*sy*sx - sz*cx, cz*sy*cx sz*sx; sz*cy, sz*sy*sx cz*cx, sz*sy*cx - cz*sx; -sy, cy*sx, cy*cx ]; t [x; y; z]; P_global R * params.P_local t; figure(Color, w); hold on; axis equal; grid on; view(45, 30); plot3(params.B(1,:), params.B(2,:), params.B(3,:), ko, ... MarkerFaceColor, k, MarkerSize, 8); plot3(P_global(1,:), P_global(2,:), P_global(3,:), ro, ... MarkerFaceColor, r, MarkerSize, 8); th linspace(0, 2*pi, 100); plot3(params.Rb*cos(th), params.Rb*sin(th), zeros(1,100), k--); plot3(params.Rp*cos(th), params.Rp*sin(th), ones(1,100)*z, r--); for i 1:6 plot3([params.B(1,i), P_global(1,i)], ... [params.B(2,i), P_global(2,i)], ... [params.B(3,i), P_global(3,i)], b-, LineWidth, 2); end fill3(P_global(1,:), P_global(2,:), P_global(3,:), ... r, FaceAlpha, 0.3, EdgeColor, none); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(sprintf(x%.2f y%.2f z%.2f | roll%.1f° pitch%.1f° yaw%.1f°, ... x, y, z, roll*180/pi, pitch*180/pi, yaw*180/pi)); end调用方式就是visualize_platform(params, pose_demo)。看三维图的时候重点关注三件事平台倾斜方向和角度是否符合你设置的roll/pitch六条腿有没有出现某根明显被拉得过长或压得过短对应杆长数值是否在合理范围内平台的姿态变换是否连续自然而不是突然翻转或扭曲。三维图加上数值量检查一起看逆解的正确性基本就锁死了。4.3 走一段轨迹杆长曲线是否平滑是另类正确性判据单点位姿验证过关之后再做一个动态轨迹测试。让平台沿一条圆形轨迹运动同时叠加一个小的偏航摆动记录六个杆长随轨迹参数的变化曲线。如果逆解是对的杆长曲线应该平滑连续六个通道的幅值关系符合机构对称性如果哪个铰点角度配错曲线通常会出现突变或者某两根之间失去对称性。t linspace(0, 2*pi, 200); r 0.15; L_traj zeros(6, 200); for k 1:200 pose_k [r*cos(t(k)); r*sin(t(k)); ... params.h0 0.05*sin(2*t(k)); ... 0; 0; 8*pi/180*sin(t(k))]; L_traj(:, k) stewart_ik(params, pose_k); end figure(Color, w); plot(t, L_traj, LineWidth, 1.5); xlabel(轨迹参数 t (rad)); ylabel(杆长 (m)); legend(腿1,腿2,腿3,腿4,腿5,腿6, Location, best); grid on;画出来的曲线里每条腿基本都围绕初始杆长附近变化幅值大约几厘米到十几厘米周期和轨迹周期一致。我曾经在这条曲线上抓到过一个特别隐蔽的问题某条腿在轨迹某一段出现尖角其余五条都很平滑最后发现是那个铰点的角度从260度写成了250度导致该腿的杆长变化方向和其他腿不完全匹配。如果不是画曲线光靠单点验证很难发现这种错。轨迹测试的另一层意义是给后面的轨迹规划打基础杆长曲线的平滑度直接反映了位姿轨迹的连续性如果这里出现毛刺到了伺服驱动环节就会被放大成抖动。5. 调试中反复踩到的五个坑以及我的处理办法5.1 欧拉角顺序不一致图像上平台拧成麻花第一个坑在2.1里提过这里再展开讲。不同领域对横滚、俯仰、偏航的旋转顺序定义并不统一有的用ZYX有的用XYZ还有的绕固定坐标轴和绕动坐标轴混着来。如果你从某个论文里抄了一个旋转矩阵但没有确认它和你的欧拉角定义是同一个顺序平台在纯roll或纯pitch时可能看起来还行一旦同时给三个姿态角平台就会像拧麻花一样歪向错误方向。我的处理办法是给自己定一个铁律凡是手写旋转矩阵必须在旁边注释明确写出R Rz(yaw) * Ry(pitch) * Rx(roll)这行字凡是调用eul2rotm必须显式传第三个参数ZYX绝不依赖默认值。一旦发现姿态不对先检查这行注释和实际矩阵是否一致能省下大量排查时间。还有个小技巧在纸上把平台绕X轴单独转30度的期望结果画出来然后用代码算一组杆长手算互相印证这个简单测试几乎能立刻暴露顺序问题。5.2 万向锁附近姿态抖振仿真能过、实物会抖ZYX欧拉角在pitch等于±90度时会遇到万向锁此时yaw和roll的旋转轴重合两个角度无法唯一描述姿态。逆解本身在万向锁附近并不会报错但杆长对姿态的灵敏度会变得极差数值上表现为姿态角的小变化引起杆长的大幅变化放到实际系统里就是控制量的抖振。如果你的平台工作空间覆盖了接近pitch正负90度的区域建议尽早换成四元数描述姿态避免在奇异附近做控制。如果暂时不想换四元数至少要在轨迹规划时对姿态角做限幅让pitch远离开90度。我在做模拟台时给姿态指令加了一个饱和度处理虽然损失了一点运动范围但换来的是杆长指令的平滑和稳定实物调试时少了很多麻烦。设计阶段也可以早点用蒙特卡洛方式扫一遍工作空间把每个网格点的pitch记下来确定奇异边界到底在哪里而不是等样机做出来再叫苦。5.3 铰点z坐标放错导致零位杆长全乱前面说的初始高度问题实际操作中非常容易犯。常见错误是两类一类是把平台铰点的动系坐标写成[Rpcos(phi_p); Rpsin(phi_p); h0]之后又在位姿向量的z里加一次h0导致平台跑到2*h0的位置另一类是完全忘了h0把位姿向量的z设成0结果平台贴在基座平面上六根杆全成了接近水平的斜线。我的建议是在P_local的注释里写明动系坐标z恒为0在位姿向量的注释里写明xyz为动系原点在固定系中的坐标零位时zh0两处各写各的职责。如果发现零位杆长明显偏短或偏长先检查这个这比对着矩阵找半天快得多。实际上这个坑很好排查零位杆长偏大很多基本都是h0被重复加了偏小很多基本都是h0被遗漏了。5.4 算完杆长就完事铰点角度范围往往被忽略运动学逆解只给出杆长但Stewart平台的每条腿两端还有球铰和万向铰它们的转角范围是有限制的。有些位形下杆长算出来完全正常但某个铰点的转角已经超过机械限位如果直接下发长度指令机构会卡死甚至损坏。这个我在仿真阶段基本不管但做实物时吃过亏有一次让平台做一个大角度俯仰杆长全都合理一个球铰却到了极限位置机构嘎嘎响。所以做实物之前建议在逆解函数后面补一个铰点角度检查。粗略的办法是计算杆长向量与平台法向的夹角或者把球铰的初始安装方向考虑进去计算平台铰点处的杆向量相对平台坐标系的姿态角超出预设范围的直接在程序里报警。这一步不需要很高的精度但能把看起来能动和真的能安全动区分开。对于仿真阶段也可以在遍历工作空间时记录每个铰点的最大转角提前画成热力图给机械设计提供输入。5.5 从MATLAB脚本到实时控制向量化与预计算如果只是做仿真分析上面这套脚本的性能完全够用。但如果你想把它移植到实时控制系统或者放在Simulink模型里做控制环的每个周期调用有几个优化点值得注意。首先是向量化把for循环改成矩阵运算可以明显提速先预计算六个平台铰点坐标整体变换然后用vecnorm一次性算出六根杆长。其次是预计算旋转矩阵和位置向量在每个周期都不同但铰点坐标、基座坐标这些在运行过程中完全不变应该全部提前算好不要在实时循环里反复生成。L vecnorm(R * params.P_local t - params.B, 2, 1);这行向量化代码和前面的for循环在数学上完全等价但一次能用满MATLAB的矩阵运算能力我在做批量轨迹规划时从它身上省了不少时间。当然移植到C或C时用循环反而可能更直观这个看目标环境来定核心逻辑都是同一个R*P_local t减B取范数。我在实际测试里体会最深的一点是逆解代码写对只是第一步真正决定项目进度的是验证习惯。每改一次参数就跑一遍零位检查每画一条轨迹就瞄一眼杆长曲线这些习惯能帮你把错误拦在最早的环节。等逆解稳了后面的正解、速度雅可比、力分析、轨迹生成才有可靠的地基。最后再分享一个自己常用的土办法找个周末把上面这段代码从零敲一遍不复制粘贴敲的过程中你会把每个矩阵元素都过一遍脑子很多我好像懂了的地方会立刻现出原形。这个办法比看十遍教程都管用。