consistent triangulation for mobile robot …font/downloads/cim_kalman.pdfjosep m. font llagunes...
TRANSCRIPT
![Page 1: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/1.jpg)
Josep M. Font Llagunes
Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular Measurements
Department of Mechanical Engineering
Universitat Politècnica de Catalunya (UPC)
Barcelona, Catalonia, Spain
Seminar on Mechanical Robotic Systems
Centre for Intelligent Machines McGill University
![Page 2: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/2.jpg)
Department of Mechanical Engineering at UPC
• The university is located in Barcelona (Spain).
Barcelona
School of Engineering (UPC)
![Page 3: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/3.jpg)
Department of Mechanical Engineering at UPC
• The university is located in Barcelona (Spain).
Soccer Team
Works by the famous architect Antoni Gaudí
![Page 4: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/4.jpg)
Department of Mechanical Engineering at UPC
• Kinematics and Localization of Mobile Robots.
• Dynamics and Impact of Multibody Systems.
• Technologies for Disabled People.
• Mechanical Design and Manufacturing.
• Automotive Engineering.
• Acoustics and Vibrations.
Lines of research of the Department:
![Page 5: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/5.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 6: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/6.jpg)
Projects supported by the Research Network in Advanced Production Techniques:
Mobile Robot (1998-2002)
Positioning and Guidance of Mobile Robots Using a Laser System (2003-2007)
SPHERIK-3x3(2001)
STAFF(1999)
TAP (2003)
Research on Mobile Robotics at UPC
Introduction
![Page 7: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/7.jpg)
Laser Positioning System (LPS): laser scanner and reflective landmarks (external measurements)
landmarks
laser scanner
angular encoders
metrological system
Sensors used for the localization of the robot
Angular encoders at the motor axes (internal odometric measurements)
Introduction
![Page 8: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/8.jpg)
Laser Positioning System
Introduction
angular encoder
![Page 9: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/9.jpg)
Position of landmarks Ri
Angular measurements θi
Geometric Triangulation
Position of the scanner centre
Orientation of the robot
Introduction
Laser Positioning System
![Page 10: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/10.jpg)
The landmarks are detected from different robot poses.
The triangulation methods cannot be consistently applied using the direct sensor measurements.
Robot in motion: Dynamic Localization Problem
Introduction
![Page 11: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/11.jpg)
• Adequate accuracy (mm, mrad)
• Robust to outliers
• Dynamic error filtering
Development of a dynamic angular estimation algorithm in order to
guarantee the consistent use of triangulation when the robot moves.
Test the accuracy of the proposed localization method.
Computer simulations
Experiments
Objectives of the work
Introduction
![Page 12: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/12.jpg)
Structure of the localization approach
Introduction
Odometers Localization Using Triangulation
Dynamic Angular Estimation
Sensors
( )i tθ ( )
( )( )
x ty t
tψ
Laser Sensor
(1) Data Fusion Problem (2) Geometric Problem
![Page 13: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/13.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 14: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/14.jpg)
Definition of the geometric problem
Geometric Localization Using Triangulation
• Position of R1, R2, R3
• Angular measurements θ1, θ2, θ3
Determine the position and the orientation of the robot, from the angular
measurements θ1, θ2, θ3 (relative to the robot frame) associated with the lines
from P to three known landmarks R1, R2, R3.
![Page 15: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/15.jpg)
geometric triangulation
• Position of P: p = {x,y}T
• Robot orientation: ψ
• Position of R1, R2, R3
• Angular measurements θ1, θ2, θ3
Definition of the geometric problem
Geometric Localization Using Triangulation
Determine the position and the orientation of the robot, from the angular
measurements θ1, θ2, θ3 (relative to the robot frame) associated with the lines
from P to three known landmarks R1, R2, R3.
![Page 16: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/16.jpg)
Triangulation based on circle intersection
Geometric Localization Using Triangulation
The angle αij (= θj – θi) between the lines connecting P and any two landmarks Ri and Rj,
constrains the robot position to be on a circular arc.
ij j iα θ θ= −
![Page 17: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/17.jpg)
2 1α θ θ≡ − ( ), , 0f x yα α =
Geometric Localization Using Triangulation
Triangulation based on circle intersection
The angle αij (= θj – θi) between the lines connecting P and any two landmarks Ri and Rj,
constrains the robot position to be on a circular arc.
The use of three landmarks (R1, R2, R3) allows to determine the position of P from the
intersection of the arcs associated with each pair of landmarks.
![Page 18: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/18.jpg)
3 2β θ θ≡ − ( ), , 0f x yβ β =
2 1α θ θ≡ − ( ), , 0f x yα α =
Geometric Localization Using Triangulation
Triangulation based on circle intersection
The use of three landmarks (R1, R2, R3) allows to determine the position of P from the
intersection of the arcs associated with each pair of landmarks.
The angle αij (= θj – θi) between the lines connecting P and any two landmarks Ri and Rj,
constrains the robot position to be on a circular arc.
![Page 19: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/19.jpg)
3 1γ θ θ≡ − ( ), , 0f x yγ γ =
3 2β θ θ≡ − ( ), , 0f x yβ β =
2 1α θ θ≡ − ( ), , 0f x yα α =
Geometric Localization Using Triangulation
Triangulation based on circle intersection
The use of three landmarks (R1, R2, R3) allows to determine the position of P from the
intersection of the arcs associated with each pair of landmarks.
The angle αij (= θj – θi) between the lines connecting P and any two landmarks Ri and Rj,
constrains the robot position to be on a circular arc.
![Page 20: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/20.jpg)
Geometric Localization Using Triangulation
Triangulation based on circle intersection
The use of three landmarks (R1, R2, R3) allows to determine the position of P from the
intersection of the arcs associated with each pair of landmarks.
( ), , 0f x yα α =
( ), , 0f x yβ β =
xy
⇒ =
p
Calculation of position:
3 1γ θ θ≡ − ( ), , 0f x yγ γ =
3 2β θ θ≡ − ( ), , 0f x yβ β =
2 1α θ θ≡ − ( ), , 0f x yα α =
The angle αij (= θj – θi) between the lines connecting P and any two landmarks Ri and Rj,
constrains the robot position to be on a circular arc.
![Page 21: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/21.jpg)
Geometric Localization Using Triangulation
( ), arctan
1,2,3
ii i
i
y yx x
i
ψ θ θ−= −
−∀ =
p
( ), , 0f x yα α =
( ), , 0f x yβ β =
xy
⇒ =
p
Calculation of orientation:
Triangulation based on circle intersection
The use of three landmarks (R1, R2, R3) allows to determine the position of P from the
intersection of the arcs associated with each pair of landmarks.
Calculation of position:
The angle αij (= θj – θi) between the lines connecting P and any two landmarks Ri and Rj,
constrains the robot position to be on a circular arc.
![Page 22: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/22.jpg)
Geometric Localization Using Triangulation
The position of P is undetermined since the three circumferences are the same and
they cannot be intersected.
Singularities of the method
Singularities arise when point P lies on the critical circumference that define the three
landmarks used (R1 , R2 , R3).
criticalcircumference
![Page 23: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/23.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 24: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/24.jpg)
Error analysis. Bounded error in the measurements
Bounded error in the measurements: . ,r r ri i m mi i iθ θ δθ θ θ θ θ = + ∈ − ∆ + ∆
Each circumference has an uncertainty area associated: Zα , Zβ , Zγ .
2 , 2r rm mα α θ α θ ∈ − ∆ + ∆ Zα
Error Analysis for the Robot Position
![Page 25: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/25.jpg)
2 , 2r rm mβ β θ β θ ∈ − ∆ + ∆ Zβ
2 , 2r rm mα α θ α θ ∈ − ∆ + ∆ Zα
Bounded error in the measurements: . ,r r ri i m mi i iθ θ δθ θ θ θ θ = + ∈ − ∆ + ∆
Each circumference has an uncertainty area associated: Zα , Zβ , Zγ .
Error analysis. Bounded error in the measurements
Error Analysis for the Robot Position
![Page 26: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/26.jpg)
2 , 2r rm mγ γ θ γ θ ∈ − ∆ + ∆ Zγ
2 , 2r rm mβ β θ β θ ∈ − ∆ + ∆ Zβ
2 , 2r rm mα α θ α θ ∈ − ∆ + ∆ Zα
Bounded error in the measurements: . ,r r ri i m mi i iθ θ δθ θ θ θ θ = + ∈ − ∆ + ∆
Each circumference has an uncertainty area associated: Zα , Zβ , Zγ .
Error analysis. Bounded error in the measurements
Error Analysis for the Robot Position
![Page 27: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/27.jpg)
3
, 1ijP
i ji j
α α β γ=
≠
= = ∩ ∩
Z Z Z Z Z
2 , 2r rm mγ γ θ γ θ ∈ − ∆ + ∆ Zγ
2 , 2r rm mβ β θ β θ ∈ − ∆ + ∆ Zβ
2 , 2r rm mα α θ α θ ∈ − ∆ + ∆ Zα
Bounded error in the measurements: . ,r r ri i m mi i iθ θ δθ θ θ θ θ = + ∈ − ∆ + ∆
Each circumference has an uncertainty area associated: Zα , Zβ , Zγ .
Error analysis. Bounded error in the measurements
Error Analysis for the Robot Position
![Page 28: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/28.jpg)
Monte Carlo Simulation
maximum error
Bounded error in the measurements: . ,r r ri i m mi i iθ θ δθ θ θ θ θ = + ∈ − ∆ + ∆
Each circumference has an uncertainty area associated: Zα , Zβ , Zγ .
Error analysis. Bounded error in the measurements
Error Analysis for the Robot Position
![Page 29: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/29.jpg)
x (m)y
(m)
-5 0 5
-2
0
2
4
6
8
10
12
0
5
10
15
20
25
30
x (m)
y (m
)
0 5 10
-2
0
2
4
6
8
10
12
0
2
4
6
8
10
12
14
16
18
20(mm)(mm)
Mapping of the Maximum Error (for ∆θm = 0,2 mrad)
Error analysis. Bounded error in the measurements
Error Analysis for the Robot Position
![Page 30: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/30.jpg)
Error analysis. Gaussian error in the measurements
Gaussian error in the measurements:
Position error:
( )2; 0,ri i ii N θθ θ δθ δθ σ= +
3 3 3 23
1 1 1
12
ri i j
i i ji i jOδ δθ δθ δθ
θ θ θ= = =
∂ ∂= − = + +
∂ ∂ ∂∑ ∑∑p pp p p
Error Analysis for the Robot Position
![Page 31: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/31.jpg)
Error analysis. Gaussian error in the measurements
Gaussian error in the measurements:
Position error: 3 3 3 2
3
1 1 1
12
ri i j
i i ji i jOδ δθ δθ δθ
θ θ θ= = =
∂ ∂= − = + +
∂ ∂ ∂∑ ∑∑p pp p p
Expected value of the position error:
{ }2 2 2 2
2 2 21 2 32
E θσδθ θ θ
∂ ∂ ∂= + + ∂ ∂ ∂
p p pp
Error Analysis for the Robot Position
( )2; 0,ri i ii N θθ θ δθ δθ σ= +
![Page 32: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/32.jpg)
Mapping of the Expected Value of the Position Error (for σθ = 1 mrad)
x (m)
y (m
)
0 5 10
-2
0
2
4
6
8
10
12
0
0,1
0,2
0,3
0,4
0,5
x (m)y
(m)
-5 0 5
-2
0
2
4
6
8
10
12
0
0,1
0,2
0,3
0,4
0,5(mm)(mm)
Error Analysis for the Robot Position
Error analysis. Gaussian error in the measurements
![Page 33: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/33.jpg)
{ } 2 ;T TE θδ δ σ ∂ = ∂
pC p p J J J =θ
Error analysis. Gaussian error in the measurements
Gaussian error in the measurements:
Position error:
( )2; 0,ri i ii N θθ θ δθ δθ σ= +
3 3 3 23
1 1 1
12
ri i j
i i ji i jOδ δθ δθ δθ
θ θ θ= = =
∂ ∂= − = + +
∂ ∂ ∂∑ ∑∑p pp p p
Expected value of the position error:
{ }2 2 2 2
2 2 21 2 32
E θσδθ θ θ
∂ ∂ ∂= + + ∂ ∂ ∂
p p pp
Covariance of the position error:
( ) ( ){ }1Tr rS | K−≡ − − ≤p p p C p pUncertainty Ellipsoid:
Error Analysis for the Robot Position
![Page 34: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/34.jpg)
The dispersion in the robot position is represented by an Uncertainty Ellipsoid.
Uncertainty ellipsoid
Error Analysis for the Robot Position
![Page 35: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/35.jpg)
Uncertainty Ellipsoid (S)
The ellipsoid represents the set of points in the x-y plane that have a certain probability to contain the measured position of P (K defines this probability).
Uncertainty ellipsoid
Error Analysis for the Robot Position
( ) ( ){ }1Tr rS | K−≡ − − ≤p p p C p p
The dispersion in the robot position is represented by an Uncertainty Ellipsoid.
![Page 36: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/36.jpg)
Uncertainty ellipsoid
Error Analysis for the Robot Position
Ellipsoids for different
probabilities (K)
( ) ( ){ }1Tr rS | K−≡ − − ≤p p p C p p
The dispersion in the robot position is represented by an Uncertainty Ellipsoid.
The ellipsoid represents the set of points in the x-y plane that have a certain probability to contain the measured position of P (K defines this probability).
![Page 37: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/37.jpg)
σθ = 0,1 mrad σθ = 1 mrad
Uncertainty ellipsoid (for K defining 95% probability)
Error Analysis for the Robot Position
![Page 38: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/38.jpg)
σθ = 0,1 mrad σθ = 1 mrad
Uncertainty ellipsoid (for K defining 95% probability)
Error Analysis for the Robot Position
![Page 39: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/39.jpg)
The method has no singularities if three unaligned landmarks are used.
If the orientation angle ψ is known, the position of P can be determined using straight-line intersection.
Novel triangulation method based on straight-line intersection
P not aligned with Ri i RjP aligned with Ri i Rj a third
unaligned landmark is needed (Rk)
Novel Triangulation Method Based on Straight Lines
![Page 40: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/40.jpg)
Before triangulating the orientation ψ is not known, but an approximation of it can be used:
If ψe ≠ ψ , then the lines intersect in three points O12 , O13 , O23 (these define the error triangle).
Triangulation based on straight lines. Error triangle
Orientation Error e ψψ ψ ε≡ +
e ψψ ψ ε≡ +Approximate Orientation
Iterative Method
[Cohen and Koss 1992]
error triangle
Novel Triangulation Method Based on Straight Lines
![Page 41: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/41.jpg)
Is there any geometric relationship between the error triangle and εψ
Error triangles for different values of εψ :?
Novel Triangulation Method Based on Straight Lines
Triangulation based on straight lines. Error triangle
![Page 42: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/42.jpg)
α sin2εψ
α sinεψ
Area
Perimeter
Is there any geometric relationship between the error triangle and εψ
Error triangles for different values of εψ :?
Novel Triangulation Method Based on Straight Lines
Triangulation based on straight lines. Error triangle
![Page 43: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/43.jpg)
Novel Triangulation Method Based on Straight Lines
error triangle
centres triangle
Triangulation based on straight lines. Error and centres triangles
![Page 44: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/44.jpg)
Geometric relationships
Similarity ratio between the triangles:
O
C2sinr ψ
ρ ερ
≡ = O O
arcsin2 C C
ij ik
ij ikψε =
Novel Triangulation Method Based on Straight Lines
Font-Llagunes, J.M. and Batlle, J.A., 2007, “A new method that solves the three-point resection problem using straight lines intersection,” Journal of Surveying Engineering (accepted for publication).
![Page 45: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/45.jpg)
( ) ( )z
sign sign O O C Cij ik ij ikψε = ∧
The angle between corresponding sides of the triangles is π/2 – εψ rad.
The sign of εψ can be determined using the following expression:
Similarity ratio between the triangles:
O
C2sinr ψ
ρ ερ
≡ = O O
arcsin2 C C
ij ik
ij ikψε =
Novel Triangulation Method Based on Straight Lines
Geometric relationships
Font-Llagunes, J.M. and Batlle, J.A., 2007, “A new method that solves the three-point resection problem using straight lines intersection,” Journal of Surveying Engineering (accepted for publication).
![Page 46: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/46.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 47: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/47.jpg)
Dynamic Localization From Discontinuous Measurements
Landmarks are detected from different configurations of the robot
Triangulation is inconsistentif the sensor measurements are directly used
The Dynamic Localization Problem
![Page 48: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/48.jpg)
Suggested solution: Dynamic Angular Estimation
Real-time simulation of the landmark angles based on odometric information
Guarantees the consistent use of the triangulation methods using the simulated angles
Dynamic Localization From Discontinuous Measurements
![Page 49: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/49.jpg)
( ) ( )0
0sin cost L i T i
i it i
v vt t dtθ θθ θ ψρ−
= + −∫
Angular odometry
Between actual sensor measurements, angle θi can be predicted integrating the following equation that governs its time rate of change:
( ) sin cosL i T ii
i
v vt
θ θψ θρ
∂ −+ =
∂
Dynamic Localization From Discontinuous Measurements
![Page 50: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/50.jpg)
( ) ( )0
0sin cost L i T i
i it i
v vt t dtθ θθ θ ψρ−
= + −∫
( ) sin cosL i T ii
i
v vt
θ θψ θρ
∂ −+ =
∂
Cumulative error in the odometric prediction
Variables vL , vT , ψ are obtained from the odometric measurements
Error in the discontinuous angular observationsKalman Filtering EKF
Dynamic Localization From Discontinuous Measurements
Angular odometry
Between actual sensor measurements, angle θi can be predicted integrating the following equation that governs its time rate of change:
![Page 51: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/51.jpg)
Angular odometry. Kinematics of the robot SPHERIK-3x3
3-DOF mobile robot with 3 omnidirectional wheels consisting of two spherical rollers. Each wheel is actuated by a single motor.
Motorized motionControlled by the motor
Free motionIt adapts to the kinematics
imposed by the set of motors
Dynamic Localization From Discontinuous Measurements
motorized motion
freemotion
![Page 52: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/52.jpg)
1
2
3
0 11 cos sin
cos sin
L
T
L vs v
rs
Jω
ωω α αω α α ψ
− − = − − −
11
2
3
L
T
vv Jω
ωω
ψ ω
−
=
motor encoders
Dynamic Localization From Discontinuous Measurements
Angular odometry. Kinematics of the robot SPHERIK-3x3
3-DOF mobile robot with 3 omnidirectional wheels consisting of two spherical rollers. Each wheel is actuated by a single motor.
![Page 53: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/53.jpg)
LONGITUDINAL DOF
Dynamic Localization From Discontinuous Measurements
![Page 54: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/54.jpg)
TRANSVERSAL DOF
Dynamic Localization From Discontinuous Measurements
![Page 55: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/55.jpg)
ROTATIONAL DOF
Dynamic Localization From Discontinuous Measurements
![Page 56: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/56.jpg)
Angular-State Extended Kalman Filter (EKF)
• Prediction of the angular state-vector from odometric measurements:
• Prediction of the error covariance:
, 1 ,T T
x k x k kP f P f f Q f+ = ∇ ∇ + ∇ ∇x x w w
Prediction Phase(odometric evolution of the state-vector)
( )1 , ,k k k kf+ =x x u w
, , , ,
, 1 ,,
sin cosL k i k T k i ki k i k k
i k
v vt θ θθ θ ψρ+
−= + ∆ −
( )1 , ,0k k kf+ = x x u
Angular state-vector: { }1, 2, 3,, , Tk k k kθ θ θ=x
State transition equation
Dynamic Localization From Discontinuous Measurements
![Page 57: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/57.jpg)
• Correction of the odometrically predicted state-vector:
• Estimation of the error covariance:
Correction Phase(angle θi,k is measured by the sensor)
( ), ,k k k i k i kK θ θ= + −x x
( )
2
2i
i
i i i iRθ
θ
σθ θ θ θ
σ= + −
+
( )2 ;i j
i
j j i i i jR
θ θ
θ
σθ θ θ θ
σ= + − ≠
+
( ), ,i
x k k x kP I K h P= − ∇ x
• Kalman gain matrix is determined:
,2
,
1
i
i Tk x k
kK P h
Rθσ= ∇
+x
minimizessquared error
laser sensormeasurement
Dynamic Localization From Discontinuous Measurements
Angular-State Extended Kalman Filter (EKF)
![Page 58: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/58.jpg)
No error due to LINEARIZATION
Dynamic Localization From Discontinuous Measurements
• Correction of the odometrically predicted state-vector:
• Estimation of the error covariance:
Correction Phase(angle θi,k is measured by the sensor)
( ), ,k k k i k i kK θ θ= + −x x
( )
2
2i
i
i i i iRθ
θ
σθ θ θ θ
σ= + −
+
( )2 ;i j
i
j j i i i jR
θ θ
θ
σθ θ θ θ
σ= + − ≠
+
( ), ,i
x k k x kP I K h P= − ∇ x
• Kalman gain matrix is determined:
,2
,
1
i
i Tk x k
kK P h
Rθσ= ∇
+x
minimizessquared error
Angular-State Extended Kalman Filter (EKF)
![Page 59: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/59.jpg)
Comparison with the standard Pose-State EKF
POSE-STATE EKF
[Wiklund et al. 1988]
[Durrant-Whyte 1994]
[Hu i Gu 2000]
[Sgorbissa 2000]
[Piaggio et al. 2001]
The usual method to dynamically estimate the robot pose from discontinuous angular measurements is the Pose-State EKF.
State-transition equation:
Measurement equation:
, arctan i ki k k m
i k
y yx x
θ ψ δθ− = − + − non-linear
1
1
1
cos sinsin cos
k k L k T k
k k L k T k
k k
x x v vy y t v v
ψ ψψ ψ
ψ ψ ψ
+
+
+
− = + ∆ +
{ }, , Tk k k kx y ψ=y Pose state-vector:
Dynamic Localization From Discontinuous Measurements
![Page 60: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/60.jpg)
Change of the State Space of the Kalman Filter
Dynamic Localization From Discontinuous Measurements
Comparison with the standard Pose-State EKF
![Page 61: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/61.jpg)
• The algorithm fuses the pose odometry with the discontinuous angular measurements.
• The algorithm fuses the angular odometry with the discontinuous angular measurements.
Pose-State EKF Angular-State EKF
Dynamic Localization From Discontinuous Measurements
Comparison with the standard Pose-State EKF
![Page 62: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/62.jpg)
• The algorithm fuses the pose odometry with the discontinuous angular measurements.
• The measurement equation is non-linear gives rise to important errors when the state prediction is uncertain.
• The algorithm fuses the angular odometry with the discontinuous angular measurements.
• The measurement equation islinear there is no error (due to linearization of the measurement eq.) in the correction phase.
Dynamic Localization From Discontinuous Measurements
Pose-State EKF Angular-State EKF
Comparison with the standard Pose-State EKF
![Page 63: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/63.jpg)
• The algorithm fuses the pose odometry with the discontinuous angular measurements.
• The measurement equation is non-linear gives rise to important errors when the state prediction is uncertain.
• Each angular measurement corrects the odometric pose estimation only in the perpendicular direction to the viewed landmark.
• The algorithm fuses the angular odometry with the discontinuous angular measurements.
• The measurement equation islinear there is no error (due to linearization of the measurement eq.) in the correction phase.
• Once the angular state-vector is estimated, the robot pose is geometrically determined using triangulation.
Dynamic Localization From Discontinuous Measurements
Pose-State EKF Angular-State EKF
Comparison with the standard Pose-State EKF
![Page 64: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/64.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 65: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/65.jpg)
1,
1,
cossin
mk kk
mk kk
lxly
ϕϕ
=
Experimental Setup
We need a system to measure the trajectory of the centre of the laser sensor and the
robot orientation with an accuracy below mm (in position) and mrad (in orientation).
We designed a metrological system consisting of two rotary encoders and a linear
potentiometer.
Position of P:
Robot orientation:
Experimental Results
1, 2,mk k kψ ϕ ϕ= +
![Page 66: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/66.jpg)
Experimental Results
1,
1,
cossin
mk kk
mk kk
lxly
ϕϕ
=
Position of P:
Robot orientation: 1, 2,
mk k kψ ϕ ϕ= +
We need a system to measure the trajectory of the centre of the laser sensor and the
robot orientation with an accuracy below mm (in position) and mrad (in orientation).
We designed a metrological system consisting of two rotary encoders and a linear
potentiometer.
Experimental Setup
![Page 67: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/67.jpg)
Dynamic estimation of the robot pose from odometric and angular measurements.
Methodology
Experimental Results
Pose measurement by means of the high-accuracy metrological system.
![Page 68: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/68.jpg)
potentiometer
Control system and data adquisition module
Experimental Results
Layout of the electronic components on the robot.
laser head
power amplifiers
DAQ-module and DSP
![Page 69: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/69.jpg)
Experimental environment and trajectories Three trajectories were tested.
trajectory 1
Experimental Results
![Page 70: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/70.jpg)
Experimental Results
trajectory 2
Experimental environment and trajectories Three trajectories were tested.
![Page 71: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/71.jpg)
Experimental Results
Experimental environment and trajectories Three trajectories were tested.
trajectory 3
![Page 72: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/72.jpg)
Angular-State EKF –4,24 mm
Pose-State EKF –84,31 mm
Results. Lateral error (trajectory 1)
elat
Angular-State EKF 1,30 mm
Pose-State EKF 4,94 mm
RMS (elat)
Experimental Results
3rd detection
![Page 73: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/73.jpg)
Angular-State EKF –1,74 mrad
Pose-State EKF 23,28 mrad
eψ
Pose-State EKF 3,26 mrad
RMS (eψ)
Experimental Results
Angular-State EKF 1,06 mrad
Results. Orientation error (trajectory 1)
3rd detection
![Page 74: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/74.jpg)
Angular-State EKF + Triangulation
Pose-State EKF
Trajectory RMS (elat)
(mm)
RMS (eψ)
(mrad)
RMS (elat)
(mm)
RMS (eψ)
(mrad)
(1) 1,30 1,06 4,94 3,26
(2) 2,43 4,89 3,37 4,90
(3) 3,20 5,13 17,24 5,37
RMS value of the lateral error and the orientation error.
Table of results
1,30 3,20 mm 3,37 17,24 mm
Experimental Results
Font-Llagunes, J.M. and Batlle, J.A., 2008, “Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular Measurements,” Robotics and Autonomous Systems (under review).
![Page 75: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/75.jpg)
Experimental Results
Angular-State EKF + Triangulation
Pose-State EKF
Trajectory RMS (elat)
(mm)
RMS (eψ)
(mrad)
RMS (elat)
(mm)
RMS (eψ)
(mrad)
(1) 1,30 1,06 4,94 3,26
(2) 2,43 4,89 3,37 4,90
(3) 3,20 5,13 17,24 5,37
1,06 ÷ 5,13 mrad 3,26 ÷ 5,37 mrad
RMS value of the lateral error and the orientation error.
Table of results
Font-Llagunes, J.M. and Batlle, J.A., 2008, “Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular Measurements,” Robotics and Autonomous Systems (under review).
![Page 76: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/76.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 77: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/77.jpg)
Conclusions
Conclusions
• It guarantees the consistent and continuous use of the triangulation methods.
A Dynamic Angular-State Estimator based on Kalman Filtering has been developed.
It uses odometric data to track the evolution of the angular measurements in
between discontinuous landmark detections.
• Together with triangulation, it improves the accuracy of the widespread Pose-State Extended Kalman Filter.
• The method is robust: It is easy to notice erroneous measurements (due to bad reflections) or non-detected landmarks.
![Page 78: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/78.jpg)
Introduction
Geometric Localization Using Triangulation
Error Analysis for the Robot Position
Dynamic Localization From Discontinuous Measurements
Experimental Results
Conclusions
Presentation Contents
Design of an Automatic Omnidirectional Wheelchair
![Page 79: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/79.jpg)
Tri-Spherical Wheelchair
- Same concept as SPHERIK-3x3
- 3 motors 3 DOF
![Page 80: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/80.jpg)
The 3 motion modes of the wheelchair
Tri-Spherical Wheelchair
![Page 81: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/81.jpg)
Tri-Spherical Wheelchair
![Page 82: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/82.jpg)
Tri-Spherical Wheelchair
Longitudinal motion
Rotation
Transverse motion
General motion
![Page 83: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/83.jpg)
Advantages of the omnidirectional wheelchair
![Page 84: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/84.jpg)
Advantages of the omnidirectional wheelchair
Lateral motion Lateral motion is impossible (no slipping)
Possibility of a lateral motion
![Page 85: Consistent Triangulation for Mobile Robot …font/downloads/CIM_Kalman.pdfJosep M. Font Llagunes Consistent Triangulation for Mobile Robot Localization Using Discontinuous Angular](https://reader034.vdocuments.mx/reader034/viewer/2022042119/5e9944b7f5ba05202b51b02d/html5/thumbnails/85.jpg)
Advantages of the omnidirectional wheelchair
Minimum space rotation