CN113721633B - 一种基于行人轨迹预测的移动机器人路径规划方法 - Google Patents

Abstract

本发明提供了一种基于行人轨迹预测的移动机器人路径规划方法，包括如下步骤：步骤1：检测、跟踪和定位每个行人相对于相机帧的位置，获得组特征；步骤2：将检测到的组特征用于内聚估计，获得群体凝聚力指标；步骤3：利用群体凝聚力指标引入符合社交规则的导航算法。本发明利用机器人上的RGB‑D摄像头对行人的各种视觉特征进行计算。具体来说，通过检测与人之间的接近、他们的相对行走速度、群体大小和群体成员之间的相互作用相对应的特征，并使用凝聚力指标来设计和改进导航方案。当机器人在人群中移动时，导航方案可以实现不同级别的群体凝聚力，再依据内聚度量来设计一种新颖的机器人导航算法，该算法产生符合社交标准的机器人路径。

Description

一种基于行人轨迹预测的移动机器人路径规划方法

技术领域

本发明属于机器人应用技术领域，具体涉及一种基于行人轨迹预测的移动机器人路径规划方法。

背景技术

自传统工业机器人诞生以来，机器人技术在短短的60年内便取得了显著的进步。如今，机器人已经不再受限于工业应用，它的研究重点和发展方向更趋向于日常生活的应用，如餐饮、迎宾、娱乐、陪伴等。在诸多形式的移动机器人中，全向轮式移动机器人又以其灵活的运动学性能而备受关注。而伴随现代传感技术、计算机技术以及人工智能技术的飞速发展，轮式移动机器人的路径规划和运动过程中的避障技术也成为了机器人技术的研究热点。其中，移动机器人越来越多地用于室内和室外环境中拥挤的场景。底层应用包括监视、交付、物流等。在这种情况下，机器人需要以一种不引人注目的方式导航，还需要避免与突然转弯或冻结相关的问题。此外，机器人需要很好地融入物理和社会环境。

Frozone是一种导航方法，可以解决人群中出现的冷冻机器人问题(FreezingRobot Problem，FRP)。同时，它可以生成对行人不那么突兀的路径。但底层算法计算潜在冻结区(Potential Freezing Zone，PFZ)，它对应于障碍物的配置，其中机器人的规划器在它认为所有速度都可能导致碰撞时停止机器人并开始振荡一段时间。此外，Frozone的公式比较保守，不适合密集人群导航(>1人每平方米)。

发明内容

本发明的目的在于，克服现有技术存在的技术缺陷，解决上述技术问题，提出一种基于行人轨迹预测的移动机器人路径规划方法，并以此改善机器人在拥挤场景中的导航。

为解决上述技术问题，本发明采用的技术方案为：

一种基于行人轨迹预测的移动机器人路径规划方法，包括如下步骤：

步骤1：检测、跟踪和定位每个行人相对于相机帧的位置，获得组特征；

步骤2：将检测到的组特征用于内聚估计，获得群体凝聚力指标；

步骤3：利用群体凝聚力指标引入符合社交规则的导航算法。

步骤1中，检测和跟踪的步骤如下：

步骤1.1，移动机器人的相机获得是在时刻t捕获的宽度为w和高度为h的RGB图像，YOLOv5在时刻t为RGB图像中的每个检测到的行人i输出一组边界框β＝{βⁱ}；βⁱ使用其在图像空间或像素坐标中的左上角和右下角表示，并为每个检测到的行人i分配一个唯一的整数作为ID；

其中，βⁱ是ID为i的人的边界框；

步骤1.2，使用深度图像来估计每个检测到的行人相对于相机坐标系的距离，其每个像素都包含图像该位置处物体的接近度，以米为单位；/>中的像素包含最小和最大距离范围之间的值，取决于捕获图像的相机；

其中，是移动机器人的相机在时刻t捕获的宽度为w和高度为h的深度图像；

获得每个检测到的人相对于相机坐标系的距离后，定义组分类、估计接近度、估计步行速度和方向、估计组大小和检测交互。

步骤1.2.1，组分类：考虑任何一组人的IDs，IDs为任何一组人的ID的集合，在任何时间t，如果以下条件成立：

||p^i,t-p^j,t||₂≥||p^i,t+v^i,t-(p^j,t+v^j,t)||₂ (2)

那么集合G^k,t在图像中被归类为一个组；

其中，ID^t是在中检测到的人员ID集；p^i,t为人i在时间t相对于机器人/相机坐标系的位置向量，p^j,t为人j在时间t相对于机器人/相机坐标系的位置向量，p^i,t＝(x^i,t,y^{i ,t})，p^j,t＝(x^j,t,y^j,t)，x^i,t、y^i,t、x^j,t、y^j,t均为机器人/相机坐标系的位置向量；v^i,t是人i在时间t相对于机器人/相机坐标系的步行速度向量，/>v^j,t是人j在时间t相对于机器人/相机坐标系的步行速度向量，/>

X^i,t是卡尔曼滤波器中使用的状态向量，用于在时刻t对人i进行步行向量估计，Γ是手动设置的距离阈值；式(1)确保人们彼此靠近，式(2)确保组成员朝同一方向行走；当|vⁱ||,||v^j||＝0时为静态组，此时只使用式(1)进行分组；

步骤1.2.2，估计接近度：为了估计t时刻人与人之间的接近度，首先将YOLOv5在RGB图像中检测到的边界框叠加在深度图像上；为了估计人i与相机的距离d^i,t，计算以为中心的小方块内所有像素值的平均值；人i相对于相机的角位移ψ^i,t计算为：

其中，是边界框βⁱ的质心，FOV_RGBD是RGB-D相机的视野，w是YOLOv5算法中的模型参数；人i相对于相机的位置计算为：

[x^i,ty^i,t]＝d^i,t×[cosψ^i,tsinψ^i,t] (4)

人i和人j之间的距离可以计算为：

步骤1.2.3，估计步行速度和方向：使用具有恒速运动模型的卡尔曼滤波器来估计中第i个人的步行向量vⁱ；/>中所有检测到的人都使用状态向量X^t进行建模，/>中所有检测到的人的ID存储在集合ID^t中，对于在先前RGB图像中检测到的所有行人，即具有先前初始化状态的行人，此时即为建模前的行人的状态向量X^t，先使用标准卡尔曼预测和更新步骤更新他们的状态，再使用具有预设方差的零均值高斯噪声来模拟过程和感测噪声；

步骤1.2.4，估计组大小：组的大小为集合G^k,t中ID的数量；

步骤1.2.5，检测交互：使用两个3-D向量来表示一个人脸在中的位置和方向；

使用OpenFace在RGB图像上定位人i相对于相机坐标系的面部位置，并获得面部方向的单位向量/>如果两个人的面部位置和方向满足以下条件，则认为他们正在交互：

其中，和/>是人i相对于相机坐标系的面部位置和方向的向量；该条件检查两个人的面部位置之间的距离是否大于通过外推面部方向计算出的点之间的距离。

步骤2中将检测到的组特征用于内聚估计：

步骤2.1，邻近凝聚力得分：人与人之间的凝聚力与他们之间的距离成反比；因此，由于接近度导致的凝聚力分数被公式化为组成员之间平均距离的倒数，如下所示：

其中，n^k是组G^k,t中的成员数量；K_p是邻近度程度系数；C_p(G^k,t)是(G^k,t)组的邻近凝聚力得分；

步骤2.2，步行速度凝聚力得分：将每个组的平均步行速度与中所有检测到的人的平均步行速度进行比较；因此，步行组/>因其步行速度而产生的凝聚力分数的公式化为，

其中，N是在RGB图像中检测到的人数；对于如果∑||v^j||＝0，即该组是静态的，则C_w(G^k,t)＝K_w·η，η是用户设置的最大常数值；K_w是步行速度系数；C_w(G^k,t)是(G^{k ,t})组的步行速度凝聚力得分；

步骤2.3，群体凝聚力得分：群体k的凝聚力与群体规模n^k成正比，组大小凝聚力分数计算为，

C_s(G^k,t)＝K_s·n^k (9)

其中，K_s是群体规模系数；C_s(G^k,t)是(G^k,t)组的群体凝聚力得分；

步骤2.4，交互凝聚力得分：组中任意两个人之间的交互条件，即式(6)应用于组中的所有配对，它对组内凝聚力得分的贡献重写为：

其中，θ_i,j是相机坐标系的X-Y平面中人脸方向向量和/>之间的角度；θ_i,j在/>区间内，即，当人们面对面时，凝聚力得分为正且大于1更佳，否则为负；故选择比率/>因为当/>时，它属于范围/>由于cos()是偶函数，因此sign()函数可确保公式对角度的符号敏感；K_i是交互系数；C_i(G^k,t)是(G^k,t)组的交互凝聚力得分；

步骤2.5，整体群体凝聚力指标：使用式(7)～(10)中的个体凝聚力分数，在时间t(G^k,t)时一组的总凝聚力分数给出为，

C_tot(G^k,t)＝(C_p+C_w+C_s+C_i) (11)

在将K_p,K_w,K_s,K_i不同特征添加到总内聚分数之前，先对其进行权衡；如果无法检测到任何特征，则它们对C_tot的贡献为零；一个组的整体内聚度量C_tot(G^k,t)的值是有界的；其中，K_p,K_w,K_s,K_i是每个特征的比例和权重常数；K_p+K_w+K_s+K_i＝1；被用作对人进行分组的阈值；C_w的最大值是K_w·η，这是一个大的有限常数，当组是静态时使用；C_s以K_g·n^k为界，K_g·n^k是有限的；C_i的取值范围如下：

在凝聚力度量上使用这些界限来计算适当的阈值，这些阈值用于将组分类为低、中和高凝聚力组。

步骤3中为了提高机器人路径的自然度，提出的方法包括以下步骤：

步骤3.1，在机器人的传感区域内识别潜在的冻结组，并在时间段t_h后预测它们的位置；

步骤3.2，使用每个组成员的预测未来位置为每个组构建一个潜在冻结组PFZ；

步骤3.3，计算偏差角以避免潜在冻结组PFZ，同时考虑到每个组的凝聚力；如果没有找到可行的解决方案，机器人在场景中凝聚力最低的组之间导航。

步骤3.1中的潜在的冻结组为时间t_h后可能引起冷冻机器人问题FRP的行人组；这些组是根据他们平均步行方向的条件确定的；随着时间的推移，满足这些条件的组会更靠近机器人，故预测潜在的冻结组成员的未来位置为：

其中，是平均组步行向量，计算为组成员的步行向量的平均值，M是潜在的冻结组的总数；/>是坐标系的位置向量。

步骤3.2中的潜在冻结组PFZ为机器人可能冻结的组附近区域；为每个可能冻结的组构建一个PFZ：

其中，CH()以点为参数的凸包函数；每个潜在冻结组PFZ都是针对未来时刻t+t_h计算的。

步骤3.3中的计算偏差角的方法为：如果机器人的当前路径将其导航到任何一组PFZ^k，则计算偏差角以避免它；使用绕Z轴的旋转矩阵，机器人的当前速度v_rob偏离了/>如下所示：

其中，g_rob是机器人的目标点；v′_rob是机器人的当前速度v_rob偏离了后的速度；是绕Z轴的3-D旋转矩阵，即绕垂直于机器人平面的3-D旋转矩阵；

在密集场景中，当机器人遇到许多潜在的冻结组及其对应的潜在冻结组PFZ时，式(16)无法计算的可行解；在这种情况下，解决方案是让机器人通过低内聚群的PFZ；在这种情况下，将偏差角公式化为，

其中，PFZ^min是场景中具有最小内聚力的组的潜在冻结组PFZ；是潜在冻结组PFZ中的低内聚群的PFZ。

本发明的有益效果：

本发明利用机器人上的RGB-D摄像头对行人的各种视觉特征进行计算。具体来说，通过检测与人之间的接近、他们的相对行走速度、群体大小和群体成员之间的相互作用相对应的特征，并使用凝聚力指标来设计和改进导航方案。当机器人在人群中移动时，导航方案可以实现不同级别的群体凝聚力，再依据内聚度量来设计一种新颖的机器人导航算法，该算法产生符合社交标准的机器人路径。

本发明提供了一种使用视觉特征计算人群中一群人的凝聚力的新方法，主要依据内聚度量来设计一种新颖的机器人导航算法，该算法会产生符合社交标准的机器人路径。与先前算法比较，主要优势在于减少冻结、偏差角度和路径长度。

附图说明

图1为本发明的算法框架和原理图。

具体实施方式

下面结合附图以及具体实施方法对本发明一种基于行人轨迹预测的移动机器人路径规划方法作进一步详细说明。

如图1所示，一种基于行人轨迹预测的移动机器人路径规划方法，包括如下步骤：

步骤一：检测、跟踪和定位每个行人相对于相机帧的位置；

步骤二：将检测到的组特征用于内聚估计；

步骤三：利用群体凝聚力指标引入符合社交规则的导航算法。

所述步骤一中分别使用YOLOv5和Deep Sort算法来检测人和跟踪人，主要步骤如下：

步骤1.1，YOLOv5在时刻t(记为)为RGB图像中的每个检测到的行人i输出一组边界框β＝{βⁱ}。βⁱ使用其在图像空间或像素坐标中的左上角和右下角表示，并为每个检测到的行人分配一个唯一的整数作为ID。

其中，是在时刻t捕获的宽度为w和高度为h的RGB图像，βⁱ是ID为i的人的边界框。

步骤1.2，使用深度图像来估计每个检测到的人相对于相机坐标系的距离，其每个像素都包含图像该位置处物体的接近度(以米为单位)。/>中的像素包含最小和最大距离范围之间的值取决于捕获图像的相机。捕获图像的相机不同，/>中的像素的最小距离与最大距离之间的值也会不同。每个机器人上的相机不会有很多，一般是1-2个。

其中，是在时刻t捕获的宽度为w和高度为h的深度图像。

所述步骤一的子步骤1.2中，分别定义组分类、估计接近度、估计步行速度和方向、估计组大小和检测交互：

步骤1.2.1，组分类：考虑任何一组人的IDs，在任何时间t，如果以下条件成立：

||p^i,t-p^j,t||₂≥||p^i,t+v^i,t-(p^j,t+v^j,t)||₂ (2)

那么集合G^k,t在图像中被归类为一个组。其中，ID^t是在/>中检测到的人员ID集。p^i,t为人i在时间t相对于机器人/相机坐标系的位置向量，p^i,t＝(x^i,t,y^i,t)。v^i,t是人i在时间t相对于机器人/相机坐标系的步行速度向量，/>X^i,t是卡尔曼滤波器中使用的状态向量，用于在时刻t对人i进行步行向量估计，/>Γ是手动设置的距离阈值。第一个条件确保人们彼此靠近，第二个条件确保组成员朝同一方向行走。当|vⁱ||,||v^j||＝0(静态组)，只使用第一个条件进行分组。

步骤1.2.2，估计接近度：为了估计t时刻人与人之间的接近度，首先将YOLOv5在RGB图像中检测到的边界框叠加在深度图像上。为了估计人i与相机的距离d^i,t，计算以为中心的小方块内所有像素值的平均值。人i相对于相机的角位移ψ^i,t可以计算为:

其中，是边界框βⁱ的质心，FOV_RGBD是RGB-D相机的视野，w是YOLOv5算法中的模型参数。人i相对于相机的位置可以计算为：

[x^i,ty^i,t]＝d^i,t×[cosψ^i,tsinψ^i,t] (4)

人i和人j之间的距离可以计算为：

步骤1.2.3，估计步行速度和方向：使用具有恒速运动模型的卡尔曼滤波器来估计中第i个人的步行向量vⁱ。/>中所有检测到的人(ID存储在集合ID^t中)都使用状态向量X^t进行建模。对于在先前RGB图像中检测到的所有行人，即具有先前初始化状态的行人，先使用标准卡尔曼预测和更新步骤更新他们的状态，再使用具有预设方差的零均值高斯噪声来模拟过程和感测噪声。

标准的卡尔曼滤波***方程如下：

θ_k＝Aθ_k-1+Bu_k-1+s_k

z_k＝Cθ_k+v_k

上面的两个式子分别叫作状态转移方程和观测方程；其中A叫作状态转移矩阵，B和u_k是***模型的参数，C是观测矩阵，s_k是状态转移噪声向量或者叫过程噪声，v_k是观测噪声向量，θ_k是k时刻的真实状态向量，z_k是k时刻的观测向量。θ_k,z_k,s_k,v_k往往是列向量的形式，可以理解为同时对多个属性做滤波估计操作。

状态转移噪声向量s_k一般是服从多维高斯分布的，其均值为0向量，协方差矩阵为Q；观测噪声向量v_k一般也是服从多维高斯分布的，其均值为0向量，协方差矩阵是R。最终滤波后得出的估计向量值用θ_k表示。

步骤1.2.4，估计组大小：组的大小可以简单地计算为集合G^k,t中ID的数量。

步骤1.2.5，检测交互：使用两个3-D向量来表示一个人脸在中的位置和方向。

使用OpenFace在RGB图像上定位人i相对于相机坐标系的面部位置，并获得面部方向的单位向量/>如果两个人的面部位置和方向满足以下条件，则认为他们正在交互：

其中，和/>是人i相对于相机坐标系的面部位置和方向的向量。该条件检查两个人的面部位置之间的距离是否大于通过外推面部方向计算出的点之间的距离。

所述步骤二中将检测到的组特征用于内聚估计：

步骤2.1，邻近凝聚力得分：人与人之间的凝聚力与他们之间的距离成反比。因此，由于接近度导致的凝聚力分数被公式化为组成员之间平均距离的倒数，如下所示：

其中，n^k是组G^k,t中的成员数量。

步骤2.2，步行速度凝聚力得分：将每个组的平均步行速度与中所有检测到的人的平均步行速度进行比较。因此，步行组/>因其步行速度而产生的凝聚力分数的公式化为，

其中，N是在RGB图像中检测到的人数。对于如果∑||v^j||＝0，即该组是静态的，则C_w(G^k,t)＝K_w·η，η是用户设置的最大常数值。

步骤2.3，群体凝聚力得分：群体k的凝聚力与群体规模n^k成正比，组大小凝聚力分数计算为，

C_s(G^k,t)＝K_s·n^k (9)

步骤2.4，交互凝聚力得分：组中任意两个人之间的交互条件(方程6)可以应用于组中的所有配对，它对组内凝聚力得分的贡献可以重写为：

其中，θ_i,j是相机坐标系的X-Y平面中人脸方向向量和/>之间的角度。θ_i,j在/>区间内，因为面部方向在此范围内是准确的。直观地说，当人们面对面时，凝聚力得分为正且大于1更佳，否则为负。故选择比率/>因为当/>时，它属于范围/> 由于cos()是偶函数，因此sign()函数可确保公式对角度的符号敏感。

步骤2.5，整体群体凝聚力指标：使用等式7、8、9、10中的个体凝聚力分数，在时间t(G^k,t)时一组的总凝聚力分数给出为，

C_tot(G^k,t)＝(C_p+C_w+C_s+C_i) (11)

在将K_p,K_w,K_s,K_i不同特征添加到总内聚分数之前，先对其进行权衡。如果无法检测到任何特征，则它们对C_tot的贡献将为零。一个组的整体内聚度量C_tot(G^k,t)的值是有界的。其中，K_p,K_w,K_s,K_i是每个特征的比例和权重常数，K_p+K_w+K_s+K_i＝1。C_p∈(0,K_p·Γ)，Γ被用作对人进行分组的阈值。C_w的最大值是K_w·η，这是一个大的有限常数，当组是静态时使用。C_s以K_g·n^k为界，K_g·n^k是有限的。C_i的取值范围如下：

在凝聚力度量上使用这些界限来计算适当的阈值，这些阈值用于将组分类为低、中和高凝聚力组。

所述步骤三中为了提高机器人路径的自然度，提出的方法包括以下步骤：

步骤3.1，在机器人的传感区域内识别潜在的冻结组，并在时间段t_h后预测它们的位置；

步骤3.2，使用每个组成员的预测未来位置为每个组构建一个PFZ；

步骤3.3，计算偏差角以避免组PFZ，同时考虑到每个组的凝聚力。如果没有找到可行的解决方案，机器人将在场景中凝聚力最低的组之间导航。

所述步骤三的子步骤3.1中的可能冻结组：时间t_h后极有可能引起FRP的行人组。这些组是根据他们平均步行方向的条件确定的。随着时间的推移，满足这些条件的组会更靠近机器人，故预测潜在冻结组成员的未来位置为

其中，是平均组步行向量，计算为组成员的步行向量的平均值，M是潜在冻结组的总数。

所述步骤三的子步骤3.2中的组PFZ：机器人极有可能冻结的组附近区域。为每个可能冻结的组构建一个PFZ作为

其中，CH()以点为参数的凸包函数。每个潜在冻结组的PFZ都是针对未来时刻t+t_h计算的。

所述步骤三中的子步骤3.3中的计算偏差角：如果机器人的当前路径将其导航到任何一组PFZ^k(意味着在时间t_h后发生FRP)，则计算偏差角以避免它。使用绕Z轴的旋转矩阵，机器人的当前速度v_rob偏离了/>如下所示：

这个方程意味着本发明的导航方法使机器人偏离其目标的量最小，因此它不会进入任何组的PFZ。然而，在密集场景中，当机器人遇到许多潜在的冻结组及其对应的PFZ时，等式16可能无法计算的可行解。在这种情况下，一个潜在的解决方案是让机器人通过低内聚群的PFZ。在这种情况下，将偏差角公式化为，

其中PFZ^min是场景中具有最小内聚力的组的PFZ。由于低内聚基团的渗透性高，上述公式也降低了冻结的可能性。

在本发明一个较佳的实例中，在类似走廊的场景中，行人数量不同(10-50)，机器人需要穿过行人才能到达目标。行人被赋予随机的初始位置，根据这些位置，他们被分类为组，并计算PFZ_froz和组PFZs(PFZ^k)。使用以下指标：相对于连接起点和目标位置的线测量的平均偏差角；冻结率测量为机器人在总试验次数中停止/冻结的次数；标准化路径长度测量为机器人在连接起点和目标位置的线段上的路径长度。与相同场景下的Frozone相比，本发明的方法在所有这些指标方面的值都较低。随着人群规模、密度或群体数量的增加，Frozone的保守公式使机器人以非常高的速度冻结。从这个分析中可以观察到本发明的方法产生了具有高度社会依从性和自然性的路径。此外，该方法显着减少了冻结行为的发生。

在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下被实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。此外，应当注意，本说明书中使用的语言主要是为了可读性和教导的目的而选择的，而不是为了解释或者限定本发明的主题而选择的。因此，在不偏离保护范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于行人轨迹预测的移动机器人路径规划方法，其特征在于，包括如下步骤：

步骤1：检测、跟踪和定位每个行人相对于相机帧的位置，获得组特征；

步骤2：将检测到的组特征用于内聚估计，获得群体凝聚力指标；

步骤3：利用群体凝聚力指标引入符合社交规则的导航算法；

步骤1中，检测和跟踪的步骤如下：

步骤1.1，移动机器人的相机获得 是在时刻t捕获的宽度为w和高度为h的RGB图像，YOLOv5在时刻t为RGB图像中的每个检测到的行人i输出一组边界框β＝{βⁱ}；βⁱ使用其在图像空间或像素坐标中的左上角和右下角表示，并为每个检测到的行人i分配一个唯一的整数作为ID；

其中，βⁱ是ID为i的人的边界框；

步骤1.2，使用深度图像来估计每个检测到的行人相对于相机坐标系的距离，其每个像素都包含图像该位置处物体的接近度，以米为单位；/>中的像素包含最小和最大距离范围之间的值，取决于捕获图像的相机；

其中，是移动机器人的相机在时刻t捕获的宽度为w和高度为h的深度图像；

获得每个检测到的人相对于相机坐标系的距离后，定义组分类、估计接近度、估计步行速度和方向、估计组大小和检测交互；

步骤1.2.1，组分类：考虑任何一组人的IDs，IDs为任何一组人的ID的集合，在任何时间t，如果以下条件成立：

||p^i,t-p^j,t||₂≥||p^i,t+v^i,t-(p^j,t+v^j,t)||₂ (2)

那么集合G^k,t在图像中被归类为一个组；

其中，ID^t是在中检测到的人员ID集；p^i,t为人i在时间t相对于机器人/相机坐标系的位置向量，p^j,t为人j在时间t相对于机器人/相机坐标系的位置向量，p^i,t＝(x^i,t,y^i,t)，p^j,t＝(x^j,t,y^j,t)，x^i,t、y^i,t、x^j,t、y^j,t均为机器人/相机坐标系的位置向量；v^i,t是人i在时间t相对于机器人/相机坐标系的步行速度向量，/>v^j,t是人j在时间t相对于机器人/相机坐标系的步行速度向量，/>

X^i,t是卡尔曼滤波器中使用的状态向量，用于在时刻t对人i进行步行向量估计，Γ是手动设置的距离阈值；式(1)确保人们彼此靠近，式(2)确保组成员朝同一方向行走；当|vⁱ||,||v^j||＝0时为静态组，此时只使用式(1)进行分组；

步骤1.2.2，估计接近度：为了估计t时刻人与人之间的接近度，首先将YOLOv5在RGB图像中检测到的边界框叠加在深度图像上；为了估计人i与相机的距离d^i,t，计算以为中心的小方块内所有像素值的平均值；人i相对于相机的角位移ψ^i,t计算为：

其中，是边界框βⁱ的质心，FOV_RGBD是RGB-D相机的视野，w是YOLOv5算法中的模型参数；人i相对于相机的位置计算为：

[x^i,ty^i,t]＝d^i,t×[cosψ^i,tsinψ^i,t] (4)

人i和人j之间的距离可以计算为：

步骤1.2.3，估计步行速度和方向：使用具有恒速运动模型的卡尔曼滤波器来估计中第i个人的步行向量vⁱ；/>中所有检测到的人都使用状态向量X^t进行建模，/>中所有检测到的人的ID存储在集合ID^t中，对于在先前RGB图像中检测到的所有行人，即具有先前初始化状态的行人，此时即为建模前的行人的状态向量X^t，先使用标准卡尔曼预测和更新步骤更新他们的状态，再使用具有预设方差的零均值高斯噪声来模拟过程和感测噪声；

步骤1.2.4，估计组大小：组的大小为集合G^k,t中ID的数量；

步骤1.2.5，检测交互：使用两个3-D向量来表示一个人脸在中的位置和方向；

使用OpenFace在RGB图像上定位人i相对于相机坐标系的面部位置，并获得面部方向的单位向量/>如果两个人的面部位置和方向满足以下条件，则认为他们正在交互：

其中，和/>是人i相对于相机坐标系的面部位置和方向的向量；该条件检查两个人的面部位置之间的距离是否大于通过外推面部方向计算出的点之间的距离；

步骤2中将检测到的组特征用于内聚估计：

步骤2.1，邻近凝聚力得分：人与人之间的凝聚力与他们之间的距离成反比；因此，由于接近度导致的凝聚力分数被公式化为组成员之间平均距离的倒数，如下所示：

其中，n^k是组G^k,t中的成员数量；K_p是邻近度程度系数；C_p(G^k,t)是(G^k,t)组的邻近凝聚力得分；

步骤2.2，步行速度凝聚力得分：将每个组的平均步行速度与中所有检测到的人的平均步行速度进行比较；因此，步行组/>因其步行速度而产生的凝聚力分数的公式化为，

其中，N是在RGB图像中检测到的人数；对于如果∑||v^j||＝0，即该组是静态的，则C_w(G^k,t)＝K_w·η，η是用户设置的最大常数值；K_w是步行速度系数；C_w(G^k,t)是(G^k,t)组的步行速度凝聚力得分；

步骤2.3，群体凝聚力得分：群体k的凝聚力与群体规模n^k成正比，组大小凝聚力分数计算为，

C_s(G^k,t)＝K_s·n^k (9)

其中，K_s是群体规模系数；C_s(G^k,t)是(G^k,t)组的群体凝聚力得分；

步骤2.4，交互凝聚力得分：组中任意两个人之间的交互条件，即式(6)应用于组中的所有配对，它对组内凝聚力得分的贡献重写为：

其中，θ_i,j是相机坐标系的X-Y平面中人脸方向向量和/>之间的角度；θ_i,j在/>区间内，即，当人们面对面时，凝聚力得分为正且大于1更佳，否则为负；故选择比率/>因为当/>时，它属于范围/>由于cos()是偶函数，因此sign()函数可确保公式对角度的符号敏感；K_i是交互系数；C_i(G^k,t)是(G^k,t)组的交互凝聚力得分；

步骤2.5，整体群体凝聚力指标：使用式(7)～(10)中的个体凝聚力分数，在时间t(G^k,t)时一组的总凝聚力分数给出为，

C_tot(G^k,t)＝(C_p+C_w+C_s+C_i)(11)

在将K_p,K_w,K_s,K_i不同特征添加到总内聚分数之前，先对其进行权衡；如果无法检测到任何特征，则它们对C_tot的贡献为零；一个组的整体内聚度量C_tot(G^k,t)的值是有界的；其中，K_p,K_w,K_s,K_i是每个特征的比例和权重常数；K_p+K_w+K_s+K_i＝1； 被用作对人进行分组的阈值；C_w的最大值是K_w·η，这是一个大的有限常数，当组是静态时使用；C_s以K_g·n^k为界，K_g·n^k是有限的；C_i的取值范围如下：

在凝聚力度量上使用这些界限来计算适当的阈值，这些阈值用于将组分类为低、中和高凝聚力组；

步骤3中为了提高机器人路径的自然度，提出的方法包括以下步骤：

步骤3.1，在机器人的传感区域内识别潜在的冻结组，并在时间段t_h后预测它们的位置；

步骤3.2，使用每个组成员的预测未来位置为每个组构建一个潜在冻结组PFZ；

步骤3.3，计算偏差角以避免潜在冻结组PFZ，同时考虑到每个组的凝聚力；如果没有找到可行的解决方案，机器人在场景中凝聚力最低的组之间导航。

2.权利要求1所述的一种基于行人轨迹预测的移动机器人路径规划方法，其特征在于，

步骤3.1中的潜在的冻结组为时间t_h后可能引起冷冻机器人问题FRP的行人组；这些组是根据他们平均步行方向的条件确定的；随着时间的推移，满足这些条件的组会更靠近机器人，故预测潜在的冻结组成员的未来位置为：

其中，是平均组步行向量，计算为组成员的步行向量的平均值，M是潜在的冻结组的总数；/>是坐标系的位置向量。

3.权利要求2所述的一种基于行人轨迹预测的移动机器人路径规划方法，其特征在于，

步骤3.2中的潜在冻结组PFZ为机器人可能冻结的组附近区域；为每个可能冻结的组构建一个PFZ：

其中，CH()以点为参数的凸包函数；每个潜在冻结组PFZ都是针对未来时刻t+t_h计算的。

4.权利要求3所述的一种基于行人轨迹预测的移动机器人路径规划方法，其特征在于，步骤3.3中的计算偏差角的方法为：如果机器人的当前路径将其导航到任何一组PFZ^k，则计算偏差角以避免它；使用绕Z轴的旋转矩阵，机器人的当前速度v_rob偏离了/>如下所示：

其中，g_rob是机器人的目标点；v′_rob是机器人的当前速度v_rob偏离了后的速度；/>是绕Z轴的3-D旋转矩阵，即绕垂直于机器人平面的3-D旋转矩阵。

5.权利要求3所述的一种基于行人轨迹预测的移动机器人路径规划方法，其特征在于，在密集场景中，当机器人遇到许多潜在的冻结组及其对应的潜在冻结组PFZ时，式(16)无法计算的可行解；在这种情况下，解决方案是让机器人通过低内聚群的PFZ；在这种情况下，将偏差角公式化为，

其中，PFZ^min是场景中具有最小内聚力的组的潜在冻结组PFZ；是潜在冻结组PFZ中的低内聚群的PFZ。