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

1.本发明属于机器人路径规划领域，是一种基于行人轨迹预测与社交约束的移动机器人路径规划方法。


背景技术：

2.随着人工智能与机器人技术的不断发展与进步，移动机器人在工业、服务业、农业、医疗等领域得到了快速发展和广泛应用，人们对机器人性能的要求也逐步提升。机器人在复杂的多人动态环境中能充分考虑行人运动与社交规律等因素，实现智能行走成为机器人的一项重要功能。
3.传统的路径规划往往只是让机器人在行走过程中实时躲避环境中的障碍物，从而规划出一条从起点到目标点的无障碍路径。但对于有意识的行人不能进行简单的避障，机器人需要在路径规划的过程中对行人的运动行为、多个行人之间的交互进行提前预判并避让，避免机器人行走过程中影响行人的正常活动与交流。比如：当两个行人正在以为1米以内的间距同向行走时，机器人不能从他们中间穿过，这不符合人类的社交规律，故应该让机器人绕行。因此，在机器人路径规划时提前感知行人的运动意图，增加机器人的社交约束属性，能够降低机器人行走过程中对行人活动的影响，实现机器人与行人安全交互、和谐共处的目标。


技术实现要素：

4.鉴于上述传统技术中的不足之处，本发明提供了一种基于行人轨迹预测与社交约束的移动机器人路径规划方法。该方法所采用的技术方案是通过实时预测行人的未来轨迹，并根据行人的未来轨迹和行人之间的距离，在行人周围生成机器人不可通行的空间，使机器人路径规划时能及时避让行人空间，达到增加机器人的预判性和社交约束属性的目的。
5.本发明具体方案如下所述：
6.所述的基于行人轨迹预测与社交约束的移动机器人路径规划方法包括两个模块，构建行人实时检测跟踪并预测模块和构建具有社交约束的机器人路径规划模块，其中：
7.所述的行人实时检测跟踪并预测模块具体方案如下：
8.s1.搭建yolov5_deepsort网络，使用公开数据集对yolov5_deepsort进行预训练，保存训练好的权重文件用于行人的实时检测与跟踪；
9.s2.采用realsense d435深度相机向yolov5_deepsort网络实时输入数据，加载s1步骤中保存的权重文件，然后通过yolov5_deepsort的yolov5网络部分对场景中的行人目标进行实时检测，得到行人目标的检测框，deepsort部分以yolov5输出的行人检测结果为输入，对行人在每一时刻的位置进行跟踪并更新位置信息；
10.s3.以s2步骤中跟踪到的每一时刻的检测框中心点像素坐标代替行人检测框，得
到每个行人在时间上的像素坐标序列，记录每个行人的编号和像素坐标序列，将每一帧的像素坐标与行人的深度信息进行帧间匹配，用于将行人像素坐标转换为世界坐标；
11.s4.将相机位置设为世界坐标系的原点，此时相机坐标等于世界坐标。通过相机的内参将像素坐标转换为图像坐标再转换到相机坐标，即可实时获取行人的相机坐标，即世界坐标；
12.s5.使用pytorch深度学习框架搭建social gan网络，使用公开数据集中对social gan网络进行预训练，social gan网络包括生成器和判别器，其中生成器的输入是观测到的行人最近时刻的轨迹序列，通过对输入的轨迹进行非线性运算，提取行人的位移状态向量，经过预测时段序列的传递，获得行人未来时刻的轨迹序列；
13.s6.social gan判别器的输入是s5步骤中生成的预测轨迹序列与真实轨迹序列，判别器判断输入序列是否为真实轨迹。通过计算网络的损失函数，优化生成器和判别器的各层网络，生成器和判别器交替训练，直至生成器输出的预测轨迹可以模拟真实轨迹。
14.s7.保存网络训练后的权重，在实时检测跟踪与预测时加载该网络权重，将实时检测跟踪得到的最近4秒的轨迹送入social gan网络的生成器部分，用来获取预测的未来6秒的行人轨迹。
15.所述的具有社交约束的机器人路径规划模块具体方案如下：
16.s1.当场景中的行人之间的距离大于一定阈值时，说明行人之间无交互的可能性。此时，根据权利要求2得到的每个行人的预测轨迹的长度，在行人周围建立机器人不可通行的安全空间，如图2所示，以此增加机器人路径规划时的预判性；
17.s2.当场景中的行人之间的距离小于一定阈值时，说明行人之间有交互的可能性。此时，将s1步骤中的每个行人的安全空间合并为行人交互空间，如图3所示，在多个行人之间建立机器人不可通行的交互空间，以此增加机器人路径规划时的社交约束属性；
18.s3.将s1步骤中的行人安全空间和s2步骤中的行人交互空间融入路径规划的代价地图，使其作为障碍物出现在机器人路径规划的栅格地图中；
19.s4.采用a*算法进行全局路径规划，规划出一条从起点到目标点的最短路径；采用动态窗口法进行局部路径规划，使机器人在运动过程中对行人的安全空间和交互空间合理避让，从而保证了行人运动的安全性和舒适性。
20.进一步地，所述s1步骤中的行人安全空间映射在栅格地图上近似一个动态组合椭圆，如图2所示。该动态组合椭圆包括行人行走的前方是一个根据二维高斯分布确定的一个动态椭圆区域，行人行走的后方是一个半圆区域。其中，行人前方的动态椭圆的长轴取决于即预测的行人轨迹长度，当行人速度越快，预测的轨迹长度越长，行人前方的椭圆越扁，决定行人左右两侧的安全距离。f(x，y)表示不同速度下的行人空间大小，x0，y0分别表示行人当前的位置；
21.进一步地，所述s1步骤中的行人安全空间和s2步骤中的行人交互空间是根据人与人在社交时对不同的社交距离感到安全度和舒适度进行划分的，一般行人感到安全的距离在0.45-1.2m，行人之间交互距离在1.2-3.6m。
22.有益效果
23.本发明所述的基于行人轨迹预测与社交约束的移动机器人路径规划方法，通过对实时检测跟踪到的行人进行轨迹预测，并建立基于预测轨迹的行人空间和行人交互空间，使机器人在行驶过程中能提前感知行人的运动轨迹，对行人的交互空间合理避让，保障了行人运动的安全性和舒适性。
附图说明
24.图1是本发明的整体流程图。
25.图2是行人的二维高斯空间模型图。
26.图3是行人交互空间模型图。
27.图4是行实时跟踪预测前视图。
28.图5是机器人路径规划仿真环境图。
29.图6是保障行人空间的机器人路径规划仿真图。
30.图7是保障行人交互的机器人路径规划仿真图。
具体实施方式
31.参照图1，一种基于行人轨迹预测与社交约束的移动机器人路径规划方法，包括两个模块，行人实时检测跟踪并预测模块和具有社交约束的机器人路径规划模块，其中：
32.行人实时检测跟踪并预测模块包括以下行人的实时检测与跟踪、将实时跟踪到的行人的像素坐标转换为世界坐标以及对跟踪的轨迹进行实时预测三个步骤，具体步骤如下：
33.1.行人实时检测与跟踪包括如下步骤：
34.1.1)采用voc，coco，imagenet等经典目标检测数据集对yolov5_deepsort目标检测与跟踪模型进行预训练；
35.1.2)将realsense d435深度相机固定在室内大厅的天花板，用于采集实时检测跟踪的数据，将相机的位置设为世界坐标系的原点，此时相机坐标等于世界坐标；
36.1.3)加载已训练的权重数据，通过外接realsense d435深度相机向网络模型实时发送视频流，通过yolov5_deepsort的yolov5网络部分对场景中的行人目标进行实时检测，得到行人目标的检测框，deepsort部分以yolov5输出的行人检测结果为输入，对行人在每一时刻的位置进行跟踪并更新位置信息，最后实时渲染到图像；
37.1.4)将跟踪到的每一时刻的检测框中心点像素坐标代替行人检测框，得到每个行人在时间上的像素坐标序列，记录每个行人的编号和像素坐标序列。然后获取深度传感器的深度值，创建对齐对象，将深度图与rgb图像的每一帧对齐并获取，用于将行人像素坐标转换为世界坐标；
38.2.将跟踪的行人像素坐标转换为世界坐标包括如下步骤：
39.2.1)像素坐标系与世界坐标系之间的转换公式如下：
40.41.式中，zc是通过深度相机获得行人的深度信息，u0和v0表示图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数；f表示焦距，dx和dy表示一个像素在横向和纵向所占长度；[xe，ye，ze，1]
t
表示物体所在的世界坐标，r和t分别是旋转矩阵和平移矩阵，两者组成3*4矩阵即为相机的外参矩阵；
[0042]
2.2)通过在s3中的公式的左右两边同时左乘的逆矩阵得到行人的相机坐标，由于已将相机坐标设为世界坐标系的原点，得到的相机坐标即世界坐标；
[0043]
3.对跟踪的轨迹进行实时预测的过程包括如下步骤；
[0044]
3.1)使用pytorch深度学习框架搭建social gan网络，设置学习率α＝0.001，学习衰减率y＝0.6，优化器使用adam算法，使用公开数据集中对social gan网络进行预训练，social gan网络包括生成器和判别器，其中生成器的输入是观测到的行人最近时刻的轨迹序列，通过对输入的轨迹进行非线性运算，提取行人的位移状态向量，经过预测时段序列的传递，获得行人未来时刻的轨迹序列；
[0045]
3.2)social gan判别器的输入是3.1步骤中生成的预测轨迹序列与真实轨迹序列，判别器判断输入序列是否为真实轨迹。通过计算网络的损失函数，优化生成器和判别器的各层网络，生成器和判别器交替训练，直至生成器输出的预测轨迹可以模拟真实轨迹；
[0046]
3.3)加载已训练的权重数据，以每秒2帧的速率采集行人世界坐标，输入前8帧作为真实轨迹用来预测未来12帧的坐标轨迹，即每一时刻实时获取行人最近4秒的坐标轨迹用于预测，得到未来6秒的预测轨迹；
[0047]
3.4)通过一个数组存放实时检测跟踪得到的行人轨迹序列，如果数组不为空，则根据行人编号对行人的轨迹进行过滤，去除不连续的坐标轨迹，保留每个时刻连续的最后4秒的坐标轨迹记为行人历史轨迹；对于每一个行人，用后一帧的坐标减去前一帧的坐标，计算每个行人两帧之间的位移变化；
[0048]
3.5)将3.4步骤中的行人历史轨迹、行人两帧之间的相对位移以及轨迹序列的始末位置三个参数转换为pytorch的数据格式，最后将数据送入social gan网络进行预测；
[0049]
3.6)将3.5步骤中的三个参数输入到social gan生成器中，经过计算各行人的位移隐藏状态向量，经过预测时段序列的传递得到预测轨迹序列，将历史轨迹与预测轨迹连接为生成轨迹输出，实时预测的轨迹如图4所示；
[0050]
具有社交约束的机器人路径规划模块根据行人的预测轨迹和个人空间模型在行人周围建立机器人不可通行区域，从而达到增加机器人预判性和社交约束属性的目的。个人空间模型是根据人与人在社交时对不同的社交距离感到的安全度和舒适度，将人的空间划分为四个区域，分别是私密空间，个人空间，社交空间和公共空间。一般行人感到安全的个人空间距离在0.45-1.2m，行人之间交互空间的距离在1.2-3.6m。具有社交约束的机器人路径规划模块包括以下步骤：
[0051]
1.当行人之间距离大于一定阈值时，该阈值一般设为3.6m，即当行人之间距离大于3.6米时，判定当前行人之间无交互。此时，根据行人预测轨迹建立行人的个人安全空间
包括如下步骤：
[0052]
1.1)首先，在gazebo中搭建如图5所示的机器人工作的室内大厅仿真环境，通过栅格法建立仿真环境的栅格地图；设定机器人在环境中的起始点，添加两个以上的行人模型，并对行人模型进行实时检测跟踪并预测，得到行人模型未来时刻的坐标轨迹；
[0053]
1.2)在已知行人下一时刻可能的运动轨迹后，需要通过预测轨迹建立行人安全空间。首先，行人安全空间映射在栅格地图上近似一个动态组合椭圆，如图2所示。该动态组合椭圆包括行人行走的前方是一个根据二维高斯分布确定的一个动态椭圆区域，行人行走的后方是一个半圆区域。
[0054]
1.3)其中，行人前方的动态椭圆的长轴取决于即预测的行人轨迹长度，当行人速度越快，预测的轨迹长度越长，行人前方的椭圆越扁，决定行人左右两侧的安全距离，一般取0.45m。f(x，y)表示不同速度下的行人空间大小，xo，yo分别表示行人当前的位置；
[0055]
2.当行人之间距离小于3.6m时，判定当前行人之间可能交互。此时，将行人的安全空间合并为行人交互空间，在行人周围建立机器人不可通行的交互空间；
[0056]
3.将步骤1，步骤2得到的行人安全空间和行人交互空间融入路径规划的代价地图，使其作为障碍物出现在机器人路径规划的栅格地图中；采用a*算法进行全局路径规划，规划出一条从起点到目标点的最短路径；采用动态窗口法进行局部路径规划，使机器人在运动过程中对行人的安全空间和交互空间合理避让，从而保证了行人运动的安全性和舒适性。运行gazebo中创建的如图5所示的室内大厅仿真环境，启动rviz，显示该仿真环境的轮廓。使用rviz的2d nav goal工具标定完目标点之后，会在地图中生成规划好的路径，如图6所示。该路径能够及时规避环境中的障碍物与行人，规划出一条安全无碰撞的路径。当环境中有两人以上并行或交互时，机器人能够对行人簇绕行，而不是从中间穿过，如图7所示。