Non-convex Obstacles Research Articles

Reactive collision-free motion generation in joint space via dynamical systems and sampling-based MPC

Dynamical system (DS) based motion planning offers collision-free motion, with closed-loop reactivity thanks to their analytical expression. It ensures that obstacles are not penetrated by reshaping a nominal DS through matrix modulation, which is constructed using continuously differentiable obstacle representations. However, state-of-the-art approaches may suffer from local minima induced by non-convex obstacles, thus failing to scale to complex, high-dimensional joint spaces. On the other hand, sampling-based Model Predictive Control (MPC) techniques provide feasible collision-free paths in joint-space, yet are limited to quasi-reactive scenarios due to computational complexity that grows cubically with space dimensionality and horizon length. To control the robot in the cluttered environment with moving obstacles, and to generate feasible and highly reactive collision-free motion in robots’ joint space, we present an approach for modulating joint-space DS using sampling-based MPC. Specifically, a nominal DS representing an unconstrained desired joint space motion to a target is locally deflected with obstacle-tangential velocity components navigating the robot around obstacles and avoiding local minima. Such tangential velocity components are constructed from receding horizon collision-free paths generated asynchronously by the sampling-based MPC. Notably, the MPC is not required to run constantly, but only activated when the local minima is detected. The approach is validated in simulation and real-world experiments on a 7-DoF robot demonstrating the capability of avoiding concave obstacles, while maintaining local attractor stability in both quasi-static and highly dynamic cluttered environments.

A powered descent trajectory planning method with quantitative consideration of safe distance to obstacle

High scientific value areas on celestial bodies such as the Moon and Mars are often located in hazardous terrains. To achieve safe landing exploration, a novel planning method is proposed, which can ensure that the planned trajectory maintains a user-specified distance from obstacles, thus reducing potential collision risk induced by factors such as the body size and model uncertainties. Firstly, the basic model of the trajectory optimization problem and its convexification version is given. The obstacles are modeled as polynomial functions, based on which the “if-then” obstacle avoidance logic explicitly considering the safe distance, is described as a sum of squares constraint. This constraint formulation applies to any obstacle described by a finite number of polynomials, independent of the specific expression of the polynomials (reflecting the shape of the obstacle). Subsequently, the convexification process for the obstacle avoidance constraint is given. Finally, the sequential sum of squares programming problem for the obstacle avoidance trajectory is established, which boils down to a series of semidefinite programming problems. Simulation results show that the closest distance between the planned trajectory and obstacles strictly satisfies the specified distance constraint, and the trajectory could avoid non-convex obstacles. With the promising convergence properties of underlying convex optimization algorithms, advanced autonomous obstacle avoidance guidance schemes are expected to be formed based on the proposed trajectory planning method.

On-Manifold Strategies for Reactive Dynamical System Modulation With Nonconvex Obstacles

On-Manifold Strategies for Reactive Dynamical System Modulation With Nonconvex Obstacles

NR-RRT: Neural Risk-Aware Near-Optimal Path Planning in Uncertain Nonconvex Environments

Balancing the trade-off between safety and efficiency is of significant importance for path planning under uncertainty. Many risk-aware path planners have been developed to explicitly limit the probability of collision to an acceptable bound in uncertain environments. However, convex obstacles or Gaussian uncertainties are usually assumed to make the problem tractable in the existing method. These assumptions limit the generalization and application of path planners in real-world implementations. In this article, we propose to apply deep learning methods to the sampling-based planner, developing a novel risk bounded near-optimal path planning algorithm named neural risk-aware RRT (NR-RRT). Specifically, a deterministic risk contours map is maintained by perceiving the probabilistic nonconvex obstacles, and a neural network sampler is proposed to predict the next most-promising safe state. Furthermore, the recursive divide-and-conquer planning and bidirectional search strategies are used to accelerate the convergence to a near-optimal solution with guaranteed bounded risk. Worst-case theoretical guarantees can also be proven owing to a standby safety guaranteed planner utilizing a uniform sampling distribution. Simulation experiments demonstrate that the proposed algorithm outperforms the state-of-the-art remarkably for finding risk bounded low-cost paths in seen and unseen environments with uncertainty and nonconvex constraints.

Iterative On Surface Radiation Conditions for fast and reliable single and multiple scattering analyses of arbitrarily shaped obstacles

In this paper we develop an iterative method to perform single and multiple scattering analyses of both convex and non-convex scatterers at very low computational cost and time. The computational method proposed will resolve the current inaccuracy of the On Surface Radiation Conditions (OSRC) in estimating the scattered field from non-convex obstacles and allows a fast and reliable single- and multiple-scattering analyses in reduced spatial dimensions. We demonstrate the reliability and efficiency of the proposed method in solving two- and three-dimensional time-harmonic acoustic scattering problems. Non-Uniform Rational B-Splines were used to represent the obstacle boundaries providing a seamless connection to geometrical representation. The proposed Iterative On Surface Radiation Conditions (ITOSRC) method tremendously reduces the meshing and analysis time and paves the way for shape and topology optimization of future devices relying on wave prpagation phenomena.

R2: Optimal vector-based and any-angle 2D path planning with non-convex obstacles

A novel vector-based path planner, R2 (R in two dimensions), is introduced in this paper. R2 is optimal and online, returning any-angle paths by applying heuristic costs to vector-based searches. R2 delays line-of-sight checks to expand the most promising path that has the least detours from the start and goal points. As delayed checks can cause severe path cost underestimates, R2 infers the smallest known convex hull, the best hull, of obstacles while moving around them. To construct the best hull, phantom points are introduced, which are imaginary turning points lying on non-convex corners to guide future searches. Tracing rules are introduced to ensure that the estimated path cost from the best hull increases admissibly and monotonically between queues to the open-list. The distance between the start and goal points have little impact on R2’s performance when compared to the number of line-of-sight checks that collide with obstacles. While having an exponential search time in the worst case with respect to the number of collided line-of-sight checks, R2 is much faster than state-of-the-art when the optimal path is expected to turn around few obstacles, especially on large maps with few disjoint and non-convex obstacles.

Trajectory Optimization for the Nonholonomic Space Rover in Cluttered Environments Using Safe Convex Corridors

Due to the limitation of space rover onboard computing resources and energy, there is an urgent need for high-quality drive trajectories in complex environments, which can be provided by delicately designed motion optimization methods. The nonconvexity of the collision avoidance constraints poses a significant challenge to the optimization-based motion planning of nonholonomic vehicles, especially in unstructured cluttered environments. In this paper, a novel obstacle decomposition approach, which swiftly decomposes nonconvex obstacles into their constituent convex substructures while concurrently minimizing the proliferation of resultant subobstacles, is proposed. A safe convex corridor construction method is introduced to formulate the collision avoidance constraints. The numerical approximation methods are applied to transfer the resulting continuous motion optimization problem to a nonlinear programming problem (NLP). Simulation experiments are conducted to illustrate the feasibility and superiority of the proposed methods over the rectangle safe corridor method and the area method.

CACTO: Continuous Actor-Critic With Trajectory Optimization—Towards Global Optimality

This paper presents a novel algorithm for the continuous control of dynamical systems that combines Trajectory Optimization (TO) and Reinforcement Learning (RL) in a single framework. The motivations behind this algorithm are the two main limitations of TO and RL when applied to continuous nonlinear systems to minimize a non-convex cost function. Specifically, TO can get stuck in poor local minima when the search is not initialized close to a “good” minimum. On the other hand, when dealing with continuous state and control spaces, the RL training process may be excessively long and strongly dependent on the exploration strategy. Thus, our algorithm learns a “good” control policy via TO-guided RL policy search that, when used as initial guess provider for TO, makes the trajectory optimization process less prone to converge to poor local optima. Our method is validated on several reaching problems featuring non-convex obstacle avoidance with different dynamical systems, including a car model with 6D state, and a 3-joint planar manipulator. Our results show the great capabilities of CACTO in escaping local minima, while being more computationally efficient than the Deep Deterministic Policy Gradient (DDPG) and Proximal Policy Optimization (PPO) RL algorithms.

Safe deep learning-based global path planning using a fast collision-free path generator

In this research, a global path planning method based on recurrent neural networks by means of a new Loss function is presented, which regardless of the complexity of the configuration space, generates the path in a relatively constant time. The new Loss function is defined in such a way that in addition to learning the input data of the network, it creates an adjustable safety margin around the obstacles and ultimately creates a safe path. Moreover, a new global path planning method is also introduced, which is used to create the dataset required to train the proposed neural network. The convergence of this method is mathematically proven and it is shown that this method can also produce a suboptimal path in a much shorter time than the common methods of global path planning reported in the literature. In short, the main purpose of this research consists in providing a method which can create a suboptimal, fast and safe path for a mobile robot from any random starting point to any random destination in a known environment. First, the proposed methods will be implemented for different two-dimensional environments consisting of convex and non-convex obstacles, considering the robot as a point-mass, and then it will be implemented in a simulation environment, AI2THOR. Compared to classical global path planning algorithms, such as RRT and A*, the proposed approach demonstrates better performance in complex and challenging environments.

Motion Planning Under Uncertainty with Complex Agents and Environments via Hybrid Search

As autonomous systems and robots are applied to more real world situations, they must reason about uncertainty when planning actions. Mission success oftentimes cannot be guaranteed and the planner must reason about the probability of failure. Unfortunately, computing a trajectory that satisfies mission goals while constraining the probability of failure is difficult because of the need to reason about complex, multidimensional probability distributions. Recent methods have seen success using chance-constrained, model-based planning. However, the majority of these methods can only handle simple environment and agent models. We argue that there are two main drawbacks of current approaches to goal-directed motion planning under uncertainty. First, current methods suffer from an inability to deal with expressive environment models such as 3D non-convex obstacles. Second, most planners rely on considerable simplifications when computing trajectory risk including approximating the agent’s dynamics, geometry, and uncertainty. In this article, we apply hybrid search to the risk-bound, goal-directed planning problem. The hybrid search consists of a region planner and a trajectory planner. The region planner makes discrete choices by reasoning about geometric regions that the autonomous agent should visit in order to accomplish its mission. In formulating the region planner, we propose landmark regions that help produce obstacle-free paths. The region planner passes paths through the environment to a trajectory planner; the task of the trajectory planner is to optimize trajectories that respect the agent’s dynamics and the user’s desired risk of mission failure. We discuss three approaches to modeling trajectory risk: a CDF-based approach, a sampling-based collocation method, and an algorithm named Shooting Method Monte Carlo. These models allow computation of trajectory risk with more complex environments, agent dynamics, geometries, and models of uncertainty than past approaches. A variety of 2D and 3D test cases are presented including a linear case, a Dubins car model, and an underwater autonomous vehicle. The method is shown to outperform other methods in terms of speed and utility of the solution. Additionally, the models of trajectory risk are shown to better approximate risk in simulation.

Security-Based Resilient Robust Model Predictive Control for Polytopic Uncertain Systems Subject to Deception Attacks and RR Protocol

This article is concerned with the security-based resilient robust model predictive control (RMPC) problem for a class of discrete-time polytopic uncertain systems with deception attacks and the round-robin (RR) protocol. The well-known RR protocol is adopted with the hope to reduce the communication burden, where the control signal is transmitted only at the given transmission instant. By means of the concept of bounded energy and the independent Bernoulli-distributed (BD) white sequences, a representative attack model is given. A definition of mean-square (MS) security in <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">H</i> <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sub> -sense is employed to reasonably explain the dynamics process of the controlled systems. We aim at designing a set of resilient RMPC controllers so as to make the closed-loop system state under consideration of deception attacks and the RR protocol enter and stay in a certain bounded region. Furthermore, with the aid of stochastic analysis methods and inequality analysis techniques, sufficient conditions are obtained to satisfy the desirable security requirements. To tackle the nonconvex obstacles resulting from attacks and the RR protocol, the cone complementary linearization (CCL) method is exploited to cast them into a convex optimization problem (OP) for its solvability. Then, an online OP regarding a certain upper bound of the concerned objective is provided for the solvability and resilient RMPC-based controller gains are obtained. Finally, two examples, including a high-purity distillation one and a numerical one, are used to demonstrate the effectiveness and the validity of the proposed techniques.

Magnetic-Field-Inspired Navigation for Robots in Complex and Unknown Environments.

Over the course of the past decade, we have witnessed a huge expansion in robotic applications, most notably from well-defined industrial environments into considerably more complex environments. The obstacles that these environments often contain present robotics with a new challenge - to equip robots with a real-time capability of avoiding them. In this paper, we propose a magnetic-field-inspired navigation method that significantly has several advantages over alternative systems. Most importantly, 1) it guarantees obstacle avoidance for both convex and non-convex obstacles, 2) goal convergence is still guaranteed for point-like robots in environments with convex obstacles and non-maze concave obstacles, 3) no prior knowledge of the environment, such as the position and geometry of the obstacles, is needed, 4) it only requires temporally and spatially local environmental sensor information, and 5) it can be implemented on a wide range of robotic platforms in both 2D and 3D environments. The proposed navigation algorithm is validated in simulation scenarios as well as through experimentation. The results demonstrate that robotic platforms, ranging from planar point-like robots to robot arm structures such as the Baxter robot, can successfully navigate toward desired targets within an obstacle-laden environment.

Distributionally Safe Path Planning: Wasserstein Safe RRT

In this paper, we propose a Wasserstein metric-based random path planning algorithm. Wasserstein Safe RRT (W-Safe RRT) provides finite-sample probabilistic guarantees on the safety of a returned path in an uncertain obstacle environment. Vehicle and obstacle states are modeled as distributions based upon state and model observations. We define limits on distributional sampling error so the Wasserstein distance between a vehicle state distribution and obstacle distributions can be bounded. This enables the algorithm to return safe paths with a confidence bound through combining finite sampling error bounds with calculations of the Wasserstein distance between discrete distributions. W-Safe RRT is compared against a baseline minimum encompassing ball algorithm, which ensures balls that minimally encompass discrete state and obstacle distributions do not overlap. The improved performance is verified in a 3D environment using single, multi, and rotating non-convex obstacle cases, with and without forced obstacle error in adversarial directions, showing that W-Safe RRT can handle poorly modeled complex environments.

Nonlinear Model Predictive Control on SE(3) for Quadrotor Aggressive Maneuvers

Applications involving Unmanned Aerial Vehicles (UAVs) have increasingly required faster and more accurate movements to reduce flight time and to improve efficiency in the obstacle avoidance capability. In this context, this work proposes a nonlinear model predictive control (NMPC) strategy formulated on the Special Euclidean group SE(3) for quadrotor trajectory tracking within cluttered environments with unknown obstacles. The approach considers constraints in the states and inputs, with constant disturbance rejection and capable of executing aggressive maneuvers. The UAV attitude is considered as an optimization variable within the control problem thanks to an algebraic ellipsoidal set approach. As a consequence, the collision check takes the UAV attitude into account, allowing aggressive maneuvers. Numerical experiments under realistic conditions allow evaluating the performance of the proposed approach for the UAV. The tested maneuvers are throwing a narrow gap, passing by a nonconvex obstacle gap, avoiding a convex obstacle, and doing slalom movements. In all cases, uncertainties are considered. The achieved results indicate the advantages of executing aggressive maneuvers.

Semi-infinite programming for trajectory optimization with non-convex obstacles

This article presents a novel optimization method that handles collision constraints with complex, non-convex 3D geometries. The optimization problem is cast as a semi-infinite program in which each collision constraint is implicitly treated as an infinite number of numeric constraints. The approach progressively generates some of these constraints for inclusion in a finite nonlinear program. Constraint generation uses an oracle to detect points of deepest penetration, and this oracle is implemented efficiently via signed distance field (SDF) versus point cloud collision detection. This approach is applied to pose optimization and trajectory optimization for both free-flying rigid bodies and articulated robots. Experiments demonstrate performance improvements compared with optimizers that handle only convex polyhedra, and demonstrate efficient collision avoidance between non-convex CAD models and point clouds in a variety of pose and trajectory optimization settings.

On obstacle avoidance path planning in unknown 3D environments: A fluid-based framework

Compared with preprocessed obstacle environments, unknown environments are more challenging for path planning. In unknown environments, an agent can make decisions only by relying on the obstacle information detected by its onboard sensors. However, when facing non-convex obstacles, this limited detection information can easily trap the agent in a local optimum. In this paper, a nature-inspired methodology called Interfered Fluid Dynamic System (IFDS) is extended to anti-local-optimum obstacle avoidance in unknown 3D environments for the first time and a novel fluid-based path planning framework is proposed. First, the detection region of the agent is discretized. Then, spherical virtual obstacles (SVOs) located at detected discrete points are generated and memorized. Thus, obstacle avoidance in unknown environments is transformed into the avoidance of known SVOs. Next, the currently generated and memorized SVOs are input to the core of the framework, the IFDS algorithm, to produce repulsive effects, and the corresponding 3D avoidance path is resolved. On this basis, to address local optimum in cases with non-convex obstacles, and considering compatibility with the IFDS, the direction coefficient and sink-heading angular rate adjustment strategies, which belong to the same system as the IFDS, are introduced to modify the IFDS in this framework. Finally, receding horizon control is introduced to improve the obstacle avoidance performance. Simulations show that the proposed framework can enable the agent to autonomously jump out of the 3D non-convex obstacle environments with typical features of the local optimum, including wall-like and cave-like obstacles, and safely reach the destination.

Application of Swarm Robotic System in a Dynamic Environment using Cohort Intelligence

The nature inspired Swarm Intelligence has laid the foundation for many eclectic applications. This work considers a solution to one such application of Search and Rescue operation, based on Cohort Intelligence (CI) methodology which aims at modelling the behaviour of candidates based on the interaction amongst each other to achieve a common goal. Every candidate improves its own behaviour by observing all other candidates in the cohort. This method results in the refinement of the performance of the entire system. The research done so far in this application using CI is associated with robots deployed in a static alien establishment. However, this assumption is not suitable in real-life scenarios. In this paper, the obstacle avoidance and path planning of a swarm of robots was implemented while considering a dynamic alien establishment. The problem of robots occasionally getting stuck in the non-convex obstacles has also been solved using a perturbation technique. The following test cases were implemented - No Obstacle Case (NOC), Stationary Obstacles Case (SOC), Single Dynamic Obstacle Case (SDOC), Multiple Dynamic Obstacles Case with Same Velocity (MDOC-SV) and Multiple Dynamic Obstacles Case with Different Velocities (MDOC-DV).

On the consensus of nonlinear agents in unknown cluttered environments using random planning

The consensus of multi-agent dynamic systems is a metaphor for many different tasks involving group agreement. However, ensuring consensus in real-world scenarios, with non-convex obstacles and kinodynamic motion constraints, proves to be a hard task, since it is quite difficult to model such a problem analytically. Therefore, this paper studies the problem of state agreement for Multi-Robot Systems (MRS) in unknown cluttered complex environments. Here, we propose and analyze a distributed consensus algorithm combined with a Rapidly-exploring Random Tree-based planner, which allows linear and nonlinear systems to reach a common target on their states inside bi- or three-dimensional spaces filled with static obstacles. We demonstrate that, with enough time, our planning strategy ensures probabilistic completeness convergence independently of the topological communication network employed, since some connectivity constraints are observed. Simulated results with linear and nonlinear models are provided, showing the effectiveness of our proposed method in comparison with the state-of-the-art literature for the specific case of position consensus (rendezvous) missions.

Collective navigation of a multi-robot system in an unknown environment

The navigation of autonomous, mobile multi-robot systems in changing environments is a challenging problem investigated over the past years. Cooperative, multiple robots are employed for many different tasks to increase the efficiency and success of a mission. However, many of the existing collective path planning approaches do not guarantee a reliable escape in environments with complex, non-convex obstacles without any prior knowledge. In this study, we developed a navigation framework for multi-robot systems in unknown areas that solely exploit the sensing information and shared data among the agents. The key contribution of this paper is the simultaneous, collision-free motion planning for fully autonomous robots in a collective manner. Furthermore, our communication architecture enables the robots to find an appropriate path to a desired, joint target position, despite the limited sensing and communication range.

Two-Stage Optimized Trajectory Planning for ASVs Under Polygonal Obstacle Constraints: Theory and Experiments

We propose a method for energy-optimized trajectory planning for autonomous surface vehicles (ASVs), which can handle arbitrary polygonal maps as obstacle constraints. The method comprises two stages: The first is a hybrid A* search that finds a dynamically feasible trajectory in a polygonal map on a discretized configuration space using optimal motion primitives. The second stage uses the resulting hybrid A* trajectory as an initial guess to an optimal control problem (OCP) solver. In addition to providing the OCP with a warm start, we use the initial guess to create convex regions encoded as halfspace descriptions, which converts the inherent nonconvex obstacle constraints into a convex and smooth representation. The OCP uses this representation in order to optimize the initial guess within a collision-free corridor. The OCP solves the trajectory planning problem in continuous state space. Our approach solves two challenges related to optimization-based trajectory planning: The need for a dynamically feasible initial guess that can guide the solver away from undesirable local optima and the ability to represent arbitrary obstacle shapes as smooth constraints. The method can take into account external disturbances such as wind or ocean currents. We compare our method to two similar trajectory planning methods in simulation and have found significant computation time improvements. Additionally, we have validated the method in full-scale experiments in the Trondheim harbor area.