IROS2019国际学术会议论文集 0167

Map Based Human Motion Prediction for People Tracking Florian Beck1and Markus Bader2 Abstract Mobile service robots deployed in populated en vironments like train stations airports or offi ces are not only required to move safely but also in socially acceptable ways In order to achieve this robots need to be able to track people within their vicinity This work presents an approach to tracking people from a mobile robot platform incorporating a novel approach to human motion prediction Unfavorable viewing angles motion blur and ever changing light conditions are constant issues for sensors on mobile vehicles Therefore a system is needed which increases the tracking quality of humans in order to cope with a low detection rate The scientifi c contribution of this paper lies in a precise human model for tracking which utilizes historical spatial data of pedestrians from previous detection and from simulation The model is em bedded into a particle fi lter based tracking approach designed for the use on a moving platform and is able to incorporate a variety of person detectors Experiments conducted prove that the proposed method increases tracking quality especially at a low detection rate I INTRODUCTION Mobile robotics is currently a very active fi eld of research and engineering with a broad range of applications Mobile robots used for tasks like transportation of goods 1 guid ance 2 or in home care assistance 3 4 are increasing in popularity A signifi cant factor in the success of such vehicles is their ability to complete tasks in populated areas alongside humans or in collaboration In order to attain this ability a robot has to keep track of the people in its surroundings This work concerns an approach for tracking people near robots focusing on precise motion prediction In this paper we propose a particle fi lter based approach to people tracking which utilizes a novel human motion model This motion model not only takes a map of the environment into account but is also able to make use of historical data on people s movements within the environment A sample visualization of the proposed forward prediction approach visualized in ROS rviz is shown in Fig 1 The results of our experiments suggest that our map based motion prior leads to signifi cant improvements in forward prediction accuracy providing high tracking quality even with low frequency person detection algorithms This is especially useful when modern deep learning based detection approaches are applied on battery powered mobile robots The research leading to these results has received funding from the Austrian Research Promotion Agency FFG according to grant agreement No 854865 TransportBuddy and No 855409 AutonomousFleet 1Florian BeckiswiththeAutomationandControlInstitute ViennaUniversityofTechnology Vienna1040 Austria beck acin tuwien ac at 2Markus BaderiswiththeInstituteofComputerEngineering ViennaUniversityofTechnology Vienna1040 Austria markus bader tuwien ac at Fig 1 This Figure shows a forward particle fi lter motion forward prediction using the proposed motion model Blue arrows are particles at the current time step red in the following one and green in the one after that One can see that the model nicely respects the map and explores all possible directions The rest of this paper is organized as follows fi rst we discuss related work on people tracking as well as on human motion prediction in Section II In Section III we introduce a model for human motion prediction based on a static map and past human activity in said map which is then utilized in the proposed approach to people tracking described in Section IV Section V discusses experimental results obtained by tracking people from a mobile robot platform in our lab II RELATED WORK Several approaches to people tracking for mobile robots have been proposed in the past Bellotto and Hu 5 compare fi ltering techniques for people tracking with a mobile robot Their approach relies on separate state estimation for each person to track requiring one fi lter each The tracker receives detection measurements from a laser based leg detector and a facial recognition algorithm for RGB images For the prediction a constant velocity model with zero mean Gaussian noise is used In order to match existing tracks and incoming measurements the authors apply a nearest neighbor data association strategy Their results show that given their detection models and the constant velocity motion model an unscented Kalman fi lter UKF performs almost as well as the particle fi lter However their use of the constant velocity motion model does not take advantage of the particle fi lter s capabilities A similar approach has been taken in 6 which proposes the use of an online color based classifi er to improve data association Regarding data association 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE7842 a Initial grid map b Distance transform c Likelihood fi eld d Inverse likelihood fi eld e Historical data integration 00 20 40 60 81 Fig 2 This fi gure illustrates the steps taken in order to construct an initial heat map based on a grid map more advanced algorithms such as multi hypothesis tracking MHT 7 as well as joint probability data association JPDA 8 have also been used however Linder et al 9 have shown that such techniques do not necessarily grant better results even though they are theoretically superior As the authors suggest this might be due to computational limitations or perhaps it arises from the fact that the lower number of parameters in nearest neighbor approaches make them easier to generalize Schulz et al 10 propose the use of particle fi lters with a sampling based variation of JPDA showing that particle fi lters can deal with more accurate motion priors due to their capability of representing a multi modal distribution Single particle fi lter approaches have also been suggested in literature Their disadvantage lies in high dimensional states which are present in multi target tracking for larger numbers of targets requiring an excessive amount of samples As shown by Khan et al 11 12 Markov chain Monte Carlo MCMC can be used for effi cient sampling in order to model target interactions in a single fi lter approach Another attempt to improve tracking performance is to improve forward prediction of the Bayesian fi lter in use which is the focus of this work as well A natural enhancement to the constant velocity CV model is the use of an interacting multiple model IMM fi lter as proposed in 13 which combines several motion models such as constant acceleration and coordinated turn models in addition to the constant velocity model Furthermore there is work on hu man motion prediction based on previously observed patterns using hidden Markov models HMM 14 or recurrent neural networks 15 Additionally the use of spatial force models has been proposed in 16 and 17 to naturally constrain the predicted paths and hence model motion more accurately Pellegrini et al 18 suggest a similar model however they specifi cally consider people s goal based decisions instead of relying on purely reactive behavior The work of Luber et al 19 is closely related to our approach The authors also make use of place dependencies in people s movements through counting appearances in grid cells of the map They employ an auxiliary particle fi lter which introduces a second layer of sample weights affecting the likelihood for sample selection Those weights are determined by a forward simulation of particles and their likelihood in the map In contrast to the approaches discussed in this Section we propose a tracking approach which derives a motion model directly from a map and from historical spatial data of human motion In comparison with 19 the forward simulation for particles with possibly low likelihood is circumvented in our approach since a change of direction towards high likelihood map regions is encouraged In contrast to simple CV prediction such an approach enables reasonably accurate prediction while being simple enough to avoid modelling hu man interaction and behavior Furthermore particle fi ltering enables the representation of multi modal predictions utilized by the map based prior III MAP BASED HUMAN MOTION PREDICTION Tomodelthestatetransitionprobabilitydensity fxk xk 1 xk xk 1 we need to model human motion In contrast to commonly used motion models such as the constant velocity model 5 we provide a more accurate model specifi cally considering the static part of the robot s vicinity defi ned by the map We assume that a static map is known which enables us to compute parts of the motion model in advance Furthermore if the map is known it is possible to utilize historical data on where people are most likely to move from their current position which accounts for some of the dynamic part of the robots environment Nevertheless in essence the approach is also applicable if the robot constructs its map online through a simultaneous localization and mapping SLAM algorithm A Heat Map Construction Consider the map of our lab in Fig 2a The map is rep resented as an occupancy grid with a resolution of 0 01 m pix where black pixels indicate occupied space and white pixels 7843 free space In the fi rst step we construct a likelihood fi eld indicating where people are likely to move on the map We assume that people can move within every cell on the map that is not occupied by an object i e the white part of the map Furthermore we expect that the likelihood decreases with proximity to a wall and it is impossible to move within pixels that are occupied i e walls or locations off the map To construct the likelihood fi eld we fi rst calculate the distance of every pixel from its nearest occupied pixel using a distance transform of the map image given in Fig 2b Assuming that the distances are normally distributed with a zero mean and a variance 5 0 we construct the likelihood fi eld in Fig 2c following Algorithm 1 Note that the values are mapped with colors according to the jet colormap such that blue means least likely and red most likely similar to heat maps where hot regions are colored red and cold regions blue As stated before we need the inverse hence we subtract all of the likelihood values from the maximum in order to obtain Fig 2d In the remainder of this paper we will refer to the inverse likelihood fi eld as the initial heat map since it serves as a starting point for historical data Input map M Output initial heat map H D distanceTransform M max 0 for r 0 to M rows do for c 0 to M cols do H r c 1 2 2 exp D r c 2 2 2 if max H r c then max H r c end end end for r 0 to M rows do for c 0 to M cols do H r c max H r c end end Algorithm 1 Compute initial heat map Now if a robot is often situated in the same environment it makes sense to incorporate previously observed people into the heat map Furthermore historical data can also be obtained from pedestrian simulations using a microscopic model such as the one provided in 20 of which we make use in this work To adjust a map using historical data we employ a counter for each grid cell in the map which is increased if a person has been observed at a certain posi tion To account for discrete observation times we linearly interpolate trajectories of people observed This procedure creates a 2D histogram on top of the map representing the likelihood of observing people in specifi c locations The procedure is summarized in Algorithm 2 We denote a track at time k with the tuple Tk ID Xk x holding a unique identifi er ID the set of particles Xkas well as sample mean x Generally if historical data is recorded long Input Tk 1 Tk Output H0 initialize all values in H0to 0 for i 0 to Tk 1 do for j 0 to Tk 1 1 do if Tk i ID Tk 1 j ID then idxx k convertToMapIndex Tk i x x idxy k convertToMapIndex Tk i x y idxx k 1 convertToMapIndex Tk 1 j x x idxy k 1 convertToMapIndex Tk 1 j x y P0 idxx k 1 idx y k 1 P1 idxx k idx y k for a b along line from P0to P1do H0 a b 1 end end end end Algorithm 2 Aggregate historical data enough initial likelihood fades as values are insignifi cantly small in comparison to historical data To avoid this we record historical data separately and incorporate a normalized version into the initial heat map Also keeping the non normalized version provides the advantage of historical data being easily added to at a later point in time This data can then be normalized each time an updated version of the heat map is generated Since people do not always follow the exact same paths it makes sense to apply a Gaussian blur to historical data in order to smooth likelihoods of path deviations Incorporating data into the initial heat map and applying a Gaussian blur with a kernel size of 15x15 results in Fig 2e B Motion Prediction In order to predict human motion we have to gen erate samples from fxk xk 1 xk xk 1 with state xk x y x y T In order to predict the motion of a person from their current position and velocity we utilize the local neighborhood of the person s state estimate on the heat map We consider the next position reachable from the current position given the person s current velocity estimate Since the person might change direction we take into consideration not only the position attainable from the current angle of the velocity vector but also the change in direction with a resolution of 2 p where p is the number of different directions The number of directions depends on the grid map s granularity and also on the person s velocity p 8 or p 16 are reasonable options Fig 3 shows a velocity vector r and all possible rotational changes with p 8 7844 r r0 r0 r0 r0 r0 r0 r0 0 Fig 3 Given the current velocity vector r at an angle we take into account different changes in direction 0in order to end up with a new velocity vector r0 For each direction considered we multiply the values on a line using Bresenham s line algorithm from the current position towards the end point of the velocity vector the position reachable in one time step Multiplication enables correct behavior if there is a wall along the line i e the product respectively the likelihood is zero Taking velocity into account here bares the advantage that enough space is available to move in a certain direction with the current speed Of course since velocities differ we have to normal ize the product by the number of steps on the line The value obtained here serves as a pseudo likelihood for the considered direction Again normalizing over all directions one can create a discrete probability distribution f 0 0 of directions from which new directions for those particles can be sampled Suppose the new direction of the velocity vector is rotated by 0 resulting in the bold vector r0 One important thing to consider here is that using this method a prediction might get stuck in a local minimum if grid map cells in the local environment have similar values This could cause the prediction to basically spin in place In order to work around this issue we apply an additional weighting function for sampling 0 making it more likely for particles to maintain their direction A natural way of doing this is to weight the angular change by a normal distributed variable w 0 N 0 2 0 Now that we have determined the new direction for a particle the rest of the forward prediction is similar to the constant velocity motion model As stated before 0is obtained by drawing a sample from f 0 0 The variables xand yare noise terms sampled from zero mean Gaussian distributions which enable continuous changes in direction from the discrete angles 0 Algorithm 3 summarizes the procedure for particle forward prediction IV PEOPLE TRACKING The motion prediction approach proposed in the previous section is subsequently applied in order to construct motion priors for particle fi lter based tracking of people A Particle fi lter based State Estimation Using particle fi lters to keep track of a person s move ments instead of commonly used Kalman fi lters including Input particle xk 1 real time between step k 1 and k dt angle partitions p heat map H Output particle xk angle probabilities p person orientation from velocity atan2 xk 1 y xk 1 x r px k 1 x2 xk 1 y2 for i 0 to p 1 do tar 2 i p x0 k xk 1 rcos tar y0 k yk 1 rsin tar for a b along a line from xk 1 yk 1 to x0 k y 0 k do idxx convertToMapIndex a idxy convertToMapIndex b angle probabilites i H idxx idxy end angle probabilites i w 2 i p normalize angle probabilities to 0 1 end draw d from angle probabilities 0 2 p d next atan2 sin 0 cos 0 dt draw x N 0 2 x draw y N 0 2 y xk xk 1 xk 1dt 1 2 xdt 2 yk yk 1 yk 1dt 1 2 ydt 2 xk rcos next xdt yk rsin next ydt Algorithm 3 Generate Forward predicted sample their non linear variants comes with the advantage that when a heat map motion model is used densities can fl oat around obstacles given that they are not restricted to Gaussian distributions as reported in 10 In this work we employ an SIR particle fi lter as described in 21 with low variance re sampling to estimate the probability density fxk z1 k xk z1 k of a person s state xkgiven the measurements z1 k As a proposal distribution we utilize a human motion model defi ned by the pdf fxk xk 1 xk xk 1 In order to calculate sample weights we utilize the importance density defi ned by a measurement model namely fzk xk zk xk B Measurement Model The purpose of the detection model is to describe how a detection is generated from a person s state We make use of a simple Gaussian measurement model defi ned as follows zk Hxk vk 1 Here xkis as used above a random variable describing the state of a person zkrepresents the detection H is the observation matrix extracting the measurement from the state and vk N 0 Czk is a noise vector Since the 2D position of the person is already part of their state it 7845 is suffi cient to use a linear time invariant model with the observation matri