Research Status of Vehicle Trajectory Planning

Research Article
Open access

Research Status of Vehicle Trajectory Planning

Du Wang 1*
  • 1 School of Electrical and Information Engineering, Beihua University, Jilin, China    
  • *corresponding author 3442060094du@gmail.com
Published on 29 November 2024 | https://doi.org/10.54254/2755-2721/111/2024CH0093
ACE Vol.111
ISSN (Print): 2755-273X
ISSN (Online): 2755-2721
ISBN (Print): 978-1-83558-745-4
ISBN (Online): 978-1-83558-746-1

Abstract

As autonomous driving technology has advanced, it has drawn attention from all around the world. The implementation of autonomous driving technology has the potential to enhance traffic safety, minimize traffic accidents, boost efficiency, facilitate travel, conserve energy, and lower emissions. Autonomous driving technology includes environmental perception, path planning, behavioral decision-making and other technologies. Among them, trajectory planning and control technology is the key technology to realize autonomous driving of automobiles and is the concrete embodiment of automobile intelligence. Graph search algorithms, numerical optimization algorithms, curve fitting algorithms, artificial potential field algorithms, random sampling algorithms, etc. are currently in widespread usage in the field of autonomous driving research. This article will introduce vehicle trajectory planning based on these commonly used algorithms. The necessity of autonomous driving technology research is not only reflected in technological progress, but also covers social security, economic benefits, environmental protection, travel convenience and global competition. By studying autonomous driving technology, humans can better cope with the current challenges of traffic and environment, and at the same time provide strong support for future intelligent transportation and urban planning.

Keywords:

Autonomous driving, Trajectory planning, Algorithm.

Wang,D. (2024). Research Status of Vehicle Trajectory Planning. Applied and Computational Engineering,111,54-59.
Export citation

1. Introduction

By 2023, China's car ownership has exceeded 320 million, continuing to rank first in the world. China's car ownership has grown rapidly, especially in the past 20 years, with the number of cars rising rapidly from less than 20 million in 2000 to 320 million today [1]. Cars play a huge role in people's daily lives and travel. However, the large-scale use of cars has also brought a huge burden to the transportation system, causing a series of traffic accidents. The World Health Organization reports that each year, automobile accidents cause around 50 million injuries and nearly 1.3 million deaths globally [2]. Autonomous driving technology can greatly improve traffic safety by eliminating or reducing human errors. Autonomous driving systems will not get tired, distracted or lose control of their emotions, and can always maintain an efficient response speed and comply with traffic rules. Through precise sensors, real-time data analysis and rapid response, autonomous vehicles can foresee and avoid potential dangers and reduce the occurrence of collision accidents. Autonomous vehicles can also optimize vehicle speed and reduce emergency stops and brakes through real-time communication with other vehicles and infrastructure, ensuring smooth operation of vehicles on the road and avoiding traffic jams. Especially during peak hours in the city, the autonomous driving system can automatically adjust the speed and change lanes to ensure more orderly traffic flow.

The three primary modules of the autonomous driving system are motion control, decision planning, and environmental perception. The environmental perception module of the autonomous driving system is responsible for accurately identifying and understanding the surrounding driving environment through the fusion technology of multiple sensors on board. For the purpose of path planning and control later on, real-time information gathering about the vehicle's own posture, surrounding obstacles' positions and shapes, and traffic signs and markings allow for a detailed understanding of the environmental condition. The fundamental component of autonomous driving cars is decision planning, which has two types of path planning: local path planning and global path planning. They are responsible for different tasks, but cooperate with each other to achieve safe and reliable autonomous driving. Global planning is the generation of an overall driving path from point A to point B by the autonomous driving system based on the starting point and destination. Local planning is to adjust the driving strategy in real time based on the dynamic environment around the vehicle on the basis of the global path. In order to guarantee that the vehicle can drive safely, local planning is in charge of managing real-world driving scenarios, such as dodging obstacles, managing traffic flow, following cars, changing lanes, passing, etc. The main function of the motion control module is to convert high-level planning output (such as paths and trajectories) into control commands that the vehicle actually uses to operate its steering, braking, and power systems. The core goal of the motion control module is to ensure that the vehicle moves safely and smoothly according to the predetermined trajectory, and to ensure driving comfort and safety.

Driving safely and effectively requires autonomous vehicles to behave in a certain way, including changing lanes. At present, the main challenges of lane changing behavior are traffic conditions change frequently, and when changing lanes, it is necessary to judge the speed and position of surrounding vehicles in time to avoid potential dangers. In complex scenes such as urban environments and intersections, lane changing behavior needs to comprehensively consider multiple factors, such as pedestrians, non-motorized vehicles, traffic signals, etc., which increases the difficulty of decision-making. The trajectory planning and decision-making capabilities of autonomous vehicles dictate the accuracy of the control execution system, and the control execution system's quality determines the success of autonomous driving technology. Therefore, studying trajectory planning is a very important topic. This article introduces five widely used algorithms in autonomous driving trajectory planning. The iterative process of each algorithm is shown in this article. Problems solved by the improved algorithm and new problems that arise, the principles of each algorithm, and their advantages and disadvantages in different applications. This article helps researchers and practitioners in the field of autonomous driving to better grasp the nuances of trajectory planning, enabling them to make informed decisions on which algorithms to adopt or improve upon for safer and more efficient autonomous driving systems.

2. Current status of vehicle trajectory planning research

2.1. Graph search algorithm

A set of algorithms known as graph search algorithms is used to locate pathways or look for certain data inside a graph structure (nodes and edges). Among graph search algorithms, the most common algorithms are the Dijkstra algorithm, D algorithm and A* algorithm. Dijkstra proposed Dijkstra's algorithm in 1959 to determine the best route from the beginning to the destination [3]. However, when the starting point and the end point are far apart, the algorithm needs to traverse a large number of nodes, resulting in low operating efficiency. In order to bias the search direction toward the target location, the A* algorithm incorporates heuristic data derived from the Dijkstra algorithm. Reduce the number of node traversals and narrow the search scope, effectively improving the efficiency of the algorithm. The paths generated by the A* algorithm are highly discrete and do not consider moving bodies with non-integrity constraints such as vehicles [4]. The hybrid A* algorithm optimizes the solution through nonlinear optimization based on the A* algorithm to obtain a better global or local optimal solution [5]. The D* algorithm was first proposed by Stentz in 1994. The overall time needed for both the initial path computation and any follow-up re-planning activities can be greatly decreased by using this technique. To achieve complete and optimal planning of paths in a changing environment [6].

2.2. Numerical optimization algorithms

Minimizing the objective function under different constraint function conditions for optimization and solution is the fundamental principle of numerical optimization techniques. However, the computational complexity and sensitivity to initial conditions of numerical optimization algorithms are their main disadvantages. In practical applications, it is usually necessary to balance the solution accuracy and real-time performance of the algorithm and combine other methods to meet specific needs. Eiras et al. [7] proposed an improved constrained optimization method, which transforms the initial planning problem into a nonlinear non-convex optimization problem through a mixed integer linear programming (MILP) model to improve the convergence and solution quality of autonomous driving safety planning. The nonlinear model predictive control (NMPC) method used in the autonomous driving trajectory planner presented by Micheli et al. [8] improved the vehicle trajectory planning system's resilience.

2.3. Curve fitting algorithm

Curve fitting algorithms are used to find a curve that is as close as possible to a set of data points. Curve fitting's primary objective is to identify a curve, by parameter adjustment, that minimizes the error between it and the provided data.

2.3.1. Line segment + Arc. By using a combination of line segments and arcs, the shortest and smoothest path can be generated given the position vectors of two points. The curve fitting algorithm of line segments and arcs is a class of algorithms specifically used to fit curves composed of straight line segments and arc segments. The more representative ones are Dubins and Reeds-Shepp. For vehicles or robots with limited forward direction, the Dubins curve ensures that the path is the shortest in a flat environment without obstacles. The Dubins curve assumes that the vehicle cannot go backward, so it is not suitable for robots or vehicles that can go backward. In addition, this method can only generate the optimal path in a flat environment without obstacles, and needs to be combined with other path planning algorithms when encountering obstacles. The Reeds-Shepp curve allows the vehicle to not only drive forward but also go backward during path planning. This makes the path more flexible, especially for scenarios where the direction needs to be adjusted multiple times. Due to its simplicity, subsequent studies often integrate these methods into other algorithms. For instance, the CL-RRT algorithm uses the Dubins curve to increase the efficacy of sampling point generation [9], while the Hybrid A* algorithm uses the Reeds-Shepp curve to improve the quality of the search path [5].

2.3.2. Polynomial curve. The characteristics of polynomial curves are the geometric continuity of the path and the smooth transition of acceleration. For polynomial curves of appropriate order, both lateral and longitudinal motion parameters can ensure continuous and smooth changes. Werling et al. [10] proposed a control theory method based on the Frenet framework, which transforms the trajectory generation problem into a two-dimensional problem and uses a quintic polynomial function to generate the optimal trajectory to adapt to different tasks and imitate human driving behavior. Wang et al. [11] adopted a point-to-point trajectory planning method, using fourth-order quartic polynomial acceleration and constant speed combined with near-real-time optimization to minimize execution time and improve efficiency.

2.3.3. Bezier curve. Bezier curves are known for their ability to shape curves directly from control points, and only a small number of control points are needed to generate complex and smooth curves. Chen and colleagues [12] presented a fourth-order Bezier curve-based approach for autonomous vehicle trajectory planning. To guarantee the continuity of acceleration and velocity, a continuous trajectory that complies with kinematic constraints is created using parameterized expressions and control point selection. Han et al. [13] used Bézier curves for path planning to achieve path tracking of continuous trajectories, while selecting appropriate control points to ensure the smoothness of the path and avoid obstacles. The chaotic particle swarm optimization (CPSO) approach was used by Tharwat et al. [14] to optimize the control points of the Bezier curve in a route planning model based on Bezier curves, and the result was the shortest and smoothest path between the beginning point and the endpoint.

2.3.4. B-spline curve. Similar to a Bezier curve, the real curve's shape is determined by a group of control points; however, instead of passing through the surrounding control points, the actual curve is sculpted into a smooth curve. A path planning technique based on B-spline curves was suggested by Qu et al. [15] for autonomous vehicles. Using a step-by-step planning model, the method creates an ideal path that satisfies the vehicle's kinematic requirements and is free of collisions.

2.3.5. Spiral. This type of curve is defined in the form of Fresnel integrals. Since the curvature varies linearly with the arc length, a smooth transition between straight and curved segments can be achieved. Kim et al. [16] presented a mobile robot motion planning method based on Bloyd curves. This approach reduces the path's curvature change as well as its length.

2.4. Artificial potential field method

The conventional path planning approach, the artificial potential field algorithm, enables the robot to find a path to the objective via obstacles by synthesizing gravitational and repulsive forces. Although the basic algorithm has problems such as local minima, its application effect in complex environments can be greatly improved by improving the algorithm and combining other technologies, such as reinforcement learning and dynamic programming. Tu et al. [17] proposed an improved artificial potential field algorithm, which solves the problems of unreachable targets, local minima traps and long path lengths encountered by the traditional artificial potential field method in the obstacle avoidance path planning of unmanned aerial vehicles by introducing relative distance, control force and detection factor. A path planning technique for autonomous cars based on obstacle avoidance and an enhanced artificial potential field algorithm was presented by Wang et al. [18]. A safety model was created by examining how drivers avoid impediments, and the artificial potential field method was enhanced to recreate the repulsive field range of obstacles.

2.5. Random sampling algorithm

Path planning issues in high-dimensional and complicated settings are well-suited for the efficient family of algorithms known as random sampling algorithms. By randomly sampling the environment, they investigate viable paths without incurring the substantial computing cost associated with standard grid approaches. The fundamental principle of random sampling algorithms is to progressively get closer to the best route by means of the random point distribution. The probabilistic roadmap (PRM) and the quickly explored random tree (RRT) algorithm are two instances of sampling-based path planning methods. The fundamental principle of PRM is to create nodes in free space by random sampling, connect these nodes with a sparse network (roadmap), and then run a path search across the graph to identify a workable route that leads from the starting point to the destination. It works well in complex static surroundings and high-dimensional spaces. The RRT-Connect approach was introduced by Kuffner et al. [19] in 2000 for single query path planning. It may be utilized for interactive performance assessment and does not require parameter change or preprocessing. In order to tackle difficult geometric path planning issues, this method builds two RRT trees, investigates the initial and target spaces, and then uses a straightforward greedy strategy to proceed. It is especially well-suited for quick searches in high-dimensional space. The RRT technique can locate a feasible path very rapidly, but it cannot ensure that the path found is the best option. Karaman et al. [20] proposed a new algorithm based on RRT*, which ensures asymptotic optimality while maintaining the computationally feasible solution and computational complexity characteristics of the standard RRT algorithm. The algorithm has the characteristic of asymptotically converging to the optimal path. A novel path planning technique called Informed RRT* was presented by Gammell et al. [21]. It directly samples a subset of solutions that can be improved, increasing both the final solution's quality and speed of convergence. The completeness and optimality guarantees of RRT* are maintained by informed RRT*, but it is less dependent on the state dimension and the size of the planning challenge.

3. Conclusions

In the context of today's rapid technological development, autonomous vehicles, as the core of intelligent transportation systems and smart cities, have attracted widespread attention from the global scientific research community and the automotive manufacturing industry. The key to autonomous driving technology lies in precise vehicle control and path tracking to ensure driving safety and efficiency. This article presents the fundamentals of five popular algorithms for autonomous driving trajectory planning, along with the benefits and drawbacks of each application. Trajectory planning for autonomous driving will advance in the future in a more intelligent, adaptable, and secure manner; it will also enhance system performance through cutting-edge techniques and creative thinking, and it will encourage the broad use and popularization of autonomous driving technology. In addition to increasing driving efficiency and safety, this will also facilitate better traffic flow, lessen its negative effects on the environment, and provide the groundwork for the creation of future intelligent transportation systems.


References

[1]. Ministry of Public Security website. (2024). The number of motor vehicles in China reached 435 million, the number of drivers reached 523 million, and the number of new energy vehicles exceeded 20 million https://www.gov.cn/lianbo/bumen/202401/content_6925362.htm

[2]. Xing, X., Zhang, Y., Shen, X. Many countries explore road traffic safety measures[N]. People's Daily, 2022-08-22(016)

[3]. Dijkstra, E. W. (2022). A note on two problems in connexion with graphs. In Edsger Wybe Dijkstra: his life, work, and legacy (pp. 287-290)

[4]. Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A formal basis for the heuristic determination of minimum cost paths. IEEE transactions on Systems Science and Cybernetics, 4(2), 100-107.

[5]. Dolgov, D., Thrun, S., Montemerlo, M., & Diebel, J. (2010). Path planning for autonomous vehicles in unknown semi-structured environments. The international journal of robotics research, 29(5), 485-501

[6]. Stentz, A. (1995). The focussed d^* algorithm for real-time replanning. In IJCAI (Vol. 95, pp. 1652-1659)

[7]. Eiras, F., Hawasly, M., Albrecht, S. V., & Ramamoorthy, S. (2021). A two-stage optimization-based motion planner for safe urban driving. IEEE Transactions on Robotics, 38(2), 822-834

[8]. Micheli, F., Bersani, M., Arrigoni, S., Braghin, F., & Cheli, F. (2023). NMPC trajectory planner for urban autonomous driving. Vehicle system dynamics, 61(5), 1387-1409

[9]. Kuwata, Y., Teo, J., Fiore, G., Karaman, S., Frazzoli, E., & How, J. P. (2009). Real-time motion planning with applications to autonomous urban driving. IEEE Transactions on control systems technology, 17(5), 1105-1118

[10]. Werling, M., Ziegler, J., Kammel, S., & Thrun, S. (2010). Optimal trajectory generation for dynamic street scenarios in a frenet frame. In 2010 IEEE international conference on robotics and automation (pp. 987-993). IEEE

[11]. Wang, H., Wang, H., Huang, J., Zhao, B., & Quan, L. (2019). Smooth point-to-point trajectory planning for industrial robots with kinematical constraints based on high-order polynomial curve. Mechanism and Machine Theory, 139, 284-293

[12]. Chen, C., He, Y., Bu, C., & Han, J. (2015). Feasible trajectory planning for unmanned vehicles based on fourth-order Bezier curves. Acta Automatica Sinica (03), 486-496.

[13]. Han, L., Yashiro, H., Nejad, H. T. N., Do, Q. H., & Mita, S. (2010). Bezier curve based path planning for autonomous vehicle in urban environment. In 2010 IEEE intelligent vehicles symposium (pp. 1036-1042). IEEE

[14]. Tharwat, A., Elhoseny, M., Hassanien, A. E., Gabel, T., & Kumar, A. (2019). Intelligent Bézier curve-based path planning model using Chaotic Particle Swarm Optimization algorithm. Cluster Computing, 22, 4745-4766

[15]. Qu, P., Li, L., Ren, X., & Jing, L. (2016). Path planning algorithm for unmanned vehicles based on B-spline curve. Computer Knowledge and Technology (26), 235-237.

[16]. Kim, Y. H., Park, J. B., Son, W. S., & Yoon, T. S. (2017). Modified turn algorithm for motion planning based on clothoid curve. Electronics Letters, 53(24), 1574-1576

[17]. Tu, K., Hou, H., Su, W. (2022). Unmanned aerial vehicle obstacle avoidance path planning based on improved artificial potential field method. Journal of Xi'an Technological University (02), 170-177.

[18]. Wang, P., Gao, S., Li, L., Sun, B., & Cheng, S. (2019). Obstacle avoidance path planning design for autonomous driving vehicles based on an improved artificial potential field algorithm. Energies, 12(12), 2342

[19]. Kuffner, J. J., LaValle, S. M. (2000, April). RRT-connect: An efficient approach to single-query path planning. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065) (Vol. 2, pp. 995-1001). IEEE

[20]. Karaman, S., Frazzoli, E. (2010, December). Optimal kinodynamic motion planning using incremental sampling-based methods. In 49th IEEE conference on decision and control (CDC) (pp. 7681-7687). IEEE

[21]. Gammell, J. D., Srinivasa, S. S., Barfoot, T. D. (2014). Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic. In 2014 IEEE/RSJ international conference on intelligent robots and systems (pp. 2997-3004). IEEE


Cite this article

Wang,D. (2024). Research Status of Vehicle Trajectory Planning. Applied and Computational Engineering,111,54-59.

Data availability

The datasets used and/or analyzed during the current study will be available from the authors upon reasonable request.

Disclaimer/Publisher's Note

The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of EWA Publishing and/or the editor(s). EWA Publishing and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

About volume

Volume title: Proceedings of CONF-MLA 2024 Workshop: Mastering the Art of GANs: Unleashing Creativity with Generative Adversarial Networks

ISBN:978-1-83558-745-4(Print) / 978-1-83558-746-1(Online)
Editor:Mustafa ISTANBULLU, Marwan Omar
Conference website: https://2024.confmla.org/
Conference date: 21 November 2024
Series: Applied and Computational Engineering
Volume number: Vol.111
ISSN:2755-2721(Print) / 2755-273X(Online)

© 2024 by the author(s). Licensee EWA Publishing, Oxford, UK. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license. Authors who publish this series agree to the following terms:
1. Authors retain copyright and grant the series right of first publication with the work simultaneously licensed under a Creative Commons Attribution License that allows others to share the work with an acknowledgment of the work's authorship and initial publication in this series.
2. Authors are able to enter into separate, additional contractual arrangements for the non-exclusive distribution of the series's published version of the work (e.g., post it to an institutional repository or publish it in a book), with an acknowledgment of its initial publication in this series.
3. Authors are permitted and encouraged to post their work online (e.g., in institutional repositories or on their website) prior to and during the submission process, as it can lead to productive exchanges, as well as earlier and greater citation of published work (See Open access policy for details).

References

[1]. Ministry of Public Security website. (2024). The number of motor vehicles in China reached 435 million, the number of drivers reached 523 million, and the number of new energy vehicles exceeded 20 million https://www.gov.cn/lianbo/bumen/202401/content_6925362.htm

[2]. Xing, X., Zhang, Y., Shen, X. Many countries explore road traffic safety measures[N]. People's Daily, 2022-08-22(016)

[3]. Dijkstra, E. W. (2022). A note on two problems in connexion with graphs. In Edsger Wybe Dijkstra: his life, work, and legacy (pp. 287-290)

[4]. Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A formal basis for the heuristic determination of minimum cost paths. IEEE transactions on Systems Science and Cybernetics, 4(2), 100-107.

[5]. Dolgov, D., Thrun, S., Montemerlo, M., & Diebel, J. (2010). Path planning for autonomous vehicles in unknown semi-structured environments. The international journal of robotics research, 29(5), 485-501

[6]. Stentz, A. (1995). The focussed d^* algorithm for real-time replanning. In IJCAI (Vol. 95, pp. 1652-1659)

[7]. Eiras, F., Hawasly, M., Albrecht, S. V., & Ramamoorthy, S. (2021). A two-stage optimization-based motion planner for safe urban driving. IEEE Transactions on Robotics, 38(2), 822-834

[8]. Micheli, F., Bersani, M., Arrigoni, S., Braghin, F., & Cheli, F. (2023). NMPC trajectory planner for urban autonomous driving. Vehicle system dynamics, 61(5), 1387-1409

[9]. Kuwata, Y., Teo, J., Fiore, G., Karaman, S., Frazzoli, E., & How, J. P. (2009). Real-time motion planning with applications to autonomous urban driving. IEEE Transactions on control systems technology, 17(5), 1105-1118

[10]. Werling, M., Ziegler, J., Kammel, S., & Thrun, S. (2010). Optimal trajectory generation for dynamic street scenarios in a frenet frame. In 2010 IEEE international conference on robotics and automation (pp. 987-993). IEEE

[11]. Wang, H., Wang, H., Huang, J., Zhao, B., & Quan, L. (2019). Smooth point-to-point trajectory planning for industrial robots with kinematical constraints based on high-order polynomial curve. Mechanism and Machine Theory, 139, 284-293

[12]. Chen, C., He, Y., Bu, C., & Han, J. (2015). Feasible trajectory planning for unmanned vehicles based on fourth-order Bezier curves. Acta Automatica Sinica (03), 486-496.

[13]. Han, L., Yashiro, H., Nejad, H. T. N., Do, Q. H., & Mita, S. (2010). Bezier curve based path planning for autonomous vehicle in urban environment. In 2010 IEEE intelligent vehicles symposium (pp. 1036-1042). IEEE

[14]. Tharwat, A., Elhoseny, M., Hassanien, A. E., Gabel, T., & Kumar, A. (2019). Intelligent Bézier curve-based path planning model using Chaotic Particle Swarm Optimization algorithm. Cluster Computing, 22, 4745-4766

[15]. Qu, P., Li, L., Ren, X., & Jing, L. (2016). Path planning algorithm for unmanned vehicles based on B-spline curve. Computer Knowledge and Technology (26), 235-237.

[16]. Kim, Y. H., Park, J. B., Son, W. S., & Yoon, T. S. (2017). Modified turn algorithm for motion planning based on clothoid curve. Electronics Letters, 53(24), 1574-1576

[17]. Tu, K., Hou, H., Su, W. (2022). Unmanned aerial vehicle obstacle avoidance path planning based on improved artificial potential field method. Journal of Xi'an Technological University (02), 170-177.

[18]. Wang, P., Gao, S., Li, L., Sun, B., & Cheng, S. (2019). Obstacle avoidance path planning design for autonomous driving vehicles based on an improved artificial potential field algorithm. Energies, 12(12), 2342

[19]. Kuffner, J. J., LaValle, S. M. (2000, April). RRT-connect: An efficient approach to single-query path planning. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065) (Vol. 2, pp. 995-1001). IEEE

[20]. Karaman, S., Frazzoli, E. (2010, December). Optimal kinodynamic motion planning using incremental sampling-based methods. In 49th IEEE conference on decision and control (CDC) (pp. 7681-7687). IEEE

[21]. Gammell, J. D., Srinivasa, S. S., Barfoot, T. D. (2014). Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic. In 2014 IEEE/RSJ international conference on intelligent robots and systems (pp. 2997-3004). IEEE