GPS/Optical Encoder Based Navigation Methods for dsPIC ...319617/FULLTEXT01.pdf · Microchips MPLAB® ICD is used for in circuit debugging and programming. 2.1.1 The Universal Asynchronous

Technical report, IDE1011, April 2010

GPS/Optical Encoder Based Navigation

Methods for dsPIC Microcontroled

Mobile Vehicle

Master’s Thesis in Embedded and Intelligent Systems

Berkan Dinçay

School of Information Science, Computer and Electrical Engineering Halmstad University

GPS/Optical Encoder Based Navigation Methods for dsPIC Microcontroled Mobile Vehicle

Master Thesis in Embedded and Intelligent Systems

School of Information Science, Computer and Electrical Engineering

Halmstad University

Box 823, S-301 18 Halmstad, Sweden

April 2010

Introduction

1

Preface

This thesis is the result of the project which is a partial fulfilment of the degree of Master of Science in

Intelligent and Embedded Systems, to be acquired at Halmstad University. In this research, I look at

the problem of robot localization in many aspects and I tried to investigate architecture of methods and

modules in order to achieve the precise navigation.

The support of many people has contributed for this achievement. I want to thank to my supervisor

Bjorn Åstrand, my family and all my friends in Halmstad for their endless help and support.

Berkan Dinçay

Halmstad University, April 2010

GPS/Optical Encoder Based Navigation Methods for dsPIC Microcontroled Mobile

Vehicle

2

Introduction

3

Abstract

Optical encoders are being widely suggested for precise mobile navigation. Combining such sensor information with Global Positioning System (GPS) is a practical solution for reducing the accumulated errors from encoders and moving the navigational base into global coordinates with high accuracy.

This thesis presents integration methods of GPS and optical encoders for a mobile vehicle that is controlled by microcontroller. The system analyzed includes a commercial GPS receiver, dsPIC microcontroller and mobile vehicle with optical encoders. Extended kalman filtering (EKF), real time curve matching, GPS filtering methods are compared and contrasted which are used for integrating sensors data. Moreover, computer interface, encoder interface and motor control module of dsPIC microprocessor have been used and explained.

Navigation quality on low speeds highly depends greatly upon the processing of GPS data. Integration of sensor data is simulated for both EKF and real time curve matching technique and different behaviors are observed. Both methods have significantly improved the accuracy of the navigation. However, EKF has more advantages on solving the localization problem where it is also dealing with the uncertainties of the systems.


Vehicle

4

Introduction

5

Contents

PREFACE...............................................................................................................................................................1

ABSTRACT............................................................................................................................................................3

CONTENTS............................................................................................................................................................5

1 INTRODUCTION ........................................................................................................................................7

2 DIFFERENTIAL DRIVE SYSTEM...........................................................................................................9

2.1 DSPIC / DSP CONTROL OF MOBILE ROBOT ............................................................................................9

2.2 SUMMARY............................................................................................................................................16

3 ODOMETRY ..............................................................................................................................................19

3.1 SYSTEM MODEL .................................................................................................................................19

3.2 SYSTEM CALIBRATION.........................................................................................................................21

3.3 SUMMARY............................................................................................................................................25

4 GLOBAL POSITIONING SYSTEM (GPS) ............................................................................................27

4.1 GPS ACCURACY ..................................................................................................................................28

4.2 BUTTERWORTH FILTER ........................................................................................................................31

4.3 LEAST SQUARES CURVE FITTING.........................................................................................................32

4.4 SUMMARY............................................................................................................................................36

5 REAL TIME CURVE MATCHING METHOD......................................................................................37

6 KALMAN FILTERS..................................................................................................................................39

6.1 IDEA OF PREDICTION AND ESTIMATION ...............................................................................................39

6.2 EXTENDED KALMAN FILTER ................................................................................................................42

7 SENSOR FUSION SIMULATIONS.........................................................................................................45

7.1 REAL TIME CURVE MATCHING SIMULATIONS .....................................................................................45

7.2 EXTENDED KALMAN FILTER SIMULATIONS .........................................................................................49

7.3 SUMMARY............................................................................................................................................53

8 CONCLUSIONS.........................................................................................................................................55

9 REFERENCES ...........................................................................................................................................57

10 APPENDIX .................................................................................................................................................59

10.1 MODULE INITIALIZATIONS ...................................................................................................................59

10.2 SCHEMATICS OF THE SYSTEM ..............................................................................................................62


Vehicle

6

Introduction

7

1 Introduction

Precise navigation is being researched and applied in various fields. The applied sensors and the relevant data processing primarily determine the accuracy of navigation. In order to get accurate results, it is essential to combine measurements from different sources. In this paper, GPS and optical encoders are used as navigation sensors. However, the physical conditions of the area such as rain, cloud, big trees or buildings can affect the measurements of GPS. These noisy measurements from GPS result in loss of accuracy in navigational positioning. Additions to those optical encoders, which are attached to the wheels, provide very high precision but their errors gets bigger proportionally with time.

Due to the errors from optical encoders, a positioning system based on optical encoders is not reliable on the long run. In addition, a positioning system just based on GPS is not trustworthy and accurate in every situation. Integration of GPS and optical encoders to form a new positioning system is a solution which will be able to retain all these advantages of GPS and optical encoders.

Communication with the mobile vehicle for reading the encoder values from the computer is the first goal of this project. A microprocessor programmed and connected to the encoders for reading the sensors data after the communication of PC and microprocessor was successful. Modules of microprocessor, motor control and solutions of possible problems is described in hardware chapter.

In the second part, kinematics equations of the system are derived that will be used for position estimation in EKF and uncertainty analyses [1]. Calibration of the system is done for reducing the accumulated errors from the encoders. Results showed that accuracy of data coming from the encoders is decreases proportionally with the path that vehicle traveled.

The accuracy of GPS is related to the physical conditions of the area. In order to investigate the errors in GPS sensor data, measurements are converted to world coordinates in meters. GPS measurements are taken in different areas to investigate the accuracy and errors of the sensor in different situations.


Vehicle

8

Matching GPS data with encoder data is the most important goal of this project. EKF and real time curve matching algorithms are used for combining optical encoder and GPS data. Two different algorithms have been implemented for combining GPS and encoder measurements. The extended kalman filtering method is used [3], specifically for integration of GPS and encoder measurements. EKF is a recursive algorithm that uses a series of predictions and measurement update steps to obtain an optimal estimate of the state vector in the sense of minimizing the mean square errors. In addition, paper [2] is using real time curve matching algorithm for combining sensors data. Both methods results in improved positioning resolution of GPS in both global range and single-point.

A butterworth filter and curve fitting algorithms are implemented for searching a way to improve the quality of GPS data. Encoder and GPS sensors data are combined by both algorithms, where GPS data have been simulated in different noise values for comparison of both algorithms.

Differential Drive System

9

2 Differential Drive System

A differential wheeled robot is used for the experiments, whereby the movement is based on two wheels placed on either side of the mobile robot. The direction of the robot changes by varying the speed of the wheel so that it does not require an additional steering motion. Motor control cards that are taking the power from two car batteries control the DC motors of the robot. The robot that has been used is shown in Figure 2.1.

Figure 2.1: Differential wheeled robot

The following chapters are explaining how microcontroller can be used in order to read encoder data from the PC and how to control the motors of the vehicle. See appendix for electrical schematics of the vehicle

2.1 dsPIC / Dsp Control of Mobile Robot

Deciding upon the processor is the first important step for starting the project. For that reason, a microprocessor has to consider all the needs. A microprocessor with two encoder interfaces for quadrate encoders, digital to analog converters for motor control, UART connection for PC interface, are the basic needs. Moreover, a microprocessor had to support enough empty pins for future sensor additions. Microchips dsPIC33FJ128MC804, which provided for all the needs, is selected for the project.


Vehicle

10

Three main modules of microprocessor is used in this project:

• Communication of PC and dsPIC is established by UART module. • Data transfer of encoders is established by QEI module. • Motor Control is implemented by DAC module.

C language is used for programming the microprocessor. MPLAB IDE is an environment that is used for writing code, simulation, debugging and uploading which can be integrated with couple of different compilers. As a programming tool, Microchip’s MPLAB C 30 Compiler is used for programming. In addition to that, Microchips MPLAB® ICD is used for in circuit debugging and programming.

2.1.1 The Universal Asynchronous Receiver Transmitter (UART)

The Universal Asynchronous Receiver Transmitter (UART) module is one of the serial I/O modules that are available in the microprocessor. UART is a full-duplex asynchronous system that can communicate with peripheral devices, such as personal computers, RS-232 and RS-485 interfaces. In digital communications, symbol rate, also known as “baud rate”, is the number of symbol changes made to the transmission medium per second using a digitally modulated signal or a line code. The Symbol rate is measured in baud (Bd) or symbols/second. In the case of a line code, the symbol rate is the pulse rate in pulses/second.

The UART module consists of the following key hardware elements:

• Baud rate generator

• Asynchronous transmitter

• Asynchronous receiver

The UART module is used with RS-232 line for PC interface. Microprocessor has two UART modules, the UART2 module is used to receive and transmit. BAUD Rate is set for 9600 and 1 stop bit, no parity, 8-data bits is used for transmission, which is synchronized with XT oscillator with PLL (phase locked loop).

(2.1)

1

4

)1(4

−×

=

+×=

BaudRate

FcyUxBRG

UxBRG

FcyBaudRate


11

The UART module includes a dedicated 16-bit Baud Rate Generator (BRG). The UxBRG register controls the period of a free-running, 16-bit timer. FCY denotes the instruction cycle clock frequency (FOSC/2). In this system, the instruction cycle clock frequency (FCY) is 4 MHz and the desired baud rate is 9600 Bd. For the desired baud rate, the UxBRG register has to be set related to the equation (2.1).

HyperTerminal

HyperTerminal is an application of Windows that allows connection of a PC to other remote systems. HyperTerminal comes with Windows 95, 98, Me, NT, XP. These remote systems can include other computers, bulletin board systems, servers, Telnet sites, and online services. HyperTerminal is used as a communication interface with the PC and the microprocessor.

Selection of COM port is the first step. However, an RS-232 to USB converter is used to connect the microprocessor to the PC. After connecting the device via USB port, the corresponding COM port is found from device manager in Windows XP.

After selection of the COM port, the properties are arranged to synchronize with the microprocessor. 9600 bits per second, 8 data bits, no parity and one stop bits are used for communication.

2.1.2 Quadrature Encoder Interface (QEI)

Quadrature encoders are used in position and speed detection of rotating motion systems. A typical incremental encoder includes a slotted wheel attached to the shaft of the motor and an emitter/detector module senses the slots in the wheel. There are three outputs: Phase A, Phase B and INDEX, which provide information that can be decoded into information of movement of the wheels.

The two channels, Phase A (QEA) and Phase B (QEB), have a unique relationship. If phase A leads phase B, then the direction is taken as positive or forward. A third channel, index pulse, occurs once per revolution, which is used as a reference to establish an absolute position. See Figure 2.2 for the relative timing diagram of these three signals.

A quadrature signals has four unique states. These states are indicated for one count cycle. Note that the order of the states is reversed when the direction of travel is changed.


Vehicle

12

Figure 2.2: Quadrature Encoders (adapted from data sheet of dsPIC33F)

A quadrature decoder captures the phase signals and index pulse and converts the information into a numeric count of the position pulses. Generally, the count will increment when the shaft is rotating one direction and decrement when the shaft is rotating in the other direction.

Each wheel is driven by a DC motor. Optical encoders are mounted on the motor, where the rotor of the motor is directly connected to the encoder. Encoders have a resolution of 1000 pulses per revolution. QEI of the microprocessor is connected to both of the encoders. See appendix for initialization of the module.

Position and Speed Calculation

A timer is generated which sends an interrupt periodically for processing the encoders’ data precisely. Each periodic interrupt reads two following positions of the wheel for calculation of angular velocity and position.

(2.2)

Timer1 module is a 16-bit timer, which can serve as the time counter for the real-time clock, or operate as a free-running interval timer/counter. The timer module is used for speed calculation from the encoders. The formula shown in equation (2.2) is used for calculating the interrupt period. Assuming the maximum speed of the vehicle is 4 km/h, and knowing that diameter of one wheel is 20 cm, then the interrupt period is calculated as 23.

RPMSpeedxMAXperiodInterrupt

__2

60_ =


13

Overflow prevention

The microchip unit provides a 16-bit register (POSXCNT) for position count (32768) in the encoder interface. After the assembly of encoders, one revolution of the wheel is counted approximately as 13537 counts, which results as overflow of the register (POSXCNT) after four full revolutions of the wheel. This problem is solved by using a 32 bit register and using the direction status bit. The QEI1CONbits.UPDN is the direction status bit that takes the difference of two following readings and controls the difference with up/down direction status.

2.1.3 Audio Digital to Analog Converter (DAC)

The digital-to-analog converter (DAC) module is a 16-bit signal converter that is designed for audio applications. It has two output channels, left and right to support stereo applications. Each DAC output channel provides three voltage outputs, positive DAC output, negative DAC output, and the midpoint voltage output for the microprocessor devices. The microprocessor provides positive DAC output and negative DAC output voltages. These positive and negative DAC outputs are differential about a midpoint voltage of approximately 1.65 volts.

The DAC provides an analog output proportional to the digital input value. DAC provides differential analog outputs that common mode output voltage is a nominal 1.65 volt with a supply voltage of 3.3 volts. The voltage swing is approximately ±1 volt about the 1.65 volt midpoint into a 1 kΩ load.

Basic digital values:

. 0x7FFF = most positive output voltage

. 0x0000 = mid point output voltage

. 0xFFFF = value just below the midpoint

. 0x8000 = minimum output voltage


Vehicle

14

Figure 2.3: DAC output (adapted from data sheet of dsPIC33F)

DAC output is used for controlling motor control drives. However, this module hasbeen designed for implementing audio applications, which provides two controllable outputs. Each output provides positive and negative differential about the midpoint value and this differentially provides a voltage swing of between -1 and +1 volts. DAC output is chosen as signed data and the midpoint value chosen is 1.65 volts to get a voltage value of between 0 and +1 volts to control the speed of the wheels. Positive and negative outputs of DAC with respect to the digital input is shown in the Figure 2.3.

2.1.4 Operational Amplifier

In this project 4Q-PWM-Servostarker (MMC-QR0605 10A/20A) from Maxon Motor used for motor control. The motor driver card gets its supply voltage as 24V directly from the batteries. A input between -10V and 10V to the motor control car results the desired speed linearly proportional with the input voltage.

As mentioned in the DAC chapter, the output of the converter is symmetrical to the middle voltage values that are coming out from two channels. From the difference of the two voltage values, 0 to 2 volts can be provided. However, this difference is not relative to the zero ground, where the motor driver needs a zero ground for operation. In addition, the voltage span of the microcontroller is not sufficient for the motor drivers and dsPIC can supply limited current, which may result in some problems in the microchip. For these reasons, an op-amp is used for voltage gain and robustness on each motor driver.


15

An operational amplifier is an integrated circuit that uses external feedback to control its functions. Typically, the output of an op-amp is controlled by a negative feedback, which determines the magnitude of output voltage gain. The amplifier's differential inputs consist of a V + input and a V − input. An op-amp amplifies the difference in voltage between the two inputs. This is called differential input voltage. Operational amplifiers are used usually with feedback loops, where the output of the amplifier would influence one of its inputs.

The circuit shown in Figure 2.4 is used for finding the difference between two voltages each multiplied by some constant (determined by the resistors) in equation (2.3).

Figure 2.4: Op-amp Circuitry

Whenever:

Ω== kRR 121 Is input on V1;

Ω== kRR gf 8.1 Is input on V2;

=≅ 1/ RRA f 1.8.

(2.3)

(2.4)

The desired voltage of the motor controller is supplied from the op-amp, which is connected to DAC output of the microprocessor. The op-amp circuitry supplies a gain of A=1.8 that increases the output voltage of the system between 0 and 4.7 V.

1

1

2

12

1

)(

)(V

R

RV

RRR

RRRVout

f

g

gf −+

+=

)(8.1 12 VVVout −=


Vehicle

16

2.2 Summary

The project starts with a ds33Fj128mc802 microprocessor, which is supporting all the needs. A microprocessor is used with an Olimex PIC-P28 development board that supports 28 pin PIC microcontrollers is shown in Picture 4. However, some changes on the board are necessary in order to use new models of dsPIC. Another PIC socket is soldered to the board. Connections with the modules of the board that are voltage sources, grounds, oscillators, inputs of the in-circuit serial programmer, RX TX connection for RS-232 and some capacitors that are mentioned in the data sheet of the device are soldered on to the new PIC socket. After the soldering operation, the device was operating. However, bad solders were causing as broken connections due to physical disturbances, and it was time consuming to fix the system. Therefore, a new decision is taken to change the Olimex development board in Figure 2.5(left) with Explorer 16 development board in Figure 2.5(right).

Figure 2.5: Olimex development board (left) changed with Explorer 16 Development Board

(right).

The Explorer 16 development board is a product of “microchip” company that is developed for dsPIC’s. In addition, Explorer 16 has more features like more buttons, LEDs, alpha-numeric 16 x 2 LCD display, temperature sensor and expansion connectors to access full devices pin-out and bread board prototyping area. Explorer 16 has 100 pin connections for connecting different kinds of dsPICs. 44pin dsPIC33Fj128mc804 is connected, which includes more I/O ports than the one used in the Olimex board. The chip comes with a plug-in module that converts 44 pin to 100 pin for the development board.


17

Additional soldering is done onto plug-in module in order to use UART module of the board. Microchip has 44 pins and the board has 100 pins, so some pins are not connected to the board where UART (TX, RX) connections was one of these pins that are not connected with development board. A connection is made to the 49th and 50th pins of the board from the pin 4 and pin 5 for using an RS-232 connection that can be seen in Figure 2.6. After additional soldering, the UART connection was successful in reading the data from HyperTerminal.

Figure 2.6: Plug-in module with additional soldering

As described in the methods chapter, in order to get good reading from the encoder sensors, selection of interrupt period is very crucial. This interrupt interval must be less than the minimum time required for half revolution at maximum speed. Too short period selection results in bad resolution and better sensitivity, because of so many readings. Longer periods result in better average speed value but less sensitivity. The timer period is selected as approximately 23 for achieving a successful reading from QEI.

Motor control is managed by two op-amps that are connected to the microprocessor. Selection of the op-amp model is important before construction of the circuitry because the Explorer 16 development board only supplies +3.3V, +5V and +9V supply voltage. LM741 is one of the most popular op-amps in the market. In the first design of the circuitry, LM741 op-amp has been used, which needs a positive voltage Vss and a negative voltage Vcc=-Vss for operation. A voltage inverter was necessary for negative supply. Therefore, the op-amp is exchanged for the CA1450 op-amp, which can operate only with a Vss and can supply only positive output, and this is sufficient for motor drivers. After the op-amps have been assembled, more stable results and higher speeds from the motors are obtained. Moreover, the microprocessor is protected by the op-amp from high current flow in case of undesired situations.


Vehicle

18

Construction of op-amp circuitry has made motor controlling mechanism more stable. However, with the RS-232 connection, it was possible to control the speed of both wheels by using a PC. As a conclusion, all the modules in the system, which are UART, QEI, DAC, op-amp, Dc motors and optical encoders, were working successfully. However high rate of speed change commands from the computer were making the motor speeds unstable due to some problems in the motor controller cards. Problem can be solved with stronger amplification.

After connecting all the elements in the microprocessor then it is possible to take measurements from the encoders. However, these measurements have to be analyzed respect to the physical properties of the robotic vehicle. Therefore, a mathematical model of the driving system is derived for converting the encoder measurement into motion of the vehicle.

Odometry

19

3 Odometry

Odometry is the first basic method for mobile vehicle navigation. The idea of odometry is the integration of incremental motion information over time. By using optical encoders that are attached to the wheels, it is possible to measure the distance that robot travelled and its heading angle.

3.1 System Model

The mobile vehicle is consisted of two opposed wheels where encoders are mounted on each wheel. Wheels are connected at the left and right sides of the vehicle, and have been connected by an axis. For calculating the traveled distance and the orientation angle of the vehicle, kinematic equations of the system are derived. Kinematic equations and the error model of the system is used for position prediction and finding the uncertainty of the position.

Figure 3.1: Illustration of drive system. A and C are left and right wheels of

the robot respectively.

Figure 3.1 shows the model of the robot, A and C are the initial positions of the wheels that are moving to points A' and C' respectively. Dr∆ and Dl∆ , denote the covered distances of the right and left wheels respectively. L is the wheel base distance, D∆ is the average of the outer and inner arcs and θ∆ is proportional to the difference of the outer and inner arcs.


Vehicle

20

(3.1)

Kinematic equations of the system are derived for calculation of the position (state variables). In equation set (3.1), rS and lS , are the distances taken by the right and left wheels and b is the wheel base. Function ),,,,( lr ssyxf ∆∆θ is used for prediction of next step, where x, y and θ are the state variables of the system (3.2). Assuming that [ ]'θYXP = and ),( lr ss ∆∆ are uncorrelated, then the error of propagation law (3.4) can be applied [1].

(3.2)

Error model of the vehicle (3.3), where rk and lk are constants that are taken as

m310 − [6]:

(3.3)

Error of Propagation Law (3.4), which determines the uncertainty in the predicted position:

(3.4)

The Jacobeans, which are the transposed of the gradient of )(xf in (3.5) and (3.6) [1]:

(3.5)

2

)2/sin(

)2/cos(

lr

lr

sss

b

ss

sy

sx

∆+∆=∆

∆−∆=∆

∆+∆=∆

∆+∆=∆

θ

θθ

θθ

∆−∆

∆−∆+∆

∆−∆+∆

+

=∆∆=

b

ssb

sss

b

sss

y

x

ssyxfp

lr

lr

lr

lr )2

sin(

)2

cos(

),,,,(' θ

θ

θ

θ

∆

∆=∆∆∑∆

ll

rr

lrsk

skss

0

0)cov(

∑∑∑ ∆ ∆∆ ∇∇+∇⋅∇= T

rlp rl

T

pp p ffff'

∆+∆

∆+∆−

=

∂

∂

∂

∂

∂

∂=∇=

100

)2/cos(10

)2/sin(01

θθ

θθ

θD

Df

y

f

x

ffF pp

Odometry

21

(3.6)

After driving these equations, driving system is modeled into system model which monitors the relative measurements from the encoders and result in new locations. Furthermore, it is possible to estimate the position error of the system by using the error of propagation law after all equations have been derived.

3.2 System Calibration

Integration of incremental motion leads inevitably to the accumulation of errors. Moreover, these accumulated errors would cause large position errors that increase proportionally with the distance traveled by the robot. Odometry provides high resolution in shot term travels. However, it gets less accurate proportionally with the length of the path, which has a typical error between 1.6% to 5% of the distance traveled.

There are different reasons for inaccuracies in the translation of encoder readings from the wheels into linear motion. These errors can be classified as systematic errors and non-systematic errors.

Systematic errors might be caused by unequal wheel diameters, measurement errors of wheelbase and wheel diameters, misalignment of wheels, finite encoder sampling rate and resolution. Non-systematic errors might be caused by uneven travel floors, unexpected objects on the path and wheel slippage [4].

All errors coming from systematic and non-systematic errors can be translated into range error, turn error and drift error, for classification of integration errors.

• Range error: this is the difference between the length from encoder readings and actual position.

• Turn error: this is similar to range error, but in angles for turns. • Drift error: this is the difference in errors from each wheel leads to an error in

the robots angular orientation that is similar to range error.

−

∆+∆

−∆+∆+∆

+∆+

∆+∆

+∆+∆+∆

−∆+

=∆

bb

b

D

b

Db

D

b

D

F rl

11

)2/cos(2

)2/sin(2

1)2/cos(

2)2/sin(

2

1

)2/sin(2

)2/cos(2

1)2/sin(

2)2/cos(

2

1

θθθθθθθθ

θθθθθθθθ


Vehicle

22

Figure 3.2: Growing uncertainty of a driving system.

After a long period of run, drift errors and turn errors are far outweigh than range errors. These errors have more uncertainty in the ‘y’ direction, which is coming from drift errors, results in error ellipses. Figure 3.2 shows the “error ellipses” that are indicating the growing uncertainty. It is necessary to make uncertainty analysis for finding the predicted position of the vehicle. Therefore, calibration of the system parameters is essential for accurate odometry.

After the connection of encoders and the PC, it is necessary to calibrate the system in order to get true results. There are three main parameters which have been used for estimating the position using odometry in a differential-drive system:

• The distance per increment for the left wheel. • The distance per increments for the right wheel. • The distance between the two wheels (axis length).

The number of increments of encoder in one tour of the wheel is constant which depends on the encoder and the gearbox type that have been used. The wheel is marked and rotated once to find the number of increments. After one rotation, positioning the marker back to the same location is a simple way of experimenting to find the number of increments per wheel tour. For the differential-drive system that is used, the increments per tour value is 13537 with an average of two full turns.

Axis length (455mm) and wheel diameter (200mm) are measured with standard measurement tools. In addition, distance per increment is calculated as dividing the circumference of one wheel to increments per tour value. For precise calibration of the system, a square-path experiment is conducted.

Start

position

Uncertainty error elipses

Estimated path of the robot

Odometry

23

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 x 10

0

0.5

1

1.5

2

2.5

3

3.5

x 10

X [mm]

Y [mm]

Uncertainity Prediction

Encoder Uncertainty

3.2.1 Square-Path Experiment

Encoder data is accumulating errors as the vehicle moves in time. In Figure 3.3, the blue line is shows the way that is measured from the encoder data and red lines show the uncertainty in the current position. Uncertainty gets bigger as vehicle moves, which is not practical considering a long travels. A square-path test is conducted to test the errors of optical encoders.

Figure 3.3: Growing Uncertainty

Verification of encoder errors is essential for estimating the position errors. Figure 3.4 shows a square path that the vehicle has traveled. The robot starts out at a position x0, y0, which labeled as, “Start”. To conduct the test, the robot has to traverse the four legs of the square path and turn back to its initial position. Then measurements are calculated for comparison of the absolute position and orientation of the vehicle.


Vehicle

24

End

(Final

position)

x0+ C

Direction of Travel

Start

(Initial

position)

x0, y0,

-2000 -1500 -1000 -500 0 500

0

500

1000

1500

2000

X: -44.54Y: -46.49

The Unidirectional Square-Path Test

X [mm]

Y [

mm

]

-2000 -1500 -1000 -500 0 500

0

500

1000

1500

2000

X [mm]

Y [

mm

]

Encoder Calibration

Encoder

Figure 3.4: Illustration of the Square-Path Experiment

The test is conducted on a 2390mm x1260mm rectangular path, after one round the vehicle is returned to its exact initial position. In Figure 3.5(left), a blue line shows the path taken by the vehicle. At the end point, the blue line circle is not completed. The difference between the start point and end point measure 44 mm in X, 46 mm in Y direction, which equals 6.3 cm in absolute range error.

Figure 3.5: Before (left) and after (right) the calibration of wheelbase by square path test.

After running approximately 7.3 meter path, 6.3cm error is accumulated which is not practical when long run ways are considered. For accurate calibration, wheel base distance is calibrated to match the start and end points onto each other, which is achieved by reducing the wheel base (453mm) distance manually by 2mm as shown in Figure 3.5 (right). After calibration has been done, errors from the encoders are minimized.

Odometry

25

3.3 Summary

Optical encoders provide very high resolution in small range for position estimation. However, integration of incremental motion information over time leads inevitably to the accumulation of errors. These errors would cause large position errors which increase proportionally with the distance traveled.

The positioning system that is based on optical encoders is not enough to navigate in long distances. It is necessary to integrate GPS and optical encoders to form a new positioning system, which will be able to retain all these advantages of GPS and optical encoders.


Vehicle

26

Global Positioning System (GPS)

27

6.3101 6.3101 6.3101 6.3101 6.3101 6.3102 6.3102 6.3102

x 106

8.0295

8.0296

8.0297

8.0298

8.0299

8.03

8.0301

8.0302x 10

5

GPS data

4 Global Positioning System (GPS)

Unlike to the optical encoders, Global Positioning System (GPS) has good resolution in global range but it has poor resolution for single point positioning. There are different situations that could mask or disorientate the signals from satellites such as, a vehicle passing by a high building, a bridge, ionosphere activities and satellite failures. Data output of GPS signal can indicate the quality of the GPS signal. Figure 4.1 shows the accuracy of GPS in different operating modes. [9]

Autonomous with S.A Accuracy 15 - 100 meters

Differential GPS

(DGPS) Accuracy 3 - 5 meters

WAAS Accuracy <3 meter

Figure 4.1: Operating modes of GPS.

Satellite geometry and pseudorange errors determine the position errors of GPS. Components of pseudorange errors can be modeled as either a Gauss-Markov process, or as white noise individually, where these errors are caused by combined satellite errors, atmospheric errors, and receiver errors. [11]

Figure 4.2 GPS measurements from a static receiver

Figure 4.2 shows a set of GPS data, where the GPS receiver is static at the same location. Error can be up to 82 meters with autonomous GPS fix. Error of the GPS is getting smaller when the receiver travels faster. However, considering low speeds of travel, error is very high and random.


Vehicle

28

Measurements from GPS in low speeds of travel are containing serious errors of accuracy. Moreover, there are possible different sources that can affect the measurements.

The robot vehicle which is used for this project had been designed for indoors. So, GPS measurements are taken in various places and paths outdoors without the robot vehicle. These measurements are taken by moving or placing the GPS device on real lines on the ground, where comparison of accuracy is considered. These data are used for examining the accuracy of GPS, various errors, implementing GPS filtering and least squares algorithm.

4.1 GPS Accuracy

GPS measurements might include different levels of noise which can arise from different sources. A set of experiments was conducted for examining the accuracy of GPS data. A BU-353 USB GPS receiver is used for measurements. Measurements are taken with a frequency of 10 Hz. Output data of GPS, which is in NMEA (WGS-84) mode is converted to meters in world coordinates. Two different areas, wide open and surrounded area are selected for observing different kinds of measurement errors.

Wide Open Area

A basketball field in a wide-open area is chosen for measurements. The ease of following the lines and the edges of the field are considered. The field is shown in Figure 4.3, where a blue line shows the measurement points.

Figure 4.3: GPS measurements at wide open area, picture adopted from eniro.se


29

8.0277 8.0277 8.0278 8.0279 8.0279 8.028 8.028

x 105

6.3094

6.3094

6.3094

6.3094

6.3094

x 106

LONGITUDE [m]

LA

TT

ITU

DE

[m

]

Mean of corners

Path 1

Path 2

The receiver is placed on each corner respectively for duration of 20 minutes and data from each corner is saved to find the absolute position of the field. The position of the corner is found by taking the mean of the measurement. After the corners of the field are measured, the border lines of the basketball field are traveled. Round trips are conducted on different days.

Figure 4.4: GPS data measured at open field

Figure 4.4 shows the measured data, where circles are the mean values of accumulated data of each corner (in cyan) and two different path are showing high and low quality data measured, where the speed of travel is approximately 4km/h.

Measurements of basketball field are showed that the corners are founded within the distance of 1.0 compared with the satellite map. In addition, the circumference of the field calculated as 87.9 meters, where the difference is less than 0.6 meters, which is very accurate. The bias of the GPS, which is the difference between expected value and true value is found within the distance of 2.15 meter.

Surrounded Area

GPS measurements are taken at the city center, where the area is surrounded by buildings. The number of satellites to which the GPS has been connected was less than the number in the wide-open field. A straight line is traveled. Start and end points of the path have been measured individually during 20 minutes similar to the basketball field measurements. In Figure 4.5, the blue line shows the measurement points.


Vehicle

30

0 1 2 3 4 5 6 7 8 9 10

x 104

-4

-3

-2

-1

0

1

2

3

4x 10

4

X [mm]

Y [

mm

]

Mean of end points

Avg. speed ~4km/h

Avg. speed ~10km/h

Figure 4.5: GPS measurements at surrounded area, picture adopted from eniro.se.

Figure 4.6 shows the measured data, where circles are the mean value of accumulated data of start and end points (in cyan) and two routes travelled at different speeds are presented.

Figure 4.6: GPS data measured at surrounded area


31

The maximum error of GPS during travel is measured as 7 meters with respect to the actual point. However, error can increase up to 30 meters when GPS is in a static position. Data measured at surrounded area is including more noise in comparison to wide-open area at similar speeds of travel. The noise of the system is including higher frequencies in surrounded area. Moreover, travel speed is affecting the noise in the system.

High and low frequency noises in GPS measurements are decreasing at higher speeds. Therefore, a digital filter for GPS and least square curve fitting algorithms are attempted for reducing the high frequency and low frequency noise from GPS.

4.2 Butterworth Filter

The paths that are taken by the wheels are very smooth and straighter in comparison to GPS measurements. In order to remove high frequency noises from GPS measurements a low pass filter can be used. A Butterworth filter which is a kind of low pass filter is constructed in the form in equation (4.1) [2].

(4.1)

k=0, 1, 2Z2N-1

N=3,

Ωc =6.28rad/s

Transformation of filter from s plane to z plane is shown in equation (4.2).

(4.2)

Where,

Sampling time could be taken as 10Hz considering that is the measuring frequency of the GPS. High frequency errors should be reduced by using low pass filter.

Ω

Ω=

−

Ω=

+

∏

Nevence

Noddcesk

sssG

Nkj

Njk

k k

n

c

.

.

)()(

/)12(

/

π

π

)1(

)1(21

1

−

−

+

−=

zT

zs

321

321

29.02217.17983.11

0167.005.005.00167.0)(

−−−

−−−

−+−

+++=

zzz

zzzzG


Vehicle

32

3 3.5 4 4.5 5 5.5 6 6.5 7 7.5

x 104

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

x 104

X [mm]

Y [

mm

]

GPS

Filtered GPS

GPS measurements at low speeds of travel can involve high frequency errors with respect to the real position. A low pass filter is implemented to reduce the high frequency errors in GPS measurements. Filtering is done by the Butterworth filter (low pass filter) that have been given in equation (4.2).

Figure 4.7: Before and after the low pass filtering of GPS

Part of GPS data that has been measured in the surrounded area is filtered, shown in Figure 4.7. The blue crosses show the GPS values that are taken along the way and red dots shows the filtered data. High frequency noise is smoothed out by using a low pass filter. However, low pass filtering real GPS data is not much improving the results of GPS measurements. Therefore, it is introducing a time delay like every low pass filter. As a result, low pass filtering is not improving the measurements significantly besides it is introducing a time delay. Therefore, it is not essential to implement in the system.

4.3 Least Squares Curve Fitting

The data taken from the observations include some noise. In the GPS filtering part, high frequency noises are reduced. However, the measurements still include some noise, where the curve of this system may not be smooth enough to process the encoders’ data. The method of least square data fitting is to approximate the data into a simpler polynomial curve or a line. This polynomial equation is derived respect to the minimal sum of deviations squared (least square) from a given set of data [10].


33

2cxbxay ++=

The Least-Squares Parabola

The least-squares parabola method involves using a second or third degree curve for curve fitting.

(4.3)

Assume that the data points are ),( 11 yx , ),( 22 yx , Z, ),( nn yx , where 3≥n , x is the independent variable and y is the dependent variable. Coefficients a, b and c are the unknown coefficients while all ix and iy are given. The fitting curve )(xf has the deviation (error) d from each data point, )( 111 xfyd −= , )( 222 xfyd −= ,...,

)( nnn xfyd −= . According to the method of least squares, the best fitting curve has the property:

(4.4)

To obtain the least square error the unknown coefficients a ,b and c , must yield zero first derivatives (4.5).

(4.5)

Expanding the above equations, we have

(4.6)

The unknown coefficients a, b and c can obtained by solving the above linear equations.

The curve fitting process is fitting the equations of approximating curves to the raw field data. For a given set of data, the fitting curves of a given type are generally not unique. However, a curve with a minimal deviation from all data points is desired. Best-fitting curve can be obtained by the method of least squares curve fitting.

min)(....2

11

222

2

2

1 =++−==+++=∏ ∑∑==

ii

n

i

i

n

i

in cxbxaydddd

[ ]

[ ]

[ ] 0)(2

0)(2

0)(2

1

22

1

2

1

2

=++−=∂

∏∂

=++−=∂

∏∂

=++−=∂

∏∂

∑

∑

∑

=

=

=

n

i

iiii

n

i

iiii

n

i

iii

cxbxayxc

cxbxayxb

cxbxaya

∑∑∑∑

∑∑∑∑

∑∑∑∑

====

====

====

++=

++=

++=

n

i

i

n

i

i

n

i

i

n

i

ii

n

i

i

n

i

i

n

i

i

n

i

ii

n

i

i

n

i

i

n

i

n

i

i

xcxbxaxy

xcxbxayx

xcxbay

1

4

1

3

1

2

1

2

1

3

1

2

11

1

2

111

1


Vehicle

34

1 2 3 4 5 6 7 8 9 10

x 104

-4

-3

-2

-1

0

1

2

3

x 104

X [mm]

Y [

mm

]

GPS

Filtered GPS

Least Square Curve Fitting

Low pass filtering can reduce the high frequency components. However, when the GPS noise level is high, the output of the filter is not close enough to the actual path of travel. The least square curve fitting algorithm takes the GPS data and simplifies that curve into a simpler parabola.

Figure 4.8: Illustration of Least Square Curve Fitting algorithm

In Figure 4.8, crosses (blue) shows the GPS data, the black line shows the low pass filtered GPS data and the red line shows the least square curve fit of the filtered GPS data. High frequency in GPS values are eliminated by filtering. However, the low pass filtering method is not significantly improved the output. Applying the least square curve fitting method onto the output of the low pass filter results in a smoother line, which is closer to the real path. The distance error from the actual path is minimized on the red line. The results are getting closer to the real path, when low pass filtering is used together with least square curve fitting.


35

40 50 60 70 80 90 100 110 120 130

-10

0

10

20

30

40

50

60

GPS curve

Least Square Curve Fit

50 60 70 80 90 100 110 120 130-10

0

10

20

30

40

50

60

GPS curve

Least Square Curve Fit

Essence

The least square curve fitting algorithm is eliminating some parts of the noise considering that travelled line is close to a straight line. However, examining the fitted curves there are some cases found that curve fitting algorithm is failing.

Figure 4.9: Least squares curve fitting of a non-single value function, fitting axis=y (left), fitting

axis=x (right).

In Figure 4.9, least square method is implemented for a non-single valued function. The fitting axis taken as y in the Figure 4.9 (left) which is the same direction of travel. However, the end points of the fitting has large errors respect the GPS curve. On the other hand, if the fitting axis is taken as x in the Figure 4.9 (right) then the fitted line of the GPS are not matching the real GPS line.

First, fitted curves have the largest errors at the beginning and the end where the end of the curve is the most important information for vehicle positioning. Second, the fitting axis has to be chosen perpendicular to the direction of travel. For example, if the vehicle is travelling on the x axis then the fitting axis has to be chosen on y axis so that fitted curve will be in the direction of motion. As a result, if the direction of the vehicle changing continuously then the fitting axis has to be selected carefully for a correct fit. Moreover, if the curve that will be fitted is a non-single value function circle then the fitted curve is not in the direction of vehicles motion. In other words, if the vehicle is changing its direction frequently then fitted curves will be failed in most cases. Respect to these reasons, using least square algorithm is not a trustable choice for eliminating the low frequency noise of GPS measurements.


Vehicle

36

4.4 Summary

Errors from GPS are highly dependent on the area and speed. GPS measurements that are taken in surrounded areas at low speeds of travel are including strong high and low frequency noise. Least square curve fitting and Butterworth filtering methods attempted to eliminate the low and high frequency noises in GPS measurements. However, it is found out that these methods have more disadvantages than their advantages in practice. Therefore, improving GPS quality using these methods is not the best choice in reality. Moreover, Butterworth filter might be used to simulate more realistic GPS measurements that are created by Gaussian distribution.

A combination of GPS and encoders can inherit the advantages of both and overcome their disadvantages. Two different algorithms are evaluated for matching the GPS data and encoder data to compare the similarities and differences of both methods. Kalman Filtering and real time curve matching algorithm are implemented separately for sensor fusion. Following chapters are explaining and analyzing the properties of these sensor fusion methods.

Real Time Curve Matching Method

37

5 Real Time Curve Matching Method

Optical encoders have very high precision over a short run. In other words, encoder precision is limited with a distance that the vehicle is travelled. Encoder information could be moved periodically into global coordinates with each GPS measurement. Therefore, encoders precision could be transferred into global coordinates.

The real time curve matching (RTCM) algorithm [2] is a fast and simple optimization algorithm. The position ),( pp yx is simply determined by the first point of GPS data to be matched and the end point of the last matched-curve, which is illustrated in Figure 5.1. It is expressed as:

(5.1)

Where ),( cece yx is the end point of the last matched curve, a and b are weighting

coefficients for the GPS curve and the matched curve. The first estimated position of GPS is ),( pipi yx . By the equation (5.1) encoder data is moved to the point ),( pp yx . Weighing coefficient a determines the place of the matched curve with respect to the GPS curve. If the weighting coefficient is low, then the matched curve moves closer to the encoders curve and if value of a close to one, matched curve moves closer to the GPS curve.

1

),(),(),(

=+

+=

ba

yxbyxayx cecepipipp


Vehicle

38

-1000 -500 0 500 1000 1500

0

200

400

600

800

1000

1200

1400

1600

1800

2000

← first match

GPS(2)

Match(2)

GPS(3)

Match(3)

GPS(4)

Match(4)

GPS(5)

Match(5)

GPS(6) Match(6)

Figure 5.1: Illustration of RTCM algorithm, Dead reckoning (dotted, green), Matching

point(circle, blue),GPS measurement(diamond, blue)

In Figure 5.1, illustration of the method is plotted. Robot vehicle is started to travel from the point (0, 0) and travelled 20 steps that is shown in blue dots. Assumed that first GPS measurement is taken at 20th step. Therefore, all the data accumulated in 20 steps moved to the first GPS measurement that is shown in red dots. After the initial matching robot continued to travel five steps from the matching point which is shown in green dots. The next matching circle (blue) is done respect to the new GPS point and the end of the dead reckoning shown in x (blue). The matching point is always between the GPS point in diamond (blue) and the end point of the last matched curve in cross (blue) that is determined by the weighing coefficient. As a result method is finding a optimized matching point between encoders and the measurement.

Matching steps should be shorter than the limit of required resolution from encoders. For example, if the required resolution is achieved at 40 meters of travel then taking 20 meters is better for matching because of cumulative errors from encoders. Then, 18 meters can be used for matching and the remaining 2 meters is long enough for dead reckoning for a vehicle moving at human walking speed with a GPS, which has 10Hz sampling frequency [2].

Kalman Filters

39

6 Kalman Filters

A Kalman Filter (KF) [3, 5, 7, 8, 14] is a recursive algorithm that uses a series of predictions and measurement update steps to obtain an optimal estimate of the state of the noisy system. State of a system is a vector that could include information about the system. The location of a robot, including x and y coordinates, and the orientation a of a robot can be an example of the state vector of the system.

In reality, every measurement that forms the state of the system includes some noise and makes a problem of state estimation. A KF uses all the measurements of the system to estimate the state of the system optimally. These measurements could be both accurate and inaccurate. All the knowledge about the system, which might be sensor dynamics, measurement noises, system descriptions and data about the initial properties of the system, are all usable information for a KF. Combining all the information available for the best state estimation makes the KF a statistically optimal estimator respect to any measure of optimality [12].

The KF has many application areas in order to control of a dynamic system ever since the method is discovered by Richard Kalman in 1960. Inertial measurement units of aircrafts, ships, spaceships, rockets and controlling some manufacturing processes are some examples of the application areas of a KF. In addition, KFs are used to predict the future of a dynamic system such as trajectories of celestial bodies.

6.1 Idea of Prediction and Estimation

To understand the idea of the KF, a robot vehicle navigating in an environment could be given as an example to start. Suppose that a robot vehicle navigating at a constant speed c. The system model shows the true location kx of the robot at time k [13],

(6.1)

The location of the robot kx is decided by previous location 1−kx , the constant speed c, and the noise term kw . Noise term is assumed as Gaussian distributed where on average, the noise is zero and it has deviation of wσ .

Assuming that the robot has access to absolute measurements to estimate its location. The measurement model shows relation between the location of the robot

kx and measurement kz .

,1 kkk wcxx ++= −


Vehicle

40

(6.2)

where measurement sensor gives location of the robot kx corrupted by measurement

noise kv . The noise is Gaussian distributed with zero mean and deviation

vσ .

Prediction

Assume that initial estimate of the robots position is 0x with an uncertainty where

variance of 2

0σ . Considering, on average, the location of the robot change with about c. Then by using system model, we can predict the new location 1x after each step.

0ˆˆ01 ++= cxx (6.3)

The state is corrupted by noise kw in the system model however, the exact noise is

unknown at certain time k and it is known that the noise on average is zero because noise is Gaussian distributed. Therefore, noise term is 0=kw for calculating the location prediction. In addition, noise term varies around zero and new uncertainty

2

1σ estimated as,

22

0

2

1 wσσσ += (6.4)

Correction

Without absolute measurements, the uncertainty of the robot is continuously increasing as it keeps driving. Correction step of the filter is combining the predicted location 1x and absolute measurement kz has taken. These information add up together in a weighted way to correct the prediction has been made in equation (6.3).

(6.5)

The equation (6.5) can be simplified by introducing K that is representing the weighting factor

,kkk vxz +=

122

1

2

1122

1

2

1ˆ zxx

vv

v

σσσ

σσσ

++

+=+

)ˆ(ˆ1122

1

2

11 xzx

v

−+

+=σσ

σ

Kalman Filters

41

(6.6)

Then equation (6.5) becomes as

)ˆ(ˆ1111 xzKxx −+=+

The weight of incorporating the measurement is highly depending on the uncertainty in the measurement. If the uncertainty in the measurement 2

vσ is high respect to the uncertainty in the prediction 2

1σ then the measurement is not much taken into account.

Moreover, incorporating absolute measurements are decreasing the uncertainty in the location estimate. The new uncertainty in the location estimate +.2

1σ is found by combining the uncertainty of the prediction and the measurement.

(6.7)

Rewrite into

2

1

2

1

,2

1 σσσ K−=+

2

1)1( σK−= (6.8)

From the equation (6.8), the uncertainty +,2

1σ is smaller than both the estimated uncertainty 2

1σ and uncertainty of the measurement 2

vσ . Introducing new information to the system is result in improved uncertainty.

The amount of information from the measurements that must be taken into account is determined by weighing factor K. If uncertainty in the measurement is very high, then K will approach to zero and no measurement information taken into account. If the last location estimate has very high uncertainty respect to measurement then K will approach to one where measurement is highly taken into account. The weighting factor K is called Kalman Gain.

The example of a robot vehicle navigating in an environment has one variable in the system model and measurement model. The KF which can be called as Linear Kalman Filter (LKF) is a state estimator where the system model and measurement model are described by a linear equations. However, in some situations problems could be nonlinear, then the KF does not provide the optimal solution. Problems that are including nonlinear terms can be solved with extended Kalman Filter (EKF) [14].

22

1

2

1

v

Kσσ

σ+

=

22

1

.2

1

111

vσσσ+=

+


Vehicle

42

6.2 Extended Kalman Filter

The EKF method is used for many applications for sensor fusion, EKF method is used specifically for integration of GPS and encoder data. To estimate the position and orientation based on GPS data and encoder data, EKF is used for sensor fusion. Figure 6.1, shows the system of EKF,

321 ,, ΣΣΣ are the covariance of the encoder, GPS and the estimated output of EKF respectively. The encoder measurements that are the position (x, y) and the heading angle (a) are integrated with GPS measurements. The total output is (x, y, ψ), where (x, y) are the position of the reference point in the global coordinate and (ψ) is the vehicle heading angle.

Figure 6.1: EKF inputs and outputs

6.2.1 System Model

In EKF, the state vector is [x, y, ψ]T. The position and angle data from encoders are taken as the control input. The system model of the EKF is the same model as used in uncertainty analysis.

,)( 11 −− += kkk wxfx (6.9)

where )( 1−kxf is in the form of equation (3.2). The error vector kw of the system model is with zero-mean and Q2 error covariance. The location of the vehicle at time step k depends on the location at one step before. Therefore, system noise is increasing the variance.

The measurement model kz of EKF shown in equation (6.10). Assuming that there is a Gaussian distributed noise with zero mean and error covariance kR that is incorporated in the measurements. Measurement model returns the location of the robot )( kxh that is influenced by measurement noise kv .

EKF

ENCODER

(x, y,a), ∑1

GPS receiver

(x, y), ∑3

SYSTEM OUTPUT

(x,y,ψ), ∑2

Kalman Filters

43

kkk vxhz += )( (6.10)

The measurement model is corrupted by independent Gaussian distributed noise vector

kv with zero mean and deviationvσ . Covariance of the noise described with

kR where

(6.11)

6.2.2 Algorithm

Initialization

The EKF has to be initialized for the first step of posterior state estimate +0x and

uncertainty +0P .

State prediction

Prediction of the next state is done respect to the posterior state estimate at each time step. Predicted uncertainty is calculated respect to the Jacobian matrix kF of the system function f(x) that is evaluated at posterior state estimate +

−1ˆkx is shown in

equation (6.13).

)ˆ(ˆ1

+−

− = kk xfx (6.12)

11 −−− += k

T

kkkk QFPFP (6.13)

Since system is propagating the noise coming from the encoders influence the predicted uncertainty. Hence, the uncertainty of propagation 1−kQ is added to the predicted uncertainty (6.13), refer equation (3.6) and (3.3).

T

krlkkrlk FFQ ,,, ∆∆∆ Σ= (6.14)

==

==

)()()(

)()()()(

22

11

kykxkh

kxkxkhxh k

[ ]

[ ]

=

2

,

2

,

0

0

kyv

kxv

kR σ

σ


Vehicle

44

Correction

The predicted state and uncertainty is corrected by the new measurement which is weighted by Kalman Gain kK ,

1)( −−− += k

T

kkk

T

kkk RHPHHPK (6.15)

))ˆ((ˆˆ −−+ −+= kkkkk xhzKxx (6.16)

−+ −= kkkk PHKIP )( (6.17)

where kH is the Jacobian matrix respect to the partial derivatives of )( kxh ,

(6.18)

This algorithm combines two different input data with different uncertainties and output as a better estimation with lower uncertaint

=

∂

∂=

−= 010

001

ˆkxx

kk

x

hH

Sensor Fusion Simulations

45

7 Sensor Fusion Simulations

In order to implement and evaluate the theories that have been explained in the previous chapters, a differential drive vehicle is used for odometric measurements. In order to communicate and control the mobile vehicle, a programmable interface controller (PIC) is used for reading the encoder values and controlling the motors.

The comparison of EKF and RTCM method is done by simulating GPS measurements onto real encoder data. The expected output should be similar to the encoders’ output in world coordinates for maximum precision. Simulated GPS measurements are considered as every GPS measurement is taken from the center of the vehicle, which varies with respect to the noise of operation mode of GPS. Noise of GPS can be considered as Gaussian noise. Outputs of both algorithms are considered and examined at different noise levels. Initialization of encoder data should be based on GPS measurements that GPS points can be simulated on measured encoder data with given noise. These simulations are done on the assumption of having the availably of GPS readings all the time. However, in reality odometric measurements must be initialized by the latest GPS measurement before starting to navigating [10].

7.1 Real Time Curve Matching Simulations

Estimations of RTCM method are affected by three main parameters. Therefore, simulations are done for different levels measurement noise, weighing coefficient a and the sampling rate of GPS. GPS measurements are simulated as Gaussian distributed with zero mean.

7.1.1 Estimation Error

Estimation error is analyzed to compare the effects of different parameters. Therefore, estimation error is calculated by taking the difference of true position and the matching point for each matching step. In Figure 7.1, the matching process is plotted. It is assumed that a measurement is taken every 7th step of the encoder. The weighting coefficient a and the variance of each measurement taken as constant.


Vehicle

46

-2000 -1500 -1000 -500 0 500 1000

0

500

1000

1500

2000

2500

← f irst match

GPS(2)

M(2)

GPS(3)

M(3) GPS(4)

M(4) GPS(5)

M(5)

GPS(6) M(6)

GPS(7)

M(7)

GPS(8) M(8)

GPS(9)

M(9) GPS(10) M(10)

GPS(11)

M(11)

GPS(12) M(12)

GPS(13)

M(13)

GPS(14)

M(14)

GPS(15)

M(15)

GPS(16)

M(16)

GPS(17)

M(17)

GPS(18)

M(18)

GPS(19) M(19) GPS(20) M(20)

GPS(21)

M(21) GPS(22)

M(22)

X [mm]

Y [

mm

]

0 5 10 15 20 250

20

40

60

80

100

120

140

160

STD(34)

Maen(44)

Estimation error

Mean of Estimation

0 20 40 60 80 100 120 140 1600

20

40

60

80

100

120

140

160

STD(35)

Mean(46)

Estimation error

Mean of Estimation

Figure 7.1: Real time curve matching algorithm with measurement taken every 7th step, dead

reckoning (dotted, green), matching point(circle, blue), GPS measurement(diamond, blue), true

trajectory(line, blue) ,weighing coefficient a=0.2

The process of matching is analyzed by calculating the estimation error at every step. In Figure 7.2 (left), the estimation error of the curve matching process in Figure 7.1 is plotted for each step.

Figure 7.2: Estimation error at different measurement frequencies, measurement is

taken every 7th step (left), measurement taken at each step (right).

By analyzing the error, that RTCM method is estimating, the effects of the sampling rate of the measurements, and weighting coefficient are observed.


47

-2500 -2000 -1500 -1000 -500 0 500 1000 1500

0

500

1000

1500

2000

2500

X [mm]

Y [

mm

]

7.1.2 Sampling Rate

Considering that, after each matching, the robot continues to travel by dead reckoning respect to the last matching that has been estimated. Besides the error of the encoders that are increasing at each step, also the frequency of the measurements might be affect the overall error. In Figure 7.2 (right), it is assumed that a measurement is taken at each step of the encoder. However, there is no significant change is observed in the mean of the estimation error by changing the frequency of the measurements. Considering the GPS measurements are simulated respect to the Gaussian distributed noise, the mean of the estimated error is not affected much respect to the measurement frequency.

Figure 7.3: Butterforth filtered Gaussian noise for realistic GPS measurements. Filtered data

(dotted, red), True position (line, blue), Gaussian noise (cross, blue).

In realty, the GPS measurements are markov processes. In Figure 7.3, the measurements that are simulated as a Gaussian distribution is filtered by a butterworth filter that is explained in the previous chapter. After filtering the Gaussian distributed GPS data, the separated GPS measurements are combined and fitted in a line that is looking more likely to be a markov process. Therefore, the simulation of the GPS is reflecting the reality by the filtering operation. The affects of sampling rate of the measurement is considered again. Assuming that weighting coefficient and the noise of GPS are constants then it is found out that the estimated error is decreasing as the frequency of the measurement increase.


Vehicle

48

0 20 40 60 80 100 120 140 1600

5

10

15

20

25

30

35

40

45

50

Estimation error

Mean of Estimation

0 2 4 6 8 10 12 14 16 18 200

10

20

30

40

50

60

70

Estimation error

Mean of Estimation

Figure 7.4: Estimation error at different measurement frequencies, measurement taken at each

step (left), measurement taken at each 8th step (right).

In Figure 7.4, estimation errors of different measurement frequencies are plotted. In comparison, measurement taken at each 8th step has 44% higher estimation error than the measurement taken at each step. Therefore, the estimation error is decreasing as increasing the frequency of the measurements. Unlike the KF, the estimated error is not converging to a steady phase. Moreover, it is continuing to oscillate because of the constant weighting coefficient.

7.1.3 Weighting Coefficient

As explained in KF section the amount of information from the measurement that is going taken into account is primarily decided by in Kalman Gain. In analogy, the weighting coefficient a of RTCM method is similar to the Kalman Gain, because it is deciding the amount of information from the measurement that is going taken into account. However, in RTCM, weighting coefficient a is a constant. Therefore, it has to be set respect to the noise in the measurement and encoder errors at the initialization. Moreover, if GPS values a have higher noise then weighting coefficient has to be small because the measurements are not trustworthy. The weighing coefficient a could be taken as 0.2 or 0.3 depending on the measurement noise. Therefore, comparison of different levels of measurement noise is not required for observing how is the system working in different noises, because the weighing coefficient a has to be decided respect to the measurement noise.


49

7.1.4 Re-Initialization

Initialization of the robot is done respect to the first measurement from the GPS. However, in some cases robot could be transferred or drift to another location suddenly. Then it will result as unexpected measurements from the new location. In other words, the difference between the end of the last matched curve and the measurement will be significantly large. However, method will continue to work and recover itself after some time but during this time, the matching points will be incorrect. A sub algorithm could be used to detect the unexpected measurement. If the difference in the measurement and the predicted measurement is larger than its limit, than the system could re-initialize itself.

7.2 Extended Kalman Filter Simulations

In the EKF section, the prediction and correction equations have been derived. In this part EKF is simulated in the computer environment. The EKF is simulated respect to the system model and measurement model of differential drive robot. The system model is using the real encoder data taken from differential drive robot has been build. As explained in the previous sections, measurements of the GPS are simulated as a measurement model. Measurement and corrections are done in every step of the travel. Measurements modeled as Gaussian distributed noise with zero mean. Different levels of measurement noise is considered.

7.2.1 Measurement Noise

In Figure 7.5 and 7.6, the EKF simulated respect to high and low measurement noise values. The GPS measurements are simulated as Gaussian distributed with zero mean. In both figures, measurements are shown in x (blue). The initialization of the starting position done respect to the first GPS measurement that has been taken. Therefore, the uncertainty of the initial point has the same covariance of the GPS measurements because absolute position of the robot has the same precision of the GPS measurement. Estimated position uncertainty of the EKF is shown in red circles.


Vehicle

50

-2000 -1500 -1000 -500 0 500 1000 1500 2000

-500

0

500

1000

1500

2000

2500

X [mm]

Y [

mm

]

Kalman estimated position uncertainty

Encoder position uncertainity

GPS measurements

-1500 -1000 -500 0 500 1000 1500

0

500

1000

1500

2000

2500

X [mm]

Y [

mm

]

Kalman estimated position uncertainty

Encoder position uncertainity

GPS measurements

Figure 7.5: EKF position estimation for high noise measurements

Starting point has the highest uncertainty that the robot could be in any location inside the red circle. Also encoders position uncertainty is plotted in blue circles that shows the true position of the robot. As explained in the previous chapters at each step encoders are accumulating errors so that the uncertainty of the encoders is growing. However, estimated position of the EKF is decreasing at each step.

Figure 7.6: EKF position estimation for low noise measurements


51

0 20 40 60 80 100 120 140 160 1800

0.1

0.2

0.3

0.4

0.5

0.6

0.7

step

valu

e

Kalman Gain

0 20 40 60 80 100 120 140 160 180 2000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

step

valu

e

Kalman Gain

7.2.2 Kalman Gain

Kalman Gain primarily decides the amount of information coming from measurements that should be taken into account for state estimation. If the measurements have high precision respect to the encoders precision then EKF should be count them as trustworthy information. Therefore, Kalman Gain should be high because measurements are reflecting the true state of the system. However, if the measurements have high uncertainty then Kalman Gain should be low because they are not trustworthy.

In Figure 7.5 and Figure 7.6, the estimated state of the EKF has been plotted respect to high and low measurement noise. In Figure 7.7, Kalman Gain of both runs are plotted respectively for each step. Both figures start with high Kalman Gain and converge close to zero. For the first 20 steps, Kalman Gain is high because measurement information is more trustworthy than the encoders. However, Kalman Gain is decreasing and become stable after some point where the measurement information is counted as less trustworthy.

Figure 7.7: Kalman Gain at each estimation step respect to high measurement noise (left) and

low measurement noise (right)

In Figure 7.7 (left), the measurement model has higher noise, as a result overall Kalman Gain is lower than the Kalman Gain in Figure 7.7 (right). Moreover, system is converging to its stable point faster. The uncertainty in the prior state estimate and uncertainty in the measurement are affecting the Kalman Gain. Kalman Gain is finding a balance between these two uncertainties.


Vehicle

52

7.2.3 Convergence of Uncertainty

In Figure 7.5 and 7.6, the estimation of EKF has the highest uncertainty at initial position. Although the increasing uncertainty of the encoders, the estimated uncertainty of EKF is decreasing at each step. Moreover, in Figure 7.6, estimated position becomes more precise than the encoders precision before the vehicle complates its turn respect to the low noise in the measurement model.

Convergence of estimated uncertainty occurs until the Kalman Gain becomes stable. In other words converging occurs until the EKF finds a balance between two uncertainties, considering a constant measurement noise at each step. Moreover, if the noise of the measurements is low, then it is taking less time to converge before Kalman Gain reaches its stable state. As a result, EKF finds equilibrium between the uncertainty of the measurement and uncertainty of the prior state estimate that the estimated uncertainty and converges to a stable phase eventually.

7.2.4 Re-Initialization

Initialization of the robot is done respect to the first measurement from the GPS. Since the robot does not know its exact location then it could be anywhere inside the location space that determined by the uncertainty of the measurement. As the vehicle travels the uncertainty is decreasing by the estimations of EKF where robot is getting more aware of its location. However, in some cases robot could be transferred or drift to another location suddenly. Then it will result as unexpected measurements from the new location. In other words, the difference between measurement predictions and the measurement will be significantly large. Nevertheless, EKF will continue to work and recover itself after some time but during this time, the state estimates will be incorrect. However, in this situation, EKF should trust more on the measurements because robot is in a new location. As a result state estimates will be wrong until the Kalman Gain adjusts itself.

A sub algorithm could be used to detect the unexpected measurements. If the difference in the measurement and the predicted measurement is larger than its limit EKF could re-initialize itself.


53

7.3 Summary

The uncertainty of the encoders is increasing as the vehicle moves. For practical applications, the motion of the vehicle is not limited in small range. Hence, integration of GPS is crucial for long range runs.

The EKF and the RTCM methods are implemented for sensor fusion. Both methods are processing the information iteratively respect to the last estimation and the measurement taken. It could be said that both methods have similar way to approach to the problem of state estimation. However, EKF is more advanced than the RTCM because it is using all the available information in a probabilistic way to find an optimal solution.

In RTCM, the amount of information from the measurement that should be taken into account is decided by weighting coefficient, which is a constant. Considering that, the uncertainty of the differential drive is increasing at each step and uncertainty of the measurement is a constant. Then, using a constant weighing coefficient at every step is far from ideal for finding the optimal estimation. However, in EKF, the Kalman Gain is a dynamic weighting coefficient that is arranging itself optimality at each step, respect to the uncertainty of the last state estimation and the measurement. Therefore, EKF finds a balance between the information that should be trusted. As a result, EKF is adapting itself to the situation and finds an optimal solution to the localization problem.


Vehicle

54

Conclusions

55

8 Conclusions

This project is a combination of both hardware and software systems. For the hardware part, the selection of the microcontroller was the first important part. However, some connection problems with processor and development board have been found and documented. The selection of the dsPIC33 family microprocessor was a good choice, where all the modules that have been embedded in the microprocessor.

Odometric measurement from encoder data by using kinematic equations results high accuracy for a short period, and where good calibration is essential for precision. However, if long run paths are considered, odometric measurements are result in high orientation errors. Moreover, odometric measurements do not provide information about world coordinates. However, GPS provides good resolution in the global range, but it has poor resolution compared to the odometric measurements.

In reality GPS signals can become blocked or unavailable at some times, in these situations, the vehicle has to navigate based on odometric measurements together with the last GPS measurement have been taken. When the GPS is available, odometric measurements have to initialize again, based on latest GPS data. In addition, GPS measurements include higher noise when the travel speed is lower.

RTCM and EKF methods are implemented for integration of GPS and odometric data for obtaining accurate global navigation. The results of the experiments have shown that good results are achieved proportionally with the accuracy of the GPS data by both of the methods. Moreover, in both methods the frequency of the measurements are affecting the error in state estimates. Having fewer measurements is resulting in lower precision in the state estimations.

As a conclusion, EKF is advanced version of RTCM method in some sense. The EKF is incorporating all the information available that is including the variance of each sensor for finding an optimal solution to the state estimation. It is adapting itself at each step to find an equilibrium point between the information that should be trusted. Moreover, it is providing the information of state estimate uncertainty at each step as an advantage. Therefore, EKF is finding the optimal solution to the robot localization problem.


Vehicle

56

The purpose of this work is to find a way to design a robot vehicle that can navigate precisely in a global range. With the presented work a robotic drive system is investigated in many aspects. In the investigation of this embedded and intelligent system, different kinds of modules and methods are combined together. By using GPS and encoder combining methods, I am hoping that an automated vehicle can be commanded precisely through internet maps like “Google Earth”. By using EKF, a robot vehicle can navigate to a specific location precisely by using internet maps. Even after loosing its GPS signal and it would be possible to navigate precisely in the mean time. So that, methods can be used both indoors and outdoors mutually, where a robot vehicle has to complete some tasks.

References

57

9 References

[1] C. Ming Wang, “LOCATION ESTIMATION AND UNCERTAINTY ANALYSIS FOR MOBILE ROBOTS” Computer Science Department, General Motors Research Laboratories, Warren, Michigan 1988.

[2] Lei Tian , Yunshan Zhou, Lie Tang, “Improving GPS positioning precision by using optical encoders”, IEEE Intelligent Transportation Systems, USA Octoberl-3, 2000.

[3] Bo He, Danwei Wang, Minhtuan Pham, Tieniu Yu, ”GPS/Encoder Based Precise Navigation for a 4WS Mobile Robot” Seventh International Conference on Control and Automation, Robotics and Vision (ICARCV’02) Singapore. Dec 2002.

[4] J. Borenstein, H. R. Everett, and L. Feng, Contributing authors: S. W. Lee and R. H. Byrne “Where am I?” April 1996.

[5] Sultan Kocaman,”GPS and INS Integration with Kalman Filtering for Direct Georeferencing of Airborne Imagery”, GEODETIC SEMINAR REPORT

Institute of Geodesy and Photogrammetry, ETH Hönggerberg, 2003, Zurich.

[6] Kai O. Arras, Nicola Tomatis, Björn T. Jensen, Roland Siegwart, “Multisensor on-the-fly localization: Precision and reliability for applications”,Autonomous Systems Lab, Swiss Federal Institute of Technology Lausanne (EPFL), CH-1015 Lausanne, Switzerland.

[7] Peter S.Maybeck,”Scholastic models, Estimation and control”, Volume1, Department of Electrical Engineering Air force institute of Technology ,1999 Ohio,

[8] Robert Grover Brown. Introduction to Random Signal Analysis and Kalman Filtering. John Wiley and Sons, 1983.

[9] Sweet, Robert J. GPS for Mariners. McGraw-Hill, 1Mar 2003.

[10] John H. Mathews and Kurtis K. Fink. Numerical Methods Using Matlab, 4th Edition, 2004

[11] James Rankin, Ph.D., “P.E GPS and Differential GPS: An Error Model for Sensor Simulation”, IEEE, St. Cloud State University', St. Cloud, I994.


Vehicle

58

[12] M. Grewal and A. Andrews, Kalman Filtering: theory and practice,Prentice-Hall, Inc., Englewood Cliffs, New Jersey (1993).

[13] Rudy Negenborn, Robot Localization and Kalman Filters, “On finding your position in a noisy world”, UTRECHT UNIVERSITY, 2003.

[14] Roland Siegwart, Illah Noutbakhsh, Introduction to autonomous mobile robots MIT Press, Cambridge, 2004.

Appendix

59

10 Appendix

10.1 Module Initializations

10.1.1 Initialization of the DAC module

1) DAC Module Enabled

DAC1CONbits.DACEN = 1;

2) Left and right channels enabled which are used for left and right wheels respectively

DAC1STATbits.ROEN = 1; /* Right Channel DAC Output Enabled */

DAC1STATbits.LOEN = 1; /* Left Channel DAC Output Enabled */

3) Left and right channels interrupt if FIFO (First in First out) is not Full

DAC1STATbits.RITYPE = 0; /* Right Channel Interrupt if FIFO is not Full */

DAC1STATbits.LITYPE = 0; /* Left Channel Interrupt if FIFO is not Full */

4) Amplifier Disabled during Sleep and Idle Modes

DAC1CONbits.AMPON = 0;

5) Divide Clock by 1 (Assumes Clock is 25.6MHz) because really fast response speed doesn’t needed for not effecting other processes in the system.

DAC1CONbits.DACFDIV = 0; */

6) Data Format is signed and Mid Point of the voltage set as 1.65 volts. Positive values for forward travel, negative values are for backward travel proportionally with the DAC control values respectively.

DAC1CONbits.FORM = 1;

DAC1DFLT = 0x0000;


Vehicle

60

7) Interrupts are used for more stable system responses both in left and right chanel

IFS4bits.DAC1RIF = 0; /* Clear Right Channel Interrupt Flag */

IFS4bits.DAC1LIF = 0; /* Clear Left Channel Interrupt Flag */

IEC4bits.DAC1RIE = 1; /* Right Channel Interrupt Enabled */

IEC4bits.DAC1LIE = 1; /* Left Channel Interrupt Enabled */

8) Initialization of DAC Clock FRC with PLL selected as the Auxiliary Clock Divider and Auxiliary Oscillator disabled and Auxiliary Clock Output Divider by 1.

ACLKCONbits.SELACLK = 0; /* FRC w/ Pll as Clock Source */ ACLKCONbits.AOSCMD = 0;

ACLKCONbits.ASRCSEL = 0;

ACLKCONbits.APSTSCLR = 7; /* FRC divide by 1 */

After initialization two methods defined setvoltL, setvoltR takes input as an integer value

which provides desired output for the DAC.

10.1.2 Initialization of QEI

1) Continue module operation in idle mode QEI1CONbits.QEISIDL = 0

2) Position Counter Direction is positive (+) QEI1CONbits.UPDN = 1

3) Read only - Index pin state status pin. QEI1CONbits.INDX = 0

4) Use 4x Update mode with reset on index reset QEI1CONbits.QEIM = 7

5) Phase A and Phase B inputs are swapped QEI1CONbits.SWPAB = 1

6) Counter Direction Status Output QEI1CONbits.PCDOUT = 0

7) Index Pulse resets Position Counter QEI1CONbits.POSRES = 1;

8) Interrupts due to count errors are disabled DFLT1CONbits.CEID = 1;

Appendix

61

9) Digital filter outputs enabled DFLT1CONbits.QEOUT = 0

10)1:128 clock divide for digital filter for Index DFLT1CONbits.QECK = 2

11) Maximum counter set for preventing overflow. MAX1CNT=32000;

Initialization of the right channel is same as the left channel except Phase A and Phase B

inputs are swapped QEI2CONbits.SWPAB = 0 because the direction of the rotation mirrored

on the other wheel.

10.1.3 Initialization of Timer

1) Reset timer counter TMR1 = 0

2) Turn off timer 1 T1CONbits.TON = 0;

3) Continue operation during sleep T1CONbits.TSIDL = 0;

4) Gated timer accumulation disabled T1CONbits.TGATE = 0;

5) Use Tcy as source clock T1CONbits.TCS = 0;

6) Tcy / 64 as input clock T1CONbits.TCKPS = 2;

7) Interrupt period = 0.75 sec with a 64 prescaler PR1 = 1728100;

8) Clear timer 1 interrupt flag IFS0bits.T1IF = 0;

9) Enable timer 1 interrupts IEC0bits.T1IE = 1;

10) Turn on timer 1T1CONbits.TON = 1;


Vehicle

62

DAC OUTPUT

-

+

U2

CA1450

3

4

6

7

2

En

co

de

r1

Red Vcc=+5vBlue A'Green AGray B'Violet BOrange GNDWhite indexBlack Index

-

+

U3

CA1450

3

4

6

7

2

Enc

od

er2

Red Vcc=+5vBlue A'Green AGray B'Violet BOrange GNDWhite indexBlack Index

DAC OUTPUT

Mo

torc

on

tro

l1

4b6b8d

10d12b14b16b

1822d26d30d32z

OPAMP 1

Mo

torc

on

tro

l2

4b6b8d

10d12b14b16b

1822d26d30d32z

OPAMP 2

QEI

LEFT

+9v

+9v

DSPIC

QEI

RIGHT

black

Transf ormator 2

13

24

Switch F1

1 2

black

black

motor +

Moto

rcontr

ol1

4b6b8d

10d12b14b16b18

22d26d30d32z

motor +

Moto

rcontr

ol2

4b6b8d

10d12b14b16b18

22d26d30d32z

Transf ormator 1

13

24

motor -

emergency 1

MOTOR AC 2

12

motor -

connector1

13

24

24V

batery 1

12V

connector1

13

24

Switich F2

1 2

24V

black

Charger

PLUG FEMALE

1 2

3

batery 2

12VSWITCH On/OFF

MOTOR AC 1

12

10.2 Schematics of the System

Figure 10.1 (Encoder and Motor Control Pin connections)

Figure 10.2 General circuitry of the System

Appendix

63


Vehicle

64

Appendix

65

Documents

GPS/Optical Encoder Based Navigation Methods for dsPIC ...319617/FULLTEXT01.pdf · Microchips MPLAB® ICD is used for in circuit debugging and programming. 2.1.1 The Universal Asynchronous