---
title: 'CAP: Capsizing-Aware Trajectory Planner'
url: https://www.emergentmind.com/topics/capsizing-aware-trajectory-planner-cap
type: topic
---

# CAP: Capsizing-Aware Trajectory Planner

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 [2508.08108]. 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 [2508.08108]. 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 [2508.08108].

## 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 [2508.08108]. 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 \(\mathbf f_g\) onto the contact plane

The paper gives a **necessary and sufficient condition for no tip-over**: \(\mathbf O_g\) must lie strictly inside \(\mathcal R\). An equivalent edge-wise test is then defined. For each edge \(i\), the **tip-over normal** is

$$
\mathbf l_i  = \bigl(\mathbf I - \hat{\mathbf t}_i\hat{\mathbf t}_i^{\!\top}\bigr)\bigl(\mathbf p_{i+1}-\mathbf p_c\bigr),
$$

with sign

$$
\epsilon_i =  \begin{cases}
+1,& (\hat{\mathbf l}_i\times\hat{\mathbf f}_g)\cdot\hat{\mathbf t}_i<0,\\
-1,&\text{otherwise.}
\end{cases}
$$

The robot is stable if and only if

$$
\min_{i=1,\dots,N} \;\epsilon_i\;\arccos\!\bigl(\hat{\mathbf f}_g\cdot\hat{\mathbf l}_i\bigr) >0
\quad\Longleftrightarrow\quad
\mathbf O_g\in\mathcal R.
$$

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 [2508.08108]. The local surface normal \(\mathbf n\) and gravity \(\mathbf f_g\) define a **stability angle**

$$
\xi \;=\;\arccos\bigl(\hat{\mathbf f}_g\cdot(-\mathbf n)\bigr),
$$

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

$$
L \;=\; h\,\tan\xi,
$$

where \(h\) is the vertical height of the center of mass above the contact plane. Let the vehicle’s plan-view footprint have half-width \(w/2\) and half-length \(l/2\).

The paper distinguishes four regimes.

**Case A: \(L<\tfrac w2\).** The robot can safely assume any heading \(\theta\).

**Case B: \(\tfrac w2\le L<\tfrac l2\).** Define

$$
\theta_r \;=\;\arcsin\!\Bigl(\frac{w}{2L}\Bigr).
$$

Writing \(\theta_{\nabla}\) for the heading relative to the downhill gradient direction, the safe headings satisfy

$$
|\theta_{\nabla}|<\theta_r
\quad\lor\quad
\pi-\theta_r<|\theta_{\nabla}|<\pi.
$$

**Case C: \(\tfrac l2\le L<\tfrac{\sqrt{w^2+l^2}}{2}\).** Define

$$
\theta_{r1}=\arcsin\!\Bigl(\frac{w}{2L}\Bigr),\quad
\theta_{r2}=\arccos\!\Bigl(\frac{l}{2L}\Bigr).
$$

Then safe headings satisfy

$$
\theta_{r2}<|\theta_{\nabla}|<\theta_{r1}
\quad\lor\quad
\pi-\theta_{r1}<|\theta_{\nabla}|<\pi-\theta_{r2}.
$$

**Case D: \(L\ge\tfrac{\sqrt{w^2+l^2}}{2}\).** No heading is safe; capsizing is inevitable.

These conditions are compactly encoded by a piecewise Heaviside test \(H(\theta_{\nabla};L)\), and whenever \(H(\theta_{\nabla};L)=1\), the orientation \(\theta\) belongs to the **traversable orientation set** \(\Theta\). In operational terms, \(\Theta\) 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

$$
\mathcal S=\{(\mathbf s_i,\Delta T_i)\}_{i=0}^n,
$$

where \(\mathbf s_i=[x_i,y_i,\theta_i]\in SE(2)\) and \(\Delta T_i\ge 0\) is the time to the next waypoint [2508.08108]. Following the TEB paradigm, the planner is written as the constrained problem

$$
\begin{aligned}
\min_{\{\mathbf s_i,\Delta T_i\}} \;&\sum_{i=0}^{n-1} \Delta T_i^2\\
\text{s.t.}\quad
&\kappa(\mathbf s_i,\mathbf s_{i+1})=0\quad\text{(kinematics)},\\
&\theta_i\in\Theta\bigl(L(\mathbf s_i),\,\theta_{\nabla,i}\bigr)\quad\text{(capsize)},\\
&\mathbf 0\le\bm\nu_i\le\bm\nu_{\max},\quad \mathbf 0\le\mathbf a_i\le\mathbf a_{\max},\\
&\mathbf s_0=\mathbf s_{\rm start},\;\mathbf s_n=\mathbf s_{\rm goal}.
\end{aligned}
$$

Here, \(\bm\nu_i\) and \(\mathbf a_i\) are the discrete velocity and acceleration computed from \(\{\mathbf s_i,\Delta T_i\}\).

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

$$
\min_{\mathcal S}\;\sum_{i=0}^{n-1}\Bigl\{
\gamma_T\,\Delta T_i^2
\;+\;
\gamma_\kappa\,f_\kappa(\mathbf s_i,\mathbf s_{i+1})
\;+\;
\gamma_\Theta\,f_\Theta(\theta_i)
\;+\;
\gamma_u\,f_u(\mathbf s_i,\mathbf s_{i+1})
\Bigr\}.
$$

The terms \(f_\kappa\) and \(f_u\) are quadratic penalties on kinematic and dynamic bounds. The **capsizing-safety penalty** is

$$
f_\Theta(\theta_i) =
\begin{cases}
\infty,&L_i\ge\frac{\sqrt{w^2+l^2}}{2}, \quad\text{(forbid)}\\[4pt]
\|\theta_i-\theta_{c,i}\|^2,&\text{otherwise},
\end{cases}
$$

where \(\theta_{c,i}\) is the center of the allowed heading interval in which \(\theta_i\) 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 s_i,\Delta T_i)\) for \(i=0,\dots,n\) [2508.08108]. The factor edges correspond directly to the penalty terms:

- a **time edge** from \(\Delta T_i\) to the cost \(\gamma_T\Delta T_i^2\)
- a **kinematic edge** connecting \(\mathbf s_i,\mathbf s_{i+1}\) with cost \(f_\kappa\)
- a **capsize edge** on node \(\theta_i\) with cost \(f_\Theta(\theta_i)\)
- a **smoothness/dynamics edge** on successive triplets for \(f_u\)

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:

```text
Given 2.5D grid map M, global path σ (optional)
Initialize S₀ by straight line or previous solution
repeat
  regress ground normal field F_GN from M
  for k = 1..K do
    build factor graph constraints C from S and F_GN
    δS ← solve graph(C, S)      # LM update
    S ← S + δS
  end
  send first segment of S to low-level tracker
until goal reached
```

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 [2508.08108]. In simulation, the setup is:

- **Platform**: ROS Noetic + Gazebo on Intel i7-10875H, 16 GB RAM
- **Robot**: differential drive, footprint \([w,l,h]=[0.7,0.93,0.35]\) m
- **Scenes**: “hilliness” and “forest” terrains
- **Metrics**:
  - navigation time \(\mathcal T\)
  - stability measure \(\Upsilon\) = sum of peak \(|\text{roll}|+|\text{pitch}|\)
- **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 \(\Upsilon\) (best stability) with competitive \(\mathcal T\)**, 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 \(L<\tfrac w2\) or satisfy the Case B/C heading constraints. The onboard logs confirm \(\theta_i\in\Theta\) 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 [2508.08108].

Source: https://www.emergentmind.com/topics/capsizing-aware-trajectory-planner-cap