Autonomous Mobile Robot Design

Transcription

1 Autonomous Mobile Robot Design Topic: EKF-based SLAM Dr. Kostas Alexis (CSE)

2 These slides have partially relied on the course of C. Stachniss, Robot Mapping - WS 2013/14 Autonomous Robot Challenges Where am I? What is my environment?

3 Simultaneous Localization and Mapping Building a map and locate the robot in the map at the same time Chicken or Egg problem Map Localize

4 Definition of the SLAM Problem Given: The robot s controls Observations Wanted: Map of the environment Path of the robot

5 Recall: The Bayes filter concept For each time step, do: Apply motion model: Apply sensor model: η is a normalization factor to ensure that the probability is maximum 1.

6 EKF for Online SLAM The EKF provides a solution to the online SLAM problem, i.e.: Find the latest pose of the robot

7 Recall: The Extended Kalman Filter

8 Recall: The Extended Kalman Filter Prediction Step Correction Step

9 Recall: The Extended Kalman Filter EKF works for nonlinear models Prediction Step Correction Step

10 Recall: The Extended Kalman Filter Process noise Prediction Step Correction Step

11 Recall: The Extended Kalman Filter Prediction Step Kalman Gain: how certain the robot for its predicted belief and its measurements? Correction Step

12 Recall: The Extended Kalman Filter Prediction Step Sensor noise Correction Step

13 Recall: The Extended Kalman Filter Prediction Step Correction Step

14 Recall: The Extended Kalman Filter Prediction Step Correction Step

15 EKF SLAM concept Application of the EKF to SLAM Estimate robot s pose and location of features in the environment Assumption: feature correspondence is known State space parametrization:

16 Assume a simple 3DoF robot The pose of a simplified robot in the 2D configuration space is defined by its x-y coordinates and the heading angle θ. The heading angle of the robot affects its dynamic trajectory in the x-y space. Let us defined the rotation matrix:

17 EKF SLAM: State representation Map with n landmarks: (3+2n)-dimensional Gaussian Belief is represented through the covariance matrix

18 EKF SLAM: State representation Compact representation of the covariance matrix

19 EKF SLAM: State representation More compactly (and simplifying notation as ):

20 EKF SLAM: Filter Cycle 1. State Prediction 2. Measurement Prediction 3. Measurement 4. Data Association 5. Update

21 EKF SLAM: State Prediction

22 EKF SLAM: State Prediction

23 EKF SLAM: Measurement Prediction

24 EKF SLAM: Obtained Measurement

25 EKF SLAM: Data Association data association and distance calculation

26 EKF SLAM: Update Step

27 EKF SLAM: Update Step

28 EKF-SLAM: Comprehensive Example Setup: Robot moves in the 2D plane Velocity-based motion model Robot observes point landmarks (x,y) Range-bearing sensor Known data association Known-fixed number of landmarks

29 Initialization Robot starts in its own reference coordinate frame (all landmarks unknown) 2N+3 dimensions

30 Initialization Robot starts in its own reference coordinate frame (all landmarks unknown) 2N+3 dimensions

31 Initialization Robot starts in its own reference coordinate frame (all landmarks unknown) 2N+3 dimensions

32 EKF Algorithm

33 Prediction Step (motion) Goal: Update state space based on the robot s motion (Velocity-based) Robot motion on the plane

34 Prediction Step (motion) Goal: Update state space based on the robot s motion (Velocity-based) Robot motion on the plane How can this be mapped to the 2N+3 dimensional space?

35 Update the State Space From the motion on the plane To the 2N+3 dimensional space

36 EKF Algorithm Done!

37 Update Covariance The function g only affects the robot s motion and not the landmarks Jacobian of the motion (3x3) Identity (2Nx2N)

38 Update Covariance The function g only affects the robot s motion and not the landmarks Jacobian of the motion (3x3) Identity (2Nx2N) The landmark positions don t change during the prediction step

39 Jacobian of the Motion Calculate the Jacobian: no dependency on x and y (velocity-based model)

40 Jacobian of the Motion Calculate the Jacobian: no dependency on x and y (velocity-based model)

41 Then this moves us to the update Done!

42 Then this moves us to the update Done! not updated during the prediction step

43 EKF Algorithm Done! Done!

44 EKF SLAM - Prediction Maps from the low-dim space to the high-dim space

45 EKF Algorithm Done! Apply & Done!

46 Indexing of measurements EKF SLAM - Correction Known data association : i-th measurement observes the landmark with index j Initialize landmark if unobserved Compute the expected observation Compute the Jacobian of h Then, proceed with computing the Kalman Gain

47 Range-Bearing Observation Range-bearing observation If landmark has not been observed observed location of landmark j estimated robot s location relative measurement

48 Expected Observation Compute expected observation according to the current estimate Where the robot expects to see the landmark

49 Jacobian of the Observation Based on: Compute the Jacobian: low-dim space

50 Jacobian of the Observation Based on: Compute the Jacobian: low-dim space

51 Jacobian of the Observation Use the computed Jacobian: Map it to the high dimensional space

52 Next steps Done! Done!

53 Next steps Done! Done! Apply & Done Apply & Done Apply & Done

54 EKF SLAM Correction [1/2]

55 EKF SLAM Correction [2/2]

56 Implementation Notes Measurement update in a single step requires only one full belief update Always normalize the angular components

57

58 almost

59 Loop Closing Recognizing an already mapped area Data association with High ambiguity Possible environmental symmetries Uncertainties collapse after a loop closure (even if the loop closure was not correct )

60 Before the Loop Closure

61 After the Loop Closure

62 Code Examples and Tasks Conduct Camera Calibration MATLAB: /localization-mapping/ekf-mono-slam

63 How does this apply to my project? To estimate the pose of the robot and the map within the environment that it navigates

64 Find out more ionfornavigation_newman06.pdf /SLAM%20course.pdf Course of C. Stachniss, Robot Mapping - WS 2013/14

65 Thank you! Please ask your question!