Intelligent robotic v2

Engineering of Intelligent Robotic (Doctor Course)

Abdul Halim Bin Ismail

D1, System & Control Laboratory

Toyohashi University of Technology

Chapter 4: Robot Navigation

CONTENTS

4.0 INTRODUCTION

4.1 COORDINATE SYSTEMS

4.2 EARTH-CENTERED EATRH-FIXED COORDINATES SYSTEMS

4.3 ASSOCIATED COORDINATE SYSTEMS

4.4 UNIVERSAL TRANSVERSE MERCATOR (UTM) COORDINATE

SYSTEM

4.5 GLOBAL POSITIONING SYSTEM (GPS)

4.6 COMPUTING RECEIVER LOCATION USING GPS, NUMERICAL

METHOD

4.6.1 Computing Receiver Location Using GPS via Newton’s

Method

4.6.2 Computing Receiver Location Using GPS via

Minimization

of a Performance Index

4.7 ARRAY OF GPS ANTENNAS

4.8 GIMBALED INERTIAL NAVIGATION SYSTEMS

4.9 STRAP-DOWN INERTIAL NAVIGATION SYSTEMS

4.10 DEAD RECKONING OR DEDUCED RECKONING

4.11 INCLINOMETER/COMPASS

INTRODUCTION

This chapter introduces the topic of navigation system and the various means of accomplishing this.

The focus is on the Global Positioning System (GPS) and the inertial navigation system (gimbaled and strap-down)

Also, briefly discussed is deduced reckoning utilizing less sophisticated methodology.

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization







COORDINATE SYSTEM

Navigation is the process of accurately determining position and velocity relative to a known reference.

Navigation also the process of planning and executing the maneuvers necessary to move between desired locations.

Important factor in navigation is the understanding of the different coordinate systems.

In this sub-chapter, 6 coordinates system is discussed, which are: Coordinate System I, Associated systems such as Coordinate System II, Coordinate System III, Coordinate System IV, Coordinate System V, and lastly the Universal Transverse Mercator (UTM) coordinate system.

Figure 4.1: Earth and Several Different Coordinate Frames

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization







EARTH-CENTERED EARTH-FIXED COORDINATE SYSTEM

In coordinate system I,

• z axis points to North Pole

• x axis points through equator at prime meridian

• y axis completes the right-handed coordinate system

This set of axes is called Earth-Centered Earth-Fixed axes (ECEF).

ECEF has its origin at the center of Earth and rotates with Earth.

ECEF is sometimes known as a conventional terrestrial system. It represents positions as an X, Y, and Z coordinate. The point (0,0,0) is defined as the center of mass of the Earth [1].

[1] Alfred Leick, 2004, GPS Satellite Surveying, Wiley

There is a unique relation between the ECEF coordinates of a point on the surface of the earth and its longitude,

Measured positively Eastward from the prime meridian running through Greenwich, England.

Relation between the ECEF coordinates of a point on the surface of the earth and its latitude,

Measured positively Northward from the equator.

The earth however, is not perfect sphere. In this book,• R = 6,357.7 km Earth radius at poles• R = 6,378.1 km Earth radius at the equator

cos( )cos( )

cos( )sin( )

Z sin( )

X R lat long

Y R lat long

R lat

Eq. 4.1 (a-c)

Eq 4.1 (a-c) can be reversed if the ECEF coordinates were known and the latitude and longitude have to be determined.

ECEF in thin book is known as Coordinate System I

1

2 2

1

tan

tan

Zlat

X Y

Ylong

X

Eq. 4.2 (a,b)

http://www.colorado.edu/geography/gcraft/notes/coordsys/coordsys.html

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization







ASSOCIATED COORDINATE SYSTEM Other coordinates are useful in describing motion on the surface

of the earth. In this book, lat and long is expressed in radians, while

Lat and Long is in degrees.

Relationship between variables in coordinate system II and I are;

Coordinate frame II has been rotated counter-clockwise about the ZI axis by an amount of long.

XII axis is now pointing through the equator at longitude long.

cos sin 0

sin cos 0

0 0 1II I

X long long X

Y long long Y

Z Z

Eq. 4.3

Relationship between variables in coordinate system III and II are;

Coordinate frame III has been rotated clockwise about the YII

axis by an amount of lat. The XIII axis now points through the meridian of longitude long

and the parallel of latitude lat.

This rotation matrix is given by

cos 0 sin

0 1 0

sin 0 cosIII II

X lat lat X

Y Y

Z lat lat Z

Eq. 4.4

1 ( ) or ( ) or ( )T

roll roll rollR lat R lat R lat

Relationship between variables in coordinate system IV and III are;

For Coordinate frame IV, the origin is now has been moved from center of the earth to the surface of the earth.

The YIV axis is parallel to the ZIII axis The ZIV axis is parallel to the XIII axis The XIV axis is parallel to the YIII axis

One can think of the orientation of frame IV as one obtained by rotating frame III about its z axis by 90˚ counter clock-wise.

0 1 0 0

0 0 1 0

1 1 0IV III

X X

Y Y

Z Z R

Eq. 4.5

The rotation matrix is given by,

Where,

and

This coordinate frame attached to the surface of the earth with the y axis pointing North, the XIV axis pointing East, and the ZIV

axis pointing outward from the earth’s surface is a useful local coordinates system.

One can describe x-y locations with respect to this frame in terms of longitude and latitude of the origin of the coordinate system

1

( / 2)R ( / 2) or ( / 2)R ( / 2)T

yaw pitch yaw pitchR R

0 1 0

( 2) 1 0 0

0 0 1

yawR

1 0 0

( 2) 0 0 1

0 1 0

pitchR

By assuming a spherical earth, starting with an initial point,

Defining latitude and longitude of the origin of the final frame to be long0 and lat0, then

cos cos

sin cos

sinI

X R long lat

Y R long lat

Z R lat

0 0

0 0

0 0 0

0 0 0

cos sin cos sin cos cos

cos cos cos sin

sin sin cos sin sin cos

cos cos cos sin sin

IV

IV

IV

X R long long lat R long long lat

Y R long long lat lat

R long long lat lat R lat lat

Z R long long lat lat R lat lat R

Which reduce to,

For points on the surface of the earth in the vicinity of the origin of the final frame, 4.6(a-c) may be approximately quite accurately as,

0

0 0 0

0 0

0

cos sin

cos sin sin cos

cos cos sin sin

cos

IV

IV

IV

X R lat long long

Y R lat lat long long R lat lat

Z R lat lat R lat lat R

R lat lat R

Eq. 4.6 (a-c)

0

0

cos( )

0

IV

IV

IV

X R lat long long

Y R lat lat

Z

Eq. 4.7 (a-c)

Example 3:A local coordinate system is set up at Long=70deg W=-70deg and Lat=38deg N. A mobile robot is at Long=69.998deg W=-69.998deg

and Lat=38.001deg N. Find the X,Y coordinates for the robot. Take X-East and Y-North.

Solution:

0

0 0

cos( )

( )cos( )( /180)

6,378,137(70 69.998)cos( 38.001)( /180)

6,378,137(.002)(.788)( /180) 175.4

IV

local

X R lat long long

X R Long Long Lat

m

0

0 ( /180)

6,378,137(38 38.001)( /180) 111.3

IV

local

Y R lat lat

Y R Lat Lat

m

Figure 4.2 shows a final local coordinate system rotated such that the x axis of frame V is at an angle αwith respect to the x axis of frame IV.

The appropriate rotation matrix is given by,

This transformation matrix is given by,

cos sin 0

sin cos 0

0 0 1V IV

X X

Y Y

Z Z

Eq. 4.8

1( ) or ( )T

yaw yawR R

For all of these transformation matrix previously, the inverse (or transpose) is required because the coordinates is being converted from their expression in the old frame to their expression in new frame.

Applying eq 4.8 into eq. 4.7 yields,

0 0 0

0 0 0

cos cos sin

sin cos cos

V

V

X R lat long long lat lat

Y R lat long long lat lat

Eq. 4.9 (a-b)

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization







UNIVERSAL TRANSVERSE MERCATOR (UTM) COORDINATE SYSTEM

Previously in coordinate system I-IV, all the discussion is based on spherical shape of the earth.

UTM works on different basis.

It was more commonly used throughout the navigation world, such as aviation, maritime and during SAR (Search and Rescue Mission)

Mercator projection results from projecting the sphere onto a cylinder tangent to the equator.

Transverse Mercator projections results from projecting the sphere onto a cylinder tangent to the central meridian.

Common Mercator Projection Transverse Mercator Projection

Images taken from http://en.wikipedia.org/wiki/Transverse_Mercator_projection

For common Mercator:• Regions near the poles are greatly distorted appearing larger

than they are.• Regions near the equator are most accurate.• The main purpose is to convert the spherical shape of the

earth to a flat surface.

For Transverse Mercator:• Regions near the central meridian are most accurate.• Distortion of scale, distance, direction and area increases as

one moves away from the central meridian.

Transverse Mercator maps are often used to portray areas with larger north-south than east-west extent.

In the UTM coordinate system, longitudinal zones are only six degrees of longitude wide, extending three degrees to either side of central meridian.

These 6 degree longitudinal zones extent from 80deg South latitude to 84deg North latitude.

There are sixty of these longitudinal zones covering the entire earth, labeled with the numbers from 1-60.

Each longitudinal zone is further divided into zones of latitude, beginning with zone C at 80deg South up to M just below the equator.

To the North, the zones run from N just above the equator to X at 84deg north.

All the zones span eight degrees in the north-south direction except zone X, which spans 12 degrees

Reference: http://www.dmap.co.uk/utmworld.htm

Formulas relating latitude/longitude to UTM:

Firstly, compute the longitudinal zone number, I

The Central Meridian for longitudinal zone,

Using the earth spherical approximation and ignoring the projections distortion, northen and eastern is roughly,

180int 1

6

Longi

Eq. 4.10

0 177 1 6Long i Eq. 4.11

0

180

180 cos 500,000

Northing R Lat

Easting R Long Long Lat

Eq. 4.12 (a-b)

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization







GLOBAL POSITIONING SYSTEM (GPS)

The space age began on October 4, 1957 with the launch of the first artificial satellite, Sputnik 1.

As of Oct’13, there are 1071 operational satellites in orbit around the Earth, which 50% of them launched by the USA [1] .

GPS provides a means for a receiver/user to determine its location anywhere on the earth surface.

Also referred as geolocation.

GPS systems includes a constellation of satellites.

[1] http://www.universetoday.com/42198/how-many-satellites-in-space/

Satellite’s orbital radii are approximately 20,200km. They are spaced in six orbits with four satellites per orbit.

The orbits have inclination angles of 55˚ with respect to the equator, and their orbital period is 12 hours.

Each satellite is equipped with an atomic clock and a radio transmitter & receiver.

The status and operational capability of the satellites is monitored on ground stations.

These entire operation depends on the use of encoded radio signals.

The Standard Positioning Service (SPS):• Utilizes 1.023MHz repeating pseudo random code• Called Coarse Acquisition (C/A) code• Available for public use• Resolution of 30m or better

The Precise Positioning Service (PPS):• Utilizes 10.23MHz repeating pseudo random code• Called Precise Acquisition (P) code• Can be encrypted to make available for Department of

Defense only. [1]

• Resolution of 3m or better.

[1] Further reading about PPS P Code Encryption: Cox Jr, Thomas M. PPS GPS: What Is It? And How Do I Get It. Vol. 225. ARMY TOPOGRAPHIC ENGINEERING CENTER ALEXANDRIA VA, 1994.

Geolocation (Positioning) is based on the use of modulated signal transmitted from the satellite and received by the on-ground user.

Based on signal travel time, distance could be determined.>> distance α time

Distance calculation from the user to the satellites, combined with the known satellite position at the signal transmission time, allows triangulation computation, and therefore determines the user location.

Potential resolution of distance calculation from the satellites to the user can be compute by computing the time duration of one bit in the pseudo random codes multiply with speed of light.

C/A codes resolution of 30m and better, P codes of 3m and better.

If the GPS in surveying mode (receiver remain stationary for hours), distance resolution able to be in centimeter range.

Errors in GPS geolocations:1. Error due to receiver local clock

• GPS (satellite & ground station) uses atomic clock, which was measured starting at 24:00:00 January 5, 1980.

• Receiver/User clock however does not as accurate as atomic clock, these local clock normally exhibit bias.

2. Error due to atmospheric effects• Ionospheric delays caused by the layer of the atmosphere containing

ionized air.• Tropospheric delays caused by changes in temp, pressure, and humidity.

3. Error due to ephemeris data• Decomposed into tangential, radial, and cross track components.• Radial ephemeris error has greatest impact to geolocation

4. Error due to multipath transmission• Reflected signals near the receiver maybe interferences or mistook as

original signal.

* ephemeris data gives the positions of naturally occurring astronomical objects as well as artificial satellites in the sky at a given time or times, either in printed tables, or modern computer computation.

Knowing the time the signal was transmitted from the time-tagged data, and having determined the time of arrival with respect to receiver clock (local clock), the travel time for each received signal may be computed and converted to the distance from the receiver to the respective satellites.

With signal from two satellites, the receiver is placed on a sphere about each of two points with their intersection being a circle.

Using a measurement from a third satellite, the receiver is now placed on a sphere about this third point.

The intersection of third point to the previous circle yields two point, where only one of those is near/on the earth surface.

Therefore, in principle, three satellites is enough to triangulate user location, if there are no local clock error.

In practice, this error does exists, and to compensate the error, signal must be received from a fourth satellite.

This extra equation allows one to determine the three dimensional position as well as the local clock error.

If more than four satellites are visible, the redundancy can be used to reduce other types of errors.

Geolocation triangulation phase[1]

[1] http://giscommons.org/chapter-2-input/

Geometric Dilution of Precision (GDOP) is computed from the geometric relationship between the receiver position and the positions of the satellites the receiver is using for navigation.

If there are no good spread among the visible satellites, GDOP will be high.

Imagine two satellites are close to each other. Thus, the distance of each of these satellites to the receiver yields a sphere.

Considering the similarity of these two satellites, then their intersection will be very sensitive to any kind of error.

GDOP components includes: • PDOP – Position Dilution of Precision • HDOP – Horizontal Dilution of Precision • VDOP – Vertical Dilution of Precision • TDOP – Time Dilution of Precision

Differential GPS (DGPS) improved precision in computation of the receiver’s location.

DGPS employs an additional known receiver, i.e. a base station. A base station is fixed, while the receiver is free to roam.

The difference between these two receivers is evaluated, which the errors and information is later transmitted to the free roaming receiver.

Thus, GPS geolocation can be substantially improved by cancelling the common-mode errors.

However, the effectiveness of DGPS degrades when the rovers are separated from the base station by as much as ten of miles.

The base station should broadcast the following set of information: Satellite Identification Number, Range Correction, Ephemeris Set Identifier, and Reference Time.

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization







The times of arrival of the signals from the satellites can be extracted when the correlations of the signal received from visible satellites with the shifted signals generated within the receiver has been performed.

Then the travel times are determined and the pseudo distances from the receiver are computed.

Once this has been accomplished, we could proceed to an iterative process to determine the receiver location.

In this subchapter, two type of location computing will be discussed, which are

• Computing Receiver Location Using GPS via Newton’s Method• Computing Receiver Location Using GPS via Minimization of a Performance

Index

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Newton’s Method


Minimization







Image taken from: http://www.geneko.rs/en/gps-technology

This is a system of nonlinear equations that are based on measurement of

distance from four or more different satellites to the receiver.

di – computed from travel time x speed of light

(xi,yi,zi) – ECEF coordinates of the ith satellite

(x,y,z) – assumed ECEF coordinates of the receiver

tb – receiver clock bias

c – speed of light

Unknown (x,y,z) and tb

0.52 2

1 1 1

0.52 2

2 2 2

0.52 2

3 3 3

0.52 2

4 4 4

b

b

b

b

x x y y d ct

x x y y d ct

x x y y d ct

x x y y d ct

Eq. 4.13 (a-d)

Equation 4.13(a-d) may be rearranged to express the error between the left

hand-side distance (ranges from the assumed receiver location to the

respective satellites) and the right hand-side distance (corrected pseudo

ranges which was determined from the signal time of travel).

Since this is a nonlinear equations, the solution is not straightforward and

requires iterative process.

Firstly, make initial guess (zero is reasonable), then iteratively compute

until a stopping criterion is found.

0.52 2

1 1 1 1

0.52 2

2 2 2 2

0.52 2

3 3 3 3

0.52 2

4 4 4 4

b

b

b

b

E x x y y d ct

E x x y y d ct

E x x y y d ct

E x x y y d ct

Eq. 4.14 (a-d)

Newton’s method is used to force the error vector to zero. Rewritting

the errors equation more concisely,

Where the receiver location and the the ith satellite location is,

With N is the number of visible satellites. Next if we now define,

Then equation 4.15 became,

0.5

( ) , 1,2T

i i i i

bE X X X X d ct i N

Eq. 4.15

x

X y

z

i

i i

i

x

X y

z

0.5

2 2 2i i i ir x x y y z z

( ) , 1,2i i i

bE r d ct i N Eq. 4.16

The error equations is then expended using the Taylor series through

the linear term,

Differentiating equation for the error yields,

And

Eq. 4.17

1 1

2 2

( )

( )

( )

b

b

b

N Nbb

xr d ct

yr d ctE E E X E ct

z

ctr d ct

0.5 1T T Ti

i i i i

iE X X X X X X X X

X r

1i

bE ct

Expended form of equation 4.17,

1 1 1

1 1 11 1

2 2 22 2

2 2 2

1 1 11

( )1 1 1

1( )

( )1 1 1

1

b

b

N Nbb

N N N

N N N

x x y y z zr r r xr d ct

yx x y y z zr d ctE E r r r

z

ctr d ct

x x y y z zr r r

iEX

i

d

Ect

Eq. 4.18

One seeks the appropriate values for ΔE to force this error to zero.

Setting E + ΔE = 0 and solving for the changes in estimated

receiver location and receiver clock bias yields

Or in its expandable form,

1 1

2 21

( )

( )

( )

b

b

b

N Nb b

x r d ct

y r d ctE EX ctz

ct r d ct

Eq. 4.19

1

1 1 1

1 1 11 1

2 2 2 2 2

2 2 2

1 1 11

( )1 1 1

1 ( )

( )1 1 1

1

b

b

N Nb b

N N N

N N N

x x y y z zr r rx r d ct

y x x y y z z r d ctr r r

z

ct r d ct

x x y y z zr r r

Eq. 4.20

In practice, a scale factor less than one is often introduced to aid in

the convergence of the solution.

For cases where the number of visible satellites is greater than 4,

there will be more equations than the unknowns.

To proceed, one may utilize the least-squares solution in which case

the matrix inverse becomes a generalized inverse.

Eq. 4.21

1

1 1

2 2

( )

( )

( )

T

b b

b

b

T

b

d

N N

b

x

yE E E E

X ct X ctz

ct

r d ct

r d ctE EX ct

r d ct

Fortunately, these equations are all consistent, meaning that there

exists an exact solution even though the number of equations exceeds

the umber of the unknown.

After solving equation 4.21, one could updates the receiver location

and local clock bias according to

This correction process is repeated until it reaches steady state, i.e.

the correction approaches zero.

At this time, the distance computed from the equations involving

receiver location should agree with the pseudo distances which has

been previously measured by signal travel time with local clock bias

corrections.

Eq. 4.22

b b b

x x x

y y y

z z z

ct ct ct

Hand calculated solution is very tough for iterative processes.

Therefore, a programming is built using Matlab for this particular problem.

Result is then plotted in a graph

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

-4

-2

0

2

4

6x 10

6 Convergence of the Coordinate at Mobile Robot Location, K=1

Number of Iteration

Successsiv

e C

oord

inate

Com

puta

tion Receiver location:

x = -2,430,745

y = -4,702,345

z = 3,546,569

tb = -3.3342156

0 2 4 6 8 10 12 14 16 18 20-5

-4

-3

-2

-1

0

1

2

3

4x 10

6 Convergence of the Coordinate at Mobile Robot Location, K=0.5

Number of Iteration

Successsiv

e C

oord

inate

Com

puta

tion

Receiver location:

x = -2,430,742

y = -4,702,339

z = 3,546,564

tb = -3.3342092

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method


Minimization of a Performance Index






An alternative approach to determine the receiver’s coordinate is to

formulate a positive definite performance index based on the sum

of squares of the distance errors describe in equation 4.16.

The performance measure used to represent total error in geo-

location will be taken as,

Utilizing Xi and X,

or

Eq. 4.23 2

0.52 2 2

1

Ni i i i

b

i

L x x y y z z d ct

2

0.5

1

NT

i i i

b

i

L X X X X d ct

Eq. 4.24 2

1

Ni i

b

i

L r d ct

The performance index, L will be used to define the iterative

procedure.

The goal is to determine the coordinate of the receiver which

cause L to be minimized.

Gradient of L must be determine in order to know in what

direction to perturb the coordinates of the receiver.

Using the definition of ri previously,

Eq. 4.25

0.5

1

0.5

2{[( ) ( )] ( )

1[( ) ( )] 2 )

2

Ni T i i

b

i

i T i i T

L X X X X d ctX

X X X X X X

1

2 { ( )}( )N

i i i T i

b

i

L r d ct X X rX

The equation 4.25 with dL/dX points in the direction of

increasing L.

As we want to minimize it, it should be then –dL/dX

We also need to adjust the local clock bias, which is given by,

Or in minimizing directions,

Eq. 4.261

2{ ( )}( 1)N

i i

bb i

L r d ctct

1

2 { ( )}( )N

i i i T i

b

i

L r d ct X X rX

1

2{ ( )}N

i i

bb i

L r d ctct

A means of determining not only the direction but also the step

size for the iterative process is utilizing a more complex Taylor

series including the second derivative of the function to be

minimized, i.e.,

Then minimizing f(w+Δw) with respect to Δw yields

This may be interpreted as using the Newton method to

determine the point where the first derivative is zero.

Eq. 4.27

1

{( )} ( )T Tw f w f ww

1( ) ( ) [ ( ) ]

2

T Tf w w f w f w w w f w w w

Performing these operations on the performance index of interest,

one obtains as the matrix of second derivatives,

The change in X and ctb then becomes,

Eq. 4.28

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method








An array of four GPS antennas is shown in Fig 4.5 as above may

be used to compute vehicle attitude.

These are attached to a frame in the shape of a cross with antennas

labeled 1 (front), 2 (left side), 3 (rear), and 4 (right side).

The x axis runs across to the right side of the frame.

The y axis runs from rear to front of the frame.

The z axis completes the right-handed set.

Figure 4.5 Convergence of Coordinates as a Function of Iteration Number Array of Four GPS Antennas

BEFORE applying any rotations to the vehicle i.e., zero pitch,

yaw and roll, the coordinates are,

The coordinates AFTER applying rotation of the vehicle through a

yaw angle of ψ, a pitch angle of θ, and a roll angle of ϕ in

respective order,

1 1 1

2 2 2

3 3 3

4 4 4

, / 2 y ,

/ 2 , y ,

, / 2 y ,

/ 2 , y ,

veh veh veh

veh veh veh

veh veh veh

veh veh veh

x x y L z z

x W x y z z

x x y L z z

x W x y z z

Eq. 4.29(a-d)

1

1

1

2

2

2

3

3

3

/ 2(sin cos )

/ 2(cos cos ) y

/ 2(sin )

/ 2(cos cos sin sin sin )

/ 2(sin cos cos sin sin ) y

/ 2(cos sin )

/ 2(sin cos )

/ 2(cos cos ) y

/

veh

veh

veh

veh

veh

veh

veh

veh

x L x

y L

z L z

x W x

y W

z W z

x L x

y L

z L

4

4

4

2(sin )

/ 2(cos cos sin sin sin )

/ 2(sin cos cos sin sin ) y

/ 2(cos sin )

veh

veh

veh

veh

z

x W x

y W

z W z

Eq. 4.30(a-c)

Eq. 4.31(a-c)

Eq. 4.32(a-c)

Eq. 4.33(a-c)

By manipulating these equations, it is possible to isolate the

attitude angles in terms of the measured variables.

• Pitch

• Yaw

• Roll

Eq. 4.34

Eq. 4.35

Eq. 4.36

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method








Figure 4.6 Schematic Diagram of a Gimbaled Platform

A gimbaled platform is shown in Figure 4.6.

It has an actuated platform that which are mounted three

orthogonal gyros.

The gyros on the platform in conjunction with gimbal motors

maintain the platform at a fixed attitude in an inertial frame,

while the mounting frame may rotates.

Thus, the platform is called a stable platform.

Gimbaled INS image from http://inertialnavigations.blogspot.com/

Equation that governs the behavior of rotating body,

L is the angular momentum of the gyro and its points along the

spin vector.

Vector Ω is the angular precession velocity of this momentum

vector.

Vector τ is the associated torque.

Vehicle attitude changes (thus resulting in Ω and L) which may

occur as a result of slight friction in the imperfect gimbal bearings.

The torque τ is sensed at the gyro bearings support.

Through feedback control, the gimbal motors react to negate this

torque thus maintaining a stable platform.

From the Figure 4.6, it can be seen that there is a gimbal and a

gimbal motor each for yaw (or azimuth), pitch and roll

L Eq. 4.24

In addition to the gyros, three accelerometers are also mounted on

the stable platform.

Integrating the accelerometers signal twice with respect over

time, a changes in position in all three coordinates could be

obtained.

Combining these information with the original position, current

position in the inertial space could be obtained.

Common Errors;

• Offset/Bias error – the process of double integration that

grows as the square of time

• Attitude errors caused by drift – the system thinks it is

rotating when it is not.

Obtaining vehicle attitude and position from discrete-time outputs

of the gimbaled gyroscope;

• Yaw:

• Pitch:

• Roll:

In other words, the measurements of the gimbaled angles are the

same as the attitude measurements of the vehicle with respect to the

stable platform.

1 1( ) ( )k measured kt t



The equations for positions;

• For x,

• For y,

• For z,

1 1

2

11 1 1

( ) ( ) a ( ) ( )

( )( ) ( ) ( )( ) a ( )

2

k x k measured k k

k kk k k k k x k measured

x t x t t t t

t tx t x t x t t t t

Eq. 4.38(a,b)

1 1

2

11 1 1

( ) ( ) a ( ) ( )

( )( ) ( ) ( )( ) a ( )

2

k y k measured k k

k kk k k k k y k measured

y t y t t t t

t ty t y t y t t t t

Eq. 4.38(c,d)

1 1

2

11 1 1

( ) ( ) a ( ) ( )

( )( ) ( ) ( )( ) a ( )

2

k z k measured k k

k kk k k k k z k measured

z t z t t t t

t tz t z t z t t t t

Eq. 4.38(e,f)

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method








Another type of INS is known as Strap-down systems.

The spin axis for the gyros are rigidly attached to the vehicle’s

body.

The gyros then change attitude as the vehicle itself changes its

attitude.

The equation τ=ΩxL can be broken into;

0

0

0

x z y x

y z x y

z y x z

L L

L L

L L

Eq. 4.39

For gyro aligned with x axis of the platform,

For gyro aligned with y axis of the platform,

For gyro aligned with z axis of the platform,

0

0

xy y

xz zgyro x

L

L

Eq. 4.40(a)

0

0

yx x

yz zgyro y

L

L

Eq. 4.40(b)

0

0

x xz

y yzgyro z

L

L

Eq. 4.40(c)

Combining,

The solutions of Ωx (pitch), Ωy (roll) and Ωz (yaw) obtained via the

least-squares method as ,

0 0

0 0

0 0

0 0

0 0

0 0

y gyro x x

z gyro x x

x

x gyro y y

y

z gyro y y

z

x gyro z z

y gyro z z

L

L

L

L

L

L

Eq. 4.41

Eq. 4.42(a-c)

, y,2 2

, y,2 2

y, x,2 2

1

1

1

x y z gyro y z gyro z

y z

y x z gyro x z gyro z

x z

z x gyro x y gyro y

x y

L LL L

L LL L

L LL L

Although equation 4.42(a-c) is a least square solution, the equation

are consistent and there is no error in the solution for the body rates.

These rates related to the attitude rates according to the following

equations,

Which can be view in matrix form as,

And inverted to obtain angular rates for yaw, pitch, and roll,

Eq. 4.43(a-c)

Eq. 4.44

cos sin cos

sin

cos cos sin

x

y

z

cos 0 sin cos

0 1 sin

sin 0 cos cos

x

y

z

Eq. 4.45

cos 0 sin

sin tan 1 cos tan

sin cos 0 cos cos

x

y

z

Once these angular rates for attitude have been determined, the

attitude angles themselves can be updated via numerical integration.

Using the Euler integration method, i.e., derivative approximated

by forward difference, yields

Another approach to the problem of obtaining vehicle orientation

from body angular rates involves the use of quarternions, which

vector is defines as,

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

t t t t t

t t t t t

t t t t t

Eq. 4.46

0

1

2

3

q

qq

q

q

Components of the quaternions;

Quaternions vector obeys the differential equations,

0

1

2

3

cos( / 2)cos( / 2)cos( / 2) sin( / 2)sin( / 2)sin( / 2)

cos( / 2)cos( / 2)sin( / 2) sin( / 2)sin( / 2)cos( / 2)

cos( / 2)sin( / 2)cos( / 2) sin( / 2)cos( / 2)sin( / 2)

sin( / 2)cos( / 2)cos( / 2) cos( / 2)sin( / 2)

q

q

q

q

sin( / 2)

Eq. 4.47(a-d)

( )

where

0

01( )

02

0

z y x

z x y

y x z

x y z

q A t q

A t

Eq. 4.48

Eq. 4.49

After integrating the quaternions the current values for yaw, pitch

and roll angles can be determined from the entries of q via

Regarding position calculation, the platform is rigidly attached to

the body of the vehicle means that the accelerator measure

acceleration in vehicle coordinates.

It must be converted into inertial coordinates

Eq. 4.50(a-c)

1 1 2 0 3

2 2

2 3

1 3 0 2

1 2 3 0 1

2 2

1 2

2 2tan

1 2 2

sin 2 2

2 2tan

1 2 2

q q q q

q q

q q q q

q q q q

q q

( ) ( ) ( )

x x

y y

z zinertial coords vehicle coords

a a

a Rot Rot Rot a

a a

Eq. 4.51

Gimbaled VS Strap-Down INS

Other type of gyroscopes (page 136);

• Ring laser gyro

• MEMS (micro-electromechanical-systems)

Criteria Gimbal Strap-down

Equations Fairly simple More complex

Cost High Lower

Mechanical Complex Simpler

Software Operation

Simpler Complex

Some of navigation errors that result from inertial systems:

1) Instrumentation Errors: The sensed values may not be

the same as physical values. This due to imperfection of

sensors (e.g., bias, scale factor, non-linearity, random

noise, etc)

2) Computational Errors: The navigation equation are

basically came from computer iteration. Imperfect

solutions of differential equation as a result of

approximations may lead to this kind of error.

3) Alignment Errors: Errors caused by the fact that the

sensors and their platform may not be aligned perfectly

with their assumed directions.

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method








Dead reckoning uses shaft encoders (or similar devices) to

measure angular rotation of the wheels.

The simple formula in the following is then used to convert

this measurement to distance traveled,

In equation 4.52, r is the wheel’s radius.

A complete rotation of θ yields a distance traveled by the

wheel’s diameter.

But it does not contains the information about curvature of the

path travelled.

Therefore, two encoders are normally used to record

directions.

Encoders are put on both sides of mobile robot wheels.

Eq. 4.52S r

Using the profile of each encoder readings, the vehicle motion in

terms of direction and distance traveled can be tracked, and its

new position can be computed given its initial location.

The following equations gives incremental changes in x position,

y position, and heading.

W is lateral distance between the wheels, r is wheel radius, and

Δθ’s are the incremental encoder readings expressed in radians.

Eq. 4.53 (a-c)

sin2

cos2

R L

R l

R l

r

W

rx

ry

Expressing equation 5.53 as difference equations,

It apparent that a little wheel slippage can cause large error

buildups.

For example, a slight error in heading can cause a large error in

calculated location if the distance traveled is great.

Thus, dead reckoning can only be used in short distances and

needs frequent re-calibration.

Eq. 4.54 (a-c)

( 1) ( ) [ ( 1) ] [ ( 1) ]

( 1) ( ) [ ( 1) ] [ ( 1) ] sin ( 1)2

( 1) ( ) [ ( 1) ] [ ( 1) ] cos ( 1)2

r r l l

r r l l

r r l l

rk k k k

W

rx k x k k k k

ry k y k k k k

CONTENTS

4.0 INTRODUCTION





SYSTEM



METHOD


Method








The Inclinometer-Compass measures the rotation of the longitudinal

axis about the original z axis (yaw) via a digital compass.

It measures the angle of the longitudinal axis with respect to the

original xy plane (pitch) via gravity vector.

It measures the rotational of the body about its longitudinal axis

(roll), also via gravity vector.

For the determination of pitch and roll from the sensed gravitational

force, one may use general rotational matrix,

1

21

2

cos cos sin sin sin sin cos cos sin sin sin cos

cos cos sin sin sin cos cos sin sin cos sin cos

cos sin sin cos cos

x

y

z

x

y

z

g

g

g

g

g

g

The rotation matrix converts the forces measured in their own axis to

the original axis (z is the vertical axis).

In the original frame, the gravitational force is zero except z axis.

Now, the gravitational force components are independent of yaw

(ψ), and the equation can be simplifies into

Thus, roll

And pitch

1 21 2

0 cos 0 sin

0 sin sin cos sin cos

1 cos sin sin cos cos

x

y

z

g

g

g

1 1

0

0

x

y

z

g

g

g g

Eq. 4.55

1tan x zg g Eq. 4.56(a)

1tan cosy zg g Eq. 4.56(b)

The magnetic compass is used to determine yaw through the

detection of the magnetic field along each axis.

Consider: (1) vehicle pointing North with zero pitch and roll, thus

magnetic field detected will exclusive along y axis.

Consider: (2) the vehicle yawed but still with zero pitch and roll, thus

magnetic field will be along both x and y axis.

cos sin 0

( ) sin cos 0

0 0 1

yawR

2 2

2 2

2 2

0 cos sin

sin cos

sin cos tan

x y

y x

x y

m m

m m

m m

2

1 2

2

0 cos sin 0

sin cos 0

0 0 0 1

x

y y

z

m

m m

m

This equation enable one to determine yaw

from the component of magnetic field

detected along each axis.

This result was derived from an assumption

of zero pitch and roll.

Next includes pitch and roll, as well as yaw.

The relation between the vehicle with yaw alone and the one with

yaw, pitch and roll would be

or

2

2

2

3

3

3

cos sin 0

sin cos 0

0 0 1

cos sin 0 1 0 0 cos 0 sin

sin cos 0 0 cos sin 0 1 0

0 0 1 0 sin cos sin 0 cos

x

y

z

x

y

z

m

m

m

m

m

m

2 3

2 3

2 3

1 0 0 cos 0 sin

0 cos sin 0 1 0

0 sin cos sin 0 cos

x x

y y

z z

m m

m m

m m

Or

With solution

And

And finally one has

2 3

2 3

2 3

cos 0 sin

sin sin cos sin cos

cos sin sin cos cos

x x

y y

z z

m m

m m

m m

2 3 3cos sinx x zm m m

2 3 3 3sin sin cos sin cosy x y zm m m m

2 2

3 3 3 3 3

tan

(cos sin ) (sin sin cos sin cos )

x y

x z x y z

m m

m m m m m

Eq. 4.56(c)

It is important to realize that inclinometer responses to acceleration.

If the vehicle is stationary or moving in a straight line at constant rate,

the only acceleration involves is gravity, and the instrument provides a

correct indication of pitch and roll.

However, for any other case the indicated pitch and roll will be

erroneous. This dynamic situations need other means of computation.

Inclinometer application example:

• The robot comes to a stop,

• And then performs some actions (such as acquiring radar image/or

infrared image)

• The attitude measurement from the Inclinometer/Compass could

be then used to convert the image from robot coordinates to earth

coordinates.