Linköpings universitet
Linköping University | Department of Electrical Engineering
Master’s thesis, 30 ECTS | Path planning and control
2021 | LiTH-ISY-EX–21/5384--SE
Optimal Control and Race Line
Planning for an Autonomous
Race Car
Optimal Styrning och Racelinje-planering för en Autonom
Race-bil
Jacob Olausson
Jacob Larsson
Supervisor : Viktor Leek Examiner : Erik Frisk
Upphovsrätt
Detta dokument hålls tillgängligt på Internet - eller dess framtida ersättare - under 25 år från publicer-ingsdatum under förutsättning att inga extraordinära omständigheter uppstår.
Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner, skriva ut enstaka ko-pior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervis-ning. Överföring av upphovsrätten vid en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av dokumentet kräver upphovsmannens medgivande. För att garantera äktheten, säker-heten och tillgängligsäker-heten finns lösningar av teknisk och administrativ art.
Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman i den omfattning som god sed kräver vid användning av dokumentet på ovan beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan form eller i sådant sammanhang som är kränkande för upphovsman-nens litterära eller konstnärliga anseende eller egenart.
För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/.
Copyright
The publishers will keep this document online on the Internet - or its possible replacement - for a period of 25 years starting from the date of publication barring exceptional circumstances.
The online availability of the document implies permanent permission for anyone to read, to down-load, or to print out single copies for his/hers own use and to use it unchanged for non-commercial research and educational purpose. Subsequent transfers of copyright cannot revoke this permission. All other uses of the document are conditional upon the consent of the copyright owner. The publisher has taken technical and administrative measures to assure authenticity, security and accessibility.
According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected against infringement.
For additional information about the Linköping University Electronic Press and its procedures for publication and for assurance of document integrity, please refer to its www home page: http://www.ep.liu.se/.
© Jacob Olausson Jacob Larsson
Abstract
In autonomous racing, arguably the most impactful modules in terms of lap time min-imization are planning and control. Linköping University’s Formula Student team has re-cently begun an organizational evolution towards building electric and autonomous race cars and this thesis aims to provide a solid foundation for a planning and control solu-tion that the organizasolu-tion can continue to build upon. This thesis compares and evaluates different combinations of planners, controllers, and vehicle models to suggest the combi-nation best suited for a racing scenario. The planners considered in this thesis were two sampling-based planners with different curves connecting the sampled points, and one minimum curvature planner. To control the vehicle to follow the planned trajectory one non-linear model predictive controller (MPC) and one linear MPC were implemented and tested using both a kinematic and a dynamic single-track vehicle model. The optimal com-bination turned out to be the minimum curvature planner with a non-linear MPC using a kinematic vehicle model.
Acknowledgments
First and foremost we want to thank our supervisor Viktor Leek and examiner Erik Frisk for their commitment and engagement during the process of writing this thesis. They pro-vided invaluable ideas and support and the results would have looked very different without their input. Since this thesis is written in collaboration with the driverless team of Linköping University’s Formula Student organization we would also like to thank our team leader Si-mon Bazso as well as our team members Bence Nagy, Filip Berglund, Matheus Bernat, Jesper Jensen, and Simon Tsehaie Russom. We are also thankful for the trust put in us by the presi-dent of the organization Johannes Wenngren and by the rest of the members.
Contents
Abstract iii Acknowledgments iv Contents v 1 Introduction 1 1.1 Motivation . . . 1 1.2 Aim . . . 2 1.3 Research questions . . . 2 1.4 Delimitation . . . 2 1.5 Thesis outline . . . 3 2 Background 4 2.1 Racing fundamentals . . . 42.2 An autonomous vehicle system . . . 5
2.3 Formula student competition . . . 6
3 Theory 7 3.1 Related work . . . 7
3.2 Sampling based planning . . . 8
3.3 Minimum curvature planning . . . 14
3.4 Velocity planning . . . 17
3.5 Model predictive control . . . 19
3.6 Vehicle modelling . . . 20
4 Implementation 25 4.1 Sampling based planner . . . 25
4.2 Minimum curvature planner . . . 27
4.3 Velocity planning . . . 29
4.4 Control . . . 29
5 Experimental setup 32 5.1 Simulation and system architecture . . . 32
5.2 Maps . . . 34
5.3 The simulated vehicle . . . 36
5.4 Methodology . . . 36
6 Results 38 6.1 Planning . . . 38
6.2 Control . . . 51
7.1 Results . . . 60
7.2 Method . . . 65
7.3 Real life scenario . . . 67
7.4 Future work . . . 71
8 Conclusion 72
1
Introduction
Motorsports have been pushing the limits of vehicle engineering ever since its introduction in the early 20th century. The purpose of motorsports is to create the fastest vehicle possible within the boundaries of the competition rules. It has therefore always been striving to im-prove the performance of the car by developing new technologies and inventing innovative solutions. One of the largest motorsport series is the Formula series, where the highest class is Formula One1. In 2014 a class for electric cars was introduced to the collection of Formula series to use and extend the knowledge of electric cars in the vehicle industry. Because of the rapid evolution of autonomous cars in modern society, there have recently been tests to introduce a class for autonomous cars as well. This series is called Roborace2and will run its second testing season in 2020/2021.
There is also a series for engineering students called Formula Student3, which provides a platform for teams of students to test their skills by competing against other teams across the world in building the fastest car. There are 3 categories within the Formula Student competi-tion: combustion, electric and autonomous cars. Formula Student aims to force the students to develop their knowledge in vehicle engineering to reflect the skills needed in the modern vehicle industry. This is partly done by changing the rule book each year to match evolving trends in the industry. Today these skills involve electric and autonomous cars and there-fore the rules from 2022/23 and forward demand that all cars are partly autonomous which provides a whole new challenge for the teams of Formula Student.
1.1
Motivation
LiU Formula Student is Linköping University’s team and has been taking part in the compe-tition since the season of 2012. However, the only category which LiU Formula Student has been taking part in is the combustion category. The organization aims to have a fully func-tional autonomous system by the summer of 2022, but this must be researched and tested long before that. For the season of 2020/21 one department of LiU Formula Student is focusing on researching and implementing an autonomous radio-controlled car as a stepping-stone before building a real autonomous Formula Student car during the 2021/22 season.
1https://www.formula1.com/ 2https://roborace.com/
1.2. Aim
Since LiU Formula Student has never developed an autonomous driving system before, it will take a lot of time to get it to work properly. Therefore, this thesis aims to provide the knowledge needed to create a competitive solution to the planning and control module of the autonomous driving system. A fully autonomous system requires a perception and mapping system as well, but they will not be covered in this master thesis. These modules will be developed by the organization’s driverless department and hopefully integrated with the work of this thesis for a complete system at the end of the 2020/21 season.
1.2
Aim
This thesis aims to investigate and evaluate how to combine a trajectory planner and a motion controller to minimize the lap time of an autonomous race car. To achieve this the algorithm must consider the mechanical boundaries of the vehicle to be able to drive as fast as physi-cally possible. The goal is to provide a researched and documented foundation for a greater understanding of the planning and control problem in autonomous racing and which type of solutions would fit best for the Formula Student competition. In this thesis, we will assume that an internal map of the track already exists, something that would come from a mapping module on the real car.
1.3
Research questions
The thesis investigates the following questions:
1. How can a model predictive controller be implemented in an autonomous race car to track a reference trajectory?
2. How does the choice of the controller’s internal vehicle model impact the lap time? 3. Is using sampling-based planners a feasible approach and how does it compare to
opti-mization based planning for calculating the optimal racing trajectory?
4. What combination of the investigated planners, controllers, and vehicle models pro-duces the best lap time in a robot operating system-based simulation environment?
1.4
Delimitation
The driverless competition rules in Formula Student states that the first lap is a ’recognition lap’, which means that the car can use it to discover the track structure. To make the car travel around the unknown track inside the track limits is a planning and control problem we do not consider in this thesis. This thesis only deals with the planning and control problem from the second lap onwards, when the track boundaries are already known.
The accuracy of the planning and control algorithms heavily depends on the quality of the vehicle model. However, adapting and tuning the model will not be something this thesis will deal with in great detail. The vehicle model used in this thesis will be taken from the simulation environment, which is described in the experimental setup chapter.
1.5. Thesis outline
1.5
Thesis outline
Chapter 2 presents background information related to the thesis. It describes racing funda-mentals, a brief overview of a full autonomous system as well as some information about the Formula Student competition. Chapter 3 covers related work for planning and control as well as the mathematical and algorithmic basis from which the methods used in this thesis derives. How the methods described in Chapter 3 have been implemented in the thesis is covered in Chapter 4. It covers the details behind how the different methods have been used and more specifically how they have been implemented for this thesis. Chapter 5 goes into detail on how the experiments that produced the results were set up, which metrics were measured, and information about the system and platform that was used for the evaluation. In Chapter 6 all the results are presented. The results show how the different methods per-form in simulation according to a set of perper-formance metrics. Thoughts and analyses on the results and the methods are be presented in Chapter 7, together with some predictions about how the system could perform in a real-life scenario and how this work could be built upon. Lastly, in Chapter 8 a summary of the thesis, together with answers to the research questions is given.
2
Background
This section describes the racing fundamentals and how it relates to the planning and control problem as well as the details of the Formula Student driverless competition and an overview of a full autonomous system.
2.1
Racing fundamentals
Racing is all about crossing the finish line faster than your opponents. The race tracks are designed to challenge the drivers and to entertain the audience as much as possible. In some race forms like NASCAR1, the tracks are formed as an ellipse and only contain high-speed corners. In those kinds of competitions, the overtaking skills of the driver and the positioning on the track are vital. High-speed corners are often designed so the vehicle does not need to reduce the speed much to keep the traction and to exit the corner with high velocity. NASCAR cars, therefore, have a much higher average speed than for example a Formula One car2,3,
even though the Formula One car is faster. This is since in Formula One, and in most other race forms, the track consists of a mix of high- and low-speed corners. Low-speed corners are where the good drivers distinguish themselves from the average drivers. An imaginary hairpin corner is shown in Figure 2.1 to demonstrate the different problems the driver faces. In point (A) the braking-zone is illustrated. This is the point where the driver has to brake to reduce the speed enough for the vehicle to turn the corner without losing traction and to hit the apex at maximum speed. The braking zone moves depending on the state of the track and the vehicle. For example, when the track is wet the braking zone is a lot longer than if the track is dry. The apex is illustrated in point (B) and is the point where the driver wants to touch the inside of the corner. One of the most important aspects of cornering, in particular when the corner is followed by a straight, is to maximize the exit velocity. This is done primarily by straightening the wheels as early as possible to be able to accelerate the car without losing traction and spin.
Autonomous race cars have no drivers and the performance in a race solely depends on the skills of the engineers and the intelligence of the system. However, the racing fundamen-tals still exist for an autonomous system, and the same techniques for minimizing the lap time
1https://www.nascar.com/
2https://www.crash.net/f1/feature/937592/1/fastest-f1-tracks-calendar
2.2. An autonomous vehicle system
Figure 2.1: Racing line in a slow speed corner.
apply. This means the system should mimic the driving behavior of a human driver, or even optimize it further. The downside with a human driver is that he or she can not know exactly how the vehicle behaves in any situation. They rely on experience and intuition which, for a professional driver, often puts them close to what is physically possible. A machine however can use sensor data and models to predict the vehicle behaviour in a much more precise man-ner than the human driver. This means that the autonomous vehicle could potentially stay closer to the mechanical and physical boundaries of the vehicle than a human driver ever could.
2.2
An autonomous vehicle system
The pipeline for an autonomous vehicle consists of four main sub-systems: sensors, a per-ception system, a planning system and an activation system. Sensors are used to collect data about the surrounding environment and the current state of the vehicle. Common sen-sors for gathering environmental data are cameras and LiDARs, and in order to collect data about the vehicle IMUs and gyroscopes can be used. The information produced by the sen-sors is compiled in a perception system that takes the data and uses it to creates an internal representation of the surrounding environment. Usually, the representation is a map of the surroundings and the vehicle’s location within it, as well as properties of the vehicle at the current point in time. The exact structure of the representation depends on the scenario, for example, a vehicle driving in a city would benefit from being able to specifically detect traffic lights, something a racing vehicle would not need. Using the data from the perception sys-tem and some sort of goal, a planning syssys-tem can make a strategy of what the vehicle should do next to come closer to realising the goal. The planning can be done both locally and glob-ally. When done locally, the plan produced is only for a finite time into the future and made to reach a sub-goal. The vehicle can then repeatedly replan until the actual goal is reached. This approach fits well for dynamic environments, where objects might move around. On the other hand, global planning creates a plan from the start to the end and usually only needs to be done once. The realisation of the steps in the plan is handled by the activation system, which commonly comes in the form of a controller that produces activation signals, such as how much to steer or accelerate, from the steps in the plan.
2.3. Formula student competition
2.3
Formula student competition
As stated in the introduction, Formula Student is a competition for engineering students to test and hone their skills in vehicle engineering by building and competing with a Formula car. It has been held annually since 1998 and currently consists of three classes: cars with combustion engines (CV), cars with electrical engines (EV) and driverless cars (DV).
Figure 2.2: Trackdrive set-up4.
The competition is divided into a set of static and dynamic events. Most events are the same for all classes and only differs in the driving events. Static events are events not re-lated to the performance of the car and dynamic events are specifically performance rere-lated events. The static events consist of Engineering Design, Cost and Manufacturing and Busi-ness Plan Presentation. The dynamic events for the driverless class are Skidpad, Accelera-tion, Autocross and Trackdrive. The set-up for the Trackdrive event can be seen in Figure 2.2. For electric and combustion classes the Trackdrive event is replaced by an event called En-durance, which is similar to Trackdrive but designed for cars without autonomous capabil-ities. The different dynamic events aim to challenge the car in different ways so that the overall score the team gets on all dynamic events also reflects the overall performance of the car. Trackdrive is the only event specifically for the driverless class and consists of a short track where the car is supposed to drive ten laps as fast as possible. The track is marked by yellow cones on the right side and blue cones on the left and a pair of orange cones for the finish line. Each team gets a maximum of two tries and the best one is counted. The amount of points a team gets is relative to the best performing team in the event, where the best team gets 150 points. Also, the amount of energy consumed during the event is measured as well so that energy efficient vehicles receive extra points. More details of the competition can be found at the Formula Student Germany’s website5.
3
Theory
This chapter covers the theory of trajectory planning using sampling-based methods and optimization techniques, velocity planning as well as the theory of model predictive control. This chapter will also cover previous research on the subject and different solutions to the planning and control problem within autonomous racing in general.
3.1
Related work
In 2017 the AMZ driverless team of ETH Zurich built an autonomous system and competed in the first edition of the Formula Student driverless class [1]. Of interest to this thesis, is their implementation of planning and vehicle control. It mainly consists of an MPC-based controller designed by Liniger et al. [2] that tries to maximize progression along a given centre line and planning using tree expansion of possible paths through a discretized space. The car went on to win all three competitions it participated in. Another team, TUW Racing from TU Wien, has also competed in the driverless class. Their approach to the planning and control part is similar in that the centre line is extracted and an MPC implementation is used to find the current local optimal trajectory [3]. A different approach is taken in [4], where the solution is applied on a smaller RC car. Instead of using solely the centre line, a trajectory planner is used to minimize the lap time. The trajectory is then tracked by a simplified linear time-variant MPC.
3.1.1
Planning
Sampling-based planners are commonly used in the literature to explore unknown environ-ments to find a feasible path by avoiding obstacles [5, 6]. This idea has been applied to autonomous racing by using a computational efficient sampling-based solution, taking ad-vantage of the sparse stable trees (SST) and the RRT* algorithms to plan an optimal trajectory for an RC car [7]. The results showed a planned trajectory comparable to a trajectory driven by a professional RC car driver. An RRT based path planning solution for autonomous ve-hicles using Bezier curves to connect the vertices during the expansion of the tree has also been implemented [8]. The vehicle used in that article is a UAV with motion constraints and the results show a smooth feasible trajectory that the vehicle can follow without violating the motion constraints. Another approach to using Bezier curves for feasible path planning is
3.2. Sampling based planning
presented in [9]. There a pre-existing path of straight segments between vertices is assumed and Bezier curves is applied as a post-processing step to make the path feasible for a car with motion constraints. The results show that the idea of adding curves as a post-processing step is valid and can be done in such a way that the motion constraints of the car are not violated. A similar approach using RRT* to plan a feasible path for a motion constrained vehicle can be taken, but using Dubins curves as the connection between the vertices instead [10]. This means that there is potential for an RRT* algorithm to be used with either Dubins curves or Bezier curves to plan a distance optimal path between the initial node and the goal node.
There also exist planning algorithms for autonomous racing using curvature optimization techniques [11]. Separating geometric path planning and velocity planning to generate a trajectory is commonly done in the literature, and in [11] this separation technique is used to limit the necessary execution time. The results showed that a planning algorithm using these approaches is fast, smooth and has a similar lap time to a time-optimal race line. In his master thesis, Gustavsson [12] presents another similar solution. In his work, the time of the planned trajectory is minimized instead of the curvature. The presented results are similar-looking to when using a curvature optimization technique, but the planning has to be done on several segments of the track independently and combine them as a post-processing step, indicating that the execution time is above acceptable level for the entire track at once.
3.1.2
Control
In previous work related to control of autonomous cars, model predictive control solutions are common due to their ability to handle non-linear behaviours which often occurs when racing. For example, non-linear MPCs have been shown to be able to achieve performance very near a human expert driving an RC car [13]. Another successful implementation of a linear MPC has been done using a minimum time objective [14]. However, even if non-linear MPC has been shown to perform well, they can be somewhat restricted to lower speeds due to the increasing solving times. Therefore linear MPCs have also been explored which could handle higher speeds but generally had slightly larger positional errors [15, 16]. A more recent paper shows the feasibility of non-linear MPCs by comparing a time-optimal formu-lation and a trajectory tracking formuformu-lation using miniature race cars [17]. The results show good performance for both. Compared to [15] a somewhat simplified model is used which does not consider slip, but the paper shows the possibility of using non-linear models with more modern hardware. In this thesis, both linear and non-linear MPCs will be evaluated.
Out of scope for this thesis, the more recent work on the subject of autonomous racing controllers has been focused on integrating learning components into the controllers. Learn-ing model predictive control solutions have been implemented for usLearn-ing data from previous laps to learn how to drive the next lap better [18, 19]. The vehicle learns over time how to drive aggressive and as fast as possible around the track, while still having control over itself. A similar solution is presented in [20], which instead try to learn the vehicle model gradually to correct mismatches in its original model using Gaussian processes.
3.2
Sampling based planning
Planning is the problem of specifying a finite state space within which the system should aim to stay. In autonomous racing, the planned state-space either holds low dimensional states usually containing simply the geometric position in the global coordinate system (called path) or states with higher dimensions typically containing velocity, acceleration etc. beyond the geometric position where each state is parameterized by time (called trajectory).
There exist multiple sampling-based planning algorithms of which the two most cited and used ones are called probabilistic roadmaps (PRM)[21] and rapidly-exploring random trees (RRT)[22]. Both methods use the same fundamental principles by connecting randomly sampled points but differ in the way the graphs are created by connecting these points[23].
3.2. Sampling based planning
The PRM method is multiple-query in the sense that it first creates the graph by combining the randomly sampled points and then provides the shortest path within the graph structure between an initial node and a goal node. RRT on the other hand is a single-query method simply providing the shortest path between an initial node and a goal node by incrementally constructing the graph and return the shortest path whenever the graph is dense enough. As presented by LaValle [22] the RRT method is less computationally heavy than the PRM method since it can keep the connected edges minimal in contrast to the PRM method which generates many connected edges in order to generate a connected roadmap, resulting in a less efficient solution in comparison to the RRT method. This comes from the fact that the RRT method connects the single nearest neighbour when a new sample is generated whereas PRM connects k-nearest neighbours. Because of these results, and the demand for low execution time this thesis will only cover RRT-based solutions.
3.2.1
RRT
RRT is, as previously mentioned, a sampling-based method to generate a graph of nodes and edges in an obstacle-free state space. The algorithm is based on four main functions:
• SAMPLE_FREE(): Returns a randomly sampled state node within the obstacle-free state space.
• NEAREST(V, N): Returns the nearest neighbour to the node N, in the graph V. • STEER(N, K): Returns a new node located as close to node K as possible within a set
of constraints derived from node N.
• OBSTACLE_FREE(N,K): Returns true if all states on the path between node N and node K lies within the obstacle-free state space, otherwise false.
Algorithm 1:RRT V = x_init; E = H; for 1 ... K do
x_rand = SAMPLE_FREE(); x_nearest = NEAREST(V, x_rand); x_new = STEER(x_nearest, x_rand); if OBSTACLE_FREE(x_nearest, x_new) then
V = V Ytx_newu;
E = E Yt(x_nearest, x_new)u; end
end returnV, E
The main characteristics of the algorithm lie in the function STEER(N, K). A common approach when the goal is to find the shortest path between two points is to simply minimize the euclidean distance between the new node P and the randomly sampled point K and max-imize the distance between the nearest point in the graph N and P. The only constraint given in this scenario is the distance r in which the point P at max can deviate from N. An example of an RRT generated tree can be seen in Figure 4.1.
This technique will result in trees with edges in any direction, which can not be followed by a vehicle with motion constraints. Generating feasible trees for a vehicle with motion constraints becomes a more complex problem. It is proposed by Lavelle [22] that input signals u should be calculated before STEER is invoked, passing simply the nearest node N on the graph and the steering signals to STEER(N,u). By introducing additional constraints to u the sampled point P would be feasible.
3.2. Sampling based planning
Figure 3.1: Illustration of STEER(N,K) using only distance as constraint.
3.2.2
RRT*
While the RRT algorithm is fast, it converges to a sub-optimal solution [24]. In 2011 an exten-sion to RRT called RRT* was presented to deal with this problem [23]. The presented algo-rithm introduces the function REWIRE(V, N,∆d) which takes a graph of nodes (V), a new node not included in the graph (N) and a rewire radius (∆d). The function looks for all nodes within the radius∆d of the position of Ns (NEAR(V,∆d)) and connects N along a minimum-cost path, instead of always connecting to the nearest node. Furthermore, it checks for edges from N to all near nodes and if the cost of traversing through N is less than the already ex-isting cost in a neighbouring node, the parent of that node is set to N (REWIRE(V, N,∆d)). REWIRE(V,N,∆d) is invoked after the collision check in the RRT algorithm (Algorithm 1).
Algorithm 2:REWIRE Xnear =NEAR(V, N,∆d);
xmin=NEAREST(V, N);
cmin=Cost(xmin)+c(Path(xmin, N));
for xnearPXneardo
if OBSTACLE_FREE(xnear, N) ^Cost(xnear) + c(Path(xnear,N)) < cminthen
xmin= xnear;
cmin= Cost(xnear) + c(Path(xnear,N));
end end
E = E Yt(xmin, N)};
for xnearPXneardo
if OBSTACLE_FREE(N, xnear) ^
Cost(N) + c(Path(N,xnear)) < Cost(xnear) then
xparent= Parent(xnear);
E = E zt(xparent,xnear)};
E = E Yt(N,xnear)};
end end
In Algorithm 2 the function Cost(K) is the total cost to travel to node K, c(P) is the cost of traversing the path P, Path(K,P) is the path between node K and node P and Parent(K) is the parent node of node K. RRT* provides asymptotic optimality but introduces computational complexity to the algorithm, thus increasing the time required to find a solution.
3.2.3
Sparse-RRT*
Sparse-RRT* is introduced in [7] and based on Sparse-RRT [24]. The main feature of this work is the introduction of the function DRAIN(V,N,∆d) which removes sub-optimal paths
3.2. Sampling based planning
within a finite distance ∆d of N from the graph V. The most time consuming functions in RRT* are the NEAREST(V,N) and NEAR(V,N) functions which both are hard to solve exactly in O(log(n))time [23]. Solving the problem exactly takes O(n)time and by reducing the number of iterations needed, the time for the algorithm to converge will be reduced. DRAIN(V,N,∆d) will be invoked after REWIRE(V,N,∆d).
Algorithm 3:DRAIN Xnear= NEAR(V, N,∆d);
if Cost(N) ě argminxPXnearzNCost(x) then
V = V ztNu; else
for xiPXnearzN do
V = V zxi;
xdel= xi;
while IsLea f(xdel)^xdelPVinactivedo
xnext= Parent(xdel);
V = V zxnext; E = E zt(xnext, xdel)}; xdel= xnext; end end end
The function IsLeaf(N) in this algorithm returns true if N is not an ancestor to another node in V. Another feature in [24], originally introduced in [25] is the proposed substitute to NEAREST(V,N)called BEST_NEAREST(V,N).
Algorithm 4:BEST_NEAREST Xnear =NEAR(V, N,∆d);
if Xnear‰ Hthen
return argminxPXnearCost(x); else
return NEAREST(V, N); end
This modification to NEAREST(V, N) minimizes an arbitrary cost instead of the geomet-ric distance between the nodes which is useful in any scenario where the cost between two nodes is not equal to the euclidean distance. An example of an RRT-tree with DRAIN can be seen in Figure 4.3.
3.2.4
Curves between initial and terminal states
An essential part of generating a path or trajectory is to connect states with curves. The problem is stated as finding a feasible curve connecting an initial state qi with the terminal
state qtwhere the states are denoted q = (x, y, θ), the control signal as u=ρand the cost of the curve is defined as the function L(q, u).
3.2.4.1 Dubins curves
L.E Dubins proved in 1957 [26] that the minimal feasible distance for a vehicle with constant speed and constraints on the maximum steering angle can be expressed as a combination of three motion primitives. The motion primitives are chosen from the set in Table 3.1.
3.2. Sampling based planning
Symbol Steering signal L u = -ρmax
S u = 0
R u = ρmax
Table 3.1: The three motion primitives any Dubins curve are constructed from.
The control signal in each primitive is applied constantly during the entire segment and since ´ρmax ă ρ ă ρmax this can be considered a bounded-curve shortest-path problem [27].
The curve cost, which the goal is to minimize, is describes as:
L(qg, ˆu) =
żtf
0
b
˙x(t)2+ ˙y(t)2 (3.1)
where tf is the time it takes to reach qg. If no solution exists, then L(qg, ˆu) =8.
Since the speed of the vehicle is assumed to be constant, the system can be described as: ˙x=cos(θ)
˙y=sin(θ) ˙θ=uˆ
(3.2)
where ˆu P[´tan(ρmax), tan(ρmax)]. The combination of the motion primitives introduced in
Table 3.1 are called words and since there is no need for two consecutive primitives of the same type (they can be merged into one) 10 possible combinations of motion primitives exist. Of these, Dubins showed that only six are possibly optimal
tLRL, RLR, RSL, LSR, LSL, RSRu. (3.3)
The shortest path between two states can always be described by one of these words, called Dubins curves. Illustrations of two Dubins curves are shown in Figure 3.2.
3.2. Sampling based planning
3.2.4.2 Quadratic Bezier curves
Bezier curves are another common solution to the same problem. A general equation for Bezier curves are
P(t) = n ÿ i=0 Bi,n(t)q(i) 0 ď t ď 1 (3.4) Bi,n(t) = n i (1 ´ t)n´iti (3.5)
where Biis a Bernstein polynomial of degree n and qiis the control points [9]. For n=2, (3.4)
results in a quadratic Bezier curve. These curves consist of three points, where qiand qf are
two of them. The choice of the third point P1 (the control point) will determine the radius of the generated curve. Assuming 0 ď t ď 1 and defining the line between qiand P1 as L1 and
the line between P1 and qf as L2, in each time step the two points Q0, Q1are calculated as
the point on distance dkalong Lk, k P[0, 1]where
d0=||qi´P1||t (3.6)
d1=||P1 ´ qf||t. (3.7)
A new line is formed from Q0 to Q1(L3) and the generated path point at time t (Z) is
calculated as the point d=|Q0´Q1|t from Q0along L3. An illustration of a quadratic Bezier
curve can be seen in Figure 3.3.
Figure 3.3: Illustration of a quadratic Bezier curve.
3.2.4.3 Cubic Bezier curves
The same methodology applies for cubic Bezier curves (and for each polynomial of higher order as well). Instead of three points as was the case in 2nd order polynomial, the cubic Bezier curves require four points, i.e., two control points besides the start and endpoints. The two control points are denoted as P1 and P2. The lines L1, L2 and L3 are defined as the lines
3.3. Minimum curvature planning
between qi and P1, P1 and P2 and P2 and qf respectively. In each time step the points Q0,
Q1and Q2are calculated as previously explained resulting in two lines. The same
method-ology applied on these lines and the generated path point Z can be extracted as illustrated in Figure 3.4.
Figure 3.4: Illustration of a cubic Bezier curve.
3.3
Minimum curvature planning
In [11] a solution to the planning problem for an autonomous race car using optimization techniques to minimize the race line curvature is presented. The parameters to optimize are the distances α from the centre line points p = (x, y)in the direction of the normal n where the race line points r= (ˆx, ˆy) are placed. The race line point riis defined as
¯
ri =p¯i+n¯iαi (3.8)
where α is limited to be less than the distance to the track boundaries (Wl, Wr) along the
normal on each side in order for the path to only pass through free space. To make sure the vehicle can travel along the path, the width of the car (Wcar) is subtracted causing a constraint
on α as: α P[´Wl+ Wcar 2 , Wr´ Wcar 2 ]. (3.9)
The target is to minimize the total squared curvature k2along the racing line containing N points as: min α0,...,αN N ÿ i=0 k2i. (3.10)
The race line can be defined as third-order spline interpolations of the discrete points r in x and y direction separately [11]. Third-order splines are a good representation since it enables the first and second derivative to be calculated at any time. Since the x and y values are
3.3. Minimum curvature planning
calculated separately using the same method, [11] focuses on the x value calculations hence we do the same for the remainder of this section.
The third order spline and its first and second derivative with respect to the time t are stated as: xi(t) =ai+bit+cit2+dit3 (3.11) x1i(t) =bi+2cit+3dit2 (3.12) x2i(t) =2ci+6dit (3.13) ti(s) = s ´ si0 ∆si (3.14) where t is the normalized curvilinear parameter between riand ri+1, starting at the distance
si0. The time ti is, because of the normalized nature of the curvilinear parameter, limited to
the interval 0 ď ti ď1. For all spline segments to connect well and create a globally feasible
path, segment i has to have the same first and second derivative at t=1 as segment i+1 at t = 0. This means that for the points at which the segments overlap the direction of travel, as well as the curvature, has to be equal. Together with the constraints that the start position of the segment must be the same as the position of the sample point and that the end of the segment must be placed at the position of the next sampling point, this yields a globally connected spline containing all points.
To find the spline coefficients, the linear equation system Az = b has to be solved. z is the vector containing the four coefficients and the matrix A and the vector b formulates the constraints described above. To create matrix A the expressions in (3.11), (3.12), (3.13) and (3.14) need to be evaluated depending on the value of t. The dimension of matrix A is 4 ˆN ˚ 4 ˆN where ˆN is the number of interpolated points along the race line and is a repetition of a predefined pattern every four rows (because of the four constraints). The pattern realises the constraints and repeats itself for every pair of points along the path as:
Atemplate = 1 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 1 2 3 0 ´1 0 0 0 0 2 6 0 0 ´2 0 .
The first four columns represents the current point on the reference path i and the last four columns the next point i+1. The rows states the different constraints where the first row is (3.11) evaluated at t= 0, i.e., the start point of the spline which should equal the x-value of i. The second row is the same equations evaluated at t= 1, i.e., the end point of the spline which should equal the x-value of i+1. The third row is (3.12) evaluated at t =1 for i and t=0 for i+1 (subtracted to have target value 0), i.e., to make sure the direction is the same at the end of the current segment as at the start of the next. The last row is (3.13) evaluated at t=1 for i and t=0 for i+1 (subtracted by the same reason as before) which states that the curvature must be the same at the end of the current segment as at the start of the next.
The matrix Atemplate is then, as mentioned earlier, repeated for every pair of points to
create the matrix A. For a more convenient notation the first four columns of Atemplate is
3.3. Minimum curvature planning A= At,1 At,2 0 0 ¨ ¨ ¨ 0 0 0 At,1 At,2 0 ¨ ¨ ¨ 0 0 0 0 At,1 At,2 ¨ ¨ ¨ 0 0 .. . ... ... ... . .. At,2 0 0 0 0 0 ¨ ¨ ¨ At,1 At,2 At,2 0 0 0 0 At,1 .
To be able to minimize the curvature according to (3.10), an expression for the squared curvature k2i, i P[0, N], is required. In [11] a mathematical formula collection [28] is referred to for the derivation of the expression of the squared curvature described as:
k2i = x 1 i2y 2 i2´2x 1 ix 2 iy 1 iy 2 i +y 1 i2x 2 i2 (x1i2+y1 i2)3 (3.15) and by extracting the second derivatives the squared curvature can be written as:
k2i =y2i2 x1i2 (x1i2+y1 i2)3 ´x2i2y 2 i2 ´2x1i2y1i2 (x1i2+y1 i2)3 +x2i2 y1i2 (x1i2+y1 i2)3 . (3.16)
Equation (3.10) thus expands to
min α0,...,αN N ÿ i=0 ¯x2TPxx¯x 2 + ¯y2TPxy¯x 2 +¯y2TPyy¯y 2 (3.17)
where ¯x2 = [x20, x21, ¨ ¨ ¨ , x2N] and ¯y2 = [y20, y21, ¨ ¨ ¨ , y2N]. The matrices Pxx,Pxy and Pyy are
diagonal matrices containing the first derivative factors from (3.16).
Under the assumption that the first derivative in any race line point is approximately the same as the first derivative at the centre line path at that point, the matrices Pxx,Pxyand
Pyy becomes constant. This assumption enables the problem to be treated as a quadratic
programming problem (QP), thus improving the execution time, but limits the quality of the produced race line. To make sure the quality of the race line is retained in addition to treating the problem as QP, the minimum curvature optimization is run multiple times where the race line from the previous iteration is used as the centre line to the next [11].
The second derivatives is calculated from (3.13) for when t=0 as:
x2i =2ci. (3.18)
The same can be done for y2i. The coefficient can be extracted from the z vector, which as described contains all coefficients. The z can be found from b and the inverse of A and since we are only interested in the c coefficient, an extraction matrix Aex,ccan be used to get it from
z:
3.4. Velocity planning
then expanding b, so that the expression is formulated as dependent on α, the resulting ex-pression on vector form becomes:
x21 x22 .. . x2N =2Aex,cA´1( p1,x p2,x 0 0 p2,x p3,x .. . pN,x p1,x 0 0 + n1,x 0 0 0 . . . 0 0 n2,x 0 0 . . . 0 0 0 0 0 . . . 0 0 0 0 0 . . . 0 0 n2,x 0 0 . . . 0 0 0 n3,x 0 . . . 0 .. . ... ... ... . .. ... 0 0 0 0 . . . 0 0 0 0 0 . . . 0 0 0 0 0 . . . nN,x n1,x 0 0 0 . . . 0 0 0 0 0 . . . 0 α1 α2 .. . αN ). (3.20)
To simplify the notation a bit, the matrices Tc, Tn,xand Tn,yare constructed as:
Tc=2Aex,cA´1 (3.21)
Tn,x =2Aex,cA´1Mx (3.22)
Tn,y =2Aex,cA´1My (3.23)
where Mx and My is the normal matrix from (3.20) for the x and y values [11]. Now the
second derivative for both the x and y can be expressed as:
¯x2=Tc¯qx+Tn,xα (3.24)
¯y2=Tc¯qy+Tn,yα (3.25)
where ¯qxand ¯qyare the matrix from (3.20) which contains x and y values for the centre line
nodes p. Using this, the optimization problem in (3.17) can be solved.
3.4
Velocity planning
When a geometric path is constructed it is possible to generate a velocity profile for that path. The goal of the velocity profile is to maximize the velocity in each sample point on the path given the constraints set by the vehicle dynamics. In [29] a commonly cited method to solve this problem using the radius profile of the path is presented. The method is divided into four steps:
1. Identify interest points along with the radius profile.
2. Generate a characteristic velocity profile for each interest point. 3. Generate a characteristic velocity profile for the start and endpoint.
3.4. Velocity planning
Figure 3.5: A general radius profile with identified interest points [29].
The points on the radius profile to be identified are local minimums and points of constant radius. An example of a general radius profile with identified interest points is shown in Figure 3.5 where P1, P2 and P5 are local minimums and P3 and P4 are points of constant radius.
For each interest point, a characteristic velocity profile is then generated. The character-istic depends on the nature of the interest point. If the point is a local minimum the charac-teristic is constructed as full deceleration before and full acceleration after (characcharac-teristic (i), (ii) and (iv) in Figure 3.6). If the point is the start or end of a constant radius section the char-acteristic is constructed as a constant velocity (charchar-acteristic (iii) in Figure 3.6) . The constant velocity should be close (but not equal) to the critical velocity vcriticaldefined as
vcritical = fnmax
c R
m (3.26)
where fnmaxis the maximum lateral force, R is the radius and m the mass of the vehicle. The
characteristics for the points in Figure 3.5 are shown in Figure 3.6.
Figure 3.6: A general velocity profile with characteristics for the interest points [29].
The characteristics in the start point are constructed as full acceleration and the charac-teristic in the endpoint as full deceleration (I respectively II in Figure 3.7). The final velocity
3.5. Model predictive control
profile in point k is the minimum value of all the characteristics in k. The velocity profile generated for a general radius profile is shown in Figure 3.7.
Figure 3.7: The final velocity profile for a general radius profile [29].
3.5
Model predictive control
Model Predictive Control (MPC) revolves around the concept of controlling a system by con-tinuously solving an optimization problem that has the ability to take constraints and physi-cal models into account. The controller tries to minimize a cost function by simulating future states from the current state and the control variables according to a model of the system. These states are bounded by constraints given to the controller so that undesirable states are not reached. The control variables can then be chosen so that the generated states minimize the cost. How far into the future it can simulate depends on a fixed finite prediction hori-zon, for a larger horizon the resulting tracking becomes smoother, but it also becomes more computationally heavy. The resulting control variables can then be used as actuation for the controlled system in that time instant. MPC uses the concept of receding horizon, which means that after an actuation has been applied a new set of control variables is calculated for the new state of the vehicle. This is done repeatedly and helps the controller handle unex-pected behaviours that were not predicted. By continuously applying the optimal actuations the system can be forced towards some desired reference which is reached when the cost function is minimized. To maintain good tracking the update frequency needs to be low.
3.5.1
System dynamics
A model for the system dynamics can be described as the continuous equation:
˙x(t) = f(x(t), u(t)) (3.27) where x(t) P Rn is the system state, u(t) P Rm control inputs and f : RnˆRm the system function. It is not possible to solve the equation analytically and it must therefore be solved numerically, which can be done using one of the Runge-Kutta methods. Resulting in the discrete system dynamics described by:
3.6. Vehicle modelling
where F : RnˆRm is the discretized version of the integral of f and k P N. The time discretization is given by xk = x(tk)and tk+1 =tk+dt for the sampling time dt, where tkis
the time at index k. Due to the discretization there will be slight errors in practical solutions compared to a continuous model, but the error is negligible in this thesis since the system will be in closed loop.
3.5.2
Constraints
As stated, one of the beneficial aspects of MPCs is their ability to take system constraints into account efficiently. It is a crucial component since most real systems are constrained by some boundaries. Linear constraints can be formulated as just a lower and upper bound on some variable as:
¯
x ď xkď ¯x (3.29)
where the variable x can be one of the control variables or a part of the state. Rate of change in a variable can be bounded as well by formulating the constraint as:
¯
∆x ď xk´xk+1ď ¯∆x. (3.30)
The combination of all constraints on the system state and the control variables results in a set of accepted states and variables. The set X represent all allowable states so that xk PX,
similarly the set U represents all allowable control variables so that ukPU.
3.5.3
Problem formulation
To formulate a controller that moves the system to a desired state, the system must be in closed loop. This means the control variables u that are computed at each instance depends on the error between a set of reference states xre f and the simulated states x. For the reference
vehicle states xre f(t)and the simulated states along the prediction horizon x(t), as well as the
control inputs u(t)and the horizon length tmax. An example of a cost function J for reference
tracking can be formulated as:
J=
żtmax
0
(l1||(x(t)´xre f(t))||2+l2||u(t)||2)dt (3.31)
where l1and l2are weights. The cost function penalizes the difference between the simulated
states and the reference states as well as large control inputs so that the desired state is reached with minimum control input. By using the concepts discussed above a general formulation of a model predictive controller can be created, where N=tmax/dt:
minimize u l1||(xN´xN,re f)|| 2+ N´1 ÿ k=0 l1||(xk´xk,re f)||2+l2||uk||2 subject to xk+1=F(xk, uk) xkPX ukPU x0=x(t0). (3.32)
3.6
Vehicle modelling
The system dynamics described by (3.27), is needed for the controller to be able to simulate the change in the state of the vehicle correctly. This is a set of differential equations describing the change in each state variable. There exists many different simplifications of the system model which are more or less detailed but covers the general behaviour of the vehicle. These
3.6. Vehicle modelling
are useful since it is difficult to model a real vehicle exactly and increasing the complexity of the model also quickly increase the complexity of the controller. In this section two com-mon models will be described based on previous work by Althoff et al. [30]. The models are presented in depth in CommonRoad’s documentation on vehicle models1.
3.6.1
Kinematic single-track model
The kinematic single-track model simplifies the modelled vehicle by combining the two front wheels into one, and the same for the tires at the back, making it resemble a bicycle. The model only covers in-plane motion so roll and pitch movements are disregarded, making the bicycle simplification justified. As stated in CommonRoad’s vehicle model documentation the model is popular in work related to motion planning, but the model can differ slightly depending on if the input variables are speed and steering or acceleration and steering ve-locity. It can also differ in how the velocity is described. Some only considers speed along the length of the vehicle [30], which is what will be used in this thesis, while others let it be two-dimensional [31]. The notations of the model can be seen in Figure 3.8.
Figure 3.8: Kinematic single-track model with reference around rear axis.
The state x and the input variables u for the kinematic model are: x= (sx, sy, ψ, vl, δ)T
u= (a, vδ)
T (3.33)
where s denotes the position of the vehicle, vl the speed along the longitudinal axis, ψ the
orientation of the vehicle, δ the steering angle of the front wheels, as well as a and vδwhich are the control variables acceleration and steering angle velocity. The set of differential equations that describe the state change for a kinematic single-track model are then:
˙sx=vlcos(ψ) ˙sy=vlsin(ψ) ˙ ψ= vl lwbtan (δ) ˙v=a ˙δ=vδ (3.34) 1https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf
3.6. Vehicle modelling
where lwb is the wheelbase length of the vehicle from wheel to wheel, it is equal to lf +lr
based on the parameters from Table 3.2.
3.6.2
Dynamic single-track model
The kinematic model is simplified, but it is applicable in many scenarios. However, when the vehicle is driving closer to the edge of its physical capabilities the kinematic model is not enough since tire slip starts to have a bigger impact when the forces on the car increases. For this reason, the dynamic single-track model builds upon the kinematic model by incorporat-ing weight transfer into the model, used in findincorporat-ing the change in slip angle. The model adds the slip angle as a new state variable, which is calculated around the centre of gravity, as well as expanding the derivations of the previous state variables. Since slip is taken into account, it is not possible to keep using only longitudinal speeds since the current change in position now also depends on the current slip angle. However, under the assumption of rear-wheel drive the acceleration is still longitudinal. The notation for the dynamic single-track model can be seen in Figure 3.9.
Figure 3.9: Dynamic single-track model with reference around centre of gravity.
With the addition of the angular velocity vψ and the slip angle β in the state, the state
vector and input variables for the dynamic model are: x= (sx, sy, ψ, v, δ, vψ, β)T
u= (a, vδ)T.
3.6. Vehicle modelling
Based on the discussion above the set of differential equations for the model becomes: ˙sx=vcos(ψ+β) ˙sy=vsin(ψ+β) ˙ ψ=vψ ˙v=a ˙δ=vδ ¨ ψ= µm Iz(lr+lf) (lfCS f(glr´ahc g)δ + (lrCSr(glf +ahc g)´lfCS f(glr´ahc g))β ´(l2fCS f(glr´ahc g) +l2rCSr(glf +ahc g)) vψ v ) ˙β= µ v(lr+lf) (CS f(glr´ahc g)δ ´(CSr(glf +ahc g) +CS f(glr´ahc g))β + (CSr(glf +ahc g)lr´CS f(glr´ahc g)lf) vψ v )´vψ. (3.36)
All vehicle parameters used in (3.36) are described in Table 3.2. As can be seen in 3.36, the equation for the change in angular velocity ¨ψand slip angle ˙β contains a factor 1/v which means for low speeds the model is not solvable. Therefore an alternative model is proposed in CommonRoad’s vehicle model documentation that needs to be introduced temporarily when the vehicle is moving too slow or standing still. This model is a modified version of the kinematic model introduced earlier. The previous kinematic model was formulated around the rear axis so it will need to be reformulated for the centre of gravity as the reference point. Also, the model used a state with no notion of angular velocity or slip angle so these will have to be introduced and their rate of change added to the model. We can obtain the change in angular velocity as the derivative of the change in orientation from (3.34). Similarly, we can calculate the derivative of the slip angle from an expression for slip angle in a kinematic model [32] as
β=tan´1lftan
(δr) +lrtan(δf)
lf +lr (3.37)
where δf and δr is the steering angle of the front and rear wheels, but under the assumption
of only front wheel steering the expression becomes:
β=tan´1lrtan(δ)
lf+lr . (3.38)
The extended kinematic model then becomes: ˙sx=vcos(ψ+β) ˙sy=vsin(ψ+β) ˙ ψ= v lwbtan (δ) ˙v=a ˙δ=vδ ¨ ψ= 1 lr+lf
(tan(δ)cos(β)a ´ sin(β)tan(δ)˙βv+vδvcos(β) cos(δ)2 ) ˙β= 1 1+ (tan(δ)lr+llr f) 2 lr (lr+lf)cos(δ)2vδ. (3.39)
3.6. Vehicle modelling
Symbol Name
Iz Moment of inertia
lr Longitudinal distance from rear wheel to center of gravity
lf Longitudinal distance from front wheel to center of gravity
hcg Center of gravity height
µ Friction coefficient m Mass of the vehicle
Cs f Front cornering stiffness coefficient
Csr Rear cornering stiffness coefficient
g The gravitational force
4
Implementation
This chapter covers how the methods from Chapter 3 have been implemented.
4.1
Sampling based planner
The planners were implemented with the same interface towards the velocity planning and the controllers, allowing comparison between the produced geometric paths independently. S-RRT* was chosen as the sampling-based planner and the base algorithm was implemented as described in Section 3.2.1. The main task was to make sure the motion constraints of the vehicle was taken into consideration when constructing the path. Using solely the euclidean distance as cost and the basic steer function as illustrated in Figure 3.1 constructs the tree in Figure 4.1.
Figure 4.1: RRT generated tree using simple steer function in a hairpin turn at Silverstone circuit.
4.1. Sampling based planner
It is clear that the vehicle will not be able to travel along a path generated by an RRT algorithm with such a STEER function, because of the sharp turns. The sampling of the new points in Algorithm 1 has to use the physical constraints of the vehicle to make sure the planned path is feasible.
4.1.1
STEER
implementation
To sample feasible points during the growth of the RRT tree the STEER function must sample points which can be reached by applying steer signals that does not violate the physical con-straints of the vehicle. Because of the assumed low velocity during sampling, a model with no slip is considered, as commonly done in the literature, which makes it possible to express the relationship between the steering angle and the turning radius [33]:
R=
b l2wb+l2
rcot2δ (4.1)
where R is the resulting turning radius and lwband lrare shown in Figure 3.8 and Figure 3.9
respectively.
It is straightforward to express the steering angle as a function of R given (4.1), meaning the necessary steering angle to reach a specific state can be calculated. Using this information a circle containing the randomly sampled point K and the nearest state N at a point on the circle where the derivative equals the direction in N, can be created. Using the radius R of this circle and (4.1), the necessary δ can be calculated and constrained to ´δmax ă δ ă δmax.
The sampled point P is then calculated when applying the steering signal δ to the state N. An illustration of this STEER implementation is shown in Figure 4.2.
Figure 4.2: Steer function using constraints on steering angle.
The RRT tree created using this approach is feasible for the vehicle to follow as can be seen in Figure 4.3.
4.1.2
Curve inclusion
Even though the sampled points are feasible it is vital to plan the path between the sparse sample points, not the least after the inclusion of REWIRE which reevaluates the connections between the states and connects along an optimal path. There exist two approaches to include
4.2. Minimum curvature planner
Figure 4.3: RRT generated tree using modified steer function in a hairpin turn at Silverstone circuit.
curves between the sampled points. Either they are added after the tree is generated [9] or they are added during the growth of the tree [34, 8]. Adding curves afterwards is compu-tationally cheaper, however, optimality can not be guaranteed. Adding curves continuously will provide an optimal result based on the choice of technique, but will take a longer time to finish. Since the goal of the planning module is to find the optimal path, the curves in the sampling-based planners in this thesis will be added continuously.
Adding curves continuously means to add curves between any newly sampled node N and its parent P, and the cost in each new node should therefore be
Ncost=Pcost+cost(curve(N, P)). (4.2)
In REWIRE where the goal is to connect along a minimum cost path, it is also required that the cost of the rewired tree is calculated as in (4.2).
4.2
Minimum curvature planner
The implemented minimum curvature planner is based on [11] as described in Section 3.3.
4.2.1
Center line extraction
In [11] the center line extraction is vital for good performance. It is assumed that the received map is a binary map that has been filtered before published by the other modules of the vehicle (see Chapter 1.4).
The center line extraction is handled purely as an image processing problem. By applying the euclidean distance transform to the map and selecting one group of pixels outside the track and one group of pixels inside the track as valleys, the watershed algorithm [35] can be used. The image obtained from the watershed algorithm consists of a single line that is located inside the track boundaries and as far away from the borders as possible, i.e., at the center of the track. The three steps can be seen in Figure 4.4.
The coordinates of all zero valued pixels are identified and the order of the center line pixels are determined by finding an arbitrary zero valued pixel and search the neighboring pixels for another zero valued pixel that will be the target for the next iteration in the search. Each found pixel is added to a vector of ordered coordinates and marked as ’visited’, which
4.2. Minimum curvature planner
(a) Original map (b) Euclidean distance map
(c) Extracted center line
Figure 4.4: The center line extraction procedure.
means it can not be found again by the search. This search method works based on the assumption that the extracted center line is one pixel wide, which should be guaranteed by the watershed algorithm.
To be able to interpolate the center line into constant distances between the sample points and to extract the first and second derivative in any such point, a third order spline is fitted to the ordered coordinate vector. However, to enhance the smoothness of the produced spline a Savitzky-Golay filter [36] is applied to the data before fitting. The fitted spline is interpolated at a constant distance around the center line and the normal in each interpolated point is calculated. The distance to the boundaries, Wrand Wl, are then calculated as the least distance
from the interpolated point to a non-zero pixel along the normal.
4.2.2
Optimization
To find the path along the track that minimizes the curvature, an optimization problem is solved in accordance with [11] as was described in Section 3.3. The expression to be mini-mized is (3.17), with ¯x2 and ¯y2 expressed as (3.24) and (3.25). This has been constructed as quadratic programming optimal control problem:
minimize ¯α ¯α TH¯α+ fT¯α subject to αiP[´Wl+ Wcar 2 , Wr´ Wcar 2 ] @i P[1, N] (4.3)
where H and f are given by
H=Tn,xT PxxTn,x+Tn,yT PxyTn,x+Tn,yT PyyTn,y (4.4)
f =2Tn,xT PxxTTc¯qx+Tn,yT PxyTTc¯qx+Tn,xT PxyTTc¯qy+2Tn,yT PyyTTc¯qy (4.5)
which in practice has been implemented using CasADi [37] and solved using the built-in Qrqp solver. Since the derived curvature expression assumes the first derivative of the centre
4.3. Velocity planning
line and the resulting race line to be the same, the optimization can only find an approxi-mately optimal solution. Therefore the optimization is run twice, where the previous solution is used as the centre line in the second run, removing some of the sub-optimalities introduced because of the constant first derivatives.
4.3
Velocity planning
The velocity planning in this thesis is inspired by the work of Velenis and Tsiotras [29] as described in chapter 3.4, but predicts the required action based on future states rather than minimizing over a number of characteristics.
The critical velocity for each sample point on the path is calculated, generating a profile with the maximum speed possible in each such point. The vehicle must stay below the critical velocity at all times to avoid losing traction. To make sure the vehicle brakes in time to hit the critical velocity in each turn the brake distance (sbrake) to each point within the horizon is
calculated as sbrake=v0kt+ 1 2at 2 (4.6) t= V crit x ´vk0 a (4.7)
where x is a point within the horizon and Vxcritis the critical velocity at that point. The velocity in the current point k is vk0and a the maximum deceleration possible. The difference between the required distance and the actual distance has to be larger than a threshold T:
sbrake´ ||k ´ x|| ą T. (4.8)
If this constraint is not fulfilled, the required deceleration to reach Vxcritis set in state k.
However, if this constraint is fulfilled for all points within the horizon the acceleration is set to maximum acceleration.
4.4
Control
The controller solutions implemented in this thesis are a non-linear model predictive con-troller, using both the kinematic vehicle model and the dynamic vehicle model described in Section 3.6, as well as linearized versions of both the models.
4.4.1
Non-linear MPC
The vehicle models from the previous chapter are inherently non-linear and using them di-rectly in the construction of the MPC makes the optimization problem non-convex. The MPC has been formulated as a reference tracker, meaning that it tries to minimize the er-rors between the state x and a set of reference states calculated beforehand. This state error ˜x = x ´ xre f will be minimized along with the control signal so that the system is pushed
4.4. Control
xre f is generated from the planning and velocity profiling. The formulation of this kind of
MPC is minimize u ˜x T NQN˜xN+ N´1 ÿ k=0 (˜xkTQ˜xk+uTkPuk) subject to xk+1=F(tk, xk, uk) xkPX ukPU x0=x(t0) (4.9)
where the matrices P, Q and QNare weights for the state and control variables. The function
F is, as described in Section 3.5, the function which predicts the future states of the system using the non-linear vehicle models and the Runge-Kutta fourth-order method. The state constraints X that the MPC has to adhere to relate to the physical capabilities of the simulated vehicle. This means that the constraints bound the maximum and minimum speed of the vehicle as well as the steering. The constraints for control variables bound the change of those properties, that is maximum and minimum acceleration as well as change in steering. The initial state x0is set as the current state of the car and the initial time t0is set as the time
at the reference state that is closest to the current state of the vehicle. Closest is determined based on euclidean distance. The non-linear MPC has been implemented using CasADi [37] and the Ipopt [38] solver was chosen to solve the optimization problem.
4.4.2
Linear MPC
As stated, the vehicle models are non-linear which makes the optimization problem much more complex and does not guarantee a global minimum solution. This poses two possi-ble propossi-blems, the first being that the solution might not be the best one, the second is that the added complexity makes it take significantly more time to solve. In a racing scenario, where the vehicle is travelling at very high speeds, this drawback could be crucial. Therefore, a linear MPC has been formulated by linearizing the system dynamics on-line around the reference trajectory at each time instant. This is done by a first order Taylor expansion:
˙x(t)« f(xre f(t), ure f(t)) + d f(x(t), u(t) dx(t) ˇ ˇ ˇx=xre f u=ure f (x(t)´xre f(t)) + d f(x(t), u(t) du(t) ˇ ˇ ˇx=xre f u=ure f (u(t)´ure f(t)). (4.10) The change in the error with regards to the reference trajectory can then be extracted by subtracting the first term in (4.10) from itself, which then becomes:
˙˜x(t) = d f(x(t), u(t) dx(t) ˇ ˇ ˇx=xre f u=ure f ˜x(t) + d f(x(t), u(t) du(t) ˇ ˇ ˇx=xre f u=ure f ˜ u(t) (4.11)
where ˜x(t) = (x(t)´xre f(t))and ˜u(t) = (u(t)´ure f(t)). This equation is a linear system and
can be incorporated into the MPC as the prediction model, which now predicts the change in the error relative to the reference trajectory instead of the change in the vehicle state. Since it is linear, the function ˜F uses the forward Euler method to predict the future state, instead
4.4. Control
of fourth-order Runge-Kutta, together with the prediction model. The MPC formulation for this thus becomes:
minimize u ˜x T NQ˜xN+ N´1 ÿ k=0 ˜xkTQ˜xk+u˜TkPu˜k subject to ˜xk+1 =F˜(˜xk, ˜uk, tk) ˜xk PX ˜ ukPU ˜x0= ˜x(t0) (4.12)
where the constraints X and U are formulated with regards to the state and control errors. In practice they become equal to the direct constraints formulated in the non-linear MPC, due to the relation x(t) = ˜x(t) +xre f(t). The same relation also gives the optimal control
variables from the solutions optimal control error. The initial state error is calculated from the reference trajectory and the current state of the car, however, the reference angular velocity and slip angle are not generated from the planner and has to be approximated. Since no slip is preferred, the reference slip angle is set to 0. The angular velocity is approximated using forward finite-difference of the reference angle. The linearized MPC has been implemented using CasADi [37] and the Qrqp solver was chosen to solve the optimization problem.
