Section: New Results
Motion planning and Autonomous Navigation in the physical world
Partial Motion Planning
Dynamic environments, ie environments with moving objects, impose a strict upper-bound on the time available to a robotic system in order to determine its future course of action. This constraint is henceforth called the decision time constraint. The decision time available is a function of what is called the dynamicity of the environment which is directly related to the dynamics of both the moving objects and the robotic system. Failure to meet the decision time constraint may put a robotic system in a situation where a collision eventually occurs. Lately, a number of motion planning-based navigation schemes dealing with dynamic environments have been proposed, eg  ,  ,  ,  . Unlike earlier reactive navigation schemes (that seeks to determine the motion to execute during the next time-step only), they aim at computing a complete motion all the way to the goal. To do so, they rely upon the most efficient motion planning techniques available today, namely randomised techniques such as the Probabilistic Roadmap  or the Rapidly Exploring Random Tree  , whose average running times are low enough so that they can be used on-line. This type of approach is very enticing since it makes up for the lack of lookahead of reactive navigation schemes. However, it is argued that the decision time constraint can be such that it prevents the use of such schemes since the running time of a randomised technique cannot be upper bounded.
Given the intrinsic complexity of motion planning in dynamic environments (see the complexity results established in  and  ), it seems unlikely that a hard decision time constraint could ever be met in realistic situations. We advocate instead the Partial Motion Planning principle  as a general way to handle the decision time constraint and to address autonomous navigation in dynamic environments. Partial Motion Planning (PMP) is an interruptible planning scheme: when the time available is over, it returns either a complete motion to the goal or a partial motion only, ie a motion that may not necessarily reach the goal. This partial motion is then passed along to the navigation system of the robot for execution. Of course, since only a partial motion is computed, it is necessary to iterate the partial motion planning process until the goal is reached. The iterative nature of PMP is doubly required when the robotic system at hand is placed in a uncertain dynamic environment, ie an environment for which everything is not known in advance (in particular, the future behaviour of the moving objects). Indeed, motion planning means reasoning about the future. When the future is unknown, one has to resort to predictions, predictions whose validity duration is limited in most cases. An iterative planning scheme permits to take into account the unexpected changes of the environment by updating the predictions at a given frequency (which is also determined by the environment dynamicity). Like reactive approaches, PMP faces two issues, namely the convergence and the safety issues:
What guarantee do we have that the goal will ever be reached?
What guarantee do we have that the robotic system will never found itself in a dangerous situations?
As for the convergence issue, the unrealistic convergence conditions established in  leave little hope (it is hardly surprising if the system is placed in an environment with no a priori information about the moving obstacles). To address the safety issue, a solution relying upon the concept of Inevitable Collision States is explored ( cf section 6.3.2 ). To address the safety issue, different solutions are explored, mainly centered around the concept of Inevitable Collision States ( cf section 6.3.2 ).
In 2008, work on PMP has mostly been furthered in the scope of the sabbatical stay of Thierry Fraichard at the Swiss Federal Institute of Technology in Zurich (within Prof. Roland Siegwart's group). A PMP module has been developed and integrated in the navigation architecture developed in Zurich. The PMP module is coupled with a route planner and connected with the control, perception and environment modelling modules of the architecture.
A diffusion technique is used to build a tree of admissible trajectories embedded in the state × time space of the robotic system and to extract from this tree a partial motion that is used during the next time cycle to drive the system towards its goal. From a theoretical point of view, the safety of the system is guaranteed if and only each partial motion is ICS-free up to the time that corresponds to the initial state of the partial motion that is to be computed at the next navigation cycle), because then, at the next navigation cycle, the navigation module always has a safe evasive manoeuvre available. Now, checking whether a given state is an ICS or not requires in theory the full knowledge of the environment of and its future evolution. In practice however, one has to deal with the sensors' limited field of views and the elusive nature of the future. Knowledge about the environment of the system is thus limited both spatially and temporally . This is the very reason why it is impossible to guarantee an absolute level of safety (absolute in the sense that it can be guaranteed that the system will never crash eventually). This intrinsic impossibility compels us to settle for weaker levels of safety. Although weaker, the important thing is that such levels of safety will be guaranteed given the information that the system knows about its environment. We have explored two different levels of safety:
- Passive Safety.
The first safety level we have seeked to enforce is the simplest one maybe. It guarantees that, should a collision ever occur, the system will be at rest. In other words, if a collision is inevitable, it can be guaranteed that the system always have the possibility to brake down and stop before the collision occurs.
- Passive Friendly Safety.
In an environment where the moving objects are assumed to be friendly , ie seeking to avoid collisions, and for which a certain knowledge about their dynamic properties is available, it can be desirable to enforce a stronger level of safety. This second safety level guarantees that, should a collision ever occur, the system will be at rest and the colliding object would have had the time to slow down and stop before the collision had it wanted to.
Other safety levels could be proposed. The ultimate one of course is to determine safety with respect to the set of ICS defined by the current environment model. Given the complexity of characterizing this ICS set, Passive Safety and Passive Friendly Safety constitutes interesting alternatives in the sense that they can be computed efficiently and provide an adequate level of safety. Fig. 12 illustrates PMP-based navigation in a simulated dynamic environment. This work has already yielded two publications in international conferences  ,  Experiments with the SmartTer platform (Fig. 13 ) are underway. They will be documented in Kristijan Macek's Ph.D thesis.
Inevitable Collision States
Autonomous mobile robots/vehicles navigation has a long history by now. Remember Shakey's pioneering efforts in the late sixties  . Today, the situation has dramatically changed as illustrated rather brilliantly by the 2007 DARPA Urban Challenge (http://www.darpa.mil/grandchallenge .). The challenge called for autonomous car-like vehicles to drive 96 kilometers through an urban environment amidst other vehicles (11 self-driving and 50 human-driven). Six autonomous vehicles finished the race thus proving that autonomous urban driving could become a reality. Note however that, despite their strengths, the Urban Challenge vehicles have not yet met the challenge of fully autonomous urban driving (how about handling traffic lights or pedestrians for instance?). Another point worth mentioning is that at least one collision took place between two competitors. It raises the important issue of motion safety , ie the ability for an autonomous robotic system to avoid collision with the objects of its environment.
To address this issue, we have explored the novel concept of Inevitable Collision States (ICS) since 2002. An ICS for a given robotic system is a state for which, no matter what the future trajectory followed by the system is, a collision with an object eventually occurs. For obvious safety reasons, a robotic system should never ever end up in an ICS. ICS have already been used in a number of applications: (i) mobile robot subject to sensing constraints, ie a limited field of view, and moving in a partially known static environment  , (ii) car-like vehicle moving in a roadway-like environment   , (iii) spaceship moving in an asteroid field  . In all cases, the future motion of the robotic system at hand is computed so as to keep the system away from ICS. To that end, an ICS-Checker is used. As the name suggests, it is an algorithm that determines whether a given state is an ICS or not. Similar to a Collision-Checker that plays a key role in path planning and navigation in static environments, it could be argued that an ICS-Checker is a fundamental tool for motion planning and navigation in dynamic environments. Like its static counterpart, an ICS-Checker must be computationally efficient so that it can meet the real-time constraint imposed by dynamic environments.
Since 2007, we have been working on generic and efficient ICS-Checker for planar robotic systems with arbitrary dynamics moving in dynamic environments. The efficiency is obtained by applying three principles: (a) reasoning on 2D slices of the state space of the robotic system, (b) precomputing off-line as many things as possible, and (c) exploiting graphics hardware performances. A preliminary version of the ICS-Checker was presented at the 2007 IEEE Int. Conf. on Robotics and Automation (ICRA)  . A final version have been presented in 2008 at the IEEE-RSJ Int. Conf. on Intelligent Robots and Systems (IROS) wherein the ICS-Checker was applied to two different robotic systems: a car-like vehicle and a spaceship.
Next, we moved to design an ICS-based collision avoidance scheme, ie a decision-making module whose primary task is to keep the robotic system at hand safe from collisions. To that end, the ICS-Checker proposed in  is used. This collision avoidance scheme, henceforth called Ics-Avoid , guarantees motion safety with respect to the model of the future which is used . To demonstrate the efficiency of Ics-Avoid , it has been extensively compared with two state-of-the-art collision avoidance schemes, both of which have been explicitly designed to handle dynamic environments. The first one has been proposed by  and is a straightforward extension of the popular Dynamic Window approach  . The second one builds upon the concept of Velocity Obstacle concept  .
If Ics-Avoid were provided with full knowledge about the future, it would guarantee motion safety no matter what. Given the elusive nature of the future, this assumption is unrealistic. In practice, knowledge about the future is limited. However, the results obtained show that, when provided with the same amount of information about the future evolution of the environment, Ics-Avoid performs significantly better than the other two schemes. The first reason for this has to do with the respective time-horizon of each collision avoidance scheme thus emphasizing the fact that, reasoning about the future is not nearly enough, it must be done with an appropriate time horizon. The second reason has to do with the decision part of each collision avoidance scheme. In all cases, their operating principle is to first characterize forbidden regions in a given control space and then select an admissible control. Accordingly motion safety also depends on the ability of the collision avoidance scheme at hand to find a such an admissible control. In the absence of a formal characterization of the forbidden regions, all schemes resort to sampling (with the inherent risk of missing the admissible regions). In contrast Ics-Avoid , through the concept of Safe Control Kernel , is the only one for which it is guaranteed that, if an admissible control exists, it will be part of the sampling set (thus guaranteeing safe transitions between non-ICS states). A paper describing the results of this comparison has been submitted to the 2009 IEEE Int. Conf. on Robotics and Automation (ICRA).
Where to move next? is a key question for an autonomous robotic system. This fundamental issue has been largely addressed in the past forty years. Many motion determination strategies have been proposed. They can broadly be classified into deliberative versus reactive strategies: deliberative strategies aim at computing a complete motion all the way to the goal, whereas reactive strategies determine the motion to execute during the next time-step only. Deliberative strategies have to solve a motion planning problem  . They require a model of the environment as complete as possible and their intrinsic complexity is such that it may preclude their application in dynamic environments. Reactive strategies on the other hand can operate on-line using local sensor information: they can be used in any kind of environment whether unknown, changing or dynamic, but convergence towards the goal is difficult to guarantee.
To bridge the gap between deliberative and reactive approaches, a complementary approach has been proposed based upon motion deformation . The principle is simple: a complete motion to the goal is computed first using a priori information. It is then passed on to the robotic system for execution. During the course of the execution, the still-to-be-executed part of the motion is continuously deformed in response to sensor information acquired on-line, thus accounting for the incompleteness and inaccuracies of the a priori world model. Deformation usually results from the application of constraints both external (imposed by the obstacles) and internal (to maintain motion feasibility and connectivity). Provided that the motion connectivity can be maintained, convergence towards the goal is achieved.
The different motion deformation techniques that have been proposed  ,  ,  ,  ,  all performs path deformation . In other words, what is deformed is a geometric curve, ie the sequence of positions that the robotic system is to take in order to reach its goal. The problem with path deformation techniques is that, by design, they cannot take into account the time dimension of a dynamic environment. For instance in a scenario such as the one depicted in Fig. 15 , it would be more appropriate to leave the path as it is and adjust the velocity of the robotic system along the path so as to avoid collision with the moving obstacle (by slowing down or accelerating). To achieve this, it is necessary to depart from the path deformation paradigm and resort to trajectory deformationinstead. A trajectory is essentially a geometric path parametrized by time. It tells us where the robotic system should be but also when and with what velocity. Unlike path deformation wherein spatial deformation only takes place, trajectory deformation features both spatial and temporal deformation meaning that the planned velocity of the robotic system can be altered thus permitting to handle gracefully situations such as the one depicted in Fig. 15 .
In 2007, we presented the first trajectory deformation scheme  . It operates in two stages (collision avoidance and connectivity maintenance stages) and is geared towards manipulator arms. Since then, we have been working on a novel trajectory deformation scheme, henceforth called Teddy (for Trajectory Deformer). It operates in one stage only and is designed to handle arbitrary robotic systems. Teddy is designed to be one component of an otherwise complete autonomous navigation architecture. A motion planning module is required to provide Teddy with the nominal trajectory to be deformed. Teddy operates periodically with a given time period. At each cycle, Teddy outputs a deformed trajectory which is passed to a motion control module that determines the actual commands for the actuators of the robotic system.
Teddy was initially developed in the scope of the Master's thesis of Vivien Delsart  that addressed the case of a double integrator system (linear dynamics). This work was presented at the 2008 EUROS Conference  and yielded a journal article due to appear in 2009  . Results of the trajectory deformation process are depicted in Fig. 16 .
In 2008, we have studied the case of a car-like system (non-linear dynamics). The computation of the internal forces for such a system becomes complex since it requires the characterization of its set of reachable states, complex to the point that it may be incompatible with the real-time requirements of a dynamic environment. To solve this problem, we have decided instead to base the internal forces computation upon a steering method , ie a function that computes a feasible trajectory between arbitrary pairs of states. To that end, we have developed a novel steering method derived from that of  . Unlike standard steering methods, ours must be able to compute feasible trajectories between states with a fixed time component. Examples of the trajectories generated are depicted in Fig. 17 . This work yielded a presentation at the 2008 Int. Conf. on Intelligent Robots and Systems (IROS)  and a submission at the 2009 Int. Conf. on Robotics and Automation (ICRA).
Goal oriented navigation in dynamic uncertain environment
The problem of autonomous navigation in dynamic environment is a real challenge for nowadays robotics. Many techniques have been developed to address navigation in known static environment (path planning algorithms) and for exploration tasks. In dynamic environment, the developed approaches are usually based on reactive algorithms which make the assumption that the local environment is perfectly known and that use a static representation that is updated on-line with the new observations. A minor part of reactive techniques (Velocity Obstacles   and Dynamic Object Velocity) and partial planning take explicitly into account the fact that the obstacles are moving and make the hypothesis that the robot knows exactly the trajectory of this last ones. Our aim is then to put in relation the decision about motion and the on-line uncertain perception of the world: the uncertainty and incompleteness of the information of the static and dynamic world cannot be ignored and is not possible to compute a full plan in advance.
At first we focused our attention on a reactive method based on the Velocity Obstacle approach. We developed a novel technique and integrate the velocity obstacles with a probabilistic perception of the occupation of the space and of the velocity of the obstacles. A space-time occupancy grid estimates the occupation of the space and a probability distribution over velocities for each object in the environment; the probability of collision in time can be estimated and a safe control is chosen. The algorithm takes into account uncertainty coming from sensors limited range and error probability, occluded space, uncertain velocity estimation, unknown obstacle shape. Our results were published at ICRA 2007  and used in the LOVe project.
These last two years we worked with the purpose to develop a more complex navigation structure which could integrate medium-term prediction, short-term prediction and reactive behaviour. While reactive behaviour is based on the previously described algorithm, short-term and medium-term prediction are used within a partial planner which takes into account the different accuracy and reliability of prediction to find safe control sequences. We developed a search method which is a novel extension of the Rapidly-exploring Random Trees algorithm for the case of probabilistic uncertainty. For each partial path searched, a probability of success is computed on the basis of the environment information and probabilistic prediction. The search is biased not only by the goal direction but also by the likelihood of the paths. This search method has been integrated with a partial planning algorithm, which accounts for safety issues and chooses an output partial path. Short-term predictions are given by a multi-target tracking algorithm. These predictions are reliable only within a short period and are represented by one or more Gaussians (respectively in the case of Kalman Filter tracking or of Multi Hypotheses Tracking) with covariance growing in time. We tested the partial planner with real data acquired with the Cycab (Fig. 18 (a)); The environment is observed with a laser range finder: the static environment is mapped in an occupancy grid, while moving pedestrians are tracked with a Kalman filter (Fig. 18 (b)); our navigation algorithm searches the space and provides a safe partial path: in Fig 18 (c) the cones represent the 1 standard deviation ray uncertainty in obstacle prediction, the green and red lines represent the partial paths explored and a threshold has been applied to show safe and unsafe nodes. The blue line is the chosen path. The partial paths issued at each timestep have been then tested with the real observations acquired and have been proved to be consistent with the predictions.
However, the motion model estimated by the tracking is valid only in a short-time period. So, medium-term prediction is based on pre-learned typical patterns. The moving obstacles are supposed to be intelligent agents moving with the intention to reach a specific goal. With this hypothesis it is possible to observe the environment in order to learn the typical behaviour of the obstacles and predict their future motion with a certain confidence. We consider that the system has already learnt a set of typical patterns. We integrated our search algorithm with two different representations of typical patterns developed in the team: Markov graphs by Dizan Vasquez  and Gaussian Processes by Christopher Tay ( see section 6.1.6 ). In the case of Markov graphs, the obstacles trajectories are represented by discrete nodes and the prediction is made estimating the state of the obstacle and letting the graph evolve. For each future time step a discretized distribution is obtained, represented by a set of particles with weight proportional to their probability. In the case of Gaussian Processes representation, obstacles trajectories are represented by continuous functions each characterised by a mean function and a covariance function. To predict the future state, the sequence of observations is considered and the probability that an object is following one or the other pattern is estimated. The probability distribution is so given by a Gaussian mixture at each time instant. Differently from the Multi Hypothesis Tracking, the Gaussians do not grow indefinitely with time, but follow the typical patterns (Fig. 19 (a)). Our algorithm is able to integrate these different kinds of predictions and to update on-line the probabilistic information to give a safe an goal solution with the current information. These algorithm have been tested on the Cycabtk simulator with real and simulated trajectories acquired in collaboration with Dizan Vasquez and Christopher Tay (Fig. 19 (b)). Our results have been published in IROS 2008 (  )
This work is supported by a grant from the European Community under the Marie-Curie project VISITOR and implied in the LOVe and BACS projects.