The Convex Feasible Set Algorithm for Real Time Optimization in Motion Planning

With the development of robotics, there are growing needs for real time motion planning. However, due to obstacles in the environment, the planning problem is highly non-convex, which makes it difficult to achieve real time computation using existing non-convex optimization algorithms. This paper introduces the convex feasible set algorithm (CFS) which is a fast algorithm for non-convex optimization problems that have convex costs and non-convex constraints. The idea is to find a convex feasible set for the original problem and iteratively solve a sequence of subproblems using the convex constraints. The feasibility and the convergence of the proposed algorithm are proved in the paper. The application of this method on motion planning for mobile robots is discussed. The simulations demonstrate the effectiveness of the proposed algorithm.Comment: in SIAM Journal on Control and Optimizatio

A Convex Optimization Approach to Smooth Trajectories for Motion Planning with Car-Like Robots

In the recent past, several sampling-based algorithms have been proposed to compute trajectories that are collision-free and dynamically-feasible. However, the outputs of such algorithms are notoriously jagged. In this paper, by focusing on robots with car-like dynamics, we present a fast and simple heuristic algorithm, named Convex Elastic Smoothing (CES) algorithm, for trajectory smoothing and speed optimization. The CES algorithm is inspired by earlier work on elastic band planning and iteratively performs shape and speed optimization. The key feature of the algorithm is that both optimization problems can be solved via convex programming, making CES particularly fast. A range of numerical experiments show that the CES algorithm returns high-quality solutions in a matter of a few hundreds of milliseconds and hence appears amenable to a real-time implementation

Robot Safe Interaction System for Intelligent Industrial Co-Robots

Human-robot interactions have been recognized to be a key element of future industrial collaborative robots (co-robots). Unlike traditional robots that work in structured and deterministic environments, co-robots need to operate in highly unstructured and stochastic environments. To ensure that co-robots operate efficiently and safely in dynamic uncertain environments, this paper introduces the robot safe interaction system. In order to address the uncertainties during human-robot interactions, a unique parallel planning and control architecture is proposed, which has a long term global planner to ensure efficiency of robot behavior, and a short term local planner to ensure real time safety under uncertainties. In order for the robot to respond immediately to environmental changes, fast algorithms are used for real-time computation, i.e., the convex feasible set algorithm for the long term optimization, and the safe set algorithm for the short term optimization. Several test platforms are introduced for safe evaluation of the developed system in the early phase of deployment. The effectiveness and the efficiency of the proposed method have been verified in experiment with an industrial robot manipulator.Comment: 12 page

3D-OGSE: Online Safe and Smooth Trajectory Generation using Generalized Shape Expansion in Unknown 3-D Environments

In this paper, we present an online motion planning algorithm (3D-OGSE) for generating smooth, collision-free trajectories over multiple planning iterations for 3-D agents operating in an unknown obstacle-cluttered 3-D environment. Our approach constructs a safe-region, termed 'generalized shape', at each planning iteration, which represents the obstacle-free region based on locally-sensed environment information. A collision-free path is computed by sampling points in the generalized shape and is used to generate a smooth, time-parametrized trajectory by minimizing snap. The generated trajectories are constrained to lie within the generalized shape, which ensures the agent maneuvers in the locally obstacle-free space. As the agent reaches boundary of 'sensing shape' in a planning iteration, a re-plan is triggered by receding horizon planning mechanism that also enables initialization of the next planning iteration. Theoretical guarantee of probabilistic completeness over the entire environment and of completely collision-free trajectory generation is provided. We evaluate the proposed method in simulation on complex 3-D environments with varied obstacle-densities. We observe that each re-planing computation takes $\sim$1.4 milliseconds on a single thread of an Intel Core i5-8500 3.0 GHz CPU. In addition, our method is found to perform 4-10 times faster than several existing algorithms. In simulation over complex scenarios such as narrow passages also we observe less conservative behavior.Comment: Submitted to Robotics and Automation Letters (RA-L) with ICRA 2021 option. 9 pages and 8 figure

SERoCS: Safe and Efficient Robot Collaborative Systems for Next Generation Intelligent Industrial Co-Robots

Human-robot collaborations have been recognized as an essential component for future factories. It remains challenging to properly design the behavior of those co-robots. Those robots operate in dynamic uncertain environment with limited computation capacity. The design objective is to maximize their task efficiency while guaranteeing safety. This paper discusses a set of design principles of a safe and efficient robot collaboration system (SERoCS) for the next generation co-robots, which consists of robust cognition algorithms for environment monitoring, efficient task planning algorithms for reference generations, and safe motion planning and control algorithms for safe human-robot interactions. The proposed SERoCS will address the design challenges and significantly expand the skill sets of the co-robots to allow them to work safely and efficiently with their human counterparts. The development of SERoCS will create a significant advancement toward adoption of co-robots in various industries. The experiments validate the effectiveness of SERoCS.Comment: 19 page

Non-Gaussian SLAP: Simultaneous Localization and Planning Under Non-Gaussian Uncertainty in Static and Dynamic Environments

Simultaneous Localization and Planning (SLAP) under process and measurement uncertainties is a challenge. It involves solving a stochastic control problem modeled as a Partially Observed Markov Decision Process (POMDP) in a general framework. For a convex environment, we propose an optimization-based open-loop optimal control problem coupled with receding horizon control strategy to plan for high quality trajectories along which the uncertainty of the state localization is reduced while the system reaches to a goal state with minimum control effort. In a static environment with non-convex state constraints, the optimization is modified by defining barrier functions to obtain collision-free paths while maintaining the previous goals. By initializing the optimization with trajectories in different homotopy classes and comparing the resultant costs, we improve the quality of the solution in the presence of action and measurement uncertainties. In dynamic environments with time-varying constraints such as moving obstacles or banned areas, the approach is extended to find collision-free trajectories. In this paper, the underlying spaces are continuous, and beliefs are non-Gaussian. Without obstacles, the optimization is a globally convex problem, while in the presence of obstacles it becomes locally convex. We demonstrate the performance of the method on different scenarios.Comment: 10 page

Finding a Feasible Initial Solution for Flatness-Based Multi-Link Manipulator Motion Planning under State and Control Constraints

In this paper, we present a method to initialize at a feasible point and unfailingly solve a non-convex optimization problem in which a set-point motion is planned for a multi-link manipulator under state and control constraints. We construct an initial feasible solution by analyzing the final time effect for feasibility problems of flatness based motion planning problems. More specifically, we first find a feasible time-optimal trajectory under state constraints without a control constraint by solving a linear programming problem. Then, we find a feasible trajectory under control constraints by scaling the trajectory. To evaluate the practical applicability of the proposed method, we did numerical experiments to solve a multi-link manipulator motion planning problem by combining the method with recursive inverse dynamics algorithms.Comment: accepted to the SICE International Symposium on Control Systems 201

A Penalty Method Based Approach for Autonomous Navigation using Nonlinear Model Predictive Control

This paper presents a novel model predictive control strategy for controlling autonomous motion systems moving through an environment with obstacles of general shape. In order to solve such a generic non-convex optimization problem and find a feasible trajectory that reaches the destination, the approach employs a quadratic penalty method to enforce the obstacle avoidance constraints, and several heuristics to bypass local minima behind an obstacle. The quadratic penalty method itself aids in avoiding such local minima by gradually finding a path around the obstacle as the penalty factors are successively increased. The inner optimization problems are solved in real time using the proximal averaged Newton-type method for optimal control (PANOC), a first-order method which exhibits low runtime and is suited for embedded applications. The method is validated by extensive numerical simulations and shown to outperform state-of-the-art solvers in runtime and robustness.Comment: 7 page

Real-time Trajectory Generation for Quadrotors using B-spline based Non-uniform Kinodynamic Search

In this paper, we propose a time-efficient approach to generate safe, smooth and dynamically feasible trajectories for quadrotors in obstacle-cluttered environment. By using the uniform B-spline to represent trajectories, we transform the trajectory planning to a graph-search problem of B-spline control points in discretized space. Highly strict convex hull property of B-spline is derived to guarantee the dynamical feasibility of the entire trajectory. A novel non-uniform kinodynamic search strategy is adopted, and the step length is dynamically adjusted during the search process according to the Euclidean signed distance field (ESDF), making the trajectory achieve reasonable time-allocation and be away from obstacles. Non-static initial and goal states are allowed, therefore it can be used for online local replanning as well as global planning. Extensive simulation and hardware experiments show that our method achieves higher performance comparing with the state-of-the-art method.Comment: 7 pages,6 figures, conferenc

Bridging the Gap Between Safety and Real-Time Performance in Receding-Horizon Trajectory Design for Mobile Robots

To operate with limited sensor horizons in unpredictable environments, autonomous robots use a receding-horizon strategy to plan trajectories, wherein they execute a short plan while creating the next plan. However, creating safe, dynamically-feasible trajectories in real time is challenging; and, planners must ensure persistent feasibility, meaning a new trajectory is always available before the previous one has finished executing. Existing approaches make a tradeoff between model complexity and planning speed, which can require sacrificing guarantees of safety and dynamic feasibility. This work presents the Reachability-based Trajectory Design (RTD) method for trajectory planning. RTD begins with an offline Forward Reachable Set (FRS) computation of a robot's motion when tracking parameterized trajectories; the FRS provably bounds tracking error. At runtime, the FRS is used to map obstacles to parameterized trajectories, allowing RTD to select a safe trajectory at every planning iteration. RTD prescribes an obstacle representation to ensure that obstacle constraints can be created and evaluated in real time while maintaining safety. Persistent feasibility is achieved by prescribing a minimum sensor horizon and a minimum duration for the planned trajectories. A system decomposition approach is used to improve the tractability of computing the FRS, allowing RTD to create more complex plans at runtime. RTD is compared in simulation with Rapidly-Exploring Random Trees and Nonlinear Model-Predictive Control. RTD is also demonstrated in randomly-crafted environments on two hardware platforms: a differential-drive Segway, and a car-like Rover. The proposed method is safe and persistently feasible across thousands of simulations and dozens of real-world hardware demos.Comment: The first two authors contributed equally to this wor