在人群中导航：嵌入社会力动力学的非线性MPC人机感知机器人导航

论文：Navigating the Crowd: Non-linear MPC with Social Forces Dynamics for Human-Aware Robot Navigation
作者：Stefano Trepella、Andrea Ostuni、Mauro Martini、Pablo Pueyo、Noé Pérez-Higueras、Marcello Chiaberge、Fernando Caballero、Luis Merino
收录：IROS 2026
论文链接：arXiv:2607.10374
代码：PIC4SeR/sfm-nmpc

一句话总结

SFM-NMPC 把社会力模型嵌进非线性模型预测控制的预测方程和代价函数，使机器人在 20 Hz 控制频率下同时预测自身与行人的未来轨迹，并以更小的社会做功和更近的舒适距离完成人群导航。

研究背景：为什么“避碰”不等于“会走人堆”

在商场、医院、机场和室内服务场景中，机器人面对的不只是静态障碍物。人会对机器人的路径作出反应，机器人也会改变人的行走意图；如果控制器只把人当作圆柱体来避碰，往往会出现贴身通过、突然刹车、逼人绕路等让人不适的行为。论文要解决的问题因此不是单次碰撞检测，而是短时未来中机器人与多个人之间的耦合演化。

传统局部规划器通常在路径跟踪、代价地图和速度采样之间折衷。它们可以保证运动学可行，却很难显式表达“这个人正在加速”“机器人正面朝人走”“横穿时应从人后方通过”这类社会几何。结果是把社会性压缩成一个距离惩罚，控制器只能被动退让，而不能预测交互的演化。

学习式方法给出了另一种路线：用强化学习或轨迹预测网络从人群数据中学习策略。问题在于，这类方法往往需要大量数据，行为解释性弱，安全约束也不容易像模型式控制那样直接加入优化问题。作者特别强调，在安全关键场景中，控制器的可解释性和显式约束仍是不可替代的属性。

模型预测控制看起来处在两者之间：它有有限时域预测、显式代价和速度约束，但传统 MPC 通常把行人轨迹当作外部输入，先预测人再规划机器人。这样的单向流程忽略了机器人自身动作对人的影响。SFM-NMPC 的关键改动是把社会力模型放进 MPC 内部，让每个候选机器人轨迹同时产生一套行人响应轨迹。

这不是简单增加一个“人距离惩罚”。论文的贡献包括三点：第一，把 SFM 作为智能体动力学嵌入 NMPC；第二，设计近距、社会做功、朝向和横穿四个社会代价项；第三，用 Ceres 非线性最小二乘求解，保持 20 Hz 实时运行。整体实验覆盖开放人群、混合场景和消融研究，说明这些模块确实各自有贡献。

方法基础：单轮预测里发生了什么

控制器输入全局路径、局部代价地图、当前机器人位姿、上一轮控制序列和被感知到的人群状态。每一轮先用调节纯跟踪算法从全局路径裁剪出局部参考轨迹，再以历史最优控制作为热启动。优化变量不是完整轨迹点，而是未来速度块，因此维度被控制在实时求解可承受的范围内。

机器人采用单自行车运动学。设 $\mathbf{p}_i^R \in SE(2)$ 表示第 $i$ 个预测时刻的机器人位姿，$\mathbf{u}_i=[v_i,\omega_i]^\top$ 是线速度和角速度，$\Delta t_s$ 为预测步长。机器人状态传播为：

$$\mathbf{p}_{i+1}^{R}=\mathbf{p}_{i}^{R}+\mathbf{G}(\theta_i)\mathbf{u}_i\Delta t_s$$

其中 $\mathbf{G}(\theta_i)$ 把机体速度映射到世界坐标。论文实验使用 $T_p=2\,\mathrm{s}$、$\Delta t_s=0.1\,\mathrm{s}$，对应 20 个预测点。代码仓库进一步引入控制时域和参数块长度：控制时域内的速度可变，超出后保持最后命令；每个参数块连续若个时间步共享同一组 $v,\omega$，以减少优化变量数量。

对第 $k$ 个行人，控制器不只沿直线外推其速度，而是用社会力更新它的状态。行人速度的变化由社会排斥力和保持自身运动趋势的驱动力共同决定：

$$\mathbf{v}_{i+1}^{A_k}=\mathbf{v}_i^{A_k}+\left(\mathbf{F}_{soc}(\mathbf{p}_i^R,\mathbf{p}_i^{A_k})+\mathbf{F}_{drive}(\mathbf{v}_i^{A_k})\right)\Delta t_s$$

关键在 $\mathbf{F}_{soc}$ 的参数：它依赖当前预测时刻的机器人位姿 $\mathbf{p}_i^R$ 和行人位姿 $\mathbf{p}_i^{A_k}$。也就是说，候选速度序列改变机器人未来位置，未来位置又改变行人受到的社会力，进而改变行人的未来位置。闭环就此形成。

论文采用考虑相对速度的排斥力形式：

$$\mathbf{F}_{soc}=A_{soc}(f_v\mathbf{n}_D+f_\theta\mathbf{n}_{\perp})$$

这里 $\mathbf{n}_D$ 来自交互向量 $\mathbf{D}=\lambda(\mathbf{v}_i^R-\mathbf{v}_i^{A_k})+\mathbf{n}_{ra}$，$\mathbf{n}_{\perp}$ 是其垂直方向。标量 $f_v$ 与 $f_\theta$ 分别对应纵向减速和横向转向，随机器人—行人距离和角度按指数衰减。行人位置随后用半隐式欧拉积分：

$$\mathbf{p}_{i+1}^{A_k}=\mathbf{p}_i^{A_k}+\mathbf{v}_{i+1}^{A_k}\Delta t_s$$

flowchart LR
  A["全局路径
局部代价地图
人群状态"] --> B["调节纯跟踪
生成局部参考与热启动"]
  B --> C["速度块优化变量
v, omega"]
  C --> D["单自行车传播
机器人未来位姿"]
  D --> E["社会力共同演化
行人未来状态"]
  E --> F["代价项
障碍 / 目标 / 平滑 / 社会"]
  F --> G["Ceres 非线性最小二乘"]
  G --> H["执行首步控制
缓存剩余序列"]
  H --> B

代价函数：把“舒适”拆成可优化的几何量

所有预测步的代价被组织为一个非线性最小二quares问题。若 $\chi$ 表示待优化的速度序列，目标为：

$$\chi^*=\min_\chi J(\chi):=J_{\mathrm{obs}}+J_{\mathrm{goal}}+J_{\mathrm{dyn}}+J_{\mathrm{soc}}$$

其中每个部分再由加权残差组成。障碍项通过局部代价地图的 Ceres 双三次插值给出平滑梯度；目标项包含路径跟随、路径对齐、终点朝向和终点吸引；动力学项约束期望速度和相邻控制的变化。社会项则由四个更具体的惩罚组成。

第一个社会项是近距惩罚，它直接保护个人空间：

$$J_{\rho}=\alpha_{\rho}\sum_{i=1}^{N_p}\sum_{k=1}^{\mathcal{N_A}}\beta\cdot\exp\left(-\frac{\|\mathbf{p}_{xy,i}^{R}-\mathbf{p}_{xy,i}^{A_k}\|^2}{d_0^2}\right)$$

$d_0$ 是特征近距，$\beta$ 是缩放因子。指数形式保证远离时影响快速衰减，同时所有可见行人都参与求和，而不是只取最近者。这对 Ceres 自动微分也更友好。

第二个是社会做功惩罚：

$$J_{\mathrm{work}}=\alpha_{\mathrm{work}}\sum_{i=1}^{N_p}\left(\sum_{k=1}^{\mathcal{N_A}}\|\mathbf{F}_{robot\to agent_k}\|^2\right)$$

该项关注机器人对行人施加的社会力。若机器人逼近、逼迫行人减速或变向，社会力范数会上升。它弥补了纯距离项的不足：同样距离下，正面逼近和并行经过造成的社会扰动并不相同。

第三和第四个是朝向社会代价。$J_{s\theta long}$ 惩罚机器人朝向行人或与行人同向行驶；$J_{s\theta cross}$ 专门处理近似垂直的横穿场景。后者包含速度项和转向项：

$$J_{s\theta cross}=\alpha_{cross}\sum_{i=1}^{N_p}e^{-d^2/d_s^2}\left(v_i\sin^2(\theta_i-\psi_{k^*})+\alpha_{bear}\operatorname{sp}(c\omega_i s)\sin^2(\theta_i-\psi_{k^*})\right)$$

其中 $\sin^2(\Delta\theta)$ 在 90° 横穿时最大，同向或对向时趋近于零；$c=\mathbf{n}_{ra}\times\mathbf{n}_\psi$ 判断行人来自左侧或右侧，进而惩罚从人前方横穿的转向方向。Softplus $\operatorname{sp}$ 提供平滑激活，避免硬阈值造成梯度断裂。

代码落地：论文公式对应到 ROS 2 实现

官方仓库实现为 Nav2 controller 插件，核心求解器是 Ceres。仓库中的 include/sfm_nmpc/update_state.hpp 提供 computeSFMState：它在同一个循环中传播机器人单自行车状态，并为每个有效行人计算社会力与驱动力，再更新行人速度、位置和朝向。这正对应论文第 (3) 式和第 (5) 式，也是“机器人轨迹影响行人预测”的实现位置。

另一个直接对应在 src/optimizer.cpp。该文件把路径、障碍、速度和社会代价逐时间步加入 Ceres Problem，并根据控制时域和参数块长度决定残差连接到哪些速度变量。配置文件 params/params.yaml 暴露控制时域、参数块长度和各 critic 权重，使论文的加权目标可复现和可调。

四个社会 critic 分别位于 src/critics/ 与对应头文件中：SocialWorkCost 计算 SFM 力，ProxemicsCost 实现指数近距惩罚，AgentAngleCost 处理朝向对齐，CrossingCost 用横穿强度和方向惩罚从行人前方穿行。仓库文档 docs/mpc_critics_methodology.md 还列出了完整残差公式，方便读者把论文数学和 C++ 自动微分代码逐项对照。

实验设置：六个基线、两个地图、五个指标

作者将所有方法接入 Nav2，统一使用全局 A* 路径、代价地图和 20 Hz 控制频率。基线包括带高斯社会代价图的 MPPI 与 DWA、ORCA、SARL、纯 SFM，以及去掉 SFM 和社会代价的基础 NMPC。地图分为开放空间和混合空间，指标包括成功率、路径长度、到达时间、每步社会做功 $SW_{step}$ 和平均最近距离 AMD。

开放空间里，SARL 的社会做功最低，但路径长度和时间明显变差；论文在轨迹分析中指出它伴随振荡和不确定行为。SFM-NMPC 社会做功略高于 SARL，却保持 100% 成功率和最远平均最近距离，且路径与时间仍接近高效基线。这说明它的收益不是简单“慢下来”换来的。

总体结果更能说明稳健性：SFM-NMPC 成功率 98%，比 MPPI-GSC 高 4 个百分点，比基础 NMPC 高 11 个百分点；社会做功从 12.27 降到 10.70，平均最近距离从 1.10 升到 1.24。代价是路径长 0.48 m、时间多 1.93 s，属于可接受的社会性折衷。

论文还展示了不同近距区域的侵入时间比例。SFM-NMPC 在更靠近个人空间外缘的位置通过，而不是贴到人身边；这解释了 AMD 改善与社交做功降低为何能同时出现。作者用 Tukey HSD 检验比较分布，报告了显著差异，避免只凭均值判断。

消融实验从基础 NMPC 开始，逐步加入近距项、社会做功项和朝向社会项。基础 NMPC 路径最短、时间最快，但社会指标最差；加入近距和社会做功后，$SW_{step}$ 与 AMD 逐步改善；再加入朝向项后，社会指标的中位数和方差显著下降。这个顺序说明公式中的社会动力学与社会代价是互补关系，单独任何一个都不够。

局限与批判性解读

第一，实验仍然限于仿真。作者明确把真机验证列为未来工作。仿真中的行人检测、跟踪延迟、遮挡和噪声被理想化；而真实感知误差会让 $\mathbf{p}^{A_k}$ 与 $\mathbf{v}^{A_k}$ 不再准确，SFM 预测误差会直接进入优化梯度。论文提到未来将把人和障碍的感知不确定性作为噪声加入优化，但这尚未完成。

第二，人类主观舒适度没有被直接测量。$SW_{step}$、AMD 和近距侵入比例是物理代理指标，能解释安全距离和干扰强度，却不能完全等价于人的感知、信任或接受度。作者也承认需要真机环境中的人类评价协议。

第三，SFM 是简化行为模型。它假设行人以排斥力和驱动力响应，难以覆盖群组关系、视野遮挡、社会规范、目标变化和主动让行策略。对高密度或非规则人群，预测误差可能放大。未来提出的机器人—人轨迹协同优化是合理方向，但也引入了博弈论式建模和求解复杂度。

第四，实验代价来自固定参数与场景。虽然开源参数让结果可复现，但权重、控制时域、行人数量上限和地图类型的选择仍可能影响结论。真实部署需要系统的参数敏感性分析和在线适应机制，否则一次配置未必跨场景稳健。

总结

SFM-NMPC 的价值不在于发明全新的运动学模型，而在于把一个经过长期验证的人群行为模型放进 MPC 的内部循环，让“人对机器人的反应”变成优化问题的状态方程，而不是外部常数。社会做功、近距、朝向和横穿代价把舒适度拆成可微几何量，使 Ceres 能直接对社会行为求梯度。

从控制论视角看，这相当于把交互从“开环预测”改成“闭环预测”。传统方法先预测人，再规划机器人；SFM-NMPC 则在评估每个候选速度序列时重新计算人的响应。预测时域只有两秒，看似很短，却足够覆盖一次横穿、让行或减速协商。论文的结果表明，在这个时间尺度上显式建模交互，比单纯加大距离惩罚更有效。

实现层面也有一个容易被忽略的取舍。作者没有把每个未来时刻都设为独立自由变量，而是使用速度块：一段控制块内共享同一组 $v,\omega$，控制时域之外保持最后命令。这降低了问题维度，也让相邻时刻的控制天然平滑。代价是轨迹表达能力略受限，但 20 Hz 的实时结果说明该折衷合理。

社会代价的粒度同样值得借鉴。距离项解决“不能太近”，社会做功解决“不能逼迫人改变运动”，朝向项解决“不能正对着人走”，横穿项解决“不能从人前方抢行”。四者覆盖了社会导航中最常见的失败模式，而且都能从几何量导出梯度。相比一个笼统的舒适度评分，这种分解更容易调试和迁移到其他机器人平台。

总体实验说明，这种方法在社会指标和鲁棒成功率上取得明显优势，同时没有牺牲过多导航效率。更重要的是消融结果：每个社会项都有可观察的贡献，尤其是朝向代价显著改善分布的中位数和方差。这使论文不只是端到端排行榜胜出，而是提供了可解释的机制证据。

不过，结果也应按场景解读。开放空间里行人分布较规则，SFM 的力学假设较贴近；混合场景包含静态结构、窄通道和更复杂的流动，控制器必须同时处理代价地图约束与社会目标。总体 98% 成功率说明它在两类环境中都稳，但 10.35 m 路径长度和 18.52 s 到达时间显示安全余量来自适度绕行。部署者需要根据任务时间预算判断这 1–2 秒是否可接受。

另一个实用观察是评价指标的分离。路径长度和时间衡量效率，社会做功和平均最近距离衡量对人的影响，成功率衡量任务完成。若只看单一指标，SARL 的低社会做功可能被误认为最佳；结合路径、时间和轨迹形态后，才能看到它以振荡和低效率为代价。这个案例提醒我们，社会导航评价必须多维，而不是围绕一个标量排名。

对于想复现实验的读者，官方仓库的价值不只是发布源码，还在于配置与文档的对应关系。论文公式、Ceres 残差、YAML 权重和 ROS 2 插件接口可以逐层对照。参数文件中的控制时域、块长度和 critic 开关使消融实验可以直接扩展到新的代价项，而不必重写控制器。

落地时应把它看作一个强基线而非完整产品。真实机器人还需要低延迟行人跟踪、不确定性传播、人群行为验证和参数自适应。幸运的是，官方 ROS 2/Nav2 插件结构让这些扩展可以逐模块替换和测试。

真正的社会导航不是等人让路，而是在机器人选择速度的那一刻，已经把人的下一步反应算进未来。