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

(1)

GPS/Optical Encoder Based Navigation Methods for dsPIC Microcontroled

Mobile Vehicle

Master’s Thesis in Embedded and Intelligent Systems

Berkan Dinçay

(2)

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

(3)

(4)

(5)

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

(6)

(7)

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.

(8)

(9)

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.

(12)

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.

(13)

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.

(14)

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) 4 1

) 1 (

4 × −

=

+

= ×

BaudRate UxBRG Fcy

UxBRG

BaudRate Fcy

(15)

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

(16)

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.

RPM Speed period xMAX

Interrupt

_ _

2 _ = 60

(17)

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

(18)

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.

(19)

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:

Ω

=

= R k

R

₁ ₂

1 Is input on V

₁

; Ω

=

= R k

R

_f _g

1 . 8 Is input on V

2

;

=

≅ R / R

₁

A

_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 1 2 1

) (

)

( V

R V R R R R

R R

Vout R

^f

g

f

−

+

= +

) (

8 .

1 V

₂

V

₁

Vout = −

(20)

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.

(21)

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 49

^th

and 50

^th

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

V

_ss

and a negative voltage V

_cc

=-V

_ss

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 V

ss

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.

(22)

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.

(23)

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.

(24)

(3.1)

Kinematic equations of the system are derived for calculation of the position (state variables). In equation set (3.1), S

_r

and S , are the distances taken by the right and

_l

left wheels and b is the wheel base. Function f ( x , y , θ , ∆ s

_r

, ∆ s

_l

) is used for prediction of next step, where x, y and θ are the state variables of the system (3.2). Assuming that P = [ X Y θ ] ' and ( ∆ s ∆

_r

, s

_l

) are uncorrelated, then the error of propagation law (3.4) can be applied [1].

(3.2)

Error model of the vehicle (3.3), where k

_r

and k are constants that are taken as

_l

3

m 10

⁻

[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 f (x ) in (3.5) and (3.6) [1]:

(3.5) 2

) 2 / sin(

) 2 / cos(

l r

s s s

b s s s y

s x

∆ +

= ∆

∆

−

= ∆

∆

∆ +

∆

=

∆

∆ +

∆

=

∆

θ

θ θ

 





 





∆

−

∆

− + ∆

∆

− + ∆

∆ +

 







 







=

∆

=

b s s

b s s s

y x s s y x f p

l r

l

r

)

sin( 2

2 ) cos(

) , , , , (

' θ

θ

θ θ

 

 





∆

= ∆

∆

∑

∆

l l r r r l

s k s s k

s 0

) 0 cov(

∑

∑ = ∇ ⋅ ∇ + ∇

∆ ∆

∇

∆_rl ^T

p rl

T

p p

f

p

f f f

'

 







 







∆ +

∆

∆ +

∆

−

 =



 





∂

= ∂

∇

=

1 0

0 ) 2 / cos(

1 0

) 2 / sin(

0 1

θ θ

θ ^D

f D

y

f

x

f f

F

_p _p

(25)

(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













−

∆

∆ +

−

∆ +

∆

∆ + +

∆ +

∆

∆ + +

∆ +

∆

∆ +

−

∆ +

∆ =

b b

b D b

D b

D

F_rl

1 1

) 2 / 2 cos(

) 2 / 2sin(

) 1 2 / 2 cos(

) 2 / 2sin(

1

) 2 / 2 sin(

) 2 / 2cos(

) 1 2 / 2 sin(

) 2 / 2cos(

1

θ θ θ

θ θ

θ

θ θ θ

θ θ

θ

(26)

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

(27)

-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

x

⁰

, y

⁰

, 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.

(28)

End (Final position) x_{0+ C}

Direction of Travel

Start (Initial position) x0, y0,

-2000 -1500 -1000 -500 0 500

0 500 1000 1500 2000

X: -44.54 Y: -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.

(29)

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.

(30)

(31)

6.3101 6.3101 6.3101 6.3101 6.3101 6.3102 6.3102 6.3102 x 10⁶ 8.0295

8.0296 8.0297 8.0298 8.0299 8.03 8.0301 8.0302x 10⁵

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

(32)

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

(33)

8.0277 8.0277 8.0278 8.0279 8.0279 8.028 8.028 x 10⁵ 6.3094

6.3094 6.3094 6.3094 6.3094

x 10⁶

LONGITUDE [m]

LATTITUDE [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

(34)

0 1 2 3 4 5 6 7 8 9 10 x 10⁴ -4

-3 -2 -1 0 1 2 3 4x 10⁴

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

(35)

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,



 

 Ω

= Ω

−

= Ω

+

∏

N even ce

N odd sk ce

s s s

G

N k j

N jk

k k

n c

. . ) ) (

(

/ ) 1 2 (

/ π π

) 1 (

) 1 ( 2

1 1

−

+

= − z T s z

3 2

1

3 2

1

29 . 0 2217 . 1 7983 . 1 1

0167 . 0 05 . 0 05 . 0 0167 . ) 0

(

₋ ₋ ₋

−

− +

−

+ +

= +

z z

z

z z

G

(36)

3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 x 10⁴ -2.5

-2 -1.5 -1 -0.5 0 0.5 1

x 10⁴

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].

(37)

cx

2

bx a y = + +

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 ( x

₁

, y

₁

) , ( x

₂

, y

₂

) , Z, ( x

_n

, y

_n

) , where n ≥ 3 , x is the independent variable and y is the dependent variable. Coefficients a, b and c are the unknown coefficients while all x and

_i

y are given. The fitting curve

_i

f (x ) has the deviation (error) d from each data point, d

₁

= y

₁

− f ( x

₁

) , d

₂

= y

₂

− f ( x

₂

) _,...,

) (

_n

n

y f x

d = − . 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.

min ) (

....

²

1 1

2 2

2 2 2

1

+ + + = = − + + =

=

∏ ∑ ∑

=

= i i

n

i i n

i i

n

d y a bx cx

d d

d

[ ]

[ ⁽ ⁾ ] ⁰

2 0 ) (

2

1 2 2 1

2 1

2

= +

+

−

∂ =

∏

∂

= + +

−

∂ =

∏

∂

= +

+

−

∂ =

∏

∂

∑

=

n

i

i i i i

n

i

i i i

i n

i

i i i

cx bx a y c x

cx bx a y b x

cx bx a a y

∑

=

+ +

=

+ +

=

+ +

=

n

i i n

i i i

n

i i n

i i i

n

i i n

i n

i i

x c x b x a x y

x c x b x a y x

x c x b a y

1 4

1 3

1 2

1 3

1 2

1 1

1 2

1 1 1

1

(38)

1 2 3 4 5 6 7 8 9 10 x 10⁴ -4

-3 -2 -1 0 1 2 3

x 10⁴

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.

(39)

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.

(40)

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.

(41)

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 ( x

_p

, y

_p

) 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 ( x

_ce

, y

_ce

) 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 ( x

_pi

, y

_pi

) . By the equation (5.1) encoder data is moved to the point ( x

_p

, y

_p

) . 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 ) , ( ) , ( ) , (

= +

+

= b a

y x b y x a y

x

_p _p _pi _pi _ce _ce

(42)

-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 20

^th

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].

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