人机共融的楼宇间路径规划方法、计算机装置及程序产品

1.本发明涉及楼宇内路径规划领域，特别是一种人机共融的楼宇间路径规划方法、计算机装置及程序产品。


背景技术：

2.楼宇内针对人机共融环境下的路径规划是室内机器人导航规划领域的重要内容之一，通过建立三维栅格地图，并基于行人与机器人运动特征的差异进行二维投影，分别生成面向行人和机器人的二维栅格地图。对面向行人的二维栅格地图采用引入时间信息的a*算法可实现行人室内可跨越障碍物的导航技术，对于大型室内环境不熟悉的行人起到了很好的导航指引作用。目前室内机器人有着广泛的应用前景，考虑到室内机器人行驶过程中的人机安全性，对面向机器人的二维栅格地图采用一种适用于人机共融环境中的a*算法，生成一条基于行人预测与时空一致性约束的全局路径，保证了机器人在行驶过程中可提前预测行人在一段时间内的行驶轨迹并提前避开。但考虑到在实现a*算法过程中行人预测的时间为估计值，与实际行人行驶时间存在一定的差异，因此需要采用基于背景对消与dwa局部规划算法实现动态行人轨迹预测与避障的功能；其中背景对消法可根据相邻两帧之间的激光雷达点云数据，通过一系列的坐标变换识别出环境中的静态障碍物与行人，并针对行人进行轨迹预测，而后结合dwa局部规划算法对行人进行提前避障，从而不影响行人的行驶速度、方位等，提高了行驶过程中的安全性。而本发明中所述的跨楼层导航通过结合a*算法和电梯调度模块，可实现自主运输机器人从起始楼层前往目标楼层而不需要人为干扰，在智能化程度上更高。然而，在实际场景中，仍然有以下问题制约机器人在楼宇场景内的应用。
3.(1)针对行人在大型室内场景不熟悉的问题，需要设计一种可适用于行人参考的全局路径；考虑到行人运动特征与机器人的差异：行人可跨越低矮障碍物、在机场等环境行人可通过闸机通道等，需要根据行人运动特征生成面向行人的二维栅格地图，并基于该地图采用全局规划算法生成行人全局路径，行人参考该路径行驶可跨越低矮障碍物、通过闸机通道等。
4.(2)针对室内机器人要实现不同楼层间的导航，需要进行跨楼层行驶，如何生成一条基于行人预测与时空一致性约束、可跨楼层的楼宇间全局路径，是当下一大难点。而本发明通过建立三维点云地图并机器人的运动特征进行二维投影，生成面向机器人的二维栅格地图，对二维栅格地图采用一种适用于人机共融环境中的a*算法实现机器人在单楼层间的无碰撞行驶，待机器人行驶至电梯处，通过结合机器人与电梯调度模块间实现一种全智能化的电梯运输技术，可将机器人从起始楼层运输至目标楼层，随后在目标楼层导航至目标点。该多层二维栅格地图可扩展性高，在程序运行资源占用上更少并且运行时间更快。
5.(3)针对室内机器人在导航过程中的避障问题，本发明提出了一种基于背景对消与dwa局部规划算法的行人轨迹预测与动态避障方法。该方法可通过相邻两帧间的激光雷达点云数据实现静态障碍物与行人的分辨，同时对行人进行轨迹预测，再结合dwa局部规划
算法对行人进行提前避让，大大提高了行驶过程中的人及安全性。
6.发明专利申请《一种基于vfh*局部路径规划方法的全向移动机器人自主导航系统》(申请号：202010043476.9)公开了一种基于vfh*局部路径规划方法的全向移动机器人自主导航系统，该方法通过激光雷达构建栅格地图，并将a*算法的思想引入vfh*避障方法中进行改进，使得移动机器人能够选择尽可能优的避障方向。但该发明仅局限于二维平面下未考虑行人预测的机器人自主导航，在人机共融环境下的行驶具有很大的隐患；同时该发明生成的局部轨迹存在不连续性导致不适合机器人进行跟踪行驶。
7.发明专利申请《一种室内移动机器人人机共融导航装置及方法》(申请号：201910588551.7)公开了一种室内移动机器人人机共融导航装置及方法，该方法实现的仅仅是室内单楼层的导航，并且该方法仅针对行人进行了一种简单的轨迹预测。
8.硕士学位论文《基于多层代价地图的移动机器人人机共融导航技术研究》提出了一种human-aware导航方法，该方法能够提升路径规划的性能、提高人类的舒适度并遵守一定的社会规则。同时该方法虽然建立了多层代价地图，但该多层代价地图仅用于单楼层的导航以及行人预测。


技术实现要素：

9.本发明所要解决的技术问题是，针对现有技术不足，提供一种人机共融的楼宇间路径规划方法、计算机装置及程序产品，提高面向人机共融环境中的机器人行驶时的人机安全性。
10.为解决上述技术问题，本发明所采用的技术方案是：一种人机共融的楼宇间路径规划方法，包括以下步骤：
11.s1、装配有激光雷达的机器人在楼宇间行驶时，采集行驶过程中激光雷达环境信息，构建三维栅格地图并基于行人与机器人运动特征的差异进行二维投影，生成面向行人的二维栅格地图和面向机器人的二维栅格地图；
12.s2、对面向行人的二维栅格地图采用引入时间信息的a*算法生成具有时空间信息的、面向行人的全局路径；
13.s3、获取机器人全局位置，根据面向行人的全局路径对面向机器人的二维栅格地图采用a*算法，生成一条基于行人预测与时空一致性约束的、面向机器人的全局路径；
14.s4、根据激光雷达采集到的环境信息，基于背景对消法进行动态行人轨迹预测，并依据预测轨迹以及面向机器人的全局路径，采用dwa局部规划算法进行动态避障。
15.本发明公开了人机共融的楼宇间路径规划方法、计算机装置及程序产品，本发明通过步骤s1基于行人与机器人运动特征的差异进行二维投影，生成面向行人的二维栅格地图和面向机器人的二维栅格地图；其中面向行人的二维栅格地图将行人可跨越的低矮障碍物置为空闲栅格，在后续采用步骤s2生成的具有时空间信息、面向行人的全局路径可能会经过这些低矮障碍物，但均属于行人可通行区域；而对于机器人来说，即使是低矮障碍物却仍然无法跨越，因此在面向机器人的二维栅格地图中该部分为障碍物栅格，机器人需要避开该区域。同时步骤s3中不仅需要机器人的全局位置信息、目标位置信息，还需要步骤s2生成的具有时空间信息、面向行人的全局路径信息，并采用a*算法生成一条基于行人预测与时空一致性约束的、面向机器人的全局路径，该路径通过对行人行驶轨迹的预测，实现提前
避让功能；但考虑到行人行驶速度的不确定性，因此步骤s3生成的全局路径可信度不充分，为了保障行驶过程中的人机安全性，本发明基于背景对消法进行动态行人轨迹预测，并采用dwa局部规划算法进行动态避障，提高了机器人行驶过程中的人机安全性。
16.步骤s1的具体实现过程包括：
17.s1.1、对于激光雷达探测到的点云数据，采用点云配准算法得到三维点云地图；
18.s1.2、对三维点云地图进行滤波处理，将滤波后的三维点云地图转为分辨率为resolution的三维栅格地图；
19.s1.3、设定高度范围h
pedestrianmin
≤z
gridoctree
，其中h
pedestrianmin
为行人可翻越障碍物的最大高度，对三维栅格地图在z轴上投影至xoy平面内，得到面向行人的二维栅格地图；若在三维栅格地图中横、纵坐标为(x
gridoctree
,y
gridoctree
)，z轴满足h
pedestrianmin
≤z
gridoctree
的范围内不存在三维栅格，则二维栅格(x
gridoctree
,y
gridoctree
)为空闲栅格；若存在三维栅格，则二维栅格为障碍物栅格，其中(x
gridoctree
,y
gridoctree
,z
gridoctree
)为三维栅格中心点坐标；
20.s1.4、设定高度范围z
gridrobot
≤h
robotmax
，其中h
robotmax
为机器人最大高度，对三维栅格地图在z轴上投影至xoy平面内，得到面向机器人的二维栅格地图；若在三维栅格地图中横、纵坐标为(x
gridrobot
,y
gridrobot
)，z轴满足0≤z
gridrobot
≤h
robotmax
的范围内不存在三维栅格，则二维栅格(x
gridrobot
,y
gridrobot
)为空闲栅格；若存在三维栅格，则二维栅格为障碍物栅格，其中(x
gridrobot
,y
gridrobot
,z
gridrobot
)为三维栅格中心点坐标；
21.s1.5、返回步骤s1.1，构建其它楼层的三维栅格地图，并基于行人与机器人运动特征的差异进行二维投影，生成面向行人的二维栅格地图和面向机器人的二维栅格地图。
22.考虑到行人与机器人行驶特征的差异，无法通过一个二维栅格地图同时生成既可适用于行人又可适用于机器人导航的二维栅格地图，本发明根据激光雷达采集的环境数据建立的三维栅格地图，基于行人与机器人行驶特征的差异进行二维投影，生成面向行人和机器人的二维栅格地图，并且生成的二维栅格地图尺寸一致，因此对面向行人的二维栅格地图生成的全局路径也可用于面向机器人的二维栅格地图中，便于后续机器人的全局路径生成，实现人机协同。
23.步骤s2的具体实现过程包括：
24.s2.1、输入目标点goal
pedestrian
(x
pedestriangoal
,y
pedestriangoal
,level
pedestriangoal
)、行人当前所处位置start
pedestrian
(x
pedestrianstart
,y
pedestrianstart
,level
pedestrianstart
)，其中x
pedestriangoal
和y
pedestriangoal
为目标点在xoy平面内的二维坐标，level
pedestriangoal
为目标所在楼层，x
pedestrianstart
和y
pedestrianstart
为行人在xoy平面内的二维坐标，level
pedestrianstart
为行人当前所在楼层；
25.s2.2、判断是否满足level
pedestriangoal
＝level
pedestrianstart
；若满足，则说明目标点与行人当前处于同一楼层，转到步骤s2.3中；若不满足，则说明目标点与行人当前处于不同楼层处，转到步骤s2.4中；
26.s2.3、若目标点与行人当前处于同一楼层，采用引入时间信息的a*算法生成具有时空间信息的、面向行人的全局路径，起点为(x
pedestrianstart
,y
pedestrianstart
)，目标点为(x
pedestriangoal
,y
pedestriangoal
)；
27.s2.4、若目标点与行人当前处于不同楼层处，采用引入时间信息的a*全局路径规划算法首先生成第level
pedestrianstart
层的全局路径，其中起点为(x
pedestrianstart
,ypedestrianstart
)，目标点为第level
pedestrianstart
层的电梯所在处position
elevator
(x
elevator
,y
elevator
)；当行人乘坐电梯前往目标楼层level
goal
时，更新二维栅格地图为第level
goal
层的二维栅格地图；生成第level
pedestriangoal
楼层的全局路径，起点为第level
pedestriangoal
楼层电梯所在处position
elevator
(x
elevator
,y
elevator
)，目标点为(x
pedestriangoal
,y
pedestriangoal
)。
28.本发明通过对面向行人的二维栅格地图采用引入时间信息的a*全局路径规划方法生成符合行人运动特征的全局路径，行人根据该路径的指引行驶可能会遇到障碍物，但均属于行人可跨越的低矮障碍物等类别，对于灵活的行人来说可视为可行驶区域。
29.采用引入时间信息的a*算法生成具有时空间信息的、面向行人的全局路径的具体实现过程包括：
30.1)接收起点start(x
start
,y
start
)，目标点goal(x
goal
,y
goal
)信息，定义两个空集合open和close；
31.2)计算起点start(x
start
,y
start
)的启发值f
start
以及其抵达起点start(x
start
,y
start
)的时刻t
start
，存入集合open＝{(x
start
,y
start
,f
start
,t
start
,0,0)}中；启发值f
start
的计算公式为：
32.3)判断集合open是否为空集，若不是，则转到步骤4)中；若是，则全局规划结束，未找到一条从起点到目标点的具有时空间信息、面向行人的可行驶路径；
33.4)从集合open中选取启发值最小的栅格点作为下一步栅格点gridn(xn,yn)，获取其时间信息tn，将栅格点gridn从集合open移入集合close中；
34.5)判断栅格点gridn是否为目标点goal；若是，则根据栅格点gridn的父节点依次回溯，找到一条从起点到目标点的具有时空间信息、面向行人的可行驶路径；若否，则转到步骤6)中；
35.6)找到栅格点gridn八邻域方位且不在集合close中的非障碍物栅格，记为临近可达栅格grid
nc
(x
nc
,y
nc
)，计算临近可达栅格grid
nc
(x
nc
,y
nc
)的启发值f
nc
以及抵达临近可达栅格的时刻t
nc
＝tn+tf，设置临近可达栅格的父节点为gridn，存入集合open＝{(x
start
,y
start
,f
start
,t
start
,0,0)、
…
、(x
nc
,y
nc
,f
nc
,t
nc
,xn,yn)}中；其中tf为从栅格点gridn到grid
nc
的预计耗时；启发值f
nc
的计算公式为：
[0036][0037]
其中in为膨胀成本，若临近可达栅格grid
nc
为空闲栅格，in＝0；若临近可达栅格grid
nc
为膨胀栅格，其中p1、p2为给定参数，distance_obstacle为膨胀栅格距离最近障碍物栅格的距离；其中非障碍物栅格包括空闲栅格和膨胀栅格；所述空闲栅格和膨胀栅格的确定过程包括：根据生成的面向行人的二维栅格地图，在该二维栅格地图中标注出每一楼层的电梯所在处，记为position
elevator
(x
elevator
,y
elevator
)，在二维栅格地图中将电梯门障碍物栅格标记为空闲栅格；对二维栅格地图进行障碍物膨胀操作，即以
障碍物栅格为膨胀中心，膨胀半径为radius
pedestrianinflation
，若某一障碍物栅格膨胀面积不足该障碍物栅格面积一半，则该障碍物栅格为空闲栅格，否则为膨胀栅格；
[0038]
7)跳转至步骤3)，直至全局规划结束。
[0039]
本发明全局规划算法生成的全局路径包含预计行人行驶的时间，便于后续可适用于人机共融环境中的a*算法实现。
[0040]
步骤s3的具体实现过程包括：
[0041]
s3.1、机器人通过amcl定位获取机器人在xoy平面内的位置(x
robotstart
,y
robotstart
)，机器人当前所处楼层为level
robotstart
；
[0042]
s3.2、输入机器人导航目标点goal
robot
(x
robotgoal
,y
robotgoal
,level
robotgoal
)，其中x
robotgoal
和y
robotgoal
为目标点在xoy平面内的二维坐标，level
robotgoal
为目标所在楼层；
[0043]
s3.3、判断是否满足level
robotgoal
＝level
robotstart
；若满足，则说明目标点与机器人当前处于同一楼层，转到步骤s3.4中；若不满足，则说明目标点与机器人当前处于不同楼层，转到步骤s3.5中；
[0044]
s3.4、若目标点与机器人当前处于同一楼层，采用a*算法生成一条基于行人预测与时空一致性约束的、面向机器人的全局路径，起点为(x
robotstart
,y
robotstart
)，目标点为(x
robotgoal
,y
robotgoal
)；
[0045]
s3.5、若目标点与机器人当前处于不同楼层，采用a*算法结合电梯调度模块进行跨楼层间的导航，具体步骤如下：
[0046]
1)，采用a*算法首先生成第level
robotstart
层的全局路径，其中起点为(x
robotstart
,y
robotstart
)，目标点为第level
robotstart
层的电梯所在处position
elevator
(x
elevator
,y
elevator
)；
[0047]
2)，计算机器人根据步骤1)生成的全局路径抵达目标点处耗时t
rs
，其中s
robotglobal
为从机器人当前位置抵达电梯所在处的全局路径长度，v
robotassign
为给定的速度；
[0048]
3)，机器人获取电梯当前所处楼层level
elecurrent
，计算电梯所在楼层level
elecurrent
抵达机器人所在楼层level
robotstart
的时间t
es
，其中s
elevatorstart
为电梯所在楼层level
elecurrent
距离机器人所在楼层level
robotstart
的高度，v
elevator
为可事先测算出来的电梯运行速度，为固定值；
[0049]
4)，判断是否满足s
robotglobal
≤s
thresold
，其中s
thresold
为给定的距离阈值；若是，则转到步骤5)；若否，则机器人继续行驶，同时更新机器人抵达目标点处耗时t
rs
，返回步骤3)；
[0050]
5)，判断是否满足|t
rs-t
es
|≤t
thresold
，其中t
thresold
为给定的时间阈值；若是，则说明待机器人抵达目标点后，电梯也将抵达机器人所在楼层level
robotstart
，发布呼叫电梯指令给电梯调度模块；若否，则说明机器人在行驶途中因为存在障碍物导致耽搁，则在抵达目标点后再发送呼叫电梯指令；
[0051]
6)，电梯抵达机器人所在楼层后，进行电梯开门；同时机器人进行是否存在行人进出电梯的判断，若存在，则机器人后退一段距离，待行人进出电梯结束后机器人再进入电梯；若不存在行人进出电梯，则机器人直接进入电梯；
[0052]
7)，机器人进入电梯后，向电梯调度模块发布成功进入电梯的指令，在电梯调度模
块接收到机器人发布的成功进入电梯的指令后，电梯调度模块发布指令关上电梯门，电梯前往目标楼层level
robotgoal
，同时发布指令给机器人，进行二维栅格地图的切换；
[0053]
8)，待电梯抵达目标楼层后，由电梯调度模块发布指令电梯开门，同时发布指令给予机器人开始目标楼层导航；
[0054]
9)，机器人接收到来自电梯调度模块的目标楼层抵达指令后，开始目标楼层导航，采用a*算法生成第level
robotgoal
层的全局路径，其中起点为第level
robotgoal
楼层电梯所在处position
elevator
(x
elevator
,y
elevator
)，目标点为(x
robotgoal
,y
robotgoal
)。
[0055]
本发明通过对面向机器人的二维栅格地图采用一种可适用于人机共融环境的a*算法，生成一条基于行人预测与时空一致性约束的全局路径，该路径可提前预测行人在某一时段内的行驶轨迹并提前避让，不影响行人的行驶速度、方位等；同时为了实现机器人的跨楼层导航，本发明将电梯调度模块与a*算法相结合，可实现自主运输机器人从起始楼层前往目标楼层，不需要人为干扰，智能化程度更高。
[0056]
采用a*算法生成一条基于行人预测与时空一致性约束的、面向机器人的全局路径的具体实现过程包括：
[0057]
a)接收目标点goal
robot
(x
robotgoal
,y
robotgoal
)二维信息、面向行人生成的全局路径参考点，定义两个空集合openr和closer；
[0058]
b)计算机器人起点start
robot
(x
robotstart
,y
robotstart
)的启发值f
robotstart
以及其抵达起点start
robot
的时刻t
robotstart
，存入集合openr＝{(x
robotstart
,y
robotstart
,f
robotstart
,t
robotstart
,0,0)}中；其中启发值f
start
的计算公式如下：
[0059]
c)判断集合openr是否为空集，若否，则转到步骤d)中；若是，则全局规划结束，未找到一条从起点到目标点的基于行人预测与时空一致性约束的、面向机器人的全局路径；
[0060]
d)从集合openr中选取启发值最小的栅格点作为下一步栅格点grid
rn
(x
rn
,y
rn
)，获取grid
rn
(x
rn
,y
rn
)的时间信息t
rn
，将栅格点grid
rn
从集合openr移入集合closer中；
[0061]
e)判断栅格点grid
rn
是否为目标点goal
robot
；若是，则根据栅格点grid
rn
的父节点依次回溯，找到一条从起点到目标点的基于行人预测与时空一致性约束的、面向机器人的全局路径；若不是，则转到步骤f)中；
[0062]
f)依据栅格点grid
rn
的时间信息t
rn
，获取从t
rn
时刻开始往后的t
sim
秒内的行人全局路径参考点二维信息，记为point
pedestrian
(x
pedestrian
,y
pedestrian
)，其中t
sim
为事先给定的时间，并将行人全局路径参考点所占据的栅格标记为障碍物栅格，同时进行障碍物膨胀，膨胀中心为障碍物栅格，膨胀半径为radius
pedestrian
；
[0063]
g)找到栅格点grid
rn
八邻域方位且不在集合closer中的非障碍物栅格，记为临近可达栅格grid
rnc
(x
rnc
,y
rnc
)，计算临近可达栅格的启发值f
rnc
以及抵达临近可达栅格的时刻t
rnc
＝t
rn
+t
rf
，设置临近可达栅格的父节点为grid
rn
，存入集合openr＝{(x
robotstart
,y
robotstart
,f
robotstart
,t
robotstart
,0,0)、
…
、(x
rnc
,y
rnc
,f
rnc
,t
rnc
,x
rn
,y
rn
)}中；其中t
rf
为从栅格点grid
rn
到grid
rnc
的预计耗时；其中启发值f
rnc
的计算公式如下：
其中i
rn
为膨胀成本，若临近可达栅格grid
rnc
为空闲栅格，i
rn
＝0；若临近可达栅格grid
rnc
为膨胀栅格，其中p
r1
、p
r2
为给定参数，distance_obstacle_r为膨胀栅格距离最近障碍物栅格的距离；其中非障碍物栅格包括空闲栅格和膨胀栅格；所述空闲栅格和膨胀栅格的确定过程包括：在面向机器人的二维栅格地图中标注出每一楼层的电梯所在处，记为position
elevator
(x
elevator
,y
elevator
)，在二维栅格地图中将电梯门障碍物栅格转为空闲栅格；对二维栅格地图进行障碍物膨胀，即以障碍物栅格为膨胀中心，膨胀半径为radius
robotinflation
，若某一障碍物栅格膨胀面积不足该障碍物栅格面积一半，则该障碍物栅格为空闲栅格，否则为膨胀栅格；
[0064]
h)跳转至步骤c)，直至全局规划结束。
[0065]
由于机器人需要提前预测行人行驶轨迹并避让，所以需要预先知道行人的行驶路径以及时间信息并做出相应处理，预测行人在某一时段内的行驶轨迹并将其作为障碍物进行避让处理。
[0066]
步骤s4的具体实现过程包括：
[0067]
i)定义机器人局部代价地图为以机器人当前位置为中心，长、宽分别为length
robotlocal
、width
robotlocal
；
[0068]
ii)根据动态行人的二维坐标(x
rpc,t
,y
rpc,t
)以及其行驶速度v
do
、航向角θ
do
预测动态行人行驶轨迹：其中，t
predict
＝1,2,
…
,t
dopredict
，t
dopredict
为轨迹预测时长；动态行人行驶速度v
do
及航向角θ
do
计算公式为：(x
rpc,t
,y
rpc,t
)为聚类点pointcloud
rt
的坐标，(x
rkmeanstranspc,t-1
,y
rkmeanstranspc,t-1
)为聚类点pointcloud
rkmeanstrans,t-1
的坐标，pointcloud
rt
(x
rpc,t
,y
rpc,t
)为聚类点pointcloud
t
(x
pc,t
,y
pc,t
)在局部代价地图内的聚类点；pointcloud
t
(x
pc,t
,y
pc,t
)为当前帧点云数据二维坐标；pointcloud
rkmeanstrans,t-1
(x
rkmeanstranspc,t-1
,y
rkmeanstranspc,t-1
)为聚类点pointcloud
kmeanstrans,t-1
(x
kmeanstranspc,t-1
,y
kmeanstranspc,t-1
)在局部代价地图内的聚类点，pointcloud
kmeanstrans,t-1
(x
kmeanstranspc,t-1
,y
kmeanstranspc,t-1
)为对聚类点pointcloud
kmeans,t-1
(x
kmeanspc,t-1
,y
kmeanspc,t-1
)进行坐标转换后得到的聚类点；pointcloud
kmeans,t-1
(x
kmeanspc,t-1
,y
kmeanspc,t-1
)为对上一帧点云数据二维坐标pointcloud
t-1
(x
pc,t-1
,y
pc,t-1
)进行k-means聚类获得的聚类点；
[0069]
iii)以预测得到的行人轨迹(x
pedestrianpredict
,y
pedestrianpredict
)为圆心，半径为radius
pedestrianpredict
进行障碍物膨胀；遍历局部代价地图中所有的障碍物，计算轨迹traj
robot
中最后一个轨迹点距离局部代价地图中最近障碍物的距离distance
localtoobstacle
作
为碰撞成本；并判断是否满足distance
localtoobstacle
≥2
×
radius
robotinflation
，若满足，则distance
localtoobstacle
＝2
×
radius
robotinflation
；其中radius
robotinflation
为二维栅格地图进行障碍物膨胀的膨胀半径，(x
robot
,y
robot
)为机器人当前时刻的位置信息；
[0070]
iv)选取轨迹traj
robot
中最后一个轨迹点的绝对速度作为速度成本distance
vel
；traj
robot
为遍历机器人每一个速度v
robot
、角速度ω
robot
组合生成的一条在t
robotsim
秒内的轨迹点序列；
[0071]
v)令集合其中num
trajrobot
为所有采样轨迹的总数量，distance
localtoglobal,1
为第1条轨迹的全局路径跟踪成本，distance
localtoobstacle,1
为第1条轨迹的碰撞成本，distance
vel,1
为第1条轨迹的速度成本；distance
localtoglobal,1
＝d-distance
mintoglobal，1
；distance
mintoglobal，1
为第1条轨迹中最后一个轨迹点距离全局路径的最小距离；d为给定的值；distance
vel，1
为第1条轨迹中最后一个轨迹点的绝对速度，以该绝对速度值作为速度成本；
[0072]
vi)对集合evaluate中每一类型的数据进行归一化处理：
[0073][0074]
vii)计算每一条采样轨迹的总成本值，具体计算式如下：evaluate
traj,k
＝α
·
normal_distance
localtoglobal,k
+β
·
normal_distance
localtoobstacle,k
+γ
·
normal_distance
vel,k
；其中，α、β、γ为给定的权重值；
[0075]
viii)选取总成本值最大的轨迹作为局部避障轨迹。
[0076]
本发明的可适用于人机共融环境中的a*算法生成的全局路径可预估行人行驶的时间，但由于行人行驶速度具有不确定性，因此行人行驶的时间预估不可能非常准确，因此需要针对激光雷达探测到的环境信息进行动态避障，而采用背景对消法通过两帧的激光雷达点云数据可以很好的辨认出环境中的静态障碍物和动态障碍物，并且预估动态障碍物的行驶轨迹，达到了一种能够进行轨迹预测及动态避障的效果。
[0077]
计算动态行人行驶速度v
do
及航向角θ
do
之前，还进行如下判断：对聚类点
pointcloud
rt
(x
rpc,t
,y
rpc,t
)和聚类点pointcloud
rkmeanstrans,t-1
(x
rkmeanstranspc,t-1
,y
rkmeanstranspc,t-1
)进行作差处理，若满足则说明坐标为(x
rpc,t
,y
rpc,t
)的障碍物为静态障碍物，其中range
thresold
为给定的阈值；若不满足则说明坐标为(x
rpc,t
,y
rpc,t
)的障碍物为动态行人，计算动态行人行驶速度v
do
及航向角θ
do
。
[0078]
本发明所述的基于背景对消法的动态行人轨迹预测仅需相邻两帧的激光雷达点云数据即可进行动静态障碍物的辨别，同时计算出动态行人轨迹的行驶速度和航向角，在计算资源上占用更少，并且更容易实现。
[0079]
本发明还提供了一种计算机装置，包括存储器、处理器及存储在存储器上的计算机程序；所述处理器执行所述计算机程序，以实现本发明方法的步骤。
[0080]
本发明还提供了一种计算机程序产品，包括计算机程序/指令；该计算机程序/指令被处理器执行时实现本发明方法的步骤。
[0081]
与现有技术相比，本发明所具有的有益效果为：
[0082]
1)本发明首先根据机器人在行驶过程中激光雷达采集到的环境信息建立三维栅格地图，并基于行人与机器人运动特征的差异进行二维投影，生成面向行人与机器人的二维栅格地图。对不同的路径需求采用不同的二维栅格地图，生成符合行人/机器人运动特征的全局路径。对面向行人的二维栅格地图采用引入时间信息的a*全局路径规划算法生成的可跨越低矮障碍物的全局路径，行人基于该路径可以在不熟悉的大型室内场景中快速的抵达目标位置，同时避开途中障碍物。对面向机器人的二维栅格地图采用一种可适用于人机共融环境的a*算法，生成一条基于行人预测和时空一致性约束的全局路径，保证了机器人在室内行驶时可提前预测行人在某一时段内的行驶轨迹并提前避让。
[0083]
2)针对由于行人行驶速度的不确定性导致的预测时间偏差问题，提出了一种基于背景对消和dwa局部轨迹规划算法实现行人预测与动态避障功能，保证了机器人行驶过程中的人机安全性。针对机器人多楼层间的导航问题，通过结合机器人与电梯调度模块实现一种全智能化的电梯运输技术，多层二维栅格地图可用于楼宇内多楼层间的导航，并且无需建立多层代价地图就可进行行人预测，在资源占用上更少，可实现功能更强大，可将机器人从起始楼层运输至目标楼层而不需要人为干扰，大大减少了人力成本。
[0084]
3)本发明通过建立三维栅格地图并基于行人与机器人运动特征的差异实现二维投影，生成的面向行人与机器人二维栅格地图既可对大型室内场景不熟悉的行人进行导航指引，同时由于面向行人与机器人的二维栅格地图尺寸一致，因此可将生成的面向行人的全局路径与面向机器人的二维栅格地图一同用于机器人的全局路径生成中，结合时空间信息生成一种基于行人预测与时空一致性约束的全局路径，提高了面向人机共融环境中的机器人行驶时的人机安全性。
附图说明
[0085]
图1为人机共融的楼宇间路径规划流程图；
[0086]
图2为面向行人的二维栅格地图；
[0087]
图3为面向机器人的二维栅格地图；
[0088]
图4为可跨越低矮障碍物的行人全局路径；
[0089]
图5为不考虑行人预测的机器人全局路径；
[0090]
图6为基于行人预测和时空一致性约束的机器人全局路径；
[0091]
图7(a)为相邻两帧的激光雷达点云数据聚类点；图7(b)为经过变换后的两帧聚类点；
[0092]
图8为基于dwa局部规划算法的动态避障。
具体实施方式
[0093]
本发明实施例提供的一种人机共融的楼宇间路径规划方法、计算机装置及程序产品，如图1所示，包括以下步骤：
[0094]
步骤一：装配有激光雷达的机器人在楼宇间行驶时，采集行驶过程中激光雷达环境信息，构建三维栅格地图并基于行人与机器人运动特征的差异进行二维投影，生成面向行人和机器人的二维栅格地图：
[0095]
步骤1，装配有激光雷达的机器人在楼宇间单层楼层行驶时，采用激光雷达采集行驶过程中的环境信息；
[0096]
步骤2，构建三维栅格地图，具体步骤如下：
[0097]
步骤2.1，根据上层得到的感知信息采用点云配准算法得到三维点云地图；
[0098]
步骤2.2，对三维点云进行滤波处理，将滤波后的三维点云地图转为分辨率为resolution的三维栅格地图，其中resolution＝0.05；
[0099]
步骤3，设定一个高度范围h
pedestrianmin
≤z
gridoctree
，其中h
pedestrianmin
＝0.8米为行人可翻越障碍物的最大高度，对三维栅格地图在z轴上进行投影至xoy平面内得到面向行人的二维栅格地图，若在三维栅格地图中横纵坐标为(x
gridoctree
,y
gridoctree
)，z轴满足h
pedestrianmin
≤z
gridoctree
的范围内不存在三维栅格，则其投影在xoy平面内的二维栅格(x
gridoctree
,y
gridoctree
)为空闲栅格；若存在三维栅格，则其投影在xoy平面内的二维栅格为障碍物栅格，其中(x
gridoctree
,y
gridoctree
,z
gridoctree
)为三维栅格中心点坐标；
[0100]
步骤4，设定一个高度范围z
gridrobot
≤h
robotmax
，其中h
robotmax
＝0.5米为机器人最大高度，对三维栅格地图在z轴上进行投影至xoy平面内得到面向机器人的二维栅格地图，若在三维栅格地图中横纵坐标为(x
gridrobot
,y
gridrobot
)，z轴满足0≤z
gridrobot
≤h
robotmax
的范围内不存在三维栅格，则其投影在xoy平面内的二维栅格(x
gridrobot
,y
gridrobot
)为空闲栅格；若存在三维栅格，则其投影在xoy平面内的二维栅格为障碍物栅格，其中(x
gridrobot
,y
gridrobot
,z
gridrobot
)为三维栅格中心点坐标；
[0101]
步骤5，返回步骤1，构建其它楼层的三维栅格地图并基于行人与机器人运动特征的差异进行二维投影，生成面向行人和机器人的二维栅格地图。
[0102]
如图2、3所示，其中图2是生成的面向行人的二维栅格地图，图3是面向机器人的二维栅格地图，其中虚线框为行人可跨越的低矮障碍物，但对于机器人来说该障碍物区域是无法通过的。
[0103]
步骤二：对面向行人的二维栅格地图采用a*全局路径规划算法生成可跨越低矮障碍物的全局路径：
[0104]
步骤1，根据生成的面向行人的二维栅格地图，在地图中标注出每一楼层的电梯所在处，记为position
elevator
(x
elevator
,y
elevator
)，将二维栅格地图中将电梯门障碍物栅格转为空闲栅格；
[0105]
步骤2，对二维栅格地图进行障碍物膨胀，其中以障碍物栅格为膨胀中心，膨胀半径为radius
pedestrianinflation
，若某一栅格膨胀面积不足该栅格面积一半，则该栅格为空闲栅格，否则为膨胀栅格，其中radius
pedestrianinflation
＝0.2米；
[0106]
步骤3，行人输入目标点goal
pedestrian
(x
pedestriangoal
,y
pedestriangoal
,level
pedestriangoal
)、行人当前所处位置start
pedestrian
(x
pedestrianstart
,y
pedestrianstart
,level
pedestrianstart
)，其中x
pedestriangoal
和y
pedestriangoal
为目标点在xoy平面内的二维坐标，level
pedestriangoal
为目标所在楼层，x
pedestrianstart
和y
pedestrianstart
为行人在xoy平面内的二维坐标，level
pedestrianstart
为行人当前所在楼层；
[0107]
步骤4，判断是否满足level
pedestriangoal
＝level
pedestrianstart
；若满足，则说明目标点与行人当前处于同一楼层，转到步骤4.1中；若不满足，则说明目标点与行人当前处于不同楼层处，转到步骤4.2中；
[0108]
步骤4.1，若目标点与行人当前处于同一楼层，采用引入时间信息的a*算法生成具有时空间信息、可跨越低矮障碍物的全局路径，起点为(x
pedestrianstart
,y
pedestrianstart
)，目标点为(x
pedestriangoal
,y
pedestriangoal
)；
[0109]
步骤4.2，若目标点与行人当前处于不同楼层处，采用引入时间信息的a*全局路径规划算法首先生成第level
pedestrianstart
层的全局路径，其中起点为(x
pedestrianstart
,y
pedestrianstart
)，目标点为第level
pedestrianstart
层的电梯所在处position
elevator
(x
elevator
,y
elevator
)；随后，行人乘坐电梯前往目标楼层level
goal
，并更新二维栅格地图为第level
goal
层的二维栅格地图；最后，生成第level
pedestriangoal
楼层的全局路径，起点为第level
pedestriangoal
楼层电梯所在处position
elevator
(x
elevator
,y
elevator
)，目标点为(x
pedestriangoal
,y
pedestriangoal
)。
[0110]
其中引入时间信息的a*全局路径规划算法具体实现如下：
[0111]
1)，接收起点start(x
start
,y
start
)，目标点goal(x
goal
,y
goal
)信息，定义两个空集合open和close；
[0112]
2)，计算起点start(x
start
,y
start
)的启发值f
start
以及其抵达起点start(x
start
,y
start
)的时刻t
start
＝0，存入集合open＝{(x
start
,y
start
,f
start
,t
start
,0,0)}中；其中启发值f
start
的计算公式如下：
[0113][0114]
3)，判断集合open是否为空集，若不是，则转到4)中；若是，则全局规划结束，没有找到一条从起点到目标点的可行驶路径；
[0115]
4)，从集合open中选取启发值最小的栅格点作为下一步栅格点gridn(xn,yn)，获取其时间信息tn，将栅格点gridn从集合open移入集合close中；
[0116]
5)，判断栅格点gridn是否为目标点goal；若是，则根据栅格点gridn的父节点依次
回溯，找到一条从起点到目标点的可行驶路径；若不是，则转到6)中；
[0117]
6)，找到栅格点gridn八邻域方位且不在集合close中的非障碍物栅格，记为临近可达栅格grid
nc
(x
nc
,y
nc
)，计算这些栅格的启发值f
nc
以及抵达这些栅格的时刻t
nc
＝tn+tf，设置这些栅格的父节点为gridn，存入集合open＝{(x
start
,y
start
,f
start
,t
start
,0,0)、
…
、(x
nc
,y
nc
,f
nc
,t
nc
,xn,yn)}中；其中tf＝1秒为从栅格点gridn到grid
nc
的预计耗时；其中启发值f
nc
的计算公式如下：
[0118][0119]
其中in为膨胀成本，若临近可达栅格grid
nc
为空闲栅格时，in＝0；若临近可达栅格grid
nc
为膨胀栅格时，其中p1、p2为给定参数，分别为20、5，distance_obstacle为膨胀栅格距离最近障碍物栅格的距离；
[0120]
7)，转到3)中，直至全局规划结束。
[0121]
如图4所示，其中虚线为行人全局路径，从图中可以看出生成的路径可以很好的避开障碍物抵达目标位置。
[0122]
步骤三：对面向机器人的二维栅格地图采用一种可适用于人机共融环境中的a*算法，生成一条基于行人预测与时空一致性约束的全局路径，具体步骤如下：
[0123]
步骤1，机器人通过amcl定位获取机器人在xoy平面内位置(x
robotstart
,y
robotstart
)，机器人当前所处楼层为level
robotstart
；
[0124]
步骤2，根据生成的面向机器人的二维栅格地图，在地图中标注出每一楼层的电梯所在处，记为position
elevator
(x
elevator
,y
elevator
)，将二维栅格地图中将电梯门障碍物栅格转为空闲栅格；
[0125]
步骤3，对二维栅格地图进行障碍物膨胀，其中以障碍物栅格为膨胀中心，膨胀半径为radius
robotinflation
，若某一栅格膨胀面积不足该栅格面积一半，则该栅格为空闲栅格，否则为膨胀栅格，其中radius
robotinflation
＝0.5米；
[0126]
步骤4，输入机器人导航目标点goal
robot
(x
robotgoal
,y
robotgoal
,level
robotgoal
)，其中x
robotgoal
和y
robotgoal
为目标点在xoy平面内的二维坐标，level
robotgoal
为目标所在楼层；
[0127]
步骤5，判断是否满足level
robotgoal
＝level
robotstart
；若满足，则说明目标点与机器人当前处于同一楼层，转到步骤5.1中；若不满足，则说明目标点与机器人当前处于不同楼层，转到步骤5.2中；
[0128]
步骤5.1，若目标点与机器人当前处于同一楼层，采用a*算法生成一条基于行人预测与时空一致性约束的全局路径，起点为(x
robotstart
,y
robotstart
)，目标点为(x
robotgoal
,y
robotgoal
)；
[0129]
步骤5.2，若目标点与机器人当前处于不同楼层，采用a*算法结合电梯调度模块进行跨楼层间的导航，具体步骤如下：
[0130]
1)，采用a*算法首先生成第level
robotstart
层的全局路径，其中起点为(x
robotstart
,y
robotstart
)，目标点为第level
robotstart
层的电梯所在处position
elevator
(x
elevator
,y
elevator
)；
[0131]
2)，计算机器人根据步骤1)生成的全局路径抵达目标点处耗时t
rs
，其中s
robotglobal
为从机器人当前位置抵达电梯所在处的全局路径长度，v
robotassign
为给定的速度；
[0132]
3)，机器人获取电梯当前所处楼层level
elecurrent
，计算电梯所在楼层level
elecurrent
抵达机器人所在楼层level
robotstart
的时间t
es
，其中s
elevatorstart
为电梯所在楼层level
elecurrent
距离机器人所在楼层level
robotstart
的高度，v
elevator
为可事先测算出来的电梯运行速度，为固定值；
[0133]
4)，判断是否满足s
robotglobal
≤s
thresold
，其中s
thresold
为给定的距离阈值；若是，则转到步骤5)；若否，则机器人继续行驶，同时更新机器人抵达目标点处耗时t
rs
，返回步骤3)；
[0134]
5)，判断是否满足|t
rs-t
es
|≤t
thresold
，其中t
thresold
为给定的时间阈值；若是，则说明待机器人抵达目标点后，电梯也将抵达机器人所在楼层level
robotstart
，发布呼叫电梯指令给电梯调度模块；若否，则说明机器人在行驶途中因为存在障碍物导致耽搁，则在抵达目标点后再发送呼叫电梯指令；
[0135]
6)，电梯抵达机器人所在楼层后，进行电梯开门；同时机器人进行是否存在行人进出电梯的判断，若存在，则机器人后退一段距离，待行人进出电梯结束后机器人再进入电梯；若不存在行人进出电梯，则机器人直接进入电梯；
[0136]
7)，机器人进入电梯后，向电梯调度模块发布成功进入电梯的指令，在电梯调度模块接收到机器人发布的成功进入电梯的指令后，电梯调度模块发布指令关上电梯门，电梯前往目标楼层level
robotgoal
，同时发布指令给机器人，进行二维栅格地图的切换；
[0137]
8)，待电梯抵达目标楼层后，由电梯调度模块发布指令电梯开门，同时发布指令给予机器人开始目标楼层导航；
[0138]
9)，机器人接收到来自电梯调度模块的目标楼层抵达指令后，开始目标楼层导航，采用a*算法生成第level
robotgoal
层的全局路径，其中起点为第level
robotgoal
楼层电梯所在处position
elevator
(x
elevator
,y
elevator
)，目标点为(x
robotgoal
,y
robotgoal
)。
[0139]
适用于人机共融环境中的a*算法具体实现如下：
[0140]
1)，接收目标点goal
robot
(x
robotgoal
,y
robotgoal
)二维信息、面向行人生成的全局路径参考点，定义两个空集合openr和closer；
[0141]
2)，计算机器人起点start
robot
(x
robotstart
,y
robotstart
)的启发值f
robotstart
以及其抵达起点start
robot
的时刻t
robotstart
＝0，存入集合openr＝{(x
robotstart
,y
robotstart
,f
robotstart
,t
robotstart
,0,0)}中；其中启发值f
start
的计算公式如下：
[0142][0143]
3)，判断集合openr是否为空集，若不是，则转到4)中；若是，则全局规划结束，没有找到一条从起点到目标点的可行驶路径；
[0144]
4)，从集合openr中选取启发值最小的栅格点作为下一步栅格点grid
rn
(x
rn
,y
rn
)，
获取其时间信息t
rn
，将栅格点grid
rn
从集合openr移入集合closer中；
[0145]
5)，判断栅格点grid
rn
是否为目标点goal
robot
；若是，则根据栅格点grid
rn
的父节点依次回溯，找到一条从起点到目标点的可行驶路径；若不是，则转到6)中；
[0146]
6)，依据栅格点grid
rn
的时间信息t
rn
，获取从t
rn
时刻开始往后的t
sim
秒内的行人全局路径参考点二维信息，记为point
pedestrian
(x
pedestrian
,y
pedestrian
)，其中t
sim
＝3为事先给定的时间，并将这些参考点所占据的栅格标记为障碍物栅格，同时进行障碍物膨胀，膨胀中心为障碍物栅格，膨胀半径为radius
pedestrian
＝0.5米；
[0147]
7)，找到栅格点grid
rn
八邻域方位且不在集合closer中的非障碍物栅格，记为临近可达栅格grid
rnc
(x
rnc
,y
rnc
)，计算这些栅格的启发值f
rnc
以及抵达这些栅格的时刻t
rnc
＝t
rn
+t
rf
，设置这些栅格的父节点为grid
rn
，存入集合openr＝{(x
robotstart
,y
robotstart
,f
robotstart
,t
robotstart
,0,0)、
…
、(x
rnc
,y
rnc
,f
rnc
,t
rnc
,x
rn
,y
rn
)}中；其中t
rf
＝1秒为从栅格点grid
rn
到grid
rnc
的预计耗时；其中启发值f
rnc
的计算公式如下：
[0148][0149]
其中i
rn
为膨胀成本，若临近可达栅格grid
rnc
为空闲栅格时，i
rn
＝0；若临近可达栅格grid
rnc
为膨胀栅格时，其中p
r1
、p
r2
为给定参数，分别为20、5，distance_obstacle_r为膨胀栅格距离最近障碍物栅格的距离；
[0150]
8)，转到3)中，直至全局规划结束。
[0151]
如图5、6所示，其中图5、6中机器人轨迹不同的原因在于图6是采用可适用于人机共融环境的全局路径规划算法生成的一条基于行人预测和时空一致性约束的全局路径，而图5并未考虑行人轨迹预测。由于设定的行人速度比机器人快，图6中行人是从机器人后方行驶而来的，为了给行人让路，机器人偏离了原来的轨迹。
[0152]
步骤四：基于背景对消与dwa局部规划算法进行动态行人轨迹预测与避障具体步骤如下：
[0153]
步骤1，定义机器人局部代价地图为以机器人当前位置为中心，长宽分别为length
robotlocal
＝12、width
robotlocal
＝12；
[0154]
步骤2，基于背景对消法的动态行人轨迹预测步骤如下：
[0155]
步骤2.1，装配有激光雷达的机器人在自主导航行驶时，采用激光雷达获取相邻帧的点云数据，获取其二维坐标，其时间差记为δt，分别记两帧点云数据二维坐标为pointcloud
t-1
(x
pc,t-1
,y
pc,t-1
)、pointcloud
t
(x
pc,t
,y
pc,t
)，其中当前帧点云数据记录时刻为t，上一帧点云数据记录时刻为t-1；
[0156]
步骤2.2，对两帧点云数据分别采用k-means聚类法获取聚类点，分别记为pointcloud
kmeans,t-1
(x
kmeanspc,t-1
,y
kmeanspc,t-1
)、pointcloud
kmeans,t
(x
kmeanspc,t
,y
kmeanspc,t
)；
[0157]
步骤2.3，根据机器人底盘数据获取其行驶速度v
robot
及航向角增量δθ
robot
，其中δθ
robot
为t-1到t时刻机器人航向角的增量；
[0158]
步骤2.4，计算δt时间内机器人在x、y方向上的行驶距离，具体计算式如下：
[0159][0160]
其中，(x
t-1
,y
t-1
)为t-1时刻机器人在全局坐标系内的二维坐标信息；
[0161]
步骤2.5，对聚类点pointcloud
kmeans,t-1
(x
kmeanspc,t-1
,y
kmeanspc,t-1
)采用步骤2.4的公式进行坐标转换，得到转换后的聚类点pointcloud
kmeanstrans,t-1
(x
kmeanstranspc,t-1
,y
kmeanstranspc,t-1
)；
[0162]
步骤2.6，对聚类点pointcloud
t
(x
pc,t
,y
pc,t
)、pointcloud
kmeanstrans,t-1
(x
kmeanstranspc,t-1
,y
kmeanstranspc,t-1
)分别进行范围限制，获取在局部代价地图内的聚类点，分别记为pointcloud
rt
(x
rpc,t
,y
rpc,t
)、pointcloud
rkmeanstrans,t-1
(x
rkmeanstranspc,t-1
,y
rkmeanstranspc,t-1
)；
[0163]
步骤2.7，分别对聚类点pointcloud
rt
(x
rpc,t
,y
rpc,t
)和聚类点pointcloud
rkmeanstrans,t-1
(x
rkmeanstranspc,t-1
,y
rkmeanstranspc,t-1
)进行作差处理，若满足则说明坐标为(x
rpc,t
,y
rpc,t
)的障碍物为静态障碍物，其中range
thresold
＝0.05为给定的阈值；若不满足则说明坐标为(x
rpc,t
,y
rpc,t
)的障碍物为动态行人，并转到步骤2.8中；
[0164]
步骤2.8，计算动态行人行驶速度v
do
及航向角θ
do
，具体计算式如下：
[0165][0166]
步骤2.9，根据动态行人的二维坐标(x
rpc,t
,y
rpc,t
)以及其行驶速度v
do
、航向角θ
do
预测其行驶轨迹，具体计算式如下：
[0167][0168]
其中，t
predict
＝1,2,
…
,t
dopredict
，t
dopredict
为轨迹预测时长；
[0169]
步骤3，基于dwa局部规划算法结合行人预测轨迹进行动态避障，具体步骤如下：
[0170]
步骤3.1，根据机器人状态信息限定机器人速度、角速度采样空间；
[0171]
步骤3.2，对局部代价地图中的每一个障碍物进行膨胀处理，圆心为障碍物中心，膨胀半径为radius
robotinflation
＝0.5；
[0172]
步骤3.3，遍历机器人每一个速度v
robot
、角速度ω
robot
组合生成一条在t
robotsim
＝3秒内的轨迹点序列traj
robot
，具体计算式如下：
[0173]
[0174]
其中，(x
robot
,y
robot
)为机器人当前时刻的位置信息；
[0175]
步骤3.4，评估轨迹traj
robot
的跟踪全局路径成本、碰撞成本、速度成本，具体步骤如下：
[0176]
1)，计算轨迹traj
robot
中最后一个轨迹点距离全局路径的最小距离distance
mintoglobal
，随后计算distance
localtoglobal
＝d-distance
mintoglobal
作为跟踪全局路径成本，其中d＝20为事先给定的值；
[0177]
2)，以预测得到的行人轨迹(x
pedestrianpredict
,y
pedestrianpredict
)为圆心，半径为radius
pedestrianpredict
进行障碍物膨胀，其中radius
pedestrianpredict
的计算式如下：
[0178][0179]
随后遍历局部代价地图中所有的障碍物，计算轨迹traj
robot
中最后一个轨迹点距离局部代价地图中最近障碍物的距离distance
localtoobstacle
作为碰撞成本；并判断是否满足distance
localtoobstacle
≥2
×
radius
robotinflation
，若满足，则distance
localtoobstacle
＝2
×
radius
robotinflation
；
[0180]
3)，选取轨迹traj
robot
中最后一个轨迹点的绝对速度作为速度成本distance
vel
；
[0181]
4)，将上述三个成本值存入集合evaluate＝{(distance
localtoglobal
,distance
localtoobstacle
,distance
vel
),
…
}中；
[0182]
5)，对集合evaluate中每一类型的数据进行归一化处理，具体计算式如下：
[0183][0184]
其中，num
trajrobot
为采样的所有轨迹traj
robot
；
[0185]
6)，计算每一条采样轨迹的总成本值，具体计算式如下：
[0186]
evaluate
traj,k
＝α
·
normal_distance
localtoglobal,k
+β
·
normal_distance
localtoobstacle,k
+γ
·
normal_distance
vel,k
[0187]
其中，α、β、γ为事先给定的权重值，分别为0.1、0.1、0.05；
[0188]
7)，选取总成本值最大的轨迹作为局部避障轨迹。
[0189]
图7(a)为相邻两帧的激光雷达点云数据聚类点，其中空心圆为前一帧的聚类点，实心圆为当前帧的聚类点，由于采集两帧点云数据之间存在时间差，因此两帧聚类点并没有重合；图7(b)为经过变换后的两帧聚类点，可以发现25个聚类点中有24个聚类点基本重合，由于存在噪声的原因导致并未完全重合，并且从图中方框部分可以看出其位置偏差很
大，说明此聚类点对应的障碍物为动态障碍物，即行人。而后续即可根据这两帧的聚类点进行行人的轨迹预测。
[0190]
图8为采用dwa局部规划算法进行动态避障的仿真图，其中同时存在动静态障碍物，并且结果表明机器人可进行行人轨迹预测以及提前避让。