Active Pedestrian Following Using Laser Range Finder

I. INTRODUCTION

The ability of robots to track and follow moving targets is essential to many real life applications such as museum guidance, office or library assistance. On top of being able to track the pedestrians, one aspect of human-robot interaction is robot’s ability to follow a pedestrian target in an indoor environment. There are various scenarios where the robot can be given instructions such as holding books in a library or carrying groceries at a store while following the pedestrian target.

The key components of moving target following technique include Simultaneous Localization and Mapping(SLAM), Detecting and Tracking Moving Objects (DATMO),and motion planning. More often than not, the robots are required to operate in dynamic environments where there are multiple pedestrians and obstacles in the surroundings.Consequently, tracking and following a specific target pedestrian become much more challenging. In other words, the following behaviors must be robust enough to deal with constant occlusions and obstacle avoidances.

When designing the following algorithm, one intuitive approach is to set the target location as the destination for the robot. However, this approach can easily lead to losing the target because it does not react to the target’s motion nor consider the visibility problem (since the target may be occluded by obstacles and become invisible). For achieving robust target following and tracking, the robot should have the intelligent to predict target motion and gather observations actively.

In this paper, we propose a moving target following planner which is able to manage obstacle avoidance and target visibility problems. Experimental results are shown to compare the intuitive approach with our approach and prove the importance of active information gathering in planning.This paper is organized as follow: Section II discusses related works of DATMO and planning algorithms. Section III describes our DATMO system and introduces our target following planner. Lastly, Section IV illustrates the experimental results.

II. RELATED WORKS

There are various approaches to detect and track moving objects such as building static and dynamic occupancy grid maps proposed by Wolf & Sukhatme [1], finding local minima in the laser scan as in Horiuchi et al.’s work [2] or using machine learning methods in Spinello et al.’s work [3]. Most of DATMO approaches assume that the robot is stationary or has perfect odometry. When tracking moving objects using mobile robots, it has been proven in Wang et al.’s work [4], that SLAM and DATMO can be done simultaneously if the measurements can be divided into static

and dynamic parts. In our work, we implement a DATMO algorithm which is similar to the one in Montesano et al.’s work [5]. A scan matching method is used to correct robot odometry and moving points are detected by maintaining a local occupancy grid map. Moreover, Extended Kalman Filter(EKF) is applied to track the moving objects.

This paper aims to solve moving target following problem with the existence of obstacles. For navigating in static environments, there are many successful works such as Fox et al. [6], Ulrich & Borenstein [7] Minguez & Montano [8], Seder & Petrovi’c [9]. However, those methods are designed to reach a fixed goal and assume that the environment and robot states are fully-observable. Applying traditional obstacle avoidance algorithms on the target following task can fail easily because a moving target can change its speed and moving direction at anytime and the target can be occluded by obstacles. For planning under imperfect perception, Partially-Observable Markov Decision Process (POMDP) provides a general framework. However, using POMDP to compute optimal policies are usually very computationally expensive because it has to compute a plan over belief space (typically N-1 dimensional for an N-state problem.). For applying POMDPs on practical problems, most works aimed on reducing the dimension of belief space. Such like PBVI Pineau et al. [10], AMDP Roy et al. [11] and MOMDP Sylvie et al. [12]. Those methods are much faster than original POMDP, but their computational complexity are still too high to do real-time planning. Our method in this paper is more like a sub-optimal but fast approach: assuming that the robot always receives the most possible observations and plan a path which can reach the goal and minimize the uncertainty on particular states. For example, in Prentice & Roy [13], the robot aims on reaching the goal with minimum uncertainty on its position, so it may choose a path which is long but has enough localization landmarks.

In this paper, we propose a motion planner for moving target following. The planner uses an extension of dynamic window approach propsed by Chou & Lian [14] to find collision-free velocities and choose a proper velocity using heuristic search. Cost functions are designed for minimizing the distance between robot and target and maximize the possibility that the robot can keep observing the target in a fixed time horizon. Additionally, we apply the concept in nearness diagram algorithm such as the one in Minguez &Montanos’ work [8] for computing a better estimation of the distance between robot and target and therefore achieve a smooth, non-hesitating performance.

III. MOVING TARGET FOLLOWING AND OBSTACLE AVOIDANCE

It is essential that our DATMO system is able to track the moving target, pedestrian in this case, with great accuracy.The more precise pedestrian location acquired, the better the robot performs when following the target.

A． Detecting and Tracking Moving Objects

For detecting moving points in the laser data, we adopt the concept of occupancy grid map proposed by Wolf &Sukhatme [1]. A local occupancy grid map is aintained and used to differentiate the moving points. For robot localization, a scan matching technique called Iterated Closest Point (ICP) is used to acquire robot pose. The moving points are filtered out of the data prior to scan matching in order to maintain the pose accuracy. The detected moving points are segmented into numerous clusters and determined if they belong to a pedestrian using features such as motion velocity, local minima, and size. Finally, each of the pedestrian is trackedusing Extended Kalman Filter (EKF) which solves the occlusion problem and provides us the computational advantage in real time performance.

B. Following Method

Our goal is to accomplish moving target following with the existence of obstacles. A reasonable solution is to see it as a path planning problem and use obstacle avoidance algorithms to find collision-free actions. In this paper, we apply our previous work, DWA*, Chou et al. [10] as the obstacle avoidance algorithm.

The procedure of DWA* is shown in Fig. 1 (a), it is a trajectory-rollout algorithm. The right side of Fig. 1 (a) shows the procedure for computing proper motion commands. First,the environment information is realized as interval configuration for faster processing. Each interval value represents the maximum distance can be raveled by the robot on a certain circular trajectory. Second, the intervals are clustered as navigable areas. Third, for each area, a candidate velocity is determined according to an objective function. For each candidate velocity, a new robot position is computed as a new node and saved in a trajectory tree. Then a node with the minimum estimated cost value will be extracted as the base node for generating new nodes. The procedure is repeated until goal location is expanded or the tree depth reaches a certain value. After the tree expansion stops, the deepest node is determined as a temporal goal, and the present candidate velocity which can lead the robot to the temporal goal is selected.

In our work, we used two different methods for integrating DATMO and DWA*: pseudo goal method and trajectory optimization method. Pseudo-goal method is a more intuitive approach which does not consider the target velocity when following. Another approach is trajectory-optimization where a trajectory-rollout controller is used to approximate a predicted target trajectory and maximize target visibility.Their performances will be shown and compared in the next section.

1) Pseudo-goal method

When implementing people following algorithm, an intuitive approach is setting the present location of the moving target as the goal of navigation algorithm. Consider the tracked target at an angle with respect to the robot, DWA* algorithm will generate an angular and translational velocity which produces an arc-like trajectory. If the goal is within a close proximity of the robot, the angular velocity will be small and the rc-like route often results in an indirect or detour path for the robot to reach the goal (as shown in Fig. 2). On the contrary, if the goal is set further away from the robot at the same angle, any movement of the goal will result in a much larger displacement change compare to when the goal is close to the robot. Therefore, the angular velocity would increase in response to the substantial change of the goal location.Consequently, the trajectory becomes more direct toward the goal.

In the pseudo-goal method, the space in front of the robot is divided into 7 trajectories at 35_ , 55_ , 70_ , 90_ ,110_ , 130_ , 145_ as shown in Fig. 3. A pseudo goal is set 3 times the original distance between the goal and robot along each trajectory. After acquiring the pedestrian location (red circle in Fig. 3), the pseudo goal with trajectory closest to the pedestrian location is then selected. By setting the goal further away from the robot, it remedies the issue of small angular velocity and provides a much more direct path to reach the goal.

Another problem is the limited sensing ability. In thispaper, the algorithm is implemented on a mobile robot with a 180-degree POV LRF. Here we added an “information gathering mode.” When the moving target enters the“dangerous zone,” (0~35 degree and 145~180 degree in the robot coordinate) the robot will stop and turn to the target direction rapidly. Adding this mode greatly lowers the risk of losing the target.

Pseudo-goal method is straight-forward to implement and performs well when the target velocity is nearly constant.However, when the target velocity changes, the pseudo-goal trajectory selected may also alter from one to another and causes the robot to change its moving direction drastically.

2) Trajectory optimization method

Though the pseudo-goal method is very simple to implement, it can easily lose track of the target because it does not take the robot’s limited observability into consideration.Another drawback is that the pseudo-goal method can’t react quickly to the change of target velocity. A better method is to plan according to a reference trajectory but not a fixed location. In this way, target velocity can be considered and the robot observability can be considered by adding terms to the cost function.

附录 二 外文翻译（中文）

基于激光测距仪的行人跟踪

一、引言

在现实生活中机器人跟踪运动目标的能力得到广泛的应用，如博物馆导引，办公室或者图书馆帮助。除了能跟踪行人，仿人机器人的一个方面就是在室内环境下跟踪行人的能力。有很多种情况，机器人可以可发出指令，如当跟踪行人目标时可以保护图书馆的书籍或商店里的可携带物品。

运动目标关键部位的跟踪技术包括即时定位与地图构建（SLAM），运动目标的检测和跟踪（DATMO），和运动规划。通常机器人需要在周围有很多行人和障碍物的动态的环境下进行操作。因此，跟踪一个特定的行人目标变得更具挑战性的。换句话说，跟踪行为必须强大到足以处理在经常拥堵的环境中躲避障碍物。

当设计一个跟踪算法时，一个直观的方法是将目标位置作为机器人目标。然而，这种方法很容易导致失去目标，因为它不会对目标的运动做出反应也没有考虑可见性的问题（因为目标可能由于障碍物的遮挡变成无形的）。为实现强大的目标追寻和跟踪，机器人应该有智能预测目标的运动和聚集的观察能力。

在本文中，我们提出了一个能够处理避障和目标可见性问题的移动的目标跟踪方案。实验结果显示相比直观的方法，我们的方法证明了活动信息收集的重要性。本文的结构如下：第二节讨论相关DATMO和规划算法的问题。第三节介绍了我国DATMO系统以及我们的目标和下一步的计划。最后，第四部分阐述了实验结果。

二、相关作品

有很多种不同的方法来探测和跟踪移动对象，如Wolf & Sukhatme提出的建立静态和动态网格地图法，利用激光扫描在很小的地方寻找就像Horiuchi等人的工作或在Spinello等人的工作使用的机器学习方法。大多数DATMO方法假定机器人是固定的或具有完善的里程表。当使用移动机器人跟踪运动物体时，它已被证明在王等人的工作。就是，如果测量可分为静态和动态的部分SLAM和DATMO可以同时使用。在我们的工作中，我们执行的DATMO算法类似于在蒙特等人使用的一个。扫描匹配方法就是对机器人里程计和本地网格地图中移动的点是检测。此外，扩展卡尔曼滤波（EKF）被施加到跟踪移动的物体。

本文旨在解决移动目标伴随着障碍物存在时的跟踪问题。导航在静态的环境中，有许多成功的例子如狐狸等。Ulrich & Borenstein Minguez & Montano,Seder & Petrovi’c。然而，假定环境和机器人的状态完全可观，这些方法设计达成既定的目标，。应用传统的避障算法对目标的跟踪任务很容易失败。因为一个移动的目标可以改变其速度和移动方向以及目标可以随时被障碍遮挡。对于不完善计划，马尔可夫决策过程（POMDP）提供了一个总体框架。然而，利用POMDP解决最优策略通常是非常复杂的，因为它的计算规划已经超出了传统的空间（通常是n-1维的状态的问题。）。在实际问题中，大多数作品是以减少空间的维数使用s。如PBVI等人，AMDP Roy等人和MOMDP Sylvie等人。这些方法比原来的POMDP要快得多，但其计算复杂度仍然太高而不能做实时规划。我们在本文的方法更像是一个最优而且快速的方法：假设机器人总是接受最可能的可观察到的路径，并且规划一个可以达到的目标和减少不确定性特定的状态的路径。例如，在Prentice和Roy中，机器人的目的是在最低的不确定性情况下到达目标的位置，因此可以选择一条长但是有足够的定位标志的路径。

在本文中，我们提出了一个运动规划的移动目标跟踪。策划者采用Chou&Lian提出的动态扩展窗口的方法找到无碰撞速度和选择合适的速度使用启发式搜索。成本函数被设计为机器人与目标之间的距离最小化和，可以使机器人在固定的时间范围观察目标的一种最大化的可能性。此外，我们应用的概念接近图算法如Minguez和Montanos用于计算一个更好的估计机器人与目标之间的距离，从而实现光滑，快捷的性能。

三、运动目标跟踪和避障

我们DATMO系统的本质是能够精准的跟踪在这种情况下的行人运动的目标。行人的位置获得的越精确， 机器人跟踪目标越好。

A 运动目标的检测与跟踪

在激光数据下检测移动的点，我们采用由Wolf 和Sukhatme提出的栅格地图法。用本地网格地图来区分移动点。机器人定位，一个称为迭代最近点（ICP）的扫描匹配技术用于获取机器人姿势。移动的点被从之前的数据扫描过滤匹配以保持位姿精度。检测到的移动点分割成许多集群和确定它们是否属于一个行人使用的要素诸如运动速度，位置大小，尺寸。最后，每个行人跟踪采用扩展卡尔曼滤波器（EKF）解决了闭塞的问题，同时提供我们计算在实时性能上的优势。

B 跟踪算法

我们的目标是实现在有障碍物存在情况下的运动目标跟踪。一个合理的解决办法是把它作为路径规划问题，使用避障算法找到无碰撞的行为。在本文中，我们应用DWA *，Chou等前人的方法作为避障算法。

DWA的过程如图1(a)所示，它是一个轨道铺设算法。图1(a)的右侧显示用于适当的运动命令的程序。首先,这样的环境信息实现更快的处理过程。每个区间值代表可由机器人在一定的圆轨迹上行进的最大距离。第二，这个区间应在适航区域内。第三，各地区，一个机器人速度是根据目标函数的确定。为每个候选人的速度，一个新的机器人的位置被计算为在一个轨迹树保存的新的节点。然后，一个最小估计值节点将被提取为生成新的节点的基础节点。重复这一过程直到目标位置扩大或树的深度达到一定的确切值。树扩展停止后，最深的节点被确定为时间的目标，机器人的时间目标的当前速度被选择出来。

在我们的工作中，我们使用了两种不同的方法整合DATMO和DWA *：伪目标法及轨迹优化法。伪目标法是一种更直观的方法，不考虑当前目标的速度。另一种方法是轨迹优化法，是用轨迹控制器近似的预测目标的运动轨迹和最大限度地提高目标能见度。他们的运行将在下部分进行展示和比较。

1）伪目标法

当执行行人跟踪算法时，一个直观的方法是设置移动目标的当前位置作为导航算法的目标。考虑跟踪目标相对于机器人的一个角，DWA *算法将产生轨迹弧。如果目标是在接近机器人，角速度会很小和圆弧形路线的往往致使机器人在间接的或迂回路径上达到要达到的目标（如图2所示）。相反，如果目标是在相同的角度进一步远离机器人，任何的目标运动将导致一个比当目标接近机器人更大的位移变化。因此，角速度会增加响应目标位置的实际变化。因此，到达目标的轨迹更加直接了。

在伪目标法中，在机器人面前的空间被7 条轨迹分开，35，55，70，90，110，130，145，如图3所示。伪目标沿着每个轨迹把目标和机器人之间的距离设定为原来3倍。获取行人的位置后（图3红圈），伪目标轨迹接近行人位置，然后选择出来。按设定的比机器人更远的目标，弥补了小的角速度问题速，同时提供更直接的路径到达目标。

另一个问题是有限的感知能力。在本文里，是一个实现移动机器人180度的POV LRF。在这里，我们增加了一个“信息采集模式”。当移动目标进入“危险地带，”（在机器人坐标0 ~ 35度和145~180度）机器人将停止前进，并向目标相反方向迅速返回。加入这种模式大大降低了失去目标的风险。

伪目标法是实施的直线前进，运行时的目标速度几乎是恒定的。然而，当目标速度的变化时，假目标轨迹的选择也可以从一个改变到另一个使机器人明显改变其运动方向。

2）轨迹优化方法

虽然伪目标法实现起来非常简单，但是很容易失去跟踪目标。因为它不对机器人的有限的可观性进行考虑。另一个缺点是，伪目标的方法不能快速反应目标速度的变化。一个更好的方法是根据一个参考轨迹规划轨迹，但不是一个固定位置。以这种方式，可以考虑目标速度也可以通过添加函数考虑机器人的可观测性。