4.1. Sampling space configuration

This paper uses the sampling method in octree space so that a high operation efficiency can be gained in embedded devices. Different from the path planning of drones or robotic arms, it is difficult for us to plan the movement of the vehicle on the z-axis in the path planning task in the rugged ground. However, this kind of movement is critical based on the analysis above. In this paper, we adopted a random sampling strategy to find a feasible front-end path for the ground vehicle in the octomap. The sampling space is set as the section of the initial height of the vehicle in the octomap instead of the entire octree space. The terrain constraints are introduced into the sampling process, including ground search and the TTA. For each sample point, a ray is used by the algorithm to project upward or downward to find the ground. This method greatly reduces the sampling time and avoids the loss of terrain information in 2D planning at the same time.

(1) \begin{equation} \left\{ \begin{array}{*{20}{c}} {Node_{x,y,z} = \Gamma ({p_{x,y}}),if\mathop {}\limits^{} \Gamma ({p_{x,y}})\mathop {}\limits^{} true} \\[5pt] {None = \Gamma ({p_{x,y}}),if\mathop {}\limits^{} \Gamma ({p_{x,y}})\mathop {}\limits^{} false} \\[5pt] \end{array} \right.\end{equation}

where $Node_{x,y,z} $ is the final sampled point. $p_{x,y} $ is the random sampling point on the initial height section of the vehicle, and $ \Gamma $ means the mapping of ground search. If the search is successful, it returns the Node value, otherwise it returns None.

4.2. Path planning method

Because of the sparsity of the octomap, for each sampling point collected by the planner, the local octree nodes in a certain area around it will be analyzed, which can be a sphere, a cube, or a polygonal cylinder. Considering the shape of the vehicle, a cube is adopted as the candidate area. The parameters box(x,y,z) are related to the size of the vehicle. The search set $\Omega$ of the sampling point $ p_{x,y} $ can be written as follows:

(2) \begin{equation} \Omega = \left\{ {\begin{array}{*{20}{c}} {\Gamma \left( {{p_{i,j}}} \right)}\\[5pt] {{{x - box_{x}} \mathord{\left/ {\vphantom {{x - box_{x}} 2}} \right. } 2} - padding \le i \le x + {{box_{x}} \mathord{\left/ {\vphantom {{box_{x}} {2 + padding}}} \right. } {2 + padding}}}\\[5pt] {{{y - box_{y}} \mathord{\left/ {\vphantom {{y - box_{y}} 2}} \right. } 2} - padding \le j \le y + {{box_{y}} \mathord{\left/ {\vphantom {{box_{y}} {2 + padding}}} \right. } {2 + padding}}} \end{array}} \right\}\end{equation}

Terrain reliability analysis: Generally, in the search set $\Omega$ around the sampling point, the more octree nodes with ground elevation information there are, the higher reliability of the sampling point at the ground and the higher accuracy of the ground height calculation are. So the reliability of the terrain is calculated as follows:

(3) \begin{equation} \begin{array}{c} \eta = {{{n_{node}}} \mathord{\left/ {\vphantom {{{n_{node}}} {{n_{\max }}}}} \right. } {{n_{\max }}}}\\[5pt] {n_{\max }} = card(\Omega )\\[5pt] {n_{node}} = card(\left\{ {x\left| {x \in \Omega ,x \ne None} \right.} \right\}) \end{array}\end{equation}

where $ n_{\max} $ is the number of all the nodes in the search set $ \Omega $ , and $ n_{node} $ is the number of nodes with ground elevation information.

Terrain flatness analysis: According to the analysis above, in the path planning task of the vehicle, it is important to analyze the ruggedness of the terrain. We use the elevation variance of the ground nodes to describe the terrain flatness:

(4) \begin{equation} \begin{array}{c} mean = {{\sum {Nod{e_z}} } \mathord{\left/ {\vphantom {{\sum {Node_{z}} } {{n_{node}}}}} \right. } {{n_{node}}}},Node \in \Omega \\[5pt] stddev = {{{{\left\| {Node_{z} - mean} \right\|}_2}} \mathord{\left/ {\vphantom {{{{\left\| {Node_{z} - mean} \right\|}_2}} {({n_{node}} - 1)}}} \right. } {({n_{node}} - 1)}},Node_{z} \in \Omega \mathop {}\limits^{} and\mathop {}\limits^{} Node \ne None \end{array}\end{equation}

Terrain slope analysis: In the calculation of slope, it will take a lot of time to calculate the global gradient directly, as the constructed octomap may be discontinuous, which is determined by the sparsity of the 3D LIDAR. Furthermore, both the 2D and 2.5D methods have the gradient calculation along with the x and y direction of the navigation map, which makes traverse from a certain direction impossible when the negative obstacles exist. This paper calculates the terrain slope between the new node that is added to the path and its parent-node, instead of analyzing the terrain slope of each sampling node. In this way, it will not only reduce the cost of calculation but also provide the possibility of traversing negative obstacles from a certain direction.

(5) \begin{equation} \theta = ac\sin \left(\frac{{\left| {Node_{new_{z}} - Node_{nearst_z}} \right|}} {{{{\left\| {Node_{new} - Node_{nearst}} \right\|}_2}}}\right)\end{equation}

A cost function is designed to evaluate whether a new node can be added to the path and whether the relationship of path node should be changed. In this paper, we can assume that the cost function for each node can be expressed as a weighted linear combination of a set of weights and some terrain feature functions. And it can be defined as:

(6) \begin{equation} c(Node_n) = {\omega _1}\sum\limits_{i = 0}^n {{{\left\| {Node_i - Nod{e_{father}}} \right\|}_2}} + {\omega _2}\sum\limits_{i = 0}^n {{\theta _i}}\end{equation}

Therefore, the steps of the 3D path sampling algorithm in octomap space based on the terrain analysis are shown in Algorithm 1.

4.3. Extension to mapless navigation

The autonomous navigation without a prepared map is crucial. As in most of the off-road unmanned vehicle navigation scenarios, there is no priori map information. In the case of mapless navigation, the terrain information around the navigation target point is unknown, and the map information observed in the early stage is very sparse, which is determined by the sparsity of the LiDAR point cloud and the occlusion relationship of the off-road terrain. To implement the proposed method to mapless navigation tasks, we keep exploring toward the target by selecting suitable local target points among the existing sampling points based on the Algorithm 1. Due to the number of sampling points during the path finder process may be large and only the outermost of them are beneficial for the task of exploration toward the target. Therefore, in order to avoid unnecessary calculations and select suitable local target points, we propose the following approach as shown in Algorithm 2 and Fig. 3.

Figure 3.

Local target point selection.

Outermost sampling points set: we define $ d_{i} $ as the distance between $ i_{th} $ point( $ P_{i} $ ) in $\Omega $ and the start point, and the D as the distance between the closest point to the end point and the start point. The outermost sampling points set can be written as follows:

(7) \begin{equation} \Omega_{outer} = \{ P_{i} |P_{i} \subset \Omega , 0.8 < d_{i}/D < 1 \}\end{equation}

Points clustering: We perform clustering of the outermost sampling points using a method similar to that in ref. [Reference Pérez-Higueras, Jardón, Rodríguez and Balaguer25], which can divide the outermost region into several independent regions. As mentioned in Section 4.2, the sampled points are the points that meet the TTA. Therefore, a region with more sampling points has better traversability, where the candidate local target point should also come from.

The local target point: In order to avoid local target points guiding the vehicle away from the direction of the final target, we finally select three sets in $ \Omega_{cluster} $ with the largest number of sampled points to form the final target candidate points set $ \Theta_{CandiPts} $ . And a cost function is set to select the final local target point in the set of candidate target points. We seek for a point which can minimize the function as shown in the following equation:

(8) \begin{equation} P_{localGoal} = argmin(\Arrowvert P_{i} - P_{goal} \Arrowvert_{2} + c(P_{i})), P_{i} \subset \Theta_{CandiPts}\end{equation}

where $ c(P_{i}) $ is the cost of the point $ P_{i} $ to start point defined in Eq. (6).

5.1 Traversable corridor generation

Based on the waypoints obtained in Section 4, we analyze the traversable state of the area around the waypoints to determine whether there are steep slopes and pits around the sampling point, thereby providing reasonable constraints for trajectory optimization. According to the searched waypoints, we initialize a cube which is based on the size of the vehicle and then expand the cube in the order of X-Y-Z with a fixed step $\varepsilon $ . The expansion scales of ${\varepsilon _x},{\varepsilon _y}$ are equal, but the expansion in the z direction ${\varepsilon _z}$ is far less than the ${\varepsilon _x},{\varepsilon _y}$ . This is because it is primarily designed to eliminate collisions between the vehicle and obstacles on the Z-axis caused by the undulations of the terrain. Then it maximizes the cube by analyzing the collision in Z direction and the corresponding terrain state information in X and Y directions. The TTA method is similar to that in Section 4.2. After that, we reduce the number of traversable corridors by merging overlapping areas to reduce the computational burden of back-end optimization. And the traversable corridor generation is shown in Algorithm 3.

5.2. Trajectory optimization

In this paper, the trajectory will be expressed using a Bernstein polynomial [Reference Gao, Wu, Lin and Shen43], which can be written as follows:

(9) \begin{equation} \begin{array}{c} P_j(t) = \sum\limits_{i = 0}^n {p_{i,j}\cdot{B_{i,n}}(t),t \in [0,1]} \\[5pt] {B_{i,n}}(t) = C_n^j \cdot {t^i} \cdot {(1 - t)^{n - i}} = \frac{{n!}}{{i!\left( {n - i} \right)!}} \cdot {t^i} \cdot {(1 - t)^{n - i}},i = 0,1,2,....n \end{array}\end{equation}

where $p_{i,j}$ is the $ i{\rm th} $ control point of the ${j{\rm th}}$ Bezier curve, and n is the order of Bezier curve. The trajectory is divided into m segments according to the number of corridors in Section 5.1. Since the time period of the Bezier curve is [0, 1], a time scale factor r needs to be set to realize any time allocation of this segment of the curve. And the piecewise Bezier curve can be represented using the following equation:

(10) \begin{equation} {P^\mu }(t) = \left\{ {\begin{array}{*{20}{c}} {{r_1} \cdot \sum\limits_{i = 0}^n {p_{i,1}^\mu B_{i,n}(\frac{{t - {T_0}}}{{{r_1}}}),\ t \in [{T_0},{T_1}]} }\\[5pt] {{r_2} \cdot \sum\limits_{i = 0}^n {p_{i,2}^\mu B_{i,n}(\frac{{t - {T_1}}}{{{r_2}}}),\ t \in [{T_1},{T_2}]} }\\[5pt] \vdots \\[5pt] {{r_m} \cdot \sum\limits_{i = 0}^n {p_{i,m}^\mu B_{i,n}(\frac{{t - {T_{m - 1}}}}{{{r_m}}}),\ t \in [{T_{m - 1}},{T_m}]} } \end{array}} \right.\end{equation}

where ${T_1},{T_2}, \ldots {T_m}$ is the end time of each segment trajectory. $p_{i,m}^\mu$ is the ${i{\rm th}}$ control point of the ${m{\rm th}}$ segment. And $\mu $ is one of the three dimensions of X, Y, Z. ${r_1},{r_2}, \ldots {r_m}$ represents the time scale factor of each segment ( $ r_i = T_i -T_{i-1} ,i=0,1,2,...m $ ), which scales the segment time from $\left[ {{T_{i - 1}},{T_i}} \right]$ to $\left[ {0,1} \right]$ .

Based on the analysis, this paper uses the minimum snap trajectory optimization method, and the cost function of the optimization can be written as:

(11) \begin{equation} C = {\sum\limits_{\mu \in \{ x,y\} } {\int_0^T {\left( {\frac{{{d^4}{P^\mu }(t)}}{{d{t^4}}}} \right)} } ^2}dt\end{equation}

It can be represented as a quadratic matrix of ${P^T}{Q_0}P$ , where $ \textbf{P} $ is the vector composed of all control points, and ${Q_0}$ is the Hessian matrix of the objective function. It can be proved that ${Q_0}$ is a positive semi-definite matrix, so the trajectory optimization problem can be transformed into a convex quadratic programming problem.

(12) \begin{equation} \begin{array}{c} \min {\textbf{P}^T}{Q_0}\textbf{P}\\[5pt] s.t.\mathop {}\limits_{} {A_{eq}}\textbf{P} = {b_{eq}}\\[5pt] {A_{ieq}}\textbf{P} \le {b_{ieq}} \end{array}\end{equation}

Position constraints: The optimized trajectories must pass through some specific points whose positions, velocities, and accelerations are known during the planning process, such as the start point and the goal point. According to the properties of the Bezier curve, the coefficients of the high-order derivative can be composed of the coefficients of the lower-order derivative. Take the Bezier curve in a certain dimension of a certain segment as an example:

(13) \begin{equation} \begin{array}{l} a_i^{\left( 0 \right)} = {p_i}\\[5pt] a_i^{(l)} = \frac{{n!}}{{(n - l)!}} \cdot (a_{i + 1}^{(l - 1)} - a_i^{(l - 1)}) \end{array}\end{equation}

where l is the order of derivative of the Bernstein basis. Therefore, in a certain dimension of a certain segment, the position constraint can be represented as:

(14) \begin{equation} a_i^{(l)} \cdot {r^{(1 - l)}} = {\gamma^{(l)}}\end{equation}

where ${\gamma^{(l)}}$ can be the position, velocity, or acceleration of a specific waypoint.

Continuity constraints: Continuity constraints are mainly required for the connection points of each segment trajectory. In order to ensure the smoothness of the trajectory and the continuity of the movement, it is necessary to ensure that the l-order derivative of the connection point is continuous, where $0 \le l \le k - 1$ . For the ${j{\rm th}}$ and ${(j + 1){\rm th}}$ segment trajectories, the last control point of the ${j{\rm th}}$ segment is continuous with the first control point of the ${(j + 1){\rm th}}$ segment in the $ l{\rm th} $ order derivative.

(15) \begin{equation} \begin{array}{c} a_{n,j}^{(l)} \cdot r_j^{(1 - l)} = a_{0,j + 1}^{(l)} \cdot r_{j + 1}^{(1 - l)}\\[5pt] a_{i,j}^{(0)} = p_{i,j}^{(0)} \end{array}\end{equation}

Corridor constraints: In the unmanned vehicle trajectory optimization, it is necessary to ensure the safety and feasibility of the optimized trajectory. The usual approach is to process the optimized trajectory for trajectory checking and impose additional constraints on waypoints that do not meet the safety or enforceability requirement. The trajectory optimization and trajectory check process are then repeated until all waypoints on the trajectory meet safety and enforceability requirements [Reference Chen, Liu and Shen44]. This paper uses the convex hull property of the Bezier curve to enforce all the control points of the trajectory to be constrained within the boundaries of each direction of the corridor which is mentioned in Section 5.1. For example, for the ${j{\rm th}}$ trajectory:

(16) \begin{equation} \chi _{i,j}^ {\mu-} \le p_{i ,j}^\mu \le \chi _{i,j}^ {\mu+} ,\mu \in \left\{ {x,y,z} \right\},i = 0,1,2...n\end{equation}

where $\chi _{i,j}^ {\mu-} ,\chi _{i,j}^ {\mu+} $ are the upper and lower boundary of the ${j{\rm th}}$ corridor in a certain dimension of x, y, z.

Kinematic constraints: Generally, considering the feasibility of the unmanned vehicle movement, its speed and acceleration need to be constrained within a certain workable interval. For the ${i{\rm th}}$ control point:

Based on the property of the Bezier curve: $a_i^{(l)} = \frac{{n!}}{{(n - l)!}} \cdot (a_{i + 1}^{(l - 1)} - a_i^{(l - 1)})$

(17) \begin{equation} \begin{array}{l} {V_{\min }} \le n({p_{i + 1}} - {p_i}) \le {V_{\max }}\\[5pt] {a_{\min }} \le n \cdot (n - 1) \cdot ({p_i} - 2{p_{i - 1}} + {p_{i - 2}})/{r_j} \le {a_{\max }} \end{array}\end{equation}

where ${V_{\min }},{V_{\max }},{a_{\min }},{a_{\max }}$ are the minimum and maximum speed and acceleration of the vehicle.

6.1. Simulation experimental setup

6.1.1 Simulation environment

First, we present several simulation experiments of the proposed local target selection, path search, and trajectory optimization algorithms using Gazebo. In order to make the simulation more meaningful and simplify the development and debugging process in the real environment, in this paper a URDF unmanned vehicle simulation model is used, whose dynamic performance and motion restrictions are similar to those of real vehicles. A 3D LIDAR and binocular camera are added to the model for context awareness, as is shown in Fig. 4. An off-road environment is built in the software platform UNITY and then imported into Gazebo to gain gravity and physical properties, as is shown in Fig. 5(a). The size of the environment is 200 m $\times$ 200 m, with multiple pits and steep slopes of different diameters distributed, and the maximum undulation of the terrain is about 3 m.

For each segment of the Bezier curve, we must set a series of constraints to ensure the safety and feasibility of the optimized trajectory.

Figure 4.

The simulation UGV and the simulation sensors. In our experiment, a simulation Velodyne VLP-16 and a simulation depth camera whose data format is same as that of the real sensors are used (in order to better represent the pose between the sensors, the connection between them is hidden in the simulation).

Figure 5.

Off-road environment built in gazebo.

6.1.2. Result of the proposed method

Octomap building: The laser slam algorithm is used to build the sparse point cloud map of the constructed off-road scenario and get the six-dimensional pose estimation of the vehicle. In order to reduce the interference of the real field environment on the point cloud and the huge computational cost of navigation directly on the point cloud map, the map representation method of the octomap is adopted in this paper. Point cloud within a certain range centered on the vehicle location will be added to the octomap where the terrain information will be maintained and updated, which allows the vehicle to have lower map maintenance costs when real-time navigation tasks are performed. And we use the color to distinguish the elevation, as is shown in Fig. 5(c). The global map coordinate system is established based on the location of the centroid when the vehicle is started. A warmer color shows a higher elevation of the terrain, indicating a larger value of Z-axis, which can be slopes. Conversely, colder colors refer to lower elevation, which can be pits.

Path finding result: In this paper, we analyze the terrain information effectively based on the octomap, which described in Section 4, and evaluate the front-end path search method using the simulation environment mentioned above. We set the navigation destinations at the bottom of the pit or a relatively flat area and change the boundary threshold parameters to test the efficiency of the path finding algorithm and the feasibility of the path.

By setting the boundary threshold reasonably, the path searched by the front-end pathfinder module performs well in avoiding unknown areas, negative obstacles like pits, and steep slopes. Figure 6 shows the successful front-end path finding process, where the cube is the sampled node. The white line is the sampled path, and the red line is the final searched path. As shown in the picture, rejection area A is an example of avoiding negative obstacles like pits and steep slopes. The red nodes are the accepted nodes during the path search process, and magenta nodes are the rejected nodes which do not match the terrain analysis results during the process. After the terrain analysis, some nodes that cannot be safely reached by the vehicles will be refused to join the path, thereby ensuring the successful avoidance of the dangerous areas such as pits and steep slopes. Rejection area B shows an example of avoiding unknown areas. They are some areas that cannot be observed by some sensors, where there may be pits and occlusions. Considering the safety of the car, the algorithm adopts an evasion strategy.

Figure 6.

Unknown area, negative obstacle rejection.

Figure 7 shows the effect of different boundary thresholds on path search results, while Table I shows the search time. From the results, it can be seen that if the threshold of $ stddev_{th} $ is too small, the path will be difficult to extend and the search time will be greatly increased at the same time, such as $ stddev=0.05 $ in Fig. 7. But when the threshold becomes larger, the path becomes smoother and the search time reduces. The parameter $ stddev_{th} $ is related to the flatness of the terrain, which needs to be set according to the off-road capability of the unmanned vehicle in the real environment.

Figure 7.

Result of different path-expansion boundaries and cost functions.

Table I.

Time of path finder module process ( ${\eta _{th}} = 3$ )

Path optimization result: As is introduced in Section 5, the analysis of the traversable corridor and the trajectory optimization of the back-end are performed in this paper based on the path found by the front-end path finder module. Considering the vehicle’s controller capability and the safety of the navigation, we set an erosion area of the range of six resolution units to constrain the traversable corridor within the absolute safety area during the analysis of the traversable corridor. As is shown in Fig. 8(a), in order to visualize the results, a semi-transparent cube is used to represent the generated traversable corridor. In generating the traversable corridor, we expanded the size on the Z-axis of the corridor, which is relevant to the height of the vehicle. Figure 8(b) and (c) shows the results of the generated traversable corridor to avoid the pits and steep slopes, showing its ability to navigate safely.

Figure 8.

Corridor generation result.

According to Section 5.2, in the process of back-end optimization, constraining the optimized path within the generated passable corridor can not only ensure the safety of the trajectory but also smooth the trajectory and increase the feasibility of it. As is shown in Fig. 9, there are the trajectory generation results for three different sets of starting points, where transparent gray cube is the visualized traversable corridor, the red line is the path searched by the front-end module, and the blue smooth line is the path optimized by the mini-snap method. As is shown in the picture, a smooth trajectory can be generated effectively through the method proposed in this paper, and the trajectory can be constrained within a passable and safe corridor through a single calculation. The comparison of the running time of each part in generating three sets of trajectories is listed in Table II. As is shown in the table, the front-end path finder module and the passable corridor analysis module take approximately the same time, while the back-end trajectory optimization module spends far less time than the first two modules.

Figure 9.

Trajectory generation results.

Table II.

Navigation time distribution

6.2. Comparisons and analysis

In this part, our method is compared with a popular 2.5D navigation map representation method proposed by P. Fankhauser using GridMap library [Reference Fankhauser and Hutter46]. In order to meet the need for real-time computing, only the normal vectors, smoothening, and variance of each cell are considered when performing the TTA in GridMap. Moreover, only the body-centered 20 m $\times$ 20 m size local map is processed and updated to reduce the time of TTA and improve the real-time performance. However, the re-planning process works only when dynamic obstacles or scene changes are located within 10 m of the vehicle. In the navigation, the sample-based path planning in the 2D map and the dynamic window approach are used for effective control and autonomous navigation.

Table IV.

Comparison of re-planning time when encounter dynamic obstacles

6.2.1 Mapless navigation

In this part, we compared the proposed method with the GridMap method in a mapless navigation task, in which we do not provide a pre-generated map model. The unmanned vehicle needs to build a map while exploring toward the target. Twenty trials are conducted using each of the two methods, with a target point on a rugged terrain about 40 m from the starting point, and the maximum speed of the vehicle is set as 1m/s. We compared the average navigation time, trajectory length, re-planning times, and success rate.

Table III shows that our proposed method has better performance in most metrics, including the average navigation time, re-planning times, and success rate. It is worth noting that the average trajectory length of our method is longer than that of the GridMap method. This is because the navigation approach of GridMap is more aggressive, which marking unknown regions as free. However, such aggressiveness also leads to a higher probability of failure in the mapless navigation tasks. In contrast, our approach can significantly improve navigation by selecting appropriate local target points and re-planning. As the instance shown in Fig. 10, in most of the failure-prone regions of the GridMap method shown in Fig. 10(b), our approach can effectively re-plan to avoid the risk, as shown in Fig. 10(a).

Figure 10.

Comparison results with the GridMap method in mapless navigation task.

Table III.

Comparison results with the GridMap method in mapless navigation task

6.2.2. Dynamic obstacle avoidance

Three sets of experiments are used to compare the dynamic performance of the two methods in specific scenarios including avoiding large negative obstacles (experiment A), passing through narrow passable area (experiment B), and navigating to the bottom of a depression (experiment C). The results are shown in Fig. 11.

Figure 11.

Comparison results with the GridMap method in dynamic obstacle avoidance task.

As is shown in Table IV, kinodynamic collision-free trajectories are both generated in scenarios with and without dynamic obstacles in experiments A and B. However, our method is faster in the re-planning process and tends to generate a smoother trajectory. In the experiment C, the GridMap method fails to perform the re-planning process when dynamic obstacles are encountered while navigating to the depression. This is because the sudden detection of dynamic objects makes it difficult to avoid when the vehicle encounters negative obstacles due to the limited view of the sensor detection.

6.3. Real-world experiments

We validate our proposed approach in a real-world, unstructured environment using the hardware platform mentioned in Section 3 to show the capability of its real-time navigation. The environment we test is a field site about 500 m in length and 200 m in width, with the maximum undulation of 5 m. And there are pits, soil slopes, ponds, and other obstacles that are difficult for unmanned vehicles to cross, as shown in Fig. 12.

Figure 12.

The real-world off-road experimental area (point cloud map is built by laser-slam).

Figure 13 shows some instances of the proposed method. When the target point is on the other side of a huge depression as shown in Fig. 13(a), our method can reasonably plan a path such that the vehicle can avoid the huge negative obstacle along the periphery. In Fig. 13(b), when reaching a target point requires passing through a narrow area, our approach can drive the vehicle to pass in a reasonable posture, avoiding being stuck or detouring.

Figure 13.

Real navigation in off-road experiment.