\documentstyle [11pt,twocolumn] {article} \input{psfig} \pagestyle{empty} \setlength{\topmargin}{-0.1 cm} \setlength{\textheight}{247mm} \setlength{\columnsep}{10mm} \setlength{\textwidth}{170mm} \setlength{\footheight}{0.0in} \setlength{\leftmargin}{20mm} \setlength{\rightmargin}{20mm} \setlength{\headsep}{0.0in} \setlength{\headheight}{0.0in} \setlength{\oddsidemargin}{-0.5cm} \setlength{\parindent}{1pc} \begin{document} \LARGE \bibliographystyle{ieeetr} \twocolumn[% \vspace{5mm} \begin{center} {\bf Real-Time Tracking of an Unpredictable Target\\ Amidst Unknown Obstacles} \end{center} \large \begin{center} {\bf Cheng-Yu Lee$^{(1)}$ \ \ \ Hector Gonz\'alez-Ba\~{n}os$^{(2)}$ \ \ \ Jean-Claude Latombe$^{(1)}$} \end{center} \normalsize \begin{center} (1) Robotics Laboratory, Computer Science Department, Stanford University, CA, USA\\ (2) Honda's Fundamental Research Labs., Mountain View, CA, USA\\ Email: chengyu@robotics.stanford.edu, hhg@hra.com, latombe@cs.stanford.edu \end{center} ] \small \begin{center} {\bf Abstract} \end{center} This paper describes a new approach to keep a moving target into the field of view of a robot, despite obstacles. Unlike previous work, this approach does not assume that a complete or partial model of the target's behavior is available, nor that a map of the obstacles is given. At approximately 10 Hz, the robot constructs a local map of the environment from range data, localizes the target in this map, and computes its new velocity command. This command is selected to minimize a function that measures the risk that the target may escape the robot's field of view by crossing an occlusion ray created by an obstacle. The risk function defined in this paper combines a reactive term, whose minimization handles immediate threats, and a look-ahead term, whose minimization improves the robot's future ability to react to evading motions of the target. The experimental robotic system has been remarkably successful at tracking a target performing brisk maneuvers and swift turns behind obstacles. \section{Introduction} This paper describes a robot, called the {\em observer}, whose task is to keep a moving target into its field of view. While previous tracking techniques assume that both a model of the target's bahavior and a map of the obstacles are given, our approach makes no such assumptions. At approximately 10 Hz, the implemented observer constructs a local map of the environment from sensed range data, localizes the target in this map, and computes a new velocity command that is sent to its servo-controller. This command is selected to minimize a measure of the risk that the target gets hidden by obstacles. \begin{figure}[thp!] \centerline{ \psfig{figure=observer.PS,height=1.2in} } \label{f:system} \caption{The observer (right) and the target} \end{figure} Our observer is a mobile robot (SuperScout platform of Nomadic Technology) equipped with a polar laser range sensor (see Fig. 1). The target is another mobile robot. The observer and the target operate in an in-door environment with solid and mostly static obstacles. The observer's range finder provides a 180-dg horizontal scan of the environment. Both the target's position and the local obstacle map are extracted from this scan. Despite the paucity of its sensing equipment, our observer has been remarkably successful at tracking a target performing brisk maneuvers and swift turns behind obstacles in non-engineered environments. The observer also tolerates transcient obstacles, though it does not explicitly distinguish them from stationary obstacles. We attribute much of its success to both our new risk-based tracking approach and the specific risk function embedded in this approach, which combines a reactive and a look-ahead term. These concepts are presented in this paper. \section{Previous Work} {\bf Automatic target tracking and interception.} Tracking has been widely studied in the field of automatic control, for such applications as missile control~\cite{missile}. The tracker and the target moves in 3-D space. Unlike in our work, obstacles are few (if any) and occlusion of the target is not a concern. Instead, the emphasis is on dealing with tight dynamic restrictions on the tracker's motion. \vspace*{0.05in} \noindent {\bf Visual tracking.} Here the goal is to track an object in a sequence of images~\cite{huttenlocher}, despite partial occlusions of the target by other objects, and aspect changes and self-occlusions of the target as it moves. There is no attempt to prevent occlusions from happening by moving the camera appropriately. Visual tracking and sensor placement are two complementary subproblems in target tracking among obstacles. Our work addresses the second subproblem. \vspace*{0.05in} \noindent {\bf Visual servo-control.} Here visual tracking is used to control the motion of a robot~\cite{hutchinson}. The robot has a certain goal, like docking against another machine. Visual feedback is used to track an object -- e.g., the machine against which the robot intends to dock -- in order to adjust the robot's trajectory. Again, there is no attempt to move the camera to avoid undesirable occlusions. \vspace*{0.05in} \noindent {\bf Guarding an art gallery.} The basic art-gallery problem is to compute the positions of fixed observers (guards), such that they collectively see a given space~\cite{orourke}. Finding the minimum number of guards in a polygonal space is NP-hard, but approximation algorithms exist~\cite{gonzalez}. Such an algorithm could be used to compute the placements of sensors such that the target would always be visible from one of them. Instead, our work assumes that there is only one sensor, but this sensor moves. \vspace*{0.05in} \noindent {\bf Off-line planning.} A similar problem as ours is considered in~\cite{becker}, assumming that both a global map of the obstacles and the target's trajectory are given in advance. An off-line planner backprojects visibility regions to compute a tracking trajectory. This trajectory is then executed in open loop by the observer. The assumptions are too constraining to lead a practical robotic system, but this method has been applied to computer graphic animation~\cite{li}. \vspace*{0.05in} \noindent {\bf Game-theoretic approach.} The tracking algorithm in~\cite{lavalle} is based on a game-theoretic approach and minimizes a cost function using dynamic programming. When the target's trajectory is known in advance, along with the global obstacle map, an optimal trajectory is computed off-line. When the target's trajectory is not known, a probabilistic model of the target's behavior is used to compute and update in real-time a strategy maximizing the probability that the observer keeps the target in view within some time horizon $h$. But the computational cost grows quickly with $h$, hence $h$ must be small. Systems built using this method~\cite{spie,lavalle,rafael} give reasonably good results when the target does not move too swiftly. The risk-based method presented below does not require a model of the target's behavior nor does it use costly dynamic programming. Experimental results show that it is faster and handles swift targets more reliably. \vspace*{0.05in} \noindent {\bf Observer self-localization.} When the observer uses a global map, it must localize itself in this map. The above game-theoretic approach is extended in~\cite{fabiani} to take into account the impact of self-localization uncertainties on tracking. A cost function allows the observer to decide at each stage of the tracking process whether it is preferable to perform the best tracking motion, or to deviate from this motion in order to see a landmark and achieve better self-localization. Our method does not make use of a prior global map. It eliminates the need for the observer to localize itself, hence saving precious computational time and avoiding sporadic major self-localization errors. \section{Tracking Procedure} \label{s:procedure} Our tracking approach is based on minimizing a function $\varphi$ of the observer's position that estimates the {\em risk} that the target may escape the observer's field of view by crossing an occlusion ray created by an obstacle. The commanded velocity is set to $-\nabla \varphi$. In our implementation it is updated at 10~Hz. \begin{figure}[thp!] \centerline{ \psfig{figure=scan2.PS,height=1.3in} } \caption{Range scan and extracted information} \label{f:scan} \end{figure} Each sensor reading provides a 180-dg horizontal scan of the environment made of 360 points at regular angular spacing. This scan is first segmented into maximal sequences of adjacent points. The target is then recognized (as a circular arc of known radius) and localized in the observer's coordinate system. Polygonal lines (polylines) are fit through the remaining sequences of points. Each endpoint of a polyline is an {\em occluding vertex} through which an {\em occluding ray} is erected. Fig.~\ref{f:scan} shows the outcome of the treatment of a scan. The localized target is shown in plain (blue) line. The occlusion rays are the straight line segments converging toward the observer (the black point at the base of the figure). The outer circular arcs bounding the shaded area corresponds to the sensor's maximum range. \begin{figure}[thp!] \centerline{ \psfig{figure=ept.PS,height=1.2in} } \caption{Escape-path tree} \label{f:ept} \end{figure} The tracking algorithm then computes the shortest escape path of the target to every occlusion ray. The obtained paths form a tree that we call the {\em escape-path tree} ({\sc ept} for short). An {\sc ept} is shown in Fig.~\ref{f:ept} (here, the paths have been computed as if the target was a point). The paths in the {\sc ept} are used to assign an individual escape risk $\varphi_e$ to each occlusion ray $e$. These individual risks are aggregated into a global risk function $\varphi$. As indicated below, this function is differentiable, hence the algorithm easily calculates $-\nabla \varphi$. The overall tracking algorithm is the following: \vspace*{0.05in} \noindent { {\bf Procedure} {\sc track}\\ \hspace*{0.1in} Repeat:\\ \hspace*{0.2in} {\bf 1.} Acquire a scan.\\ \hspace*{0.2in} {\bf 2.} Localize the target.\\ \hspace*{0.2in} {\bf 3.} If step 2 failed, then exit with failure.\\ \hspace*{0.2in} {\bf 4.} Extract occluding vertices and occlusion rays.\\ \hspace*{0.2in} {\bf 5.} Compute the {\sc ept}.\\ \hspace*{0.2in} {\bf 6.} Set the velocity command to $- \nabla \varphi$. } \vspace*{0.05in} An efficient algorithm given in~\cite{icra02,cyl-thesis} computes the {\sc ept} by exploiting geometric properties of shortest escape paths in star-shaped polygons. Below, we focus on the description of the risk function $\varphi$. \begin{figure}[thp!] \begin{center} \begin{tabular}{c} \psfig{figure=approx1.PS,height=2.0in}\\ (a)\\ \psfig{figure=approx2.PS,height=2.0in}\\ (b) \end{tabular} \end{center} \caption{Approximate strategies} \label{fig:approx} \end{figure} \section{Risk Function} In the absence of any model of the target's behavior, a tracking approach suggested in~\cite{lavalle} is to compute the velocity command that maximizes the escape time for the worst-case trajectory of the target. However, when a global map of the obstacles is given, this computation is extremely costly. When no map is given, it is simply impossible. This led us to investigate approximations of this strategy. \subsection{Escape Risk for Occlusion Ray} For a moment, let us focus our attention to a single occlusion ray $e$ extracted from the current scan, and let $\tau_e$ denote the shortest escape path of the target to $e$. If the target's motion is not restricted by any nonholonomic or dynamic constraint, the ratio of the length of $\tau_e$ by the target's maximal velocity is exactly the minimum time $\varepsilon$ that the target needs to escape the {\em current} field of view of the observer through $e$. If the target's motion is restricted, this ratio remains in general a good lower-bound approximation of this time. Hence, a possible approximate strategy for the observer is to move in the direction that yields the greatest differential increase of the length of $\tau_e$. To illustrate consider the example of Fig.~\ref{fig:approx}(a), where the path $\tau_e$ consists of a single line segment between the target's position and an escape point $p$ in an occlusion ray $e$ erected from the occluding vertex $v$. The observer's motion that yields the greatest differential increase of the length of $\tau_e$ is perpendicular to the line supporting $e$. We can push this reasoning further. During the time $\varepsilon$ it would take for the target to reach $p$ by moving at maximal velocity along $\tau_e$, the observer can reach any point within a circle of radius $\delta$ centered at its current position, where $\delta$ is the product of the observer's maximal velocity by $\varepsilon$ (assuming that no nonholonomic or dynamic constraint limits the observer's motion). Consider the tangent line to this circle drawn from $v$. A better direction of motion for the observer than the vector shown in Fig.~\ref{fig:approx}(a) is the vector shown in Fig.~\ref{fig:approx}(b), which points toward the tangency point. It combines a component perpendicular to the line supporting $e$ and a component in this line pointing toward $v$. The key observation that must be made here is that the time needed by the target to escape the observer's current field of view provides some ``slack'' that the observer can exploit to reach a position where it will have greater ability to react to future evading moves of the target. More specifically, by getting closer to the occluding vertex $v$, while it still has time to do so, the observer will later be able to perform swinging motions leading the occluding edge $e$ to rotate at a faster rate around $v$. This discussion yields the risk function $\varphi_e$ for one occlusion edge $e$. We define $\varphi_e$ to be of the form: \begin{equation} \varphi_e = c \, r^2 \, f(1/h), \label{eq:varphi} \end{equation} where $c$ is a scaling factor, $r$ is the distance between the observer's position and the occluding vertex $v$, $h$ is the length of the shortest path $\tau_e$ ending in $e$, and $f$ is an increasing function. The experimental results of Section~\ref{s:experiments} where obtained for $f(x) = \ln (x^2 + 1)$. Hence, $\varphi_e$ goes to $+\infty$ when $h$ goes to 0, and to 0 when the observer gets arbitrarily close to $v$. One can regard $\varphi_e$ as being made of two terms: \begin{enumerate} \item The {\em reactive} term, $f(1/h)$: Its minimization increases the length of the escape path to $e$. \item The {\em look-ahead} term, $r^2$: Its minimization improves the future ability of the observer to react to target's evading moves. \end{enumerate} The component of the commanded velocity associated with $\varphi_e$ is: \[ - \nabla \varphi_e \; = \; - 2cr f(1/h) \nabla r \, + \, c \frac{r^2}{h^2} f'(1/h) \nabla h. \] It can easily be computed in closed form by treating $r$ and $h$ as functions of the observer's position~\cite{icra02,cyl-thesis}. When $h$ is very small, there is an immediate threat; then, the dominant term is the reactive term and the commanded velocity is almost perpendicular to the supporting line of $e$ (direction of $\nabla h$). Instead, if $h$ is large, the dominant term is the look-ahead component and the commanded velocity has a substantial component pointing toward $v$ (direction of $-\nabla r$). \subsection{Global Risk Function} Let us now consider all occlusion rays and all escape paths in the {\sc ept}. We use Eq.~(\ref{eq:varphi}) to compute the risk associated with every occlusion edge. Note that the shortest escape path (over all possible occlusion edges) may not result in the greatest risk. An occlusion ray with a longer escape path to it may represent a greater risk if the observer is located further away from this ray's occluding vertex. So, the issue is: How can we combine the individual risks associated with the occlusion edges to define the global risk $\varphi$? One possibility is to identify the occlusion ray $e$ that yields the greatest risk $\varphi_e$ and set $\varphi = \varphi_e$. But, as the observer and the target move, the greatest-risk occlusion ray, and so the commanded velocity, may change abruptly and frequently. In an early implementation, this resulted into a shattering control causing erratic moves of the observer. To avoid this problem, we define the global risk function $\varphi$ by averaging the escape risks associated with several occlusion rays. In the ``standard'' average, the risks associated with all occlusion rays are counted equally. But many escape paths often share the same initial structure. Such paths are then over-represented in $\varphi$ at the expense of the solitary escape paths. Two averages that work better in practice are the following: \begin{itemize} \item {\em Recursive average}: The risks of the {\sc ept} nodes sharing the same parent are first averaged and the result is propagated upward to the parent. \item {\em Tournament average}: The global risk is the average of the risks associated with the $k$ greatest-risk occlusion rays (with $k = 3$ or 4). \end{itemize} Since the gradient is a linear operation, we apply directly these averaging techniques to the gradients of the individual risks. No risk $\varphi_e$, nor $\varphi$, is ever computed by the tracking algorithm. The experimental results presented below were obtained with the recursive average. \section{Experimental Results} \label{s:experiments} We have implemented the above tracking approach a SuperScout mobile robot of Nomadic Technologies equipped with an LMS200 laser range finder of Sick OpticElectronic. The tracking algorithm runs on an off-board 410MHz Pentium II processor connected to the robot by wireless ethernet. The target, a joysticked Nomad-200 robot, is modeled as a disc of non-zero radius and the escape paths are computed accordingly. The SuperScout robot is an almost cylindrical nonholonomic vehicle with zero-turning radius. A simple trajectory follower is used to track the flow of the successive velocity commands $- \nabla \varphi$~\cite{icra02,cyl-thesis}. In addition, we use a second regulator to deal with the fact that the range sensor has limited range (8 meters) and a 180-dg cone. This regulator is a simple proportional feedback of the target's deviation from an ideal position in the sensor's field of view. The maximal linear velocities of the observer and the target are approximately the same, with the observer slightly faster (0.5~m/sec). \begin{figure}[thp!] \centerline{ \psfig{figure=corner.ps,height=2.2in} } \caption{Tracking a target around a corner} \label{f:ex1} \end{figure} We have conducted various experiments with this system. In some experiments, we have compared the risk-based algorithm with a pure visual-tracking algorithm. This second algorithm only tries to keep the target close to an ideal position in the sensor's field of view. It ignores obstacles, hence does not take potential occlusion into account. Unlike the risk-based algorithm, the pure visual-tracking algorithm is easily tangled by a target moving toward an obstacle corner and swiftly turning around this corner. Fig.~\ref{f:ex1} shows a record of the target's and observer's trajectories in one experiment with the risk-based algorithm. The successive positions of the target and the observer are shown with triangles and circles, respectively. Segments connects the pairs of simultaneous positions. Note how well the observer anticipates the risk that the target may turn around the corner and swings around this corner. We have also successfully experimented with our observer in environments with transient obstacles, obtained by suddenly adding an obstacle in the field of view of the observer and either leaving it there, or removing it a short amount of time later. The observer has proven to be tolerant to such obstacles~\cite{cyl-thesis}. Fig.~\ref{f:ex2} shows snapshots of another experiment where the observer successfully tracks the target along a long tour throughout our laboratory environment. The tour starts in a corridor, then traverses a large area with many different objects (chairs, boxes), and finally ends in another corridor. The scans and {\sc ept}'s corresponding to these snapshots are displayed in Fig.~\ref{f:ex2-bis}. In a number of experiments, the observer also failed to track the target correctly. Most failures occurred because the observer either failed to detect the target though it was visible, or mis-located it. Such failures occurred mainly in the presence of cylindrical objects (e.g., waste baskets) similar to the target. But imagine yourself having to track a target in a dark environment by using a flashlight projecting only a plane of light! Given the paucity of its sensor, our observer has been remarkably successful. \section{Conclusion} This paper describes a new approach to target tracking based on risk minimization. It proposes a risk function that combines a reactive term, to deal with immediate threats, and a look-ahead term, to better use available time. Experiments have shown that the observer is able to keep the target in sight in both densely and sparsely cluttered environments, even when the target performs sudden and swift moves. An obvious improvement is to equip the observer with an additional sensor, for instance a stereo-vision system, to better detect and localize the target. An interesting extension will be to perform tracking and environment mapping simultaneously. A more difficult extension would be to coordinate a team of independent observers with limited communication to track multiple targets~\cite{rafael}. \vspace*{0.05in} \noindent {\small {\bf Acknowledgements:} We thank Vicky Chao for her help in running the experiments.} {\small \begin{thebibliography}{10} \bibitem{spie} H. Gonz\'alez-Ba\~nos, J.L~Gordillo, J.C.~Latombe, D.~Lin, A.~Sarmiento, C.~Tomasi, ``The Autonomous Observer: A Tool for Remote Experimentation in Robotics,'' {\em SPIE Proc. Telemanipulator and Telepresence Technologies VI}, M.~Stein (ed.), vol. 3840, pp.~210-221, 1999. \bibitem{becker} C.~Becker, H. Gonz\'alez-Ba\~nos, J.C.~Latombe, C.~Tomasi, ``An Intelligent Observer,'' {\em Proc. 4th Int. Symp. on Experimental Robotics (ISER)}, pp.~153-160, 1995. \bibitem{fabiani} P.~Fabiani, H. Gonz\'alez-Ba\~nos, J.C.~Latombe, D.~Lin, ``Tracking a Partially Predictable Target with Uncertainties and Visibility Constraints,'' {\em J.~of Robotics and Autonomous Systems}, 38(1):31-48, 2002. \bibitem{gonzalez} H.~Gonz\'alez-Ba\~nos, J.C.~Latombe, ``A Randomized Art-Gallery Algorithm for Sensor Placement,'' {\em Proc ACM Symp. on Computational Geometry}, pp. 232-240, 2001. \bibitem{icra02} H.~Gonz\'alez-Ba\~nos, C.Y.~Lee, J.C.~Latombe, ``Real-Time Combinatorial Tracking of a Target Moving Unpredictably Among Obstacles,'' {\em Proc. Int. Conf. Robotics \& Automation}, 2002. \bibitem{hutchinson} S.~Hutchinson, G.D. Hager, P.I. Corke, ``A Tutorial on Visual Servo Control,'' {\em IEEE Tr. Robotics \& Automation}, 12(5):651--670, 1996. \bibitem{huttenlocher} D.P.~Huttenlocher, J.J.~Noh, W.J.~Rucklidge, ``Tracking Non-Rigid Objects in Complex Scenes,'' {\em Proc. 4th Int. Conf. on Computer Vision}, pp.~93--101, 1993. \bibitem{lavalle} S.~LaValle, H. Gonz\'alez-Ba\~nos, C.~Becker, J.C.~Latombe, ``Motion Strategies for Maintaining Visibility with a Moving Target,'' {\em Proc. IEEE Int. Conf. Robotics \& Automation}, pp.~731--736, 1997. \bibitem{cyl-thesis} C.Y.~Lee, {\em Reat-Time Target Tracking in an Indoor Environment}, Ph.D.~Thesis, Dept. of Aeronautics and Astronautics, Stanford University, Stanford, CA, June 2002. \bibitem{li} T.Y.~Li, T.H.~Yu, ``Planning Tracking Motions for an Intelligent Virtual Camera,'' {\em Proc. IEEE Int. Conf. Robotics \& Automation}, pp.~1353--1358, 1999. \bibitem{rafael} R.~Murrieta-Cid, H. Gonz\'alez-Ba\~nos, B.~Tovar, ``A Reactive Motion Planner to Maintain Visibility of Unpredictable Targets,'' {\em Proc. IEEE Int. Conf. Robotics \& Automation}, 2002. \bibitem{orourke} J.~O'Rourke, ``Visibility,'' in {\em Handbook of Discrete and Computational Geometry}, J.E.~Goodman and J.~O'Rourke (eds.), CRC Press, Boca Raton, FL, pp.~467-479, 1997. \bibitem{missile} T.L.~Song, T.Y~Um, ``Practical Guidance for Homing Missiles with Bearings-Only Measurements,'' {\em IEEE Tr. Aerospace and Electronic Syst.}, 32:434--443, 1996. \end{thebibliography}} % \begin{figure}[thp!] \begin{center} \begin{tabular}{c} \psfig{figure=tour1.ps,height=1.4in}\\ \psfig{figure=tour3.ps,height=1.4in}\\ \psfig{figure=tour5.ps,height=1.4in}\\ \psfig{figure=tour6.ps,height=1.4in}\\ \psfig{figure=tour8.ps,height=1.4in}\\ \psfig{figure=tour10.ps,height=1.4in} \end{tabular} \end{center} \caption{Tracking a target performing a long tour} \label{f:ex2} \end{figure} % % \begin{figure}[thp!] \begin{center} \begin{tabular}{c} \psfig{figure=ept1.ps,height=1.4in}\\ \psfig{figure=ept3.ps,height=1.4in}\\ \psfig{figure=ept5.ps,height=1.4in}\\ \psfig{figure=ept6.ps,height=1.4in}\\ \psfig{figure=ept8.ps,height=1.4in}\\ \psfig{figure=ept10.ps,height=1.4in} \end{tabular} \end{center} \caption{Scans and {\sc ept}'s} \label{f:ex2-bis} \end{figure} % %\bibliography{mthesis} \end{document}