Localización Dinámica de Robots Móviles Basada en Filtrado de Kalman y Triangulación

Universidad Pública de Navarra 13 de Noviembre de 2008 Departamento de Ingeniería Mecánica, Energética y de Materiales Localización Dinámica de Robots Móviles Basada en Filtrado de Kalman y Triangulación Josep Maria Font Llagunes josep.m.font@upc.edu Departamento de Ingeniería Mecánica

Presentation Contents Introduction Geometric Localization Using Triangulation Error Analysis for the Robot Position Dynamic Localization From Discontinuous Measurements Experimental Results Conclusions A Novel Triangulation Approach

Research on Mobile Robotics at UPC 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) STAFF (1999) SPHERIK-3x3 (2001) TAP (2003) Introduction

Sensors used for the localization of the robot Laser Positioning System (LPS): laser scanner and reflective landmarks (external measurements) Angular encoders at the motor axes (internal odometric measurements) landmarks laser scanner metrological system angular encoders Introduction

Laser Positioning System angular encoder Introduction

Laser Positioning System Position of landmarks R i Angular measurements θ i Geometric Triangulation Position of the scanner centre Orientation of the robot Introduction

Robot in motion: Dynamic Localization Problem The landmarks are detected from different robot poses. The triangulation methods cannot be consistently applied using the direct sensor measurements. Introduction

Objectives of the work Development of a dynamic angular estimation algorithm in order to guarantee the consistent use of triangulation when the robot moves. Adequate accuracy (mm, mrad) Robust to outliers Dynamic error filtering Test the accuracy of the proposed localization method. Computer simulations Experiments Introduction

Structure of the localization approach Sensors Odometers Laser Sensor Dynamic Angular Estimation x θ i ( t) ( t) Localization Using y( t) Triangulation ψ ( t) (1) Data Fusion Problem (2) Geometric Problem Introduction

Definition of the geometric problem 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 R 1, R 2, R 3. Position of R 1, R 2, R 3 Angular measurements θ 1, θ 2, θ 3 Geometric Localization Using Triangulation

Triangulation based on circle intersection The angle α ij (= θ j θ i ) between the lines connecting P and any two landmarks R i and R j, constrains the robot position to be on a circular arc. The use of three landmarks (R 1, R 2, R 3 ) allows to determine the position of P from the intersection of the arcs associated with each pair of landmarks. f ( xy ) α θ2 θ1 α,, α = 0 Geometric Localization Using Triangulation

Triangulation based on circle intersection The angle α ij (= θ j θ i ) between the lines connecting P and any two landmarks R i and R j, constrains the robot position to be on a circular arc. The use of three landmarks (R 1, R 2, R 3 ) allows to determine the position of P from the intersection of the arcs associated with each pair of landmarks. f ( xy ) α θ2 θ1 f ( xy ) β θ3 θ2 α,, α = 0 β,, β = 0 Geometric Localization Using Triangulation

Triangulation based on circle intersection The angle α ij (= θ j θ i ) between the lines connecting P and any two landmarks R i and R j, constrains the robot position to be on a circular arc. The use of three landmarks (R 1, R 2, R 3 ) allows to determine the position of P from the intersection of the arcs associated with each pair of landmarks. f ( xy ) f ( xy ) α θ2 θ1 β θ3 θ2 γ θ3 θ1 α,, α = 0 β,, β = 0 f ( xy ) γ,, γ = 0 Geometric Localization Using Triangulation

Triangulation based on circle intersection The angle α ij (= θ j θ i ) between the lines connecting P and any two landmarks R i and R j, constrains the robot position to be on a circular arc. The use of three landmarks (R 1, R 2, R 3 ) allows to determine the position of P from the intersection of the arcs associated with each pair of landmarks. f ( xy ) f ( xy ) f ( xy ) α θ2 θ1 β θ3 θ2 γ θ3 θ1 α,, α = 0 β,, β = 0 γ,, γ = 0 Calculation of position: ( xy ) fα,, α = 0 fβ xy,, β = 0 ( ) p 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 R i and R j, constrains the robot position to be on a circular arc. The use of three landmarks (R 1, R 2, R 3 ) allows to determine the position of P from the intersection of the arcs associated with each pair of landmarks. Calculation of orientation: yi y ψ ( p, θi) = arctan θi x x i = 1, 2, 3 i Calculation of position: ( xy ) fα,, α = 0 fβ xy,, β = 0 ( ) p x = y Geometric Localization Using Triangulation

Singularities of the method Singularities arise when point P lies on the critical circumference that define the three landmarks used (R 1, R 2, R 3 ). The position of P is undetermined since the three circumferences are the same and they cannot be intersected. critical circumference Geometric Localization Using Triangulation

Error analysis. Bounded error in the measurements r r r Bounded error in the measurements: θi = θi + δθi θi θm, θi θm +. Each circumference has an uncertainty area associated: Z α, Z β, Z γ. r r α α 2 θm, α + 2 θm Z α Error Analysis for the Robot Position

Error analysis. Bounded error in the measurements r r r Bounded error in the measurements: θi = θi + δθi θi θm, θi θm +. Each circumference has an uncertainty area associated: Z α, Z β, Z γ. maximum error Monte Carlo Simulation Error Analysis for the Robot Position

Error analysis. Bounded error in the measurements Mapping of the Maximum Error (for θ m = 0,2 mrad) ε (mm) ε (mm) 20 30 12 10 18 16 12 10 25 y (m) 8 6 4 2 14 12 10 8 6 y (m) 8 6 4 2 20 15 10 0-2 4 2 0-2 5 0 5 10 x (m) 0-5 0 5 x (m) 0 Error Analysis for the Robot Position

Error analysis. Gaussian error in the measurements Gaussian error in the measurements: 2 ( ) r i i i i N θ θ = θ + δθ ; δθ 0, σ Position error: 3 3 3 2 r p 1 p δ p= p p = δθ + δθ δθ + O i i j i= 1 θi 2 i= 1 j= 1 θi θj 3 Expected value of the position error: E{ δ p} 2 2 2 2 σ θ p p p = + + 2 2 2 2 θ1 θ2 θ3 Error Analysis for the Robot Position

Error analysis. Gaussian error in the measurements Mapping of the Expected Value of the Position Error (for σ θ = 1 mrad) E{δp} (mm) E{δp} (mm) 0,5 0,5 12 12 10 0,4 10 0,4 y (m) 8 6 4 2 0,3 0,2 y (m) 8 6 4 2 0,3 0,2 0 0,1 0 0,1-2 -2 0 5 10 x (m) 0-5 0 5 x (m) 0 Error Analysis for the Robot Position

Uncertainty ellipsoid The dispersion in the robot position is represented by an Uncertainty Ellipsoid. Error Analysis for the Robot Position

Uncertainty ellipsoid The dispersion in the robot position is represented by an Uncertainty Ellipsoid. { T ( ) 1 ( ) } r r p p p C p p S K 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). Error Analysis for the Robot Position

Uncertainty ellipsoid The dispersion in the robot position is represented by an Uncertainty Ellipsoid. { T ( ) 1 ( ) } r r p p p C p p S K Ellipsoids for different probabilities (K) 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). Error Analysis for the Robot Position

Uncertainty ellipsoid (for K defining 95% probability) σ θ = 0,1 mrad σ θ = 1 mrad Error Analysis for the Robot Position

The Dynamic Localization Problem Landmarks are detected from different configurations of the robot Triangulation is inconsistent if the sensor measurements are directly used Dynamic Localization From Discontinuous Measurements

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

Angular odometry Between actual sensor measurements, angle θ i can be predicted integrating the following equation that governs its time rate of change: t ( ψ θi ) + = v sinθ v ρ cosθ L i T i i t vlsinθi vt cosθi θi( t) = θi( t0 ) + ψ dt t 0 ρ i Variables v L, v T, ψ are obtained from the odometric measurements Cumulative error in the odometric prediction Error in the discontinuous angular observations Kalman Filtering EKF 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. Motorized motion Controlled by the motor motorized motion free motion Free motion It adapts to the kinematics imposed by the set of motors 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. ω1 0 1 L vl 1 ω 2 cos α sin α s = vt r ω 3 cosα sinα s ψ J ω vl ω1 1 vt = J ω ω2 ψ ω 3 motor encoders Dynamic Localization From Discontinuous Measurements

LONGITUDINAL DOF Dynamic Localization From Discontinuous Measurements

TRANSVERSAL DOF Dynamic Localization From Discontinuous Measurements

ROTATIONAL DOF Dynamic Localization From Discontinuous Measurements

Angular-State Extended Kalman Filter (EKF) Angular state-vector: xk = { θ1, k, θ2, k, θ3, k} T Prediction Phase (odometric evolution of the state-vector) ( ) x + 1 = f x, u, w k k k k State transition equation θ θ v sinθ v cosθ ψ Lk, ik, Tk, ik, ik, + 1 = ik, + t k ρik, Prediction of the angular state-vector from odometric measurements: ( ) xk+ 1 = f x k, uk,0 Prediction of the error covariance: P + = f P f + f Q f T T xk, 1 x xk, x w k w Dynamic Localization From Discontinuous Measurements

Angular-State Extended Kalman Filter (EKF) Correction Phase (angle θ i,k is measured by the sensor) Kalman gain matrix is determined: 1 Kk = Pxk, h + R Correction of the odometrically predicted state-vector: x 2 σ θ, k i (,, ) = x + K θ θ k k k ik ik σ θ θ θ θ 2 i = θi i + 2 σθ + R i i i ( ) σ θj = θj + ( θi θi) i j σ θθ i j ; 2 θ + R i it x minimizes squared error laser sensor measurement Estimation of the error covariance: i ( x ) P = I K h P xk, k xk, Dynamic Localization From Discontinuous Measurements

Angular-State Extended Kalman Filter (EKF) Correction Phase (angle θ i,k is measured by the sensor) Kalman gain matrix is determined: 1 Kk = Pxk, h + R Correction of the odometrically predicted state-vector: x 2 σ θ, k i (,, ) = x + K θ θ k k k ik ik σ θ θ θ θ 2 i = θi i + 2 σθ + R i i i ( ) σ θj = θj + ( θi θi) i j σ θθ i j ; 2 θ + R i it x minimizes squared error No error due to LINEARIZATION Estimation of the error covariance: i ( x ) P = I K h P xk, k xk, Dynamic Localization From Discontinuous Measurements

Comparison with the standard Pose-State EKF The usual method to dynamically estimate the robot pose from discontinuous angular measurements is the Pose-State EKF. Pose state-vector: y = { x, y, ψ } k k k k T POSE-STATE EKF [Wiklund et al. 1988] [Durrant-Whyte 1994] [Hu i Gu 2000] [Sgorbissa 2000] [Piaggio et al. 2001] State-transition equation: xk+ 1 xk vlcosψk vt sinψk yk+ 1 = yk + t vlsinψk + vt cosψk ψ ψ ψ k+ 1 k Measurement equation: θ yi yk ik, = arctan k m xi x ψ + δθ k non-linear Dynamic Localization From Discontinuous Measurements

Comparison with the standard Pose-State EKF Change of the State Space of the Kalman Filter Dynamic Localization From Discontinuous Measurements

Comparison with the standard Pose-State EKF Pose-State EKF Angular-State EKF The algorithm fuses the pose odometry with the discontinuous angular measurements. The algorithm fuses the angular odometry with the discontinuous angular measurements. The measurement equation is nonlinear 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 measurement equation is linear 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

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: m xk lk cosϕ 1, k m = yk lk sinϕ1, k Robot orientation: ψ = ϕ + ϕ m k 1, k 2, k Experimental Results

Methodology Dynamic estimation of the robot pose from odometric and angular measurements. Pose measurement by means of the high-accuracy metrological system. Experimental Results

Control system and data adquisition module Layout of the electronic components on the robot. laser head power amplifiers potentiometer DAQ-module and DSP Experimental Results

Experimental environment and trajectories Three trajectories were tested. trajectory 1 Experimental Results

Experimental environment and trajectories Three trajectories were tested. trajectory 2 Experimental Results

Experimental environment and trajectories Three trajectories were tested. trajectory 3 Experimental Results

Results. Lateral error (trajectory 1) 3rd detection e lat RMS (e lat ) Angular-State EKF 4,24 mm Angular-State EKF 1,30 mm Pose-State EKF 84,31 mm Pose-State EKF 4,94 mm Experimental Results

Results. Orientation error (trajectory 1) 3rd detection e ψ RMS (e ψ ) Angular-State EKF 1,74 mrad Angular-State EKF 1,06 mrad Pose-State EKF 23,28 mrad Pose-State EKF 3,26 mrad Experimental Results

Table of results RMS value of the lateral error and the orientation error. Trajectory Angular-State EKF + Triangulation RMS (e lat ) (mm) RMS (e ψ ) (mrad) Pose-State EKF RMS (e lat ) RMS (e ψ ) (mm) (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,30 3,20 mm 3,37 17,24 mm 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. Experimental Results

Table of results RMS value of the lateral error and the orientation error. Trajectory Angular-State EKF + Triangulation RMS (e lat ) (mm) RMS (e ψ ) (mrad) Pose-State EKF RMS (e lat ) RMS (e ψ ) (mm) (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 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. Experimental Results

Conclusions 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. It guarantees the consistent and continuous use of the triangulation methods. 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. Conclusions

Novel triangulation method based on straight-line intersection If the orientation angle ψ is known, the position of P can be determined using straightline intersection. The method has no singularities if three unaligned landmarks are used. P not aligned with R i i R j P aligned with R i i R j a third unaligned landmark is needed (R k ) A Novel Triangulation Approach

Triangulation based on straight lines. Error triangle Before triangulating the orientation ψ is not known, but an approximation of it can be used: ψe ψ + εψ Orientation Error If ψ e ψ, then the lines intersect in three points O 12, O 13, O 23 (these define the error triangle). error triangle Approximate Orientation ψ ψ + ε e ψ Iterative Method [Cohen and Koss 1992] A Novel Triangulation Approach

Triangulation based on straight lines. Error triangle Is there any geometric relationship between the error triangle and ε ψ Error triangles for different values of ε ψ :? A Novel Triangulation Approach

Triangulation based on straight lines. Error triangle Is there any geometric relationship between the error triangle and ε ψ Error triangles for different values of ε ψ :? Area α sin 2 ε ψ Perimeter α sinε ψ A Novel Triangulation Approach

Triangulation based on straight lines. Error and centres triangles error triangle centres triangle A Novel Triangulation Approach

Geometric relationships Similarity ratio between the triangles: ρo r = 2sinε ρ C ψ OO ij ik ε ψ = arcsin 2C C ij ik Font-Llagunes, J.M. and Batlle, J.A., 2008, A new method that solves the three-point resection problem using straight lines intersection, Journal of Surveying Engineering, to appear. A Novel Triangulation Approach

Geometric relationships Similarity ratio between the triangles: ρo r = 2sinε ρ C ψ OO ij ik ε ψ = arcsin 2C C ij ik The angle between corresponding sides of the triangles is π/2 ε ψ rad. The sign of ε ψ can be determined using the following expression: sign sign O O C C ( ε ψ ) = ( ij ik ij ik ) z Font-Llagunes, J.M. and Batlle, J.A., 2008, A new method that solves the three-point resection problem using straight lines intersection, Journal of Surveying Engineering, to appear. A Novel Triangulation Approach