Motion Planning

Ask about this topic

Overview

Motion planning determines how a robot or vehicle can move from a starting state to a goal while avoiding obstacles and respecting limits on dynamics, sensing, and interaction with others. Classical approaches include sampling-based planners such as RRT and PRM, graph search and motion primitives, and trajectory optimization methods such as model predictive control and convex optimization. Learning-based methods, including imitation learning, diffusion models, and learned world models, increasingly propose or refine plans, often alongside geometric checks, reachability analysis, and control barrier functions for safety. A central challenge is balancing speed, solution quality, and formal reliability in uncertain, dynamic, high-dimensional settings, especially when plans must transfer from simulation to physical execution.

Latest papers

CardsList

414 papers60 papers in the last four weeks, up 11% on the four weeks before. 0.3% of all new papers.

Mar 31, 2026

Long-Horizon Geometry-Aware Navigation among Polytopes via MILP-MPC and Minkowski-Based CBFs

Autonomous navigation in complex, non-convex environments remains challenging when robot dynamics, control limits, and exact robot geometry must all be taken into account. In this paper, we propose a hierarchical planning and control framework that bridges long-horizon guidance and geometry-aware safety guarantees for a polytopic robot navigating among polytopic obstacles. At the high level, Mixed-Integer Linear Programming (MILP) is embedded within a Model Predictive Control (MPC) framework to generate a nominal trajectory around polytopic obstacles while modeling the robot as a point mass for computational tractability. At the low level, we employ a control barrier function (CBF) based on the exact signed distance in the Minkowski-difference space as a safety filter to explicitly enforce the geometric constraints of the robot shape, and further extend its formulation to a high-order CBF (HOCBF). We demonstrate the proposed framework in U-shaped and maze-like environments under single- and double-integrator dynamics. The results show that the proposed architecture mitigates the topology-induced local-minimum behavior of purely reactive CBF-based navigation while enabling safe, real-time, geometry-aware navigation.

Yi-Hsuan Chen, Salman Ghori, Ania Adil +2

Dept. of Aerospace Engineering, University of Maryland, College Park, MD, USA. · Computer, Electrical and Mathematical Science & Engineering Division (CEMSE), KAUST, Saudi Arabia. · Dept. Electrical and Computer Engineering and Dept. Computer Science, University of Maryland, College Park, MD, USA.

Safe Robot NavigationMotion PlanningConstrained Motion PlanningControl Barrier FunctionsModel Predictive Control
AskPDF
Mar 31, 2026

Video Models Reason Early: Exploiting Plan Commitment for Maze Solving

Video diffusion models exhibit emergent reasoning capabilities like solving mazes and puzzles, yet little is understood about how they reason during generation. We take a first step towards understanding this and study the internal planning dynamics of video models using 2D maze solving as a controlled testbed. Our investigations reveal two findings. Our first finding is early plan commitment: video diffusion models commit to a high-level motion plan within the first few denoising steps, after which further denoising alters visual details but not the underlying trajectory. Our second finding is that path length, not obstacle density, is the dominant predictor of maze difficulty, with a sharp failure threshold at 12 steps. This means video models can only reason over long mazes by chaining together multiple sequential generations. To demonstrate the practical benefits of our findings, we introduce Chaining with Early Planning, or ChEaP, which only spends compute on seeds with promising early plans and chains them together to tackle complex mazes. This improves accuracy from 7% to 67% on long-horizon mazes and by 2.5x overall on hard tasks in Frozen Lake and VR-Bench across Wan2.2-14B and HunyuanVideo-1.5. Our analysis reveals that current video models possess deeper reasoning capabilities than previously recognized, which can be elicited more reliably with better inference-time scaling.

Kaleb Newman, Tyler Zhu, Olga Russakovsky

Princeton University

Video Diffusion ModelsVideo ReasoningDiffusion ModelsMotion PlanningDiffusion-Based PlanningInference-Time Scaling
AskPDF
Mar 31, 2026

SparseDriveV2: Scoring is All You Need for End-to-End Autonomous Driving

End-to-end multi-modal planning has been widely adopted to model the uncertainty of driving behavior, typically by scoring candidate trajectories and selecting the optimal one. Existing approaches generally fall into two categories: scoring a large static trajectory vocabulary, or scoring a small set of dynamically generated proposals. While static vocabularies often suffer from coarse discretization of the action space, dynamic proposals provide finer-grained precision and have shown stronger empirical performance on existing benchmarks. However, it remains unclear whether dynamic generation is fundamentally necessary, or whether static vocabularies can already achieve comparable performance when they are sufficiently dense to cover the action space. In this work, we start with a systematic scaling study of Hydra-MDP, a representative scoring-based method, revealing that performance consistently improves as trajectory anchors become denser, without exhibiting saturation before computational constraints are reached. Motivated by this observation, we propose SparseDriveV2 to push the performance boundary of scoring-based planning through two complementary innovations: (1) a scalable vocabulary representation with a factorized structure that decomposes trajectories into geometric paths and velocity profiles, enabling combinatorial coverage of the action space, and (2) a scalable scoring strategy with coarse factorized scoring over paths and velocity profiles followed by fine-grained scoring on a small set of composed trajectories. By combining these two techniques, SparseDriveV2 achieves 92.0 PDMS and 90.1 EPDMS on NAVSIM, with 89.15 Driving Score and 70.00 Success Rate on Bench2Drive with a lightweight ResNet-34 as backbone. Code and model are released at https://github.com/swc-17/SparseDriveV2.

Wenchao Sun, Xuewu Lin, Keyu Chen +4

Tsinghua University, Beijing, China · Horizon Continental Technology, Beijing, China · Horizon, Beijing, China

Autonomous Driving PlanningEnd-to-End Autonomous DrivingMotion Planning
AskPDF
Mar 30, 2026

Serialized Red-Green-Gray: Quicker Heuristic Validation of Edges in Dynamic Roadmap Graphs

Motion planning in dynamic environments, such as robotic warehouses, requires fast adaptation to frequent changes in obstacle poses. Traditional roadmap-based methods struggle in such settings, relying on inefficient reconstruction of a roadmap or expensive collision detection to update the existing roadmap. To address these challenges we introduce the Red-Green-Gray (RGG) framework, a method that builds on SPITE to quickly classify roadmap edges as invalid (red), valid (green), or uncertain (gray) using conservative geometric approximations. Serial RGG provides a high-performance variant leveraging batch serialization and vectorization to enable efficient GPU acceleration. Empirical results demonstrate that while RGG effectively reduces the number of unknown edges requiring full validation, SerRGG achieves a 2-9x speedup compared to the sequential implementation. This combination of geometric precision and computational speed makes SerRGG highly effective for time-critical robotic applications.

Yulie Arad, Stav Ashur, Marta Markowicz +3

Siebel School of Computing and Data Science, University of Illinois, 201 N. Goodwin Avenue, Urbana, IL 61801, USA · Department of Computer Science, Instituto Tecnológico Autónomo de México (ITAM), Mexico City, 01080, México

Roadmap-Based Motion PlanningMotion PlanningDynamic Obstacle Avoidance
AskPDF
Mar 30, 2026

osmAG-Nav: A Hierarchical Semantic Topometric Navigation Stack for Robust Lifelong Indoor Autonomy

The deployment of mobile robots in large-scale, multi-floor environments demands navigation systems that achieve spatial scalability without compromising local kinematic precision. Traditional navigation stacks, reliant on monolithic occupancy grid maps, face severe bottlenecks in storage efficiency, cross-floor reasoning, and long-horizon planning. To address these limitations, this paper presents osmAG-Nav, a complete, open-source ROS2 navigation stack built upon the hierarchical semantic topometric OpenStreetMap Area Graph (osmAG) map standard. The system follows a "System of Systems" architecture that decouples global topological reasoning from local metric execution. A Hierarchical osmAG planner replaces dense grid searches with an LCA-anchored pipeline on a passage-centric graph whose edge costs derive from local raster traversability rather than Euclidean distance, yielding low-millisecond planning on long campus-scale routes. A Rolling Window mechanism rasterizes a fixed-size local metric grid around the robot, keeping the local costmap memory footprint independent of the total mapped area, while a Segmented Execution strategy dispatches intermediate goals to standard ROS2 controllers for smooth handoffs. System robustness is reinforced by a structure-aware LiDAR localization framework that filters dynamic clutter against permanent architectural priors. Extensive experiments on a real-world multi-story indoor-outdoor campus (>11,025 m^2) show that, on the same-floor benchmark subset, osmAG-Nav delivers up to 7816x lower planning latency than a grid-based baseline on long routes while maintaining low path-length overhead and lifelong localization stability. A single-floor long-range robot mission further validates the integrated stack reliability. The full stack is released as modular ROS2 Lifecycle Nodes.

Yongqi Zhang, Jiajie Zhang, Chengqian Li +2

School of Computer Science and Technology, ShanghaiTech University, 393 Huaxia Road, Shanghai, 201210, Shanghai, China.

Robot NavigationIndoor Robot NavigationMotion PlanningMap-Based Localization
AskPDF
Mar 29, 2026

Spectral Decomposition of Inverse Dynamics for Fast Exploration in Model-Based Manipulation

Planning long duration robotic manipulation sequences is challenging because of the complexity of exploring feasible trajectories through nonlinear contact dynamics and many contact modes. Moreover, this complexity grows with the problem's horizon length. We propose a search tree method that generates trajectories using the spectral decomposition of the inverse dynamics equation. This equation maps actuator displacement to object displacement, and its spectrum is efficient for exploration because its components are orthogonal and they approximate the reachable set of the object while remaining dynamically feasible. These trajectories can be combined with any search based method, such as Rapidly-Exploring Random Trees (RRT), for long-horizon planning. Our method performs similarly to recent work in model-based planning for short-horizon tasks, and differentiates itself with its ability to solve long-horizon tasks: whereas existing methods fail, ours can generate 45 second duration, 10+ contact mode plans using 15 seconds of computation, demonstrating real-time capability in highly complex domains.

Solvin Sigurdson, Benjamin Riviere, Joel Burdick

Department of Control and Dynamical Systems, California Institute of Technology, Pasadena, CA 91125, USA · Department of Mechanical and Aerospace Engineering and the Department of Computer Science, New York University, New York City, NY 10012, USA

Long-Horizon Robotic ManipulationMotion PlanningModel-Based PlanningContact-Rich Manipulation
AskPDF
Mar 27, 2026

Visual-RRT: Finding Paths toward Visual-Goals via Differentiable Rendering

Rapidly-exploring random trees (RRTs) have been widely adopted for robot motion planning due to their robustness and theoretical guarantees. However, existing RRT-based planners require explicit goal configurations specified as numerical joint angles, while many practical applications provide goal specifications through visual observations such as images or demonstration videos where precise goal configurations are unavailable. In this paper, we propose visual-RRT (vRRT), a motion planner that enables visual-goal planning by unifying gradient-based exploitation from differentiable robot rendering with sampling-based exploration from RRTs. We further introduce (i) a frontier-based exploration-exploitation strategy that adaptively prioritizes visually promising search regions, and (ii) inertial gradient tree expansion that inherits optimization states across tree branches for momentum-consistent gradient exploitation. Extensive experiments across various robot manipulators including Franka, UR5e, and Fetch demonstrate that vRRT achieves effective visual-goal planning in both simulated and real-world settings, bridging the gap between sampling-based planning and vision-centric robot applications. Our code is available at https://sgvr.kaist.ac.kr/Visual-RRT.

Sebin Lee, Jumin Lee, Taeyeon Kim +3

1KAIST · 2Samsung Electronics

Motion PlanningRobot Motion PlanningSampling-Based Motion Planning
AskPDF
Mar 26, 2026

Connectivity-Aware Representations for Constrained Motion Planning via Multi-Scale Contrastive Learning

The objective of constrained motion planning is to connect start and goal configurations while satisfying task-specific constraints. Motion planning becomes inefficient or infeasible when the configurations lie in disconnected regions, known as essentially mutually disconnected (EMD) components. Constraints further restrict feasible space to a lower-dimensional submanifold, while redundancy introduces additional complexity because a single end-effector pose admits infinitely many inverse kinematic solutions that may form discrete self-motion manifolds. This paper addresses these challenges by learning a connectivity-aware representation for selecting start and goal configurations prior to planning. Joint configurations are embedded into a latent space through multi-scale manifold learning across neighborhood ranges from local to global, and clustering generates pseudo-labels that supervise a contrastive learning framework. The proposed framework provides a connectivity-aware measure that biases the selection of start and goal configurations in connected regions, avoiding EMDs and yielding higher success rates with reduced planning time. Experiments on various manipulation tasks showed that our method achieves 1.9 times higher success rates and reduces the planning time by a factor of 0.43 compared to baselines.

Suhyun Jeon, Yumin Lim, Woo-Jeong Baek +3

Department of Intelligence and Information, Graduate School of Convergence Science and Technology, Seoul National University, Republic of Korea. · Artificial Intelligence Institute (AIIS), Seoul National University, Republic of Korea. · Department of Mechanical Engineering, Seoul National University, Republic of Korea. +2

Constrained Motion PlanningMotion PlanningMotion Representation Learning
AskPDF
Mar 25, 2026

Toward Generalist Neural Motion Planners for Robotic Manipulators: Challenges and Opportunities

State-of-the-art generalist manipulation policies have enabled the deployment of robotic manipulators in unstructured human environments. However, these frameworks struggle in cluttered environments primarily because they utilize auxiliary modules for low-level motion planning and control. Motion planning remains challenging due to the high dimensionality of the robot's configuration space and the presence of workspace obstacles. Neural motion planners have enhanced motion planning efficiency by offering fast inference and effectively handling the inherent multi-modality of the motion planning problem. Despite such benefits, current neural motion planners often struggle to generalize to unseen, out-of-distribution planning settings. This paper reviews and analyzes the state-of-the-art neural motion planners, highlighting both their benefits and limitations. It also outlines a path toward establishing generalist neural motion planners capable of handling domain-specific challenges. For a list of the reviewed papers, please refer to https://davoodsz.github.io/planning-manip-survey.github.io/.

Davood Soleymanzadeh, Ivan Lopez-Sanchez, Hao Su +3

J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX, USA · Lab of Biomechatronics and Intelligent Robotics, Department of Biomedical Engineering, Tandon School of Engineering, New York University, NY, USA · Department of Computer Science, Columbia University, NY, USA +1

Motion PlanningRobot Motion PlanningRobotic Manipulation
AskPDF
Mar 25, 2026

Accelerated Spline-Based Time-Optimal Motion Planning with Continuous Safety Guarantees for Non-Differentially Flat Systems

Generating time-optimal, collision-free trajectories for autonomous mobile robots involves a fundamental trade-off between guaranteeing safety and managing computational complexity. State-of-the-art approaches formulate spline-based motion planning as a single Optimal Control Problem (OCP) but often suffer from high computational cost because they include separating hyperplane parameters as decision variables to enforce continuous collision avoidance. This paper presents a novel method that alleviates this bottleneck by decoupling the determination of separating hyperplanes from the OCP. By treating the separation theorem as an independent classification problem solvable via a linear system or quadratic program, the proposed method eliminates hyperplane parameters from the optimisation variables, effectively transforming non-convex constraints into linear ones. Experimental validation demonstrates that this decoupled approach reduces trajectory computation times up to almost 60% compared to fully coupled methods in obstacle-rich environments, while maintaining rigorous continuous safety guarantees.

Dries Dirckx, Jan Swevers, Wilm Decré

Motion PlanningSafe Motion PlanningTrajectory OptimizationCollision Avoidance
AskPDF
Mar 24, 2026

Planning over MAPF Agent Dependencies via Multi-Dependency PIBT

Modern Multi-Agent Path Finding (MAPF) algorithms must plan for hundreds to thousands of agents in congested environments within a second, requiring highly efficient algorithms. Priority Inheritance with Backtracking (PIBT) is a popular algorithm capable of effectively planning in such situations. However, PIBT, and its variants like Enhanced PIBT (EPIBT), is constrained by its rule-based planning procedure and lacks generality because it restricts its search to paths that collide with at most one other agent. In this paper, we describe a new perspective on solving MAPF by planning over agent dependencies. Taking inspiration from PIBT's priority inheritance logic, we define the concept of agent dependencies and propose Multi-Dependency PIBT (MD-PIBT) that searches over agent dependencies. MD-PIBT is a general framework where specific parameterizations can reproduce PIBT and EPIBT. At the same time, alternative configurations generalize PIBT and EPIBT to multi-step planning capable of reasoning paths that collide with more than one other agent. Our experiments demonstrate that MD-PIBT effectively plans for as many as 10,000 homogeneous agents under various kinodynamic constraints, including pebble motion, rotation motion, and differential drive robots with speed and acceleration limits. We perform thorough evaluations on different variants of MAPF and find that MD-PIBT is particularly effective in MAPF with large agents. Our code is available at https://github.com/lunjohnzhang/MD-PIBT.

Zixiang Jiang, Yulun Zhang, Rishi Veerapaneni +1

University Of Melbourne · Robotics Institute, Carnegie Mellon University

Kinodynamic Motion PlanningMotion PlanningMulti-Agent PlanningMulti-Agent Path Finding
AskPDF
Mar 23, 2026

IGV-RRT: Prior-Real-Time Observation Fusion for Active Object Search in Changing Environments

Object Goal Navigation (ObjectNav) in temporally changing indoor environments is challenging because object relocation can invalidate historical scene knowledge. To address this issue, we propose a probabilistic planning framework that combines uncertainty-aware scene priors with online target relevance estimates derived from a Vision Language Model (VLM). The framework contains a dual-layer semantic mapping module and a real-time planner. The mapping module includes an Information Gain Map (IGM) built from a 3D scene graph (3DSG) during prior exploration to model object co-occurrence relations and provide global guidance on likely target regions. It also maintains a VLM score map (VLM-SM) that fuses confidence-weighted semantic observations into the map for local validation of the current scene. Based on these two cues, we develop a planner that jointly exploits information gain and semantic evidence for online decision making. The planner biases tree expansion toward semantically salient regions with high prior likelihood and strong online relevance (IGV-RRT), while preserving kinematic feasibility through gradient-based analysis. Simulation and real-world experiments demonstrate that the proposed method effectively mitigates the impact of object rearrangement, achieving higher search efficiency and success rates than representative baselines in complex indoor environments.

Wei Zhang, Ping Gong, Yujie Wang +7

The School of Control Science and Engineering, Shandong University · Department of Data and Systems Engineering, HKU

Robot NavigationObject-Goal NavigationMotion PlanningSampling-Based Motion PlanningActive PerceptionSemantic Mapping
AskPDF
Mar 23, 2026

Optimal Solutions for the Moving Target Vehicle Routing Problem with Obstacles via Lazy Branch-and-Price

The Moving Target Vehicle Routing Problem with Obstacles (MT-VRP-O) seeks trajectories for several agents that collectively intercept a set of moving targets. Each target has one or more time windows where it can be visited, and the agents must avoid static obstacles and satisfy speed and capacity constraints. Previously studied VRPs are often addressed using a framework called branch-and-price. This framework requires computing the cost for a single agent to visit a given sequence of target-time window pairings, for several candidate sequences. These sequences are called tours. Computing tour costs is more expensive in the MT-VRP-O than in previously studied VRPs due to the presence of both moving targets and static obstacles. Thus, we introduce a new exact algorithm, Lazy Branch-and-Price with Relaxed Continuity (Lazy BPRC), for the MT-VRP-O. The key idea in Lazy BPRC is to use cheap-to-compute lower bounds on tour costs where costs are traditionally used, and lazily update lower bounds to true costs. When computing a tour's true cost is needed, we do so by searching for a shortest path on a graph of convex sets, and we introduce a new heuristic to accelerate the search. We demonstrate that Lazy BPRC runs up to an order of magnitude faster than two ablations.

Anoop Bhat, Geordan Gutow, David Neiman +4

Robotics Institute at Carnegie Mellon University, 5000 Forbes Ave., Pittsburgh, PA 15213. · Mechanical and Aerospace Engineering at Michigan Technological University, Houghton, MI 49931 · Robotics and AI Institute, Cambridge, MA 02142. +2

Vehicle RoutingMotion Planning
AskPDF
Mar 23, 2026

RTD-RAX: Fast, Safe Trajectory Planning for Systems under Unknown Disturbances

Reachability-based Trajectory Design (RTD) is a provably safe, real-time trajectory planning framework that combines offline reachable-set computation with online trajectory optimization. However, standard RTD implementations suffer from two key limitations: conservatism induced by worst-case reachable-set overapproximations, and an inability to account for real-time disturbances during execution. This paper presents RTD-RAX, a runtime-assurance extension of RTD that utilizes a non-conservative RTD formulation to rapidly generate goal-directed candidate trajectories, and utilizes mixed monotone reachability for fast, disturbance-aware online safety certification. When proposed trajectories fail safety certification under real-time uncertainty, a repair procedure finds nearby safe trajectories that preserve progress toward the goal while guaranteeing safety under real-time disturbances.

Evanns Morales-Cuadrado, Long Kiu Chung, Shreyas Kousik +1

S. Coogan is also with the School of Civil and Environmental Engineering.

Safe Motion PlanningMotion PlanningTrajectory OptimizationRuntime AssuranceReachability Analysis
AskPDF
Mar 20, 2026

Multi-Agent Motion Planning on Industrial Magnetic Levitation Platforms: A Hybrid ADMM-HOCBF approach

This paper presents a novel hybrid motion planning method for holonomic multi-agent systems. The proposed decentralised model predictive control (MPC) framework tackles the intractability of classical centralised MPC for a growing number of agents while providing safety guarantees. This is achieved by combining a decentralised version of the alternating direction method of multipliers (ADMM) with a centralised high-order control barrier function (HOCBF) architecture. Simulation results show significant improvement in scalability over classical centralised MPC. We validate the efficacy and real-time capability of the proposed method by developing a highly efficient C++ implementation and deploying the resulting trajectories on a real industrial magnetic levitation platform.

Bavo Tistaert, Stan Servaes, Alejandro Gonzalez-Garcia +4

Decentralized Multi-Agent Motion PlanningMotion PlanningSafe Motion PlanningModel Predictive Control
AskPDF
Mar 20, 2026

Planning Autonomous Vehicle Maneuvering in Work Zones Through Game-Theoretic Trajectory Generation

Work zone navigation remains one of the most challenging manoeuvres for autonomous vehicles (AVs), where constrained geometries and unpredictable traffic patterns create a high-risk environment. Despite extensive research on AV trajectory planning, few studies address the decision-making required to navigate work zones safely. This paper proposes a novel game-theoretic framework for trajectory generation and control to enhance the safety of lane changes in a work zone environment. By modelling the lane change manoeuvre as a non-cooperative game between vehicles, we use a game-theoretic planner to generate trajectories that balance safety, progress, and traffic stability. The simulation results show that the proposed game-theoretic model reduces the frequency of conflicts by 35 percent and decreases the probability of high risk safety events compared to traditional vehicle behaviour planning models in safety-critical highway work-zone scenarios.

Mayar Nour, Atrisha Sarkar, Mohamed H. Zaki

Civil and Environmental Engineering, University of Western Ontario, London, Ontario, Canada · Electrical and Computer Engineering, University of Western Ontario, London, Ontario, Canada

Autonomous Driving PlanningAutonomous DrivingMotion Planning
AskPDF
Mar 19, 2026

REST: Receding Horizon Explorative Steiner Tree for Zero-Shot Object-Goal Navigation

Zero-shot object-goal navigation (ZSON) requires navigating unknown environments to find a target object without task-specific training. Prior hierarchical solutions mainly focus on either scene understanding and representations (belief) or high-level decision-making and planning (policy), yet treat the option, i.e., the subgoal candidate that belief proposes and policy selects, as an interface inherited from adjacent modules rather than a design axis in its own right. In practice, options are predominantly single waypoints scored by destination utility: a lone destination hides the value gathered en route, and a flat list obscures the relationships among candidates. Our insight is that the option space should be a tree of paths. Full paths expose en-route information gain that destination-only scoring systematically neglects; a tree of shared segments enables coarse-to-fine LLM reasoning that dismisses or pursues entire branches before examining individual leaves, compressing the combinatorial path space into an efficient hierarchy. We instantiate this insight in REST (Receding Horizon Explorative Steiner Tree), a training-free framework that (1) builds an explicit open-vocabulary 3D map from online RGB-D streams; (2) grows an agent-centric tree of safe and informative paths as the option space via sampling-based planning; and (3) textualizes each branch into a spatial narrative and selects the next-best path through chain-of-thought LLM reasoning. Across the Gibson, HM3D, and HSSD benchmarks, REST consistently ranks among the top methods in success rate and path efficiency.

Shuqi Xiao, Maani Ghaffari, Chengzhong Xu +1

State Key Laboratory of Internet of Things for Smart City (SKL-IOTSC), University of Macau · Department of Naval Architecture and Marine Engineering, and Department of Robotics, University of Michigan

Zero-Shot LearningLarge Language Model-Based Robot PlanningInformative Path PlanningMotion PlanningObject-Goal NavigationOpen-Vocabulary Object Navigation
AskPDF
Mar 17, 2026

Coverage First Next Best View for Inspection of Cluttered Pipe Networks Using Mobile Manipulators

Robotic inspection of radioactive areas enables operators to be removed from hazardous environments; however, planning and operating in confined, cluttered environments remain challenging. These systems must autonomously reconstruct the unknown environment and cover its surfaces, whilst estimating and avoiding collisions with objects in the environment. In this paper, we propose a new planning approach based on next-best-view that enables simultaneous exploration and exploitation of the environment by reformulating the coverage path planning problem in terms of information gain. To handle obstacle avoidance under uncertainty, we extend the vector-field-inequalities framework to explicitly account for stochastic measurements of geometric primitives in the environment via chance constraints in a constrained optimal control law. The stochastic constraints were evaluated experimentally alongside the planner on a mobile manipulator in a confined environment to inspect a pipe network. These experiments demonstrate that the system can autonomously plan and execute inspection and coverage paths to reconstruct and fully cover the simplified pipe network. Moreover, the system successfully estimated geometric primitives online and avoided collisions during motion between viewpoints.

Joshua Raymond Bettles, Jiaxu Wu, Bruno Vilhena Adorno +2

Manchester Centre for Robotics and AI, The University of Manchester, Manchester, UK. · The University of Tokyo, Tokyo, Japan.

Chance-Constrained Motion PlanningRobotic InspectionMotion PlanningCoverage Path PlanningNext-Best-View PlanningInformative Path Planning
AskPDF
Mar 17, 2026

Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness

Motion planning under dynamics constraints, i.e, kinodynamic planning, enables safe robot operation by generating dynamically feasible trajectories that the robot can accurately track. For high-DOF robots such as manipulators, sampling-based motion planners are commonly used, especially for complex tasks in cluttered environments. However, enforcing constraints on robot dynamics in such planners requires solving either challenging two-point boundary value problems (BVPs) or propagating robot dynamics, both of which cause computational bottlenecks that drastically increase planning times. Meanwhile, recent efforts have shown that sampling-based motion planners can generate plans in microseconds using parallelization, but are limited to geometric paths. This paper develops FLASK, a fast parallelized sampling-based kinodynamic motion planning framework for a broad class of differentially flat robot systems, including manipulators, ground and aerial vehicles, and more. Differential flatness allows us to transform the motion planning problem from the original state space to a flat output space, where an analytical time-parameterized solution of the BVP problem can be obtained. A trajectory in the flat output space is then converted back to a closed-form dynamically feasible trajectory in the original state space, enabling fast validation via ``single instruction, multiple data" parallelism. Our framework is fast, exact, and compatible with any sampling-based motion planner, while offering theoretical guarantees on probabilistic exhaustivity and asymptotic optimality based on the closed-form BVP solutions. We extensively verify the effectiveness of our approach in both simulated benchmarks and real experiments with cluttered and dynamic environments, requiring mere microseconds to milliseconds of planning time.

Thai Duong, Clayton W. Ramsey, Zachary Kingston +2

Department of Computer Science, Rice University, Houston, TX, 77005, USA

Constrained Motion PlanningKinodynamic Motion PlanningMotion PlanningDifferential FlatnessSampling-Based Motion Planning
AskPDF
Mar 17, 2026

Geometry-Aligned LLM Fine-Tuning for Sequential Narrow-Opening Planning

We study rigid-body motion planning through multiple sequential narrow openings, which requires long-horizon geometric reasoning because the configuration used to traverse an early opening constrains the set of reachable configurations for subsequent ones. To achieve this, we propose a geometry-aligned large language model (LLM) fine-tuning framework that generates fixed-length, machine-readable waypoint sequences that are both geometrically feasible and coordinated across openings. Our approach uses a bi-level training pipeline. First, we perform failure-driven LoRA supervised fine-tuning (SFT) on human demonstrations, which incorporates structured failure feedback to teach the model common failure modes and enforce the output format. Second, we refine the same LoRA adapters using Group Relative Policy Optimization (GRPO) with geometric verification: each sampled waypoint sequence is densified by a model-based planner and scored with a deterministic geometry-derived reward to achieve continuous-motion feasibility. To validate the effectiveness of our proposed method, we provide both quantitative and qualitative results from simulations. Our method achieves the highest success rate in both in-distribution and out-of-distribution environments and qualitatively exhibits long-horizon geometric reasoning by selecting exit poses that facilitate entry into subsequent openings.

Al Jaber Mahmud, Xuan Wang

George Mason University

Motion PlanningRobot Motion PlanningLarge Language Model-Based Robot Planning
AskPDF
Mar 16, 2026

Topological Motion Planning Diffusion: Generative Tangle-Free Path Planning for Tethered Robots in Obstacle-Rich Environments

In extreme environments such as underwater exploration and post-disaster rescue, tethered robots require continuous navigation while avoiding cable entanglement. Traditional planners struggle in these lifelong planning scenarios due to topological unawareness, while topology-augmented graph-search methods face computational bottlenecks in obstacle-rich environments where the number of candidate topological classes increases. To address these challenges, we propose Topological Motion Planning Diffusion (TMPD), a novel generative planning framework that integrates lifelong topological memory. Instead of relying on sequential graph search, TMPD leverages a diffusion model to propose a multimodal front-end of kinematically feasible trajectory candidates across various homotopy classes. A tether-aware topological back-end then filters and optimizes these candidates by computing generalized winding numbers to evaluate their topological energy against the accumulated tether configuration. Benchmarking in obstacle-rich simulated environments demonstrates that TMPD achieves a collision-free reach of 100% and a tangle-free rate of 97.0%, outperforming traditional topological search and purely kinematic diffusion baselines in both geometric smoothness and computational efficiency. Simulation with realistic cable dynamics further validates the practicality of the proposed approach.

Yifu Tian, Xinhang Xu, Thien-Minh Nguyen +1

Motion PlanningRobot Motion PlanningDiffusion-Based Planning
AskPDF
Mar 16, 2026

Surgical Robot, Path Planning, Joint Space, Riemannian Manifolds

Robotic surgery for minimally invasive surgery can reduce the surgeon's workload by autonomously guiding robotic forceps. Movement of the robot is restricted around a fixed insertion port. The robot often encounters angle limitations during operation. Also, the surface of the abdominal cavity is non-concave, making it computationally expensive to find the desired path.In this work, to solve these problems, we propose a method for path planning in joint space by transforming the position into a Riemannian manifold. An edge cost function is defined to search for a desired path in the joint space and reduce the range of motion of the joints. We found that the organ is mostly non-concave, making it easy to find the optimal path using gradient descent method. Experimental results demonstrated that the proposed method reduces the range of joint angle movement compared to calculations in position space.

Yoshiki Yamamoto, Maina Sogabe, Shunichi Hirahara +3

The University of Tokyo, Department of Information Physics and Computing, Tokyo, 113-8656, Japan · The University of Tokyo, Department of Precision Engineering, Tokyo, 113-8656, Japan · National Institute of Informatics, Principles of Informatics Research Division, Tokyo, 101-8430, Japan

Robot Motion PlanningMotion PlanningSurgical RoboticsConstrained Motion Planning
AskPDF
Mar 13, 2026

CarPLAN: Context-Adaptive and Robust Planning with Dynamic Scene Awareness for Autonomous Driving

Imitation learning (IL) is widely used for motion planning in autonomous driving due to its data efficiency and access to real-world driving data. For safe and robust real-world driving, IL-based planning requires capturing the complex driving contexts inherent in real-world data and enabling context-adaptive decision-making, rather than relying solely on expert trajectory imitation. In this paper, we propose CarPLAN, a novel IL-based motion planning framework that explicitly enhances driving context understanding and enables adaptive planning across diverse traffic scenarios. Our contributions are twofold: We introduce Displacement-Aware Predictive Encoding (DPE) to improve the model's spatial awareness by predicting future displacement vectors between the Autonomous Vehicle (AV) and surrounding scene elements. This allows the planner to account for relational spacing when generating trajectories. In addition to the standard imitation loss, we incorporate an augmented loss term that captures displacement prediction errors, ensuring planning decisions consider relative distances from other agents. To improve the model's ability to handle diverse driving contexts, we propose Context-Adaptive Multi-Expert Decoder (CMD), which leverages the Mixture of Experts (MoE) framework. CMD dynamically selects the most suitable expert decoders based on scene structure at each Transformer layer, enabling adaptive and context-aware planning in dynamic environments. We evaluate CarPLAN on the nuPlan benchmark and demonstrate state-of-the-art performance across all closed-loop simulation metrics. In particular, CarPLAN exhibits robust performance on challenging scenarios such as Test14-Hard, validating its effectiveness in complex driving conditions. Additional experiments on the Waymax benchmark further demonstrate its generalization capability across different benchmark settings.

Junyong Yun, Jungho Kim, ByungHyun Lee +5

Department of Artificial Intelligence, Hanyang University, 04763, Republic of Korea. · Interdisciplinary Program in Artificial Intelligence, Seoul National University, 08826, Seoul, Republic of Korea · Department of Electrical and Computer Engineering, Seoul National University, 08826, Seoul, Republic of Korea. +1

Motion PlanningAutonomous Driving PlanningAutonomous DrivingMixture of Experts
AskPDF
Mar 12, 2026

CoAd: Constant-Time Planning for Continuous Goal Manipulation with Compressed Library and Online Adaptation

In many robotic manipulation tasks, the robot repeatedly solves motion-planning problems that differ mainly in the location of the goal object and its associated obstacle, while the surrounding workspace remains fixed. Prior works have shown that leveraging experience and offline computation can accelerate repeated planning queries, but they lack guarantees of covering the continuous task space and require storing large libraries of solutions. In this work, we present COAD, a framework that provides constant-time planning over a continuous goal-parameterized task space. COAD discretizes the continuous task space into finitely many Task Coverage Regions. Instead of planning and storing solutions for every region offline, it constructs a compressed library by only solving representative root problems. Other problems are handled through fast adaptation from these root solutions. At query time, the system retrieves a root motion in constant time and adapts it to the desired goal using lightweight adaptation modules such as linear interpolation, Dynamic Movement Primitives, or simple trajectory optimization. We evaluate the framework on various manipulators and environments in simulation and the real world, showing that COAD achieves substantial compression of the motion library while maintaining high success rates and sub-millisecond-level queries, outperforming baseline methods in both efficiency and path quality. The source code is available at https://github.com/elpis-lab/CoAd.

Adil Shiyas, Zhuoyun Zhong, Constantinos Chamzas

Department of Robotics Engineering, Worcester Polytechnic Institute (WPI), Worcester, MA 01609, USA

Robot Motion PlanningMotion PlanningRobotic Manipulation
AskPDF
Mar 12, 2026

Push, Press, Slide: Mode-Aware Planar Contact Manipulation via Reduced-Order Models

Non-prehensile planar manipulation, including pushing and press-and-slide, is critical for diverse robotic tasks, but notoriously challenging due to hybrid contact mechanics, under-actuation, and asymmetric friction limits that traditionally necessitate computationally expensive iterative control. In this paper, we propose a mode-aware framework for planar manipulation with one or two robotic arms based on contact topology selection and reduced-order kinematic modeling. Our core insight is that complex wrench-twist limit surface mechanics can be abstracted into a discrete library of physically intuitive models. We systematically map various single-arm and bimanual contact topologies to simple non-holonomic formulations, e.g. unicycle for simplified press-and-slide motion. By anchoring trajectory generation to these reduced-order models, our framework computes the required object wrench and distributes feasible, friction-bounded contact forces via a direct algebraic allocator. We incorporate manipulator kinematics to ensure long-horizon feasibility and demonstrate our fast, optimization-free approach in simulation across diverse single-arm and bimanual manipulation tasks. Supplementary videos and additional information are available at: https://sites.google.com/view/pushpressslide

Melih Özcan, Umut Orguner, Ozgur S. Oguz

Dept. of Electrical and Electronics Engineering, Middle East Technical University, Ankara, Turkiye. · Dept. of Computer Engineering, Bilkent University, Ankara, Turkiye.

Non-Prehensile Robotic ManipulationRobotic ManipulationContact-Rich Robotic ManipulationRobotic Object PushingMotion PlanningBimanual Robotic Manipulation+1
AskPDF
Mar 12, 2026

GNN-DIP: Neural Corridor Selection for Decomposition-Based Motion Planning

Motion planning through narrow passages remains a core challenge: sampling-based planners rarely place samples inside these narrow but critical regions, and even when samples land inside a passage, the straight-line connections between them run close to obstacle boundaries and are frequently rejected by collision checking. Decomposition-based planners resolve both issues by partitioning free space into convex cells -- every passage is captured exactly as a cell boundary, and any path within a cell is collision-free by construction. However, the number of candidate corridors through the cell graph grows combinatorially with environment complexity, creating a bottleneck in corridor selection. We present GNN-DIP, a framework that addresses this by integrating a Graph Neural Network (GNN) with a two-phase Decomposition-Informed Planner (DIP). The GNN predicts portal scores on the cell adjacency graph to bias corridor search toward near-optimal regions while preserving completeness. In 2D, Constrained Delaunay Triangulation (CDT) with the Funnel algorithm yields exact shortest paths within corridors; in 3D, Slab convex decomposition with portal-face sampling provides near-optimal path evaluation. Benchmarks on 2D narrow-passage scenarios, 3D bottleneck environments with up to 246 obstacles, and dynamic 2D settings show that GNN-DIP achieves 99--100% success rates with 2--280 times speedup over sampling-based baselines.

Peng Xie, Yanliang Huang, Wenyuan Wu +1

TUM School of Computation, Information and Technology, Department of Computer Engineering, Technical University of Munich, 74076 Heilbronn, Germany

Motion Planning
AskPDF
Mar 12, 2026

Coupling Tensor Trains with Graph of Convex Sets: Effective Compression, Exploration, and Planning in the C-Space

We present TANGO (Tensor ANd Graph Optimization), a novel motion planning framework that integrates tensor-based compression with structured graph optimization to enable efficient and scalable trajectory generation. While optimization-based planners such as the Graph of Convex Sets (GCS) offer powerful tools for generating smooth, optimal trajectories, they typically rely on a predefined convex characterization of the high-dimensional configuration space-a requirement that is often intractable for general robotic tasks. TANGO builds further by using Tensor Train decomposition to approximate the feasible configuration space in a compressed form, enabling rapid discovery and estimation of task-relevant regions. These regions are then embedded into a GCS-like structure, allowing for geometry-aware motion planning that respects both system constraints and environmental complexity. By coupling tensor-based compression with structured graph reasoning, TANGO enables efficient, geometry-aware motion planning and lays the groundwork for more expressive and scalable representations of configuration space in future robotic systems. Rigorous simulation studies on planar and real robots reinforce our claims of effective compression and higher quality trajectories.

Gerhard Reinerth, Riddhiman Laha, Marcello Romano

Technical University of Munich, Germany · Northeastern University, Boston, USA

Motion PlanningRobot Motion PlanningTrajectory Generation
AskPDF
Mar 11, 2026

ADMM-based Continuous Trajectory Optimization in Graphs of Convex Sets

This paper presents a numerical solver for computing continuous trajectories in non-convex environments. Our approach relies on a customized implementation of the Alternating Direction Method of Multipliers (ADMM) built upon two key components: First, we parameterize trajectories as polynomials, allowing the primal update to be computed in closed form as a minimum-control-effort problem. Second, we introduce the concept of a spatio-temporal allocation graph based on a mixed-integer formulation and pose the slack update as a shortest-path search. The combination of these ingredients results in a solver with several distinct advantages over the state of the art. By jointly optimizing over both discrete and continuous domains, our method accesses a larger search space than existing decoupled approaches, enabling the discovery of superior trajectories. Additionally, the solver's structural robustness ensures reliable convergence from naive initializations, removing the bottleneck of complex warm starting in non-convex environments.

Lukas Pries, Jon Arrizabalaga, Zachary Manchester +1

Autonomous Aerial Systems Lab, Dep. of Aerospace and Geodesy, TU Munich, Germany · Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, USA

Trajectory OptimizationAlternating Direction Method of MultipliersNonconvex OptimizationConstrained OptimizationMotion Planning
AskPDF
Mar 11, 2026

OnFly: Onboard Zero-Shot Aerial Vision-Language Navigation toward Safety and Efficiency

Aerial vision-language navigation (AVLN) enables UAVs to follow natural-language instructions in complex 3D environments. However, existing zero-shot AVLN methods often suffer from unstable single-stream Vision-Language Model decision-making, unreliable long-horizon progress monitoring, and a trade-off between safety and efficiency. We propose OnFly, a fully onboard, real-time framework for zero-shot AVLN. OnFly adopts a shared-perception dual-agent architecture that decouples high-frequency target generation from low-frequency progress monitoring, thereby stabilizing decision-making. It further employs a hybrid keyframe-recent-frame memory to preserve global trajectory context while maintaining KV-cache prefix stability, enabling reliable long-horizon monitoring with termination and recovery signals. In addition, a semantic-geometric verifier refines VLM-predicted targets for instruction consistency and geometric safety using VLM features and depth cues, while a receding-horizon planner generates optimized collision-free trajectories under geometric safety constraints, improving both safety and efficiency. In simulation, OnFly improves task success from 26.4% to 67.8%, compared with the strongest state-of-the-art baseline, while fully onboard real-world flights validate its feasibility for real-time deployment. The code will be released at https://github.com/Robotics-STAR-Lab/OnFly

Guiyong Zheng, Yueting Ban, Mingjie Zhang +2

School of Artificial Intelligence, Sun Yat-Sen University, Zhuhai, China. · Southern University of Science and Technology, Shenzhen, China · The Hong Kong University of Science and Technology, Guangzhou, China.

Vision-Language NavigationAerial VLNUAV NavigationMemory-Augmented VLMsMotion Planning
AskPDF
Mar 11, 2026

Interleaving Scheduling and Motion Planning with Incremental Learning of Symbolic Space-Time Motion Abstractions

Task and Motion Planning combines high-level task sequencing (what to do) with low-level motion planning (how to do it) to generate feasible, collision-free execution plans. However, in many real-world domains, such as automated warehouses, tasks are predefined, shifting the challenge to if, when, and how to execute them safely and efficiently under resource, time and motion constraints. In this paper, we formalize this as the Scheduling and Motion Planning problem for multi-object navigation in shared workspaces. We propose a novel solution framework that interleaves off-the-shelf schedulers and motion planners in an incremental learning loop. The scheduler generates candidate plans, while the motion planner checks feasibility and returns symbolic feedback, i.e., spatial conflicts and timing adjustments, to guide the scheduler towards motion-feasible solutions. We validate our proposal on logistics and job-shop scheduling benchmarks augmented with motion tasks, using state-of-the-art schedulers and sampling-based motion planners. Our results show the effectiveness of our framework in generating valid plans under complex temporal and spatial constraints, where synchronized motion is critical.

Elisa Tosello, Arthur Bit-Monnot, Davide Lusuardi +2

Fondazione Bruno Kessler, Trento, Italy · LAAS-CNRS, Université de Toulouse, CNRS, INSA, Toulouse, France

Motion PlanningConstrained Motion PlanningMulti-Robot Motion Planning
AskPDF