Distributed Intelligent Systems W4 An Introduction to Localization Methods for Mobile Robots

Transcription

1 Distributed Intelligent Systems W4 An Introduction to Localization Methods for Mobile Robots 1

2 Outline Positioning systems Indoor Outdoor Robot localization using proprioceptive sensors without uncertainties Kinematic models and odometry Robot localization using proprioceptive sensors with uncertainties Error sources Methods for handling uncertainties Robot localization using proprioceptive and exteroceptive sensors Odometry-based and feature-based methods

3 Key task for: Path planning Mapping Referencing Coordination Type of localization Absolute coordinates Local coordinates Topological information Robot Localization? N E

4 Positioning Systems 4

5 Classification axes Indoor vs. outdoor techniques Absolute vs. relative positioning systems Line-of-sight vs. non-line-of-sight Underlying physical principle and channel Positioning available on-board vs. off-board Scalability in terms of number of nodes 5

6 Selected Indoor Positioning Systems Overhead cameras and Motion Capture Systems (MCSs) Impulse Radio Ultra Wide Band (IR-UWB) Infrared (IR) + RF technology 6

7 Overhead (Multi-)Camera Systems Tracking objects with one (or more) overhead cameras Absolute positions, available outside the robot/sensor Active, passive, or no markers Open-source software available (e.g., SwisTrack, developed at DISAL) Major issues: light, calibration Essentially D Performance 1 camera system Accuracy ~ 1 cm (D) Update rate ~ 0 Hz # agents ~ 100 Area ~ 10 m 7

8 6D Multi-Camera System Called also Motion Capture System (MCS) cameras mm accuracy Hz update ms latency 6D pose estimation 4-5 passive markers per object to be tracked needed 00 m^3 arena ETHZ coordinated ball: throwing UPenn GRASP: lab 8

9 IR-UWB System - Principles Impulse Radio Ultra-Wide Band Based on time-of-flight (TDOA, Time Difference of Arrival) UWB tags (emitters, a few cm, low-power) and multiple synchronized receivers Emitters can be unsynchronized but then dealing with interferences not trivial (e.g., Ubisense system synchronized) Absolute positions available on the receiving system Positioning information can be fed back to robots using a standard narrow-band channel 6-8 GHz central frequency Very large bandwidth (>0.5GHz) high material penetrability Fine time resolution high theoretical ranging accuracy (order of cm) 9

10 IR-UWB System Performances Accuracy 15 cm (3D) Ex. State-of-art system (e.g., Ubisense 7000 Series, Compact Tag) Update rate 34 Hz / tag # agents ~ Area ~ 1000 m Degraded accuracy performance if Inter-emitter interferences Non-Line-of-Sight (NLOS) bias Multi-path 10

11 Infrared + Radio Technology Principle: Belt of IR emitters (LED) and receivers (photodiode) IR LED used as antennas; modulated light (carrier 10.7 MHz), RF chip behind Range: measurement of the Received Signal Strength Intensity (RSSI) Bearing: signal correlation over multiple receivers Measure range & bearing can be coupled with standard RF channel (e.g ) for heading assessment Can also be used for 0 kbit/s IR com channel Robot ID communicated with the IR channel (ad hoc protocol) [Pugh et al., IEEE Trans. on Mechatronics, 009] 11

12 Infrared + Radio Technology Performance summary: Range: 3.5 m Update frequency 5 Hz with 10 neighboring robots (or 50 Hz with ) Accuracy range: <7% (MAX), generally decrease 1/d Accuracy bearing: < 9º (RMS) LOS method Possible extension in 3D, larger range (but more power) and better bearing accuracy with more photodiodes (e.g. Bergbreiter, PhD UCB 008, dedicated ASIC, up to 15 m, 56 photodiodes, single emitter with conic lense) 1

13 Selected Outdoor Positioning Techniques GPS Differential GPS (dgps) 13

14 Global Positioning System (image from Wikipedia) Note: the first and still most prominent example of GNSS systems (Global Navigation Satellite Systems) 14

15 Global Positioning System Initially 4 satellites (including three spares), 3 as of December 01, orbiting the earth every 1 hours at a height of km. Satellites synchronize their transmission (location + time stamp) so that signals are broadcasted at the same time (ground stations updating + atomic clocks on satellites) Location of any GPS receiver is determined through a time of flight measurement (ns accuracy!) Real time update of the exact location of the satellites: - monitoring the satellites from a number of widely distributed ground stations - a master station analyses all the measurements and transmits the actual position to each of the satellites Exact measurement of the time of flight the receiver correlates a pseudocode with the same code coming from the satellite the delay time for best correlation represents the time of flight. quartz clock on the GPS receivers are not very precise the range measurement with (at least) four satellites allows to identify the three values (x, y, z) for the position and the clock correction ΔT Recent commercial GPS receiver devices allows position accuracies down to a few meters with best satellite visibility conditions ms latency, so max 5 Hz GPS updates 15

16 dgps Position accuracy: typically from a few to a few tens of cm 16

17 Robot Localization using On-Board Sensors 17

18 Proprioceptive sensors: Epuck: Sensors for Localization 3D accelerometer Motor step counter Others: Wheel encoder Odometer IMU (inertial measurement unit) Exteroceptive sensors: Epuck: IR range proximity sensor Camera Others: Laser range finder Ultrasonic range finder 18

19 Using proprioceptive sensory data influenced by the movement of actuators to estimate change in pose over time Start: initial position Actuators: Legs Wheels Propeller Odometry Definition & Idea Sensors (proprioceptive): Wheel encoders (DC motors), step counters (stepper motors) Inertial measurement units, accelerometers Nervous systems, neural chains Idea: navigating a room with the light turned off 19

20 Example from Week 1 Example: Cataglyphis desert ant Excellent study by Prof. R. Wehner (University of Zuerich, Emeritus) Individual foraging strategy Underlying mechanisms Internal compass (polarization of sun light) Dead-reckoning (path integration on neural chains for leg control; note: in robotics typically using also heading sensors) Local search (around 1- m from the nest) Extremely accurate navigation: averaged error of a few tens of cm over 500 m path! 0

21 More examples Human in the dark Very bad odometry sensors d Odometry = O(1/m) (Nuclear) Submarine Very good odometry sensors d Odometry = O(1/10 3 km) Navigation system in tunnel uses dead reckoning based on Last velocity as measured by GPS Car s odometer, compass Picture: Courtesy of US Navy Picture: Courtesy of NavNGo 1

22 Odometry using Wheel Encoders

23 Optical Encoders Measure displacement (or speed) of the wheels Principle: mechanical light chopper consisting of photo-barriers (pair of light emitter and optical receiver) + pattern on a disc anchored to the motor shaft Quadrature encoder: 90º placement of complete photo-barriers, 4x increase resolution + direction of movement Integrate wheel movements to get an estimate of the position -> odometry Typical resolutions: increments per revolution. 3

24 Pose (Position and Orientation) of a Differential-Drive Robot y I Y I Y R y R x R x I P θ X R ξ I = xi yi θ X I xr ξ R = y R = R( θ ) ξi θ cosθ sinθ R( θ ) = sinθ cosθ 0 0 Orthogonal Rotation Matrix From Introduction to Autonomous Mobile Robots, Siegwart R. and Nourbakhsh I. R. 4

25 Absolute and Relative Motion of a Differential-Drive Robot = = θ θ θ I I I I R R x y y x y x X I Y I P X R Y R θ Ex. θ =π/ R R ξ I θ ξ ) ( = x I y I 5

26 Y I Forward Kinematic Model How does the robot move given the wheel speeds and geometry? Assumption: no wheel slip (rolling mode only)! In miniature robots no major dynamic effects due to low massto-power ratio l ω(t) 1 θ v(t) ξ r (wheel radius) x y θ I = = ϕ i f l, r, θ, ϕ 1, ϕ ) ( = wheel i speed X I 6

27 Recap ME/PHY Fundamentals Wheel Motion t-δt v r r P φ C t P ω ω v v Rolling! = ω r = r ϕ v = tangential speed ω = rotational speed r = rotation radius φ = rotation angle C = rotation center P = peripheral point P = contact point at time t 7

28 Forward Kinematic Model Linear speed = average wheel speed 1 and : r ϕ v = + r 1 ϕ Rotational speed = sum of rotation speeds (wheel 1 forward speed -> ω anticlockwise, wheel forward speed -> ω clockwise): ω = r ϕ 1 r ϕ + l l Y I l Y R ω P 1 r θ v X R X I Idea: linear superposition of individual wheel contributions 8

29 + + = l r l r r r I cos sin 0 sin cos 1 1 ϕ ϕ ϕ ϕ θ θ θ θ ξ I R ξ R θ ξ ) ( 1 = = 0 y R 1 ϕ ϕ r r v x R + = = l r l r ϕ 1 ϕ ω θ + = = Forward Kinematic Model Y I X I v θ ω l r 1 X R Y R P

30 Odometry Given our absolute pose over time, we can calculate the robot pose after some time t through integration Given the kinematic forward model, and assuming no slip on both wheels ξ T T I ( T ) = I + Idt = I Given an initial pose ξ I0, after time T, the pose of the vehicle will be ξ I (T) ξ I (T) computable with wheel speed 1, wheel speed, and parameters r and l R 1 ξ ξ ξ θ ξ 0 ( ) R dt 30

31 Localization Uncertainities in Odometry 31

32 Deterministic Error Sources Limited encoder resolution Wheel misalignment and small differences in wheel diameter Can be fixed by calibration 3

33 Non-Deterministic Error Sources From Week 3: no deterministic prediction possible we have to describe them probabilistically Example: accelerometer-based odometry MEMS-Based accelerometer (e.g., on e-puck) 33

34 Odometry in 1D 34

35 Odometry in 1D 34

36 Odometry in 1D 34

37 Odometry in 1D 34

38 Odometry in 1D 34

39 Odometry in 1D 34

40 Odometry in 1D 34

41 Odometry in 1D 34

42 Error happens! 1D Odometry: Error Modeling Odometry error is cumulative. grows without bound We need to be aware of it. We need to model odometry error. We need to model sensor error. Acceleration is random variable A drawn from mean-free Gaussian ( Normal ) distribution. Position X is random variable with Gaussian distribution. 35

43 1D Odometry with Gaussian Uncertainty 36

44 1D Odometry with Gaussian Uncertainty 36

45 1D Odometry with Gaussian Uncertainty 36

46 1D Odometry with Gaussian Uncertainty 36

47 1D Odometry with Gaussian Uncertainty 36

48 1D Odometry with Gaussian Uncertainty 36

49 Non-Deterministic Uncertainities in Odometry based on Wheel Encoders 37

50 Nondeterministic Error Sources Variation of the contact point of the wheel Unequal floor contact (e.g., wheel slip, nonplanar surface) Wheels cannot be assumed to roll perfectly Measured encoder values do not perfectly reflect the actual motion Pose error is cumulative and incrementally increases Probabilistic modeling for assessing quantitatively the error 38

51 Odometric Error Types Range error: sum of the wheel movements Turn error: difference of wheel motion Drift error: difference between wheel errors lead to heading error 39

52 Pose Variation During t = = = = = + = + = + = + = b s s b s s s s b s s s s y x s s y x p y x p y x p b s s s y s x s s s l r l r l r l r l r t t t l r l r ) sin( ) cos( ) / sin( ) / cos( ) sin( ) cos( θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ b = l = inter-wheel distance s r = traveled distance right wheel s l = traveled distance left wheel θ = orientation change of the vehicle 40

53 = = = ), cov( l r s s l l r r l r s k s k s s σ σ Noise modeling Model error in each dimension with a Gaussian Assumptions: Covariance matrix Σ p at the beginning is known Errors of the two individual wheels are independent Errors are independent of direction of motion Errors are proportional to the distance traveled (k r, k l model parameters) = Σ θθ θ θ θ θ σ σ σ σ σ σ σ σ σ y x y yy yx x xy xx p σ x σ y σ θ θ θ σ σ, ;, ;, y x y y x x x, y,θ σ θ 41

54 Actuator Noise Pose Noise How is the actuator noise (D) propagated to the pose (3D)? σ sr = 0 1D to 1D example 0 Σ σ s l x σ sr σ? s l N( µ s, σ s ) N( µ x, σ x ) r r σ x σ y σ θ σ σ σ xx yx θx σ σ σ xy yy θy σ σ σ xθ yθ θθ = Σ p µ x + σ x f ( s r ) µ x µ x σ x µ σ µ µ + σ r r sr s r s sr s We need to linearize Taylor Series 1 f 1 f x f ( sr ) f ( s ) + ( ) + ( ) = r sr µ s s s r r µ s r µ s r r 1! s! s r r s r + 4

55 Actuator Noise Pose Noise f1 x f1( sr ) f1( sr ) + ( sr µ s ) sr = µ r sr sr f1 x f1( sl ) f1( sl ) + ( sl µ s ) sl = µ l sl sl f y f( sr ) f( sr ) + ( sr µ s ) sr = µ r sr s r µ + xy σ xy µ xy µ xy xy σ xy f ( s ) 1 r l F rl f1 s f = s f3 s r r r f1 s f s f3 s General error propagation law l l l Jacobian µ s σ µ s r l l l r s s r r µ s + σ rl s lr = k r s 0 r k l 0 s l s lr Σ rl = F rl Σ F T rl 43

56 Actuator Noise Pose Noise How does the pose covariance evolve over time? Initial covariance of vehicle at t=0: Additional noise at each time step Δt: Covariance at t=1δt: Covariance at t=δt: T rl rl rl F F Σ = Σ = = Σ = ) ( θθ θ θ θ θ σ σ σ σ σ σ σ σ σ y x y yy yx x xy xx t p rl rl t p t t p = = = Σ + Σ = Σ Σ 0) ( ) 1 ( T rl rl T p t t p p t t p F F F F = = Σ + Σ = Σ ) 1 ( ) ( = θ θ θ f f f f f f f f f F y x y x y x p Σ p 44

57 Algorithm Actuator Noise Pose Noise Precompute: Determine actuator noise Compute mapping actuator-to-pose noise incremental Compute mapping actuator-to-pose noise absolute Initialize: Initialize Iterate: Σ ( 0) Σ t= p ( t= ( k+ 1) t) p = = [ 0] F p Σ Σ ( t= k t) p F T p + F rl Σ F T rl F p F rl 45

58 Classical D Representation Courtesy of R. Siegwart and R. Nourbakhsh Ellipses: typical 3σ bounds 46

59 Robot Localization based on Proprioceptive and Exteroceptive Sensors 47

60 Features Odometry based position error grows without bound. Use relative measurement to features ( landmarks ) to reduce position uncertainty Feature: Uniquely identifiable Position is known We can obtain relative measurements between robot and feature (usually angle or range). Examples: Doors, walls, corners, hand rails Buildings, trees, lanes GPS satellites Courtesy of Albert Huang 48

61 Automatic Feature Extraction High level features: Doors, persons Simple visual features: Edges (Canny Edge Detector 1983) Corner (Harris Corner Detector 1988) Scale Invariant Feature Transformation (004) Simple geometric features Lines Corners Binary feature Complexity Courtesy of R. Siegwart and R. Nourbakhsh 49

62 Feature-Based Navigation 50

63 Feature-Based Navigation 50

64 Feature-Based Navigation 50

65 Feature-Based Navigation 50

66 Given: Sensor Fusion Position estimate X N(μ=5;σ=1) Range estimate R N(μ=3.;σ=1.) What is the best estimate AFTER incorporating r? Kalman Filter Requires: Gaussian noise distribution for all measurements Linear motion and measurement model R N(μ=3.;σ=1.) X N(μ=5;σ=1) X N(μ=5.5;σ=0.6) Kalman Filter 51

67 Feature-Based Navigation 5

68 Feature-Based Navigation 5

69 Feature-Based Navigation 5

70 Feature-Based Navigation 5

71 Feature-Based Navigation Belief representation through Gaussian distribution Advantages: Compact (only mean and variance required) Continuous Powerful tools (Kalman Filter) Disadvantages: Requires Gaussian noise assumption Uni-modal Cannot represent ignorance ( kidnapped robot problem ) 53

72 Feature-Based Navigation 54

73 Feature-Based Navigation 54

74 Feature-Based Navigation 54

75 Feature-Based Navigation 54

76 Feature-Based Navigation 54

77 Feature-Based Navigation 54

78 Feature-Based Navigation 55

79 Feature-Based Navigation 55

80 Feature-Based Navigation 55

81 Feature-Based Navigation 55

82 Feature-Based Navigation 55

83 Feature-Based Navigation 55

84 Feature-Based Navigation 55

85 Feature-Based Navigation 56

86 Feature-Based Navigation 56

87 Feature-Based Navigation 56

88 Feature-Based Navigation 56

89 Feature-Based Navigation 56

90 Feature-Based Navigation 56

91 Feature-Based Navigation 56

92 Feature-Based Navigation Belief representation through particle distribution Advantages: Can model arbitrary beliefs No assumptions on noise characteristic Disadvantages: No unique solution Not continuous Computationally expensive Tuning required 57

93 Feature-Based Navigation 58

94 Feature-Based Navigation t= t=3 t=4 59

95 Conclusion 60

96 Take Home Messages There are several localization techniques for indoor and outdoor systems Each of the localization methods/positioning system has advantage and drawbacks Odometry is an absolute positioning method using only proprioceptive sensors but affected by a cumulative error Localization error can be modeled and estimated The error propagation methods/filtering techniques are tools applicable to a large variety of noisy problems, not just localization and navigation Feature-based navigation is a way to compensate odometry limitations 61

97 Additional Literature Week 4 Books Siegwart R., Nourbakhsh I. R., and Scaramuzza D., Introduction to Autonomous Mobile Robots, MIT Press, 011 ( nd edition). Choset H., Lynch K. M., Hutchinson S., Kantor G., Burgard W., Kavraki L., and Thrun S., Principles of Robot Motion. MIT Press, 005. Thrun S., Burgard W., and Fox D., Probabilistic Robotics, MIT Press,