该控制系统架构采用了典型的ROS (Robot Operating System) 节点式设计思想，主要分为四个并发运行的核心线程：主控调度线程、底盘与安全线程、视觉感知线程、运动采摘线程。

🧠 核心逻辑与多线程机制深度解析

在上述流程图中，控制系统的软硬件通过四个“泳道（Swimlanes）”进行协作，展示了复杂的并发逻辑与异常处理机制：

1. 异步感知与解耦调度 (Asynchronous Perception)

• 视觉感知线程

  （绿色泳道）是一个完全独立的无限循环线程。它不会阻塞机械臂的运动和底盘的行走。

• 它像雷达一样不断扫描，一旦发现苹果并计算好3D坐标，就将其压入目标队列地图（Target Queue）。主控线程只需要读取这个队列，解耦了“看”和“动”的时间差，极大提高了系统流畅度。

2. 最高优先级的安全机制 (Anti-Rollover Safety)

• 底盘与安全线程

  （蓝色泳道）具有系统的最高抢占权限（Preemptive Priority）。

• 丘陵地形的致命弱点是翻车

  。在采摘时，升降平台升起导致重心极高，此时如果履带底端突然滑坡，后果不堪设想。因此该线程以高频（如100Hz）持续监控IMU。

• 一旦发现重心偏移(ZMP)异常或倾角超限（图中标红的判断节点），它会跨线程强行中断机械臂的任务，瞬间降下升降台以降低重心，这是确保高价值机器人硬件安全的底线逻辑。

3. 宏微协同与采摘博弈 (Macro-Micro Manipulation)

• 运动采摘线程

  （黄色泳道）展示了独有的**“升降台（宏观Z轴）+ 6轴机械臂（微观6D位姿）”**的协同规划。

• 为了省电并减少车体晃动，系统不会摘一个苹果就动一次升降台。而是计算一堆苹果的平均高度，升降台一次到位后，依靠6轴机械臂的灵活工作空间，将该高度层的所有果实“一网打尽”。

• 在末端执行采摘动作时，流程图中特别定义了 “扭转 -> 下拉 -> 撤回” 的复合轨迹，结合力觉柔顺控制，完美模拟了果农摘苹果的手法，避免了柔性手爪拉扯导致大面积断枝断叶的问题。