Autonomous Vehicles Traversability Mapping Fusing Semantic–Geometric in Off-Road Navigation

Abstract

This paper proposes an evaluating and mapping methodology of terrain traversability for off-road navigation of autonomous vehicles in unstructured environments. Terrain features are extracted from RGB images and 3D point clouds to create a traversal cost map. The cost map is then employed to plan safe trajectories. Bayesian generalized kernel inference is employed to assess unknown grid attributes due to the sparse raw point cloud data. A Kalman filter also creates density local elevation maps in real time by fusing multiframe information. Consequently, the terrain semantic mapping procedure considers the uncertainty of semantic segmentation and the impact of sensor noise. A Bayesian filter is used to update the surface semantic information in a probabilistic manner. Ultimately, the elevation map is utilized to extract geometric characteristics, which are then integrated with the probabilistic semantic map. This combined map is then used in conjunction with the extended motion primitive planner to plan the most effective trajectory. The experimental results demonstrate that the autonomous vehicles obtain a success rate enhancement ranging from 4.4% to 13.6% and a decrease in trajectory roughness ranging from 5.1% to 35.8% when compared with the most developed outdoor navigation algorithms. Additionally, the autonomous vehicles maintain a terrain surface selection accuracy of over 85% during the navigation process.

1. Introduction

Autonomous vehicles are extensively employed for outdoor applications such as agriculture, mining, delivery, and exploration. These autonomous vehicles need to possess the capacity to traverse over various challenging terrains, including grass, dirt, potholes, and stones. As illustrated in Figure 1, the varied and uneven landscapes pose significant challenges for autonomous vehicles in detecting safe and traversable areas, thus impacting autonomous navigation capabilities.

Geometry-based methods, which involve creating 3D occupancy maps [1] or 2.5D elevation maps [2], are standard for assessing terrain traversability. These methods identify geometric features like slope and roughness, as discussed in [3,4]. On the other hand, appearance-based algorithms commonly employ image processing [5] or semantic segmentation techniques [6,7] to classify terrain. Conventional approaches involve manually extracting features from images for classifier training, such as SVM [8]. Advanced terrain classification now utilizes semantic segmentation networks [9,10], which automatically extract features, reducing the need for specialized knowledge and effectively identifying various terrain types. Both appearance-based and geometric feature-based techniques have constraints in offering a thorough understanding of the environment for autonomous off-road navigation [11]. Appearance-based approaches confront difficulties in detecting ground surface fallouts because the features fail to fully capture elevation changes [12], which causes a substantial risk to the stability and mobility of autonomous vehicles. However, because of their incapacity to take into account the physical characteristics of the environment, geometric feature-based techniques fail to effectively identify hidden threats like muddy places and deceptive impediments like thick grass [13]. These challenges may impact the autonomous vehicles’s movement efficiency and could lead to the autonomous vehicles becoming immobilized.

This paper presents a methodology that combines semantic and geometrical features to address the constraints of terrain techniques focused solely on geometry or appearance. Bayesian generalized kernel (BGK) inference is used for sparse LiDAR data, and Kalman filtering is used to improve the stability of detection from continuous frames. Accounting for segmentation uncertainty, semantic information is updated probabilistically. The semantic cost map, which takes into account both the semantic types and physical constraints of the autonomous vehicles, provides an accurate and reliable evaluation of traversability. This method efficiently integrates geometric and semantic information to develop an accurate indication of the traversability of the terrain.

Some of the aspects to which our research has contributed are as follows:

(1) The proposed approach provides a real-time assessment of the navigability of challenging off-road terrains. It utilizes a combination of geometric and semantic characteristics to mitigate the impact of movement uncertainty. The method improves temporal reliability by consolidating observations across frames to reduce noise. Spatially, it utilizes an elevation model to improve the reliability of radar data, resulting in a more accurate evaluation of the terrain.

(2) An algorithm is suggested for autonomous off-road navigation that guarantees dynamic feasibility and produces trajectories without collisions. The cost of traversing various terrain surfaces and collision risk are both taken into account by the proposed methodology.

(3) This research presents a comprehensive off-road navigation evaluation metric and uses two thoroughly designed experimental scenarios to compare the proposed method to the current literature. Rate of success, trajectory smoothness, trajectory length, selection of trajectories, and average velocity are some of the metrics being evaluated. The proposed method is obviously superior, as shown by the comparison.

2. Related Work

2.1. Traversability Assessment

Traditional image processing for terrain classification extracts features like color and texture, often limited to specific scenes and lacking adaptability. Deep learning’s emergence has popularized semantic segmentation for terrain analysis, enabling more versatile classification [9,10]. Parker et al. [14] enhance this by using Bayesian inference to assess terrain friction post segmentation. However, the challenge of semantic segmentation inaccuracies arising from insufficient training data can cause the path planner to devise unsafe or impractical trajectories, thereby heightening the potential for collisions.

Ronneberge et al. [15] present a U-Net network that efficiently utilizes limited annotated samples through data augmentation strategies to effectively improve the performance of semantic segmentation. Alshawi et al. [16] enhance this work by introducing deeply separable convolutions and multiscale filters to improve the training speed of this model and reduce the risk of overfitting. However, due to factors such as sensor noise and poor lighting, the labeled results can be affected. Some approaches correlate interaction data (torque [17], vibrations [18]) with visual features for self-supervised learning of surface traits like bumpiness and traction. However, dependence on traversed-area data creates uncertainty in assessing navigability, potentially undermining navigation reliability due to the impracticality of collecting data from inaccessible regions.

Some current state-of-the-art terrain classification methods are built on deep semantic segmentation networks, such as Segformer [6] and PSPNet [7]. Segformer achieves efficient and robust semantic segmentation by introducing a hierarchical transformer encoder and a lightweight full MLP (Multi-Layer Perceptron) decoder. PSPNet, on the other hand, significantly improves the performance of semantic segmentation in complex scenarios by using a pyramid pooling module and a deep supervised optimization strategy. However, these terrain segmentation methods may also return inconsistent labels under changes in viewpoints or illumination.

Geometry-based Traversability Assessment uses LiDAR, or depth cameras, to capture 3D point clouds and depth data to discern terrain features and obstacles. Studies such as those in [3,4,19] have laid the foundation of geometry-based methods. Krusi et al. [20] determine roughness via plane fitting on point clouds. However, the accuracy of planning using this method is influenced by the density of the point cloud of the environment. Ahtiainen et al. [3] extend a Gaussian model with features like roughness and slope and apply an SVM to evaluate traversability. Differently, Dixit et al. [21] present a method for terrain risk mapping by extracting CVaR from point clouds, providing a data-driven approach to traversability. Chavez-Garcia et al. [22] simulate autonomous vehicles to deduce traversability, and Wellhausen et al. [23] extend this to legged robots, evaluating energy expenditure and traversal risk with simulation data. However, it is limited in identifying the physical properties of the terrain, including friction, support, terrain type, and so forth.

Recent advances in deep learning have been applied to semantic segmentation of 3D point cloud data. Bozkurt et al. [24] extended the PointNet++ [25] algorithm for UAV systems by incorporating color information and employing a hierarchical approach to progressively learn local features, which significantly improved the accuracy of semantic segmentation. Regarding the data augmentation strategies from [15,16], the paper proposes a methodology that fuses semantic and geometrical elements to overcome the limitations of these terrain traversability assessment methods, which solely consider geometry or appearance.

2.2. Off-Road Autonomous Navigation

Early outdoor navigation relied on binary classification for obstacle delineation and used potential fields for collision avoidance. Modern methods focus on geometry-based and appearance-based analysis to evaluate the cost of traversing a surface, enabling safe and efficient trajectory planning. Our approach builds on related work such as [11,26] by integrating geometric and visual features for improved unstructured terrain characterization. However, these methods often depend on single-frame segmentation, which can be inconsistent due to different viewpoints and lighting. In addition, sparse point cloud data can hinder the creation of a coherent map.

Several approaches have been developed for assessing the navigability of uneven terrain with the progress of deep learning. Imitative learning, as shown in [27], uses sensor inputs and environmental data for evaluation. Demonstrative learning, as shown in [28], learns navigation strategies from observations. In addition, Deep Reinforcement Learning (DRL), discussed in [29], pursues end-to-end navigation by linking critical events to autonomous vehicle actions. However, learning-based methods still face challenges in consistently achieving optimal navigation metrics such as minimum cost, shortest paths, and dynamic feasibility.

3. Method and Implementation

Our autonomous navigation method, as illustrated in Figure 2, uses traversability map data to estimate terrain navigability and plan safe and efficient paths. It begins with semantic segmentation for terrain classification, integrating this with point clouds for semantic mapping that extracts insights and generates local elevation maps through BGK inference. This method is employed to generate geometric traversability maps and enables a path planning module to generate global paths. The paths will serve as the inputs for the path-following module implemented by the model predictive controller (MPC). The controller is utilized to generate control commands for the autonomous vehicle to accurately track its path in real-time.

3.1. Traversability Semantic Mapping

Semantic Segmentation: Semantic segmentation is a procedure that categorizes terrain by assigning a probability of category label to each distinct pixel in an image. Each of the images is represented as $I\in {\mathbb{R}}^{W\times H\times 3}$, where W and H denote the width and height of the image, respectively. Each pixel on image I is evaluated by the semantic segmentation network to determine the confidence that it belongs to category i, where $i\in \{1,2,\dots ,K\}$. The confidence is then normalized to the category label probability using a regularization function. Eventually, the network generates a K-dimensional categorical distribution that denotes the label probability of pixel-wise terrain category $S\in {\mathbb{R}}^{W\times H\times K}$ of K terrain types. The semantic label of the pixel is determined by selecting the category that has the highest label probability value.

Recursive Semantic Mapping: Terrain semantic traversability is assessed by fusing images with LiDAR data to generate semantic point clouds, essential for mapping. The extrinsic calibration process establishes the transformation matrix linking the camera and LiDAR, enabling the projection of semantic data onto the point cloud and creating the enriched semantic point cloud.

The Bayesian filter updates the occupancy probabilities and labels for 3D grids. It considers sensor observations from the start to time t as $o_{1:t}$, allowing the calculation of each grid’s occupancy probability $p(x\mid o_{1:t})$.

$$\begin{array}{cc}\hfill & p\left(x\mid o_{1:t}\right)=\hfill \\ \hfill & {\left[1+{\displaystyle \frac{1-p\left(x\mid o_t\right)}{p\left(x\mid o_t\right)}}{\displaystyle \frac{1-p\left(x\mid o_{1:t-1}\right)}{p\left(x\mid o_{1:t-1}\right)}}{\displaystyle \frac{p\left(x\right)}{1-p\left(x\right)}}\right]}^{-1},\hfill \end{array}$$

(1)

where $p\left(x\mid o_t\right)$ is the observed value at the current moment, $p\left(x\mid o_{1:t-1}\right)$ is the predicted value, and $p\left(x\right)$ is the prior probability, which is usually set to a constant of 0.5.

Semantic segmentation errors can lead to different category observations for each grid over time. To resolve this, multiple observations must be fused to ascertain each grid’s label. Given the current observation $o_t$ and the label sequence $l_{1:t}$, the label probability $p\left(l_{1:t}\right|o_{1:t})$ is updated using the previous posterior $p\left(l_{1:t-1}\right|o_{1:t-1})$ and the current measurement $p\left(l_t\right|o_t)$:

$$\begin{array}{c}p\left(l_{1:t}\mid o_{1:t}\right)=\hfill \\ \left\{\begin{array}{cc}max\left\{p\left(l_{1:t-1}\mid o_{1:t-1}\right),p\left(l_t\mid o_t\right)\right\}\ast \epsilon ,\hfill & \hfill l_{1:t-1}\ne l_{1:t}\\ {\displaystyle \frac{p\left(l_{1:t-1}\mid o_{1:t-1}\right)+p\left(l_t\mid o_t\right)}{2}},\hfill & \hfill \mathrm{otherwise}\end{array}\right.,\hfill \end{array}$$

(2)

where the penalty factor $\epsilon \in (0,1)$ ensures label consistency. If the current measurement conflicts with the previous posterior, the more probable label is kept, with its probability reduced by $\epsilon$ as a penalty for the discrepancy, allowing semantic updates even with high probability labels.

For the category label, it is updated based on the prior probability and the currently observed label probability:

$$l_{1:t}=\left\{\begin{array}{cc}{l}_{1:t-1},\hfill & p\left(l_{1:t-1}\mid o_{1:t-1}\right)\ge p\left(l_t\mid o_t\right)\hfill \\ l_t,\hfill & \mathrm{otherwise}\hfill \end{array}\right..$$

(3)

Semantic Cost Projection: A semantic cost map is created by projecting the 3D semantic map using voxel semantic labels and heights. The projection is skipped for voxels with height h greater than the autonomous vehicle height $h_r$. For eligible voxels, the highest one $v_{i,j}^n$, indexed by its $xy$ coordinates $(i,j)$, is selected for projection, as depicted in Figure 3.

3.2. Traversability Geometric Assessment

Single-frame Point Cloud Processing: The reliability and accuracy of the resulting elevation maps can be affected by the sparseness of the initial point cloud map.

The integrity and precision of the elevation map are affected by the sparsity of the original point cloud. The point cloud data is first aligned to the ground using IMU pitch and roll angles for rotation compensation. The data are then downsampled into a grid map, with each cell representing elevation z, traversal cost c, and occupancy $occ$. The elevation for cell ${\mathbf{x}}_i$ is modeled as a normal distribution $\mathcal{E}({\mu }_i,{\sigma }_i^2)$ from the observed heights of the $n_i$ points within the cell.

$$\left\{\begin{array}{c}{\mu }_i={\displaystyle \frac{1}{n_i}}\sum _{j=1}^{n_i}{z}_{i,j},\hfill \\ {\sigma }_i^2={\displaystyle \frac{1}{n_i}}\sum _{j=1}^{n_i}{z_{i,j}}^2-{{\mu }_i}^2,\hfill \end{array}\right.$$

(4)

where ${\mu }_i$ and ${\sigma }_i^2$ represent the mean and variance of all observed heights, respectively.

BGK Inference-based Elevation Prediction: To enhance the completeness of terrain modeling, BGK inference, as described in reference [30], predicts elevation in unobserved regions. The inputs to BGK inference are the observed free terrain cells, denoted by the vector $\mathit{B}={\left\{\left({\mathbf{x}}_i,{\mathcal{E}}_i\right)\right\}}_{i=1:N_B}$, where ${\mathcal{E}}_i$ represents the elevation distribution of cell ${\mathbf{x}}_i$ and $N_B$ is the number of cells in $\mathit{B}$. The purpose of the BGK inference is to assess the elevation distribution ${\mathcal{E}}_{*}$ of the unknown cell ${\mathbf{x}}_{*}$ based on the input set $\mathit{B}$, which can be expressed as follows:

$$p\left({\mathcal{E}}_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)\propto \int p\left({\mathcal{E}}_{*}\mid {\theta }_{*}\right)p\left({\theta }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)d{\theta }_{*},$$

(5)

where $p\left({\theta }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)$ is the posterior probability of the latent parameter associated with ${\mathbf{x}}_{*}$, which can be inferred as

$$p\left({\theta }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)\propto \int \prod _{i=1}^{N_B}p\left({\mathcal{E}}_i\mid {\theta }_i\right)p\left({\theta }_B,{\theta }_{*}\mid {\mathbf{x}}_B,{\mathbf{x}}_{*}\right)d{\theta }_B,$$

(6)

where ${\mathbf{x}}_B={\mathbf{x}}_{1:N_B}$. ${\theta }_B$ and ${\theta }_{*}$ are the latent parameters associated with the input set $\mathit{B}$ and the target set ${\mathbf{x}}_{*}$, respectively. ${\theta }_B$ and ${\theta }_{*}$ are assumed to be conditionally independent. According to the smooth extended likelihood model proposed in [31], the posterior probability $p\left({\theta }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)$ can be further represented as

$$p\left({\theta }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)\propto \left[\prod _{i=1}^{N_B}p{\left({\mathcal{E}}_i\mid {\theta }_{*}\right)}^{k\left({\mathbf{x}}_i,{\mathbf{x}}_{*}\right)}\right]p\left({\theta }_{*}\mid {\mathbf{x}}_{*}\right),$$

(7)

where $k\left(·,·\right)$ is a kernel function.

The variance information ${\sigma }_i^2$ of each grid cell ${\mathbf{x}}_{*}$ is introduced as a weight in BGK inference. A larger variance means greater measurement uncertainty. Therefore, grid cells with a higher variance are expected to have a lower contribution to the elevation prediction process. It is assumed that the likelihood distribution $p\left({\mathcal{E}}_i\mid {\theta }_{*}\right)$ satisfies $\mathcal{E}\left({\mu }_i,{\sigma }_i^2\right)$, and the prior distribution $p\left({\theta }_{*}\mid {\mathbf{x}}_{*}\right)$ satisfies $\mathcal{E}\left({\mu }_0,{\sigma }_0^2\right)$. Hence, Equation (7) can be further transformed to

$$\begin{array}{cc}\hfill & p\left({\mu }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)\propto \hfill \\ \hfill & \prod _{i=1}^{N_C}exp\left\{-{\displaystyle \frac{{\left({\mu }_i-\mu \right)}^2}{2{\sigma }_i^2}}k\left({\mathbf{x}}_i,{\mathbf{x}}_{*}\right)\right\}exp\left\{{\displaystyle \frac{-{\left(\mu -{\mu }_0\right)}^2}{2{\sigma }_0^2}}\right\}.\hfill \end{array}$$

(8)

The mean and variance of the posterior distribution $p\left({\mu }_{*}\mid {\mathbf{x}}_{*},\mathit{B}\right)$ can be computed as

$$\left\{\begin{array}{c}{\mu }_{*}={\displaystyle \frac{\frac{{\mu }_0}{{\Sigma }_0}+{\sum }_{i=1}^{N_o}\frac{{\mu }_i}{{\Sigma }_i}k\left({\mathbf{x}}_i,{\mathbf{x}}_{*}\right)}{\frac{1}{{\Sigma }_0}+{\sum }_{i=1}^{N_o}\frac{1}{{\Sigma }_i}k\left({\mathbf{x}}_i,{\mathbf{x}}_{*}\right)}},\hfill \\ {\sigma }_{*}^2={\displaystyle \frac{1}{\frac{1}{{\Sigma }_0}+{\sum }_{i=1}^{N_o}\frac{1}{{\Sigma }_i}k\left({\mathbf{x}}_i,{\mathbf{x}}_{*}\right)}},\hfill \end{array}\right.$$

(9)

where ${\Sigma }_0={\sigma }_0^2$ and ${\Sigma }_i={\sigma }_i^2$. For unobserved grid cells, the mean ${\mu }_0$ is set to 0, and the variance ${\sigma }_0^2$ is set to infinity. To limit the search neighborhood, the sparse kernel proposed in [30] is selected:

$$\begin{array}{c}k\left({\mathbf{x}}_i,{\mathbf{x}}_{*}\right)=\hfill \\ \left\{\begin{array}{cc}{\displaystyle \frac{2+cos\left(2\pi \frac{L_i}{\varphi }\right)}{3}}\left(1-{\displaystyle \frac{L_i}{\varphi }}\right)+{\displaystyle \frac{sin\left(2\pi \frac{L_i}{\varphi }\right)}{2\pi }},\hfill & \hfill L_i\le \varphi \\ 0,\hfill & \hfill L_i>\varphi \end{array}\right.,\hfill \end{array}$$

(10)

where $L_i={∥{\mathbf{x}}_i-{\mathbf{x}}_{*}∥}_2$, and $\varphi$ is the support range of the kernel.

In each estimation round, observed cells within radius r constitute the training data O. The height $z\ast$ at location $x\ast$ is estimated using Equation (9), with all cells in range contributing to the calculation. The estimation is weighted by the distance $d_i$ from each observed cell to the unknown cell, where closer cells have more influence due to the sparse kernel.

Elevation Map Computing: In order to enhance the accuracy and reliability of the estimated elevation outcomes, a Kalman filter is utilized to integrate elevation data from a multitude of frames. Initially, a local grid map of dimensions $L\times L$ is established, where each grid cell records the posterior mean $z_{t-1}$ and variance $v_{t-1}$ of the ground height from the preceding filtering stage. With the current height measurement ${\overline{z}}_t$ for each cell, the updated posterior height $z_t$ is calculated as follows:

$$\left\{\begin{array}{c}{z}_t={\widehat{z}}_t+k_t\left({\overline{z}}_t-c_t{\widehat{z}}_t\right),\hfill \\ v_t=\left(1-k_t{c}_t\right){\widehat{v}}_t,\hfill \\ k_t={\displaystyle \frac{{\widehat{v}}_t{c}_t}{c_t^2{\widehat{v}}_t+q_t}},\hfill \\ {\widehat{z}}_t=a{z}_{t-1},\hfill \\ {\widehat{v}}_t=a^2{v}_{t-1}+{\epsilon }_t,\hfill \end{array}\right.$$

(11)

where ${\widehat{z}}_t$ is the predicted height at the current moment. $k_t$ is the filtering gain. a is the parameter of the prediction model, and $c_t$ is the observation coefficient. ${\epsilon }_t$ and $q_t$ denote the variance of process noise and measurement noise, respectively. $q_t$ is determined by the distance between the cell and the autonomous vehicle and the number of times the cell is observed.

Geometric Traversal Cost Calculation: Once a dense elevation map is created, each grid’s geometric traversal score is calculated by assessing the terrain’s slope and step height. Local plane fitting is followed by PCA to derive the slope s, as in [32]. The largest height divergence between a grid and its $K^{\prime }$ closest neighbors is identified as the step height h. The thresholds $(s_{cri},h_{cri},s_{safe},h_{safe})$ measure safe and dangerous conditions based on the autonomous vehicle’s ability to avoid obstacles and minimize calculations on flat surfaces. The traversability score $T_{geo}$ regarding each cell can be determined by using the formula as follows:

$$\begin{array}{c}\hfill T_{geo}=\left\{\begin{array}{c}0,s>s_{cri}\mathrm{or}h>h_{cri}\hfill \\ 1,s<s_{safe}\mathrm{and}h<h_{safe}\hfill \\ max\left\{1-\left({\alpha }_1{\displaystyle \frac{s}{s_{cri}}}+{\alpha }_2{\displaystyle \frac{h}{h_{cri}}}\right),0\right\},\mathrm{otherwise}\hfill \end{array}\right.,\end{array}$$

(12)

where the weights ${\alpha }_1$ and ${\alpha }_2$ sum up to 1, and $T_{geo}\in [0,1]$. Larger values indicate a smoother surface. The traversability score will be set to zero in the event that any attribute exceeds the threshold, indicating that the cell is not traversable.

3.3. Traversability Mapping

The proposed strategy fusing geometry–semantic assigns a continuous traversability score $T_{fuse}$ in the range $[0,1]$ to evaluate terrain navigability by combining geometric and semantic data. Regions with lower travel costs, favoring flat and smooth terrain, are scored higher to ensure off-road autonomous navigation safety. The overall traversability score for each grid can be computed by combining the semantic traversability coefficient $k_{st}$ and the geometric traversability $T_{geo}$ as

$$T_{fuse}=\left\{\begin{array}{cc}0,\hfill & k_{st}=0\hfill \\ k_{st}\ast T_{geo},\hfill & 0<k_{st}\le 1\mathrm{and}{T}_{geo}>0\hfill \\ T_{geo},\hfill & \mathrm{otherwise}\hfill \end{array}.\right.$$

(13)

3.4. Motion Planning

Following the acquisition of the local traversal cost map, the subsequent task is trajectory planning for navigating the autonomous vehicle to the target. Our navigation method integrates three components: global planning, trajectory library creation, and real-time motion optimization. Initially, informed RRT* (Rapidly Exploring Random Trees) [33] is an efficient sampling-based path planning algorithm that leverages heuristic information to guide its search towards the goal, and generates a global path using the traversal cost map, identifying a local target. Next, the autonomous vehicle’s control space is sampled to simulate and discretize trajectories, compiling a motion primitive library. Finally, the optimal trajectory is selected through an objective function from the library in real time.

Trajectory Library Generation: The set including all possible velocities of the autonomous vehicle is constructed as the search space $V_s=\left\{(v,\omega )|v\in \left[0,v_{max}\right],\omega \in \left[-{\omega }_{max},\right.\right.$$\left.\left.{\omega }_{max}\right]\right\}$, $v_{max}$ and ${\omega }_{max}$ represent the maximum linear velocity and angular velocity that the autonomous vehicle can reach, respectively. Considering the acceleration limit of the autonomous vehicle within the sampling interval $\Delta t$, a velocity set is constructed as $V_d=\left\{(v,\omega )\right|v\in \left[v_c-a_v\Delta t,v_c+a_v\Delta t\right]\}$, $\omega \in \left[{\omega }_c-a_{\omega }\Delta t,{\omega }_c+a_{\omega }\Delta t\right]\}$. The $\left(v_c,{\omega }_c\right)$ represents the current linear velocity and angular velocity. The $a_v$ and $a_{\omega }$ denote the maximum linear acceleration and angular acceleration, respectively. The control space is constructed as $C=V_s\cap V_d$. Assuming that the forward speed of the autonomous vehicle is $v_c$ during the time from 0 to T, then each trajectory ${\xi }_i$ can be calculated as

$$\left\{\begin{array}{c}{x}_t=x_{t-1}+v_c\cos \left({\theta }_{t-1}\right)\Delta t,\hfill \\ y_t=y_{t-1}+v_c\sin \left({\theta }_{t-1}\right)\Delta t,\hfill \\ {\theta }_t={\theta }_{t-1}+{\omega }_t\Delta t,\hfill \end{array}\right.$$

(14)

where ${\theta }_t$ represents the orientation angle at time t, and $t\in [1,M]$, $M=T/\Delta t$.

The initial method’s trajectory actions—left turn, right turn, and straight ahead—are inadequate for navigating rough terrains and obstacles. To enhance trajectory diversity, paths are expanded using Equation (14), with each path’s end state serving as a new expanded path’s start. This process creates a three-tier trajectory library of motion primitives, which enables the autonomous vehicle to select various collision-free routes from any initial state to the sensor boundary F.

Local Trajectory Optimization: To ensure the optimization of the navigation trajectory length and traversal cost, the optimal trajectory in the current state of the autonomous vehicle needs to be evaluated from the trajectory library. The evaluation function $O_{{\xi }_i}$ for the optimal trajectory is designed to consider the distance constraint and the traversal cost constraint for each trajectory while introducing a local trajectory safety cost indicator.

$$O_{{\xi }_i}={\alpha }_d{f}_d\left({\xi }_i\right)+{\alpha }_c{f}_c\left({\xi }_i\right)+{\alpha }_s{f}_s\left({\xi }_i\right),$$

(15)

where ${\alpha }_d$, ${\alpha }_c$, ${\alpha }_s$ are weight coefficients. The $f_d\left({\xi }_i\right)$ is measured by the Euclidean distance between the endpoint of the primitive path ${\xi }_i$ and the target point. The $f_c\left({\xi }_i\right)$ represents the total traversal cost of the path ${\xi }_i$, which evaluates the difficulty of traversing along the path. The $f_s\left({\xi }_i\right)$ denotes the safety cost, a cost function term that expects the local path to retain a certain distance from the impassable region to maximize the probability of reaching the target position. The $f_c\left({\xi }_i\right)$ and $f_s\left({\xi }_i\right)$ can be calculated as follows:

$$f_c\left({\xi }_i\right)={\displaystyle \frac{1}{N_{{\xi }_i}}}\sum _{x_k\in {\xi }_i}c\left(x_k\right),$$

(16)

$$f_s\left({\xi }_i\right)={\displaystyle \frac{1}{N_{{\xi }_i}}}\sum _{x_k\in {\xi }_i}\sum _{x_j\in R}occ\left(x_j\right),$$

(17)

where $x_k$ is the discrete sampling point of the path ${\xi }_i$, and $N_{{\xi }_i}$ is the total number of sampled path points. The R is a square window centered on $x_k$, which can be set according to the map resolution and the size of the autonomous vehicle. The $c\left(x_k\right)$ and $occ\left(x_j\right)$ represent the traversal cost and occupancy value, respectively.

After eliminating the collision trajectory in the motion trajectory library and selecting the current optimal local trajectory, the MPC-based tracking controller is used to perform trajectory tracking and generate autonomous vehicle motion control instructions to realize the navigation task.

3.5. Path Following

In this paper, a model-based predictive control approach is applied to develop a tracking controller to track the trajectories from the trajectory planning module. The use of predictive control approaches in mobile robotics usually relies on a linearized (or also nonlinear) kinematic model or dynamic model to predict future system states.

The kinematic model of the four-wheeled differential-wheeled vehicle Equation (14) can be simplified and indicated as ${\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_k,u_k)$, ${\mathbf{x}}_{k+1}=(x_k,y_k,{\theta }_k)$, ${\mathbf{u}}_k=(v_c,{\omega }_c)$, and k is the discrete sampling time. The application of model predictive control (MPC) entails the resolution of an optimization problem at successive control intervals. This optimization paradigm aims to ascertain an optimal sequence of control inputs that mitigates the divergence between the projected trajectory and the predefined reference path, thereby ensuring precise vehicular navigation. The optimization problem is formulated as

$$\begin{array}{c}{\displaystyle \underset{\{{\mathbf{x}}_k,{\mathbf{u}}_k\}}{min}}{\displaystyle \sum _{k=0}^{N-1}}\left({∥{\mathbf{x}}_k-{\mathbf{x}}_k^d∥}_{\mathbf{\Lambda }}^2+{∥{\mathbf{u}}_k∥}_{\mathbf{\Phi }}^2\right)\\ \mathrm{s}.\mathrm{t}.{\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k)\\ {\mathbf{x}}_0={\mathbf{x}}_{start}\\ {\mathbf{x}}_k\in \mathcal{X},{\mathbf{u}}_k\in \mathcal{U}\end{array}$$

(18)

where ${\mathbf{x}}_k^d$ denotes the desired state in the kth control step, which is obtained by extracting the optimal trajectory generated by the motion planning module in discrete time. ${\parallel \mathbf{x}\parallel }_{\mathbf{\Lambda }}:={\displaystyle \frac{1}{2}}\sqrt{{\mathbf{x}}^T\mathbf{\Lambda }\mathbf{x}}$, ${\parallel \mathbf{x}\parallel }_{\mathbf{\Phi }}:={\displaystyle \frac{1}{2}}\sqrt{{\mathbf{x}}^T\mathbf{\Phi }\mathbf{x}}$. The two positive definite matrices $\mathbf{\Lambda }$ and $\mathbf{\Phi }$ are, respectively, positive definite matrices that serve as weighting parameters in the optimization procedure. The vehicle initial state is denoted as ${\mathbf{x}}_{start}$, and the state set and the input set are respectively denoted as $\mathcal{X},\mathcal{U}$. By solving the optimization problem, the optimal control action to be implemented by the wheeled vehicle during the current control horizon can be determined, which enables accurate trajectory tracking of the vehicle.

4. Experimental Setup

The proposed framework is implemented on a Scout-2.0 UGV [34] as an experimental platform. The platform is equipped with a Robosense RS-16 LiDAR (made by RoboSense Technology Co., Ltd., Shenzhen, China) and an Intel Realsense D435 camera (Intel Corporation, Santa Clara, CA, USA). The output frequency of images and point clouds is 10 Hz. FAST-LIO2 [35] is implemented using point clouds and IMU measures for pose estimation. The code runs on an onboard computer with an Intel Core i7 CPU and 6GB RTX 2060 GPU made by NVIDIA (Santa Clara, CA, USA).

4.1. Terrain Traversability Evaluation

This section validates the proposed framework’s effectiveness with traversability mapping experiments in unstructured scenarios. The experiments employed the semantic segmentation network, GA-Nav, which incorporates the Group-wise Attention Mechanism (GAM) [36] for semantic segmentation. The traversability evaluation results of the proposed method in this paper are compared in Figure 4 for two an example scenarios. The results demonstrate the benefits of semantic and geometric fusion. In these cases, geometric computation fails to accurately identify certain types of areas (e.g., puddles), whereas the semantic segmentation method appears to identify tall vegetation as an impassable obstacle. Nevertheless, the combined visual and semantic approach proposed in this paper can precisely identify and evaluate both cases.

An occupancy grid map is created using the generated traversability map, to which an occupancy threshold of 0.65 is applied. Afterwards that, trajectory planning is carried out by randomly choosing the starting and ending places based on the occupancy map. These results of the autonomous navigation experiments in three scenarios are depicted in Figure 5. Despite being impassable, the radar blind red area in Scenario 1, which is a 15 cm raised shoulder, is erroneously categorized as passable by geometry-based approaches [30]. The geometric technique mistakenly classifies the blue high grass area in Scenarios 2 and 3, which is passable but obstructs radar detection, as impassable. By comparison, the proposed method precisely detects the red area as impassable in Scenario 1 and the blue areas as passable in Scenarios 2 and 3, as depicted in Figure 5.

4.2. Outdoor Navigation

The performance of the proposed navigation framework is assessed on different terrains around the university campus, including off-road terrains, sidewalks, and asphalt roads. The navigation performance of the proposed approach is compared with planners based on DWA (Dynamic Window Approach) [37], which uses velocity and direction adjustments based on motion capabilities and environment; BGK (Bayesian generalized kernel) [30], focusing on behavior kinematics and geometric planning for navigation; PUTN (Plane-fitting based Uneven Terrain Navigation) [38], constructing a probabilistic roadmap for pathfinding; and RSPMP (Real-time Semantic Perception and Motion Planning) [39], enhancing RRT with predefined motion sequences for efficient exploration. The following metrics are used to assess performance against these planners.

• Success Rate: The proportion of trials in which the autonomous vehicle successfully traveled to its target without failing or colliding with obstacles.
• Trajectory Roughness: The total of all vertical motion gradients encountered by the autonomous vehicle through its trajectory.
• Normalized Trajectory Length: The ratio of the length of the navigation trajectory to the linear distance between the autonomous vehicle and the target point for all successful experiments.
• Trajectories Selection: The ratio of the distance traveled by the autonomous vehicle on the most efficiently traversable surface to the total planned trajectory length.
• Mean Velocity: The autonomous vehicle’s mean velocity while traversing various surfaces along the planned path.

4.3. Comparision and Analysis

The performance of the navigation framework with the proposed approach is evaluated both qualitatively (Figure 6) and quantitatively (

Table 1. Navigation performance of the proposed method compared with other methods in four various scenarios.

Metrics	Method	Scenario 1	Scenario 2	Scenario 3	Scenario 4
Success Rate (%)	DWA	75	70	55	65
	BGK	85	80	50	75
	PUTN	90	90	55	75
	RSPMP	75	70	70	70
	Proposed	95	95	75	80
Traj. Roughness	DWA	0.183	0.234	0.638	0.230
	BGK	0.159	0.276	0.749	0.259
	PUTN	0.139	0.176	0.673	0.217
	RSPMP	0.114	0.138	0.477	0.233
	Proposed	0.087	0.114	0.306	0.206
Norm. Traj. Length	DWA	1.127	1.093	1.139	1.084
	BGK	1.187	1.347	1.287	1.335
	PUTN	1.235	1.184	1.173	1.165
	RSPMP	1.423	1.203	1.409	1.196
	Proposed	1.208	1.147	1.168	1.098
Traj. Selection (%)	DWA	19	23	64	72
	BGK	25	48	71	75
	PUTN	37	63	68	93
	RSPMP	88	83	85	86
	Proposed	97	93	89	90
Mean Velocity	DWA	0.578	0.541	0.516	0.527
	BGK	0.486	0.431	0.449	0.417
	PUTN	0.515	0.497	0.481	0.483
	RSPMP	0.475	0.467	0.443	0.508
	Proposed	0.541	0.553	0.498	0.544

and Figure 7) in four various off-road scenarios. It can be observed that in all scenarios, the proposed approach achieves the highest rate of success. Further, the autonomous vehicle obtains a decrease in trajectory roughness ranging from 5.1% to 35.8%. Additionally, the autonomous vehicle maintains a terrain surface selection accuracy of over 85% during the navigation process. Geometry-based methods such as DWA and BGK are susceptible to being affected by grass height (Scenario 2) or getting stuck in mud (Scenario 3), making it difficult to reach the goal. PUTN, which is based on plane fitting, is less affected by protruding weeds. Nevertheless, it still cannot distinguish the muddy surfaces and is prone to slipping (Scenario 3). In addition, other algorithms tend to follow shorter paths without considering path width, which increases the probability of collision (Scenario 4). The proposed method maximizes the probability of reaching the goal by prioritizing broader and lower-cost paths.

In terms of trajectory length, the proposed method plans a longer trajectory to avoid high-cost areas. However, due to the influence of the $f_d\left({\xi }_i\right)$ term in the local path evaluation function, the normalized length of the trajectory is still close to 1. In some scenarios, DWA and BGK generate shorter paths by directly moving toward the goal without considering surface changes when there are no obstacles.

The proposed method also outperforms other methods in all test scenarios on the metrics of trajectory roughness and trajectory selection, as it plans trajectories as much as possible on smooth and low-cost surfaces. RSPMP is closer to the proposed method for these two metrics, but it plans a steeper path at the endpoint of Scenario 2 due to not considering the surface slope information. BGK and PUTN only use elevation information to compute the cost map, without accounting for the semantic information, thus selecting rougher surfaces for trajectory planning.

With regard to the average velocity, it is evident that the proposed method consistently maintains an even speed for navigation in numerous scenarios. This is achieved by navigating on cost-efficient surfaces and avoiding any obstacles caused by wheel slipping or blocking. Conversely, BGK and PUTN are influenced by vegetation height easily, causing disorientation in their paths. They are also affected by muddy and slippery terrain, causing the autonomous vehicle to skid and reduce overall speed.

The experimental results show that the method proposed in this study has achieved significant improvements in key performance indicators. The success rate of the navigation algorithm was significantly improved from 4.4% to 13.6%, demonstrating its adaptability and effectiveness in complex outdoor environments. Meanwhile, the reduction in trajectory roughness ranged from 5.1% to 35.8%, which optimized the smoothness of navigation. In addition, the autonomous vehicle maintains a terrain surface selection accuracy of more than 85%, ensuring accurate decision making and reliable navigation.

Applying this paper’s methodology to a wide range of scenarios, computational and storage costs are not negligible. Therefore, future research needs to focus on designing more efficient data structures for unified terrain representation to enable terrain perception tasks over larger and more complex environments.

As shown in the attached Video S1 in Supplementary Materials, the proposed method achieves safe and reliable autonomous navigation in off-road terrain environments.

5. Conclusions

In this paper, a real-time terrain traversability method is proposed to generate accurate traversability maps for off-road navigation. It utilizes semantic features and Bayesian generalized kernel inference for dense elevation mapping. An efficient real-time terrain evaluation method is proposed that fuses semantic and geometric data, resulting in accurate traversability maps. The robustness and utility of the algorithm are demonstrated through UGV-based mapping and navigation in real-world scenarios. The focus of future research will be on the integration of deep learning and Bayesian approaches into semantic segmentation networks. This integration aims to more reliably estimate the uncertainty of semantic predictions and to incorporate semantic uncertainty into the mapping process, thereby increasing the reliability and accuracy of the map.