当前位置：首页 > ds >正文

运动规划实战案例 | 基于行人社交模型的移动机器人动态避障(附ROS C++仿真)

ds 2025/9/2 5:44:34

1 社会规范化导航

区别于普通的动态障碍，行人的行为及其社交空间内包含丰富的社交信号，例如人际关系、潜在意图等。在人机共存环境中的导航不能仅限于安全和效率，出于对人类更高层次的社交需求的考虑，机器人需要具备更完善的社会属性。如图所示，社会规范化导航(Social-aware Navigation)提出了

自然性(Naturalness)
舒适性(Comfort)
社会性(Sociability)

三个主要目标。，使机器人的行为更符合人类逻辑，提升服务水平。社会规范的融入，不仅对改善规划性能很重要，也有助于提高人机交互的信任感。

在这里插入图片描述

本文聚焦于社会规范化导航中的舒适性指标，建模移动机器人的动态避障策略

2 行人社交距离模型

如图所示为基于社会工程学和心理学提出的距离空间模型，主要包括以人为中心的四个层次：

私密空间：通常用于如家庭成员等最亲密的关系中，在这一距离内，人的肢体接触和非言语交流较为频繁；
个人空间：通常用于熟悉的朋友之间；
社交空间：通常用于较为正式的场合，如商业谈判或与不太熟悉的人交往等，保持一定的社交距离有助于体现尊重和职业性；
公共空间：通常用于公众场合，如演讲、讲座等，适合大规模的交流和信息传播。

对距离空间的建模，是理解和尊重他人社交需求的重要一环。距离空间的形状通常是不对称的，且不同文化和社会背景中的人们对于这些空间距离的理解和使用会有所不同。例如，在西方文化中，人们通常倾向于使用较大的个人空间，而在一些亚洲文化中，人与人之间的空间距离可能较小。此外，个人空间的大小还会根据行走速度以及其他因素发生变化

在这里插入图片描述

为了更灵活地表示行人社交空间中的不对称性，通过拼接两个二维高斯函数的半曲面来进行建模。具体而言，设以第 $i$ 个行人中心 $(xih,yih)\left( x_{i}^{h},y_{i}^{h} \right)$ 为坐标原点，其正方向 $θih\theta _{i}^{h}$ 为 $x$ 轴的局部坐标系的权重分布为

$Ni(x,y)={N(x,y),∣vih∣=0Nifront(x,y),∣vih∣>0andx⩾0Niback(x,y),∣vih∣>0andx<0\mathcal{N} _i\left( x,y \right) =\begin{cases} \mathcal{N} \left( x,y \right) \,\, , \left| v_{i}^{h} \right|=0\\ \mathcal{N} _{i}^{\mathrm{front}}\left( x,y \right) , \left| v_{i}^{h} \right|>0 \mathrm{and} x\geqslant 0\\ \mathcal{N} _{i}^{\mathrm{back}}\left( x,y \right) , \left| v_{i}^{h} \right|>0 \mathrm{and} x<0\\\end{cases}$

其中速度绝对值为零，即行人静止时为标准二维正态分布。行人运动时，其前向权重分布为

$Nifront(x,y)=Ahexp⁡(−((dcos⁡(θ−θih)2σ0)2+(dsin⁡(θ−θih)2vih⋅σ0)2))\mathcal{N} _{i}^{\mathrm{front}}\left( x,y \right) =A^h\exp \left( -\left( \left( \frac{d\cos \left( \theta -\theta _{i}^{h} \right)}{\sqrt{2}\sigma _0} \right) ^2+\left( \frac{d\sin \left( \theta -\theta _{i}^{h} \right)}{\sqrt{2}v_{i}^{h}\cdot \sigma _0} \right) ^2 \right) \right)$

后向权重分布为

$Niback(x,y)=Ahexp⁡(−((vih⋅dcos⁡(θ−θih)2σ0)2+(dsin⁡(θ−θih)2σ0)2))\mathcal{N} _{i}^{\mathrm{back}}\left( x,y \right) =A^h\exp \left( -\left( \left( \frac{v_{i}^{h}\cdot d\cos \left( \theta -\theta _{i}^{h} \right)}{\sqrt{2}\sigma _0} \right) ^2+\left( \frac{d\sin \left( \theta -\theta _{i}^{h} \right)}{\sqrt{2}\sigma _0} \right) ^2 \right) \right)$

这里 $A^h$ 为行人中心权重峰值； $σ0\sigma_0$ 是标准差； $v_i^h$ 是行人速度；中心距离 $d=(x−xih)2+(y−yih)2d=\sqrt{\left( x-x_{i}^{h} \right) ^2+\left( y-y_{i}^{h} \right) ^2}$ ；中心角 $θ=atan⁡2(y−yih,x−xih)\theta =\mathrm{a}\tan 2\left( y-y_{i}^{h},x-x_{i}^{h} \right)$ 。如图所示为该距离空间模型的可视化，由于人类的前进方向包含视觉信号等更丰富的特征，因此设置比后向范围更广的分布。

在这里插入图片描述

如下图所示为多个行人场景下的社会性成本地图

在这里插入图片描述

3 算法仿真

3.1 社会性成本地图建模

核心算法如下所示：

const double direction = speed > 1e-3 ? std::atan2(person.vy(), person.vx()) : 0.0;
double rel_x = dx * std::cos(direction) + dy * std::sin(direction);
double rel_y = -dx * std::sin(direction) + dy * std::cos(direction);if (rel_x >= 0)
{double ellipse_value =(rel_x * rel_x) / (ellipse_major * ellipse_major) + (rel_y * rel_y) / (ellipse_minor * ellipse_minor);if (ellipse_value <= 1.0){return (1.0 - ellipse_value) * costmap_2d::LETHAL_OBSTACLE;}else{return 0.0;}
}
else
{double distance = std::sqrt(rel_x * rel_x + rel_y * rel_y);if (distance <= circle_radius_){return (1.0 - distance / circle_radius_) * costmap_2d::LETHAL_OBSTACLE;}else{return 0.0;}
}