EffMoP: Efﬁcient Motion Planning Based on Heuristic-Guided Motion Primitives Pruning and Path Optimization With Sparse-Banded Structure

—To solve the autonomous navigation problem in complex environments, an efﬁcient motion planning approach called EffMoP is presented in this paper. Considering the challenges from large-scale, partially unknown complex environments, a three-layer motion planning framework is elaborately designed, including global path planning, local path optimization, and time-optimal velocity planning. Compared with existing approaches, the novelty of this work is twofold: 1) a novel heuristic-guided pruning strategy of motion primitives is proposed and fully integrated into the state lattice-based global path planner to further improve the computational efﬁciency of graph search, and 2) a new soft-constrained local path optimization approach is proposed, wherein the sparse-banded system structure of the underlying optimization problem is fully exploited to efﬁciently solve the problem. We validate the safety, smoothness, ﬂexibility, and efﬁciency of EffMoP in various complex simulation scenarios and challenging real-world tasks. It is shown that the computational efﬁciency is improved by 66.21% in the global planning stage and the motion efﬁciency of the robot is improved by 22.87% compared with the recent quintic B´ezier curve-based state space sampling approach.


I. INTRODUCTION
M OBILE robots have been widely applied in various scenarios, such as family service, logistics, searchand-rescue, and so on.Motion planning is one of the key technologies for mobile robots to achieve full autonomy in these scenarios [1]- [3], which has been comprehensively investigated in the literature [4].However, it is still challenging to design a motion planning approach that can ensure both safety, smoothness, flexibility, and efficiency in large-scale, unknown, or partially unknown complex environments [5].
For computational efficiency reasons, motion planning problems of mobile robots in complex environments are commonly decoupled to global path planning and local planning [6].The global path planner is employed to provide a rough route from the current robot pose to the goal pose [7]- [10], followed by a local planner supposed to generate safe and smooth motions according to real-time sensor data [11]- [13].In [14], Zhang et al. present a multilevel human-like motion planning framework, wherein global-level path planning, sensor-level path planning, and action-level trajectory planning corresponding to the functions of the human brain, eyes, and legs are designed respectively.In the work [15], Marder-Eppstein et al. propose a robust navigation system for mobile robots in office environments by combining a global planner with Dijkstra's algorithm [16] and a local planner with the wellknown dynamic window approach (DWA) [17].In [18], Wang et al. design a motion planning framework for autonomous mobile robot navigation in uneven and unstructured indoor environments, in which an improved rapidly-exploring random tree (RRT)-based global planner and the elastic bands (EB) local planner [19] are employed.

A. Global Planning
Sampling-based planning and grid-based planning are two popular global path planning approaches.Typical samplingbased planning algorithms include probabilistic roadmap (PRM) [20] and RRT [21].These algorithms represent the configuration space (C-space) with a roadmap of randomly sampled configurations, which has considerable advantages in computational efficiency, especially in high-dimensional planning problems.However, sampling-based planning is limited by completeness and optimality, even some excellent variants such as RRT* can only guarantee asymptotic optimality [22].In this work, we focus on grid-based path planning approaches.
Grid-based planning overlays a grid on C-space and assumes each configuration is identified with a grid-cell center.Then search algorithms such as Dijkstra's algorithm [16] and A* [23] are used to find a path from the start to the goal, which can be complete and optimal in the discrete search space.However, original grid-based planning algorithms tend to produce paths that are non-smooth and do not generally satisfy the kinodynamic constraints of the robot.In [24], Pivtoraiko et al. propose the state lattice approach for differentially constrained mobile robot motion planning, where the connectivity between two configurations is built from a motion primitive which fulfill the kinematic constraints of the robot.Furthermore, to improve the computational efficiency of graph search, informative heuristics are employed to guide the search towards the most promising areas of the search space [23].The Euclidean distance function is perhaps the most well-known heuristic.However, it has no knowledge of environmental information and is thus a poor heuristic when applied in an environment with dense obstacles since it seriously underestimates the actual path costs [25].In [26], Likhachev and Ferguson propose a 2-D grid-based heuristic h 2D , which is derived by Dijkstra's algorithm incorporating the up-to-date environmental information.h 2D computes the costs of shortest paths from the goal cell to other cells in the 2-D grid and captures the geometry of obstacles in the environment.However, in [26] h 2D is only employed to estimate the cost of the shortest path from a given search state to the goal state, while the information about the graph search direction behind the heuristic has not been fully exploited.

B. Local Planning
Sampling-based local planning and optimization-based local planning are two popular local planning approaches in the field of mobile robots.A sampling-based local planner usually generates a set of candidate trajectories/paths first and then selects the best one which minimizes an elaborately designed evaluation function.The evaluation function usually includes a variety of factors, for instance, the distance to obstacles and the deviation from the global path.Some sampling-based local planners work in control space, such as the popular DWA [17].Compared with sampling in control space, sampling in state space is superior in terms of sampling efficiency and robustness to initial conditions [27].In the work [28], Howard et al. propose a state space sampling approach with a vehicle model-based trajectory generation approach [29], which has been successfully applied in the DARPA Urban Challenge [30].However, the environmental constraints are not taken into account during trajectory generation in [28], and considerable time is wasted in generating infeasible trajectories [31].In [14], Zhang et al. propose a state space sampling-based local planner by generating quintic Bézier curve-based paths with different initial curvatures offline.These local paths are saved in a lookup table and retrieved in the local planning stage according to the curvature condition, thus a significant amount of time is saved for online path generation.However, the endpoints of the offline generated paths are fixed, somehow limiting the flexibility of local planning.And in an environ-ment with dense obstacles, sampling-based local planners may even fail to find a solution.
Optimization-based local planning formulates the local planning problem as a non-linear optimization problem, which takes the global path in the local window as input and deforms the local path to make the optimization problem converge.In [32], Dolgov et al. propose a conjugate gradient (CG)-based path optimization approach for autonomous vehicle-free space planning, wherein the smoothness and safety of the path and the curvature constraint of the vehicle are both considered.In the work [33], [34], Rösmann et al. propose the wellknown timed elastic bands (TEB) local planner.Different from the classical EB [19], TEB explicitly considers the temporal aspects of motions, thus the initial path generated by the global path planner can be optimized with respect to minimizing the trajectory execution time, and kinodynamic constraints of robots such as maximum velocities and accelerations can be incorporated into the optimization objective as soft constraints.Inspired by the back-end of simulations localization and mapping (SLAM), TEB formulates the optimization problem in the hyper-graph framework and employs the graph optimization solver g2o [35] to solve the problem.However, the essential banded system structure behind the optimization problem has not been fully discussed and exploited in TEB.Furthermore, although TEB considers the velocity and acceleration constraints, it can not guarantee that these kinodynamic constraints are strictly satisfied in the soft constraint framework.In addition, too many constraints may lead to mutual compromises.For example, keeping a minimum distance to obstacles may be conflicting with acquiring a minimum-time trajectory.Therefore, the final optimized trajectory may not achieve good results in both motion efficiency and safety.

C. Contributions and Novelties
Motivated by the challenges of motion planning problems of mobile robots and the aforementioned limitations of existing approaches, a three-layer motion planning framework is proposed in this paper, including global path planning, local path optimization, and time-optimal velocity planning.Specifically, we propose new approaches in the module of global planning and local path optimization part: 1) A* search algorithm combined with motion primitives is used to address global path planning problems.Inspired by the environment-constrained heuristic h 2D presented in [26], a novel heuristic-guided pruning strategy of motion primitives is proposed to further improve the computational efficiency of graph search.Given a set of motion primitives as the action set, the branching factor, i.e., the average number of successors per state, is fixed.Specifically, all the motion primitives are involved during every expanding process in A* search.In this work, the search direction information behind h 2D is exploited to provide a one-step forward prediction for A* search, and only part of the motion primitives are involved in each expanding process.Therefore, the branching factor of graph search is decreased dramatically, and the computational efficiency is significantly improved.
2) Local motion planning problems are addressed by a path/velocity decoupled planning framework.A softconstrained multi-objective local path optimization approach is newly proposed, wherein the constraints including safety and smoothness are both considered.Furthermore, we notice that each sub-objective only depends on a few local consecutive variables, thus the partial derivatives of the irrelevant variables in the Jacobian are all zero and the Hessian matrix of the whole optimization problem is sparse-banded.Based on this property, the optimization problem is efficiently solved through the Levenberg-Marquardt (LM) algorithm combined with the forward elimination and back substitution algorithm, and high real-time optimization performance can be achieved.After path optimization, cubic spline interpolation is employed to further smooth the local path.
Finally, a numerical integration-based velocity planning algorithm described in [14] is utilized to generate a feasible linear velocity profile along the smoothed local path under the velocity and acceleration constraints.
To summarize, the main contributions and novelties of this work are as follows: 1) A novel heuristic-guided pruning strategy of motion primitives is proposed, which is fully integrated into the state lattice-based global path planner to further improve the computational efficiency of graph search.2) A new soft-constrained local path optimization approach is proposed.The sparse-banded system structure of the underlying optimization problem is fully exploited to efficiently solve the problem, which converges quickly to generate a safe and smooth local path.The smoothness of the local path benefits the subsequent time-optimal velocity planning.3) Autonomous navigation is realized in large-scale, partially unknown complex environments.Extensive simulation and experimental evaluations are presented to verify the safety, smoothness, flexibility, and efficiency of the proposed approach.
The remainder of the paper is organized as follows.Section II introduces the system framework of autonomous navigation.The pruning strategy of motion primitives and the local path optimization approach are detailed in Sections III and IV respectively.Section V provides some implementation details.The results of simulations and experiments are presented and discussed in Sections VI and VII respectively.Finally, this paper is concluded in Section VIII.

II. SYSTEM FRAMEWORK
An overview of the software system architecture designed for autonomous navigation is outlined in Fig. 1.The navigation system consists of a variety of modules, for instance, localization, mapping, global path planner, local planner, and so on.All the modules run in parallel and communicate with each other via messages based on Robot Operating System (ROS) [36].

A. Localization
Perception is a fundamental module for autonomous navigation [37], [38].In this work, a feature-based localization approach is employed.When a new laser scan is received, an efficient and robust 2-D line segment extraction algorithm described in [39] is utilized to extract line segment features.Then, the endpoints of these line segment features are matched with a prior feature map by the Random Sample Consensus (RANSAC) algorithm [40].If the number of the matched pairs is greater than a preset threshold, the robot pose is obtained by computing the transformation between the matched point features.Otherwise, dead rocking is temporarily employed.It should be noted that the proposed navigation system is highly modular and the localization module can be replaced by other alternatives.

B. Environment Models
In this work, the environment is represented in the form of Euclidean Distance Grid (EDG), wherein each cell stores the distance to the closest obstacle in the grid.Cells occupied by obstacles have a zero value.Such representation is convenient and efficient to evaluate whether a configuration is in collision with obstacles or not.For example, a collision-free judgment can be obtained if the minimum value of the cells covered by the robot is larger than the circumscribed radius of the robot footprint.With the help of EDG, considerable computing time for directly employing the geometric footprint of the robot to collision detection can be saved.

C. Motion Planning
Motion planning, which plays an essential role in generating safe, smooth, efficient, and flexible motions for mobile robots, is the main focus of this work.Considering the challenges from large-scale, partially unknown complex environments, an efficient motion planning approach called EffMoP is newly proposed.As illustrated in Fig. 2, a three-layer motion planning framework is carefully designed, which consists of global path planning, local path optimization, and velocity planning.The global path planner is employed to provide a rough route from the current robot pose to the goal pose, and the local path optimization combined with time-optimal velocity planning is used to generate safe, smooth, and efficient motion commands according to real-time sensor data.Different from the twolayer motion planning framework, local motion planning is decoupled into local path optimization and velocity planning in this work.Compared with the path/velocity coupled planning approaches such as TEB, the adopted path/velocity decoupled framework can achieve better motion efficiency and computational efficiency while ensuring smoothness.The reason is twofold: 1) Decoupled approaches divide the complex motion planning problem into two subproblems.Compared with the original problem, each subproblem has a lower dimension, and the computational efficiency of solving two subproblems separately is higher than directly solving the original complex problem; 2) Too many constraints need to be considered when jointly planning path and velocity, which may lead to mutual compromises.Based on the above careful consideration, the three-layer motion planning framework is designed to consider different constraints and objectives in different layers to avoid mutual conflicts.

PRIMITIVES
To efficiently address motion planning problems of mobile robots in large-scale, partially unknown complex environments, A* search algorithm combined with motion primitives is employed to compute a kinematically feasible collision-free path first.In this section, a novel pruning strategy of motion primitives based on an environment-constrained 2-D heuristic is detailed.

A. Problem Formulation
In this work, we adopt a three-dimensional state representation (x, y, θ), where (x, y) denotes the position of the robot in the world, θ represents the heading of the robot and is normalized to (−π, π].For the sake of computational efficiency, the velocities of the robot are not taken into account in the state representation in the global path planning stage.Instead, we temporarily assume that the robot travels at constant linear and angular velocities.Furthermore, in order to obtain a kinematically feasible path, the connectivity between two states is built from motion primitives which fulfill the kinematic constraints of the robot.A motion primitive γ (s, s ) consists of a sequence of internal robot poses when moving from state s to state s .In this work, the trajectory generation algorithm described in [29] is employed to generate motion primitives offline.If a motion primitive γ (s, s ) is collisionfree, the cost g (γ (s, s )) of this motion primitive is defined as the travel time spent on it.Otherwise, g (γ (s, s )) is set to infinity.
Based on the above preliminaries, the path planning problem is defined as follows.The input of the path planner is an up-to-date EDG, the kinematically feasible motion primitives, the current robot state s start and a goal state s goal .And the output is a path that is collision-free and satisfies the kinematic constraints of the robot.Meanwhile, the generated path is expected to be optimal or sub-optimal with respect to the travel time in the state space.

B. Environment-Constrained 2-D Heuristic
To cope with the complex environmental constraints, it is usual to solve a simplified search problem online and take the result of the simplified search to guide the original, complex search.In this work, the environment-constrained 2-D heuristic h 2D presented in [26] is used to guide A* search.Given the up-to-date environmental information, a 2-D version ((x, y)) of the original path planning problem is solved by Dijkstra's algorithm.Namely, the original 3-D search problem is reduced to a 2-D search in an 8-connected or 16-connected grid, and the non-holonomic constraints of the robot are ignored.Such a simplified search procedure computes the costs of the shortest paths from the goal cell to other cells in the 2-D grid and stores them as a lookup table.Furthermore, to make the heuristic more informative, the costs of those cells whose distances to obstacles are less than the inscribed radius of the robot footprint are set to infinity.Intuitively, h 2D captures the geometry of obstacles in the environment and guides the more expensive 3-D search away from those areas with dense obstacles.Therefore, h 2D needs to be updated whenever the environment changes.An environment-constrained heuristic path is illustrated in Fig. 3.

C. Motion Primitives Pruning
Fully investigating h 2D and designing a pruning strategy of motion primitives to further improve the computational efficiency of graph search is the main novelty of this section.In [26], h 2D is only used to estimate the cost of the shortest path from a given search state to the goal state.In this work, h 2D is further exploited to provide a one-step forward prediction for the search direction, and only part of the motion primitives are involved in each expanding process.Therefore, the branching factor of graph search is decreased, bringing significant benefits for computational efficiency and memory consumption.
Above all, it should be noted that the simplified 2-D search is performed in a backward manner, i.e., the start of the 2-D search is the goal of the 3-D search.According to the angular resolution of the 3-D search, the 2-D search is correspondingly performed in a 16-connected grid, as illustrated in Fig. 3.The procedure of the proposed pruning strategy of motion primitives is as follows: 1) For every cell c in the 2-D grid except the goal cell, the shortest 2-D path from the goal cell to c is computed by Dijkstra's algorithm.Furthermore, the predecessor c of c in the 2-D heuristic path is retrieved and the angle α between the positive x-axis and the vector from c to c is computed and recorded in the cell c. 2) During the 3-D search stage, for the currently expanded 3-D (x, y, θ) search state s, every one of its successor s is retrieved according to the designed motion primitives.The angle β between the positive x-axis and the vector from s to s is computed, as shown in Fig. 4(a).Furthermore, the corresponding 2-D cell c of s in the 2-D grid is derived, and the angle α stored in c is retrieved.If (α − θ) and (β − θ) have opposite signs, it is reasonable to believe that the vector from s to s points to an unpromising search direction, as shown in Fig. 4(a).Therefore, the corresponding motion primitives is not involved in the actual expanding process.3) In order not to sacrifice the completeness of the algorithm in the discrete search space, the motion primitives of "taking a step forward" and "turning in place" are preserved ignoring the pruning strategy.The reason is that we can always get a feasible solution by only combining these two actions if the feasible solution exists in the discrete search space.4) In the end, only part of the designed motion primitives are involved in each expanding process, as shown in Fig. 4(b).In the above process, all angles are normalized to (−π, π].The positive sign of the angle corresponds to the counterclockwise direction.
Intuitively, the 2-D cell c and its predecessor c in the environment-constrained heuristic path provide a one-step forward prediction for the 3-D search direction.Based on the pruning strategy, the branching factor of graph search is decreased dramatically and the computational efficiency is significantly improved.Theoretically, we cannot guarantee the optimality of the proposed path planner with motion primitives pruning.However, the practical performance is satisfactory, which will be demonstrated through extensive simulations and experiments in Sections VI and VII.

IV. SOFT-CONSTRAINED LOCAL PATH OPTIMIZATION WITH SPARSE-BANDED SYSTEM STRUCTURE
The path generated by the global path planner is kinematically feasible, but it is still piecewise linear and not suitable for velocity planning.In addition, the global path planner updates with a relatively low frequency.Therefore, the safety of the path needs to be further improved by the real-time sensor data.
In this section, a new soft-constrained local path optimization approach is proposed to deform an initial path generated by the global path planner.The sparse-banded system structure of the underlying optimization problem is fully investigated and the LM algorithm combined with the forward elimination and back substitution algorithm is utilized to efficiently solve the problem.

A. Problem Formulation
Given the world coordinates of N path vertices formulation is defined as and the optimal parameter vector x * are obtained by solving the non-linear least squares problem Here, T is a 2N -dimensional parameter vector, ∆x i = x i −x i−1 , 2 ≤ i ≤ N denotes the displacement vector at the vertex x i , and ω s and ω o are the weights of the cost terms.f o (x i ) is a piecewise continuous differentiable cost function with d s specifying the minimum safe distance to obstacles: wherein τ (x i ) represents the Euclidean distance between the path vertex x i and the closest obstacle.The first term in Eq. ( 1) is a measure of the smoothness of the local path.The cost function ∆x i+1 − ∆x i can be rewritten as From a physical point of view, this cost term treats the local path as a series spring system, where F i+1,i + F i−1,i is the resultant force of two springs connecting the vertices x i+1 , x i and x i−1 , x i , respectively.If all the resultant forces are equal to zero, all the vertices would uniformly distribute in a straight line, and the local path is ideally smooth.
The second term in Eq. ( 1) is a measure of the safety of the local path, which is efficiently evaluated in a gridinterpolation scheme.Firstly, an EDG is built by performing distance transform on top of an occupancy grid, wherein each cell of EDG stores the Euclidean distance f d (P ) between the center P of this cell and the center of the closest cell occupied by obstacles.Secondly, considering the discrete nature of EDG, which limits the precision of the evaluation of safety and does not allow the direct computation of derivatives, bilinear interpolation is employed to approximate gradients [41].As depicted in Fig. 5, given the world coordinates x i = (x i , y i ) T of a path vertex Q, τ (x i ), i.e., the measure of the distance between Q and the closest obstacle, is approximated as ) where P 00 , P 10 , P 01 , and P 11 denote the centers of four closest cells surrounding Q, and (x 0 , y 0 ), (x 1 , y 0 ), (x 0 , y 1 ), and (x 1 , y 1 ) are the world coordinates of P 00 , P 10 , P 01 , and P 11 respectively.The gradient ∇τ Based on the above grid-interpolation scheme, the safety of the local path is efficiently evaluated while the problem of discretization is overcome.Remark: Eq. ( 2) has a solution if the initial guess of Eq. ( 2) is feasible and the weight of the safety constraint is dominant.
If the weight of the safety constraint is dominant, the LM algorithm will try to minimize the safety term as much as possible, and each path vertex x i will be adjusted independently along the direction of the negative gradient of f o (x i ).Namely, each path vertex will be pushed away from obstacles until the zero gradient occurs.For f o (x i ), the zero gradient occurs in the safe areas where the distance to the closest obstacle is greater than d s .Therefore, x will converge to a "safer" solution than the initial guess.If the initial guess is feasible, the solution of Eq. ( 2) is guaranteed to be feasible.

B. Least Squares Optimization
For simplicity of notation, in the rest of this paper we take and Eq. ( 1) is rewritten as Assume that a good initial guess x of the parameter vector x is known, a numerical solution of Eq. ( 2) can be obtained by using the popular Gauss-Newton or LM algorithms.The idea is to approximate the cost term by its first order Taylor expansion around the current initial guess where J s i−1,i,i+1 and J o i are the Jacobians of S i−1,i,i+1 (x) and O i (x) computed in x, respectively.For simplicity of notation we take S i−1,i,i+1 := S i−1,i,i+1 (x) and O i := O i (x).Substituting Eq. ( 9) in the cost terms in Eq. ( 8), we obtain

and
Oi(x + ∆x) T Oi (x + ∆x) With the above local approximations, we can linearize the function given in Eq. ( 8) around the current initial guess where Eq. ( 12) can be minimized in ∆x by taking the derivative of f (x + ∆x) with respective to ∆x and setting the result to zero where H is the system matrix of the optimization problem.
The solution of the path optimization problem is obtained by adding the increment ∆x * to the initial guess The Gauss-Newton algorithm iterates the linearization in Eq. ( 12), the solution in Eq. ( 14), and the update step in Eq. ( 15).In every iteration, the previous solution is used as the linearization point and the initial guess until a given termination criterion is met.The LM algorithm introduces a damping factor and backup actions to Gauss-Newton to control the convergence.Instead of solving Eq. ( 14), the LM algorithm solves a damped version where λ is a damping factor to control the step size in case of nonlinear surfaces.

C. Sparse-Banded System Structure
An important property of the underlying optimization problem is the sparse-banded structure of the system matrix H is a function of the three consecutive variables x i−1 , x i , and x i+1 .Therefore, the partial derivatives of the variables other than these three variables in J s i−1,i,i+1 (x) are all zero and For simplicity of notation we omit the zero blocks.There is a similar property for J o i (x) and H o i (x).In the end, H is a 2N × 2N banded matrix with bandwidth 5, as illustrated in Fig. 6.
For simplicity of notation, Eq. ( 16) is rewritten as where A = H + λI.According to the LM algorithm, A is symmetric positive-definite.To solve Eq. ( 20) efficiently, A is decomposed as A = LU by LU decomposition where L is a unit lower triangular matrix and U is an upper triangular matrix.We can obtain x by first solving and then solving Ux = y.
Eq. ( 22) can be solved by forward elimination since L is unit lower triangular.To solve Eq. ( 23), we can use back substitution since U is upper triangular.The forward elimination and back substitution algorithm is essentially the Gaussian elimination algorithm, but it makes full use of the nonzero element distribution characteristics of the sparse-banded matrix and significantly reduces the computational complexity.
The time complexity of the forward elimination and back substitution algorithm is O(k 2 • n), while that of the Gaussian elimination algorithm is O(n 3 ), where n and k are the size and bandwidth of the banded matrix respectively.In addition, the forward elimination and back substitution algorithm is substantially faster than a general sparse factorization because it avoids having to store the factored form of the matrix.Readers can refer to [42] for more details about the forward elimination and back substitution algorithm.

D. Velocity Profile Generation
After path optimization, a smooth local path is obtained, but it is still piecewise linear and not suitable for velocity planning.
To address this problem, the local path is further smoothed via cubic spline interpolation.Finally, a numerical integration (NI)-based time-optimal velocity planning algorithm presented in [14] is employed to generate a feasible linear velocity profile along the smoothed local path.The NI-based algorithm can acquire provably time-optimal trajectory with low computational complexity [43]- [46], which solves the problem by computing maximum velocity curve (MVC) considering both kinematic and environmental constraints and then performing numerical integration under MVC.Readers can refer to [14] for more details about the proofs of feasibility, completeness, and timeoptimality of this algorithm.

V. IMPLEMENTATION DETAILS A. Setup
EffMoP is implemented in C/C++.The maximum number of iterations of the LM algorithm is set to 100.The initial guess of local path optimization is obtained by sampling in the global path with an interval of 0.1 m.In consideration of the computational efficiency and sensing range, the cumulative length of the initial local path is set to 3 m.EDG is computed through an efficient distance transform algorithm described in [47].We maintain two EDGs of different resolutions for the hierarchical motion planning framework, wherein 0.1 m/cell for the global grid map and 0.05 m/cell for the local grid map.The dimension of the global grid map corresponds to the global prior map, while the local grid map is a sliding window whose map center is corresponding to the robot pose and the map dimension is set to 8 × 8 m 2 .The safe distance d s is set to 0.5 m.Moreover, the modules of the global path planning and EDG updating run in two independent threads, with cycles of 250 ms and 40 ms respectively.All the simulations and experiments are tested on a laptop with an Intel Core i7-9750H processor and 16 GB RAM.
To obtain a set of feasible and effective weights for the path optimization formulation, we fix ω s to 1 firstly and then set an interval for ω o .After that, ω o is tuned by dichotomy and visual inspection with the help of the ROS visualization tool RViz.In this work, ω o is set to 10.In the future, we plan to use machine learning techniques to tune these weights adaptively.

B. Metrics
The proposed pruning strategy of motion primitives is integrated into the standard state lattice-based path planner (A* + motion primitives) [26] to derive a new path planner (A* + motion primitives + motion primitives pruning).And the new path planner is compared with the standard state latticebased path planner to validate the effectiveness of the pruning strategy.Both path planners require offline designed motion primitives.As mentioned before, the trajectory generation algorithm described in [29] is employed with the differentialdrive motion model to generate motion primitives.Furthermore, the metrics of the number of expanded states and the planning time are used to evaluate the computational efficiency of graph search [26], and the graph size, i.e., the number of nodes in the search graph, is employed to evaluate the memory consumption.
To validate the efficiency, smoothness, and flexibility of the proposed optimization-based local planner, we compare it with the advanced quintic Bézier curve-based state space sampling local planner (QBC) [14].The total travel time is utilized to evaluate the motion efficiency of local planners.In addition, we carefully design various simulation and experimental scenarios to compare the smoothness and flexibility of local planners, which will be detailed in Sections VI and VII.

VI. SIMULATIONS
In this section, we verify the applicability of the proposed EffMoP in simulation.We choose Stage [48] as the simulation platform since its lightweight advantage.

A. Simulation Setup
To make the simulations more realistic and exhibit the real-world cluster, noise, and occlusion effects, the simulation environment is built on top of a map generated from real sensor data.As depicted in Fig. 7, we simulate a 2-D environment based on a map built from the Intel Research Lab data set, which is available from the Radish data set repository [49].The map is constructed by an open-source 2-D laser SLAM system Karto 1 .The size of the environment is approximately 30 × 30 m 2 .

B. Comparison on Global Planning
We randomly select three sets of start poses and goal poses in the simulation environment to test global path planners, as shown in Fig. 7(a)-(c).The state lattice-based path planner and the proposed path planner generate equal quality paths.Therefore, we only show one path in each sub-figure.Table I enumerates some quantitative statistics of path planning results in these three sets of simulations.
1) Comparison on Computational Efficiency: Thanks to the pruning strategy of motion primitives, the search direction of the proposed path planner is focused towards the most promising search areas, and the computational efficiency of graph search is significantly improved.Compared with the state lattice-based path planner, the number of expanded states of the proposed path planner decreases by an average of 66.21%.As a result, planning with the proposed path planner is more than three times faster than planning with the state lattice-based path planner.
2) Comparison on Memory Consumption: Both the state lattice-based path planner and the proposed path planner employ the implicit graph representation.Namely, the memory is allocated according to the need of creating new nodes during the search process, rather than allocating memory for the whole search space in advance.Taking advantage of the pruning strategy of motion primitives, plenty of hopeless search branches are pruned, and the number of the created nodes is reduced.The graph size of the proposed path planner is about 33.87% of that of the state lattice-based path planner.
In conclusion, the proposed path planner generates equal quality paths with much less time and memory consumption than the state lattice-based path planner, which demonstrates the effectiveness of the proposed pruning strategy of motion primitives and implicitly validates that the advantage of the pruning strategy is not incorporated in a sophisticated heuristic such as h 2D .

C. Comparison on Local Planning 1) Comparison on Motion Flexibility:
To compare the motion flexibility of local planners, we make the robot move along the planned path shown in Fig. 7(a).This is an extremely challenging navigation task.Firstly, the robot needs to make a sharp U-turn at the corner, as illustrated in Fig. 7(a) and Fig. 8.This process requires local planners to provide flexible motions.Secondly, after going around the corner, the robot needs to go through a narrow polyline corridor to reach the goal, which requires safe motions.The whole navigation process poses a huge challenge to the flexibility and safety of local planners.
In the test, QBC guides the robot around the corner slowly.At the turning point of the corner, it selects a going-forward motion instead of a turning-left motion, as depicted in Fig. 9(a).In the end, the robot comes to a dead end.All candidate paths are infeasible and the local planner fails.On the contrary, the proposed local planner guides the robot around the Uturn smoothly, as shown in Fig. 9(b).This demonstrates the proposed local planner performs better motion flexibility.
2) Comparison on Motion Efficiency: To compare the motion efficiency of local planners, we make the robot move along the planned paths shown in Fig. 7(b) and (c).The local path generated by QBC is made up of many connected pieces of incomplete Bézier curves and is somewhat rough.On the contrary, the smoothness constraint is explicitly considered in the proposed local path optimization approach.Therefore, the local path generated by the proposed local planner is much smoother than that of QBC.The performance in smoothness is intuitively reflected in motion efficiency.In the tests, the proposed local planner takes 49.78 s and 50.59 s respectively to guide the robot to the goals, while QBC costs 54.43 s and 55.35 s respectively under the same conditions.This supports the proposed local planner has higher motion efficiency.
In conclusion, the proposed optimization-based local planner performs better motion flexibility and efficiency than QBC.

VII. EXPERIMENTS
In this section, we elaborately design three sets of challenging experimental scenarios to verify the efficiency, flexibility, smoothness, and safety of the proposed EffMoP.The experimental results are presented and discussed in detail to validate the superior performance of EffMoP.

A. Experimental Setup
In this work, the experiments are conducted on a Pioneer 3-DX differential-drive robot equipped with a Hokuyo UTM-30LX laser rangefinder, as shown in Fig. 10(a).The maximum linear velocity of the robot is 1.2 m/s.Considering the safety of indoor navigation, we set the upper bound of the linear velocity to 0.7 m/s.The laser rangefinder has a scanning range of 270 • with the angular resolution being 0.25 • , and the effective measurement range is 0.1 m to 30 m. Due to the limited angular range, backward motions are not considered in this work.

B. Comparison on Global Planning
To validate the superior performance of the proposed path planner, Scenario I is designed as depicted in Fig. 10(b).A box is placed in front of the robot as an obstacle.The robot is required to reach the door.
To make a fair comparison on global path planners, we use the same local planner in the test of Scenario I.In both sets of tests, the thread of global path planning is triggered 61 times, and the experimental results are illustrated in Fig. 11.The average number of expanded states and the graph size of the state lattice-based path planner are 1794 and 4226 respectively, while those of the proposed path planner are 978 and 1815  respectively.Compared with the state lattice-based path planner, the computational efficiency and memory consumption of the proposed path planner are improved by 45.48% and 57.05% respectively, which demonstrates the effectiveness of the proposed pruning strategy of motion primitives.

C. Comparison on Local Planning
To validate the flexibility, smoothness, and efficiency of the proposed local planner, Scenario II is designed as illustrated in Fig. 12(a).In this scenario, the robot is required to move along a long corridor and make a sharp right turn to pass through the door.In addition, there are two boxes placed inside the door as obstacles.Such a scenario requires local planners to provide smooth and flexible motions to guide the robot to the goal.
In the test of Scenario II, the proposed local planner exhibits excellent motion efficiency and flexibility.In particular, when the robot approaches the door and needs to make a sharp right turn, the proposed local planner guides the robot through the door smoothly, as shown in Fig. 12(b).In contrast, QBC guides the robot through the door slowly.As illustrated in Fig. 12(c), the endpoints of the offline designed Bézier curves are fixed and cannot be adjusted according to the real-time planning task, limiting the motion flexibility of local planning.We repeat the experiment several times, and the experimental results are presented in Table II.QBC takes approximately 25.76 s to guide the robot to the goal, while the proposed local To validate the advantage of the proposed path/velocity decoupled local planner, we compare it with the popular opensource path/velocity coupled local planner TEB [33], [34].As detailed in Section IV-A, the proposed local path optimization approach is based on a purely geometric formulation consisting of smoothness and safety constraints, while the geometric smoothness of the local path is not incorporated in the formulation of TEB.As a result, the local path obtained by the proposed local planner is obviously smoother than that of TEB, as shown in Fig. 12(d) and (e).We also repeat the experiment several times, and the experimental results are presented in Table II.TEB takes 23.75 s on average to guide the robot to the goal.Compared with TEB, the motion efficiency of the proposed local planner is improved by 16.37%.
Base on the above comparative experimental results, it is concluded that the proposed local planner achieves superior performance in smoothness, motion efficiency, and flexibility.

D. Navigation Experiments
Finally, we test EffMoP in a 92.9 × 26.5 m 2 laboratory environment.As shown in Fig. 13, the robot is required to move from (53.7, 3.8) to (31.0, 4.0).To make the test more challenging, we place some boxes as static obstacles in the long corridor that the robot passes through.Furthermore, we also design a dynamic pedestrian scene to challenge the safety, flexibility, and robustness of EffMoP.The total travel distance of the navigation experiment is approximately 44.2 m, and EffMoP takes 62.83 s to guide the robot to the goal.
Here we summarize several representative experimental results of the navigation experiment to demonstrate the key characteristics of EffMoP.
1) Dealing with static obstacles: Fig. 14(a) illustrates the testing scenario with static obstacles.The robot avoids some boxes and passes through a narrow gap smoothly, according to the reliable local planning results.
2) Dealing with dynamic obstacles: Fig. 14(b) shows the testing scenario with dynamic obstacles.The robot implements fast re-planning and avoids an oncoming person successfully, thanks to the efficient local planner.
3) Passing through the crowd: Fig. 15 shows the robot smoothly passes through the crowd under the guidance of the local planner.This challenging scenario requires the local plan- ner to provide flexible, safe, and smooth motion commands.

VIII. CONCLUSION
In this paper, a three-layer motion planning framework called EffMoP is proposed to address the motion planning problem of mobile robots in complex environments.A novel heuristic-guided pruning strategy of motion primitives is proposed to improve the computational efficiency of graph search.And a soft-constrained local path optimization approach combined with time-optimal velocity planning is presented to generate safe, smooth, and efficient motion commands according to real-time sensor data.Furthermore, the sparse-banded system structure of the underlying path optimization formulation is fully exploited to efficiently solve the problem.Extensive simulations and experiments are presented and discussed to validate that EffMoP has superior performance in terms of safety, smoothness, flexibility, and efficiency.
The objective function of the local path optimization that results from convex and concave terms is non-convex, thus the local planner may get stuck in local optima and is unable to transit across obstacles.In the future, we plan to extend the proposed local path optimization approach with the theory of homology classes [50] to maintain several homotopically distinct local paths and seek global optima.

Fig. 2 .
Fig. 2. Flow chart of the three-layer motion planning framework.

Fig. 3 .
Fig. 3.An illustration of the simplified 2-D search in a 16-connected grid.Obstacles are shown in black, and the gray cells denote the dangerous areas around obstacles.An environment-constrained heuristic path is represented by a red polyline, which provides significant guidance for the original 3-D search in complex environments.

Fig. 4 .
Fig. 4. Illustration of the pruning strategy of motion primitives.Obstacles are shown in black.The red polyline denotes the environment-constrained heuristic path, and the light gray curves represent the designed motion primitives.(a) Planning with the standard state lattice-based path planner.(b)Planning with the pruning strategy of motion primitives.It should be noted that the start and end points of motion primitives are designed in the cell center.To make the illustration clear, we make an appropriate offset.

Fig. 5 .
Fig. 5. Illustration of bilinear interpolation.Bilinear interpolation first performs linear interpolation along the x-axis and then does linear interpolation along the y-axis.Q 0 and Q 1 are the intermediate results of bilinear interpolation in the x-axis.

2N 6 Fig. 6 .
Fig.6.Superposition construction process of the sparse-banded Hessian matrix of the path optimization problem.Top and bottom matrices denote the Hessian matrices of the smoothness and safety constraints, respectively.

Fig. 7 .
Fig. 7. Intel Research Lab.A map derived from real sensor data is used to simulate a 2-D environment.We randomly select three sets of start poses and goal poses to test global path planners, as shown in (a)-(c).The light blue strips indicate the footprints of the robot along the planned paths.

Fig. 11 .
Fig. 11.Comparative experimental results of global path planners in the test of Scenario I. (a) The number of expanded states.(b) The number of nodes in the search graph.

Fig. 12 .
Fig. 12.(a) Experimental scenario II.(b) Planning result of the proposed local planner.(c) Planning result of QBC.(d) Planning result of the proposed local planner.(e) Planning result of TEB.

Fig. 15 .
Fig. 15.Screenshots of the robot passing through the crowd.

TABLE II COMPARISONS
OF MOTION EFFICIENCY (SECONDS) IN SCENARIO