This paper presents a method for online trajectory planning in known environments. The proposed algorithm is a fusion of sampling-based techniques and model-based optimization via quadratic programming. The former is used to efficiently generate an obstacle-free path while the latter takes into account the robot dynamical constraints to generate a time-dependent trajectory. The main contribution of this work lies on the formulation of a convex optimization problem over the generated obstacle-free path that is guaranteed to be feasible. Thus, in contrast with previously proposed methods, iterative formulations are not required. The proposed method has been compared with state-of-the-art approaches showing a significant improvement in success rate and computation time. To illustrate the effectiveness of this approach for online planning, the proposed method was applied to the fluid autonomous navigation of a quadcopter in multiple environments consisting of up to two hundred obstacles. The scenarios hereinafter presented are some of the most densely cluttered experiments for online planning and navigation reported to date. See video at https://youtu.be/DJ1IZRL5t1Q
Stanford University（斯坦福大学）；Carnegie Mellon University（卡内基梅隆大学）；University of Washington（华盛顿大学）
We present an algorithm for obtaining an optimal control policy for hybrid dynamical systems in cluttered environments. To the best of our knowledge, this is the first attempt to have a locally optimal solution for this specific problem setting. Our approach extends an optimal control algorithm for hybrid dynamical systems in the obstacle-free case to environments with obstacles. Our method does not require any preset mode sequence or heuristics to prune the exponential search of mode sequences. By first solving the relaxed problem of getting an obstacle-free, dynamically feasible trajectory a…
Efficient and Safe Trajectory Planning for Autonomous Agricultural Vehicle Headland Turning in Cluttered Orchard Environments
ZJU-Hangzhou Global Scientific and Technological Innovation Center, Zhejiang University（浙江大学ZJU-杭州全球科技创新中心）；University of California, Davis（加州大学戴维斯分校）；College Of Biosystems Engineering And Food Science, Zhejiang University（浙江大学生物系统工程与食品科学学院）
Collision avoidance in unknown obstacle-cluttered environments may not always be feasible. This paper focuses on an emerging paradigm shift in which potential collisions with the environment can be harnessed instead of being avoided altogether. To this end, we introduce a new sampling-based online planning algorithm that can explicitly handle the risk of colliding with the environment and can switch between collision avoidance and collision exploitation. Central to the planner's capabilities is a novel joint optimization function that evaluates the effect of possible collisions using a reflect…
Collision-Free Trajectory Optimization in Cluttered Environments Using Sums-of-Squares Programming
The Hong Kong University of Science and Technology（香港科技大学）；The Hong Kong University of Science and Technology (Guangzhou)（香港科技大学（广州））；Hong Kong Baptist University（香港浸会大学）；Great Bay University（湾区大学）
Autonomous Systems Lab, ETH Zürich（苏黎世联邦理工学院自主系统实验室）
In order to enable Micro-Aerial Vehicles (MAVs) to assist in complex, unknown, unstructured environments, they must be able to navigate with guaranteed safety, even when faced with a cluttered environment they have no prior knowledge of. While trajectory optimization-based local planners have been shown to perform well in these cases, prior work either does not address how to deal with local minima in the optimization problem, or solves it by using an optimistic global planner. We present a conservative trajectory optimization-based local planner, coupled with a local exploration strategy that…
An MPC framework for efficient navigation of mobile robots in cluttered environments
Institute for Dynamic Systems and Control, ETH Zurich（苏黎世联邦理工学院动态系统与控制研究所）；Department of Mechanical Engineering, Imperial College London（伦敦帝国理工学院机械工程系）；ETH Zurich（苏黎世联邦理工学院）；Automatic Control Laboratory, ETH Zurich（苏黎世联邦理工学院自动控制实验室）
Although Trajectory Optimization (TO) is one of the most powerful motion planning tools, it suffers from expensive computational complexity as a time horizon increases in cluttered environments. It can also fail to converge to a globally optimal solution. In this paper, we present Lazy Trajectory Optimization (LTO) that unifies local short-horizon TO and global Graph-Search Planning (GSP) to generate a long-horizon global optimal trajectory. LTO solves TO with the same constraints as the original long-horizon TO with improved time complexity. We also propose a TO-aware cost function that can b…
GPU-Accelerated Motion Planning of an Underactuated Forestry Crane in Cluttered Environments
Automation & Control Institute (ACIN), TU Wien（自动化与控制研究所（ACIN），维也纳技术大学）；Center for Vision, Automation & Control, AIT Austrian Institute of Technology GmbH（奥地利技术研究院（AIT）视觉、自动化与控制中心）；Institute for Artificial Intelligence, University of Bremen（不来梅大学人工智能研究所）
King Fahad University of Petroleum & Minerals（法赫德国王石油与矿产大学）
This paper suggests an integrated navigation system for an unmanned ground vehicle operating in an unknown cluttered environment. The navigator supports time-critical mobility making it possible for a mobile robot to reach a target from the first attempt without the need for a dedicated exploration and mapping stage. The robot only uses necessary and sufficient egocentric local sensory data collected on its way to the target. The construction of the navigation structure revolves around key properties of the harmonic potential field approach to motion planning. The robots trajectory is well-beh…
We present a novel class of robots belonging to Constrained Collaborative Mobile Agents (CCMA) family which consists of ground mobile bases with non-holonomic constraints. Moreover, these mobile robots are constrained by closed-loop kinematic chains consisting of revolute joints which can be either passive or actuated. We also describe a novel trajectory optimization method which is general with respect to number of mobile robots, topology of the closed-loop kinematic chains and placement of the actuators at the revolute joints. We also extend the standalone trajectory optimization method to o…
Precise trajectory tracking is a crucial property for \acp{MAV} to operate in cluttered environment or under disturbances. In this paper we present a detailed comparison between two state-of-the-art model-based control techniques for \ac{MAV} trajectory tracking. A classical \ac{LMPC} is presented and compared against a more advanced \ac{NMPC} that considers the full system model. In a careful analysis we show the advantages and disadvantages of the two implementations in terms of speed and tracking performance. This is achieved by evaluating hovering performance, step response, and aggressive…
In this paper, we present a novel approach to efficiently generate collision-free optimal trajectories for multiple non-holonomic mobile robots in obstacle-rich environments. Our approach first employs a graph-based multi-agent path planner to find an initial discrete solution, and then refines this solution into smooth trajectories using nonlinear optimization. We divide the robot team into small groups and propose a prioritized trajectory optimization method to improve the scalability of the algorithm. Infeasible sub-problems may arise in some scenarios because of the decoupled optimization …
This article presents a multi-robot trajectory planning method which not only guarantees optimization feasibility and but also resolves deadlocks in obstacle-dense environments. The method is proposed via formulating a recursive optimization problem, where a novel safe corridor is generated online to ensure obstacle avoidance in trajectory planning. A dynamic-priority mechanism is combined with the right-hand rule to handle potential deadlocks that are much harder to resolve due to static obstacles. Comparisons with other state-of-the-art results are conducted to validate the improved safety a…
RoboCup Middle Size League (RoboCup MSL) provides a standardized testbed for research on mobile robot navigation, multi-robot cooperation, communication and integration via robot soccer competition in which the environment is highly dynamic and adversarial. One of important research topic in such area is kinodynamic motion planning that plan the trajectory of the robot while avoiding obstacles and obeying its dynamics. Kinodynamic motion planning for omnidirectional robot based on kinodynamic-RRT* method is presented in this work. Trajectory tracking control to execute the planned trajectory i…
TopAY: Efficient Trajectory Planning for Differential Drive Mobile Manipulators via Topological Paths Search and Arc Length-Yaw Parameterization
State Key Laboratory of Industrial Control Technology, Zhejiang University, Hangzhou 310027, China（浙江大学工业控制技术状态重点实验室）；Huzhou Institute of Zhejiang University, Huzhou 313000, China（浙江大学湖州研究院）
Italian Institute of Technology（意大利技术研究院）；University of Genoa（热那亚大学）
This paper presents a simplified model-based trajectory optimization (TO) formulation for motion planning on quadruped mobile manipulators that carry heavy payload of known mass. The proposed payload-aware formulation simultaneously plans locomotion, payload manipulation and considers both robot and payload model dynamics while remaining computationally efficient. At the presence of heavy payload, the approach exhibits reduced leg outstretching (thus increased manipulability) in kinematically demanding motions due to the contribution of payload manipulation in the optimization. The framework's…
Universitat Politècnica de Catalunya（加泰罗尼亚理工大学）；Institute of Industrial and Control Engineering（工业与控制工程研究所）；Rice University（莱斯大学）
Planning motions to grasp an object in cluttered and uncertain environments is a challenging task, particularly when a collision-free trajectory does not exist and objects obstructing the way are required to be carefully grasped and moved out. This paper takes a different approach and proposes to address this problem by using a randomized physics-based motion planner that permits robot-object and object-object interactions. The main idea is to avoid an explicit high-level reasoning of the task by providing the motion planner with a physics engine to evaluate possible complex multi-body dynamic…
This paper addresses planning and control of robot motion under uncertainty that is formulated as a continuous-time, continuous-space stochastic optimal control problem, by developing a topology-guided path integral control method. The path integral control framework, which forms the backbone of the proposed method, re-writes the Hamilton-Jacobi-Bellman equation as a statistical inference problem; the resulting inference problem is solved by a sampling procedure that computes the distribution of controlled trajectories around the trajectory by the passive dynamics. For motion control of robots…
Robust Dynamic Control Barrier Function Based Trajectory Planning for Mobile Manipulator
School of Mechanical Engineering and Automation, Harbin Institute of Technology Shenzhen（哈尔滨工业大学（深圳）机械工程与自动化学院）；Graduated School of Information Science and Technology, Osaka University（大阪大学信息科学与技术研究生院）
Real-Time Nonlinear Model Predictive Control of Heavy-Duty Skid-Steered Mobile Platform for Trajectory Tracking Tasks
Unit of Automation Technology and Mechanical Engineering, Faculty of Engineering and Natural Sciences, Tampere University（自动化技术与机械工程单元，工程与自然科学学院，塔尔皮亚大学）
We tackle the problem of minimum-time flight for a quadrotor through a sequence of waypoints in the presence of obstacles while exploiting the full quadrotor dynamics. Early works relied on simplified dynamics or polynomial trajectory representations that did not exploit the full actuator potential of the quadrotor, and, thus, resulted in suboptimal solutions. Recent works can plan minimum-time trajectories; yet, the trajectories are executed with control methods that do not account for obstacles. Thus, a successful execution of such trajectories is prone to errors due to model mismatch and in…
In robot manipulation, planning the motion of a robot manipulator to grasp an object is a fundamental problem. A manipulation planner needs to generate a trajectory of the manipulator arm to avoid obstacles in the environment and plan an end-effector pose for grasping. While trajectory planning and grasp planning are often tackled separately, how to efficiently integrate the two planning problems remains a challenge. In this work, we present a novel method for joint motion and grasp planning. Our method integrates manipulation trajectory optimization with online grasp synthesis and selection, …
Zhejiang University（浙江大学）；Huzhou Institute, Zhejiang University（浙江大学湖州研究院）
Multicopter swarms with decentralized structure possess the nature of flexibility and robustness, while efficient spatial-temporal trajectory planning still remains a challenge. This report introduces decentralized spatial-temporal trajectory planning, which puts a well-formed trajectory representation named MINCO into multi-agent scenarios. Our method ensures high-quality local planning for each agent subject to any constraint from either the coordination of the swarm or safety requirements in cluttered environments. Then, the local trajectory generation is formulated as an unconstrained opti…
Zhejiang University（浙江大学）；Huzhou institute of Zhejiang University（浙江大学湖州研究院）
Online trajectory planners enable quadrotors to safely and smoothly navigate in unknown cluttered environments. However, tuning parameters is challenging since modern planners have become too complex to mathematically model and predict their interaction with unstructured environments. This work takes humans out of the loop by proposing a planner parameter adaptation framework that formulates objectives into two complementary categories and optimizes them asynchronously. Objectives evaluated with and without trajectory execution are optimized using Bayesian Optimization (BayesOpt) and Particle …
We present a batch trajectory optimizer that can simultaneously solve hundreds of different instances of the problem in real-time. We consider holonomic robots but relax the assumption of circular base footprint. Our main algorithmic contributions lie in: (i) improving the computational tractability of the underlying non-convex problem and (ii) leveraging batch computation to mitigate initialization bottlenecks and improve solution quality. We achieve both goals by deriving a multi-convex reformulation of the kinematics and collision avoidance constraints. We exploit these structures through a…
Prediction of human motions is key for safe navigation of autonomous robots among humans. In cluttered environments, several motion hypotheses may exist for a pedestrian, due to its interactions with the environment and other pedestrians. Previous works for estimating multiple motion hypotheses require a large number of samples which limits their applicability in real-time motion planning. In this paper, we present a variational learning approach for interaction-aware and multi-modal trajectory prediction based on deep generative neural networks. Our approach can achieve faster convergence and…
University of Zagreb Faculty of Electrical Engineering and Computing（萨格勒布大学电气工程与计算学院）
Trajectory optimization methods for motion planning attempt to generate trajectories that minimize a suitable objective function. Such methods efficiently find solutions even for high degree-of-freedom robots. However, a globally optimal solution is often intractable in practice and state-of-the-art trajectory optimization methods are thus prone to local minima, especially in cluttered environments. In this paper, we propose a novel motion planning algorithm that employs stochastic optimization based on the cross-entropy method in order to tackle the local minima problem. We represent trajecto…
University of Toronto Institute for Aerospace Studies（多伦多大学航空航天研究所）；Technical University of Munich（慕尼黑工业大学）；Vector Institute for Artificial Intelligence（向量人工智能研究所）；University of Tartu（塔尔图大学）
This paper presents a scalable online algorithm to generate safe and kinematically feasible trajectories for quadrotor swarms. Existing approaches rely on linearizing Euclidean distance-based collision constraints and on axis-wise decoupling of kinematic constraints to reduce the trajectory optimization problem for each quadrotor to a quadratic program (QP). This conservative approximation often fails to find a solution in cluttered environments. We present a novel alternative that handles collision constraints without linearization and kinematic constraints in their quadratic form while still…
In this paper, a real-time quasi-optimal trajectory planning scheme is employed to guide an autonomous underwater vehicle (AUV) safely into a funnel-shape stationary docking station. By taking advantage of the direct method of calculus of variation and inverse dynamics optimization, the proposed trajectory planner provides a computationally efficient framework for autonomous underwater docking in a 3D cluttered undersea environment. Vehicular constraints, such as constraints on AUV states and actuators; boundary conditions, including initial and final vehicle poses; and environmental constrain…
Mapless Collision-Free Flight via MPC using Dual KD-Trees in Cluttered Environments
Shanghai Key Laboratory of Navigation and Location-Based Services, School of Sensing Science and Engineering, Shanghai Jiao Tong University（上海导航与位置服务重点实验室，传感科学与工程学院，上海交通大学）
The University of Hong Kong（香港大学）；Baidu Research（百度研究院）
In this paper, we introduce a generalized continuous collision detection (CCD) framework for the mobile robot along the polynomial trajectory in cluttered environments including various static obstacle models. Specifically, we find that the collision conditions between robots and obstacles could be transformed into a set of polynomial inequalities, whose roots can be efficiently solved by the proposed solver. In addition, we test different types of mobile robots with various kinematic and dynamic constraints in our generalized CCD framework and validate that it allows the provable collision ch…
University of Tartu（塔尔图大学）；Tampere University（坦佩雷大学）；Johannes Kepler University（约翰内斯·开普勒大学）
In this paper, we propose a novel trajectory optimization algorithm for mobile manipulators under end-effector path, collision avoidance and various kinematic constraints. Our key contribution lies in showing how this highly non-linear and non-convex problem can be solved as a sequence of convex unconstrained quadratic programs (QPs). This is achieved by reformulating the non-linear constraints that arise out of manipulator kinematics and its coupling with the mobile base in a multi-affine form. We then use techniques from Alternating Direction Method of Multipliers (ADMM) to formulate and sol…
Systematic Constraint Formulation and Collision-Free Trajectory Planning Using Space-Time Graphs of Convex Sets
Department of Electrical and Computer Engineering, Brigham Young University（电气与计算机工程系，尤因斯大学）；Department of Mechanical Engineering, Brigham Young University（机械工程系，尤因斯大学）
