TiFA: A terrain-informed navigation framework for articulated tracked robots in rescue missions

Navigation of normal ground robots in unstructured environments

For ground robots navigating unstructured environments, assessing the ground’s traversability is crucial. Krüsi et al. (2017) pioneered the use of planar fitting near surface points in point cloud maps to assess traversability, which was further used as a basis for state validity checks in sampling-based planners to generate initial 3D paths. Similarly, PUTN (Jian et al., 2022) employed a dimensional augmentation strategy during sampling, where poses are first sampled in 2D space and then projected onto the ground to recover 3D poses. This approach reduced invalid sampling and enhanced sampling efficiency. Xu et al. (2023) extended this idea by iteratively optimizing the robot’s pose in SE(2) space to obtain the corresponding pose in SE(3) space. Their work provided a more detailed and specific assessment of ground traversability, and the polynomial trajectory optimization improved planning efficiency in complex terrains. To address navigation in multi-floor environments, Wang et al. (2023) identified traversable areas directly in large-scale point cloud maps, eliminating the need for the 2D-to-3D recovery process.

Extracting more detailed maps with terrain information from point clouds is also a common approach. Wang et al. (2017) converted point clouds into multi-layer maps by abstracting spatial features and using slope information to create traversability maps for practical navigation. In contrast, STEP’s abstracted map incorporated more realistic environmental elements such as different terrains and obstacles, allowing the robot to find feasible paths in complex mining environments (Fan et al., 2021).

Moreover, learning-based methods can also assist robots in obtaining traversability maps in complex environments. Weerakoon et al. (2022) and Hu et al. (2021) employed reinforcement learning to identify traversable areas for robots and then used sampling-based planning methods to find feasible paths within these areas. Cai et al. (2023) proposed a more interpretable learning-based method that takes semantic and elevation maps as inputs to a neural network, producing a traction distribution map to guide robot navigation in outdoor environments. However, learning-based path planning methods often exhibit limited robustness and require multiple training rounds for specific scenarios, making them challenging to deploy in practical rescue missions.

Navigation of ATRs in unstructured environments

Tracked robots equipped with auxiliary flippers have more adaptability in unstructured environments. However, algorithms designed for wheeled and traditional tracked robots cannot be directly adapted to solve the ATR navigation problem.

Yuan et al. (2019) proposed a stability evaluation criterion, suggesting that tracked robots with auxiliary flippers should maintain maximum contact with the ground during maneuvers in complex environments to ensure stability. In their work, feasible ATR chassis poses were generated from discrete terrain point clouds, and the optimal path waypoints (including auxiliary flipper joint states) were selected from the corresponding possible configurations. A feedback controller was then used to execute the robot’s path. Similarly, Chen et al. (2023) introduced an evaluation system incorporating sequential considerations and pose prediction to ensure the robot’s motion stability and continuity. Xu et al. (2024) further classified ATR configuration states into several forms and achieved segmented whole-body motion planning and control through mixed optimization.

Gianni et al. (2016) proposed an ATR tracking control framework specifically designed for rescue scenarios. The framework utilized the robot’s centroid path and abstracted terrain planes to design the configuration of the auxiliary flippers. A complex kinematic solver and a feedback controller were then employed to achieve whole-body control of the robot.

Except for traditional control methods, reinforcement-learning-based control for ATRs has been validated in scenarios where sufficient prior knowledge is available (Mitriakov et al., 2021; Pan et al., 2023). These approaches abandon traditional methods’ manually defined, often intricate evaluation criteria and instead rely on the explore-exploit mechanism of reinforcement learning. Through extensive offline simulations, exclusive control strategies were trained for ATRs to perform tasks such as stair climbing and many others. To reduce the learning cost for robots, Azayev and Zimmermann (2022) utilized human supervision to guide the motion planning and control of the robot. Human-in-the-loop methods enabled the robot to achieve adaptive whole-body control through learning-based methods. However, these techniques require substantial training and complex environmental abstractions, making them challenging to deploy in complex rescue scenarios.

Previous research has commonly employed terrain point clouds to guide ground robots in unstructured environments. However, they often lacked sufficient abstraction and management of terrain information, leading to longer path generation times and more limited applicability. The safety and adaptability of robot paths across various terrains have also not been adequately addressed. Additionally, existing ATR control methods remain relatively simple and do not fully consider the safety of motion control in complex terrain situations. Although learning-based control methods avoided sophisticated logical inference and theoretical derivation, they entailed significantly higher deployment costs and time consumption than traditional methods. These methods also struggled to cover some common corner cases in real-world scenarios and suffer from serious generalization issues. Thus, this paper proposes a terrain-informed navigation framework that effectively addresses the limitations of existing approaches and offers practical solutions for deploying autonomous ATRs in complex 3D disaster environments.

Discrete terrain information abstraction

In complex 3D rescue environments, ground robot path planning has to fully use the obtained terrain geometry information. However, for large-scale terrain information abstraction, general point cloud fitting methods often homogenize a lot of discontinuous terrain. For example, the floors of different disconnected rooms on the same level might have similar geometric information. Similarly, many traversable surfaces are at the same height but belong to different buildings. Moreover, many general terrain analysis methods fail to capture the topological relationships and distance information between traversable regions (Krüsi et al., 2017; Lee et al., 2022), which limits their utility for ground robot navigation in complex environments.

We adopted a grid-based terrain information abstraction method to address the abovementioned issue. The robot’s workspace X is divided into a 3D grid map M _grid with a certain resolution. Points from the point cloud map M _pc are assigned to corresponding grid cells, where terrain planes are extracted using the RANSAC method. If the point cloud within a grid cell is sufficiently dense and geometrically consistent, a terrain plane P _terr : ax + by + cz + d = 0 is extracted and stored. As shown in Figure 4, grid cells are classified as traversable if the angle δ between the P _terr ’s normal vector n _P and the vertical vector r _ver is below a pre-defined threshold ɛ _vP . Cells containing too many outliers or non-compliant planes are classified as non-traversable. Parallel processing accelerates this abstraction, and the resulting grid map provides spatial topological terrain information for robot path planning.OPEN IN VIEWER

Terrain-informed hierarchical global planner

Although ground robots typically operate near the seemingly low-dimensional ground surfaces in 3D rescue environments, their centroid poses in the global coordinate frame reside in SE(3) space. In addition to the extra DOFs introduced by the ATR’s flippers, global path generation for the rescue ATR is inherently a high-dimensional problem.

The combined effects of the high dimensionality of the robot’s state, the complexity of 3D environments, and the workspace constraints of the robot collectively make it extremely challenging to generate feasible paths for ATR using a direct, monolithic global planning approach. To address these issues, we first decouple the ATR’s flipper motion from the planning stage, instead relying on the robustness of subsequent controllers within the framework to achieve reliable flipper regulation and control. Meanwhile, we introduce a hierarchical global planner that employs a bi-level strategy to alleviate these inherent challenges effectively. Hierarchical strategies are commonly used to address large-scale planning problems. For example, TARE (Cao et al., 2021) generated a crude path based on the topological connectivity of the graph map and then refined this path within specific regions to meet the navigation requirements. However, such methods have to handle the connection between paths in different regions carefully; otherwise, gaps between path waypoints may occur, leading to fluctuations in path resolution that can negatively impact navigation.

In this paper, unlike traditional methods, the lower-level planner does not correct or optimize the crude path generated by the upper-level planner. Instead, the terrain information near the crude path was utilized to guide the lower-level planner’s operation space.

As illustrated in Figure 3, the upper-level planner quickly generates a crude path, _CΓ _base , using a graph-search method on the previously established terrain information grid map. At this level, the robot’s orientation is temporarily disregarded, reducing the search space dimensionality and enabling fast, approximate path generation while preserving path-finding completeness. This path, derived from grid topology and connectivity, is neither smooth nor capable of passing feasibility checks in complex environments. Additionally, the inaccessible areas in large-scale rescue scenarios negatively impact the robustness of the graph search, leading to redundant global paths. However, the region _CΓ _base traversed contains much feasible and valuable terrain information, which serves as a good prior for subsequent processing.

The lower-level planner utilizes a sampling-based planning (SBP) method to ensure path traversability while incorporating agnostic randomness. For ground robots engaged in rescue missions, most 3D space is non-traversable (e.g., regions far from the ground), with the robot’s pose constrained near the terrain. The algorithm utilizes terrain information P _terr from the grid map M _grid near the crude path _CΓ _base as priors for sampling, thereby limiting the sampled states to regions close to the ground while enhancing the completeness of global path generation with stochasticity. For a traversable terrain surface P t e r r : a x + b y + c z + d = 0 within the sampling region, the lower-level planner samples a random position [x_r, y_r] in the x and y directions and a random yaw angle Y r . Based on the plane equation, an augmented random SE(3) pose can be directly computed on the terrain plane as x_r = [x_r, y_r, z_r, q_r] ∈ SE(3).

The dependent random position in the z direction is calculated as:

z r = − a x r + b y r + d c .

(1)

The dependent random orientation q_r is derived based on the projection of the direction vector v (determined by the sampled yaw angle Y r ) onto the terrain plane P _terr , as:

q r = M a t 2 Q u a t ( [ a , b , c ] ) ,

(2)

The time consumption of sampling-based planning algorithms is mainly concentrated on the feasibility checks of the sampled states. In a 3D environment, a feasible state has to ensure that the ATR maintains stable contact with the ground, avoids collisions within the environment, and stays away from dangerous edges (e.g., cliffs). Our algorithm simplifies the robot model into a convex rectangular cuboid to reduce the time spent on collision detection. By combining this convex primitive with a kDTree that stores map information (Șucan et al., 2012), the efficiency of collision detection is significantly enhanced.

Additionally, the stability can be determined by the projection of the robot’s footprint on the ground. As shown in Figure 5, since the simplified robot model is a rectangular cuboid, when its rectangular chassis footprint is vertically projected onto the ground, a 3D convex polygon is formed. Affected by the terrain, the polygon’s vertices set can be expressed as V = { v 1 , … , v n } , where v _n ≔ [x, y, z]. If the number of polygon vertices n is less than 4 (as shown in Figure 5(b)), the robot is at a dangerous edge that is not traversable. When the number of vertices equals 4, the 3D quadrilateral is then triangulated. If the maximum angle δ_n between the normal vectors of any two divided triangles exceeds a certain threshold ɛ _vs (as shown in Figures 5(c) and (d)), the contact between the robot and the ground at this location is considered unstable.OPEN IN VIEWER

Furthermore, a divide-and-conquer approach can be employed to parallelize and accelerate the planning process in large-scale environments, as illustrated in Figure 6. For long-range paths, the causal relationships between distant waypoints are typically weak. Therefore, the grids within the sampling region are grouped according to the graph-searched crude path. Then, perform sampling for feasible segment endpoints x _seg in the overlap areas between adjacent grouped grids. These endpoints serve as the start and goal points for the segmented paths, each of which is planned in parallel. Once all segmented paths are generated, they are stitched together to form the complete global path, _GΓ _base .OPEN IN VIEWER

Thus, based on a terrain information grid map, this framework introduces a novel and fast hierarchical global path generation method, further enhancing planning efficiency by adopting a divide-and-conquer strategy, ensuring that the robot can quickly generate a feasible path in complex 3D environments.

Manifold-optimization-based local planner

For ATR navigation in large-scale environments, global paths often lack the precision required to accommodate the robot’s motion characteristics and adaptability to complex, fluctuating terrains. To ensure smooth, feasible, and terrain-adaptive local paths, this paper adopts a manifold-optimization-based local planner that exploits the terrain manifold to efficiently refine coarse global paths under various constraints.

Mathematical representation of SE(3)-path optimization

For optimization-based 3D ground robot path planning algorithms, the basic form of the optimization problem can be expressed as:

x * = argmin x ∈ S E ( 3 ) F ( x ) , = argmin x ∈ S E ( 3 ) ∑ i = 0 : N − 1 , s ∈ S e s ( x i , E s ) ⊤ Ω s e s ( x i , E s ) ,

(3)

where x = ( x 0 ⊤ , … , x N − 1 ⊤ ) ⊤ ∈ R 6 N represents the vector of all SE(3) poses in the path Γ _base ; each waypoint is characterized by x i ≔ [ x , y , z , R , P , Y ] ; the parameter i denotes the index of the waypoint; e _s denotes the cost function for various optimization objectives; E s represents external elements related to the optimization objectives; the type of cost function is denoted by s ∈ S , and Ω _s indicates the weight of the corresponding cost function in the optimization process. For the application scenario of ATR in a rescue mission, the costs in SE(3) path optimization can be categorized into five types.

Kinematic cost

The robot’s mobility is constrained by its inherent kinematic limitations. The optimization has to incorporate a cost function that accounts for kinematics to produce a smooth optimized path. The kinematic model of a tracked chassis can be simplified to a differential drive model with non-holonomic motion constraints. The kinematic cost function, denoted as e _k (x _i ), can be expressed in the following form (Rösmann et al., 2017):

e k ( x i ) = c o s ( x i Y ) + c o s ( x i + 1 Y ) s i n ( x i Y ) + s i n ( x i + 1 Y ) 0 × d i , i + 1 2 ,

(4)

where x i Y represents the yaw angle at waypoint i, and d i , i + 1 = [ x i + 1 x − x i x , x i + 1 y − x i y , 0 ] ⊤ which represents the augmented XOY position difference between adjacent waypoints.

Obstacle avoidance cost

The robot has to maintain a safe clearance from obstacles X obs to avoid collisions. For real-time and reliable path planning, onboard sensors continuously gather data to identify traversable areas and obstacles. Obstacles are represented as convex hulls, simplifying distance calculations while expanding the safety margin between the robot and the obstacles. Then, the cost function is defined as the reciprocal of the distance, formulated as follows:

e o ( x i , X obs ) = 1 d ( x i , X obs ) = 1 x i − X obs 2 .

(5)

Ground adherence cost

In a 3D environment, the waypoints x _i of a ground robot need to remain closely adhered to the ground S terr . The cost can be represented as e t ( x i , S terr ) . Here, we use the 3D planes P _terr from the previously generated terrain information grid map M _grid as ground information in the cost function. As equation (6) shows, the cost function is defined based on the distance between the chassis’s footprint and the ground, measured as the sum of distances from the four corner points x_i,n of the footprint to the ground, encouraging the path to adhere to the terrain.

e t ( x i , S terr ) = d ( x i , S terr ) = ∑ n d ( x i , n , P terr ) = ∑ n | a x i , n x + b x i , n y + c x i , n z + d | a 2 + b 2 + c 2 ,

(6)

where a, b, c, d are the plane equation parameters of P _terr .

Pitch stability cost

In rugged terrain, maneuvering requires constraining the robot’s pitch angle to ensure safety and stability. Moving along a straight path on excessively steep slopes may result in insufficient traction or cause the robot to slip. The robot should follow a winding path, prioritizing safety over path length to mitigate these risks. Thus, the method constrains the robot’s pitch angle within a safe range of [ − ε P , ε P ] . The nonlinear cost function is formulated as follows:

e P ( x i ) = 0 , − ε P ≤ x i P ≤ ε P x i P − ε P , x i P > ε P − ε P − x i P , x i P < − ε P .

(7)

Anisotropic traversability cost

In most cases, a robot’s maneuverability on normal terrain is consistent and isotropic. However, stair-like terrain, commonly found in buildings, introduces anisotropy in traversability. Empirical evidence indicates that tracked robots exhibit optimal performance when traversing perpendicularly to the edges of stairs. Because the chassis’s sinuous motion would make it difficult for the tracks to overcome the structural pressure at stair edges, and the chassis may be lifted by the edges, resulting in a temporary loss of maneuverability. Thus, we define an anisotropic traversability cost e a ( x i , S terr ) based on terrain characteristics, to ensure the maximum maneuverability of the ATR on stairs while maintaining safety.

The staircases’ pose and the steps’ geometric attributes can be precisely identified using LiDAR point clouds or RGB-D images (Perez-Yus et al., 2017; Sriganesh et al., 2023; Westfechtel et al., 2018). In this framework, terrain information in the grid map is updated and semantically annotated in real-time. If the traversability of the terrain P _terr in the grid is anisotropic, the direction a Y with the highest traversability (e.g., the direction perpendicular to the stair treads, as shown in Figure 7) is selected for anisotropic traversability labeling of the plane P _terr in that grid cell under the global frame F _G . During path optimization, anisotropic traversability information is queried based on the grid containing the waypoint x _i . The cost function is expressed as:

e a ( x i , S terr ) = | s i n ( x i Y − a Y ) | ,

(8)

to minimize the angular difference between the waypoint’s yaw angle x i Y and the anisotropic traversability direction a Y while maximizing the robot’s traversability at each waypoint, ensuring that the pose of the waypoints satisfies the conditions for optimal maneuverability.OPEN IN VIEWER

Figure 7.

The anisotropic traversability on the staircases.

Mathematical representation of path optimization on the terrain manifold

Multi-objective SE(3)-path optimization may fail due to disturbances in certain optimization directions. For instance, the optimized path might deviate from the ground to avoid obstacles, failing to meet ground adherence. This issue arises because the ground adherence cost is a soft constraint in the optimization problem (3), and significantly increasing its cost weight does not guarantee path feasibility and may lead to a weight imbalance in the optimization problem. To address this, we propose a path optimization method based on terrain manifolds to integrate the ground adherence cost as an implicit hard constraint within the manifold.

Considering the ground adherence, the waypoints can be bijectively transformed between the terrain manifold space and SE(3) space within a small range. The bijective transformation allows the path optimization problem in SE(3) to be mapped to a path optimization problem on the terrain manifold.

On manifold M , for a cost function F : M → R , the basic form of the manifold optimization problem is given as follows:

x * = argmin x ∈ M F ( x ) .

(9)

The optimization process on a manifold has to consider the gradient of the cost function on that manifold. If F is smooth on the manifold M , the Riemannian gradient grad _x F at point x is the projection of the Euclidean gradient ∇F(x) onto the tangent space T x M , denoted as grad _x F = Proj _x ∇F(x).

Optimizing a waypoint x on a manifold requires updating the point in the descent direction of the cost function while ensuring it stays on the manifold. Thus, after obtaining the descent direction v, the algorithm uses a retraction map R _x (v) to update the waypoint x on the manifold. As illustrated in Figure 8, the retraction map R x ( v ) : T x M → M not only ensures continuity at point x but also satisfies the condition R _x (0) = x. Analogous to the gradient descent method in Euclidean space, the update formula for gradient descent on a manifold is given by:

x k + 1 = R x k ( − t k grad x k F ) ,

(10)

where t _k represents the step size, and R x k denotes the retraction map at the point x _k .OPEN IN VIEWER

Figure 8.

The retraction map R _x (v) on point x on manifold M .

Optimization on manifold extensively utilizes the tangent space to perform iterations (Boumal, 2023). The manifold used here is derived from the terrain planes P _terr stored in the grid map M _grid . This approach ensures that tangent spaces are identical across all points on a manifold, thereby facilitating the calculation of the Riemannian gradients grad _x F and allowing for direct and efficient cost computation on the manifold. It also simplifies the mapping and inverse mapping between spaces, thereby improving optimization efficiency.

For the local planning, the information of the waypoint z _i on the terrain manifold is defined as [ x , y , Y ] due to the mapping relationship z _i = Proj(x _i , P _terr ), which reduces the dimensionality of the original path. Consequently, the path on the terrain manifold is defined as z = ( z 0 ⊤ , … , z N − 1 ⊤ ) ⊤ ∈ R 3 N , where N represents the number of waypoints.

For path optimization on the terrain manifold, the problem (3) is transformed into:

z * = argmin z ∈ M F ( z ) , = argmin z ∈ M ∑ i = 0 : N − 1 , s ∈ S e s ( z i , E s ) ⊤ Ω s e s ( z i , E s ) ,

(11)

where the kinematic, obstacle avoidance, and anisotropic traversability costs remain unchanged and still act as soft constraints. In contrast, compared to problem (3), the ground adherence cost becomes a hard constraint due to the introduction of the manifold in problem (11). This hard constraint ensures that the robot’s pose is strictly confined to the ground, preventing poor optimization directions. Considering the mathematical form of P _terr and the inverse mapping relationship of the manifold, the pitch angle of a waypoint on the terrain manifold can be expressed as z i P ¯ = a t a n ( a + b t a n ( z i Y ) / c 1 + t a n 2 ( z i Y ) ) . The basic form of the pitch stability cost also remains unchanged.

The manifold optimization (ManiOpt) resolves the issue of the path deviation from the traversable ground while significantly receding the dimension of the optimization variables and further reduces the cost type S in the optimization problem. This approach avoids the weight imbalance problem while improving the optimization process’s efficiency.

The efficiency of ManiOpt helps prevent overshooting in path-tracking control that may occur due to delayed reference updates. Moreover, the optimized paths generated by ManiOpt reduce the risk of environmental misinterpretation during execution. For instance, ManiOpt mitigates the likelihood of holistic control failures induced by obstacle points in terrain analysis. In addition, ManiOpt’s capability to incorporate terrain-specific optimization objectives in particular scenarios further decreases the complexity of ATR control. Finally, as the optimized paths remain close to the terrain surface, these paths provide high-quality information for terrain-aware motion control, thereby minimizing discrepancies between the reference terrain point cloud and the actual terrain.

On-manifold model predictive controller for ATR

The ATR should be able to adjust the flipper states adaptively, ensuring that the body adheres to the terrain and lowers its centroid to achieve stable maneuvering and better stress distribution in complex environments, thereby reducing the risk of malfunctions such as overturning or becoming trapped (Chen et al., 2023; Okada et al., 2009). Existing research often treats the flippers as a special mechanism of the chassis. After determining the centroid-joint space path for the next period, the robot utilizes a decoupled feedback control method to manage the tracks’ and flippers’ motion. However, suppose the flipper actuators do not perform as expected. In that case, the planned joint space path may not be accurately followed, potentially leading to terrain-configuration mismatches that could place the robot in an unsafe operational condition.

The primary concern for ATR navigation in disaster rescue missions is whether the robot can reach its assigned target locations. Inspired by some well-established navigation frameworks (Sleiman et al., 2023), the method proposed in this paper emphasizes mission-oriented whole-body control. To address the challenge of coordinated and stable ATR motion on rugged terrain, we propose an on-manifold model predictive controller (M²PC) that integrates terrain information with the robot’s whole-body kinematics. This controller enables the ATR’s chassis to follow the optimized local centroid path holistically and stably, without requiring an explicitly organized flipper joint space path.

Therefore, M²PC not only enhances whole-body control safety but also significantly reduces the dimensional burden on the planning phases within the framework. On the one hand, M²PC eliminates the need for the global planner to sample flipper’s joint states or perform complex feasibility checks during initial path generation. On the other hand, M²PC allows the local planner to optimize paths without considering optimization terms related to the flippers, thus reducing computational costs.

As illustrated in Figure 9, the core conception of M²PC involves mathematically abstracting the terrain into a continuously differentiable manifold and then leveraging the manifold’s additive properties to couple the terrain manifold with the robot’s motion manifold. The coupled manifold acts like a system model that guides the model predictive control of ATRs navigating rugged terrain. Within this manifold, the proposed controller adaptively coordinates the robot’s whole-body motion under terrain and robot constraints, significantly enhancing safe maneuverability in extreme environments.OPEN IN VIEWER

The manifold M is an n-dimensional topological space where each point’s neighborhood is homeomorphic to an open subset of Euclidean space R n . And local coordinate charts made the mapping ϕ x : M → R n and the inverse mapping ϕ x − 1 : R n → M between M and R n possible. Based on the premise mentioned above, the robot system model on the manifold can be expressed as follows:

x k + 1 = x k ⊕ Δ t f x k , u k ,

(12)

For the optimization on the manifold, the operation ⊟ : M × M → R n is introduced to compute the difference on the manifold M in its homeomorphic tangent space R n . Thus, the optimization problem corresponding to the on-manifold model predictive control can be formulated as follows:

min u k ∑ k = 0 N − 1 x k ⊟ x k d Q k 2 + u k − u k d R k 2 + x N ⊟ x N d Q N 2 , s.t. x k + 1 = x k ⊕ Δ t f x k , u k , x 0 = x int , u k ∈ U , x k ∈ X , k = 0 , … , N − 1 ,

(13)

The optimization problem (13) is constrained by both variable bounds and the system model defined on the manifold, where the current state depends on the previous state and control inputs’ projection on the homeomorphic tangent space of the manifold (Kalabić et al., 2017; Lu et al., 2023). As the robot’s configuration space definition shapes the system model (12), we propose a system model incorporating terrain manifold curvature and robot mechanism to keep the ATR adhered to the ground.

The ground point clouds in the local submap are utilized for terrain manifold abstraction, and the surfaces are fitted near each path waypoint, with a quadratic form of Z terr = { ( x , y , z ) ∈ R 3 ∣ z = F ( x , y ) = [ x , y ] A [ x , y ] ⊤ + B ⊤ [ x , y ] ⊤ + C } . According to the invariance theorem of surfaces (Do Carmo, 2016) (the shape characteristics of a surface are the same in different coordinate systems), the frame of the surface is aligned with the global frame F _G . Given that the quadratic formed surface information, the first and second partial derivatives of the surface at the path waypoints [ x , y , Y ] can be obtained as F _x , F _y , F _xy , F _xx , F _yy . Consequently, the normal curvature K of the surface can be expressed as the ratio of the second fundamental form to the first fundamental form as follows:

K = I I T I T ,

(14)

I = E 2 F G = F x 2 + 1 2 F x F y F y 2 + 1 , I I = L 2 M N = F x x 2 F x y F y y / 1 + F x 2 + F y 2 , T = c o s 2 ( Y ) c o s ( Y ) s i n ( Y ) s i n 2 ( Y ) ⊤ .

(15)

We emphasize that the safe path tracking of the ATR’s centroid is the primary focus for maneuvering over rugged terrain, with the flipper state adjusted through adaptive coordination control to regulate the ATR’s configuration. In the proposed method, the desired flipper state depends not only on the characteristics of the terrain manifold but also on the ATR’s motion. Conversely, the alignment between the ATR’s configuration and the terrain manifold should influence the ATR’s motion to prevent reckless maneuvers in unstable configurations.

As illustrated in Figure 10, the geometric relationship shows that the arc fitted to the ATR’s configuration when maneuvering over rugged terrain should continuously align with the ground’s normal curvature to ensure the ATR adapts effectively to terrain variations. To ensure the alignment, the joint angles of the front and rear flippers in the chassis frame should be ϕ _i = 1/2α ≈ 1/2KS. The robot’s total span length S is approximately equal to the arc enveloped by the ATR’s configuration, and K is the normal curvature of the manifold at the waypoint. According to equations (14) and (15) and the invariance theorem of surfaces, the normal curvature of the surface near the ATR depends solely on its centroid’s pose. The state variables for the flippers are defined as the differences between the actual and desired flipper joint angles in the chassis frame ϵ _i , i ∈ {f, r}, establishing the relationship between the ATR’s centroid pose and the desired configuration for coordinated control. The differences are intended to be maintained at zero in the form of:

ϵ i = ϕ i − 1 2 K S ≡ 0 , i ∈ { f , r } .

(16)

OPEN IN VIEWER

Figure 10.

The geometrical relationship between the ATR configuration and the terrain manifold normal curvature.

Since the robot is constrained to the terrain surface, not only does its centroid position adhere to the terrain manifold, but the rotational axis of its body frame also aligns with the normal vector at that position of the terrain manifold. Thus, the robot’s chassis position and orientation on the terrain manifold can be represented by p ∈ Z terr and R ∈ SO(2).

In summary, the state variables and control variables of the M²PC are set as follows:

M = Z terr × S O ( 2 ) × R 1 × R 1 , dim ( M ) = 5 , x = p R ϵ f ϵ r ∈ M , u = v x ω z ψ f ψ r ∈ R 4 ,

(17)

The robot’s chassis motion model can be represented by (Lu et al., 2023):

p ˙ = α R e 1 v x , R ˙ = β ω z ,

(18)

α ( p , R ) = 1 1 + h ⊤ R e 1 2 , β ( p ) = 1 1 + h ⊤ h , h = F x F y ⊤ , e 1 = 1 0 ⊤ , a n d e 2 = 0 1 ⊤ .

(19)

And the motion relationship between ϵ i ˙ and the control inputs u can be represented as follow:

ϵ i ˙ = ψ i − ( c v K p ′ α R e 1 v x + c ω K R ′ β ω z ) S , i ∈ { f , r } ,

(20)

The state transition function for the M²PC problem (13) is formulated as follows:

f ( x , u ) = α R e 1 v x β ω z ψ f − c v K p ′ S α R e 1 v x − c ω K R ′ S β ω z ψ r − c v K p ′ S α R e 1 v x − c ω K R ′ S β ω z ∈ R 5 .

(21)

The QP problem (13) is addressed using the OSQP (Stellato et al., 2020) open-source solver. In this problem, the linear constraint matrix is derived from the differential composition of the state transition function (21). Specifically, the expressions for the partial derivatives ∂f(x ⊞ δx, u)/∂δx and ∂f(x, u ⊞ δu)/∂δu are calculated within the homeomorphic tangent spaces of the manifold M , as detailed in equations (22) and (23):

∂ f ( x ⊞ δ x , u ) ∂ δ x = R e 1 v x ∂ α ∂ δ p R ∂ α ∂ δ R e 1 + α e 2 v x 0 2 × 2 ∂ β ∂ δ p ω z 0 0 1 × 2 − ( c v K p ′ S α R e 1 v x + c ω K R ′ S β ω z ) | p ′ − ( c v K p ′ S α R e 1 v x + c ω K R ′ S β ω z ) | R ′ 0 1 × 2 − ( c v K p ′ S α R e 1 v x + c ω K R ′ S β ω z ) | p ′ − ( c v K p ′ S α R e 1 v x + c ω K R ′ S β ω z ) | R ′ 0 1 × 2 ∈ R 5 × 5 ,

(22)

∂ f ( x , u ⊞ δ u ) ∂ δ u = α R e 1 0 2 × 1 0 0 0 β 0 0 − c v K p ′ S α R e 1 − c ω K R ′ S β 1 0 − c v K p ′ S α R e 1 − c ω K R ′ S β 0 1 ∈ R 5 × 4 .

(23)

Preparation

We validated and quantified the framework’s effectiveness by deploying the algorithms on our self-developed articulated tracked robot, Slugcat, as shown in Figure 11. Slugcat features coaxial front and rear articulated flippers, with flipper lengths of approximately 350 mm and a chassis length of about 600 mm. The total weight of the robot is around 55.7 kg. The articulated flipper joints are driven by high-reduction motors, providing strong self-supporting capability and resistance to significant external disturbances. Three MID-360 LiDARs are mounted on the robot for terrain perception and localization. The onboard computer has an Intel-i7-11700K CPU (8 cores, 3.60 GHz) and an RTX-4060 GPU with 8 GB of VRAM. We also built various simulation environments based on the Gazebo physics engine, including structured building environments, unstructured outdoor scenes, and ideal experimental setups. The simulated articulated tracked robot matches the real robot regarding dimensions, kinematics, and dynamic characteristics (with a realistic drive system adapted from the work of Okada et al. (2020)).OPEN IN VIEWER

Although the proposed framework is compatible with online point cloud maps constructed by the real-time SLAM method, for rigorous comparative evaluations, we employed prior point cloud maps in all simulations and in several real-world experiments. In addition, we validated the framework’s performance using online point cloud maps generated by the SLAM module in an unknown environment. In the simulations, the robot’s poses were directly provided by Gazebo. In all real-world experiments, the robot’s odometry was provided by a SLAM module equipped with a re-localization function. Specifically, for the discrete terrain information abstraction module deployed throughout the work, the resolution of the 3D terrain-info grid map was uniformly set to 0.5 m (considering the robot’s physical size). The threshold for planar traversability determination, ɛ _vP , was set to 45°. Related experimental videos are available in the supplemental materials.

Validation of the hierarchical global planner

As shown in Figures 12(a)–(c), we conducted efficiency evaluations in large-scale 3D simulation environments to validate the effectiveness of the hierarchical global planner, with consistent start and goal positions for every scenario, respectively. In this work, the maximum angular deviation ɛ _vs between the normal vectors of triangulated planes within a feasible ATR footprint was set to 15° (see Figure 5).OPEN IN VIEWER

The six-story building depicted in Figure 12(a) had a floor height of 2.5 m, a floor area of 900 m² per level, and a total volume of 1.35 × 10⁴ m³. There were continuous staircases at the corners of the building. The start point was located at one corner outside the building, with global coordinates (−16.0 m, 14.0 m, 0.1 m), while the goal point was at the center of the rooftop, with global coordinates (0.0 m, 0.0 m, 15.1 m). The shortest feasible path length was approximately 60 m. Additionally, the possible path solution was restricted by setting only two staircases for each floor in the opposite corners of the building, significantly increasing the length of the traversable path (with the shortest feasible path length of approximately 250 m). The restricted six-story building scenario was also used to evaluate the adaptability of the proposed framework to various sampling-based planning algorithms in long-range planning scenarios.

The simulated wild scenario included ponds, mountainous areas, a narrow bridge, and mine caves. The scene depicted in Figure 12(b) covered an area of approximately 1.0 × 10⁴ m². The start point was located behind the solar panel, with global coordinates (−19.0 m, 58.0 m, 3.0 m), and the goal point was located in the cave at global coordinates (30.0 m, − 10.0 m, 4.0 m). The shortest feasible path length was approximately 120 m.

The scenario in Figure 12(c) was adapted from DARPA’s publicly available simulation scene, DARPA_cave_02. This scenario consisted of a massive underground cave, with the size of approximately 300 m × 300 m × 70 m, which occupied a volume of 6.3 × 10⁶ m³. The start point was located in the open area outside the cave, with global coordinates (0.0 m, 0.0 m, 0.0 m), and the goal point was located in the mine at global coordinates (200.2 m, − 36.5 m, − 19.8 m), with the shortest feasible path length being approximately 450 m. The large-scale and complex connectivity of the simulation environments presented significant challenges for the planning task. During the experiments, the upper-level planner employed the A* algorithm with a heuristic function based on Euclidean distance. The lower-level planner employed the RRT algorithm.

We compared our method with some existing works (Colas et al., 2013; Jian et al., 2022; Krüsi et al., 2017) to verify the homotopy, consistency, and efficiency of the path generation process. Each method was tested 25 times under identical conditions, and various metrics were calculated from the 20 best results, as shown in Table 1. The generated typical paths in the wild scenario are illustrated in Figure 13.

OPEN IN VIEWER

Table 1.

Quantifications of different global path generation algorithms in the simulations: L _p denotes the average generated path length, L _p .σ² denotes the variance of the generated path length, and T _p denotes the average planning times (In the DARPA_cave_02 scenario, our method reduced 78.81% of the time cost compared to the SOTA methods).

	Six-story building			Restricted six-story building			Wild			DARPA_cave_02
	L p (m)	L p .σ2(m2)	T p (s)	L p (m)	L p .σ2(m2)	T p (s)	L p (m)	L p .σ2(m2)	T p (s)	L p (m)	L p .σ2(m2)	T p (s)
CT-DC	63.34	4.02	0.23	232.24	92.40	0.25	135.81	19.45	0.04	415.68	44.91	4.04
Colas’	50.93	0.0	1.80	313.27	0.0	9.75	111.34	0 .0	0.26	464.42	0.0	19.07
Krüsi’s	97.68	753.28	3.83	294.99	113.42	23.59	157.29	212.08	0.15	626.38	358.23	22.66
PUTN	–	–	–	–	–	–	160.58	159.41	0.11	–	–	–

OPEN IN VIEWER

Figure 13.

The typical path generation performance in the wild scenario with the proposed and the existing SOTA methods.

Colas et al. (2013) constructed a topological map for graph-search planning by identifying feasible points and connectivity in raw point clouds. Thus, its performance is highly sensitive to the scale of the environment, and the resulting paths are often rugged due to limited terrain details. Nevertheless, the deterministic nature of graph-search methods results in lower variance in path lengths, providing more consistent planning outcomes. Krüsi et al. (2017)’s work achieved smooth paths through post-processing and optimization; however, the initial solution had a significant impact on path length and trends, leading to greater variance and extended planning times, which would negatively affect execution efficiency. PUTN (Jian et al., 2022) increased the sampling step size in the SBP and used a 2D space sampler to enhance planning efficiency. However, due to the non-uniqueness of restoring 2D poses into 3D space, it cannot effectively handle path planning in multi-floor building structures. Our method (CT-DC) relies on well-organized terrain information, ensuring consistency and completeness from the upper-level planner as well as efficiency and randomness from the lower-level planner. The generated paths exhibit strong homotopy across multiple tests while maintaining superior performance in large-scale and complex environments.

As illustrated in Figure 14, when the lower-level planner sampled all terrain information (AT) within the map, the planning process suffered from inefficiency (planning time up to 29.11 s) and path redundancy. In contrast, the proposed method, which sampled only the terrain information near the crude plan (CT), significantly reduced average planning time to 0.51 s by limiting the randomness of SBP. Moreover, the divide-and-conquer strategy (DC) further accelerated path generation without altering the sampling range, reducing the average planning time to 0.27 s (see Table 2).OPEN IN VIEWER

OPEN IN VIEWER

Table 2.

The average planning time (s) for different deployed sampling-based planning (SBP) methods in the six-story building scenarios (RRT-C refers to RRT-connect).

	6F building	Restricted 6F building
	RRT	PRM	RRT	RRT-C	RRT*
AT	29.11	44.19	96.99	101.14	–
CT	0.51	7.13	7.70	8.86	–
CT-DC	0.27	0.43	0.25	0.21	11.95

The choice of the sampling-based planner plays a crucial role in the final results. For instance, the RRT-connect method can significantly speed up path generation, while the RRT* method reduces path redundancy and improves overall path quality (see Figure 15). Although the time required for path generation varies depending on the sampling-based algorithm used, as shown in Table 2, the proposed CT-DC strategy notably accelerates path generation.OPEN IN VIEWER

A series of ablation studies was conducted across different planning strategies in four real-world environments to evaluate their impacts on the quality of generated paths. We performed 3D reconstructions of an exterior landscape of a library, a fire-fighting rescue stairway, and a litchi forest using a handheld MID-360 LiDAR scanner (see Figures 16(a)–(c)). Additionally, we reconstructed a typical campus scenario at SYSU (see Figure 16(d)) based on the airborne LiDAR data from GrAco’s public dataset (Zhu et al., 2023), minimizing potential traversable bias from ground-based mapping. These large-scale environments feature extensive traversable areas and complex topological connectivity. Across the four ablation experiment scenarios, the start and goal points can be connected via multiple feasible non-homotopic paths, and are separated by long navigation distances.OPEN IN VIEWER

In addition to the challenges posed by large-scale environments and complex topological connectivity, each scenario was designed with a specific test focus. The library exterior evaluated planning ability in confined passageways. The rescue stairway scenario assessed planning capability in multi-floor environments with staircases. The litchi forest tested planning robustness in cluttered, unstructured terrain. Finally, the campus scenario examined planning performance in sparse near-terrain point clouds with minimal prior traversability information.

The path quality of different strategies was evaluated across all scenarios. To quantify discrete path smoothness, a specific metric (Smot.) was adapted from the work of Dolgov and Thrun (2009) as:

S m o t . = ∑ i = 1 N − 2 Δ x i + 1 p − Δ x i p 2 ∑ i = 1 N − 1 Δ x i p 2 ,

(24)

Each strategy was tested 25 times under identical conditions, and various metrics were calculated from the 20 best results. As the data shown in Table 3 and the paths shown in Figure 16, using all terrain (AT) information for planning introduces higher randomness and leads to longer global paths. In contrast, planning with terrain information along the crude path (CT) significantly reduces path length and improves consistency. Compared to CT, the divide-and-conquer strategy (CT-DC) yields slightly longer and rougher paths with more variation. This is mainly due to randomness introduced by path segmentation (e.g., selection of the sub-paths’ endpoints, x _seg ), which weakens the coherence between path segments. Despite this, CT-DC offers a major improvement in planning efficiency. While CT produces higher-quality paths, it is less efficient in large-scale environments (e.g., in the campus scenario, the average planning time of CT-DC is only about 3.9% of that of CT). For real-time ATR navigation in complex terrains, CT-DC offers a more practical solution, achieving significantly faster planning with only a minor loss in path quality. Within our framework, such suboptimalities can be effectively corrected by the downstream local planner and controller.

OPEN IN VIEWER

Table 3.

Quantifications of the global path generation ablation experiments in real-world scenarios: L _p denotes the average generated path length, Smot. denotes the average generated path smoothness (a smaller number indicates a smoother path), and T _p denotes the average planning time.

	Uni-town library			Staircase			Forest			Campus
	L p (m)	Smot.	T p (s)	L p (m)	Smot.	T p (s)	L p (m)	Smot.	T p (s)	L p (m)	Smot.	T p (s)
AT	223.34	0.09	21.33	89.62	0.15	9.73	226.81	0.16	15.17	332.68	0.17	37.22
CT	192.25	0.10	4.46	70.27	0.13	4.71	180.14	0.11	6.08	243.44	0.12	7.97
CT-DC	191.41	0.10	0.13	73.83	0.21	0.12	184.55	0.25	0.11	251.91	0.31	0.31

Validation of the manifold-optimization-based local planner

The path optimization problems introduced in Section Manifold-optimization-based local planner involved complex nonlinear cost functions. Thus, we employed a graph-based optimization method (Kümmerle et al., 2011) to address problem (3) and problem (11). Apart from the inherent differences in the ground adherence cost, the weights in these two optimization problems were consistent (the weight [ Ω k , Ω o , Ω t , Ω P , Ω a ] is set as [100, 100, 100, 100, 100]). The robot’s pitch angle safe threshold ε P is set to 30°. For clarity, the local planners guided by problems (3) and (11) are referred to as SE(3)Opt and ManiOpt, respectively. We also reproduced an open-source navigation framework, 3D2M (proposed by Wang et al. (2023)), which demonstrates competitive performance in multi-floor 3D environments. The 3D2M framework includes a dedicated terrain analysis module capable of directly processing dense point cloud data, a graph-search-based global planner (denoted as 3D2M-G), an optimization-based local planner (denoted as 3D2M-L, which imposes various costs as soft constraints on polynomial-continuous trajectories), and a tracking controller for wheeled robots. In this subsection, to control the experimental variables, the comparison primarily focuses on evaluating the local planner of 3D2M (i.e., 3D2M-L), which operates based on its intrinsic terrain analysis module.

We systematically evaluate three optimization-based local planners, SE(3)Opt, ManiOpt, and 3D2M-L, across four simulated scenarios constructed in Gazebo. For consistency, all planners were provided with the same dense point cloud maps of the environment (resolution: 0.10 m), prior terrain texture attributes, and identical global initial paths. A comparative summary of their performance is presented in Table 4 and visualized in Figure 17.

OPEN IN VIEWER

Table 4.

The capability comparison between different optimization-based local planners: The marker √ indicates that a safe path was successfully optimized, while the marker ◦ signifies that a perilous path was produced. The marker × denotes that the local planner failed to provide a feasible path.

	Steep slope	Wide stairs	Obst. Slope	Spiral stairs
ManiOpt	√	√	√	√
SE(3)Opt	◦	◦	×	◦
3D2M-L	◦	◦	×	×

OPEN IN VIEWER

Figure 17.

The typical path or trajectory optimization results in the simulation scenarios. The red trajectories represent the pre-assigned initial paths, while the optimized trajectories of 3D2M are indicated in a gradient rainbow color. The optimized paths of SE(3)Opt and ManiOpt are indicated in blue. The obstacles in the scenarios are colored purple.

In the first two simulated scenarios shown in Figures 17(a) and (b), 3D2M-L’s reliance on highly accurate terrain analysis became a significant bottleneck during optimization, as steep slopes and sharp stair edges were often misclassified as non-traversable regions. Thus, the parameters of 3D2M-L required careful fine-tuning to ensure a successful process. However, when the terrain analysis thresholds were relaxed, 3D2M-L lacked a proper cost formulation to reasonably handle steep inclines and anisotropic traversability, resulting in little practical improvement in optimized trajectories over the initial paths.

In contrast, the 3D terrain-info grid map effectively addressed challenges in complex terrain representation, mitigating the roughness of the point cloud and ensuring better optimization performance. The results of SE(3)Opt in the first two simulations were less pronounced, primarily due to weight imbalances, which hindered the planner’s ability to optimize other objectives while maintaining ground adherence. As depicted in the lower subfigure of Figure 17(a), by introducing slight perturbations to the initial path on a steep slope (45°), the path optimized by ManiOpt is shown to be meandering and safe. In the scenario illustrated in Figure 17(b), the initial path diagonally crossed a wide staircase with anisotropic traversability. To avoid dangerous maneuvers on the stairs, ManiOpt adaptively adjusted the path along the direction of the stairs.

As illustrated in Figure 17(c), the presence of initial global paths that occasionally penetrate obstacles severely disrupted the optimization process. In this scenario, both SE(3)Opt and 3D2M-L failed to produce feasible paths. Although 3D2M demonstrated effective obstacle detection, its soft-constraint design and high sensitivity to initial paths led to solutions that traversed obstacles. SE(3)Opt avoided obstacles to some extent, but due to its soft-constraint formulation of ground adherence, the optimized paths deviated significantly from the actual traversable terrain. In contrast, ManiOpt, with its terrain-manifold-based hard constraints, successfully generated feasible paths that closely adhered to the terrain while avoiding obstacles.

The scene in Figure 17(d) illustrates a typical spiral staircase scenario. The inner region of this structure features steeper slopes and pronounced changes in anisotropic traversability. In this test, the trajectory optimized by 3D2M-L deviated substantially from the spiral staircase, resulting in an entirely infeasible solution. The path optimized by SE(3)Opt is more zigzagged, closer to the central axis of the stairway, and involves perilous pitch angles. In contrast, the path optimized by ManiOpt is smoother, more gradual, and better adheres to the ground. The benefits of ManiOpt also include fewer optimization variables and cost types, leading to shorter computational times compared to SE(3)Opt. In the planning task depicted in Figure 17(d), the optimized path spanned approximately 10 m with around 30 waypoints. The average optimization time of ManiOpt was about 31.83 ms, while SE(3)Opt averaged 41.22 ms, saving 22.76% of the time cost.

Validation of M²PC for ATR

This subsection evaluates the proposed M²PC for path tracking of an ATR in both a Gazebo-simulated scenario and a real-world staircase setup with a slope of 33.7° (see Figure 19). For comparison, we reproduced the control algorithm presented by Gianni et al. (2016), which utilizes feedback-based control to track the robot’s centroid and flippers, with controller gains set to K _p = 10.0, K _i = 0.005, and K _d = 0.1. Moreover, we implemented an integrated planning-control method based on configuration geometry and terrain analysis from Chen et al. (2023)’s work for further comparison (denoted as GEO).

In the optimization problem of equation (13) in M²PC for ATR, the state penalty matrices Q _k and Q _N were set to diag([20, 20, 20, 100, 100]), indicated a greater emphasis on the stability and safety of the robot’s configuration compared to path tracking of the robot’s centroid. The control penalty matrix R _k was set to diag([0.5, 0.5, 0.5, 0.5]) in the experiments.

According to the chassis and flipper size of Slugcat, the span S of the robot was set to 1.3 m. Considering the robot’s physical dimensions, the sensing range of its onboard sensors, and the accuracy of surface fitting, terrain manifolds in M²PC are locally extracted from point clouds around each waypoint. Specifically, only the down-sampled ground points within a 1.3 m radius around each waypoint were used to efficiently extract spatial uniformity and reliable local terrain manifolds for M²PC.

An excessively long horizon for M²PC is neither necessary nor beneficial. On the one hand, too many waypoints dilute the optimization cost near significant terrain transitions, potentially compromising robot stability. On the other hand, the limited update range of the local submap hinders the robot from accurately acquiring terrain information around distant waypoints, which can lead to inaccurate terrain estimation that may interfere with the controller’s system model. Therefore, the prediction horizon of M²PC was limited to 10. The influence factors c _v and c _ω in the system model (21) were set to 10.0 and 0.0, respectively, to ensure stable maneuvering on stair-like terrain, which has anisotropic traversability.

The same reference path was utilized for consistent simulations. The robot would first climb up the staircase and then descend from the other side. Considering the actuator capabilities and the transmission design of the flipper, the maximum flipper rotation velocities were set as 0.1 rad/s. To evaluate how well the robot aligned with the terrain under different control methods, 100 points on the robot’s base footprint were uniformly sampled, and the average distance between these points and the terrain surface was computed. Furthermore, the robot’s centroid position along the z direction and the variations in pitch angle throughout the maneuver were monitored and recorded.

The quantitative performances of the control algorithms in the simulated scenario featuring severe terrain variations are presented in Figure 18. Gianni’s method adjusts the flipper angles in real-time based on the terrain plane fitted from the point cloud around the robot, aiming to maintain close contact with the ground. As a result, under conditions where the flipper motion speed is limited, Gianni’s method tends to suffer from delayed configuration adaptation, leading to issues such as free-spinning or robot impact against the terrain across various phases. Meanwhile, GEO’s core assumption posits that ATR’s stability is not solely dependent on robot-ground adherence. Considering the reference robot centroid path, GEO would sample flipper configurations near the waypoints and evaluate them based on the robot’s constraints and the dense prior point cloud map. An elaborately defined cost function was then employed to select the optimal flipper configuration, and an integrated controller was utilized for general tracking. Consequently, during the transition from flat ground to stair climbing, although the robot body does not closely adhere to the terrain, the ascent in the z direction and pitch angle variation remains relatively smooth (see Figure 18(a)). However, in the transition from stairs to elevated platforms and throughout all descent phases, GEO fails to adapt the configuration in response to rapid terrain changes, resulting in abrupt changes in the robot’s posture. Moreover, during the descent from stairs to flat ground, GEO is prone to misinterpret terrain configurations, leading to flipper misoperations (see Figure 18(d)).OPEN IN VIEWER

OPEN IN VIEWER

Figure 19.

The ATR maneuvering in the Gazebo-simulated scenario (a) and the lab-built staircase (b).

Compared to the baseline methods, M²PC enabled smoother maneuvering of the robot, more continuous and safer adjustments of the body pitch angle, and better contact performance between the robot and the ground. By incorporating information about surface curvature changes, M²PC allowed the robot to achieve better whole-body control. Especially when the flipper angle has not yet reached the desired position, the chassis actively slows down, achieving coordinated stair climbing. This whole-body movement mitigates impacts in the z direction and suppresses rapid changes in pitch angle. Additionally, when descending, the robot adjusted the joint configuration and reduced body oscillations.

Figure 20 illustrates the robot’s performances on a lab-built staircase using various control strategies. During the ascent phase, Gianni’s feedback control method lacked coordination, resulting in the robot’s inability to adjust its configuration in response to terrain changes and leading to impacts with the stairs. When the robot moved onto the landing, this strategy also failed to coordinate the flippers’ posture, causing the robot to experience a significant impact of up to 8G. This issue also occurred during the descent phase, where shaking in the z direction further confirms the problem. GEO exhibited similar performance, with the chassis experiencing significant vibrations throughout the stair-crossing.OPEN IN VIEWER

In contrast, M²PC effectively satisfied both terrain manifold constraints and robot kinematic limitations, thereby mitigating the aforementioned issues. During the stair-crossing, M²PC adjusted the robot’s configuration to ensure a smoother transition between terrains. Compared to the baseline methods, M²PC’s maneuvering strategy was more conservative, safer, and prevented severe impacts.

Validation of the framework in simulated scenarios

We comprehensively validated the proposed framework in two challenging simulation environments. The wild simulation scenario (shown in Figure 12(b)) features a lake, a bridge, caves, and significant ground undulation (an elevation difference of approximately 10 m). The robot has to cross the lake to reach the goal point in the inner cave. The building simulation scenario (the simple version of the six-story building, as shown in Figure 12(a)) features extensive and complex connectivity, requiring the robot to traverse multiple stairways to reach the goal point that is elevationally distant from the start point. Throughout the simulations, the robot’s pose, flipper joint states, global point cloud maps of the environment, and the local submap surrounding the robot were directly obtained from Gazebo.

We conducted extensive ablation studies by comparing multiple combinations of the proposed methods and several state-of-the-art methods, to comprehensively evaluate the performance of the proposed framework and its key modules. Multiple times of simulations were performed for each combination across the two Gazebo-simulated scenarios, and the success rates of different algorithmic combinations were compared. The detailed quantitative evaluation results are summarized in Table 5, and the best-performing executed trajectories for each combination are illustrated in Figure 21. In all simulations, the planning and tracking control processes were performed online. For clarity, we labeled each combination from A to J. Combinations A, B, C, D, and J were primarily used to evaluate the impact of different global path generation methods on overall navigation performance. Combinations F, G, and J focused on analyzing the contributions of various local path or trajectory optimization methods. Combinations H, I, and J were designed to assess the influence of different tracking control methods. Furthermore, due to the limited availability of comprehensive and well-established frameworks in the current literature, the 3D2M framework (excluding its wheeled robot controller) was selected as a representative baseline for fair comparison at the framework level, denoted as combination E.

OPEN IN VIEWER

Table 5.

Quantifications of the exhaustive ablation simulations in the Gazebo-simulated scenarios: Succ. denotes the success rate, d_min denotes the minimal Euclidean distance from the robot to the navigation goal, and Asc. denotes the maximum ascent in the building, L _e denotes the average executed trajectory length, and T _e denotes the average execution time.

	Global	Local	Ctrl.	Wild				Building
				Succ.(%)	dmin(m)	L e (m)	T e (s)	Succ.(%)	Asc.(m)	L e (m)	T e (s)
A	Colas’	ManiOpt	M2PC	50	≤ 0.2	93.63	187.7	0	0.03	–	–
B	Colas’	NULL	M2PC	0	59.44	–	–	0	0.19	–	–
C	Krüsi’s	ManiOpt	M2PC	20	≤ 0.2	120.07	246.0	0	2.74	–	–
D	Krüsi’s	NULL	M2PC	0	55.19	–	–	10	15.13	102.42	318.5
E	3D2M-G	3D2M-L	M2PC	–	–	–	–	0	2.16	–	–
F	CT-DC	3D2M-L	M2PC	0	53.74	–	–	0	2.63	–	–
G	CT-DC	NULL	M2PC	0	54.74	–	–	0	4.97	–	–
H	CT-DC	ManiOpt	Gianni’s	100	≤ 0.2	93.17	184.0	100	15.13	53.97	128.8
I	CT-DC	ManiOpt	GEO	90	≤ 0.2	100.02	358.3	50	15.13	60.26	488.8
J	CT-DC	ManiOpt	M2PC	100	≤ 0.2	92.89	185.3	100	15.13	53.82	145.1

OPEN IN VIEWER

Figure 21.

The best-performing executed trajectories of different combinations in the Gazebo-simulated scenarios.

Validation of the framework in real-world scenarios

We selected several real-world scenarios characterized by considerable challenges, to systematically evaluate the proposed framework alongside baseline methods. Each experiment presented in Architecture complex, Challenging uneven terrains, Debris and Interlaced stairways was conducted using prior point cloud maps with an identical size and a uniform resolution of 0.1 m. The experiment in Unknown construction site, however, utilized online point cloud maps generated by the SLAM module. Throughout all real-world experiments, the robot’s localization was provided by a modified version of FAST-LIO2 (Xu et al., 2022), which includes a re-localization function. The joint states of the flippers and the local submaps were continuously updated based on data collected from the onboard sensors.

Architecture complex

We conducted a series of experiments within an architecture complex (see Figure 22). The primary objective of this experimental setup was to evaluate the practical effectiveness of various global path planning algorithms (including CT-DC, Colas’ method, Krüsi’s method, and parts of the 3D2M planning pipeline) in large-scale, complex-connected environments with varying heights of traversable ground. In these experiments, M²PC is consistently used as the tracking controller. Except for the comparative combination involving the 3D2M framework, all experimental combinations employ the ManiOpt local path optimizer. The start point for all navigation tasks are located near (12.0 m, 3.0 m, 0.2 m) in the global coordinate system, with goal point near (96.3 m, − 45.1 m, 5.9 m). After conducting five trials for each combination, the best-performing executed trajectories were visualized in Figure 23. The corresponding key quantitative metrics are summarized in Table 6.OPEN IN VIEWER

Figure 22.

A typical architecture complex on a campus: (a) the reconstructed point cloud map (including the first three floors), (b) the general CG rendering, and (c) a corner of the scenario.

OPEN IN VIEWER

Figure 23.

The best-performing executed trajectories of different combinations in the architecture complex. The deterministic nature of CT-DC and Colas’ methods allowed the robot to efficiently navigate through stairways within the building and reach the goal via significantly shorter trajectories. In contrast, the stochastic Krüsi’s method resulted in detoured trajectories.

OPEN IN VIEWER

Table 6.

Quantifications of the experiments in the architecture complex: Succ. denotes the success rate, L _e denotes the average executed trajectory length, T _p denotes the average global planning time at the start point, and T _e denotes the average execution time.

Global	Local	Ctrl	Succ. (%)	L e (m)	T p /T e (s)
CT-DC	ManiOpt	M2PC	80	118.7	0.3/310.6
Colas’	ManiOpt	M2PC	40	119.8	3.9/339.8
Krüsi’s	ManiOpt	M2PC	20	258.3	43.2/659.7
3D2M-G	3D2M-L	M2PC	0	0.0	≥ 1200 /–

Except for one failure caused by crowd interference, the proposed framework consistently outperformed the comparative combinations in terms of success rate, average executed trajectory length, global planning time at the start point, and overall navigation duration. Colas’ method also achieved relatively efficient execution trajectories, but its planning time at the start point was longer due to the large-scale environment. Moreover, local path optimization occasionally failed in certain areas (like long corridors) due to poor initial paths from Colas’ method, resulting in collisions with obstacles and ultimately compromising the overall navigation success rate.

Krüsi’s method, affected by the superfluous traversable areas in the environment, produced highly stochastic global paths. This randomness resulted in frequent changes in heading direction (failed four times on the staircases and one success with an offline-generated path), substantially increasing the executed trajectory length, planning time, and overall execution duration. The terrain analysis module in 3D2M imposed heavy computational demands. At practical resolutions (below 0.5 m), it failed to complete terrain processing on onboard computers. At coarser resolutions (around 1.0 m), the algorithm produced inaccurate traversability estimates, resulting in global planning failures.

Challenging uneven terrains

We conducted a series of experiments on a steep grassy slope and a set of multi-segment staircases to further evaluate the effectiveness of different local path or trajectory optimization algorithms in real-world environments. The grassy slope, shown in Figure 24(a), featured soft, wet terrain with a high risk of slippage, posing significant challenges to ATR navigation. This experimental setup aimed to assess how well different local optimization algorithms could refine aggressive initial paths in steep environments. The multi-segment staircases, shown in Figure 24(b), were used to test each algorithm’s ability to optimize initially direct, non-adaptive paths in terrains with pronounced anisotropic traversability.OPEN IN VIEWER

Figure 24.

Two types of terrains with distinct challenges: (a) grassland with steep slope and (b) multi-segment wide staircases.

In these experiments, M²PC was consistently employed as the tracking controller. The initial paths were unified, adhered to the ground, and designated manually to ensure experimental consistency. Each algorithm was tested five times on both environments, and the best-performing executed trajectory for each was selected for visual presentation (see Figure 25). The corresponding key quantitative metrics for each method are summarized in Table 7.OPEN IN VIEWER

Figure 25.

The best-performing executed trajectories of different combinations on the two challenging uneven terrains.

OPEN IN VIEWER

Table 7.

Quantifications of the experiments on the challenging uneven terrains: Succ. denotes the success rate, d_min denotes the minimal Euclidean distance from the robot to the navigation goal, a_max denotes the maximum linear acceleration norm during the best-performing navigation, and Mal. denotes the average robot’s malfunction index (a smaller number indicates better control performance).

			Grassland				Multi-segment staircases
Global	Local	Ctrl	Succ.(%)	dmin(m)	amax(m/s2)	Mal	Succ.(%)	dmin(m)	amax(m/s2)	Mal
NAN	ManiOpt	M2PC	100	≤ 0.2	29.92	0.07	100	≤ 0.2	46.28	0.16
NAN	3D2M-L	M2PC	–	–	–	–	40	≤ 0.2	71.40	0.33
NAN	NULL	M2PC	100	≤ 0.2	41.89	0.19	0	3.97	–	–

To evaluate the execution performance of the robot, we defined a malfunction metric (Mal.) based on the deviation between the desired commands and actual velocity over a certain time horizon. Let v t , c m d x and v t , a x denote the longitudinal velocity command and the actual longitudinal velocity in the chassis coordinate frame F _b at time t, respectively. Over a T time steps of navigation on the uneven terrains, the Mal. index is computed as:

Mal. = 1 T ∑ t = 0 T v t , a x − v t , c m d x .

(25)

This metric serves as an indicator of how accurately the robot follows the intended control commands. A lower Mal. value reflects better control consistency.

On the grassland, the initial path descended rapidly, which posed stability challenges. The paths optimized by ManiOpt on the grassy slope effectively circumvented steep slopes and significantly reduced the pitch stability cost e P in the steep regions. These optimized paths adhered closely to the terrain manifold and exhibited strong disturbance rejection. More importantly, the conservative, terrain-adaptive paths generated by ManiOpt substantially lowered the risks of chassis destabilization and reduced the robot’s peak centroid accelerations. These results underscore the critical importance of terrain-manifold-based hard constraints in optimizing feasible and stable local paths. Meanwhile, 3D2M-L failed to execute its optimization process, as its terrain analysis module was unable to process traversability estimation in this scenario due to the size and high resolution of the point cloud map.

On the staircases, ManiOpt employed an adaptive optimization objective tailored to the specific terrain, ensuring safe traversal on the anisotropically traversable stairs. It also enabled coordinated adjustment on flat, isotropically traversable grounds, helping to shorten the overall path length. Throughout all test runs, ManiOpt consistently avoided path-terrain detachment, demonstrating advantages in mitigating potential controller malfunctions and failures. These results strongly validate the effectiveness of the manifold optimization approach. In contrast, the 3D2M-L optimizer failed to produce a trajectory that maintains good ground adherence or accounts for anisotropic traversability, resulting in lateral slips and partial loss of control on the second-segment staircase. In experiments conducted without optimization, the robot was unable to perform adequate obstacle avoidance, resulting in perilous collisions.

Debris

We conducted a series of experiments at a post-demolition site to evaluate the performance of different ATR whole-body control methods in real-world scenarios, as shown in Figure 26. The experiment area covered approximately 400 m² and presented significant challenges, including unstable rubble piles, soft soil slopes, and numerous hard protrusions, realistically simulating a post-disaster environment. Additionally, we compared the performance of the 3D2M framework when integrated with M²PC. Each algorithm combination was tested five times, with a fixed start and goal point located near (0.0 m, 0.0 m, 0.1 m) and (16.5 m, 0.0 m, 3.4 m), respectively. The best-performing executed trajectories were selected for visual presentation (see Figure 27). The corresponding key quantitative metrics for each combination are summarized in Table 8.OPEN IN VIEWER

Figure 26.

The post-demolition debris: (a) the reconstructed point cloud map and (b) the front side of the debris.

OPEN IN VIEWER

Figure 27.

The best-performing executed trajectories of different combinations on the debris.

OPEN IN VIEWER

Table 8.

Quantifications of the experiments on the debris: Succ. denotes the success rate, d_min denotes the minimal Euclidean distance from the robot to the navigation goal, a_max denotes the maximum linear acceleration norm during the best-performing navigation, T _e denotes the average execution time, L _e denotes the average executed trajectory length, Smot. denotes the average executed path smoothness, and Mal. denotes the average robot’s malfunction index.

Global	Local	Ctrl	Succ.(%)	dmin(m)	amax(m/s2)	T e (s)	L e (m)	Smot	Mal
CT-DC	ManiOpt	M2PC	60	≤ 0.2	18.01	59.00	17.96	0.30	0.38
CT-DC	ManiOpt	Gianni’s	0	2.11	21.73	–	–	0.53	0.51
CT-DC	ManiOpt	GEO	0	5.98	19.22	–	–	0.58	0.51
3D2M-G	3D2M-L	M2PC	0	7.24	20.90	–	–	0.54	0.47

In the experiments, the global path generated by 3D2M was relatively straight and consistent. However, its local optimization was heavily influenced by terrain roughness analysis, resulting in a highly tortuous path. This path caused the robot to perform excessive twists over the rubble, leading to entrapment on small, rigid protrusions (see Figure 27). The combination using Gianni’s controller frequently experienced poor contact between the robot’s body and the soft soil slopes, which often resulted in track subsidence. Moreover, when encountering minor rigid obstacles on the ground, Gianni’s controller also tends to get stuck. Furthermore, GEO failed in all five trials due to the significant computation failure of flipper configurations in rough terrain.

In contrast, the proposed framework is the only solution capable of completing the rubble terrain navigation task. Although two failures occurred due to gravel slides and rebar obstructions, the overall performance remained robust. With the integration of the extracted quadratic-form terrain information and the robot’s inherent constraints, M²PC demonstrated safer control behaviors during execution, effectively reducing the risks of the robot becoming trapped in soft soil or immobilized by rubble.

Interlaced stairways

We conducted a series of experiments on a fire rescue stairway and an irregular spiral stairway (see Figure 28), to evaluate the capabilities of different ablation combinations in multi-floor scenarios. Both scenarios consisted of multiple staircases and platforms, with a maximum traversable elevation of approximately 6 to 7 m. The horizontal overlap of the stair structures and the complex traversability conditions provided an excellent setting for assessing the performance of each algorithm combination.OPEN IN VIEWER

Figure 28.

Two stairways with multi-floor structure: (a) rescue stairway and (b) spiral stairway.

Due to the constrained connectivity of the experiment environments, Krüsi’s method and the CT exhibited similar path generation patterns; thus, further comparison with Krüsi’s method was not conducted. In each experiment, the robot started from a low-elevation start point, climbed the stairs to reach goal point 1 on a high platform, and then descended to return to goal point 2 near the original start point. For the rescue stairway scenario, the start point and goal point 2 were located near (0.0 m, 0.0 m, 0.1 m), while goal point 1 was placed at (5.6 m, 1.9 m, 6.4 m), as the colored circle illustrated in Figure 29. For the spiral stairway scenario, the start point and goal point 2 were located near (0.0 m, 0.0 m, 0.1 m), with goal point 1 set at (11.0 m, 4.5 m, 7.1 m), as the colored circle illustrated in Figure 30. Each algorithm combination was tested five times, and the best-performing executed trajectories were selected for visual presentation (see Figures 29 and 30). The corresponding key quantitative metrics for each combination are summarized in Table 9.OPEN IN VIEWER

Figure 29.

The best-performing executed trajectories of different combinations on the rescue stairway.

OPEN IN VIEWER

Figure 30.

The best-performing executed trajectories of different combinations on the spiral stairway.

OPEN IN VIEWER

Table 9.

Quantifications of the ablation experiments on the stairways: Succ. denotes the success rate, Asc. denotes the maximum ascent on the staircases, a_max denotes the maximum linear acceleration norm during the best-performing navigation, and Smot. denotes the average executed path smoothness.

	Global	Local	Ctrl	Rescue stairway				Spiral stairway
				Succ.(%)	Asc.(m)	amax(m/s2)	Smot	Succ.(%)	Asc.(m)	amax(m/s2)	Smot
A	Colas’	ManiOpt	M2PC	20	6.37	22.17	0.37	20	7.13	31.30	0.53
B	Colas’	NULL	M2PC	0	1.99	30.66	0.54	0	1.97	39.87	0.31
E	3D2M-G	3D2M-L	M2PC	–	–	–	–	0	0.30	50.49	0.50
F	CT-DC	3D2M-L	M2PC	0	5.87	23.10	0.49	20	7.13	31.09	0.30
G	CT-DC	NULL	M2PC	20	6.37	53.11	0.44	0	5.26	40.26	0.46
H	CT-DC	ManiOpt	Gianni’s	60	6.37	80.93	0.36	40	7.13	74.35	0.41
I	CT-DC	ManiOpt	GEO	0	0.97	57.34	1.17	0	0.41	40.01	0.40
J	CT-DC	ManiOpt	M2PC	100	6.37	23.14	0.32	100	7.13	25.57	0.28

During the rescue stairway experiments, only combinations A, G, H, and J completed the navigation task at least once. Our proposed framework J achieved the highest success rate, exhibiting the smoothest executed trajectory and robustness during rapid terrain shifts. For combinations A and B, the graph-search-based Colas’ method generated extreme paths that were close to the guardrails. Thus, in combination B, collisions were inevitable. However, in combination A, ManiOpt was able to partially mitigate this issue (with a 20% success rate).

For combination E and F, 3D2M’s terrain analysis struggled in environments containing irregular under-surfaces, such as beams on the ceiling, mistakenly classifying them as obstacles. This misinterpretation led to failures in both global path planning and local path optimization. Even initialized with a reasonable global path, the optimized trajectory in combination F tended to deviate from the ground. In combination G, without local optimization, CT-DC alone produced coarse and stochastic global paths, resulting in unsafe slippage on anisotropically traversable staircases.

Combination H, employing Gianni’s controller, exhibited inadequate response, causing robot-ground impacts at every first and every final stage of staircase climbing. The two failures of combination H were attributed to the onboard computer crash due to these impacts. Meanwhile, the combination I with GEO required excessively high resolutions in both local path and map representations. In practical online navigation, GEO consistently failed to process valid flipper configuration analysis, resulting in flipper misbehavior and subsequent entrapment of the robot on stair edges, accompanied by high-frequency oscillations of the body.

The spiral stairway provides slightly more navigable space compared to the tightly confined scissor-style rescue stairway. However, it introduces significant challenges due to steep tangential inclinations on the inner arc. In the experiments, four combinations completed at least one navigation test. The proposed framework J again demonstrated the best overall performance. Although the global paths generated by Colas’ method in combinations A and B were feasible, they remained risky. Successful navigation was only possible when coupled with ManiOpt to refine the path and avoid collisions.

Due to the refractive properties of glass, parts of the spiral stairway’s glass guardrails were missing from the point cloud map. 3D2M failed to detect the hazardous obstructions along both sides of the ground-adjacent staircases, leading its global planner to erroneously produce paths that passed through the missing glass walls, resulting in navigation failure. This failure in obstacle detection also impaired its local trajectory optimization. Even when using global paths generated by CT-DC, 3D2M-L’s local optimizer frequently produced trajectories too close to the handrails, which again led to several unsuccessful navigation attempts.

Combination H, which utilized Gianni’s controller, also completed the task. However, as the robot tended to follow paths along the outer arc of the stairway, frequent critical impacts (even causing SLAM drift, see Figure 30(b)) between the robot chassis and the staircase were observed on the larger inter-step distance staircases. That is because this type of control strategy is incapable of accommodating long-term processing and lacks the responsiveness required to adapt to rapidly changing terrains. Combination I with GEO exhibited similar issues to those in the rescue stairway scenario, struggling with the configuration analysis of flippers and failing to maintain smooth and stable motion.