Dealing with Geometric Constraints in Game-Theoretic Planning

1 Introduction

Dealing with uncertainty is a major problem in Robot­ ics (e.g. see [Dorst et al., 1996] for an overview), which raises and combines a number of issues often associated with estimation, planning or execution control. It is usu­ ally unrealistic to assume that a complete and absolutely precise model of the environment can ever be obtained. Yet, a robot is expected to compute and execute ro­ bust plans. While Computational Geometry offers tools to deal with geometric constraints (collision, visibility) arising from the robot's workspace, Artificial Intelligence techniques make it possible to make decision in the pres­ ence uncertainty. But the techniques from those two fields have rarely been combined and in fact are seldom compatible.

Latombe, Jean-Claude

Stanford University Department of Computer Science Robotics Laboratory Stanford, CA 94305-9010, USA.

In this paper we show how methods developed in­ dependently can be combined to solve a target track­ ing problem presented in § 2. In this problem, a mo­ bile robot must keep a moving target into its field of view. The workspace is cluttered by obstacles that create motion and visibility constraints. Uncertainties in the robot's self-localization and in the target beha­ vior add to the complexity of the problem. Game the­ ory [Luce and Raiffa, 1957] offers a convenient frame­ work for both decision making under uncertainty [Sav­ age, 1972] and robot motion planning [La Valle, 1995]. The game-theoretic formulation presented in § 4 is spe­ cifically proposed for the online computation and execu­ tion of target-tracking motion strategies in the presence of uncertainties (§ 6). We have implemented the tech­ niques presented in this paper and tested them with real robots: characteristics of the produced motion strategies are discussed in § 7. Some shortcomings lead us to pro­ pose several extensions in § 8.

2 Tracking problem

A mobile robot equipped with visual sensors, called the observer, operates in a workspace cluttered with static obstacles. Its task is to keep another moving object, the target, in its field of view while avoiding collision with the obstacles. All the obstacles create visual occlusion. Hence, the observer must move in such a way that the target is never hidden behind an obstacle.

We assume that the workspace is realistically de­ scribed by a 2-D map (planar layout) of the obstacles. Both the observer and the target are modeled as discs. An accurate map of the obstacles is given to the observer before target tracking begins.

The observer uses a camera mounted on a turret to detect the target. The turret rotates fast enough to make it possible to consider that the observer has omni­ directional vision, independent of its current heading. The observer detects and recognizes the target whenever it has a cone of sight to the entire target's disk not obstructed by any obstacle. In our implementation, a black-and-white geometric pattern is mounted on the target to simplify visual detection. There exist more sophisticated vision techniques to detect natural targets (e.g. see [Bregler and Malik, 1998]) .

The target is mostly unpredictable, that is, its fu­ ture trajectory is not known in advance, but it moves collision-free and its velocity has a known upper bound. The observer localizes itself relative to the workspace us­ ing both odometric and landmark techniques. Our land­ marks are simple geometric patterns placed at the ceiling of the robot environment; they are detected and recog­ nized using a camera mounted vertically on the observer platform [Becker et al., 1995]. Natural landmarks could be used as well, but would require more sophisticated vis­ ion techniques. Whenever the observer sees a landmark, it localizes itself in the workspace with a given precision (on the order of 1-2in) that has been experimentally es­ tablished. When the robot does not see any landmark, the imprecision of its location estimate (dead-reckoning) grows linearly with the distance traveled. To keep our work realistic and compatible with the use of natural landmarks, the artificial landmarks are sparsely distrib­ uted over the workspace. The two vision systems used for target tracking and landmark detection, respectively, are independent and operate concurrently.

The observer uses the vision system that tracks the target to estimate the target's position relative to it­ self and (through a simple transform) to the workspace. Note that the observer cannot track the target by only using their position relative to one another. It must also estimate both its own position relative to the workspace to avoid colliding with the obstacles and the target's pos­ ition in the workspace in order to move in such a way that it prevents the target from hiding behind an obstacle.

3 Related work

This tracking problem is a challenging combination of different planning problems that have previously been studied in Robotics, but separately. Visibility and col­ lision constraints must be satisfied in the presence of uncertainties in the positions of the observer and the target. The problem of maintaining visibility with the target while avoiding collision with the obstacles is ad­ dressed in [La Valle et al., 1997], but position uncertain­ ties are not taken into account. When the target is fully predictable, that is, when its future trajectory is com­ pletely known in advance, a dynamic programming ap­ proach [Bertsekas, 1986] can be used to compute a tra­ jectory of the observer that has minimal length.

As shown in [La Valle et al., 1997], this approach be­ comes untractable in practice if the target is only par­ tially predictable. Then, the approach can be applied to choose a motion command that aims to maximize the likelihood that the target will remain visible during a short interval of time in the future. The observer it­ erates this computation while tracking the target and updates its motion heading at each iteration. This is essentially the approach taken in this paper. The main difference is that we take into account the uncertainties in the observer's and target's positions. This difference will motivate the embedding of this approach into the game-theoretic framework presented in § 4.

Taking position uncertainties into account naturally leads to planning the observer's motions in order to take advantage of the landmarks in the workspace to reduce the imprecision of the estimate of the observer's pos­ ition (and consequently that of the target), whenever this does not immediately conflict with keeping the tar­ get in the observer's field of view. Landmark-based nav­ igation has been addressed from different points of view in the literature (e.g. see [Bouilly and Simeon, 1996; Saffiotti and Wesley, 1996; Takeda et al., 1994; Lazanas and Latombe, 1995]). The principle is simple: if the ro­ bot primarily localizes itself relative to landmarks, the planner must guarantee that the robot will see land­ marks often enough along its path [Fualdes and Barrouil, 1993]. To our knowledge, no previous work on landmark­ based navigation simultaneously deals with achieving a visual task such as maintaining visibility with a moving target.

4 Game-theoretic approach

Different discrete processes intervene in the behavior of the observer. During each iteration, the observer chooses its next move according to the last estimate of the tar­ get's position, executes it, localizes itself (by using a landmark, if one is visible), and computes a new estimate of the target's position. Meanwhile, the target is moving to another location.

Let t 1 be the initial time point, 6.t the duration of the time interval between two sampling time (the dur­ ation of the global decision loop), and k ~ 1 an index corresponding to the time point tk = t 1 + (k - 1) * 6.t. The observer and the target move in the same work­ space, but since their respective shapes and sizes may differ, their free configurations belong to two different configuration spaces, defined with standard parameter­ izations [Latombe, 1991], respectively Cf~~e and C}~tee · Let q'// 8 and q~gt denote the respective configurations of the observer and the target at time k. Then, the global state at time k is given by the couple Wk = (qk_bs, q!9t) E Cf~~e X c}~tee. Let n = Cf~~e X c}~~e denote the set of all possible states. Let vtut ( qtut) designate the visibility region of the target at configuration qtut , i.e., the set of all configurations qobs from which the observer has an unobstructed cone of sight to the entire body of the tar­ get. Let R'kt:;1 (qk_bs) (resp. R~~1 (q~ 9 t)) stand for the set of all the configurations in CJ~~e (resp. C}~~e) that are reachable without collision from qk_bs (resp. q~ 9 t) between times k and k + 1.

Game theory provides a convenient framework to express decision problems in the presence of uncer­ tainty. Yet, most work in that field has focused on the probabilistic part of the theory, especially on the Bayesian computation of the probability distribu­ tions (POMDP [White, 1993], Bayesian Networks [Pearl, 1988]). Game theory rests on the axiomati:.:~ation of a relation of preference defined over the possible choices. This relation rests on the definition of both a probability measure and a utility function over the possible con­ sequences. We think that the utility part of the theory is as important as the probabilistic part.

Let us define a decision problem over the triplet {D, D, K}, where n is the set of atomic states, D is the set of possible decisions, and K is the set of possible consequences k;j = dj(w;). Each possible decision di is a function { di : n --+ K}.

A decision problem also requires the definition of a relation of preference ~ over D x D. For each (d, d') E D x D, d ~ d' must be read "dis preferable than d'". Von Neumann's theorem tells us that this relation of preference can be defined as follows [Von Neumann and Morgenstern, 1944]:

• Let P be a probability distribution over the set n of states,
• Let U be a utility function over the set K of con­ sequences,
• Posing:

(EpU(d) is the expected utility of choosing dE D) we have:

With this formulation, the best decision d* is:

In our target tracking problem, suppose that at time k + 1 the target is at configuration q~~~ and the observer has reached q%': 1 after the motion decision d'kbs was made · t t ( obs tgt) Th t t ( obs tgt ) · m s a e qk , qk . e new s a e qk+l, qk+l IS a consequence of dk. The need that q'k': 1 be inside the vis­ ibility region vtgt(q~~l) can be expressed in the utility function of the problem.

More precisely, our tracking problem is a sequence of motion decision problems (one at each time k) that are all defined on the same state space f! = Cj~~e X C}~~e . The set K of possible consequences is also fixed. As the observer may collide with obstacles by mistake (due to localization errors), there is an additional consequence call (standing for "collision"). The resulting set of consequences is K ={ Cf~~e U {call}} xC}~~e· At each time k the motion decision problem can be defined as follows:

• The observer's and target's positions are not pre­ cisely known at time k. Let prs (resp. P! 9 t) denote the probability distribution of the observer's (resp. , ) . . cabs ( ctgt ) Th · · target s pos1tl0n over fr ee resp. free . e JOmt distribution P = (Pkbs, P! 9 t) is the probability dis­ tribution over n.
• Let qf.bs be the current estimate of the position of the observer according to pkbs . This estimate can be a singleton or a set of configurations.
• Let D'kbs be the set of possible motion decisions for the ob~erver from the current estimated configura­ tion qkbs. In practice, D'kbs is finite.
• The utility function Uk over K (here, the set of pos­ sible consequences at time k + 1) can be simply defined to be 1 if the observer sees the target at time k + 1 and 0 if it does not see the target or has collided with an obstacle. More formally:

Geometric constraints illustrated in Fig. 1 directly affect the evaluation of the above utility function: Is the ob­ server colliding with obstacles, or not? Is the observer in the visibility region of the target? As we will see in the next section, they also affect the computation of the probability distributions Pk and Pk+1 , hence that of the best motion decision defined by Equ. (1): can the ob­ server see a landmark?

Note that more complex utility functions could be con­ sidered. For example, Uk(q'k': 1 , qk~1 ) could evaluate to the minimal distance that the target would have to travel before q%': 1 is no longer in the target's visibility region.

5 Modeling uncertainties

In our work will only consider uncertainties caused by inaccuracies in sensing in motion control and in lack of prior knowledge of the target's trajectory. Uncertainty in the observer's location depends critically on whether it sees a landmark, or not.

Whenever the observer moves and no landmark is vis­ ible, dead-reckoning errors occur and accumulate. Con­ sequently, the uncertainty on the position of the observer increases (see Fig. 2) and the observer may risk colli­ sion along a supposedly collision-free path. The actu­ ally reached configuration at time k + 1, q'k':P can be modeled as a random variable that is conditionally de­ pendent upon both the observer's configuration at time k, q'kbs, and the decision made at time k, d'kbs.

The conditional probability Pr(q'k': 1 I d'kbs, qkbs) is given a priori and allows the computation (at time k) of the predictive probability Pk! 8 1 law at time k + 1 which models the imprecision of dead-reckoning.

If localization fails at time k + 1, the predictive probabil­ ity law Fk'is 1 cannot be updated: then the best estimate of the observer's location at time k+ 1 is the one given by dead-reckoning and Pk'i 8 1 =· Fk'i 8 1 (the predictive prob­ ability law). The location uncertainty reduces whenever

the observer sees a landmark. Let loc%b_t. 1 be a boolean variable equal to 1 whenever the observer reaches a po­ sition at time k + 1 where it can see a landmark, and 0 otherwise (or if the observation of the landmark fails) . When a landmark is visible, the positioning accuracy that is obtained after the updating process at time k + 1 depends both on the predictive probability law Fk'i 8 1 , and on the experimentally established precision of the localization. This is modeled by the updating function _cobs in the following equation:

Eventually, the probability distribution Pk'i 8 1 at time k depends both on Fk'i 8 1 and on the observation of a land­ mark, if a landmark has been observed. Pk'i 8 1 is thus given by equations (5) to (8) as a function of the prob­ ability measure Pk'bs, the decision d%bs made at time k, and the value of the variable locJ.b_t. 1 . This is summarized in the following evolution equation, where the evolution function Fobs models both the possible motion errors and the reduction of positioning imprecision whenever a landmark is detected:

Similarly, due to the limited capabilities of its tracking camera, the observer can only obtain an imprecise es­ timate of the position of the target relative to own cur­ rent position (see Fig. 3). Let p~ut denote the probab­ ility over the possible configurations of the target after detection by the observer at time k. The uncertainty on the position of the target in the workspace combines the uncertainty on the target's position relative to the observer and the uncertainty on the observer's position - t t t t in the workspace. Hence, P/ depends on both P/ and Pk'bs and it is necessarily more imprecise than Pk'bs (see Fig. 3) . The target's partial predictability between times k and k + 1 can be written: P!~1 = stgt(P! 9 t) . Since the target's trajectory is not known in advance, stgt depends only the known target's maximal velocity. This means that the uncertainty on the target's posi­ tion at time k + 1 (as predicted at time k) may be much greater than for the observer. On the other hand, the target is observed at every time step before the predic­ tion stage, which considerably reduces uncertainty. The evolution equation for the target takes into account how the probability measure P! 9 t is updated at time k by the observer and how it evolves from time k to time k + 1 due to partial predictability of the target:

6 Tracking strategies

At each time step, the observer. decides where to move according to P~gt and Pk'bs. Pk = (Pk'bs, P~ 9 t) is the in­ formation state of the observer at time k and the global evolution equation Pk+1 = F(Pb dJ.bs, locJ.b_t. 1 ) follows from Equ. (9) and (11). A strategy 1r for the tracking problem with finite time horizon {1, h} [White, 1993) is a sequence { 7rk } kE{l, h} of decision rules 'Trk (Pk) = dJ.bs. An optimal strategy is obtained by discrete dynamic pro­ gramming [White, 1993). Let denote Vh(P1 ) the optimal value function for the h-stage tracking problem begin­ ning with an initial probability distribution P1 on n at time t 1 , i.e. the maximum expected reward that the observer can get with this initial condition. Bellman's optimality equations give: Vk E {1, h}

Unfortunately, as already mentioned in § 3, the cost of computing an optimal strategy is exponential in the ho­ rizon h (number of time steps). This difficulty is well­ known in the field of POMDP's [White, 1993) where dynamic programming is nevertheless widely used. In target tracking, a greater h would make it possible to compute better strategies if the computation time was negligible. But since this is not the case and the target moves while the observer computes its next motion, a compromise is necessary. In our experiments in simula­ tion, we computed strategies with h = 2. In our exper­ iments with real robots, we used h = 1. The empiric­ ally established real-time constraint with real robots is that the observer must choose a motion command within 50ms. On a SUN SPARCstation 20 the computation time for h = 1 ranges between 1 and 10ms, but grows up to 120-1200ms for h = 2.

For h = 1, the decision rule at time k when Pk is the probability distribution over n corresponds to V 1 (Pk) in Equ. (12) . Hence, the best decision is:

7 Experimentation

Experiments were conducted to test our game-theoretic approach : the observer and the target are two Nomad200 mobile robots of Nomadic Technologies. Each Nomad-200 has a rigid cylindrical body that allows its representation as a disc in the 2-D workspace. The ob­ server detects landmarks placed at the ceiling of the laboratory. It can track the target regardless of its cur­ rent heading, thanks to a camera that is kept aimed at the target by servoing the observer's turret appropri­ ately. All the obstacles in the environment are polygonal and obstruct both visibility and motion. More sophist­ icated geometric models for the obstacles are possible. Simple probabilistic models of the robots were intro­ duced in the evolution equations (Equ. (9)-(11)): they can be improved experimentally. Their good behavior in practice is due to the fact that they are rather conser­ vative. Yet, it can be argued that little improvement is to be expected from more precise probabilistic models, while so little is known about the target's future motion.

Thanks to the fact that all obstacles are static and known in advance, a number of precomputations are performed in advance in order to reduce on-line com­ putation times (e.g. visibility and reachability regions). The utility function Uk at each time step k is obtained through a small amount of calculation (Equ. (2)-(4)) and with nice properties of piecewise smoothness. uk could have been chosen to give a higher penalty to collision. For a given state (qf/", qigt) at time k, a more soph­ isticated uk could have been proposed, for example to penalize states where the observer is close to loosing the target and/or where the observer's turret needs to ro­ tate by a large amount. Uk could also be designed to take into account the speed of the observer at stage k in order to advantage future configurations that correspond to small accelerations of the observer in either direction. Yet, simplicity was preferred.

Fig. 4 represents an 8-step decision sequence generated by the observer's planner in response to target's moves. This sample sequence shows snapshots of the graphic dis­ play of the conducted experiments: the target appears as a dark disk and the observer as a light-grey disc with a little tail. In each snapshot, the small grey circle with variable radius and the same center as the observer de­ picts the observer's positioning uncertainty. The other static light-grey disks represent landmark regions (re­ gions in which the observer's center must lie in order to see a landmark). The landmark that was detected the most recently, recognized and used for location updating by the observer is marked with a small blue circle in the center. The arrows describe each agent's moves. Please note that the target's moves are represented by arrows reflecting the target's next move in the scenario, whereas for the observer the arrows reflect the subsequent move that is performed by the observer in response to the ob­ served move of the target at the previous stage.

The conducted implementation successfully validates our approach. The observer is able to manage prop­ erly its positioning uncertainty. Whenever a landmark visibility region is reachable, the motion decision corres­ ponds to the best compromise between the tracking task and the need for location updating. There is no con­ flict because the observer localizes itself so as to better track the target afterward. More precisely, the following features are remarkable in its behavior in Fig. 4:

• the observer takes its positioning uncertainty into account before choosing its next move: it localizes itself when useful, but does not need to do so when it can travel in a wide enough free area (stage 1-6);

• the observer goes and localizes itself so as to reduce its positioning imprecision (if possible) prior to en­ tering a narrow region (stages 7-8);
• the observer always minimizes its probability of col­ lision because this improves the probability of vis­ ibility of the target at next time step;
• in practice the observer's probability of collision re­ mains equal to zero whenever possible (stages 1-8 here), unless the observer gets trapped by the tar­ get in a narrow region where it cannot reach any landmark region in order to localize itself.

8 Perspectives

The nice experimental behavior of the designed plan­ ner shows that the choice of modeling this problem in a game-theoretic framework was more than a theoretical exercise: it should allow geometric algorithms to be effi­ ciently combined with algorithms designed for POMDP's or Bayesian Networks. Still, this would need to be fur­ ther formalized with more generality, so to be applicable to a wider range of problems.

Furthermore, the proposed game-theoretic framework allows dealing with symbolic constraints (e.g. "a ro­ bot cannot see/travel through walls") and reasoning with probabilistic uncertainty to be efficiently combined through the computation of well-adapted utility func­ tions. The introduction of reasoning mechanisms, such as in Fuzzy Sets theory [Saffiotti and Wesley, 1996] or Possibility theory [Dubois and Prade, 1995] seem to be a promising approach for reasoning with utility functions.

In that perspective, the conducted experiments provide interesting ideas for future developments. For instance, when the observer happens to estimate the pos­ ition of the target on a configuration that is not collision­ free, this may be due to location and detection errors, but also to the fact that some obstacles in the modeled workspace of the robots do not exist in the real envir­ onment: our 2-D layout of the workspace actually is a very conservative model of the real indoor environment. Dealing with an incomplete ·and inaccurate map in full generality is a really tough challenge. The proposed gen­ eralized game-theoretic approach should provide an in­ teresting framework in order to release progressively the hypothesis of a perfectly known environment.

Acknowledgments

We thank Hector H. Gonzales-Banos and David Lin for their technical inputs throughout this research and for their assistance in setting up the robotic experiments. This research was funded in part by ARO MURI grant DAAH04-96-1-007. Patrick Fabiani was supported by DGA and ONERA.