需要完整示例代码，请点击这里。

1. 前言：感知与导航在机器人系统中做什么

一台能在真实环境里走起来的机器人，通常同时跑着两类完全不同的问题：
感知模块回答的是“我是谁、我在哪、周围有什么”。IMU、关节编码器、激光雷达、深度相机各自只能看到世界的一个切片。感知要把这些切片融合成机器人能用的状态：本体位姿与速度、局部地图、障碍物几何。没有可靠感知，后面所有规划都是在猜。
导航模块回答的是“我要去哪、这一步该怎么走”。它消费感知结果，产出机器人能执行的运动指令。对四足 / 轮足 / 人形这类用强化学习部署的机体，导航几乎从不直接写关节力矩，而是给出机体系速度指令 ((v_x, v_y, \omega_z))，再交给已经训练好的 locomotion 策略变成关节 PD 目标。
本文将结合rl_sar项目，讲解如何分层设计感知导航模块。rl_sar 今天已经把后半段做完了。它的控制闭环是：
键盘 / 手柄 /cmd_vel → obs.commands → RL Policy → 关节 PD → 电机
按下 N 进入 navigation_mode 后，策略不再读手柄速度，而是订阅 /cmd_vel。也就是说，运动执行层已经为导航预留了标准接口。缺的是前面整段：状态估计、建图定位、障碍物、语义目标、局部避障。
如果把 EKF、Fast-LIO2、障碍物检测、VLN、避障策略和 rl_sar 揉进同一个节点，系统会在三个维度同时崩：

• 频率对不上。 IMU 预测要 200–500 Hz，Fast-LIO2 大约 10 Hz，障碍物 10–20 Hz，VLN 往往 1–2 Hz，RL 策略在 Go2 上是 dt * decimation = 0.005 * 4 = 20 ms（50 Hz）。
• 故障域绑死。 雷达掉线不该让关节控制停；VLM 超时不该让机体倒下。
• 传感器无法插拔。 换 Livox 为深度相机，或临时关掉雷达，会被迫改导航和策略代码。

因此必须分层。下面给出一套 五层架构：传感器接入、本体状态估计、环境感知、导航决策、运动执行。五层是工程上的甜点——再少会把 LIO 和障碍物、VLN 和避障揉在一起；再多会为了分层而分层，接口成本高于收益。

2. 总体架构：五层、接口与数据流

2.1 为什么是五层

层号	名称	典型频率	核心问题
L1	传感器接入层	驱动原频率	原始数据如何变成统一样本，如何热插拔
L2	本体状态估计层	200–500 Hz 预测 / 10–50 Hz 更新	机体位姿、速度、IMU 偏置
L3	环境感知层	10–20 Hz	Fast-LIO2 定位建图 + 局部障碍
L4	导航决策层	VLN 1–2 Hz / 避障 10–20 Hz	语言目标 + 局部避障 → /cmd_vel
L5	运动执行层	50 Hz 推理 / 200 Hz 关节	rl_sar 已有的 RL locomotion

分层的原则只有三条：

• 上层只依赖下层的稳定话题，不依赖下层的实现。 L4 只认 /perception/odom 和 /perception/height_scan，不认 Livox 还是 RealSense。
• 每一层可以独立降级。 雷达掉线时 L3 切到深度相机；VLN 超时 L4 输出零速；L5 始终能靠手柄接管。
• 训练好的策略只挂在一个接口上。 避障策略吃高度扫描 + 目标速度，吐 cmd_vel；locomotion 策略继续吃 obs.commands。不要让两个网络同时抢关节。

2.2 五层总图

2.3 一次控制周期的时序

3. L1 传感器接入层：为插拔而生

3.1 定义

L1 是硬件世界进入软件世界的唯一入口。它不估计状态，不检测障碍，只做三件事：把厂商消息转成层内样本类型、维护传感器心跳、在超时后宣布该通道死亡并让上层降级。

3.2 核心组件

组件	职责
ISensor	插件接口：start / stop / status
ImuSensor / LidarSensor / DepthCameraSensor	具体驱动适配器
SensorManager	注册、批量启动、按 SensorKind 查询存活
stack.yaml	用 enabled 开关实现配置级插拔

插拔策略分两级：

• 配置插拔： YAML 里关掉 lidar.enabled，节点启动时根本不订阅雷达，L3 自动走深度相机。
• 运行时插拔： 话题超时后 alive=false。L3 若发现雷达死亡且深度相机存活，切数据源；两者都死则停止向 L4 发布新障碍，避障策略按“未知环境”输出零速或原地旋转。

3.3 数据流

核心接口（完整实现见 include/robot_perception_nav/sensor_interface.hpp）：

class ISensor {
public:
    virtual ~ISensor() = default;
    virtual std::string name() const = 0;
    virtual SensorKind kind() const = 0;
    virtual std::string topic() const = 0;
    virtual void start(rclcpp::Node& node) = 0;
    virtual void stop() = 0;
    virtual SensorStatus status() const = 0;
};

L2 / L3 只接收 ImuSample、CloudFrame，不出现 livox_ros_driver2 或 realsense2_camera 的头文件。换传感器 = 换一个 ISensor 实现，上层零修改。

4. L2 本体状态估计层：EKF

4.1 定义

L2 维护机器人本体在世界系 / odom 系下的连续状态，供 VLN、避障、以及需要线速度观测的策略使用。它不是 SLAM。SLAM 负责约束漂移；EKF 负责把 IMU 的高频动力学和低频外部位姿缝在一起。

4.2 状态与职责

符号	维数	含义
(p)	3	位置，odom 系
(v)	3	线速度，odom 系
(q)	4（误差 3）	姿态
(b_a)	3	加速度计偏置
(b_g)	3	陀螺偏置

• 预测： IMU 加速度、角速度，200 Hz 以上。名义状态积分，误差协方差用 (F, G, Q) 传播。
• 更新： Fast-LIO2 的 /Odometry（位置 + 姿态）。没有 LIO 时，EKF 退化为姿态传播 + 速度漂移，L4 应限制速度指令。
• 输出： /perception/odom（含机体系速度）和 odom → base TF。

rl_sar 当前 locomotion 观测默认是 commands, ang_vel, gravity_vec, dof_pos, dof_vel, actions，并不使用 lin_vel。所以 L2 的第一消费者是导航，不是策略。只有当你的避障策略在训练时用了本体速度，才需要把 EKF 的 body_velocity 喂进 L4。

4.3 数据流

输入	来源	EKF 角色
accel, gyro	L1 IMU	预测
pose	Fast-LIO2 /Odometry	更新
quat（可选）	IMU 内部 AHRS	仅用于冷启动

完整滤波器在 include/robot_perception_nav/ekf_estimator.hpp。不要在这一层做点云处理。

5. L3 环境感知层：Fast-LIO2 + 障碍物检测

5.1 定义

L3 描述机体周围的空间。它拆成两个并列组件，而不是一个“感知大杂烩”：

1. Fast-LIO2： 激光惯性里程计与局部地图，给 L2 一个低漂移位姿，给可视化 / 全局规划一张注册点云。
2. 障碍物检测： 把当前帧点云（雷达优先，否则深度相机反投影）压成导航能用的几何——极坐标高度扫描 height_scan 和局部栅格 occupancy。

Fast-LIO2 是成熟开源系统，不要重写。本项目示例只通过 launch 启动它，并用 /Odometry 作为 EKF 观测。障碍物检测必须自己写，因为它要匹配你训练避障策略时的观测定义。

5.2 Fast-LIO2 的职责与接口

话题	方向	用途
/livox/lidar 或 /points	入	原始点云
/imu	入	与雷达时间对齐的 IMU
/Odometry	出	L2 位姿更新
/cloud_registered	出	可视化、可选的全局障碍

外参、IMU 噪声、雷达类型全部留在 Fast-LIO2 自己的 YAML，感知栈不要复制一份。

5.3 障碍物检测

避障策略最常见的局部观测是 机体系极坐标距离扫描：把身体周围 360° 分成 (N) 个 bin（示例用 72，即 5°），每个 bin 记录 ([z_{min}, z_{max}]) 高度带内最近障碍距离。这和 2D LaserScan 同构，方便可视化，也方便和训练时的 height map / scan 对齐。

传感器降级规则写在 L3，不写在 L4：

雷达	深度相机	L3 行为
存活	*	用雷达点云
死亡	存活	深度图反投影成点云，同一套 ObstacleDetector
死亡	死亡	停止刷新扫描；避障侧超时后输出零速

6. L4 导航决策层：VLN + 避障

6.1 定义

L4 是“想去哪”到“这一步速度”的转换层。它必须再拆两个模块：

• VLN： 语言指令 + 当前位姿（将来可加图像）→ 全局目标 /nav/goal 和粗速度 /nav/vln_cmd。
• 避障： 粗速度 + height_scan → 安全的 /cmd_vel。这里挂你已经训练好的策略。

不要让 VLN 直接发 /cmd_vel。否则语言模型一次幻觉就会让机体撞墙。避障是最后一道几何闸门。

6.2 VLN 职责

当前示例用可替换的解释器：地标表（door / table / charger）+ 相对指令（前进 2m、左转）。把 VlnNavigator::interpret() 换成 VLM / 导航大模型时，话题契约不变。这是分层的直接收益。
VLN 输出的粗速度由 goal_to_cmd() 做比例导引：距离大则给 (v_x)，航向误差大则降速加 (\omega_z)。它不考虑障碍。

6.3 避障职责

AvoidancePolicy 优先加载训练好的 ONNX；模型路径为空或加载失败时，退回极坐标势场，保证没模型也能在仿真里把链路跑通。
输入向量建议与训练时严格对齐：
$$x=[\mathrm{scan}/r_{\max },v_x^{\mathrm{vln}},v_y^{\mathrm{vln}},{\omega }^{\mathrm{vln}},\mathrm{extra}]$$
输出限幅后再发布，防止策略越界把 locomotion 指令打满。

7. L5 运动执行层：rl_sar 已有的闭环

7.1 定义

L5 就是现有 rl_sar。不要把它改成导航节点。它的职责是：在 RLFSMStateRLLocomotion 中加载策略，把 obs.commands 编进观测，50 Hz Forward()，200 Hz 把队列里的关节目标写成 PD 指令。
Go2 仿真里关键路径如下。
控制与推理分两个 LoopFunc：

    this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.Get<float>("dt"), std::bind(&RL_Sim::RobotControl, this));
    this->loop_rl = std::make_shared<LoopFunc>("loop_rl", this->params.Get<float>("dt") * this->params.Get<int>("decimation"), std::bind(&RL_Sim::RunModel, this));

navigation_mode 切换写在 SDK 里，键盘 N / 手柄 X：

    if (this->control.current_keyboard == Input::Keyboard::N || this->control.current_gamepad == Input::Gamepad::X)
    {
        this->control.navigation_mode = !this->control.navigation_mode;
    }

策略真正读 /cmd_vel 的地方：

        this->obs.commands[0] = this->control.x;
        this->obs.commands[1] = this->control.y;
        this->obs.commands[2] = this->control.yaw;
        if (this->control.navigation_mode)
        {
            this->obs.commands[0] = (float)this->cmd_vel.linear.x;
            this->obs.commands[1] = (float)this->cmd_vel.linear.y;
            this->obs.commands[2] = (float)this->cmd_vel.angular.z;
        }

Go2 策略配置 policy/go2/himloco/config.yaml 里 observations 为 commands, ang_vel, gravity_vec, dof_pos, dof_vel, actions，dt=0.005，decimation=4。因此把速度级的避障策略接到 /cmd_vel，L5 一行都不用改。

7.2 与上层的契约

项目	约定
话题	/cmd_vel，geometry_msgs/Twist
坐标系	机体系：linear.x 前进 / 后退，linear.y 左移 / 右移，angular.z 偏航
使能	机体站起进入 RLFSMStateRLLocomotion 后按 N
超时	上游避障应持续发布；rl_sar 当前不会因消息中断自动停车，这是原项目已知行为

8. 在 rl_sar 上扩展：设计选择

已有训练好的避障策略时，有两种接法。先看策略在训练时输出的是什么。

	Mode A：速度级避障	Mode B：观测级避障
策略输出	((v_x, v_y, \omega_z))	关节动作（与 locomotion 是同一个网络）
改 rl_sar	否	是，要加 height_scan 观测
与 VLN 组合	自然：VLN → 避障 → /cmd_vel	VLN 只能改 goal 项，不能单独换避障
推荐场景	先有 locomotion，再训局部避障 / DRL 导航	end-to-end 视觉运动策略

大多数“已经训好避障导航策略、还想加 VLN”的项目属于 Mode A。本文按 Mode A 给出完整示例代码；Mode B 的补丁放在 examples/perception_nav/rl_sar_height_scan_patch.hpp。
工程上建议把新代码做成独立 ROS2 包，而不是改 src/rl_sar/CMakeLists.txt。rl_sar 继续编译它自己的 rl_sim / rl_real_go2。新包通过话题协作。
包布局：

examples/perception_nav/robot_perception_nav/
├── CMakeLists.txt
├── package.xml
├── config/stack.yaml
├── launch/stack.launch.py
├── include/robot_perception_nav/
│   ├── types.hpp
│   ├── sensor_interface.hpp
│   ├── ekf_estimator.hpp
│   ├── obstacle_detector.hpp
│   ├── avoidance_policy.hpp
│   └── vln_navigator.hpp
└── src/
    ├── perception_node.cpp
    ├── vln_node.cpp
    └── avoidance_node.cpp

9. 结语

感知解决状态与环境，导航解决目标与安全速度，rl_sar 解决把速度变成能跑的步态。五层把这三件事按频率和故障域切开：

• L1 用插件消化传感器插拔；
• L2 用 EKF 给导航一条连续本体轨迹；
• L3 让 Fast-LIO2 管定位，让障碍物检测管局部几何；
• L4 让 VLN 管语义、让已训练避障策略管安全，只在 /cmd_vel 汇合；
• L5 继续用现成的 navigation_mode，尽量不改运动栈。

接口比算法更值钱。换雷达、换 VLM、换避障网络，只要话题契约不变，分层就没有白做。