Lego NXT: Navigation and localization using infrared distance sensors and Extended Kalman Filter Miguel Pinto, A. Paulo Moreira, Aníbal Matos 1
Resume LegoFeup Localization Real and simulated scenarios LegoFeup Control Loop Trajectory Control module Landmark association module Observation module Estimation module Extended Kalman Filter Results Conclusions 2
Lego Feup Differential traction robot Cada motor possui um Encoder 2 infra-red sensors to measure distances 1 Brick/Controller with Java-machine for Lego Mindstorms 3
Lego Feup 4
Pose estimation based on odometry It leads to an unbounded growth in the localization error variance, due to: Slipping Wheel diameter changes with differences in floor and load on the robot The distance between wheels changes with the variation of the contact point between the wheels and the floor 5
Localization based on landmarks Landmarks must exist within the navigation area of the robot Whenever observations of these landmarks are available, the localization given by the odometry can be corrected An observation can be distance and angle relative to the landmarks 6
Deterministic or probabilistic localization? Deterministically, with 100% confidence and ignoring the sensors noise Most often it is not the right option because there are not noise-free sensors Using a probabilistic The localization is based on the probability of the robot to be in the estimated position Considers the probabilistic characterization of error in odometry (motion model) and sensors Extended Kalman Filter (EKF), Particulate filter, etc 7
Experiment description Lego Feup navigates describing a random or predefined path (for example a square) It has the ability to measure the distance and orientation relative to a white wall (artificial landmarks) With strategically placed landmarks the estimated pose can be corrected 8
Experiment description 9
Our Scenario 4 walls are placed in known positions, forming a square The desired trajectory for LegoFeup is a path in square The walls are placed at the beginning of each side of the square 10
Our Scenario (simulation) 11
Our real scenario 12
Exchange between the real scenario and the simulation Only changing the communication class (UDP simulation, real Bluetooth) Interface for visualizing the variables of the algorithm Algorithm code writen in JAVA Advantages of JAVA Object-oriented language, easy class exchange Runs on any machine with JDK 13
14
Control cycle of LegoFeup Trajectory control module Landmark association module Observation module, generated due to infrared sensors Estimation module using Extend Kalman Filter (EKF) 15
LegoFeup Loop 16
Trajectory control module GoXYTheta FollowLine Follow Circle FollowPath Inputs: Estimated x,y and theta Reference values for V and W Outputs: Reference velocity for each wheel: 17
Landmark association module After an observation is generated, you need to associate it with the correct wall 18
Observation IR sensor Emits infrared light to the obstacle and measures the reflected light angle Operation in outdoor environments is difficult The infrared sensor measures distances between 4 and 40 centimeters Non-linear sensor não 19
Observation IR sensors Better fitting to measured data 20
Observation IR sensors 21
Observation 22
Estimation module - EKF Non-linear models of kinematics and observation models are linearized (Taylor expansion) for updating the covariance Noise modeled as zero mean Gaussian noise (see chapt "Nonlinear Estimation [1]) EKF as two steps [2] 1) Predicting state and covariance of the robot (EKF prediction) 2) If there is a new observation, this is associated with the correct landmark and the robot state and covariance are updated (EKF update) 23
Estimation module - EKF 24
Odometry and Sensors errors Odometry errors are reflected in the distance traveled (d) and the variation in angle (ΔΘ), due by errors d1, d2 b: IR sharp sensors errors in the measured distances to the walls: 25
State Prediction Kinematics of the vehicle (relying on the error): The noise is considered with a Gaussian distribution (zero mean and covariance matrix equal to Q) 26
State Prediction Estimated covariance: In this step the covariance increases. 27
State Correction The correction is made whenever an observation is obtained:: W is the gain of the Kalman Filter, V is the innovation and R the error covariance of the measures: 28
Observations State Correction Measures ( Z = h(x,u,r) ) depend on the detected landmark: with: 29
Observations State Correction The gradientes of the measures Z = h(.), are: When the walls are: wallx+/- then C1=1, C2=0 wally+/- then C1=0, C2=1 with: 30
Observations State Correction The propagation of the state covariance matrix is given by: Covariance always decreases this step due to: 31
Covariância do erro na Odometria e nos Sensores Covariance Q of the odometry error depends on the distance traveled, d and Δϴ Covariance of the sharp IR sensor error is assumed constant in the range of 4 to 40 cm The filter tuning can be done by adjusting Q after appropriate modeling of R 32
Results 33
Results 34
Highlights on the results: The diagonal of the covariance matrix grows when the location is performed using only odometry The maximum values corresponded to the moment that a new wall is found, an observation is made and the estimated state corrected Once the EKF reaches steady state, then the covariance remains to oscillate between certain limits. The angle is corrected in all observations while x and y coordinate only on the correspondent walls. 35
References [1] A. Gelb, J. F. Kasper Jr., R. A. Nash Jr., C.F. Price and A.A. Sutherland Jr., Apllied Optimal Estimation,The MIT Press, Massachssets & London, 2001. [2] Maria Isabel Ribeiro, Kalman and Extended Kalman Filters: Concept, Derivation and Properties, Instituto de Sistemas e Robótica, Instituto Superio Técnico, Fevereiro de 2004. 36