EE565:Mobile Robotics Lecture 6
Transcript of EE565:Mobile Robotics Lecture 6
![Page 1: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/1.jpg)
EE565:Mobile Robotics Lecture 6
Welcome
Dr. Ahmad Kamal Nasir
![Page 2: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/2.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Announcement
β’ Mid-Term Examination # 1 (25%) β Understand basic wheel robot kinematics, common mobile
robot sensors and actuators knowledge. β Understand and able to apply various robot motion and sensor
models used for recursive state estimation techniques. β Demonstrate Inertial/visual odometeric techniques for mobile
robots pose calculations.
β’ Date: Wednesday 4th March 2015 β’ Time: 1700-1800 β’ Venue: SDSB 203!!! β’ Format: Objective type β’ Curriculum: Class and Lab Lecture Slides
02.03.2015 Dr. Ahmad Kamal Nasir 2
![Page 3: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/3.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Todayβs Objectives
β’ Inertial sensors models β Euler Angle Representation
β Accelerometer β’ Inertial Odometry
β Gyroscope β’ Euler Angle Rate
β Magnetometer β’ Earth Magnetic Field
β GPS β’ Geodetic to Cartesian Coordinates
02.03.2015 Dr. Ahmad Kamal Nasir 3
![Page 4: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/4.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles
02.03.2015 Dr. Ahmad Kamal Nasir 4
Inertial Frame Vehicle-1 Frame
Vehicle-2 Frame Body Frame
![Page 5: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/5.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles (Cont.)
β’ When using Euler angles then a defined sequence, Yaw(π)Pitch(π)Roll(π), is used to transform body from one orientation to another orientation in 3D space. For example the transformation from inertial frame to body frame using Euler notation is
Inertial frame π πΌπ£1 π Vehicle-1 frame π π£1
π£2 π Vehicle-2frame π π£2π΅ π Body frame
π πΌπ΅ π, π, π = π π£2
π΅ π β π π£1π£2 π β π πΌ
π£1 π
β’ Similarly the transformation from body frame to inertial frame is
π π΅πΌ π, π, π = π πΌ
π£2 βπ β π π£1π£2 βπ β π π£2
π΅ βπ
02.03.2015 Dr. Ahmad Kamal Nasir 5
![Page 6: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/6.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Rotation Matrices
π π π = π π£2π΅ π =
1 0 00 c π s π
0 βs π c π
π π π = π π£1π£2 π =
c π 0 β s π0 1 0
s π 0 c π
π π§ π = π πΌπ£1 π =
c π s π 0
βs π c π 00 0 1
Whereβs πππ π½ = π π½ and πππ π½ = π(π½)
02.03.2015 Dr. Ahmad Kamal Nasir 6
![Page 7: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/7.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles (Cont.)
π πΌπ΅ π, π, π
=
c π c π c π s π β s π
c π s π s π β c π s π c π c π + s π s π π π c π s π
π π π π + π π π π π π π π π π π π β π π π π π π π π
π π΅πΌ π, π, π
=
c π c π c π s π s π β c π s π π π π π + π π π π π π
c π s π c π c π + s π s π π π π π π π π π β π π π π
β s π c π s π π π π π
π πΌπ΅ π, π, π = π π΅
πΌ π, π, ππ
02.03.2015 Dr. Ahmad Kamal Nasir 7
![Page 8: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/8.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Inertial Sensor Model
β’ We require sensor models to calibrate sensors in order to remove noise from sensor measurements thus enhances the accuracy of sensor measurements
02.03.2015 Dr. Ahmad Kamal Nasir 8
![Page 9: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/9.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Measurement Errors
β’ Sensor errors are the deviations of sensor measurements from its expected output.
β’ Systematic errors: periodic, able to be removed by calibration process
β’ Stochastic errors: random
02.03.2015 Dr. Ahmad Kamal Nasir 9
![Page 10: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/10.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
x y
z
β’ Measures all external forces acting upon them (including gravity)
β’ To obtain inertial acceleration (due to motion alone), gravity must be subtracted
β’ Accelerometers behave as a damped mass on a spring. Acceleration causes displacement of this "spring" proportional to the acceleration experienced.
β This room = your weight = 1g
β Bugatti Veyron, 0 to 100Km/h in 2.4s= 1.55g
β Space Shuttle reentry & launch = 3g
β Max experienced by a human* = 46.2g
β Death or extensive & severe injuries= +50g
Accelerometer
02.02.2015 Dr. Ahmad Kamal Nasir 10
![Page 11: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/11.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Accelerometer (Cont.)
β’ It measures acceleration indirectly by the use of force applied on its body.
β’ It measures force in opposite direction of acceleration. β’ Imagine a cube with pressure sensitive walls and a
sphere inside it β In space the ball floats in the middle of box β If we accelerate(g) to left, the right wall measure
acceleration(-g)
02.03.2015 Dr. Ahmad Kamal Nasir 11
![Page 12: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/12.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Accelerometer (Cont.)
β’ If we are on earth the ball will fall on the Z- wall and will apply a force of 1g (gravitation force) on the bottom wall
02.03.2015 Dr. Ahmad Kamal Nasir 12
![Page 13: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/13.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Accelerometer (Cont.)
β’ If we rotate the accelerometer an angle of 45 the X and Z axis will measure the acceleration.
β’ If the accelerometer is stationary then the measured acceleration due to gravity can be calculated as follows
π¨π = π¨πΏπ + π¨π
π + π¨ππ
02.03.2015 Dr. Ahmad Kamal Nasir 13
![Page 14: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/14.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Accelerometer Raw Measurements
To convert the raw accelerometerβs ADC value we use the following equation
π¨πΏ =π¨π ππ½πππππΏ Γ
π½πππππ¨π ππ©πππ β π
β π½ππ
πΊππππππππππ=(π¨π ππ½ππππ β π¨π ππ½ππππππ) Γ π½πππ
πΊππππππππππ Γ (ππ¨π ππ©πππ β π)
Assuming π΄ππππππ’ππ₯ = 530 ππππ = 3.3π, 10bits
ADC, π0 = 1.65π, ADC ππππ ππ‘ππ£ππ‘π¦ =0.4785π
π
530 β 512 Γ 3.3
0.4785 Γ 1023= 0.1213π
02.03.2015 Dr. Ahmad Kamal Nasir 14
![Page 15: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/15.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Accelerometer Measurement Noise
02.03.2015 Dr. Ahmad Kamal Nasir 15
100 200 300 400 500 600 700 800 900
-0.06
-0.04
-0.02
Time (Sec)
Acc.
due t
o g
ravity (
g)
Accelerometer measurements during static condition
X-Acc
y-acc
0 100 200 300 400 500 600 700 800 900
-1.06
-1.04
-1.02
-1
Time (Sec)
Acc.
due t
o g
ravity (
g)
Z-Acc
![Page 16: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/16.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Accelerometer Noise Statistics
02.03.2015 Dr. Ahmad Kamal Nasir 16
![Page 17: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/17.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles by Accelerometer
β’ Gravity vector is perpendicular to the horizontal plane made by the earth surface. Therefore, the gravity vector can be used to determine the roll and pitch angle of the horizontal plane.
β’ Since the horizontal plan can be rotated to any yaw angle with respect to gravity vector direction, therefore, the yaw angle cannot be uniquely determined.
β’ The accelerometer readings are in body frame and the gravity vector is in inertial frame.
β’ If we assume that no external acceleration is acted on the accelerometer then the gravity sensed by the sensor in accelerometer body frame is
π΄π = ππ₯ ππ¦ ππ§π
where the gravity in the inertial/reference frame is π΄π = 0 0 π π.
02.03.2015 Dr. Ahmad Kamal Nasir 17
![Page 18: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/18.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles by Accelerometer (Cont.)
β’ The relationship between π΄π and π΄π is as follows π΄π = πΆπ
π π, π, π β π΄π π΄π = π π₯ π β π π¦ π β π π§ π β π΄π
β’ Accelerometer measures the components of acceleration due to gravity of an oriented sensor.
β’ Since we know the gravity vector, therefore the roll and pitch angles are measured by applying the inverse roll and pitch angle transformation to the accelerometer measurements π π¦ βπ β π π₯ βπ β π΄π = π π¦ βπ β π π₯ βπ β π π₯ π β π π¦ π β π π§ π β π΄π
02.03.2015 Dr. Ahmad Kamal Nasir 18
![Page 19: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/19.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles by Accelerometer (Cont.)
π π¦ βπ β π π₯ βπ β π΄π = π π§ π β π΄π π π¦ βπ β π π₯ βπ β π΄π = π΄π
cos π 0 sin π0 1 0
β sin π 0 cos πβ
1 0 00 cos π βsin π
0 sin π cos πβ
ππ₯ππ¦ππ§
=00π
ππ₯ cos π + ππ¦ sin π sin π + ππ§cos (π)sin (π)
ππ¦ cos π β ππ§sin (π)
βππ₯ sin π + ππ¦ sin π cos π + ππ§cos (π)cos (π)
=00π
02.03.2015 Dr. Ahmad Kamal Nasir 19
![Page 20: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/20.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angles by Accelerometer (Cont.)
ππ₯ cos π + ππ¦ sin π sin π + ππ§cos (π)sin (π)
ππ¦ cos π β ππ§sin (π)
βππ₯ sin π + ππ¦ sin π cos π + ππ§cos (π)cos (π)
=00π
Solving the above equationfor π πππ π results into
π = βarctanππ¦
ππ§
π = βarctanππ₯
ππ¦2 + ππ§
2
= βarctanππ₯
ππ¦ sin π + ππ§cos (π)
02.03.2015 Dr. Ahmad Kamal Nasir 20
![Page 21: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/21.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Inertial Odometry by Accelerometer
β’ To perform inertial odometry the linear acceleration which is being applied on the body has to be determined.
β’ Since the accelerometer measure the acceleration due to gravity in addition to acceleration being applied on the body, a normal force is being applied on the accelerometer which keep it moving towards the center of earth.
β’ Therefore, In order to extract the linear acceleration from the accelerometer measurements, first the measurements are rotated by the accelerometer yaw-pitch-roll angles so that they are in the inertial frame and then gravity vector is being added to it which prevent the accelerometer from moving toward center of earth
02.03.2015 Dr. Ahmad Kamal Nasir 21
![Page 22: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/22.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Inertial Odometry
ππ₯ππ¦ππ§ π
=
c π c π c π s π s π β c π s π π π π π + π π π π π π
c π s π c π c π + s π s π π π π π π π π π β π π π π
β s π c π s π π π π π
β
ππ₯ππ¦ππ§ π
+00π
β’ Position can be estimated using the rectangular integration method as follows π£π = π£πβ1 + ππ β Ξπ‘ ππ = ππβ1 + π£π β Ξπ‘
02.03.2015 Dr. Ahmad Kamal Nasir 22
0 100 200 300 400 500 600 700 800 900 1000-25
-20
-15
-10
-5
0
Time (Sec)
Velo
city (
m/s
ec)
Integrated velocities from Accelerometer measurements during static condition
X-Vel
y-Vel
-6000 -5000 -4000 -3000 -2000 -1000 0-6000
-5000
-4000
-3000
-2000
-1000
0Integrated position trajectory from Accelerometer measurements during static condition
X (m)
Y (
m)
![Page 23: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/23.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Inertial Odometry
β’ In addition to the accelerometer noise errors the biggest error terms come from the orientation (transformation matrix) errors.
β’ If the accelerometer measurements are assumed to be free from noise then the errors in position and velocity due to the errors in the orientation errors can be surmised from the following table
02.03.2015 Dr. Ahmad Kamal Nasir 23
![Page 24: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/24.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Inertial Odometry
02.03.2015 Dr. Ahmad Kamal Nasir 24
![Page 25: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/25.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope
β’ A gyroscope is a device for measuring or maintaining orientation, based on the principles of conservation of angular momentum
β’ Measures orientation (standard gyro) or angular velocity (rate gyro, needs integration for angle)
02.02.2015 Dr. Ahmad Kamal Nasir 25
![Page 26: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/26.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Raw Measurements
β’ Gyroscope measures the rate of changes of the angles. For instance a 3-axes gyroscope will measure the rotation rate about the X, Y and Z axes.
β’ In practice we'll get an ADC value that need to
converted to ΞΈΒ°
Sec using the following formula
π½ =π¨π ππ½ππππ β π¨π ππ½πππππ Γ π½πππ
πΊππππππππππ Γ (ππ¨π ππ©πππ β π)
02.03.2015 Dr. Ahmad Kamal Nasir 26
![Page 27: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/27.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Raw Measurements
β’ π΄ππππππ’π0 is the value which gyroscope outputs when it is not subjected to rotation
β’ Sensitivity: is the scale of measurements and expressed in mV
Β°/π ππ
β’ For example using a 10bits ADC with ππππ = 3.3π, AdcValue0 = 512, Sensitivit =0.002V/Β°/π ππ and π΄ππππππ’π = 571
π½ =πππ β πππ Γ π. π
π. πππ Γ ππππ= 95Β°/π ππ
02.03.2015 Dr. Ahmad Kamal Nasir 27
![Page 28: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/28.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Bias Drift
β’ Gyroscope measurements are integrated to get angle. The blue line represents the drift. The following data shows the bias drift rate of 24Β°/ππ
02.03.2015 Dr. Ahmad Kamal Nasir 28
![Page 29: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/29.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Temperature Drift
02.03.2015 Dr. Ahmad Kamal Nasir 29
0 100 200 300 400 500 600 700 800 900 100022.5
22.55
22.6
22.65
22.7
22.75
22.8
22.85
22.9Temperature variation duing experiment
Time (Sec)
Tem
pera
ture
(C
)
![Page 30: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/30.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Measurements Noise
02.03.2015 Dr. Ahmad Kamal Nasir 30
0 100 200 300 400 500 600 700 800 900 1000-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
Time (Sec)
Angula
r R
ate
(D
eg/S
ec)
Gyro measurements during static condition
X-Gyro
y-Gyro
Z-Gyro
![Page 31: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/31.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Noise Statistics
02.03.2015 Dr. Ahmad Kamal Nasir 31
![Page 32: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/32.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Gyroscope Integration
02.03.2015 Dr. Ahmad Kamal Nasir 32
0 100 200 300 400 500 600 700 800 900 1000-6
-4
-2
0
2
4
6
8
10
12
14
Time (Sec)
Angle
(D
eg)
Integrated angles from Gyro measurements during static condition
X-Angle
Y-Angle
Z-Angle
ππ‘ = ππ‘β1 + ππ‘ β Ξπ‘
![Page 33: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/33.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angular Rate Using Gyroscope
β’ The gyroscopes measurements canβt be directly used because it measures body angular rates not the Euler angular rate.
β’ The gyroscope measurements can be converted into Euler angular rate using the following transformation
π
π
π =
1 sin π tan (π) cos π tan (π)0 cos π βsin (π)0 sin π /cos (π) cos π /cos (π)
β πππ
02.03.2015 Dr. Ahmad Kamal Nasir 33
![Page 34: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/34.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Euler Angular Rate Using Gyroscope
π· π, π, π =
1 sin π tan (π) cos π tan (π)
0 cos π βsin (π)
0 sin π /cos (π) cos π /cos (π)
β’ The above transformation matrix is obtained by
β first rotating the z-axis gyro in inertial frame
β then the y-axis gyro is rotated into vehicle-1 frame
β And finally x-axis gyro is rotated in vehicle-2 frame
02.03.2015 Dr. Ahmad Kamal Nasir 34
![Page 35: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/35.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Magnetometer
β’ Compass invented by the Chinese in the 4th century, Carl Gauss invented the "magnetometer" in 1833. MEMS magnetometer measures the components of earthβs magnetic field intensity
02.02.2015 Dr. Ahmad Kamal Nasir 35
![Page 36: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/36.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Earth Magnetic Field
β’ The earth magnetic field intensity is 20ππ - 70ππ Gauss and has a component of magnetic field parallel to earth surface that always point North while the vertical component points toward the center of earth. The vertical component of magnetic field is not required to find magnetic north.
02.03.2015 Dr. Ahmad Kamal Nasir 36
β’ The geomagnetic field has strength of πΉ and made an inclination angle of πΌ depending upon the latitude and longitude. The difference between geographic north and magnetic north has an declination angle D which is also location dependent
![Page 37: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/37.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Magnetometer (Cont.)
02.03.2015 Dr. Ahmad Kamal Nasir 37
0 100 200 300 400 500 600 700 800 900
0.7
0.8
0.9
X-Magnetometer measurements during static condition
0 100 200 300 400 500 600 700 800 900
-0.5
-0.45
-0.4
-0.35
Y-Magnetometer measurements during static condition
Measure
ments
(
T)
0 100 200 300 400 500 600 700 800 900
-0.2
-0.19
-0.18
Z-Magnetometer measurements during static condition
Time (Sec)
![Page 38: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/38.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Magnetometer (Cont.)
02.03.2015 Dr. Ahmad Kamal Nasir 38
0.65 0.7 0.75 0.8 0.85 0.9 0.95 10
500
1000Histogram of X-Magnetometer measurement during static condition
-0.55 -0.5 -0.45 -0.4 -0.35 -0.30
1000
2000Histogram of Y-Magnetometer measurement during static condition
Fre
quency (
Counts
)
-0.21 -0.205 -0.2 -0.195 -0.19 -0.185 -0.18 -0.175 -0.17 -0.1650
5000Histogram of Z-Magnetometer measurement during static condition
Measurements ( T)
![Page 39: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/39.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Hard/Soft Iron Effects
β’ The accuracy of the magnetometer measurements depends on the stray magnetic field inside and around the magnetometer.
β’ The fixed part is called hard-iron effect and the part due to geomagnetic field is called soft-iron effect. The magnetic field strength and inclination angle depends upon the location (latitude and longitude) on the surface of earth.
02.03.2015 Dr. Ahmad Kamal Nasir 39
β’ When the x-axis of the magnetometer is pointing northward and downward then it experiences maximum geomagnetic field strength
![Page 40: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/40.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Earth Magnetic Field
02.03.2015 Dr. Ahmad Kamal Nasir 40
http://wdc.kugi.kyoto-u.ac.jp/igrf/point/index.html
![Page 41: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/41.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Orientation Using Magnetometer
β’ It can be used to determine heading direction using the geomagnetic measurements
ππ= ππ₯ ππ¦ ππ§π
and the roll and pitch angle measured by the
accelerometer.
02.03.2015 Dr. Ahmad Kamal Nasir 41
![Page 42: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/42.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Orientation Using Magnetometer
β’ The measurements of the magnetometer in sensor frame can be calculated as follows
ππ = π π₯ π β π π¦ π β π π§ π β ππ + π
β’ V is the soft iron effects due to the metallic objects near the magnetometer; the soft iron effects vector is independent from rotations because the objects rotate along magnetometer such as the PCB or casing.
02.03.2015 Dr. Ahmad Kamal Nasir 42
![Page 43: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/43.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Orientation Using Magnetometer
β’ The magnetometer readings are applied with inverse roll and pitch angles so that the measurements are only affected by the yaw angle i.e.
π π¦ βπ β π π₯ βπ β (ππ β π) = π π¦ βπ β π π₯ βπ β π π₯ π β π π¦ π β π π§ π β ππ
π π¦ βπ β π π₯ βπ β (ππβπ) = π π§ π β ππ
02.03.2015 Dr. Ahmad Kamal Nasir 43
![Page 44: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/44.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Orientation Using Magnetometer
cos π 0 sin π0 1 0
β sin π 0 cos πβ
1 0 00 cos π βsin π
0 sin π cos πβ
ππ₯ β π£π₯ππ¦ β π£π¦ππ§ β π£π§
=cos (π) sin (π) 0βsin (π) cos (π) 0
0 0 1
βπ΅ β cos πΏ
0π΅ β sin πΏ
(ππ₯ β π£π₯) cos π + (ππ¦ β π£π¦) sin π sin π + (ππ§ β π£π§)cos (π)sin (π)
(ππ¦ β π£π¦) cos π β (ππ§ β π£π§)sin (π)
β(ππ₯ β π£π₯) sin π + (ππ¦ β π£π¦) sin π cos π + (ππ§ β π£π§)cos (π)cos (π)
=
π΅ β cos πΏ β cos (π)
βπ΅ β cos πΏ β sin (π)
π΅ β sin πΏ
Dividing the y-component by the x-component gives us the yaw angle
π = arctanππ§ β π£π§ sin π β (ππ¦ β π£π¦) cos π
(ππ₯ β π£π₯) cos π + (ππ¦ β π£π¦) sin π sin π + (ππ§ β π£π§)cos (π)sin (π)
02.03.2015 Dr. Ahmad Kamal Nasir 44
![Page 45: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/45.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
IMU
β’ 3-axes MEMS gyroscope
β Provides angular velocity
β Integrate for angular position
β’ 3-axes MEMS accelerometer
β Provides accelerations (including gravity)
02.02.2015 Dr. Ahmad Kamal Nasir 45
![Page 46: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/46.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Global Positioning System
02.02.2015 Dr. Ahmad Kamal Nasir 46
β’ 24+ satellites, 12 hour orbit, 20.190 km height β’ 6 orbital planes, 4+ satellites per orbit, 60deg distance β’ Every satellite transmits its position and time β’ Requires measurements of 4 different satellites β’ Low accuracy (3-15m) but absolute
![Page 47: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/47.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Global Positioning System (Cont.)
β’ GPS is a space based satellite navigation system that provides location and time information anywhere on earth or near earth where there is direct line of sight with at least four satellites.
β’ We have to understand some coordinate frame to perform conversion to/from geodetic coordinates (latitude π , longitude Ξ¦ ) to local Cartesian coordinates π₯, π¦, π§ .
02.03.2015 Dr. Ahmad Kamal Nasir 47
![Page 48: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/48.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Geodetic to Cartesian Coordinates
β’ The first frame is a tangent plane Ο which is tangent to earth surface at some latitude and longitude. X-axis of this plane is pointing north, Y-axis toward east and Z-axis toward the center of earth.
β’ The second frame is Earth Centered Earth Fixed (ECEF) frame π which is used by the GPS and is centered at the center of earth and rotating with earth.
02.03.2015 Dr. Ahmad Kamal Nasir 48
![Page 49: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/49.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Geodetic to Cartesian Coordinates
β’ The X-axis is passing through the prime meridian and the Z-axis is passing through the North Pole, right hand coordinate system.
β’ The third plane is a Body plane π½ which is attached to the center of mass of the body, it could be NEU (North-East-Up) mostly used by tracker and NED (North-East-Down) mostly used by aero planes.
02.03.2015 Dr. Ahmad Kamal Nasir 49
![Page 50: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/50.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Geodetic to Cartesian Coordinates
β’ The conversion from geodetic to Cartesian coordinate is a simple process because there exist a simple and closed form solution.
β’ The inverse process is complicated because no simple relation relates Ξ¦ to X,Y,Z. Broadly there are indirect methods and direct or closed form solution.
β’ Indirect methods are preferred because of the simplicity.
02.03.2015 Dr. Ahmad Kamal Nasir 50
![Page 51: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/51.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Geodetic to Cartesian Coordinates
β’ The earth is considered as a ellipse with semi-major axis π = 6378137 π and semi-minor axis π =6356752.31424 π and π = 6.694 379 990 Γ 10β3.
β’ The coordinates of the GPS receiver in ECEF frame are calculated as follows given geodetic coordinates
π₯π¦π§
=
π£ Ξ¦ + β β cos Ξ¦ β cos ππ£ Ξ¦ + β β cos Ξ¦ β sin π
π£ Ξ¦ β (1 β π2) + β β sin Ξ¦
π£ Ξ¦ =π2
π2 cos Ξ¦ 2+π2 sin Ξ¦ 2 is the radius of curvature in
prime vertical plane.
02.03.2015 Dr. Ahmad Kamal Nasir 51
![Page 52: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/52.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Summary
β’ Inertial sensors models β Euler Angle Representation
β Accelerometer β’ Inertial Odometry
β Gyroscope β’ Euler Angle Rate
β Magnetometer β’ Earth Magnetic Field
β GPS β’ Geodetic to Cartesian Coordinates
02.03.2015 Dr. Ahmad Kamal Nasir 52
![Page 53: EE565:Mobile Robotics Lecture 6](https://reader031.fdocuments.us/reader031/viewer/2022012101/6169f0c511a7b741a34d0efd/html5/thumbnails/53.jpg)
EE565: Mobile Robotics Module 3: Inertial and Visual Odometry
Questions
02.03.2015 Dr. Ahmad Kamal Nasir 53