第一次接触Stewart平台是在一台六自由度飞行模拟器上。六根电动缸推着座舱做滚转、俯仰、偏航动作当时觉得这东西比六轴机械臂还带感。后来自己用MATLAB复现它的运动学逆解才发现并联机构和串联机械臂的思考方式很不一样串联机器人是给关节角求末端位姿并联机器人是给末端位姿求杆长恰好反着来。这篇文章就写清楚逆解到底在解什么、几何模型怎么搭、代码怎么写以及新手最容易踩的坑。文末附完整可运行的MATLAB代码不依赖额外工具箱适合课程设计、毕业设计和工程初版验证。1. 先搞懂Stewart平台为什么它是六自由度并联机构的一号代表1.1 机构构成六根腿撑起一张平台的闭环结构Stewart平台的结构说穿了很简单一块不动的底座静平台、一块会动的顶板动平台中间用六根可伸缩的支腿连接。每根腿两端通常用球铰或者虎克铰连接这样腿本身只受拉压不会承受额外弯矩。六个杆长各自独立伸缩动平台就能获得六个方向上的运动能力沿X、Y、Z轴的平移以及绕三个轴的旋转也就是常说的六自由度。这种机构在工程里到处都是飞行模拟器、舰船稳定平台、卫星天线的指向机构、并联机床、汽车测试台架还有手术机器人里的精密定位环节都能看到它的身影。它受青睐的原因也很直接——多根腿共同承载刚度和负载能力比同样尺寸的串联机械臂高出一大截电机和传动件都放在底座附近运动部分惯性小再加上没有串联关节的误差累积定位精度通常更稳定。代价是工作空间小而且运动学正解非常麻烦。新手容易卡在一个概念上正解和逆解到底哪个难。串联机械臂比如常见的六轴工业机器人正解容易逆解难——已知各关节角度求末端位置就是链式乘矩阵但反过来要给末端位姿反推每个关节的角度往往有多组解推导麻烦。而并联机器人恰好相反逆解是纯几何问题六个杆长用一个向量范数就能算出来简单到可以不用任何优化库反倒是正解已知六个杆长求平台位姿要解一个强耦合的非线性方程组数值迭代都未必收敛得快。所以做并联机构控制第一件事就是把逆解吃透。1.2 逆解在控制链路里的位置在实际控制系统中逆解是上层规划与底层驱动之间的一座桥。上层轨迹规划给出来的是平台中心应该在哪、平台姿态应该是什么这类信息底层伺服驱动器需要的却是第几根腿要伸到多长。这中间做转换的就是运动学逆解。控制流程通常长这样期望位姿 → 逆解计算六根腿的目标杆长 → 目标杆长换算成伺服电机的位移指令 → 驱动器推动伸缩腿 → 达到实际杆长 → 平台运动到期望位姿。只要逆解计算够快、够准整个控制环路的性能就有了基础。我说够快是有原因的。如果逆解只是简单几何公式单次计算量很小完全可以放在10kHz甚至更高频率的控制循环里。如果逆解代码写得太绕比如用了符号计算、或者每次循环都反复分配大矩阵循环频率一高就会有明显延迟。所以学习阶段就要养成核心逆解做成纯函数、输入输出干净清晰的习惯后面做实时控制或代码生成时直接搬过去用会省掉大量移植和调试时间。2. 逆解建模从两个坐标系的几何关系推导杆长公式2.1 建立基坐标系与动坐标系逆解的第一步不是写代码而是把空间关系用坐标系刻画清楚。我们需要两个坐标系一个固定在静平台上叫基坐标系记为{B}原点通常在静平台中心Z轴竖直向上另一个固连在动平台上叫动坐标系记为{P}原点在动平台中心初始时刻和{B}平行。两块平台上各有六个铰点。静平台铰点在基坐标系里的坐标记为 B_ii1,...,6因为静平台不动这组数是常量。动平台铰点在动坐标系里的坐标记为 A_i这也是常量——它描述的是每个铰点相对动平台中心的位置。难点在于动平台会动A_i在基坐标系下的坐标需要经过旋转和平移才能得到。铰点在平台上的分布也值得较真。通常六根腿沿圆周均布但上下平台的铰点不会简单正对而是错开一定角度常用的是错开30°。这么做是为了让上下平台铰点在XY平面投影形成交错六边形避免支腿之间打架、减少奇异位形。我做仿真时习惯这么排静平台铰点角度0°、60°、120°、180°、240°、300°动平台铰点角度30°、90°、150°、210°、270°、330°角度从X轴正方向开始逆时针为正。静平台半径和动平台半径一般也不同常见的设定是静平台略大比如 R_b0.5m、R_p0.4m。半径差越大稳定性通常越好但工作空间也会收窄这里需要权衡。取一个实际例子R_b0.5m静平台第一个铰点在角度0°处它的坐标就是 [0.5; 0; 0]。若动平台 R_p0.4m、初始高度 Z00.6m动平台第一个铰点在动坐标系里是 [0.4cos30°; 0.4sin30°; 0] ≈ [0.3464; 0.2; 0]在零位姿态且平移到Z0高度后它在基坐标系下就是 [0.3464; 0.2; 0.6]。这个局部坐标 平移的习惯搞清楚后面整个公式就顺了。2.2 用旋转矩阵描述姿态位置用三维向量 p 表示这个简单。姿态用旋转矩阵 R 表示物理含义是把动坐标系里的向量变换到基坐标系。对新手来说不需要背一堆矩阵元只要记住核心式子v_base R * v_localR 的三个列向量分别是动坐标系三个轴在基坐标系下的方向余弦。它的构造方式很多可以用欧拉角、轴角、四元数。为了让代码直观可读我推荐用ZYX欧拉角也就是先绕X轴滚转(roll)再绕Y轴俯仰(pitch)最后绕Z轴偏航(yaw)。对应的旋转矩阵是R Rz(yaw) * Ry(pitch) * Rx(roll)三个基本旋转矩阵展开如下Rx(roll) [1 0 0; 0 cos(roll) -sin(roll); 0 sin(roll) cos(roll)]Ry(pitch) [cos(pitch) 0 sin(pitch); 0 1 0; -sin(pitch) 0 cos(pitch)]Rz(yaw) [cos(yaw) -sin(yaw) 0; sin(yaw) cos(yaw) 0; 0 0 1]这个顺序在代码里会直接变成矩阵乘法。不同教材可能用YZX、ZYX之类的不同约定不用慌只要选定一种并在注释里写清楚全项目保持一致就行。2.3 单腿向量推导杆长就是空间两点的距离平台处于某一时刻的位姿由位置向量 p 和旋转矩阵 R 确定。此时第i根腿连接的两个端点分别是静平台端 B_i固定和动平台端在基坐标系里的全局坐标 p R*A_i。于是第i根腿的向量就是从静平台端指向动平台端的矢量d_i p R*A_i - B_i杆长就是这根向量的模长L_i ||d_i|| sqrt(d_i * d_i)六根腿各算一次逆解就完成了。这里有个小细节算出来的是当前杆长的绝对值。如果后面要给伺服驱动器发指令往往需要的是相对初始状态的变化量也就是 ΔL_i L_i - L0_iL0_i 是零位姿时的初始杆长。这个绝对长度 vs 变化量的问题在后面坑点里会专门说现在先记住即可。3. MATLAB实现几何初始化、IK核心函数与轨迹求解3.1 参数化建模半径、铰点相位角与初始杆长写MATLAB逆解最忌讳把一堆魔法数字散落在代码各个角落。我习惯把机构参数集中放在脚本最前面这样改结构参数、换平台尺寸都很快。核心参数就是四类静平台半径、动平台半径、上下铰点角度、初始高度。先放一段初始化代码这段代码生成两组铰点坐标并计算零位姿下的初始杆长%% 参数定义 R_b 0.5; % 静平台铰点分布圆半径 (m) R_p 0.4; % 动平台铰点分布圆半径 (m) Z0 0.6; % 动平台初始高度 (m) theta_b_deg [0 60 120 180 240 300]; % 静平台铰点角度 theta_p_deg [30 90 150 210 270 330]; % 动平台铰点角度 phi_b deg2rad(theta_b_deg); phi_p deg2rad(theta_p_deg); %% 铰点坐标 B [R_b*cos(phi_b); R_b*sin(phi_b); zeros(1,6)]; % 静平台铰点基坐标系 A0 [R_p*cos(phi_p); R_p*sin(phi_p); zeros(1,6)]; % 动平台铰点动坐标系 %% 零位初始杆长 L0 zeros(1,6); for i 1:6 L0(i) norm(A0(:,i) [0;0;Z0] - B(:,i)); end这段代码里的关键是铰点角度用了度数表达但在计算坐标时用 deg2rad 转成弧度。很多新手败在角度制和弧度制混用上三角函数括号里必须传弧度这是一个容易埋雷的地方。另一个细节是 A0 是在动坐标系下的局部坐标零位姿时动平台没有旋转、只抬升了Z0所以全局坐标就是 A0(:,i) [0;0;Z0]再减去 B(:,i) 得到腿向量。3.2 核心逆解函数与旋转矩阵逆解函数我建议单独写保持输入输出纯净。输入是动平台局部铰点坐标、静平台铰点坐标、当前旋转矩阵、当前位置向量输出是六个杆长function L stewart_ik(A0, B, R, p) n size(A0, 2); L zeros(1, n); for i 1:n d p R * A0(:,i) - B(:,i); % 腿向量 L(i) norm(d); % 杆长 end end旋转矩阵函数同样独立function R rpy2R(roll, pitch, yaw) 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; % ZYX 旋转顺序 end这里我再强调一遍旋转顺序一定和你的位姿定义一致。如果你用的是先yaw再pitch再roll的ZYX固定坐标系内旋顺序那矩阵乘法顺序就是 RzRyRx。如果换一种约定比如先绕Z轴偏航再绕Y轴俯仰但代码里先乘了Rx最后算出来的杆长会完全不是你期望的而且这个问题在单一位姿下很难发现因为数字看起来好像合理。3.3 测试轨迹生成、求解循环与可视化几何有了、核心函数有了接下来让平台动起来。我习惯用一组包含滚转、俯仰、偏航和垂向升降的复合轨迹做测试这样能一次性看到六个杆长的耦合关系%% 时域轨迹 t linspace(0, 10, 300); roll 8*sin(2*pi*0.2*t) * pi/180; % 横滚角 pitch 5*sin(2*pi*0.15*t pi/4) * pi/180; % 俯仰角 yaw 3*sin(2*pi*0.1*t) * pi/180; % 偏航角 z_off 0.04*sin(2*pi*0.3*t); % 垂向位移 N length(t); L_hist zeros(N, 6); P_hist zeros(3, N); R_hist zeros(3, 3, N); %% 逐帧逆解 for k 1:N R rpy2R(roll(k), pitch(k), yaw(k)); p [0; 0; Z0 z_off(k)]; L_hist(k, :) stewart_ik(A0, B, R, p); P_hist(:, k) p; R_hist(:, :, k) R; end dL L_hist - L0; % 杆长变化量 %% 绘图 figure(Name,杆长变化量,Color,w); plot(t, dL*1000, LineWidth, 1.5); xlabel(时间 (s)); ylabel(杆长变化量 (mm)); legend(杆1,杆2,杆3,杆4,杆5,杆6, Location, best); grid on;杆长变化量单位为mm时量级通常在几十毫米以内。如果你画出几十米的变化量八成是某个角度没有正确转弧度或者半径单位搞错了。正常结果应该是六条光滑连续的正弦状曲线围绕0附近波动因为输入姿态是连续正弦轨迹杆长必然也是连续光滑的。如果想看平台动画可以再加一个绘制函数把静平台铰点、动平台铰点和六根腿实时画出来figure(Name,平台姿态动画,Color,w); hold on; axis equal; grid on; xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); view(135, 25); for k 1:5:N A P_hist(:,k) R_hist(:,:,k) * A0; cla; plot3(B(1,:), B(2,:), B(3,:), bo, MarkerSize, 6, LineWidth, 2); plot3([B(1,:) B(1,1)], [B(2,:) B(2,1)], [B(3,:) B(3,1)], b-, LineWidth, 1.5); plot3(A(1,:), A(2,:), A(3,:), ro, MarkerSize, 8, LineWidth, 2); plot3([A(1,:) A(1,1)], [A(2,:) A(2,1)], [A(3,:) A(3,1)], r-, LineWidth, 1.5); for i 1:6 plot3([B(1,i) A(1,i)], [B(2,i) A(2,i)], [B(3,i) A(3,i)], k-, LineWidth, 1.2); end plot3(P_hist(1,k), P_hist(2,k), P_hist(3,k), r^, MarkerSize, 10); title(sprintf(t%.2fs roll%.1f pitch%.1f yaw%.1f, ... t(k), rad2deg(roll(k)), rad2deg(pitch(k)), rad2deg(yaw(k)))); drawnow; end细节提醒MATLAB里如果脚本末尾有函数定义需要R2016b及以上版本。代码中我把函数写在主脚本后面即可这属于官方支持的写法运行前确认版本就行。4. 新手最常踩的坑从代码错误到机构学误区4.1 角度与弧度混用最隐蔽的一类bug所有三角函数都按弧度计算。但人脑习惯用度数这就产生了最常见的错误——写 sin(30) 以为算的是30°实际算的是30弧度。由于 MATLAB 默认不报错数字看起来还算正常结果在姿态轨迹里就会出现完全错误的频率和幅值。我的建议是所有可读性输入都用度数比如轨迹定义时写 8*sin(...) * pi/180一目了然进入数学计算后全部用弧度不再切回。如果从外部文件读到了以度为单位的姿态数据先统一做 deg2rad 转换再进入逆解函数。这条约定在全项目里保持一致能帮你省下无数排查时间。4.2 旋转矩阵顺序与欧拉角约定结果错得理所当然同样的roll、pitch、yaw用不同旋转顺序会得到完全不同的旋转矩阵。很多网上的资料写得不清不楚抄过来直接用最后平台姿态和预期根本对不上。我推荐一个固定套路ZYX顺序。含义是动平台先绕自身X轴滚转 roll再绕自身Y轴俯仰 pitch最后绕自身Z轴偏航 yaw。在矩阵乘法上统一写作 R Rz * Ry * Rx。如果你的模型恰好需要其它顺序也不是不行但一定要在代码注释里写清楚并且保证正解验证时也使用同一个约定。反正我在实际项目里吃过苦头后来所有逆解代码第一行注释就是ZYX Euler angle convention, R RzRyRx。如果不确定自己有没有写对用一个小技巧检验令 roll90°、pitch0°、yaw0°看旋转矩阵是否把动坐标系的Y轴正确映射到Z轴方向也就是 R[1 0 0; 0 0 1; 0 -1 0] 这个列向量关系。手动验证几次之后再小的约定问题都能暴露出来。4.3 铰点相位角排布与奇异位形上下平台铰点如果完全正对投影看起来是整齐的六边形甚至是三角形但这在运动学上相当不利——某些方向上会出现奇异杆长变化对位姿变化不再敏感控制失效。更常见的做法是上下平台铰点错开30°。计算铰点坐标时要注意角度0°基准在X轴正方向不是Y轴。很多人图方便从Y轴开始排最后平台姿态对不上。还有一种排布是角平分线均布比如静平台取15°、75°、135°...动平台取45°、105°、165°...效果上也起到错开作用。我给出的代码采用最简单的一组错开30°的排布新手最好先在这种布局下跑通再改其它排布验证理解。4.4 工作空间边界能算出来不等于能做出来逆解是纯几何公式理论上你给它任何位姿它都能吐出六个杆长。可实际机构里每个杆长都有行程范围平台铰点也有转角限制还有支腿之间互相干涉。所以算出来了不表示机构能达到。工程上常用的做法是离散采样扫掠工作空间在位置和姿态角范围内铺网格逐点做逆解检查每根腿的长度是否落在 [Lmin, Lmax] 范围里以及各腿与平台、腿与腿之间有没有交叉。这样在工作空间边缘附近规划轨迹时能提前发现潜在死角。对于学习阶段至少先保证你给的roll、pitch不超过±10°z_off不超过0.05m这个范围内上述参数基本是安全的。想做大角度姿态就要先做一次工作空间扫描别凭感觉给一个大角度轨迹然后发现动画里平台转得像脱缰野马。4.5 杆长输出的绝对长度与变化量控制指令的差别逆解函数返回的是杆长绝对值。伺服系统控制对象却是伸长量。如果你把绝对杆长当成位移指令发给驱动器系统在初始状态就会收到一个巨大的偏移量轻则上电抖动重则撞限位。正确做法是初始零位算一次 L0控制时输出 L - L0。这个差值才是驱动器真正需要的目标位移增量。我在代码里特意把 L0 和 dL 分开存放就是为了防止混用。如果你在硬件上调试这个区分会直接决定电机到底是停在原位还是冲出行程。常见现象可能原因快速检查杆长曲线突变或NaN角度/弧度混用、铰点排布有误检查 deg2rad 与铰点角度表零位时杆长变化量不为0初始杆长与逆解公式不一致用零位姿代入两个算法核对平台动画姿态错乱旋转顺序/欧拉角约定不一致检查 R RzRyRx 并做单轴验证杆长量级异常半径单位、初始高度单位错误统一长度单位并检查数量级5. 验证逆解结果三个无需额外工具箱的检查方法5.1 零位自检最快暴露系统性错误在代码里刚初始化完 L0 后立刻做一次零位姿逆解。零位姿就是六个角度全0、垂向偏移也为0。如果一切正确这一帧算出来的杆长应该和 L0 完全一致dL 全为0。如果零位时 dL 不是0说明初始杆长计算与逆解公式之间有不一致的地方。最常见的错误是初始杆长里用了错误的铰点排布或者旋转矩阵函数实际返回的不是单位阵。检查旋转矩阵 R 在零角度时是否为 [1 0 0; 0 1 0; 0 0 1]这是最快的定位手段。5.2 几何距离反查看懂杆长公式背后的几何意义逆解公式本质上就是求两点距离那就可以直接从几何上反查验证。随便选一个位姿用 p 和 R 算出当前时刻六个动平台铰点的全局坐标再和静平台铰点坐标做距离计算。这个结果必须和逆解函数返回的 L 完全一致。你可以把这段验证代码临时加在主脚本里k 1; % 检查第一帧 R R_hist(:,:,k); p P_hist(:,k); A p R * A0; L_check sqrt(sum((A - B).^2, 1)); max(abs(L_check - L_hist(k,:))) % 应接近 0如果这个误差不是零问题几乎一定出在坐标变换这一段比如向量方向写反、p 忘记加进去、或者旋转矩阵没作用到 A0 上。这个方法不依赖任何工具箱纯几何原理是我调试逆解代码时最常用的第一道关卡。5.3 正解交叉验证更高标准的闭环检查几何距离反查验证的是代码自身内部一致性但还不足以证明逆解模型对着真实机构是对的。更强有力的验证是把逆解结果反过来做一次正解用逆解输出的六个杆长作为输入反求平台位姿再和最初给定的位姿对比。正解没有解析式通常用数值迭代求解。如果你有 Optimization Toolbox可以直接用 fsolvefunction pose stewart_fk_fsolve(L_desired, B, A0, Z0) x0 [0; 0; Z0; 0; 0; 0]; % 初始猜测位姿 options optimoptions(fsolve, ... Display, off, Algorithm, levenberg-marquardt, ... FunctionTolerance, 1e-12, StepTolerance, 1e-12); pose fsolve((x) ik_residual(x, L_desired, B, A0), x0, options); end function res ik_residual(x, L_desired, B, A0) p x(1:3); R rpy2R(x(4), x(5), x(6)); L stewart_ik(A0, B, R, p); res L - L_desired; end取轨迹第一帧的杆长 L_hist(1,:) 作为 L_desired调用正解反解出的位姿应该非常接近初始给的 roll(1)、pitch(1)、yaw(1) 和 p(1)。通常位置误差在微米量级、姿态误差在微弧度量级和 fsolve 的容差设置有关。如果正解结果偏离很大说明逆解模型与机构几何不一致需要回到铰点坐标、旋转矩阵和杆长公式逐项排查。5.4 杆长曲线平滑性检查连续轨迹的隐形验证最后一个验证藏在绘图里。输入的轨迹是连续光滑的逆解结果也必须是连续光滑的。如果某根腿的杆长曲线在某处出现尖角、跳变或者断点往往意味着轨迹经过了奇异位形或者工作空间边界也可能是某些计算在跨边界时出现了符号翻转。这个检查不需要代码看曲线即可。我习惯是先看曲线形状再看数值量级最后才相信结果。曲线平滑是机构运动学连续性的直接体现任何一处突变都值得停下来追根究底而不是直接拿去发论文或者做控制。如果接下来要做实际项目我的个人建议是把这套逆解函数直接封装成 Simulink 里的 MATLAB Function 模块或者用 MATLAB Coder 生成C代码放进实时控制器里跑。平台标定好后再用工作空间扫描脚本生成一组安全轨迹把逆解结果和实际反馈杆长比对系统误差基本就心里有数了。我自己做完第一版逆解后最想告诉新手的一句话是并联机构的逆解公式不难难的是让每个坐标系、每个角度约定都在同一个频道上。先把零位自检和几何反查写进主程序后续所有调试都会轻松得多。
