CAP: Capsizing-Aware Trajectory Planner

• The paper introduces CAP, a trajectory-planning method that integrates rigorous tip-over stability analysis with terrain-aware traversability constraints.
• CAP employs a graph-based optimizer using the Levenberg–Marquardt algorithm to balance safety and efficiency by ensuring safe robot orientations on uneven ground.
• Simulation and real-world experiments validate CAP’s ability to minimize tip-over incidents while achieving competitive navigation performance in challenging terrains.

The Capsizing-Aware Trajectory Planner (CAP) is a trajectory-planning method for autonomous ground robots operating on uneven terrain. It is presented in “Capsizing-Guided Trajectory Optimization for Autonomous Navigation with Rough Terrain” and is designed to balance safety and efficiency by explicitly preventing tip-over during navigation (Zhang et al., 11 Aug 2025). CAP couples a tip-over stability analysis with a definition of traversable orientation, then embeds the resulting capsizing-safety condition into a graph-based trajectory optimizer solved by Levenberg–Marquardt. In the reported formulation, the method addresses the primary challenge of generating a feasible trajectory that prevents robot from tip-over while ensuring effective navigation on rough ground.

1. Problem formulation and scope

CAP is introduced for autonomous navigation in harsh environments containing non-trivial obstacles and uneven terrain. The stated objective is to produce a robust and feasible trajectory that remains safe with respect to capsizing while preserving effective motion toward the goal (Zhang et al., 11 Aug 2025). The method is framed for wheeled ground robots and is instantiated in the paper for a four-wheeled contact model, although the stability construction is written for a general set of contact points $\{\mathbf p_i\}_{i=1}^N$.

The planner is explicitly capsizing-aware: instead of treating terrain only as a geometric traversability problem, it evaluates whether a robot orientation is admissible under gravity and local surface inclination. This is operationalized through a terrain-dependent orientation set $\Theta$, which is then used as a constraint in trajectory optimization. A plausible implication is that CAP differs from planners that merely avoid geometric collisions or optimize smoothness, because its safety model depends directly on local ground normals and the robot’s center-of-mass geometry.

The paper states that CAP follows the TEB paradigm for trajectory representation and optimization, but augments that structure with a terrain-derived capsizing-safety term. The reported result is a planner that outperforms existing state-of-the-art approaches, with enhanced navigation performance on uneven terrains in both simulation and real-world experiments (Zhang et al., 11 Aug 2025).

2. Tip-over stability model

The stability analysis is based on a stability pyramid whose base is the convex hull of the robot’s contact points in the contact plane (Zhang et al., 11 Aug 2025). The notation introduced in the paper is:

• $\mathcal R\subset\mathbb R^3$: the stability polygon in the contact plane
• $\mathbf p_c$: the robot’s center of mass
• $\mathbf f_g = m\,\mathbf g$: the gravity vector
• $\mathbf n$: the unit normal of the contact plane, with $\tilde{\mathbf n}=-\mathbf n$
• $\mathbf t_i = \mathbf p_{i+1}-\mathbf p_i$: the $i$th edge of the polygon
• $\mathbf O_g$: the projection of $\Theta$0 onto the contact plane

The paper gives a necessary and sufficient condition for no tip-over: $\Theta$1 must lie strictly inside $\Theta$2. An equivalent edge-wise test is then defined. For each edge $\Theta$3, the tip-over normal is

$\Theta$4

with sign

$\Theta$5

The robot is stable if and only if

$\Theta$6

Here, hats denote unit-normalization.

This construction makes the stability test geometric rather than heuristic. The decisive quantity is not only terrain slope, but the relative placement of the projected gravity line and the support polygon. This suggests that CAP’s safety model is sensitive to both body orientation and center-of-mass placement.

3. Traversable orientation on uneven terrain

From the tip-over analysis, the paper defines traversable orientation, namely the safe range of robot headings at a fixed map cell (Zhang et al., 11 Aug 2025). The local surface normal $\Theta$7 and gravity $\Theta$8 define a stability angle

$\Theta$9

and the horizontal offset of the gravity line from the mass-center projection is

$\mathcal R\subset\mathbb R^3$0

where $\mathcal R\subset\mathbb R^3$1 is the vertical height of the center of mass above the contact plane. Let the vehicle’s plan-view footprint have half-width $\mathcal R\subset\mathbb R^3$2 and half-length $\mathcal R\subset\mathbb R^3$3.

The paper distinguishes four regimes.

Case A: $\mathcal R\subset\mathbb R^3$4. The robot can safely assume any heading $\mathcal R\subset\mathbb R^3$5.

Case B: $\mathcal R\subset\mathbb R^3$6. Define

$\mathcal R\subset\mathbb R^3$7

Writing $\mathcal R\subset\mathbb R^3$8 for the heading relative to the downhill gradient direction, the safe headings satisfy

$\mathcal R\subset\mathbb R^3$9

Case C: $\mathbf p_c$0. Define

$\mathbf p_c$1

Then safe headings satisfy

$\mathbf p_c$2

Case D: $\mathbf p_c$3. No heading is safe; capsizing is inevitable.

These conditions are compactly encoded by a piecewise Heaviside test $\mathbf p_c$4, and whenever $\mathbf p_c$5, the orientation $\mathbf p_c$6 belongs to the traversable orientation set $\mathbf p_c$7. In operational terms, $\mathbf p_c$8 converts local terrain geometry into a heading-feasibility condition. A plausible implication is that CAP does not merely classify a cell as traversable or non-traversable; it classifies which orientations at that cell remain dynamically safe with respect to tip-over.

4. Capsizing-safety constraint in trajectory optimization

A candidate 2D trajectory is represented as

$\mathbf p_c$9

where $\mathbf f_g = m\,\mathbf g$0 and $\mathbf f_g = m\,\mathbf g$1 is the time to the next waypoint (Zhang et al., 11 Aug 2025). Following the TEB paradigm, the planner is written as the constrained problem

$\mathbf f_g = m\,\mathbf g$2

Here, $\mathbf f_g = m\,\mathbf g$3 and $\mathbf f_g = m\,\mathbf g$4 are the discrete velocity and acceleration computed from $\mathbf f_g = m\,\mathbf g$5.

To solve the problem in a single graph-based least-squares, the paper introduces soft penalties:

$\mathbf f_g = m\,\mathbf g$6

The terms $\mathbf f_g = m\,\mathbf g$7 and $\mathbf f_g = m\,\mathbf g$8 are quadratic penalties on kinematic and dynamic bounds. The capsizing-safety penalty is

$\mathbf f_g = m\,\mathbf g$9

where $\mathbf n$0 is the center of the allowed heading interval in which $\mathbf n$1 must lie.

The formulation integrates terrain-induced safety directly into the optimization objective. In the stated design, the planner simultaneously minimizes total time, enforces kinematics and dynamic bounds, and biases headings toward the safe interval center. This suggests a compromise between hard infeasibility in clearly unsafe terrain and soft regularization inside admissible orientation intervals.

5. Graph-based solver and online workflow

The optimization is implemented as a factor graph whose variable nodes are $\mathbf n$2 for $\mathbf n$3 (Zhang et al., 11 Aug 2025). The factor edges correspond directly to the penalty terms:

• a time edge from $\mathbf n$4 to the cost $\mathbf n$5
• a kinematic edge connecting $\mathbf n$6 with cost $\mathbf n$7
• a capsize edge on node $\mathbf n$8 with cost $\mathbf n$9
• a smoothness/dynamics edge on successive triplets for $\tilde{\mathbf n}=-\mathbf n$0

At each iteration, the solver linearizes all residuals, assembles the normal equations, and solves for a damping-adjusted step using the Levenberg–Marquardt algorithm. According to the paper, convergence yields a trajectory that minimizes total time, satisfies vehicle kinematics, and strictly remains within safe orientation intervals to avoid tip-over.

The algorithmic outline given in the paper is:

$\tilde{\mathbf n}=-\mathbf n$9

This workflow combines on-the-fly ground-normal estimation with repeated graph optimization. The role of the optional global path is explicitly limited in the algorithmic description; the planner can also initialize from a straight line or previous solution.

6. Simulation, real-world validation, and reported behavior

The paper reports both simulation and real-world experiments validating CAP (Zhang et al., 11 Aug 2025). In simulation, the setup is:

• Platform: ROS Noetic + Gazebo on Intel i7-10875H, 16 GB RAM
• Robot: differential drive, footprint $\tilde{\mathbf n}=-\mathbf n$1 m
• Scenes: “hilliness” and “forest” terrains
• Metrics:
  • navigation time $\tilde{\mathbf n}=-\mathbf n$2
  • stability measure $\tilde{\mathbf n}=-\mathbf n$3 = sum of peak $\tilde{\mathbf n}=-\mathbf n$4
• Baselines: TEB-Planner, PUTN-RRT, Uneven-Planner, Hybrid A*/PF-RRT guides, and target-only

The reported results in Table I are that CAP achieves lowest $\tilde{\mathbf n}=-\mathbf n$5 (best stability) with competitive $\tilde{\mathbf n}=-\mathbf n$6, that baselines either tip-over or exhibit large attitude variations, and that CAP succeeds even with no global path (“target-only”), other methods fail.

The real-world system uses:

• Hardware: Scout 2.0 UGV with Ouster OS-32 LiDAR, Kinect RGB-D, IMU, and NVIDIA Jetson Orin NX
• Localization: ROLO-SLAM
• Scenario: steep red-shaded slope identified as non-traversable
• Planner: CAP with no pre-computed global path

The reported behavior is that the UGV automatically circumnavigates the steep region, choosing a path whose local surface normals yield $\tilde{\mathbf n}=-\mathbf n$7 or satisfy the Case B/C heading constraints. The onboard logs confirm $\tilde{\mathbf n}=-\mathbf n$8 at every waypoint and zero tip-over events.

Taken together, the simulation and real-world results support the paper’s summary claim that CAP couples a rigorous tip-over stability analysis (stability pyramid) with on-the-fly ground-normal estimation and embeds the resulting heading-safety constraint into a graph-based trajectory optimizer. The reported outcome is fast, safe navigation in highly uneven terrain, with validation in both simulated and physical settings (Zhang et al., 11 Aug 2025).