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

TL;DR

This work introduces a hierarchical trajectory planning framework for floating-base multi-link robots that integrates global guidance with configuration-aware local optimization and enables an articulated aerial robot to exploit its morphology for maneuvering that rigid robots cannot achieve.

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 (13)

• Figure 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.
• Figure 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.
• Figure 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.
• Figure 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.
• Figure 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.