cs.ROFeb 25, 2026

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

Authors: Yicheng Chen, Jinjie Li, Haokun Liu, Zicheng Luo, Kotaro Kaneko, Moju Zhao

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

Read PDF
Ask AI
Cite

Copy citation

DOINot available for this paper

Abstract

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

Figures & tables

Fig. 1: Top-down view of a floating-base multi-link robot maneuvering through a U-shaped passage by deforming its structure using the hierarchical trajectory planning framework.
SymbolDomainDescription
Robot Configuration
DDZ+\mathbb{Z}^{+}Dimension of configuration space
mm{2,3}\{2,3\}Dimension of root link operational space
q\boldsymbol{q}RD\mathbb{R}^{D}Full robot configuration vector
pr\boldsymbol{p}_{r}Rm\mathbb{R}^{m}Root link position vector
Rr\boldsymbol{R}_{r}SO(m)\mathrm{SO}(m)Root link rotation matrix
TABLE I: Notation used in this paper
Fig. 2: Kinematic model of the floating-base multi-link robot. The root link position is denoted by pr\boldsymbol{p}_{r} . Consecutive links are connected by revolute joints, each allowing a single degree of rotational freedom. The joint angle θ1\theta_{1} , for example, is defined as the relative rotation between two adjacent links. The origin of the frame {CoG} is the center of gravity of the robot.
Fig. 3: System overview. The hierarchical trajectory planning framework decomposes the planning problem into independently solvable segments by introducing global anchor states, enabling parallel local planning. With clamped B-spline trajectory parameterization, continuity between adjacent segments is inherently maintained in the overall trajectory. Each segment is optimized in parallel with fully differential objective and constraint functions. The optimized local segments are directly concatenated in sequence to produce the final trajectory.
Fig. 4: Illustration of generating the candidate local target set Q\mathcal{Q} from a given local initial state qinit\boldsymbol{q}^{\text{init}} . The set Q\mathcal{Q} is obtained by sampling the root link’s new pose within the allowable joint angle range, exploring configurations located one link length ahead of the initial state.
Fig. 5: Illustration of candidate target evaluation. Each candidate configuration is evaluated based on two factors: the distance between its root position and the global reference path, and the fraction of the path that remains ahead of this closest point. An ideal target should lie close to the reference path while advancing along it.
Fig. 6: Overview of the perception, planning, and control pipeline. Coordinate transformation is managed by the TF tools in ROS.
Fig. 7: Simulation of a floating-base multi-link robot navigating through a confined environment with narrow passages. (a) Illustration of the global anchor states, each specifying a complete robot configuration, with the global initial and target states serving as the first and last global anchor states. There were seven anchor states in total. (b) Snapshots of the robot along the flight trajectory. (c) Changing histories of the objective function for all six trajectory segments, where red dots mark the minimum values of the objective. (d) Desired trajectories of the root link position and yaw angle (top) and the joint angles (bottom), with background shading indicating different planning segments.
Param.ValueParam.ValueParam.Value
Robot
DD66RpR_{p}0.2025 m0.2025\text{\,}\mathrm{m}θmin⁡\theta_{\min}−90 \SIUnitSymbolDegree-90\text{\,}\mathrm{\SIUnitSymbolDegree}
mm22njn_{j} , nrn_{r}3, 43,\ 4θmax⁡\theta_{\max}90 \SIUnitSymbolDegree90\text{\,}\mathrm{\SIUnitSymbolDegree}
LL0.6 m0.6\text{\,}\mathrm{m}δτ\delta_{\tau}0.001\text{\,}\mathrm{N}\text{\mathord{\cdot}}\mathrm{m}κ\kappa−0.0182 m-0.0182\text{\,}\mathrm{m}
Trajectory Planning
NN55vmax⁡v_{\max}1.0 m/s1.0\text{\,}\mathrm{m}\text{/}\mathrm{s}nθn_{\theta}6060
TABLE II: Robot and algorithm parameters
Fig. 8: Computation time comparison between our planner and its sequential counterpart (w/o PC). Parallel computation substantially reduces runtime while preserving the same planning formulation.
Fig. 9: Comparison of planned motions from different algorithms in one trial. (a) Mission setup with start and goal states. (b) The proposed planner generated a feasible trajectory through the narrow gap. (c) The DK-based planner produced a motion that passed the gap but lacked the flexibility to reach the designated target state. (d) The variant without global anchor states (w/o AS) completely failed to find a feasible trajectory. (e) The variant without local planning (w/o LP) produced a trajectory that collides with the obstacle. (f) The anchor states obtained in (e) were individually feasible, but simple linear interpolation between them led to a collision, underscoring the necessity of local planning within the framework.
AlgorithmSuccess Rate ∗ ( %\mathrm{\%} ) ↑\uparrowComputation Time ( s\mathrm{s} ) ↓\downarrowRoot Trajectory Length ( m\mathrm{m} ) ↓\downarrowGeneralized Trajectory Length ↓\downarrow
AverageStandard DeviationAverageStandard DeviationAverageStandard Deviation
Ours92.5 92.5\text{\,}9.01 9.01\text{\,}1.66 1.66\text{\,}2.84 2.84\text{\,}0.302 0.302\text{\,}10.2 10.2\text{\,}0.601 0.601\text{\,}
w/o AS0.0 0.0\text{\,}––––––
w/o LP14.0 14.0\text{\,}0.307 0.307\text{\,}0.0240 0.0240\text{\,}––––
w/o PC92.0 92.0\text{\,}24.2 24.2\text{\,}4.30 4.30\text{\,}2.87 2.87\text{\,}0.417 0.417\text{\,}10.3 10.3\text{\,}0.667 0.667\text{\,}
DK [ 49 ]13.0 13.0\text{\,}0.0580 0.0580\text{\,}0.003 53 0.003\,53\text{\,}––––
TABLE III: Results of ablation and benchmark tests over 200 trials
Fig. 10: Influence of hyperparameters on planning performance. The plots showed the effects of the number of candidate configurations nθn_{\theta} generated per anchor-state iteration (Cand Num) and the sampling density αK\alpha_{K} used for continuous-time constraints (Sample Dens) on (a) the success rate and (b) the average computation time. The results indicated that the success rate was primarily governed by the candidate number, whereas the average computation time was dominated by the sampling density.
Fig. 11: Evaluation with real-world point clouds under environment changes. A handheld Livox Mid-360 LiDAR was used to capture the scene, and FAST-LIO was used to reconstruct two registered point clouds before and after the obstacles were rearranged. (a) Experimental scene. (b) Point cloud of the initial scene. (c) Online volumetric occupancy map reconstructed from (b) and used for the initial plan. (d) Updated occupancy map after switching the input to the second point cloud, where the original passage was blocked (red boxes). (e) Execution snapshots in Gazebo (time was measured from the onset of motion): the robot started with the initial plan, the map update blocked the original passage at t=t=17\text{,}\mathrm{s} , replanning produced a new feasible trajectory at $t=$19\text{\,}\mathrm{s} , and the robot switched to it and continued to the goal.
Fig. 12: The robot used in the experiments. In its square configuration, the robot’s diameter was 0.89 m0.89\text{\,}\mathrm{m} , larger than the gap width, requiring it to transform its shape to maneuver through the gaps.
Fig. 13: Experimental demonstration of the floating-base multi-link robot performing complex maneuvers in confined environments. The first four rows show four representative scenarios: (a) single gap, (b) triple gaps, (c) pole obstacles, and (d) U-shaped passage. Columns 1–5 depict sequential snapshots where the robot adapted its articulated configuration to maneuver through narrow gaps and obstacles, with white arrows in the first column indicating motion direction. Subfigures (e)–(l) present the corresponding CoG and joint angle trajectories. Together, these results demonstrate that the hierarchical trajectory planning framework generated smooth, feasible motions and effectively exploited the robot’s multi-link morphology to achieve maneuvering infeasible for conventional rigid robots.
Maximum root-link velocities on trajectory
TrialX-Trans. ( m/s\mathrm{m}\text{/}\mathrm{s} )Y-Trans. ( m/s\mathrm{m}\text{/}\mathrm{s} )Yaw-Rot. ( rad/s\mathrm{rad}\text{/}\mathrm{s} )
a0.077 0.077\text{\,} (15%)0.100 0.100\text{\,} (20%)0.169 0.169\text{\,} (68%)
b0.146 0.146\text{\,} (29%)0.149 0.149\text{\,} (30%)0.202 0.202\text{\,} (81%)
c0.149 0.149\text{\,} (30%)0.133 0.133\text{\,} (27%)0.139 0.139\text{\,} (56%)
d0.0975 0.0975\text{\,} (20%)0.114 0.114\text{\,} (23%)0.155 0.155\text{\,} (62%)
Maximum joint angular velocities on trajectory ( rad/s\mathrm{rad}\text{/}\mathrm{s} )
TABLE IV: Peak trajectory velocities and the controllability metric in four experimental trials.
Figure 18
Figure 19
Figure 20
Figure 21
Figure 22
Figure 23
More in this topic

Robot Motion Planning

Robot motion planning finds safe, feasible paths and trajectories for robots moving through constrained, changing environments.

See all 271 papers

Explore similar work

CardsList
Aug 9, 2026

Hierarchical Topology-Aware Planning and Control of Underwater Vehicle-Manipulator Systems in Confined Environments

This paper addresses autonomous intervention with an underwater vehicle--manipulator system (UVMS) in confined, cluttered, and partially known environments, where poor maneuverability, narrow passages, and uncertain execution may cause the robot to enter unrecoverable regions. We propose MANTA, a three-layer hierarchical planning-and-control framework that couples passage accessibility, manipulation feasibility, and closed-loop execution. The first layer performs global connectivity reasoning in a conservative reduced base space to extract traversable corridor candidates toward the task region. The second layer refines each candidate corridor by jointly optimizing the continuous base motion and arm trajectory, producing a collision-free base--arm trajectory. The third layer learns a reach-and-hold base policy using Gaussian-process model-based reinforcement learning (MBRL) through MC-PILCO, enabling trajectory tracking and station keeping at the planned manipulation state. During execution, the framework monitors map updates and can trigger recovery and route repair when the active passage becomes infeasible. MANTA is evaluated in confined UVMS planning and closed-loop tracking experiments. Across 120 matched planning queries, it achieves higher task success than full-state sampling-based baselines while producing larger clearance margins and lower arm motion. The learned MC-PILCO policy further reduces position and yaw tracking errors on both training and unseen tube-like references. These results show MANTA as a structured and data-efficient framework for safe autonomous underwater intervention in caves, tubes, and cluttered subsea structures.

Mohamed Abdelwahab, Ruggero Carli, Damiano Varagnolo +1

Department of Information Engineering, University of Padova, Via Gradenigo 6/B, 35131 Padova, Italy · Department of Electrical and Information Engineering, Polytechnic University of Bari, Via Orabona 4, 70125 Bari, Italy · Department of Engineering Cybernetics, Norwegian University of Science and Technology, O. S. Bragstads plass 2D, 7034 Trondheim, Norway

Underwater RoboticsSafe Motion PlanningMotion PlanningAutonomous Underwater NavigationRobot Motion Planning
AskPDF
Aug 10, 2026

Whole-Body Planning for Humanoids Navigating Confined Spaces via Self-Collision Avoidance References

Humanoid locomotion in highly confined environments requires navigating dense environmental obstacles and complex self-collision bounds while maintaining multi-contact dynamic feasibility. Traditional trajectory optimizers frequently struggle in these restricted spaces, as navigating the large collision space with splines on particle abstractions is insufficient and leads to poor local minima. To address this, we propose a three-stage whole-body planning framework that formulates kinematic path planning directly over kinematically reachable rigid-body volumes. By integrating differentiable collision avoidance into a reachability-constrained formulation, our framework synthesizes volume-informed guides that reliably guide a full-order trajectory optimizer over long horizons. We show that these optimized plans serve as high-quality references to train a residual reinforcement learning policy for robust online execution. We validate our approach on the Unitree G1 humanoid across three benchmark testbeds exceeding NIST emergency response standards, achieving restricted confinement ratios (Cr<1.5C_r < 1.5). Our framework generates feasible trajectories across 12-to-18-second tasks with complex foot and hand contacts where standard baselines fail, while the learned policy successfully tracks these plans under extensive domain randomization in physics simulation.

Carlos Gonzalez, Luis Sentis

Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712, USA.

Constrained Motion PlanningHumanoid Robot LocomotionKinodynamic Motion PlanningRobot Motion PlanningRobot Collision Avoidance
AskPDF
Jun 8, 2026

Motion planning for hundreds of floating robots

Planning collision-free motion for large robot fleets is difficult because collision avoidance induces strong inter-agent coupling that grows rapidly with team size. We consider omnidirectional floating robots on water, where choreographies are specified by sparse keyframes and an interactive tool must generate trajectories within seconds, even when transitions span minutes and thousands of time steps. We propose a scalable pipeline that builds a collision graph from an initialization, decomposes the coupled problem into interaction clusters, and solves clusters independently (and in parallel) with robustness mechanisms for common decomposition pathologies. We validate the approach in simulations up to 500 robots. The synthesized trajectories have also been deployed in two real-world demonstrations, on Lake Zürich with a fleet of 24 Way of Water crafts and at the Time Space Existence 2025 Venice Biennale.

Jan Kamm, Antonio Terpin, Raffaello D'Andrea +1

Institute for Dynamic Systems and Control, ETH Zürich

Multi-Robot Motion PlanningTrajectory GenerationRobot Motion GenerationRobot Collision Avoidance
AskPDF