本页聚焦推理系统的安全纵深防御体系，从策略输出、观测输入、硬件通信到服务接口，逐层剖析系统如何通过数值限幅、状态监测与故障熔断机制，在实时双线程环境下保障机器人本体安全。理解这些机制是现场调试与部署前的必要前提，也是后续性能调优与新增观测源时不可逾越的边界条件。

安全架构总览

系统的安全机制并非集中式处理，而是分布在策略推理层、实时控制层与硬件驱动层三个维度，形成“输入限幅 → 状态监测 → 输出平滑 → 硬件熔断”的闭环。推理线程以 decimation_ 倍于控制线程的频率运行，负责高层安全决策；控制线程以固定周期 dt_ 执行动作插值与下发；硬件层则通过响应计数与初始化原子标志实现最后一道防线。

flowchart TD
    A[手柄 / cmd_vel 输入] -->|clip_cmd 限幅| B[指令观测值]
    C[IMU / 电机反馈] -->|clip_obs 限幅<br/>gravity_z_upper 跌倒检测<br/>joint_limits 超限检测| D[归一化观测值]
    B --> E[ONNX 推理]
    D --> E
    E -->|clip_actions 限幅| F[原始动作输出]
    F -->|action_scale 缩放| G[目标关节位置]
    G -->|act_alpha 平滑| H[控制线程 apply_action]
    H --> I[RobotInterface<br/>motor_offline 检测]
    I -->|MIT 指令| J[电机驱动]
    
    style C fill:#f9f,stroke:#333
    style E fill:#bbf,stroke:#333
    style I fill:#fbb,stroke:#333

Sources: inference_node.hpp, robot_interface.hpp

观测值安全限幅与异常检测

观测值裁剪

在推理线程将观测向量送入 ONNX 模型前，系统会对所有浮点观测值执行硬边界裁剪，防止极端传感器噪声或坐标变换异常导致模型输入发散。裁剪边界由 clip_observations_ 参数定义，默认值为 100.0，可在配置中按策略独立调整。该操作发生在帧堆叠更新之后、模型推理之前，确保历史帧中的异常值同样受到约束。

Sources: inference_node.cpp

跌倒检测（重力方向监测）

系统利用 IMU 四元数将世界坐标系下的重力向量 (0, 0, -1) 旋转到机体坐标系，得到 gravity_b。当机体倾斜过大时，gravity_b.z() 将上升并超过阈值 gravity_z_upper_（默认 -0.5）。此时系统判定机器人已跌倒，立即触发 RCLCPP_FATAL 日志并调用 rclcpp::shutdown() 终止进程，防止倒地后继续输出错误力矩造成二次损伤。

Sources: obs_manager.cpp

关节位置超限监测

在组装 dof_pos 观测段时，系统逐关节比对当前位置与 joint_limits_ 配置的硬限位。joint_limits_ 以 [min_0, max_0, min_1, max_1, ...] 的扁平数组形式存储，若任一关节突破边界，同样触发致命日志与进程关闭。需要注意的是，该机制仅用于监测与熔断，并不直接对模型输出做限位裁剪；动作安全由后文的 clip_actions_ 与 action_scale_ 共同约束。

Sources: obs_manager.cpp

动作输出安全限幅与平滑

动作裁剪与缩放

模型原始输出首先经过 clip_actions_ 硬限幅（默认 100.0，实际部署中常根据关节行程调低），再乘以 action_scale_（典型值 0.25）映射到目标关节角度，最后叠加 joint_default_angle_ 得到绝对位置指令。这一“裁剪 → 缩放 → 偏移”的三段式处理确保了即使策略网络输出饱和，最终电机指令仍落在物理可接受范围内。

Sources: inference_node.cpp

动作指数平滑

控制线程并非直接将推理线程的最新动作下发给电机，而是执行一阶指数平滑：last_act_ = act_alpha_ * act_ + (1 - act_alpha_) * last_act_。当 act_alpha_ = 1.0 时无平滑；当 act_alpha_ < 1.0 时，动作变化率被抑制，可有效降低策略切换或高频抖动导致的机械冲击。平滑后的 last_act_ 才是最终通过 RobotInterface::apply_action() 写入电机的目标值。

Sources: inference_node.cpp

指令输入安全限幅

无论是手柄摇杆还是 /cmd_vel 话题，所有外部运动指令在进入观测缓冲区前都必须经过 clip_cmd_ 六元组裁剪。数组顺序为 [x_min, x_max, y_min, y_max, yaw_min, yaw_max]，例如 [-0.4, 0.6, -0.4, 0.4, -0.8, 0.8]。手柄回调还会根据左右扳机键（LT/RT）的符号差异分别映射到偏航角速度，避免摇杆零点漂移产生非预期运动。

Sources: ros_interface.cpp, ros_interface.cpp

硬件层故障检测

电机离线检测

RobotInterface::apply_action() 在每次动作下发前会并行读取所有电机的状态，并检查 motor->get_response_count() 是否超过 offline_threshold_（硬编码为 25）。该计数器由底层电机驱动维护，若总线丢包或电机掉电导致连续未响应帧数超过阈值，系统立即抛出 std::runtime_error，异常将上抛至控制线程或推理线程的 catch 块，最终触发进程安全关闭。

Sources: robot_interface.cpp, robot_interface.hpp

初始化状态互锁

RobotInterface 内部以原子布尔值 is_init_ 标记电机初始化状态。apply_action() 与所有关节状态读取接口均会在 is_init_ 为 false 时直接返回或抛出异常，防止未上电、未标零的电机收到力矩指令。主节点中的 InferenceNode::apply_action() 同样会二次检查 robot_->is_init_，形成双重门控。

Sources: robot_interface.cpp, robot_interface.hpp, inference_node.cpp

关节复位安全策略

执行 reset_joints 时，系统不会一次性施加全刚度 KP，而是先以 kp_/2.5 的柔度驱动关节回中位并保持 1 秒，再切换至全刚度。这种渐进式上电策略避免了从自由态到高增益控制态的瞬时冲击，对减速器与连杆结构更为友好。

Sources: robot_interface.cpp

运行时异常处理与线程保护

控制线程与推理线程均包裹在 try-catch 块中，任何未捕获的异常（包括电机离线、关节超限、模型输入尺寸不匹配等）都会被拦截为 RCLCPP_FATAL 日志，随后调用 rclcpp::shutdown() 终止 ROS 2 执行器。由于主线程通过 MultiThreadedExecutor 管理节点生命周期，两个实时线程会在 rclcpp::ok() 返回 false 后自然退出循环，避免异常逃逸导致线程僵死或野指针访问。

Sources: inference_node.cpp, inference_node.cpp

服务级安全互锁

节点对外暴露的多个 ROS 2 Service 均设有前置状态检查，构成操作层面的安全互锁矩阵：

Sources: ros_interface.cpp

安全配置参数速查

以下参数均在 YAML 配置文件中通过 ROS 2 declare_parameter / get_parameter 机制加载，修改后需重启节点生效：

Sources: ros_interface.cpp, robot_interface.hpp

故障处理流程示例

当机器人在运行中因碰撞导致某关节超出硬限位时，系统按以下时序响应：

sequenceDiagram
    participant IM as 推理线程
    participant OM as ObsManager
    participant RI as RobotInterface
    participant MT as 控制线程
    participant HW as 电机硬件

    IM->>OM: 调用 get_dof_pos_obs()
    OM->>OM: 读取 joint_pos_buffer_
    OM->>OM: 对比 joint_limits_
    OM-->>IM: 超限 → throw std::runtime_error
    IM->>IM: catch 块捕获
    IM->>IM: RCLCPP_FATAL + rclcpp::shutdown()
    MT->>MT: rclcpp::ok() 返回 false
    MT->>RI: 停止调用 apply_action()
    RI->>HW: 不再下发新指令
    Note over HW: 电机保持最后力矩或进入保护态

Sources: obs_manager.cpp, inference_node.cpp

下一步阅读建议

安全机制与系统的实时性、配置体系紧密耦合。若需深入了解推理线程与控制线程的调度细节，请参阅 实时双线程设计与调度策略；若计划修改观测布局或调整动作缩放参数，建议先阅读 观测布局配置与动态解析 与 多策略 YAML 配置详解。对于电机通信与闭环解耦的底层实现，可参考 电机驱动与 CAN/CANFD 通信 和 闭环解耦与运动学映射。