Impact of model accuracy on the performance of an optimization-based path planning algorithm for an articulated vehicle

Examensarbete MMK TRITA-ITM-EX 2019:510

Inverkan av modellnoggrannhet på en optimeringsbaserad vägplaneringsalgoritm för

midjestyrda fordon

Deyuan Chen Zhiqiang Yang

Godkänt

2019-8-26

Examinator

Lei Feng

Handledare

Lars Svensson

Uppdragsgivare

Volvo Construction Equipment

Kontaktperson

Viktor Gustavsson

Sammanfattning

Detta examensarbete presenterar en studie av hur valet av fordonsmodell påverkar prestanda hos en optimeringsbaserad vägplaneringsalgoritm, vid planering för en ramstyrd dumper. En traditionell kinematisk cykelmodell och en mer komplex kinematisk ramstyrd modell studeras.

Vägplaneringsalgoritmen genererar en plan i två steg. Först genereras en approximativ plan med hjälp av en modifierad A*-algoritm. Därefter används den approximativa lösningen som initialgissning för en ickelinjär optimeringsprocedur, vilken genererar en optimal lösning.

Planeringsprestanda så som väglängd och beräkningstid jämförs mellan de två modellerna, och

resultat från fullskaliga experiment presenteras. Modellernas prestanda utvärderas med avseende

på magnituden av reglerfelet i lateralt avstånd och orientering från den planerade vägen, när

dumpern körs autonomt med befintlig regulator. Reslutaten indikerar att den ramstyrda modellen

ger högre precision och en mer realistisk plan, men i vissa körfall ger cykelmodellen lika bra

precision, till en lägre beräkningskostnad.

Master of Science Thesis MMK TRITA-ITM-EX 2019:510

Impact of model accuracy on the performance of an optimization-based path planning algorithm for an

articulated vehicle

Deyuan Chen Zhiqiang Yang

Approved

2019-8-26

Examiner

Lei Feng

Supervisor

Lars Svensson

Commissioner

Volvo Construction Equipment

Contact person

Viktor Gustavsson

Abstract

This thesis presents a study on how the accuracy of vehicle kinematic models can affect the performance of an optimization-based path planner. A bicycle model and an articulated machine model are chosen to be different accurate levels of modelling. The path planner generates a final path in two steps. First a coarse path is obtained by an adapted A* algorithm as an initial guess, then the final smooth collision-free path is generated by a nonlinear optimization algorithm according to the initial guess and the selected vehicle model.

The differences regarding path length, success rate and computation time between these two

models are compared in this thesis. The generated paths are further used in real tests as reference

paths of an autonomous articulated hauler. The performance of different models is evaluated by

the lateral error and the heading error. The findings suggest that for the articulated hauler, the

articulated machine model provides a more accurate and more realistic path, yet in some cases, it

can be replaced by the bicycle model that has better time efficiency in computation while

maintaining the performance of the generated path.

FOREWORD

We would like to thank Viktor Gustavsson, Johan Sjöberg and Johanna Huggare in Volvo Construction Equipment for providing us with huge support on research resource. We would like to thank our KTH supervisor Lars Svensson for guiding us and providing many suggestions of completing this thesis. We also would like to thank KTH Professor Lei Feng for examining this thesis. Last but not the least, we would like to thank Fredrik Asplund in KTH for advising us on formulating qualified research questions.

Deyuan Chen & Zhiqiang Yang

Stockholm, August, 2019

Contents

1 Introduction 1

1.1 Background . . . 1

1.2 Purpose . . . 2

1.3 Scope and Delimitation . . . 2

1.4 Ethics and Sustainability . . . 2

1.5 Method . . . 3

2 Related Work 4 3 Theory 5 3.1 Path Planning . . . 5

3.1.1 A* Search Algorithm . . . 5

3.1.2 State Lattice . . . 8

3.2 Optimization Problem . . . 9

3.2.1 Convex Optimization . . . 9

3.2.2 Lagrange Duality . . . 10

3.2.3 Nonlinear Optimization . . . 13

3.3 Modelling . . . 14

3.3.1 Bicycle Model . . . 15

3.3.2 Articulated Machine Model . . . 16

4 Method 19 4.1 Generating Initial Guess . . . 19

4.2 Modelling . . . 21

4.3 Path Optimization with Collision Avoidance . . . 22

5 Experiment Setup 25 5.1 Modelling Parameter . . . 25

5.2 Test Cases . . . 25

5.3 Real Tests . . . 27

5.4 Evaluation Items . . . 27

5.5 Implementation Tools . . . 28

6 Result and Discussion 30

6.2 Real Test Result . . . 37

7 Conclusion 40 8 Future Work 42 Bibliography 43 Appendices 45 A Appendix 46 A.1 Theory . . . 46

A.2 Modelling . . . 47

A.3 Implementation . . . 47

A.4 Division of Work . . . 49

Chapter 1

Introduction

This chapter introduces the autonomous project in Volvo Construction Equipment and the motivation of this thesis.

1.1 Background

Autonomous transport is globally in growing demand and gaining increasing re- search efforts in both academic and industrial area since the last decades[1, 2, 3, 4].

Autonomous transportation improves driving safety by reducing accidents caused by the human element and ensures the comfort of transportation with lower energy consumption. Considerable benefits fuel the development of autonomous transport in the automobile industry.

For construction equipment providers, the demand for the autonomous transport solution is also rocketing and expanding. In many field work tasks, operators are exposed in harsh environments and have enormous workload. These lead to high risk in human safety and high labour cost in the industry with low efficient output.

A fully autonomous vehicle with enough fuel can operate ceaselessly and complete intensive tasks in the extreme environments. Furthermore, the optimization concept behind the autonomy facilitates low energy consumption and high working efficiency since the optimization method can formulate these aspects as objective functions to get an optimal solution. All the above advantages stimulate the advance in the autonomous technology in the construction equipment industry.

Among construction equipment, the articulated hauler is a widely used vehicle consisting of two sections, a tractor and a trailer. A hydraulic hinge connects these two sections and the torsion of this linkage provides the steering capability. This kind of vehicle has an excellent performance in off-road terrains due to full wheel drive and steering mechanism [5]. In terms of an autonomous driving scenario using this articulated hauler, the hauler normally arrives at an assigned loading position in a confined area and goes out of this area after completing some tasks. Given this common scenario, an automatic and computation efficient path planner is needed to generate a feasible path for the autonomous hauler since its working environment

is dynamic and diverse. The operator cannot spend the whole day on waiting for the planner to generate a feasible path which may be not valid anymore due to the changed loading position caused by the changed pile. Intuitively, the path planner should automatically generate paths within a soft time constraint according to the latest map.

1.2 Purpose

The industrial task of this thesis is to develop a path planner for articulated hauler.

In general, most of academic studies in path planning for non-holonimic vehicles use the kinematic bicycle model [6, 7] to ensure dynamic feasibility of the resulting path. In those path planning applications, the kinematic bicycle model represents a reasonable trade-off between accuracy and complexity. However, given the ar- ticulated machine with two-body movement which is the thesis objective, it is less accurate in predicting the motions of such a machine at low speeds [8]. Simplified model may lead to some unexpected situations. For example, the real hauler does not follow the generated path leading to collision with obstacles. On the other hand, a more complex model may significantly increase the computational cost, as the number of states in the model increase. In this thesis, we make comparison in the performance implications of using an articulated machine model [5] and using the traditional bicycle model for the path planning task. We discuss about these two models’ pros and cons.

1.3 Scope and Delimitation

The main focus of this thesis is the impact of model accuracy on the optimization- based path planning algorithm for the articulated machine. The kinematic bicycle model and the kinematic articulated machine model are used to represent a different level of model accuracy of the articulated hauler. The performance of these two models in the same algorithm is evaluated in terms of generated path length, total success rate of generating feasible paths, computation time; the validity of the path is further verified by real tests using an autonomous articulated hauler.

The paths are generated off-line, the path planner is limited in generating paths in small scale off-road environments and the slope of the ground is not considered.

In this thesis, it is assumed that the vehicle is always running slower than 8m/s.

1.4 Ethics and Sustainability

The most relevant ethical consideration is the safety concern. When conducting the practical tests, the test area has to be checked and warning signs have to be set up.

The vehicle is equipped with an emergency stop function, which can be executed remotely if a hazardous condition occurs, for an example, when people accidentally rush into the test area.

CHAPTER 1. INTRODUCTION

In terms of sustainability, the path planning can contribute to fuel economy by designing a path that consumes less fuel. In this thesis, a less-fuel path is generated by limiting the frequency of acceleration and deceleration of the vehicle. Considering the real usage cases, the same amount of work can be done with fewer machines by reducing the computation time of the path planner and the real execution time of the path, which also leads to less fuel consumption.

1.5 Method

The performance of the path planner in this thesis is mainly evaluated by the generated path length, the success rate of generating a feasible path and the com- putation time which is the time of generating a path. Firstly, the longer planned path length means the more fuel consumption. So the planned path should be rea- sonable to reach the target position without any detour. Secondly, path feasibility must be considered in practical applications. The path cannot guide the hauler to move sideways or turn an excessive angle which cannot be achieved by the steering system. The success rate of generating such a feasible path should be taken into consideration in the industry. Last but not least, too much computation time is not tolerated. Because the environment for the hauler is dynamic, the timeliness of the path is required. The above three aspects are mainly affected by the complex- ity of modelling and planning algorithm. In this thesis, the impacts of kinematic modelling on these three aspects are discussed within the same planning algorithm.

The optimization-based method is chosen for path planning because this method allows encoding desired behavior in terms of cost function and constraints. When this method is applied as a path planner, it has proved capable of handling kine- matic constraints and smoothing precomputed trajectories [7]. By applying it, au- tonomous vehicle constraints such as steering angle, speed limit, etc can be easily taken into account. Since it is a model-based approach, the choice of the vehicle model in this thesis has a remarkable impact on the solution generated by this optimization-based method [9].

Many path planning algorithms like A* algorithm [10] mainly focus on dealing with path planning problems by using a point mass model or a bicycle model rather than the model that matches the practical usage scenarios [11]. Since the bicycle model is structurally different from the articulated machine model [12] which is our research object, it is considered to compare the differences between the bicycle model and the articulated machine model given the same path planning algorithm [13] and to study the trade-off between model accuracy and algorithm performance which is mainly reflected by the generated path.

Related Work

The structure of the path planning algorithm in this thesis is based on the works in [14, 15]. In [15] Zhang et al. divided the path planning into two parts, firstly Hybrid A* algorithm [14] was used to compute a coarse trajectory, secondly, the trajectory was recomputed to improve smoothness and to meet the constraints of vehicle dynamics.

In this thesis, a motion primitive set is used in the A* path planning algorithm to explore the search space. Unlike the 4-connected or 8-connected grid space search, motion primitives take the heading direction into consideration, which makes the trajectory more consistent with the vehicle non-holonomic constraints. The generation of the motion primitive set is based on [16, 17]. The authors proposed a method for designing a minimal set of feasible motions to eliminate the redundancies of the search space.

An optimization method is used as the second part of the path planning. The final path is generated as the optimal solution to the optimization problem, of which the path is collision-free and meets the vehicle kinematic constraints. The selected optimization method and collision avoidance algorithm in this thesis are adapted from [13]. The paper utilized the strong duality of convex optimization to formulate the collision avoidance constraints as smooth nonlinear constraints, which can be included in the optimization problem. The problem can be further solved using standard nonlinear solver like Ipopt [18]. In [13], a bicycle model is used to represent the vehicle kinematics and for the collision avoidance algorithm. In addition to the bicycle model, an articulated machine model with two axles is added in this thesis.

These two kinematic models are used to compare the impact of model accuracy on the performance of the optimization-based path planner. The kinematic model of the articulated hauler is built based on [5], where two non-holonomic constraints are applied on the front and rear wheels given the assumption that the hauler moves without slipping of the wheels.

4

Chapter 3

Theory

This chapter contains the theoretical introductions of the methods that are used in this thesis.

3.1 Path Planning

State-of-the-art path planning algorithm includes graph search based planners, sampling-based planners, interpolating curve planners and numerical optimization approaches [7]. All these algorithm groups contain various techniques. In this thesis, a graph search based planner with A* search technique and numerical optimization approaches with function optimization technique are used.

3.1.1 A* Search Algorithm

In this thesis, the first part of the path planning problem is considered as discrete planning, which means that the state space in a problem is finite. The model discussed in this thesis is completely known and predictable so we need not take probability into consideration [19]. For a start, we use a robust and efficient discrete planning algorithm, A* search algorithm [10]. It is regarded as a variant of Dijkstra’s algorithm [20] and can find a minimum cost path from a given state to a required state with less computation cost [19].

A* search is a kind of graph search based planner, which is generally defined as a graph encompassing a set of nodes {ni} which stand for state and a set of actions {ui} which are applied to the current node to explore successor of nodes.

Every transition from the current node to one successor yields a non-negative cost ci. A generated path from the initial node to the goal node consists of sequenced nodes. In a whole generated path, the interval path from one node ni to successor n_i+1exists if and only if ni can access to ni+1 by applying one action in action set {u_i}. As a result, all generated paths from the initial node to the goal node have corresponding total costs by summing associated transition cost ci. Among these paths there exists at least a path which has less total cost than other paths and it

is defined as the optimal path. A* search algorithm finds this optimal path by an evaluation function f(n) [10].

f(n) = g(n) + h(n) (3.1)

where g(n) is the minimum cumulative cost function of the path so far generated by A* and h(n) is a heuristic estimation function incorporating the cost from current node to the goal node [10, 19].

Combining these two cost functions, A* search generates the optimal path and is more computationally efficient than Dijkstra’s algorithm. The A* algorithm retains the generated path with minimum cost because the algorithm keeps updating g(n) of each expanded node when this expanded node is revisited by the current node and g(n) of this expanded node is found to be less than before. In terms of computation efficiency, via using h(n), A* search direction is guided from current node to the goal node so that the total number of nodes expanded from this current node is reduced. A simple path planning problem can be found in Figure 3.1.

Figure 3.1: A* search

The A* algorithm, referring to Algorithm 1, is summarized according to [21]

and we add some details of function Expand() to make the algorithm more clear.

In Algorithm 1, open set is the set that stores the explored nodes which are not expanded yet and sorted by the value of the function (3.1) from low to high.

close setis the set storing the expanded nodes, which is used to check if these nodes are expanded repeatedly. g(s) is the least cost function from the starting node to the current node. h(s) is the heuristic function from the current node to the goal node, which is defined by users according to a specific problem. Besides these two

CHAPTER 3. THEORY

functions, parent(s) is needed to store the parent node information to find the path from goal node back to the starting node in the future.

Algorithm 1 starts from initiating the open set, close set and starting node information including g(s) and h(s). As long as the open set is not empty, it will keep exploring new nodes from the node with the lowest value of function (3.1).

If the node is the goal node, the optimal path is found. If not, expanding this node to explore new nodes via defined direction or path by function Expand().

Every explored node should be checked and updated to make sure its cost is the lowest and its parent node information is updated accordingly by following steps.

First, if the explored node is not in close set and not in the open set, this node is a newly explored node which should be initiated with infinite cost and null parent node. Next step is when this explored node is not in close set, the cost and parent information is updated by function Update(). To some explored nodes, their past costs are higher than the new cost that is summed by current node cost g(s) and transition cost c(s, s⁰). So the cost should be updated if less cost occurs when it is explored by the current node and the parent node is changed to the current expanded node as well. After expanding, checking and updating, the open set is sorted by the value of g(s) + h(s) in ascending order. The above procedure is repeated until either the open set is empty which means no solution found or the explored node is goal node. The optimal path can be traced back according to the parent node information.

Algorithm 1 A* Algorithm

1: function Main()

2: open set= ∅;

3: close set= ∅;

4: g(sstart) = 0;

5: calculate h(sstart);

6: parent(sstart) = sstart;

7: open set.inset(sstart) = (sstart, parent(sstart), g(sstart) + h(sstart));

8: while open set 6= ∅ do

9: s= open set.P opT op();

10: if s= sgoal then

11: return “ solution found ”;

12: end if

13: close set= close set ∪ {s};

14: s set= Expand(s);

15: for every s⁰ ∈ s set do

16: if s⁰ ∈ close set then/

17: if s⁰ ∈ open set then/

18: g(s⁰) = ∞;

19: parent(s⁰) = NULL;

20: end if

21: U pdate(s⁰, s);

22: end if

23: end for

24: open set.sort()

25: end while

26: return “ no solution ”;

27: end function

28:

29: function Expand(s)

30: s set= ∅;

31: for every m ∈ motion set do

32: s⁰ = m(s);

33: s set= s set ∪ s⁰;

34: end for

35: return s set;

36: end function

3.1.2 State Lattice

In this thesis, the path found by A* search algorithm cannot be used as an initial guess for the optimization problem because it does not encode vehicle kinematics, which is very likely to lead to no convergence. Some direction changes generated

8

CHAPTER 3. THEORY

37: function Update(s’, s)

38: if g(s) + c(s, s⁰) ≤ g(s⁰) then

39: g(s⁰) = g(s) + c(s, s⁰);

40: parent(s⁰) = s;

41: if s⁰∈ open set then

42: open set.remove(s⁰);

43: end if

44: open set.insert(s⁰, parent(s⁰), g(s⁰) + h(s⁰));

45: end if

46: end function

by A* search are infeasible in the real application. To improve the coarse path searched by A*, a state lattice approach is proposed [16]. The state lattice is used to formulate a non-holonomic motion planning problem in the manner of graph search and it only incorporates feasible and local connections between states. Similar to A*

search, the state lattice technique converts the planning problem in continuous space into discrete planning problem. But, unlike A*, it connects states while taking the motion constraints into consideration, which is more realistic and computationally efficient [22].

3.2 Optimization Problem

In this thesis, the path generated by the state lattice and A* search is used as an initial guess of a nonlinear optimization function considering the kinematic vehicle model and collision avoidance. The collision avoidance constraints are obtained using the strong duality of convex optimization.

3.2.1 Convex Optimization

Convex optimization problem is used to solve problems of minimizing convex func- tions over convex sets. A typical description of a convex set and a non-convex set is shown in Figure 3.2. A standard convex optimization problem can be expressed

by minimizex V(x)

subject to fi(x) ≤ bi, i= 1, . . . , m, (3.2) Where both the objective function V and the constraint functions f are convex functions. A function f : Rⁿ −→ R is convex when the following constraints are satisfied:

f(αx + βy) ≤ αf(x) + βf(y) α+ β = 1, α ≥ 0, β ≥ 0,

with all x, y ∈ Rⁿ and all α, β ∈ R.

The convex optimization problem is more general than the linear programming problem and the least-squares problem. The last two problems can be described as special cases of general convex optimization problems. When a convex optimization problem is properly structured, it can be solved effectively using methods such as Interior-point methods(also referred to as barrier methods) [23].

Figure 3.2: Convex set and non-convex set

3.2.2 Lagrange Duality

Lagrange duality is widely used in solving optimization problems. The basic idea is to first convert the primal objective function V (x) into a Lagrangian L(x, λ, µ).

Secondly, generate function g(λ, µ), which is the dual function of the Lagrangian L. Within the domain of all variables, it can be proven that the dual function g is always concave and is a lower bound for the optimal value of primal problem V . Therefore it is easier to calculate the maximum value of function g and the result can be used as an approximation of the optimal solution to the primal problem V . Under certain conditions, i.e., when strong duality holds, the solution of the dual problem is equal to the primal problem.

An optimization problem can be transformed into a Lagrange dual problem, which provides another perspective on the primal problem mathematically. In terms of formulating an optimization problem, sometimes it is easier to represent the primal problem in its dual form especially in cases when strong duality holds.

Given a standard optimization problem with objective function V : Rⁿ−→ R, x ∈ D and optimal value of V is denoted by v^∗. The convexity of the constraints or objective function are not restricted:

minimizex V(x)

subject to fi(x) ≤ 0, i = 1, . . . , m, h_j(x) = 0, j = 1, . . . , p.

(3.3)

The Lagrangian L can be defined as the sum of objective function and weighted

10

CHAPTER 3. THEORY

sum of constraints in problem (3.3):

L(x, λ, µ) = V (x) +

m

X

i=1

λ_if_i(x) +

p

X

j=1

µ_jh_j(x), (3.4) with the domain domL = D × R^m₊ × R^p. The vectors λ and µ are referred to Lagrange multiplier vectors associated with the primal problem (3.3).

Define the Lagrangian dual function g : R^m× R^p −→ R, g is the infimum of the Lagrangian L over variable x:

g(λ, µ) = inf

x∈DL(x, λ, µ) = inf

x∈D(V (x) +^Xm

i=1

λifi(x) +

p

X

j=1

µjhj(x)). (3.5) The dual function is always a lower bound for the optimal value v^∗ of the primal problem (3.3).

g(λ, µ) ≤ v^∗. (3.6)

This property can be verified by assuming a feasible point xe to problem (3.3), i.e., fi(xe) ≤ 0, λi ≥0, hj(xe) = 0, µ ∈ R^p, i = 1, 2, . . . m and j = 1, 2, . . . , p. Then from the constraint function:

m

X

i=1

λifi(xe) +

p

X

j=1

µjhj(xe) ≤ 0,

since the first sum is non-positive and the last sum always equals to 0, therefore

L(x, λ, µe ) = V (xe) +

m

X

i=1

λifi(xe) +

p

X

j=1

µjhj(xe) ≤ V (xe) (3.7) holds for all feasible points, an example is shown in figure 3.3. Recall that

g(λ, µ) = inf

x∈DL(x, λ, µ) ≤ L(x, λ, µ).

The inequality (3.6) is established.

Let g^∗ denote the optimal value of dual function g(λ, µ). The inequality holds

g^∗ ≤ v^∗, (3.8)

Even if the primal problem is not convex, the property is called weak duality. If the primal problem (3.3) is convex, the dual function usually has strong duality

g^∗ = v^∗, (3.9)

in this case, the dual problem is equivalent to the primal problem.

Figure 3.3: Lower bound from a dual feasible point. (from [24], page.217) In Figure 3.3, The solid curve represents the objective function V , the dashed curve represents the constraint function f. Since f ≤ 0, the feasible set [−0.46, 0.46]

is indicated by the two dotted vertical lines, point (−0.46, 1.54) represents the op- timal solution and optimal value (x^∗, v^∗). The dotted curve shows the Lagrangian L(x, λ, µ) with different value of Lagrange multiplier λ = 0.1, 0.2, . . . , 1.0. Within the feasible range, each L curve is always smaller than V , which indicates the in- equality (3.7).

12

CHAPTER 3. THEORY

Figure 3.4: The dual function g(λ). (from [24], page.217)

Figure 3.4 shows the dual function of the primal problem presented in Figure 3.3 which is always concave. For the dual function in Figure 3.3, the maximum value is obtained around λ = 0.58, which is regarded as the lower bound of the primal problem. The dashed horizontal line shows v^∗, the optimal objective value of the primal problem. Since the primal problem is not convex, the weak duality holds, i.e., g^∗ ≤ v^∗. For more details see [24], Section 5.

3.2.3 Nonlinear Optimization

A Nonlinear optimization problem can be expressed in the same form as the standard optimization problem (3.2) while the objective function and constraint functions are nonlinear. In most cases, a nonlinear problem is non-convex and therefore challenging to solve. Generally a nonlinear optimization problem can be solved in two ways, global optimization or local optimization [24].

In global optimization, a lot of feasible points are explored and the one reach- ing the minimal value of the objective function is the global solution. The method of exploring feasible points is crucial for the computational time and optimality of the global optimization method. This method generally has low computational efficiency, because the worst-case computing time grows exponentially with the di- mension of variables and the number of constraints. Global optimization is normally used for small scale problems or the problems that are not time critical.

In local optimization, instead of finding a global optimal solution, the algorithm finds a point that is only local optimal, which means the value of the objective function is smaller than the points near it but not necessarily smaller than all other feasible points. This method is not guaranteed to find the global optimal value but increases the convergence speed. Therefore local optimization is widely applicable and can handle large-scale problems. To find a local optimum, a starting point is

required, which is called the initial guess. the value of the initial guess is assigned to the variables before the algorithm starts exploring feasible points. The choice of initial guess is critical and have a strong influence on the value of the optimal objective function and the convergence speed.

Figure 3.5: Example: a nonlinear problem

In Figure 3.5, a nonlinear curve is given, when the initial guess (circle) is given as x = 1.5. A local optimization algorithm tends to find a local optimal solution at point x = 2.4. A global optimization algorithm may find the global optimal solution (x = −2.9) without the initial guess, however, the computation time takes longer.

In terms of solving a nonlinear optimization problem, the interior-point or bar- rier methods provide the capability of handling problems with large numbers of inequality constraints. The interior-point method has been extensively studied and there are many developed efficient tools to solve nonlinear problems by using this method to obtain desirable convergence properties for both global optimization and local optimization [18].

3.3 Modelling

A bicycle model and an articulated machine model are used in the optimization function to describe the kinematics of the hauler. The main difference between them is the steering system. The steering system of the bicycle model is positioned either at the front wheels or at the rear wheels. The wheels can be steered if they are the driving wheels. While the wheels of the front body and rear body are non-steerable in articulated machine model. The steering action is achieved by the hydraulic system in the linkage between the front body and rear body. This steering mechanism makes it more flexible for achieving larger curvature than the bicycle when making turning operation. Due to this difference, when using the optimization method to generate a feasible and collision-free path, the constraints in the optimization function are different in the bicycle model and the articulated

14

CHAPTER 3. THEORY

machine model, which makes the generated path different. Detailed turning radius difference can be referred to Appendix A.2.

3.3.1 Bicycle Model

The typical bicycle model is shown in Figure 3.6. It is considered that the kinematic model is enough to describe the movement of the hauler due to the low driving speed which is around 8m/s . Adding more dynamic details may make the planning and control problems over complicated, so the slip angle and dynamics effect caused by accelerations is neglected. However, one limitation of this model is that the model allows the hauler to make instantaneous steering angle change, which might generate some infeasible path interval [25].

Figure 3.6: Bicycle Model

In this thesis, for the bicycle model, we adopt the steering system in the front wheel. The state of the front wheel can be described as the state vector x :=

[x, y, θ, v, β]. The (x, y) is the center of the front axle, θ corresponds to the heading angle of the model, v is the velocity of the front axle center and the β is the steering angle. The control input of the system is u := [ω, a], ω is the angular velocity of the steering, and a is the longitudinal acceleration. The kinematics of the vehicle can be described as follow:

˙x =v cos(θ + β)

˙y =v sin(θ + β)

˙θ =vsin(β) L

(3.10)

3.3.2 Articulated Machine Model

The kinematic model is selected to create the articulated machine model by making the same assumption for the bicycle model. The typical articulated machine model is shown in Figure 3.7. (x0, y0) and (x1, y1) are the positions of front axle center and rear axle center separately. θ0 and θ1 represent the heading angles for the front body and rear body respectively. β is the steering angle where β = θ0− θ₁. L0 and L1 are wheelbase length from junction to front axle center and to rear axle center correspondingly.

Figure 3.7: Articulated Machine Model

Based on the configuration shown in Figure 3.7, we can get two nonholonomic constraints [5].

˙x0sin θ0− ˙y0cos θ0= 0

˙x1sin θ1− ˙y1cos θ1= 0 (3.11) Two holonomic constraints

x₀ = x1+ L0cos θ0+ L1cos θ1

y₀ = y1+ L0sin θ0+ L1sin θ1

(3.12)

16

CHAPTER 3. THEORY

We can derive the differential equation

˙x0 = ˙x1− L₀ ˙θ₀sin θ0− L₁ ˙θ₁sin θ1

˙y0= ˙y1+ L0 ˙θ₀cos θ0+ L1 ˙θ₁cos θ1

(3.13) Vehicle dynamic, where i = 0, 1

˙xi = vicos θi

˙yi = visin θi

(3.14) As a result, we can derive and describe the front axle and rear axle by the means of kinematic equations with the equations in(3.14) [5]. The derivation of the third equation in (3.16) is given in Appendix A.1.

˙x0 = v0cos θ0

˙y0 = v0sin θ0

˙θ₀= v₀sin β + L1 ˙β L₁+ L0cos β

(3.15)

˙x1 = v1cos θ1

˙y1 = v1sin θ1

˙θ₁= v₁sin β − L0 ˙β L₀+ L1cos β

(3.16)

Considering the geometry of the hauler, the relationship of front axle speed vector and rear axle speed vector is as follows, where the second equation comes from replacing ˙θ0 by the third sub-equation in (3.15).

v₁ = v0cos β + L0sin β ˙θ0

= (L0+ L1cos β)v0+ L0L₁sin β ˙β L₁+ L0cos β

(3.17)

Further, we can derive the relationship of ˙θ1 and ˙β by combining the third line of (3.15), the third line of (3.16) and the first line of (3.17).

˙θ₁= v0sin β − L0cos β ˙β

L₁+ L0cos β , (3.18)

In this thesis, for the articulated machine model, we use the state vector x :=

[x0, y₀, θ₀, v₀, θ₁, β] and the control input of the system is u := [ω, a], where ω = ˙β and a = ˙v0 [12]. Final representation of the kinematics for the articulated machine model is as follows

˙x0= v0cos θ0

˙y0 = v0sin θ0

˙θ₀ = v0sin β + L1˙β L₁+ L0cos β

˙θ₁ = v0sin β − L0cos β ˙β L₁+ L0cos β

(3.19)

In terms of the rear axle center position, we use the geometry constraints to describe.

x₁=x0− L₀cos θ0− L₁cos θ1

y1=y0− L₀sin θ0− L₁sin θ1

(3.20)

18

Chapter 4

Method

In this thesis, a collision-free path is computed in two steps. Firstly, a coarse path is generated by A* algorithm with motion primitives, under the simplification that the hauler is considered as a point mass model. Secondly, the generated path is used as the initial guess for the optimization function, where the kinematic vehicle model and collision avoidance for the full-dimensional model are considered. The path generated from optimization also contains reference speed profile and steering angle velocity, which can be used as the input of the simulation model or the path tracking controller on the real hauler.

4.1 Generating Initial Guess

In this thesis, A* search is the basic algorithm to generate an initial path. Addi- tionally, we adopt two concepts introduced in [22] to adapt A* search algorithm to generate a good initial guess for optimization.

The first concept is the state lattice concept, that is we expand the dimen- sion of the node in general A* search algorithm from two dimensions to four dimensions. In other words, we construct the state vector in each node with (x, y, θ, motion primitive), in which (x, y) stands for the coordinate of the hauler, θ is the heading of the hauler and motion primitive is the interval path from the current node to potential nodes. We also discretize the space of 2π into 24 direc- tions and we have 6 motion primitives for each direction shown in Figure 4.1. These motion primitives are applied into A* search to make the generated path more re- alistic as the initial guess for both bicycle model and articulated machine model.

Motion primitives that we construct in this thesis are based on the hauler’s steering capability and we aim to make the coarse path as smooth as possible.

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 1

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 2

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 3

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 4

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 5

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 6

-4 -3 -2 -1 0 1 2 3 4 -4

-3 -2 -1 0 1 2 3

4 heading is 7

Figure 4.1: Motion Primitive in The First Quadrant, 6 constant trajectories are available for each heading angle.

The second concept is the weight value in the cost function of A* algorithm to generate a feasible path. In the cost function, we balance distance and heading with a certain weight value called Kato make the initial path reasonable enough for solving the optimization problem. When the map is large enough, the distance cost value tends to be much larger than heading cost value, which leads to the state of heading having little impact on pathfinding. The results of different weight values Kaof heading are shown in Figure 4.2. Seen from this figure, different Kainfluence the generated path. Since the final path is generated by improving the initial guess, choosing a proper initial path planner is important. In this thesis, we choose Ka= 1 based on the map size of the current test track.

(a) Ka= 1 (b) Ka= 20

Figure 4.2: Ka influence on A* search path 20

CHAPTER 4. METHOD

4.2 Modelling

In real applications, we need to discretize the continuous state space. Here we transform the bicycle model and the articulated machine model into discretized formats by using the Euler Forward method.

The discrete-time bicycle model is as follows













 xn+1

yn+1

θ_n+1 vn+1

βn+1















=













 xn

yn

θ_n v_n βn













 + T s ·















˙xn

˙yn

˙θ_n

˙vn

˙β_n















(4.1)

where

˙xn=(vn+T s

2 an) · cos(θn+T s

2 ·(vn+T s

2 an) · sin(βn+T s

2 ωn)/L + βn+ T s 2 ωn)

˙yn=(vn+T s

2 a_n) · sin(θn+T s

2 ·(vn+T s

2 a_n) · sin(βn+T s

2 ω_n)/L + βn+T s 2 ω_n)

˙θ_n=(vn+T s

2 an) · sin(βn+T s 2 ωn)/L

˙vn=an

˙β_n=ωn

The articulated machine model is as follows

















 x_0,n+1 y_0,n+1 θ0,n+1

v_0,n+1 θ_1,n+1 βn+1



















=

















 x_0,n y_0,n θ0,n

v_0,n θ_1,n βn

















 + T s ·



















˙x0,n

˙y0,n

˙θ_0,n

˙v0,n

˙θ_1,n

˙β_n



















(4.2)

where

˙x0,n =(v0,n+T s

2 a_n) · cos(θ0,n+T s

2 ·(v0,n+^{T s}₂ a_n) · sin(βn+^{T s}₂ ω_n) + L1ω_n L1+ L0cos(βn+^{T s}₂ ωn) )

˙y0,n =(v0,n+T s

2 an) · sin(θ0,n+T s

2 ·(v0,n+ ^{T s}₂ an) · sin(βn+^{T s}₂ ωn) + L1ωn

L₁+ L0cos(βn+^{T s}₂ ω_n) )

˙θ_0,n =(v0,n+^{T s}₂ an) · sin(βn+^{T s}₂ ωn) + L1ωn

L₁+ L0cos(βn+^{T s}₂ ωn)

˙v0,n =an

˙θ_1,n =(v0,n+^{T s}₂ an) · sin(βn+^{T s}₂ ωn) − L0cos(βn+^{T s}₂ ωn) · ωn

L₁+ L0cos(βn+^{T s}₂ ωn)

˙β_n=ωn

And we get the rear axle state by using Equations (3.17) and (3.20)

x_1,n=x0,n− L₀cos θ0,n− L₁cos θ1,n

y_1,n=y0,n− L₀sin θ0,n− L₁sin θ1,n

v_1,n=(L0+ L1cos βn)v0,n+ L0L₁sin βn· ω_n L1+ L0cosβn

4.3 Path Optimization with Collision Avoidance

The aim of using an optimization method is to obtain a more efficient path which is kinematically feasible, smooth and collision free. The objective function of the optimization problem is as follows.

V =

N

X

k=0

V(xk, u_k)

=

N

X

k=0

0.01 · ωk²+ 0.5 · a²k+

N −1

X

k=0

0.1 · ˙ωk²+ 0.1 · ˙a²k

(4.3)

Generally, the objective function is used to limit the ’consumption’ and the changes in the control inputs so that the path is more smooth and more energy efficient. Coefficients are assigned to regulate the terms and indicate the priorities of the parameters.

The constraints in this optimization problem can be divided into two parts, hauler kinematics and collision avoidance.

The hauler kinematic constraints describe how the hauler should move between each sampling time T s, as represented by the state transfer function (4.2). De- pending on the real hauler specifications and safety concerns, more constraints are required such as maximum acceleration, speed limitation, max steering angle, ve- locity of steering angle, etc. Also, in this optimization problem, the initial state and final state are regarded as constraints.

h(xk, u_k)  0 ⇐⇒



























x₁ = xstart

x_{N +1} = xf inal

|ω_k| ≤ ω_max k= 0, . . . , N − 1

|α_k| ≤ α_max k= 0, . . . , N − 1

|v_0,k| ≤ v_max k= 0, . . . , N

|v_1,k| ≤ v_max k= 0, . . . , N

|β_k| ≤ β_max k= 0, . . . , N

(4.4)

Besides the kinematic constraints, collision avoidance is achieved by incorporat- ing additional constraint on the model states. The collision avoidance algorithm is adapted from [13].

22

CHAPTER 4. METHOD

The full-dimension body of two models are represented by rectangles. One rectangle is used for bicycle model and two rectangles for articulated machine model.

We take articulated machine model as an example to show the collision avoidance implementation. Front body and rear body are represented as E0(xk), E1(xk) where x_k is the state of vehicle at time k. Obstacles are represented by polygons O^{(M )} where M is the total number of obstacles. Since both hauler model and obstacles are defined as convex sets, convex optimization can be applied:

E₀(xk) =R(xk)B0+ t(xk), B0 := { p : G0p  g₀}, E₁(xk) =R(xk)B1+ t(xk), B1 := { p : G1p  g₁},

O^(m) ={ q ∈ Rⁿ: A^(m)q  b^(m)}

(4.5)

Bis the convex set which describes the initial position of the vehicle. The matrix G and g determine the edges of the convex set. E(xk) can be seen as the rotation and translation of the initial convex set B. R is the rotation matrix and t is the translation matrix. Matrix A can be regarded as the normal vector of the obstacle’s edge. p and q are a pair of points which are in vehicle’s convex set and in obstacle’s convex set respectively.

The collision avoidance constraint for the front body at time k is:

E₀(xk) ∩ O^(m) = ∅, ∀m = 1, . . . , M. (4.6) The constraint (4.6) is generally non-convex and not smooth[26], which is not guaranteed to be solvable. To use convex optimization, the constraint is reformu- lated to a convex optimization problem:

arg min

p, q dist

subject to p ∈ E0(xk),

q ∈ O^(m), m= 1, . . . M, dist= ||p − q||

(4.7)

The above formula finds the minimal distance between two closed regions E0(xk) and O^(m). Only when there are no tangent point or intersecting point between two polygons, the dist equals to a positive value, which means no collision is detected.

The same optimization problem can be built for the rear body E1. When the values of dist are positive for both bodies, the articulated machine model is collision-free.

Instead of finding the minimal value of dist, it is enough to evaluate the collision by judging the value range of dist, assuming dmin is the minimum distance allowed between vehicle and obstacle, the vehicle is collision-free when dist at all instances are greater than dmin. Using the strong duality of convex optimization, collision avoidance constraint (4.7) can be reformulated:

dist(E(x), O) > dmin

⇐⇒ ∃ λ 0, µ  0 : −g^>µ+ (A · t(x) − b)^>λ > d_min, G^>µ+ R(x)^>A^>λ= 0, ||A^>λ|| ≤1,

(4.8)

where λ, µ are Lagrange multiplier vectors and the constraints are convex so they can be easily encoded in the optimization problem. Details on the reformulation are found in [13].

Combining objective function (4.3) and constraints (4.4), (4.8), the optimization problem for the full dimensional model can be built:

arg min x, u, λ, µ

N

X

k=0

V(xk, u_k) subject to xk+1= f(xk, uk),

h(xk, xk) ≤ 0,

− g^>µ^(m)_k + (A^(m)· t(xk) − b^(m))^>λ^(m)_k > d_min, G^>µ^(m)_k + R(xk)^>A^(m)>λ^(m)_k = 0,

||A^(m)>λ^(m)_k || ≤1, λ^(m)_k ≥0, µ^(m)_k ≥0, k= 0, . . . , N, m = 1, . . . , M,

(4.9)

where f represents the behavior of the kinematic model, h includes various vehicle constraints and environmental constraints, the rest of constraints ensure that the final path is collision-free.

The nonlinear problem is solved using Ipopt which is a tool for solving nonlinear problems through interior point method. The output is used as the reference input to the autonomous hauler, which includes position, velocity and steering angle.

24

Chapter 5

Experiment Setup

This chapter introduces the design of the test cases and the set up of real tests.

5.1 Modelling Parameter

In this thesis, both models have the same width and the total length of the artic- ulated model is equal to the length of the bicycle model, ie, L = L0+ L1. For the state variables, the steering angle are all limited between β ∈ [−0.6, 0.6] rad and the velocity between v ∈ [−2, 2] m/s. For the control inputs, the acceleration α and the angular velocity ω of the steering angle are limited between α ∈ [−0.5, 0.5] m/s² and ω ∈ [−0.6, 0.6] rad/s. The cost function of the optimization problem consists of the sum of the weighted L2-norms of two control input arrays. The sampling time T = 1 is selected.

5.2 Test Cases

Two scenarios are considered to verify the path planner: reverse parking and trans- portation.

1. Reverse parking scenario.

This scenario aims to test the ability of path planner to find the path in a tight environment using different models. Test cases are divided into two groups.

In the first group, 20 start points are selected evenly in the map with the same heading toward east. In the second group, 5 start points are evenly selected on a line parallel to the X axis. All start points in this group have two initial headings: north and south. Test cases are shown in Figure 5.1.

Figure 5.1: Test Cases: reverse parking. 30 test cases in total.

2. Transportation scenario

The transportation scenario map is created according to the real test track.

In this map, Obstacles 1 to 4 are used to define the boundaries of the test track area. Obstacle 5 to Obstacle 7 are imaginary obstacles that the hauler must avoid. We use the path planner to generate final paths and compare the results. Some of the paths are used in the real hauler tests.

The test cases are shown in Figure 5.2. Test cases are divided into four groups.

In the first three groups, three pairs of start and target points are specified along with four different heading directions corresponding to the arrows in the figure. The paths are computed given all combinations of start directions and target heading directions, that is, 16 cases for each pair of the start point and target point. The fourth group contains several random start points and target points with random heading directions.

26

CHAPTER 5. EXPERIMENT SETUP

Figure 5.2: Test Cases: transportation. 64 test cases in total

5.3 Real Tests

We applied 5 test cases out of 64 test cases in transportation scenario into real hauler test. The validity of the computed path is evaluated by these 5 real test cases. To reduce the impact of external interference each case is repeated 5 times.

An autonomous hauler is used for testing and the data is collected by the Global Navigation Satellite System(GNSS), during each test, both paths generated by the bicycle model and articulated machine model are executed one after the other.

5.4 Evaluation Items

In Section 6.1, we make a comparison between two models in a simulated environ- ment with all test cases mentioned in Figure 5.1 and Figure 5.2. Additionally we show two examples of real test results in Section 6.2.