This paper reports on an algorithm for planning trajectories that allow a multirotor micro aerial vehicle (MAV) to quickly identify a set of unknown parameters. In many problems like self calibration or model parameter identification some states are only observable under a specific motion. These motions are often hard to find, especially for inexperienced users. Therefore, we consider system model identification in an active setting, where the vehicle autonomously decides what actions to take in order to quickly identify the model. Our algorithm approximates the belief dynamics of the system around a candidate trajectory using an extended Kalman filter (EKF). It uses sampling-based motion planning to explore the space of possible beliefs and find a maximally informative trajectory within a user-defined budget. We validate our method in simulation and on a real system showing the feasibility and repeatability of the proposed approach. Our planner creates trajectories which reduce model parameter convergence time and uncertainty by a factor of four.

In this paper, we introduce an informative path planning (IPP) framework for active classification using unmanned aerial vehicles (UAVs). Our algorithm uses a combination of global viewpoint selection and evolutionary optimization to refine the planned trajectory in continuous 3D space while satisfying dynamic constraints. Our approach is evaluated on the application of weed detection for precision agriculture. We model the presence of weeds on farmland using an occupancy grid and generate adaptive plans according to information-theoretic objectives, enabling the UAV to gather data efficiently. We validate our approach in simulation by comparing against existing methods, and study the effects of different planning strategies. Our results show that the proposed algorithm builds maps with over 50% lower entropy compared to traditional “lawnmower” coverage in the same amount of time. We demonstrate the planning scheme on a multirotor platform with different artificial farmland set-ups.

Multirotor unmanned aerial vehicles (UAVs) are rapidly gaining popularity for many applications. However, safe operation in partially unknown, unstructured environments remains an open question. In this paper, we present a continuous-time trajectory optimization method for real-time collision avoidance on multirotor UAVs. We then propose a system where this motion planning method is used as a local replanner, that runs at a high rate to continuously recompute safe trajectories as the robot gains information about its environment. We validate our approach by comparing against existing methods and demonstrate the complete system avoiding obstacles on a multirotor UAV platform.

This paper reports on a data-driven motion planning approach for interaction-aware, socially-compliant robot navigation among human agents. Autonomous mobile robots navigating in workspaces shared with human agents require motion planning techniques providing seamless integration and smooth navigation in such. Smooth integration in mixed scenarios calls for two abilities of the robot: predicting actions of others and acting predictably for them. The former requirement requests trainable models of agent behaviors in order to accurately forecast their actions in the future, taking into account their reaction on the robot's decisions. A human-like navigation style of the robot facilitates other agents-most likely not aware of the underlying planning technique applied-to predict the robot motion vice versa, resulting in smoother joint navigation. The approach presented in this paper is based on a feature-based maximum entropy model and is able to guide a robot in an unstructured, real-world environment. The model is trained to predict joint behavior of heterogeneous groups of agents from onboard data of a mobile platform. We evaluate the benefit of interaction-aware motion planning in a realistic public setting with a total distance traveled of over 4 km. Interestingly the motion models learned from human-human interaction did not hold for robot-human interaction, due to the high attention and interest of pedestrians in testing basic braking functionality of the robot.

Real-world autonomous driving in city traffic must cope with dynamic environments including other agents with uncertain intentions. This poses a challenging decision-making problem, e.g., deciding when to perform a passing maneuver or how to safely merge into traffic. Previous work in the literature has typically approached the problem using ad-hoc solutions that do not consider the possible future states of other agents, and thus have difficulty scaling to complex traffic scenarios where the actions of participating agents are tightly conditioned on one another. In this paper we present multipolicy decision-making (MPDM), a decision-making algorithm that exploits knowledge from the autonomous driving domain to make decisions online for an autonomous vehicle navigating in traffic. By assuming the controlled vehicle and other traffic participants execute a policy from a set of plausible closed-loop policies at every timestep, the algorithm selects the best available policy for the controlled vehicle to execute. We perform policy election using forward simulation of both the controlled vehicle and other agents, efficiently sampling from the high-likelihood outcomes of their interactions. We then score the resulting outcomes using a user-defined cost function to accommodate different driving preferences, and select the policy with the highest score. We demonstrate the algorithm on a real-world autonomous vehicle performing passing maneuvers and in a simulated merging scenario.

We present a framework for planning collision-free paths online for autonomous underwater vehicles (AUVs) in unknown environments. It is composed of three main modules (mapping, planning and mission handler) that incrementally explore the environment while solving start-to-goal queries. We use an octree-based representation of the environment and we extend the optimal rapidly-exploring random tree (RRT*) using concepts of anytime algorithms and lazy collision evaluation, thus including the capability to replan paths according to nearby obstacles perceived during the execution of the mission. To validate our approach, we plan paths for the SPARUS-II AUV, a torpedo-shaped vehicle performing autonomous missions in a 2-dimensional workspace. We demonstrate its feasibility with the SPARUS-II AUV in both simulation and real-world in-water trials.

This paper reports on a Gaussian belief-space planning formulation for mobile robots that includes random measurement acquisition variables that model whether or not each measurement is actually acquired. We show that maintaining the stochasticity of these variables in the planning formulation leads to a random belief covariance matrix, allowing us to consider the risk associated with the acquisition in the objective function. Inspired by modern portfolio theory and utility optimization, we design objective functions that are risk-averse, and show that risk-averse planning leads to decisions made by the robot that are desirable when operating under uncertainty. We show the benefit of this approach using simulations of a planar robot traversing an uncertain environment and of an underwater robot searching for loop-closure actions while performing visual SLAM.

This paper reports on an algorithm to support autonomous vehicles in reasoning about occluded regions of their environment to make safe, reliable decisions. In autonomous driving scenarios, other traffic participants are often occluded from sensor measurements by buildings or large vehicles like buses or trucks, which makes tracking dynamic objects challenging.We present a method to augment standard dynamic object trackers with means to 1) estimate the occluded state of other traffic agents and 2) robustly associate the occluded estimates with new observations after the tracked object reenters the visible region of the sensor horizon. We perform occluded state estimation using a dynamics model that accounts for the driving behavior of traffic agents and a hybrid Gaussian mixture model (hGMM) to capture multiple hypotheses over discrete behavior, such as driving along different lanes or turning left or right at an intersection. Upon new observations, we associate them to existing estimates in terms of the Kullback-Leibler divergence (KLD). We evaluate the proposed method in simulation and using a real-world traffic-tracking dataset from an autonomous vehicle platform. Results show that our method can handle significantly prolonged occlusions when compared to a standard dynamic object tracking system.

This paper reports on a system for dynamic obstacle tracking for autonomous vehicles. In this work, we seek to simultaneously estimate both the trajectory of the obstacle and the obstacle's shape. These two tasks are inherently coupled-given only noisy partial views, one cannot accurately estimate the trajectory of an obstacle if its shape is unknown, nor can one estimate its shape without knowing its trajectory. To address this challenge, we note that simultaneous localization and mapping (SLAM), where a robot must build a map of the environment while localizing itself within the map, presents similar challenges. By treating the obstacle's shape as a “map” in the obstacle's moving reference frame, we can formulate the obstacle tracking and shape estimation similarly to SLAM. Additionally, we use a continuous time estimation framework to incorporate sensor data that is collected at a fast rate (e.g., light detection and ranging (LIDAR)). Using these methods, we are able to obtain smooth trajectories and crisp point clouds for tracked obstacles. We test our proposed tracker on real-world data collected by our autonomous vehicle platform and demonstrate that it produces improved results when compared to a standard centroid-based extended Kalman filter (EKF) tracker.

This paper reports on the inclusion of a probabilistic channel model within a cooperative localization planning framework. Underwater cooperative localization reduces positioning errors by sharing sensor data across a team of underwater vehicles. Relative range constraints between vehicles are measured by the one-way-travel-time of successfully received acoustic communication broadcasts. The quality of the navigation solution is intimately linked to the geometry of the network and, therefore, can benefit from planning informative relative trajectories. We cast this planning problem as an instance of belief space planning. In order to weight packet loss over the acoustic channel, we introduce a probabilistic channel model into the planning framework. We propose an optimization algorithm that allows us to plan open-loop control actions and, by extension, closed-loop parameterized trajectories.

This paper proposes an integrated motion planning and control approach for autonomous car navigation. Existing approaches to autonomous vehicle navigation typically plan a trajectory and pass it on to a steering controller that commands steering wheel angle (SWA) or curvature at every timestep to minimize tracking error. However, this approach exhibits large amounts of control effort, and ignores other criteria such as smoothness or the importance of staying on plan at different times. Conversely, our proposed approach leverages the concept of potential fields to represent a driving corridor with a desired tracking error tolerance and direct torque-based steering control to smoothly steer the vehicle with a much smaller control effort. Further, using potential fields allows us to naturally incorporate obstacles in the driving corridor to circumvent them, with typically no need for explicit trajectory planning. We compare our approach to a standard steering controller in experiments with a real-world autonomous vehicle platform. Results show that our proposed approach achieves similar path tracking performance as a high-gain SWA controller, but with much less actuator effort.

We present a novel method for planning 3D coverage paths for inspection of complex structures on the ocean floor (such as seamounts or coral reefs) using an autonomous underwater vehicle (AUV). Our method initially uses an a priori map to plan a nominal coverage path that allows the AUV to pass its sensors over all points on the target structure. We then go beyond previous approaches in the literature by considering the vehicle's position uncertainty rather than relying on the unrealistic assumption of an idealized path execution. To this aim, we present a replanning algorithm based on stochastic trajectory optimization that reshapes the nominal path to cope with the actual target structure perceived in situ. The replanning algorithm runs onboard the AUV in realtime during the inspection mission, adapting the path according to the measurements provided by the vehicle's range sensing sonars. We demonstrate the efficacy of our method in experiments at sea using the GIRONA 500 AUV where we cover a concrete block of a breakwater structure in a harbor and an underwater boulder rising from 40 m up to 27 m depth. Moreover, we apply state-of-the-art surface reconstruction techniques to the data acquired by the AUV and obtain 3D models of the inspected structures that show the benefits of our planning method for 3D mapping.

This paper proposes a coverage path planning (CPP) method for inspection of 3D natural structures on the ocean floor charted as 2.5D bathymetric maps. This task is integral to many marine robotics applications, such as microbathymetry mapping and image photo-mosaicing. We consider an autonomous underwater vehicle (AUV) with hovering capabilities imaging the ocean floor with an orientable sensor, such as a camera or a sonar. While standard lawnmower-type surveys at constant altitude are well-suited for covering effectively planar areas, two major problems arise when tracing such paths over high-relief terrain. First, the sudden depth changes required by such paths imply very costly motions, as moving in the vertical axis is expensive for most AUVs. Second, some time is required to adjust the vehicle depth after a sudden change in the relief, resulting in a varying distance from the target surface which deteriorates the overall quality of the collected imaging data. The method proposed in this paper accounts for these facts and generates different coverage patterns according to terrain's relief, resulting in a well-suited coverage path for imaging tasks. The proposed CPP method is fast and easy to implement, and provides a valuable tool for planning coverage paths in marine environments. We tested the proposed method on a real-world bathymetric dataset of a lava tongue obtained during recent sea trials in the Santorini caldera in Greece and compares favorably to a standard lawnmower-type survey path.

Highly articulated robot locomotion systems, such as snake robots, present special motion planning challenges. They possess many degrees of freedom, and therefore are modeled by a high dimensional configuration space which must be searched to plan a path. Kinematic and dynamic constraints further complicate the selection of effective controls. Finally, snake robots often have multiple modes of interaction with the terrain as contacts are made and broken, leading to complex and imperfect motion models. We believe that the space of useful controls that provides desirable motions, however, is much smaller. Useful net motions for such systems are often generated via gaits, or cyclic motions in the shape space. Gaits transform a high-dimensional continuum search into a relatively tractable discrete search. In this paper, we put forward a framework which allows a planner to generate paths in a low dimensional work space and select among gaits, pre-planned motions in the robot's shape space. The contribution of this paper rests on the “virtual chassis” which is a choice of body frame for the snake robot that allows the planner to efficiently select among and plan with gaits to direct the robot along the work space path. We demonstrate this planner running on a simulated snake robot navigating through a variety of clutter scenarios. The virtual chassis also has the benefit of allowing us to generalize notions of controllability to gait motions.

We present a novel survey path planning technique which minimizes the robot's position uncertainty along the planned path while taking into account area coverage performance. The proposed technique especially targets bathymetric mapping applications and respects application constraints such as the desire to survey in parallel tracks and to avoid turns in the target area to maximize sonar measurements quality. While accounting for uncertainty in the survey planning process can lead to more accurate data products, existing survey planning tools typically ignore it. Our method bridges this gap using the saliency on an a priori map to predict how the terrain will affect the robot's belief at every point on the target area. Based on this magnitude, we provide an algorithm that computes the order in which to trace parallel tracks to cover the target area minimizing the overall uncertainty along the path. A particle filter keeps track of the robot's position uncertainty during the planning process and, in order to find useful loop-closures for mapping, crossing tracks that visit salient locations are added when the uncertainty surpasses a user-provided threshold. We test our method on real-world datasets collected off the coasts of Spain, Greece and Australia. We evaluate the expected robot's position uncertainty along the planned paths and assess their associated mapping performance using a bathymetric mapping algorithm. Results show that our method offers benefits over a standard lawnmower-type path both in terms of position uncertainty and map quality.

Coverage path planning is the problem of moving an effector (e.g. a robot, a sensor) over all points in a given region. In marine robotics, a number of applications require to cover a region on the seafloor while navigating above it at a constant depth. This is the case of Autonomous Surface Vehicles, that always navigate at the water surface level, but also of several Autonomous Underwater Vehicle tasks as well. Most existing coverage algorithms sweep the free space in the target region using lawnmower-like back-and-forth motions, and the inter-lap spacing between these back-and-forth laps is determined by the robot's sensor coverage range. However, while covering the seafloor surface by navigating above it at a constant depth, the sensor's field of view varies depending on the seafloor height. Therefore, to ensure full coverage one would need to use the inter-lap spacing determined by the shallowest point on the target surface, resulting in undesired coverage overlapping among the back-and-forth laps. In this work, we propose a novel method to generate a coverage path that completely covers a surface of interest on the seafloor by navigating in a constant-depth plane above it. The proposed method uses environment information to minimize the coverage overlapping by segmenting the target surface in regions of similar depth features and addressing them as individual coverage path planning problems. A cell decomposition coverage method is applied to each region. The surface gradient is used to determine the best sweep orientation in each cell, and the inter-lap spacing in the lawnmower-like paths used to cover each cell is maximized on a lap-by-lap basis, hence obtaining a shorter, more efficient coverage path. The proposal is validated in simulation experiments conducted with a real-world bathymetric dataset that show a significant increase on path efficiency in comparison with a standard boustrophedon coverage path.

This paper proposes a field application of a high-level Reinforcement Learning (RL) control system for solving the action selection problem of an autonomous robot in a cable tracking task. The underwater vehicle ICTINEUAUVlearns to perform a visual based cable tracking task in a two step learning process. First, a policy is computed by means of simulation where a hydrodynamic model of the vehicle simulates the cable following task. Once the simulated results are accurate enough, in a second step, the learned-in-simulation policy is transferred to the vehicle where the learning procedure continues in a real environment, improving the initial policy. The natural actor-critic (NAC) algorithm has been selected to solve the problem in both steps. This algorithm aims to take advantage of policy gradient and value function techniques for fast convergence. Actor's policy gradient gives convergence guarantees under function approximation and partial observability while critic's value function reduces variance of the estimates update improving the convergence process.