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.
IEEE Transactions on Automation Science and Engineering
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.
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.
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 s, replanning produced a new feasible trajectory at t = 19 s, and the robot switched to it and continued to the goal.
