Gunnar Vilson, Kim ¨ Angalid

SPATIAL AND REACTIVE NAVIGATION FOR AN

AUTONOMOUS VEHICLE IN AN UNKNOWN

ENVIRONMENT

Gunnar Vilson, Kim ¨ Angalid

September 25, 2011

Master’s Thesis in Computing Science, 2x30 ECTS credits Supervisor at CS-UmU: Thomas Johansson

Examiner: Fredrik Georgsson

Ume˚ a University

Department of Computing Science SE-901 87 UME˚ A

SWEDEN

Abstract

This thesis report describes the process of developing an autonomous non-holonomic robot that can navigate and find targets in unknown environments. The goal was to create a system for a robot that was good enough to reach an honorable position in the SICK Robot Day competition 2009. The challenge of the competition was to build a autonomous vehicle that as fast as possible, found and marked number plates in a predefined order. To achieve this goal, we combined old and well known research with algorithms developed for this very purpose.

The outcome of this project was an AI that could process sensor data and use it to plan smooth and safe routes to the destination. Behaviors for exploring unknown environments, detecting dead-ends, and avoiding collisions with other vehicles was also implemented.

The report further explains and describes techniques such as SLAM and motion planning,

which could have been applied to achieve a better and more accurate result.

Contents

1 Introduction 1

1.1 Background . . . . 1

1.1.1 Rules of the competition . . . . 1

1.1.2 The vehicle . . . . 2

1.1.3 Image processing module . . . . 3

1.2 Problem statement and goal . . . . 3

1.3 Thesis report outline . . . . 4

2 Methods 5 2.1 Sensing the environment . . . . 5

2.2 Image analysis . . . . 5

2.3 Dead-Reckoning . . . . 6

2.4 Obstacle avoidance . . . . 6

2.5 Programming language . . . . 6

3 System Description 7 3.1 Hardware . . . . 7

3.2 Software . . . . 8

3.2.1 Steering and speed control . . . . 8

4 Gunnar Vilson: SLAM 13 4.1 SLAM as an estimation problem . . . 13

4.2 The recursive Bayesian estimator . . . . 14

4.3 Kalman Filters . . . . 15

4.4 Extended Kalman Filters . . . . 16

4.5 Implementation of the time update step . . . . 17

4.6 Implementation of the measurement update step . . . . 18

4.7 Augmentic landmarks to the state vector . . . . 20

4.8 Landmark extraction . . . . 21

4.9 Data association . . . . 22

4.10 Overview of the SLAM process . . . . 24

iii

4.11 Time complexity and optimization . . . . 24

5 Kim ¨ Angalid: Path Planning and Obstacle Avoidance 27 5.1 Introduction . . . . 27

5.2 The virtual force field method . . . . 28

5.3 The vector field histogram method . . . . 29

5.4 Circle sector expansion . . . . 30

5.4.1 Alternative circle sector selections . . . . 31

5.5 Evaluating and choosing path planners . . . . 32

6 Kim ¨ Angalid: Motion Planning 33 6.1 Motion planning for non-holonomic vehicles . . . . 33

6.1.1 Goal and ambitions for Gordon’s trajectory . . . . 33

6.2 Spline curves . . . . 34

6.2.1 Polynomial curves . . . . 35

6.2.2 Cubic Hermite splines . . . . 38

6.2.3 Cardinal splines . . . . 39

6.2.4 Bezier curves . . . . 39

6.2.5 B-splines . . . . 40

6.3 Motion planning for Gordon . . . . 40

6.3.1 Motion requirements and goal . . . . 41

6.3.2 Curve building . . . . 42

6.3.3 Collision testing . . . . 43

6.4 Following the path . . . . 43

6.5 Evaluation . . . . 44

7 Accomplishment 45 7.1 Setting up an virtual test-environment . . . . 45

7.2 The implementation of CSE . . . . 45

7.3 Generating sub targets . . . . 46

7.4 Drivers and hardware . . . . 46

7.5 Turn arounds . . . . 46

7.6 Motion planning . . . 47

7.7 SLAM . . . . 47

7.8 Integration of image analysis module . . . . 47

8 Results 49 8.1 Navigation . . . . 49

8.2 Robot detection and avoidance . . . . 49

8.3 Searching . . . . 51

8.4 Participation in the SICK Robot Day 2009 . . . . 51

CONTENTS v

9 Conclusions 53

9.1 Limitations . . . . 54

9.2 Future work . . . . 55

10 Acknowledgments 57 References 59 A Rules of procedure for SICK Robot Day 2009 61 A.1 General . . . . 61

A.2 The course . . . . 61

A.3 Vehicles . . . . 62

A.4 Procedure . . . . 62

A.5 Practice runs . . . . 63

A.6 FAQ: . . . . 63

A.7 Obstacle course . . . 63

List of Figures

1.1 The robot Gordon and the team. From the left: Kim ¨ Angalid, Gunnar Vilson,

Sharhrouz Yousefi, Jonas Larsson, Farid Kondori. . . . . 2

1.2 Example of how the obstacle course could look like. The picture was sent to us from SICK, but the language is changed from German to English. . . . 3

2.1 Two web cameras mounted on the top of the vehicle. . . . 6

3.1 Gordon, the robot. . . . 7

3.2 A block diagram of the system. Made and drawn by Jonas Larsson. . . . . . 9

3.3 The system communication flow. The figure shows how the communication links are initiated by the AI. . . . . 10

3.4 Interface for the simulator which includes the AI. . . . 11

5.1 Image representation of three canditate valleys. The upper valley is selected since its direction is closest to the target. . . . . 30

5.2 A representation of a selected canditate valley. . . . . 31

5.3 Expended circle sectors. . . . 32

6.1 A situation where the circle sectors midpoints are not forming a smooth path. 34 6.2 Interpolating spline curve. . . . 35

6.3 Approximating spline curve. . . . 35

6.4 Image representation of a cardinal spline curve. Tension is set to 0.1. . . 39

6.5 The midpoint z

of the circle sector is a potential control point for a spline curve. . . . 41

6.6 Shows how a b-spline is generated from the output of CSE. The curve provides a smooth passage through the corridor. . . . . 42

6.7 Natural cubic spline curve built from 4 control points. . . . 43

6.8 Two natural cubic spline curves. The right curve is a copy of the left apart from a point that was not needed to make a collision free trajectory. . . . 43

6.9 An illustration of a collision test. If an obstacle is inside of one of the safety circles, the route has to be recalculated. . . . 44

vii

7.1 An illustration of how Gordon is responding in a dead end situation. Gordon makes a three point turn around. In narrow environments, more than three turns are required for a complete 180 turn. . . . 46 8.1 The picture shows a path of circle sectors that has been expanded from the

position of the robot to the goal. The aim, represented by then polygon starting from the vehicle’s midpoint, is set to point at the midpoint of the third circle sector. . . . . 50 8.2 The picture shows how a larger object consisting of 7 scan points have been

detected in front of our robot. A sub-target was set on the right hand side of the moving object, and a new path was calculated. . . . 50 8.3 Last minute preparations before the race. . . . 51 8.4 The picture shows a stationary Gordon in the first round. The picture is

taken right after the network crash. . . . 52 9.1 A parking lot in Waldkrich was transformed into a test environment with

number plates. . . . 54

A.1 Examples of how the obstacle course could look like. . . . 64

Chapter 1

Introduction

This chapter describes the background to this thesis, what it is about and our goals with the project. Methods and related work are described as well.

1.1 Background

The German sensor producer SICK[4], organized a robot competition called SICK Robot Day 2009[5] in Waldkirch, Germany, on the 3th October 2009.

Sven R¨ onnb¨ ack at the department of Applied Physics and Electronics - who participated in a similar competition in 2007 - applied one team for the competition and it soon became clear that Ume˚ a Institute of Technology was selected, as one among 15 other teams, for the competition. During late 2008 and first half of 2009, a team was formed consisting of five students and a teacher. See figure 1.1.

• Gunnar Vilson, Computer Science - Implementation of AI and vehicle behavior.

• Kim ¨ Angalid, Computer Science - Implementation of AI and vehicle behavior.

• Jonas Larsson, Mechanical Engineering - Responsible for the vehicle design and hardware structure.

• Farid Kondori, Master student - Implementation of image analysis algorithms.

• Sharhrouz Yousefi, Master student - Implementation of image analysis algorithms.

• Sven R¨onnb¨ack, Senior Lecturer at Applied Physics and Electronics.

1.1.1 Rules of the competition

The competing robots were supposed to find signs numbered 0-9 in an obstacle course that was approximately 30 x 40 meters. Two robots were competing in each heat: One robot started at the zero sign and searched for signs in increasing order, and one started at the sign nine and searched for signs in decreasing order.

Each sign was marked when at least two wheels where inside a circle. The circle had a radius of one meter and surrounded the sign. Collision between robots would lead to

1

Figure 1.1: The robot Gordon and the team. From the left: Kim ¨ Angalid, Gunnar Vilson, Sharhrouz Yousefi, Jonas Larsson, Farid Kondori.

disqualification of the robot that caused the collision. Various objects that could be as low as 20 cm were used as obstacles.

Collision with these objects, or the boundary of the course, resulted in a time penalty.

The free space where the robot could move without colliding with obstacles was almost flat, which meant that the environment was perfectly suited for a 2D LIDAR.

The robots were required to be autonomous, and no wireless equipment was allowed.

There were no other restrictions on the sensors. The robot needed to be big enough to be detectable by other robots, and small enough to be able to navigate through the course without colliding with obstacles.

Some rules was added only a couple of months before the competition, like the fact that all signs where placed along the boundary of the course, and that each sign was visible from any point on the course. Each heat was also limited to 10 minutes. This restriction meant that the competition became more focused on finding as many signs as possible, than having a good finish time.

Read the rules from SICK in their entirety in appendix A.

1.1.2 The vehicle

The robot, named Gordon, was built by mechanical engineering student Jonas Larsson who was responsible for hardware, vehicle design, and structure. At this time, Gordon was still just a vehicle without any kind of logic for autonomous movement.

The vehicle had the ability to respond on commands on a serial port. It needed an

AI responsible for telling Gordon information about speed and direction in any particular

1.2. Problem statement and goal 3

Figure 1.2: Example of how the obstacle course could look like. The picture was sent to us from SICK, but the language is changed from German to English.

situation. This became the purpose of our thesis.

More about the vehicle and its hardware components in chapter 3.1.

1.1.3 Image processing module

Fharid Kondori and Sharouz Yousefi, former students at the Master’s programme in Robotics and Control, implemented an image processing system for extracting and classifying number plates. This system was designed to be an independent module that could be used by the rest of the system. This module is the only part of Gordon’s software, excluding Gordon’s internal control system that is not a part of this thesis.

1.2 Problem statement and goal

The purpose of this project was to implement an AI for the robot Gordon who already was registered to participate in SICK Robot Day 2009. To achieve this, a lot of testing, implementation, and evaluation of different algorithms had to be done to find a suitable overall system to fit the vehicle and the rules of the competition.

Since the competition included many different elements, we divided the project into

several parts. Movement, localization, search, obstacle avoidance, and path planning were

some of the sub-tasks. We hoped to solve the general problem of how to get our robot to

navigate and search for targets in an unknown environment, as well as following the rules of the competition by merging these tasks into a complete system. The rules for the contest can be found in appendix.

Our goal was to within two months develop, implement and test an AI so that it became ready and prepared to compete in SICK Robot Day 2009. The main goal was not necessarily to win the competition, but it was important that we - as the only non-German team - performed well.

A summary of the goals and what it takes to achieve them is explained below.

• Implement and evaluate an obstacle avoidance and path finding algorithm.

• Develop an appropriate search algorithm, which together with the obstacle avoidance algorithm searches the environment for targets according to the rules of the competi- tion.

• Participate in SICK Robot Day 2009.

• Research and implement techniques and methods that might increase the performance of the AI.

1.3 Thesis report outline

• Chapter 2 - Methods. This chapter explains which method we used, and how we intend to work with this thesis project.

• Chapter 3 - System Description. A brief overview of the hardware and software.

• Chapter 4 - Gunnar Vilson: SLAM. An introduction to simultaneous localization and mapping.

• Chapter 5 - Kim ¨ Angalid: Path Planning and Obstacle Avoidance. This chapter addresses some of the best known algorithms for path planning and obstacle avoidance.

• Chapter 6 - Kim ¨ Angalid: Motion Planning. Covers background and information on why the use of motion planning provides benefits. Some techniques on how the output from CSE can be used to generate smooth trajectories.

• Chapter 7 - Accomplishment. This chapter explains and summarizes what has been achieved.

• Chapter 8 - Results. This chapter presents the results of the project. The outcome of the practical work.

• Chapter 9 - Discussion and Conclusions. In this chapter we discuss the outcome of the project and its limitations. Future work is discussed as well.

• Chapter 10 - Acknowledgments. Credits to people and departments who have helped

and supported us during the project.

Chapter 2

Methods

In order to achieve the desired outcome, we needed to combine old and proven methods and algorithms together with new and interesting algorithms that have not yet been acclaimed.

Since the area around this type of robotics and real-time navigation is relatively new and unexplored, we developed new algorithms to meet up with the demands of the specific contest and its rules.

Since the project easily could be divided into smaller parts, we decided to divide the work among us when possible. The AI used for the competition was, even though we split up certain parts of the software, developed in close cooperation. When the contest was over, we continued to develop the AI software by experimenting and working on separate specialization areas.

• Gunnar Vilson: Implementation of SLAM (Simultaneous localization and mapping).

This is described in chapter 5.

• Kim ¨ Angalid: Explore techniques for obstacle avoidance and path planning as well as implementing a motion planner for Gordon. This is described in chapter 6 and 7.

2.1 Sensing the environment

To get a representation of the environment, we used a SICK laser range finder with 270- degree scanning sector. The remaining 90 degrees behind the vehicle were covered by ultra sound sensors.

2.2 Image analysis

To find the plates and the numbers, we choose to use two web cameras mounted on a servo mechanism. See figure 2.1.

This enabled us to adjust the cameras without having to turn the vehicle. The images were taken and processed by a computer suited for image analysis. The results were sent to the AI computer where appropriate actions were taken depending on the outcome.

5

Figure 2.1: Two web cameras mounted on the top of the vehicle.

2.3 Dead-Reckoning

The vehicle locates itself by the use of Dead-Reckoning. Data from a gyroscope, which gave us a direction, was combined with data from the left and right center wheels, which gave information about the traveled distance.

2.4 Obstacle avoidance

The obstacle avoidance was based on the CSE-algorithm, but was heavily customized to suit our needs for the competition. Read more about CSE and its properties in section 5.4.

2.5 Programming language

The whole AI module, together with drivers for laser and gyro, were programmed with the

Java SE 6 platform. The module for image analysis was developed in MATLAB together

with some Java code for its TCP/IP client. Drivers for ultra sound sensors, camera, servo

mechanism and vehicle steering, were developed in the C programming language.

Chapter 3

System Description

This chapter describes the systems hardware and software.

3.1 Hardware

The robot used in this thesis is named Gordon and is a six wheeled vehicle.

Figure 3.1: Gordon, the robot.

Gordon is driven by six electric motors, one for each wheel, and is using an Ackermann steering model. This means that the orthogonal direction vector of both front and a rear wheel, are intersecting in a point on the direction of the non-moving center-axis. This is also the center of the turning circle.

The vehicle is non-holmonic which means that it cannot turn in place, oppose to holo- nomic vehicle, which has the ability to adjust its heading at any given time. The main idea for the design was to build a vehicle that meets the rules for the contest, while being built as a small scale real vehicle.

7

Gordon’s hardware architecture, see figure 3.2, was designed and built by Jonas Larsson, who was also responsible for other hardware and materials in the project.

• Sensors

We used a SICK LMS111[2] laser range finder for obstacle detection which has a scanning sector of 270

. For obstacle detection behind the vehicle, three ultra sound sensors were used. One in each corner, and one in the middle.

• Cameras

For image analysis, we used two Logitech QuickCam 9000 Pro[3] webcams installed on top of a servo mechanism. This enabled two pictures to be taken at the time in different angles without turning the servo.

• Gyroscope

The gyroscope, used for getting the direction of the vehicle, was built by Mattias Bergstr¨ om. The MEMS chip is an ADIS16255[8] and the uC controller is of type ATMEL AVR ATmega168[9].

• Odometry

Odometry was achieved by wheel encoders. The vehicle’s internal control system counts the number of turns of the center wheels, and sends the data to the AI through a serial port.

• Computers

Two computers were used. For the AI, we used a Asus EEE PC 1000H[1]. For image processing, we used an Acer Timeline X 1810TZ.

• Network

The AI-computer and the image analysis computer were communicating over the TCP/IP protocol. The computers were connected with Ethernet through a router.

3.2 Software

The system was designed to be structured and centralized around the AI, which contains a simulation environment that enables testing without the real robot. The AI computer communicates with the external modules; the image analysis computer, and the vehicle control system. It also contains drivers for the sensors: LMS111 laser range finder, ultra sound, and gyroscope. See figure 3.3 for communication flow.

The simulation interface of the AI module, figure 3.4, allows different settings for speed, maximum steering angle, and simulated noise for the sensor data. It has also been used to connect the AI module with the other parts of the system, and visualizes the sensor data when the module is used.

3.2.1 Steering and speed control

A communication channel between the AI-computer and the vehicle control system was needed to set reference values for steering and speed.

The serial port RS-232 was used for this communication, with the following protocol:

3.2. Software 9

Figure 3.2: A block diagram of the system. Made and drawn by Jonas Larsson.

Command controls

The following section explains the commands that can be sent directly to the vehicle.

• a - Steering control, -240 <n <+240, µS from 1500 µS bias.

• b - Speed, -125 <n <+125.

• c - Camera angle, -90 <n <+90 degrees.

• h - Reset odometry.

• s - Activate streaming mode.

• r - Remote echo mode (default).

• p - Camera power bias (1450 default).

• m - Camera Control multiplier (default 10).

Data controls

The protocol below describes the sensor data from the vehicle.

• a - Steering control, -240 <n <+240, µS from 1500 µS bias.

• b - Speed, -125 <n <+125.

• c - Camera angle, -90 <n <+90 degrees.

• l - Left wheel encoder.

• r - Right wheel encoder.

• v - Supply voltage times 10.

AI

Image analysis

Vehicle LMS111

Ultra sound Cameras

Gyroscope

Figure 3.3: The system communication flow. The figure shows how the communication links are initiated by the AI.

• i - The current through the motor times 10A.

• g - Green light, 1 go, 0 stop.

• s - Emergency Stop, 1 go, 0 stop.

• d - Ultra sound [cm], front left.

• e - Ultra sound [cm], front middle.

• f - Ultra sound [cm], front right.

• D - Ultra sound [cm], rear left.

• E - Ultra sound [cm], rear middle.

• F - Ultra sound [cm], rear right.

All commands begin with a prefix and ends with an x. Below is an example of how a series of commands could look like in streaming mode.

a+000x b+000x c+000x l+00007460x r+00007479x v+119x i+000x g0x o0x

s0x d00039x e00037x f00123x D00242x E00252x F00000x

3.2. Software 11

Figure 3.4: Interface for the simulator which includes the AI.

Chapter 4

Gunnar Vilson: SLAM

Simultaneous localization and mapping, or SLAM, is the problem of localizing a robot within a map that is simultaneously generated from robot relative observations [14]. Mapping and localization is two related problems since the localization of the robot requires an accurate map, and mapping the environment requires a accurate pose of the robot. A solution to SLAM is a requirement for autonomous navigation planning, without external navigation systems such as GPS, and it is therefore one of the fundamental problems of AI robotics.

One of the oldest and most widely used methods for solving SLAM is to handle it as a single estimation problem, and solve it with a Extended Kalman Filter or EKF[14]. Another method that has become popular during the previous decade is to use a particle filter based solution. This method is known as FastSLAM, since it has some time complexity related advantages compared to the EKF approach [27]. This thesis will only focus on the EKF based solution.

4.1 SLAM as an estimation problem

SLAM can be formulated as the problem of estimating the state of a system that progress in discrete time steps. The system state at each time step is given by a function of the previous system state and a control input. Control inputs are stochastic since we can’t exactly know exactly in which direction the robot moves or how fast the robot moves. We can only read these inputs with sensors like gyros and odometers. The state estimation is based on outputs measurements, which may be modeled as a stochastic function of the current state. These measurements are the observed landmarks in the case of SLAM.

x(k) = f (x(k − 1), u(k), w(k)) z(k) = h(x(k), v(k))

The state vector x contains the robot’s pose (position and heading) and a map of the environment. The map is represented as a set of landmarks.

This thesis will only consider the case where the pose consists of one 2D coordinate and a direction, and the landmarks are 2D-line segments. Each landmark in the state vector has a polar representation (r, α), where v is the angle of the line’s normal, and r is the distance from the origin to the line. The polar coordinate (r, α) is with other words also the orthogonal projection of the origin onto the line [12]. This polar representation is used instead of the slope-intersect form since it is more numerical stable when the line is parallel

13

to y-xis. The end points of the line segments are also stored to distinguish collinear segments in the data association process, but they are not used in the state estimation.

x(k) =



 

 

 

 

 

  p

p

p

r

α

.. . r

α



 

 

 

 

 

 

Measurement are on the same form as the landmarks in the state vector but in a robot relative frame.

Z(k) =



 

 

  d

β

.. . d

β



 

 

 

The function f models how the state propagates given the control input and input noise.

This function models the kinematics of the vehicle in SLAM. The second function h is the measurement model which describes the relation between system states and robot relative observations. The vector w is noise of the state progression, u is the control input of the system and v is the noise vector of the measurement.

The problem is now to estimate x(k) based on the observations z(0), . . . , z(k) and the control inputs u(0), . . . , u(k). It is assumed that the initial state x(0) is known. So far this problem is identical to the common tracking problem where the position of a vehicle is tracked based on readings from a radar [23]. SLAM differs however in that new landmarks need to be augmented to the state between time steps.

4.2 The recursive Bayesian estimator

A general probabilistic solution to SLAM involves computing P (x(k) |z(0), . . . , z(k), u(0), . . . , u(k), x(0)): The probability distribution of the state given the current observations and control inputs [25]. This is done by first computing

P (x(k) |, z(0), . . . , z(k−1), u(0), . . . , x(k), x(0)), the PDF given the previous observations and the current control input, using the Chapman-Kolmogorov equation.

P (x(k) ∫ |, z(0), ..., z(k − 1), u(0), . . . ., u(k), x(0)) =

P (x(k) |x(k − 1)u(k))P (x(k − 1)|, z(0), .., z(k − 1), u(0), . . . , u(k), x(0)dx(k − 1) Where P (x(k) |x(k − 1) is the distribution of the state prediction function.

The next step is to computevP (x(k) |, z(0), .., z(k − 1), u(0), . . . , u(k), x(0)) using the

recursive form of Baye’s rule [25].

4.3. Kalman Filters 15

P (x(k) |, z(0), . . . , z(k − 1), u(0), . . . , u(k), x(0)) = P (x(k) |, z(0), . . . , z(k − 1), u(0), . . . , u(k), x(0))P (z(k)|x(k))

P (z(k) |z(0), . . . , z(k − 1), u(0), . . . , u(k), x(0)) = P (x(k) |, z(0), . . . , z(k − 1), u(0), . . . , u(k), x(0))P (z(k)|x(k))

∫ P (z(k) |x(k))P (x(k)|, z(0), . . . , z(k − 1), u(0), . . . , u(k), x(0))dx(k)

This estimator is obviously not very practical in real world implementations, especially if the state space is contentious, since the computation involves integration of the complete state space. But this recursive formulation of the problem is the theoretical foundation of solutions that are more suited for real world implementations [23].

4.3 Kalman Filters

The Kalman filter is a linear estimator that is based on the assumption that the noise vectors are white, are Gaussian, and have a zero mean [19]. It is also assumed that f and h are linear functions. These assumptions simplifies the mathematics of the problem significantly, but are also reasonable assumption for many real world applications [13].

x(k) = F x(k − 1) + Bu(k) + w z(k) = Hx(k) + v

w ∼ N(0, Q) v ∼ N(0, R) The F, B and H matrices may be time dependent.

Since the input noise is assumed to be Gaussisan and the state progression is linear the states distribution will also be Gaussian [13]. This means that only the first and second order statistics need to be computed to get all information about the states probability distribution.

A optimal linear estimate of x(t), given the observation z(0), . . . , z(t), is the estimate that can be formulated as a linear function of z(0),. . . ,z(t), and have expected square error that is equal or lower than any other linear estimate of x(t). Kalman derived a set of recursive equations for computing this estimate, and its covariance, for the kind of linear dynamic systems described above [19].

The estimation is done in two steps. First the estimated state vector and the covariance matrix are predicted in what is known as the time update step [14]. Then the new mea- surement is used to update the estimation in what is known as the measurement update step.

Let P

(k) denote predicted covariance matrix, x

(k) the predicted state after the time update, ˆ x(k) the estimated state after the measurement update and ˆ P (k) the covariance matrix after the measurement step.

x

(k) = F ˆ x(k − 1) + Bu

P

(k) = F ˆ P (k − 1)F

+ Q

Where Q = cov(w)

Then the covariance matrix and the state vector in updated based on the new mea- surements. First the covariance of the innovation – the difference between measurements positions and the expected positions of landmarks – is calculated by adding the covariance caused by the uncertainty of the state to the covariance of the measurement [13].

S = HP H

+ R

The optimal Kalman gain is then calculated. The Kalman gain is a matrix that deter- mines how much the state should be corrected according to the measurements. This depends on how accurate the measurements are compared to the current state estimation [13].

K = P

(k)H

S

Finally the state vector and the covariance matrix are updated.

ˆ

x(k) = x

(k) + K(z(k) − Hx

(k)) P (k) ˆ = (I − KH)P

(k)

4.4 Extended Kalman Filters

The Extended Kalman filter estimates the state of a none-linear system. This is done by creating a linearised model of the system at the estimated state. EKF is used for SLAM since neither the state progression or the measurement functions are linear.

The two functions of the state space model are no longer assumed to be linear.

x(k) = f (x(k − 1), u(k), w) z(k) = h(x(k), v)

It is however assumed that f can be linearized around the previous estimated state at the time update, and that h can be linearized around the predicted state at the measurement update. The following Jacoibans are then used to create a linearized model of the problem [13].

F = df

dx (ˆ x(k − 1), u(k), 0)

U = df

du (ˆ x(k − 1), u(k), 0)

W = df

dw (ˆ x(k − 1), u(k), 0)

H = dh

dx (x

(k), 0)

V = dh

dv (x

(k), 0)

V v ∼ N(0, V RV

)

W w ∼ N(0, W QW

)

4.5. Implementation of the time update step 17

x(k) ≈ f(ˆx(k − 1), u(k), 0) + F (x(k − 1) − ˆx(k − 1)) + W w z(k) ≈ h(ˆx(k), 0) + H(x(k) − ˆx(k)) + V v

An ordinary Kalman filter can then be used to to estimate the state of this linearized model.

Time update

x

(k) = f (ˆ x(k − 1), u(k), 0) P

(k) = F ˆ P (k − 1)F

+ V QV

Measurement update .

S = HP

(k)H

+ V RV

K = P

(K)H

S

ˆ

x(k) = x

(k) + K(z(k) − h(x

(k), 0)) P (k) ˆ = (I − KH)P

(k)

How well this filter performs depends on how well the system can be approximated as linear near the estimated state [23].

4.5 Implementation of the time update step

The control input from Gordon is ϕ, the change in direction measured by a gyro, and s, the distance traveled in the vehicles direction, which is measured by an odometer at the middle wheels. The middle wheels of the robot is always forward facing.

p

(k) = p

(k − 1) + s cos(θ(k − 1) + ϕ) p

(k) = p

(k − 1) + s sin(θ(k − 1) + ϕ) p

(k) = p

(k − 1) + ϕ

The time update step only affects the pose of the robot, not the landmarks.

The Jacoiban

needs to be computed for the time update of the covariance matrix.

F = df dx =



 

 

 

 

1 0 −s sin(θ(k − 1) + ϕ) 0 . . . 0 0 1 s cos(θ(k − 1) + ϕ) 0 . . . 0

0 0 1 0 . . . 0

0 0 0 0 . . . 0

.. . .. . .. . .. . . . . 0

0 0 0 0 0 0



 

 

 

 

The structure of this matrix can be exploited to reduce the time complexity of the time update from O(n

) to O(n) by simplifying the computation of F P F

[27]. Let F

denote the upper left 3 × 3 sub matrix of F .

F P F

=

( F

0

0 I

) ( P

P

P

P

) ( F

0

0 I

)

= ( F

P

F

F

P

P

F

P

)

It is reasonable to assume that he noise in both the distance and the direction difference is proportionate to time difference between time steps. So it is more appropriate to consider the state prediction as a function of the forward velocity v and the angular velocity G when the Jacobian W is computed.

p

(k) = p

(k − 1) + vdt cos(θ(k − 1) + ωdt) p

(k) = p

(k − 1) + vdt sin(θ(k − 1) + ωdt) p

(k) = t(k − 1) + ωdt

The time difference is assumed to be known but is in fact a measurement that deviates.

This error is however insignificant compared to the error of the other measurements. It is also assumed that the noise of the angular velocity measurement and the noise of the forward velocity measurement are uncorrelated. The matrices W and Q only affects the upper left 3 × 3 sub matrix of P since the prediction only affects the pose of the robot, and not the map.

W = df d(v, ω) =



 dt cos(θ(k − 1) + dtω) −v ∗ dt

sin(θ(k − 1) + ωdt) dt ∗ sin(θ(k − 1) + ωdt) vdt

∗ cos(θ(k − 1) + ω ∗ dt)

0 dt





Q =

[ σ

0 0 σ

]

σ

is the standard deviation of the gyros measurement and σ

is the standard deviation of the odometer measurement. These standard deviations were grossly approximated by measurements.

The time update of the coriance can finally be computed with the following equation.

P

(k) =

[ F

P ˆ

(k − 1)F

+ W QW

F

P ˆ

(k − 1) P ˆ

(k − 1)F

P ˆ

(k − 1)

]

4.6 Implementation of the measurement update step

Each measurement is actually a least-square estimation based on several bearing and range measurements from the laser range finder. The covariance of the measurement is thus the covariance of this estimation. Let R

be the covariance of each bearing and range measurement.

R

=

[ σ

σ

σ

σ

]

σ

is the standard deviation of the range measurement and σ

is the standard deviation of the measurements bearing angle.

Let (u

, v

), . . . , (u

, v

) be the robot relative Cartesian coordinates of the points in a measurement. The polar representation of the least square linear fit of these points can then be expressed with the following function [6].

r = f

(u

, v

, . . . , u

, v

) = ¯ ucosα + ¯ vsinα v = f

(u

, v

, . . . , u

, v

) = 1

2 arctan( −2S

/(S

− S

))

4.6. Implementation of the measurement update step 19

where

¯

u = 1

n

∑

u

¯

v = 1

n

∑

v

S

=

∑

(u

− ¯u)

S

=

∑

(u

− ¯u)

S

=

∑

(u

− ¯u)(v

− ¯v)

Let A

denote the Jacobian

i,vi)

. A

=

[

1

dui

df₁ vi

df2

du_i df2

v_i

]

The elements of this matrix is given by the following equations.

df

du

= (¯ v − v

)(S

− S

) + 2S

(¯ u − u

) (S

− S

)

+ 4S

df

dv

= (¯ u − u

)(S

) − 2S

(¯ v − vi) (S

− S

)

+ 4S

df

du

= cosa

n − ¯usinα df

du

+ ¯ vcosα df

dv

df

du

= sina

n − ¯usinα df

du

+ ¯ vcosα df

dv

Let g(d, β) be the function that transforms a polar measurement point to a Cartesian coordinate in the robot relative frame, and B

be the Jacobian

.

g

(d, β) = d cos β, g

(d, β) = d sin β

B

=

[ cosβ −d sin β sinβ d cos β

]

The jacobian

i,β_i)

is then computed by multiplying the Jacobian A

with B

. J

= A

B

The covariance of the linear fit j is then given by the following equation [6].

R

=

∑

J

R

J

The covariance matrix of all measurements R is finally a block diagonal matrix with the form:

R =



 

  R

R

. . .

R



 

 

The relation between a landmark in the global map and robot relative measurements is given by the function h

.

If the robot and the origin are on the same side of the line:

h

(p

, p

, p

, r, α) =

[ r − p

cos α − p

sin α α − p

]

The distance is negated and the angle is rotated π radians, if the robot and the origin are not on the same side of the line.

h

(p

, p

, p

, r, α) =

[ −r + p

cos α + p

sin α α − p

+ π

]

Let H

bet the Jacobian of this function with regards to the state vector x is:

H

=

[ −cosα −sinα 0 . . . 1 p

sinα − p

cosα . . .

0 0 −1 . . . 0 1 . . .

]

Or if the the robot and the origin are on different sides of the line:

H

=

[ cosα sinα 0 . . . −1 −p

sinα + p

cosα . . .

0 0 −1 . . . 0 1 . . .

]

All columns that corresponds to any other landmark than the i:th are zero.

H is obtained by augmenting these Jacobians in to a single matrix.

H =



  H

.. . H



 

4.7 Augmentic landmarks to the state vector

The EKF-SLAM algorithm has an additional step that is not found in an ordinary EKF, where new landmarks are augmented to the state vector and the covariance matrix [6].

x

(x, r

, α

) =



 

 

 

 

 

 

 

  p

p

p

r

α

.. . r

α

r

α



 

 

 

 

 

 

 

 

4.8. Landmark extraction 21

Let t be the function that transforms a measurements to a landmark (r, α) in the global frame.

If the origin and the robot are on the same side of the line:

t(p

, p

, p

, d, β) =

( d + p

cos(p

+ β) + p

sin(p

+ β) β + p

)

If the origin and the robot are on different sides of the line:

t(p

, p

, p

, d, β) =

( −d + p

cos(p

+ β) + p

sin(p

+ β) β + p

+ π

)

The Jacobians of t with regards to state and measurement is then used to augment the covariance matrix.

J

= 

 cos(p

+ β) sin(p

+ β) p

cos(p

+ β) − p

sin(p

+ β) 0 . . . 0

0 0 1 0 . . . 0

  J

= 

 1 p

cos(p

+ β) − p

sin(p

+ β)

0 1

Or

J

= 

 −1 p

cos(p

+ β) − p

sin(p

+ β)

0 1

The covariance of the transformed measurement is first computed . m = J

P J

+ J

R

J

R

is the covariance of the measurement from the measurement step of the EKF. The cross covariance between the pose and the rest of the map is then computed.

n = J

P

Then the augmented covariance matrix has finally the following structure [6].

P

=

( P n

n m

)

4.8 Landmark extraction

Extracting landmarks from the sensor is generally not defined as a part of SLAM. It is problem that varies depending on the type of sensors that are used and the environment that the robot operates in. Landmarks can be either natural features in the environment or artificial landmarks that are added to environment. Natural landmarks are preferable for true autonomy, but are harder to extract. Landmarks should be easy to observe from differ- ent directions and be a regularly occurring feature in the environment so that the EKF can update the state frequently. Line segments was chosen as landmarks in the Gordon system since it is primarily made for manmade environments where lines are common geometric features.

There are several algorithms suitable for extracting line segments from the data of a laser

range finder. Split-and-merge is the most widely used algorithm and has time complexity

of O(n log n) [28].

Algorithm: Split-and-merge (P ) P : A set of points.

Output:A set of line segments 1. Add P to the stack S 2. Pop the list P

from S

3. Let l

be the linear fit of all points in p

4. Find the point p

in p

with the maximum distance to l

.

5. If d

< d

: Split P

into two lists P

and P

at P

and push both lists to S.

6. Else: Add l

toL

7. If S is not empty: Go to 2.

8. Merge all segments in L that are collinear and overlapping. A new linear fit is com- puted for each merged segment.

9. return L

The projection of the segment endpoints onto these lines are also computed and used to distinguish collinear segments in the data association process.

4.9 Data association

Landmarks that are observed in the environment can either be new landmarks that needs to be augmented to the state, or re-observed landmarks that are used to update the state.

The problem of making this classification is called data association [26].

A simple approach is to match each observed with its nearest neighbor that passes a validation test. The distance between landmarks and measurements is in this case defined as a the Mahalanobis distance between the line parameters of the landmarks and the mea- surement.The matching passes the validation test if the distance is less than limit value.

The Mahalanobis distance is used to determine if the line segments are collinear since it, unlike the Euclidean distance, takes the covariance into account. The Mahalanobis distance is a statistical measurement of the similarity between two random vectors [21].

D(x, y) =

√

(x − y)

C

(x − y) C = cov(x − y)

The distance has a χ

-distribution if x − y is normally distributed[26].

Note that we use the covariance of the innovation when we are using the distance for data association. This covariance matrix is computed in the same way as in the measurement update step of the Extended Kalman filter.

We also use second constraint in validation process which ensures the two measurement

line segment is overlapping the landmark line segment. This is done by transforming the

measurement that passes the collinearity test to the global reference frame and merging the

4.9. Data association 23

two segments. Let l

be the length of the merged segment, l

the legth of the landmark segment and l

the length of the measurement segment. We then use the overlapping rate to determine if the two collinear segments are overlapping.

r

= l

− l

− l

The segment passes this test if the overlapping rate is lesser than a limit value.

If no landmark passes the first validation test, a second validation test is done to deter- mine if the measurement can be used as a new landmark. This is done in the same way as the first test but with a greater limit in collinearity test and a lesser limit in the overlapping rate. The measurement is classified as a new, or potentially new, landmark if no landmark passes the test. This second validation is used to minimize the risk of faulty data association in later time steps. Faulty data association is a common reason for inconsistent results by the EKF-SLAM algorithm [14].

A list of pending new landmarks may be used instead of augmenting the new landmarks directly to the state [29]. Each measurement that has been classified as a potential new landmark is tested for matching landmarks in the list of pending landmarks to determine if this measurment has been observed before. The measurement is added to the list if no matching landmark is found. A landmark is augmented to the state only after a certain number of observations. This technique reduces the risk of adding faulty measurements to the state, which makes the association more robust and saves computation time in the EKF estimation.

A great disadvantage of the nearest neighbor method is that it only considers the com- patibility of each matching [18]. Some of the individual associations may be inconsistent since it does not take the landmarks relative positions to each other into account. A better approach is to also test the joint compatibility of the complete hypothesis. This is what the joint compatibility branch bound algorithm does. It searches a tree, where each level of the tree represents a measurement and each node a permutation of measurement-landmark matches, for the hypothesis with most matches. Both the individual compatibility and the joint compatibility with the rest of the hypothesis are tested when a branch is expanded.

A depth-first search is used, and branches where the best possible descending hypothesis is worse than the current best hypothesis are pruned. This algorithm gives a more robust data association than ordinary nearest neighbor association. The worst-case time complexity is however exponential, which means that number of landmarks that are tested needs to be reduced using some criteria to limit the width of the tree.

Algorithm: JointCompatibilityTest(i

, j

, . . . , i

, j

) i

, i

, . . . , i

: State landmark indices of the hypothesis.

j

, j

, . . . , j

: Measurement indices of the hypothesis

H

=



 

  H

H

.. . H



 

 

R

=



 

  R

R

. . . 0 R



 

 

S

= H

P

(k)H

+ R

v =



 

 

h

− z

h

− z

.. . h

− z



 

 

d

= v

S

v return d

< d

Algorithm: JCBB(H,i,L) H: Current hypothesis.

i: Measurement index (1 to m) H

: The best hypothesis if m ¡ I:

if CountPairs(H) ¡ CountPairs(H

):

H

= H else:

forj = 1 to n:

if CompatibilityTest(i,j) and JointCompatibility( H,i, j):

JCBB ((H,i, j),i + 1)

if CountPairs(H

) ¡ CountPairs(H) + m − i JCBB(H, i + 1)

Validation limit based data association is known to fail when landmarks are re-observed after a long traverse. This problem is known as the loop-closure problem, and is commonly handled as a special case in the data association process [14]. Data association algorithms that uses batch validation like JCBB are more robust than ordinary nearest neighbor asso- ciation in loop-closing situations [29].

The new endpoints of the landmark is computed by first computing the extreme points of the old endpoints and the endpoint of the measurement, and then projecting these points onto the landmark line. This is done after the measurement update step of the EKF.

4.10 Overview of the SLAM process

The SLAM algorithm can now be summarized with following five steps.

1. Use data from kinematic sensors to predict the state vector and the covariance matrix.

2. Extract landmarks.

3. Perform data association.

4. Update the state vector and covariance matrix based on the re-observed landmarks.

5. Augment the new landmarks to the state vector and the covariance matrix.

4.11 Time complexity and optimization

The time complexity of the EKF, with the optimizations mentioned in the previous sections

is O(n

) , where n is the number of dimensions in the state space [27]. This means that

4.11. Time complexity and optimization 25

a naive implementations of EKF based SLAM do not scale very well with regards to the number of landmarks in the map.

An alternative representation of the map may however reduce the dimensions of the EKF estimation. One type of methods that is known as portioned updates only uses a local map for the EKF updates. This local map is later merged with the global map, but this is done less frequently than the EKF updates are performed [27].

The Constrained Local Submap Filter is a portioned update method where the local map uses the robot’s pose at the moment of the map creation as frame of reference. The ordinary EKF based algorithm is then used to generated the local map and localize the robot within it. The reference frame of the local map is saved in the global map and is used to transform the local map to the global map when the two maps are merged. A data association algorithm is then used to find intersecting features, and a constraint is created for each intersecting feature. The update of the global map is similar to the measurement update of the EKF, but since the intersecting constraints are independent of each other the complexity becomesO(n

n

) . Where ni is the number of intersecting features and ng is the number of features in the global frame.

One of advantage of this method is that the update of the global map can be done simultaneously as the new local map is being generated. This mean the workload of the global map update can be either moved to another CPU core or done in several time steps.

More sophisticated data association algorithms may also be used during the updates of the

local map since the data association only needs to test the compatibility of features in the

local map instead of the complete global map.

Chapter 5

Kim ¨ Angalid: Path Planning and Obstacle Avoidance

This chapter describes path planning in robotics and some techniques and algorithms that can be applied to achieve real-time path planning and obstacle avoidance.

5.1 Introduction

In the field of AI robotics, navigation includes a variety of different tasks. Sensing, act- ing, planning, architecture design, computational efficiencies and problem solving. All are important parts of navigation. Because of these reasons, navigation is one of the most chal- lenging tasks in artificial intelligence. Almost every subfields of ai-robotics is included into it[22, p. 316].

This chapter’s main focus is path planning which is one of the main areas in navigation.

According to Robin R Murphy [22, p. 316-317], path planning is one of the four important functions in navigation expressed by the following questions:

1. Where am I going?

The goal, the position where the robot is heading for, is usually predefined by a human. Though in some cases, the robots must be able to search be itself. This is usually accomplished by allowing the robot to define sub targets to head for, while searching for the object.

2. What’s the best way there?

Path planning is described as the task of providing information to the robot about how to reach a specific target. This is probably the subject which has received the most attention. There are two main types of path planning. Topological path planning, or qualitative path planning, which is described as a simple and natural way of planning with landmarks and gateways. The opposite to topological path planning is metric path planning, which is also known as quantitative path planning. The main difference between the two of them, is that topological path planning is usually satisfied with an acceptable solution, while a metric path planner often requires an optimal, or close to optimal route.

27