简介:《基于MATLAB的Stewart并联平台运动学逆解与动力学仿真研究》面向机器人控制、并联机构方向的科研人员与技术爱好者,围绕六自由度平台从理论推导到仿真验证的完整链路展开,适合具备一定MATLAB基础、希望打通逆解建模与动力学仿真环节的读者。压缩包共1个PDF文件,约116KB,为集中呈现技术要点的研究文档,便于对照代码阅读与随查随用。内容从坐标变换与欧拉角转旋转矩阵入手,给出支腿长度向量化计算、雅可比矩阵求解与数值验证思路,继而以牛顿—欧拉法建立动力学方程,并结合Simscape Multibody搭建三维仿真模型;对实时绘制平台运动的可视化代码、drawnow limitrate刷新优化、刚性系统选用ode15s求解器等工程细节亦有说明。文中还专门讨论奇异位形检测、轨迹规划不合理等常见问题的排查与改进方案,读者可据此复现螺旋上升等典型轨迹算例,减少调试弯路。目前已有192人学习下载。
1. 六条腿的反解其实不难:这套 MATLAB 工程到底能解决什么
把上平台挪到[0.1, -0.05, 0.3]、再横滚 5 度,六条电缸各该伸多长?这个问题的答案,就是 Stewart 并联平台运动学逆解的全部工作量,也是这套 MATLAB 工程的第一块地基。逆解把六自由度位姿翻译成六根腿长,动力学仿真接着回答“电机出力够不够、会不会顶到行程”,三维可视化负责让你在拖参数的时候不至于对着一堆数字发呆。它适合已经能写 MATLAB 脚本、但没真正拆过并联机构的人:串联机械臂那套 DH 参数在这儿完全用不上,六条腿互相耦合,动其中一条,其余五条全得跟着改。说句反直觉的,并联平台的逆解比正解简单得多——不需要解方程,只是坐标变换加求模长;真正咬人的是后面的雅可比、奇异位形和动力学闭环。
2. 工程骨架:目录分层、铰点参数表与位姿向量约定
拿到一堆代码片段直接开跑,大概率会在第三个函数报错。并联机构的坑有一半不在数学上,在约定上:上平台的点到底存成 3×6 还是 6×3、欧拉角是弧度还是角度、位姿向量的第 4 位是 roll 还是 yaw。这些约定只要有一处对不上,腿长算出来照样是六个正数,但平台就是歪的。所以在写逆解之前,先把工程骨架和数据结构定死。
2.1 目录分层:数学、模型和画图拆成三块
我一般按“算的、仿的、看的”把目录分成三层,配置文件放最外面当唯一数据源:
stewart_sim/ ├── config/ │ └── platform_params.m % 几何、质量、惯量、行程,全部常量的唯一来源 ├── kinematics/ │ ├── inverse_kinematics.m % 位姿 -> 六条腿长 │ ├── compute_jacobian.m % 数值微分法求雅可比 │ └── check_singularity.m % 条件数判据 ├── dynamics/ │ ├── dynamics_solver.m % 牛顿-欧拉逆动力学 │ └── friction_term.m % 库伦 + 粘滞摩擦 ├── sim/ │ ├── stewart_sfun.m % S-Function 状态方程 │ ├── stewart_model.slx % Simulink 闭环 │ └── stewart_assembly.xml % Simscape 导入的装配体 ├── viz/ │ ├── init_scene.m % 句柄只建一次 │ └── update_scene.m % 单帧只改数据 └── run_demo.m % 入口脚本这样分的好处很直接:Simulink 模型里不允许出现任何魔数,所有半径、质量都从platform_params.m取;改机构尺寸只动一个文件,逆解、动力学、可视化自动跟着变。血泪经验是,早期我把上平台半径同时写进了逆解函数和 slx 里的模块参数,调了一次尺寸之后两边差了 3 cm,腿长曲线看着正常,动画里平台却是斜的,查了整整一个下午。
入口脚本保持极薄,只做三件事:加载参数、跑一条轨迹、把结果画出来。别把逻辑写进run_demo.m,否则以后想批量扫参数,就得把整个脚本复制一遍。
2.2 铰点参数表:半径、相位与坐标生成
六个铰点在上下平台上的分布,决定了这个机构的工作空间长什么样。常用参数我列成一张表,值可以改,但含义别改:
| 参数 | 变量名 | 典型取值 | 作用 |
|---|---|---|---|
| 下平台铰点半径 | R_base_m | 0.50 m | 决定底座尺寸与支腿摆角 |
| 上平台铰点半径 | R_top_m | 0.30 m | 越小越灵活,但力臂变差 |
| 上平台相位偏置 | top_twist_rad | 30° | 让上下铰点错开,避免零位干涉 |
| 零位高度 | h0_m | 0.30 m | 逆解的初始工作点 |
| 支腿行程 | L_min/L_max | 0.35 / 0.65 m | 逆解越界报警的硬阈值 |
| 上平台质量 | m_platform_kg | 8 kg | 动力学重力项 |
坐标生成写成函数,别在脚本里手敲六行数字:
function [base_pts, top_pts] = hinge_points(cfg) % 返回 3x6,每一列是一个铰点在自身坐标系下的坐标,单位 m th = (0:5)' * pi/3 + cfg.phase_rad; % 每 60 度一个铰点 base_pts = cfg.R_base_m * [cos(th), sin(th), zeros(6,1)]'; top_pts = cfg.R_top_m * [cos(th + cfg.top_twist_rad), ... sin(th + cfg.top_twist_rad), zeros(6,1)]'; end这里是第一个高频翻车点:linspace(0, 2*pi, 7)会生成 7 个点,首尾重合,取前 6 个虽然能跑,但角度间隔是 72 度而不是 60 度,机构看着像个歪六边形。要么用(0:5)*pi/3,要么linspace(0, 2*pi, 7)之后明确取1:6并知道自己在干什么。返回 3×6 而不是 6×3,是为了后面R * top_pts能直接做矩阵乘,省掉一次转置,也避免vecnorm的维度参数选错。
2.3 位姿向量:一个六元组定死顺序和单位
全工程只认一种位姿表示:pose = [x y z roll pitch yaw],平移单位米,姿态单位弧度,姿态按内旋 ZYX 解释。写成两个互逆的小函数,谁也别绕过它们:
function T = pose2T(pose) % pose: 1x6,[x y z roll pitch yaw],单位 m / rad R = eul2rotm(pose(4:6), 'ZYX'); % 顺序必须和轨迹生成端完全一致 T = [R, pose(1:3)'; 0 0 0 1]; % 4x4 齐次变换 end function pose = T2pose(T) R = T(1:3,1:3); eul = rotm2eul(R, 'ZYX'); % 返回顺序是 [yaw pitch roll] pose = [T(1:3,4)', eul(3), eul(2), eul(1)]; end注意eul2rotm吃的是行向量,pose(4:6)正好是行向量;如果上游传进来的是列向量,MATLAB 会静默按列解释或者直接报维度错。rotm2eul的返回顺序和eul2rotm的输入顺序并不对称,它是[yaw pitch roll],所以要手动倒序,这一步我见过不止一个人写错,结果做正逆解闭环验证时误差永远收不敛。
3. 运动学逆解:从欧拉角到六条腿长度
约定定完,逆解本身其实只有三行有效代码。但要让这三行在整条链路里都成立,还得处理旋转顺序、矩阵维度、行程越界和奇异位形四件事。这一章按“旋转 → 腿长 → 雅可比 → 可视化快检”的顺序推一遍,每步都能单独跑起来验证。
3.1 旋转顺序:整条链路只认 ZYX
eul2rotm的默认顺序是'ZYZ',这是给欧拉角经典定义用的,不是给机器人姿态用的。同样的三个角度,换成不同顺序,旋转矩阵能差出一个可观的角度:
pose = [0.1, -0.05, 0.35, deg2rad(5), deg2rad(3), deg2rad(-2)]; R_zyx = eul2rotm(pose(4:6), 'ZYX'); R_zyz = eul2rotm(pose(4:6), 'ZYZ'); fprintf('两种顺序的矩阵差: %.4f\n', norm(R_zyx - R_zyz, 'fro'));5 度、3 度、-2 度这么大的小角度,两种顺序的差在腿长上就能到毫米级;实机上毫米级就是撞限位和密封圈磨损。所以工程里我强制所有地方写'ZYX',包括可视化、轨迹生成、Simulink 里的姿态输入口。ZYX 对应的是“先绕 Z 转 yaw、再绕 Y 转 pitch、最后绕 X 转 roll”的内旋解释,和航空、车辆姿态那套习惯一致,跟串联机械臂常见的 ZYZ 不是一回事。
有个细节值得单独说:如果你从别处拿到一组[roll pitch yaw]数据,但不确定对方用的是外旋还是内旋,用rotm2eul反解一次再正解回去,看能不能复现原矩阵,比对着公式推半天快得多。
3.2 逆解函数:一次算完六条腿,外加两处校验
核心逻辑就三步:算旋转矩阵、把上平台铰点变换到基坐标系、求每条腿的向量长度。写成向量化版本,六条腿一次算完:
function [L, leg_vec] = inverse_kinematics(pose, base_pts, top_pts, cfg) % pose: 1x6 [x y z roll pitch yaw] % base_pts / top_pts: 3x6,列向量 % L: 6x1 腿长,leg_vec: 3x6 腿向量(基坐标系下,由下铰点指向上铰点) R = eul2rotm(pose(4:6), 'ZYX'); p = pose(1:3)'; top_w = R * top_pts + p; % 3x6,六次旋转乘法一次做完 leg_vec = top_w - base_pts; % 3x6 L = vecnorm(leg_vec, 2, 1)';% 沿第 1 维求二范数 -> 1x6 -> 转置成 6x1 if any(L < cfg.L_min_m) || any(L > cfg.L_max_m) warning('支腿超出行程: min=%.4f, max=%.4f', min(L), max(L)); end end参数说明:pose是行向量,base_pts/top_pts是 3×6 列排布,cfg里带L_min_m/L_max_m。返回的leg_vec后面算雅可比和动力学都要用,别只返回长度。
vecnorm的第三个参数是维度,这里是第 1 维,因为数据按列排。原始代码里出现过vecnorm(leg_vectors, 2, 2)的写法,那是对 6×3 的行排布求行范数,两种写法的结果顺序不一样,混用就会出现“腿长顺序错位”——曲线看着挺正常,动画里平台却像被拧了一把。我的做法是:全工程只允许 3×6 列排布,vecnorm的维度参数永远写 1。
向量化相比 for 循环在这个规模上大概快 5 到 10 倍,单次调用感知不明显,但你后面要做工作空间扫描(比如三个姿态角各取 21 个点,就是 9000 多次逆解),差距立刻出来了。我做过一次粗测,纯循环版本扫一遍工作空间要 40 多秒,向量化之后 6 秒左右,调参体验完全不是一个量级。
3.3 雅可比:先数值微分兜底,再上解析解
雅可比描述的是“位姿动一点,腿长动多少”,J是 6×6,第 i 列对应第 i 条腿。推导解析解要用到腿方向单位向量和位置向量的叉乘,符号容易推错,所以我习惯先用数值微分把整条链路跑通:
function J = compute_jacobian(pose, base_pts, top_pts, cfg, h) % 数值微分法:对 6 个位姿分量各扰动一次 if nargin < 5, h = 1e-6; end J = zeros(6,6); L0 = inverse_kinematics(pose, base_pts, top_pts, cfg); for i = 1:6 dp = zeros(1,6); dp(i) = h; % 位置用 m,姿态用 rad,量纲不同但可接受 J(:,i) = (inverse_kinematics(pose + dp, base_pts, top_pts, cfg) - L0) / h; end end代价是一次调用要做 7 次逆解(含基准点),对 6×6 的矩阵来说完全可以接受。等数值版稳定之后,再拿它当标准答案去校验解析解:norm(J_num - J_ana, 'fro') / norm(J_num, 'fro')小于 1e-6 才算对。这个顺序很重要,先写解析解再找 bug,你根本分不清是推导错还是坐标约定错。
顺带说一句h的取法。位置分量扰动用 1e-6 m 没问题,姿态分量扰动用 1e-6 rad 会让差分结果被浮点噪声淹掉,实际操作里我对姿态分量单独用 1e-4。这种细节不上台面,但会让你的雅可比在奇异位形附近表现得像随机数。
3.4 奇异判据:用条件数,别用行列式
平台走到某些位姿会“卡死”,腿长变化对位姿变化不再敏感,控制器一给指令就爆。原始代码里用det(J)判零,我建议换成条件数:
function [ok, kappa] = check_singularity(J, cfg) kappa = cond(J); % 无量纲,尺度变化不影响阈值 ok = kappa < cfg.kappa_max; % 常用 1e4 if ~ok warning('接近奇异位形, cond(J) = %.2e', kappa); end end原因很实在:行列式是有量纲的,机构尺寸从 0.5 m 改成 0.8 m,行列式的量级就整体变了,原来设的 1e-6 阈值直接失效,要么天天误报,要么真进了奇异区还不响。条件数没有这个问题,1e4 这个阈值换机构基本还能用。
真进了奇异区附近,求速度要用阻尼最小二乘,别硬求逆:
lambda = 1e-3; % 阻尼系数,从 1e-6 试起 dq = (J'*J + lambda*eye(6)) \ (J'*v_desired); % 6x1 关节速度lambda太大会让跟踪变迟钝,太小压不住奇异点附近的数值爆炸。我的习惯是先固定 1e-6 跑一遍,看腿速曲线的峰值,再往上加一个量级,直到峰值落到电缸能接受的范围。
3.5 用 hgtransform 做逆解快检
逆解算对了没有,看数字不如看画面。这里的关键是句柄只建一次,之后每帧只改数据或变换矩阵:
function h = init_scene(base_pts, top_pts) figure('Color','w'); ax = axes('XLim',[-0.8 0.8],'YLim',[-0.8 0.8],'ZLim',[0 1.2]); hold(ax,'on'); grid(ax,'on'); view(ax,35,20); axis(ax,'equal'); h.base = patch('Vertices',base_pts','Faces',1:6, ... 'FaceColor',[.6 .6 .6],'FaceAlpha',.6,'Parent',ax); h.tf = hgtransform('Parent',ax); % 刚体变换节点 h.top = patch('Vertices',top_pts','Faces',1:6, ... 'FaceColor',[.2 .5 .9],'FaceAlpha',.7,'Parent',h.tf); h.legs = plot3(ax, zeros(2,6), zeros(2,6), zeros(2,6), 'k-', 'LineWidth', 2); end function update_scene(h, pose, base_pts, top_pts) T = pose2T(pose); set(h.tf, 'Matrix', T); % 顶点不动,只换矩阵 top_w = T(1:3,1:3)*top_pts + T(1:3,4); set(h.legs, 'XData', [base_pts(1,:); top_w(1,:)], ... 'YData', [base_pts(2,:); top_w(2,:)], ... 'ZData', [base_pts(3,:); top_w(3,:)]); drawnow limitrate; % 比 drawnow 少排图形队列 endhgtransform的价值在于上平台的六个顶点只在初始化时上传一次,之后 GPU 侧只做一次矩阵变换,比每帧set(Vertices, ...)轻得多。drawnow limitrate会把刷新限制在大概 20 帧每秒,循环跑 100 Hz 也不会积压事件队列——这一点很关键,用普通drawnow的时候,图形刷新会把主循环拖慢两到三倍,你以为是算法慢,其实是画图慢了。
patch的Faces写成1:6是把六个顶点按顺序连成一个多边形,上下平台都是正六边形所以够用;如果要画成有厚度的板,得另外构造侧面顶点,别在这上面花太多时间,仿真阶段能看清姿态就够。
4. 动力学链路:牛顿-欧拉、S-Function 与 ode15s
运动学解决的是“腿该多长”,动力学解决的是“腿要出多大力”。Stewart 平台的动力学有两个绕不开的麻烦:一是六条腿和上平台强耦合,写不出干净的串链递推;二是符号推导展开后项数爆炸。工程上的解法是老老实实走数值路线,把质量矩阵、科氏项、重力项拆成独立函数,哪个错了单独查。
4.1 数值化牛顿-欧拉:符号推导会炸,别硬刚
我试过用syms把六条腿的动能、势能全展开再求拉格朗日方程,结果表达式长到没法看,生成代码那一步直接内存不足。后来改成矩阵形式,用雅可比把笛卡尔空间的惯性映射到关节空间:
function tau = dynamics_solver(q, dq, ddq, p) % q/dq/ddq: 6x1,这里用支腿长度及其一、二阶导 % p: 参数结构体,含 m_platform_kg、I_body(3x3)、摩擦系数 [Jv, Jw] = jacobian_split(q, p); % 3x6 线速度 / 3x6 角速度映射 M = p.m_platform_kg * (Jv'*Jv) + Jw' * p.I_body * Jw; % 6x6 广义质量阵 g = [0; 0; -9.81]; tau_g = -Jv' * (p.m_platform_kg * g); % 重力项 tau_c = coriolis_term(q, dq, p); % 科氏/离心项 tau = M*ddq + tau_c + tau_g + friction_term(dq, p); end参数上,m_platform_kg只算上平台加负载,六条腿的质量要么忽略,要么等效折算进M的对角线;I_body是上平台在体坐标系下的 3×3 惯量张量,从 CAD 里读,别手填。friction_term用库伦加粘滞两项,Fc*sign(dq) + Fv*dq,实测库伦摩擦对定位精度的影响比想象中大,尤其是低速换向那一下。
这套写法有个前提:Jv和Jw必须和逆解用的是同一套位姿约定,否则力算出来方向都是错的。我的做法是把jacobian_split放在kinematics/目录下,和inverse_kinematics共用pose2T,从源头上不给不一致留口子。
4.2 S-Function:状态方程写对,求解器选对
要在 Simulink 里闭环,就得把动力学写成状态空间形式。状态取 12 维:6 个腿长加 6 个腿速,输入是六条腿的驱动力:
function sys = mdlDerivatives(~, ~, x, u, p) q = x(1:6); % 支腿长度 dq = x(7:12); % 支腿速度 F = u(1:6); % 控制器给出的驱动力 M = mass_matrix(q, p); % 6x6 b = bias_force(q, dq, p); % 科氏 + 重力 + 摩擦 ddq = M \ (F - b); % 正动力学,用左除别用 inv sys = [dq; ddq]; % 12x1 状态导数 end两个细节:M \ (...)不要写成inv(M)*(...),前者是求解,后者是求逆再乘,数值稳定性和速度都差一截;F - b里的b必须包含重力项,否则零输入时平台会永远停不住。
求解器配置直接决定仿真能不能跑完:
set_param('stewart_model', 'Solver', 'ode15s', ... 'MaxStep', '5e-4', 'RelTol', '1e-4', 'AbsTol', '1e-6');ode15s是给刚性系统准备的,这类带闭环约束和强耦合的机构方程,用ode45经常跑到一半步长掉到 1e-9,十分钟仿真跑一晚上。MaxStep设成 5e-4 对应 0.5 ms 的步长上限,是原文里提到的量级,配合RelTol1e-4 基本够用。别把MaxStep设太大,变步长会直接跳过奇异位形附近的瞬态,你以为平台稳稳过去了,其实是在数值上被抹平了。
4.3 Simscape Multibody 联合仿真:CAD 进来先别急着跑
自己写的动力学模型适合做控制器验证,但要看“真实感”,还是得上 Simscape Multibody。从 CAD 导出的装配体可以直接生成模型骨架:
smimport('stewart_assembly.xml'); % 生成 Simscape 模型与关节 mdl = 'stewart_model'; set_param([mdl '/UpperPlatform/Revolute Joint'], 'Motion', 'Provided by Input'); set_param(mdl, 'StopTime', '10');导入之后有三件事必查:球铰和虎克铰的类型对不对(上下铰点通常一端球铰一端虎克铰,全用球铰会多出自由度,平台会自己转)、各连杆的质量和惯量有没有从 CAD 带进来(没带进来就是默认 1 kg,仿真结果毫无意义)、闭环约束有没有过约束报警。并联机构是闭环结构,六条腿同时约束上平台,约束数量一旦超过 6,求解器就会报过约束或者直接崩。
排查手段是Mechanics Explorer,把仿真暂停在第一步,手动拖一下上平台,看哪个关节在飘。哪个关节飘,就回去查那条腿的约束副定义。这个可视化调试器比看日志高效得多,我一般导入模型第一件事就是打开它空跑一次。
5. 避坑排查:奇异位形、单位制与栈溢出这五笔账
这套东西跑通之后回头看,真正花时间的不是推公式,是排查那些“数学没错但结果不对”的问题。下面五条都是我实际撞过的,按现象、原因、解决写,能省一次通宵就值了。
5.1 腿长曲线突然打出一个尖峰
现象:跑螺旋上升轨迹,位置曲线光滑,但某条腿在 t≈4.2 s 处速度跳变,腿长本身连续,导数不连续,控制器跟着抖。
原因:轨迹在笛卡尔空间做直线插补,位置线性、欧拉角线性。欧拉角在 pitch 接近 ±90° 附近本身就有表示奇异,角速度会趋向无穷,映射到关节空间就是突变。这类问题是“轨迹空间选错了”,不是逆解算错了。
解决:改用旋量插值或者分段贝塞尔曲线,让姿态在 SO(3) 上走短弧;懒得改的话,至少把 pitch 幅值限制在 ±60° 以内,并对欧拉角做unwrap。判定方法很简单:把cond(J)随时间画出来,尖峰位置和腿速尖峰对得上,就基本可以确定是插补方式的问题。
5.2 仿真一跑,平台直接飞到天上
现象:位移曲线指数发散,一秒内 z 冲到几十米,动画里平台瞬间消失。
原因:单位制不统一。要么 CAD 导出的装配体是英寸、逆解用的是米;要么惯量张量按 mm 算、质量按 kg 算,量级差了 1e6。还有一种隐蔽情况:g写成了 9.81 而长度单位是 mm,重力项大了三个数量级。
解决:变量名里强制带单位后缀,R_base_m、m_platform_kg、I_body_kgm2,看一眼就知道有没有混。再加一步静态自检:给零驱动力、零初速,跑 0.1 s,看ddq是不是接近零,不接近就说明重力项或质量矩阵有问题。这一步十秒钟,能省掉半天。
5.3 只给了横滚,平台却自己偏航了
现象:目标位姿里 yaw 是 0,画面上平台绕竖直轴转了二十来度。
原因:旋转顺序不一致。逆解用的是'ZYX',可视化用makehgtform('euler', ...),后者默认按 XYZ 组合,两边差一个复合旋转;另一种情况是上平台铰点用了下平台的相位,机构本身就歪了。
解决:全链路只保留pose2T一个入口,可视化也用它算出的 4×4 矩阵set到hgtransform的Matrix上,makehgtform的'euler'参数一律不用。另外,上下平台的相位偏置要在hinge_points里显式写出来,别在两处各写一半。
5.4 命令窗口刷栈溢出,MATLAB 直接卡死
现象:跑久了报Stack overflow或者Out of memory,任务管理器里 MATLAB 内存一路涨。
原因:两类。一类是把逆解写成了递归,每次位姿修正后再调用自己,终止条件写得不够严格,误差在 1e-9 附近反复横跳;另一类是syms推导的表达式太大,matlabFunction生成代码时直接爆内存。
解决:逆解必须是纯函数,无状态、不递归。位姿修正在外层用while循环,显式设最大迭代次数(我一般设 20)和收敛阈值,两个条件任一满足就退出。符号推导只用来在小规模下校验公式,验证完立刻扔掉,跑仿真一律走数值版本。从那以后我每个带迭代的函数都强制加次数上限,这个习惯救过我好几次。
5.5 动画卡成 PPT,曲线也跟着延迟
现象:实时循环里画面一卡一卡,腿长曲线在示波器里明显滞后。
原因:每帧都用plot3重建图形对象,句柄不断累积;或者用了drawnow而不是drawnow limitrate,图形事件队列积压,主循环被拖慢。
解决:初始化只建一次句柄,更新只set数据字段;刷新用drawnow limitrate;如果循环频率超过 100 Hz,直接在循环里加计数器,每 5 帧刷一次。刷新间隔别小于 20 ms,低于这个值人眼也分辨不出来,纯粹是浪费算力。
6. 二次开发:换轨迹、换机构、接控制器
拿到这套工程之后,绝大多数人第一件想干的事是换条轨迹。改trajectory_func就行,输出仍然是 6×1 的位姿序列,但有三处必须跟着验证,不然改完跑出来的曲线会很漂亮、机构却已经超行程了。
| 改造点 | 改动文件 | 必须重新验证的内容 |
|---|---|---|
| 换轨迹形状 | traj/trajectory_func.m | 腿长包络、cond(J)峰值、腿速上限 |
| 换机构尺寸 | config/platform_params.m | 工作空间扫描、奇异面位置、零位干涉 |
| 换负载质量 | 同上,m_platform_kg | 重力项量级、电机峰值推力 |
| 接外部控制器 | sim/stewart_sfun.m | 状态维度、输入端口顺序、单位 |
换机构尺寸这一项最容易被低估。半径一改,奇异面的位置就整体移动,原来安全的工作空间可能有一块直接落进奇异区。我一般写一个scan_workspace.m,三个姿态角各取 21 个点、z 取 15 层,批量跑逆解,输出腿长越界和cond(J) > 1e4的点云。几十行代码,跑一遍十几秒,比在动画里瞎拖靠谱得多。
还有一个我逢改必做的技巧:逆解没有正解对照,也能自己验自己。把算出来的六条腿长灌回一个正运动学牛顿迭代求解器,看能不能迭代回原来的位姿,误差小于 1e-8 才算这条链路干净。这个回归脚本run_regression.m我固定放一组位姿(零位、纯平移、纯横滚、接近奇异四组),每次动 config 就跑一遍,红灯了绝不去看动画。
从那以后我每次改platform_params.m里的任何一个半径或相位,都强制先跑工作空间扫描、再看cond(J)曲线、最后才打开可视化。跳过前两步直接看动画,看着顺滑不代表数值上安全。希望帮到你。
本文还有配套的精品资源,点击获取