3、状态机基础：有限状态机（FSM）的定义、状态、转移、事件、动作，以及状态图绘制方法

在机器人软件架构中，行为树（Behavior Tree）与状态机（State Machine）是两种最主流的任务编排范式。行为树以“决策树+黑板”的方式驱动行为选择，而状态机则以“状态+事件”的方式驱动系统迁移。对于ROS开发者而言，理解有限状态机（Finite State Machine, FSM）是构建可靠机器人应用（如自主导航、机械臂抓取、多传感器融合调度）的基石。本章将系统性地剖析FSM的五大核心要素——状态、转移、事件、动作，以及状态图的绘制规范，并结合ROS 2中的实际工程场景，帮助你建立从理论到代码的完整认知。

有限状态机（FSM）核心要素与状态图示例 初始 IDLE 等待指令 NAVIGATING 执行路径规划 GRASPING 控制机械臂 COMPLETED 任务成功 ERROR 异常处理 start_nav / 发布目标点 arrived / 触发抓取 grasp_success timeout / 重试超限 collision / 规划失败 reset / 恢复初始 图例： 状态节点 转移（事件/动作） 异常转移 初始状态

上图展示了一个典型的机器人“导航-抓取”任务FSM。从初始状态进入IDLE，通过事件触发逐步迁移至NAVIGATING、GRASPING，最终到达COMPLETED或ERROR。图中清晰地标注了每个状态对应的动作（如“发布目标点”）以及触发迁移的事件（如“arrived”）。接下来，我们将深入拆解FSM的每一个构成要素。

3.1 有限状态机的定义与核心价值

核心原理：有限状态机是一种数学模型，用于描述系统在有限个状态之间迁移的行为。它由五个元组定义：状态集合（S）、输入事件集合（E）、转移函数（δ）、初始状态（s₀）和动作集合（A）。在ROS机器人中，FSM常用于管理节点的生命周期、任务调度或安全监控。例如，导航栈中的nav2_behavior_tree虽然使用行为树，但其底层仍依赖状态机来管理“未初始化”、“激活”、“暂停”等生命周期状态。

为什么需要FSM？在机器人系统中，任务往往具有明确的阶段性（如“待机→导航→抓取→放置”）。FSM强制开发者将系统行为划分为离散的、可预测的状态，从而避免“面条式”的if-else逻辑。其核心价值在于：

  • 确定性：给定当前状态和输入事件，系统的下一个状态是唯一确定的，便于调试和验证。
  • 可维护性：每个状态独立封装，修改一个状态的行为不会影响其他状态。
  • 可测试性：可以针对每个状态和转移编写单元测试，提高代码覆盖率。
经验提醒：在ROS 2中，rclcpp_lifecycle节点本身就是FSM的典型实现（Unconfigured→Inactive→Active→Finalized）。理解FSM有助于你更好地掌握ROS 2节点的生命周期管理，避免在节点启动顺序上踩坑。

3.2 状态（State）与动作（Action）的深度解析

状态的定义：状态是系统在某一时刻的稳定“快照”，它由内部变量（如位置坐标、传感器读数）和外部表现（如正在执行的行为）共同决定。在FSM中，状态必须是互斥且穷尽的——系统在任何时刻只能处于一个状态，且所有可能的情况都应被覆盖。

动作的类型：动作是状态内部或转移过程中执行的具体操作。在ROS中，动作通常表现为发布话题、调用服务、执行行为树节点或控制电机。动作分为三类：

动作类型 触发时机 ROS 2实现示例
入口动作（Entry Action） 进入状态时执行一次 在NAVIGATING状态入口调用nav_to_pose动作客户端发送目标点
转移动作（Transition Action） 状态迁移过程中执行 从IDLE到NAVIGATING时，发布/goal_pose话题
状态内动作（Do Action） 状态持续期间周期性执行 在GRASPING状态中，循环读取夹爪力传感器数据，直到达到阈值

操作流程：在ROS 2中实现状态动作时，推荐使用rclpy或rclcpp的定时器（Timer）来执行状态内动作，避免阻塞事件循环。例如：

def _grasp_timer_callback(self):
    if self.current_state == 'GRASPING':
        force = self.force_sensor.read()
        if force > self.threshold:
            self.trigger_transition('grasp_success')
        elif self.retry_count > 3:
            self.trigger_transition('timeout')

3.3 事件（Event）与转移（Transition）的机制

事件的定义：事件是触发状态迁移的外部或内部信号。在ROS中，事件可以是话题消息（如/odom的更新）、服务响应、定时器超时或用户自定义信号。事件分为同步事件（如函数调用）和异步事件（如订阅回调）。

转移的分类：转移定义了系统如何响应事件。根据触发条件，转移可分为：

  1. 无守卫转移：事件发生即转移，无需额外条件。例如，收到shutdown信号立即进入IDLE。
  2. 带守卫转移：事件发生且守卫条件（Guard Condition）为真时才转移。例如，收到arrived事件，但还需检查goal_reached标志位。
  3. 内部转移：事件触发动作但不改变状态。例如，在NAVIGATING中收到progress_update事件，仅更新进度条。

判断方法：设计转移时，应遵循“事件驱动、条件守卫、动作执行”三步法。首先明确触发事件，其次定义守卫条件（避免非法迁移），最后指定转移动作。在ROS 2中，可以使用rclpy的Transition类或第三方库smach（ROS 1）/yaml-cpp配置状态机。

风险提示：在ROS 2中，回调函数（如订阅回调）默认在单线程执行器中串行执行。如果在回调中直接执行耗时动作（如阻塞式服务调用），会导致事件响应延迟，甚至造成死锁。建议将耗时操作放入异步动作服务器或独立线程中。

3.4 状态图的绘制方法：从UML到ROS实践

绘制规范：状态图（Statechart）是FSM的可视化表示，遵循UML（统一建模语言）规范。核心元素包括：圆角矩形（状态）、箭头（转移）、箭头上的标签（事件[守卫条件]/动作）。绘制时需注意：

  • 初始状态：使用实心圆点表示，只能有一个。
  • 终止状态：使用牛眼符号（实心圆外加圆圈）表示，可有多个。
  • 转移标签：格式为“事件[守卫条件]/动作”，例如“arrived[goal_reached==true]/trigger_grasp”。
  • 层次结构：对于复杂系统，可使用复合状态（Composite State）或正交区域（Orthogonal Region）来减少线条交叉。

案例拆解：以ROS 2导航中的nav2_controller为例，其内部状态机包含“跟随路径”、“避障”、“恢复”等状态。绘制时，我们首先列出所有状态，然后识别事件（如path_updated、obstacle_detected），最后定义转移。下图展示了简化版的状态图绘制逻辑（已在上方SVG中体现）。

常见误区：

  • 状态爆炸：将连续变量（如速度）直接作为状态，导致状态数量无限。解决方法是使用状态变量（如“高速/中速/低速”）或引入行为树进行连续控制。
  • 遗漏守卫条件：仅凭事件触发转移，未检查系统是否处于合法状态，导致非法迁移。例如，在GRASPING中收到start_nav事件，应忽略或报错。
  • 动作与状态耦合：将动作逻辑直接写在状态类内部，导致代码复用性差。建议将动作封装为独立的ROS节点或行为树节点。
实战复盘：在某AGV（自动导引车）项目中，我们使用FSM管理“充电-任务-待机”流程。初始版本未设置守卫条件，导致在充电状态下收到任务指令时直接迁移，造成电池损坏。后来增加守卫条件[battery_level > 30%]，并在转移动作中先暂停充电再启动任务，问题解决。这提醒我们：守卫条件不是可选项，而是安全底线。

3.5 状态机在ROS 2中的工程实现模式

实现方案对比：在ROS 2中实现FSM有多种方式，各有优劣：

方案 语言 优点 缺点 适用场景
手写if-else C++/Python 简单直接，无依赖 难以维护，状态一多即混乱 状态少于5个的简单逻辑
SMACH（ROS 1） Python 可视化调试工具，状态复用 ROS 2支持不完善，性能开销大 遗留项目迁移
rclpy_lifecycle C++/Python 与ROS 2节点生命周期集成 仅适用于节点级状态，不适用于任务级 管理节点启动/关闭
YAML+状态机库（如transitions） Python 配置与代码分离，易于修改 需额外学习库API 任务级状态机，快速迭代

推荐实践：对于ROS 2项目，建议使用transitions库（Python）或boost::statechart（C++）。以transitions为例，核心代码仅需几行：

from transitions import Machine
class Robot:
    states = ['idle', 'navigating', 'grasping', 'completed', 'error']
    def __init__(self):
        self.machine = Machine(model=self, states=Robot.states, initial='idle')
        self.machine.add_transition(trigger='start_nav', source='idle', dest='navigating', after='publish_goal')
        self.machine.add_transition(trigger='arrived', source='navigating', dest='grasping', conditions='check_pose')
        self.machine.add_transition(trigger='grasp_success', source='grasping', dest='completed')
        self.machine.add_transition(trigger='timeout', source='grasping', dest='error')
← 上一章 📖 返回目录 下一章 →