We present a neural network verification toolbox to 1) assess the probability of satisfaction of a constraint, and 2) modify the set to achieve the probability of satisfaction. Specifically, the tool box establishes with a user-specified level of confidence whether the output of the neural network for a given input distribution is likely to be contained within a given set. Should the tool determine that the given set cannot satisfy the likelihood constraint, the tool also implements an approach outlined in this paper to alter the set to ensure that the user-defined satisfaction probability is achieved. The toolbox is comprised of sampling-based approaches which exploit the properties of signed distance function to define set containment.

This paper addresses the problem of optimizing communicated information among heterogeneous, resourceaware robot teams to facilitate their navigation. In such operations, a mobile robot compresses its local map to assist another robot in reaching a target within an uncharted environment. The primary challenge lies in ensuring that the map compression step balances network load while transmitting only the most essential information for effective navigation. We propose a communication framework that sequentially selects the optimal map compression in a task-driven, communicationaware manner. It introduces a decoder capable of iterative map estimation, handling noise through Kalman filter techniques. The computational speed of our decoder allows for a larger compression template set compared to previous methods, and enables applications in more challenging environments. Specifically, our simulations demonstrate a remarkable 98% reduction in communicated information, compared to a framework that transmits the raw data, on a large Mars inclination map and an Earth map, all while maintaining similar planning costs. Furthermore, our method significantly reduces computational time compared to the state-of-the-art approach.

We present Residual Descent Differential Dynamic Game (RD3G), a Newton-based solver for constrained multiagent game-control problems. The proposed solver seeks a local Nash equilibrium for games where agents are coupled through their rewards and state constraints. By maintaining a dynamic set of active constraints, combined with a barrier function on satisfied constraints and a backtracking line search, the proposed method is able to satisfy state constraints while keeping the dimension of the Newton descent direction problem to a minimum. We compare the proposed method against state-of-the-art techniques and showcase the computational benefits of the RD3G algorithm on several example problems. The RD3G is up to$\mathbf{4 X}$faster and has$\mathbf{2 X}$higher convergence rate than existing approaches in higher dimensional games.

Schrodinger Bridges (SBs) are diffusion processes that steer, in finite time, a given initial distribution to another final one while minimizing a suitable cost functional. Although various methods for computing SBs have recently been proposed in the literature, most of these approaches require computationally expensive training schemes, even for solving low-dimensional problems. In this work, we propose an analytic parametrization of a set of feasible policies for steering the distribution of a dynamical system from one Gaussian Mixture Model (GMM) to another. Instead of relying on standard non-convex optimization techniques, the optimal policy within the set can be approximated as the solution of a low-dimensional linear program whose dimension scales linearly with the number of components in each mixture. The proposed method generalizes naturally to more general classes of dynamical systems, such as controllable linear time-varying systems, enabling efficient solutions to multi-marginal momentum SBs between GMMs, a challenging distribution interpolation problem. We showcase the potential of this approach in low-to-moderate dimensional problems such as image-to-image translation in the latent space of an autoencoder, learning of cellular dynamics using multi-marginal momentum SBs, and various other examples. The implementation is publicly available at https://github.com/georgeRapa/GMMflow.

This work studies the behaviors of two large-population teams competing in a discrete environment. The team-level interactions are modeled as a zero-sum game while the agent dynamics within each team is formulated as a collaborative mean-field team problem. Drawing inspiration from the mean-field literature, we first approximate the large-population team game with its infinite-population limit. Subsequently, we construct a fictitious centralized system and transform the infinite-population game to an equivalent zero-sum game between two coordinators. Via a novel reachability analysis, we study the optimality of coordination strategies, which induce decentralized strategies under the original information structure. The optimality of the resulting strategies is established in the original finite-population game, and the theoretical guarantees are verified by numerical examples.

This paper presents Neural Visibility Field (NVF), a novel uncertainty quantification method for Neural Radi-ance Fields (NeRF) applied to active mapping. Our key insight is that regions not visible in the training views lead to inherently unreliable color predictions by NeRF at this region, resulting in increased uncertainty in the synthesized views. To address this, we propose to use Bayesian Networks to composite position-based field uncertainty into ray-based uncertainty in camera observations. Consequently, NVF nat-urally assigns higher uncertainty to unobserved regions, aiding robots to select the most informative next viewpoints. Extensive evaluations show that NVF excels not only in un-certainty quantification but also in scene reconstruction for active mapping, outperforming existing methods. More de-tails can be found at https://sites.google.com/view/nvf-cvpr24/.

Merging into dense highway traffic for an autonomous vehicle is a complex decision-making task, wherein the vehicle must identify a potential gap and coordinate with surrounding human drivers, each of whom may exhibit diverse driving behaviors. Many existing methods consider other drivers to be dynamic obstacles and, as a result, they are incapable of capturing the full intent of the human drivers through this passive planning. In this paper, we propose a novel dual control framework based on Model Predictive Path-Integral control to generate interactive trajectories. This framework incorporates a Bayesian inference approach to actively learn the agents’ parameters, i.e., other drivers’ model parameters. The proposed framework employs a sampling-based approach that is suitable for real-time implementation through the utilization of GPUs. We illustrate the effectiveness of our proposed methodology through comprehensive numerical simulations conducted in both high and low-fidelity simulation scenarios focusing on autonomous on-ramp merging.

This paper addresses the problem of the communication of optimally compressed information for mobile robot path-planning. In this context, mobile robots compress their current local maps to assist another robot in reaching a target in an unknown environment. We propose a framework that sequentially selects the optimal level of compression, guided by the robot’s path, by balancing map resolution and communication cost. Our approach is tractable in close-to-real scenarios and does not necessitate prior environment knowledge. We design a novel decoder that leverages compressed information to estimate the unknown environment via convex optimization with linear constraints and an encoder that utilizes the decoder to select the optimal compression. Numerical simulations are conducted both in a large close-to-real map and a maze map and compared with two alternative approaches. The results confirm the effectiveness of our framework in assisting the robot reach its target by reducing transmitted information, on average, by approximately 50%, while maintaining satisfactory performance.

In this work, we propose the Informed Batch Belief Trees (IBBT) algorithm for motion planning under motion and sensing uncertainties. The original stochastic motion planning problem is divided into a deterministic motion planning problem and a graph search problem. First, we solve the deterministic planning problem using Rapidly-exploring Random Graph (RRG) to construct a nominal trajectory graph. Then, an informed cost-to-go heuristic for the original problem is computed based on the nominal trajectory graph. Finally, we grow a belief tree by searching the graph using the proposed heuristic. IBBT interleaves batch state sampling, nominal trajectory graph construction, heuristic computing, and searching over the graph to find belief space motion plans. IBBT is an anytime, incremental algorithm. With an increasing number of batches of samples added to the graph, the algorithm finds improved plans. IBBT is efficient by reusing results between sequential iterations. The belief tree search is an ordered search guided by an informed heuristic. We test IBBT in different planning environments. Our numerical investigation confirms that IBBT finds non-trivial motion plans and is faster compared with previous similar methods.

We present BuzzRacer, a palm-sized autonomous vehicle platform suitable for multi-agent autonomous racing. BuzzRacer consists of two parts. First, a software framework with multiple racetrack environments, dynamic simulation, visualization, and control pipelines. Second, a miniature autonomous vehicle platform capable of 1g acceleration and 3.5m/s top speed. BuzzRacer is an open-source project currently used at Georgia Tech in a project-based robotics course and research projects for experimental validation and benchmarking of novel planning and control algorithms.

A new belief space planning algorithm, called covariance steering Belief RoadMap (CS-BRM), is introduced, analyzed, and numerically and experimentally tested. CS-BRM is a multi-query algorithm for motion planning for dynamical systems under simultaneous motion and observation uncertainties. CS-BRM extends the probabilistic roadmap (PRM) approach to belief spaces based on the recently developed theory of covariance steering (CS) that enables guaranteed satisfaction of terminal belief constraints in finite time. The nodes in the CS-BRM are sampled in the belief space and represent distributions of the system states. A covariance steering controller steers the system from one BRM node to another, thus acting as an edge controller of the corresponding belief graph that ensures belief constraint satisfaction. After the edge controller is computed, a specific edge cost is assigned to that edge. The CS-BRM algorithm allows the sampling of non-stationary belief nodes and thus is able to explore the velocity space and find much more efficient trajectories than previous BRM methods. The performance of CS-BRM is evaluated and compared to previous belief space planning approaches using several numerical examples and experimental demonstrations, illustrating the benefits of the proposed approach.

Sampling-based algorithms solve the path planning problem by generating random samples in the searchspace and incrementally growing a connectivity graph or a tree. Conventionally, the sampling strategy used in these algorithms is biased towards exploration to acquire information about the search-space. In contrast, this work proposes an optimization-based procedure that generates new samples so as to improve the cost-to-come value of vertices in a given neighborhood. The application of the proposed algorithm adds an exploitativebias to sampling and results in a faster convergence to the optimal solution compared to other state-of-the-art sampling techniques. This is demonstrated using benchmarking experiments performed for 7 DOF Panda and 14 DOF Baxter robots.

In this paper, we develop an approach that enables autonomous robots to build and compress semantic environment representations from point-cloud data. Our approach builds a three-dimensional, semantic tree representation of the environment from raw sensor data which is then compressed by a novel information-theoretic tree-pruning approach. The proposed approach is probabilistic and incorporates the uncertainty in semantic classification inherent in real-world environments. Moreover, our approach allows robots to prioritize individual semantic classes when generating the compressed trees, so as to design multi-resolution representations that retain the relevant semantic information while simultaneously discarding unwanted semantic categories. We demonstrate the approach by compressing semantic octree models of a large outdoor, semantically rich, real-world environment. In addition, we show how the octree abstractions can be used to create semantically-informed graphs for motion planning, and provide a comparison of our approach with uninformed graph construction methods such as Halton sequences.

In this paper, we present a novel Model Predictive Control method for autonomous robot planning and control subject to arbitrary forms of uncertainty. The proposed Risk-Aware Model Predictive Path Integral (RA-MPPI) control utilizes the Conditional Value-at-Risk (CVaR) measure to generate optimal control actions for safety-critical robotic applications. Different from most existing Stochastic MPCs and CVaR optimization methods that linearize the original dynamics and formulate control tasks as convex programs, the proposed method directly uses the original dynamics without restricting the form of the cost functions or the noise. We apply the novel RA-MPPI controller to an autonomous vehicle to perform aggressive driving maneuvers in cluttered environments. Our simulations and experiments show that the proposed RA-MPPI controller can achieve similar lap times with the baseline MPPI controller while encountering significantly fewer collisions. The proposed controller performs online computation at an update frequency of up to 80 Hz, utilizing modern Graphics Processing Units (GPUs) to multi-thread the generation of trajectories as well as the CVaR values.

We propose factor graph optimization for simultaneous planning, control, and trajectory estimation for collision-free navigation of autonomous systems in environments with moving objects. The proposed online probabilistic motion planning and trajectory estimation navigation technique generates optimal collision-free state and control trajectories for autonomous vehicles when the obstacle motion model is both unknown and known. We evaluate the utility of the algorithm to support future autonomous robotic space missions.

This paper presents a novel control approach for autonomous systems operating under uncertainty. We combine Model Predictive Path Integral (MPPI) control with Covariance Steering (CS) theory to obtain a robust controller for general nonlinear systems. The proposed Covariance-Controlled Model Predictive Path Integral (CC-MPPI) controller addresses the performance degradation observed in some MPPI implementations owing to unexpected disturbances and uncertainties. Namely, in cases where the environment changes too fast or the simulated dynamics during the MPPI rollouts do not capture the noise and uncertainty in the actual dynamics, the baseline MPPI implementation may lead to divergence. The proposed CC-MPPI controller avoids divergence by controlling the dispersion of the rollout trajectories at the end of the prediction horizon. Furthermore, the CC-MPPI has adjustable trajectory sampling distributions that can be changed according to the environment to achieve efficient sampling. Numerical examples using a ground vehicle navigating in challenging environments demonstrate the proposed approach.

A new belief space planning algorithm, called covariance steering Belief RoadMap (CS-BRM), is introduced, which is a multi-query algorithm for motion planning of dynamical systems under simultaneous motion and observation uncertainties. CS-BRM extends the probabilistic roadmap (PRM) approach to belief spaces and is based on the recently developed theory of covariance steering (CS) that enables guaranteed satisfaction of terminal belief constraints in finitetime. The CS-BRM algorithm allows the sampling of non-stationary belief nodes, and thus is able to explore the velocity space and find efficient motion plans. We evaluate CS-BRM in different planning problems and demonstrate the benefits of the proposed approach.

We present an incremental search algorithm, called Lifelong-GLS, which combines the vertex efficiency of Lifelong Planning A* (LPA*) and the edge efficiency of Generalized Lazy Search (GLS) for efficient replanning on dynamic graphs where edge evaluation is expensive. We use a lazily evaluated LPA* to repair the cost-to-come inconsistencies of the relevant region of the current search tree based on the previous search results, and then we restrict the expensive edge evaluations only to the current shortest subpath as in the GLS framework. The proposed algorithm is complete and correct in finding the optimal solution in the current graph, if one exists. We also show the efficiency of the proposed algorithm compared to the standard LPA* and the GLS algorithms over consecutive search episodes in a dynamic environment.

Both geometric and semantic information of the search space are imperative for a good plan. We encode those properties in a weighted colored graph (geometric information in terms of edge weight and semantic information in terms of edge and vertex color) and propose a generalized A∗to find the shortest path among the set of paths with minimal inclusion of low-ranked color edges. We prove the completeness and optimality of this Class-Ordered A∗(COA∗) algorithm with respect to the hereto defined notion of optimality. The utility of COA∗is numerically validated in a ternary graph with feasible, infeasible, and unknown vertices and edges for the cases of a 2D mobile robot, a 3D robotic arm, and a 5D robotic arm with limited sensing capabilities. We compare the results of COA∗to that of the regular A∗algorithm, the latter of which finds a shortest path regardless of the semantic information, and we show that the COA∗dominates the A∗solution in terms of finding less uncertain paths.

We explore the use of policy approximations to reduce the computational cost of learning Nash equilibria in zero-sum stochastic games. We propose a new Q-learning type algorithm that uses a sequence of entropy-regularized soft policies to approximate the Nash policy during the Q-function updates. We prove that under certain conditions, by updating the entropy regularization, the algorithm converges to a Nash equilibrium. We also demonstrate the proposed algorithm's ability to transfer previous training experiences, enabling the agents to adapt quickly to new environments. We provide a dynamic hyper-parameter scheduling scheme to further expedite convergence. Empirical results applied to a number of stochastic games verify that the proposed algorithm converges to the Nash equilibrium, while exhibiting a major speed-up over existing algorithms.

Replanning is an essential problem for robots operating in a dynamic and complex environment for responsive and robust autonomy. Previous incremental-search algorithms efficiently reuse existing search results to facilitate a new plan when the environment changes. Yet, they rely solely on geometric information of the environment encoded in an edge-weighted graph. However, semantic information often provides valuable insights that cannot easily be captured quantitatively. We encode both semantic and geometric information of the environment in a weighted colored graph, in which the edges are partitioned into a finite set of ordered semantic classes (e.g., colors), and then we incrementally search for the shortest path among the set of paths with minimal inclusion of inferior classes, using information from the previous search using ideas similar to LPA*. The proposed Class-Ordered LPA* (COLPA*) algorithm inherits the strong theoretical properties of LPA*, namely, optimality and efficiency, but optimality now is with respect to the total path order. Numerical examples show that semantic information helps reduce the relevant search space in a dynamic environment.

Sampling-based motion planning algorithms such as RRT* are well-known for their ability to quickly find an initial solution and then converge to the optimal solution asymptotically as the number of samples tends to infinity. However, the convergence rate can be slow for high-dimensional planning problems, particularly for dynamical systems where the sampling space is not just the configuration space but the full state space. In this paper, we introduce the idea of using a partial-final-state-free (PFF) optimal controller in kinodynamic RRT* [1] to reduce the dimensionality of the sampling space. Instead of sampling the full state space, the proposed accelerated kinodynamic RRT*, called Kino-RRT*, only samples part of the state space, while the rest of the states are selected by the PFF optimal controller. We also propose a delayed and intermittent update of the optimal arrival time of all the edges in the RRT* tree to decrease the computation complexity. We tested the proposed algorithm using 4-D and 10-D state-space linear systems and showed that Kino-RRT* converges much faster than the kinodynamic RRT* algorithm.

We present a multi-scale forward search algorithm for distributed agents to solve single-query shortest path planning problems. Each agent first builds a representation of its own search space of the common environment as a multi-resolution graph, it communicates with the other agents the result of its local search, and it uses received information from other agents to refine its own graph and update the local inconsistency conditions. As a result, all agents attain a common subgraph that includes a provably optimal path in the most informative graph available among all agents, if one exists, without necessarily communicating the entire graph. We prove the completeness and optimality of the proposed algorithm, and present numerical results supporting the advantages of the proposed approach.

Asymptotically optimal sampling-based planners require an intelligent exploration strategy to accelerate convergence. After an initial solution is found, a necessary condition for improvement is to generate new samples in the so-called "Informed Set". However, Informed Sampling can be ineffective in focusing search if the chosen heuristic fails to provide a good estimate of the solution cost. This work proposes an algorithm to sample the "Relevant Region" instead, which is a subset of the Informed Set. The Relevant Region utilizes cost-to-come information from the planner's tree structure, reduces dependence on the heuristic, and further focuses the search. Benchmarking tests in uniform and general cost-space settings demonstrate the efficacy of Relevant Region sampling.

Sampling-based planning has become a de facto standard for complex robots given its superior ability to rapidly explore high-dimensional configuration spaces. Most existing optimal sampling-based planning algorithms are sequential in nature and cannot take advantage of wide parallelism available on modern computer hardware. Further, tight synchronization of exploration and exploitation phases in these algorithms limits sample throughput and planner performance. Policy Iteration RRT# (PI-RRT#) exposes fine-grained parallelism during the exploitation phase, but this parallelism has not yet been evaluated using a concrete implementation. We first present a novel GPU implementation of PI-RRT#'s exploitation phase and discuss data structure considerations to maximize parallel performance. Our implementation achieves 3-4× speedup over a serial PI-RRT# implementation for a 77.9% decrease in overall planning time on average. As a second contribution, we introduce the Batched-Extension RRT# algorithm, which loosens the synchronization present in PI-RRT# to realize independent 12.97× and 12.54× speedups under serial and parallel exploitation, respectively.

This paper addresses the trajectory planning problem for autonomous vehicles in traffic. We build a stochastic Markov decision process (MDP) model to represent the behaviors of the vehicles. This MDP model takes into account the road geometry and is able to reproduce more diverse driving styles. We introduce a new concept, namely, the “dynamic cell,” to dynamically modify the state of the traffic according to different vehicle velocities, driver intents (signals), and the sizes of the surrounding vehicles (i.e., truck, sedan, and so on). We then use Bézier curves to plan smooth paths for lane switching. The maximum curvature of the path is enforced via certain design parameters. By designing suitable reward functions, different desired driving styles of the intelligent vehicle can be achieved by solving a reinforcement learning problem. The desired driving behaviors (i.e., autonomous highway overtaking) are demonstrated with an in-house developed traffic simulator.

In this article, we develop a framework to obtain graph abstractions for decision-making where the abstractions emerge as a function of the agent's available resources. We discuss the connection of the proposed approach with information-theoretic signal compression and formulate a novel optimization problem to obtain tree-based abstractions that are a function of the agent's computational resources. The structural properties of the new problem are discussed in detail and two algorithmic approaches are proposed. We discuss the quality of, and prove relationships between, the solutions obtained by the two proposed algorithms. The framework is applied to a variety of environments to obtain hierarchical abstractions.

Efficient exploration of the search space is crucial for faster convergence in sampling-based motion planning. An effective sampling method must first concentrate on quickly finding a good initial solution and then focus the search on regions that can potentially improve the current best solution. In this paper, we propose a non-parametric exploration technique that addresses these challenges. The proposed algorithm prioritizes search by utilizing heuristics. After an initial solution is found, the method generates samples in the “$L_{2} -$informed set”, while leveraging collision data to reduce the number of samples in the obstacle space. We demonstrate the efficiency of the proposed approach with several benchmarking experiments.

This paper studies the decision making problem of autonomous vehicles in traffic. We model the interaction between an autonomous vehicle and the environment as a stochastic Markov decision process (MDP) and consider the driving style of an experienced driver as the target to be learned. The road geometry is taken into consideration in the MDP model in order to incorporate more diverse driving styles. By designing the reward function of the MDP, the desired, driving behavior of the autonomous vehicle is obtained using reinforcement learning. Simulated results demonstrate the desired driving behaviors of an autonomous vehicle.

Motion planning under differential constraints is one of the canonical problems in robotics. State-of-the-art methods evolve around kinodynamic variants of popular sampling-based algorithms, such as Rapidly-exploring Random Trees (RRTs). However, there are still challenges remaining, for example, how to include complex dynamics while guaranteeing optimality. If the open-loop dynamics are unstable, exploration by random sampling in control space becomes inefficient. We describe CL-RRT#, which leverages ideas from the RRT# algorithm and a variant of the RRT algorithm, which generates trajectories using closed-loop prediction. Planning with closed-loop prediction allows us to handle complex unstable dynamics and avoids the need to find computationally hard steering procedures. The search technique presented in the RRT# algorithm allows us to improve the solution quality by searching over alternative reference trajectories. We show the benefits of the proposed approach on an autonomous-driving scenario.

In the development of advanced driver-assist systems (ADAS) for lane-keeping or cornering, one important design objective is to appropriately share the steering control with the driver. The steering behavior of the driver must therefore be well characterized for the design of a high-performance ADAS controller. This paper adopts the well-known two-point visual driver model to characterize the steering behavior of the driver, and conducts a series of field tests to identify the model parameters and validate this model in real-world scenarios. An extended Kalman filter and an unscented Kalman filter are implemented for estimating the driver parameters using either a joint-state estimation algorithm or a dual estimation algorithm. The estimated parameters for different types of drivers are analyzed and compared. The results show that the two-point visual driver model captures realistic driving behavior with time-varying, but not necessarily constant, parameters. A wavelet analysis of the driver steering command shows that distinct driver classes can be identified by analyzing the smoothness of the driver command using the Lipschitz exponents of the recorded signals.

We present several modifications to the previously proposed MSPP algorithm that can speed-up its execution considerably. The MSPP algorithm leverages a multi-scale representation of the environment in n dimensions encoded in tree structure constructed by recursive dyadic partitioning of the search space. We first present a new method to compute the graph neighbors in order to reduce the complexity of each iteration, from O(|V|2) to O(|V| log |V|). We then show how to delay expensive intermediate computations until we know that new information will be required, hence saving time by not operating on information that is never used during the search. Finally, we present a way to remove the very expensive need to calculate a full multi-scale map with the use of sampling and derive an upper bound on the probability of failure as a function of the number of samples.

Many driver models assume that a human driver can be modeled as a linear time-invariant system. Although during specific execution tasks this can be a reasonably good model, in general, this is an unrealistic and quite restrictive assumption for most real-life situations where more complex cognitive functions need to be evoked, such as long-term, deliberative planning, prioritization among several possible alternatives, etc. In this paper we model a human driver as a hybrid controller that switches between long-term, discrete planning tasks and short-term, continuous trajectory tracking tasks. The new driver model is based on the well-known two-point visual driver model, and it uses a model predictive control (MPC) module in the anticipatory control channel to better predict deliberative, human driver actions. We evaluate this model's performance, and compare it with other driver models using numerical simulations. The results show that the proposed new driver model reacts to the variation of the direction angle in the same way as most human drivers do, and outperforms previous similar driver models.

In the development of advanced driver-assist systems (ADAS) for lane-keeping, one important design objective is to appropriately share the steering control with the driver. Hence, the steering behavior of the driver must be well known beforehand. This paper adopts the well-known two-point visual driver model to characterize the steering behavior of the driver, and conducts a series of field tests to identify the model parameters to validate the two-point visual driver model in real scenarios. Both an extended Kalman filter and an unscented Kalman filter are implemented for estimating the unknown driver parameters, using a joint-state estimation algorithm and a dual estimation algorithm, and the results are compared.

Several sampling-based algorithms have been recently proposed that ensure asymptotic optimality. The convergence of these algorithms can be improved if sampling is guided toward the most promising region of the search space where the solution is more likely to be found. In this paper we propose three sample rejection methods that leverage the classification of the samples according to their potential of being part of the optimal solution to guide the exploration of the motion planner to promising regions of the search space. These sampling strategies are a direct by-product of the exploitation phase of the algorithm, which uses a dynamic programming (DP) step while planning on random graphs as, for example, is done in the RRT# algorithm. It is shown that the proposed sampling strategies are able to compute high-quality solutions, much faster than existing algorithms. We provide numerical results and compare the performance of the proposed algorithm with the original RRT# and the RRT* algorithms.

We present a path-planning algorithm that leverages a multi-scale representation of the environment. The algorithm works in n dimensions. The information of the environment is stored in a tree representing a recursive dyadic partitioning of the search space. The information used by the algorithm is the probability that a node of the tree corresponds to an obstacle in the search space. The complexity of the proposed algorithm is analyzed and its completeness is shown.

We propose a machine learning (ML)-inspired approach to estimate the relevant region of a motion planning problem during the exploration phase of sampling-based path-planners. The algorithm guides the exploration so that it draws more samples from the relevant region as the number of iterations increases. The approach works in two steps: first, it predicts if a given sample is collision-free (classification phase) without calling the collision-checker, and it then estimates if it is a promising sample, i.e., if it has the potential to improve the current best solution (regression phase), without solving the local steering problem. The proposed exploration strategy is integrated to the RRT#algorithm. Numerical simulations demonstrate the efficiency of the proposed approach.

This paper considers optimal control of dynamical systems which are represented by nonlinear stochastic differential equations. It is well-known that the optimal control policy for this problem can be obtained as a function of a value function that satisfies a nonlinear partial differential equation, namely, the Hamilton-Jacobi-Bellman equation. This nonlinear PDE must be solved backwards in time, and this computation is intractable for large scale systems. Under certain assumptions, and after applying a logarithmic transformation, an alternative characterization of the optimal policy can be given in terms of a path integral. Path Integral (PI) based control methods have recently been shown to provide elegant solutions to a broad class of stochastic optimal control problems. One of the implementation challenges with this formalism is the computation of the expectation of a cost functional over the trajectories of the unforced dynamics. Computing such expectation over trajectories that are sampled uniformly may induce numerical instabilities due to the exponentiation of the cost. Therefore, sampling of low-cost trajectories is essential for the practical implementation of PI-based methods. In this paper, we use incremental sampling-based algorithms to sample useful trajectories from the unforced system dynamics, and make a novel connection between Rapidly-exploring Random Trees (RRTs) and information-theoretic stochastic optimal control. We show the results from the numerical implementation of the proposed approach to several examples.

In this work, we revisit the continuous-time Differential Dynamic Programming (DDP) approach for solving optimal control problems with terminal state constraints. We derive two algorithms, each for different order of expansion of the system dynamics and we investigate their performance in terms of their convergence speed. Compared to previous work, we provide a set of backward differential equations for the value function expansion by relaxing the assumption that the initial nominal control must be very close to the optimal control solution. We apply the derived algorithms to two classical optimal control problems, namely, the inverted pendulum and the Dreyfus rocket problem and show the benefit of second order expansion.

We consider the geometric problem of deciding whether a narrow planar passage can be traversed by a curve that satisfies prespecified upper bounds on its curvature. This problem is of importance for path- and motion-planning of autonomous mobile robots, particularly when vehicle dynamical constraints are considered during planning. For a special case of narrow passages, namely, rectangular channels, we present a fast numerical algorithm to determine if a given channel may be traversed via curvature-bounded paths. We demonstrate that the proposed algorithm can affirm traversability in cases where the most recent result in the literature fails.

Several variants of incremental sampling-based algorithms have been recently proposed in order to optimally solve motion planning problems. Popular examples include the RRT* and the PRM* algorithms. These algorithms are asymptotically optimal and thus provide high quality solutions. However, the convergence rate to the optimal solution may still be slow. Borrowing from ideas used in the well-known LPA* algorithm, in this paper we present a new incremental sampling-based motion planning algorithm based on Rapidly-exploring Random Graphs (RRG), denoted by RRT#(RRT “sharp”), which also guarantees asymptotic optimality, but, in addition, it also ensures that the constructed spanning tree rooted at the initial state contains lowest-cost path information for vertices which have the potential to be part of the optimal solution. This implies that the best possible solution is readily computed if there are some vertices in the current graph that are already in the goal region.

Motion planners for autonomous vehicles often involve a two-level hierarchical structure consisting of a high-level, discrete planner and a low-level trajectory generation scheme. To ensure compatibility between these two levels of planning, we previously introduced a motion planning framework based on multiple-edge transition costs in the graph used by the discrete planner. This framework is enabled by a special local trajectory generation problem, which we address in this paper. In particular, we discuss a trajectory planner based on model predictive control for complex vehicle dynamical models. We demonstrate the efficacy of our overall motion planning approach via examples involving non-trivial vehicle models and complex environments, and we offer comparisons of our motion planner with state-of-the-art randomized sampling-based motion planners.

Motion planning for mobile vehicles involves the solution of two disparate subproblems: the satisfaction of high-level logical task specifications and the design of low-level vehicle control laws. A hierarchical solution of these two subproblems is efficient, but it may not ensure compatibility between the high-level planner and the constraints that are imposed by the vehicle dynamics. To guarantee such compatibility, we propose a motion-planning framework that is based on a special interaction between these two levels of planning. In particular, we solve a special shortest path problem on a graph at a higher level of planning, and we use a lower level planner to determine the costs of the paths in that graph. The overall approach hinges on two novel ingredients: a graph-search algorithm that operates on sequences of vertices and a lower level planner that ensures consistency between the two levels of hierarchy by providing meaningful costs for the edge transitions of a higher level planner using dynamically feasible, collision-free trajectories.

We present a path- and motion-planning scheme that is "multiresolution" both in the sense of representing the environment with high accuracy only locally and in the sense of addressing the vehicle kinematic and dynamic constraints only locally. The proposed scheme uses rectangular multiresolution cell decompositions, efficiently generated using the wavelet transform. The wavelet transform is widely used in signal and image processing, with emerging applications in autonomous sensing and perception systems. The proposed motion planner enables the simultaneous use of the wavelet transform in both the perception and in the motion-planning layers of vehicle autonomy, thus potentially reducing online computations. We rigorously prove the completeness of the proposed path-planning scheme, and we provide numerical simulation results to illustrate its efficacy.

Auditory is a convenient and efficient way for Human-Robot Interaction, however implementing a sound source localization system based on TDOA method encounters many problems, such as noise of real environments, and resolution of nonlinear equations, switch between far field and near field and lack of microphones for geometric positioning localization method. In this paper, a new spectral weighting GCC-PHAT method is proposed to deal with noise. Furthermore, the time difference feature of sound source and its spatial distribution are analyzed. Based on prosperities of the distribution, a space grid matching (SGM) algorithm is proposed for localization step, which handles those problems that geometric positioning method faces effectively. Decision tree and valid feature detection algorithm are also proposed to reduce computational complexity and improve performance. Experiments are achieved in real environments on a mobile robot platform, in which 2016 sets of speech data are tested using four microphones in 3D space. More than 95% azimuth localization rate with error less than 5 degrees and approximate 90% horizontal distance localization rate are obtained.

This paper summarizes some recent developments on a new motion planning framework for autonomous vehicles. The main novelties of the current work include: a provably complete multi-resolution path planning scheme using wavelet-based workspace cell decompositions; a general technique for incorporating vehicle dynamic constraints in the geometric path planner; and a local trajectory generation scheme based on model predictive control.

In this paper we consider dynamic shortest path-planning problems on a graph with a single endpoint pair and with potentially changing edge weights over time. Several incremental algorithms exist in the literature that solve this problem, notably among them the Lifelong Planning A* (LPA*) algorithm. Although, in most cases, the LPA* algorithm requires a relatively small number of updates, in some other cases the amount of work required by the LPA* to find the optimal path can be overwhelming. To address this issue, in this paper we propose an extension of the baseline LPA* algorithm, by making efficient use of a multiscale representation of the environment.

Path-planning (equivalently, path-finding) problems are fundamental in many applications, such as transportation, VLSI design, robot navigation, and many more. In this paper, we consider dynamic shortest path-planning problems on a graph with a single endpoint pair and with potentially changing edge weights over time. Several algorithms exist in the literature that solve this problem, notably among them the Lifelong Planning algorithm. The algorithm is an incremental search algorithm that replans the path when there are changes in the environment. In numerical experiments, however, it was observed that the performance of is sensitive in the number of vertex expansions required to update the graph when an edge weight value changes or when a vertex is added or deleted. Although, in most cases, the classical requires a relatively small number of updates, in some other cases the amount of work required by the to find the optimal path can be overwhelming. To address this issue, in this paper, we propose an extension of the baseline algorithm, by making efficient use of a multiscale representation of the environment. This multiscale representation allows one to quickly localize the changed edges, and subsequently update the priority queue efficiently. This incremental multiscale ( for short) algorithm leads to an improvement both in terms of robustness and computational complexity-in the worst case-when compared to the classical . Numerical experiments validate the aforementioned claims.

Mobile robots offer a typical example of systems with nonholonomic constraints. Several controllers have been proposed in the literature for stabilizing these systems. However, few experimental studies have been reported comparing the characteristics and the performance of these controllers with respect to neglected dynamics, quantization, noise, delays, etc. In this paper, we use a Khepera mobile robot to perform experimental comparison of several control laws. Khepera has two dc motor-powered wheels and introduces many realistic difficulties, such as different motor dynamics for the two wheels, time delay, quantization, sensor noise, and saturation. We emphasize the implementation difficulties of two discontinuous controllers proposed herein, and we compare their performance with several other controllers reported in the literature. Ways to improve the performance of each controller are also discussed.