A Safe and Efficient Navigation Framework for Ground Vehicles on Uneven Terrain Considering Kinematic Constraints and Terrain Traversability

TL;DR

This paper introduces a new navigation framework for ground vehicles on uneven terrain that ensures safety and efficiency by integrating terrain factors and vehicle constraints.

Contribution

The novel three-phase path planning algorithm with a Kinematic and Terrain-Aware Probabilistic Roadmap (KT-PRM) is introduced for safer and smoother navigation.

Findings

The proposed framework generates safer, smoother, and shorter paths on uneven terrain.

The three-phase algorithm outperforms existing frameworks in terms of safety and efficiency.

Experiments in simulated and real-world environments validate the framework's effectiveness.

Abstract

Ground vehicles navigating uneven terrain must simultaneously guarantee motion safety and efficiency. Safety requires that the planned waypoints lie in highly traversable terrain, while ensuring vehicle reachability to these waypoints, which must be kinematically feasible. Efficiency demands fewer detours and smoother paths that avoid excessive vehicle acceleration and steering. However, existing path planning research for uneven terrain fails to comprehensively integrate vehicle kinematic constraints, terrain factors, path smoothness, rollover risk, and total path length. To address this problem, this paper proposes a novel navigation framework. It first integrates terrain slope, flatness, elevation variation, and sparsity to generate a 2D global terrain traversability cost map. Subsequently, a three-phase path planning algorithm integrates A*, guided Rapidly-exploring Random Tree (RRT), and our proposed Kinematic and Terrain-Aware Probabilistic Roadmap (KT-PRM) local re-planning algorithm, which jointly considers multiple factors including ground vehicle kinematic constraints, terrain factors, path smoothness, rollover risk, and path length. This three-phase combination delivers safe, smooth, and short global paths over uneven terrain within a relatively short planning time. Finally, Nonlinear Model Predictive Control (NMPC) is employed for path tracking in the framework. Experiments were conducted in both simulated and real-world uneven terrain environments. The results demonstrated that the three-phase path planning algorithm integrated with our proposed KT-PRM algorithm achieves comprehensive performance in generating safer, smoother, and shorter paths. Our proposed navigation framework achieves safer and more efficient navigation compared with existing navigation frameworks.

Figures

Full text

1. Introduction

Uneven terrain is common in unstructured outdoor environments such as woodland, construction sites and mining areas [1,2,3]. The irregular surface of uneven terrain significantly affects the safety and efficiency of ground vehicle autonomous navigation [4,5,6,7]. The safety of navigation requires the planned waypoints to avoid low traversability terrain, such as steep slopes, highly undulated regions, and pits, while ensuring kinematically feasible paths that guarantee vehicle reachability [8,9,10]. These requirements further demand vehicle stability and rollover prevention for safe path tracking. Efficiency demands fewer detours and path smoothness without excessive vehicle acceleration and steering, which would raise motion cost during path tracking. Therefore, developing a global path planning method that comprehensively and effectively integrates multiple factors, namely vehicle kinematic constraints, terrain traversability, path smoothness, rollover risk, and path length, remains an open challenge.

A key limitation of existing uneven terrain path planning methods is their partial consideration of these factors. Some studies incorporate terrain factors such as slope, flatness, or elevation variation into sampling-based or graph-based planning algorithms (e.g., Rapidly-exploring Random Tree (RRT), A*, Dijkstra) to avoid hazardous regions [11,12,13,14]. Although these terrain-aware algorithms effectively enhance terrain-related safety of global paths, they often ignore strict kinematic constraints. Consequently, the generated paths either incur significant vehicle motion cost when tracked directly via the controller or require heavy smoothing before execution. Moreover, these algorithms neglect the vehicle’s rollover risk during traversal, which corresponds to the vehicle’s motion direction into the local terrain [15,16], thereby compromising the absolute safety of vehicle execution.

Other research formulates planning problems on uneven terrain as nonlinear optimization problems [17,18]. Solving those problems requires complex models, and the solutions are dependent on initial solution quality. Reinforcement learning-based terrain navigation methods [19,20,21,22] have recently emerged, but they rely on extensive training data and high-quality perception, limiting their applicability in real-world outdoor navigation [23].

Overall, despite recent advances in uneven terrain navigation, jointly accounting for vehicle kinematic constraints, terrain traversability, path smoothness, rollover risk, and path length within a computationally efficient navigation framework remains an open challenge.

To address this challenge, we propose a safe and efficient navigation framework. In this framework, a global terrain traversability cost map is first constructed, which fuses slope, flatness, elevation variation, and point cloud sparsity into a unified terrain traversability index. On this map, a globally kinematically feasible and safe path is then generated by a three-phase path planning algorithm, which comprises A*, guided RRT, and a novel local re-planning algorithm, termed Kinematic and Terrain-Aware Probabilistic Roadmap (KT-PRM). Unlike the classic Probabilistic Roadmap (PRM), the proposed KT-PRM considers the vehicle kinematic constraints and integrates terrain traversability cost, motion cost, deviation cost, and rollover cost, accounting for the vehicle’s direction of motion, enabling the planner to produce safe, smooth, and short global paths without a post-smoothing process. The navigation framework is finalized by an NMPC (Nonlinear Model Predictive Control) tracker for real-world deployment.

The main contributions of this paper are summarized as follows:

1. We propose a novel path planning algorithm termed KT-PRM based on the PRM, which comprehensively and effectively incorporates the kinematic constraints of ground vehicles, terrain traversability cost, motion cost, deviation cost, and rollover cost, accounting for the vehicle’s direction of motion. This algorithm introduces an innovative roadmap construction strategy and an extended accumulated cost function in graph search.
2. We propose a novel and comprehensive framework for safe and efficient navigation on uneven terrain. It comprises a global terrain traversability cost map construction algorithm, a three-phase path planning algorithm, and an NMPC controller for path tracking.
3. The proposed navigation framework is validated through both simulated and real-world uneven terrain environments, evaluating its safety, efficiency, and superiority in navigation.

2. Related Works

Path planning on uneven terrain demands dealing with complex and highly variable topography while guaranteeing vehicle kinematic feasibility, smooth paths, and high traversal efficiency. Existing research can be grouped into three categories: (1) terrain-aware path planning on uneven terrain, (2) kinematic and dynamic constrained path planning on uneven terrain, and (3) hierarchical planning and PRM variants on uneven terrain. This section reviews the progress and limitations of each category.

2.1. Terrain-Aware Path Planning on Uneven Terrain

Incorporating terrain characteristics such as slope, flatness, and point cloud sparsity into path planning algorithms is a common strategy to avoid low traversability regions. Plane-fitting and local geometry features of uneven terrain have been widely used to calculate terrain traversability for sampling-based planners. For example, Jian et al. [11] applied plane-fitting to estimate local terrain slope and integrated terrain traversability into the PF-RRT* algorithm. Probabilistic sampling biased by terrain structure has been introduced in uneven terrain environments such as Gaussian-adaptive samplers [24] and history-aware terrain labeling for navigating unknown terrain [25]. For graph-based planners, some research constructed supervoxel graphs to calculate minimal cost with the Dijkstra graph search algorithms [26] or formulated Traversal Risk Graphs for path optimization in unstructured environments [15]. But vehicle kinematic constraints, path smoothness, and rollover risk were not taken into account for planning in the aforementioned algorithms, affecting tracking effectiveness and safety. Liu et al. [27] combined multi-layer elevation information with a traversability cost module in Hybrid A* planning. A cost function combining terrain metrics and estimated vehicle effort was used in A* or graph-based planners in [28]. These algorithms generated terrain-aware and kinematically feasible paths, but they still neglected rollover risk, and their cost functions in graph search omitted kinematic parameters such as vehicle acceleration and path curvature.

These methods effectively guide planned paths away from low traversability regions. However, they focus solely on terrain factors and basic kinematic feasibility, neglecting comprehensive incorporation of kinematic constraints, path smoothness, rollover risk, and path length. Consequently, some resulting paths violate vehicle physical limits, exhibit low waypoint reachability, and suffer from reduced safety and efficiency during traversal [29], while others are jagged, raising motion cost in path tracking.

2.2. Kinematics and Dynamics Constrained Path Planning on Uneven Terrain

Some research emphasizes the compliance with kinematics, dynamics, and curvature constraints of vehicles, which can generate paths that are inherently feasible for Ackermann-steering vehicles or car-like mobile robots. In these algorithms, path generation is formulated as a constrained optimization problem. Savkin et al. [18] minimized fuel consumption on rugged terrain through nonlinear optimization. Xu et al. [30] incorporated curvature and dynamic feasibility constraints into a nonlinear optimization framework for 3D terrain. Wang et al. [31] extended 2D trajectory generation into 3D environments with smoothness, collision, and terrain-related constraints.

Although these optimization-based planners can generate kinematically and dynamically feasible paths, their effectiveness remains constrained by initial solution quality, limiting potential for breakthrough optimization improvements [32,33]. Additionally, it is difficult to construct accurate dynamics models of the robot in contact with the terrain in more complex scenarios [30].

2.3. Hierarchical Planning and PRM Variants on Uneven Terrain

Hierarchical or multi-phase navigation frameworks decompose complex path planning tasks into manageable subproblems, including global planning and local refinement. Multi-scale navigation systems have been proposed for semi-structured or partially known environments [34]. Two-stage planners for reconfigurable tracked robots combine global search with detailed local optimization [35]. Shen et al. [36] introduced a three-phase algorithm including A*, RRT, and local trajectory refining to generate trajectories satisfying nonholonomic constraints on 3D terrain. PRM has also been extended to uneven terrain due to its flexibility in incorporating terrain factors during roadmap construction and graph search. Variants of PRM have been developed for long-distance uneven terrain navigation [37], graph search using quantified soil mechanics [38], personalized driving style path planning [39], and semantic belief path planning [40].

Despite these advances, PRM-based algorithms share a critical limitation in path planning on uneven terrain: the sampling process of roadmap construction strategies is point-based, and the edges are typically validated only via collision checking without kinematic constraints. These limitations result in ignoring the vehicle’s actual motion limits, resulting in insufficient feasibility of the planned paths.

In summary, the reviewed algorithms have made significant progress in improving terrain safety and enforcing kinematic feasibility. However, no existing algorithms or navigation frameworks jointly incorporate comprehensive factors, including terrain traversability, path smoothness, path length, and rollover avoidance. This motivates the navigation framework proposed in this paper. By combining the construction of a global terrain traversability cost map, a three-phase path planning algorithm including A*, guided RRT, and a novel local re-planning algorithm termed KT-PRM, which integrates comprehensive factors, our navigation framework enables safe and efficient navigation via a final NMPC tracker on uneven terrain.

The rest of this article is structured as follows. Section 3.1 and Section 3.2 describe the construction of the global terrain traversability cost map and the vehicle kinematic model, including the kinematic constraint model for Ackermann-steering vehicles and a neural network for estimating motion cost during path planning. Section 3.3 and Section 3.4 introduce a three-phase path planning algorithm, in which Section 3.3 details A* and guided RRT as the first two phases in path planning. Section 3.4 details the proposed KT-PRM as the third phase in path planning. Experimental results and discussions are presented in Section 4, encompassing both simulated and real-world environments. This paper ends with conclusions and references.

3. Methods

Figure 1 illustrates the proposed navigation framework. An initial point cloud map is constructed offline from LiDAR point cloud data via Simultaneous Localization and Mapping (SLAM). This initial map undergoes segmentation algorithms and terrain traversability cost (TTC) calculation through the traversability processor to generate a global terrain traversability cost (GTTC) map. Subsequently, a three-phase path planning algorithm generates global paths, which are tracked using NMPC. Specifically, the three-phase path planning algorithm comprises the following: (1) the classic A* generates a global obstacle avoidance path, (2) the A*-guided RRT generates an initial global path that reaches the goal while considering vehicle kinematic constraints, and (3) a KT-PRM algorithm re-plans the waypoints of the initial global path that lie within low traversability regions (LTRs). The re-planned waypoints and other waypoints together form the final global path.

3.1. GTTC Map Construction

The map processing workflow is shown in Figure 2. The initial point cloud map constructed via SLAM is segmented into ground point cloud and obstacle point cloud using a region growing segmentation algorithm. A 2D grid map is generated from the ground point cloud, with each grid storing the TTC value corresponding to its center point to form a GTTC map. Obstacles are approximated as rectangles to obtain the corresponding 2D grid location. The resolution of the GTTC map in this paper is 0.2 m.

For each 3D coordinate point $[eqn]$ in the ground point cloud, a square kernel $[eqn]$ is defined as the set of points within a square region centered at $[eqn]$ with side length $[eqn]$ . Using the SVD algorithm, a plane is fitted in $[eqn]$ , and the corresponding unit normal vector is represented as $[eqn]$ . This paper employs four metrics, including slope (s), flatness (f), elevation variation (g), and sparsity ( $[eqn]$ ), to evaluate the terrain traversability. These four metrics are defined as follows:

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$

where k is the unit vector in 3D Euclidean space perpendicular to the xy plane. $[eqn]$ is the local vector from the center point to all points in $[eqn]$ . Flatness measures the degree of terrain undulation, while elevation variation quantifies height variation within a point’s local area. Variables $[eqn]$ and $[eqn]$ represent the minimum and maximum elevations within the coordinate point’s local range, respectively. Sparsity $[eqn]$ denotes the proportion of vacant parts in $[eqn]$ . $[eqn]$ and $[eqn]$ represent the minimum and maximum acceptable vacant ratio, respectively. The presence of concentrated empty parts indicates the existence of pits on the ground surface.

TTC, denoted as $[eqn]$ , is the weighted sum of slope, flatness, elevation variation, and sparsity. A higher TTC indicates poorer traversability at that point. The TTC is calculated as follows:

$$[eqn]$$

where $[eqn]$ , $[eqn]$ , $[eqn]$ and $[eqn]$ represent the corresponding weights for each term. Specifically, different terrain characteristics will lead to different types of vehicle navigation failure risks: high slope ( $[eqn]$ ) increases the risk of vehicle rollover, low flatness ( $[eqn]$ ) increases the risk of bumpy motion and unstable steering, high elevation variation ( $[eqn]$ ) increases the risk of abrupt pitch changes and loss of control, and high sparsity ( $[eqn]$ ) increases the risk of vehicle drop-in and collision. The weight allocation is mainly determined intuitively by observing terrain characteristics in specific environments and analyzing the types of vehicle navigation failures that will be caused and their corresponding failure risk levels, following the main principle that a higher weight is assigned to the terrain characteristic that induces a higher navigation failure risk. Based on this principle, we provide two sets of weight allocations for the simulated environments and real-world environments used in this paper, considering the differences in the distribution of terrain characteristics between these two types of environments. The specific weight parameters are presented in Section 4.1 of the experimental results and discussion.

After obtaining the GTTC map, grid cells with TTC exceeding the set threshold are clustered using Density-Based Spatial Clustering of Applications with Noise (DBSCAN). Each clustered grid area is then evaluated for its two-dimensional range, enclosed within a bounding box. By expanding the bounding box by a certain length in two directions, a 2D distribution of LTRs is obtained.

3.2. Vehicle Kinematic Model Construction

3.2.1. Bicycle Model and Vehicle Motion Representation in Global Coordinates

All motions discussed herein are described in a 2D plane. The Ackermann-steering vehicle can be modeled as a rear-axle-referenced bicycle model. Control input consists of rear-wheel speed v and front-wheel steering angle $[eqn]$ . State variable comprises the world-coordinate-based global position (x, y) and the yaw angle $[eqn]$ . The vehicle wheelbase is denoted as L. The kinematic model is given by the following:

$$[eqn]$$

The establishment of the global coordinate system for global path planning is shown in Figure 3. Within the global coordinate system, node sampling is performed by discretizing the bicycle model over time.

3.2.2. Kinematic Constraints and Computation of Motion Cost

Based on the mechanical limits of the vehicle’s actuators, the control input bounds are set as follows:

$$[eqn]$$

From the kinematic model (6), the maximum variation of yaw angle is proportional to velocity and constrained by the maximum front-wheel steering angle. Considering that large front-wheel steering angles under uneven terrain conditions are detrimental to safe vehicle motion, we have imposed restrictions on the variation of the yaw angle and defined a threshold $[eqn]$ . The range of the variation of the yaw angle at corresponding velocity v is thus defined as follows:

$$[eqn]$$

In the global coordinate system, after discretizing the vehicle motion time to obtain several states, the path-tracking problem is formulated as a discrete-time optimal control problem (OCP), which can be expressed as follows:

$$[eqn]$$

where J is the cost function, x is the state, $[eqn]$ is the reference state, u is the control input, Q is the state weighting matrix, and R is the control weighting matrix. The OCP is solved, subject to the kinematic model (6) and kinematic constraints ((7) and (8)).

The optimal control input obtained from solving the OCP is used to calculate the motion cost. Each state corresponds to a node. The motion cost from the parent node to the child node is calculated as follows:

$$[eqn]$$

in which the first and the second terms penalize the travel distance and the variation of the yaw angle, respectively. The third term regulates the control effort. The parameters $[eqn]$ , $[eqn]$ , and $[eqn]$ are the corresponding weights for each term. In this paper, the weights are set to 0.4, 0.4, and 0.2, respectively. The equal weights of 0.4 ensure balanced penalization of the travel distance and the variation of the yaw angle, while the lower weight of 0.2 on control effort is due to these factors being less critical for path smoothness in off-road uneven terrain.

Since in our developed path planning algorithm, frequent computation of motion cost is required, and solving OCPs via nonlinear programming requires high computational cost, we employ a neural network-based method to directly output motion cost value based on two input parameters, including the vehicle travel distance $[eqn]$ and the variation of yaw angle $[eqn]$ .

The dataset for training the network was created through grid sampling. The range and step size for sampling are shown in Table 1. The ground truth of corresponding motion cost was calculated through solving the OCP in Equation (9) via Interior Point OPTimizer (IPOPT) and applying the optimal control to Equation (10). A dataset of 28,000 samples was created in this study and divided into training, validation, and testing sets at ratios of 70%, 15%, and 15%, respectively. This network featured two hidden fully-connected layers, each containing 100 neurons, with Rectified Linear Unit (ReLU) activation functions between layers.

The average accuracy is used to evaluate the performance of the neural network model, as shown below:

$$[eqn]$$

where N is the total number of samples in the testing dataset, $[eqn]$ is the ground-truth motion cost for the ith data, and $[eqn]$ is the motion cost output by the neural network. The mean accuracy achieved is 99.46%, satisfying the requirement for precise cost estimation. Furthermore, we compared the runtime of the IPOPT-based method against the network-based method on 30,000 newly constructed cases. The time statistics are summarized in Table 2. The neural network-based method reduced the time per motion cost output by approximately 97.69% compared with the IPOPT-based method.

3.3. Initial Global Path Generation

This section covers the first and second phases of the three-phase algorithm, which generate length-suboptimal paths that satisfy vehicle kinematic constraints. This method is primarily divided into two steps. First, an A* algorithm was applied to a binary gridmap representing only obstacle distribution to generate the safe and shortest path from the start to the goal. Then, a guided RRT algorithm was applied to refine the path to satisfy the kinematic constraints. The A*-guided sampling details are as follows (Table 3).

A*-guided sampling proceeds sequentially. The principle is as follows: based on the ratio $[eqn]$ of map resolution between the binary gridmap used in the A* algorithm and the GTTC map, the A*-guided node advances to the next A* waypoint after every $[eqn]$ nodes generated by the guided RRT. The hyperparameters of the sampling process in the A*-guided RRT are configured as shown in Table 3. The A*-guided sampling process is illustrated in Figure 4, with sampled input parameters following these distributions:

$$[eqn]$$ $$[eqn]$$

where $[eqn]$ is the sampled distance, and $[eqn]$ is the sampled variation of the yaw angle. They jointly determine the coordinates of the randomly sampled node on the GTTC map.

Algorithm 1 describes the complete A*-guided RRT algorithm. This algorithm is similar to the RRT proposed by Shen [36], with several modifications made in the functions FindNearestNode and Steer, and a new function CheckinLTR was added to integrate terrain traversability.

Algorithm 1: A*-guided RRT

The FindNearestNode function only calculates Euclidean distance without involving complex kinematic connecting costs. The CheckinLTR function determines whether the node is within the LTRs. The CalculateCost function is removed to skip motion cost calculation between nodes. These simplifications were made to enable rapid path search that satisfies reaching the destination while meeting kinematic constraints, without considering obstacle avoidance in LTRs and motion cost comparisons.

The modified Steer function (Algorithm 2) is to ensure that the vector from $[eqn]$ to $[eqn]$ satisfies kinematic constraints. Additionally, we modified the Steer function to account for terrain factors. When sampling in LTRs, we employ a strategy using half the GTTC map resolution as the sampling distance. This ensures sufficiently dense node generation within LTRs to serve as reference waypoints for local path re-planning.

After these two steps, the A* and guided RRT, an initial global path is generated, which is kinematically feasible and of suboptimal length. This initial path maintains dense waypoints within LTRs while avoiding obstacles in regions outside LTRs.

3.4. Local Path Re-Planning

This section describes our proposed algorithm for local path re-planning, termed KT-PRM, as the third phase of path planning. Based on the initial global path obtained in A*-guided RRT, KT-PRM can re-plan a local path to generate a safe, smooth, and short path that satisfies kinematic constraints. Given the high calculation cost of re-planning all waypoints of the initial global path and the limited benefits of TTC optimization on relatively flat terrain, local re-planning is restricted to waypoints within LTRs. The KT-PRM algorithm includes roadmap construction and graph search.

3.4.1. Roadmap Construction

As the first step in local path re-planning, a roadmap is constructed as a graph comprising all vertices and edges satisfying the kinematic constraints. The roadmap construction procedure is detailed as follows:

Algorithm 2: Modified Steer Function

Let the initial path generated by A*-guided RRT be denoted as $[eqn]$ , which includes waypoints $[eqn]$ , $[eqn]$ ,…, $[eqn]$ . The kth LTR in the GTTC map is denoted as $[eqn]$ . Therefore, the set of waypoints within the LTR $[eqn]$ is denoted as $[eqn]$ , which is defined as follows:

$$[eqn]$$

whereas the set of all waypoints in the high-traversability region (HTR) is denoted as $[eqn]$ .

The inputs to the roadmap construction algorithm include the waypoints $[eqn]$ and the corresponding local TTC map. The start and goal points in the local TTC map are determined from the waypoints $[eqn]$ . The local TTC map is extracted from the GTTC map, enclosing all waypoints in the $[eqn]$ . The cost values stored in the local TTC map undergo normalization processing. The roadmap construction process is illustrated in Figure 5 and detailed in Algorithm 3, which mainly consists of three steps: (1) edge pair sampling (SampleKiEdge function) and internal connections of edge pairs (InternalConnect function); (2) external connections of edge pairs (ExternalConnect function); (3) connections to start/goal (ConnectStart and ConnectGoal function).

Algorithm 3: Roadmap Construction

The first step of roadmap construction is sampling edge pairs and building internal connections, which is depicted in Figure 6. For each point $[eqn]$ sampled on the local TTC map, a function SampleKiEdge is employed to generate two adjacent points on either side of $[eqn]$ . These two points ( $[eqn]$ and $[eqn]$ ) are quickly ranked according to their Manhattan distance to the start point on the local TTC map, while $[eqn]$ , $[eqn]$ , and $[eqn]$ satisfy the following constraints:

$$[eqn]$$

where $[eqn]$ is calculated using Equation (8), and $[eqn]$ is related to the vehicle’s maximum speed. These guarantee the satisfaction of the kinematic constraints.

An edge is formed by connecting two points. Edges $[eqn]$ and $[eqn]$ form an edge pair. The collision with the obstacles is then checked by verifying whether either edge of this edge pair intersects with the obstacle regions on the local TTC map. In addition, to ensure edge pairs are concentrated in areas with higher traversability, a probabilistic elimination process is applied based on the TTC value of each edge pair. The probability for elimination is as follows:

$$[eqn]$$

where $[eqn]$ is the TTC value (Equation (10)) of the corresponding point after normalization on the local TTC map, and $[eqn]$ is the threshold for normalized local TTC value, $[eqn]$ . $[eqn]$ , $[eqn]$ . Set $[eqn]$ to form edge pair sampling favor areas with lower TTC. In our study, we set $[eqn]$ , $[eqn]$ , and $[eqn]$ . If an edge pair is rejected due to the elimination process or collision checking, resampling is performed until N edge pairs are obtained.

When an edge pair is retained, its three points are added to the vertex set V, which already contains the start and goal points, and its two edges are added to the edge set E. The points $[eqn]$ and $[eqn]$ of the edge pair are added to the vertex set $[eqn]$ and $[eqn]$ , respectively, for subsequent external connections.

The second step of roadmap construction is building external connections; this involves connecting the vertices in $[eqn]$ to the vertices in $[eqn]$ . Kinematic feasibility and collision are checked before vertice connection. External connection scenarios are also shown in Figure 6. When an external connection of edge pairs succeeds, the external connecting edge is added to the edge set E.

The third step of roadmap construction is building connections between all edge pairs with the start and the goal; this step is similar to building external connections. The kinematic feasibility of the new connections is verified using vectors from the start’s parent node to the start and from the goal to its child node.

After these three steps, a roadmap $[eqn]$ is constructed, encompassing all vertices and kinematically feasible edges.

3.4.2. Graph Search

As the second step of local path re-planning, graph search is applied to the constructed roadmap $[eqn]$ to obtain the optimal path connecting the start and the goal. While our graph search builds upon the A* algorithm as a heuristic optimal search, existing cost functions are limited to optimizing path length exclusively. We address this limitation by proposing an extended accumulated cost function that integrates terrain factors, path smoothness, rollover risk, and path length into a unified metric. Let $[eqn]$ denote the parent vertex during the graph search process and $[eqn]$ denote the candidate vertex for expansion from $[eqn]$ . When expanding from $[eqn]$ to $[eqn]$ , the accumulated cost $[eqn]$ from the start vertex to $[eqn]$ is calculated as follows:

$$[eqn]$$

where $[eqn]$ , $[eqn]$ , $[eqn]$ , $[eqn]$ are the terrain cost, complete motion cost, rollover cost, and deviation cost from $[eqn]$ to $[eqn]$ , respectively. The parameters $[eqn]$ , $[eqn]$ , $[eqn]$ , and $[eqn]$ are the corresponding weights for each term. In this paper, the weights are set to 0.5, 0.2, 1, and 0.3, respectively. This allocation of weights prioritizes terrain factors, followed by path length and path smoothness. The rollover cost weight is set to 1 because the rollover cost value is either ∞ or 0, ensuring that any non-zero rollover contribution is heavily penalized. Cost terms $[eqn]$ , $[eqn]$ , $[eqn]$ , and $[eqn]$ penalize terrain non-traversability, motion effort, rollover risk, and deviation from the length-suboptimal reference path, respectively, which are detailed as follows:

The terrain cost $[eqn]$ is computed as the sum of the normalized TTC values at $[eqn]$ and $[eqn]$ :

$$[eqn]$$

The motion cost $[eqn]$ represents the complete cost for path smoothness. $[eqn]$ is computed as follows:

$$[eqn]$$

where $[eqn]$ is generated by a neural network, which penalizes travel distance and heading variation and regulates control effort (speed and front-wheel steering angle). The second and third terms penalize vehicle acceleration and approximate curvature of the paths, respectively. Specifically, $[eqn]$ and $[eqn]$ denote the lengths of two adjacent path segments, with $[eqn]$ representing the angle between them. The parameters $[eqn]$ , $[eqn]$ , and $[eqn]$ are the corresponding weights for each term. In this paper, the weights are set to 0.5, 0.15, and 0.6, respectively, achieving a well-balanced contribution of each cost term within the motion cost.

The rollover cost $[eqn]$ is evaluated based on the vehicle’s wheelbase, track width, center-of-gravity (CoG) height, and posture at the corresponding vertex. The vehicle’s posture is derived from the locally fitted plane, assuming simultaneous contact of both front and rear wheels with the plane. The vehicle’s posture model on the fitted plane is illustrated in Figure 7.

According to the vehicle’s posture model in Figure 7, $[eqn]$ is set as follows:

$$[eqn]$$

For the FR-07 Pro Ackermann-steering vehicle, which was used for the experiments in this paper ( $[eqn]$ m, $[eqn]$ m, $[eqn]$ m), this gives the static rollover angle threshold $[eqn]$ . Note that this paper considers only the geometric static rollover threshold, but off-road conditions introduce terrain and tyre deformation as well as transient lateral accelerations, both of which can reduce the actual rollover limit below the static value $[eqn]$ [41,42,43]. Terrain and tyre deformation, as well as transient lateral accelerations, are not explicitly modeled in this paper. To account for this, we adopted 24° as the maximum allowable roll and pitch angle, derived from the static rollover threshold plus a 6° safety margin [44]. A full and accurate dynamic rollover model will be developed in future work.

The deviation cost $[eqn]$ quantifies how far $[eqn]$ and $[eqn]$ deviate from the reference waypoints $[eqn]$ , which is calculated as follows:

$$[eqn]$$

where $[eqn]$ denotes the Euclidean distance from the vertex $[eqn]$ to the jth point of reference waypoints $[eqn]$ on the local TTC map. n is the number of waypoints in $[eqn]$ . $[eqn]$ is the diagonal length of the local TTC map.

The evaluation function $[eqn]$ , integrating the accumulated cost and heuristic estimate, guides vertex expansion during graph search and is calculated as follows:

$$[eqn]$$

where $[eqn]$ denotes the accumulated cost, and $[eqn]$ is the goal vertex on the roadmap. The second term corresponds to the heuristic function, which ensures that the evaluation function incorporates the minimum distance to the goal.

Through the graph search on the roadmap, the re-planned path $[eqn]$ from the start vertex to the goal vertex is obtained. Leveraging our extended accumulated cost function, $[eqn]$ has the minimized total cost, which encompasses TTC, complete motion cost, rollover cost, and deviation cost, thereby yielding a kinematically feasible, safe, smooth, and short path that avoids low-traversability regions. The final global path is obtained by combining all re-planned paths $[eqn]$ with the paths $[eqn]$ located in HTR, which are not re-planned.

4. Experimental Results and Discussion

4.1. Experimental Setup

Experiments were conducted to evaluate the performance of the proposed navigation framework in global path planning and tracking on uneven terrain within both simulated environments and real-world environments. Specifically, in simulated environments, our algorithm for global path planning was compared with the state-of-the-art algorithms, and the effectiveness of the proposed KT-PRM in the three-phase algorithm was demonstrated. In real-world experiments, the proposed navigation framework was compared with other frameworks to verify its safety and efficiency.

4.1.1. Simulated Environments

Two types of simulated environments were constructed in Gazebo: woodland environments and mining site environments. The woodland environment features typical uneven terrain characteristics, with detailed topography shown in Figure 8. The mining site environment similarly contains areas with slopes and high unevenness, but with a higher density distribution. For each type of environment, we prepared five maps with a size of 40 m × 40 m, each with distinct terrain distribution characteristics, yielding a total of ten maps across both environments.

Our algorithm was compared against PUTN [11] and T-Hybrid A* [27] on the aforementioned maps using three metrics for the path planning and three for simulated vehicle execution. The metrics for path planning were angle-over-length (AOL), total path length, and the average TTC of waypoints. The AOL, defined in [45], quantifies path smoothness as the ratio of total heading change to path length. The TTC of a waypoint was calculated for all algorithms using identical slope, flatness, and sparsity formulas with the same weight distribution. The metrics for execution were navigation success rate (defined as the proportion of the vehicle’s uninterrupted executions from start to goal out of total trials), average roll angle, and average pitch angle (sampled at 0.5 s intervals). The simulation experiment parameters are summarized in Table 4. The simulation experiments were executed on an Ubuntu 20.04 system equipped with an i7-13650HX processor and 24 GB RAM.

4.1.2. Real-World Environments

Three testing environments were used for evaluating our navigation framework: a 30 m × 30 m woodland area with concentrated mounds, rocks, and trunk obstacles; a 50 m × 50 m uneven grassland with small slopes and dense undulations; and a 50 m × 20 m arch bridge setting spanning an impassable pond. The vehicle was required to avoid obstacles while considering terrain factors in the woodland, assess terrain traversability for efficient path planning in the grassland, and execute safe crossing maneuvers between flat regions in the bridge environment.

In the real-world experiments, we used a ground vehicle with Ackermann steering (FR-07 Pro, YUHESEN, China) as a testing platform (Figure 9). It featured a LiDAR sensor (Helios 32, RoboSense, China) for 3D terrain mapping. The positioning was obtained using a GNSS/INS module. The proposed navigation framework was deployed on the DTB-3079-C246 vehicle-mounted industrial personal computer (IPC) with an Intel Core i7-9700 processor. The real-world experiment parameters are summarized in Table 5.

The 3D maps for path planning were constructed using the A-LOAM [46] algorithm in both simulated and real-world environments.

4.2. Simulation Environment Experiments

4.2.1. Performance Comparison of Path Planning Algorithms on Uneven Terrain

For each simulated map, ten path planning experiments were conducted with the start and goal points strategically positioned at map edges. A total of 100 path planning experiments were conducted among our algorithm, PUTN, and T-Hybrid A*. The distance from start to goal was constrained to 35–45 m.

As shown in Figure 10, the paths generated by the PUTN algorithm mostly traversed steep slope areas and exhibited jagged paths, while T-Hybrid A* detoured around steep terrain, yielding longer trajectories. In contrast, our algorithm consistently produced safe and efficient paths across all scenarios by integrating kinematic constraints, terrain factors, path smoothness, rollover risk, and path length.

The quantitative results of path planning in Figure 11 corroborate these qualitative findings. The AOL of our algorithm and the average TTC of waypoints were 53.9% and 61.8% lower than PUTN, respectively. This improvement stems from the graph-search-based re-planning method within LTRs, whereas the sampling of PUTN terminated upon reaching the goal, leaving some paths unoptimized. In addition, compared with PUTN, the planned path length of our algorithm was decreased by 7.5%, reflecting both the advantages of A*-guided RRT for global path planning and the consideration of deviation cost in KT-PRM for local path planning. The T-Hybrid A* algorithm generated significant detours that increased path length by 248% and required 6× longer planning time than our algorithm. This is mainly because T-Hybrid A* evaluates its defined terrain traversability cost globally to produce minimally traversable-cost paths, but suffers from low efficiency and excessive conservativeness on maps. Overall, our algorithm efficiently produces paths with low heading variation between adjacent path segments and low TTC.

After path planning with different algorithms, a simulated vehicle modeled from our FR-07 Pro vehicle (Figure 9) was executed to track the planned paths using an NMPC controller, with vehicle roll and pitch angles recorded at 0.5 s intervals. The statistical results are shown in Figure 12. Tracking paths generated by our algorithm and T-Hybrid A* both achieved 100% success, while tracking paths generated by PUTN caused several rollovers (recorded as failures) due to the high TTC of partial waypoints. Comparing with PUTN, paths generated by our algorithm reduced the average roll and pitch angles by 42.3% and 67.8%, respectively, confirming the advantages in safety of our algorithm. Similarly, T-Hybrid A* achieved relatively stable vehicle execution by planning paths that avoid high TTC regions. However, its low planning efficiency and long path lengths pose practical challenges for vehicle execution.

Overall, through both planning and tracking experiments, our three-phase algorithm, which integrates KT-PRM, demonstrated comprehensive performance through hierarchical planning. The third-phase KT-PRM comprehensively considers kinematic constraints, path smoothness, rollover risk, and path length, obtaining optimal paths via graph search on the constructed roadmap to achieve smooth, safe, and efficient paths. In path planning, our three-phase algorithm reduced AOL and terrain traversability cost compared with PUTN, and it reduced total path length and planning time compared with T-Hybrid A*. In path tracking, our algorithm achieved 100% execution success rate and low roll/pitch angles. These results confirm the excellent performance of our three-phase algorithm.

4.2.2. Validity Testing of the KT-PRM Algorithm

An ablation study was conducted to verify the effectiveness of our KT-RPM algorithm for path re-planning. Based on our proposed three-phase algorithm, four variants were constructed, as summarized in Table 6. AR removes Phase 3 (KT-PRM) to evaluate its primary impact on terrain-aware safety of the planned path. ARPU substitutes Phase 3 with PF-RRT* from PUTN, which employs a sampling-based, incremental tree construction strategy without kinematic constraints. ARPRT replaces Phase 3 with PRM-T, which uses explicit point sampling and connection but considers only TTC during graph search. ARPRC extends PRM-T by augmenting the graph search cost function with motion, rollover, and deviation costs to validate the impact of comprehensive multi-factor optimization on path smoothness, safety, and length. Our algorithm employs the proposed KT-PRM, which utilizes kinematically feasible edge-pair sampling and connection in explicit graph construction, and incorporates comprehensive cost terms compared with these variants.

For each of the ten simulated maps, ten path planning experiments were conducted with the start and goal points strategically positioned at map edges. A total of 100 comparative path planning experiments were conducted between our algorithm and the four variants. Statistical analysis of corresponding metrics for both path planning and execution was conducted.

The experimental results of path planning are shown in Figure 13. The AR algorithm, which considers only kinematic constraints while neglecting terrain factors, achieved the lowest AOL but the highest TTC. ARPU reduced TTC by 18.8% compared to AR through terrain cost integration, yet still exhibited 109.9% and 100.8% higher AOL and TTC values than our algorithm, respectively, due to its lack of kinematic constraints and the additional iterations required for optimization during incremental tree construction. ARPRT achieved a 44.9% lower TTC than ARPU, demonstrating that explicit graph construction with integrated TTC achieves exact optimality, whereas ARPU’s incremental tree construction provides only asymptotic optimization. ARPRC achieved 33% and 4.4% lower AOL and TTC values than ARPRT, respectively, through comprehensive cost terms incorporation, where the motion cost specifically yields smooth and easily traversable paths. With KT-PRM as Phase 3, our algorithm attained the lowest TTC and a sub-optimal AOL, which is only 5.2% higher than the best, but notably 24.5% lower than ARPRC. This superiority stems from KT-PRM’s consideration of deviation cost and edge-pair sampling/connection, which is fully kinematically feasible and produces smaller heading variation throughout the path compared to point sampling/connection methods. All four algorithms achieved path lengths shorter than PUTN and T-Hybrid A*, attributed to the A*-guided RRT method and LTR-confined re-planning strategy.

After path planning with these algorithms, path tracking experiments were conducted subsequently using the simulated vehicle with an NMPC controller. The statistical results are presented in Figure 14. The AR algorithm exhibited high rollover probabilities during tracking due to TTC neglect. ARPU and ARPRT, which lack kinematic constraints and optimize solely for TTC, still experienced rollover with larger roll and pitch angles during tracking, even when the planned paths of ARPRT exhibited low TTC values. Both ARPRC and our algorithm achieved 100% tracking success through motion cost incorporation. However, our algorithm further guarantees kinematic feasibility, robustly ensuring waypoint reachability and enhancing path smoothness compared with ARPRC.

Overall, compared with these four variants (AR, ARPU, ARPRT, and ARPRC) through both planning and tracking experiments, the KT-PRM algorithm achieves comprehensive functionality by planning with integrated consideration of kinematic constraints, TTC, motion cost, rollover cost, and path length, verifying the validity of the KT-PRM algorithm. The KT-PRM algorithm ensures kinematic feasibility through its innovative edge-pair sampling and connection method, guaranteeing reachability to planned waypoints and smoothness of the path. It integrates multiple cost terms to achieve comprehensive path optimization: TTC to steer clear of low-traversability areas, motion cost to further enhance path smoothness, and deviation cost to maintain proximity to the sub-optimal path generated from the A*-guided RRT. Finally, the graph-search-based method of KT-PRM guarantees acquisition of the exact optimal solution with respect to all cost terms. Ablation experiments validated these findings. Compared with other variants, KT-PRM demonstrates superior performance, optimizing terrain-aware metrics (TTC, success rate, and roll/pitch angles) and path smoothness (AOL).

4.3. Real-World Experiments

We benchmarked our navigation framework against the PUTN framework and the T-Hybrid A* framework in woodland, uneven grassland, and arch bridge environments in the real world. All paths were tracked using our vehicle with an identical NMPC controller, with ten trials per environment. The experimental outcomes are presented as follows:

As shown in Figure 15, our framework and the T-Hybrid A* framework consistently achieved safe navigation across all trials. Conversely, the PUTN framework experienced two failures in the woodland environment due to wheel lift-off caused by traversal on a steep slope.

The quantitative results are presented in Table 7. Our navigation framework achieved the optimal total path length across different environments, demonstrating the inherent advantages of the A*-guided planning method. It also exhibited stable and safe execution in real environments without vehicle tilting, wheel suspension, or rollover incidents. Furthermore, our framework attained the lowest average pitch angle in the woodland environment and the lowest average roll angle in the arch bridge environment among all tested algorithms.

5. Conclusions

This paper proposes a navigation framework for uneven terrain. It first integrates terrain slope, flatness, elevation variation, and sparsity to generate a GTTC map. Subsequently, a three-phase path planning algorithm integrates A*, guided RRT, and the KT-PRM local re-planning algorithm, which jointly considers ground vehicle kinematic constraints, terrain factors, path smoothness, and rollover risk. This three-phase combination delivers safe and smooth global short paths over uneven terrain within relatively short planning time.

Experiments were conducted in both simulated and real-world uneven terrain environments. Through comparison with the state-of-the-art path planning algorithms on simulated uneven terrain, our three-phase algorithm integrating KT-PRM demonstrated comprehensive performance in generating safe and smooth short paths via hierarchical planning. Ablation studies further validated that KT-PRM achieves comprehensive path planning functionality compared to other variants, as its innovative edge-pair sampling and connection in roadmap construction guarantee kinematic feasibility while incorporating TTC, motion cost, rollover cost, and deviation cost into the extended accumulated cost function to obtain the optimal total cost on the roadmap. Finally, in real-world environments, compared with the PUTN framework and the T-Hybrid A* framework, our navigation framework achieved 100% safe navigation with the shortest path lengths across all scenarios, confirming its safety and efficiency. Our future work will focus on optimizing the algorithm for large-scale outdoor unstructured environments featuring dense small obstacles and complex topography. Additionally, we will develop a terrain–tyre coupled dynamic rollover model to capture the full interaction between off-road surfaces and vehicle compliance, enabling a more accurate analysis of rollover limits under real-world conditions.