Resilient Navigation through Online Probabilistic Modality Reconfiguration

Transcription

1 Resilient Navigation through Online Probabilistic Modality Reconfiguration Thierry Peynot, Robert Fitch, Rowan McAllister and Alen Alempijevic Abstract This paper proposes an approach to achieve resilient navigation in the context of indoor mobile robots. Resilient navigation seeks to mitigate the impact of control, localisation, or map errors on the safety of the platform while enforcing the robot s ability to achieve its goal. We show that resilience to unpredictable errors can be achieved by combining the benefits of independent and complementary algorithmic approaches to navigation, or modalities, each tuned to a particular type of environment or situation. In this paper, the modalities comprise a path planning method and a reactive motion strategy. While the robot navigates, a Hidden Markov Model estimates the most appropriate modality online, or recommended modality, based on two types of information: context (using mainly information known a priori) and monitoring (evaluating unpredictable aspects of the current situation). The robot then uses the recommended modality, switching between one and another dynamically. The experimental validation shows that our approach can enable failure mitigation while maintaining the safety of the platform and making the most of the available modalities. In particular, the robot is shown to reach its goal successfully in the presence of: unpredicted control error, unexpected map errors and large injected localisation fault. I. INTRODUCTION Motion planning and control of a mobile robot necessarily involves multiple sources of uncertain information such as control uncertainty, localisation uncertainty, and mapping errors. Current research in motion planning seeks to address these sources of uncertainty by modelling them in the context of the planning problem [1], [2]. However, problems arising during the execution of a plan are not always predictable (and hence able to be modelled). For example, it is difficult to predict localisation errors ahead of time, or to anticipate which map locations actually contain large errors. We are interested in mitigating the impact of such unpredictable errors on robot performance and safety. We introduce the term resilience to refer to this goal. Resilient navigation seeks to mitigate the impact of control, localisation, and map errors on the safety of the platform while enforcing the robot s ability to achieve its goal. In this paper, we study resilient navigation in the context of indoor mobile robots. We believe resilience is best achieved This work was supported in part by the Australian Centre for Field Robotics (ACFR) and the NSW State Government. This material is based on research sponsored by the Air Force Research Laboratory, under agreement number FA The U.S. Government is authorized to reproduce and distribute reprints for Governmental purposes notwithstanding any copyright notation thereon. T. Peynot, R. Fitch and R. McAllister are with the Australian Centre for Field Robotics, The University of Sydney, NSW 2006, Australia {tpeynot,rfitch,r.mcallister}@acfr.usyd.edu.au A. Alempijevic is with the University of Technology, Sydney, Australia a.alempijevic@cas.edu.au by combining the benefits of multiple independent algorithmic approaches, or modalities, each tuned to a particular type of environment or situation. The idea is to develop a set of modalities that covers the range of possible situations, and then to reconfigure the system dynamically in response to unpredicted errors. The key challenges are: 1) how to choose a suitable set of modalities, 2) how to represent information that describes the robot s context, and 3) how to decide which modality is most appropriate at any given time. Because we are dealing with uncertain information, the representation and decision making challenges require solutions in probabilistic form. Our approach in choosing a set of modalities appropriate to indoor robot navigation is to include a motion planning strategy that requires global information, and a reactive strategy that requires only local information. These two modalities are complementary. If the navigation goal is within the field of view (FOV) of the robot, a reactive obstacle avoidance approach (e.g. [3], [4]) can be feasible. However, reactive approaches have known limitations. They can become trapped in dead-ends or U-shape obstacles, and it is difficult to obtain smooth trajectories. If the goal is located outside of the robot s FOV, the recommended strategy is to use a motion planning algorithm that reasons more globally about the world, especially if some prior knowledge of the environment is available. In addition, smoother and more efficient paths can be obtained. However, in cluttered environments (e.g. driving through a narrow doorway, a common situation in indoor environments), such a strategy can only be effective if sufficiently accurate map and global localisation are available. In addition, the control of the platform needs to be robust and precise enough to follow the planned trajectory nearly perfectly. An alternative is to combine the two strategies to obtain a hybrid system [5]. Typically, a motion planner algorithm computes a global plan, generating a list of waypoints along the computed trajectory which are passed to a reactive motion method. The latter then controls the robot to achieve these waypoints sequentially. A drawback of these hybrid techniques is that even if the motion planner can produce smooth trajectories (or trajectories respecting some predefined constraints), the execution of such types of trajectories cannot be enforced (Fig. 1). Another inconvenience of the combination of strategies is that events that provoke failures of one of the methods involved in the combination will often provoke the failure of the combination, while this can be mitigated by using only the appropriate method at the right time. A comparison of the different strategies discussed

2 in this paper is shown in Table I. TABLE I COMPARISON OF NAVIGATION STRATEGIES Strategy: Planning Reactive Hybrid Our approach Robust to deadends Robust to dynamic obstacles Robust to errors in localisa- tion/map Optimised paths (when possible) Instead, we propose a modality-switching algorithm based on a Hidden Markov Model (HMM) that considers context and monitoring information. If the system is aware that the path execution cannot safely handle a situation where the platform gets too close to obstacles (e.g. the robot has to drive through a narrow doorway), it is appropriate to switch to a reactive strategy. This situation can be evaluated using the localisation of the robot in a map, and detecting the presence of this narrow passage on the way to the goal. However, reasoning only on this context information based on the global situation on the map will not be sufficient to handle situations where the error/uncertainty of the global localisation is too important, where elements of the map have moved since the last plan search (e.g. moved furniture), or a new dynamic obstacle appeared. Fast local replanning can partially address this problem but it is computationally expensive and can lead to instabilities in the control. Therefore, we propose to make the decision on the modality to use based on context information and monitoring information (such as proximity to obstacles, directly based on laser measurements). We evaluate our approach through hardware experiments using an indoor mobile robot operating in an office environment. We show that failures can be mitigated in challenging situations, while maintaining the safety of the platform and keeping it in operation. The challenging situations considered in our experimental evaluation include: large localisation errors, unpredictable map errors (presence of unexpected obstacle), and unpredicted control errors. The paper is organised as follows. Sect. II discusses further related work. Sect. III presents the proposed approach, based on prior work by the authors. Sect. IV describes our robot platform and details the set of modalities. Sects. V and VI discuss the information available for the decision to select the most appropriate modality. Sect. VII presents experimental results and Sect. VIII provides conclusions and discusses future perspectives. II. RELATED WORK Previous work has considered multi-modal systems for the navigation of an indoor mobile robot. [6] proposed the Robel system: a robot controller that learns from experience (a) Planning (b) Hybrid Fig. 1. Trajectory obtained using our motion planner (a) and a hybrid approach. Obstacles in the map are in black. Circles represent the radius of the robot. Approximate size of the area shown: 6.5m 7.5m. different ways of combining sensory-motor functions to achieve a navigation task. Robel uses a Markov decision process (MDP) to provide a policy for the task. However, MDPs are generally computationally expensive and policies often have to be computed off-line or at low frequencies. Our system was designed to be simple enough to compute so that the robot can be reactive: modality switching can happen quickly when needed. Besides, an MDP requires the states to be fully observable, which is rarely the case in practice. [7] proposed a system based on a partially observable MDP (POMDP) that can be used to detect, diagnose and recover from faults. However, the policy has to be computed offline and the robot does not have a real alternative navigation modality when the path planning strategy fails. Our approach does not require us to explicitly detect and identify specific faults such as a localisation error, as it focuses on mitigating failures that could occur in consequence, finding alternatives to obtain a robustness while maintaining safety. Motion planning and obstacle avoidance are well-studied problems in the literature. See [8] for a comprehensive review up to More recently, researchers have sought to address motion planning under uncertainty in control [1], localisation [2] or sensing and environment [9]. However, these studies typically require the ability to predict possible errors, as they need to model the incertainty in the context of the planning problem. In this paper, we are interested in mitigating the impact of unpredictable errors. III. APPROACH The approach we propose to use is a probabilistic framework that was introduced in our previous work for an outdoor mobile robot endowed with two navigation modalities of perception and motion, one dedicated to flat terrain and one appropriate for rough terrain [10]. In this paper we propose to adapt a similar approach to an indoor robot endowed with two main modalities of navigation: one based on a global planner (PLAN) and one based on a reactive motion approach (REACT), in addition to a STOP modality for emergency and safety. This approach estimates the likelihood of each modality being most suitable (i.e. modality recommendation), based on two types of information:

3 PLAN x0 p0,2 p0,1 p1,0 p2,0 p2,1 STOP x2 REACT x1 p1,2 Fig. 2. Graphical representation of the 3 state HMM global environment knowledge, or context data, online execution knowledge, or monitoring information. The estimation is made using a Hidden Markov Model (HMM), since the states are not directly observable. The probabilistic approach allows the system to account for uncertainties in the different sources of information, and the HMM provides a time integration that prevents jitter (too frequent modality changes). A. Context Information The context information concerns the general context the robot is in. This information is usually known a priori, i.e. before the robot arrives on location (e.g. taken from a map that was built beforehand). Estimating the most appropriate modality could be done based on this type of information alone, provided that it is sufficiently accurate (see Section V). This information can give the opportunity to make predictions and therefore plans that take into account what modality is likely to be used at that location in the future. B. Monitoring Information The purpose of this monitoring is to check the immediate situation of the robot, through data only observable by the robot once on location. Therefore, it cannot be evaluated in anticipation (only once the robot is in the situation), contrary to the context information. In this implementation, the aspect that is checked with this information is the actual appropriateness of the current situation, with regards to the possible modalities, based on the immediate surroundings of the robot (proximity to obstacles). Note that typically the monitoring information can be available at a higher frequency than the context information. In this case, we may want to update the HMM estimator as often as this type of information is updated. C. The HMM These two categories of information are the input of a HMM whose number of states is equal to the number of available modalities (see Fig.2). Each state x k corresponds to the following proposition: modality m k is the appropriate modality to apply at this point in time, i.e. the goal of this estimation chain is to provide a modality recommendation. The framework is designed as a Markov conditional estimation system [11]. It estimates the conditional state x k,t at time t, knowing context observation until time t, Fig. 3. RobotAssist platform O 1:t and online monitoring information M 1:t. If the robot is endowed with N different modalities, the probability that m k is the appropriate modality to apply at time t can be written, k [1, N ], P (x k,t O 1:t, M 1:t ) N P (O t x k,t ) P (x k,t x i,t 1, M t )P (x i,t 1 O 1:t 1, M 1:t 1 ) i=1 where: P (O t x k,t ) is an observation probability (computed using the context information), P (x k,t x i,t 1, M t ) is the conditional probability of transition from state x i to state x k, knowing the monitoring data M t at time t. The following sections describe more specifically the different modalities of the robot used in this paper and the nature of the context and monitoring information. IV. SYSTEM DESCRIPTION A. Platform and Test Environment 1) The Robot: The platform is built from readily available hardware and open source software. It consists of the Segway RMP100 base with an Exact Dynamics iarm manipulator and onboard PCs (see Fig. 3). The robot is equipped with various sensors, including a Hokuyo UTM-30LX laser range finder (used for obstacle avoidance and global localisation), with a 30m and 270 scanning range (limited to 220 in practice because of the robot configuration) and encoders in the mobile base (for odometry). The robot is described in more details in [12]. 2) The Localisation Algorithm: To compute our mobile robot localisation we employ the well studied probabilistic localisation algorithm known as Monte Carlo Localisation (MCL) [13]. The robot s belief is represented by a set of weighted hypotheses (probability distribution using a particle filter), which approximate the posterior under a common Bayesian formulation of the localisation problem. We update this distribution using data from odometry and the laser range-finder and a pre-defined map of the environment against which to compare observed sensor values. The uncertainty of the robot localisation is represented by the standard

4 deviations in x and y, noted σ x and σ y, respectively. In practice, to account for different orientations of the robot, we use σ loc = max(σ x, σ y ) to represent the localisation uncertainty. 3) The Environment: The platform has been developed and tested at the University of Technology Sydney (UTS). Our test arena is well suited for evaluating the real world applicability being populated with over 25 people and consisting mainly of student workstations and fixed and movable furniture. B. Available Modalities 1) Nonholonomic Planning (m 0, or PLAN): Modality m 0 is PLAN. In this modality, the robot plans a path from its current location to a goal location using a motion planning algorithm. The robot then executes the plan if one is found. PLAN requires a global map, localisation of the robot within this map, and a goal pose defined with respect to the map. The algorithm we use to implement motion planning is the well-known Latombe Grid-Search algorithm [14], [8]. Although the name may seem to imply a discrete search space, the algorithm does use continuous coordinates. For convenience, we summarise the algorithm here. The robot is modelled as a disc, and controlled by linear and angular velocity commands. Obstacles in the world are stored in a 2D traversability map. The planner accepts a start position and goal position in global coordinates and returns a trajectory represented as a sequence of velocity commands. Each velocity command has an associated time interval. The returned trajectory can thus be executed directly. The planner builds a search tree in configuration space. Its basic operation is to expand a search node by applying a given velocity command for a fixed time interval. The resulting configuration is computed via forward integration. A fixed set of commands, sometimes referred to as a path set, is available for expansion. A proximity grid is maintained to prevent revisiting areas that have already been searched. This grid discretizes the configuration space (R2 S1) into cells of uniform size. The planner is complete with respect to the resolution of its proximity grid and the time interval of the path set [14]. In other words, the planner is guaranteed to find a path as long as these parameters are chosen sufficiently small. Because this proof is not constructive, however, we do not have a method for determining parameter values analytically. We hand-tuned them empirically and found a reasonable grid resolution of 0.2m 0.2m π 8 rad and path set time intervals of 2 or 4 seconds. Our path set has angular velocities chosen from { π 4, π 8, π 16, π 32, 0, π 32, π 16, π 8, π 4 } and linear velocities from {0.2m/s, 0.1m/s}. Our priority queue uses a cost function that combines minimum distance to goal with minimum change in curvature. The path is executed in open-loop fashion with fixedfrequency replanning. Because the robot is moving quite slowly (under 0.35m/s) we replan every 2 seconds (0.5Hz). 2) Reactive Motion (m 1, or REACT): Modality m 1 is a reactive collision avoidance method. This modality avoids obstacles based on the information provided by the laser rangefinder of the robot. REACT requires a goal provided in local (robot-centred) coordinates. We implement the REACT modality using a potential field method derived from a model of human navigation [4]. This method directly controls angular acceleration and produces smooth paths. We chose this method because the robot operates in an indoor, office-like environment amenable to human-like paths. The algorithm makes decisions based on obstacles as currently sensed by the laser rangefinder. Therefore, this type of methods is subject to failure in cases such as large U-shape obstacles or dead-ends that are outside the immediate field of view of the robot. In this work, because the laser cannot scan all 360 around the robot, the perception data that REACT uses is a local fusion of laser scans.the localisation used in this modality needs to be local and smooth, so as not to be affected by global localisation drift, possible errors in the global maps, and jumps that could make the behaviour of this obstacle detection method unstable. Since the operating velocities of the robot remain limited, in this modality we use odometry. 3) STOP (m 2 ): The STOP modality simply corresponds to stopping the robot. This is the safety modality of the system. It will apply if the robot has come too close to an obstacle and the only reasonable option is to stop the current motion. Note that since the robot is not moving in STOP, if this stop was provoked by the monitoring information (i.e. readings of the laser scan), the only possibility that the robot may resume its motion is if the obstacle that was responsible for the stop is dynamic and has moved away from the robot. To account for that possibility, even though this modality has been selected and the robot has stopped, our system keeps estimating the appropriateness of each modality. In this way, if the object moves away, clearing out the immediate surroundings of the robot, the robot will have the opportunity to resume its motion. C. Modality Switching PLAN is the default (i.e. starting) modality, as it is given the highest prior probability. When a switch to REACT is decided and executed, since a plan is available, if the final goal is far from the robot, the goal that this modality uses is the first waypoint of the planned path that is seen to be clear from obstacles, according to REACT. Indeed, because of localisation or map errors, some waypoints may be into obstacles. In fact, this may be the reason why the system had to switch to the REACT alternative modality in the first place. If this intermediate goal is reached before a switch back to PLAN is executed, the next waypoint of the plan that is confirmed as clear from obstacles becomes the new goal for REACT. Clearly, it is preferable that the robot switches back to PLAN within a reasonable period of time, to avoid risks of seeing REACT going into dead-ends. V. CONTEXT INFORMATION The context information relates to the distance d from the robot boundaries to the closest obstacles as seen on the a

5 priori global map. Indeed, we determined experimentally that the PLAN modality is likely to fail in situations where the robot is too close to obstacles, i.e. closer than a security distance d s = 0.15m, equal to half the radius of the robot. Therefore, intuitively, the a priori recommendation based on the context information is to use PLAN in areas sufficiently clear from obstacles (d > d s ), REACT in areas that are close to an obstacle on the global map (d d s ), and to use STOP in places located in the immediate proximity of obstacles (d < d c, critical distance). The map is an occupancy grid that was built using the laser of the robot, while perfect localisation of the robot was assumed. Therefore, the map uncertainty can be expressed as σ map = σ laser, the standard deviation of the range measurements of the laser scanner. σ laser = 0.03 m for the Hokuyo laser on our robot. d (the observation O t ) is calculated online using the current localisation of the robot in the map. To integrate this observation in the system (HMM), probability density functions (pdf) are used to take into account uncertainties. The main sources of uncertainties are the (x, y) localisation of the robot in the map and the location of the obstacles in the map itself. The uncertainty in the a priori map is independent of the uncertainty of the current localisation, as the map was built beforehand, using a different localisation. Therefore, the standard deviation on the observation of d can be expressed as the sum of the uncertainties: σ = σ map +σ loc, where σ loc was defined in IV-A.2. This section describes the pdfs p(o t x i ) representing the context information for the three available modality recommendations. For convenience and simplification for the reader, hereafter we will use the notation for the three states x k and the name of the corresponding modalities (m 0 = P LAN, m 1 = REACT or m 2 = ST OP ) interchangeably. However, let us keep in mind the definition of the states of the HMM, which remains as given in Section III. A. Modality STOP We define the distribution of p(o t x 2 ), or p(d ST OP ), as an inverse sigmoid (or soft threshold ) centred on the critical distance d c (see Fig. 4 in red): P (d ST OP ) = 1 1 α 1 + e (d dc)/σ (1) where σ partly defines the curvature of the sigmoid. σ (similar to the standard deviation of a Gaussian) corresponds to the uncertainty in d, and d c = 0 is the critical distance, as defined above. This distribution p(d ST OP ) represents the likelihood that the observation d is made, knowing that the robot should be stopping. The soft threshold accounts for the uncertainty in the observation and in the knowledge of this threshold value. Note that the limit of this sigmoid when d tends to infinity is superior to zero (see Fig. 4). This is to account for the fact that the map does not capture all the information in the world, in particular dynamic obstacles. Therefore, the value of this limit represents the chance of having to stop Fig. 4. Probability density functions for the context information (shown before normalisation, with σ loc = 0). These are functions of d, representing the distance to the closest obstacles to the robot, as seen in the map. the robot while infinitely away from map obstacles, i.e. the chance of having a dynamic object appearing withing less than d c of the robot, α. A prior probability for this chance could be evaluated statistically. However, such an event, similarly to the fault events in the literature of diagnosis and fault detection, typically has a very little probability of occurrence. To maintain the safety of the robot it is crucial to account for the possibility of that event sufficiently so that the system maintains a chance of capturing it (e.g. see [15] for details on this discussion). Thus, this value is set to a value which is higher than the actual probability of occurrence of this event, as it would be determined statistically. In our implementation we set α = B. Modality REACT The distribution of p(o t x 1 ), or p(d REACT ), is defined as a sigmoid centred on d c (see Fig. 4 in blue): P (d REACT ) = 1 (α + γ) 1 + e (d dc)/σ (2) where σ = σ map + σ loc, as defined earlier. To guarantee the safety of the robot, the main restriction for this modality is that it cannot handle to be too close to obstacles (d < d c ), hence the sigmoid. The secondary restriction is twofold and is not dependent on d as observed on the map. The first aspect is the chance of having a dynamic object appearing within a distance d c to the robot, i.e. α = 0.05, as defined above. The second aspect is the a priori chance of failure of REACT in general, even in an open map (recall that this modality will fail in case of situations such as the presence of U-shape obstacles and dead-ends). This chance highly depends on the environment. To try to maintain a reasonable value for various types of environment we consider that chance with the probability: γ = Considering the events represented by α and γ as independent, the limit of the sigmoid p(d REACT ) when d tends to infinity is then set to 1 (α + γ) = 0.75.

6 C. Modality PLAN The distribution of p(o t x 0 ), or p(d P LAN), is also defined as a sigmoid, centred on the security distance d s (see Fig. 4 in green): P (d P LAN) = 1 β 1 + e (d ds)/σ (3) Note that once again the limit of the sigmoid p(o t m 0 ) when d tends to infinity is lower than 1. This is to account for the chance of having dynamic objects appearing within d s of the robot bounds. We consider the prior probability of this event to be β = 0.10 (β > α), therefore the limit of the sigmoid distribution is 1 β = 0.9. Note that for high values of d it is important to set the chance of success of PLAN as being higher than REACT (as if the goal is far, it is known that PLAN is more likely to succeed), i.e. β < α+γ. Finally, note that these distributions need to be normalised before integration in the HMM. VI. ONLINE MONITORING The monitoring information is based on the same current laser measurement that are used in the REACT modality (a simple fusion of recent laser scans, as described in Section IV-B.2). These measurements give an observation of the surroundings of the robot at the current time. They are independent of the global localisation. The online monitoring uses δ, the distance from the robot bounds to the closest obstacle detected in these laser measurements. Recall that the online monitoring contributes to the computation of the transition probabilities of the HMM. The intuitive rules of transitions specify that, for safety and better handling of cluttered environment, if the robot is getting too close to obstacles seen in current laser scans while operating in PLAN, it should switch to REACT, since this modality uses local information only. In this way, if the global localisation is temporarily significantly inaccurate, or if obstacle points are in a different location than on the (static) global map, this situation can be handled by REACT, contrary to PLAN. More specifically, the intuitive rules of transitions (given here without uncertainty, for convenience) are the following. The corresponding transition probabilities used in the HMM are given in parenthesis, in both full (e.g. P (x 2 x 1, δ)) and equivalent reduced form (e.g. p 1,2 ). First, let us consider the output transitions of x 0 (i.e. PLAN). The transition P (x 1 x 0, δ) = p 0,1 (PLAN to REACT) is likely if d c < δ < d s, i.e. an obstacle is detected by the laser in the intermediate proximity of the robot. The transition P (x 2 x 0, δ) = p 0,2 (PLAN to STOP) is likely if δ < d c, i.e. an obstacle is detected by the laser in the immediate proximity of the robot. P (x 0 x 0, δ) = p 0,0 (PLAN to PLAN) is likely if δ > d s, i.e. the robot is clear from obstacles. The other transitions can be defined similarly, using the same short notations as above: p 1,0 = p 2,0 = p 0,0, p 1,1 = p 2,1 = p 0,1 p 1,2 = p 2,2 = p 0,2, Because of the uncertainty in δ, i.e. in the laser measurements, these rules are defined probabilistically, using sigmoid distributions similar to these defined in Section V and shown in Fig. 4. In this case the main source of uncertainty is the relative inaccuracy of the laser measurements, therefore the σ of the sigmoids is: σ = σ laser (recall that the uncertainty in the localisation estimation is not relevant for the monitoring). Note that the output transition probabilities from each state are normalised, as their sum must be equal to 1. VII. EXPERIMENTS The experimental validation in this section illustrates the resilience of our online reconfiguration approach. It shows that various unpredictable failures can be mitigated with our system. Examples of causes of such failures are: errors of the controller while executing a planned path, errors in the map (i.e. presence of objects that could not be integrated in the map early enough) and large localisation uncertainty or error (e.g. offset). Results were obtained using the platform described in Section IV. For convenience, the experiments selected for the illustration in the paper are named with a letter, starting with Exp. A. The illustrations show the estimated trajectory of the robot during each test, using coloured points to represent the modality used at the time, following the recommendation given by the HMM. This selected modality corresponds to the highest output probability P (x k,t O 1:t, M 1:t ) at each time step t. The HMM and the modality selection were updated at 10Hz onboard the robot. A. Modality reconfiguration in static environment 1) Unpredicted control error: The experiment in Fig. 5(a) illustrates how our framework of modality reconfiguration allowed the system to maintain the robot s safety in the presence of unpredicted errors of the controller during the execution of the planned trajectory. The robot started executing a planned path similar to the one in Fig. 1(a), which was successful using PLAN only. However, at the (expected) end of the turn around the first corner, the controller overshot, risking the safety of the platform. This dangerous event was detected by our system, which switched to REACT to recover the error. Once the safety was safe again, the robot returned to the PLAN modality to complete its mission. 2) Going through a narrow doorway: In this test (Fig. 6) the goal given to the robot was inside a room. To achieve this goal, the robot had to follow a corridor and then pass through a 0.85m wide doorway (recall that the diameter of the robot is approximately 0.6m). As the corridor is reasonably large (about 1.7m in average), the robot could use PLAN most of the time in that area. However, switching to REACT was needed to negotiate the doorway passage, which the robot did successfully. In this situation, context and monitoring information were in agreement, i.e. if considered individually, both would have caused a recommendation to use REACT to go through the doorway.

7 (a) Exp. C: Dynamic obstacle (b) Exp. D: Localisation fault (a) Exp. A (b) Exp. B: Error in map: unexpected obstacle (grey box) Fig. 5. Examples of robot trajectory executed with modality switching. Known obstacles in the map are shown in black, while the colour points show the (estimated) positions of the centre of the robot. Green means the recommended modality is PLAN, while blue means the recommended modality is REACT. Approximate size of the area shown: 6.5m 7.5m. Fig. 7. (a): A pedestrian forces the robot to switch to REACT and try to get around him (see events C). PLAN can be used again once the human has cleared the way. (b): Modality switching with injected localisation fault (sudden offset to the right of 1m, see D). As a result, the estimated positions of the robot appear to be on the right wall (blue line). The magenta dashed line shows the reference localisation (i.e. without the injected fault). Fig. 6. Exp. E estimated trajectory. Same colour code as before. Size of the area shown: 16m (horizontal) x 5m (vertical). B. Unexpected Error in the Map Fig. 5(b) shows another example of modality switching to negotiate an unexpected situation safely: an unpredictable large error in the map. This situation is caused by the presence of an unexpected obstacle (box shown in brown in the figure). This object was added at the last minute, therefore it does not feature in the map that PLAN is using (simulating a map error). In order to avoid it the robot switches to REACT, before returning to PLAN once the situation is safe again, and the map is more consistent with the current observation. A very likely collision due to an unexpected event was avoided. C. Presence of dynamic obstacles We validated that the robot is resilient to the presence of highly dynamic obstacles, e.g. people that the robot may only perceive at the last minute, when they already are at immediate proximity. In the test illustrated in Fig. 7(a), a pedestrian, coming from the top left of the scene, walked fast to the robot. As he arrived very close to the robot, the latter first switched to REACT and then tried to get around him (event C). Once the human had cleared its immediate surroundings, the robot could resume its mission. A similar situation occurred again later in the test, with an even later appearance of the human in the FOV of the robot. This event was again safely handled by the robot. This test shows that thanks to our multi-modality framework, while the robot can plan and execute optimised trajectories when no dynamic obstacles are in close proximity, it still has the ability to Fig. 8. Distance to closest obstacles, as seen on the map (d, in green) and from the current laser scans (δ, in blue). safely react to the presence of such dynamic obstacles, comparably to a pure reactive motion or hybrid strategy. D. Injected localisation fault In this experiment, a significant localisation fault was artificially created by introducing a sudden and unexpected offset of x = 1m to the output of the localisation estimator. Fig. 7(b) shows the clear offset to the right between the estimated position and the reality. However, the robot was still able to safely achieve its mission by switching to REACT when appropriate. Fig. 8 shows the distance to obstacles as seen on the map (i.e. d, used in the context information) compared to what was seen by the laser (i.e. δ, used in the monitoring). Note the values are comparable for the first 35s approximately, before the fault injection. The injection of the localisation offset is clearly illustrated by the sudden drop of δ to zero, since according to its estimated position the robot appears to be on the location of known obstacles in the map. The context information is shown in Fig. 9 and the corresponding partial probabilities of modality recommendation are shown in Fig. 10. Note that Fig. 9 indicates that according to the context

8 Fig. 9. Exp. D: Partial probabilities for Context information, corresponding to the test in Fig. 7(b). Note that the recommendation based on context information only would be to STOP, due to the immediate proximity of obstacles in the map, according to the current global localisation (see Fig. 8). Fig. 10. Exp. D: Partial probabilities for all three states (i.e. modality recommendation), corresponding to the test in Fig. 7(b). information alone the robot should definitely STOP, i.e. if the robot could only rely on its current global localisation estimate and map, it would not have been safe to continue its mission. However, our system only recommended a prudent switch to REACT and then remained comfortable enough to keep using this modality, considering what the monitoring was indicating (see distance to obstacles as seen by the laser scan, in Fig. 8). The localisation algorithm then progressively corrected its estimation, up to a point when the situation became comfortable enough again to use PLAN to finish the mission. Thanks to our modality recommendation framework, the robot was able to continue its mission despite this large localisation error, while being aware of a problem with the localisation and/or the map, through the context information. Our system is shown to be resilient to large unpredicted localisation or map errors. VIII. CONCLUSION We have shown that a multiple modality strategy for resilient navigation, based on a probabilistic framework, can be applied to an indoor mobile robot to combine the advantages of navigation modalities while maintaining the safety of the platform. In our implementation, the robot is able to plan and execute smooth paths (minimising change in curvature) when possible, while being very reactive when needed. The system was applied online onboard the robot and shown to be reliable and robust in the presence of map errors and large localisation uncertainties or offsets. Note that the concept is demonstrated with one particular choice of planning and reactive modality, however, these planning and reactive methods are easily interchangeable. Future work will better exploit the monitoring and context information for diagnosis and recovery of particular components of the system. In the current implementation, both types of information are only exploited to compute a modality recommendation at any point in time. However, we saw in Section VII-D that the discrepancy between the context information and the monitoring indicates a likely error in the map or global localisation. This could be used to actively repair these components, while the robot uses a reactive modality. REFERENCES [1] R. Alterovitz, T. Simeon, and K. Goldberg, The stochastic motion roadmap: A sampling framework for planning with markov motion uncertainty, in Robotics: Science and Systems II, [2] A. Bry and N. Roy, Rapidly-exploring random belief trees for motion planning under uncertainty, in Proc. of IEEE ICRA, [3] J. Minguez and L. Montano, Nearness diagram navigation (ND): Collision avoidance in troublesome scenarios, IEEE Trans. on Robotics and Automation, vol. 20, no. 1, pp , [4] W. H. Huang, B. R. Fajen, J. R. Fink, and W. H. Warren, Visual navigation and obstacle avoidance using a steering potential function, Robotics and Autonomous Systems, vol. 54, no. 4, pp , [5] O. Brock and O. Khatib, High-speed navigation using the global dynamic window approach, in Proc. of IEEE ICRA, vol. 1, 1999, pp vol.1. [6] B. Morisset and M. Ghallab, Learning how to combine sensory-motor functions into a robust behavior, Artificial Intelligence, vol. 172, pp , [7] J. F. V. Verma, R. Simmons, Probabilistic models for monitoring and fault diagnosis, in 2d IARP and IEEE/RAS Joint Workshop on Technical Challenges for Dependable Robots in Human, [8] H. Choset, K. M. Lynch, S. Hutchinson, G. A. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations. Cambridge, MA: MIT Press, [9] H. Kurniawati, T. Bandyopadhyay, and N. M. Patrikalakis, Global motion planning under uncertain motion, sensing, and environment map, in Robotics: Science and Systems VII, [10] T. Peynot and S. Lacroix, Selection and monitoring of navigation modes for an autonomous rover, in Experimental Robotics: The 10th Int. Symposium on Experimental Robotics, O. Khatib, V. Kumar, and D. Rus, Eds. Springer-Verlag, Berlin, 2008, vol. 39, pp [11] E. Arnaud, E. Memin, and B. Cernuschi-Frias, Conditional filters for image sequence based tracking - application to point tracking, IEEE Trans. on Image Processing, vol. 14, no. 1, pp , [12] N. Kirchner et al., RobotAssist - a Platform for Human Robot Interaction Research, in Proc. of ARAA Australasian Conf. on Robotics and Automation, [13] S. Thrun, D. Fox, W. Burgard, and F. Dellaert, Robust monte carlo localization for mobile robots, Artificial Intelligence, vol. 128, no. 1-2, pp , [14] J. Barraquand and J. Latombe, Nonholonomic multibody mobile robots: Controllability and motion planning in the presence of obstacles, Algorithmica, vol. 10, pp , [15] R. Dearden and D. Clancy, Particle filters for real-time fault detection in planetary rovers, in 12th Int. Workshop on Principles of Diagnosis, 2002.