Flexible trajectory planning by a model AGV

(1)

Flexible trajectory

planning by a

model AGV

A research assignment

by

L.A. van der Plas

Project numer: 2018.TEL.8289 Student number: 4296702

Project duration: September 24, 2018 – December 21, 2018

(2)

ii

(3)

Delft University of Technology

FACULTY MECHANICAL, MARITIME AND MATERIALS ENGINEERING

Department Marine and Transport Technology Mekelweg 2 2628 CD Delft the Netherlands Phone +31 (0)15-2782889 Fax +31 (0)15-2781397 www.mtt.tudelft.nl

Specialization:

Transport Engineering and Logistics

Report number:

2018.TEL.8289

Title:

Flexible trajectory

planning by a model AGV

Author:

L.A. van der Plas

Title (in Dutch) Flexibele traject planning door een model AGV

Assignment: Research assignment Confidential: no

Initiator (university): ir. M.B. Duinkerken Supervisor: ir. M.B. Duinkerken

Date: December 21, 2018

This report consists of 78 pages and 2 appendices. It may only be reproduced literally and as a whole. For commercial purposes only with written authorization of Delft University of Technology. Requests for consult are only taken into consideration under the condition that the applicant denies all legal rights on liabilities concerning the contents of the advice.

(4)

iv

(5)

Preface

This report is written as part of a research assignment conducted for the study Transport Engineering and Logistics at the Delft University of Technology.

Due to the booming economy, and the increasing demand for transportation, container terminals have to keep innovating. Increasing efficiency through automation seems to be a promising solution and I am glad to be able to contribute to new developments in this area. This research assignment threw me in the world of container terminal automation and helped me develop multiple technical skills.

I would like to thank Olof for always trying to help me with technical details in the labo-ratory and always being prepared to discuss new ideas.

At last I would like to thank Mark Duinkerken for providing the opportunity to dive into this field of transportation and his supervision and feedback during the project.

L.A. van der Plas Delft

(6)

(7)

A.1.4 Optitrack . . . 49 B Code 53 B.1 Proof of Concept . . . 53 B.1.1 TalkerAGV . . . 53 B.1.2 ListenerAGV . . . 53 B.1.3 Arduino code . . . 54 B.1.4 Matlab plotter . . . 55 B.2 Implementation . . . 55 B.2.1 Initialization . . . 55 B.2.2 Obstacle detection . . . 56 B.2.3 Path planning . . . 57 B.2.4 Complete code . . . 58

(9)

List of Figures

1.1 Examples of fixed paths between QC and GC [37] . . . 1

1.2 Scheme of the AGV laboratory . . . 2

2.1 Overview of the implemented AGV terminal in the laboratory [26] . . . 5

2.2 Top view of model AGV used in the AGV laboratory [20] . . . 6

2.3 Bottom view of model AGV used in the AGV laboratory [20] . . . 6

2.4 Current communication and distribution layout of the AGV system. The red arrow indicates that no data is transferred but that data is acquired from the AGV by the cameras [26]. . . 7

2.5 Estimated velocity profile of a MAGV [17] . . . 8

2.6 Examples of possible Dubin trajectories [45] . . . 9

2.7 Basic working principle of a MPC [4] . . . 9

2.8 Overview of levels implemented in ALTOS, with corresponding agents and com-munication strategy. The green marked agents are the tasks to be carried out by a single MAGV . . . 10

3.1 Visualization of cell decomposition, where (a) uses approximate cell decompo-sition, which is easier to implement, but has the drawback that more cells are marked occupied which might lead to infeasible paths. (b) uses adaptive cell decomposition, which changes the cell size in presence of obstacles. This is harder to implement, but gives a better approximation of the reality [7]. . . 12

3.2 Possible neighbors for a cell [24] . . . 13

3.3 Example of 3D search space, the left image shows the three different types of movement for the vehicle, the center image shows the graph after two steps and the right image shows the graph after multiple steps [32] . . . 14

3.4 Example of a RRT for a non holonomic constrained vehicle [29] . . . 15

4.1 Visualization of ROS architecture . . . 18

4.2 Visualization of Euler angles [2] . . . 19

4.3 Visualization of the MAGV main computer ROS connection for the proof of concept 20 4.4 Visualization of OptiTrack Matlab ROS connections for the proof of concept . . 20

4.5 Visualization of ROS connections for the proof of concept . . . 20

4.6 Updated scheme of ALTOS, where both the database connections and ROS are implemented. The green rectangles are tasks to be carried out by the individual MAGVs. . . 21

4.7 Communication architecture between the several technical components / agents 22 4.8 Evaluation of minimal radius of rotation for the MAGV . . . 23

4.9 Example of obstacle detection . . . 25

4.10 Flow chart implemented 𝐴∗ algorithm . . . 27

4.11 Flowchart of the complete algorithm . . . 28

4.12 Example of planned trajectory with the implemented algorithm. The estimated arrival time of the AGV at the node is marked next to the node. . . 29

4.13 Example of planned trajectory with the implemented algorithm. The start and destination are the same as in Figure 4.12, but the obstacle location has been changed. . . 29

5.1 Computational cost for trajectory planning on a Raspberry Pi 3B+, powered with an accu . . . 32

(10)

x List of Figures

5.2 Computational cost for trajectory planning on a Raspberry Pi 3B+, powered with

an power adapter . . . 32

5.3 Plot of resolution versus the travel time divided by the required computation time. Results greater than 1 are assumed to be feasible . . . 33

5.4 Minimal lengths of 100 trajectories found using Dijkstra . . . 34

5.5 Required computational time for 100 trajectories using Dijkstra . . . 35

5.6 Distances of 100 trajectories comparing the heuristics . . . 36

5.7 Distances of 100 trajectories comparing the heuristics . . . 37

5.8 Paths with varying penalties on turning edges . . . 39

A.1 Example of finding the IP-address . . . 47

A.2 Example of starting the ROS master . . . 48

A.3 Example of starting a ROS program . . . 49

A.4 Opening the data streaming panel . . . 50

A.5 Example of the data streaming panel . . . 50

(11)

List of Tables

3.1 Comparison of evaluated path planning approaches . . . 16 4.1 Results from the communication time test . . . 21 5.1 Absolute length of found trajectories in meters, both with and without heuristics 36 5.2 Relative length of found trajectories, in relation with the minimal trajectory . . 37 5.3 Computation time in seconds, both with and without heuristics . . . 38 5.4 Absoluted length multiplied with the computation time in seconds, both without

and with heuristics . . . 38 5.5 Table with estimated travel time for varying penalties on turning edge weights . 39

(12)

(13)

Glossary

AGV Automated Guided Vehicle. ix, xv, xvi, 1– 3, 5–7, 9–11, 14–17, 20, 21, 23–26, 28, 30, 31, 39, 41, 42, 47, 48

ALTOS AGV Laboratory Terminal Operating Sys-tem. xv, 9, 10, 16, 21, 30

DEFT Dynamic, Evasive Free-ranging Trajecto-ries. 15, 16, 42

GC Gantry Crane. ix, 1, 5

KPI Key Performance Indicator. 31, 39

MAGV Mini Automated Guided Vehicle. vii, 6–8, 10, 16, 17, 19–24, 26, 28, 30, 33, 36, 38– 42

MPC Model Predictive Control. ix, 9

QC Quay Crane. ix, 1, 5

ROS Robot Operating System. x, xv, 17–21, 24, 26, 28, 30, 41, 42, 47–51

RRT Rapidly-exploring Random Tree. 14

(14)

(15)

Summary

In order to keep up with (expected) growth in container transport, container terminals have to keep innovating. Research on container terminals is conducted in the AGV laboratory of the Delft University of Technology. However, research in this laboratory is limited for up to three AGVs due to a centralized architecture. Therefore a distributed architecture is pro-posed in order to reduce computational efforts per computer, eliminating the constraint on a maximum number of AGVs.

Furthermore, prior research on trajectory planning techniques did not take into account obstacles or flexible layouts. Therefore this research is conducted with main question: ”How can real-time trajectory planning be realized for a small scale AGV in the TU Delft AGV labora-tory, under a distributed architecture?”.

The current state of the AGV laboratory is further investigated, concluding that prior research focused on a centralized architecture, encountering computational limitations. Re-cently a distributed architecture called the AGV Laboratory Terminal Operating System (AL-TOS) is proposed. ALTOS, however lacks real-time implementation for the operational and tactical layer. Furthermore, prior research has not yet been implemented in this new archi-tecture, leaving the laboratory in a transition phase.

In order to find a suitable trajectory planning method for the AGV laboratory a literature study is performed. During this literature study it has been taken into account that the AGV is subject to non holonomic constraints. A possible solution is found by implementing state-lattice graphs, which incorporates the (possible) heading of the AGV as well as the pos-sible positions. By taking non holonomic constraints in account while generating the edges between nodes, standard graph-searching methods could be used for finding a path subject to non holonomic constraints.

A proof of concept is performed while implementing the Robot Operating System (ROS) for the distributed operational and tactical layer. An AGV is driven around the laboratory, while communicating with a master computer. The movements of the AGV could be tracked with Optitrack coupled to ROS as well. The state-lattice graph approach is implemented on a Raspberry Pi 3B+, coupled to an AGV. The graph is initialized, obstacles are detected through ROS while marking corresponding nodes occupied. The origin and destination of the AGV are communicated through ROS, upon which the AGV plans a trajectory using the 𝐴∗ algorithm and the velocity profile of the AGV.

Several experiments have been conducted to prove the feasibility of individual trajectory planning by an AGV. At first, the effect of varying number of nodes in the graph on required computation time is evaluated. In an ideal setting, a resolution of 0.16 meter between the nodes in the AGV laboratory could be used for real time trajectory planning. In order to com-pensate for errors, a resolution of 0.25 meter is used. Several heuristics are tested for the 𝐴∗ algorithm and Euclidean distance turned out to have best overall performance. At last, the effect of edge weights on the number of rotational movements of the AGV is assessed.

All together, an operational and tactical layer based on ROS in combination with state-lattice graph trajectory planning turned out to be a promising solution for the AGV laboratory. However, a functioning controller for ROS is lacking, making real testing in the AGV labora-tory impossible.

(16)

xvi 0. Summary

Furthermore, several recommendations are proposed for further study in the AGV labo-ratory; a more sophisticated obstacle detection approach is required in order to test real-life like situations in the laboratory. In this research, limited number of various headings are taken into account. Increasing this number would lead to more flexible trajectory planning. Additionally, other graph searching methods than 𝐴∗ should be tested on feasibility to find the optimal relation between computation time and trajectory length optimality. Since this research only takes into account a single AGV, the effect of this trajectory planning approach on a container terminal with multiple AGVs is yet unknown.

(17)

1

Introduction

1.1. Research motivation

The economy is booming. In the port of Rotterdam, container transport keeps on growing [34]. In order to cope with this continuous growth, it is essential for container terminals to keep on innovating. Back in 1993, the port of Rotterdam started implementing Automated Guided Vehicles (AGVs), in order to reduce manual labor, and increase efficiency [33].

Currently, a ship arrives in the port and is unloaded by a Quay Crane (QC), on an AGV. This AGV automatically transports the container to the container stack, where the container is unloaded by a Gantry Crane (GC). Here the container waits for further transport. It is common to have these AGVs travel in fixed paths, which isn’t necessarily the most efficient route. Theoretically, the shortest-path between two points is a straight line, also known as free range trajectories. However, driving in a straight line is not always possible due to obsta-cles, both static or dynamic. Several examples of fixed path layouts are provided by Figure 1.1

Figure 1.1: Examples of fixed paths between QC and GC [37]

In order to increase the efficiency of AGV’s, which would lead to increased container throughput, finding the shortest-path for a container movement is crucial. In order to re-search aspects of container terminal automation, the Delft University of Technology built a scale model AGV container terminal. In this model, 1:25 scale model AGVs, based on the AGVs deployed in the port of Rotterdam, are used to transport containers between QCs and GCs. All aspects of a container terminal are reviewed in this lab. A general scheme of re-search topics for the AGV laboratory is provided by Figure 1.2. The AGVs in the laboratory are actuated using an Arduino UNO board, DC motors and servos. Localization of the AGV’s is done using a camera system (Optitrack).

(18)

2 1. Introduction

Figure 1.2: Scheme of the AGV laboratory

In the past, a literature study on non holonomic trajectory planning for free ranging AGVs has been conducted by Smeets [38]. In this literature study, varying path planning methods are reviewed and compared. These methods were characterized as single robot holonomic, single robot non holonomic and multiple robot path planning. During this study, however, none of these path planning methods have been implemented in the laboratory.

Furthermore de Groot [9] implemented trajectory planning based on Dubin’s trajectories in the AGV laboratory. This research, however, is limited to one AGV without obstacles on the terminal. This, of course, is not really representable for a real container terminal, where multiple AGVs drive around at the same time. Furthermore, prior research used a centralized approach, where all tasks, e.g. path planning, localization, scheduling etc., was performed on one computer, resulting in high computational costs.

Overbeek [31] researched three different types of routing methods for AGVs. Traditional fixed topology, free ranging and a combination of both were simulated. The combination of a fixed topology with free ranging AGVs seems to be a promising solution, but has not been tested in the laboratory yet.

In the past, implemented solutions in the AGV laboratory, built upon each other in a cen-tralized structure. Each part of the terminal operations has to run on the same computer, encountering computational limitations. This problem might be tackled by decentralizing the system. Haak [19] therefore proposed the AGV Laboratory Terminal Operating System (ALTOS), which intends to disassemble the different tasks of the terminal, along with their corresponding code and perform them on several different computers.

The first steps towards the implementation of ALTOS were made by Haak [19]. A MySQL database is used to allow the communication between the several agents. This approach, however, turned out to be insufficient for real-time operation of the AGVs.

Therefore, a different method has to be investigated for the real-time control of the AGVs in a distributed architecture. In this report, research on trajectory planning of the AGVs in a distributed architecture in the AGV laboratory is described.

(19)

1.2. Research questions 3

1.2. Research questions

In order to resolve the shortcomings mentioned in the previous section, the following research question could be composed:

How can real-time trajectory planning be realized for a small scale AGV in the TU Delft AGV laboratory, under a distributed architecture?

In order to answer this question, several subquestion should be addressed: • What is the current state of the AGV lab?

• Which trajectory planning algorithms are available, and which would best suit an AGV in the TU Delft laboratory?

• How should (different) trajectory planning algorithm(s) be implemented for an AGV in the TU Delft laboratory, under a distributed architecture?

• What parameters should be used for the implemented algorithm(s) to optimize the tra-jectory planning performed in the AGV laboratory?

1.3. Report structure

The structure for the remainder of this report is as follows. Chapter 2 elaborates on the cur-rent state of the AGV laboratory. A literature study on diffecur-rent trajectory planning methods is provided in Chapter 3. The design and implementation of a trajectory planning algorithm is discussed in Chapter 4. Chapter 5 addresses experiments to find optimal parameters for the implemented trajectory planning algorithm and Chapter 6 concludes this reports and proposes recommendations for further studies.

(20)

(21)

2

AGV Laboratory

Introduction

In order to conduct experiments regarding automated transport, the maritime and transport technology section of the Delft University of Technology implemented a scale model AGV ter-minal. In this chapter, the subquestion ”What is the current state of the AGV laboratory? is addressed.

At first, a general description of the AGV laboratory is provided. Secondly, the constraints of the used AGVs are discussed, upon which prior research regarding trajectory planning, controlling the vehicle and the Terminal Operating System is addressed.

2.1. AGV laboratory setup

The model container terminal consists of several container stacks and quay cranes. The idea is that a certain list of orders is put into the system, upon which the AGV’s start to act and perform the tasks automatically. A schematic lay-out of the container terminal is shown in Figure 2.1. On the left side there are QCs and at the right side GCs.

Figure 2.1: Overview of the implemented AGV terminal in the laboratory [26]

(22)

6 2. AGV Laboratory

In the laboratory, mini AGVs are used (MAGVs), which are scaled 1:25 with regard to ’real’ size AGVs. The MAGVs are actuated, using DC motors for forward movement and servos for steering actions. The actuators gain input from an Arduino UNO, which is connected to a main computer using XBEE series 2. Through this setup, the main computer is responsible for all computational tasks.

An overview of the MAGV’s, with their respective components is provided by Figure 2.2 and Figure 2.3

Figure 2.2: Top view of model AGV used in the AGV laboratory [20]

Figure 2.3: Bottom view of model AGV used in the AGV laboratory [20]

The source and destination for the MAGV are put into a Matlab script, using an excel document. The Matlab script will compute the shortest path and sends required actuator information to the Arduino. While the MAGV tends to follow its path, the MAGVs is tracked using OptiTrack. This system consists of several cameras, distributed over the terminal. Through specific placement of markers, OptiTrack recognizes the different MAGVs and sends their positions to Matlab. Matlab checks the current position, compares it with where the MAGV should be and uses a controller to correct for deviations. An overview of the current communication and distribution layout is provided by Figure 2.4

(23)

2.2. Non holonomic constraints 7

Figure 2.4: Current communication and distribution layout of the AGV system. The red arrow indicates that no data is transferred but that data is acquired from the AGV by the cameras [26].

This setup seems to function properly, using one or two AGVs. However, if the system has to cope with more AGVs, the computational costs outgrows the capabilities of the central computer. Therefore, recently a distributed structure has been proposed and researched as solution for the laboratory.

2.2. Non holonomic constraints

One of the major kinematic aspects of car like vehicles, is the presence of non holonomic constraints. Non holonomic constraints limit possible directions of motion at a point. The desired position could still be reached, but it requires extra local maneuvering of the vehicle [42].

Based on these constraints, a mathematical model could be constructed to predict real-world behavior. Jonker [26] used a bicycle model for the construction of this mathematical model.

This resulted in the kinematic model shown in Equation 2.1

𝑓 = [ ̇ 𝑋 ̇𝑌 ̇ 𝜓 ] = [ V cos(𝜓) V sin(𝜓) V tan( ) ] (2.1)

Where V is the speed vector of the MAGV, 𝜓 is the yaw angle of the MAGV with respect to the global reference frame, 𝑙 is the distance between both wheels to the center of gravity of the MAGV. At last, 𝑑 is the angle over which the front and rear wheel are rotated, with respect to the orientation of the MAGV.

The inputs of Equation 2.1 are limited by operational constraints of the MAGV. These limitations are shown in Equation 2.2

−𝛿 ≤𝛿 ≤ 𝛿

0 ≤V ≤ Vmax

(2.2) Where Valk [43] determined the maximum velocity V to be 0.3 m/s, and 𝛿 to be 70 deg.

(24)

8 2. AGV Laboratory

With a maximum constant acceleration of 0.3 𝑚/𝑠 [17], the velocity profile of the MAGV is as shown in Figure 2.5. In addition, it should be stated that the maximum velocity during a rotational movement is 0.22 m/s [17].

Figure 2.5: Estimated velocity profile of a MAGV [17]

2.3. Trajectory planning

Currently, the trajectory planning of the MAGVs is done based on the research by de Groot [9]. This trajectory planning algorithm is based on Dubins trajectories. The theorem of Du-bins trajectories proved that the shortest path between two points exists of curves of circular arcs with maximum curvature and straight lines [14].

The optimal trajectory can thus be constructed by maximum right turns (R), maximum left turns (L) and heading straight (S), meaning that only six different types of trajectories could be formed: • LSL • LSR • LRL • RSL • RSR • RLR

An example of several types of Dubins trajectories is shown in Figure 2.6

The main drawback of this approach is that it does not take into account obstacles, either static or dynamic. This results in inflexible terminal lay-outs, where, for example, safety areas for humans could not be implemented.

(25)

2.4. Controller 9

(a) RSL (b) RSR (c) LRL

Figure 2.6: Examples of possible Dubin trajectories [45]

2.4. Controller

In order to move along a planned trajectory, Jonker [26] developed a controller for the AGV laboratory. This controller uses model predictive control (MPC). MPC uses a receding horizon where control inputs are predicted over this horizon. The controller predicts an output based on this input and aims to match the predicted output with the reference trajectory. At each time step, this optimization is performed in order to follow the trajectory. A visualization of how MPC works is provided in Figure 2.7

Figure 2.7: Basic working principle of a MPC [4]

2.5. Distributed setup

Recent research in the laboratory emphasized on the transition to a distributed architecture. Haak [19], proposed a Terminal Operating System, where the several tasks regarding con-tainer terminal operations are separated from each other. This Terminal Operating System got the name AGV Laboratory Terminal Operating System, or ALTOS. Through implemen-tation of ALTOS, separated tasks could be researched and developed individually.

ALTOS is divided in three basic levels. The strategic, tactical and operational level. The strategic level is responsible for the arrangement of the entire system. It comprises the lay-out of the terminal and is responsible for the preparation of inputs in the terminal. The tactical level takes care of the scheduling, dispatching and routing of the AGVs, whereas the operational level accounts for the control of the AGVs. These levels are shown in Figure 2.8.

(26)

10 2. AGV Laboratory

Each of these levels consists of several different agents. Each agent is responsible for its own task, and uses communication when required. An example of such agent is the routing agent, which is responsible for routing the AGVs.

At last Haak [19] implemented a MySQL connection to Matlab for communication be-tween various agents. This MySQL connection, however, doesn’t meet the communication time requirements regarding real-time performance in the operational and tactical level. An adequate solution for the operational and tactical level is not yet implemented.

2.6. Concluding remarks

In this chapter, the subquestion ”What is the current state of the AGV laboratory?” is ad-dressed. It could be concluded that various research has been conducted on either strategic, tactical and operational layer. Most notable are the research based on trajectory planning and controlling MAGVs. Prior research, however, used a centralized approach, encounter-ing computational limitations of the central computer. Therefore, a distributed setup, called ALTOS, is considered and the implementation of this setup was started. The new system, however, lacks a sufficient approach for the tactical and operational layer.

Furthermore, prior research on trajectory planning is limited to the implementation of either fixed-paths or Dubins trajectories, both having limited flexibility.

In order to meet both shortcomings, this research aims to acquire a feasible, real-time, distributed approach for the tactical and operational layer. In addition, individual trajectory planning for a MAGV will be researched in order to test the distributed setup, for both tactical and operational layers are required for flexible trajectory planning for MAGVs.

Since trajectory planning requires aspects of both the tactical layer (scheduling, routing) and the operational layer (real-time localization of both vehicle and obstacle), it is an inter-esting opportunity to test the distributed implementation. Besides trajectory planning, each MAGV could be responsible for the other tasks in the operational layer to further distribute the computational tasks. ALTOS, with the varying architecture levels and corresponding agents, along with the current communication strategy is visualized in Figure 2.8.

Figure 2.8: Overview of levels implemented in ALTOS, with corresponding agents and communication strategy. The green marked agents are the tasks to be carried out by a single MAGV

(27)

3

Literature review on different path

planning methods

In this chapter the subquestion ”Which trajectory planning algorithms are available, and which would best suit an AGV in the TU Delft laboratory?” is handled. In order to answer this question, a literature review on different path planning approaches is performed. Since there are many different path planning techniques are developed, this section will not cover all of these techniques. The most notable approaches will be discussed and compared in this section.

It should be noted that this chapter discusses path planning approaches, whereas trajec-tories are required for implementation in the AGV laboratory. Trajectrajec-tories differ from paths, by having a time- or velocity-profile along with coordinates, creating a ’path through time’. Therefore, in order to plan a trajectory, the velocity profile of the vehicle should be taken into account as well.

According to Smeets [38], path planning algorithms could be divided to single holonomic robot, single non holonomic robot, and multiple robots. This research focuses on single robots only, but it is taken into account that the approach has to be scalable for multiple robots.

3.1. Holonomic Path planning algorithms

Robots solely subject to holonomic constraints are called holonomic robots. According to Goldstein and Herbert [18] a holonomic constraint is only dependent on the coordinates of the vehicle and the time. The orientation and/or velocity are thus not taken into account. This implicates that the robot is capable of rotating at every instant, without having to travel.

This section reviews several notable holonomic path planning approaches.

3.1.1. Dijkstra

The most well-known path planning algorithm might be the algorithm proposed by Dijkstra [10]. This algorithm uses a graph, containing nodes, edges and weights of these edges. This graph is usually build through cell decomposition. Through this method, the area is divided into non-overlapping cells, which are connected with each other, like a graph. If an obstacle is present in a cell, this particular cell is marked blocked or occupied.

The idea behind the algorithm is that it marks all nodes unvisited. From the starting node the neighbor with the lowest distance is marked visited. After this, both the unvisited neighbors of the starting node and the earlier found node are evaluated. The neighbor with

(28)

12 3. Literature review on different path planning methods

(a) Approximate cell decomposition (b) Adaptive cell decomposition

Figure 3.1: Visualization of cell decomposition, where (a) uses approximate cell decomposition, which is easier to implement, but has the drawback that more cells are marked occupied which might lead to infeasible paths. (b) uses adaptive cell decomposition, which changes the cell size in presence of obstacles. This is harder to implement, but gives a better approximation of the reality [7].

lowest cost is visited etc. This goes on until the goal node is reached, upon which the path to the goal could be reviewed from the earlier visited nodes. This approach will always lead to the shortest possible path [13].

3.1.2. 𝐴

∗

A variant on the Dijkstra algorithm, mentioned in section 3.1.1, is the 𝐴∗ algorithm [21]. Like the Dijkstra algorithm, 𝐴∗ aims to find the shortest path from source to destination by minimizing the travel costs. 𝐴∗, however, differs from Dijkstra by taking into account heuristics, along with the edge costs. The 𝐴∗ algorithm, finds a path by minimizing the cost function shown in equation 3.1

𝑓(𝑛) = 𝑔(𝑛) + ℎ(𝑛) (3.1)

With

• n: next node in graph

• g(n): path cost from source to node n

• h(n): heuristic estimating cost from node n to destination • f(n): function to be minimized

Several varying heuristics could be taken into account. Through literature Euclidean or absolute distance between node and goal turned out to be popular choices. By implementing a heuristic, the neighbor nodes are prioritized based on expected costs, rather than costs from the starting position. This prioritizing leads to a reduction of computation time, but will not always find the shortest path [21].

3.1.3. 𝐷

∗

Based on the 𝐴∗ algorithm, Stentz and Mellon [40] developed 𝐷∗. Whereas 𝐴∗ is a static al-gorithm, which suggests that the path is planned using only initial values. 𝐷∗ is a dynamic variant of 𝐴∗, implying that cost parameters could change during the problem-solving. If the cost parameters are changed, the path is re planned, according to these changes.

The algorithm therefore consists of two functions; process-state and modify-cost, where the process-state computes the optimal path costs to the goal and modify-cost changes the arc cost function. If the arc cost function is changed, the affected states are put back in the list of unvisited nodes, even though they might have been visited before.

A streamlined variant of 𝐷∗ is proposed by Koenig and Likhachev [27]. This 𝐷∗ Lite algo-rithm has the same efficiency as 𝐷∗, but is easier to understand and extend than the original 𝐷∗algorithm.

(29)

3.2. Path planning with non holonomic constraints 13

3.2. Path planning with non holonomic constraints

The algorithms reviewed in section 3.1 assumes the object for which the path is to be planned is capable of turning at every instant, without having to take into account minimal turning radius etc. This results in infeasible paths if these algorithms are implemented for car like vehicles, subject to non holonomic constraints. This section will review some path planning techniques for non holonomic vehicles.

3.2.1. Adapted

𝐴

∗

In order to cope with the non holonomic constraints of a car like vehicle, Dolgov et al. [12] invented the hybrid 𝐴∗ algorithm. The regular 𝐴∗ algorithm is limited to discrete spaces, re-quiring a vehicle to turn on the spot, whereas the hybrid 𝐴∗ algorithm takes in account the continuous nature of the search space [24]. The implementation of continuous search spaces is visualized in Figure 3.2. In order to create feasible paths for vehicles with non holonomic constraints, the heuristic function is based on Dubin’s trajectories [12]. More information on Dubin trajectories is provided in section 2.3.

Hybrid 𝐴∗ has been implemented by several different projects, e.g. Dolgov et al. [12], J. Petereit et al. [24] and Hemmat et al. [22]. All of these projects succeeded in running the algorithm on real-time basis.

(a) Regular ∗ _{(b) Hybrid} ∗

Figure 3.2: Possible neighbors for a cell [24]

Wang et al. [44] implemented a slightly different approach of 𝐴∗, called 𝐴𝑅𝐴∗. This algo-rithm computes a sub-optimal solution quickly, and tries to improve the solution until the specified time frame has ended. This guarantees real-time implementation, but leads to sub-optimal paths. Wang et al. [44] used this algorithm in a warehouse setting where localization was done through barcode recognition and inertial measurement units (IMU). Due to this lo-calization technique, the path planning has to take into account the location of the barcodes, resulting in inflexible paths. In order to guarantee the non holonomic constraints, Béziers curves are used to make feasible curves while implementing waypoints.

(30)

3.2.2. State lattice graph

A different approach was taken by Pivtoraiko et al. [32]. Their approach takes kinematic constraints in account during the graph building phase. This means that nodes and edges are constructed based on several conditions; the vehicle drives straight, or it turns with max-imum steering angle either to the left or the right. The resulting position after one time step of these actions is considered a node, and the path towards this node from the initial position becomes the edge. This process is repeated, until the map is sufficiently covered.

This process results in a three dimensional graph, containing both position of the nodes, as well as the heading of the vehicle. The graph building process is visualized in Figure 3.3

Figure 3.3: Example of 3D search space, the left image shows the three different types of movement for the vehicle, the center image shows the graph after two steps and the right image shows the graph after multiple steps [32]

After the construction of this graph, the graph based path planning algorithms discussed in section 3.1 could be used to find the shortest path, while subject to non holonomic con-straints.

In order to take dynamic obstacles into account, Kushleyev and Likhachev [28] and Cirillo et al. [6] uses time-bounded lattice graphs. This approach takes the time t at which the AGV will occupy a certain node in account as well. This information is shared among the other AGVs that has to plan a path, which could compare this time of occupancy, with its own (predicted) time it could be on that certain node. Using this information, AGVs could decide to wait until the node will be free in the future, or to travel along a different route.

3.2.3. RRT

A randomized path planning technique is proposed by Lavalle [29], employing a tree struc-ture. This concept is called a Rapidly-exploring Random Tree (RRT). RRTs uses the con-figuration space of a rigid body, which could differ per path planning problem. For a non holonomic case, this configuration space encompasses both the configuration of the vehicle (position and heading), along with the speed of the vehicle.

The RRT algorithm picks a random configuration space and finds the nearest neighbor of this space. If this space lies in a predefined obstacle region, the space is abandoned. This process is repeated until a path from origin to destination has been found. An example of this search process is provided in Figure 3.4.

The initial RRT algorithm, doesn’t provide a solution for path planning with dynamic ob-stacles. Jayasree et al. [25] adapted the original RRT algorithm and proved capable of incorpo-rating dynamic obstacles. This adapted algorithm, however, doesn’t operate under real-time conditions.

The implementation of RRTs for multi-robot path planning is done by Dobson et al. [11], proving that this algorithm could be used for multi-robot path planning. The disadvantage, however, is that has high computational costs, making real time path planning infeasible.

(31)

3.2. Path planning with non holonomic constraints 15

Figure 3.4: Example of a RRT for a non holonomic constrained vehicle [29]

3.2.4. DEFT

An algorithm for free range route planning for AGVs is proposed by Duinkerken et al. [16] and has later on been renamed to DEFT, which is an acronym for Dynamic, Evasive Free-ranging Trajectories [15]. This idea is based on the route choice method from the NOMAD model [23]. The DEFT algorithm considers a discrete time system with state 𝑥 , input 𝑣 and disturbance 𝑤 and has the following form:

𝑥 = 𝑓 (𝑥 , 𝑣 , 𝑤 ) (3.2)

The algorithm aims to finding the minimal cost function J:

𝐽 = ∑ 𝐿 (𝑥 , 𝑣 ) + 𝜙(𝑥 ) (3.3)

Where 𝐿 is the cost function for time k and the parameter 𝜙 represents the final costs at time N. The parameter 𝜙 depends on the end state 𝑥 of the system. The cost 𝐿 consists of several costs. At first the cost of speed is taken into account, which aims to keep a minimal constant speed. Secondly, time pressure is added to the cost in order to gain a preference for the fastest route in time. Furthermore static obstacles are avoided using a cost based on the distance of the state 𝑥 to the obstacles, which could be applied to dynamic obstacles as well. Although the DEFT approach is capable of planning free ranging routes for multiple vehi-cles, no kinematic restrictions are taken into account. These kinematic restrictions might be implemented using the length of Dubins trajectories in the cost function as well, but this has yet be tested. At last, the DEFT algorithm has only been tested through simulation, making it unclear how the algorithm will perform in a real-life setting.

(32)

3.3. Comparison and concluding remarks

In order to answer the subquestion ”Which trajectory planning algorithms are available, and which would best suit an AGV in the TU Delft laboratory?” a literature review is conducted. The results of this literature review are evaluated based on whether or not they incorporate non holonomic constraints, the (predicted) feasibility for multi-robot implementation, real-time possibilities and obstacle avoidance. The result of this evaluation is presented in table 3.1.

Table 3.1: Comparison of evaluated path planning approaches

Research Algorithm Non Holonomic Multiple robot Realtime Obstacles

Hart et al. [21] A* Static

Wang et al. [44] ARA* X X Static

Duinkerken et al. [16] DEFT Possibilities X ? Static / dynamic

Dijkstra [10] Dijkstra Static

Hemmat et al. [22] Hybrid A* X X Static / dynamic

J. Petereit et al. [24] Hybrid A* X X Static / dynamic

Lavalle [29] RRT X Static

Jayasree et al. [25] RRT X Static / dynamic

Dobson et al. [11] RRT X X Static / dynamic

Pivtoraiko et al. [32] State lattice X X Static

Cirillo et al. [6] State lattice X X X Static / dynamic

Kushleyev and Likhachev [28] State lattice X X X Static / dynamic

As shown in table 3.1, the state lattice approach is promising for implementation in the AGV laboratory. The approach has been proven to achieve the desired results, and could therefore be used to check the capability of a MAGV to function in a distributed architecture, with each MAGV responsible for its own path planning.

Another interesting approach would be to improve the DEFT approach, to cope with the non holonomic constraints. Further advantage is that knowledge of this algorithm is at hand and that an interesting research direction for this approach would be to implement this ap-proach in a real-life situation.

Due to limited time available for this research, it is decided to implement the state lattice graph approach, since it is relatively simple to implement. Furthermore, due to its proven functionality, the implementation of this approach is a suitable way to test the capabilities of a distributed operational and tactical layer in ALTOS.

(33)

4

Design and implementation of a

trajectory planning AGV

In this chapter, the subquestion ”how should (different) trajectory planning algorithm(s) be implemented for an AGV in the TU Delft laboratory” is discussed. At first, the required steps for a distributed operational and tactical layer are explained. Secondly, The design and implementation of a trajectory planning algorithm through a state-lattice graph are explained. Furthermore, the lack of a controller for real-life experiments is deliberated. Lastly, the chapter is concluded.

4.1. Distributed AGV lab setup

Before a MAGV is able to plan its own trajectory, a distributed architecture has to be imple-mented in the AGV laboratory. This section describes the implementation of this architecture through the Robot Operating System (ROS). Additionally, the required steps for connecting the laboratory equipment with ROS are examined. At last a proof of concept is performed, using three different computers to give a command to drive to a MAGV, let the MAGV drive upon this command, while monitoring the movement of this MAGV.

4.1.1. ROS

An emerging alternative for distributed software architectures, is the open-source initiative named ’Robot Operating System’ (ROS). ROS is a framework for writing (robot related) soft-ware, and is a collection of tools, libraries and conventions across a wide variety of (robotic) platforms [30].

Due to its capability to operate with a variety of programming languages and programs, as well as the possibility to connect different pieces of hardware with each other, ROS seems to be an interesting option to investigate for the AGV laboratory.

For the implementation of ROS a master is required. This master is responsible for name registration and connection of nodes. Without the master, nodes would not be able to com-municate with each other. Nodes are the modules in the distributed system. Each node is responsible for a different aspect of the system. These nodes are able to communicate with each other by either publishing or subscribing on a topic. Topics could thus be regarded the communication channels of the system. Due to this structure, many subscribers could receive information from one publisher. This communication structure is visualized in Figure 4.1.

Furthermore, a message type has to be defined on both the publisher and subscribers of a specific topic. This message type could vary significantly. Examples of standard messages are a ’String’ or ’Int’ message, but it could consist of an array of ’Int’ types as well. While

(34)

18 4. Design and implementation of a trajectory planning AGV

Figure 4.1: Visualization of ROS architecture

there are lots of ’standard’ message types, there is the possibility to define a new message type, according to the desired system specifications.

The ROS master requires a Linux distribution for operation. Therefore a dedicated com-puter is installed with Ubuntu 16.04, which is the recommended Linux distribution by the ROS community. Afterwards, ROS kinetic is installed on this computer. Even though ROS kinetic is not the newest version of ROS, it is still the most widely supported version and is therefore recommended by the ROS community as well. ROS kinetic is expected to be sup-ported until at least 2021, which implicates that it is future proof as well [47].

Even though the ROS master requires Linux, the ROS nodes do not necessarily require Linux. Using the Robotic Toolbox in Matlab, other operating systems could be used for the ROS nodes. This approach however, requires Matlab to be installed, which is a licensed pro-gram and has relatively high system requirements [41].

For ROS nodes on a Linux operating system, either Python, or C++ could be used as programming language. In this research Python and Matlab are exclusively used for pro-gramming purposes, due to prior knowledge of Python and Matlab.

4.1.2. Optitrack

In order to connect Optitrack to ROS, a client named vrpn_client_ros is implemented [5]. This client creates a ROS node for Optitrack and a topic for each trackable object and pub-lishes the position data of the object. A listener could be subscribed to this topic in order to get the position data of the object. For the proof of concept, the Robotic Toolbox from Matlab is used to subscribe to the various topic, and plots the position using scatter plot.

The message type used by the Optitrack client, is called PoseStamped and is part of the geometry messages type [36]. A PoseStamped message consists of the time of publishing and the pose information. The pose information is divided in the position (x, y, z) and the orientation (quaternion x, y, z, w) values of the object.

The position of the object could be obtained easily by accessing the information in the message, e.g. accessing data.pose.position.x gives the x component of the position of the object. The heading, however, is published in quaternions, rather than the commonly used Euler angles. Quaternions are used to rotate a point in a three dimensional space, whereas Euler angles provide the orientation of an object with regard to the principle axes, which is shown in Figure 4.2. Euler angles are therefore considered a convenient method to describe the orientation of objects in the laboratory.

(35)

4.1. Distributed AGV lab setup 19

Figure 4.2: Visualization of Euler angles [2]

Quaternions could be transformed to Euler angles using a set of formulas. Since the MAGV is only expected to change its heading (Yaw), only this value has to be derived from the quaternions. In this derivation, the arctangent of quaternion values is taken. This approach, however, limits the Yaw to vary between 0 and 180 degrees, instead of 0 to 360 degrees. Therefore the computational function atan2 is used [3]. Equation 4.1 is used to determine the Yaw from the quaternions [3]. With qx being the x component of the quaternion etc.

𝑌𝑎𝑤 = arctan 2(2 ∗ 𝑞𝑦 ∗ 𝑞𝑤 − 2 ∗ 𝑞𝑥 ∗ 𝑞𝑧, 1 − 2 ∗ 𝑞𝑦 − 2 ∗ 𝑞𝑧 ) (4.1) Equation 4.1 returns values between -180 and +180 degrees, and is therefore suitable for determining the heading of the MAGV.

4.1.3. Altered MAGV

The current hardware setup of the MAGV, as described in section 2.1, is insufficient for the implementation of a distributed system through ROS, due to two main reasons.

Firstly, only XBEE connection for series 1 XBEEs is supported directly by ROS. Series 2 XBEEs differ in their protocol with the series 1 variant, and is not supported by ROS [39]. A workaround could be created by using Matlab as hub for both the XBEE and ROS. Using this workaround, the communication delay is assumed to increase, since a message has to be passed down twice.

Secondly, the Arduino UNO used on the MAGV, has rather limited computational power [1] and would therefore not be able to perform its own trajectory planning.

Above mentioned problems could be solved by adding a Raspberry Pi 3B+ [35] to the MAGV. Since the Raspberry Pi uses a Linux operating system, ROS could be installed directly, and the built-in WiFi module makes communication through ROS possible without having to use workarounds. For this research, a Lubuntu 16.04 image with pre-installed ROS kinetic is flashed on the Raspberry Pi. Furthermore, the specifications of the Raspberry Pi seems promising for decentralized trajectory planning, but is yet to be tested.

(36)

4.1.4. Proof of Concept

In order to test whether ROS could be implemented in the AGV laboratory, a proof of concept is performed. In this test, the MAGV gets data from the main ROS computer, and has to act upon this data. Therefore a publisher /subscriber structure is used, where the subscriber forwards the received data to the Arduino. This connection is visualized in Figure 4.3. The code is provided in Appendix B.1

Figure 4.3: Visualization of the MAGV main computer ROS connection for the proof of concept

In order to plot the movement of the MAGV, a Matlab script is used. This script acts as subscriber and registers the coordinates of the vehicle. While the script subscribes to this information, it plots the received coordinates using a scatter plot. This ROS connection is shown in Figure 4.4. The code is provided in Appendix B.1.4.

Figure 4.4: Visualization of OptiTrack Matlab ROS connections for the proof of concept

The total overview of the ROS connections is shown in Figure 4.5

Figure 4.5: Visualization of ROS connections for the proof of concept

Using this setup, commands could be sent from the main ROS computer to the MAGV. Using the numbers on the keypad, the MAGV could manually be steered (e.g. the number ’8’ means driving forward). To test this setup, the MAGV through the laboratory.

Communication time

The feasibility of real time trajectory planning in a distributed architecture is partly depend-ing on the time it takes for a message to go from one computer to another. This is therefore tested under the implementation of ROS. This test is performed by sending an Int64 type message, containing the Unix time [46] of that very instant from the ROS master computer. Once this message is received by the Raspberry Pi, the Unix time is compared with the data of the message. Since Unix time is predefined, all computers have exactly the same time and therefore the compared time is the time it takes the message to arrive at the Raspberry Pi. This test is done both with the Raspberry Pi connected to the network through WiFi and an Ethernet cable. This test is repeated 100 times, and the results are shown in table 4.1.

From the results in table 4.1 it can be seen that the average time for a message to arrive differs slightly between WiFi and the wired connection. The standard deviation, however, is significantly higher for WiFi. This might be resolved by installing a better WiFi router, but it is

(37)

4.1. Distributed AGV lab setup 21

Table 4.1: Results from the communication time test

N = 100 WiFi Cable

Average time [s] 0.123 0.113 STD Dev time [s] 0.016 0.004

expected that it will never meet the standards of the Ethernet cable. For now, both methods seem to be sufficient for real-time implementation.

4.1.5. ALTOS

After the concept has been proven to function with all individual technical components of the AGV laboratory, connected through ROS, ROS could be implemented further in both opera-tional and tactical layer.

By implementing a distributed system, the MAGVs could become responsible for several tasks. The routing, localization, obstacle detection and controller could be implemented on the individual MAGVs, migrating several computational demanding tasks to the individual AGVs. An updated scheme of ALTOS with the implementation of ROS is provided in Figure 4.6. It should be noted that connection with a database might still be useful for non real-time tasks, and should therefore still be considered in ALTOS

Figure 4.6: Updated scheme of ALTOS, where both the database connections and ROS are implemented. The green rectangles are tasks to be carried out by the individual MAGVs.

The required components and communication architecture between them is shown in Figure 4.7.

(38)

Figure 4.7: Communication architecture between the several technical components / agents

4.2. Trajectory planning

The implementation of the state-lattice graph based algorithm, described in section 3.2.2 for a single MAGV is further elaborated in this section. As stated in section 3.2.2, this algorithm uses a graph, where the edges meet the non holonomic constraints of the vehicle. Using this graph, a relatively simple graph based path planning method could be used, like Dijkstra (3.1.1) or 𝐴∗(3.1.2). This section describes how the map is built, including obstacle detection and the determination of the start position, along with the destination. The planning of a path between this start and destination, using the built map is discussed and at last, the path is transformed to a trajectory taking into account the velocity profile of the MAGV.

4.2.1. Map building

The map building using a state-lattice graph consists of two main parts; the building of the initial map, and the adaption of this map upon detecting obstacles. Whereas, the initializa-tion has to run only once, at the start of the operainitializa-tion, the obstacle detecinitializa-tion is put in a loop to perceive changes in obstacle locations.

Node resolution determination

At first, the spatial resolution of the nodes has to be determined. Theoretically, the ideal resolution of the grid would enable the graph to look like the state lattice in Figure 3.3. The ’curving’ edges allow the vehicle to change its angle with either -90 or +90 degrees. This rotation implicates that the vehicle travels a quarter of a circle with radius r, to reach the

(39)

4.2. Trajectory planning 23

next node. This radius is dependent on the steering angle 𝛿 (see section 2.2) and the distance between the wheel bases.

The equation to determine the relationship between wheel base, steering angle and the radius of the turning circle is provided by equations 4.2 - 4.4 [8].

𝑅𝑎𝑑𝑖𝑢𝑠 = 𝑅 + 𝑟 2 (4.2) with 𝑅 = 𝑤 sin(𝛿) (4.3) and 𝑟 = 𝑤 tan(𝛿) (4.4)

Using W = 0.5 meter and 𝛿 = 60 degrees, the radius is 0.43 meter. Note that 𝛿 is smaller than the maximum 𝛿 defined in section 2.2. Using a smaller 𝛿 enables the AGV to correct driving errors by increasing the steering angle.

This determined radius is evaluated by driving the MAGV in circles, using the proof of concept as discussed in section 4.1.4. This is tested both rotating clock- and counterclock-wise, which is shown respectively in Figure 4.8a and Figure 4.8b.

(a) Turning the MAGV clockwise (b) Turning the MAGV counterclockwise

Figure 4.8: Evaluation of minimal radius of rotation for the MAGV

From Figure 4.8a and Figure 4.8b the radius could be determined. For the clockwise mo-tion, this is 0.155 meter, whereas for the counterclockwise motion it is 0.160 meter, these results differ slightly from each other, which is probably due to the wheel alignment, which differs a bit from the center of the vehicle. More notable is the difference between the the-oretically determined radius, and the radius from the test results. This difference could be explained by the assumption that the back wheel axis doesn’t change its angle in equation 4.2, which is the case for almost all vehicles. The MAGV, however, is capable of changing both front and back wheel axes, which decreases the radius of rotation.

Based on the above mentioned turning radius, a resolution of 0.25 meter is chosen. This resolution provides sufficient space for the MAGV to turn 90 degrees, which is required for path planning through a state lattice graph. Since the resolution is larger than the minimal

(40)

radius of rotation, a smaller angle of rotation 𝛿 is required, allowing the MAGV to correct for eventual errors. With a terminal lay-out of 3.5 by 5.5 meter, a resolution of 0.25 meter results in a 14 x 22 node graph, with a total of 308 nodes.

Initialization

The initialization of the map builds the ’plain’ graph, which contains no obstacles. For the initialization of this graph, approximate cell decomposition of the map, as discussed in sec-tion 3.1.1, is used. At each of these nodes, the MAGV could have 4 major headings, 0, 90, 180, 270 degrees, where 0 degrees means a heading in positive x direction. As visualized in Figure 3.3, the MAGV could change its heading while moving between nodes.

During this research, the MAGV is allowed to change its direction by either -90 degrees, or +90 degrees. This means that each initial heading, has three follow up possibilities for the next node; steering maximum to the left, no steering and steering maximum to the right. Therefore, each node has 12 incoming and 12 outgoing edges.

To store this information, a dictionary is used, where the keys encompass the node coor-dinates and the heading of the AGV. The corresponding values of the keys are the connected nodes, along with the weight of the edges towards these nodes. An example of a node, with its corresponding edges is provided below.

(4, 3, 0) ∶ {(3, 4, 270) ∶ 10, (4, 4, 0) ∶ 1, (5, 4, 90) ∶ 10} (4, 3, 90) ∶ {(5, 2, 180) ∶ 10, (5, 3, 90) ∶ 1, (5, 4, 0) ∶ 10} (4, 3, 180) ∶ {(3, 2, 270) ∶ 10, (4, 2, 180) ∶ 1, (5, 2, 90) ∶ 10} (4, 3, 270) ∶ {(3, 2, 180) ∶ 10, (3, 3, 270) ∶ 1, (3, 4, 0) ∶ 10}

The initialization function, creates these nodes, along with its connections for the speci-fied number of nodes. The specispeci-fied number of nodes could be determined either by setting the required number of nodes in both x and z direction or by setting the map size in both x and z direction, while specifying a fixed resolution.

The above mentioned method will, however, create connections with ’non-existing’ nodes. This will occur on the edges of the graph. To counteract this problem, a final search is done for all connections of the graph. If a connection to such ’non-existing’ node is found, the weight of this connection is set to a significantly large value (e.g. 99999) to discourage the path planning algorithm of looking for a path through this edge.

The implemented code for the map initialization is provided in Appendix B.2.1. Obstacle detection

The obstacle detection is done through Optitrack, which is coupled to the ROS environment as described in section 4.1.2. Upon receiving the coordinates of the obstacle, the algorithm determines the corresponding cell in the built map. In order to determine the corresponding cell, the map is divided using a fixed resolution. The coordinates of the obstacle are divided by the resolution and rounded up to the nearest integer, to get the corresponding cell.

𝑛𝑜𝑑𝑒 = (⌈ 𝑥

𝑟𝑒𝑠𝑜𝑙𝑢𝑡𝑖𝑜𝑛⌉, ⌈ 𝑦

𝑟𝑒𝑠𝑜𝑙𝑢𝑡𝑖𝑜𝑛⌉) (4.5)

For example, 5.5 meter by 3.5 meter map, having a resolution of 0.25 meter. An obstacle is detected with coordinates (3.17 ; 2.7), which is shown in Figure 4.9. This corresponds with node (13, 11).

(41)

Figure 4.9: Example of obstacle detection

After the obstacle has been detected and the corresponding node is determined, the gorithm should be enabled to act upon the obstacle. Since graph based path planning al-gorithms aim to find the path with lowest cost, the edges towards and from the obstacle containing node should get a significantly high value, e.g. 99999. This discourages the path planning algorithm from incorporating the obstacle node in the path.

Optitrack only sends the position and orientation of the center of an object. Therefore, us-ing this approach, would limit obstacle detection to point masses. However, real life objects have a certain shape and could thus obstruct multiple nodes. In order to correct for this, the length and width of a rectangle obstacle could be provided in the code. Using the shape, position and orientation of an obstacle, the coordinates of the corners of the obstacle could be derived. The corresponding nodes could be determined by equation 4.5. The nodes between these corners could be interpolated, resulting in a list of occupied nodes of the obstacle. This approach, however, is limited to rectangular objects with predefined length and width. A more sophisticated obstacle detection approach is required to re-enact real life scenarios.

The code for obstacle detection, for obstacles with fixed length and width, could be found in Appendix B.2.2.

4.2.2. Path planning algorithm

Besides a graph with all possible connections, a starting and destination point is required. Once these points, along with the graph are known, path planning could be done.

Start position

To determine the start or current position of the AGV, the same algorithm as for obstacle detection (section 4.2.1) could be used. The path planning algorithm, however, requires the node as well as the heading of the AGV before it could find its possible path.

(42)

Destination

The destination of the AGV is communicated through ROS. This could be done through a sim-ple publisher / subscriber construction as described in section 4.1.1. Since the destination exists of an x and z coordinate, along with a desired heading, an Int64MultiArray message type is used. An Int64MultiArray message has a data part, which could be filled with an array of integers, upon which it could be published. The subscriber handles the data like a regu-lar array of integers, enabling the subscriber to process only the relevant data of the message. This data could be assigned either by input in a console, through a (random) function in the publishing script, or by extracting data from a database.

Path planning

Using a graph for path planning could be performed by various algorithms, as discussed in section 3.1. For this research, 𝐴∗ is used, due to its simplicity and relatively low computa-tional effort. In section 3.1.2, this approach is briefly discussed, but will further elaborated in this section.

Like the Dijkstra algorithm (section 3.1.1), 𝐴∗ starts by marking all nodes in the graph ’unvisited’. From the start node, all neighbors are evaluated, based on costs from the start node, along with the expected cost to the destination. These expected cost could be deter-mined through various heuristic functions. Through the implementation of this heuristic function, accessible nodes are prioritized based on their expected costs to the destination, enhancing the required computation time. This process is repeated until the destination is reached, upon which the path is reconstructed through the nodes leading to the lowest path cost from start to destination.

Even though the computational effort is reduced through the implementation of heuris-tics, this might also lead to paths which are not necessarily optimal. Therefore a considera-tion is to be made on path optimality versus computaconsidera-tional effort. This is further discussed in section 5.3.

A flow chart of the implemented 𝐴∗ algorithm is provided in Figure 4.10. The algorithm starts with the initialization step. Here a PriorityQueue is generated, ranking nodes with low-est cost from start node plus expected costs to the dlow-estination. Furthermore, a list with the costs so far for the nodes is generated, along with a list containing information about through which nodes a node is connected to the start. At last, all nodes are marked unvisited.

After the start position is determined, it is put in the PriorityQueue, the Cost so far is set to zero for this node, it has no Came from, since it is the starting node. At last it is marked visited. The neighbor nodes are assessed based on their cost so far, along with the expected cost to destination. Based on these costs, it is put in the PriorityQueue, and the respective values of the neighbor node are put in the Cost so far, and Came from list. At last the neigh-bor node is marked visited.

This process is repeated for all neighbor nodes. If the destination is not reached, the next node is taken from the PriorityQueue, upon which its neighbors are assessed and so on, until the destination is reached. If the destination is reached, the path is reconstructed using the Came from list, and the total cost of the path is taken from the Cost so far list.

4.2.3. Path to trajectory conversion

Since paths merely take into account the order of coordinates for the vehicle to move to, the velocity profile has to be matched with the path in order to get the trajectory. Through pairing the velocity profile provided by Figure 2.5 with the found path, the time at which the MAGV is supposed to be at a specific coordinate could be derived.

(43)

(44)

This velocity profile is assumed to be ideal (instant maximum acceleration) and no required acceleration / deceleration from going straight (0.3 m/s) to a rotational movement (0.22 m/s). Furthermore, the MAGV is assumed, after reaching maximum velocity to drive with this maximum velocity until it has to decelerate for its destination.

4.2.4. Resulting algorithm

Figure 4.11: Flowchart of the complete algorithm

The final algorithm is composed of the algorithms for map build-ing (section 4.2.1), which consists of initialization and obsta-cle detection, and path planning (section 4.2.2), involving the determination of the starting position, the destination and the planning of the path between these two. The path is con-verted to a trajectory, using the velocity profile of the MAGV. A flow chart of the complete procedure is provided by Figure 4.11

An example of a planned trajectory is provided in Fig-ure 4.12. In this example, a graph of 14x22 nodes is used. The origin is at (2,12,0) and the destination at (8,4,180). One obstacle is placed at (4,10). From this figure, it shows that the MAGV obeys the non holonomic constraints, along with the desired heading at the destina-tion. This results in an extra loop in order to turn the ve-hicle. In Figure 4.13 a trajectory is planned between the same origin and destination, but has a different obstacle lo-cation. This shows that the planner tries to avoid obsta-cles.

The order of nodes, along with the heading and estimated time of arrival at that node, from origin to destination is as follows:

1. (2,12,0, 0.00) 2. (3,11,270, 1.81) 3. (3,10,270, 2.64) 4. (3,9,270, 3.48) 5. (3,8,270, 4.31) 6. (3,7,270, 5.14) 7. (4,6,0, 6.45) 8. (5,7,90, 7.76) 9. (4,8,180, 9.57)

4.3. Controller

In order to let the MAGV follow the planned trajectory, a controller is required. Since previous research in the AGV laboratory did not incorporate ROS, prior developed controllers are not (yet) suit-able to test the ability of the MAGV to follow its planned trajec-tory.

Due to time constraints, no controller has been adapted for implementation in ROS. There-fore, the planned trajectory could not be driving in the laboratory. In order to prove the feasibility of the real-time trajectory planning capacities of a MAGV, a controller should be developed for ROS.

(45)

4.3. Controller 29

Figure 4.12: Example of planned trajectory with the implemented algorithm. The estimated arrival time of the AGV at the node is marked next to the node.

Figure 4.13: Example of planned trajectory with the implemented algorithm. The start and destination are the same as in Figure 4.12, but the obstacle location has been changed.

(46)

4.4. Concluding remarks

In this chapter, the subquestion ”how should (different) trajectory planning algorithm(s) be implemented for an AGV in the TU Delft laboratory” is considered.

At first a distributed setup through ROS is implemented and tested to be feasible using a proof of concept. For this proof of concept several computers had to be connected through ROS, and the Optitrack software is coupled with the ROS environment.

Upon ROS being proved to be a suitable candidate for a distributed operational and tac-tical layer, the trajectory planning procedure and algorithm is taken into account. For the trajectory planning, a state-lattice graph map is used, along with a graph searching algo-rithm. The procedure encompasses an initialization phase, where a state lattice graph is built, taking into account non holonomic constraints. Upon this initialization, obstacles are detected through ROS and Optitrack and handled accordingly, allowing for rectangular ob-stacles of varying size. The origin of the path is determined using Optitrack to detect the current position and orientation of the MAGV. The destination could be provided to the algo-rithm either through ROS or a database (or a combination of both). At last a trajectory could be planned and communicated across ALTOS.

Even though the resulting trajectory could be visualized and assumed to be feasible, real life testing of the MAGV has yet to be evaluated. Due to the absence of a suitable controller for ROS and time constraints, this real-life testing could not be realized in this research.

Flexible trajectory planning by a model AGV

Flexible trajectory

planning by a

model AGV

A research assignment

L.A. van der Plas

Specialization:

Transport Engineering and Logistics

Report number:

2018.TEL.8289

Title:

Flexible trajectory

planning by a model AGV

Author:

L.A. van der Plas

Preface

Contents

List of Figures

List of Tables

Glossary

Summary

1

Introduction

1.1. Research motivation

1.2. Research questions

1.3. Report structure

2

AGV Laboratory

Introduction

2.1. AGV laboratory setup

2.2. Non holonomic constraints

2.3. Trajectory planning

2.4. Controller

2.5. Distributed setup

2.6. Concluding remarks

3

Literature review on different path

planning methods

3.1. Holonomic Path planning algorithms

3.1.1. Dijkstra

3.1.2.

𝐴

3.1.3.

𝐷

3.2. Path planning with non holonomic constraints

3.2.1. Adapted

𝐴

3.2.2. State lattice graph

3.2.3. RRT

3.2.4. DEFT

3.3. Comparison and concluding remarks

4

Design and implementation of a

trajectory planning AGV

4.1. Distributed AGV lab setup

4.1.1. ROS

4.1.2. Optitrack

4.1.3. Altered MAGV

4.1.4. Proof of Concept

4.1.5. ALTOS

4.2. Trajectory planning

4.2.1. Map building

4.2.2. Path planning algorithm

4.2.3. Path to trajectory conversion

4.2.4. Resulting algorithm

4.3. Controller

4.4. Concluding remarks