ROBOT PATH PLANNING AND TRACKING OF A MOVING TARGET USING KALMAN FILTER Adya Shrotriya B.E., University of Pune, India, 2006 PROJECT Submitted in partial satisfaction of the requirements for the degree in MASTER OF SCIENCE in ELECTRICAL AND ELECTRONIC ENGINEERING at CALIFORNIA STATE UNIVERSITY, SACRAMENTO SPRING 2010 ROBOT PATH PLANNING AND TRACKING OF A MOVING TARGET USING KALMAN FILTER A Project by Adya Shrotriya Approved by: __________________________________, Committee Chair Dr. Fethi Belkhouche __________________________________, Second Reader Dr. Preetham B. Kumar ____________________________ Date ii Student: Adya Shrotriya I certify that this student has met the requirements for format contained in the university format manual and that this project is suitable for shelving in the library and credits to be rewarded for the Project. __________________________, Graduate Coordinator ________________ Dr. Preetham B. Kumar Date Department of Electrical and Electronic Engineering iii Abstract of ROBOT PATH PLANNING AND TRACKING OF A MOVING TARGET USING KALMAN FILTER by Adya Shrotriya This project develops a method for path planning and tracking of a moving target by a wheeled robot. The main kinematics variables updated in each step are the orientation and relative position of the robot and the target. A discrete extended Kalman filter is used to predict and update the states of the robot and the target and their uncertainties. The robot uses a linear proportional navigation law to track the target. Simulation of the motion kinematics of robot and the target is performed using MATLAB. It is shown using multiple simulation scenarios that the robot is able to track and reach the moving goal successfully. _______________________, Committee Chair Dr. Fethi Belkhouche _______________________ Date iv ACKNOWLEDGEMENT I would like to take this opportunity to thank the people behind successful completion of this project. First and foremost, I would like to thank my professor and mentor for this project, Dr. Fethi Belkhouche for his tremendous support and guidance throughout the project. I would like to thank him for always being there to help me in developing better understanding of the topic and encouraging me to give my best. Also, I am extremely grateful to my Graduate Coordinator, Dr. Preetham Kumar for his consistent support in my academic years at the School guiding me through the entire process, right from enrolling for a class until submission of my master’s project. Finally, I would like to thank my professors throughout my Master’s, my department and my school for giving me this opportunity to learn and grow towards attainment of this degree. Lastly, I would like to express my heartfelt gratitude to my family in India who have constantly supported me and blessed me. I am extremely thankful to my husband Puru, who helped me make my dreams come true. v TABLE OF CONTENTS Page Acknowledgement.………………………………………………………………………………...v List of figures…...………………………………………………………………………………..viii Chapter 1. INTRODUCTION .................................................................................................................... 1 2. ROBOT NAVIGATION FUNDAMENTALS ......................................................................... 3 Odometry .......................................................................................................................... 4 Probabilistic Estimation .................................................................................................... 7 3. KALMAN FILTER .................................................................................................................. 9 State Vector ..................................................................................................................... 10 Dynamic Model .............................................................................................................. 11 Observation Model .......................................................................................................... 12 Kalman Filtering ............................................................................................................. 15 Predict-Match-Update Process ........................................................................................ 19 4. KALMAN FILTER ALGORITHM........................................................................................ 21 Extended Kalman Filter .................................................................................................. 24 5. PREVIOUS RESEARCH IN IMPLEMENTATION OF KALMAN FILTERING ............... 27 Mathematical Foundations of Navigation and Perception for an Autonomous Mobile Robot [1] ........................................................................................................................... 27 Kalman Filtering for Positioning and Heading Control of Ships and Offshore Rigs [5] .. 28 Accurate Odometry and Error modeling for a Mobile Robot [10] ................................... 29 6. TRACKING–NAVIGATION OF A MOVING GOAL ......................................................... 30 Linear Navigation Law ..................................................................................................... 35 7. KALMAN FILTERING IMPLEMENTATION IN NAVIGATION/TRACKING ................ 37 A Kalman Filter Model for Odometric Position Estimation [1] ....................................... 40 8. SIMULATION RESULTS ..................................................................................................... 46 Implementation of Navigation law equations ................................................................... 46 Representation of Error Ellipses ....................................................................................... 48 Robot Target Path ............................................................................................................. 49 vi Covariance Distribution .................................................................................................... 50 Error Ellipses .................................................................................................................... 51 9. CONCLUSION ....................................................................................................................... 54 Appendix ........................................................................................................................................ 55 MATLAB Code Implementing Kalman Filter to show Error Ellipses ...................................... 55 MATLAB Code Implementing the Navigation Law ................................................................. 56 MATLAB Code Implementing Kalman Filter for Path Planning .............................................. 57 References ...................................................................................................................................... 61 vii LIST OF FIGURES Page 1. Growing error ellipses indicate growing position uncertainty with odometry . .................. 8 2. Kalman Filter diagram. ...................................................................................................... 10 3. A framework of dynamic world modeling . ...................................................................... 19 4. State space diagram for an odometric system . ................................................................. 30 5. Geometrical representation of robot/target positions ........................................................ 32 6. Probability density function for noises v1 and v2 ............................................................... 34 7. Probability density function for rGR and ΨGR ..................................................................... 34 8. Probability density function for positions xGR and yGR ...................................................... 35 9. Linear Navigation law simulation for constant c and variable N ...................................... 47 10. Linear Navigation law simulation for constant N and variable c ...................................... 47 11. Simulation results depicting uncertainties through error ellipses ...................................... 48 12. Robot Target path .............................................................................................................. 49 13. Covariance matrix distribution........................................................................................... 50 14. Error Ellipse for the first and the 15th step robot/target position ...................................... 51 viii 1 Chapter 1 INTRODUCTION 1 2 This project is concerned with the application of one of the most important techniques 3 from estimation theory to the problem of navigation and tracking for a mobile robot. In 4 this project, probabilistic estimation is done to predict the next step of the robot that 5 follows a moving target under uncertainty. Translation as well as orientation of the 6 moving target with respect to the global axis work as the reference for estimation of robot 7 position. Estimation of the position of the vehicle with respect to the external world is 8 fundamental to navigation. Modeling the contents of the immediate environment is 9 equally fundamental. Estimation theory provides a basic set of tools for position 10 estimation and environmental modeling. These tools provide an elegant and formally 11 sound method for combining internal and external sensor information from different 12 sources, operating at different