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

Yicheng Chen; Jinjie Li; Haokun Liu; Zicheng Luo; Kotaro Kaneko; Moju Zhao

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

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.

Symbol	Domain	Description
Robot Configuration
$D$	$\mathbb{Z}^{+}$	Dimension of configuration space
$m$	$\{2,3\}$	Dimension of root link operational space
$\boldsymbol{q}$	$\mathbb{R}^{D}$	Full robot configuration vector
$\boldsymbol{p}_{r}$	$\mathbb{R}^{m}$	Root link position vector
$\boldsymbol{R}_{r}$	$\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 $\boldsymbol{p}_{r}$ . Consecutive links are connected by revolute joints, each allowing a single degree of rotational freedom. The joint angle $\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 $\mathcal{Q}$ from a given local initial state $\boldsymbol{q}^{\text{init}}$ . The set $\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.

Robot
Param.	Value	Param.	Value	Param.	Value
$D$	$6$	$R_{p}$	$0.2025\text{\,}\mathrm{m}$	$\theta_{\min}$	$-90\text{\,}\mathrm{\SIUnitSymbolDegree}$
$m$	$2$	$n_{j}$ , $n_{r}$	$3,\ 4$	$\theta_{\max}$	$90\text{\,}\mathrm{\SIUnitSymbolDegree}$
$L$	$0.6\text{\,}\mathrm{m}$	$\delta_{\tau}$	$0.001\text{\,}\mathrm{N}\text{\mathord{\cdot}}\mathrm{m}$	$\kappa$	$-0.0182\text{\,}\mathrm{m}$
Trajectory Planning
$N$	$5$	$v_{\max}$	$1.0\text{\,}\mathrm{m}\text{/}\mathrm{s}$	$n_{\theta}$	$60$

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.

Panel of Figure 11 — 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.

Algorithm	Success Rate ∗ ( $\mathrm{\%}$ ) $\uparrow$	Computation Time ( $\mathrm{s}$ ) $\downarrow$		Root Trajectory Length ( $\mathrm{m}$ ) $\downarrow$		Generalized Trajectory Length $\downarrow$
Algorithm	Success Rate ∗ ( $\mathrm{\%}$ ) $\uparrow$	Average	Standard Deviation	Average	Standard Deviation	Average	Standard Deviation
Ours	$92.5\text{\,}$	$9.01\text{\,}$	$1.66\text{\,}$	$2.84\text{\,}$	$0.302\text{\,}$	$10.2\text{\,}$	$0.601\text{\,}$
w/o AS	$0.0\text{\,}$	–	–	–	–	–	–
w/o LP	$14.0\text{\,}$	$0.307\text{\,}$	$0.0240\text{\,}$	–	–	–	–
w/o PC	$92.0\text{\,}$	$24.2\text{\,}$	$4.30\text{\,}$	$2.87\text{\,}$	$0.417\text{\,}$	$10.3\text{\,}$	$0.667\text{\,}$
DK [ 49 ]	$13.0\text{\,}$	$0.0580\text{\,}$	$0.003\,53\text{\,}$	–	–	–	–

TABLE III: Results of ablation and benchmark tests over 200 trials

Panel of Figure 13 — Fig. 10: Influence of hyperparameters on planning performance. The plots showed the effects of the number of candidate configurations $n_{\theta}$ generated per anchor-state iteration (Cand Num) and the sampling density $\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=$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. 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=$ 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\text{\,}\mathrm{m}$ , larger than the gap width, requiring it to transform its shape to maneuver through the gaps.

Panel of Figure 16 — 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
Trial	X-Trans. ( $\mathrm{m}\text{/}\mathrm{s}$ )	Y-Trans. ( $\mathrm{m}\text{/}\mathrm{s}$ )	Yaw-Rot. ( $\mathrm{rad}\text{/}\mathrm{s}$ )
a	$0.077\text{\,}$ (15%)	$0.100\text{\,}$ (20%)	$0.169\text{\,}$ (68%)
b	$0.146\text{\,}$ (29%)	$0.149\text{\,}$ (30%)	$0.202\text{\,}$ (81%)
c	$0.149\text{\,}$ (30%)	$0.133\text{\,}$ (27%)	$0.139\text{\,}$ (56%)
d	$0.0975\text{\,}$ (20%)	$0.114\text{\,}$ (23%)	$0.155\text{\,}$ (62%)
Maximum joint angular velocities on trajectory ( $\mathrm{rad}\text{/}\mathrm{s}$ )

TABLE IV: Peak trajectory velocities and the controllability metric in four experimental trials.

Robot Motion Planning

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

See all 271 papers

Hierarchical Topology-Aware Planning and Control of Underwater Vehicle-Manipulator Systems in Confined Environments
Aug 9, 2026Mohamed Abdelwahab, Ruggero Carli, Damiano Varagnolo +1Underwater Robotics Safe Motion Planning
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
Whole-Body Planning for Humanoids Navigating Confined Spaces via Self-Collision Avoidance References
Aug 10, 2026Carlos Gonzalez, Luis SentisConstrained Motion Planning Humanoid Robot Locomotion
Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712, USA.
Motion planning for hundreds of floating robots
Jun 8, 2026Jan Kamm, Antonio Terpin, Raffaello D'Andrea +1Multi-Robot Motion Planning Trajectory Generation
Institute for Dynamic Systems and Control, ETH Zürich

Aug 9, 2026J/K move · Enter open · S save

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

Mohamed Abdelwahab, Ruggero Carli, Damiano Varagnolo, Alberto Dalla Libera

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 Robotics Safe Motion Planning Motion Planning Autonomous Underwater Navigation Robot Motion Planning

Open paper AskPDF arXiv

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.

Abstract

Robot Motion Planning

Explore similar work

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

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

Motion planning for hundreds of floating robots