Mobile Robots Localization

Transcription

1 Mobile Robots Localization Institute for Software Technology 1

2 Today s Agenda Motivation for Localization Odometry Odometry Calibration Error Model 2

3 Robotics is Easy control behavior perception modelling domain model environment model information extraction raw data planning task cognition reasoning path planning navigation path execution actuator commands sensing acting environment/world 3

4 Motivation [Siegwart, Nourbakhsh, Scaramuzza, 2011, MIT Press] self-localization the most fundamental problem to providing a mobile robot with autonomous capabilities [Cox 1991] 4

5 RoboCup Four-Legged League 5

6 RoboCup Four-Legged League 6

7 Geometry Again we are looking for a transformation between a reference frame (the environment) a robot frame we use the following terms interchangeably transformation pose (position + orientation) location pose 2D space: 2D position + orientation (1 angle) 3D space: 3D position + orientation (quaternion, Euler angles) 7

8 Taxonomy of Localization Problems position tracking initial pose is known, only track the change of pose global localization the initial is unknown, the pose has to be estimated from scratch kidnaped robot problem robot is teleported without telling it passive/active localization passive: robot estimates its pose on the fly active: robot is able to set actions to improve its pose estimation dynamic versus static environment the environment may change over time single robot versus mutli robots 8

9 Why Localization is Hard? in general the pose cannot be directly estimated integration of local motions match against landmarks landmarks or observations can be ambiguous sensors have systematic/nonsystematic errors action execution is not completely deterministic 9

10 General Concept global position information e.g. GPS position update robot-mounted local motion sensors e.g. encoder, IMU, prediction of the position change e.g. odometry environment knowledge e.g. map, landmarks position update local observations e.g. vision, laser scanner 10

11 Dead Reckoning/Odometry simple solution for position tracking or pose prediction Odometry in general for wheeled robots use wheel encoder to estimate change of pose Dead Reckoning historic term also used for marine application means the combination of actual pose, heading, speed and time in robotics use a heading sensor, e.g. compass or IMU [Wikipedia] 11

12 we assume Ideal and Continuous Time a differential drive robot a function over time for robot s linear and rotational velocity, v t, ω(t) ideal execution continuous time then we can simply integrate the functions in two steps to get the pose t θ t = ω τ dτ 0 t 0 t 0 + θ 0 x t = v τ cos θ(t) dτ + x 0 y t = v τ sin θ(t) dτ + y 0 12

13 Example Constant Velocities constant functions: v t = v ω t = ω assume x 0 = y 0 = θ 0 = 0 lead to a circle with radius v ω x t = v sin ωt ω y t = v v cos ωt ω ω θ t = ωt 13

14 Differential Drive Odometry in reality we cannot rely on motion execution solution: measure the motion equip both wheel with a wheel encoder the odometry sensor delivers the traveled distance of the left and right wheel during the last sampling period: Δs l, Δs r measure the distances for a small time interval assume the robot drives on a circle during the interval 14

15 if we have discrete time Discrete Time we assume equidistance time steps t 0 = 0, t i+1 = t i + ΔT we assume piecewise constant functions for robot travels on a constant arc between time steps we discretely update the poses x i+1 = x i v ω sin θ i + v ω sin θ i + ωδt y i+1 = y i + v ω cos θ i v ω cos θ i + ωδt θ i+1 = θ i + ωδt 15

16 y Motion Model I ICR y 1 y 0 Δy θ 0 = 0 Δx x 0 x 1 x 16

17 Motion Model II the average travel distance of the robot is Δs = Δs l+δs r 2 the change of orientation is Δθ = Δs r Δs l b if the distances are small we can assume Δd Δs then the pose update is x t = f x t 1, u t = x t 1 y t 1 θ t 1 + Δs cos Δs sin θ t 1 + Δθ 2 θ t 1 + Δθ 2 Δθ 17

18 Relation to Transformations odometry can also be represented as transformation from a odometry coordinate system in the world to a fixed robot-centric coordinate system ROS provides odometry as 3D transformation between the frames odom and base_link, rotation is represented as quaternion, usually z, roll and pitch assumed 0 2D example A t 1,t = cosδθ sinδθ Δs cos θ t 1 + Δθ 2 sinδθ cosδθ Δs sin θ t 1 + Δθ discrete transformations can be easily combined A t 2,t = A t 2,t 1 A t 1,t 18

19 Integration of Odometry odometry information is only available at distinct time steps we integrate the information to estimate the pose we make an additional error due to integration errors accumulate boundless t 0 t 1 t 2 t 3 t 4 19

20 Systematic Errors in Odometry can be determined and corrected in general sources unequal wheel diameters actual diameter differs from nominal diameter actual wheelbase differs from nominal wheelbase misaligned wheels finite encoder resolution finite encoder sampling rate 20

21 Non-Systematic Errors in Odometry stochastic in their nature can not be corrected can be probabilistically modelled sources travel over uneven floor travel over unexpected obstacles on the floor wheel slippage slippery floor to high acceleration internal forces non-point contact of wheel, e.g. tracks instead of wheels 21

22 Reasons for Odometry Errors ideal case different wheel diameters bump and many more carpet 22 22

23 Calibration of Odometry deals with systematic errors determine correction factors to minimize the error classical approach UMBmark (square path experiment) [Borenstein, Feng 1996] for differential drive robots mainly 23

24 Two Major Types of Errors Type A uncertain wheelbase E b = b actual bnominal leads to an incorrect estimation of orientation b actual b nominal 24

25 Two Major Types of Errors Type B unequal wheel diameters E d = D R DL leads to an incremental orientation error only relative not absolute in respect to the diameter L 25

26 UMBmark I standard test to calibrate odometry 1. determine absolute initial position x 0, y 0 of the robot, reset odomety 2. drive the robot trough a 4mx4m square path in CW direction a) stop after a 4m straight line b) turn 90 on the spot c) drive slowly 3. record the absolute end position x abs, y abs and the odometry (x 4, y 4 ) 4. repeat 1-3 n times, usually n = 5 repeat the procedure CCW [Borenstein, Feng 1996] 26

27 UMBmark II the procedure leads to n positions differences for each CW and CCW ε x = x abs x odo ε y = y abs y odo the repetition allows to limit the influence of nonsystematic errors in the procedure the position usually form two clusters: x c.g.,cw/ccw = 1 n y c.g.,cw/ccw = 1 n n i=1 n i=1 εx i,cw/ccw εy i,cw/ccw [Borenstein, Feng 1996] 27

28 Results for Type A Errors Only consider first a robot only affected by type A errors assume the initial position (0,0) ε x, ε y simply becomes x c.g. respectively y c.g. approximate trigonometry for small angles: Lsinγ Lγ, Lcos γ L CCW CW x 4 2Lα y 4 2Lα x 4 = 2Lα y 4 = 2Lα [Borenstein, Feng 1996] 28

29 Results for Type B Error Only consider first a robot only affected by type B errors CCW CW x 4 2Lβ y 4 2Lβ x 4 = 2Lβ y 4 = 2Lβ [Borenstein, Feng 1996] 29

30 Determine the Angles superimpose the results for the individual errors x, CW: 2Lα 2Lβ = 2L α + β = x c.g,cw x, CCW: 2Lα + 2Lβ = 2L α β = x c.g,ccw y, CW: 2Lα 2Lβ = 2L α + β = y c.g,cw y, CCW: 2Lα 2Lβ = 2L α + β = y c.g,ccw calculate the angles by combining β = x c.g.,cw x c.g.,ccw 4L β = y c.g.,cw+y c.g.,ccw 4L α = x c.g.,cw+x c.g.,ccw 4L α = y c.g.,cw y c.g.,ccw 4L 30

31 Determine the Correction Factors for E d determine the ratio of wheel diameters E d determine the radius of the curve traveled by the robot on the straight segment R = L 2 sin β 2 determine actual ratio E d = D R D L = R+b 2 R b 2 correct the odometry assume the average wheel diameter D a = D L+D R 2 stays constant solving a system of two equations: D L = 2 E d +1 D a, D R = 2 1 E d +1 D a 31

32 Determine the Correction Factors for E b determine the ratio for the actual wheelbase E b the wheelbase is inversely proportion to the amount of rotation we can state the following proportion b_actual π 2 = b nominal π 2 α determine actual ratio E b = π 2 π 2 α correct the odometry b actual = π 2 π 2 α b nominal 32

33 Description of Random Errors modeling of the nonsystematic (random) error is important for consistent perception simple solution: treat the observation/estimation m as normal distributed random variable m N μ m, σ m PDF m = if probability density function (PDF) is not Gaussian other means have to be used, e.g. sample-based σ m 1 2π e (m μ m ) 2 2σ m 2 P a < m b = PDF x dx a b 33

34 Nonlinear Error Propagation question: how is the error propagated by a nonlinear function f assume vectors of normal distributed random variables, x N μ x, Σ x, y N μ y, Σ y, Σ covariance matrix assume function f as vector of functions y i = f i (x) use the Taylor-approximation of f [Siegwart, Nourbakhsh, Scaramuzza, 2011, MIT Press] 34

35 Nonlinear Error Propagation Taylor-approximation of error propagation μ y = f μ x Σ y = F x Σ x F x T F x = f = x f(x) T T = f 1 f m = x 1 x n f 1 f 1 x 1 n f m f m x 1 x n F x Jacobean Matrix 35

36 Differential Drive y x = x y θ x t = x t 1 + Δx Δy Δθ = f(x t 1, u t ) x [Siegwart, Nourbakhsh, Scaramuzza, 2011, MIT Press] 36

37 Differential Drive Odometry the odometry sensor delivers the traveled distance of the left and right wheel during the last sampling period: Δs l, Δs r the average travel distance of the robot is Δs = Δs l+δs r 2 the change of orientation is Δθ = Δs r Δs l b the pose update is x t = f x t 1, u t = x t 1 y t 1 θ t 1 + Δs cos Δs sin θ t 1 + Δθ 2 θ t 1 + Δθ 2 Δθ 37

38 Error Model I we treat the pose x as normal distributed vector of random variables x N μ x, Σ x we assume a covariance matrix to represent the nonsystematic errors of Δs l, Δs r Σ Δ = k r Δs r 0 0 k l Δs l the errors of Δs l and Δs r assumed as independent the error is proportional the distance, k r and k l are error constants using Taylor-approximation we get the following error propagation Σ xt = F xt 1 Σ x t 1 F x t 1 T + F Δs Σ Δ F Δs T 38

39 Error Model II we use the following Jacobean matrices: F xt 1 = f xt 1 = f x F Δ s = Δ s f = f Δ r cos θ + Δθ 2 sin θ + Δθ 2 f Δ l = f y Δ s Δθ sin θ + 2b 2 Δθ cos θ + 2b 2 + Δ s 1 b 1 0 Δ f s sin θ + θ 2 θ = 0 1 Δ s cos θ + θ cos θ + Δθ 2 sin θ + Δθ 2 + Δ s Δθ sin θ + 2b 2 Δ s Δθ cos θ + 2b 2 1 b 39

40 Growth of Uncertainty Straight Line [Siegwart, Nourbakhsh, Scaramuzza, 2011, MIT Press] errors perpendicular to the direction of movement are growing much faster! 40

41 Growth of Uncertainty Circle [Siegwart, Nourbakhsh, Scaramuzza, 2011, MIT Press] errors ellipse does not remain perpendicular to the direction of movement! 41

42 Non-Gaussian Error Model [Fox, Thrun, Burgard, Dellaert, 2000] errors are not shaped like ellipses! use non-parametric representations, e.g. particles 42

43 Questions? Thank you! 43