Over the years, the separate fields of motion planning, mapping, and human trajectory prediction have advanced considerably. However, the literature is still sparse in providing practical frameworks that enable mobile manipulators to perform whole-body movements and account for the predicted motion of moving obstacles. Previous optimisation-based motion planning approaches that use distance fields have suffered from the high computational cost required to update the environment representation. We demonstrate that GPU-accelerated predicted composite distance fields significantly reduce the computation time compared to calculating distance fields from scratch. We integrate this technique with a complete motion planning and perception framework that accounts for the predicted motion of humans in dynamic environments, enabling reactive and pre-emptive motion planning that incorporates predicted motions. To achieve this, we propose and implement a novel human trajectory prediction method that combines intention recognition with trajectory optimisation-based motion planning. We validate our resultant framework on a real-world Toyota Human Support Robot (HSR) using live RGB-D sensor data from the onboard camera. In addition to providing analysis on a publicly available dataset, we release the Oxford Indoor Human Motion (Oxford-IHM) dataset and demonstrate state-of-the-art performance in human trajectory prediction. The Oxford-IHM dataset is a human trajectory prediction dataset in which people walk between regions of interest in an indoor environment. Both static and robot-mounted RGB-D cameras observe the people while tracked with a motion-capture system.
Motion planning for multi-jointed robots is challenging. Due to the inherent complexity of the problem, most existing works decompose motion planning as easier subproblems. However, because of the inconsistent performance metrics, only sub-optimal solution can be found by decomposition based approaches. This paper presents an optimal control based approach to address the path planning and trajectory planning subproblems simultaneously. Unlike similar works which either ignore robot dynamics or require long computation time, an efficient numerical method for trajectory optimization is presented…
Collision-tolerant trajectory planning is the consideration that collisions, if they are planned appropriately, enable more effective path planning for robots capable of handling them. A mixed integer programming (MIP) optimization formulation demonstrates the computational practicality of optimizing trajectories that comprise planned collisions. A damage quantification function is proposed, and the influence of damage functions constraints on the trajectory are studied in simulation. Using a simple example, an increase in performance is achieved under this schema as compared to collision-free…
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…
Technion - Israel Institute of Technology（以色列理工学院）
An important capability of autonomous Unmanned Aerial Vehicles (UAVs) is autonomous landing while avoiding collision with obstacles in the process. Such capability requires real-time local trajectory planning. Although trajectory-planning methods have been introduced for cases such as emergency landing, they have not been evaluated in real-life scenarios where only the surface of obstacles can be sensed and detected. We propose a novel optimization framework using a pre-planned global path and a priority map of the landing area. Several trajectory planning algorithms were implemented and evalu…
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…
Planning Jerk-Optimized Trajectory with Discrete-Time Constraints for Redundant Robots
Delft University of Technology (TU Delft)（代尔夫特理工大学）；INRIA（法国国家信息与自动化研究所）；The Hong Kong Polytechnic University（香港理工大学）；The Chinese University of Hong Kong (CUHK)（香港中文大学）
Efficient trajectory planning for urban intersections is currently one of the most challenging tasks for an Autonomous Vehicle (AV). Courteous behavior towards other traffic participants, the AV's comfort and its progression in the environment are the key aspects that determine the performance of trajectory planning algorithms. To capture these aspects, we propose a novel trajectory planning framework that ensures social compliance and simultaneously optimizes the AV's comfort subject to kinematic constraints. The framework combines a local continuous optimization approach and an efficient dri…
Efficient behavior and trajectory planning is one of the major challenges for automated driving. Especially intersection scenarios are very demanding due to their complexity arising from the variety of maneuver possibilities and other traffic participants. A key challenge is to generate behaviors which optimize the comfort and progress of the ego vehicle but at the same time are not too aggressive towards other traffic participants. In order to maintain real time capability for courteous behavior and trajectory planning, an efficient formulation of the optimal control problem and corresponding…
This paper presents a hybrid robot motion planner that generates long-horizon motion plans for robot navigation in environments with obstacles. We propose a hybrid planner, RRT* with segmented trajectory optimization (RRT*-sOpt), which combines the merits of sampling-based planning, optimization-based planning, and trajectory splitting to quickly plan for a collision-free and dynamically-feasible motion plan. When generating a plan, the RRT* layer quickly samples a semi-optimal path and sets it as an initial reference path. Then, the sOpt layer splits the reference path and performs optimizati…
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…
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…
Seoul National University（首尔大学）；Automation and Systems Research Institute（自动化与系统研究院）
Automation of excavation tasks requires real-time trajectory planning satisfying various constraints. To guarantee both constraint feasibility and real-time trajectory re-plannability, we present an integrated framework for real-time optimization-based trajectory planning of a hydraulic excavator. The proposed framework is composed of two main modules: a global planner and a real-time local planner. The global planner computes the entire global trajectory considering excavation volume and energy minimization while the local counterpart tracks the global trajectory in a receding horizon manner,…
Time-Optimized Trajectory Planning for Non-Prehensile Object Transportation in 3D
Munich Institute of Robotics and Machine Intelligence (MIRMI), Technical University of Munich（慕尼黑工业大学慕尼黑机器人与机器智能研究所（MIRMI））；Centre for Tactile Internet with Human-in-the-Loop (CeTI)（以人为中心的触觉互联网中心（CeTI））
MMH-Planner: Multi-Mode Hybrid Trajectory Planning Method for UAV Efficient Flight Based on Real-Time Spatial Awareness
School of Surveying and Mapping, Information Engineering University（测绘学院，信息工程大学）；School of Geodesy and Geomatics, Hubei Luojia Laboratory（地质测绘学院，湖北珞珈实验室）
College of Control Science and Technology, Zhejiang University（浙江大学控制科学与工程学院）；Institute of Cyber-Systems and Control, Zhejiang University（浙江大学智能系统与控制研究所）
In this paper, we propose an efficient and accurate method for autonomous surface vehicles to generate a smooth and collision-free trajectory considering its dynamics constraints. We decouple the trajectory planning problem as a front-end feasible path searching and a back-end kinodynamic trajectory optimization. Firstly, we model the type of two-thrusts under-actuated surface vessel. Then we adopt a sampling-based path searching to find an asymptotic optimal path through the obstacle-surrounding environment and extract several waypoints from it. We apply a numerical optimization method in the…
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, …
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（大阪大学信息科学与技术研究生院）
We describe a method for formation-change trajectory planning for large quadrotor teams in obstacle-rich environments. Our method decomposes the planning problem into two stages: a discrete planner operating on a graph representation of the workspace, and a continuous refinement that converts the non-smooth graph plan into a set of C^k-continuous trajectories, locally optimizing an integral-squared-derivative cost. We account for the downwash effect, allowing safe flight in dense formations. We demonstrate the computational efficiency in simulation with up to 200 robots and the physical plausi…
This paper presents a non-linear optimization method for trajectory planning of tethered aerial robots. Particularly, the paper addresses the planning problem of an unmanned aerial vehicle (UAV) linked to an unmanned ground vehicle (UGV) by means of a tether. The result is a collision-free trajectory for UAV and tether, assuming the UGV position is static. The optimizer takes into account constraints related to the UAV, UGV and tether positions, obstacles and temporal aspects of the motion such as limited robot velocities and accelerations, and finally the tether state, which is not required t…
Due to nonholonomic dynamics, the motion planning of nonholonomic robots is always a difficult problem. This letter presents a Discrete States-based Trajectory Planning(DSTP) algorithm for autonomous nonholonomic robots. The proposed algorithm represents the trajectory as x and y positions, orientation angle, longitude velocity and acceleration, angular velocity, and time intervals. More variables make the expression of optimization and constraints simpler, reduce the error caused by too many approximations, and also handle the gear shifting situation. L-BFGS-B is used to deal with the optimiz…
Hong Kong University of Science and Technology（香港科技大学）
It is ubiquitously accepted that during the autonomous navigation of the quadrotors, one of the most widely adopted unmanned aerial vehicles (UAVs), safety always has the highest priority. However, it is observed that the ego airflow disturbance can be a significant adverse factor during flights, causing potential safety issues, especially in narrow and confined indoor environments. Therefore, we propose a novel method to estimate and adapt indoor ego airflow disturbance of quadrotors, meanwhile applying it to trajectory planning. Firstly, the hover experiments for different quadrotors are con…
In this paper, a novel real-time acceleration-continuous path-constrained trajectory planning algorithm is proposed with an appealing built-in tradability mechanism between cruise motion and time-optimal motion. Different from existing approaches, the proposed approach smoothens time-optimal trajectories with bang-bang input structures to generate acceleration-continuous trajectories while preserving the completeness property. More importantly, a novel built-in tradability mechanism is proposed and embedded into the trajectory planning framework, so that the proportion of the cruise motion and…
Velocity Field: An Informative Traveling Cost Representation for Trajectory Planning
The Hong Kong University of Science and Technology (Guangzhou)（香港科技大学（广州））；The Hong Kong University of Science and Technology（香港科技大学）；HKUST Shenzhen-Hong Kong Collaborative Innovation Research Institute（香港科技大学深港协同创新研究院）
针对自动驾驶轨迹规划难以穷尽设计规则的问题，提出名为 Velocity Field 的局部地图表示方法，通过学习人类驾驶示范提供朝向与速度先验，在真实数据集上比栅格化代价地图更可靠高效。
Zhejiang University（浙江大学）；Huzhou Institute of Zhejiang University（浙江大学湖州研究院）
Unmanned surface vessels (USVs) are widely used in ocean exploration and environmental protection fields. To ensure that USV can successfully perform its mission, trajectory planning and motion tracking are the two most critical technologies. In this paper, we propose a novel trajectory generation and tracking method for USV based on optimization theory. Specifically, the USV dynamic model is described with differential flatness, so that the trajectory can be generated by dynamic RRT* in a linear invariant system expression form under the objective of optimal boundary value. To reduce the samp…
This paper proposes a fast and accurate trajectory planning algorithm for autonomous parking. Nominally, an optimal control problem should be formulated to describe this scheme, but the dimensionality of the optimal control problem is usually large, because the vehicle needs to avoid collision with every obstacle at every moment during the entire dynamic process. Although an initial guess obtained by a sample-and-search based planner facilitates the numerical optimization process, it is still far from being as fast as real-time. To address this issue, we replace all of the collision-avoidance …
This paper focuses on automatic guided vehicle (AGV) trajectory planning in the presence of moving obstacles with known but complicated trajectories. In order to achieve good solution precision, optimality and unification, the concerned task should be formulated as an optimal control problem, and then discretized into a nonlinear programming (NLP) problem, which is numerically optimized thereafter. Without a near-feasible or near-optimal initial guess, the NLP-solving process is usually slow. With the purpose of accelerating the NLP solution, a search-based rough planning stage is added to gen…
Current advances in the development of autonomous cars suggest that driverless cars may see wide-scale deployment in the near future. Research by both industry and academia is driven by potential benefits of this new technology, including reductions in fatalities and improvements in traffic and fuel efficiency as well as greater mobility for people who will or cannot drive cars themselves. A deciding factor for the adoption of self-driving cars besides safety will be the comfort of the passengers. This report looks at cost functions currently used in motion planning methods for autonomous on-r…
We present a novel approach to perform probabilistic collision detection between a high-DOF robot and high-DOF obstacles in dynamic, uncertain environments. In dynamic environments with a high-DOF robot and moving obstacles, our approach efficiently computes accurate collision probability between the robot and obstacles with upper error bounds. Furthermore, we describe a prediction algorithm for future obstacle position and motion that accounts for both spatial and temporal uncertainties. We present a trajectory optimization algorithm for high-DOF robots in dynamic, uncertain environments base…
Autonomy and connectivity are considered among the most promising technologies to improve safety, mobility, fuel and time consumption in transportation systems. Some of the fuel efficiency benefits of connected and automated vehicles (CAVs) can be realized through platooning. A platoon is a virtual train of CAVs that travel together following the platoon head, with small gaps between them. Vehicles may also reduce travel time by lane changing. In this paper, we devise an optimal control-based trajectory planning model that can provide safe and efficient trajectories for the subject vehicle and…
Zhejiang University（浙江大学）；Huzhou Institute, Zhejiang University（浙江大学湖州研究院）
This paper presents a novel trajectory planning method for aerial perching. Compared with the existing work, the terminal states and the trajectory durations can be adjusted adaptively, instead of being determined in advance. Furthermore, our planner is able to minimize the tangential relative speed on the premise of safety and dynamic feasibility. This feature is especially notable on micro aerial robots with low maneuverability or scenarios where the space is not enough. Moreover, we design a flexible transformation strategy to eliminate terminal constraints along with reducing optimization …
The Hong Kong University of Science and Technology（香港科技大学）
Though great effort has been put into the study of path planning on urban roads and highways, few works have studied the driving strategy and trajectory planning in low-speed driving scenarios, e.g., driving on a university campus or driving through a housing or industrial estate. The study of planning in these scenarios is crucial as these environments often cover the first or the last one kilometer of a daily travel or logistic system. Additionally, it is essential to treat these scenarios differently as, in most cases, the driving environment is narrow, dynamic, and rich with obstacles, whi…
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（浙江大学湖州研究院）
Hong Kong University of Science and Technology（香港科技大学）
Real-time kinodynamic trajectory planning in dynamic environments is critical yet challenging for autonomous driving. In this letter, we propose an efficient trajectory planning system for autonomous driving in complex dynamic scenarios through iterative and incremental path-speed optimization. Exploiting the decoupled structure of the planning problem, a path planner based on Gaussian process first generates a continuous arc-length parameterized path in the Frenét frame, considering static obstacle avoidance and curvature constraints. We theoretically prove that it is a good generalization of…
From Field to Planning: Learning Temporal Polar Gradient Fields for Vision-based AAV Trajectory Planning in Dynamic Environments
College of Intelligence Science and Technology, National University of Defense Technology（国防科技大学智能科学学院）；College of Automation Engineering, Nanjing University of Aeronautics and Astronautics（南京航空航天大学自动化学院）
