6-PUS Stewart并联机构研究（三）：从仿真到实机——ROS 2 控制系统设计与任务空间运动验证

发表于 2026-09-16 分类于机器人

记录 6PUS Stewart 平台从仿真走向实机的过程：建立 ROS 2 任务空间控制、六轴连续 Servo、时间对齐与 FK 状态重建、双 IMU 感知，并用真实实验测量单自由度运行范围。

摘要

前两篇让 6PUS Stewart 平台在仿真中运动起来：从机构建模、运动学，到 Simscape 中的 PID、ADRC 与运动学补偿。本文继续向前一步——把它真正变成一台能够在任务空间中控制、反馈和验证的真实机器人。

这不是把 MATLAB 公式翻译成 Python。真实系统必须重新建立控制、通信、时间、反馈与安全边界：位姿如何变成六根执行器的连续运动，串行反馈如何对齐，编码器状态如何重建为平台位姿，以及运动学上“有解”的目标能否在真实闭链机构上运行。本文围绕这些问题，整理截至 J10 建立的 ROS 2 实机控制系统、双 H30 惯性感知与任务空间实测结果。

系列定位： Stewart-1 聚焦机构建模与运动学；Stewart-2 聚焦 Simscape 与 PID、ADRC、运动学补偿等控制仿真；本文 Stewart-3 聚焦 J1—J10 的 ROS 2 实机控制、状态重建、双 IMU 感知与真实运行范围验证。J11 起转入主动姿态防抖，留待下一篇展开。

1. 从仿真里的 6PUS，到真正运动的机器人

在 Simscape 中，六自由度目标可以直接送进模型。求解器知道机构几何和每个移动副的位置，控制器输出后，模型会沿统一的仿真时钟演化。

真实机器人没有这层“理所当然”。一个平台位姿下发以后，中间突然多出了一整条现实链路：

谁把变成六根执行器的目标？
六个执行器怎样在共享总线上连续、协调地运动？
六轴反馈依次返回，怎样知道机器人在同一个物理时刻的状态？
六个电机都到位，是否意味着动平台真的到位？
运动学有解、执行器未超行程，是否就代表真实机构可以安全走到那里？

所以，从仿真到实机不是代码移植，而是重新定义控制对象、通信时序、状态观测与实验边界。

系统由三条相互配合的链路组成：

控制下行：Cartesian Pose → Analytical IK → Joint Target
          → Trajectory Shaper → 50 Hz Servo → Six Actuators

状态上行：StatusFeedback → Per-axis Timestamp → Fixed-Lag
          → Synchronized Joint State → FK → Task-Space State

旁路感知：Base IMU + Platform IMU

前两篇关注“模型能不能运动”，这一篇追问：真实机器人能否听懂任务空间目标、执行它，并解释自己的运动状态。

2. 先看结果：它已经可以在任务空间连续运动

先不拆代码，直接看最终实机结果。

这一次，上层给出的已经不是六个电机 step，而是 Platform 的 XYZ 与 Roll/Pitch/Yaw。机器人能够完成单轴平移、单轴转动和六自由度复合连续运动。原 J8 final qualification 九阶段实验持续 137.128 s，执行 6898 个 Servo 周期，实测 50.30 Hz；六轴伴随反馈为 41388 / 41388，全程 0 fault、0 STOP。

复合运动段同时包含 XYZ 平移和三轴转动；返回 HOME 后，系统仍能由电机反馈重建任务空间状态。

这些结果没有回答所有精度问题，却回答了一个更基础的问题：从任务空间目标到真实六轴运动，再到反馈与状态重建，这条完整链路已经在实机上连续跑通。

现在再拆开来看：这台机器人为什么能够从“六个电机”变成“一个可在任务空间控制的平台”。

3. 真正的控制对象不是六个电机，而是一个六自由度平台

6PUS 有六个直线执行器，却不是六个独立自由度。六根支链共同约束一个刚体平台，任一执行器的变化都会通过闭链几何影响整体位姿。

对用户或上层控制器来说，真正有意义的命令是

而不是

若把六个 step 直接暴露给上层，控制算法就必须理解机构几何、执行器编号、工作零位和单位映射。任务空间入口把问题分成两层：上层表达“平台要去哪里”，运动学层计算“六根执行器应该到哪里”。这正是上层算法与底层电机解耦的基础。

从平台位姿到六根执行器

设第条支链的基座锚点为，动平台锚点在平台坐标系中的位置为，平台位姿为。动平台锚点在基座坐标系中的位置为

直线执行器沿 Z 方向移动，滑块位置记为

由支链定长约束，可以得到当前工作分支上的解析解：

这个解析式把平台 Pose 映射为六个彼此耦合、顺序固定的 Joint Target。

系统以 OPERATING_HOME=[1000]*6 作为工作参考。解析 IK 先计算目标位姿相对 HOME 的六轴位移，再按 1 step = 0.025 mm 转成硬件整数目标。STORAGE_PARK=[0]*6 则只用于机构收拢和停放，不是姿态控制的工作零位。

Cartesian Pose 到 Analytical IK 再到 Joint Target 的几何关系示意图 — 图 4：Cartesian Pose → Analytical IK → Joint Target 几何关系示意图

4. 控制下行：Pose → IK → Shaper → 50 Hz Servo

控制下行把持续更新的任务空间目标变成执行器可连续执行的命令：

Cartesian Pose
    ↓
Analytical IK
    ↓
HOME-relative Joint Target
    ↓
Jerk-Limited Trajectory Shaper
    ↓
Latest-Value-Wins Command
    ↓
50 Hz Six-Axis Servo
    ↓
UART / RS485 → Six Actuators

位姿进入 ROS 2 运动学链后，先转换为相对 HOME 的六轴位移，再映射为六个整数 step 交给电机驱动。

IK 解决“去哪”，轨迹整形解决“怎样过去”

若 IK 直接连接电机，位姿跳变就会变成六轴目标突变。解析解只回答“去哪”，不负责“怎样过去”。

项目使用有状态三阶轨迹整形器：

整形器跨周期保留、和，并限制速度、加速度与 jerk。新目标到来时，内部状态不会清零，因此机器人可以在运动中继续更新目标，而不必反复“从静止开始”。

这类设计很适合后续防抖：补偿位姿会随传感器反馈持续变化，控制链不能把每次微小更新都处理成一次独立点到点运动。

为什么使用 Latest-Value-Wins

连续控制不怕漏掉过期的中间点，更怕命令在队列里积压。若目标更新快于底层消费，FIFO 会让执行器追赶历史。驱动侧因此只保留最新目标；目标流中断时，看门狗停止刷新，而不是无限重复最后一帧。

六轴协调不等于硬件同时触发

六个执行器共享半双工总线。每个 20 ms 周期内，驱动按固定轴序发送目标并收集反馈。因此，“六轴同步”指同一控制周期内的协调批次，而非同一硬件时钟沿锁存。

系统记录每轴发送与反馈时刻，用于检查通信节拍，也为后面的时间对齐提供依据。

5. 状态上行：六个电机的位置，怎样变成平台位姿？

控制链跑起来之后，另一个问题随之出现：机器人怎样告诉我们“自己在哪里”？

目标写入会返回执行器的位置、电流和故障状态。这些信息能描述每根电动缸，却不能直接描述动平台。

六个状态又通过同一串行总线依次到达。真实数据更接近

而不是天然同步的。平台运动越快，拼接“六个最新值”的时间误差越明显。

用一点延迟，换取同一物理时刻

驱动为每个轴保留单调时钟时间戳，形成 Timed Joint Feedback。Fixed-Lag 同步器维护六条时间线，并选择稍早于当前时刻的查询点。只有每个轴都存在左右样本

时，才插值生成同步关节状态；若某一轴尚无未来样本，系统等待而不外推。

这是一种朴素而重要的交换：用一点延迟，换取同一物理时刻。 对闭链机构而言，时间一致性不是数据处理优化，而是 FK 成立的前提之一。

从同步关节状态回到平台位姿

得到同步的后，系统通过 FK 重建平台位姿。六条支链同时满足

FK 使用带解析雅可比的 Levenberg–Marquardt 迭代，以上一次结果 warm start，并检查几何残差与位姿分支连续性。状态上行由此完成从六轴反馈到任务空间位姿的映射。

科研边界也很清楚：FK 是执行器反馈 + 当前名义几何得到的模型重建状态，不是独立外部 ground truth。 它适合观察链路与检查一致性，不能替代独立测量。

6. 一条真实机器人链路，怎样证明它已经跑通？

“节点都启动了”不能证明机器人系统已经工作；“某次动作成功”也不能证明连续控制链稳定。实机验证沿着能力层级逐步增加压力。

最基础的是 PARK 与 HOME。STORAGE_PARK 用于受控收拢，OPERATING_HOME 是任务空间控制的工作参考。重复的 PARK↔HOME 运动确认了六轴方向、零位、目标范围和停止流程。

在此基础上，Servo 验证不再只发一个终点，而是连续刷新六轴目标，检查控制节拍、反馈完整性、中断与故障处理，以及运动后能否可靠返回 HOME。

原 J8 final qualification 九阶段实验覆盖 HOME 保持、XYZ 单轴平移、Roll/Pitch/Yaw 单轴旋转、六自由度复合运动和 HOME 返回，证明连续运动链路跑通。

为补充连续时序证据，后续在不修改冻结 J7/J8 控制链的前提下，仅增加旁路记录器，按同一九阶段轨迹补录了一次真实硬件实验，并同步保存 target pose、command、feedback、FK 与阶段事件时序。本次补录持续 137.132 s，执行 6900 个 Servo 周期，实测 50.32 Hz，六轴反馈 41400 / 41400 完整返回；全程 0 hardware fault、0 feedback timeout、0 driver STOP，最终返回 HOME。

J8 九阶段实机补录实验的 command、feedback 与 FK 联合结果 — 图 6：J8 九阶段实机补录实验的 command / feedback / FK 联合结果

原 J8 qualification 与这次补录共同说明：任务空间目标能连续驱动六轴，状态上行能返回完整反馈并重建平台状态。图中的 FK 是执行器反馈与名义几何得到的模型重建状态，不是外部 ground truth。这些结果不等同于硬实时或绝对精度认证，但已构成后续闭环控制的运行底座。

7. 给平台装上“眼睛”：从运动控制走向防抖感知

编码器与 FK 回答的是：“根据六根执行器的位置和名义几何，平台在哪里？”下一阶段关心的则是：“Base 被人为移动后，Platform 相对于世界坐标系有没有转动？”

这个问题不能只靠执行器编码器回答。为此，系统在 Base 与 Platform 上分别安装了一只 H30 IMU。两路数据已经接入 ROS 2，可以提供姿态四元数、角速度和线加速度，并与关节命令、关节反馈及 FK 状态联合记录。

双 IMU 的价值不只是增加两组数据，而是让系统第一次拥有研究这个问题所需的独立惯性观测：

Base 在动，而 Platform 是否真的保持不动？

Base IMU 描述扰动端，Platform IMU 直接观察被稳定对象。控制器输出补偿位姿后，可以直接检查 Platform 的世界系姿态是否被保持。

独立惯性观测由此进入真实机器人数据链，为姿态闭环提供了感知入口。

Base 与 Platform 双 IMU 姿态感知及坐标系关系图 — 图 7：Base / Platform 双 IMU 姿态感知与坐标系关系图

8. 运动学有解，不代表实机能走：真实工作空间测量

仿真阶段很容易把“逆运动学能算出结果”理解为“机器人能够到达”。实机把这个等号拆开了。

对真实 6PUS，理论运动学工作空间 ≠ 执行器行程允许范围 ≠ 真实机构运行范围：

理论运动学工作空间： 名义几何方程存在实数解；
执行器行程允许范围： 六根电动缸的目标都未越过行程；
真实机构运行范围： 机器人能够到达、保持、反馈，并可靠返回 HOME。

第三层还受真实效应约束：大倾角可能触发过流，闭链姿态会改变杆件间隙，极端扭转会使 FK 数值条件恶化，支链也可能发生干涉。

测量方法：一次只问一个问题

工作空间实验从 OPERATING_HOME 出发，每次只激励一个自由度。候选目标从小幅开始逐步扩展，到点后记录六轴反馈、FK 与 IMU，再返回 HOME 并确认状态。这样得到的是 HOME-centered、single-DOF、axis-wise 的真实硬件验证范围，而不是任意六自由度组合下的完整空间。

六个方向的实测结果

自由度	J10 最终范围	实验中观察到的边界机制
X	`[-45.75, +45.75] mm`	接近执行器绝对端点前停止
Y	`[-45.50, +49.00] mm`	接近执行器绝对端点前停止，正负方向略不对称
Z	`[-25.00, +25.00] mm` 共模能力	笛卡尔实测到 `[-20.0,+24.5] mm`，PARK 证据覆盖下端约 `-24.983 mm`
Roll	`[-35.0, +37.5]°`	更大倾角先出现电流/负载型停止
Pitch	`[-30.0, +35.0]°`	更大倾角先出现电流/负载型停止
Yaw	`[-90.0, +90.0]°`	`+95°` 出现 FK 数值执行失败与跟踪超时；`+105°` 出现杆件干涉风险

X/Y 的实验在下一候选点预计只剩不超过 20 step（0.5 mm）行程裕量时停止，因此表中的数值是已验证范围，不是撞到机械端点得到的极限。

Z 方向接近理想共模运动：六根执行器等量伸缩。上端通过任务空间逐步验证到 +24.5 mm；下端没有让 IK 直接逼近零行程，而是使用已经验证的 PARK 流程获得端点侧证据。这种处理把“任务空间实验覆盖”和“已知共模行程能力”分开保留。

Roll 与 Pitch 呈现出另一种边界。执行器尚未接近行程末端，电流与负载已经先成为限制。闭链机构在大倾角下的受力变化，不能由单纯的行程检查代替。

Yaw 则展示了数值模型与真实几何的双重边界。+95° 时没有发生机械碰撞，但 FK 数值条件恶化，最终未能完成有效跟踪；+105° 测试中，真实杆件出现空间交叉和闭链卡住。解析 IK 仍能给出一组 step，却不再代表这是一个值得执行的实机目标。

工作空间测量的价值正在于此：它识别了各方向为何停下，以及哪一层边界先失效。

9. J1—J10，我们最终完成了什么？

9.1 它已经能够听懂“我要它去哪”

上层给出 Cartesian Pose，IK 生成六轴目标，轨迹整形器决定怎样到达，50 Hz Servo 再把目标送到六根执行器。

机器人对外呈现的是任务空间，而不是六个散乱的电机接口。

9.2 它已经能够告诉我“自己在哪里”

执行器返回实际位置；Fixed-Lag 对齐六条时间线，FK 再重建平台位姿。

这条状态链不把模型重建当作外部真值，却能提供连续、可解释的任务空间状态。

9.3 我开始知道“它到底能去哪”

J10 区分了理论可达、行程允许与实机可运行，也观察到行程、负载、数值退化和几何干涉等不同边界。

双 IMU 的加入又向前推进了一步：系统开始具备在 Base 运动时，独立观察 Platform 世界系姿态的能力。

阶段总结

Stewart-3 的结果不是某个单独算法，而是机器人研究对象本身发生了变化。前两篇中的 6PUS 是一个可计算、可仿真的机构；截至 J10，它已经成为一台能够接收任务空间目标、执行连续运动、重建自身状态，并用真实实验认识运动边界的机器人。

下一篇：从“让它动”到“让它不动”

J1—J10 解决的问题是：

如何让真实 6PUS 按照任务空间目标可靠运动？

J11 开始研究另一个问题：

如果我故意移动 Base，它能否主动补偿，让 Platform 尽可能保持原来的世界系姿态？

第一阶段聚焦三轴姿态稳定，暂不要求 Platform 的 XYZ 世界系位置保持不变。目标可以写成

J11 的主闭环与扰动观测旁路为：

主闭环：
Desired Platform Attitude (R_WP_des = constant)
    ↓
Attitude Error
    ↓
PID Controller
    ↓
Compensation Pose
    ↓
Analytical IK → Six-Axis Servo
    ↓
Platform
    ↓
Platform IMU
    └── Feedback → Attitude Error

扰动观测旁路：
Base Disturbance → Base IMU → Base Attitude / Disturbance Observation

Platform IMU 提供当前 PID 三轴姿态稳定的主闭环反馈；Base IMU 观测基座扰动，为后续扰动补偿或前馈提供信息。

J11 将先建立 PID 姿态防抖 baseline；J12 再把 ADRC 迁移到实机，并加入末端外部扰动；J13 继续设计补偿器，更后续才考虑强化学习或 SAC。执行器零偏、几何参数、IMU 外参与杠杆臂等标定问题，则根据控制精度和状态估计需求继续完善。

此前，控制器让平台主动运动；接下来，它将利用同一套 IK、Servo 和双 IMU 链路，在 Base 被扰动时使 Platform 世界系姿态尽可能保持恒定。

J1 到 J10 再到 J11 的研究路线转折图 — 图 9：J1–J10 → J11 研究路线转折图

说明

本文基于项目截至 J10 的真实代码与实机实验记录整理；涉及的系统参数、实验结果与工作范围均以最终冻结记录为准。