In regions with sufficient clearance, the quadrotor is approximated as a point mass, allowing collision checking to be performed using only its center position. Let the SFC associated with the i-th trajectory segment be

(9) $$ \mathcal{P}_i^{\mathcal{H}} = \left\{ \mathbf{x}\in\mathbb{R}^{3} \;\middle|\; \mathbf{A}_i\mathbf{x}\preceq\mathbf{b}_i \right\}, $$

where $$ \mathbf{A}_i\in\mathbb{R}^{m_i\times 3} $$, $$ \mathbf{b}_i\in\mathbb{R}^{m_i} $$, and m_i is the number of planes of the i-th SFC. The position-only collision-free condition is then given by

(10) $$ \mathbf{A}_i\mathbf{p}(t) - \mathbf{b}_i \preceq \mathbf{0},\ t\in[0,T_i],\ i=0,1,\ldots,M-1, $$

where p(t) denotes the position of the quadrotor center and T_i denotes the duration of the corresponding trajectory segment. This condition ensures that the quadrotor center remains within its associated SFC throughout the segment.

To maintain additional clearance from the SFC boundary, we introduce a safety distance d_safe > 0 and strengthen the position-only collision-free condition as

(11) $$ \mathbf{A}_i\mathbf{p}(t) - \mathbf{b}_i + d_{\mathrm{safe}}\mathbf{1}_{m_i} \preceq \mathbf{0}, \ t\in[0,T_i], $$

where the rows of A_i are normalized and $$ \mathbf{1}_{m_i}\in\mathbb{R}^{m_i} $$ is a vector of ones.

In narrow regions, a point-mass approximation is insufficient because the collision-free configuration of the quadrotor depends on its attitude. We therefore represent the quadrotor body as a convex polyhedron and perform attitude-aware collision checking using all its vertices. Let $$ \mathbf{Q}\in\mathbb{R}^{3\times N_v} $$ collect the body-frame coordinates of the N_v vertices:

(12) $$ \mathbf{Q} = \begin{bmatrix} \mathbf{q}_1 & \mathbf{q}_2 & \cdots & \mathbf{q}_{N_v} \end{bmatrix}. $$

The corresponding world-frame vertex coordinates at time t are

(13) $$ \mathbf{V}(t) = \mathbf{R}(t)\mathbf{Q} + \mathbf{p}(t)\mathbf{1}_{N_v}^{\top}, $$

where R(t) ∈ SO(3) is the rotation matrix recovered from the differential-flatness mapping, p(t) is the position of the quadrotor center, and $$ \mathbf{1}_{N_v}\in\mathbb{R}^{N_v} $$ is a vector of ones.

Because both the quadrotor model and the SFC are convex, the entire quadrotor body lies within the i-th SFC if and only if all its vertices lie within that SFC. The attitude-aware collision-free condition can therefore be expressed as

(14) $$ \mathbf{A}_i \left( \mathbf{R}(t)\mathbf{Q} + \mathbf{p}(t)\mathbf{1}_{N_v}^{\top} \right) - \mathbf{b}_i\mathbf{1}_{N_v}^{\top} \preceq \mathbf{0}, \ t\in[0,T_i], \ i=0,1,\ldots,M-1. $$

Unlike the position-only condition in Equation (10), the attitude-aware condition explicitly accounts for attitude. Because the rotation matrix is derived from the planned acceleration and yaw angle, this condition captures the effect of acceleration-induced attitude changes and is applied in narrow regions where attitude adjustment is essential for collision-free traversal.

To improve the efficiency of search-based whole-body planning, we construct an ordered sequence of SFCs along a center-feasible guide path. By restricting the expansion of motion primitives to these corridors, the proposed method avoids exploring redundant regions, thereby reducing the number of expanded nodes and the overall search time. SFCs generated in spacious and narrow regions generally exhibit substantially different spatial scales. This observation motivates us to divide the generated corridors into Pos-SFCs and Att-SFCs, as illustrated in Figure 3.

The guide path is first generated by a front-end path-searching algorithm and is represented as Γ = {q₀, q₁, …, $$ \mathbf{q}_{N_g-1} $$}, where q₀ and $$ \mathbf{q}_{N_g-1} $$ denote the initial and goal positions, respectively. At this stage, the guide path guarantees only collision-free traversal of the quadrotor center and does not explicitly account for its attitude. To identify guide-path intervals where attitude adjustment may be required, we introduce a bounding sphere that conservatively encloses the cuboid model. Its radius is defined as r_s = $$ \sqrt{d_x^2+d_y^2+d_z^2} $$, where d_x, d_y, and d_z are the half-lengths defined in Equation (17). Because the radius is independent of attitude, the bounding sphere provides an efficient and conservative measure of local clearance along the guide path.

For each guide point q_k, a radius query is performed on the obstacle-point KD-tree. The corresponding sphere-collision indicator is defined as

(21) $$ \chi_k = \begin{cases} 1, & \displaystyle \min_{\mathbf{o}\in\mathcal{O}} \left\| \mathbf{o}-\mathbf{q}_k \right\|_2 ≤ r_{\mathrm{s}},\\[2mm] 0, & \text{otherwise}, \end{cases} $$

where $$ \mathcal{O} $$ denotes the obstacle-point set. A guide point satisfying χ_k = 0 provides sufficient clearance for the cuboid bounding sphere and is therefore assigned to a position-only guide. In contrast, χ_k = 1 indicates that the bounding sphere intersects the environment even though the quadrotor center remains collision-free. Such a point is assigned to an attitude-aware guide because the cuboid may need to adjust its orientation to traverse the corresponding region. The transition χ_k-1 = 0, χ_k = 1 defines the entrance of an attitude-aware interval, whereas χ_k-1 = 1, χ_k = 0 marks its end. Consecutive guide points satisfying χ_k = 1 are grouped into the same attitude-aware interval, while the remaining portions of the guide path are assigned to position-only intervals. In this way, the original guide path is converted into an ordered sequence of position-only and attitude-aware portions.

The resulting typed guide path is divided into seed segments, which are used to generate SFCs. Each generated SFC is directly inherited from its corresponding guide seed: position-only seeds generate Pos-SFCs, whereas attitude-aware seeds generate Att-SFCs. This construction restricts the translational search space while identifying regions in which attitude-aware whole-body planning may be required.

Figure 3 compares conventional corridor construction with the proposed typed-seed construction. The blue points represent the endpoints of position-only seeds, whereas the purple points represent the endpoints of attitude-aware seeds. In the conventional method shown in Figure 3A, a uniform seed-generation strategy produces two large polyhedra on opposite sides of the narrow passage. Their overlap is concentrated near the opening and does not provide sufficient space for attitude adjustment and corridor transition. In the proposed method shown in Figure 3B, the bounding-sphere test identifies the attitude-critical portion of the guide path. The purple attitude-aware seed generates the intermediate Att-SFC $$ \mathcal{P}_2 $$, while the blue position-only seeds generate the neighboring Pos-SFCs $$ \mathcal{P}_1 $$ and $$ \mathcal{P}_3 $$. Consequently, $$ \mathcal{P}_2 $$ forms an attitude-aware bridge through the narrow passage and overlaps with the adjacent Pos-SFCs, providing an ordered corridor sequence for the subsequent whole-body search.

After obtaining the ordered SFC sequence, the search is restricted to the corresponding corridor regions, thereby avoiding the expansion of redundant nodes. This property also allows the computationally expensive attitude-aware collision checking to be separated from efficient position-only collision checking.

Specifically, PosCollisionCheck(·) determines whether the sampled positions of a motion primitive remain within its associated Pos-SFC according to Equation (11). In contrast, AttCollisionCheck(·) transforms all cuboid vertices according to the attitude and verifies their containment within the associated Att-SFC using Equation (14). Finally, the accepted primitives retain their corridor types for the subsequent trajectory-refinement stage.

Whole-body path searching is built upon the kinodynamic A* algorithm for quadrotors^[3], as summarized in Algorithm 1. The open and closed lists are denoted by $$ \mathcal{O} $$ and $$ \mathcal{C} $$, respectively, and n_c denotes the currently expanded node. The function ComputeShotPath(·) computes a direct connection from the current node to the goal by analytically solving a linear quadratic minimum time (LQMT) problem^[25]. A shot trajectory is accepted only if it satisfies both the dynamic constraints and the hierarchical collision-checking conditions. Otherwise, candidate motion primitives are sampled and checked according to the type of their associated SFCs. Only collision-free successor nodes are inserted into or updated in the open list.

Figure 4 illustrates the hierarchical whole-body motion-planning mechanism. The blue convex polyhedron denotes a Pos-SFC, in which a position-only trajectory is generated, whereas the purple convex polyhedron denotes an Att-SFC, in which an attitude-aware trajectory is generated.

The acceleration-controlled search explicitly determines the duration of each motion primitive, thereby providing a natural initial time allocation for subsequent trajectory refinement. The durations of the searched trajectory segments are collected as

(22a) $$ \mathbf{T} = [T_0,T_1,\ldots,T_{M-1}]^{\top} \in \mathbb{R}_{+}^{M}, $$

(22b) $$ T_k = \begin{cases} \tau, & k=0,1,\ldots,M-2,\\ t_{\mathrm{opt}}, & k=M-1, \end{cases} $$

where τ denotes the sampling duration of each motion primitive and t_opt is the duration of the terminal shot trajectory obtained by solving the LQMT problem.

After the hierarchical search, the resulting trajectory is divided into attitude-aware and position-only segments. Let $$ \mathcal{A} $$ and $$ \mathcal{P} $$ denote their index sets, respectively, satisfying

(23) $$ \mathcal{A}\cap\mathcal{P} = \varnothing, \qquad \mathcal{A}\cup\mathcal{P} = \{0,1,\ldots,M-1\}. $$

For each k ∈ $$ \mathcal{A} $$, let $$ \bar{\mathbf{p}} $$_k^att(t) denote the searched attitude-aware trajectory, and let s_k^att and g_k^att denote its initial and terminal states, respectively. Because these segments are generated within Att-SFCs and have already passed attitude-aware whole-body collision checking, they are retained unchanged during trajectory refinement. The segments indexed by $$ \mathcal{P} $$ connect the global initial state to the first attitude-aware segment, consecutive attitude-aware segments to each other, and the last attitude-aware segment to the global goal state. These connecting segments lie within Pos-SFCs and are therefore treated as position-only trajectories. The paths obtained from the search stage are used to initialize the subsequent trajectory optimization.

Although acceleration-controlled search substantially improves computational efficiency, directly concatenating the searched motion primitives generally leads to acceleration discontinuities and, consequently, abrupt attitude changes. To improve trajectory continuity without compromising the verified safety of the attitude-aware segments, the proposed refinement strategy keeps all attitude-aware segments fixed and optimizes only the position-only connecting segments. Accordingly, each attitude-aware segment satisfies

(24) $$ \mathbf{p}_k(t) = \bar{\mathbf{p}}_k^{\mathrm{att}}(t), \ t\in[0,T_k], \ k\in\mathcal{A}. $$

The position-only segments are refined by solving the following minimum-jerk problem:

(25a) $$ \min_{\{\mathbf{p}_k(t)\}_{k\in\mathcal{P}}} \quad \sum_{k\in\mathcal{P}} \int_{0}^{T_k} \left\| \mathbf{p}_k^{(3)}(t) \right\|_2^2 \,dt, $$

(25b) $$ \mathrm{s.t.}\quad \mathbf{x}_0(0) = \mathbf{s}_{\mathrm{start}}, \quad \mathbf{x}_{M-1}(T_{M-1}) = \mathbf{s}_{\mathrm{goal}}, $$

(25c) $$ \mathbf{x}_k(T_k) = \mathbf{s}_j^{\mathrm{att}}, \quad (k,j)\in\mathcal{E}_{\mathrm{in}}, $$

(25d) $$ \mathbf{x}_k(0) = \mathbf{g}_j^{\mathrm{att}}, \quad (k,j)\in\mathcal{E}_{\mathrm{out}}, $$

(25e) $$ \mathbf{x}_{k+1}(0) = \mathbf{x}_k(T_k), \ k,k+1\in\mathcal{P}, $$

(25f) $$ \mathbf{p}_k(t) \in \mathcal{P}_k^{\mathcal{H}}, \ t\in[0,T_k], \ k\in\mathcal{P}. $$

Here, $$ \mathcal{E} $$_in denotes the set of stitching relations from position-only segments to their subsequent attitude-aware segments, whereas $$ \mathcal{E} $$_out denotes the set of stitching relations from attitude-aware segments to their succeeding position-only segments. The state of the k-th trajectory segment is defined as

(26) $$ \mathbf{x}_k(t) = \left[ \mathbf{p}_k^{\top}(t), \dot{\mathbf{p}}_k^{\top}(t), \ddot{\mathbf{p}}_k^{\top}(t) \right]^{\top}. $$

The constraints in Equations (25c) and (25d) match the boundary states of each position-only segment to those of the adjacent attitude-aware segment. Specifically, the terminal state of each incoming position-only segment is matched to the initial state of the succeeding attitude-aware segment, while the initial state of each outgoing position-only segment is matched to the terminal state of the preceding attitude-aware segment. The constraint in Equation (25e) imposes position, velocity, and acceleration continuity between adjacent position-only segments. Therefore, C² continuity is enforced at every junction involving an optimized position-only segment.

We first conducted an ablation study to evaluate the contribution of the trajectory-refinement module. We then examined the effects of the main parameters involved in the motion-primitive search and selected their values through comparative experiments in narrow-passage scenarios.

Effect of trajectory refinement: We compared two configurations: (1) Search only, in which the trajectory generated by the acceleration-controlled whole-body search was used directly without subsequent refinement; and (2) Search + refinement, in which the attitude-aware segments were retained, while the position-only segments were refined using the formulation introduced in Section 3.2.3. Each searched motion primitive applies a constant acceleration input over its duration. Although consecutive primitives are connected in position and velocity, their acceleration inputs are not necessarily identical at the junctions. Because the desired roll and pitch angles are recovered from the desired acceleration through differential flatness, these discontinuities can produce abrupt changes in the desired attitude. As shown in Figure 5A, without refinement, such abrupt acceleration-induced attitude changes can be observed at several primitive junctions. The refinement stage improves the higher-order continuity of the position-only portions while preserving the attitude-aware segments. The resulting trajectory therefore provides smoother attitude transitions while traversing narrow passages. As illustrated in Figure 5B, the refined position-only portions exhibit smoother attitude transitions.

Parameter selection. The performance of the motion-primitive search is mainly affected by the primitive cost weight ρ, the primitive duration τ, and the control-space sampling parameter r. The parameter ρ balances traversal time and control effort: a small value favors conservative but longer trajectories, whereas a large value favors faster but potentially more aggressive motions. The duration τ determines the spacing of the state lattice; a small τ increases search resolution and computational cost, while a large τ may produce a lattice that is too sparse to represent feasible transitions near narrow passages. Similarly, increasing r provides a finer control discretization but increases the branching factor as (2r + 1)³, whereas a small r may omit the control inputs required for feasible attitude transitions. Based on comparative experiments in narrow-passage scenarios, we selected ρ = 4u_max², τ = 0.2 s, and r = 2, where u_max denotes the maximum control magnitude along each axis. This setting samples five control inputs per axis and generates 125 candidate motion primitives in the three-dimensional control space, providing a practical compromise among trajectory aggressiveness, state-space coverage, and computational efficiency.

To evaluate the capability of the proposed planner in geometrically constrained environments, we conducted two groups of narrow-passage experiments. The first group evaluates passages with different widths, whereas the second group evaluates rectangular windows with different orientations. In all experiments, the start and goal states were placed on opposite sides of the obstacle, while the dynamic limits and search parameters were kept unchanged.

Passages with different widths: The quadrotor radius was set to 0.35 m, corresponding to a horizontal span of 0.70 m. As shown in Figure 6, the 0.80 m gap is wider than the quadrotor and can therefore be traversed using a position-only trajectory without triggering attitude-aware planning. In contrast, the 0.65 and 0.50 m gaps are narrower than the horizontal span of the quadrotor, requiring progressively larger attitude adjustments to reduce its projected body extent. The results demonstrate that the planner activates attitude-aware motion only when required by the available passage size.

Windows with different orientations: We further evaluated the planner using a narrow rectangular window with fixed dimensions and orientations of 35°, 45°, and 55°. As shown in Figure 7, the planner generates attitude-aware motions that align the projected body shape with the available free space, with larger window inclinations requiring larger roll adjustments.