CN113721633B - 一种基于行人轨迹预测的移动机器人路径规划方法 - Google Patents
一种基于行人轨迹预测的移动机器人路径规划方法 Download PDFInfo
- Publication number
- CN113721633B CN113721633B CN202111053999.2A CN202111053999A CN113721633B CN 113721633 B CN113721633 B CN 113721633B CN 202111053999 A CN202111053999 A CN 202111053999A CN 113721633 B CN113721633 B CN 113721633B
- Authority
- CN
- China
- Prior art keywords
- group
- robot
- person
- cohesive force
- vector
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000003993 interaction Effects 0.000 claims abstract description 12
- 239000013598 vector Substances 0.000 claims description 56
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000008014 freezing Effects 0.000 claims description 10
- 238000007710 freezing Methods 0.000 claims description 10
- 230000006870 function Effects 0.000 claims description 9
- 230000003068 static effect Effects 0.000 claims description 9
- 230000001815 facial effect Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 2
- 238000005303 weighing Methods 0.000 claims description 2
- 238000005259 measurement Methods 0.000 abstract description 3
- 230000000007 visual effect Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 5
- 238000010526 radical polymerization reaction Methods 0.000 description 4
- 230000007704 transition Effects 0.000 description 4
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000035699 permeability Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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};βi使用其在图像空间或像素坐标中的左上角和右下角表示,并为每个检测到的行人i分配一个唯一的整数作为ID;
其中,βi是ID为i的人的边界框;
步骤1.2,使用深度图像来估计每个检测到的行人相对于相机坐标系的距离,其每个像素都包含图像该位置处物体的接近度,以米为单位;/>中的像素包含最小和最大距离范围之间的值,取决于捕获图像的相机;
其中,是移动机器人的相机在时刻t捕获的宽度为w和高度为h的深度图像;
获得每个检测到的人相对于相机坐标系的距离后,定义组分类、估计接近度、估计步行速度和方向、估计组大小和检测交互。
步骤1.2.1,组分类:考虑任何一组人的IDs,IDs为任何一组人的ID的集合,在任何时间t,如果以下条件成立:
||pi,t-pj,t||2≥||pi,t+vi,t-(pj,t+vj,t)||2 (2)
那么集合Gk,t在图像中被归类为一个组;
其中,IDt是在中检测到的人员ID集;pi,t为人i在时间t相对于机器人/相机坐标系的位置向量,pj,t为人j在时间t相对于机器人/相机坐标系的位置向量,pi,t=(xi,t,yi ,t),pj,t=(xj,t,yj,t),xi,t、yi,t、xj,t、yj,t均为机器人/相机坐标系的位置向量;vi,t是人i在时间t相对于机器人/相机坐标系的步行速度向量,/>vj,t是人j在时间t相对于机器人/相机坐标系的步行速度向量,/>
Xi,t是卡尔曼滤波器中使用的状态向量,用于在时刻t对人i进行步行向量估计,Γ是手动设置的距离阈值;式(1)确保人们彼此靠近,式(2)确保组成员朝同一方向行走;当|vi||,||vj||=0时为静态组,此时只使用式(1)进行分组;
步骤1.2.2,估计接近度:为了估计t时刻人与人之间的接近度,首先将YOLOv5在RGB图像中检测到的边界框叠加在深度图像上;为了估计人i与相机的距离di,t,计算以为中心的小方块内所有像素值的平均值;人i相对于相机的角位移ψi,t计算为:
其中,是边界框βi的质心,FOVRGBD是RGB-D相机的视野,w是YOLOv5算法中的模型参数;人i相对于相机的位置计算为:
[xi,tyi,t]=di,t×[cosψi,tsinψi,t] (4)
人i和人j之间的距离可以计算为:
步骤1.2.3,估计步行速度和方向:使用具有恒速运动模型的卡尔曼滤波器来估计中第i个人的步行向量vi;/>中所有检测到的人都使用状态向量Xt进行建模,/>中所有检测到的人的ID存储在集合IDt中,对于在先前RGB图像中检测到的所有行人,即具有先前初始化状态的行人,此时即为建模前的行人的状态向量Xt,先使用标准卡尔曼预测和更新步骤更新他们的状态,再使用具有预设方差的零均值高斯噪声来模拟过程和感测噪声;
步骤1.2.4,估计组大小:组的大小为集合Gk,t中ID的数量;
步骤1.2.5,检测交互:使用两个3-D向量来表示一个人脸在中的位置和方向;
使用OpenFace在RGB图像上定位人i相对于相机坐标系的面部位置,并获得面部方向的单位向量/>如果两个人的面部位置和方向满足以下条件,则认为他们正在交互:
其中,和/>是人i相对于相机坐标系的面部位置和方向的向量;该条件检查两个人的面部位置之间的距离是否大于通过外推面部方向计算出的点之间的距离。
步骤2中将检测到的组特征用于内聚估计:
步骤2.1,邻近凝聚力得分:人与人之间的凝聚力与他们之间的距离成反比;因此,由于接近度导致的凝聚力分数被公式化为组成员之间平均距离的倒数,如下所示:
其中,nk是组Gk,t中的成员数量;Kp是邻近度程度系数;Cp(Gk,t)是(Gk,t)组的邻近凝聚力得分;
步骤2.2,步行速度凝聚力得分:将每个组的平均步行速度与中所有检测到的人的平均步行速度进行比较;因此,步行组/>因其步行速度而产生的凝聚力分数的公式化为,
其中,N是在RGB图像中检测到的人数;对于如果∑||vj||=0,即该组是静态的,则Cw(Gk,t)=Kw·η,η是用户设置的最大常数值;Kw是步行速度系数;Cw(Gk,t)是(Gk ,t)组的步行速度凝聚力得分;
步骤2.3,群体凝聚力得分:群体k的凝聚力与群体规模nk成正比,组大小凝聚力分数计算为,
Cs(Gk,t)=Ks·nk (9)
其中,Ks是群体规模系数;Cs(Gk,t)是(Gk,t)组的群体凝聚力得分;
步骤2.4,交互凝聚力得分:组中任意两个人之间的交互条件,即式(6)应用于组中的所有配对,它对组内凝聚力得分的贡献重写为:
其中,θi,j是相机坐标系的X-Y平面中人脸方向向量和/>之间的角度;θi,j在/>区间内,即,当人们面对面时,凝聚力得分为正且大于1更佳,否则为负;故选择比率/>因为当/>时,它属于范围/>由于cos()是偶函数,因此sign()函数可确保公式对角度的符号敏感;Ki是交互系数;Ci(Gk,t)是(Gk,t)组的交互凝聚力得分;
步骤2.5,整体群体凝聚力指标:使用式(7)~(10)中的个体凝聚力分数,在时间t(Gk,t)时一组的总凝聚力分数给出为,
Ctot(Gk,t)=(Cp+Cw+Cs+Ci) (11)
在将Kp,Kw,Ks,Ki不同特征添加到总内聚分数之前,先对其进行权衡;如果无法检测到任何特征,则它们对Ctot的贡献为零;一个组的整体内聚度量Ctot(Gk,t)的值是有界的;其中,Kp,Kw,Ks,Ki是每个特征的比例和权重常数;Kp+Kw+Ks+Ki=1;被用作对人进行分组的阈值;Cw的最大值是Kw·η,这是一个大的有限常数,当组是静态时使用;Cs以Kg·nk为界,Kg·nk是有限的;Ci的取值范围如下:
在凝聚力度量上使用这些界限来计算适当的阈值,这些阈值用于将组分类为低、中和高凝聚力组。
步骤3中为了提高机器人路径的自然度,提出的方法包括以下步骤:
步骤3.1,在机器人的传感区域内识别潜在的冻结组,并在时间段th后预测它们的位置;
步骤3.2,使用每个组成员的预测未来位置为每个组构建一个潜在冻结组PFZ;
步骤3.3,计算偏差角以避免潜在冻结组PFZ,同时考虑到每个组的凝聚力;如果没有找到可行的解决方案,机器人在场景中凝聚力最低的组之间导航。
步骤3.1中的潜在的冻结组为时间th后可能引起冷冻机器人问题FRP的行人组;这些组是根据他们平均步行方向的条件确定的;随着时间的推移,满足这些条件的组会更靠近机器人,故预测潜在的冻结组成员的未来位置为:
其中,是平均组步行向量,计算为组成员的步行向量的平均值,M是潜在的冻结组的总数;/>是坐标系的位置向量。
步骤3.2中的潜在冻结组PFZ为机器人可能冻结的组附近区域;为每个可能冻结的组构建一个PFZ:
其中,CH()以点为参数的凸包函数;每个潜在冻结组PFZ都是针对未来时刻t+th计算的。
步骤3.3中的计算偏差角的方法为:如果机器人的当前路径将其导航到任何一组PFZk,则计算偏差角以避免它;使用绕Z轴的旋转矩阵,机器人的当前速度vrob偏离了/>如下所示:
其中,grob是机器人的目标点;v′rob是机器人的当前速度vrob偏离了后的速度;是绕Z轴的3-D旋转矩阵,即绕垂直于机器人平面的3-D旋转矩阵;
在密集场景中,当机器人遇到许多潜在的冻结组及其对应的潜在冻结组PFZ时,式(16)无法计算的可行解;在这种情况下,解决方案是让机器人通过低内聚群的PFZ;在这种情况下,将偏差角公式化为,
其中,PFZmin是场景中具有最小内聚力的组的潜在冻结组PFZ;是潜在冻结组PFZ中的低内聚群的PFZ。
本发明的有益效果:
本发明利用机器人上的RGB-D摄像头对行人的各种视觉特征进行计算。具体来说,通过检测与人之间的接近、他们的相对行走速度、群体大小和群体成员之间的相互作用相对应的特征,并使用凝聚力指标来设计和改进导航方案。当机器人在人群中移动时,导航方案可以实现不同级别的群体凝聚力,再依据内聚度量来设计一种新颖的机器人导航算法,该算法产生符合社交标准的机器人路径。
本发明提供了一种使用视觉特征计算人群中一群人的凝聚力的新方法,主要依据内聚度量来设计一种新颖的机器人导航算法,该算法会产生符合社交标准的机器人路径。与先前算法比较,主要优势在于减少冻结、偏差角度和路径长度。
附图说明
图1为本发明的算法框架和原理图。
具体实施方式
下面结合附图以及具体实施方法对本发明一种基于行人轨迹预测的移动机器人路径规划方法作进一步详细说明。
如图1所示,一种基于行人轨迹预测的移动机器人路径规划方法,包括如下步骤:
步骤一:检测、跟踪和定位每个行人相对于相机帧的位置;
步骤二:将检测到的组特征用于内聚估计;
步骤三:利用群体凝聚力指标引入符合社交规则的导航算法。
所述步骤一中分别使用YOLOv5和Deep Sort算法来检测人和跟踪人,主要步骤如下:
步骤1.1,YOLOv5在时刻t(记为)为RGB图像中的每个检测到的行人i输出一组边界框β={βi}。βi使用其在图像空间或像素坐标中的左上角和右下角表示,并为每个检测到的行人分配一个唯一的整数作为ID。
其中,是在时刻t捕获的宽度为w和高度为h的RGB图像,βi是ID为i的人的边界框。
步骤1.2,使用深度图像来估计每个检测到的人相对于相机坐标系的距离,其每个像素都包含图像该位置处物体的接近度(以米为单位)。/>中的像素包含最小和最大距离范围之间的值取决于捕获图像的相机。捕获图像的相机不同,/>中的像素的最小距离与最大距离之间的值也会不同。每个机器人上的相机不会有很多,一般是1-2个。
其中,是在时刻t捕获的宽度为w和高度为h的深度图像。
所述步骤一的子步骤1.2中,分别定义组分类、估计接近度、估计步行速度和方向、估计组大小和检测交互:
步骤1.2.1,组分类:考虑任何一组人的IDs,在任何时间t,如果以下条件成立:
||pi,t-pj,t||2≥||pi,t+vi,t-(pj,t+vj,t)||2 (2)
那么集合Gk,t在图像中被归类为一个组。其中,IDt是在/>中检测到的人员ID集。pi,t为人i在时间t相对于机器人/相机坐标系的位置向量,pi,t=(xi,t,yi,t)。vi,t是人i在时间t相对于机器人/相机坐标系的步行速度向量,/>Xi,t是卡尔曼滤波器中使用的状态向量,用于在时刻t对人i进行步行向量估计,/>Γ是手动设置的距离阈值。第一个条件确保人们彼此靠近,第二个条件确保组成员朝同一方向行走。当|vi||,||vj||=0(静态组),只使用第一个条件进行分组。
步骤1.2.2,估计接近度:为了估计t时刻人与人之间的接近度,首先将YOLOv5在RGB图像中检测到的边界框叠加在深度图像上。为了估计人i与相机的距离di,t,计算以为中心的小方块内所有像素值的平均值。人i相对于相机的角位移ψi,t可以计算为:
其中,是边界框βi的质心,FOVRGBD是RGB-D相机的视野,w是YOLOv5算法中的模型参数。人i相对于相机的位置可以计算为:
[xi,tyi,t]=di,t×[cosψi,tsinψi,t] (4)
人i和人j之间的距离可以计算为:
步骤1.2.3,估计步行速度和方向:使用具有恒速运动模型的卡尔曼滤波器来估计中第i个人的步行向量vi。/>中所有检测到的人(ID存储在集合IDt中)都使用状态向量Xt进行建模。对于在先前RGB图像中检测到的所有行人,即具有先前初始化状态的行人,先使用标准卡尔曼预测和更新步骤更新他们的状态,再使用具有预设方差的零均值高斯噪声来模拟过程和感测噪声。
标准的卡尔曼滤波***方程如下:
θk=Aθk-1+Buk-1+sk
zk=Cθk+vk
上面的两个式子分别叫作状态转移方程和观测方程;其中A叫作状态转移矩阵,B和uk是***模型的参数,C是观测矩阵,sk是状态转移噪声向量或者叫过程噪声,vk是观测噪声向量,θk是k时刻的真实状态向量,zk是k时刻的观测向量。θk,zk,sk,vk往往是列向量的形式,可以理解为同时对多个属性做滤波估计操作。
状态转移噪声向量sk一般是服从多维高斯分布的,其均值为0向量,协方差矩阵为Q;观测噪声向量vk一般也是服从多维高斯分布的,其均值为0向量,协方差矩阵是R。最终滤波后得出的估计向量值用θk表示。
步骤1.2.4,估计组大小:组的大小可以简单地计算为集合Gk,t中ID的数量。
步骤1.2.5,检测交互:使用两个3-D向量来表示一个人脸在中的位置和方向。
使用OpenFace在RGB图像上定位人i相对于相机坐标系的面部位置,并获得面部方向的单位向量/>如果两个人的面部位置和方向满足以下条件,则认为他们正在交互:
其中,和/>是人i相对于相机坐标系的面部位置和方向的向量。该条件检查两个人的面部位置之间的距离是否大于通过外推面部方向计算出的点之间的距离。
所述步骤二中将检测到的组特征用于内聚估计:
步骤2.1,邻近凝聚力得分:人与人之间的凝聚力与他们之间的距离成反比。因此,由于接近度导致的凝聚力分数被公式化为组成员之间平均距离的倒数,如下所示:
其中,nk是组Gk,t中的成员数量。
步骤2.2,步行速度凝聚力得分:将每个组的平均步行速度与中所有检测到的人的平均步行速度进行比较。因此,步行组/>因其步行速度而产生的凝聚力分数的公式化为,
其中,N是在RGB图像中检测到的人数。对于如果∑||vj||=0,即该组是静态的,则Cw(Gk,t)=Kw·η,η是用户设置的最大常数值。
步骤2.3,群体凝聚力得分:群体k的凝聚力与群体规模nk成正比,组大小凝聚力分数计算为,
Cs(Gk,t)=Ks·nk (9)
步骤2.4,交互凝聚力得分:组中任意两个人之间的交互条件(方程6)可以应用于组中的所有配对,它对组内凝聚力得分的贡献可以重写为:
其中,θi,j是相机坐标系的X-Y平面中人脸方向向量和/>之间的角度。θi,j在/>区间内,因为面部方向在此范围内是准确的。直观地说,当人们面对面时,凝聚力得分为正且大于1更佳,否则为负。故选择比率/>因为当/>时,它属于范围/> 由于cos()是偶函数,因此sign()函数可确保公式对角度的符号敏感。
步骤2.5,整体群体凝聚力指标:使用等式7、8、9、10中的个体凝聚力分数,在时间t(Gk,t)时一组的总凝聚力分数给出为,
Ctot(Gk,t)=(Cp+Cw+Cs+Ci) (11)
在将Kp,Kw,Ks,Ki不同特征添加到总内聚分数之前,先对其进行权衡。如果无法检测到任何特征,则它们对Ctot的贡献将为零。一个组的整体内聚度量Ctot(Gk,t)的值是有界的。其中,Kp,Kw,Ks,Ki是每个特征的比例和权重常数,Kp+Kw+Ks+Ki=1。Cp∈(0,Kp·Γ),Γ被用作对人进行分组的阈值。Cw的最大值是Kw·η,这是一个大的有限常数,当组是静态时使用。Cs以Kg·nk为界,Kg·nk是有限的。Ci的取值范围如下:
在凝聚力度量上使用这些界限来计算适当的阈值,这些阈值用于将组分类为低、中和高凝聚力组。
所述步骤三中为了提高机器人路径的自然度,提出的方法包括以下步骤:
步骤3.1,在机器人的传感区域内识别潜在的冻结组,并在时间段th后预测它们的位置;
步骤3.2,使用每个组成员的预测未来位置为每个组构建一个PFZ;
步骤3.3,计算偏差角以避免组PFZ,同时考虑到每个组的凝聚力。如果没有找到可行的解决方案,机器人将在场景中凝聚力最低的组之间导航。
所述步骤三的子步骤3.1中的可能冻结组:时间th后极有可能引起FRP的行人组。这些组是根据他们平均步行方向的条件确定的。随着时间的推移,满足这些条件的组会更靠近机器人,故预测潜在冻结组成员的未来位置为
其中,是平均组步行向量,计算为组成员的步行向量的平均值,M是潜在冻结组的总数。
所述步骤三的子步骤3.2中的组PFZ:机器人极有可能冻结的组附近区域。为每个可能冻结的组构建一个PFZ作为
其中,CH()以点为参数的凸包函数。每个潜在冻结组的PFZ都是针对未来时刻t+th计算的。
所述步骤三中的子步骤3.3中的计算偏差角:如果机器人的当前路径将其导航到任何一组PFZk(意味着在时间th后发生FRP),则计算偏差角以避免它。使用绕Z轴的旋转矩阵,机器人的当前速度vrob偏离了/>如下所示:
这个方程意味着本发明的导航方法使机器人偏离其目标的量最小,因此它不会进入任何组的PFZ。然而,在密集场景中,当机器人遇到许多潜在的冻结组及其对应的PFZ时,等式16可能无法计算的可行解。在这种情况下,一个潜在的解决方案是让机器人通过低内聚群的PFZ。在这种情况下,将偏差角公式化为,
其中PFZmin是场景中具有最小内聚力的组的PFZ。由于低内聚基团的渗透性高,上述公式也降低了冻结的可能性。
在本发明一个较佳的实例中,在类似走廊的场景中,行人数量不同(10-50),机器人需要穿过行人才能到达目标。行人被赋予随机的初始位置,根据这些位置,他们被分类为组,并计算PFZfroz和组PFZs(PFZk)。使用以下指标:相对于连接起点和目标位置的线测量的平均偏差角;冻结率测量为机器人在总试验次数中停止/冻结的次数;标准化路径长度测量为机器人在连接起点和目标位置的线段上的路径长度。与相同场景下的Frozone相比,本发明的方法在所有这些指标方面的值都较低。随着人群规模、密度或群体数量的增加,Frozone的保守公式使机器人以非常高的速度冻结。从这个分析中可以观察到本发明的方法产生了具有高度社会依从性和自然性的路径。此外,该方法显着减少了冻结行为的发生。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下被实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
尽管根据有限数量的实施例描述了本发明,但是受益于上面的描述,本技术领域内的技术人员明白,在由此描述的本发明的范围内,可以设想其它实施例。此外,应当注意,本说明书中使用的语言主要是为了可读性和教导的目的而选择的,而不是为了解释或者限定本发明的主题而选择的。因此,在不偏离保护范围和精神的情况下,对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。对于本发明的范围,对本发明所做的公开是说明性的,而非限制性的。
以上所述仅是本发明的优选实施方式,应当指出:对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (5)
1.一种基于行人轨迹预测的移动机器人路径规划方法,其特征在于,包括如下步骤:
步骤1:检测、跟踪和定位每个行人相对于相机帧的位置,获得组特征;
步骤2:将检测到的组特征用于内聚估计,获得群体凝聚力指标;
步骤3:利用群体凝聚力指标引入符合社交规则的导航算法;
步骤1中,检测和跟踪的步骤如下:
步骤1.1,移动机器人的相机获得 是在时刻t捕获的宽度为w和高度为h的RGB图像,YOLOv5在时刻t为RGB图像中的每个检测到的行人i输出一组边界框β={βi};βi使用其在图像空间或像素坐标中的左上角和右下角表示,并为每个检测到的行人i分配一个唯一的整数作为ID;
其中,βi是ID为i的人的边界框;
步骤1.2,使用深度图像来估计每个检测到的行人相对于相机坐标系的距离,其每个像素都包含图像该位置处物体的接近度,以米为单位;/>中的像素包含最小和最大距离范围之间的值,取决于捕获图像的相机;
其中,是移动机器人的相机在时刻t捕获的宽度为w和高度为h的深度图像;
获得每个检测到的人相对于相机坐标系的距离后,定义组分类、估计接近度、估计步行速度和方向、估计组大小和检测交互;
步骤1.2.1,组分类:考虑任何一组人的IDs,IDs为任何一组人的ID的集合,在任何时间t,如果以下条件成立:
||pi,t-pj,t||2≥||pi,t+vi,t-(pj,t+vj,t)||2 (2)
那么集合Gk,t在图像中被归类为一个组;
其中,IDt是在中检测到的人员ID集;pi,t为人i在时间t相对于机器人/相机坐标系的位置向量,pj,t为人j在时间t相对于机器人/相机坐标系的位置向量,pi,t=(xi,t,yi,t),pj,t=(xj,t,yj,t),xi,t、yi,t、xj,t、yj,t均为机器人/相机坐标系的位置向量;vi,t是人i在时间t相对于机器人/相机坐标系的步行速度向量,/>vj,t是人j在时间t相对于机器人/相机坐标系的步行速度向量,/>
Xi,t是卡尔曼滤波器中使用的状态向量,用于在时刻t对人i进行步行向量估计,Γ是手动设置的距离阈值;式(1)确保人们彼此靠近,式(2)确保组成员朝同一方向行走;当|vi||,||vj||=0时为静态组,此时只使用式(1)进行分组;
步骤1.2.2,估计接近度:为了估计t时刻人与人之间的接近度,首先将YOLOv5在RGB图像中检测到的边界框叠加在深度图像上;为了估计人i与相机的距离di,t,计算以为中心的小方块内所有像素值的平均值;人i相对于相机的角位移ψi,t计算为:
其中,是边界框βi的质心,FOVRGBD是RGB-D相机的视野,w是YOLOv5算法中的模型参数;人i相对于相机的位置计算为:
[xi,tyi,t]=di,t×[cosψi,tsinψi,t] (4)
人i和人j之间的距离可以计算为:
步骤1.2.3,估计步行速度和方向:使用具有恒速运动模型的卡尔曼滤波器来估计中第i个人的步行向量vi;/>中所有检测到的人都使用状态向量Xt进行建模,/>中所有检测到的人的ID存储在集合IDt中,对于在先前RGB图像中检测到的所有行人,即具有先前初始化状态的行人,此时即为建模前的行人的状态向量Xt,先使用标准卡尔曼预测和更新步骤更新他们的状态,再使用具有预设方差的零均值高斯噪声来模拟过程和感测噪声;
步骤1.2.4,估计组大小:组的大小为集合Gk,t中ID的数量;
步骤1.2.5,检测交互:使用两个3-D向量来表示一个人脸在中的位置和方向;
使用OpenFace在RGB图像上定位人i相对于相机坐标系的面部位置,并获得面部方向的单位向量/>如果两个人的面部位置和方向满足以下条件,则认为他们正在交互:
其中,和/>是人i相对于相机坐标系的面部位置和方向的向量;该条件检查两个人的面部位置之间的距离是否大于通过外推面部方向计算出的点之间的距离;
步骤2中将检测到的组特征用于内聚估计:
步骤2.1,邻近凝聚力得分:人与人之间的凝聚力与他们之间的距离成反比;因此,由于接近度导致的凝聚力分数被公式化为组成员之间平均距离的倒数,如下所示:
其中,nk是组Gk,t中的成员数量;Kp是邻近度程度系数;Cp(Gk,t)是(Gk,t)组的邻近凝聚力得分;
步骤2.2,步行速度凝聚力得分:将每个组的平均步行速度与中所有检测到的人的平均步行速度进行比较;因此,步行组/>因其步行速度而产生的凝聚力分数的公式化为,
其中,N是在RGB图像中检测到的人数;对于如果∑||vj||=0,即该组是静态的,则Cw(Gk,t)=Kw·η,η是用户设置的最大常数值;Kw是步行速度系数;Cw(Gk,t)是(Gk,t)组的步行速度凝聚力得分;
步骤2.3,群体凝聚力得分:群体k的凝聚力与群体规模nk成正比,组大小凝聚力分数计算为,
Cs(Gk,t)=Ks·nk (9)
其中,Ks是群体规模系数;Cs(Gk,t)是(Gk,t)组的群体凝聚力得分;
步骤2.4,交互凝聚力得分:组中任意两个人之间的交互条件,即式(6)应用于组中的所有配对,它对组内凝聚力得分的贡献重写为:
其中,θi,j是相机坐标系的X-Y平面中人脸方向向量和/>之间的角度;θi,j在/>区间内,即,当人们面对面时,凝聚力得分为正且大于1更佳,否则为负;故选择比率/>因为当/>时,它属于范围/>由于cos()是偶函数,因此sign()函数可确保公式对角度的符号敏感;Ki是交互系数;Ci(Gk,t)是(Gk,t)组的交互凝聚力得分;
步骤2.5,整体群体凝聚力指标:使用式(7)~(10)中的个体凝聚力分数,在时间t(Gk,t)时一组的总凝聚力分数给出为,
Ctot(Gk,t)=(Cp+Cw+Cs+Ci)(11)
在将Kp,Kw,Ks,Ki不同特征添加到总内聚分数之前,先对其进行权衡;如果无法检测到任何特征,则它们对Ctot的贡献为零;一个组的整体内聚度量Ctot(Gk,t)的值是有界的;其中,Kp,Kw,Ks,Ki是每个特征的比例和权重常数;Kp+Kw+Ks+Ki=1; 被用作对人进行分组的阈值;Cw的最大值是Kw·η,这是一个大的有限常数,当组是静态时使用;Cs以Kg·nk为界,Kg·nk是有限的;Ci的取值范围如下:
在凝聚力度量上使用这些界限来计算适当的阈值,这些阈值用于将组分类为低、中和高凝聚力组;
步骤3中为了提高机器人路径的自然度,提出的方法包括以下步骤:
步骤3.1,在机器人的传感区域内识别潜在的冻结组,并在时间段th后预测它们的位置;
步骤3.2,使用每个组成员的预测未来位置为每个组构建一个潜在冻结组PFZ;
步骤3.3,计算偏差角以避免潜在冻结组PFZ,同时考虑到每个组的凝聚力;如果没有找到可行的解决方案,机器人在场景中凝聚力最低的组之间导航。
2.权利要求1所述的一种基于行人轨迹预测的移动机器人路径规划方法,其特征在于,
步骤3.1中的潜在的冻结组为时间th后可能引起冷冻机器人问题FRP的行人组;这些组是根据他们平均步行方向的条件确定的;随着时间的推移,满足这些条件的组会更靠近机器人,故预测潜在的冻结组成员的未来位置为:
其中,是平均组步行向量,计算为组成员的步行向量的平均值,M是潜在的冻结组的总数;/>是坐标系的位置向量。
3.权利要求2所述的一种基于行人轨迹预测的移动机器人路径规划方法,其特征在于,
步骤3.2中的潜在冻结组PFZ为机器人可能冻结的组附近区域;为每个可能冻结的组构建一个PFZ:
其中,CH()以点为参数的凸包函数;每个潜在冻结组PFZ都是针对未来时刻t+th计算的。
4.权利要求3所述的一种基于行人轨迹预测的移动机器人路径规划方法,其特征在于,步骤3.3中的计算偏差角的方法为:如果机器人的当前路径将其导航到任何一组PFZk,则计算偏差角以避免它;使用绕Z轴的旋转矩阵,机器人的当前速度vrob偏离了/>如下所示:
其中,grob是机器人的目标点;v′rob是机器人的当前速度vrob偏离了后的速度;/>是绕Z轴的3-D旋转矩阵,即绕垂直于机器人平面的3-D旋转矩阵。
5.权利要求3所述的一种基于行人轨迹预测的移动机器人路径规划方法,其特征在于,在密集场景中,当机器人遇到许多潜在的冻结组及其对应的潜在冻结组PFZ时,式(16)无法计算的可行解;在这种情况下,解决方案是让机器人通过低内聚群的PFZ;在这种情况下,将偏差角公式化为,
其中,PFZmin是场景中具有最小内聚力的组的潜在冻结组PFZ;是潜在冻结组PFZ中的低内聚群的PFZ。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111053999.2A CN113721633B (zh) | 2021-09-09 | 2021-09-09 | 一种基于行人轨迹预测的移动机器人路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111053999.2A CN113721633B (zh) | 2021-09-09 | 2021-09-09 | 一种基于行人轨迹预测的移动机器人路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113721633A CN113721633A (zh) | 2021-11-30 |
CN113721633B true CN113721633B (zh) | 2023-10-17 |
Family
ID=78682810
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111053999.2A Active CN113721633B (zh) | 2021-09-09 | 2021-09-09 | 一种基于行人轨迹预测的移动机器人路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113721633B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115129049B (zh) * | 2022-06-17 | 2023-03-28 | 广东工业大学 | 一种具有社交意识的移动式服务机器人路径规划***和方法 |
CN118192613A (zh) * | 2024-05-13 | 2024-06-14 | 天津市品茗科技有限公司 | 一种ai机器人动态避障方法及*** |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107643752A (zh) * | 2017-05-09 | 2018-01-30 | 清研华宇智能机器人(天津)有限责任公司 | 基于行人轨迹预测的全向移动机器人路径规划算法 |
CN110285813A (zh) * | 2019-07-01 | 2019-09-27 | 东南大学 | 一种室内移动机器人人机共融导航装置及方法 |
WO2020001395A1 (zh) * | 2018-06-29 | 2020-01-02 | 大连民族大学 | 道路行人分类方法及二维世界坐标系的俯视行人风险量化方法 |
CN111121807A (zh) * | 2019-12-18 | 2020-05-08 | 安徽机电职业技术学院 | 一种移动机器人路径规划方法 |
CN113269098A (zh) * | 2021-05-27 | 2021-08-17 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于无人机的多目标跟踪定位与运动状态估计方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2043045B1 (de) * | 2007-09-20 | 2011-09-07 | Delphi Technologies, Inc. | Verfahren zur Objektverfolgung |
-
2021
- 2021-09-09 CN CN202111053999.2A patent/CN113721633B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107643752A (zh) * | 2017-05-09 | 2018-01-30 | 清研华宇智能机器人(天津)有限责任公司 | 基于行人轨迹预测的全向移动机器人路径规划算法 |
WO2020001395A1 (zh) * | 2018-06-29 | 2020-01-02 | 大连民族大学 | 道路行人分类方法及二维世界坐标系的俯视行人风险量化方法 |
CN110285813A (zh) * | 2019-07-01 | 2019-09-27 | 东南大学 | 一种室内移动机器人人机共融导航装置及方法 |
CN111121807A (zh) * | 2019-12-18 | 2020-05-08 | 安徽机电职业技术学院 | 一种移动机器人路径规划方法 |
CN113269098A (zh) * | 2021-05-27 | 2021-08-17 | 中国人民解放军军事科学院国防科技创新研究院 | 一种基于无人机的多目标跟踪定位与运动状态估计方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113721633A (zh) | 2021-11-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10970559B2 (en) | People flow estimation device, people flow estimation method, and recording medium | |
CN113721633B (zh) | 一种基于行人轨迹预测的移动机器人路径规划方法 | |
CN110118560B (zh) | 一种基于lstm和多传感器融合的室内定位方法 | |
CN107907124A (zh) | 基于场景重识的定位方法、电子设备、存储介质、*** | |
TW201425971A (zh) | 圖資校正裝置、系統和方法 | |
Wu et al. | A pedestrian dead-reckoning system for walking and marking time mixed movement using an SHSs scheme and a foot-mounted IMU | |
JP2019212291A (ja) | コンピュータビジョンと結合される地磁気信号に基づいた室内位置決めシステム及び方法 | |
Yang et al. | Human posture recognition in intelligent healthcare | |
Zachariah et al. | Self-motion and wind velocity estimation for small-scale UAVs | |
CN114494329B (zh) | 用于移动机器人在非平面环境自主探索的导引点选取方法 | |
Luo et al. | Multisensor integrated stair recognition and parameters measurement system for dynamic stair climbing robots | |
CN103744110B (zh) | 超声与单目视觉传感器结合的障碍物识别装置 | |
Navarro-Serment et al. | LADAR-based pedestrian detection and tracking | |
Osman et al. | Indoor SLAM using a foot-mounted IMU and the local magnetic field | |
KR20160118783A (ko) | 사람 계수 방법 및 장치 | |
Seer et al. | Kinects and human kinetics: a new approach for studying crowd behavior | |
Xiong et al. | Crowd density estimation based on image potential energy model | |
Liu et al. | A visual SLAM method assisted by IMU and deep learning in indoor dynamic blurred scenes | |
Faramondi et al. | Hybrid map building for personal indoor navigation systems | |
Ding et al. | Identifying and approaching for obscured stairs | |
Kessler et al. | I'll keep you in sight: Finding a good position to observe a person | |
Song et al. | A Design for Improvement of Visual SLAM in Dynamic Environments Using Feature-Point Removal of Moving Persons | |
Zheng et al. | Dyna VIO: Real-Time Visual-Inertial Odometry with Instance Segmentation in Dynamic Environments | |
Owczarek et al. | Particle Filter for Reliable Estimation of the Ground Plane from Depth Images in a Travel Aid for the Blind | |
Абдула | One Particle Filter Based Algorithm of Tracking of a Moving Object in the Sequence of Images |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |