G²VD Planner: Efficient Motion Planning With Grid-based Generalized Voronoi Diagrams

Many path searching approaches have been proposed in terms of different theories [20], which can be classified into three categories. The sampling-based planning algorithms such as the famous probabilistic roadmap (PRM) [21] and rapidly-exploring random trees (RRTs) [22] have gained popularity for the capability of efficient searching in the configuration space (C-space). However, they are limited by completeness and optimality, and even some excellent variants such as RRT* [23] can only guarantee asymptotic optimality. The classical artificial potential field (APF) algorithm [24] finds a feasible path by following the steepest descent of a potential field. However, the APF algorithm often suffers from the local minimum problem. In this paper, we focus on grid-based planning algorithms. Grid-based planning overlays a hyper-grid on the C-space and assumes each configuration is identified with the grid-cell center [25]. Then, search algorithms are used to find a path from the start to the goal. Grid-based planning can always find a resolution-optimal path if it exists in the discrete search space, namely, completeness and optimality in the sense of resolution are guaranteed. However, these approaches depend on space discretization and do not perform fast as the environment dimension increases.

The path obtained by path searching approaches usually fails to meet the smoothness requirement for robot navigation and needs further smoothing [26]. Two types of path smoothing approaches are investigated in this paper, namely, soft-constrained approaches and hard-constrained approaches.

Motivated by the aforementioned limitations of existing works, an efficient motion planning approach called G²VD planner is newly proposed. Specifically, a G²VD is utilized to aid path searching and path smoothing to achieve better performance in terms of efficiency, safety, smoothness, etc.

The GVD is defined as the set of points in the free space to which the two closest obstacles have the same distance [39]. In this paper, an efficient, incrementally updatable GVD construction algorithm presented in [48] is utilized to convert the occupancy grid map to a GVD in discrete form. The core of this algorithm [48] is to employ a dynamic variant of the brushfire algorithm [49] to incrementally compute Euclidean distances. Specifically, the G²VD is firstly initialized based on a prior grid map, which is usually generated by simultaneous localization and mapping (SLAM) [50]. On top of the initial G²VD, both moving dynamic obstacles and unknown static obstacles are detected by external sensors mounted on the robot. Movement, insertion, and deletion of obstacles change the states of individual cells in the G²VD from free to occupied or vice versa. Newly occupied cells initiate “lower” wavefronts that update the distance to the closest obstacle of affected cells. Similarly, “raise” wavefronts start at newly freed cells and clear the distance entries of all cells whose closest obstacle is the deleted one. The processing of raised and lower wavefronts is interwoven and controlled by a single priority queue that sorts the enqueued cells by distance. Both the raised and lower wavefronts enqueue the neighbors of a processed cell to propagate the wavefronts. Readers can refer to [48] for more details about this algorithm. Fig. 2 illustrates the G²VD of an office-like environment, in which the Voronoi edge pixels are colored in blue and red.

As mentioned above, the employed incrementally updatable G²VD construction algorithm can deal with both moving dynamic obstacles and unknown static obstacles. Therefore, even if the prior grid map is not available, the robot can still observe the surrounding environment through external sensors and update the G²VD in real-time. That is to say, the proposed framework can be theoretically applied in completely unknown scenarios. In practice, if a prior grid map containing the basic environmental structural information is available, it will enable the G²VD planner to provide better global guidance.

After obtaining the G²VD, breadth-first search is employed to find the cells $C_{1}$ and $C_{2}$ closest to the start cell $C_{start}$ and the goal cell $C_{goal}$ on the GVD, respectively. And the shortest grid paths $L_{1}$ from $C_{start}$ to $C_{1}$ and $L_{2}$ from $C_{2}$ to $C_{goal}$ are also computed in this process. Then, the A* search is utilized to search the shortest grid path $L_{3}$ from $C_{1}$ to $C_{2}$ along the GVD edge pixels, in which collision detection is carried out based on the circumscribed radius of the robot. In particular, the cells whose distance to the closest obstacle is less than the circumscribed radius of the robot are considered invalid and eliminated. Therefore, the final searched 2-D grid path consisting of valid cells is guaranteed to be collision-free. As shown by the red path in Fig. 2, the final shortest Voronoi grid path from $C_{start}$ to $C_{goal}$ is composed of $L_{1}$, $L_{3}$, and $L_{2}$.

Based on the shortest Voronoi grid path searched above, a region called Voronoi corridor is constructed as follows. For every cell $C_{k}$ in the shortest Voronoi grid path, the minimum distance to the closest obstacle cell $d_{k}$ is retrieved from the G²VD. Then, a square bounding box with a side length of $2d_{k}$ is centered on $C_{k}$. The obstacle-free cells covered by all bounding boxes make up the Voronoi corridor, as illustrated by the gray region in Fig. 2. Every time the G²VD updates, the shortest Voronoi grid path is searched from scratch and the Voronoi corridor is constructed based on the newly obtained Voronoi grid path.

Different from the traditional cost map, which is usually constructed by inflating the obstacle cells and typically used to improve the efficiency of collision detection, the Voronoi corridor is constructed to broadly reduce the search space of path searching and improve the search efficiency. Therefore, elaborate collision detection considering the footprint of the robot is not involved when constructing the 2-D Voronoi corridor, which is done in the 3-D path searching stage. In the stage of the Voronoi corridor construction, only the minimum distances from the cells along the Voronoi grid path to the closest obstacle cells are used to make sure that the Voronoi corridor contains the feasible region.

The shortest Voronoi grid path is far from the actual optimal path, and its piecewise-linear form also does not satisfy the kinodynamic constraints of the robot. To address these issues, a new state lattice-based path planner is proposed to perform fine path searching within the Voronoi corridor.

A typical state lattice-based path planner consists of two parts, namely, graph construction and graph search [35]. As for graph construction, the 3-D search space $\left(x,y,\theta\right)$ is discretized first, where $\left(x,y\right)$ denotes the position of the robot in the world and $\theta$ represents the heading of the robot. In particular, the orientation space is discretized into 16 angles. Furthermore, the connectivity between two states in the graph is built from motion primitives which fulfill the kinematic constraints of the robot. In this work, a quintic Bézier curve-based path generation approach described in [47] is employed to generate motion primitives from each discretized angle offline. A motion primitive $\gamma\left(s_{1},s_{n}\right)$ consists of a sequence of internal robot poses $\left\{s_{1},s_{2},\dots,s_{n}\right\},s_{i}=\left(x_{i},y_{i},\theta_{i}\right),1\leq i\leq n$ when moving from state $s_{1}$ to state $s_{n}$, where $n$ is the number of poses contained in the motion primitive. As for graph search, the standard A* search is employed, where the 2-D heuristic $h_{2D}$ proposed in [35] is utilized to guide the A* search away from those areas with dead-ends. $h_{2D}$ is constructed by computing the costs of shortest 2-D grid paths from the goal cell to other cells in the search space through dynamic programming.

In addition, since the motion primitives from each discretized angle are designed offline in advance, the covered cells of each motion primitive considering the robot footprint can be also computed and recorded as a lookup table in advance at the origin $\left(0,0\right)$. During every A* expansion, the covered cells of the selected motion primitive are retrieved in the lookup table and translated according to the robot position. As long as one of the covered cells is occupied by obstacles, the corresponding motion primitive is considered invalid and eliminated in this expansion. Therefore, the final searched path consisting of valid motion primitives is guaranteed to be collision-free.

Different from the original state lattice-based path planner [35] that takes the whole grid map as the search space, we reduce the search space to the Voronoi corridor to further improve the search efficiency. In addition, every time the grid map is updated, $h_{2D}$ needs recalculation before performing path searching. Since the search space of the newly proposed state lattice-based path planner is reduced to the Voronoi corridor, dynamic programming is accordingly limited to only compute the costs of shortest paths from the goal cell to those cells that are within the Voronoi corridor. Therefore, considerable time is also saved for the recalculation of $h_{2D}$.

Although the path searched above is optimal in the search space, it may be very close to obstacles since only the path length is considered in the cost function. The ideal path is to keep a reasonable distance from obstacles to provide sufficient optimization margin for subsequent path smoothing. To this end, inspired by the Voronoi field presented in [41], we design the following potential function

where $d_{\mathcal{V}}\left(x,y\right)$ and $d_{\mathcal{O}}\left(x,y\right)$ denote the distances from the given path vertex $\left(x,y\right)$ to the closest Voronoi edge and the closest obstacle, respectively. The operation of squaring in Eq. (2) is to make the potential function non-negative. $d_{\mathcal{O}}^{\min}$ is a threshold specifying the minimum safety distance to obstacles. An example of the Voronoi field is shown in Fig. 3.

According to [41], this potential function has the following properties:

1.    i)

   ${\rho_{V}}\left({x,y}\right)\in[0,1]$ and is continuous on $\left(x,y\right)$ since we cannot simultaneously have $d_{\mathcal{V}}=0$ and $d_{\mathcal{O}}=0$;

2.    ii)

   ${\rho_{V}}\left({x,y}\right)$ reaches its maximum only when $\left(x,y\right)$ is within obstacle areas;

3.    iii)

   ${\rho_{V}}\left({x,y}\right)$ reaches its minimum only when $\left(x,y\right)$ is on the edges of the GVD or the distance from $\left(x,y\right)$ to the closest obstacle is greater than $d_{\mathcal{O}}^{\min}$.

It is noteworthy that the potential cost is set to zero when the distance to the closest obstacle is greater than the safety threshold (${{d_{\mathcal{O}}}>d_{\mathcal{O}}^{\min}}$). The reason for this design is as follows. Our goal is to make the searched path close to Voronoi edges through the Voronoi field so that the obtained path can keep an appropriate distance from obstacles and provide sufficient optimization margin for subsequent path smoothing. However, if the environment is wider and the Voronoi edge is far from the obstacles on both sides, which may make the searched path far away from the optimal path. Therefore, we set a safety distance threshold $d_{\mathcal{O}}^{\min}$ and regard the area where the distance to the closest obstacle exceeds $d_{\mathcal{O}}^{\min}$ as a safe region. In this way, the searched path can keep a certain distance from obstacles and will not be far away from the optimal path through the Voronoi field.

Based on the above Voronoi field, the cost of motion primitives is defined as follows. For the sake of computational efficiency, we broadly follow the work [41] and temporarily assume that the robot travels at constant linear and angular velocities. If a motion primitive $\gamma\left(s_{1},s_{n}\right)$ collides with obstacles, the cost $g\left(\gamma\left(s_{1},s_{n}\right)\right)$ is set to infinity. Otherwise, the cost of this motion primitive is defined as

where $t\left({s_{1},s_{n}}\right)$ is the minimum travel time spent on $\gamma\left(s_{1},s_{n}\right)$ assuming uniform motion under the constraints of the maximum linear velocity $v_{\mathrm{max}}$ and the maximum angular velocity $\omega_{\mathrm{max}}$

$\mathcal{Q}$ denotes the set of 2-D cells covered by the robot when moving from state $s_{1}$ to state $s_{n}$. The “+1” operation in Eq. (3) is used to ensure that $g\left(\gamma\left(s_{1},s_{n}\right)\right)$ is a positive number. Intuitively, the Voronoi field penalizes slightly more those actions for which the robot traverses high potential cost areas (e.g., obstacles) and makes the searched path as close as possible to the Voronoi edges or keep a certain distance from obstacles. In addition, the 2-D heuristic $h_{2D}$ is also multiplied by the potential costs to be consistent with the cost definition of motion primitives.

Remark: In general, the path length is considered in the cost function of path planning approaches. However, the action of rotation in place is contained in the designed motion primitives in this work since our platform is a differential-drive mobile robot. And the cost of this action will be 0 if the cost function of the motion primitive is defined as the path length. Therefore, the travel time spent on the motion primitive is considered in the cost function instead of the path length in this work.

It should be noted that the Voronoi potential field in [41] is used for path smoothing and the region of interest is the whole map. While the search space of the newly proposed state lattice-based path planner is reduced to the Voronoi corridor, and we only need to compute the Voronoi potential costs within the Voronoi corridor. In particular, an efficient distance transform algorithm presented in [51] is employed to compute the Euclidean distance to the closest Voronoi edge for those cells within the Voronoi corridor. Figs. 4(a) and 4(b) illustrate the paths searched by the original state lattice-based path planner [35] and the newly proposed path planner, respectively. Compared with the original state lattice-based path planner, the path obtained by the proposed path planner has a certain distance from obstacles and can provide wider optimization margin (green bounding boxes) for further path smoothing, which will be detailed in Section IV-D.

Remark: The final path is found within the Voronoi corridor, namely, the final solution is searched in a subset of the complete solution set to achieve high computational efficiency performance. The Voronoi corridor is generated based on the shortest Voronoi grid path searched in the G²VD and usually contains the optimal solution in the complete solution set. However, when there is a significantly large difference in the dimension of the environment, it is possible that the optimal solution is contained in a sub-optimal Voronoi corridor derived based on a sub-optimal Voronoi grid path. Therefore, we cannot theoretically guarantee the optimality of the proposed approach. However, the practical performance is satisfactory, which is demonstrated through extensive simulation and experimental tests in Sections VI and VII. In addition, there may be potential extreme scenarios where regional disconnections occur, which will cause the failures of both the Voronoi path search and the Voronoi corridor construction. In these circumstances, the proposed path searching approach degenerates into the original state lattice-based path planner.

The path obtained by the state lattice-based path planner is kinematically feasible, but it is still piecewise-linear and not suitable for velocity planning. Therefore, an efficient QP-based path smoothing approach combined with cubic spline interpolation is employed to further smooth the path.

The input of path smoothing is several path vertices, which are obtained by sampling in the path generated by the state lattice-based path planner with a fixed interval. Given $n$ reference path vertices to be further smoothed, a convex QP-based path smoothing formulation is defined as

subject to

where ${\mathbf{x}}={[{\mathbf{x}}_{1}^{\mathrm{T}}\;{\mathbf{x}}_{2}^{\mathrm{T}}\;\ldots\;{\mathbf{x}}_{n}^{\mathrm{T}}]^{\mathrm{T}}}$ is a $2n$-dimensional parameter vector, and $\mathbf{x}_{i}={\left({x_{i}},{y_{i}}\right)}^{\mathrm{T}},1\leq i\leq n$ denotes the world coordinates of a path vertex. $\mathbf{x}_{i_{ref}}=\left(x_{i_{ref}},y_{i_{ref}}\right)^{\mathrm{T}},1\leq i\leq n$ is the corresponding reference path vertex of $\mathbf{x}_{i}$, and $\omega_{s}$ and $\omega_{r}$ are the weights of cost terms. $\mathcal{B}_{i}$ is a state bubble constraining the feasible region of the path vertex $\mathbf{x}_{i}$. In this work, $\mathcal{B}_{i}$ is approximated as an inscribed square of a circle centered on $\mathbf{x}_{i_{ref}}$, where the radius of the circle is equal to the distance $d_{i}$ from $\mathbf{x}_{i_{ref}}$ to the closest obstacle minus the circumscribed radius $r_{c}$ of the robot, as shown in Fig. 5. The footprint of the robot is approximated as a rectangle in this work. To compute $r_{c}$, we sequentially calculate the distances from the center of the rectangle to its vertices. And the maximum distance is token as $r_{c}$. Therefore, $\mathbf{x}_{i}\in\mathcal{B}_{i}$ in Eq. (6b) is approximated as

where the optimization margin $b_{i}$ is defined as

Based on the constraints in Eqs. (7) and (8), the distance between every optimized path vertex and the corresponding closest obstacle is greater than the circumscribed radius of the robot, thus the final smoothed path is guaranteed to be collision-free. According to Eq. (8), a path vertex $\mathbf{x}_{i}$ will be fixed during the optimization process if the corresponding reference path vertex $\mathbf{x}_{i_{ref}}$ is close to the obstacle ($\frac{{\sqrt{2}}}{2}{d_{i}}\leq{r_{c}}$). Intuitively, a reference path with a certain distance from obstacles can provide sufficient optimization margin for path smoothing. This is one of the main reasons why we introduce the Voronoi field and redesign the cost function of path searching to make the searched path keep a reasonable distance from obstacles in Section IV-C.

The first term in Eq. (5) is a measure of the path smoothness. The cost function ${{\mathbf{x}_{i+1}}-2{\mathbf{x}_{i}}+{\mathbf{x}_{i-1}}}$ can be rewritten as $\left({{\mathbf{x}_{i+1}}-{\mathbf{x}_{i}}}\right)+\left({{\mathbf{x}_{i-1}}-{\mathbf{x}_{i}}}\right)$. As shown in Fig. 5, from a physical point of view, this cost term treats the path as a series spring system, where ${{\mathbf{x}_{i+1}}-{\mathbf{x}_{i}}}$ and ${{\mathbf{x}_{i-1}}-{\mathbf{x}_{i}}}$ are the forces on the two springs connecting the vertices ${\mathbf{x}_{i+1}}$, ${\mathbf{x}_{i}}$ and ${\mathbf{x}_{i-1}}$, ${\mathbf{x}_{i}}$, respectively. If the forces ${{\mathbf{x}_{i+1}}-{\mathbf{x}_{i}}}$ and ${{\mathbf{x}_{i-1}}-{\mathbf{x}_{i}}}$ are equal in size and opposite in direction, the resultant force $\left({{\mathbf{x}_{i+1}}-{\mathbf{x}_{i}}}\right)+\left({{\mathbf{x}_{i-1}}-{\mathbf{x}_{i}}}\right)$ is a zero vector and the norm is minimum. If all the resultant forces are zero vectors, all the vertices would uniformly distribute in a straight line, and the path is ideally smooth.

The second term in Eq. (5) is used to penalize the deviation from the original safe reference path. As mentioned before, the hard-constrained formulation does not explicitly consider the path clearance, namely, distance from feasible paths to obstacles is ignored. As a result, the optimized path may be close to obstacles. To address this issue, the penalty of the deviation from the original reference path is introduced in the optimization objective. Our goal is to obtain a smooth path while minimizing the deviation between the optimized path and the reference path. Because the reference path searched by the proposed state lattice-based path planner has certain clearance to obstacles, the safety of the path is implicitly considered in the optimization formulation, and the final optimized path will not be close to obstacles. This is also the second reason why we introduce the Voronoi field and redesign the cost function of path searching to improve the path clearance of the searched path, in addition to providing wider optimization margin for path smoothing.

Before path smoothing, the proposed state lattice-based path searching approach is used to find an initial path. Thanks to grid-based planning, the initial path is globally optimal in the sense of resolution. Furthermore, the proposed path smoothing approach formulates the path smoothing problem in the form of convex quadratic programming. The convexity ensures that the obtained solution is the global optimal solution of the underlying path smoothing problem.

Compared with traditional smoothing approaches such as cubic spline interpolation and curve optimization algorithms, the proposed QP-based path smoothing approach densely samples the searched path to serve as optimization variables. Such a framework achieves the maximum control and flexibility of the path shape to deal with complex scenarios, such as U-turns, S-shaped turns, etc.

After path optimization, a path that is much smoother than the original reference path is obtained. However, the number of the path vertices is the same as that of the input reference path and the optimized path is still piecewise-linear. Therefore, we further smooth the path via cubic spline interpolation to obtain a continuous curve. Finally, a numerical integration (NI)-based time-optimal velocity planning algorithm presented in [47] is employed to generate a feasible linear velocity profile along the smoothed path, i.e., to solve the $t\rightarrow s$ mapping problem mentioned in Section III. The NI-based algorithm can acquire a provably time-optimal trajectory with low computational complexity, which solves the problem by computing the maximum velocity curve (MVC) considering both kinematic and environmental constraints and then performing numerical integration under MVC [52, 53, 54]. Readers can refer to [47] for more details about the proofs of feasibility, completeness, and time-optimality of this algorithm.

Finally, the trajectory tracking controller proposed in the textbook [55] is employed to track the desired Cartesian trajectory $\left\{x_{d}\left(t\right),y_{d}\left(t\right),\theta_{d}\left(t\right),v\left(t\right),\omega\left(t\right)\right\}$ for the differential-drive mobile robot used in this work. In particular, the control law is as

wherein $k_{1}$, $k_{2}$, and $k_{3}$ are the proportional coefficients, which are set to $1.0$, $2.5$, and $2.5$, respectively. And the error terms of $e_{1}$, $e_{2}$, and $e_{3}$ in Eq. (9) are calculated as follows:

Readers can refer to [55] for more details about the controller.

The proposed G²VD planner is implemented in C/C++. The convex QP problem described in Section IV-D is solved by an alternating direction method of multipliers (ADMM)-based QP solver, OSQP [56]. The reference path vertices of path smoothing are obtained by sampling in the path generated by the state lattice-based path planner with an interval of $0.1$ $\mathrm{m}$. Densely sampling vertices along the path will introduce more optimization variables and increase the computational burden. In this work, the sampling interval is set according to the resolution of the underlying G²VD and the dimension of the robot. The weights $\omega_{s}$ and $\omega_{r}$ are set to $10$ and $1$, respectively. The minimum safety distance $d_{\mathcal{O}}^{\min}$ is set to $0.5$ $\mathrm{m}$. The update frequency of the G²VD is set to $20$ $\mathrm{Hz}$. Each update takes an average of $10$ $\mathrm{ms}$. All the simulations and experiments are tested on a laptop with an Intel Core i7-9750H processor and 16 GB RAM.

The proposed Voronoi corridor is integrated into the original state lattice-based path planner (A* + motion primitives) [35] to derive a new state lattice-based path planner (A* + motion primitives + Voronoi corridor). And the new path planner is compared with the original state lattice-based path planner to validate the effectiveness of the Voronoi corridor. The performance of path searching approaches is evaluated in terms of computational efficiency and memory consumption. In particular, the number of expanded states and the planning time are used to evaluate the computational efficiency, and the graph size, i.e., the number of created nodes in the search graph, is used to evaluate the memory consumption.

Remark: The runtime of the proposed path searching approach includes three parts: searching the shortest Voronoi grid path, constructing the Voronoi corridor, and searching the kinematically feasible path within the Voronoi corridor. In this work, the incrementally updatable GVD construction algorithm [48] is integrated into the mapper module. Therefore, the time used to construct the G²VD is not included in the runtime of the proposed path searching approach.

To validate the effectiveness of the proposed QP-based path smoothing approach, we compare it with an advanced sparse-banded structure-based path smoothing approach SBA [44] and the popular optimization-based timed-elastic-band planner TEB [30]. We comprehensively evaluate path smoothing approaches in terms of computational efficiency. The weights of the smoothness and safety terms in the objective function of SBA [44] are set to $1$ and $10$ respectively according to the original paper. We believe these parameters were carefully tuned by the authors to achieve the best performance, and using these parameters makes our comparisons more convincing. Similarly, TEB uses the default parameters of its open-source implementation¹¹1https://github.com/rst-tu-dortmund/teb_local_planner.

As mentioned before, the incrementally updatable GVD construction algorithm described in [48] is employed to generate the G²VD of the maze environment, as shown in Fig. 6. Furthermore, we randomly select three sets of start and goal configurations in the maze for testing. Both the original state lattice-based path planner and the proposed path planner generate equal quality paths. The quantitative statistics of path searching results in these tests are enumerated in Table I.

The cyan cells in Fig. 6 represent the constructed Voronoi corridor in Test 1. Figs. 6 and 6 show the path planning results of the two path planners in Test 1, respectively, where the green cells denote the visited cells during the searching process. Compared with the original state lattice-based path planner [35], the search space of the proposed path planner is reduced to the Voronoi corridor, thus the search effort spent in those unpromising search areas is significantly saved. As shown in Table I, the number of expanded search states of the proposed path planner decreases by an average of $12.8\%$, and the computational efficiency is improved by $17.1\%$. Furthermore, since the search branches for searching in those unpromising search areas are reduced, the number of created nodes in the search graph also decreases. Compared with the original state lattice-based path planner, the graph size of the proposed path planner decreases by an average of $13.9\%$.

In conclusion, the newly proposed path planner generates equal quality paths with less time and memory consumption than the original state lattice-based path planner.

We choose four challenging local scenarios containing continuous S-shaped or U-shaped turns in the maze environment to compare the computational efficiency of path smoothing approaches. For a fair comparison, the proposed path searching approach is employed to generate the same reference path for SBA [44], TEB [30], and the proposed QP-based path smoothing approach. In each scenario, we set the same start and goal configurations and repeat the test $20$ times. For testing purposes, we do not limit the length of the initial path for path smoothing, namely, we sample all path points obtained by path searching with an interval of $0.1$ $\mathrm{m}$. The searched paths obtained by the proposed path searching approach and the corresponding smoothed paths generated by the proposed QP-based path smoothing approach are shown in green and red in Fig. 7 respectively, and the statistics of the runtime performance are shown in Table II. According to [44] and [30], both SBA and TEB are soft-constrained path smoothing approaches, wherein both the path smoothness and path clearance to obstacles are considered in the optimization formulation. The terms of path clearance to obstacles are non-convex, resulting in the final optimization formulation is also non-convex. While the proposed path smoothing approach formulates the path smoothing problem in the form of convex quadratic programming, and the convexity allows the problem to be solved efficiently. As shown in Table II, the maximum runtime of the proposed path smoothing approach is less than $0.7$ $\mathrm{ms}$ in all four tests, and the computational efficiency of the proposed approach is approximatively $6.6$ and $53.3$ times faster than that of SBA and TEB on average, respectively. In addition, the curvature profiles of the paths generated by SBA, TEB, and the proposed path smoothing approach are presented in Fig. 8. Since the smoothness of the path is explicitly considered in the optimization objective of our approach and SBA, their curvature profiles are much smoother than that of TEB.

In conclusion, the newly proposed QP-based path smoothing approach achieves a significant performance improvement in terms of computational efficiency.

As shown in Fig. 9, the mobile robot SUMMIT-XL HL is used as the experimental platform, which is equipped with a Velodyne VLP-16 LiDAR, an Intel Realsense D435i depth camera, and an Intel NUC computer. The footprint of the robot is approximated as a $0.8$ $\mathrm{m}\times 0.8$ $\mathrm{m}$ square, and the maximum linear velocity is $3.0$ $\mathrm{m/s}$. Considering the safety of robot navigation, the upper bound of the linear velocity is set to $1.5$ $\mathrm{m/s}$ in the experiments.

To generate a prior global map for autonomous navigation, we first employ a LiDAR-inertial odometry algorithm described in [59] to build a large-scale 3-D point cloud map. And then, a point clouds segmentation algorithm presented in [60] is used to filter out point clouds that hit the ground and tree canopy. Finally, the filtered point clouds are projected to a 2-D plane to derive traversable regions in the form of the occupancy grid map, as shown in Fig. 10. The dimension of the environment is approximately $210$ $\mathrm{m}\times 220$ $\mathrm{m}$, and the resolution of the grid map is $0.25$ $\mathrm{m/cell}$. During robot navigation, the LiDAR-inertial odometry algorithm [59] and the point clouds segmentation algorithm [60] are also utilized to provide state estimation and local traversable regions for the robot, respectively.

To validate that the proposed path searching approach can provide sufficient optimization margin for hard-constrained path smoothing approaches, we select an S-shaped scenario shown in Fig. 11 to compare path searching approaches. The path shown in Fig. 11 is obtained by the original state lattice-based path planner [35]. Since the path clearance is not considered in the original state lattice-based path planner, the searched path is close to the corner of the S-shaped turn, and the corresponding path vertices are fixed in the path smoothing process. As a result, the smoothed path is intuitively rough, as shown in Fig. 11. On the contrary, the path clearance is explicitly considered in the proposed path searching approach, thus the searched path shown in Fig. 11 has a certain distance from obstacles and provides sufficient optimization margin for the path vertices near the corner. The final smoothed path shown in Fig. 11 is much smoother than the path shown in Fig. 11, which demonstrates the proposed path searching approach can provide sufficient optimization margin for hard-constrained path smoothing approaches.

Finally, the proposed path searching and path smoothing approaches are integrated into the powerful ROS navigation stack³³3https://github.com/ros-planning/navigation to validate the effectiveness and practicability of the G²VD planner. In particular, we implement a G²VD cost map layer on top of the original ROS cost map. On this basis, the proposed path searching approach and the path smoothing approach are implemented as global and local planner plugins for the ROS navigation stack, respectively. Considering the computational efficiency and sensing range, the length of the initial local path is set to $4.0$ $\mathrm{m}$. As illustrated in Fig. 10, we select two sets of different start and goal configurations for outdoor navigation. The total travel distances of these two sets of outdoor navigation are approximately $118.3$ $\mathrm{m}$ and $165.9$ $\mathrm{m}$, respectively.

Here we summarize several representative experimental results of outdoor autonomous navigation to demonstrate the key characteristics of the G²VD planner. More details are included in the video.