SEA-Nav 深度解读：让四足机器人在密集障碍中“分钟级“学会安全敏捷穿行

0. 简介

四足机器人（Quadruped）从 ANYmal、Spot 到 Unitree Go2，已经能在草地、楼梯、户外石滩等复杂地形上完成稳健的步态控制。但只要把场景换成"室内堆满桌椅、纸箱、金属杆的杂乱房间"，传统流派立刻陷入两难：基于地图与规划（SLAM + A*）的方案需要先建图再规划，速度慢且对动态障碍极为脆弱；端到端的模仿学习虽然反应快，却在 OOD（分布外）场景下会出现灾难性失效，因为它从未在训练集里见过那种走廊宽度或那种随机散落的障碍组合。

强化学习（DRL）成为近年最被看好的路线，因为它原生支持反应式控制、能够把感知与决策一次性打通。但这里的关键是，密集障碍环境给 DRL 带来两个几乎相互矛盾的工程难题：第一，碰撞惩罚的权重几乎不可调——给得太大会让机器人变成"拒绝靠近障碍物的乌龟"，给得太小则碰撞频发；第二，长程任务的样本效率极低，机器人 95% 以上的探索时间都花在没有障碍物的空地上来回兜圈，真正"在 30 厘米窄缝里贴墙过弯"这种关键经验占比极少。结果就是一个看似简单的"穿过房间"任务，往往需要数小时甚至数天的训练，并依赖多阶段课程学习与多模块拼接，部署代价极高。

上图是论文展示场景：SEA-Nav 仅用分钟级训练即可完成策略学习，并以零样本（zero-shot）方式部署到一个完全未见过的迷宫中——而且使用的是 Unitree Go2 出厂自带的稀疏 LiDAR L1 与 OOD 的内置 MPC 控制器。这一组合在以往的论文里几乎都被认为"精度太低、不够用"，但 SEA-Nav 把它跑通了，这意味着它不只是纸面 SOTA，而是真正具备了即插即用的工程价值。项目主页 https://11chens.github.io/sea-nav/

---

1. 核心思路：经验筛选 + 可微安全先验 + 动作正则

1.1 三件套总览

SEA-Nav 的全称是 Efficient Policy Learning for Safe and Agile Quadruped Navigation in Cluttered Environments，它把训练效率、部署安全、敏捷机动这三件事第一次放在一个单阶段（single-stage）的 PPO 框架里同时解决。它的核心信念是：与其在奖励函数上反复调权重，不如把"物理安全"直接嵌入网络的最后一层，让梯度可以穿过这层先验回流到策略本体；与其让机器人在大片空地里随机游走，不如主动把它"反复扔回危险瞬间"重新练习。

具体来说，SEA-Nav 由三个相互咬合的部件组成：第一是 ACSI（Adaptive Collision-State Initialization），一个基于课程的"碰撞前关键状态回放"机制，用于解决密集障碍下的样本效率瓶颈；第二是 可微 LSE-CBF 安全投影层，把多束 LiDAR 约束用 Log-Sum-Exp 平滑融合、并通过物理阻尼消除窄道震荡；第三是 运动学动作正则损失，确保策略输出的速度命令始终落在硬件可执行的安全区间内。这意味着三者共同作用下，作者只用一张 RTX 4090 训练几十分钟，就拿到了可在真实环境零样本部署的策略。

类比：把 RL 训练想象成开车学倒库。ACSI 是"教练让你反复练习倒进死角的那两秒"；LSE-CBF 是"方向盘上的隐形限位器"；动作正则是"防止你猛踩油门让车失控"。三者缺一不可。

1.2 整体流水线

2. 网络结构：共享 Backbone + 双并联输出头

2.1 观测、Encoder 与共享 Backbone

要看懂 SEA-Nav 的实现，最好的入口是 training/rsl_rl/rsl_rl/modules/cbf_actor_critic.py 里的 DifferentiableSafeActorCritic。它的输入观测包括 12 维本体感受（角速度、投影重力、自身命令）、41 束 LiDAR 距离、2 维相对目标点，再加 10 帧历史，共 550 维左右。Encoder 把历史压缩到 16 维 latent，与当前帧观测拼接后送入 Backbone。Backbone 的最后一层输出会被两个 head 共享。

2.2 双头并联：导航命令 $\bar{u}$ 与安全增益 $\alpha$

一个 Navigation Head 输出三维速度命令 $\bar{u}=({\bar{v}}_x,{\bar{v}}_y,{\bar{\omega }}_z)$；另一个 Alpha Head 通过 Softplus 激活输出严格正的自适应增益 $\alpha$。直观理解是，这种"共享主干 + 双任务头"的结构在 SEA-Nav 里有非常具体的物理含义：网络在感知到危险时不仅要重新规划速度，还要同时调整安全增益，让 Shield 在窄道里更保守、在空地里更激进。

直觉理解：把 $\bar{u}$ 想象成"司机心里想踩的油门方向"，把 $\alpha$ 想象成"司机此刻对方向盘隐形限位器的信任程度"。$\alpha$ 越大，限位器越放松，策略主导；$\alpha$ 越小，限位器越紧，Shield 主导。

2.3 关键代码：双头推理 + 闭式 CBF 投影

# cbf_actor_critic.py 关键片段：双头并联推理 + 闭式 CBF 投影
shared_features = self.backbone(obs_cat)      # 共享深度特征
u_bar     = self.nav_head(shared_features)    # 标称速度命令
alpha_raw = self.alpha_head(shared_features)  # 安全增益原始 logit

rays_real = torch.exp2(rays)                  # 还原到 0.1~3.0 m 实际距离
alpha     = F.softplus(alpha_raw)             # 保证 α > 0
self.alpha = alpha

# 通过可微 LSE-CBF 安全层得到最终安全速度
u_s = self.cbf_layer(u_bar, rays_real, alpha)

# 同时缓存 u_bar 与 u_s，用于稍后计算 Shield 干预损失
self.u_bar = u_bar
self.u_s   = u_s

2.4 Critic 为什么绕开 Shield

这里要厘清的是，SEA-Nav 的 Critic 网络刻意 绕开 了 Shield。代码里 evaluate() 直接拿 obs_buf 与 latent 拼起来送进 critic MLP 估计状态价值。这是一个工程上很关键的设计：Critic 评估的是"未来折扣回报的期望"，它的输入应该反映环境真实状态而不是被安全层过滤后的指令；而 Actor 输出的 $u_s$ 则代表"将要在物理世界执行的动作"，它需要被安全层约束。两者解耦后，价值估计的偏差不会被安全投影放大，PPO 的优势函数估计也更稳定。

难点提示（Actor / Critic 不对称）：常见误区是认为 Actor-Critic 必须对称地共享所有层。SEA-Nav 故意让 Critic 看"未过滤的世界"，让 Actor 看"过滤后的动作"。这是一个不对称设计，但它对应着"价值学世界、动作学安全"的物理含义。

---

3. ACSI：把碰撞瞬间反复回放

3.1 为什么需要 ACSI

如果说网络结构是 SEA-Nav 的骨骼，那么 ACSI 就是它的"训练加速器"。传统 RL 训练里只要碰撞就直接 reset 到初始位置，机器人下一帧又从远离障碍的地方重新走，导致它一辈子都很少见到"距墙 5 厘米、速度 1.2 m/s"这种极限场景，自然学不会精细避障。这意味着传统训练的样本结构是"线性时间换线性见识"，每一步训练边际收益极低。

直觉理解：好比驾校教练只让你练直线行驶，碰一次就把你拉回起点重新开始。你永远学不会"侧方停车快撞墙时怎么打方向盘"，因为你根本没机会反复接触那个瞬间。

3.2 ACSI 的两步操作

ACSI 的做法非常直接：每个环境维护一个长度 100 步（约 2 秒）的 root state、dof_pos、dof_vel 滚动缓冲；一旦发生碰撞，以一定概率把机器人状态回滚到碰撞前 40~150 步的某一帧，让它带着真实位姿与速度从那一刻"重新尝试"。整个过程不需要复制网络权重、不需要切换 worker，只是简单覆写 sim 状态张量，工程开销几乎可以忽略。

3.3 与成功率挂钩的隐式课程

ACSI 还做了一件聪明的事：它把回滚概率与"目标到达成功率"挂钩，形成一个隐式课程。训练初期成功率低，回滚概率小，机器人优先学会向目标移动；训练中后期成功率上升，回滚概率随之放大，机器人开始集中练习那些"差一点就失败"的关键瞬间。论文式 (1) 给出概率更新规则：

$$P_{reset}=P_{min}+(P_{max}-P_{min})\cdot \mathrm{clip}(L_{goal},0,1)$$

课程指标 $L_{goal}$ 不是凭空设的旋钮，而是由实际训练表现自动驱动，每个 episode 结束时根据机器人离目标的最终距离对它做加减，无需人工调度，也避免了"中期超参漂移"，让训练曲线天然平滑。其更新公式很简单：

$$L_{goal}\leftarrow L_{goal}+1[d<d_{up}]-1[d>d_{down}]$$

换句话说，ACSI 让最稀缺的高价值样本随着训练进度自动成为最常见的样本，这是普通 PER（Prioritized Experience Replay）做不到的，因为 PER 只是改采样权重，而 ACSI 直接改环境的状态分布。代码里 $P_{min}=0.1$、$P_{max}=0.5$、$d_{up}=0.5$ m、$d_{down}=2.0$ m，这套阈值是从消融里反复回推的经验值。

难点提示（PER vs ACSI 的本质区别）：很多读者第一反应是"这不就是 prioritized replay 吗？“——其实有本质差异。PER 只能在已经采集到的样本里调权，遇不到极限场景就永远调不出来；ACSI 直接修改环境状态分布，让仿真器主动制造极限场景。前者是"挑选已有素材”，后者是"主动拍摄新素材"。

# legged_robot_pos.py 关键片段：碰撞回放重置
def _reset_collision_replay(self, env_ids):
    undo_range = getattr(self.cfg.replay, 'undo_steps_range', [40, 80])
    undo_steps = torch.randint(undo_range[0], undo_range[1],
                               (len(env_ids),), device=self.device)
    # 不能比 episode 长度还长，也不能超出 replay 缓冲容量
    undo_steps = torch.min(undo_steps.long(),
                           self.episode_length_buf[env_ids].long())
    undo_steps = torch.min(undo_steps,
                           torch.tensor(self.replay_len - 1, device=self.device))
    # 历史不够 20 步则回退到普通 reset
    valid_replay = undo_steps > 20
    replay_ids   = env_ids[valid_replay]
    indices      = -undo_steps[valid_replay]
    # 从缓冲读出 root / dof 状态，写回 sim
    self.root_states[replay_ids] = self.replay_root_states[replay_ids, indices]
    self.dof_pos[replay_ids]     = self.replay_dof_pos[replay_ids, indices]
    self.dof_vel[replay_ids]     = self.replay_dof_vel[replay_ids, indices]

代码里这部分课程更新逻辑放在 _update_terrain_curriculum，用 move_up = distance < 0.5 与 move_down = distance > 2.0 的硬阈值决定 goal level 的涨跌，简单但有效。值得强调的一点是：即便最激进时也只有一半的碰撞会触发回放，剩余一半仍然走常规重置——这是为了避免机器人沉迷于"刷碰撞经验"而忘记长程目标。

4. LSE-CBF：让 41 束 LiDAR 约束变成可微安全屏障

4.1 为什么 min 操作不能用

控制屏障函数（CBF）是控制理论里把"安全集合的前向不变性"翻译成线性约束的经典工具，最早在 ACC 论文里被系统化。核心问题在于多约束情形下，单束 LiDAR 给一条不等式还好处理，可一旦同时考虑 41 束射线，约束之间会出现"哪条主导"的歧义问题。最朴素的写法是：

$$h(x)=\underset{i}{\min }{h}_i(x),h_i(x)={\rho }_i-d_{safe}$$

其中 ${\rho }_i$ 是第 $i$ 束 LiDAR 距离，$d_{safe}$ 是安全半径。这个 $\min$ 在切换点不可导，会让窄道里的机器人出现"左右拍球"式的方向震荡——比如从 0.50 m 左墙切换到 0.51 m 右墙时，主导约束的梯度方向会瞬间翻转 180°，于是策略会持续在两个方向之间来回拉锯。

直觉理解：如果你的方向盘只听"离你最近的那堵墙"，左右两面墙距离差一厘米的瞬间方向盘就会被反向打满，这就是所谓 ping-pong 震荡的本质。

4.2 LSE 平滑融合

SEA-Nav 在这里用了 Log-Sum-Exp 光滑近似，把 41 条独立的安全余量融合成一个处处可微的全局函数，让梯度不再只跟随一条主导射线，而是按指数权重综合 41 条射线的方向，从而消除了切换点的方向跳变，这个技巧在 ACC 与凸优化文献里被反复使用：

$$h(x)=-\frac{1}{k}\ln \left(\sum _{i=1}^N\exp (-k\cdot h_i(x))\right)$$

LSE 的数学性质是 $k\to \infty$ 时趋近 $\min$，但在 $k$ 有限时处处可微，且每束射线都按 $e^{-k\cdot h_i}$ 比例贡献梯度——离障碍越近的射线占主导，但其他射线不会突然失效，这从源头上消除了切换点的梯度突跳。论文与代码里默认 $k=10$，这个数值是作者扫超参得到的"既够锐利又够平滑"的折中。

4.3 物理阻尼项 ${ϵ}_d$ 解决奇异性

仅有 LSE 还不够。在完全对称的窄走廊里，左右两面墙的梯度大小几乎相等且方向相反，融合后净梯度趋于零，CBF 闭式解的分母 $\Vert L_{g}h(x){\Vert }^2$ 接近 0，校正量直接爆炸。SEA-Nav 通过加入一个物理阻尼项 ${ϵ}_d$ 来限幅，论文式 (4) 展示的就是这个修正后的封闭形式：

$$u_s(x)=\bar{u}(x)+\underset{\eta \text{校正幅度}}{\underbrace{\max \left\{0,\frac{-(⟨L_{g}h(x),\bar{u}(x)⟩+\alpha \cdot h(x))}{\Vert L_{g}h(x){\Vert }^2+{ϵ}_d}\right\}}}\cdot \underset{\text{校正方向}}{\underbrace{L_{g}h(x)}}$$

它把 QP 退化成一行向量代数，免去了任何外部 QP 求解器，自然也就保留了端到端的可微性。这个细节看上去很小，但它是 SEA-Nav 训练时间从小时级压到分钟级的关键工程决策，也是论文最值得复用的工程片段，可以直接搬到其他需要安全反馈的 RL 任务里。

难点提示（为什么不用 QPsolver？）：传统 CBF-QP 必须每步调用 OSQP/cvxpy，反向传播要走 implicit differentiation，慢且不稳定。SEA-Nav 把约束限制为单个不等式后，KKT 条件可解析求解，闭式解里只有矩阵乘法和 ReLU，PyTorch 自动微分天然支持。这是 LSE-CBF 比 ABS / OCR 训练快一个量级的工程根因。

# cbf_lse_layer.py：LSE-CBF 闭式安全投影
h_i = lidar_dists - self.d_safe                # 每束射线的安全余量
min_h, _ = torch.min(h_i, dim=1, keepdim=True)
h_comp = min_h - (1.0 / self.kappa) * torch.log(
    torch.sum(torch.exp(-self.kappa * (h_i - min_h)), dim=1, keepdim=True)
)                                              # LSE 光滑融合后的全局 h
lambda_i = torch.exp(-self.kappa * (h_i - h_comp)).unsqueeze(-1)
n_vecs   = self.ray_unit_vectors.unsqueeze(0)  # 各射线方向单位向量
Lg_h     = -torch.sum(lambda_i * n_vecs, dim=1)         # Lie 导数
Lgh_u    = torch.sum(Lg_h * u_2d, dim=1, keepdim=True)
Lgh_norm_sq = torch.sum(Lg_h ** 2, dim=1, keepdim=True)
# 加入物理阻尼，防止窄道里梯度归零导致的速度爆炸
eta   = -(Lgh_u + alpha * h_comp) / (Lgh_norm_sq + self.damping_factor)
u_s_2d = u_2d + F.relu(eta) * Lg_h             # ReLU 起 max{0,·} 开关作用

整段代码不到 30 行，却完成了三件事：第一，把所有 LiDAR 约束聚合成一个全局可微 $h$；第二，给出闭式校正量 $\eta$，避免在线 QP；第三，通过 F.relu(eta) 让安全动作保持原状，不安全动作沿 $L_{g}h$ 方向被推回安全集合，简洁到几乎可以贴在 PPT 上。

4.4 把 Shield 当 Inductive Bias，而不是硬约束

值得强调的是：作者并不声称 LSE-CBF 在离散时间下能保证零碰撞，他们把它定位成 “differentiable inductive bias”（可微归纳偏置）。换言之，CBF 在这里的角色不是事后过滤的硬约束，而是一种几何形状的先验，它通过梯度告诉策略网络"哪些动作方向是物理上更安全的"，让策略自己学会在空旷区域增大 $\alpha$ 以便敏捷加速、在窄道处减小 $\alpha$ 以便保守通过。这种用 Shield 当 inductive bias 的思路，是 SEA-Nav 区别于 ABS、OCR、REASAN 等方法的关键。

5. 损失函数：把所有目标写在一个反向里

5.1 总损失装配

SEA-Nav 在 PPO 标准的 surrogate loss 与 value loss 之外，叠加了三种辅助损失，每一项都对应一个具体工程问题，分别管"安全反馈"、“硬件可执行”、“轨迹平滑”，缺一项都会让真机部署崩盘，这也是这套方法最值得抄作业的部分。最终总损失即论文式 (8)：

$$L_{total}=L_{PPO}+{\lambda }_{shield}{L}_{shield}+{\lambda }_{reg}{L}_{reg}$$

默认权重为 ${\lambda }_{shield}=0.1$、${\lambda }_{reg}=1.0$。这两个权重数量级相差一个量级，是作者通过消融反复扫出来的"既不让 Shield 反客为主、又不让正则被忽略"的折中，调动这两个旋钮也是后续工作复现 SEA-Nav 时最容易踩坑的地方，不妨先固定再改。下面逐项展开它们的物理含义。

5.2 Shield 干预损失 $L_{shield}$

第一项是 Shield 干预损失，它的设计直觉是"让策略主动学会输出 Shield 不需要修改的命令"，从而把硬约束逐渐内化为策略自身的偏好，让 Shield 在训练后期成为只在偶发风险下触发的"备份保险"，这种"由外而内"的安全注入是这个损失项最大的工程价值，惩罚 $\bar{u}$ 与 $u_s$ 的差异，并对过小的 $\alpha$ 给一个二次惩罚：

$$L_{shield}=\Vert u_{s,t}-{\bar{u}}_t{\Vert }^2+[{\alpha }_{min}-{\alpha }_t{]}_{+}^2$$

其中 [ x ] + = max ⁡ { 0 , x } [x]_+ = \max\{0, x\} [x]+​=max{0,x} 等价于一个 ReLU，${\alpha }_{min}$ 是自适应增益的硬下界。这相当于告诉网络："既然 Shield 一直把你拉回来，那你不如自己学着输出更安全的 $\bar{u}$；否则我就让 $\alpha$ 变大让你少干预。"这样 Shield 与策略形成正反馈，训练越久干预越少。

直觉理解：把 $L_{shield}$ 想象成"教练罚款机制"——你每次让教练动方向盘一次罚 100 元，你就会主动学着自己提前打方向。

5.3 动作范围损失 $L_{range}$

第二项是 动作范围损失，它的设计动机是把"硬件能跑出来什么"显式注入策略学习，避免训练时输出在仿真里看似合理、上真机却变成飞刀，从而保护底层 SLR 控制器不会饱和，让 Sim-to-Real 一次过关，把超出 Go2 真机硬件能力的部分用平方距离惩罚：

L r a n g e = ∑ j ∈ { x , y , ω } ( u s , t j − c l i p ( u s , t j ,   u j min ⁡ ,   u j max ⁡ ) ) 2 L_{range} = \sum_{j \in \{x, y, \omega\}} \left( u_{s,t}^{j} - \mathrm{clip}(u_{s,t}^{j},\, u_j^{\min},\, u_j^{\max}) \right)^2 Lrange​=j∈{x,y,ω}∑​(us,tj​−clip(us,tj​,ujmin​,ujmax​))2

代码里取 $u^{\min }=(-0.5,-0.8,-1.0)$、$u^{\max }=(1.7,0.8,1.0)$，单位均为对应通道的 m/s 或 rad/s。这一项决定了策略最终能不能跑在 Go2 上，是真机部署能否一次过的关键守门员，超过这个范围 SLR 控制器会直接饱和、电机也容易过热保护。

5.4 Lipschitz 平滑损失 $L_{smooth}$

…详情请参照古月居

转载自 CSDN-专业IT技术社区

原文链接：https://blog.csdn.net/lovely_yoshino/article/details/162196949