Target tracking has numerous significant civilian and military applications, and maintaining the visibility of the target plays a vital role in ensuring the success of the tracking task. Existing visibility-aware planners primarily focus on keeping the target within the limited field of view of an onboard sensor and avoiding obstacle occlusion. However, the negative impact of system uncertainty is often neglected, rendering the planners delicate to uncertainties in practice. To bridge the gap, this work proposes a real-time, non-myopic trajectory planner for visibility-aware and safe target tracking in the presence of system uncertainty. For more accurate target motion prediction, we introduce the concept of belief-space probability of detection (BPOD) to measure the predictive visibility of the target under stochastic robot and target states. An Extended Kalman Filter variant incorporating BPOD is developed to predict target belief state under uncertain visibility within the planning horizon. To reach real-time trajectory planning, we propose a computationally efficient algorithm to uniformly calculate both BPOD and the chance-constrained collision risk by utilizing linearized signed distance function (SDF), and then design a two-stage strategy for lightweight calculation of SDF in sequential convex programming. Extensive simulation results with benchmark comparisons show the capacity of the proposed approach to robustly maintain the visibility of the target under high system uncertainty. The practicality of the proposed trajectory planner is validated by real-world experiments.
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（浙江大学生物系统工程与食品科学学院）
Intel Labs（英特尔实验室）；Intel Tecnología de México（英特尔墨西哥技术公司）
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, iterat…
Zhejiang University（浙江大学）；Huzhou Institute of Zhejiang University（浙江大学湖州研究院）
The visibility of targets determines performance and even success rate of various applications, such as active slam, exploration, and target tracking. Therefore, it is crucial to take the visibility of targets into explicit account in trajectory planning. In this paper, we propose a general metric for target visibility, considering observation distance and angle as well as occlusion effect. We formulate this metric into a differentiable visibility cost function, with which spatial trajectory and yaw can be jointly optimized. Furthermore, this visibility-aware trajectory optimization handles dy…
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…
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…
This paper addresses the problem of planning for a robot with a directional obstacle-detection sensor that must move through a cluttered environment. The planning objective is to remain safe by finding a path for the complete robot, including sensor, that guarantees that the robot will not move into any part of the workspace before it has been seen by the sensor. Although a great deal of work has addressed a version of this problem in which the "field of view" of the sensor is a sphere around the robot, there is very little work addressing robots with a narrow or occluded field of view. We giv…
Zhejiang University（浙江大学）；Huzhou Institute of Zhejiang University（浙江大学湖州研究院）；The University of Hong Kong（香港大学）
The visible capability is critical in many robot applications, such as inspection and surveillance, etc. Without the assurance of the visibility to targets, some tasks end up not being complete or even failing. In this paper, we propose a visibility guaranteed planner by star-convex constrained optimization. The visible space is modeled as star convex polytope (SCP) by nature and is generated by finding the visible points directly on point cloud. By exploiting the properties of the SCP, the visibility constraint is formulated for trajectory optimization. The trajectory is confined in the safe …
Fraunhofer Institute for Communication, Information Processing and Ergonomics FKIE（弗劳恩霍夫通信、信息处理与工效学研究所FKIE）；University of Bonn（波恩大学）
Safe navigation of Micro Aerial Vehicles (MAVs) requires not only obstacle-free flight paths according to a static environment map, but also the perception of and reaction to previously unknown and dynamic objects. This implies that the onboard sensors cover the current flight direction. Due to the limited payload of MAVs, full sensor coverage of the environment has to be traded off with flight time. Thus, often only a part of the environment is covered. We present a combined allocentric complete planning and trajectory optimization approach taking these sensor visibility constraints into acco…
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…
Zhejiang University（浙江大学）；Huzhou Institute, Zhejiang University（浙江大学湖州研究院）
This paper proposes Elastic Tracker, a flexible trajectory planning framework that can deal with challenging tracking tasks with guaranteed safety and visibility. Firstly, an object detection and intension-free motion prediction method is designed. Then an occlusion-aware path finding method is proposed to provide a proper topology. A smart safe flight corridor generation strategy is designed with the guiding path. An analytical occlusion cost is evaluated. Finally, an effective trajectory optimization approach enables to generate a spatio-temporal optimal trajectory within the resultant fligh…
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…
DMVC-Tracker: Distributed Multi-Agent Trajectory Planning for Target Tracking Using Dynamic Buffered Voronoi and Inter-Visibility Cells
Artificial Intelligence Institute of Seoul National University（首尔国立大学人工智能研究院）；Seoul National University of Science and Technology（首尔科学技术大学）；Seoul National University（首尔国立大学）
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…
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…
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…
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…
CosFly-Track: A Large-Scale Multi-Modal Dataset for UAV Visual Tracking via Multi-Constraint Trajectory Optimization
Autel Robotics（Autel机器人公司）；Nanjing University（南京大学）；Peking University（北京大学）；Southern University of Science and Technology（南方科技大学）；University of Hong Kong（香港大学）
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 …
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（上海导航与位置服务重点实验室，传感科学与工程学院，上海交通大学）
QP Chaser: Polynomial Trajectory Generation for Autonomous Aerial Tracking
Robotics Institute, Carnegie Mellon University（卡内基梅隆大学机器人研究所）；Department of Mechanical System Design Engineering, Seoul National University of Science and Technology（首尔科学技术大学机械系统设计工程系）；Department of Aerospace Engineering, Seoul National University（首尔国立大学航空航天工程系）；Samsung Research, Samsung Electronics（三星电子三星研究）
The goal of this paper is to develop a continuous optimization-based refinement of the reference trajectory to 'push it out' of the obstacle-occupied space in the global phase for Multi-rotor Aerial Vehicles in unknown environments. Our proposed approach comprises two planners: a global planner and a local planner. The global planner refines the initial reference trajectory when the trajectory goes either through an obstacle or near an obstacle and lets the local planner calculate a near-optimal control policy. The global planner comprises two convex programming approaches: the first one helps…
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（机械工程系，尤因斯大学）
NIT Surat（苏拉特国家理工学院）；Institute of Technology, University of Tartu（塔尔图大学技术学院）
Most commercially available fixed-wing aerial vehicles (FWV) can carry only small, lightweight computing hardware such as Jetson TX2 onboard. Solving non-linear trajectory optimization on these computing resources is computationally challenging even while considering only the kinematic motion model. Most importantly, the computation time increases sharply as the environment becomes more cluttered. In this paper, we take a step towards overcoming this bottleneck and propose a trajectory optimizer that achieves online performance on both conventional laptops/desktops and Jetson TX2 in a typical …
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（苏黎世联邦理工学院自动控制实验室）
Department of Aeronautics and Astronautics, Massachusetts Institute of Technology（航空航天系，麻省理工学院）；Department of Electrical and Systems Engineering, University of Pennsylvania（电气与系统工程系，宾夕法尼亚大学）
Zhejiang University（浙江大学）；Huzhou Institute, Zhejiang University（浙江大学湖州研究院）
For aerial swarms, navigation in a prescribed formation is widely practiced in various scenarios. However, the associated planning strategies typically lack the capability of avoiding obstacles in cluttered environments. To address this deficiency, we present an optimization-based method that ensures collision-free trajectory generation for formation flight. In this paper, a novel differentiable metric is proposed to quantify the overall similarity distance between formations. We then formulate this metric into an optimization framework, which achieves spatial-temporal planning using polynomia…
Natural Functional Gradients for Smooth Trajectory Optimization
Department of Artificial Intelligence, Korea University, Seoul, Republic of Korea（韩国大学人工智能系，首尔，大韩民国）；Department of Statistics, Korea University, Seoul, Republic of Korea（韩国大学统计系，首尔，大韩民国）
