Learning the Motion Patterns of Humans for Predictive Navigation Shu-Yun Chung and Han-Pang Huang, Member, IEEE

Abstract—To achieve fully autonomous mobile robot in crowded environments, an efficient and real-time motion planning is necessary. In this paper, an A*-based predictive motion planner is presented for navigation tasks. A generalized pedestrian motion model is also introduced in this paper. By understanding pedestrian motion patterns, the robot can further predict their motions and avoid the collision as early as possible. The simulations and experiments are also shown to validate the idea of this paper.

B

I. INTRODUCTION

Y the efforts of robotic researchers, there has been a great progress in robotic techniques. Robots are no longer only operated in laboratories and factories. On the contrary, lots of novel robots were designed and developed to work in the human living environment in the last two decades. Different kinds of service robots provide assistance to people in hospitals [2, 9], home environments[5], office buildings[12], museums[10], and exhibitions. (Fig. 1) In crowded and populated environment, real time motion planning of mobile robots is an important and crucial constraint. Since the motions of pedestrians are usually uncertain, configuration space will be highly transitory. Traditional motion planning which queries a complete path from current position to the goal often takes too much time to satisfy the real time requirement. Although reactive motion planning can rapidly estimate the appropriate next motion, it still easily gets blocked in complex cases. In other words, the robot not only needs to plan a path fast but also has enough look-ahead to predict the scenes. A look-ahead planner can efficiently decrease the probability of collision. However, it is required a good predictor for the scene forecast. Thus two main topics are discussed in the paper. The first part focuses on the learning of pedestrian motion models. A generalized pedestrian motion model is proposed. In the second part, a predictive motion planner is developed.

This work is partially supported by the Industrial Development Bureau, Ministry of Economic Affairs of R.O.C. under grants 97-EC-17-A-04-S1-054. Shu Yun Chung is currently a Ph.D. student in Department of Mechanical Engineering, National Taiwan University, Taipei, Taiwan ( e-mail: [email protected]). Han Pang Huang is a professor of Department of Mechanical Engineering, National Taiwan University, Taipei, Taiwan (phone: 886-2-33664478; fax: 886-2-23676064; e-mail: [email protected]). He is also the director of robotics laboratory.

Fig. 1 Service robots usually work in the crowded environments.

The sections in this paper are organized as follows. In section II, the pedestrian model and the learning method are introduced. The predictive A* planner is described in section III. Section IV represents simulation and experiment to validate the proposed planner. Finally, conclusions are summarized in section V. II. PEDESTRIAN MOTION MODEL To predict the pedestrian motion, one of the best ways is to recognize the pedestrian motion patterns. In the short term prediction, we can easily utilize certain motion models, such as constant velocity or constant acceleration, to predict the next action of pedestrians. However, in the long term prediction, it is not so obvious to define a motion pattern. The robot must understand the relationships between the pedestrians and the environments. Several previous researches have discussed the learning methods for pedestrian motion patterns. Most of them emphasized the concept of goal-directed. By recognizing the potential goals that people might go forward, we can further predict their motions in the next few seconds. For example, Foka [4] proposed to predict the human motion by manually defining the “hot points” where people may approach in daily life. Bennewitz [1] used expectation maximum (EM) to clustering the collected pedestrian trajectories and obtain the potential goals leading the movement of human in the environment. He further derived a hidden Markov model (HMM) applied to estimate the future motion of people. Yen [11] used a navigation function (NF) to provide suggestions on pedestrian motions. The transition probabilities of gradient direction based on NF are available with statistically analyzing the frequency of certain directions leaded by NF. This method can efficiently model the pedestrian motions in the certain place after locating the potential goals.

However, above methods are required either to define potential goals manually or to recollect the trajectories while environments are different. These processes are tedious and time-consuming. In general, the environment structures are similar in the same building. The size of doors and corridors are all similar. Our purpose is to learn a generalized pedestrian motion model. It allows the robot to work in different but similar places without recollecting new trajectories or re-training the motion model. A. Potential Goal Extraction At first, a generalized Voronoi graph (GVG)[3] of the environment is extracted for map division and potential goal searching. The environment map is divided into several submaps. Each submap has its exits which are regarded as potential goals. For instance, 5 submaps are shown in Fig. 2(b). The critical points of GVG are extracted for the potential goals. After map division, a navigation function (NF) of every potential goal in each submap is calculated. In the meanwhile the distance map (DistMap), which represents the distance from a grid location to its closest obstacle, is also evaluated. B. Generalized Pedestrian Motion Model Similar to [11], we also utilize NF as the framework. An example for 8-neighbor NF is shown in Fig. 3(a) and the goal is marked as “G”. By transferring the map into grid space, the location of the pedestrian can be represented as one of the grids.

Every grid state has 8 neighbors which are considered as the next potential directions. NF provides the suggestions and at least one optimal direction in each grid, shown as Fig. 3 (b).Several directional lots corresponding to the value of NF are grouped in each 45 degrees. For example, there are 5 lots in Fig. 3 (c). Because of a small number of directional lots beyond lot3, they are all marked as lot3 in this paper. To comprehend the relationships among NF, DistMap and environments, 32 trajectories of pedestrians were collected and shown in Fig. 4(a). Fig. 4(c) displays the statistical results of lots with different distance to goal. As can be seen, pedestrians often follow the directions of lot0 and lot1 which are marked as blue lines and red lines. Only when the pedestrians approach the goals, the percentage of other directions would increase. It is because the extracted goals sometimes locate in the corners or doorways. To prevent the collision, people would usually choose to “detour” around the corners and violate the optimal directions suggested by NF. Besides, one might notice that only few paths which are closed to the wall. It is reasonable since people naturally tend to walk a path that gives ample clearance from obstacles, rather than passing very close to them. All the evidence points that the use of two features, distance to goal and distance to obstacle, we could be able to construct a more generalized pedestrian model. It is the reason that NF and DistMap are generated in each submap after map division. Here we draw the pedestrian motion model as p ( ok +1 | ok , G ) . ok and ok +1 indicate the states of the pedestrian in continuous

space at time step k and k + 1 . G is the goal of the pedestrian. It covers the information for corresponding NF and DistMap..

(a)

(b)

(a) 20 0

0

2

4

6 8 10 distance to goal (m)

12

14

16

0

2

4

6

8 10 distance to goal (m)

12

14

16

0

2

4

6

8 10 distance to goal (m)

12

14

16

0

2

4

6

8 10 distance to goal (m)

12

14

16

40 Lot1

(d) (c) Fig. 2 map division. (a) environment map, (b) GVG structure and critical points for potential goals, (c) 4-neighbor NF of the potential goal 2 in the blue submap, (d) distance map of the blue submap.

Lot0

40

20 0

Lot2

40 20 0

Lot3

40 20 0

(a) (b) (c) Fig. 3 (a) Navigation function. Black grids are occupied by obstacles, (b) NF model. the best direction denoted by the long red arrow, (c) NF-based pedestrian motion model. 5 directional lots are grouped

(b) (c) Fig. 4 (a) 32 trajectories collected by a laser range finder. lot0: blue line, lot1:red line, lot2:green dot, lot3: black dot, (b) the picture of environment, (c) statistical results of different lots.

Since the pedestrian model is built on the grid space, the state of pedestrian is discretized and represented as okg in this

and the corresponding weighting function ω with a Gaussian distribution N ( μi , Σi ) . EM is an iteratively optimization

paper. Thus the pedestrian model in grid space can be rewritten as p ( okg+1 | okg , G ) .

algorithm which involves two steps, E-step and M-step. In the E-step, the responsibilities γ ni are evaluated by

We assume that the information can be factorized by NF and DistMap. The pedestrian model can be represented as

current parameters.

(

p okg+1 | okg , G

)

n

= p ( okg+1, NF , okg+1, DistMap | okg, NF , okg, DistMap , π NF , π DistMap )

(1)

= p ( okg+1, DistMap | okg, DistMap , π DistMap ) ⋅ p ( okg+1, NF | okg, NF , π NF ) g oig, DistMap is the distance from oi to the closest obstacles.

(

p okg+1, DistMap | okg, DistMap , π DistMap

)

and

(

p okg+1, NF | okg, NF , π NF

)

are the

motion models in DistMap and NF. Here we lead into a parameter m , called motion primitive. Each motion primitive has its weighting function ω and transition probabilities of directional lots θ . According to total probability and Bayes rule, p ( okg+1, NF | okg, NF , π NF ) can be further derived as

( = ∑ p (o

p okg+1, NF | okg, NF , π NF g k +1, NF

⎧

) g k , NF

, mi | o

i

, π NF

)

(

(

= ∑ p ( okg, NF | ωi ) ⋅ p okg+1, NF | θi , okg, NF , π NF i

)

(2)

)

Now p ( okg+1, NF | okg, NF , π NF ) can be viewed as the linear combination with different motion primitives (Fig. 5). The weighting function ωi of each motion primitive is modeled as a Gaussian distribution. C. Pedestrian Model Learning According to Eq.(1), we can see that the pedestrian model can be factorized into motion model in DistMap and NF. Thus the learning methods are presented and discussed respectively. 1) Motion Model in DistMap To estimate the parameters of the motion model in DistMap, the frequency of certain distance in DistMap is statistically analyzed. Here the probability from distance i to distance j is marked as pij and can be estimated by pij

( the number of =

( the number of

⎩

⎭

i

(4)

Where η1 is the normalized factor. γ ni is the responsibility of data point xn for motion primitive mi . In the M-step, the parameters are estimated with corresponding responsibilities γ ni . μinew = η2 ∑ γ ni ⋅ xn n

(5)

(

)(

Σinew = η2 ∑ γ ni ⋅ xn − μinew ⋅ xn − μinew n

θ

new i

(

(lot j ) = η3 ⋅ ∑ p xn ( lot j ) | μ n

new i

)

,Σ

new i

(6)

)

(7)

Where η2 , η3 are normalized factors. θinew (lot j ) is the probability of lot j in θinew . xn ( lot j ) is the data point xn with direction lot j .

∝ ∑ p ( okg, NF | mi ) ⋅ p okg+1, NF | mi , okg, NF , π NF i

⎫

γ ni = η1 ⋅ ∑ ln ⎨∑ p ( xn | μiold , Σiold ) ⋅ p ( xn | θiold ) ⎬

Dij ) Di )

(3)

Where Di : distance i Dij : from distance i to distance j 2) Motion Model in NF We utilize expectation-maximization (EM) to estimate the parameters of the motion model in NF. The parameters include the probabilities of lots θ in each motion primitive

In order to learn the parameters of the pedestrian model, 99 trajectories are collected from three different but similar corridor environments shown in Fig. 6(a). 5103 data points are extracted from those trajectories. To choose the number of motion primitives K, the likelihoods of the pedestrian models with different K are compared and shown in Fig. 6(b). Cross validation is applied to prevent over fitting problem. Considering the compactness and modeling performance, K=5 is chosen in this paper. The estimated parameters are listed in TABLE I.

μ m0 1.330 m1 2.871 m2 4.811 m3 6.926 m4 10.213 Unit of μ : meter

TABLE I MOTION PRIMITIVE p(lot0) p(lot1) Σ 1.629 0.247 0.568 2.078 0.276 0.532 2.779 0.522 0.405 6.486 0.414 0.565 9.091 0.434 0.556

p(lot2) 0.099 0.139 0.033 0.008 0.007

θ1

θ2

θ3

×

×

×

ω1

ω2

ω3

p(lot3) 0.084 0.051 0.039 0.012 0.003

Fig. 5 motion model in NF can be viewed as the linear combination of motion primitives.

(a) (b) Fig. 6 (a) 99 trajectories collected from different environments, (b) the fitness score with different number of motion primitives.

3) Pedestrian Model Evaluation To evaluate the pedestrian model, it is applied to a new environment. 32 trajectories are collected and classified into corresponding motion models for different goals. Some pedestrians are required to walk in unusual ways, such as walking near the wall or moving circularly in the corridor. The score of each trajectory, which indicates the likelihood of a certain motion model, is display in Fig. 7(e). 4 unusual trajectories are successfully detected and the other trajectories are accurately classified.

D. Motion Prediction Once the goal of the moving object is given, pedestrian motion model can be utilized to predict the pedestrian motions. The prediction can be modelled as p ( okg+ N | okg , G ) in grid space. According to total probability, we can further derive it as p ( okg+ N | okg , G )

∑ p (o

= =

) (

| okg+ N −1 , G ⋅ p okg+ N −1 | okg , G

g k+N

okg+ N −1

k + N −1

∏ ∑ p (o i =k

(

g i +1

oig

| oig , G

)

(8)

)

)

p oig+1 | oig , G is learned from Eq.(1). If we define Okg as

the data set of a trajectory from time step 1 to k, the prediction with multiple potential goals can be written as

(

)

(

) (

)

p okg+ N | Okg = ∑ p okg+ N | okg , Gki ⋅ p Gki | Okg

Gki     (a)

Prediction

{

(b)

Where Okg  o1g , o2g ,", okg The term p ( G

i k

|

g Ok

)

(9)

Goal Weighting

}

of Eq.(9) is represented as the goal

weighting which can be estimated sequentially from the data set of the trajectory.

(

)

( ) ( ) ) ⋅ ∑ p (G | G ) ⋅ p (G | O )

p Gki | Okg ∝ p okg | Gki , okg−1 p Gki | Okg−1

(

= p o | G ,o (c)

(d)

70 60

score

50 40 30 20 10 0

0

5

10

15 20 trajectory index

25

30

35

(e) Fig. 7 (a) environment picture, (b) 28 trajectories are successfully classified. Motion model of A : blue line. Motion model of B: red line. Motion model of C: green line, (c)(d) 4 unusual trajectories are detected, (e) trajectory score.

g k

i k

g k −1

Gka−1

i k

a k −1

a k −1

g k −1

(10)

Fig. 8 simulates the prediction results by observing sequential pedestrian motions. The video chip is shown in [13]. Four red cones marked as A to D are the potential goals of the pedestrian which are extracted from the GVG skeleton. The pedestrian moves from A to C. The simulated trajectory of the pedestrian is generated by A* planner. Here TS and PTS of figure caption mean current time step and predicted time step. The height of the orange surface represents the occupied probability at predicted time step. The higher part of surface indicates higher occupied probability and its corresponding color will approach golden yellow. To simplify the description, we use the symbol P ( A) to indicate the probability of goal A.

(a) map (c) TS = 3, PTS = 8 (b) TS = 3, PTS = 3 Fig. 8 motion prediction by the pedestrian model. TS and PTS indicate current time step and predicted time step

Two time steps TS = 3 and TS = 10 are shown in Fig. 8. At TS = 3, the estimated probability of potential goals from A to D is 0.16, 0.24, 0.35 and 0.24. Since the likelihood is almost equal, the predicted occupied areas will go toward four directions individually shown as Fig. 8 (b)(c). However, after a few seconds, the intention of pedestrian is more obvious and the trajectory causes P ( C ) to raise rapidly. At TS = 10, the likelihoods of goals are 0.0017, 0.04, 0.917 and 0.04. The predicted occupied areas are merged from four to one (Fig. 8 (d)) III. PREDICTIVE NAVIGATION In this section, a predictive planner with the pedestrian motion model is developed. By predicting pedestrian motions, the robot can generate a more appropriate path in dynamic environments. Moreover, the deliberation time of path planning is usually limited. In the complex environment, the robot sometimes cannot query a complete path in a short period. Anytime algorithms[6-8] always keep a current best solution whatever the complete and optimal planning has been finished. Thus an anytime planning framework is also implemented in our predictive planner. A. Predictive Anytime A* Planner To efficiently query a trajectory in C-T Space, an A* based planner is adopted. A NF for the goal of the robot is generated for the heuristic term of A* and utilized to guide the planner. Because of the NF, the robot only needs to consider the local area around it, called active region. The NF can make the planner follow the anytime framework and help to query a suboptimal path very fast without being trapped in the “local minima”. In Fig. 9, we show the difference between Euclidean distance and NF in a spiral shape environment.

(a) (b) Fig. 9 (a) Euchildance distance easily causes local minima. Active region is marked as blue rectangle, (b) NF is the better distance description for navigation.

(d) TS = 10, PTS = 15

IV. SIMULATION AND EXPERIMENT A. Predictive Planning in the Crowded Environment In this simulation, we simulated a crowded environment. Five pedestrians simultaneously move toward their goals. The simulated environment is displayed in Fig. 10(a). There are six potential goals in this environment. In this simulation, the positions of pedestrian and the robot are given but accompany with Gaussian noise. Data association of pedestrians is unknown. The robot is assigned to move from left side to right side. At the same time, it is required to track all the pedestrians. Deliberation time to query a trajectory of the robot is set to be 30ms in this simulation. Fig. 10(b)-(f) show the simulation results in different time steps. One thing we should notice is that the robot chooses the trajectory at bottom side in TS = 7 (Fig. 10(c)). The reason is that the bottom side area will get the lower collision cost in the next few seconds (Fig. 10(d)). The simulation results demonstrate the robot can successfully pass through a crowded environment.

(a)

(b)TS =5, PTS = 5

(c) TS = 7, PTS =7

(d) TS = 7, PTS = 10

(e)TS = 9, PTS = 9 (f) TS = 13, PTS = 13 Fig. 10 Crowded environment. TS and PTS indicate current time step and predicted time step

B. Compliant Motion In the experiment, the environment consists of a narrow corridor and several rooms. The video chip is shown in [13]. The width of corridor is limited and is only capable of one pedestrian passing at one time. The environment map is shown in Fig. 11. Four potential goals marked as A, B, C, and D are extracted. The goal of the pedestrian is located at D. The estimated trajectories of the robot and pedestrian are also represented in Fig. 11. The sequential images are demonstrated in Fig. 12. At first, since P(C) and P(D) are high, the robot comprehends that it may collide with the pedestrian in the next few seconds. As a result, the planner generates a trajectory toward the waiting point and chooses to stay there until the pedestrian passes through the corridor. To emphasize the effect of prediction, we also compare it with a reactive planner. Five experiment trials under similar conditions are done for each planner. The statistical results of travel distance and period are shown in Fig. 13. Since the trajectory generated by the reactive planner easily blocks the pedestrian, the robot is often compelled to move back. Accordingly, the predictive planner gets shorter travel distance and period than the reactive planner does.

Fig. 11 navigation task.

(a) TS = 5s (b) TS = 8s (c) TS = 20 s Fig. 12 different time steps in navigation task.

Fig. 13 Predictive planning and reactive planning

V. CONCLUSION In this paper, we proposed a predictive navigation planner applied in dynamic environments. A generalized pedestrian model was represented and trained by analyzing the collected pedestrian trajectories. The pedestrian model can be applied in any similar environments without recollecting pedestrian trajectories. The unusual trajectories can also be detected by the proposed pedestrian model. Moreover, by constructing the GVG skeleton of environment, most of potential goals can be automatically extracted and utilized for the motion prediction. By incorporating prediction and anytime framework in our planner, the robot can efficiently query trajectories in C-T Space. Simulations and experiments show that proposed planner efficiently predicts the occupied areas of pedestrians in complex dynamic environments. Finally, in the narrow corridor case, the robot can also perform compliant motion to avoid the collision. The statistical results show that the proposed predictive planner gets better performance than a reactive one. REFERENCES [1] M. Bennewitz, W. Burgard, G. Cielniak, and S. Thrun, "Learning Motion Patterns of People for Compliant Robot Motion," Int. J. Robotics Research, vol. 24, pp. 31-48, 2005. [2] F. Carreira, T. Canas, A. Silva, and C. A. C. C. Cardeira, "i-Merc: A Mobile Robot to Deliver Meals inside Health Services," Proc. IEEE Int. Conf. on Robotics, Automation and Mechatronics, pp. 1-8, 2006. [3] H. Choset and J. Burdick, "Sensor based planning. I. The generalized Voronoi graph," Proc. IEEE Int. Conf. on Robotics and Automation vol. 2, pp. 1649-1655 1995. [4] A. F. Foka and P. E. Trahanias, "Predictive autonomous robot navigation," Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and System, vol. 1, pp. 490-495, 2002. [5] B. Graf, M. Hans, and R. D. Schraft, "Care-O-bot II - Development of a Next Generation Robotic Home Assistant," Autonomous Robots, vol. 16, pp. 193-205, 2004. [6] E. A. Hansen and R. Zhou, "Anytime heuristic search," J. Artificial Intelligence Research, vol. 28, pp. 267-297, 2007. [7] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, and S. Thrun, "Anytime Dynamic A*: An Anytime, Replanning Algorithm," Proc. Int. Conf. on Automated Planning and Scheduling (ICAPS), 2005. [8] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, and S. Thrun, "Anytime search in dynamic graphs," Artificial Intelligence, vol. 172, pp. 1613-1643, 2008. [9] M. Y. Shieh, J. C. Hsieh, and C. P. Cheng, "Design of an intelligent hospital service robot and its applications," Proc. IEEE Int. Conf. on Systems, Man and Cybernetics, vol. 5, pp. 4377-4382 2004. [10] S. Thrun, M. Beetz, M. Bennewitz, W. Burgard, A. B. Cremers, F. Dellaert, D. Fox, D. Hahnel, C. Rosenberg, N. Roy, J. Schulte, and D. Schulz, "Probabilistic algorithms and the interactive museum tour-guide robot Minerva," Int. J. of Robotics Research, vol. 19, pp. 972-999, 2000. [11] H. C. Yen, H. P. Huang, and S. Y. Chung, "Goal-Directed Pedestrian Model for Long-Term Motion Prediction with Application to Robot Motion Planning," Proc. IEEE Int. Conf. on Advanced Robotics and its Social Impacts, pp. 1-6, 2008. [12] H. Zhang, J. Zhang, R. Liu, and G. Zong, "Mechanical design and dynamcis of an autonomous climbing robot for elliptic half-shell cleaning," Int. J. of Advanced Robotic Systems, vol. 4, pp. 437-446, 2007. [13] http://www.youtube.com/watch?v=1Vmu9rI1Ah0