A Bayesian framework for optimal motion planning with uncertainty

Modeling robot motion planning with uncertainty in a Bayesian framework leads to a computationally intractable stochastic control problem. We seek hypotheses that can justify a separate implementation of control, localization and planning. In the end, we reduce the stochastic control problem to path- planning in the extended space of poses x covariances; the transitions between states are modeled through the use of the Fisher information matrix. In this framework, we consider two problems: minimizing the execution time, and minimizing the final covariance, with an upper bound on the execution time. Two correct and complete algorithms are presented. The first is the direct extension of classical graph-search algorithms in the extended space. The second one is a back-projection algorithm: uncertainty constraints are propagated backward from the goal towards the start state

Path planning with pose SLAM

The probabilistic belief networks that result from standard feature-based simultaneous localization and map building (SLAM) approaches cannot be directly used to plan trajectories. The reason is that they produce a sparse graph of landmark estimates and their probabilistic relations, which is of little value to find collision free paths for navigation. In contrast, we argue in this paper that Pose SLAM graphs can be directly used as belief roadmaps (BRMs). The original BRM algorithm assumes a known model of the environment from which probabilistic sampling generates a roadmap. In our work, the roadmap is built on-line by the Pose SLAM algorithm. The result is a hybrid BRM-Pose SLAM method that devises optimal navigation strategies on-line by searching for the path with lowest accumulated uncertainty for the robot pose. The method is validated over synthetic data and standard SLAM datasets.Postprint (published version

Control and planning for vehicles with uncertainty in dynamics

Abstract-This paper describes a motion planning algorithm that accounts for uncertainty in the dynamics of vehicles. This noise is a function of the type of controller employed on the vehicle and the characteristics of the terrain and can cause the robot to deviate from a planned trajectory and collide with obstacles. Our motion planning algorithm finds trajectories that balance the trade-off between conventional performance measures such as time and energy versus safety. The key is a characterization of the vehicle's ability to follow planned paths, which allows the algorithm to explicitly calculate probabilities of successful traversal for different trajectory segments. We illustrate the method with a six-legged Rhex-like robot by experimentally characterizing different gaits (controllers) on different terrains and demonstrating the hexapod navigating a multi-terrain environment

Robotic motion planning in dynamic, cluttered, uncertain environments

This paper presents a strategy for planning robot motions in dynamic, cluttered, and uncertain environments. Successful and efficient operation in such environments requires reasoning about the future system evolution and the uncertainty associated with obstacles and moving agents in the environment. This paper presents a novel procedure to account for future information gathering (and the quality of that information) in the planning process. After first presenting a formal Dynamic Programming (DP) formulation, we present a Partially Closed-loop Receding Horizon Control algorithm whose approximation to the DP solution integrates prediction, estimation, and planning while also accounting for chance constraints that arise from the uncertain location of the robot and other moving agents. Simulation results in simple static and dynamic scenarios illustrate the benefit of the algorithm over classical approaches

Mapping, planning and exploration with Pose SLAM

This thesis reports research on mapping, path planning, and autonomous exploration. These are classical problems in robotics, typically studied independently, and here we link such problems by framing them within a common SLAM approach, adopting Pose SLAM as the basic state estimation machinery. The main contribution of this thesis is an approach that allows a mobile robot to plan a path using the map it builds with Pose SLAM and to select the appropriate actions to autonomously construct this map. Pose SLAM is the variant of SLAM where only the robot trajectory is estimated and where landmarks are only used to produce relative constraints between robot poses. In Pose SLAM, observations come in the form of relative-motion measurements between robot poses. With regards to extending the original Pose SLAM formulation, this thesis studies the computation of such measurements when they are obtained with stereo cameras and develops the appropriate noise propagation models for such case. Furthermore, the initial formulation of Pose SLAM assumes poses in SE(2) and in this thesis we extend this formulation to SE(3), parameterizing rotations either with Euler angles and quaternions. We also introduce a loop closure test that exploits the information from the filter using an independent measure of information content between poses. In the application domain, we present a technique to process the 3D volumetric maps obtained with this SLAM methodology, but with laser range scanning as the sensor modality, to derive traversability maps. Aside from these extensions to Pose SLAM, the core contribution of the thesis is an approach for path planning that exploits the modeled uncertainties in Pose SLAM to search for the path in the pose graph with the lowest accumulated robot pose uncertainty, i.e., the path that allows the robot to navigate to a given goal with the least probability of becoming lost. An added advantage of the proposed path planning approach is that since Pose SLAM is agnostic with respect to the sensor modalities used, it can be used in different environments and with different robots, and since the original pose graph may come from a previous mapping session, the paths stored in the map already satisfy constraints not easy modeled in the robot controller, such as the existence of restricted regions, or the right of way along paths. The proposed path planning methodology has been extensively tested both in simulation and with a real outdoor robot. Our path planning approach is adequate for scenarios where a robot is initially guided during map construction, but autonomous during execution. For other scenarios in which more autonomy is required, the robot should be able to explore the environment without any supervision. The second core contribution of this thesis is an autonomous exploration method that complements the aforementioned path planning strategy. The method selects the appropriate actions to drive the robot so as to maximize coverage and at the same time minimize localization and map uncertainties. An occupancy grid is maintained for the sole purpose of guaranteeing coverage. A significant advantage of the method is that since the grid is only computed to hypothesize entropy reduction of candidate map posteriors, it can be computed at a very coarse resolution since it is not used to maintain neither the robot localization estimate, nor the structure of the environment. Our technique evaluates two types of actions: exploratory actions and place revisiting actions. Action decisions are made based on entropy reduction estimates. By maintaining a Pose SLAM estimate at run time, the technique allows to replan trajectories online should significant change in the Pose SLAM estimate be detected. The proposed exploration strategy was tested in a common publicly available dataset comparing favorably against frontier based exploratio

Reliable and safe autonomy for ground vehicles in unstructured environments

This thesis is concerned with the algorithms and systems that are required to enable safe autonomous operation of an unmanned ground vehicle (UGV) in an unstructured and unknown environment; one in which there is no speci c infrastructure to assist the vehicle autonomy and complete a priori information is not available. Under these conditions it is necessary for an autonomous system to perceive the surrounding environment, in order to perform safe and reliable control actions with respect to the context of the vehicle, its task and the world. Speci cally, exteroceptive sensors measure physical properties of the world. This information is interpreted to extract a higher level perception, then mapped to provide a consistent spatial context. This map of perceived information forms an integral part of the autonomous UGV (AUGV) control system architecture, therefore any perception or mapping errors reduce the reliability and safety of the system. Currently, commercially viable autonomous systems achieve the requisite level of reliability and safety by using strong structure within their operational environment. This permits the use of powerful assumptions about the world, which greatly simplify the perception requirements. For example, in an urban context, things that look approximately like roads are roads. In an indoor environment, vertical structure must be avoided and everything else is traversable. By contrast, when this structure is not available, little can be assumed and the burden on perception is very large. In these cases, reliability and safety must currently be provided by a tightly integrated human supervisor. The major contribution of this thesis is to provide a holistic approach to identify and mitigate the primary sources of error in typical AUGV sensor feedback systems (comprising perception and mapping), to promote reliability and safety. This includes an analysis of the geometric and temporal errors that occur in the coordinate transformations that are required for mapping and methods to minimise these errors in real systems. Interpretive errors are also studied and methods to mitigate them are presented. These methods combine information theoretic measures with multiple sensor modalities, to improve perceptive classi cation and provide sensor redundancy. The work in this thesis is implemented and tested on a real AUGV system, but the methods do not rely on any particular aspects of this vehicle. They are all generally and widely applicable. This thesis provides a rm base at a low level, from which continued research in autonomous reliability and safety at ever higher levels can be performed

Belief Space-Guided Navigation for Robots and Autonomous Vehicles

Navigating through the environment is a fundamental capability for mobile robots, which is still very challenging today. Most robotic applications these days, such as mining, disaster response, and agriculture, require the robots to move and perform tasks in a variety of environments which are stochastic and sometimes even unpredictable. A robot often cannot directly observe its current state but instead estimates a distribution over the set of possible states based on sensor measurements that are both noisy and partial. The actual robot position differs from its prediction after applying a motion command, due to actuation noise. Classic algorithms for navigation should adapt themselves to where the behavior of the environment is stochastic, and the execution of the motions has great uncertainty. To solve such challenging problems, we propose to guide the robot's navigation in the belief space. Belief space-guided navigation differs fundamentally from planning without uncertainty where the state of the robot is always assumed to be known precisely. The robot senses its environment, estimates its current state due to perception uncertainty, and decides whether a new (or priori) action is appropriate. Based on that determination, it actuates its sensors to move with motion uncertainty in the environment. This inspires us to connect robot perception and motion planning, and reason about the uncertainty to improve the quality of plan so that the robot can follow a collision-free, feasible kinodynamic, and task-optimal trajectory. In this dissertation, we explore the belief space-guided robotic navigation problems, which include belief space-based scene understanding for autonomous vehicles and introduce belief space guided robotic planning. We first investigate how belief space can facilitate scene understanding under the context of lane marking quality assessment in the application of autonomous driving. We propose a new problem by measuring the quality of roads and ensuring they are ready for autonomous driving. We focus on developing three quality metrics for lane markings (LMs), correctness metric, shape metric, and visibility metric, and algorithms to assess LM qualities to facilitate scene understanding. As another example of using belief space for better scene understanding, we utilize crowdsourced images from multiple vehicles to help verify LMs for high-definition (HD) map maintenance. An LM is consistent if belief functions from the map and the image satisfy statistical hypothesis testing. We further extend the Bayesian belief model into a sequential belief update using crowdsourced images. LMs with a higher probability of existence are kept in the HD map whereas those with a lower probability of existence are removed from the HD map. Belief space can also help us to tightly connect perception and motion planning. As an example, we develop a motion planning strategy for autonomous vehicles. Named as virtual lane boundary approach, this framework considers obstacle avoidance, trajectory smoothness (to satisfy vehicle kinodynamic constraints), trajectory continuity (to avoid sudden movements), global positioning system (GPS) following quality (to execute the global plan), and lane following or partial direction following (to meet human expectation). Consequently, vehicle motion is more human-compatible than existing approaches. As another example of how belief space can help guide robots for different tasks, we propose to use it for the probabilistic boundary coverage of unknown target fields (UTFs). We employ Gaussian processes as a local belief function to approximate a field boundary distribution in an ellipse-shaped local region. The local belief function allows us to predict UTF boundary trends and establish an adjacent ellipse for further exploration. The process is governed by a depth-first search process until UTF is approximately enclosed by connected ellipses when the boundary coverage process ends. We formally prove that our boundary coverage process guarantees the enclosure above a given coverage ratio with a preset probability threshold

Adaptive Verhaltensentscheidung und Bahnplanung für kognitive Automobile

In der vorliegenden Arbeit wird eine biologisch motivierte Verhaltenssteuerung für ein kognitives Automobil vorgestellt, die ein hohes Maß an Rückmeldung liefert und damit die Grundlage für zukünftige Lernverfahren bildet. Weiterhin wird eine universell einsetzbare Bahnplanungskomponente entwickelt. Wesentliches Element des vorgestellten Ansatzes ist die Repräsentation von Fahrintention und Fahrzeugumfeld mit Hilfe von dynamischen Gefahrenkarten

Planung kooperativer Fahrmanöver für kognitive Automobile

Fahrerassistenzsysteme eröffnen die Möglichkeit für automatische Eingriffe in Gefahrensituationen und bieten dadurch ein Potenzial zur Unfallvermeidung und zur Minimierung der Unfallschwere im Straßenverkehr. Die Handlungen mehrerer kognitiver Fahrzeuge können über Funkkommunikation miteinander koordiniert werden. Diese Dissertation untersucht potenziell echtzeitfähige Bewegungsplanungsalgorithmen zur Planung von Fahrmanövern, die von mehreren Fahrzeugen kooperativ ausgeführt werden können