Trajectory Optimization

Ask about this topic

Overview

Trajectory optimization finds motions or control inputs that achieve goals efficiently while respecting dynamics, obstacles, actuator limits, and sometimes uncertainty or contact. Common approaches include direct optimal-control methods such as sequential convex programming and differential dynamic programming, spline-based formulations, and sampling methods such as MPPI and cross-entropy search; recent work also uses diffusion models and learned warm starts. Applications range from robot manipulation and legged locomotion to autonomous driving, UAV planning, and spacecraft guidance, with evaluation spanning simulation benchmarks and real-robot or flight tests. A central challenge is balancing fast, reliable solutions with global exploration and strong feasibility or safety guarantees, especially for contact-rich, uncertain, and high-dimensional systems.

Latest papers

CardsList

204 papers22 papers in the last four weeks, down 31% on the four weeks before. 0.1% of all new papers.

Mar 5, 2026

EmboAlign: Aligning Video Generation with Compositional Constraints for Zero-Shot Manipulation

Video generative models (VGMs) pretrained on large-scale internet data can produce temporally coherent rollout videos that capture rich object dynamics, offering a compelling foundation for zero-shot robotic manipulation. However, VGMs often produce physically implausible rollouts, and converting their pixel-space motion into robot actions through geometric retargeting further introduces cumulative errors from imperfect depth estimation and keypoint tracking. To address these challenges, we present EmboAlign, a data-free framework that aligns VGM outputs with compositional constraints generated by vision-language models (VLMs) at inference time. The key insight is that VLMs offer a capability complementary to VGMs: structured spatial reasoning that can identify the physical constraints critical to the success and safety of manipulation execution. Given a language instruction, EmboAlign uses a VLM to automatically extract a set of compositional constraints capturing task-specific requirements, which are then applied at two stages: (1) constraint-guided rollout selection, which scores and filters a batch of VGM rollouts to retain the most physically plausible candidate, and (2) constraint-based trajectory optimization, which uses the selected rollout as initialization and refines the robot trajectory under the same constraint set to correct retargeting errors. We evaluate EmboAlign on six real-robot manipulation tasks requiring precise, constraint-sensitive execution, improving the overall success rate by 43.3 percentage points over the strongest baseline without any task-specific training data.

Gehao Zhang, Zhenyang Ni, Payal Mohapatra +3

Northwestern University · Stanford University

Trajectory OptimizationPhysical Consistency in Video GenerationZero-Shot Robotic ManipulationRobotic Manipulation
AskPDF
Mar 5, 2026

cuRoboV2: Dynamics-Aware Motion Generation with Depth-Fused Distance Fields for High-DoF Robots

Effective robot autonomy requires motion generation that is safe, feasible, and reactive. Current methods are fragmented: fast planners output physically unexecutable trajectories, reactive controllers struggle with high-fidelity perception, and existing solvers fail on high-DoF systems. We present cuRoboV2, a unified framework with three key innovations: (1) B-spline trajectory optimization that enforces smoothness and torque limits; (2) a GPU-native TSDF/ESDF perception pipeline that generates dense signed distance fields covering the full workspace, unlike existing methods that only provide distances within sparsely allocated blocks, up to 10x faster and in 8x less memory than the state-of-the-art at manipulation scale, with up to 99% collision recall; and (3) scalable GPU-native whole-body computation, namely topology-aware kinematics, differentiable inverse dynamics, and map-reduce self-collision, that achieves up to 61x speedup while also extending to high-DoF humanoids (where previous GPU implementations fail). On benchmarks, cuRoboV2 achieves 99.7% success under 3kg payload (where baselines achieve only 72--77%), 99.6% collision-free IK on a 48-DoF humanoid (where prior methods fail entirely), and 89.5% retargeting constraint satisfaction (vs. 61% for PyRoki); these collision-free motions yield locomotion policies with 21% lower tracking error than PyRoki and 12x lower cross-seed variance than GMR. A ground-up codebase redesign for discoverability enabled LLM coding assistants to author up to 73% of new modules, including hand-optimized CUDA kernels, demonstrating that well-structured robotics code can unlock productive human-LLM collaboration. Together, these advances provide a unified, dynamics-aware motion generation stack that scales from single-arm manipulators to full humanoids. Code is available at https://github.com/NVlabs/curobo.

Balakumar Sundaralingam, Adithyavairavan Murali, Stan Birchfield

Robot Motion GenerationKinodynamic Motion PlanningRobot Collision AvoidanceTrajectory OptimizationSigned Distance Functions
AskPDF
Mar 4, 2026

Perception-Aware Time-Optimal Planning for Quadrotor Waypoint Flight

Agile quadrotor flight pushes the limits of control, actuation, and onboard perception. While time-optimal trajectory planning has been extensively studied, existing approaches typically neglect the tight coupling between vehicle dynamics, environmental geometry, and the visual requirements of onboard state estimation. As a result, trajectories that are dynamically feasible may fail in closed-loop execution due to degraded visual quality. This paper introduces a unified time-optimal trajectory optimization framework for vision-based quadrotors that explicitly incorporates perception constraints alongside full nonlinear dynamics, rotor actuation limits, aerodynamic effects, camera field-of-view constraints, and convex geometric gate representations. The proposed formulation solves minimum-time lap trajectories for arbitrary racetracks with diverse gate shapes and orientations, while remaining numerically robust and computationally efficient. We derive an information-theoretic position uncertainty metric to quantify visual state-estimation quality and integrate it into the planner through three perception objectives: position uncertainty minimization, sequential field-of-view constraints, and look-ahead alignment. This enables systematic exploration of the trade-offs between speed and perceptual reliability. To accurately track the resulting perception-aware trajectories, we develop a model predictive contouring tracking controller that separates lateral and progress errors. Experiments demonstrate real-world flight speeds up to 9.8 m/s with 0.07 m average tracking error, and closed-loop success rates improved from 55% to 100% on a challenging Split-S course. The proposed system provides a scalable benchmark for studying the fundamental limits of perception-aware, time-optimal autonomous flight.

Chao Qin, Jiaxu Xing, Rudolf Reiter +4

Authors are with the University of Toronto Institute for Aerospace Studies (UTIAS), Canada

UAV Trajectory OptimizationMotion PlanningTrajectory OptimizationQuadrotor Control
AskPDF
Mar 3, 2026

cuNRTO: GPU-Accelerated Nonlinear Robust Trajectory Optimization

Robust trajectory optimization enables autonomous systems to operate safely under uncertainty by computing control policies that satisfy the constraints for all bounded disturbances. However, these problems often lead to large Second Order Conic Programming (SOCP) constraints, which are computationally expensive. In this work, we propose the CUDA Nonlinear Robust Trajectory Optimization (cuNRTO) framework by introducing two dynamic optimization architectures that have direct application to robust decision-making and are implemented on CUDA. The first architecture, NRTO-DR, leverages the Douglas-Rachford (DR) splitting method to solve the SOCP inner subproblems of NRTO, thereby significantly reducing the computational burden through parallel SOCP projections and sparse direct solves. The second architecture, NRTO-FullADMM, is a novel variant that further exploits the problem structure to improve scalability using the Alternating Direction Method of Multipliers (ADMM). Finally, we provide GPU implementations of the proposed methodologies using custom CUDA kernels for SOC projection steps and cuBLAS GEMM chains for feedback gain updates. We validate the performance of cuNRTO through simulated experiments on unicycle, quadcopter, and Franka manipulator models, demonstrating speedups of up to 139.6×\times. More details are available at https://cunrto.github.io.

Jiawei Wang, Arshiya Taj Abdul, Evangelos A. Theodorou

University of California, San Diego · Georgia Institute of Technology, Atlanta

Second-Order Cone ProgrammingTrajectory OptimizationConstrained OptimizationGPU AccelerationAlternating Direction Method of Multipliers
AskPDF
Mar 1, 2026

Hippo: High-performance Interior-Point and Projection-based Solver for Generic Constrained Trajectory Optimization

Trajectory optimization is the core of modern model-based robotic control and motion planning. Existing trajectory optimizers, based on sequential quadratic programming (SQP) or differential dynamic programming (DDP), are often limited by their slow computation efficiency, low modeling flexibility, and poor convergence for complex tasks requiring hard constraints. In this paper, we introduce Hippo, a solver that can handle inequality constraints using the interior-point method (IPM) with an adaptive barrier update strategy and hard equality constraints via projection or IPM. Through extensive numerical benchmarks, we show that Hippo is a robust and efficient alternative to existing state-of-the-art solvers for difficult robotic trajectory optimization problems requiring high-quality solutions, such as locomotion and manipulation.

Haizhou Zhao, Ludovic Righetti, Majid Khadiv

Tandon School of Engineering, New York University, USA · Munich Institute of Robotics and Machine Intelligence (MIRMI), Technical University of Munich (TUM), Germany

Trajectory OptimizationConstrained Optimization
AskPDF
Feb 28, 2026

Online Generation of Collision-Free Trajectories in Dynamic Environments

In this paper, we present an online method for converting an arbitrary geometric path, represented by a sequence of states, and generated by any planner (e.g., sampling-based planners such as RRT or PRM, search-based planners such as ARA*, etc.), into a kinematically feasible, jerk-limited trajectory. The method generates a sequence of quintic/quartic splines that can be discretized at a user-specified control rate and streamed to a low-level robot controller. Our approach enables real-time adaptation to environmental changes and can be re-invoked at any instant to generate a new trajectory from the robot's current state to a desired target state or sequence of states. Under a bounded-obstacle-velocity assumption, the method provides conditional stopping-safety guarantees over a finite time interval in dynamic environments, while allowing bounded geometric deviation from the original path. Kinematic constraints, including jerk limits, are explicitly considered. We validate the approach in a comparative simulation study against a competing method, demonstrating favorable behavior w.r.t. smoothness, computational time, and real-time performance, particularly with frequent target-state changes (up to 1 [kHz]). Real-robot experiments demonstrate applicability in real-world scenarios, including scenarios with a human as an obstacle.

Nermin Covic, Bakir Lacevic

Faculty of Electrical Engineering, University of Sarajevo, Bosnia and Herzegovina

Constrained Motion PlanningTrajectory OptimizationTrajectory GenerationRobot Motion GenerationSafe Motion PlanningDynamic Obstacle Avoidance+1
AskPDF
Feb 25, 2026

Hierarchical Trajectory Planning of Floating-Base Multi-Link Robot for Maneuvering in Confined Environments

Floating-base multi-link robots can change their shape during flight, making them well-suited for applications in confined environments such as autonomous inspection and search and rescue. However, trajectory planning for such systems remains an open challenge because the problem lies in a high-dimensional, constraint-rich space where collision avoidance must be addressed together with kinematic limits and dynamic feasibility. This work introduces a hierarchical trajectory planning framework that integrates global guidance with configuration-aware local optimization. First, we exploit the dual nature of these robots - the root link as a rigid body for guidance and the articulated joints for flexibility - to generate global anchor states that decompose the planning problem into tractable segments. Second, we design a local trajectory planner that optimizes each segment in parallel with differentiable objectives and constraints, systematically enforcing kinematic feasibility and maintaining dynamic feasibility by avoiding control singularities. Third, we implement a complete system that directly processes point-cloud data, eliminating the need for handcrafted obstacle models. Extensive simulations and real-world experiments confirm that this framework enables an articulated aerial robot to exploit its morphology for maneuvering that rigid robots cannot achieve. To the best of our knowledge, this is the first planning framework for floating-base multi-link robots that has been demonstrated on a real robot to generate continuous, collision-free, and dynamically feasible trajectories directly from raw point-cloud inputs, without relying on handcrafted obstacle models.

Yicheng Chen, Jinjie Li, Haokun Liu +3

Department of Mechanical Engineering, The University of Tokyo, Tokyo 113-8656, Japan

Robot Motion PlanningMotion PlanningConstrained Motion PlanningTrajectory OptimizationAerial Robotics
AskPDF
Feb 24, 2026

Unified Complementarity-Based Contact Modeling and Planning for Soft Robots

Soft robots were introduced in large part to enable safe, adaptive interaction with the environment, and this interaction relies fundamentally on contact. However, modeling and planning contact-rich interactions for soft robots remain challenging: dense contact candidates along the body create redundant constraints and rank-deficient LCPs, while the disparity between high stiffness and low friction introduces severe ill-conditioning. Existing approaches rely on problem-specific approximations or penalty-based treatments. This letter presents a unified complementarity-based framework for soft-robot contact modeling and planning that brings contact modeling, manipulation, and planning into a unified, physically consistent formulation. We develop a robust Linear Complementarity Problem (LCP) model tailored to discretized soft robots and address these challenges with a three-stage conditioning pipeline: inertial rank selection to remove redundant contacts, Ruiz equilibration to correct scale disparity and ill-conditioning, and lightweight Tikhonov regularization on normal blocks. Building on the same formulation, we introduce a kinematically guided warm-start strategy that enables dynamic trajectory optimization through contact using Mathematical Programs with Complementarity Constraints (MPCC) and demonstrate its effectiveness on contact-rich ball manipulation tasks. In conclusion, CUSP provides a new foundation for unifying contact modeling, simulation, and planning in soft robotics.

Milad Azizkhani, Yue Chen

Woodruff School of Mechanical Engineering, Georgia Institute of Technology, Atlanta, GA, USA · Institute for Robotics and Intelligent Machines (IRIM), Georgia Institute of Technology, Atlanta, GA, USA · Coulter Department of Biomedical Engineering (Georgia Tech and Emory University), Georgia Institute of Technology, Atlanta, GA, USA

Soft RoboticsContact-Rich Robotic ManipulationConstrained OptimizationTrajectory Optimization
AskPDF
Feb 23, 2026

FACTO: Function-space Adaptive Constrained Trajectory Optimization for Robotic Manipulators

This paper introduces Function-space Adaptive Constrained Trajectory Optimization (FACTO), a new trajectory optimization algorithm for both single- and multi-arm manipulators. Trajectory representations are parameterized as linear combinations of orthogonal basis functions, and optimization is performed directly in the coefficient space. The constrained problem formulation consists of both an objective functional and a finite-dimensional objective defined over truncated coefficients. To address nonlinearity, FACTO uses a Gauss-Newton approximation with exponential moving averaging, yielding a smoothed quadratic subproblem. Trajectory-wide constraints are addressed using coefficient-space mappings, and an adaptive constrained update using the Levenberg-Marquardt algorithm is performed in the null space of active constraints. Comparisons with optimization-based planners (CHOMP, TrajOpt, GPMP2) and sampling-based planners (RRT-Connect, RRT*, PRM) show the improved solution quality and feasibility, especially in constrained single- and multi-arm scenarios. The experimental evaluation of FACTO on Franka robots verifies the feasibility of deployment.

Yichang Feng, Xiao Liang, Minghui Zheng

J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA · Zachry Department of Civil and Environmental Engineering, Texas A&M University, College Station, TX 77843 USA

Trajectory OptimizationRobotic OptimizationConstrained Motion PlanningRobotic Manipulation
AskPDF
Feb 23, 2026

CACTO-BIC: Scalable Actor-Critic Learning via Biased Sampling and GPU-Accelerated Trajectory Optimization

Trajectory Optimization (TO) and Reinforcement Learning (RL) offer complementary strengths for solving optimal control problems. TO efficiently computes locally optimal solutions but can struggle with non-convexity, while RL is more robust to non-convexity at the cost of significantly higher computational demands. CACTO (Continuous Actor-Critic with Trajectory Optimization) was introduced to combine these advantages by learning a warm-start policy that guides the TO solver towards low-cost trajectories. However, scalability remains a key limitation, as increasing system complexity significantly raises the computational cost of TO. This work introduces CACTO-BIC to address these challenges. CACTO-BIC improves data efficiency by biasing initial-state sampling leveraging a property of the value function associated with locally optimal policies; moreover, it reduces computation time by exploiting GPU acceleration. Empirical evaluations show improved sample efficiency and faster computation compared to CACTO. Comparisons with PPO demonstrate that our approach can achieve similar solutions in less time. Finally, experiments on the AlienGO quadruped robot demonstrate that CACTO-BIC can scale to high-dimensional systems and is suitable for real-time applications.

Elisa Alboni, Pietro Noah Crestaz, Elias Fontanari +1

Dept. of Industrial Engineering, University of Trento, Italy · LAAS-CNRS, Université de Toulouse, CNRS, Toulouse, France

Actor-Critic MethodsTrajectory OptimizationRobotic RLSample-Efficient RL
AskPDF
Feb 21, 2026

HONEST-CAV: Hierarchical Optimization of Network Signals and Trajectories for Connected and Automated Vehicles with Multi-Agent Reinforcement Learning

This study presents a hierarchical, network-level traffic flow control framework for mixed traffic consisting of Human-driven Vehicles (HVs), Connected and Automated Vehicles (CAVs). The framework jointly optimizes vehicle-level eco-driving behaviors and intersection-level traffic signal control to enhance overall network efficiency and decrease energy consumption. A decentralized Multi-Agent Reinforcement Learning (MARL) approach by Value Decomposition Network (VDN) manages cycle-based traffic signal control (TSC) at intersections, while an innovative Signal Phase and Timing (SPaT) prediction method integrates a Machine Learning-based Trajectory Planning Algorithm (MLTPA) to guide CAVs in executing Eco-Approach and Departure (EAD) maneuvers. The framework is evaluated across varying CAV proportions and powertrain types to assess its effects on mobility and energy performance. Experimental results conducted in a 4*4 real-world network demonstrate that the MARL-based TSC method outperforms the baseline model (i.e., Webster method) in speed, fuel consumption, and idling time. In addition, with MLTPA, HONEST-CAV benefits the traffic system further in energy consumption and idling time. With a 60% CAV proportion, vehicle average speed, fuel consumption, and idling time can be improved/saved by 7.67%, 10.23%, and 45.83% compared with the baseline. Furthermore, discussions on CAV proportions and powertrain types are conducted to quantify the performance of the proposed method with the impact of automation and electrification.

Ziyan Zhang, Changxin Wan, Peng Hao +5

College of Engineering, Center for Environmental Research and Technology, University of California at Riverside, Riverside, CA, 92507. · InfoTech Labs, Toyota Motor North America, Mountain View, CA, 94043.

Intelligent Transportation SystemsTraffic Signal ControlTrajectory OptimizationDecentralized MARL
AskPDF
Feb 18, 2026

Smooth trajectory generation and hybrid B-splines-Quaternions based tool path interpolation for a 3T1R parallel kinematic milling robot

This paper presents a smooth trajectory generation method for a four-degree-of-freedom parallel kinematic milling robot. The proposed approach integrates B-spline and Quaternion interpolation techniques to manage decoupled position and orientation data points. The synchronization of orientation and arc-length-parameterized position data is achieved through the fitting of smooth piece-wise Bezier curves, which describe the non-linear relationship between path length and tool orientation, solved via sequential quadratic programming. By leveraging the convex hull properties of Bezier curves, the method ensures spatial and temporal separation constraints for multi-agent trajectory generation. Unit quaternions are employed for orientation interpolation, providing a robust and efficient representation that avoids gimbal lock and facilitates smooth, continuous rotation. Modifier polynomials are used for position interpolation. Temporal trajectories are optimized using minimum jerk, time-optimal piece-wise Bezier curves in two stages: task space followed by joint space, implemented on a low-cost microcontroller. Experimental results demonstrate that the proposed method offers enhanced accuracy, reduced velocity fluctuations, and computational efficiency compared to conventional interpolation methods.

Sina Akhbari, Mehran Mahboubkhah

Intelligent Automation Centre, Wolfson School of Mechanical, Electrical & Manufacturing Engineering, Loughborough University, Loughborough, UK · Department of Mechanical Engineering, University of Tabriz, 51666-16471, Tabriz, Iran

Trajectory GenerationRoboticsRobot Motion GenerationIndustrial RoboticsTrajectory Optimization
AskPDF
Feb 17, 2026

Spatially-Aware Adaptive Trajectory Optimization with Controller-Guided Feedback for Autonomous Racing

We present a closed-loop framework for autonomous raceline optimization that combines NURBS-based trajectory representation, CMA-ES global trajectory optimization, and controller-guided spatial feedback. Instead of treating tracking errors as transient disturbances, our method exploits them as informative signals of local track characteristics via a Kalman-inspired spatial update. This enables the construction of an adaptive, acceleration-based constraint map that iteratively refines trajectories toward near-optimal performance under spatially varying track and vehicle behavior. In simulation, our approach achieves a 17.38% lap time reduction compared to a controller parametrized with maximum static acceleration. On real hardware, tested with different tire compounds ranging from high to low friction, we obtain a 7.60% lap time improvement without explicitly parametrizing friction. This demonstrates robustness to changing grip conditions in real-world scenarios.

Alexander Wachter, Alexander Willert, Marc-Philip Ecker +1

Automation & Control Institute (ACIN), TU Wien, Vienna, Austria · AIT, Austrian Institute of Technology GmbH, Vienna, Austria

Trajectory OptimizationAutonomous RacingConstrained OptimizationAdaptive Control
AskPDF
Feb 9, 2026

From Legible to Inscrutable Trajectories: (Il)legible Motion Planning Accounting for Multiple Observers

In cooperative environments, such as in factories or assistive scenarios, it is important for a robot to communicate its intentions to observers, who could be either other humans or robots. A legible trajectory allows an observer to quickly and accurately predict an agent's intention. In adversarial environments, such as in military operations or games, it is important for a robot to not communicate its intentions to observers. An illegible trajectory leads an observer to incorrectly predict the agent's intention or delays when an observer is able to make a correct prediction about the agent's intention. However, in some environments there are multiple observers, each of whom may be able to see only part of the environment, and each of whom may have different motives. In this work, we introduce the Mixed-Motive Limited-Observability Legible Motion Planning (MMLO-LMP) problem, which requires a motion planner to generate a trajectory that is legible to observers with positive motives and illegible to observers with negative motives while also considering the visibility limitations of each observer. We highlight multiple strategies an agent can take while still achieving the problem objective. We also present DUBIOUS, a trajectory optimizer that solves MMLO-LMP. Our results show that DUBIOUS can generate trajectories that balance legibility with the motives and limited visibility regions of the observers. Future work includes many variations of MMLO-LMP, including moving observers and observer teaming.

Ananya Yammanuru, Maria Lusardi, Nancy M. Amato +1

University of Illinois at Urbana-Champaign, Urbana IL 61801, USA

Trajectory OptimizationTrajectory GenerationMotion PlanningRobot Motion Planning
AskPDF
Jan 16, 2026

A Probabilistic Approach to Trajectory-Based Optimal Experimental Design

We present a novel probabilistic approach for optimal experimental path design. In this approach a discrete path optimization problem is defined on a static navigation mesh, and trajectories are modeled as random variables governed by a parametric Markov policy. The discrete path optimization problem is then replaced with an equivalent stochastic optimization problem over the policy parameters, resulting in an optimal probability model that samples estimates of the optimal discrete path. This approach enables exploration of the utility function's distribution tail and treats the utility function of the design as a black box, making it applicable to linear and nonlinear inverse problems and beyond experimental design. Numerical verification and analysis are carried out by using a parameter identification problem widely used in model-based optimal experimental design, namely a two-dimensional time-dependent advection diffusion problem in which the initial condition is the inference target. Experiments use both coarse and fine navigation meshes, with either a single moving sensor or a group of seven coordinated sensors, and the proposed approach is evaluated under D-, A-, and E-optimality criteria.

Ahmed Attia

Mathematics and Computer Science Division, Argonne National Lab, USA

Stochastic OptimizationTrajectory OptimizationBayesian Experimental Design
AskPDF
Nov 11, 2025

High-Altitude Balloon Station-Keeping with First Order Model Predictive Control

High-altitude balloons (HABs) are common in scientific research due to their wide range of applications and low cost. Because of their nonlinear, underactuated dynamics and the partial observability of wind fields, prior work has largely relied on model-free reinforcement learning (RL) methods to design near-optimal control schemes for station-keeping. These methods often compare only against hand-crafted heuristics, dismissing model-based approaches as impractical given the system complexity and uncertain wind forecasts. We revisit this assumption about the efficacy of model-based control for station-keeping by developing First-Order Model Predictive Control (FOMPC). By implementing the wind and balloon dynamics as differentiable functions in JAX, we enable gradient-based trajectory optimization for online planning. FOMPC outperforms a state-of-the-art RL policy, achieving a 24% improvement in time-within-radius (TWR) without requiring offline training, though at the cost of greater online computation per control step. Through systematic ablations of modeling assumptions and control factors, we show that online planning is effective across many configurations, including under simplified wind and dynamics models.

Myles Pasetsky, Jiawei Lin, Bradley Guo +1

Cornell University · University of California San Diego

Trajectory OptimizationModel Predictive Control
AskPDF
Oct 30, 2025

ART-TEB: Adaptive Trajectory Planning for Mobile Robots in Cluttered Environments

Trajectory planning for mobile robots remains a major challenge, particularly in cluttered environments, where existing planning algorithms often fail or produce suboptimal paths. To address this issue, we propose an adaptive trajectory refinement algorithm, ART-TEB, comprising two main stages. First, to ensure safety at the path-segment level, a segment-wise conservative collision test is applied, recursively subdividing risky path segments until the collision risk is eliminated. Second, to guarantee pose-level safety, pose correction based on separation direction and line search is applied, ensuring that each pose in the trajectory is collision-free and maximally clear from obstacles. Simulation results demonstrate that the ART-TEB achieves up to 3.12x higher success rates and up to 23.7x faster average planning times than state-of-the-art approaches. Furthermore, real-world experiments confirm that the robot can safely pass through highly constrained environments while maintaining rapid planning performance.

Hahjin Lee, Young J. Kim

Department of Computer Science and Engineering at Ewha Womans University in Korea

Trajectory OptimizationTrajectory GenerationMobile RoboticsSafe Motion PlanningRobot Motion Planning
AskPDF
Oct 23, 2025

Learning On The Job: Zero-Shot Task Execution under Parametric Uncertainty via Trajectory-Parametrized Dual Control

Model-based control can achieve reliable task performance, but its effectiveness depends on the accuracy of the underlying model. Robots operating under model uncertainty must often adapt to previously unseen payloads, objects, and interaction dynamics to complete a task successfully. Conventional approaches typically rely on a dedicated task- and control-agnostic excitation phase for estimating physics parameters, delaying task execution and collecting potentially irrelevant data. In this paper, we instead consider zero-shot task execution under parametric uncertainty, where online model learning and control proceed concurrently during task execution. Our approach formulates reference generation within a dual control framework to produce task-relevant, informative trajectories that reduce parameter uncertainty in directions critical to task success. We predefine a feedback policy with an explicit parameter adaptation law and optimize the reference through the resulting adaptive closed-loop dynamics. We propose two formulations: 1) minimizing expected task cost under parameter uncertainty, and 2) minimizing optimality loss, which quantifies the degradation in task performance caused by planning with an incorrect parameter estimate. By actively optimizing for informative trajectories via a natural Fisher information measure, we tightly approach a lower bound on task-relevant parameter uncertainty while simultaneously achieving reliable task execution. Across diverse tasks, controllers, and adaptation laws, targeted exploration enables the identification required for successful task execution, allowing robots to learn relevant physical parameters while completing the task in a single attempt.

Victor Vantilborgh, Hrishikesh Sathyanarayan, Guillaume Crevecoeur +2

Department of Electromechanical, Systems and Metal Engineering, Ghent University, Belgium · Department of Mechanical Engineering, Yale University, USA

Robotic ControlOnline System IdentificationTrajectory OptimizationAdaptive Control
AskPDF
Jul 23, 2025

LLM-Aided Joint Secrecy Precoding and Trajectory for RSMA-Based Heterogeneous UAV Networks

This paper investigates secure communications in rate-splitting multiple access (RSMA) enabled heterogeneous UAV networks, where multiple UAVs collaboratively serve ground terminals in the presence of eavesdroppers. By jointly considering secrecy rate maximization and propulsion energy consumption minimization, we formulate a multi-objective optimization problem involving UAV trajectory design, service association, power allocation, and secrecy precoding under mobility, collision-avoidance, service-capacity, and communication constraints. The formulated problem is highly non-convex due to the coupling among UAV trajectories, RSMA transmission variables, and secrecy constraints. To address the resulting non-convex and highly coupled optimization problem, we propose a hierarchical optimization framework. The inner layer uses a semidefinite relaxation (SDR)-based S2DC algorithm combining penalty functions and difference-of-convex (D.C.) programming to solve the secrecy precoding problem with fixed UAV positions. The outer layer introduces a Large Language Model (LLM)-guided heuristic multi-agent reinforcement learning approach (LLM-HeMARL) for trajectory optimization. LLM-HeMARL efficiently incorporates LLM-generated expert heuristic policy, enabling UAVs to learn energy-aware, security-driven trajectories without the inference overhead of real-time LLM calls. The simulation results show that our method outperforms existing baselines in secrecy rate and energy efficiency, with consistent robustness across varying UAV swarm sizes and random seeds.

Lijie Zheng, Ji He, Shih Yu Chang +1

School of Computer Science and Technology, Xidian University, Xi’an, 710071 China · Department of Applied Data Science, San Jose State University, San Jose, CA, U. S. A.

Trajectory OptimizationWireless CommunicationsUAV Trajectory OptimizationWireless Resource Allocation
AskPDF
Jul 18, 2025

NeHMO: Neural Hamilton-Jacobi Reachability Learning for Decentralized Safe Multi-Arm Motion Planning

Safe multi-arm motion planning is a challenging problem in robotics due to its high dimensionality, coupled configuration space, and complex collision constraints. Centralized planners are capable of coordinating all arms but often face scalability limitations, restricting applicability in real-time settings. On the other hand, decentralized methods are scalable and recent deep learning-based approaches have shown promising results. However, these depend on accurate behavior prediction or coordination protocols and may fail when other arms act unpredictably. To address these challenges, we introduce a neural Hamilton-Jacobi Reachability (HJR) learning-based approach to approximate a safety value function that captures worst-case inter-arm safety constraints. We further develop a decentralized trajectory optimization framework that uses the learned HJR representation for real-time planning. The proposed method is scalable and data-efficient, generalizes across multi-manipulator systems, and outperforms state-of-the-art baselines on challenging multi-arm motion planning tasks.

Qingyi Chen, Zachary Kingston, Ahmed H. Qureshi

Department of Computer Science at Purdue University, West Lafayette, IN, USA

Multi-Robot Motion PlanningConstrained Motion PlanningTrajectory OptimizationReachability AnalysisSafe Motion PlanningDecentralized Multi-Agent Motion Planning
AskPDF
Jul 10, 2025

SCREP: Scene Coordinate Regression and Evidential Learning-based Perception-Aware Trajectory Generation

Autonomous flight in GPS-denied indoor spaces requires trajectories that keep visual-localization error tightly bounded across varied missions. Map-based visual localization methods such as feature matching require computationally intensive map reconstruction and have feature-storage scalability issues, especially for large environments. Scene coordinate regression (SCR) provides an efficient learning-based alternative that directly predicts3D coordinates for every pixel, enabling absolute pose estimation with significant potential for onboard roboticsapplications. We present a perception-aware trajectory planner that couples an evidential learning-based SCR poseestimator with a receding-horizon trajectory optimizer. The optimizer steers the onboard camera toward reliablescene coordinates with low uncertainty, while a fixed-lag smoother fuses the low-rate SCR pose estimates with high-rate IMU data to provide a high-quality, high-rate pose estimate. In simulation, our planner reduces translationand rotation RMSE by at least 4.9% and 30.8% relative to baselines, respectively. Hardware-in-the-loop experiments validate the feasibility of our proposed trajectory planner under close-to-real deployment conditions.

Juyeop Han, Lukas Lao Beyer, Guilherme V. Cavalheiro +1

Laboratory for Information and Decision Systems (LIDS), Massachusetts Institute of Technology, Cambridge, MA 02139, USA

Trajectory OptimizationScene Coordinate RegressionMotion PlanningRobot State Estimation
AskPDF
Jun 27, 2025

An Introduction to Zero-Order Optimization Techniques for Robotics

Zero-order optimization techniques are becoming increasingly popular in robotics due to their ability to handle non-differentiable functions and escape local minima. These advantages make them particularly useful for trajectory optimization and policy optimization. In this work, we propose a mathematical tutorial on random search. It offers a simple and unifying perspective for understanding a wide range of algorithms commonly used in robotics. Leveraging this viewpoint, we classify many trajectory optimization methods under a common framework and derive novel competitive RL algorithms.

Armand Jordana, Jianghan Zhang, Joseph Amigo +1

Machines in Motion Laboratory, New York University, USA

Zeroth-Order OptimizationRobotic OptimizationRobotic RLTrajectory Optimization
AskPDF
May 8, 2025

Deep Learning Warm Starts for Trajectory Optimization on the International Space Station

Trajectory optimization is a cornerstone of modern robot autonomy, enabling systems to compute trajectories and controls in real-time while respecting safety and physical constraints. However, it has seen limited usage in spaceflight applications due to its heavy computational demands that exceed the capability of most flight computers. In this work, we provide results on the first in-space demonstration of using machine learning-based warm starts for accelerating trajectory optimization for the Astrobee free-flying robot onboard the International Space Station (ISS). We formulate a data-driven optimal control approach that trains a neural network to learn the structure of the trajectory generation problem being solved using sequential convex programming (SCP). Onboard, this trained neural network predicts solutions for the trajectory generation problem and relies on using the SCP solver to enforce safety constraints for the system. Our trained network reduces the number of solver iterations required for convergence in cases including rotational dynamics by 60% and in cases with obstacles drawn from the training distribution of the warm start model by 50%. This work represents a significant milestone in the use of learning-based control for spaceflight applications and a stepping stone for future advances in the use of machine learning for autonomous guidance, navigation, & control.

Somrita Banerjee, Abhishek Cauligi, Marco Pavone

Somrita Banerjee is a Machine Learning Researcher at Apple. Work completed during PhD at Stanford University, Stanford, CA 94305, USA. · Abhishek Cauligi is an Assistant Professor in the Department of Mechanical Engineering, Johns Hopkins University, Baltimore, MD 21218, USA. · Marco Pavone is an Associate Professor in the Department of Aeronautics and Astronautics, Stanford University, Stanford, CA 94305, USA.

Trajectory OptimizationLearning to OptimizeSpace Robotics
AskPDF
Mar 15, 2025

D4orm: Multi-Robot Trajectories with Dynamics-aware Diffusion Denoised Deformations

This work presents an optimization method for generating kinodynamically feasible and collision-free multi-robot trajectories that exploits an incremental denoising scheme in diffusion models. Our key insight is that high-quality trajectories can be discovered merely by denoising noisy trajectories sampled from a distribution. This approach has no learning component, relying instead on only two ingredients: a dynamical model of the robots to obtain feasible trajectories via rollout, and a fitness function to guide denoising with Monte Carlo gradient approximation. The proposed framework iteratively optimizes a deformation for the previous trajectory with the current denoising process, allows anytime refinement as time permits, supports different dynamics, and benefits from GPU acceleration. Our evaluations for differential-drive and holonomic teams with up to 16 robots in 2D and 3D worlds show its ability to discover high-quality solutions faster than other black-box optimization methods such as MPPI. In a 2D holonomic case with 16 robots, it is almost twice as fast. As evidence for feasibility, we demonstrate zero-shot deployment of the planned trajectories on eight multirotors. Code and video: https://github.com/proroklab/d4orm

Yuhao Zhang, Keisuke Okumura, Heedo Woo +2

University of Cambridge · UK · National Institute of Advanced Industrial Science and Technology (AIST) +1

Multi-Robot Motion PlanningConstrained Motion PlanningTrajectory OptimizationKinodynamic Motion PlanningTrajectory GenerationDiffusion-Based Planning+3
AskPDF