基于A*和TEB融合的行人感知无碰跟随方法

庞磊，曹志强，喻俊志

1. 中国科学院自动化研究所 复杂系统管理与控制国家重点实验室，北京 100190 2.中国科学院大学 人工智能学院，北京 100049 3.北京大学 工学院 先进制造与机器人系，北京 100871

机器人跟随行人可以简化人与机器人之间的交互，使机器人跟随指定行人到达期望地点的方式较为便捷，可以视为一种特殊的导航方式。行人跟随在地面机器人、空中机器人和水下机器人中均有重要的应用[1]。现阶段行人跟随多在地面机器人中发挥作用，在助老助残[2-3]、载物载货[4]及军事[5]等领域具有重要的应用前景。

机器人实现行人跟随需要具备定位目标行人的能力[6]，并需要保持对目标行人的跟随状态。在目标行人定位方面，视觉传感器和激光雷达被广泛使用，此外，IMU(Inertial Measurement Unit)、 超声波传感器、RFID(Radio-Frequency IDentification)标签、UWB(Ultra Wide Band)定位模块、蓝牙传感器等也可用于行人跟随[7-9]。以稳定的目标行人定位信息为基础，机器人以适当的方式与目标行人保持一定的位置关系进而维持跟随状态。根据不同的应用场景，机器人需要与目标行人保持不同的相对位置关系，常见的有身后跟随、身前跟随和身旁跟随等模式。为了实现相应的跟随模式，常见的方法有基于目标行人轨迹、基于目标行人方向指引与局部避障以及基于路径规划的跟随方法。

机器人沿着目标行人的运动轨迹进行跟随是一种直观的实现方案，需要机器人记录目标行人的历史运动轨迹，并按照记录的轨迹以一定的滞后距离或时间运动。Doisy等[10]提出了一种跟随目标行人路径的跟随方法，机器人通过记录目标行人运动轨迹并进行采样和筛选、平滑处理以控制机器人跟随目标行人。上述严格沿着目标行人运动轨迹跟随的方法仅适用于身后跟随模式，对于身旁跟随和身前跟随模式来说，可以通过估计行人运动状态、运动轨迹等方式实现跟随。同时，基于预测目标行人运动轨迹的方法提供了目标行人在机器人传感器视野短暂脱离问题的解决方案。Hu等[11]基于行人运动模型和无迹卡尔曼滤波器预测目标行人的运动轨迹，使机器人可以做出期望的运动决策，从而在身后、身前和身旁模式下均可以有效跟随目标行人。

目标行人方向指引以机器人与行人之间的距离和相对角度作为反馈，从而控制机器人的速度以保持对目标行人的跟随[12]。目标行人方向指引通常表现为控制机器人使目标行人位于传感器的中间位置。在开阔场景中，直接基于方向指引能够实现机器人的行人跟随，但是在复杂工作环境中，单纯方向指引并不能保证机器人安全、有效地运行，为此需要添加额外的避障算法。文献[13-14] 中的跟随机器人均采用方向指引的方式跟随目标行人，并分别基于VFH(Vector Field Histogram)算法以及VFH与PFM(Potential Field Methods)相结合的避障算法躲避机器人与目标行人之间的障碍物。

基于路径规划的跟随方法是在获取目标行人与机器人相对位姿的基础上，通过规划机器人与目标行人之间的最优路径实现跟随。该类方法在考虑保持与目标行人相对位置关系的同时兼顾避障，主要包括基于全局地图辅助的方法[15]和基于相对位姿规划的方法[16]。全局地图辅助的方法通过在环境地图中确定机器人与目标行人的全局位置，进而规划两者之间的最优无碰路径以实现行人跟随，地图的建立和机器人定位通常使用SLAM(Simultaneous Localization and Mapping)技术加以解决。基于相对位姿规划的方法并不考虑机器人和目标行人的全局位置，更倾向于以反应式跟随策略规划机器人到目标行人的局部路径以实现跟随，因此相对基于全局地图辅助的方式更加灵活。Tasaki等[16]根据目标行人在机器人坐标系下的定位信息，使用RRT(Rapidly-exploring Random Tree)算法在机器人局部环境地图上规划无碰可行的路径，进而利用PID(Proportional-Integral-Derivative)控制器控制机器人跟随路径。

为使机器人在行人跟随中具有较好的环境适应能力，同时在行人跟随过程中兼顾避障，提出了一种基于路径规划的无碰行人跟随方法。基于激光点云分割提供的静态障碍物信息及基于3D行人检测的多行人UKF(Unscented Kalman Filter)跟踪器估计的行人运动状态，分别生成静态障碍代价地图和动态行人代价地图，在此基础上，通过低频全局规划和高频局部规划结合的方式实现对目标行人安全、无碰地跟随。

1 多传感器行人跟随系统

基于路径规划的无碰行人跟随方法由感知模块和规划模块组成。感知模块负责目标行人和环境中其他行人的检测和定位问题，同时负责环境中障碍物的感知。为保证复杂环境下稳定和准确的行人定位，感知模块基于由外参数标定的RGB相机与多线激光雷达组成的多传感器方案实现。首先，基于深度神经网络的YOLOv3[17]检测器被用于在图像中确定全部行人的位置；其次，激光雷达获取的点云数据被分割为地面点云和非地面点云簇，其中非地面点云簇在后续机器人跟随过程中将被进一步用于避障；此外，通过在图像坐标系中融合由行人检测器提供的视觉行人语义信息与非地面物体点云簇，可以实现3D行人(包括目标行人和干扰行人)定位和点云簇分类，这里点云簇被分为行人点云簇和非行人点云簇。基于获得的3D行人定位信息，机器人需要进一步在多个行人之间确认目标行人，为此卡尔曼滤波器结合行人重识别(person re-identification)模块的数据关联方法被用于在连续的图像帧间持续地关联检测数据，进而确定目标行人的身份。除目标行人之外的其他行人将被进一步跟踪并估计运动信息，从而结合静态障碍物信息实现机器人行人跟随过程中行人感知的避障。感知模块中算法的具体描述参考文献[18-19]。

(1)

2 行人运动状态估计

以感知模块提供的3D行人检测结果为基础，通过基于UKF的跟踪器预测环境中其他行人的运动状态，进而基于行人的运动方向和速度预测行人未来一段时间的运动轨迹，使机器人可以预先规划更合理的路径绕过行人，从而更好地保障环境中行人的安全以及提高自身运动效率。

2.1 状态预测

(2)

χi=

(3)

(4)

(5)

(6)

式中：Q为预测噪声的协方差矩阵。

2.2 数据关联和状态更新

(7)

Zk,i=h(χk,i,0)i=0,1,…,2nx

(8)

(9)

(10)

式中：R为测量噪声协方差矩阵。

(11)

(12)

式中：B为常数，负责处理由于检测、遮挡、错误引起的非均匀概率。在NN-JPDA算法中，最多仅有一个测量值与已有的跟踪轨迹对应，将最大βk,ji对应的第i个测量值和第j个跟踪轨迹进行配对，而后按照同样的思路对剩余的M-1个跟踪轨迹和N-1个测量值进行配对，直到全部的跟踪轨迹或测量值完成配对，或者满足βk,ji<η为止，其中η为预设阈值。

2.3 状态更新和行人运动状态估计

当跟踪轨迹获得与之匹配的测量值时，将更新UKF的系统状态。首先，计算tk时刻Sigma点在状态空间和测量空间的互相关运算Ck，并根据Ck和Sk计算卡尔曼增益Kk：

(13)

(14)

式中：PZkZk为Sigma点在测量空间的自相关运算。

然后，基于卡尔曼增益Kk和预测观测值与真实测量值之间的差值更新系统状态Xk和相应的协方差矩阵Pk：

(15)

(16)

3 基于路径规划的无碰行人跟随方法

3.1 分层式代价地图

根据感知模块提供的静态障碍物信息和动态行人的运动状态分别生成静态障碍代价地图和动态行人代价地图，其中代价地图使用分层式代价地图框架[21]进行维护和更新。分层式代价地图框架中的每一个代价地图层均采用栅格地图形式，能够独立处理来自不同信息源的障碍物并生成相应的代价地图。图1给出了基于分层式代价地图的路径规划示意图。静态障碍代价地图源自环境中的非行人障碍物，利用感知模块提供的非行人点云簇在地面投影区域生成障碍栅格和相应膨胀，如图1中的蓝色区域所示；依据3D行人检测结果(目标行人除外)预测行人运动状态，并生成动态行人代价地图，如图1中的粉色线段所示。动态行人代价地图考虑环境中干扰行人的运动状态，有利于机器人更好地躲避动态障碍物。

图1 基于分层式代价地图的路径规划示意图Fig.1 Schematic diagram of path planning with layered cost maps

(17)

式中：ρ为速度系数；v为行人速度。此外，在行人运动的反方向设立固定长度Rd的虚拟障碍。这两部分虚拟障碍构成完整的动态行人代价地图，如图2(b)粉色线段所示。

图2 动态行人代价地图生成示意图Fig.2 Schematic diagram of dynamic pedestrian cost map

3.2 路径规划模块

路径规划模块包括全局规划器和局部规划器。全局规划器以静态障碍代价地图作为输入，在规划路径时不考虑机器人的机械性能和运动学约束，通过A*算法[22]规划出从机器人当前位置到期望目标位置的最优路径，并为局部规划器提供初始值。局部规划器在全局最优路径上采样路径点，并优化机器人当前位置与采样路径点之间的全局路径子集，同时结合静态障碍代价地图和动态行人代价地图，在跟随全局路径的同时将机器人的外形、动力学模型、运动性能等纳入局部规划的范围，这里使用TEB(Timed-Elastic-Band)算法[23]实现。考虑全局规划器和局部规划器不同的规划范围等因素，两者分别以1 Hz和5 Hz的频率运行。

机器人执行已规划的路径需具备局部定位能力，使用机器人底盘的里程计提供机器人短时定位信息和运动状态反馈。局部规划器以机器人里程计作为反馈输出机器人控制指令以跟随规划的路径，从而实现对目标行人的持续跟随。由于本方法以较高的频率进行路径规划，且期望的目标位姿相对于机器人坐标系计算，因此所提方法在长距离跟随中并不会受累积误差的影响。

4 实验验证

4.1 行人运动状态估计与动态行人避障实验

首先对基于UKF的多行人跟踪器进行测试，同时考虑行人交错的情况用于验证行人跟踪轨迹的持续性。实验机器人保持静止状态以便于更好地测试跟踪器性能。实验结果如图3所示，其中第1行给出了图像中被跟踪行人的位置，第2行中蓝色3D包围框表示三维行人检测方法输出的结果，多行人跟踪器输出的结果用虚拟行人模型给出，其头上数字为跟踪轨迹ID、箭头指示行人运动方向，地面上的彩色曲线描述了各行人的历史运动轨迹。

在图3(a)中创建了行人0的跟踪轨迹，而此时行人1未达到创建跟踪轨迹的条件，直到图3(b)时才创建行人1的跟踪轨迹。随着行人1的运动，行人0在图3(c)所示时刻被行人1完全遮挡，此时行人0的运动状态由UKF的预测状态提供。当行人0再次被检测到(见图3(d))，他被原有的跟踪器继续跟踪而未切换ID。与图3(c)类似的遮挡情况在图3(e)和图3(g)中再次出现，在这两个场景中分别发生了行人2遮挡行人1以及行人3遮挡行人0和行人1。可以看出遮挡消失后的跟踪轨迹依然能够维持原有跟踪轨迹ID，较好地实现了对行人状态的预测。

图3 多行人运动状态估计实验结果Fig.3 Experimental results of motion estimation of multiple pedestrians

在基于UKF的多行人跟踪器基础上，结合动态行人代价地图，对机器人避让行人的能力进行测试。图4给出了实验结果，其中每组数据由3部分组成：上方图像为第三视角图像，左下角图像为机器人相机采集的图像及其图像处理结果，右下角图片包含了激光点云数据、干扰行人跟踪结果和路径规划结果。

当机器人运行到图4(a)所示位置时，多行人跟踪器开始跟踪和预测干扰行人A的运动状态。当行人A运动到图4(b)位置时，机器人根据实时代价地图规划从行人A的前方绕过，接着行人A快速到达机器人前面，机器人转而减速并重新规划为从行人A的身后绕行，如图4(c)所示。之后干扰行人B在机器人与目标行人之间穿行，如图4(d)所示，此时机器人规划从行人B身后绕过。需要说明的是，在行人B脱离机器人相机视野后，虽然3D行人检测已经无法提供行人B的测量数据，但是跟踪器仍然会利用运动模型继续预测行人的运动状态，如图4(e)所示，直到行人B的运动轨迹被清除。该实验表明了机器人能够在跟随过程中躲避动态行人。

图4 机器人行人跟随过程中的动态避障实验结果Fig.4 Experimental results of dynamic obstacle avoidance in process of mobile robot’s person following

4.2 机器人长距离跟随实验

为进一步验证所提方法的性能，进行长距离行人跟随实验。图5给出了跟随实验结果，其中中间图像为卫星地图以及运动路线，周围图像描述了卫星图中各标记位置对应的机器人跟随情况。实验中机器人的平均运动速度约0.9 m/s，全程总计800余米。实验表明基于所提方法机器人可以在室外环境中稳定跟随目标行人。

图5 移动机器人长距离跟随实验结果Fig.5 Results of long-distance person following of mobile robot

5 结论和展望

提出了一种基于路径规划的无碰行人跟随方法，基于激光点云分割提供的静态障碍物信息及基于3D行人检测的多行人UKF跟踪器估计的行人运动状态，分别生成静态障碍代价地图和动态行人代价地图，在此基础上，通过低频全局规划和高频局部规划结合的方式实现对目标行人安全、无碰地跟随。所提机器人跟随方法同时适用于室内和室外环境，但是在特别狭窄的场景中路径规划方法的规划速度会受到影响，因此不太适用于过于狭窄的场景。此外，由于感知模块需要使用RGB相机提供的图像语义信息，因此无法在夜晚暗光条件下提供准确的定位信息。

在后续工作中，将继续提高机器人在人类环境中运行的安全性以及密集人群场景下机器人社会性的研究，在机器人跟随中考虑靠右行走、文明礼让等更多社会规范。