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

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

May 27, 2026

Integrated Exploration-Aware UAV Route Optimization and Path Planning

Uncrewed aerial vehicles (UAVs) are increasingly used for exploration-driven monitoring in hazardous environments such as disaster zones, contaminated sites, wildfire areas, and damaged infrastructure, where limited flight endurance must be allocated between visiting reported locations and gathering new information. In these settings, prior information regarding hazards is often incomplete, spatially imprecise, and subject to change during execution. For example, initial reports may identify a region where a hazard is likely to exist, but the actual hazard may be displaced, partially observed, or entirely unreported. We present an integrated exploration-aware UAV route optimization and path planning framework for hazard monitoring under uncertain and evolving prior information. The environment is represented as a spatial risk map, where each location has an associated belief of hazardous conditions. Reported hazards are modeled as uncertain regions of interest (ROIs) rather than confirmed target locations, requiring the UAV to inspect reported areas while also using its limited flight endurance to explore informative regions. The proposed method solves a vehicle routing problem over reported ROIs, augments the route with auxiliary pseudo-nodes to improve spatial coverage, allocates the remaining flight distance budget across route segments, and optimizes dynamically feasible B-spline trajectories for local exploration. During execution, UAV measurements update a grid-based belief map, and the remaining trajectory is replanned when new information and the remaining budget justify adaptation. Across 48 scenario configurations, online replanning improves average KL reduction by 15.9% over the offline optimized planner and 48.6% over straight-line traversal.

Jimin Choi, Grant Stagg, Cameron K. Peterson +1

Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI 48109, USA · Department of Electrical Engineering, Brigham Young University, Provo, UT 84602, USA · Department of Aerospace Engineering, Department of Civil and Environmental Engineering, and Department of Industrial and Operations Engineering, University of Michigan, Ann Arbor, MI 48109, USA

Belief-Space PlanningTrajectory OptimizationInformative Path PlanningUAV NavigationVehicle RoutingUAV Trajectory Optimization+1
AskPDF
May 27, 2026

Natural Functional Gradients for Smooth Trajectory Optimization

Generating collision-free and smooth motions remains a central challenge in robotic manipulation, particularly in cluttered environments and narrow passages where feasible regions are highly constrained and fragmented. We propose a trajectory optimization framework that performs geometry-aware updates directly in function space using natural functional gradients. The method optimizes a Gaussian-smoothed surrogate objective that regularizes the optimization landscape through smooth trajectory perturbations while preserving trajectory-level structure. Because the updates are defined intrinsically in function space, trajectory regularity can be controlled independently of a particular time discretization. We derive a practical Monte-Carlo estimator of the natural functional gradient that requires only black-box trajectory evaluations, making the method applicable when analytic gradients are unavailable or unreliable due to collision checking and contact-rich simulation. Experiments on constrained robotic manipulation tasks demonstrate that the proposed method improves trajectory feasibility and produces smoother motions than representative planning and trajectory optimization baselines in environments with narrow geometric clearances. Additional results, videos, and implementation details are available at the project page: https://kisangpark.github.io/natural-functional-gradient/

Kisang Park, Chanwoo Kim, Kyungjae Lee +1

Department of Artificial Intelligence, Korea University, Seoul, Republic of Korea · Department of Statistics, Korea University, Seoul, Republic of Korea · 3RLWRLD, Seoul, Republic of Korea

Constrained Motion PlanningTrajectory OptimizationTrajectory GenerationRobotic ManipulationRobot Motion PlanningRobotic Optimization
AskPDF
May 26, 2026

Pretrained Approximators for Low-Thrust Trajectory Cost and Reachability

Low-thrust trajectory design relies heavily on repeated evaluations of fuel consumption and transfer feasibility, which require expensive optimal control solutions. In this work, we show these quantities can be accurately approximated by machine learning surrogates, enabling fast and scalable evaluation across a wide range of scenarios. By increasing both dataset size and model capacity, we observe that low-thrust trajectory optimization follows a scaling law, with performance improving linearly with the logarithm of training data and network parameters, and no evidence of saturation within the explored regime. Guided by this observation, we construct a large-scale dataset using the proposed homotopy-ray strategy tailored to mission design requirements. A key is the introduction of a self-similar transformation, which allows generalization across semi-major axes, inclinations, and central bodies avoiding retraining. As a result, the same neural approximator can be applied to diverse orbital environments and mission classes. The proposed models accurately predict optimal fuel consumption and minimum transfer time for single- and multi-revolution transfers. Their performance and generalization are demonstrated on a public dataset, a multi-asteroid flyby problem from the Global Trajectory Optimization Competition, and an asteroid rendezvous mission design. The models and datasets are released as open-source to support the space community.

Zhong Zhang, Giacomo Acciarini, Dario Izzo +2

Politecnico di Milano · European Space Agency · Tsinghua University

Neural Surrogate ModelingTrajectory OptimizationNeural Scaling Laws
AskPDF
May 20, 2026

Mind the Gaps: Multi-Robot Feedback-Driven Ergodic Coverage in Unknown Environments

In this work, we address the problem of multi-robot adaptive coverage, where teams of robots perform dynamic sampling by continuously adjusting their positions to collect data in an environment. This task can be challenging, particularly when robots must be efficiently allocated to new sampling locations over time. Ergodic search methods optimize robot trajectories by ensuring that the robots' time-averaged spatial distribution aligns with the spatial distribution of environmental information. While these methods promote effective exploration provided a target distribution, they often fail to account for unknown prior distributions of the environment. To overcome this limitation, we propose an adaptive coverage strategy that utilizes real-time feedback from an environmental model to adjust robot sampling behavior in response to unknown conditions. Our approach enhances traditional ergodic trajectory optimization by constructing a target spatial information distribution based on parametric models of the environment, which are updated online. This strategy assumes that the environment is either static or changes slowly compared to the robot's motion. Our framework allows robots to dynamically prioritize regions of high interest, improving coverage efficiency, synthesizing effective control policies for individual agents, and optimizing resource use in settings with unknown prior distributions. We validate our approach through simulations, demonstrating its effectiveness in enhancing coverage and resource allocation.

Thales Costa Silva, Nora Ayanian

Department of Computer Science at Brown University, Providence-RI, USA.

Trajectory OptimizationMulti-Robot SystemsExploration-Exploitation TradeoffMulti-Robot Coordination
AskPDF
May 19, 2026

Trajectory Planning and Control near the Limits: an Open Experimental Benchmark on the RoboRacer Platform

We present a modular framework to benchmark new and existing methods for trajectory planning and control in high-acceleration maneuvers that push autonomous driving to the limits. Our framework includes time-optimal raceline generation, online time-optimal velocity replanning, geometric path tracking controllers, and a new model-structured neural network (MS-NN) to learn the inverse dynamics for steering control. We deploy our framework on a 1:10-scale RoboRacer platform, using two circuits. Through several ablations with cautious and aggressive racelines, we study the performance of single modules and their combinations. We show that our MS-NN significantly improves tracking accuracy, decreases steering oscillations, and is physically interpretable. Moreover, online velocity replanning improves lap times by compensating for execution errors, and enables the vehicle to safely reach higher speeds and accelerations. To support future research, our code, datasets, videos and results are publicly available at https://roboracer-benchmark.github.io/planning_control_benchmark/.

Mattia Piccinini, Patrick Zambiasi, Aniello Mungiello +3

Professorship of Autonomous Vehicle Systems, Technical University of Munich, 85748 Garching, Germany; Munich Institute of Robotics and Machine Intelligence (MIRMI). · Avilus GmbH, Germany. · Department of Information Technology and Electrical Engineering (DIETI), University of Naples Federico II, Naples 80125, Italy. +1

Autonomous Driving BenchmarksRobot Trajectory TrackingTrajectory OptimizationTrajectory GenerationAutonomous RacingVehicle Dynamics+1
AskPDF
May 19, 2026

Learning-Accelerated Optimization-based Trajectory Planning for Cooperative Aerial-Ground Handover Missions

This paper presents a learning-augmented trajectory planning framework for cooperative unmanned aerial vehicle (UAV) and unmanned ground vehicle (UGV) handover missions. While centralized trajectory optimization ensures dynamic feasibility and task optimality, its high computational cost limits real-time applicability. We propose a neural surrogate planner utilizing decoupled encoder-decoder long short-term memory (LSTM) networks to generate coordinated handover trajectory predictions from the task specifications. These predictions serve as informed warm starts for the downstream centralized optimizer, thereby accelerating convergence to dynamically feasible solutions. Benchmark evaluations demonstrate that the learning-augmented planning framework achieves more than a threefold speedup and 100% optimization success rate compared to cold start optimization. The results indicate that combining data-driven inference with model-based refinement enables fast and reliable trajectory generation for heterogeneous multi-robot systems.

Jingshan Chen, Bochen Yu, Henrik Ebel +1

Institute of Engineering and Computational Mechanics, University of Stuttgart, 70569 Stuttgart, Germany · Mechanical Engineering, LUT University, 53850 Lappeenranta, Finland

Multi-Robot Motion PlanningTrajectory OptimizationTrajectory GenerationMulti-Robot CoordinationUAV Trajectory OptimizationLearning-Augmented Algorithms
AskPDF
May 18, 2026

Orchestrating Wheeled Mobile Robots Online without Conflicts

Formation control of wheeled mobile robots (WMRs) has been widely studied due to its broad applications. However, existing studies primarily focus on tracking predefined formations, with limited adaptability to varying environments. To address this limitation, we propose CFOO, a Conflict-Free Online Orchestration framework for continuous formation navigation of WMRs that integrates centralized formation generation with distributed formation maintenance. The upper layer monitors the environment in real time and generates adaptive formations as needed, using the proposed TCF-R2T (Trajectory-Conflict-Free Robot-to-Target assignment) algorithm to compute optimal assignments in polynomial time for timely, conflict-free formation transitions. At the lower layer, each WMR continuously employs JSTP (Joint Spatio-Temporal trajectory Planning) to maintain the given formation by jointly optimizing spatial positions and segment durations, improving coordination over spatial-only optimization with fixed durations. By coordinating these two layers, CFOO orchestrates WMRs to adapt their formations to varying environments and thus enable continuous navigation. Moreover, we design a Time-Varying Formation Reference (TVFR) mechanism to facilitate smooth transitions between successive desired formations. Both simulation and real-world experiments validate the effectiveness and adaptability of CFOO. Experimental videos are available on our project website: https://github.com/dongjh20/CFOO.

Jianghong Dong, Yifeng Zhang, Wenhan Cao +4

School of Vehicle and Mobility, Tsinghua University, Beijing 100084, China · Department of Mechanical Engineering, National University of Singapore, 117575, Singapore · Department of Civil and Environmental Engineering, University of Michigan, Ann Arbor, MI 48109, USA

Multi-Robot Formation ControlMulti-Robot Motion PlanningTrajectory OptimizationMulti-Robot Coordination
AskPDF
May 18, 2026

CosFly-Track: A Large-Scale Multi-Modal Dataset for UAV Visual Tracking via Multi-Constraint Trajectory Optimization

Recent aerial vision-language navigation (VLN) datasets have grown rapidly, but they primarily address goal-oriented navigation to static destinations, leaving UAV visual tracking -- continuously following a moving target while maintaining visibility -- largely without dedicated training data. We introduce CosFlyTrack, a large-scale multi-modal dataset and scalable generation pipeline for UAV visual tracking in urban environments. The dataset provides approximately 12,000 expert and perturbed UAV trajectories generated from 6,000 pedestrian paths, comprising 2.4 million timesteps (approximately 334 hours) with seven aligned data channels: RGB, metric depth, semantic segmentation, six-degree-of-freedom drone pose, target state with visibility flag, bilingual (Chinese-English) instructions, and trajectory-pair metadata. To generate high-quality expert trajectories, we develop MuCO, a multi-constraint optimizer that plans directly in continuous three-dimensional space with BVH-accelerated collision and visibility queries, jointly enforcing target visibility, viewpoint quality, collision avoidance, smoothness, and kinematic feasibility, avoiding the discretization artifacts and post-hoc smoothing of grid-based planners. Fine-tuning experiments on seven vision-language models show that CosFlyTrack improves tracking performance to 78.3 to 95.6 percent SR@1 meter, a 53 to 69 percentage point gain over zero-shot baselines, supporting the dataset as a training resource for dynamic target-following agents. The dataset is publicly available at https://huggingface.co/datasets/AutelRobotics/CosFly; evaluation scripts and pre-trained checkpoints are hosted at https://huggingface.co/AutelRobotics/CosFly-Track.

Xiangyue Wang, Hanxuan Chen, Songsheng Cheng +7

Autel Robotics · Nanjing University · Peking University +2

Aerial RoboticsTrajectory OptimizationVisual Object TrackingSynthetic Data Generation for RoboticsUAV Trajectory OptimizationVLMs for Robotics
AskPDF
May 15, 2026

Structured Jacobian Construction for Motion Optimization with High-Order Time Derivatives in Multi-Link Systems

This paper presents a novel framework for Jacobian computation in motion optimization problems involving multi-link systems, where physical quantities are represented using higher-order time derivatives. In motion optimization of robots and humans, cost functions may incorporate higher-order time derivatives, such as jerk or the time variation of forces, to capture smoothness and perceptual characteristics, particularly in motion skill analysis and expressive behaviors, thereby necessitating Jacobian computations involving these quantities. However, such Jacobians are typically computed using numerical or automatic differentiation without explicitly exploiting the underlying multi-link structure, which can lead to increased computational cost and numerical instability. To address this limitation, we propose a structured Jacobian formulation for motion optimization, based on the comprehensive motion computation framework, in which physical quantities and their higher-order time derivatives are systematically represented along the multi-link structure. The proposed method systematically derives analytical expressions for Jacobians of kinematic and dynamic quantities, including momentum, forces, and joint torques, with respect to generalized coordinates and their higher-order derivatives. The resulting framework is applicable to both direct and inverse optimization. Through numerical experiments, we demonstrate that the proposed method improves computational efficiency compared to numerical and automatic differentiation, while achieving comparable accuracy. Furthermore, we demonstrate its effectiveness in inverse optimization by recovering cost function weights from motion data. Together, these results indicate that the proposed formulation provides a scalable and structured computational foundation for motion optimization involving higher-order time derivatives in multi-link systems.

Taiki Ishigaki, Ko Ayusawa, Eiichi Yoshida

Tokyo University of Science · National Institute of Advanced Industrial Science and Technology

Trajectory OptimizationMultibody Dynamics
AskPDF
May 15, 2026

Wind-Aware Optimal Trajectory Planning for Efficient Gliding of Fixed-Wing Aerial Systems

Gliding offers small fixed-wing UAVs extended endurance and silent operation but requires accurate energy management, especially under wind disturbances and obstacle constraints. Traditional Total Energy Control Systems based controllers regulate the trade between potential and kinetic energy reactively, often requiring fine-tuning and trim-conditions knowledge. In this work, we shift the regulation to the planning level and present a nonlinear, multi-cost trajectory planner for small UAV gliders. The method generates C3\mathcal{C}^3 continuous trajectories based on Bernstein polynomials, mapped into control commands through differential flatness, and re-planned online to match experimentally derived sink polar curves. A simulated netto variometer is integrated into the optimization to estimate air mass motion, constraining the glide to energy-balanced states. Consecutive gliding trajectories are linked by cruising segments computed through trajectories initialized on Dubins path-based waypoints, enabling hybrid missions that combine powered and unpowered flight. The approach is validated in CFD simulations and real-world experiments with a fixed-wing platform, showing reliable stabilization of sink rate, airspeed, and glide ratio under wind gusts and in presence of obstacles.

Luca Morando, Nishanth Bobbili, Giuseppe Loianno

New York University, NY 10012, USA · University of California Berkeley, Department of Electrical Engineering and Computer Sciences, Berkeley, CA 94720, USA

Aerial RoboticsTrajectory OptimizationUAV Trajectory Optimization
AskPDF
May 14, 2026

Behavioral Data-Driven Optimal Trajectory Generation for Rotary Cranes

With the growth of the construction industry and the global shortage of skilled labor, the automation of crane control has become increasingly important for safe and efficient operations. A central challenge in automatic crane control is the reduction of load oscillations during motion, which is primarily addressed through appropriate slewing trajectories. In this context, classical model-based control methods rely on accurate dynamical models and expert tuning, and often struggle to meet safety and precision requirements, while many learning-based approaches require large data sets and significant computational resources. This paper proposes a behavioral data-driven framework for generating open-loop slewing trajectories for rotary cranes that suppress load sway while reducing operation time and energy consumption. The approach builds on Willems' fundamental lemma and its generalizations, to bypass explicit system modeling and operate directly on measured input-output data. A practical workflow is presented in this paper to reduce the need for expert knowledge. Despite the underactuated nature of the crane dynamics, the method identifies a nonparametric representation of the system behavior and generates smooth, optimal trajectories using limited data and convex optimization. The proposed trajectory generation method is validated on a laboratory crane setup and compared against an established model-based approach, achieving up to 35% reduction in load sway, 43% reduction in tracking error, and 50% reduction in travel time.

Iskandar Khemakhem, Manuel Zobel, Johannes Schüle +3

Institute for Adaptive Mechanical Systems, University of Stuttgart, Stuttgart, 70569, Germany · Institute for System Dynamics, University of Stuttgart, Stuttgart, 70563, Germany · Department of Mechanical Engineering, Toyohashi University of Technology, Toyohashi, 441-8580, Japan +1

Robotic ControlTrajectory OptimizationData-Driven Control
AskPDF
May 13, 2026

Motion Planning for Autonomous Vehicles using Optimization over Graphs of Convex Sets

Motion planning for autonomous vehicles requires generating collision-free and dynamically feasible trajectories in complex environments under real-time constraints. While nonlinear optimal control formulations provide high-fidelity solutions, they are computationally demanding and sensitive to initialization, whereas geometric planning methods scale well but often decouple path selection from trajectory optimization. This paper studies the extent to which optimization over Graphs of Convex Sets (GCS) can approximate solutions of nonlinear optimal control problems in the context of autonomous driving. The free space is represented as a finite union of convex regions organized as a directed graph, allowing nonconvex geometry to be handled through discrete connectivity decisions while maintaining convex trajectory constraints within each region. Vehicle motion is parameterized using Bezier curves for the spatial path and a polynomial time-scaling function for temporal evolution. Under small-slip and linear tire assumptions, a simplified dynamic bicycle model enables approximate enforcement of dynamic feasibility through convex constraints on trajectory derivatives. The approach is evaluated in CommonRoad scenarios involving static obstacle avoidance and lane-changing maneuvers, and is compared against a nonlinear discrete-time optimal control formulation. The results indicate that the GCS-based method generates collision-free and dynamically consistent trajectories that closely match those obtained from the nonlinear program, while exhibiting improved computational efficiency and reduced sensitivity to initialization. These findings suggest that GCS provides a structured approximation of nonlinear motion planning problems, capturing dominant geometric and dynamic effects while preserving convexity in the continuous relaxation.

Matheus Wagner, Antônio Augusto Fröhlich

Software/Hardware Integration Lab, Federal University of Santa Catarina, Florian´opolis, Brazil

Trajectory OptimizationAutonomous DrivingSafe Motion PlanningAutonomous Driving Planning
AskPDF
May 13, 2026

Ergodic Trajectory Design by Learned Pushforward Maps: Provable Coverage via Conditional Flow Matching

Designing continuous trajectories whose time-averaged occupancy provably matches a prescribed spatial density (the ergodic coverage problem) is central to UAV-assisted data collection and sensing, robotic exploration, and mobile monitoring. For flying agents in particular, this challenge is acute: trajectories must balance coverage fidelity against tight energy budgets, no-fly zones, and acceleration limits. Existing methods either re-optimize each trajectory online (with cost growing in the horizon and re-running for every target, agent, and realization) or rely on bespoke analytical constructions that must be re-derived for each new constraint. We propose a epushforward framework that decouples ergodicity from density matching: an analytic latent trajectory provides exact uniform ergodicity on a simple annular domain, and a single map, learned offline by optimal-transport conditional flow matching, transports this latent occupancy onto the prescribed target density. The composed trajectory is then asymptotically ergodic with respect to the learned pushforward distribution, with deviation from the target controlled by the flow-matching training loss. Once trained for a given target density and constraint set, the map serves an unbounded number of trajectories and a multi-agent fleet without per-agent retraining, and many differentiable operational constraints (no-fly zones, acceleration ceilings, or fairness penalties) enter as additive soft penalties in the training loss without re-deriving the design. We prove three results (an acceleration-energy bound, an O(1/K)O(1/\sqrt{K}) ergodic convergence rate in the number of trajectory cycles KK, and an approximation-error bound) that combine into an end-to-end coverage bound estimable from CFM training diagnostics (certified given an architectural Lipschitz bound on vθv_θ).

Ehsan Aghazadeh, Masoud Malekzadeh, Ahmad Ghasemi +1

University of Massachusetts Amherst

Trajectory OptimizationTrajectory GenerationUAV Trajectory Optimization
AskPDF
May 10, 2026

Mismatch-Aware Adaptive Constraint Tightening for Bicycle-Model Trajectory Optimization

Trajectory optimization for autonomous vehicles usually relies on the kinematic bicycle model because of its computational simplicity. However, when the planned trajectory is executed under the true vehicle dynamics, which include lateral slip, tire stiffness and yaw-lateral coupling, safety constraints can be violated owing to the model mismatch. In this paper, we make three theoretical contributions. First, we derive a characteristic speed vc=CαL/Mv_c=\sqrt{C_αL/M} which separates two different mismatch regimes: below vcv_c the dynamic bicycle initially oversteers inward (safe); above vcv_c it understeers outward (safety-critical). Second, we prove that the peak outward deviation ε∗\varepsilon^* follows a T2T^2 horizon scaling whose coefficient transitions between a transient bound 12(v2−vc2)κ\frac{1}{2}(v^2-v_c^2)κ and a steady-state bound. Third, we obtain a simulation-free analytical coefficient a2anal=12(1−vc2/vmax⁡2)T2a_2^{\mathrm{anal}}=\frac{1}{2}(1-v_c^2/v_{\max}^2)T^2 that is computable from vehicle parameters and the planning horizon alone. Putting these together, we propose Mismatch-Aware Adaptive Constraint Tightening (MACT), ε(v,κ)=a2v2∣κ∣ε(v,κ)=a_2 v^2|κ|, which replaces a fixed worst-case margin by a state-dependent one that is large at high speed/curvature but nearly zero on gentle paths. Eight numerical experiments confirm the scaling laws. MACT reaches 100% safety with 84% less wasted margin than a fixed-margin baseline on the 2-DOF vehicle, extends to a nonlinear leaning bicycle, and in a closed-loop direct-shooting MPC comparison it cuts the applied margin by 34% compared with tube MPC while keeping the same safety.

Lingxue Lyu, Zihui Liu

School of Engineering and Applied Science, University of Pennsylvania, Philadelphia, PA 19104, USA · Department of Aeronautics & Astronautics, Stanford University, Stanford, CA 94305, USA

Trajectory OptimizationModel Predictive ControlVehicle DynamicsConstrained OptimizationAutonomous Driving Planning
AskPDF
May 9, 2026

IMPACT: An Implicit Active-Set Augmented Lagrangian for Fast Contact-Implicit Trajectory Optimization

Contact-implicit trajectory optimization (CITO) has attracted growing attention as a unified framework for planning and control in contact-rich robotic tasks. Recent approaches have demonstrated promising results in manipulation and locomotion without requiring a prescribed contact-mode schedule. It is well known that the underlying mathematical programs with complementarity constraints (MPCCs) remain numerically ill-conditioned, and systematic, scalable solution strategies for CITO remain an active area of research. More efficient and principled solvers that can handle contact constraints are therefore essential to broaden the applicability of CITO. In this work, we develop an augmented-Lagrangian approach to CITO for solving MPCC-based CITO with stationarity guarantees. The method can be interpreted as identifying the implicit contact-mode branches on the fly during the trajectory optimization (TO) iterations; we call this approach IMPACT (IMPlicit contact ACtive-set Trajectory optimization). We provide an efficient C++ implementation tailored to trajectory-optimization workloads and evaluate it on the open-source CITO and contact-implicit model predictive control (CI-MPC) benchmarks. On CITO, IMPACT achieves 2.9x-70x speedups over strong baselines (geometric mean 13.8x). On CI-MPC, we show improved control quality for contact-rich trajectories on dexterous manipulation tasks in simulation. Finally, we demonstrate the proposed method on real robotic hardware on a T-shaped object pushing task.

Jiayun Li, Dejian Gong, Georgia Chalvatzaki

1PEARL Lab, Dept. of Computer Science, TU Darmstadt, Germany · 2Hessian.AI, Darmstadt, Germany · 3Robotics Institute Germany (RIG)

Constrained Motion PlanningTrajectory OptimizationContact-Rich Robotic ManipulationAugmented Lagrangian MethodConstrained OptimizationOptimal Control
AskPDF
May 9, 2026

Transfer Learning of Multiobjective Indirect Low-Thrust Trajectories Using Diffusion Models and Markov Chain Monte Carlo

Preliminary low-thrust spacecraft mission design is a global search problem characterized by a complex solution landscape, multiple objectives, and numerous local minima. During this phase, mission parameters are often not yet fully defined, requiring new solutions to be generated at a high cadence across varying parameter values. When combined with the indirect approach to optimal control, diffusion models can accelerate this search by learning distributions that represent high-quality initial costates. However, generating training data remains expensive, and opportunities exist to better exploit past data. We propose a transfer-learning framework that combines homotopy in a mission parameter with Markov chain Monte Carlo (MCMC) to generate training data more efficiently. The approach reformulates a multiobjective optimization problem as sampling from an unnormalized target distribution in costate space. We compare three MCMC algorithms on a planar multi-revolution transfer in the circular restricted three-body problem, with homotopy in the system mass parameter. The results show that gradient-based MCMC variants achieve the best trade-off between sample quality and computational cost. For the test transfer, the proposed framework generates 40 % more feasible solutions and achieves a higher-quality Pareto front than a state-of-the-art indirect approach based on adjoint control transformations and gradient-based optimization. Finally, the MCMC-generated samples are used to fine-tune a diffusion model conditioned on the mass parameter, enabling it to learn a global representation of the underlying solution distribution and efficiently generate new solutions. These findings establish the transfer-learning framework as a practical method for efficiently solving indirect trajectory optimization problems with varying parameters.

Jannik Graebner, Ryne Beeson

PhD Candidate, Department of Mechanical and Aerospace Engineering, Princeton University, NJ, USA. · Assistant Professor, Department of Mechanical and Aerospace Engineering, Princeton University, NJ, USA.

Markov Chain Monte CarloTrajectory OptimizationDiffusion SamplingTransfer LearningMulti-Objective Optimization
AskPDF
May 9, 2026

Terminal Matters: Kinodynamic Planning with a Terminal Cost and Learned Uncertainty in Belief State-Cost Space

In many real-world robotic tasks, robots must generate dynamically feasible motions that reliably reach desired goals even under uncertainty. Yet existing sampling-based kinodynamic planners typically optimize accumulated trajectory costs and treat goal reaching as a feasibility check, rather than explicitly optimizing terminal-state quality, such as goal preference or goal-reaching reliability. In this work, we introduce a terminal-cost formulation for kinodynamic planning that allows terminal-state quality to be optimized alongside accumulated trajectory cost. We prove that AO-RRT, an asymptotically optimal kinodynamic planner, preserves its asymptotic optimality under this augmented objective. We further extend the formulation to belief space and prove that minimizing the Wasserstein distance between the terminal belief and the goal improves a lower bound on the probability of reaching the goal region. The resulting planner, KiTe, uses this terminal-cost objective to encode goal preferences and improve reliability under uncertainty. To support systems without analytical uncertainty models, we learn dynamics and process uncertainty directly from data and integrate the learned belief dynamics into planning. Experiments on Flappy Bird, Car Parking, and Planar Pushing show that KiTe consistently improves goal-reaching success under uncertainty. Real-world Planar Pushing experiments further demonstrate that KiTe can plan effectively with learned dynamics and uncertainty. Source code is available at https://github.com/elpis-lab/KiTe.

Zhuoyun Zhong, Seyedali Golestaneh, Constantinos Chamzas

All authors are affiliated with the Department of Robotics Engineering, Worcester Polytechnic Institute (WPI), Worcester, MA 01609, USA

Belief-Space PlanningTrajectory OptimizationKinodynamic Motion PlanningTrajectory GenerationMotion PlanningSampling-Based Motion Planning
AskPDF
May 8, 2026

PISTO: Proximal Inference for Stochastic Trajectory Optimization

Stochastic trajectory optimization methods like STOMP enable planning with non-differentiable costs, offering substantial flexibility over gradient-based approaches. We show that STOMP implicitly minimizes the KL divergence from a Boltzmann trajectory distribution, revealing an elegant Variational Inference (VI) structure underlying its updates. Building on this insight, we propose the Proximal Inference for Stochastic Trajectory Optimization (PISTO) algorithm that stabilizes the updates by augmenting the objective with a KL regularization between successive Gaussian proposals. This proximal formulation admits a trust-region interpretation and yields closed-form mean updates computable as expectations under a surrogate distribution. We estimate these expectations via importance-weighted Monte Carlo sampling, producing a simple, derivative-free algorithm that inherits STOMP's ability to handle non-differentiable and discontinuous costs without modification. On robot arm motion planning benchmarks, PISTO achieves an 89% success rate -- outperforming CHOMP (63%) and STOMP (68%) -- while producing shorter, smoother paths at twice the speed of competing stochastic methods. We further validate PISTO on contact-rich MuJoCo locomotion and manipulation tasks, where it consistently outperforms both CEM and MPPI baselines in reward.

Hongzhe Yu, Zinuo Chang, Yongxin Chen

School of Aerospace Engineering, Georgia Institute of Technology, Atlanta, GA · School of Electrical and Computer Engineering, Georgia Institute of Technology, Atlanta, GA

Trajectory OptimizationTrajectory GenerationVariational InferenceRobot Motion PlanningRobotic Optimization
AskPDF
May 7, 2026

On the Emergence of Pendular Structure in Multi-Contact Locomotion

LIPM is everywhere in legged-locomotion control, but almost always as a modeling choice rather than as something the controller's cost actually prefers. This note tries to make that link more explicit. Working from a small centroidal OCP that penalizes the rate of angular momentum, we look at what its optimum tends to look like. Three things come out. With full-rank stance, the optimum drifts toward a pendular force pattern at a rate determined by the SVD of the moment Jacobian; the constant is set by foot-span geometry and matches the experiments to within 16%. With N=2 stance, as in trot, the friction cone introduces a lower bound on ∥H˙G∥\|\dot{H}_G\| that no amount of weight tuning fixes; we also see a non-smooth feasibility kink at a critical horizontal acceleration that we can write in closed form. Adding a task term that asks for a nonzero H˙G\dot{H}_G moves the optimum off the pendular set in a predictable way. None of this is far from the classical ZMP/DCM picture. We test these claims on a point-mass quadruped and on the Unitree Go1 in MuJoCo (open-loop QP and a torque-level closed-loop controller), and we note where the asymptotic story stops being a good description of what the closed loop actually does.

Lingxue Lyu, Zihui Liu

School of Engineering and Applied Science, University of Pennsylvania, Philadelphia, PA 19104, USA · Department of Aeronautics & Astronautics, Stanford University, Stanford, CA 94305, USA

Legged RoboticsTrajectory OptimizationMomentum MethodsOptimal ControlQuadruped Locomotion
AskPDF
May 6, 2026

Tightly-Coupled Estimation and Guidance for Robust Low-Thrust Rendezvous via Adaptive Homotopy

Minimum-fuel low-thrust rendezvous guidance yields bang-bang control structures highly sensitive to estimation errors, sensor anomalies, and solver regularization, making aggressive closed-loop execution brittle for uncooperative proximity operations. This paper proposes a tightly-coupled estimation and guidance architecture where navigation confidence directly modulates the homotopy parameter of a receding-horizon indirect optimal control solver. Relative motion is modeled in the Clohessy-Wiltshire frame. The translational state is estimated via a linear Kalman filter augmented by a Multiple Tuning Factors (MTF) covariance inflation mechanism that suppresses suspicious innovation directions. A composite score from the normalized innovation and MTF activity is mapped online to the homotopy parameter, allowing the controller to relax toward a smoother, conservative regime when confidence degrades, and recover fuel-efficient bang-bang control as sensing improves. Numerical results under severe measurement degradation show fixed bang-bang guidance remains brittle; both plain-KF and MTF-KF fixed-epsilon controllers yield large terminal miss distances. Conversely, the proposed MTF-adaptive homotopy controller reduces terminal miss by roughly two orders of magnitude, from hundreds of meters to sub-meter levels, requiring only a moderate increase in control effort versus the open-loop fuel-optimal benchmark. A comparison indicates adaptive homotopy is the dominant robustness mechanism, while MTF provides additional accuracy and efficiency improvements. The receding-horizon implementation exhibits consistently fast and reliable solution times, supporting the practical online viability of the proposed method.

Batu Candan, Simone Servadio

Department of Aerospace Engineering, Iowa State University, Ames, IA 50011.

Trajectory OptimizationSpacecraft Rendezvous and Proximity OperationsRobust Control
AskPDF
May 6, 2026

Joint Optimization of Trajectory Control, Resource Allocation, and Task Offloading for Multi-UAV-Assisted IoV

This paper investigates a multi-Unmanned Aerial Vehicle (UAV) joint base station-assisted Internet of Vehicles (IoV) task offloading system in dense urban environments. To minimize system delay and energy consumption under strict coupling constraints, the complex non-convex optimization problem is decoupled into a hierarchical execution framework. First, a sequential distributed optimization algorithm based on Second-Order Cone Programming (SOCP) is proposed to optimize the 3D flight trajectory of each UAV, ensuring adaptive network coverage. Second, a novel hybrid resource scheduling paradigm synergizing Deep Reinforcement Learning (DRL) and Large Language Models (LLMs) is developed. Within this framework, the DRL agent dictates the initial resource allocation, while the LLM acts as a semantic macro-scheduler to rectify long-tail allocation imbalances for failed and surplus tasks. Crucially, a reward decoupling mechanism is introduced to isolate DRL training from external LLM interventions, thereby ensuring policy convergence. Finally, the task offloading ratios are precisely determined via Linear Programming (LP) within an alternating optimization loop. Simulation results demonstrate that the proposed method significantly outperforms traditional multi-agent reinforcement learning baselines in terms of task success rate and system efficiency.

Maoxin Ji, Qiong Wu, Pingyi Fan +4

School of Internet of Things Engineering, Jiangnan University, Wuxi 214122, China · School of Information Engineering, Jiangxi Provincial Key Laboratory of Advanced Signal Processing and Intelligent Communications, Nanchang University, Nanchang 330031, China · Department of Electronic Engineering, State Key laboratory of Space Network and Communications, Beijing National Research Center for Information Science and Technology, Tsinghua University, Beijing 100084, China +4

Trajectory OptimizationIntelligent Transportation SystemsUAV Trajectory OptimizationWireless Resource AllocationLLM Request Scheduling
AskPDF
May 4, 2026

Feedback Motion Planning for Stochastic Nonlinear Systems with Signal Temporal Logic Specifications

We study feedback motion planning for continuous-time stochastic nonlinear systems under signal temporal logic (STL) specifications. We propose a framework that synthesizes control policies for chance-constrained STL trajectory optimization problems, with the goal of ensuring that the closed-loop stochastic system satisfies a given STL formula with high probability (e.g., 99.99%). Our approach is based on a predicate erosion strategy that transforms the intractable stochastic problem into a deterministic STL trajectory optimization problem with tightened STL formula constraints. The amount of erosion is determined by a probabilistic reachable tube (PRT) that bounds the deviation between the stochastic trajectory and an associated nominal trajectory. To compute such bounds, we leverage contraction theory and feedback design, and develop several tracking controllers. This yields a complete feedback motion planning pipeline which can be implemented by numerical optimizations. We demonstrate the efficacy and versatility of the proposed framework through simulations on several robotic systems and through experiments on a real-world quadrupedal robot, and show that it is less conservative and achieves higher specification satisfaction probability than representative baselines.

Liqian Ma, Zishun Liu, Glen Chou +1

Georgia Institute of Technology, Atlanta, GA 30332

Constrained Motion PlanningTrajectory OptimizationContraction TheoryStochastic Optimal ControlRobot Motion PlanningSTL Motion Planning+2
AskPDF
May 3, 2026

Optimized and kinematically feasible multi-agent motion planning

Multi-agent motion planning (MAMP) is an important problem for autonomous systems with multiple agents. In this work we propose a two-step method for finding optimized and kinematically feasible solutions to MAMP problems. The first step finds an initial feasible solution using state-of-the-art methods such as conflict-based search (CBS) or priority-based search (PBS), and the second step is an improvement step which improves the solution by solving a multi-phase optimal control problem (OCP) where the initial solution is used to warm-start the solver. We also propose a method for generating motion primitives in an optimized way under the constraint that the primitive durations are all multiples of the same sample time. We evaluate our proposed framework on a MAMP problem for tractor-trailer systems. We extend the safe interval path planning with interval projections (SIPP-IP) algorithm so it can handle more general cost functions and larger agents, but our results show that for the tractor-trailer system a simple lattice-based planner performs better due to less conservative collision checks. Our experiments also indicate that CBS performs better than PBS for this system as it achieves a higher success rate in environments with obstacles and had a lower average runtime, although both planners achieve solutions of similar quality after the improvement step.

Anja Hellander, Kristoffer Bergman, Daniel Axehill

Division of Automatic Control, Linköping University, Sweden · Saab AB, Linköping, Sweden

Constrained Motion PlanningTrajectory OptimizationMulti-Agent System OptimizationKinodynamic Motion PlanningDecentralized Multi-Agent Motion PlanningMobile Robotics+2
AskPDF
Apr 29, 2026

Global Sampling-Based Trajectory Optimization for Contact-Rich Manipulation via KernelSOS

Contact-rich manipulation is challenging due to its high dimensionality, the requirement for long time horizons, and the presence of hybrid contact dynamics. Sampling-based methods have become a popular approach for this class of problems, but without explicit mechanisms for global exploration, they are susceptible to converging to poor local minima. In this paper, we introduce Global-MPPI, a unified trajectory optimization framework that integrates global exploration and local refinement. At the global level, we leverage kernel sum-of-squares optimization to identify globally promising regions of the solution space. To enable reliable performance for the non-smooth landscapes inherent to contact-rich manipulation, we introduce a graduated non-convexity strategy based on log-sum-exp smoothing, which transitions the optimization landscape from a smoothed surrogate to the original non-smooth objective. Finally, we employ the model-predictive path integral method to locally refine the solution. We evaluate Global-MPPI on high-dimensional, long-horizon contact-rich tasks, including the PushT task and dexterous in-hand manipulation. Experimental results demonstrate that our approach robustly uncovers high-quality solutions, achieving faster convergence and lower final costs compared to existing baseline methods.

Zhongqi Wei, Frederike Dümbgen

Department of Mechanical Engineering, Carnegie Mellon University, Pittsburgh, PA 15213, USA · Inria, Département d’informatique de l’ENS, CNRS, PSL Research University, Paris, France

Trajectory OptimizationLong-Horizon Robotic ManipulationContact-Rich Robotic Manipulation
AskPDF
Apr 29, 2026

Bi-Level Optimization for Contact and Motion Planning in Rope-Assisted Legged Robots

This paper presents a planning pipeline framework for locomotion in rope-assisted robots climbing vertical surfaces. The proposed framework is formulated as a bi-level optimization scheme that addresses a mixed-integer problem: selecting feasible terrain regions for landing while simultaneously optimizing the control inputs, namely rope tensions and leg forces, and landing location. The outer level of the optimization is solved using the Cross-Entropy Method, while the inner level relies on gradient-based nonlinear optimization to compute dynamically feasible motions. The approach is validated on a novel climbing robot platform, ALPINE, across a variety of challenging terrain configurations.

Ruben Malacarne, Ioannis Tsikelis, Enrico Mingo Hoffman +1

The authors are with the Dipartimento di Ingegneria e Scienza dell’Informazione (DISI), University of Trento. · The authors are with Inria, Université de Lorraine, CNRS, 54000 Nancy, France.

Legged RoboticsTrajectory OptimizationRobot LocomotionRobotic Optimization
AskPDF
Apr 24, 2026

ATRS: Adaptive Trajectory Re-splitting via a Shared Neural Policy for Parallel Optimization

Parallel trajectory optimization via the Alternating Direction Method of Multipliers (ADMM) has emerged as a scalable approach to long-horizon motion planning. However, existing frameworks typically decompose the problem into parallel subproblems based on a predefined fixed structure. Such structural rigidity often causes optimization stagnation in highly constrained regions, where a few lagging subproblems delay global convergence. A natural remedy is to adaptively re-split these stagnating segments online. Yet, deciding when, where, and how to split exceeds the capability of rule-based heuristics. To this end, we propose ATRS, a novel framework that embeds a shared Deep Reinforcement Learning policy into the parallel ADMM loop. We formulate this adaptive adjustment as a Multi-Agent Shared-Policy Markov Decision Process, where all trajectory segments act as homogeneous agents and share a unified neural policy network. This parameter-sharing architecture endows the system with size invariance, enabling it to handle dynamically changing segment counts during re-splitting and generalize to arbitrary trajectory lengths. Furthermore, our formulation inherently supports zero-shot generalization to unseen environments, as our network relies solely on the internal states of the numerical solver rather than on the geometric features of the environment. To ensure solver stability, a Confidence-Based Election mechanism selects only the most stagnating segment for re-splitting at each step. Extensive simulations demonstrate that ATRS accelerates convergence, reducing the number of iterations by up to 26.0% and the computation time by up to 19.1%. Real-world experiments further confirm its applicability to both large-scale offline global planning and real-time onboard replanning within 35 ms per cycle, with no sim-to-real degradation.

Jiajun Yu, Guodong Liu, Li Wang +6

Trajectory OptimizationAlternating Direction Method of MultipliersConstrained Optimization
AskPDF
Apr 23, 2026

Tempered Sequential Monte Carlo for Trajectory and Policy Optimization with Differentiable Dynamics

We propose a sampling-based framework for finite-horizon trajectory and policy optimization under differentiable dynamics by casting controller design as inference. Specifically, we minimize a KL-regularized expected trajectory cost, which yields an optimal "Boltzmann-tilted" distribution over controller parameters that concentrates on low-cost solutions as temperature decreases. To sample efficiently from this sharp, potentially multimodal target, we introduce tempered sequential Monte Carlo (TSMC): an annealing scheme that adaptively reweights and resamples particles along a tempering path from a prior to the target distribution, while using Hamiltonian Monte Carlo rejuvenation to maintain diversity and exploit exact gradients obtained by differentiating through trajectory rollouts. For policy optimization, we extend TSMC via (i) a deterministic empirical approximation of the initial-state distribution and (ii) an extended-space construction that treats rollout randomness as auxiliary variables. Experiments across trajectory- and policy-optimization benchmarks show that TSMC is broadly applicable and compares favorably to state-of-the-art baselines.

Heng Yang

Harvard University

Markov Chain Monte CarloTrajectory OptimizationPolicy Optimization
AskPDF
Apr 21, 2026

Accelerating trajectory optimization with Sobolev-trained diffusion policies

Trajectory Optimization (TO) solvers exploit known system dynamics to compute locally optimal trajectories through iterative improvements. A downside is that each new problem instance is solved independently; therefore, convergence speed and quality of the solution found depend on the initial trajectory proposed. To improve efficiency, a natural approach is to warm-start TO with initial guesses produced by a learned policy trained on trajectories previously generated by the solver. Diffusion-based policies have recently emerged as expressive imitation learning models, making them promising candidates for this role. Yet, a counterintuitive challenge comes from the local optimality of TO demonstrations: when a policy is rolled out, small non-optimal deviations may push it into situations not represented in the training data, triggering compounding errors over long horizons. In this work, we focus on learning-based warm-starting for gradient-based TO solvers that also provide feedback gains. Exploiting this specificity, we derive a first-order loss for Sobolev learning of diffusion-based policies using both trajectories and feedback gains. Through comprehensive experiments, we demonstrate that the resulting policy avoids compounding errors, and so can learn from very few trajectories to provide initial guesses reducing solving time by 2×2\times to 20×20 \times. Incorporating first-order information enables predictions with fewer diffusion steps, reducing inference latency.

Théotime Le Hellard, Franki Nguimatsia Tiofack, Quentin Le Lidec +1

Inria - Département d’Informatique de l’École normale supérieure, PSL Research University, France · Courant Institute, New York University, USA

Trajectory OptimizationTrajectory GenerationDiffusion PolicyDiffusion Model AccelerationPolicy LearningOptimal Control
AskPDF
Apr 19, 2026

Intent-aligned Autonomous Spacecraft Guidance via Reasoning Models

Future spacecraft operations require autonomy that can interpret high-level mission intent while preserving safety. However, existing trajectory optimization still relies heavily on expert-crafted formulations and does not support intent-conditioned decision-making. This paper proposes an intent-aligned spacecraft guidance framework that links high-level reasoning and safe trajectory optimization through explicit intermediate abstractions, based on behavior sequences and waypoint constraints. A foundation model first predicts an intent-aligned behavior plan, a waypoint generation model then converts it into waypoint constraints, and the safe trajectory is computed via optimization. This decomposition enables scalable supervision without sacrificing safety. Numerical experiments in close-proximity operation scenarios demonstrate that the proposed pipeline achieves over 90% SCP convergence and yields a 1.5×1.5\times higher rate of generating trajectories that satisfy the top intent-prioritized performance criteria than heuristic decision-making. These results support the use of intermediate behavior abstraction as a practical interface between foundation-model reasoning and safety-critical onboard spacecraft autonomy.

Yuji Takubo, Simone D'Amico

Stanford University · Lomita Mall, Stanford, CA, 94305 · Stanford University 496 Lomita Mall, Stanford, CA, 94305

Trajectory OptimizationLLM PlanningRobot NavigationGoal-Conditioned PlanningSafe Motion Planning
AskPDF
Apr 18, 2026

Multi-stage Planning for Multi-target Surveillance using Aircrafts Equipped with Synthetic Aperture Radars Aware of Target Visibility

Generating trajectories for synthetic aperture radar (SAR)-equipped aircraft poses significant challenges due to terrain constraints, and the need for straight-flight segments to ensure high-quality imaging. Related works usually focus on trajectory optimization for predefined straight-flight segments that do not adapt to the target visibility, which depends on the 3D terrain and aircraft orientation. In addition, this assumption does not scale well for the multi-target problem, where multiple straight-flight segments that maximize target visibility must be defined for real-time operations. For this purpose, this paper presents a multi-stage planning system. First, the waypoint sequencing to visit all the targets is estimated. Second, straight-flight segments maximizing target visibility according to the 3D terrain are predicted using a novel neural network trained with deep reinforcement learning. Finally, the segments are connected to create a trajectory via optimization that imposes 3D Dubins curves. Evaluations demonstrate the robustness of the system for SAR missions since it ensures high-quality multi-target SAR image acquisition aware of 3D terrain and target visibility, and real-time performance.

Daniel Fuertes, Carlos R. del-Blanco, Fernando Jaureguizar +2

Grupo de Tratamiento de Im´agenes, Information Processing and Telecommunications Center, ETSI Telecomunicaci´on, Universidad Polit´ecnica de Madrid, 28040 Madrid, Spain · Airbus Defence and Space, 28906 Madrid, Spain

Trajectory OptimizationRemote SensingSynthetic Aperture RadarMotion Planning
AskPDF