Tài liệu Leader follower formation control using on board sensors in noisy environment

Ph.D. Thesis LEADER-FOLLOWER FORMATION CONTROL USING ON-BOARD SENSORS IN NOISY ENVIRONMENT The Graduate School YEUNGNAM UNIVERSITY Department of Electrical Engineering Robotics and Control Major TRAN VIET HONG Advisor: Professor Lee Suk Gyu December 2010 Ph.D. Thesis LEADER-FOLLOWER FORMATION CONTROL USING ON-BOARD SENSORS IN NOISY ENVIRONMENT Advisor: Professor Lee Suk Gyu Presented in Partial Fulfillment of the Requirements for the Degree of Doctor of Engineering December 2010 The Graduate School of Yeungnam University Department of Electrical Engineering Robotics and Control Major TRAN VIET HONG Tran Viet Hong’s Ph.D. Thesis is approved by Committee members Professor Lee, Ki Dong Professor Lee, Suk Gyu Professor Lee, Hai Young Professor Park, Ju Hyun Professor Lee, Jeh Won December 2010 The Graduate School of Yeungnam University Acknowledgments To be accepted for a PhD position at Robotics and Control Laboratory, Department of Electrical Engineering, Yeungnam University under the supervision of Professor Lee Suk-Gyu is like a fate. Right at the first meeting with him, I felt that this is a good chance for my career. After four years, I have had a happy time and learnt a lot. I have even more experience of a new academic life and a new culture when following PhD course abroad. Looking back on the time that I spent to study and finish this thesis, I must admit that I enjoyed doing this research very much, even though it was not easy. I am glad to recall many wonderful people who accompanied me in that tough road to assist me in various ways. I cannot reach this point by only myself. First and foremost, my very special thank gives to Professor Lee Suk-Gyu for giving me a chance to do research on simultaneous localization and mapping, and multi-robot system. His expertise, motivation, enthusiasm, understanding, and patience, taken together, make him a great mentor. Thank you for directing me through my research and for all your help during my stay. Many thanks go to Professors at Yeungnam University, in general, and Department of Electrical Engineering, in particular, who gave me valuable lectures and advices. Especially, I would like to express my sincere appreciation to Professor Noh Seok-Kyun, my wife’s supervisor, for his valuable and numerous help to our living in Korea. I would like to thank the members of my thesis committee, Professor Lee Ki-Dong, Professor Lee Hai-Young, Professor Jessie Park Ju-Hyun and Professor Lee Jeh-Won to kindly for their time, interest, and helpful suggestions and comments. I consider myself fortunate to be with all of my past and present colleagues in Robotics and Control Laboratory and other laboratories such as Wee Sung-Gil, - iv - Park Je-Yong, Kim Jong-Uk, Joo Jin-Hwan, Lee Ho-Geun, Dilshat Saitov, Choi Kyung-Sik, Choi Yun-Won, Kim Kyung-Dong, Ryu Hee-Rack, Im Sung-Gyu, Jo Young-rae, Qu Xiaochuan, Xu Zhiguang, Dai YanYan, Liu Fenggang, Lee Tae-Hee, Kim Hon-Hee, and too many others to put all your names here. With great appreciation, I shall acknowledge Hochiminh city University of Technology (Vietnam National University, Hochiminh city), Faculty of Mechanical Engineering for the permission to study abroad. I will never forget four years of living with a solidary and affectionate community of Vietnamese students in Yeungnam University. You helped me to overcome the difficulties of living abroad. Thanks also go to Vietnamese students in Korea for kind help, encouragement, friendship, and happy times. And last but not least, I deeply thank Doctor Park Jung-Tae and Doctor Park Jin-Wook for your patience to take care of the health for my family. Mommy and daddy, please receive my gratitude for sacrificing your lives for us, and providing unconditional love and care. Viet Hung, my brother, is a wonderful model of scientific passion that gives me more self-confidence. It is lucky to have Nhon, my sister-in-law, here with us. Her help is invaluable. I am also very grateful to my wife’s family who have dealt with my personal issues in Vietnam, and encouraged me constantly. Finally, my wife is one extraordinary person deserving most of the acknowledgements. She is always right beside me with listening ears, loving smiles and gives me the feeling of warmth, hope and peace. Especially, she is spending the hard time in Vietnam to prepare for delivering our first baby. December 2010 Tran Viet Hong Yeungnam University, South Korea. -v- Contents Acknowledgments iv Contents vi List of tables viii List of Figures ix List of Abbreviations xii Abstract xiii CHAPTER 1 Introduction 14 1.1. Problem overview ..................................................................................... 14 1.2. Contributions and outline of the thesis ...................................................... 18 CHAPTER 2 Formation and Formation Control 20 2.1. Introduction to formation .......................................................................... 20 2.1.1. Applications of formation............................................................. 21 2.2. Introduction to formation control .............................................................. 25 2.2.1. Formation control structures......................................................... 26 2.2.2. Formation control approaches ...................................................... 29 2.2.3. A leader-follower formation example and basic tasks to be controlled ...................................................................................... 32 2.3. Motivation ................................................................................................. 34 CHAPTER 3 Stable On-board Sensor Based Formation Control in the Presence of Obstacles 38 3.1. Problem Statement .................................................................................... 39 3.1.2. Robot model ................................................................................. 40 3.1.3. Formation control framework for SLSF scheme .......................... 41 3.2. Proposed Control ....................................................................................... 43 3.2.1. Formation control framework for TLSF scheme .......................... 43 3.2.2. Proposed control law .................................................................... 45 - vi - 3.3. Obstacle Avoidance Algorithm .................................................................. 49 3.3.2. Flowchart ...................................................................................... 51 3.3.3. Choose new desired position ........................................................ 53 3.3.4. Stability of obstacle avoidance algorithm .................................... 54 3.4. Simulations and Analysis .......................................................................... 54 3.4.1. First simulation: small-scale robot team, merits of TLSF scheme .......................................................................................... 55 3.4.2. Second simulation: big-scale robot team, merits of TLSF scheme .......................................................................................... 61 3.4.3. Third simulation – formation switching ....................................... 63 3.4.4. Fourth simulation – single obstacle .............................................. 65 3.4.5. Fifth simulation – multiple obstacles, schemes switching ........... 68 3.5. Summary and Possible Extensions ............................................................ 70 CHAPTER 4 Wavelet-based Methods to Enhance Sonar Measurement 72 4.1. Introduction ............................................................................................... 73 4.2. Related works ............................................................................................ 75 4.2.1. Direct Cross Correlation (CC) ...................................................... 75 4.2.2. Generalized Cross Correlation (GCC).......................................... 76 4.2.3. Wavelet-based Generalized Cross Correlation ............................. 77 4.3. Enhanced Wavelet-based Methods ............................................................ 79 4.3.2. Improved Wavelet Pre-filter GCC (IWP-GCC) ........................... 80 4.3.3. Improved Wavelet-domain Inner Product GCC (IWDIP) ............ 82 4.3.4. Computational complexity comparison ........................................ 85 4.4. Simulation Results..................................................................................... 87 4.4.1. Performance analysis .................................................................... 87 4.4.2. Application in formation control .................................................. 91 4.5. Summary and Possible Extensions ............................................................ 97 CHAPTER 5 Conclusions and Future Research 99 5.1. Summary of contributions ....................................................................... 100 5.2. Future research directions ....................................................................... 101 Bibliography 103 Appendix A Calculation of ϕkm 115 Appendix B Proof of Lyapunov stability 117 - vii - List of tables Table 3.1 Parameters of the first simulation 55 Table 3.2 Displacement errors of control law [GH08] 61 Table 3.3 Displacement errors of control law (3.14) 61 Table 3.4 Parameters of the third simulation 63 Table 3.5 Displacement errors of control law [GH08] in third simulation 65 Displacement errors of control law (3.14) in third simulation 65 Table 3.7 Parameters of the fourth simulation for 4-robot team 67 Table 3.8 Parameters of the fifth simulation 68 Table 4.1 Computational time using Matlab 91 Table 4.2 Error range of each method when SNR < –40dB 92 Table 4.3 Displacement errors of control law (3.14) at various error levels 95 Table 3.6 Table 4.4 Relation between increment of std - viii - ∆d dk 0 and increment of ed 96 List of Figures Figure 1.1 Examples of biological systems exhibiting cooperative behaviors 15 Figure 1.2 Applications of formation control 16 Figure 1.3 Block diagram of a mobile robot 17 Figure 1.4 Outline of the thesis in corresponding with block diagram of the robot 18 Figure 2.1 Formation of UGVs working in a terrain and a field 22 Figure 2.2 Some applications of formation of UAVs and satellites 24 Figure 2.3 Two applications of formation on and under water 25 Figure 2.4 Centralized and decentralized structures 26 Figure 2.5 Motion and formation process of a group of robots with three basic tasks: forming, maintaining, and obstacle avoiding. 33 (a) SLSF scheme in diamond formation (b) SLSF scheme in zigzag formation (c) TLSF scheme in diamond formation of a four-robot team. 40 Figure 3.2 SLSF scheme 41 Figure 3.3 TLSF scheme 43 Figure 3.4 TLSF scheme with detailed information 45 Figure 3.5 An example of TLSF scheme in obstacle avoidance with one obstacle 50 An example of TLSF scheme in obstacle avoidance with two obstacles 50 Figure 3.1 Figure 3.6 - ix - Figure 3.7 TLSF scheme control without obstacle avoidance. 52 Figure 3.8 TLSF scheme control with obstacle avoidance algorithm. 52 Figure 3.9 Choose new desired position for follower robot to avoid obstacle 53 Figure 3.10 Performance of (a) control law [GH08] and (b) control law (3.14). 57 Figure 3.11 58 Trajectory seen from leader robot R0 of (a) R1 and (b) R2. Figure 3.12 Relative distance over time between (b) R2 and R0. (a) R1 and R0 and 59 Figure 3.13 Relative bearing angle over time between (a) R1 and R0, and (b) R2 and R0. 60 Figure 3.14 Performance of a team of 5 robots using (a) the control law [GH08] and (b) the control law (3.14) in the TLSF scheme. 62 Figure 3.15 Performance of a team of 3 robots in switching from a triangular formation to a line formation using (a) control law [GH08] and (b) control law (3.14) 64 Figure 3.16 Robot team avoids a single obstacle when switching from formation ΩC to ΩD 66 Figure 3.17 Robot team avoids a single obstacle in maintaining formation ΩE. 67 Figure 3.18 Robot team avoids obstacles without changing role of local leaders (formation ΩF). 69 Figure 3.19 Robot team avoids obstacles with changing roles (formation ΩF before the first obstacle and formation ΩG after the first obstacle). 70 Figure 4.1 Direct cross correlator configuration 75 Figure 4.2 A generalized cross correlator configuration [AH84] 76 -x- Figure 4.3 Wavelet Pre-filter GCC configuration 77 Figure 4.4 Wavelet-domain inner product GCC (WDIP) configuration 78 Figure 4.5 Denoise and recognition comparison. 80 Figure 4.6 Original WP-GCC process 80 Figure 4.7 Improved WP-GCC process (a) Delay prediction in the wavelet domain (b) Calculate the delay by the cross correlation in the time domain 82 Improved WDIP process (a) Delay time calculation (b) Optimal cross correlation 84 Block diagram of the simulation process 87 Figure 4.8 Figure 4.9 Figure 4.10 The transmitted signal 88 Figure 4.11 89 The received signal at SNR = –10 dB Figure 4.12 The delay error rate versus the SNR 90 Figure 4.13 Trajectories of 5 robots when ed = 42% 92 Figure 4.14 Trajectory of robot R2 when no error, ed = 30% and ed = 42% 93 Figure 4.15 Trajectory of robot R3 when no error, ed = 30% and ed = 42% 93 Figure 4.16 Trajectory of robot R2 in R0 coordinate when no error, ed = 30% and ed = 42% 94 Figure 4.17 Trajectory of robot R3 in R0 coordinate when no error, ed = 30% and ed = 42% 94 - xi - List of Abbreviations CC Cross Correlation DSP Digital Signal Processing DWT Discrete Wavelet Transform EERUF Error Eliminating Rapid Ultra-sonic Firing FFT Fast Fourier transform GCC Generalized Cross Correlation IWDIP Improved Wavelet-Domain Inner Product GCC IWP-GCC Improved Wavelet Pre-filter GCC PHAT Phase Transform SCOT Smoothed Coherent Transform SLSF Single Leader – Single Follower SNR Signal-to-Noise Ratio Sym8 Symplet whose vanishing moment is 8 TLSF Two Leaders – Single Follower UAV Unmanned Air Vehicle UGV Unmanned Ground Vehicle UUV Unmanned Underwater Vehicle WDIP Wavelet-Domain Inner Product GCC WP-GCC Wavelet Pre-filter GCC - xii - Abstract This thesis addresses three problems of leader-following formation control for multiple non-holonomic mobile robot system: stable control using only on-board sensors, obstacles avoidance, and noise’s effect reduction. Specifically, via kinematic analysis, we estimate the leaders’ translational and angular accelerations to build a stable controller whose inputs are only distance and angle information acquired from on-board sensors (do not need to measure velocities of leader robot). In addition, the controller is common for both single leader – single follower (SLSF) and two leaders – single follower (TLSF) schemes in order to have an ability of flexible switching between those schemes. Taking full advantage of this ability, we also extend the function of the controller by an obstacle avoidance algorithm to help the formation overcome harassment of static obstacles in the environment. Moreover, because of high error from distance measurement by using ultrasonic sensor, the stability property is not enough for disturbance rejection. Therefore, we present two enhanced wavelet-based method as a supplement to ability of reducing effect of noise. Although, the controller is such multi-functional and effective, it is still simple for quick processing, so that the time delay is kept small. Theoretical and simulation analysis show that all the functions of the controller work very well and rapidly. The controller can work with any scale of the robot team, but it shows an advantage in large scale where TLSF scheme can suppress the oscillation and damping and increase convergence rate of third, fourth, and succeeding follower robots. Even in the presence of obstacles, the formation is kept as close as required form and reform when there is no obstacle. In noisy environment, although the effect of noise is not able to be fully rejected, even a small measurement error decrement is valuable to improve the performance of formation control. - xiii - CHAPTER 1 Introduction 1.1. Problem overview Several new robotics application areas, such as underwater and space exploration, hazardous environments, service robotics in both public and private domains, the entertainment field, and so forth, can benefit from the use of multi-robot systems. In these challenging application domains, multi-robot systems can often deal with tasks that are difficult, if not impossible, to be accomplished by an individual robot. A team of robots may provide redundancy and contribute cooperatively to solve the assigned task, or they may perform the assigned task in a more reliable, faster, or cheaper way beyond what is possible with single robots. For instance, • it is usually more cost-effective to manufacture and deploy a number of cheap robots rather than a single expensive one • higher number yields better potential for a system resilient to individual robot failures - 14 - • smaller robots have obviously better mobility in tight and confined spaces, and • a group can survey a larger area than an individual robot, even if the latter is equipped with better sensors. The field of cooperative autonomous mobile robotics is still new enough that no topic area within this domain can be considered mature. Some areas have been explored more extensively, however, and the community is beginning to understand how to develop and control certain aspects of multi-robot teams [TEL02]. Therefore, nowadays control and coordination of multi-agent systems has emerged as a topic of major interest [LC+08]. This is partly due to broad applications of multi-agent systems in cooperative control of unmanned vehicles, formation control of swarms, where collective motions may emerge from groups of simple individuals through limited interactions. The world around us is teeming with examples of this emergent behavior, from a flock of birds to a school of fish, a herd of wildebeest to a swarm of locusts. In physics, a flock can be defined as the coherent motion of a group of self-propelled particles emerging from a single set of interactions between the constituents of that group. Some examples of biological systems exhibiting cooperative behaviors are shown in Fig. 1.1. Figure 1.1 Examples of biological systems exhibiting cooperative behaviors - 15 - Many swarm systems, such as flying wild geese, fighting soldiers, and robots performing a task, always form and maintain a certain kind of formation according to overlapping information structure constraints [XC08]. In practice, forming and maintaining desired formations would have great benefits for the system to perceive unknown or partially known environment, to perform its tasks. Some applications of formation control are shown in Fig. 1.2. Figure 1.2 Applications of formation control For its wide range of applicability, the formation control problem has stimulated a great deal of research in recent years. By formation control we simply mean the problem of controlling the relative position and orientation of the robots in a group while allowing the group to move as a whole. This thesis focuses on developing a formation controller for a team of mobile robots (wheeled mobile robots with non-holonomic constraints) moving in a 2D space. - 16 - The robot in the team has limited communications and uses only on-board sensor for sensing. The block diagram of a robot in this research is shown in Fig. 1.3. This means that the focus is on the formation task control only and is neither going into the details of the formation protocols for coordinating and organizing the grouped robots to accomplish the formation task, nor collision avoidance. On-board sensors Sensing the environment Sensing the leaders Obstacle avoiding controller Formation controller Controller Control signal Motors Figure 1.3 Block diagram of a mobile robot However, the formation control also considers avoiding static obstacles in environment. In addition, we also propose methods to improve the accuracy and calculation time for ultrasonic sensor measurement to provide the accurate sensing data and give them to the controller on time. In summary, the thesis deals with the problems in three blocks: on-board sensors, obstacle avoiding controller, and formation controller as shown in Fig. 1.3. - 17 - 1.2. Contributions and outline of the thesis As aforementioned in Section 1.1, the proposed methods to solve the problems in each block in Fig. 1.3 will be presented in each chapter. The correspondence between chapters and blocks are shown in Fig. 1.4. Chapter 4 On-board sensors Sensing the environment Sensing the leaders Chapter 3 Obstacle avoiding controller Formation controller Controller Control signal Motors Figure 1.4 Outline of the thesis in corresponding with block diagram of the robot In Chapter 2, an overview about formation and formation control is presented. Because the formation issue has been studied for a long time, there is a huge amount of information about it. This chapter tries to summarize it briefly, but still sufficiently, from general information such as the importance and applications, to specific information such as research directions in formation control, and up-to-date achievements. Chapter 3 deals with two scenarios. In the first scenario, the robot team is assumed to keep a formation in an - 18 - obstacle-free environment by leader-follower scheme. Every robot has limited communication with other robots in team, and uses only on-board sensor to sense the environment. Due to those limitations, it is very difficult for the robot to measure velocity of its leader robot. A TLSF scheme observer-based controller is proposed to overcome this difficulty by approximation of both translational and angular accelerations of the leader robot via kinematic analysis which have not used by any researcher in the literature. This control law stability is proved by Lyapunov stability theory. In the second scenario, the group of robots is considered to be able to meet static obstacles when moving. To take full advantage of TLSF scheme controller and to keep the easiness in application, an obstacle avoiding algorithm is added to the proposed formation control algorithm. The obstacle avoiding algorithm is simple, yet can keep the stability of the formation control algorithm and show good performance. In the above scenarios, there is no noise in the environment. Because the formation control is based mainly on the measurement from on-board sensors, and noise affects much to the accuracy of sensing data, the measurement must assure to be accurate, or the performance of the whole system will be decreased. In addition, while a fusion of sensors is often used, it is required that data should be processed and sent to the controller as quick as possible. This is like a chicken and egg problem. The more accurate and free of noise data have come, the more time to process is required. In Chapter 4, the improvement in accuracy and processing time of ultrasonic sensors will be considered. As an improvement over the existing literature, two wavelet-based methods using prediction technique are proposed to help ultrasonic sensors getting distance information quickly and precisely. In the last chapter, Chapter 5, major contributions of the thesis are summarized, and some possible directions for research in the future are highlighted. - 19 - CHAPTER 2 Formation and Formation Control 2.1. Introduction to formation Collective robotics studies the different ways of using autonomous robot teams to efficiently fulfill predefined missions. In collective robotics, several new problems have been lately introduced [RB08], such as: • consensus [CMA08, Mor05, OFM07, Tsi84] • rendezvous [AO+99, LMA07] • cyclic pursuit [MB08, MBF04, PF07, SG06] • coverage and deployment [CM+04, HMS02] • formation control [AY+08, DF08, FM04, OEH02] • connectivity/visibility maintenance [DK08, ME07, ZP08, SNB09]. Among them, formation has received a lot of attention. In formation problems, a team of mobile robots establish and maintain predetermined geometrical shapes by controlling the location of each robot relative to the group while allowing the group to move as a whole. Geometric formation can be - 20 -