并联机构Stewart平台运动学逆解：从几何原理到MATLAB仿真

六根可以伸缩的支腿上下两个平台调节每根支腿的长度上平台就能在空间里做出俯仰、横滚、偏航以及三维平移——这就是典型的Stewart平台也是并联机构里最常见的六自由度构型。做运动模拟器、并联机床、隔振平台或者机器人腿部结构的人都会碰到同一个问题给定了上平台想要的位姿六条腿分别该伸长到多少。这个“由位姿求杆长”的过程就是运动学逆解。今天这篇实战笔记就围绕这件事说清楚从几何直觉讲到MATLAB可运行代码最后附上完整的可视化脚本和三类常见的坑。整个实现不依赖Robotics Toolbox装了MATLAB就能跑适合刚接触并联机构、想快速验证算法的读者。1. Stewart平台逆解先把几何关系想清楚1.1 逆解到底在解什么六个长度从哪来要写逆解先得搞清楚Stewart平台的结构。它本质上就是两个刚体平台中间用六根独立的伸缩杆连接每根杆上下两端分别通过球铰或万向铰与平台相连。静平台固定不动动平台可以自由运动但动平台的空间位姿不是直接控制的而是通过改变六根杆的长度间接实现的。这里有一个关键的几何事实每条支腿连接的都是动平台上的一个固定点、静平台上的另一个固定点。只要这两个固定点在空间里的位置确定了这条腿的长度就唯一确定了而且可以直接用两点间距离公式算出来。所以运动学逆解要算的事情其实很直接给定动平台的位姿先算出动平台上六个铰点的空间坐标再和静平台上的六个铰点坐标一一配对求出六段空间距离这六段距离就是六条支腿的目标长度。这套逻辑完全由几何约束主导和杆是液压驱动、电机丝杠还是气动完全无关。不管末端执行器怎么装不管平台载重多大逆解算出来的都是纯几何结果。理解这一点很重要后续做控制、做动力学分析都是在这个几何关系上叠加物理规律的。1.2 建模前的几何准备让铰点的位置“有据可查”写代码前要把坐标系和铰点分布定义清楚这部分做得越规矩后面调式越省心。我习惯用两个坐标系静平台坐标系和动平台坐标系。静平台坐标系的原点放在静平台中心轴垂直于静平台底面向上动平台坐标系的原点放在动平台中心初始状态时它的三个轴方向和静平台坐标系平行。动平台的位姿就用静平台坐标系下原点的位置向量和动平台坐标系相对静平台坐标系的旋转矩阵来描述。铰点分布是另一个需要明确定义的环节。六个铰点通常均匀分布在一个圆上但动静平台的铰点并不是简单相对着安装的一般会错开一个角度避免六根杆在零位时互相干涉。我在这套代码里采用的约定是静平台六个铰点分布在30°、90°、150°、210°、270°、330°方向上动平台六个铰点分布在0°、60°、120°、180°、240°、300°方向上。这样每一对上下铰点之间正好错开30°是一种很常见的零位对称布局。平台半径和初始高度也是基本参数静平台半径取动平台半径取初始高度取。这些参数不用追求真实够做算法验证就行。后面想换成实际机构参数只需要替换这几个常量。1.3 逆解公式一个向量减去另一个向量的模几何关系清楚以后公式就不复杂了。假设动平台相对于静平台坐标系的位置向量是动平台原点在静平台坐标系中就是那个位置向量动平台坐标系绕三个轴的旋转由旋转矩阵表达。动平台第个铰点在动平台坐标系里的坐标是那么它在静平台坐标系里的坐标就可以写成这个式子把刚体运动过程拆成了两部分先绕自身坐标系旋转再把旋转后的结果平移到静平台坐标系下。静平台铰点坐标本来就是静平台坐标系下的已知量所以第根支腿的向量直接就是支腿长度就是向量的模六根支腿全部套这个公式逆解就完成了。这个式子看起来简单但它是整个并联机构运动学分析里最稳定、最不容易出错的部分。后面写代码、做验证本质都在反复用这个式子。2. MATLAB代码实现从零开始写逆解函数2.1 设计思路把“逆解”封装成一个可复用的函数写MATLAB代码的时候我习惯把逆解过程封装成一个函数输入是平台几何参数和动平台位姿输出是六根支腿的向量和长度。函数化有三个好处零位验证、轨迹规划、工作空间扫描都能直接调用同一套逻辑代码结构清晰不会在脚本里绕来绕去后面如果想把逆解做成其他语言的库直接翻译这个函数就行。这个函数不依赖Robotics Toolbox旋转矩阵我选择自己构造。读者电脑上只要有基础MATLAB环境就能直接跑。如果你装了Robotics Toolbox可以用它的函数验证结果但整套代码运行不需要它。2.2 主函数逆解计算函数直接给出函数代码function [L, l] stewart_inverse(r_b, r_a, t, R) % STEWART_INVERSE 计算Stewart平台六根支腿长度 % 输入: % r_b - 静平台铰点分布半径 % r_a - 动平台铰点分布半径 % t - 动平台原点在静平台坐标系中的位置 [x; y; z] % R - 动平台相对静平台坐标系的旋转矩阵3x3 % 输出: % L - 六根支腿向量3x6每列对应一条支腿 % l - 六根支腿长度1x6 % 铰点分布角度 theta_b (0:5) * pi/3 pi/6; % 静平台: 30,90,...,330度 theta_a (0:5) * pi/3; % 动平台: 0,60,...,300度 % 生成静平台铰点坐标和动平台自身坐标系下的铰点坐标 B zeros(3,6); A_local zeros(3,6); for i 1:6 B(:,i) [r_b*cos(theta_b(i)); r_b*sin(theta_b(i)); 0]; A_local(:,i) [r_a*cos(theta_a(i)); r_a*sin(theta_a(i)); 0]; end % 对每一根支腿计算向量和长度 L zeros(3,6); l zeros(1,6); for i 1:6 A_global t R * A_local(:,i); % 动平台铰点转到静平台坐标系 L(:,i) A_global - B(:,i); % 支腿向量 l(i) norm(L(:,i)); % 支腿长度 end end这段代码的核心就两个循环。第一个循环生成铰点坐标把角度分布转成实际的三维坐标第二个循环逐个求支腿向量和长度。注意这里生成铰点的过程用到了MATLAB的向量化写法直接一行就能生成六个角度配合循环把每个铰点坐标填进矩阵清晰且不容易出错。在平台半径和初始高度下零位时六条腿的长度可以事先手算出来做验证。静平台半径是动平台半径是初始高度是上下铰点水平投影错开30°。用余弦定理可以算出支腿水平投影长度约为然后总长度就是初始高度平方加上水平投影平方再开根号。我实测计算结果是六条腿长度都约为因为这个特殊布局下每一对铰点的水平间距恰好相等。你跑完代码之后可以先看看这个数值对不对。2.3 辅助函数欧拉角转旋转矩阵逆解函数需要旋转矩阵作为输入但实际工程中用户一般习惯直接给“俯仰多少度、横滚多少度、偏航多少度”也就是欧拉角。所以还需要一个将欧拉角转成旋转矩阵的辅助函数。function R euler_to_rotm(roll, pitch, yaw) % EULER_TO_ROTM 将ZYX欧拉角转为旋转矩阵角度制输入 % 约定: R Rz(yaw) * Ry(pitch) * Rx(roll) % 输入: roll - 横滚角, 单位度 % pitch - 俯仰角, 单位度 % yaw - 偏航角, 单位度 % 输出: R - 3x3 旋转矩阵 roll deg2rad(roll); pitch deg2rad(pitch); yaw deg2rad(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; end这里采用是航空领域常见的yaw-pitch-roll顺序也就是先绕固定坐标系的x轴滚转再绕y轴俯仰最后绕z轴偏航。这个约定和Robotics Toolbox里函数的ZYX顺序是一致的。写代码的时候如果忽略顺序问题后续会出现平台姿态完全反掉的诡异现象我在常见问题部分会详细讲。2.4 零位验证主脚本与参数初始化逆解函数写出来后第一步不是急着做复杂运动而是先做零位验证。把动平台放在初始位置姿态为“不旋转”也就是单位旋转矩阵算出六条腿的长度跟自己手算的数值比对。这一步能过滤掉很多低级错误。%% Stewart平台运动学逆解 - 零位验证 clc; clear; close all; % 平台参数 r_b 0.5; % 静平台半径 r_a 0.3; % 动平台半径 h0 0.8; % 初始高度 % 零位位姿 t0 [0; 0; h0]; R0 eye(3); % 逆解 [L0, l0] stewart_inverse(r_b, r_a, t0, R0); % 输出结果 disp(零位时六根支腿向量:); disp(L0); disp(零位时六根支腿长度:); disp(l0);运行这段脚本如果看到六个长度都在附近而且每根支腿向量的方向都是从静平台铰点指向动平台铰点那么逆解函数的基本逻辑就通过了。我建议你在零位验证通过之后再手动改一改位姿参数比如只抬高平台、只横滚一定的角度看看输出结果是否符合直觉这比直接跑大程序更容易发现隐藏问题。3. 可视化与轨迹仿真让平台动起来3.1 把平台画出来三维可视化函数怎么写验证过逆解结果还不够平台在实际空间里到底是什么姿态单靠打印数字很难建立直观感觉。需要用MATLAB把平台和支腿画出来。function draw_stewart(r_b, r_a, t, R) % DRAW_STEWART 绘制Stewart平台当前位形 % 输入参数与stewart_inverse一致 theta_b (0:5) * pi/3 pi/6; theta_a (0:5) * pi/3; B [r_b*cos(theta_b); r_b*sin(theta_b); zeros(1,6)]; A_local [r_a*cos(theta_a); r_a*sin(theta_a); zeros(1,6)]; A t R * A_local; % 绘制动平台和静平台 patch(XData, B(1,:), YData, B(2,:), ZData, B(3,:), ... FaceColor, [0.7 0.7 0.7], EdgeColor, k, FaceAlpha, 0.5); hold on; patch(XData, A(1,:), YData, A(2,:), ZData, A(3,:), ... FaceColor, [0.2 0.5 0.8], EdgeColor, k, FaceAlpha, 0.7); % 绘制六根支腿 for i 1:6 plot3([B(1,i) A(1,i)], [B(2,i) A(2,i)], [B(3,i) A(3,i)], ... r-, LineWidth, 2); end axis equal; grid on; view(3); xlabel(X); ylabel(Y); zlabel(Z); title(Stewart平台三维显示); hold off; end绘制平台面用了函数上平台用浅蓝色、下平台用灰色半透明。有一点要注意六个铰点的顺序不能乱如果顺序错了画出来的多边形会是一条折线。我这里按角度从小到大排列画出来是一个规则的六边形。支腿画成红色粗线这样能清楚看到每根杆的空间方向。3.2 加一段正弦轨迹平台动起来杆长曲线一并观察有了可视化函数可以做一个简单的轨迹仿真。我常用的方式是让动平台做正弦形式的俯仰和偏航组合运动同时让平台在水平面上做小幅圆弧轨迹。这样六条腿的长度会持续变化也方便观察不同自由度之间的耦合。%% 轨迹仿真: 俯仰 偏航 水平位置变化 dt 0.01; t_final 4; time 0:dt:t_final; n length(time); % 轨迹参数 pitch_amp 12; % 俯仰幅度, 度 yaw_amp 8; % 偏航幅度, 度 xy_amp 0.05; % 水平移动幅度, m z_amp 0.03; % 垂直起伏幅度, m % 预先分配变量 L_traj zeros(6, n); pos_traj zeros(3, n); for k 1:n t_cur time(k); pitch pitch_amp * sin(2*pi*0.4*t_cur); yaw yaw_amp * cos(2*pi*0.25*t_cur); roll 3 * sin(2*pi*0.3*t_cur); R_cur euler_to_rotm(roll, pitch, yaw); t_pos [xy_amp*sin(2*pi*0.2*t_cur); xy_amp*cos(2*pi*0.2*t_cur); h0 z_amp*sin(2*pi*0.5*t_cur)]; [~, l_cur] stewart_inverse(r_b, r_a, t_pos, R_cur); L_traj(:,k) l_cur; pos_traj(:,k) t_pos; end % 绘制杆长曲线 figure; plot(time, L_traj, LineWidth, 1.2); xlabel(时间 (s)); ylabel(支腿长度 (m)); title(六根支腿长度随时间变化); legend(L1,L2,L3,L4,L5,L6,Location,best); grid on;跑完这段代码会看到六条曲线都在零位杆长附近做周期波动不同杆之间有一个相位差这是因为平台在做复合姿态运动时每根杆经历的空间位置变化不完全同步。这个现象是正常的如果六根杆都完全同步那说明姿态部分没有生效需要检查旋转矩阵是不是乘错了地方。3.3 做动画把平台位形按帧显示出来如果只想看杆长曲线会不够直观我更推荐加一段动画让平台在三维空间里实时运动。动画的核心是循环里不断更新图形对象的位置属性而不是每次都重新创建整个图。%% 动画: 显示平台运动过程 figure; for k 1:10:n % 每隔10帧显示一次控制速度 t_cur time(k); pitch pitch_amp * sin(2*pi*0.4*t_cur); yaw yaw_amp * cos(2*pi*0.25*t_cur); roll 3 * sin(2*pi*0.3*t_cur); R_cur euler_to_rotm(roll, pitch, yaw); t_pos [xy_amp*sin(2*pi*0.2*t_cur); xy_amp*cos(2*pi*0.2*t_cur); h0 z_amp*sin(2*pi*0.5*t_cur)]; clf; draw_stewart(r_b, r_a, t_pos, R_cur); drawnow; end这段动画代码效率不是最高的因为每次循环都重新绘制全部图形。但作为学习和验证用途十几帧每秒的速度完全够用胜在代码直观。如果想让动画更流畅可以改用对象句柄加更新的方式不过这会让代码复杂不少。我先保留这种简单写法等理解原理后再考虑优化。3.4 怎么判断仿真结果合不合理仿真跑完后有几个判断结果是否合理的经验第一个零位时杆长必须是初始杆长任何偏离都说明位姿初始化有问题。第二个做单一自由度运动时杆长变化一般具有对称性比如纯俯仰运动时前三根和后三根的杆长变化方向相反且幅度接近。第三个杆长变化量的数量级要和运动幅度匹配。以这个平台的参数为例做12°俯仰运动时杆长变化量大致在到之间如果算出来杆长突然变成负值或者变化超过倍基本可以断定某个环节错了。我一开始做轨迹仿真时曾经把欧拉角顺序搞反结果平台动了十几度杆长却几乎不变。当时的表象就是杆长曲线虽然平滑但起伏幅度小得可疑。后来对照手算数据才定位到旋转矩阵的顺序写错了。所以不要只看曲线平不平滑还要看变化幅度是否在合理范围内。4. 工作空间分析与行程约束评估4.1 为什么要做工作空间扫描Stewart平台是并联机构工作空间比串联机械臂小得多而且形态不规则。不是平台上任意位姿都能到达有些位姿可能超出某条支腿的伸缩极限有些位姿则可能让支腿碰到奇异位置。在规划运动轨迹之前先扫描一遍可达空间能避免后续控制阶段出现“逆解算出来的杆长超出实际执行器行程”的尴尬问题。工作空间扫描的基本思路就是在某个位置范围内密集取点对每个点调用逆解函数判断所有杆长是否在允许的上下限内。如果都在范围内就认为这个点可达。这个方法计算量比较大但胜在直观、可靠。4.2 用栅格扫描法看平台能走到多大范围这里以一个零姿态下的水平面扫描为例固定平台高度在平面内逐点调用逆解统计所有杆长是否满足行程约束。%% 工作空间扫描: 零姿态下x-y平面可达范围 x_range -0.15:0.01:0.15; y_range -0.15:0.01:0.15; reachable zeros(length(x_range), length(y_range)); l_min 0.65; % 支腿最小长度 l_max 1.05; % 支腿最大长度 for i 1:length(x_range) for j 1:length(y_range) t_test [x_range(i); y_range(j); h0]; [~, l_test] stewart_inverse(r_b, r_a, t_test, eye(3)); if all(l_test l_min) all(l_test l_max) reachable(i,j) 1; end end end figure; imagesc(x_range, y_range, reachable); axis xy; axis equal; colorbar; xlabel(x (m)); ylabel(y (m)); title(零姿态下平台可达工作空间 (x-y平面));运行这段代码你会看到一个近似圆形的可达区域边界处因为杆长极限而参差不齐。这就是这个平台在这个高度下能够到达的水平范围。不同初始高度下这个圆的大小会变化平台越高各级向可移动的范围越小因为支腿已经接近伸长极限了。想扫描三维工作空间只需要再加一层高度循环但计算时间会明显增加建议先把步长放宽再跑。4.3 根据扫描结果调整机构和轨迹扫描结果不只是用来看的它可以直接反过来指导参数设计。如果发现平台横移范围不够可以考虑增大动平台和静平台的半径差、加大初始高度或者选用更长的支腿行程。在规划运动轨迹时我也会先在工作空间里预检一遍轨迹点把不可达的点提前标出来。如果某个目标位姿经过逆解算出的杆长超出执行范围就需要调整轨迹规划层的参数。这个“先逆解、后判断、再规划”的思路在工程实践中非常重要。5. 常见问题与排查心得5.1 问题速查表写逆解代码真正花时间的地方往往不是算法本身而是一些看起来“不该错”的小地方。这里整理了一份问题速查表都是实际操作里容易踩的坑现象可能原因处理方法计算结果整体偏离手算值初始高度或位姿向量设置错误检查位置向量第三分量是否为初始高度某根杆长度异常大或出现负值铰点序号没对上上下铰点配对错误打印铰点坐标逐对核对姿态变化时杆长曲线基本不动旋转矩阵顺序错误或欧拉角转换错误用已知姿态手动验证旋转矩阵平台动画显示翻转、扭曲绘制平台面时铰点顺序不对按角度从小到大排列铰点中文注释乱码文件编码不统一预设项中将编码改为UTF-8重新打开扫描工作空间运行很慢双层循环点数太多步长放宽到或减少点数5.2 我踩过的两个坑欧拉角约定和铰点顺序第一个坑是欧拉角约定。我有一版代码用了和完全不同的旋转矩阵构建方式结果同样的姿态输入算出来的杆长差别很大。当时我一度以为逆解公式写错了排查了很久才发现是旋转矩阵内部的顺序问题。现在我的习惯是所有代码里都统一用ZYX顺序也就是固定顺序并且每次写完旋转矩阵都拿单位矩阵和90度特殊角实测一遍。第二个坑是铰点顺序。动平台和静平台的铰点向量必须按同一套顺序排列也就是说第一个动平台铰点对应第一条支腿再对应第一个静平台铰点。如果写循环的时候把动平台角度从0开始、静平台角度也从0开始上下铰点完全正对得到的构型和真实Stewart平台就不同了。这个问题在你只看杆长数字的时候很难发现因为数字一样很平滑但一旦做动画平台就会呈现一种奇怪的自旋状态。5.3 逆解结果正确性的三个自检方法检查逆解结果是否正确我总结了三个方法第一个迭代公式自检。找一个简单位姿比如只抬高平台即此时所有杆长应该同时变长且增量近似相同。如果六根杆的变化不同步说明几何参数或铰点坐标有问题。第二个逆解和正解互检。先用正解程序从杆长算回位姿再用逆解从位姿算回杆长来回算几轮验证是否收敛到同一组数值。这个互检思路推荐大家都做一遍能发现很多隐蔽问题。第三个微小扰动测试。给位姿加一个很小的扰动比如杆长应该会产生一个很小且连续的变化。如果杆长跳变或者突变往往说明存在分支切换或者数值不稳定需要检查旋转矩阵的构造。6. 从逆解到正解下一步可以怎么走6.1 平台正解为什么难以及一种快速实现思路逆解的公式是显式表达式没有任何迭代所以又快又稳。但正解是反过来的问题给六根杆长求动平台的位姿。这是个非线性的多元方程求解问题没有简单的显式解通常需要用牛顿迭代或者数值优化方法。一种比较直接的做法是构造误差函数预设一个位姿用逆解算出杆长和目标杆长比较得到残差然后用最小二乘去迭代调整位姿直到残差足够小。这本质上就是把正解问题变成了一个优化问题。虽然计算速度不如逆解但在离线标定、仿真验证场景下完全够用。作为初学者不建议一上来就研究正解的闭式解。先跑通逆解再做正解在正解里调用逆解作为内层函数这种“正解包逆解”的思路更符合工程直觉也更容易调试。6.2 从仿真到实物零位标定与杆长补偿仿真里所有铰点坐标都是理想值但实际平台安装时会有加工误差和装配误差油缸或者电动缸也有零位偏移。直接拿仿真逆解结果去控制实物平台大概率不会停在预期位置。我的建议是实物联调前先做一次零位标定。让平台回到机械零位记录六根杆的实际长度这个值会和仿真零位长度有偏差。把这些偏差作为杆长补偿量写入控制程序对每条支腿单独补偿。如果实际平台还有铰点位置偏差就需要做更完整的运动学标定但那已经超出逆解算法的范围了。控制频率上也要注意。如果做实时控制逆解函数必须保证能在一个控制周期内算完。这套代码直接在普通电脑上跑单次逆解时间在微秒到毫秒级满足一般控制需求。但建议控制程序里不要每次都重新生成铰点坐标矩阵可以把静平台铰点坐标提前算好存成全局变量减少重复计算。6.3 给后来者的一句话建议我在做并联机构算法的时候最大的体会是所有复杂的控制算法都建立在对一个简单几何公式的深刻理解上。逆解公式虽然短但它是整个平台的“底层逻辑”。把坐标系约定、旋转矩阵、点坐标变换这三样东西练熟了后面学样条轨迹规划、动力学分析、力控制都会顺很多。如果要把这套代码继续扩展可以从这几个方向入手加入动平台姿态矩阵外参的在线输入接口、把手动输入位姿改成读取轨迹文件、把逆解模块移植到Simulink里做硬件在环仿真。顺着这个思路做下去你会发现平台从“会算”到“能动”还有很长但很有意思的路要走。