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

Transcription

1 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

2 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

3 Research on Mobile Robotics at UPC Projects supported by the Research Network in Advanced Production Techniques: Mobile Robot ( ) Positioning and Guidance of Mobile Robots Using a Laser System ( ) STAFF (1999) SPHERIK-3x3 (2001) TAP (2003) Introduction

4 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

5 Laser Positioning System angular encoder Introduction

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

7 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

8 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

9 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

10 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

11 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

12 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 triangulation Position of P: p = {x,y} T Robot orientation: ψ Geometric Localization Using Triangulation

13 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. αij = θj θi Geometric Localization Using Triangulation

14 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

15 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

16 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

17 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

18 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

19 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

20 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

21 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

22 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 α r r β β 2 θm, β + 2 θm Z β Error Analysis for the Robot Position

23 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 α r r β β 2 θm, β + 2 θm Z β r r γ γ 2 θm, γ + 2 θm Z γ Error Analysis for the Robot Position

24 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 α r r β β 2 θm, β + 2 θm Z β r r γ γ 2 θm, γ + 2 θm Z γ P 3 Z = Z = Z Z Z i, j= 1 i j α α β γ ij Error Analysis for the Robot Position

25 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

26 Error analysis. Bounded error in the measurements Mapping of the Maximum Error (for θ m = 0,2 mrad) ε (mm) ε (mm) y (m) y (m) x (m) x (m) 0 Error Analysis for the Robot Position

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

28 Error analysis. Gaussian error in the measurements Gaussian error in the measurements: 2 ( ) r i i i i N θ θ = θ + δθ ; δθ 0, σ Position error: 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} σ θ p p p = θ1 θ2 θ3 Error Analysis for the Robot Position

29 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, ,4 10 0,4 y (m) ,3 0,2 y (m) ,3 0,2 0 0,1 0 0, x (m) x (m) 0 Error Analysis for the Robot Position

30 Error analysis. Gaussian error in the measurements Gaussian error in the measurements: 2 ( ) r i i i i N θ θ = θ + δθ ; δθ 0, σ Position error: 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} σ θ p p p = θ1 θ2 θ3 Covariance of the position error: T 2 T { δ δ } σθ ; C= E p p JJ J= p θ Uncertainty Ellipsoid: { T ( ) 1 ( ) } r r p p p C p p S K Error Analysis for the Robot Position

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

32 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

33 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

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

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

36 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

37 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

38 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

39 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 Dynamic Localization From Discontinuous Measurements

40 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

41 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

42 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

43 LONGITUDINAL DOF Dynamic Localization From Discontinuous Measurements

44 TRANSVERSAL DOF Dynamic Localization From Discontinuous Measurements

45 ROTATIONAL DOF Dynamic Localization From Discontinuous Measurements

46 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

47 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

48 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

49 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

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

51 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. Dynamic Localization From Discontinuous Measurements

52 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. The measurement equation is linear there is no error (due to linearization of the measurement eq.) in the correction phase. Dynamic Localization From Discontinuous Measurements

53 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

54 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

55 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

56 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

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

58 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

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

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

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

62 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

63 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

64 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

65 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

66 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

67 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

68 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

69 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

70 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

71 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

72 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

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

74 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

75 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