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.

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

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
Z
lat
X Y
Y
long
X


 
  
 
 
  
 
Eq. 4.2 (a,b)

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

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)

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,
180
int 1
6
Long
i
 
  
 
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)

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.

 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
of a Performance Index

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 21 1 1
0.52 22 2 2
0.52 23 3 3
0.52 24 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 21 1 1 1
0.52 22 2 2 2
0.52 23 3 3 3
0.52 24 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,2
Ti 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.52 2 2i i i i
r 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 N
bb
xr d ct
yr d ct
E E E X E ct
z
ctr d ct
    
              
   
   
     
       
0.5
1T T Ti i i i i
i
E X X X X X X X X
X r
          
1i
bE ct   

 Expended form of equation 4.17,
     
     
     
1 1 1
1 1 1
1 1
2 2 22 2
2 2 2
1 1 1
1
( )
1 1 1
1( )
( )
1 1 1
1
b
b
N N
bb
N N N
N N N
x x y y z z
r r r xr d ct
yx x y y z zr d ct
E E r r r
z
ctr d ct
x x y y z z
r r r
 
    
     
                
    
            
    
 
iE
X


i
d
E
ct


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 N
b b
x r d ct
y r d ctE E
X ctz
ct r d ct

    
           
       
  
      
Eq. 4.19
     
     
     
1
1 1 1
1 1 1
1 1
2 2 2 2 2
2 2 2
1 1 1
1
( )
1 1 1
1 ( )
( )
1 1 1
1
b
b
N N
b b
N N N
N N N
x x y y z z
r r rx r d ct
y x x y y z z r d ct
r r r
z
ct r d ct
x x y y z z
r 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
y E E E E
X ct X ctz
ct
r d ct
r d ctE E
X 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
6
x 10
6
Convergence of the Coordinate at Mobile Robot Location, K=1
Number of Iteration
SuccesssiveCoordinateComputation
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
4
x 10
6
Convergence of the Coordinate at Mobile Robot Location, K=0.5
Number of Iteration
SuccesssiveCoordinateComputation
Receiver location:
x = -2,430,742
y = -4,702,339
z = 3,546,564
tb = -3.3342092

CONTENTS
4.0 INTRODUCTION
SYSTEM
METHOD
4.6.1 Computing Receiver Location Using GPS via Newton’s
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     
20.52 2 2
1
N
i i i i
b
i
L x x y y z z d ct

                

   
20.5
1
N
Ti i i
b
i
L X X X X d ct

             

Eq. 4.24 
2
1
N
i 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
N
i T i i
b
i
i T i i T
L X X X X d ct
X
X X X X X X

     

   

1
2 { ( )}( )
N
i i i T i
b
i
L r d ct X X r
X 
    
 

 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.26
1
2{ ( )}( 1)
N
i i
b
b i
L r d ct
ct 
    
 
1
2 { ( )}( )
N
i i i T i
b
i
L r d ct X X r
X 
    
 
1
2{ ( )}
N
i i
b
b i
L r d ct
ct 
   
 

 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 T
w f w f w
w

 
        
1
( ) ( ) [ ( ) ]
2
T T
f 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

 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

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  
1 1( ) ( )k measured kt t  
1 1( ) ( )k measured kt t  

 The equations for positions;
• For x,
• For y,
• For z,
1 1
2
1
1 1 1
( ) ( ) a ( ) ( )
( )
( ) ( ) ( )( ) a ( )
2
k x k measured k k
k k
k k k k k x k measured
x t x t t t t
t t
x t x t x t t t t
 

  
  

   
Eq. 4.38(a,b)
1 1
2
1
1 1 1
( ) ( ) a ( ) ( )
( )
( ) ( ) ( )( ) a ( )
2
k y k measured k k
k k
k k k k k y k measured
y t y t t t t
t t
y t y t y t t t t
 

  
  

   
Eq. 4.38(c,d)
1 1
2
1
1 1 1
( ) ( ) a ( ) ( )
( )
( ) ( ) ( )( ) a ( )
2
k z k measured k k
k k
k k k k k z k measured
z t z t t t t
t t
z t z t z t t t t
 

  
  

   
Eq. 4.38(e,f)

 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 L
L L
L L
L L
L L
L 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
q
q
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 2
tan
1 2 2
sin 2 2
2 2
tan
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.

 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)
 
 
 
sin
2
cos
2
R L
R l
R l
r
W
r
x
r
y
 

 

 

  
 
  
  
  
 

 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
r
k k k k
W
r
x k x k k k k
r
y k y k k k k
     
    
    
       
        
        

 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
 1
tan x zg g 
 Eq. 4.56(a)
 1
tan 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.

Intelligent robotic v2

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Intelligent robotic v2

Similar to Intelligent robotic v2 (20)

Recently uploaded

Recently uploaded (20)

Intelligent robotic v2