Path Planning for Autonomous Vehicles Using Clothoid Based Smoothing of A* Generated Paths and Optimal Control
Total Page:16
File Type:pdf, Size:1020Kb
DEGREE PROJECT IN MATHEMATICS, SECOND CYCLE, 30 CREDITS STOCKHOLM, SWEDEN 2017 Path planning for autonomous vehicles using clothoid based smoothing of A* generated paths and optimal control MARCUS LUNDBERG KTH ROYAL INSTITUTE OF TECHNOLOGY SCHOOL OF ENGINEERING SCIENCES Path planning for autonomous vehicles using clothoid based smoothing of A* generated paths and optimal control MARCUS LUNDBERG Degree Projects in Scientific Computing (30 ECTS credits) Degree Programme in Applied and Computational Mathematics (120 credits) KTH Royal Institute of Technology year 2017 Supervisor at at Volvo Construction Equipment: Johan Sjöberg Supervisor at KTH: Elias Jarlebring Examiner at KTH: Michael Hanke TRITA-MAT-E 2017:73 ISRN-KTH/MAT/E--17/73--SE Royal Institute of Technology School of Engineering Sciences KTH SCI SE-100 44 Stockholm, Sweden URL: www.kth.se/sci Abstract Autonomous vehicles is a rapidly expanding field and the need for robust and efficient path planners is high. We approach the global path- planning problem for an autonomous load carrier for quarry environments, developed at Volvo Construction Equipment, using two methods: a two- step path planning and path smoothing approach, and a method based on an optimal control formulation of the path planning problem. The two-step method is based on smoothing an initial path found by A*, an efficient grid search algorithm, by fitting a curve consisting of as few clothoid segments as possible to the A* path. The smoothing is done by rewriting the non-linear optimization problem to a convex form by a linearization of the deviation constraints around the curvature of the A* path. An iterative method is then used to relax the `0-norm, which measures the number of non-zero elements in a vector, with a weighted `1-norm which, in turn, is then solved efficiently using CVX in Matlab. The optimal control based path planning method solves the nonlinear optimization problem using IPOPT. It was found that the completeness of the A* algorithm, coupled with the guaranteed solution of a convex problem, resulted in a very robust method that was able to find paths through mazes and difficult situations. The optimal control approach produced better paths, but had a tendency to sometimes show inconsistent behavior. Sammanfattning V¨agplaneringf¨orautonoma fordon med hj¨alpav en klotoid- baserad utj¨amningav A*-genererade v¨agaroch optimal styr- teori. Autonoma fordon ¨arett snabbt v¨axandeforskningsomr˚adeoch behovet av robusta och effektiva l¨osningarf¨orv¨agplanering¨arstort. Vi unders¨oker ett globalt v¨agplaneringsproblem f¨oren autonom dumper som utvecklas p˚aVolvo Construction Equipment och som ¨arbyggd f¨oratt anv¨andasi bergt¨akter. Tv˚ametoder unders¨oks:en tv˚astegsmetod med grafbaserad v¨agplaneringf¨oljtav en utsl¨atningav v¨agen,och en metod som bygger p˚aoptimal styrteori. Tv˚astegsmetoden g˚arut p˚aatt f¨orsthitta en v¨agfri fr˚anhinder med hj¨alpav A*, en effektiv grafbaserad s¨okalgoritm, och sedan sl¨ata ut denna v¨aggenom att anpassa ett s˚al˚agtantal klotoid-kurvor som m¨ojligttill v¨agen.Denna utsl¨atninggenomf¨orsgenom att skriva om det icke-linj¨araproblemet p˚akonvex form med hj¨alpav en linj¨ariseringkring referensv¨agenskurvatur. En iterativ metod beskrivs f¨oratt relaxera `0- normen, som m¨aterantalet nollskilda element i en vektor, med en viktad `1-norm. Sedan l¨osesproblemet med CVX i Matlab. I metoden baserad p˚aoptimal styrteori l¨oservi det icke-linj¨araproblemet direkt med IPOPT. Det visade sig att robustheten hos A* tillsammans med ett l¨attl¨ostkon- vext problem gjorde att tv˚astegsmetoden blev mycket robust och p˚alitlig och kunde hitta till˚atnav¨agargenom labyrinter och sv˚arasituationer. V¨agarplanerade med hj¨alpav optimal styrteori var kortare och hade b¨attreegenskaper, men algoritmen hade en tendens att f˚asv˚arigheter med konvergens. CONTENTS Contents 1 Introduction1 1.1 Objective...............................1 1.2 Introduction to path planning methods...............2 1.3 Autonomous vehicles.........................6 1.4 Levels of autonomy..........................7 1.5 Electric Site..............................9 2 Related work 10 3 Theory 11 3.1 Kinematic vehicle model....................... 11 3.2 Clothoids............................... 12 3.3 Continuous clothoid optimization problem............. 14 3.4 A* grid search............................ 15 3.5 Optimal control theory approach.................. 18 3.6 IPOPT................................ 19 4 Method 20 4.1 A* implementation.......................... 20 4.2 Clothoid smoothing implementation................ 21 4.2.1 Linearization of path around reference curvature..... 23 4.2.2 Calculation of reference curvature............. 25 4.2.3 `0-optimization formulation................. 25 4.2.4 MM-algorithms........................ 26 4.2.5 Relaxation of `0-norm for sparse problems......... 27 4.2.6 MM-formulation of the optimization problem....... 32 4.2.7 Curve length issues...................... 32 4.3 Optimal control implementation.................. 33 5 Numerical simulations 34 5.1 2-phase approach........................... 34 5.1.1 Convergence of `0-norm relaxation............. 37 5.2 Optimal control approach...................... 41 5.2.1 Error for different step sizes................. 44 5.2.2 Influence of the parameter Q................ 45 5.2.3 Empirical stability analysis................. 47 6 Discussion 50 6.1 2-step approach discussion...................... 50 6.2 Optimal control discussion...................... 50 6.2.1 Issues with convergence................... 50 6.3 Future work.............................. 51 7 Conclusion 51 8 References 52 1 INTRODUCTION 1 1 Introduction Research and development on autonomy in vehicles and robots is an area that has gained much popularity in industrial applications over the last three decades [1]. The field has recently seen an increase in usage also in consumer products, such as autonomous driving features in personal cars and drones. The subject is broad and spans many different disciplines. Although there are applications of motion planning within robotics and autonomous vehicles, the subject is also of interest in other areas, such as animation, computer games and computational chemistry [2]. The underlying motivation behind research on autonomous vehicles has been, and still is, to have vehicles independently navigate themselves from some location to a goal, often requiring them to perform one or several tasks along the way. Several general and more problem-specific difficulties have to be overcome. Problems that need to be addressed in virtually all applications of autonomous vehicles are path planning and motion control of the vehicle along the path. The vehicles should be able to calculate driveable paths through their specific type of environment subject to given constraints. Common criteria are to avoid obstacles, static or in some applications moving, while satisfying physical ve- hicle limitations. Constraints can be imposed on the paths, such as curvature limits, path smoothness, and physical actuator limitations. Often position and orientation constraints for the start and goal positions need to be met. This thesis is concerned with the path planning problem for load carriers operating in a quarry environment. 1.1 Objective Two methods for non-holonomic path planning will be examined and compared in this thesis; Firstly, a 2-step lattice based planning and smoothing approach. Secondly, path planning based on an optimal control formulation. The output of both methods are driveable paths with specified boundary values, i.e. coor- dinates and headings for the start and goal positions of the vehicle. A driveable path is one which respects vehicle constraints such as maximum steering angle and physical width limitations. In addition, the path also avoids obstacles and is minimal with respect to path length, i.e. as short as possible. The lattice based method is split into two steps. First, finding an obstacle free path using a grid based search algorithm. This path is the shortest path in the grid but is not driveable. Due to the grid, each segment of the trajectory is limited to one of 8 directions, i.e. vertical, horizontal, and diagonal. This discrete set of possible headings leads to sharp corners in the path that a physical vehicle cannot follow closely, as shown in figure 1.1. 1 INTRODUCTION 2 Figure 1.1: Example of an A* solution on a grid. The path starts at the blue node and ends at the red. The black nodes represent obstacles. This path contains sharp corners and needs to be smoothed before a vehicle can follow it. The obstacle free path through the grid is then smoothed and made drive- able. This is done by fitting curves with continuous piecewise linear curvature, called clothoids, to the original path, while respecting deviation constraints. The appealing properties of clothoids make them a popular choice in path planning applications [1,3,4]. The second method is to formulate the path planning as a nonlinear optimal control problem. It is solved using a tool developed at Volvo Construction Equipment that is based on the solver IPOPT, a software package for large scale nonlinear optimization written in C++ [5]. The problem can be formulated in terms of time or distance travelled along the path. An advantage of a distance based formulation is that the system can be controlled with only one control parameter, in our case the angular steering velocity. In a formulation based on time, a control parameter for acceleration or velocity of the vehicle must