

small (250x250 max)
medium (500x500 max)
Large
Extra Large
large ( > 500x500)
Full Resolution


MODEL PREDICTIVE CONTROL OF NONHOLONOMIC MOBILE ROBOTS By FENG XIE Bachelor of Science Zhejiang University Hangzhou, CHINA 1997 Master of Science Zhejiang University Hangzhou, CHINA 2000 Oklahoma State University Stillwater, USA 2004 Submitted to the Faculty of the Graduate College of the Oklahoma State University in partial fulfillment of the requirements for the Degree of DOCTOR OF PHILOSOPHY December, 2007 MODEL PREDICTIVE CONTROL OF NONHOLONOMIC MOBILE ROBOTS Dissertation Approved: Dr. Martin Hagan Committee Chair Dr. Rafael Fierro Thesis Adviser Dr. James R. Whiteley Dr. Weihua Sheng Dr. A. Gordon Emslie Dean of the Graduate College ii PREFACE Cooperative decision and control for mobile robot teams have been of great interest in the control community. A lot of effort has been put into this area. Groups of mobile robots working cooperatively lead to many advantages in a variety of critical applications. With the expectation that unmanned mobile robots can perform key roles in future civilian or military missions, the research on mobile robot control is likely to increase rapidly in the near future. The objective of this dissertation is to use model predictive control (MPC) to coordinate the motion of nonholonomic mobile robots. Specifically, we consider the formation control of a group of mobile robots and trajectory tracking and point stabilization of nonholonomic vehicles. The formation control problem is addressed by a Lyapunovbased nonlinear controller design for unmanned aerial vehicles (UAV) and in the context of MPC for nonholonomic mobile robots. For the UAV formation problem, a twolayered hierarchical control scheme is presented. Assuming that an autopilot operating in holding mode controls the UAV dynamics at the lowlayer, a simplified nonholonomic model is constructed for the highlayer formation controller design. With the dynamic extension technique, three different nonlinear formation controllers are proposed. While the first two controllers, a feedback linearization controller and a sliding mode controller, assume full states information of the leader, the third robust controller only requires the knowledge of leader’s position. By eliminating the requirement of leader’s velocity and acceleration information, the robust controller reduces the intervehicle communication overhead and increases the reliability of the overall system. Stability properties of the controllers are proven using Lyapunov iii theory. As for the nonholonomic mobile robot formation problem, we proposed a dualmode MPC formation control algorithm. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. For the choice of stabilizing terminal controller, a comparison between an inputoutput feedback linearization controller and a robust formation controller is given. We proposed a novel firststate contractive MPC (FSCMPC) approach for the problem of trajectory tracking and point stabilization of nonholonomic mobile robots. In the literature, most of the existing controllers address the trajectory tracking problem by assuming that the trajectory needed to be track is continuously excited by a reference robot. When the reference robot stops or even moves backward, most of the controller will fail. As for the point stabilization problem, discontinuous feedback controllers and timevarying algorithms are mostly found since by Brockett’s theorem, smooth timeinvariant feedback control laws do not exist. In addition, only a few controllers can handle the tracking and stabilization problems in the same control structure. In the literature, most of the stabilizing MPC methods address the stability by adding terminal state penalties in the performance index and/or imposing constraints on the terminal state at the end of the prediction horizon. The stability of the FSCMPC algorithm is guaranteed by adding a contractive constraint on the first state at the beginning of the prediction horizon. With this firststate contractive constraint, the proposed FSCMPC algorithm for nonholonomic mobile robot motion control achieves: (i) exponential stability, (ii) the ability to track a trajectory moving backward, and (iii) the capability of simultaneous tracking and point stabilization. Simulation results are presented to verify the validity of the proposed control algorithms and demonstrate the performance of the proposed controllers. iv ACKNOWLEDGMENTS The journey to a Ph.D. degree is not easy. During this long journey, I have had the pleasure of working with several professors and colleagues, receiving help from them and learning a lot from them. I would like to express my gratitude to all of them. I would like to thank my advisor, Dr. Rafael Fierro for his guidance, support, and enthusiasm for research. I am grateful for the freedom and flexibility he gave me for doing research in model predictive control in which I am especially interested. Thanks to Dr. James R. Whiteley for leading me into the research of model predictive control and being on my committee. Also, thanks to Dr. Martin Hagan for being the chair of my committee, and Dr. Weihua Sheng for being on my committee. Your help has been instrumental in completing the work in this dissertation. I would like to thank Dr. Ximing (Tong) Zhang for his friendship and help on the controller presented in Chapter 4. Also many thanks to current and past members of the MARHES lab: James, Brent, Carlo, Yuan (Eric), Chris, Lorne, Justin, Kenny, and Jos´e. You made my life in Stillwater more exciting. Especially, I would like to thank my beautiful wife, Yan and my parents. Without your supporting and encouraging, I would not be able to finish this work. Last but not least, I gratefully acknowledge the sources of financial support. This work is supported in part by the National Science Foundation (NSF) grant CAREER #0348637 and by the U.S. Army Research Office under grant DAAD190310142 (through the University of Oklahoma). v TABLE OF CONTENTS Chapter Page 1 Introduction 1 1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.2 Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.3 Statement of Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.4 Dissertation Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2 Nonholonomic Mobile Robots and Formations 12 2.1 Nonholonomic Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . 12 2.1.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.1.2 Brockett’s Theorem . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.1.3 Nonholonomic Mobile Robot Control . . . . . . . . . . . . . . . . 18 2.2 Formations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.2.1 Shapes and Positions . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.2.2 Formation Control Strategies . . . . . . . . . . . . . . . . . . . . . 22 2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 3 Model Predictive Control 25 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 3.2 MPC Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.3 MPC Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.4 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 vi 4 Nonlinear Formation Control of Unmanned Aerial Vehicles 36 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 4.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.2.1 Aircraft Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.2.2 LeaderFollowing Formation . . . . . . . . . . . . . . . . . . . . . 42 4.3 Nonlinear Formation Control . . . . . . . . . . . . . . . . . . . . . . . . . 44 4.3.1 Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . 44 4.3.2 Sliding Mode Control . . . . . . . . . . . . . . . . . . . . . . . . 46 4.3.3 Robust Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 4.4.1 Formation under Feedback Linearization Controller . . . . . . . . . 53 4.4.2 Formation under Sliding Mode Controller . . . . . . . . . . . . . . 54 4.4.3 Formation under Robust Controller . . . . . . . . . . . . . . . . . 55 4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 5 Dualmode Model Predictive Formation Control 64 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 5.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 5.2.1 Vehicle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 5.2.2 Formation and Formation Control Graph . . . . . . . . . . . . . . 66 5.3 Controllers for MultiRobot Coordination . . . . . . . . . . . . . . . . . . 67 5.3.1 Formation Error . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 5.3.2 Dualmode MPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 5.3.3 Terminal Controller . . . . . . . . . . . . . . . . . . . . . . . . . . 72 5.4 Stability Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 5.4.1 Dualmode MPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 5.4.2 InputOutput Feedback Linearization Controller . . . . . . . . . . . 76 5.4.3 Robust Formation Controller . . . . . . . . . . . . . . . . . . . . . 77 vii 5.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 5.5.1 TrackingStabilizingTracking . . . . . . . . . . . . . . . . . . . . 78 5.5.2 Follow a Leader Moving Backward . . . . . . . . . . . . . . . . . 78 5.5.3 Control of Chain Formation . . . . . . . . . . . . . . . . . . . . . 78 5.5.4 Control of Triangle Formation . . . . . . . . . . . . . . . . . . . . 79 5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 6 FisrtState Contractive Model Predictive Control of Nonholonomic Mobile Robots 88 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 6.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 6.2.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 6.2.2 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . 90 6.2.3 Point Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . 92 6.3 FirstState Contractive MPC . . . . . . . . . . . . . . . . . . . . . . . . . 93 6.4 Stability Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 6.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 6.5.1 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . 99 6.5.2 Point Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . 101 6.5.3 Simultaneous Tracking and Stabilization . . . . . . . . . . . . . . 101 6.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 7 FSCMPC Formation Control 119 7.1 Formation Error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 7.2 FSCMPC Formation Controller . . . . . . . . . . . . . . . . . . . . . . . 120 7.3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 7.3.1 Reconfiguration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 7.3.2 Obstacle Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . 124 viii 7.3.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 7.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 8 Conclusions 135 8.1 Summary of Main Results . . . . . . . . . . . . . . . . . . . . . . . . . . 135 8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 BIBLIOGRAPHY 138 ix LIST OF FIGURES Figure Page 1.1 MARHES TXT1 robot truck. . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Unmanned aerial robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.3 Underwater vehicle by ECA HYTEC of France. . . . . . . . . . . . . . . . 2 1.4 Three robots move an object cooperatively. . . . . . . . . . . . . . . . . . 4 1.5 Birds flock. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.6 Formation flight  Thunderbirds. . . . . . . . . . . . . . . . . . . . . . . . 5 2.1 Nonholonomic wheeled mobile robot. . . . . . . . . . . . . . . . . . . . . 13 2.2 Formation shapes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.3 Formation position determination. . . . . . . . . . . . . . . . . . . . . . . 21 3.1 MPC concept. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.2 Basic structure of MPC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.3 Approximate genealogy of industrial MPC algorithms. . . . . . . . . . . . 32 4.1 Earthfixed reference frame. . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.2 Reference frames and aerodynamic angles. . . . . . . . . . . . . . . . . . . 40 4.3 Leaderfollowing formation. . . . . . . . . . . . . . . . . . . . . . . . . . 44 4.4 Trajectories of the UAVs in close formation under the action of C1. . . . . . 57 4.5 Control inputs generated by C1. . . . . . . . . . . . . . . . . . . . . . . . 58 4.6 Formation errors under the action of C1. . . . . . . . . . . . . . . . . . . . 59 4.7 Relative position under the action of C1. . . . . . . . . . . . . . . . . . . . 60 4.8 Trajectories of the UAVs in close formation under the action of C2. . . . . . 60 x 4.9 Control inputs generated by C2. . . . . . . . . . . . . . . . . . . . . . . . 61 4.10 Formation errors under the action of C2. . . . . . . . . . . . . . . . . . . . 61 4.11 Trajectories of the UAVs in close formation under the action of C3. . . . . . 62 4.12 Control inputs generated by C3. . . . . . . . . . . . . . . . . . . . . . . . 62 4.13 Formation errors under the action of C3. . . . . . . . . . . . . . . . . . . . 63 5.1 Formation configuration for two UGVs. . . . . . . . . . . . . . . . . . . . 68 5.2 Terminal constraint of dualmode MPC. . . . . . . . . . . . . . . . . . . . 70 5.3 State trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 5.4 Trajectory of a robot following a reference vehicle which moves forward, stops, and then moves backward according to an MPC controller. . . . . . . 82 5.5 Trajectory of a robot following a reference robot which is moving backward according to the robust formation controller. . . . . . . . . . . . . . . . . . 83 5.6 Five robots in chain formation according to a dualmode MPC with robust formation controller as the terminal controller. . . . . . . . . . . . . . . . . 84 5.7 Linear velocity control inputs of chain formation. . . . . . . . . . . . . . . 85 5.8 Angular velocity control inputs of chain formation. . . . . . . . . . . . . . 85 5.9 Six robots in triangular formation according to a dualmode MPC with robust formation controller as the terminal controller. . . . . . . . . . . . . . 86 5.10 Linear velocity control inputs of triangle formation. . . . . . . . . . . . . . 87 5.11 Angular velocity control inputs triangle formation. . . . . . . . . . . . . . 87 6.1 Trajectory tracking. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91 6.2 Coordinate transformation. . . . . . . . . . . . . . . . . . . . . . . . . . . 93 6.3 Tracking trajectories of three controllers. Dashed: Reference. Solid: FSCMPC. Dotted: Samson. Dashdot: Kanayama. . . . . . . . . . . . . . . . 104 6.4 Control inputs of three controllers. . . . . . . . . . . . . . . . . . . . . . . 105 6.5 Control errors of three controllers. . . . . . . . . . . . . . . . . . . . . . . 106 xi 6.6 Stabilizing trajectories of two controllers. Solid: FSCMPC. Dashed: Aicardi107 6.7 Control inputs of two controllers with [1 0 ¼=2]T as initial posture. . . . . 108 6.8 Control errors of two controllers with [1 0 ¼=2]T as initial posture. . . . . 109 6.9 Control inputs of two controllers with [¡0:5 0:867 ¼=2]T as initial posture. 110 6.10 Control errors of two controllers with [¡0:5 0:867 ¼=2]T as initial posture. 111 6.11 Control inputs of two controllers with [¡0:5 ¡ 0:867 ¼=2]T as initial posture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 6.12 Control errors of two controllers with [¡0:5 ¡ 0:867 ¼=2]T as initial posture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 6.13 Case 1, trajectories of simultaneous tracking and stabilization. Dashed: Reference. Solid: FSCMPC. DashDot: Samson. . . . . . . . . . . . . . . 114 6.14 Case 1, control inputs of two controllers. . . . . . . . . . . . . . . . . . . . 115 6.15 Case 1, control errors of two controllers. . . . . . . . . . . . . . . . . . . . 116 6.16 Case 2, trajectory of simultaneous tracking and stabilization. Dashed: Reference. Solid: FSCMPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 6.17 Case 2, control inputs and errors. . . . . . . . . . . . . . . . . . . . . . . . 118 7.1 Convert formation control to trajectory tracking. . . . . . . . . . . . . . . . 121 7.2 Formation level of five robots. . . . . . . . . . . . . . . . . . . . . . . . . 126 7.3 Position change in formation. . . . . . . . . . . . . . . . . . . . . . . . . . 126 7.4 Reconfiguration during navigation. . . . . . . . . . . . . . . . . . . . . . 128 7.5 Control inputs of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . . 129 7.6 Relative position of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . 129 7.7 Control inputs of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . . 130 7.8 Relative position of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . 130 7.9 Obstacle avoidance during navigation  three robots. . . . . . . . . . . . . 131 7.10 Control inputs of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . . 132 7.11 Relative position of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . 132 xii 7.12 Control inputs of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . . 133 7.13 Relative position of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . 133 7.14 Obstacle avoidance during navigation  five robots. . . . . . . . . . . . . . 134 xiii LIST OF TABLES Table Page 4.1 Euler angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 6.1 The integral of norm squared actual control inputs for tracking. . . . . . . . 100 6.2 The integral of norm squared actual control inputs for stabilization. . . . . . 102 xiv CHAPTER 1 Introduction From the literal meaning, mobile robots are robots which can move around in their environment and are not fixed to one physical location. In recent years, the research and development of mobile robots are very active, mostly because of their better potential than other kinds of robots in replacing human beings in civilian and military applications. By the environment in which they move, mobile robots can be classified into: ground robots (Figure 1.1), aerospace robots (Figure 1.21), and underwater robots (Figure 1.32). Figure 1.1: MARHES TXT1 robot truck. 1http://www4.army.mil 2http://www.infotechfrance.com/london/upload/photo_9273.jpg 1 Figure 1.2: Unmanned aerial robot. Figure 1.3: Underwater vehicle by ECA HYTEC of France. Due to recent substantial developments in electronics and computing, it is now possible to find onboard embedded computers which have more computing power than the super computers available a few years ago. Exchanging information between mobile robots, such as unmanned aerial/ground vehicles (UAV/UGV) distributed over an area, is now possible by means of offtheshelf adhoc wireless network devices. In addition, there are various small size, light weight sensing devices on the market ranging from laser range sensors to color CCD cameras. As a result, by exploiting current technology, one can 2 build a group of relatively small UAVs/UGVs each having satisfactory capabilities within a reasonable budget. The challenge here lies in designing control algorithms for mobile robots to perform complex tasks within dynamic environments. 1.1 Motivation Cooperative decision and control of mobile robot teams have been of great interest in civilian and military applications, as these teams are expected to be capable of performing many key roles in future civilian or battlefield missions. A lot of effort has been put into this area and the use of mobile robots is likely to increase rapidly in the near future. Applications for cooperative control of multirobot systems include [1]: Formation control, cooperative classification and surveillance, cooperative attack and rendezvous, environmental sampling, distributed aperture observing, intelligent highways, and air traffic control. In application, groups of mobile robots working cooperatively lead to many advantages. Multirobot systems are expected to outperform singlerobot systems in function, fault tolerance, flexibility, size and cost [1] [2]. A short summary describing some important multirobot capabilities is given below. ² Distribution: Multiple mobile robots can work simultaneously at different positions in the space. For example, during a surveillance task, the target can be monitored from different angles by a group of UAVs/UGVs distributed over the area. This will provide more detailed information of the target. ² Multitasking: Usually, a task can be decomposed into several subtasks which are capable of being handled at the same time. Parallel UAV/UGV operation can finish the task much faster than a single UAV/UGV can do. For example, during forestfire monitoring, the time for scanning can be reduced in half with two UAVs working side by side. 3 ² Fault tolerance: In a multirobot group, robots’ functions can overlap. Therefore, if one robot malfunctions, its functionality can be easily substituted by other robots. The whole group’s functionality will not be affected by individual’s failure. This increases the robustness of the system, which is critical in dangerous missions. ² Flexibility: The functionality of a group robots can be easily changed by combining different robots with different capabilities or enhanced by adding new robots. ² costeffectiveness: Design a versatile robot which is capable of handling different tasks sometime might not feasible due to the limitation of robot size and payloads. However, with several robots each has simple functions, a costeffective robot group can be built without losing the capability of different tasks handling. Figure 1.43 shows three mobile robots moving an object cooperatively. Figure 1.4: Three robots move an object cooperatively. Formation control was inspired by the emergent selforganization observed in nature, like birds flocking and fish schooling, see Figure 1.54. Each animal in a herd benefits by maximizing the chance of detecting predators or food and minimizing its encounters with predators. Teams of UAVs moving in formations with precisely defined geometries lead to many advantages, such as energy saving when the vortex forces are taken into account, see 3http://db.tohoku.ac.jp/whois/photo_resact/000000000841.tmp.d.jpg 4http://alanwhite.info/images/Birds_in_formation.JPG 4 Figure 1.6 5. Several experimental studies have verified the energy saved when flying in close formations [3]. In addition, formation control allows for intelligent leaders and single agent planning while followers can focus on other tasks. Leaderfollowing is a common approach to build formations of multivehicle systems. The challenge here lies in designing a formation controller that is computationally simple, yet robust. Figure 1.5: Birds flock. Figure 1.6: Formation flight  Thunderbirds. A nonholonomic model (e.g., unicycle) is commonly adopted to describe vehicle’s kine 5http://www.skyflash.com 5 matics in mobile robot motion coordination. Therefore, fundamental control problems, trajectory tracking and point stabilization of nonholonomic mobile robots, are inevitably encountered. During the past decades, those problems have received a lot of attention. For interested readers, the book [4] is a good starting point for understanding nonholonomic systems. Though numerous control algorithms can be found in the existing literature, the controller design is still challenging. For trajectory tracking, most of the controllers will fail when face a trajectory moving backward. However, a backward trajectory is common in leaderfollowing formation when the leader tries to avoid some obstacles. For point stabilization, according to Brockett’s theorem [5], a smooth timeinvariant feedback control law does not exist. Therefore, the controller design is more challenging. Most of the control design for point stabilization problem can be classified into two categories: (i) Discontinuous feedback laws, and (ii) timevarying algorithms. However, timevarying controllers are reported with slow convergence rates and the discontinuous controller design is complex. In addition, most of the existing approaches do not consider input constraints. Since general cooperative objectives can be formulated into optimal control problems, optimizationbased techniques are suited to multirobot cooperative control. Model predictive control (MPC) or receding horizon control (RHC) in particular is an optimizationbased approach. It has gained more and more attention in the control community. In addition, the inherent ability of MPC to handle constrained systems makes it a promising technique for cooperative control, especially for multivehicle formation control. With recent substantial developments in computing and solver techniques, realtime model predictive control of fast updating system can be found in literature. 6 1.2 Objective The objective of this dissertation is to use model predictive control (MPC) to coordinate the motion of nonholonomic mobile robots. Specifically, we consider the formation control of a group of vehicles (UAVs/UGVs) and the trajectory tracking and point stabilization of nonholonomic mobile robots. 1.3 Statement of Contributions The contributions of this dissertation can be summarized as follows. ² A robust formation controller is developed for the leaderfollowing formation of UAVs. With the assumption that an autopilot operating in holding mode at the lowlayer, we present a twolayered hierarchical control scheme which allows a team of UAVs to perform complex navigation tasks under limited intervehicle communication. Specifically, the robust control law eliminates the requirement of leader’s velocity and acceleration information, which reduces the communication overhead (Published [6]). ² A dualmode MPC algorithm that allows a team of mobile robots to navigate in formations is developed. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed (Published [7]). ² A firststate contractive model predictive control (FSCMPC) algorithm is developed for the trajectory tracking and point stabilization problems of nonholonomic mobile robots. The stability of the proposed MPC scheme is guaranteed by adding a firststate contractive constraint and the controller is exponentially stable. The convergence is faster and no terminal region calculation is required. Tracking a trajectory 7 moving backward is no longer a problem under this MPC controller. Moreover, the proposed MPC controller has simultaneous tracking and point stabilization capability (Submitted [8]). 1.4 Dissertation Outline The dissertation begins in Chapter 2 with a brief introduction of nonholonomic mobile robots. The kinematic model is developed and the Brockett theorem is introduced in Section 2.1.1 and 2.1.2. Then a short literature on nonholonomic mobile robots control of trajectory tracking and point stabilization is given in Section 2.1.3. Since formation control is one of the dissertation objectives, the shape and position of a formation are introduced in Section 2.2.1. Then in Section 2.2.2, different formation control approaches, such as leaderfollowing, virtual structure, behavior based and graph theory based approaches are reviewed. Chapter 3 introduces the model predictive control. The strategy of MPC is qualitatively depicted in Section 3.2. Though in industry, finite impulse response (FIR) or finite step response (FSR) models are used, MPC is always formulated based on statespace models in academia and literature. Generic MPC formulations that admits a statespace model, constraints and a quadratic performance index function are shown in Section 3.3. In detail, we give a discretetime formulation for linear timeinvariant systems and a continuoustime formulation for nonlinear systems. A short literature review is given in Section 3.4. Since in MPC schemes, the control sequence is obtained by solving a finite optimal control problem, the stability is not automatically guaranteed. Some discussion of the stability of MPC are given in Section 3.4. The key idea is to prove the monotonicity of the performance index function and use it as a Lyapunov function. For linear systems, if the openloop system is stable, the monotonicity of the performance index function is easily guaranteed if one of the the weighting matrices is choosing by solving a Lyapunov equation. Generally, by 8 adding a terminal state equality constraint to the optimal control problem, the stability of the closedloop system can be proven. As for nonlinear systems, the terminal state equality approach is still working theoretically. However, since the optimization control problem becomes nonlinear programming, finding the global optimum is computational expensive. Three approaches, the dualmode, the quasiinfinite and the contractive algorithms, are introduced in the second half of Section 3.4. In Chapter 4, we consider the formation control problem of UAVs. Lyapunovbased nonlinear controller design techniques are applied in this chapter. We start with a brief introduction of aircraft dynamics in Section 4.2. Based on the assumption that an autopilot running in the holding mode is used as a lowlevel controller, the lateral and longitudinal movements of a UAV can be decoupled and a simplified nonholonomic model of the aircraft is constructed. With this simplified model, the leaderfollowing formation if defined later in this section. We proposed three nonlinear formation controllers in Section 4.3. The first one is a feedback linearization controller 4.3.1. The dynamic extension method is applied to design the controller. With this feedback linearization controller, we reduce the nonlinear error system into a linear system. Though theocratically sound, this technique is practical only under the assumption of a perfect plant model. Therefore, we propose a sliding mode controller in Section 4.3.2. The stability proof is also given in this section. Since a generic sliding vector function is used in the control design, a variety of available sliding vector functions can be substituted in to reduce the chattering and to achieve satisfactory performance. In Section 4.3.3, we propose a robust formation controller. Compare to the first two controllers, which assume full knowledge of leader aircraft’s states, this robust controller eliminates the requirement of leader’s velocity and acceleration information. Therefore, communication overhead can be reduced. A detailed stability proof can be found in this section. Finally, the performance of the proposed controllers is examined by simulations in Section 4.4. The formation control problem is addressed again in the context of MPC in Chapter 5. 9 In Section 5.1, formation control by MPC literature is reviewed. We propose that it is more convenient to put the vehicles’s nonholonomic constraints inside the MPC framework. In Section 5.2, we redefine the formation with graph theory. Then a dualmode MPC formation controller is propose in Section 5.3. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. A detailed proof is given in Section 5.4. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. For the choice of stabilizing terminal controller, a comparison between an inputoutput feedback linearization controller and a robust formation controller is given in Section 5.4.2 and 5.4.3. Finally, simulation results are presented in Section 5.5 and concluding remarks are given in Section 5.6. In Chapter 6, we consider using MPC to control nonholonomic mobil robots. Since a nonholonomic model is commonly adopted to describe vehicle’s kinematics in mobile robot motion coordination, fundamental control problems, trajectory tracking and point stabilization of nonholonomic mobile robots need further investigation. In the literature, most of the existing controllers address the trajectory tracking problem by assuming that the trajectory needed to be track is continuously excited by a reference robot. When the reference robot stops or even moves backward, most of the controller will fail. As for the point stabilization problem, discontinuous feedback controllers and timevarying algorithms are mostly found since by Brockett’s theorem, smooth timeinvariant feedback control laws do not exist. In addition, only a few controllers can handle the tracking and stabilization problems in the same control structure. In this chapter, we proposed a novel MPC approach for the control of nonholonomic mobile robots. Most stabilizing MPC methods address stability by adding terminal state penalties in the performance index and/or imposing constraints on the terminal state at the end of the prediction horizon. In the MPC algorithm we proposed, its stability is guarantees by adding a contractive constraint on the first state at the beginning of the prediction horizon. With this firststate contractive constraint, the pro 10 posed MPC algorithm achieves: (i) Exponential stability, (ii) the ability to track a trajectory moving backward, and (iii) the capability of simultaneous tracking and point stabilization. Chapter 6 is organized as follows. In Section 6.2, we introduce the robot kinematic model and the trajectory tracking and point stabilization problems of a nonholonomic mobile robot. A firststate contractive MPC algorithm is proposed in Section 6.3. Stability results of the proposed algorithm can be found in Section 6.4. In Section 6.5, simulation results are presented to show the effectiveness of our method. Finally, a summary is given in Section 6.6. In Chapter 7, the FSCMPC algorithm is extended to multirobot formations. With the capability of simultaneous tracking and point stabilization, the FSCMPC controller can achieve some practical formations without any special treatments. Simulation results are provided. Finally, in Chapter 8, we summarize the main results of this dissertation and outline the future work. 11 CHAPTER 2 Nonholonomic Mobile Robots and Formations 2.1 Nonholonomic Mobile Robots A nonholonomic mobile robot is a robot can only move in the direction normal to the axis of the driving wheels. Most of ground mobile robots (wheeled), part of aerial and underwater vehicles which move under some specific conditions can be considered as nonholonomic mobile robots. Nonholonomic mobile robots have received a lot of attention in the past decades because of their ability to work in large application domains, such as: (i) Transportation, (ii) planetary exploration, (iii) surveillance, (iv) security inspection, (v) military targets tracking and attack, and (vi) humanmachineinterfaces for people with mobility deficiency, to mention a few. Due to the wide range of applications, the research of nonholonomic mobile robots is multidisciplinary and has many directions. As the dissertation objective stated in Section 1.2, we only consider the formation control of a group of vehicles and the trajectory tracking and point stabilization of nonholonomic mobile robots. 12 2.1.1 Kinematic Model The kinematic model for a wheeled mobile robot (assumed to be of the unicycle type) under the nonholonomic constraint of pure rolling and nonslipping is considered throughout this dissertation q_ = S(q)u; (2.1) where q(t); q_(t) 2 R3 are defined as q = [x y µ]; q_ = [x_ y_ µ_]: x(t) and y(t) are the position of the center of mass of the wheeled mobile robot in a Cartesian coordinate frame. µ(t) 2 R1 denote the orientation of the robot (see Figure 2.1). x_ (t) and y_(t) are the Cartesian components of the linear velocity v 2 R1 and µ_(t) 2 R1 denotes the angular velocity. q v X Y O Figure 2.1: Nonholonomic wheeled mobile robot. The matrix S(q) 2 R3£2 is defined as follows S(q) = 2 66664 cos µ 0 sin µ 0 0 1 3 77775 ; (2.2) 13 and the velocity vector u(t) 2 R2 is defined as u = 2 64 v ! 3 75 ; (2.3) where v and ! are the linear linear and angular velocity, respectively. The pure rolling and nonslipping constraint means the robot can only move in the direction normal to the axis of the driving wheels. Mathematically, this constraint can be expressed as x_ sin µ ¡ y_ cos µ = 0: (2.4) A detailed analytical study of the kinematics of wheeled mobile robots (includes other types such as tricycle and carlike) can be found in [9]. For nonlinear systems, linear approximations can be the first step for analysis and control design. As we know, if the tangent linearized system is controllable, then the original nonlinear system is at least locally controllable and feedback stabilizable. By linearizing the system (2.1) about the equilibrium point (q = 0; u = 0), we have q_ = 2 66664 1 0 0 0 0 1 3 77775 2 64v ! 3 75 : (2.5) Clearly, by examining the rank of the controllability matrix C = 2 66664 1 0 0 0 0 1 3 77775 ; (2.6) the linear system is not controllable. For driftless nonlinear systems in the form z_ = Xm i=1 gi(z)ui; (2.7) 14 where z 2 Rn and u 2 Rm, a sufficient condition (accessibility rank condition) for controllability is that, for any z, the dimension of the involutive closure of the distribution generated by the vector fields gi is equal to n [10], that is dim finv¢g = n; ¢ ´ span fgig: (2.8) For the system (2.1), we have n = 3, vector fields g1 = 2 66664 cos µ sin µ 0 3 77775 ; g2 = 2 66664 0 0 1 3 77775 ; and rankfg1; g2; [g1; g2]g = rank 2 66664 cos µ 0 sin µ sin µ 0 ¡cos µ 0 1 0 3 77775 = 3; (2.9) where [g1; g2] is the Lie bracket of g1 and g2 [g1; g2] = @g2 @p g1 ¡ @g1 @p g2: (2.10) Therefore, dim finv¢g = n and the system is controllable. However, for nonlinear systems, the existence of smooth timeinvariant state feedback control laws cannot be implied from the controllability. This will be discussed in Section 2.1.2. For control design, with a change of state coordinates, the model equations of the robot can be transformed into a simpler form. The following change of coordinates [11] 2 66664 x1 x2 x3 3 77775 = 2 66664 0 0 1 cos µ sin µ 0 sin µ ¡cos µ 0 3 77775 2 66664 x y µ 3 77775 ; (2.11) and the change of inputs u1 = !; u2 = v ¡ !x3; (2.12) 15 transform the system (2.1) into x_ 1 = u1; x_ 2 = u2; (2.13) x_ 3 = x2u1: This system belongs to a general class of systems called chained system [12] which has the form x_ 1 = u1; x_ 2 = u2; x_ 3 = x2u1; (2.14) ... x_ n = xn¡1u1: 2.1.2 Brockett’s Theorem The problem of smooth timeinvariant state feedback stabilization can be defined as follows. Definition 2.1 Find a state feedback u = k(q), where k(q) is a smooth function of q, such that the closedloop system q_ = S(q)k(q) = f(q); (2.15) is asymptotically stable. However, as mentioned earlier, a controllable nonlinear system does not mean that it can be stabilized by a smooth timeinvariant feedback control law. A general theorem on necessary conditions for smooth feedback stabilization of nonlinear systems is given by Brockett in [13]. 16 Theorem 2.1 Consider the nonlinear system x_ = f(x; u) with f(x0; 0) = 0 and f(¢; ¢) continuously differentiable in a neighborhood of (x0; 0), necessary conditions for the existence of a continuously differentiable control law for asymptotically stabilizing (x0; 0) are: 1. The linearized system has no uncontrollable modes associated with eigenvalues with positive real part, 2. There exists a neighborhood N of (x0; 0) such that for each » 2 N there exists a control u»(t) defined for all t > 0 that drives the solution of x_ = f(x; u») from the point x = » at t = 0 to x = x0 at t = 1, 3. The mapping ° : N £ Rm ! Rn, N a neighborhood of the origin, defined by ° : (x; u) ! f(x; u) should be onto an open set of the origin. Details and the proof of this theorem can be found in [14]. In the particular case of driftless systems, a corollary to Brockett’s theorem is the following [10]. Corollary 2.1 Consider a driftless system of the form q_ = Xm i=1 gi(q)ui; z 2 Rn u 2 Rm; m · n; (2.16) where the gi are smooth vector fields. If the vectors gi(0) are linear independent, i.e. rank[g1(0); g2(0); ¢ ¢ ¢ ; gm(0)] = m; (2.17) then a solution to the stabilization problem defined in Definition 2.1 exists if and only if m = n. Since Corollary 2.1 is not satisfied in the system (2.1) (n = 3, m = 2), stabilizing smooth timeinvariant feedback laws u = k(q) do not exist for the nonholonomic mobile robot. 17 2.1.3 Nonholonomic Mobile Robot Control In this section, a brief literature review focusing on the trajectory tracking and point stabilization problems of nonholonomic mobile robots is given. A detailed summary of developments in control of nonholonomic systems can be found in [15]. The trajectory tracking problem focuses on stabilizing robots to a timevarying trajectory. Nonlinear feedback controllers are mostly found in the literature. Early results on local controllers can be found in [16, 10] using Lyapunov direct method. The problem is globally solved in [17] by nonlinear feedback. Dynamic feedback linearization is applied in [18, 19, 20]. Other techniques include approximate linearization [21], sliding mode control [22, 23] and backstepping [24, 25, 26]. A recursive technique for trajectory tracking of nonholonomic systems in chained form is derived from the backstepping paradigm [27]. However, a major restriction is that, the tracked linear velocity and angular velocity must not converge to zero, which means the tracked trajectory must be continuously excited. This restriction makes it impossible for a single controller to handle the tracking problem and point stabilization problem simultaneously. Consequently, the range of applications of the above mentioned controllers is limited. In addition, according to the authors in [28], the nonlinear internal dynamics of the closedloop system under output feedback linearization controllers exhibit unstable properties when robots track a trajectory moving backward. To overcome this issue, fullstate tracking techniques are explored in [29]. Model predictive controllers are reported in [30] for trajectory tracking. The MPC approach is based on the quasiinfinite algorithm proposed by authors in [31]. The point stabilization problem, which considers stabilizing robots to a final goal posture, is more challenging. As pointed out in Section 2.1.2, a smooth timeinvariant feedback control law does not exist according to Brockett’s theorem. Most of the control algorithms for point stabilization can be classified into three categories: (i) Smooth timevarying algorithms, (ii) discontinuous or piecewise smooth feedback laws, and (iii) middle strategies (discontinuous and timevarying). Smooth timevarying stabilization is pioneered by Sam 18 son [11, 32, 33] and explored by other authors in [34, 35, 4, 36]. For discontinuous feedback controllers, see [37, 38, 10, 39, 40, 41, 42]. Middle strategies include [43]. As pointed out by authors in [40], timevarying control laws are extremely complex and only for a special class of nonholonomic systems a general strategy is available. Moreover, timevarying control laws produce very slow convergence and are intrinsically oscillating. A comparative experimental study of timevarying and discontinuous controllers for nonholonomic mobile robots is reported in [44]. Other techniques, such as dynamic feedback linearization [20], model predictive control [45], adaptive [46], neural networks [47], and backstepping [24] are also found in the literature. Interesting results are given in [48] and [49]. With a special choice of the system state variables in polarlike coordinates, smooth feedback control laws can be globally stable. Since in those polarlike coordinates, the state is not defined at the origin, Brockett’s Theorem does not hold anymore and a smooth timeinvariant state feedback law for global asymptotic stability is not prevented by Brocketts negative result. The problem of design controllers which can be used for both tracking and stabilization tasks has been explicitly addressed in [50] and [26]. Other controllers with simultaneous tracking and stabilization capabilities can be found in [51, 52, 20, 30]. Most of the controllers mentioned above only consider the kinematics of the vehicle. For the sake of handling dynamics, backstepping techniques are commonly used. The steps can be: (i) Design velocity controllers for the kinematic system (all the control algorithms mentioned above can be used), (ii) design a feedback velocityfollowing control law such that the robot’s velocities asymptotically converge to the velocity inputs generated by the first controller, and (iii) calculate the actual toques by a computedtorque feedback controller with the second control signal as inputs. See [25], [26] for trajectory tracking, [53] for point stabilization and [24] for both. 19 2.2 Formations 2.2.1 Shapes and Positions A formation describes the specific relationship among robots in a group. Shape and position are the two important characteristics of a formation. Figure 2.2 shows some of the common formation shapes in consideration, such as line, column, diamond, and wedge [54]. (a) Line. (b) Column. (c) Diamond. (d) Wedge. Figure 2.2: Formation shapes. Besides the shape, each robot must have a specific position in the formation. Three 20 techniques for formation position determination have been identified in [54]. They are unitcenterreferenced, leaderreferenced and neighborreferenced (see Figure 2.3). In a unitcenterreferenced position, each robot maintains its desired relative position to a unitcenter point which is the average of the x and y positions of all the robots involved in the formation. In a leaderreferenced position, each robot (except the leader) maintains its desired relative position to a leader robot. In a neighborreferenced position, each robot maintains its desired relative position with respect to one other predetermined robot. (a) Unitcenterreferenced. (b) Leaderreferenced. (c) Neighborreferenced. Figure 2.3: Formation position determination. 21 2.2.2 Formation Control Strategies Due to the attention received by formation control research, a lot of control strategies have been proposed. Approaches can be classified into leaderfollowing, virtual structure, behavior based, and graph theory based. The leaderfollowing formation control is a important method [55, 56, 57, 58, 59, 60, 61]. In this approach, one or some mobile robots are designated as leaders which take care of the assignment of followers’ relative positions and the global mission objective, such as trajectory tracking and obstacle avoidance. Simplicity is the major advantage of this approach since only the leader takes care of the global objective while the formations are guaranteed by individual robot’s control law. However, when a singleleader architecture is used, the whole system will fail if the leader fails (single point of failure). In addition, the full state of the leader must be communicated to each member of the formation. Leader failure includes robot malfunction and communication error. Some attempts have been made to overcome the single point of failure. In [55], the architecture has some leader failure detection mechanisms and redefines the formation control graph according to some predefined strategies. As the number of robots increase, string stability and mesh stability need to be addressed [62, 63]. In the virtual structure approach, the entire formation is treated as a single entity. Desired states for each vehicle in the formation can be specified by placeholders in the virtual structure [64]. Similar ideas include the virtual leader [65], and the formation reference point [66]. With these approaches, it is easy to prescribe the coordinated behavior for the group. In addition, the virtual structure approach can maintain the formation during manoeuvres and a rigid geometric relationship among multiple robots. However, the requirement of the the formation to act as a virtual structure limits the class of potential application. Using virtual structure approach for mobile robots formation control is proposed by Lewis [67] and studied in [68, 69, 70, 71, 72]. The behavior based formation control approach [73, 74, 75, 76, 77, 78] is inspired by 22 formation behaviors in nature, like flocking and schooling. In [79], the author simulates flocks of birds and schools of fish with a simple egocentric behavioral model. In [54], the authors define two steps for formation maintenance: (i) Detectformationposition which determines the robot’s position in formation, and (ii) maintainformation which generates motor commands to direct the robot toward the correct location. In behavioral approaches, the control action for each robot is defined by a weighted average of the control corresponding to each desired behavior. Possible behaviors can be, collision avoidance, obstacle avoidance, goal seeking and formation keeping. The advantage of the behavioral approach is that, when robots have multiple competing objectives, it is easy to derive control laws in a natural way. However, even this approach performs well in simulations and experiments, it is hard to do mathematical analysis of stability and robustness. Recently, graph theory is applied to multiple robot formation control [80, 81, 82, 83, 84, 85]. Formations can be described by graphs treating each robot as a vertex and the relationships (control, sensing, communicationflow) among robots as edges. The communication links among systems are described by Laplacian matrices. The stability of the multirobot system is guaranteed by the stability of each robot. However, the method is limited to linear robot models. In [86], the authors propose two controllers for nonholonomic mobile robots formation using graph theoretical methods. In the first controller, the robot model is transformed to a linear model by dynamic feedback linearization. The second controller, with the aid of the timescaling technique and the properties of Laplacian matrix, overcomes the singularity of the first controller. 2.3 Summary In this chapter, a brief review of nonholonomic mobile robots is given. Then the kinematic model is developed and the Brockett’s theorem is introduced. In addition, a short literature review on nonholonomic mobile robots control of trajectory tracking and point stabiliza 23 tion is given. Since formation control is one of the dissertation objectives, the shape and position of a formation are introduced. Different formation control algorithms, such as leaderfollowing, virtual structure, behavior based and graph theory based approaches are reviewed. 24 CHAPTER 3 Model Predictive Control 3.1 Introduction In the past decades, model predictive control (MPC), also known as receding horizon control (RHC), has received great interest in the control community. As an effective method to solve multivariable constrained control problems, MPC has appeared in industry for more than 20 years and successful applications of MPC can be easily found. The glorious past and present of MPC is due to its abilities of constraint handling, realtime prediction, optimizing and feedback correcting. Different from conventional control which uses precomputed control laws, MPC is a control of the form in which the current control action is obtained by solving a finite horizon optimal control problem online at each sampling instant. The optimization yields an open loop optimal control sequence and the first control of this sequence is applied to the plant. The whole process will repeat at the following sampling instants. Actually, the term Model Predictive Control does not designate a specific control strategy but a large range of control methods which explicitly use a plant model to calculate the control action by minimizing a finite horizon optimal control problem. The ideas appearing in all the predictive control family are: (i) A model of the plant which is used to predict the future response of the system at future time instants (horizon), (ii) the calculation of a 25 sequence of control action minimizing an optimal control problem using system’s current states as the initial condition, and (iii) the receding strategy which involves the application of the first control action at each step, so that the horizon moves towards the future at each instant. Practically, this combination of feedforward and feedback makes MPC to outperform ”passive” feedback control. As a summary, the advantages of MPC can be [87]: ”Generality  the ability to include generic models (linear or nonlinear) and constraints in the optimal control problem; Reconfigurability  the ability to redefine cost functions and constraints as needed to reflect changes in the system and/or environment”. Though MPC has gained great success in process industries, its applications for fast updating systems are dragged by the computational burden. Since an optimal control problem must be solved online, the sampling period needs to be long enough for the calculation. In process industries, usually plants under control are sufficiently ‘slow’ and can be satisfactorily linearized, the computational burden is not that critical. However, in highly nonlinear systems, an optimal solution may not be able to determined within a short sampling period. Sometimes, even a feasible solution may not be possible. Another issue is that, the stability of MPC algorithms is not automatically guaranteed since the control sequence is obtained from solving a finite optimal control problem. The implicit nature of the closedloop system makes the proofs of stability of MPC a complicated job. 3.2 MPC Strategy The strategy of MPC is depicted in Figure 3.1. This figure shows a generic MPC algorithm for a singleinputsingleoutput (SISO) system. At current time, say k, the system’s future response (predicted output) yp(k) on a finite horizon Np, say [k k + Np], is predicted by the system model and the predicted control input up(k), [k k + Nm]. Np is named as the prediction horizon and Nm is named as 26 the control horizon (Nm · Np). Usually, the system’s future response is expected to return to a desired set point s(k) by following a reference trajectory r(k) from the current states. The difference between the predicted output yp(k) and the reference trajectory r(k) is called predicted error. An finite horizon optimal control problem with a performance index that usually penalizes the predicted control input and the predicted error is solved online and an optimal control input u¤(k), [k k + Nm], which minimizes the predicted error is obtained. Only the first element of u¤(k) is implemented to the plant. All the other elements are discarded. Then, at the next time interval k + 1, the whole procedure is repeated. The predicted control input up(k + 1) at time k + 1 can be built by u¤(k) with linear extrapolation. Since the prediction horizon and control horizon move one step further into future at each time interval, MPC is also named as receding horizon control (RHC). In order to implement this strategy, the basic structure shown in Figure 3.2 is used. 3.3 MPC Formulation Though in industry, finite impulse response (FIR) or finite step response (FSR) models are used, MPC is always formulated based on statespace models in academia and literature. Consider the following discretetime linear timeinvariant system: x(k + 1) = Ax(k) + Bu(k); (3.1) where x(k) 2 Rn and u(k) 2 Rm are the state and control input, respectively. A 2 Rn£n and B 2 Rn£m are the state and input matrices, respectively. The MPC implementation can be formulated by introducing the following openloop optimization problem at every time interval k: min u(¢) J(Np;Nm)(xk); (3.2) 27 r(k) s(k) up(k) yp(k) u*(k) Figure 3.1: MPC concept. Model Optimizer Predicted Outputs Reference Trajectory Predicted Errors +  Past Inputs and Outputs Future Inputs Cost Function Constraints Figure 3.2: Basic structure of MPC. 28 subject to x(k + 1) = Ax(k) + Bu(k); xmin · x(k + i) · xmax; i = 1; 2; ¢ ¢ ¢ ;Np; umin · u(k + i) · umax; i = 0; 1; 2; ¢ ¢ ¢ ;Nm: (3.3) The performance index is defined as J(Np;Nm)(xk) = xT (Np)Px(Np) + NXp¡1 i=1 xT (k + i)Qx(k + i) + XNm i=0 uT (k + i)Ru(k + i); (3.4) where P 2 Rn£n, Q 2 Rn£n, and R 2 Rm£m are the weighting matrices, and P = PT > 0, Q = QT > 0, R = RT > 0. Np and Nm denote the length of the prediction horizon and the length of the control horizon, respectively. Usually, Np ¸ Nm. The first term on the right hand side of equation (3.4) is called terminal state penalty, the second term is called state penalty and the third term is called control penalty. Equations (3.1)(3.3) define a quadratic program problem and many algorithms and software packets are available to solve it. When the optimal control sequence u¤ (Np;Nm)(i j x(k)), i = 0; ¢ ¢ ¢ ;Nm1 is obtained, only the first control u¤ (Np;Nm)(0 j x(k)) is applied to the system. The rest of the control sequence is discarded. Then at the next time interval k + 1, x(k + 1) is used as the new initial condition of the optimal problem (Equation (3.2) and the process is repeated. As for nonlinear systems, the concept of MPC remains the same. Consider the following continuoustime nonlinear system, x_ (t) = f(x(t); u(t)); (3.5) where x(t) 2 Rn and u(t) 2 Rm are the state and control input, respectively. Similar to the linear case, the optimization problem can be defined as: min u(¢) JT (x(t); u(¢)); (3.6) 29 subject to x_ (t) = f(x(t); u(t)); xmin · x(s; x(t); t) · xmax; t · s · t + T; umin · u(s) · umax; t · s · t + T: (3.7) The performance index is defined as: JT (x(t); u(¢)) = Z t+T t (k¹x(s; x(t); t)k2 Q + ku(s)k2 R)ds +k¹x(t + T; x(t); t)k2 P : (3.8) In this case, the prediction horizon and the control horizon are the same and equal to T. The generic MPC algorithm can be described as follows, 1. At current time t or k, measure the current state x(t) or x(k) as the initial condition, 2. Solve the finite optimization control problem (3.2) or (3.6) with the initial condition obtained in 1, yielding the optimal control sequence u¤ over the control horizon 3. Apply the first element in control sequence u¤ to the system, the remaining elements of the control sequence is discarded, 4. At time t + ±t or k + 1, repeat from 1. Note that, ±t is the sampling time and k = (t ¡ t0)=±t. 3.4 Literature Review Since the objective of this dissertation is to use model predictive control to coordinate the motion of nonholonomic mobile robots, a review of theoretical results in MPC is now given. Two thorough survey papers [88], [89], which give good reviews on MPC’s past, present and future, stability and optimality, are an excellent starting point for any interested reader in this area. 30 The history of model predictive control is quite different than other control design tools. This technique has its origin from industry before any theoretical foundation. The development of MPC can be traced to the work of Kalman in the early 1960s, which is known as the Linear Quadratic Gaussian (LQG). However, at that time, the industrial process control community either had no exposure to LQG technique or regarded it as impractical. The LQG failed to have a strong impact. This environment led to a more general model based control methodology developing in industry, in which the dynamic optimization problem is solved online at each control execution. The first description of MPC appeared in 1976 [90] and later summarized in [91]. The authors called their approach model predictive heuristic control (MPHC), but the solution software is usually mentioned as IDCOM, an acronym for IDentification and COMmand. The main features of the IDCOM approach are: impulse response model, quadratic performance objective, reference trajectory for future plant output behavior, including input, and output constraints and heuristic iterative optimization algorithm. Engineers at Shell Oil developed their own MPC technology and an unconstrained multivariable control algorithm which they named dynamic matrix control (DMC) was presented in [92] and [93]. The main features of the DMC approach can be summarized as follows: linear step response model, quadratic performance objective, setpoint for future plant output behavior, and leastsquares optimization algorithm. Later, the DMC algorithm was posing into a quadratic program (QP) in which input and output constraints appear explicitly. This modified DMC algorithm is called quadratic DMC (QDMC) [94, 95]. After the publication of papers addressing IDCOM and DMC/QDMC, interest in this filed starts to surge and new algorithms have been developed. Today, MPC applications have made this machinery a multimillion dollar industry. A survey of commercially available MPC technology can be found in [96]. Figure 3.3 [96] shows the approximate genealogy of industrial MPC algorithms. Although the model predictive control formulation seems quite intuitive, the stability is 31 Figure 3.3: Approximate genealogy of industrial MPC algorithms. not automatically guaranteed since the control sequence is obtained from a finite optimal control problem. Without the fine tuning of weighting matrices, the MPC algorithm formulated in (3.2), (3.6) may lead to divergent responses. Therefore, it is not surprising that much effort has been devoted to determine sufficient conditions for stability. As a powerful analysis tool, Lyapunov methods are frequently encountered in MPC literature. Pointed out by the authors in [89], the performance index function is monotonic and it can be used as a Lyapunov function. The stability analysis of MPC has reached a relatively mature stage. A short summary is given in this section. Interested readers are referred to [89], [88] for excellent reviews of the stability properties of MPC. As for the linear system, proofs of stability based on the monotonicity property of the performance index function have been proposed in [97], [98]. To simplify the notation, the prediction horizon and the control horizon are assumed to be equal Np = Nm = N. Then we use JN to replace J(Np;Nm) = JN which is defined in Equation (3.4). The key idea of the monotonicity approach is using the performance index function JN as a Lyapunov function. This means the following inequality of the index function needs 32 to be shown JN(x(k)) ¡ JN(x(k + 1)) ¸ 0 for x 6= 0: Rewriting JN(x(k)) ¡ JN(x(k + 1)) gives: JN(x(k)) ¡ JN(x(k + 1)) = [xT (k)Qx(k) + u¤T N (x(k))Ru¤ N(x(k))] +[JN¡1(x(k + 1)) ¡ JN(x(k + 1)]: (3.9) With the assumption that Q > 0 and R > 0, the first term on the right hand side of Equation (3.9) is positive. However, in general, it is hard to assert whether the second term is nonnegative. Several approaches have been proposed to assure the constantly decrease of the performance index JN. In most cases, if the open loop system is stable, by choosing the weighting matrix P as the solution of the Lyapunov equation [99] ATPA + Q = P; (3.10) JN is nonincreasing and the stability can be guaranteed. In [100], the authors prove that when a terminal state equality constraint x(k +N) = 0 is imposed, the performance index JN is nonincreasing as a function of N. Then stability follows. Another approach is to add a terminal constraint that forces the terminal state to be inside a positively invariant region. The decreasing property of JN can be achieved by introducing a stabilizing local controller u(k + i) = Lx(k + i) for i > N. In this case, the terminal penalty and the positively invariant region need to be defined with respect to the system x(i + 1) = (A + BL)x(i) rather than x(i + 1) = Ax(i). Furthermore, the positive invariance should be defined with the respect to the input and state constraints. The local feedback controller can be chosen by the infinite horizon unconstrained LQR method [101]. 33 As for the nonlinear system, approaches introduced above are the idea underlying nonlinear MPC. With a nonlinear system model used, the optimization control problem to be solved online becomes nonlinear programming. In [97] and [102], the idea of zero terminal constraints for nonlinear MPC is analyzed. The performance index function is employed as a Lyapunov function. To guarantee stability, a global optimum must be found at each time step. Though theoretically, the optimization problem with terminal equality constraint can be solved, the computation for finding the global optimum is very expensive. Even when a feasible solution exists, the convergence to that solution is not guaranteed. A dualmode MPC algorithm is proposed by authors in [103] to deal with both the global optimality and the feasibility problems. A terminal region is introduced to relax the terminal equality constraint. At the end of the prediction horizon, the terminal region must be reached. Inside this region, an asymptotically stabilizing controller is employed. With these modifications, a global optimum is no longer required. A feasible solution at the initial time will guarantee the feasibility at all future time steps. However, the terminal region is hard to calculate except for low order systems. An algorithm called quasiinfinite MPC proposed by authors in [31] can overcome both the global optimization and the feasibility problem without using controller switching. In this method, the performance index function is formulated as JT (x(t); u(¢)) = Z t+T t (k¹x(s; x(t); t)k2 Q + ku(s)k2 R)ds + k¹x(t + T; x(t); t)k2 P : The openloop optimal control problem besomes min u(¢) JT (x(t); u(¢)); subject to ¹x(t + T; x(t); t) 2 : (3.11) A weight matrix P needs to be determined such that the penalty of the terminal state ¹x(t+ T), which is the second term on the right hand side of the performance index, is bounded 34 by the infinite horizon cost after t + T k¹x(t + T; x(t); t)k2 P · Z 1 t+T (k¹x(s; x(t); t)k2 Q + ku(s)k2 R)ds 8¹x(t + T; x(t); t) 2 (3.12) The bound is established by assuming that the nonlinear system is controlled by a linear optimal state feedback controller within the region after t + T. Again, a feasible control sequence solution at time t means feasible solutions in the future and stability of the closedloop system is guaranteed. However, the difficulty of terminal region calculation is not improved by this quasiinfinite MPC method. A contractive MPC idea is proposed in [104] and completed and proven in [105]. A constraint is added to the MPC formulation which forces both the actual and predicted state to contract. With this requirement, the stability can be proven. All the methods introduced above need to solve nonlinear programming problems at each time step. Compare to the linear case, the computational requirement is huge. Intuitively, we could linearize the system. Then with the linearized system, all the methods developed for linear systems can be employed. This kind of approach can be found in [106], [107], and [108]. 3.5 Summary In this chapter, a brief introduction of MPC is given. MPC formulations for linear systems and nonlinear systems are briefly discussed in discretetime and continuestime. Since a finite horizon optimal control problem is solved inside the MPC algorithm, the control law is not guaranteed to be stable. Different stabilizing MPC methods are introduced for linear and nonlinear systems. 35 CHAPTER 4 Nonlinear Formation Control of Unmanned Aerial Vehicles In this chapter, we consider the problem of designing nonlinear formation controllers on a team of unmanned aerial vehicles (UAV) using offtheshelf autopilots. Three nonlinear formation controllers are presented. The first two controllers require knowledge of the leader’s velocity and acceleration. The third controller, on the other hand, does not have such requirements. Under these controllers, the formation of UAVs is proven to be stable. Simulations validate the performance of the proposed controllers. 4.1 Introduction Due to recent developments in electronics and computing, it is now possible to find small size, light weight, powerful embedded computers, wireless network equipments and sensing devices on the market. As a result, by exploiting current technology, one can build a group of relatively small UAVs each having satisfactory capabilities within a reasonable budget. For tasks such as obtaining sensory measurements over a wide area (e.g., forest fire monitoring [109]), multiple UAVs are desirable because they can accomplish the task more efficiently than a single UAV. Interested readers are referred to [110] where a survey 36 of UAV cooperative control is provided. Teams of UAVs moving in formations with precisely defined geometries lead to many advantages, such as energy saving when the vortex forces are taken into account. Several experimental studies have verified the energy saved when flying in close formations [3]. In addition, formation control allows for intelligent leaders and single agent planning while followers can focus on other tasks. Leaderfollowing is a common approach to build formations of multivehicle systems. The challenge here lies in designing a formation controller that is computationally simple, yet robust. In [111], a leaderfollowing approach for formation flight is designed using input/output feedback linearization techniques. Furthermore, in [112] a framework for controlling a group of UAVs is developed. The controller design utilizes input/output dynamic linearization techniques based on a model that included the induced rolling moment generated by the lead aircraft over the wing aircraft. In [113], formation controllers are designed to maintain an optimal longitudinal separation needed to achieve the maximal reduction in the induced drag. Authors in [114] develop an interesting experimental testbed to investigate close formation flight. In this chapter, based on a cost effective autopilot1, we propose a twolayer control architecture for practical and robust formation control. In this control scenario, the autopilot provides stable velocities and height tracking on the lowerlevel during the mission flight. On the higherlevel, nonlinear formation controllers ensure that leader and follower UAVs are in formations with desired relative distances and bearing angles. The remaining of the chapter is organized as follows. Section 4.2 gives a brief introduction of aircraft dynamics and the leaderfollowing formation. In Section 4.3, we present three different nonlinear formation controllers. Stability results are also provided in this section. Simulation results are presented in Section 4.4. Finally, concluding remarks are given in Section 4.5. 1A product of Cloud Cap Technology, Inc. 37 4.2 Preliminaries Clearly, unmanned aerial vehicles are aircrafts without pilots. They are either remotely controlled or capable of conducting autonomous operations. Like manned aircrafts, UAVs can be classified by their sizes, types, methods of propulsion and their missions. They may be fixedwing aircrafts or helicopters. To control UAVs, we need to know the position and velocity of the aircraft in the air. This leads to the study of aircraft dynamics. This section gives a brief introduction of aircraft dynamics and the setup of leaderfollowing formation. 4.2.1 Aircraft Dynamics Aircraft motion may only ”make sense” when it is represented in some coordinate systems. Therefore, it is necessary to define appropriate coordinate systems. In this report, all the coordinate systems are righthanded and orthogonal. The earthfixed reference frame, FE: This coordinate system is defined like this: the origin is fixed to an arbitrary point on the surface of the Earth. The xE axis points to North, the yE axis points to East. Consequently the zE axis points to the center of the Earth. Figure 4.1 shows this coordinate system. Figure 4.1: Earthfixed reference frame. 38 The bodyfixed reference frame, FB: In this report, we use the following definition as the bodyfixed reference frame. First, we assume that the aircraft has a plane of symmetry. Then the xB and zB axes lie in that plane of symmetry. The origin is fixed to aircraft’s center of gravity. The xB axis points to the head of the aircraft, the zB axis lies in the plane of symmetry and points downward. The yB axis is determined by the righthanded rule. The stabilityaxis reference frame, FS: We consider the aircraft in steady flight condition so that the relative wind is from a constant direction as seen from the aircraft. The velocity vector Vc of the aircraft is relative to the air mass. Then the projection of this velocity vector into the aircraft plane of symmetry is defined as xS axis. The origin is fixed to aircraft’s center of gravity. The zS axis lies in the aircraft plane of symmetry and points downward. The yB axis is then determined by the righthanded rule. The windaxis reference frame, FW: The windaxis system is defined as follows. The origin is fixed to aircraft’s center of gravity. The xW axis is in the direction of the velocity vector of the aircraft relative to the air. The zW axis lies in the aircraft plane of symmetry and points downward. The yW axis is determined by the righthanded rule. Note that xW axis needs not lie in the plane of symmetry. The bodyfixed frame, the stabilityaxis frame and the windaxis frame are related by two aerodynamic angles. The angle between the xW axis and the xS axis is called sidelip and is denoted by symbol ¯. The angle between the xS axis and the xB axis is called angleof attack and is denoted by symbol ®. Figure 4.2 shows the three different frames and the two angles. A point in the space can be interpreted differently with the respect to different coordinate systems. Pointed out by the Swiss mathematician Leonhard Euler, one reference frame can be placed in alignment with any other reference frame by three successive axisrotations. The angles to rotate the axes are called Euler angles. With Euler angles, the transformation between different coordinate systems is represented by the transformation matrices and the coordinates of a point in different reference frames can be related. In 39 Aircraft plane of symmetry x S z S V c x W x B Figure 4.2: Reference frames and aerodynamic angles. aircraft dynamics, the Euler angles between different reference frames are given special symbols. They are summarized as follows: TF2;F1 µx µy µz TB;E Á µ Ã TW;E ¹ ° Â TB;W 0 ® ¡¯ Table 4.1: Euler angles. The TF2;F1 notation means transformation from reference frame 1 to reference frame 2. For example, TW;E means transformation from the earthfixed reference frame to the windaxis reference frame. With the coordinate system established, one can deduce aircrafts’ equations of motion by applying Newton’s law (see e.g., [115, 116]). We simplify our analysis to an ideal scenario that the UAVs are flying in a wingslevel steadystate and that the angle of attack and sideslip are considerably small, such that they can be ignored. With such simplification, 40 a 6DOF nonlinear model can be set up as follows: x_ = V cos ° cos Â; y_ = V cos ° sin Â; z_ = ¡V sin °; _V = ¡D m ¡ g sin °; ¹_ = p + (q sin ¹ + r cos ¹) tan °; °_ = q cos ¹ ¡ r sin ¹; Â_ = (q sin ¹ + r cos ¹) sec °; !_ = J¡1!^J! + J¡1¿ + L¹; (4.1) where x, y, z are position states in the flat earthfixed initial frame; yaw angle Â, pitch angle ° and roll angle ¹ are attitude states in the windaxis frame; roll rate p, pitch rate q and yaw rate r are angular velocity states in the bodyfixed frame; V is the linear velocity along the flying path; ! = [p q r]T ; J is the inertia matrix; ^! is a skewsymmetric operator; ¿ is the external moment vector; g is the acceleration of gravity; m is the mass of the UAV; D is the drag and ¹L = [¹L p 0 0]T is the rolling moment induced by the wake of the leader aircraft. Usually, an offtheshelf autopilot can provide two basic operational modes: (i) Waypoint tracking mode, and (ii) holding mode. In the first mode, a set of ordered points of interest (POI) can be uploaded into the autopilot before the mission or during the flight. The autopilot generates a path from the current position along these points of interest and provides control commands to the aircraft. In this mode, however, the user cannot precisely control the aircraft’s position except waypoints. In addition, the distance between two successive waypoints must be long enough such that the autopilot system can generate the flight path. As for the holding mode, usually three channels are provided as follows: (1) the Mach hold, (2) the heading turn rate hold, and (3) the altitude hold. The autopilot continuously executes control commands sent to these hold channels. Although, some successful waypointbased formation flight experiments have been reported in the literature 41 (see [117]), it would be more convenient to use the autopilot in holding mode when close formation flight is required, for instance, navigation in urban environments. In this chapter, we investigate the feasibility of using an offtheshelf autopilot [118] in holding mode for the follower UAVs. With an autopilot running in the holding mode, the lateral and longitudinal movements are decoupled [113]. We can write a simplified model of the aircraft as follows: 2 66664 x_ y_ _Ã 3 77775 = 2 66664 cos Ã 0 sin Ã 0 0 1 3 77775 2 64 V ! 3 75 ; (4.2) _V = ¡ 1 ¿v V + 1 ¿v Vc; (4.3) !_ = ¡ 1 ¿! ! + 1 ¿! !c; ¢¢ h = ¡ 1 ¿ha _h ¡ 1 ¿hb hi + 1 ¿hb hc; where x and y represent the positions in Cartesian coordinates, V is the velocity, Ã is the heading angle, and h is the altitude. Vc, !c, and hc are the commands to the Mach hold, headingturnrate hold, and altitude hold channels of the autopilot, respectively. ¿v, ¿!, ¿ha; and ¿hb are known positive constants that depend on the autopilot. We will only use this simplified aircraft model in this chapter. 4.2.2 LeaderFollowing Formation In this section, we set up a kinematic model for an UAV formation flight system. Assuming that the UAVs are flying at a constant altitude, we can consider an operator specified the ith UAV motion ai 2 R2, i 2 f1; 2; :::;Ng given as follows ai = xi^{ + yi^; (4.4) where xi and yi 2 R represent the respective Cartesian coordinates in an earthfixed reference inertial frame FE. Let Fd jk 2 R2 be a desired formation that allows a wing airplane k 42 to follow a leader airplane j given by Fd jk = [ld jk ´d jk]T ; (4.5) where ld jk 2 R+ is the desired relative distance and ´d jk 2 £¼ 2 ; 3¼ 2 ¤ is the desired relative bearing angle as shown in Figure 4.3. Consequently by changing Fd jk we are able to define different formation shapes. Then the actual formation for a wing airplane k to follow a leader airplane j is described by Fjk = [ljk ´jk]T 2 R £ · ¼ 2 ; 3¼ 2 ¸ ; (4.6) in which the relative distance is defined as ljk(t) = kaj ¡ akk2 ; (4.7) where k¢k2 denotes the standard Euclidean norm. The relative bearing is given by ´jk = ¼ + ³ ¡ Ãj ; (4.8) where ³ = arctan 2(yj ¡ yk; xj ¡ xk): Taking the time derivative of equation (4.6) with system model (4.2), we have _F jk = 2 64 _ ljk ´_jk 3 75 = 2 64 Vk cos °jk ¡ Vj cos ´jk 1 ljk (Vj sin ´jk ¡ Vk sin °jk ¡ ljk!j) 3 75 ; (4.9) with °jk , Ãj + ´jk ¡ Ãk. Vk is the linear velocity of the follower: Vj and !j are the linear and angular velocities of the leader. Our control objective is to design a formation controller which drives the wing UAV to track the desired formation Fd jk. To this end, we define the formation error as e = Fd jk ¡ Fjk; (4.10) where e 2 R2. 43 Xj Yj Zj DX DZ +DY XE YE jk ljk yyj Figure 4.3: Leaderfollowing formation. 4.3 Nonlinear Formation Control 4.3.1 Feedback Linearization Taking the derivative of (4.10) with respect to time and combining with (4.9), the following is obtained _ e = _F d jk ¡ _F jk = _F d jk ¡ 2 64cos °jk 0 ¡ 1 ljk sin °jk 0 3 75 2 64 Vk !k 3 75 ¡ 2 64 ¡Vj cos ´jk Vj ljk sin ´jk ¡ !j 3 75 : (4.11) Since !k does not appear, it is obvious that the input matrix of the error system (4.11) is not invertible. Thus we cannot design the control based on (4.11). To facilitate the control design on this system, we first use the dynamic extension method from [119] to render (4.11) into a relative degree system. Differentiating both sides of (4.11) with respect to time and after some algebraic and 44 trigonometric manipulation, we obtain Äe = Ä Fd jk ¡ Ä Fjk = 2 64 ¡cos °jk ¡Vk sin °jk 1 ljk sin °jk ¡Vk ljk cos °jk 3 75 2 64 V_k !k 3 75 + FÄd jk ¡ 2 64 s1 s2 3 75 ; (4.12) where s1 = 1 ljk V 2 k sin2 ° + 1 ljk V 2 j sin2 ´jk ¡ V_j cos ´jk ¡ Vj sin ´jk!j ¡ 2 ljk VjVk sin °jksin´jk; s2 = ¡ 2 l2 jk VjVk sin(°jk + ´jk) + V 2 j l2 jk sin 2´jk + V 2 k l2 jk sin 2°jk + V_j ljk sin ´jk ¡ 1 ljk Vj!j cos ´jk ¡ !_j : (4.13) Define a new formation control input uk , [ _V k !k]T 2 R2 and notate the input matrix as g(¢) = 2 64 ¡cos °jk ¡Vk sin °jk 1 ljk sin °jk ¡Vk ljk cos °jk 3 75 and S = [s1s2]T , we can write the error system in (4.12) as eÄ = g(¢)uk + FÄd jk ¡ S: (4.14) It is not difficult to check that the new input matrix g(¢) is nonsingular under the condition that the flight speed Vk ¸ Vmin > 0 and ljk ¸ lmin > 0, where Vmin is the UAV stalling speed, and lmin is the minimum distance to avoid collision between the two UAVs. Let us define the control input uk as uk = g¡1(¢)(¡FÄd jk + S ¡ Ke ¡ e_); (4.15) where K = Diag[k1 k2], and k1;2 2 R+. 45 Substitute (4.15) into (4.14), we obtain a new error system eÄ = ¡Ke ¡ e_: (4.16) Clearly, we reduce the nonlinear error system (4.14) into a linear error system. With a carefully chosen K, system (4.16) can be stable. Therefore, under the action of control law (4.15), the formation can be kept. 4.3.2 Sliding Mode Control In section 4.3.1, by using the feedback linearization technique, we obtain a stabilizing control law to reduce a nonlinear error system into a linear stable error system. Though theocratically sound, this technique is practical only under the assumption of a perfect plant model. However, a perfect model is not always available. The feedback linearization technique may not achieve acceptable performance in a real world application. Let us consider a sliding mode controller in this section. Rewrite (4.11) into the following form, _ e = _F d jk ¡ g1(¢)Vk ¡ g2(¢)Vj ¡ g3(¢)!j ; (4.17) where the vector fields g1(¢); g2(¢); and g3(¢) are defined as g1(¢) = 2 64 cos °jk ¡ 1 ljk sin °jk 3 75 ; (4.18) g2(¢) = 2 64 ¡cos ´jk 1 ljk sin ´jk 3 75 ; (4.19) g3(¢) = 2 64 0 ¡1 3 75 : (4.20) Differentiating both sides of (4.17) with respect to time and after some algebraic and 46 trigonometric manipulation, we obtain Äe = Ä Fd jk ¡ g_1(¢)Vk ¡ g1(¢) _V k ¡ _ g2(¢)Vj ¡ g2(¢) _V j ¡ g_3(¢)!j ¡ g3(¢)!_ j = Ä Fd jk ¡ · @g1(¢) @°jk °_ jk + @g1(¢) @ljk _ ljk ¸ Vk ¡ g1(¢) _V k ¡ _ g2(¢)Vj ¡ g2(¢) _V j ¡ g3(¢)!_ j = Ä Fd jk ¡ · @g1(¢) @°jk (!j + ´_jk ¡ !k) + @g1(¢) @ljk _ ljk ¸ Vk ¡g1(¢) _V k ¡ _ g2(¢)Vj ¡ g2(¢) _V j ¡ g3(¢)!_ j = Ä Fd jk ¡ · @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¸ Vk ¡ g1(¢) _V k + @g1(¢) @°jk Vk!k ¡g_2(¢)Vj ¡ @g1(¢) @°jk Vk!j ¡ g2(¢) _V j ¡ g3(¢)!_ j : Thus, Äe = Ä Fd jk ¡ · @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¸ Vk + g(¢)uk + f1(¢)$j + f2(¢)$_ j : (4.21) where f1(¢) = · ¡g_2(¢) ¡@g1(¢) @°jk Vk ¸ 2 R2£2; f2(¢) = · ¡g2(¢) ¡g3(¢) ¸ 2 R2£2; also, we define the leader linear and angular velocity vector as $j = [Vj !j ]T 2 R2. As a new approach in this subsection, we assume that the leader’s linear and angular velocity and acceleration vectors $j and $_ j are known to the follower aircraft (by leader’s onboard instruments and the communication link between the leader and followers). Let us design the following filtered signal r = [r1 r2]T 2 R2 r = e_ + Ke; (4.22) where K = 2 64 k1 0 0 k2 3 75 , and k1;2 2 R+ are design gain constants. Differentiating (4.22) with respect to time and substituting in (4.21) produces r_ = ¢¢ F d jk ¡ µ @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¶ Vk + g(¢)uk +f1(¢)$j + f2(¢)$_ j + Ke_: (4.23) 47 Based on the subsequent Lyapunov analysis, the formation control law becomes uk = g¡1(¢) · ¡ Ä Fd jk + µ @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¶ Vk ¡f1(¢)$j ¡ f2(¢)$_ j ¡ Ke_ ¡ Á(r) ¸ ; (4.24) where Á(r) 2 R2 is a sliding vector function so that the sliding condition is guaranteed. We now state the main stability result of this section for the proposed formation controller. Theorem 4.1 The control law of (4.24) ensures stable sliding surface dynamics of the system in (4.22) and that all system signals are bounded under closedloop operation and the tracking error is asymptotically stable in the sense that lim t!1 e; e_ = 0: (4.25) Proof: To prove the theorem, let us construct the following nonnegative function V = 1 2 rT r: (4.26) By using the control law (4.24) in equation (4.23), we have r_ = ¡Á(r): (4.27) Now, taking the time derivative of (4.26) and substituting (4.27) yields _V = rT r_ = ¡rTÁ(r): (4.28) The sliding vector function can be, for instance, Á(r) = r + a[sgn(r1) sgn(r2)]T , where a 2 R is a positive constant. Then (4.28) becomes _V = ¡krk2 ¡ a jrj · ¡a jrj : Therefore, V (4.26) is a Lyapunov function and system (4.22) is asymptotically stable, which means lim t!1 r = 0. From (4.22) and with reference to Lemma A.8 of [120], we conclude that lim t!1 e = 0 and lim t!1 e_ = 0. ¥ 48 Remark 4.1 A generic sliding vector function Á(r) is given in the above control design, so a variety of available sliding vector functions can be substituted in to reduce the chattering and to achieve satisfactory performance. 4.3.3 Robust Control The effectiveness of the control law introduced in Section 4.3.2 is based on the assumption, that the leader’s position, attitude, velocity and acceleration information are all known to the follower. Those data can be obtained by onboard instruments (on leader and/or follower vehicles) and intervehicle communications. This is a common assumption in the formation control literature. However, in reality, due to the payload limitation or communication failure (for example, under electronic countermeasures), this assumption might not always hold. Consequently, control laws that assume full knowledge of leader aircraft’s states may not guarantee a desired formation in the presence of communication failures. In [121], a failure detection and identification system based on an interacting multiplemodel Kalman filter approach is proposed. When communication is unaccessible, it can provide accurate state estimation, which are required in formation flights. In [122, 123], graph theory is used to improve the robustness and fault tolerance of formation control when communication fails. A novel solution to a class of problems in feedback stabilization of coupled systems with limited communication is presented in [124]. In this section, we propose a robust nonlinear formation controller which requires no information of the leader’s velocity and acceleration. Similar idea of the controller’s design and analysis can be found in [125]. Specifically, by expanding the second term on the right side of equation (4.23) along with equation (4.9), we can rearrange (4.23) into r_ = FÄd jk ¡ 2 64 1 ljk sin2 °jk 2 l2 jk cos °jk sin °jk 3 75 V 2 k + (4.29) g(¢)uk + f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj + Ke_; 49 where f1(¢); f2(¢); and f3(¢) are functions of Fjk, Vk, and °jk. Note that f1(¢); f2(¢); and f3(¢) are bounded since ljk ¸ lmin > 0 and Vk 2 L1. We make the following assumptions: Assumption 4.1 During the formation flight, the leader UAV is stably tracking some desired trajectories $d j = £ V d j !d j ¤T , $_ d j = h _V d j !_ d j iT 2 R2 with $d j , $_ d j , Ä$d j 2 L1 so that we can assume $j , $_ j , $Ä j 2 L1. Assumption 4.2 All other terms in (4.29) except $j and $_ j are known. Remark 4.2 Using equation (4.3), it is possible to generate a leader UAV trajectory so that Assumption 4.1 holds. The follower control vector uk in (4.23) becomes uk = g¡1(¢) Ã ¡ Ä Fd jk + 2 64 1 ljk sin2 °jk 2 l2 jk cos °jk sin °jk 3 75 V 2 k ¡2Ke_ ¡ K2e ¡ ¯1 sgn(e(t)) ! ; (4.30) where ¯1 is a constant positive control gain. After substituting uk into (4.29), the closedloop system is given by r_ = f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj ¡Kr ¡ ¯1 sgn(e(t)): (4.31) Before stating the main result of this section, we give the following lemma which will be invoked later. Lemma 4.1 Let the auxiliary function ¡(t) 2 R be defined as follows ¡ , rT [f1(¢)$j + f2(¢)u_ j + f3(¢)$jVj ¡ ¯1 sgn(e)] : (4.32) 50 If the control gain ¯1 is selected to satisfy the sufficient condition ¯1 > kf1(¢)$j + f2(¢)$_ j + f3(¢)$jVjk2 + (4.33) k¡1 min °°°° d(f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj) dt °°°° 2 ; where kmin = minfk1; k2g, then Z t t0 ¡(¿ )d¿ 6 ³b; (4.34) where the positive constant ³b 2 R is defined as ³b , ¯1ke(t0)k1 ¡ eT (t0) [f1(t0)$j(t0) + f2(t0)$_ j(t0) + f3(t0)$j(t0)Vj(t0)] (4.35) where the notation k ¢ k1 denotes the L1 norm. Proof: Before giving a formal proof of Lemma 4.1, we first show that Z t 0 sgn(y)y_d¿ = jy(t)j ¡ jy(0)j: (4.36) Since jyj = p y2; (4.37) taking the derivative of p y2 yields d p y2 dt = 1 2 ¡ y2¢¡1 2 2yy_ = y p y2 y_ = y jyj y_ = sgn(y)y_: (4.38) Then integrating both sides, we obtain Z t 0 sgn(y)y_d¿ = Z t 0 d p y2 dt d¿ = p y2jt 0 = jyjjt 0 = jy(t)j ¡ jy(0)j: (4.39) Thus, equation (4.36) holds. To simplify the notations, let us define (t) = f1(t)$j(t) + f2(t)$_ j(t) + f3(t)$j(t)Vj(t): 51 After substituting (4.22) into (4.32) and then integrating ¡(t) in time, we obtain Z t t0 ¡(¿ )d¿ = Z t t0 eTKT (¿ ) ((¿ ) ¡ ¯1 sgn e(¿ )) d¿ + Z t t0 deT (¿ ) d¿ (¿ )d¿ ¡ ¯1 Z t t0 deT (¿ ) d¿ sgn(e(¿ ))d¿ : (4.40) After integrating the second term on the righthand side of (4.40) by parts, and utilizing equation (4.36) for the third term, we have the following simplified expression Z t t0 ¡(¿ )d¿ = Z t t0 eTKT (¿ ) µ (¿ ) ¡ KT¡1 d(¿ ) d¿ ¡ ¯1 sgn(e(¿ )) ¶ d¿ + eT (t)(t) ¡ eT (t0)(t0) ¡ ¯1ke (t) k1 + ¯1ke (t0) k1: (4.41) We can now upper bound the righthand side of (4.41), that is Z t t0 ¡(¿ )d¿ 6 Z t t0 kminke (¿ ) k1 µ k(¿ )k2 + k¡1 min °°°° d(¿ ) d¿ °°°° 2 ¡ ¯1 ¶ d¿ + ke (t) k1 (k(t)k2 ¡ ¯1) + ¯1ke (t0) k1 ¡ eT (t0)(t0) (4.42) From (4.42), it is easy to see that if ¯1 is chosen according to (4.33), then the first and second term on the right hand side are less than zero, then we have Z t t0 ¡(¿ )d¿ 6 ¯1ke (t0) k1 ¡ eT (t0)(t0): (4.43) Clearly, (4.43) is (4.34). ¥ We now state the main stability result for the second controller in the following theorem. Theorem 4.2 The control law of (4.30) ensures that all system signals are bounded under closedloop operation and the tracking error is asymptotically stable in the sense that lim t!1 e(t); e_(t) = 0: (4.44) Proof: Let P(t) 2 R be an auxiliary function as follows P(t) , ³b ¡ Z t t0 ¡(¿ )d¿ > 0; (4.45) 52 where ³b and ¡(t) have been defined in Lemma 4.1. Based on the nonnegativity of P (t), we define a Lyapunov function candidate V by V , 1 2 rT r + P: (4.46) After taking the time derivative of (4.46), we have _V = rT _ r + _P . Then utilize the closedloop dynamics of (4.31) and (4.45), we can obtain the following expression _V = rT [f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj ¡Kr ¡ ¯1 sgn(e(t))] ¡ ¡ (4.47) Rearranging the first term on the right hand side of (4.47) and use the definition of ¡, we get _V = ¡rTKr 6 ¡kminkrk2: (4.48) Therefore, r (t) 2 L1 \ L2 and lim t!1 r(t) = 0. With (4.22) again, it is easy to see e(t); e_(t) 2 L1 \ L2 and lim t!1 e(t); e_(t) = 0. ¥ 4.4 Simulation Results In this section, the performance of proposed controllers is tested by simulations. In order to minimize the chattering of sliding mode controller, instead of sgn(¢), arctan(¢) is used in (4.24) and (4.30). To simplify notation, the control algorithms described in Sections 4.3.1, 4.3.2 and 4.3.3 are referred as C1, C2, and C3 , respectively. 4.4.1 Formation under Feedback Linearization Controller The desired formation in this simulation is ld jk = 100 m and ´d jk = 5 4¼ rad. Total simulation time is 60 seconds. Controller parameter is K = Diag[2; 2]. The leader is given a constant angular velocity command !jc = 0:05 rad/s and a constant velocity commandVjc = 17:5 m/s. This means that the leader moves in a circle. 53 The initial conditions of the leader are xj(0) = yj(0) = 0 m, zj(0) = 1000 m, Ãj(0) = ¼ rad, Vj(0) = 17:5 m/s, and !j(0) = 0 rad/s. For the follower, the initial conditions are xk(0) = 100 m, yk(0) = 200 m, zk(0) = 1000 m, Ãj(0) = ¼ rad, Vk = 20 m/s, and !k(0) = 0 rad/s. Figure 4.4 to Figure 4.7 show the response of the feedback linearization formation controller C1. The trajectory of the 2UAV team under the action of C1 is presented in Figure 4.4. Figure 4.5 and 4.6 depict the control inputs and formation errors when C1 is used. The actual relative position is shown in Figure 4.7. As it can be seen, formation errors converge to zero and the follower UAV is able to maintain the desired relative distance and the bearing angle with respect to the leader UAV. 4.4.2 Formation under Sliding Mode Controller In this simulation, the desired formation is ld jk = 100 m and ´d jk = 5 4¼ rad. Total simulation time is 200 seconds. Controller parameter is K = Diag[2; 2]. The leader is given a constant velocity command Vjc = 17:5 m/s. The angular velocity command !jc is 0 rad/s most of the time and !jc = 0:1 rad/s for time periods [30; 45], [60; 75] and [110; 125]. The initial conditions of the leader are xj(0) = yj(0) = 0 m, zj(0) = 1000 m, Ãj(0) = ¼ rad, Vj(0) = 17:5 m/s, and !j(0) = 0 rad/s. For the follower, the initial conditions are xk(0) = 150 m, yk(0) = 20 m, zk(0) = 900 m, Ãj(0) = ¼ rad, Vk = 20 m/s, and !k(0) = 0 rad/s. Figure 4.8 to Figure 4.10 show the response of the sliding mode formation controller C2. The trajectory of the 2UAV team under the action of C2 is presented in Figure 4.8. Figure 4.9 and 4.10 depict the control inputs and formation errors when C2 is used. As it can be seen, formation errors converge to zero the winairplane is able to maintain the desired relative distance and bearing angle with respect to the leader aircraft. 54 4.4.3 Formation under Robust Controller To compare with the sliding mode formation controller, similar simulation setup is used. In this simulation, the desired formation is ld jk = 100 m and ´d jk = 5 4¼ rad. Total simulation time is 200 seconds. Controller parameters are K = Diag[2; 2] and ¯1 = Diag[5; 5]. The leader is given a constant velocity command Vjc = 17:5 m/s. The angular velocity command !jc is 0 rad/s most of the time and !jc = 0:1 rad/s for time periods [30; 45], [60; 75] and [110; 125]. The initial conditions of the leader are xj(0) = yj(0) = 0 m, zj(0) = 1000 m, Ãj(0) = ¼ rad, Vj(0) = 17:5 m/s, and !j(0) = 0 rad/s. For the follower, the initial conditions are xk(0) = 150 m, yk(0) = 20 m, zk(0) = 900 m, Ãj(0) = ¼ rad, Vk = 20 m/s, and !k(0) = 0 rad/s. Figure 4.11 to Figure 4.13 show the response of the sliding mode formation controller C2. The trajectory of the 2UAV team under the action of C3 is presented in Figure 4.11. It is close to the result of Section 4.4.2. Figure 4.12 and 4.13 depict the control inputs and formation error when C2 is used. Again, the controller C3 can maintain the winairplane at the desired relative distance and bearing angle with respect to the lead aircraft. Remark 4.3 Note that the initial amplitudes of the control inputs under C3 are higher than the ones given by C2. We anticipate this behavior since C3 assumes no information about the velocity and acceleration of the leader aircraft. Nevertheless, the overall performance of the closedloop system under C3 is comparable with the performance under C2. C3 is the preferred controller in case of intervehicle communication failure or when the communication bandwidth is limited. A switchinglogic scheme can be designed so that C2 is used when communication between UAVs is possible and the follower UAV has access to its leader’s velocity and acceleration; otherwise, C3 should be switched in. Stability of the switching formation controller becomes an issue that would require further investigation. This analysis is out of the scope of this dissertation. 55 4.5 Summary In this chapter, we present a twolayered control system that allows a team of UAVs to navigate in leaderfollowing formations. At the lowlayer, an offtheshelf autopilot operating in holding mode stabilizes the UAV. At the higher layer, three stable nonlinear formation controllers are developed. This hierarchical control scheme allows a team of UAVs to perform complex navigation tasks under limited intervehicle communication. Specifically, the third robust control law eliminates the requirement of leader’s velocity and acceleration information, which reduces the communication overhead. 56 −300 −200 −100 0 100 200 300 −500 −400 −300 −200 −100 0 100 200 300 L−F Positions X Y Leader Follower Figure 4.4: Trajectories of the UAVs in close formation under the action of C1. 57 0 10 20 30 40 50 60 0 50 100 150 V command t V command (m/s) 0 10 20 30 40 50 60 −5 0 5 10 15 w command t w command (rad/s) Figure 4.5: Control inputs generated by C1. 58 0 10 20 30 40 50 60 −0.5 0 0.5 h error t h error (rad) 0 10 20 30 40 50 60 −200 0 200 l error t l error (m) 0 10 20 30 40 50 60 −2 0 2 y error t y error (rad) Figure 4.6: Formation errors under the action of C1. 59 0 10 20 30 40 50 60 −2.4 −2.3 −2.2 −2.1 −2 h jk t h jk (rad) 0 10 20 30 40 50 60 50 100 150 200 250 l jk t l jk (m) Figure 4.7: Relative position under the action of C1. −1000 −800 −600 −400 −200 0 200 400 600 −1000 −500 0 500 1000 900 950 1000 1050 x L−F Positions y Leader Follower Figure 4.8: Trajectories of the UAVs in close formation under the action of C2. 60 0 20 40 60 80 100 120 140 160 180 200 10 20 30 40 50 60 V command t V command (m/s) 0 20 40 60 80 100 120 140 160 180 200 −10 −5 0 5 10 w command t Figure 4.9: Control inputs generated by C2. 0 20 40 60 80 100 120 140 160 180 200 −0.5 0 0.5 1 h error t 0 20 40 60 80 100 120 140 160 180 200 −100 −50 0 50 l error t 0 20 40 60 80 100 120 140 160 180 200 −1 0 1 2 y error t y error (rad) Figure 4.10: Formation errors under the action of C2. 61 −1000 −800 −600 −400 −200 0 200 400 600 −800 −600 −400 −200 0 200 400 600 800 1000 980 1000 1020 x L−F Positions y Leader Follower Figure 4.11: Trajectories of the UAVs in close formation under the action of C3. 0 20 40 60 80 100 120 140 160 180 200 0 20 40 60 80 100 120 V command t 0 20 40 60 80 100 120 140 160 180 200 −30 −20 −10 0 10 w command t Figure 4.12: Control inputs generated by C3. 62 0 20 40 60 80 100 120 140 160 180 200 −0.5 0 0.5 1 h error t 0 20 40 60 80 100 120 140 160 180 200 −100 −50 0 50 l error t 0 20 40 60 80 100 120 140 160 180 200 −1 0 1 2 3 y error t y error (rad) Figure 4.13: Formation errors under the action of C3. 63 CHAPTER 5 Dualmode Model Predictive Formation Control In this chapter, we consider using model predictive control (MPC) to solve the problem of controlling a team of mobile robots with nonholonomic constraints to leaderfollowing formations. We propose that it is more convenient to put the nonholonomic constraints inside the model predictive control framework. As the first step of exploration, a dualmode MPC algorithm is developed. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. The effectiveness of the method is investigated by numerical simulations. 5.1 Introduction A dynamic network consists of spatially distributed dynamic nodes (e.g., autonomous vehicles, mobile sensors) which are coordinated by common set of goals and possible dynamic interaction between the nodes. There are many applications where a dynamic network may be more suitable than a single vehicle, especially where a distributed system of sensors is advantageous. For example, in searchandrescue operations, deployment of many vehicles 64 over an area can allow for more thorough and faster coverage. Other applications, to mention a few, are environmental monitoring, surveillance and reconnaissance, acquisition and tracking. Yet without coordinating the movement of agents, any advantage of multivehicle deployment may be lost and damaging collisions or interference may occur. One interesting problem in multirobot coordination is how to drive a group of robots to a desired formation. Unmanned ground vehicle (UGV) formations can provide a promising and efficient alternative to existing techniques in a wide range of applications. Many researchers have been working on formation problems, and numerous control algorithms can be found in the literature (see e.g., [126],[127], [128]). Recently, model predictive control (MPC) or receding horizon control (RHC) has gained more and more attention in the control community. The inherent ability of MPC to handle constrained systems makes it a promising technique for cooperative control, especially for multivehicle formation control. Recent work includes [127], [128]. Other applications of MPC, such as controlling nonholonomic mobile robots are described in [30], [45]; for multivehicle coordination are given in [129]. The stability and feasibility of the MPC algorithms become a new challenge (see discussion in [130]). In this chapter, based on previous work [131], [6], we show that it is more convenient to put the vehicles’s nonholonomic constraints inside the MPC framework. Moreover, we present a novel MPC algorithm for mobile robot formations. Since a stabilizing terminal controller is switched in within a specified terminal constraint set, the proposed MPC algorithm is dualmode [103]. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. For the choice of stabilizing terminal controller, a comparison between an inputoutput feedback linearization controller used in [131] and a robust formation controller used in [6] is given. The rest of the chapter is organized as follows. In Section 5.2, some preliminaries are briefly introduced. A dualmode MPC algorithm are proposed in Section 5.3. Stability results are provided in Section 5.4. Section 5.5 contains simulation results. Finally, con 65 cluding remarks are given in Section 5.6. 5.2 Preliminaries The problem considered in this paper is to drive a team of nonholonomic vehicles to a desired formation. This section describes the model used for the mobile agents and the definition of formation. 5.2.1 Vehicle Model Consider the planar motion of nonholonomic unicyclemodel robots whose kinematics are determined by 2 66664 x_ i y_i µ_i 3 77775 = 2 66664 cos µi 0 sin µi 0 0 1 3 77775 2 64 vi !i 3 75 (5.1) where the subscript i 2 [1; : : : ;N] indicates the ith UGV. (xi; yi) are the Cartesian coordinates of the robot, µi 2 (¡¼; ¼] represents the orientation of the robot with respect to the positive x axis, and vi and !i are linear and angular velocities, respectively. 5.2.2 Formation and Formation Control Graph Definition 5.1 A formation is a network of agents interconnected via their controller specifications that dictate the relationships each agent must maintain with respect to its neighbors. The interconnections between agents are modeled as edges in a directed acyclic graph, labeled by a given relationship [59], [132]. Definition 5.2 A formation control graph G = (V; E;F) is a directed acyclic graph consisting of the following: ² A finite set V = (V1; : : : ; VN) of N vertices and a map assigning each vertex Vi to a control system (5.1). 66 ² An edge set E ½ V £ V of pairwise neighbors encoding the formation between agents. If the ordered pair (Vi; Vj) 2 E, then (Vj ; Vi) =2 E, and (Vk; Vj) =2 E for all k 2 f1; : : : ;Ngni. ² A set of constants F = © Fd ij 2 R¡ £ R ª defining control objectives, or set points, for each node j, such that (Vi; Vj) 2 E for some Vi; Vj 2 V. Consequently, by changing Fd ij , we are able to define different formation shapes for the mobile robot team. 5.3 Controllers for MultiRobot Coordination 5.3.1 Formation Error Let a triplet pi = [xi yi µi]T describe the position and the orientation of the ith mobile robot. Let Fd ij = £ ¢xd ij ¢yd ij ¤T be the desired formation between robots i and j. ¢xd ij 2 R¡ and ¢yd ij 2 R are the desired position for robot j in a local Cartesian reference frame C attached to robot i. Then the actual formation for robotpair i and j is described by Fij = [¢xij ¢yij ]T . Figure 5.1 shows the formation configuration for two UGVs. Let us define the formation error for the jth robot in a robotpair (Ri; Rj) xej = 2 66664 ¢xd ij ¡ ¢xij ¢yd ij ¡ ¢yij µi ¡ µj 3 77775 : (5.2) 5.3.2 Dualmode MPC Model predictive control (MPC), as an effective method to solve multivariable constrained control problems, has been used in industry for more than 20 years. Different from conventional control which uses precomputed control laws, MPC is a technique in which the 67 XE YE i q ij h ij l Robot j Robot i j q j d P ij Dx ij Dy Figure 5.1: Formation configuration for two UGVs. current control action is obtained by solving, at each sampling instance, a finitehorizon optimal control problem. Each optimization yields an openloop optimal control sequence and the first control of this sequence is applied to the plant until the next sampling instance. For a robotpair (Ri; Rj), which has an ordered pair (Vi; Vj) 2 E in the formation control graph G and a set point Fd ij 2 F, a control input uj needs to be determined for robot j. With the assumption that robot i’s current and future control action ^ui are known to robot j, the formationerror system for robot j 2 f1; : : : ;Ng at time tk can be defined as follows xej (k + 1) = f(xej (k); uj(k)); xej (k) 2 X; uj(k) 2 U; (5.3) where f(¢) is continuous at the origin, with f(0; 0) = 0; X ½ R3 contains the origin in its interior; U is a compact subset of R2 containing the origin in its interior. To obtain the current control uj(k) at time tk, where k is a nonnegative integer (k 2 Z¤), a finitehorizon optimal control problem P(xej ; k) := min uj fJH(xej ; k; uj)g; 68 must be solved online. JH(xej ; k; uj) is the performance index and H 2 N is the horizon length (for simplicity, the prediction horizon equals the control horizon in this paper). Q and R are positive definite symmetric matrices. To ensure stability of the MPC algorithm, a terminal equality constraint xej (k+H) = 0 is commonly used. Therefore, ¢xij ! ¢xd ij , ¢yij ! ¢yd ij and µj ! µi. However, an equality constraint usually makes the optimal control problem hard to solve. To balance the stability and feasibility, the terminal equality constraint can be relaxed to a terminal region. The MPC algorithm is only required to drive the error system to the edge of the terminal region. Inside the terminal region, a stabilizing terminal controller is switched in and it drives the error system to the equilibrium point. Such MPC algorithm is dualmode [103]. Now let us define the terminal region Xf , which is a convex compact subset of X containing the origin in its interior. Therefore, we can define a set Xc f , where Xc f [Xf = X and Xc f \ Xf = ?. Inside Xf , a stabilizing terminal controller uTj is employed to drive the system (5.3) back to the origin. Note, the terminal region Xf should be positively invariant for the system xej (k + 1) = f(xej (k); uTj (k)). Methods for constructing Xf , which indeed is the terminal controller’s region of attraction, can be found in [103], [133] (local linear controller case). For a nonlinear controller which has a region of attraction X, such as (5.11), the terminal region can be a ball Br, which contains the origin. The incremental cost in the optimal control problem for robot j 2 f1; : : : ;Ng is defined in the manner of [133] L(xej ; uj) = ¸(xej )L(xej ; uj); (5.4) where ¸(xej ) = 8>< >: 0; if xej 2 Xf 1; otherwise ; and L(xej ; uj) = kxej k2 Q + kujk2 R: (5.5) Clearly, the incremental cost L(¢) is continuous at the origin, with L(0; 0) = 0. 69 Now, for robot j 2 f1; : : : ;Ng, given (Vi; Vj) 2 E and Fd ij 2 F, qi(k) and qj(k), ui(k + H ¡ 1; ¢ ¢ ¢ ; k; k) at any update time instance tk, the optimal control problem of dualmode MPC algorithm is defined P(xej ; k) := min uj fJH(xej ; k; uj)g; (5.6) where JH(xej ; k; uj) = XH m=1 L(xej (k + m; k); uj(k + m ¡ 1; k)); (5.7) subject to xej (k + 1) = f(xej (k); uj(k)); xej (k + H) 2 Xf ; uj(k) 2 U: (5.8) The constraint xej (k + H) 2 Xf requires that the final state of the prediction horizon must reach the edge of the terminal region (see Fig 5.2). f X X xe (k H) j + xe (k) j Figure 5.2: Terminal constraint of dualmode MPC. 70 From the definition of incremental cost, the objective function JH(xej ; k; uj) is greater than or equal to 0 and JH(xej ; k; uj) = 0 only when xej = 0 and uj = 0. The solution of optimal control problem (5.6) is denoted as u¤(k) = u¤j (k +m¡ 1; k). The optimal state trajectory under this control action is xej ¤(k) = xej ¤(k + m; k). The corresponding optimal performance index is J¤H (k) = JH(xej ¤(k + m; k); k; u¤j (k + m ¡ 1; k)) where m 2 [1; : : : ;H]. Now the dualmode model predictive controller for robot j 2 f1; : : : ;Ng is stated in the following algorithm. Algorithm: Data: initial states of robots pi(0), pj(0), H 2 N. Initial: At time instance tk = 0, if xej (0) 2 Xf , switch to the terminal controller uTj for all k such that xej (k) 2 Xf . Else set ^ui(l; k) = 0 and ^uj(l; k) = 0 for all l 2 [k; : : : ; k+H¡1]. Then solve optimal control problem (5.6) for robot j and obtain u¤j (l; k), where l 2 [k; : : : ; k + H ¡ 1]. Set u±j (k) = u¤j (k; k) and apply u±j (k) to the system. Controller: 1. At any time instance tk: (a) Measure current state pj(k) and measure or receive current state pi(k). (b) If xej (k) 2 Xf , switch to the terminal controller uTj for all k such that xej (k) 2 Xf . Set u± k(k) = uTj(k). (c) Else, with ^ui(l; k) and ^uj(l; k) as initial guess, solve optimal control problem (5.6) for robot j and obtain u¤j (l; k), where l 2 [k; : : : ; k+H ¡1]. Set u±j (k) = u¤j (k; k). 2. Over time interval [tk; tk+1): (a) Apply u±j (k) to the system. (b) If xej (k) 2 Xf , set ^uj(l; k + 1) = uTj (l), l 2 [k + 1; : : : ; k + H]. 71 (c) Else, compute ^uj(l; k + 1) as 8>< >: u¤j (l; k) l 2 [k + 1; : : : ; k + H ¡ 1] uTj (k + H) l = k + H (d) Transmit ^uj(¢; k + 1) to all robot n that (vj ; vn) 2 E and receive ^ui(¢; k + 1). 5.3.3 Terminal Controller Many formation controllers can be used as the terminal controller. An inputoutput feedback linearization controller (denoted as Separation Bearing Controller, SBC) developed in [56] is used in our previous paper [131]. Set points, are desired distance ld ij and desired orientation ´d ij relative to the leader. The control law determining uj = [vj !j ]T based on the position of Ri, which stabilizes the position of Rj relative to Ri, is [56] vj = sij cos °ij ¡ lij(bij + !i) sin °ij + vi cos (µi ¡ µj); !j = 1 d ¡ sij sin °ij + lij(bij + !i) cos °ij + vi sin (µi ¡ µj) ¢ ; (5.9) where °ij = ´ij + µi ¡ µj ; sij = k1(ld ij ¡ lij); bij = k2(´d ij ¡ ´ij); (5.10) and k1 and k2 are positive constants. Notice that, the SBC controller requires leader’s velocity information, which may not be available. To overcome this limitation, a robust formation control law, uRj , which stabilizes the formation between robotpair i and j is proposed in [6] uRj = g¡1(¢) Ã2 64 1 lij sin2 °ij 2 l2 ij cos °ij sin °ij 3 75 v2 j ¡2Ke_ij ¡ K2eij ¡ ¯1 sgn(eij) ! ; (5.11) 72 where g(¢) = 2 64 ¡cos °ij ¡vj sin °ij 1 lij sin °ij ¡vj lij cos °ij 3 75 2 R2£2; vj is the linear velocity of robot j, K = 2 64 k1 0 0 k2 3 75 , k1;2 2 R+, and ¯1 are positive constant control gains. Details of this robust formation controller have been explained in Section 4.3.3. 5.4 Stability Results 5.4.1 Dualmode MPC Since inside Xf , a stabilizing terminal controller uTj is used, when the state enters Xf , the error system will converge to the origin according to the stability properties of controller uTj . The stability of the system (5.3) is guaranteed if the state, xej (k), starting from any xej (0) 2 XnXf , reaches Xf within finite time under the dualmode MPC Algorithm (see Fig. 5.3). Assumption 5.1 For the incremental cost L(xej ; uj), there exists a Kfunction ·(¢) such that L(xej ; uj) ¸ ·(kxej k) for all xej 2 XnXf and all uj 2 U. Assumption 5.2 For all xej (k) 2 XnXf , u¤(k) exists. Before presenting the main result of this subsection, we state the following lemma (motivated by [103], [133]) which will be invoked later. Lemma 5.1 Suppose assumptions 5.1, 5.2 are satisfied. Then for all k 2 Z¤ such that both xej (k) and xej (k + 1) are in XnXf , under the dualmode MPC algorithm, the following inequality J¤ H(k + 1) ¡ J¤ H(k) · ¡·(kxej (k + 1)k) (5.12) 73 f X e (0) j x o j u X Tj u Figure 5.3: State trajectory. holds. Proof: By Assumption 5.2, u¤j (k) and u¤j (k + 1) exist, so do the optimal performance index J¤H (k) and J¤H (k + 1). To find J¤H (k + 1), the dualmode MPC algorithm solves the optimal control problem (5.6) from an initial control guess ^uj(l; k+1), l 2 [k + 1; : : : ; k + H], which is constructed from the result of previous optimization on time tk. Obviously, J¤H (k +1) · ^ JH(xej (¢); k + 1; ^uj(l; k + 1)). Therefore, J¤ H(k + 1) ¡ J¤ H(k) · ^ JH(xej (¢); k + 1; ^uj(l; k + 1)) ¡ J¤ H(k): (5.13) 74 Follow the construction of ^uj(l; k+1), which is described in the algorithm given in Section 5.3.2, we have ^ JH(xej (¢); k + 1; ^uj(l; k + 1)) ¡ J¤ H(k) = ¡L(xej ¤(k + 1; k); u¤ j (k; k)) +L(xej (k + H + 1; k); uLj (k + H)): (5.14) Since xej (k + H + 1; k) 2 Xf , we have L(xej (k + H + 1; k); uLj (k + H)) = 0: In addition, assuming the system model is perfect, we have L(xej ¤(k + 1; k); u¤ j (k; k)) = L(xej (k + 1); u¤ j (k; k)): Therefore, J¤ H(k + 1) ¡ J¤ H(k) · ¡L(xej (k + 1); u¤ j (k; k)): (5.15) Clearly, with Assumption 5.1, inequality (5.12) holds. ¥ We now state the main stability result for the proposed dualmode MPC in the following theorem. Theorem 5.1 Let assumptions 5.1 and 5.2 be satisfied. Using the terminal controller (5.11) and the kinematic model (5.1), the dualmode MPC is asymptotically stabilizing with a region of attraction X. Proof: According to the analysis at the beginning of this section, since inside Xf , a stabilizing terminal controller uTj is used, when the state enters Xf , the error system will converge to the origin asymptotically. We only need to prove that from any xej (0) 2 XnXf , under the dualmode model predictive controller, the state will be driven into Xf within finite time. 75 As the definition of Xf , it contains the origin in its interior. There must exist a constant r > 0 such that for all xej (¢) 2 XnXf , we have kxej (¢)k ¸ r. Then, with the definition of Kfunction, inequality ·(kxej (¢)k) ¸ ·(r) (5.16) holds. Suppose that a finite time tk does not exist such that xej (k) 2 Xf . Because of Lemma 5.1, by adding inequality (5.12) from 0 to k, we have J¤ H(k) ¡ J¤ H(0) · ¡ Xk n=0 ·(kxej (n)k) · ¡k minf·(kxej (0)k); : : : ; ·(kxej (k)k)g; (5.17) for all k 2 Z¤. Then according to inequality (5.16), we have ¡k minf·(kxej (0)k); : : : ; ·(kxej (k)k)g · ¡k·(r): (5.18) This means J¤H (k) ¡ J¤H (0) · ¡k·(r); for all k 2 Z¤. (5.19) However, (5.19) implies that J¤H (k) ! ¡1 as k ! 1. This contradicts that JH(k) ¸ 0 for all k 2 Z¤. Therefore, there exists a time tk such that xej (k) 2 Xf . The stabilizing property of the controller follows. ¥ 5.4.2 InputOutput Feedback Linearization Controller Theorem 5.2 Assume that the lead vehicle’s linear velocity along the path g(t) 2 SE(2) is lower bounded i.e., vi ¸ Vmin > 0, its angular velocity is also bounded i.e., k!ik < Wmax, the relative velocity ±v ´ vi¡vj and relative orientation ±µ ´ µi¡µj are bounded by small positive numbers "1, "2, and the initial relative orientations kµi(t0) ¡ µj(t0)k < c1¼, with 0 < c1 < 1. If the control law (5.9) is applied to robot Rj , then the formation is stable, and the system outputs lij , ´ij converge exponentially to the desired values [134]. 76 Remark 5.1 Note that, to guarantee stable behavior of Rj , we would require vi > 0. Otherwise, the internal dynamics µj of Rj may be unstable. Let the orientation error be expressed as e_µ = !i ¡ !j . After incorporating the angular velocity for the follower (5.9), we obtain e_µ = ¡ vi dj sin eµ + »(!i; eµ); (5.20) where »(¢) is a nonvanishing perturbation for the nominal system (equation (5.20) with »(¢) = 0), which is itself (locally) exponentially stable. By using the stability of perturbed systems, it can be shown that system (5.20) is stable when vi > 0. A detailed proof of Theorem 5.2 and explanation of internal dynamics can be found in [134]. 5.4.3 Robust Formation Controller A detailed stability proof can be found in Section 4.3.3. Remark 5.2 Notice that, the robust controller proposed here does not require the leader robot’s velocity information and there are no internal dynamics. This is a big improvement to the SBC controller described in Section 5.3.3. The only limitation here is that the reference robot’s velocity cannot be zero. Otherwise, vj needs to be zero and the inverse of g(¢) cannot be computed. In summary, for the SBC controller, it fails when vi · 0. For the robust controller, it only fails when vi = 0. In addition, this robust controller is globally stable, which means that it has a region of attraction X. This alleviates the difficulty of finding a terminal region. 5.5 Simulation Results The effectiveness of the control algorithms presented in Section 5.3 is investigated by numerical simulations. In the figures, each robot is depicted by an arrow within a circle. The orientation of the robot is shown by the orientation of the arrow. 77 5.5.1 TrackingStabilizingTracking A realistic scenario is illustrated in Figure 5.4. The reference robot 1 first moves forward from position (0; 0). Then it stops for some time and finally starts to move backward. This scenario happens when some algorithms are implemented for the reference robot 1 to avoid obstacles. Usually this trackingstabilizingtracking case is not considered under a single controller approach. To keep the formation, a controller switching is required. However, a simulation shows that this case can be handled within the MPC framework. The desired formation is Fd 12 = [¡20;¡10] and it is achieved by the MPC controller. Note, a conventional MPC controller is used here. 5.5.2 Follow a Leader Moving Backward Figure 5.5 shows the response of robot j following a reference robot i which is moving backward under the robust formation controller. This scenario cannot be handled by the SBC controller since it fails when the leader’s velocity is negative. The desired formation is ¹ Fd ij = £ 100; 5¼ 4 ¤ . Robot i starts from position (0; 0) and moves backward with constant speed vi = ¡17:5. Robot j starts from (150; 200) and moves backward. The formation is achieved by the robust formation controller. 5.5.3 Control of Chain Formation Simulations of five robots in chain formation under the dualmode MPC algorithm are presented in this section. The robust formation controller (5.11) is used as the terminal controller. Total simulation time is 50 seconds. The sample time is set to 0:5 second. Therefore, the total time instance is 100. The prediction horizon is set to H = 6. As shown in Figure 5.6, robot 1 moves independently and robots i, i 2 f2; 3; 4; 5g, each follows robot i ¡ 1 to form a chain of robots. The control action for robot 1 at different time instance is defined 78 by u1(k) = 8 >>>>< >>>>: [20 0]T ; k 2 [0; : : : ; 10] [20 0:2]T ; k 2 [11; : : : ; 70] [20 0]T ; k 2 [71; : : : ; 100] The formations for each robotpair are Fd 12 = Fd 23 = Fd 34 = Fd 45 = [¡20 0]T . The initial conditions for each robot are given as p1(0) = [0 0 ¼]T ; p2(0) = [50 10 ¼]T ; p3(0) = [100 ¡ 10 ¼]T ; p4(0) = [150 10 ¼]T ; p5(0) = [200 ¡ 10 ¼]T : The control constrain set is defined as U = n [V !]T 2 R2 : 15 · V · 50;¡0:3 · ! · 0:3 o : Figure 5.6 shows the formation response. The linear velocity control inputs for robots 2; 3; 4; 5 are shown in Figure 5.7 and the angular velocity control inputs are shown in Figure 5.8. Clearly, all the control inputs satisfy the control constraints. As the desired formation is achieved, the linear velocity control inputs converge to 20 m/s and the angular velocity control inputs converge to 0 rad/s, which are the final control inputs for robot 1. 5.5.4 Control of Triangle Formation A simulation of six robots in triangle formation is presented in this section. The edge set is E = f(V1; V2); (V1; V3); (V2; V4); (V2; V5); (V3; V6)g. Robot 1 moves independently. The total simulation time is 15 seconds. Again, the sample time is set to 0:5 second. Therefore, the total time instance is 30. The prediction horizon is set to H = 6. The control action for 79 robot 1 is u1(k) = [20 0]T for all the k. The set points for each robot are Fd 12 = [¡20 20]T ; Fd 13 = [¡20 ¡ 10]T ; Fd 24 = [¡20 10]T ; Fd 35 = [¡20 ¡ 10]T ; Fd 26 = [¡20 ¡ 10]T : The control constrain set is defined as U = n [V !]T 2 R2 : 15 · V · 40;¡0:2 · ! · 0:2 o . The initial conditions for each robot are given by p1(0) = [0 0 ¼]T ; p2(0) = [50 0 ¼]T ; p3(0) = [70 5 ¼]T ; p4(0) = [100 ¡ 30 ¼]T ; p5(0) = [100 0 ¼]T ; p6(0) = [100 20 ¼]T : The formation response is shown in Figure 5.9. The linear velocity control inputs for robots 2; 3; 4; 5; 6 are shown in Figure 5.10 and the angular velocity control inputs are shown in Figure 5.11. Clearly, all the control inputs satisfy the control constraints. As the desired formation is achieved, the linear velocity control inputs converge to 20 m/s and the angular velocity control inputs converge to 0 rad/s, which are the control inputs for robot 1. 5.6 Summary In this chapter, a dualmode MPC algorithm that allows a team of mobile robots to navigate in formation is developed and proven to be stable. Simulations show the effectiveness of the proposed dualmode MPC algorithm. Additionally, we show that it is more convenient to put the tracking and point stabilizing problems of nonholonomic robots inside the MPC 80 framework. For the choice of stabilizing terminal controller, analysis show that the robust formation controller is better than the SBC controller. 81 −80 −60 −40 −20 0 20 40 −40 −30 −20 −10 0 10 20 30 40 x y 1 2 1 1 2 2 Figure 5.4: Trajectory of a robot following a reference vehicle which moves forward, stops, and then moves backward according to an MPC controller. 82 −200 0 200 400 600 800 1000 1200 1400 1600 −200 0 200 400 600 800 1000 1200 x y i j Figure 5.5: Trajectory of a robot following a reference robot which is moving backward according to the robust formation controller. 83 −400 −300 −200 −100 0 100 200 −250 −200 −150 −100 −50 0 50 100 150 x 1 2 3 4 5 1 2 2 3 3 4 4 5 5 1 Figure 5.6: Five robots in chain formation according to a dualmode MPC with robust formation controller as the terminal controller. 84 0 10 20 30 40 50 60 70 80 90 100 5 10 15 20 25 30 35 40 45 50 55 60 k V (m/s) V2 V3 V4 V5 Vlb Vub Figure 5.7: Linear velocity control inputs of chain formation. 0 10 20 30 40 50
Click tabs to swap between content that is broken into logical sections.
Rating  
Title  Model Predictive Control of Nonholonomic Mobile Robots 
Date  20071201 
Author  Xie, Feng 
Keywords  Model Predictive Control; Nonholonomic Mobile Robots;Trajectory Tracking; Point Stabilization 
Department  Electrical Engineering 
Document Type  
Full Text Type  Open Access 
Abstract  In this work, we investigate the possibility of using model predictive control (MPC) for the motion coordination of nonholonomic mobile robots. The contributions of this dissertation can be summarized as follows. A robust formation controller is developed for the leaderfollowing formation of unmanned aerial vehicles (UAVs). With the assumption that an autopilot operating in holding mode at the lowlayer, we present a twolayered hierarchical control scheme which allows a team of UAVs to perform complex navigation tasks under limited intervehicle communication. Specifically, the robust control law eliminates the requirement of leader's velocity and acceleration information, which reduces the communication overhead. A dualmode MPC algorithm that allows a team of mobile robots to navigate in formations is developed. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. A firststate contractive model predictive control (FSCMPC) algorithm is developed for the trajectory tracking and point stabilization problems of nonholonomic mobile robots. The stability of the proposed MPC scheme is guaranteed by adding a firststate contractive constraint and the controller is exponentially stable. The convergence is faster and no terminal region calculation is required. Tracking a trajectory moving backward is no longer a problem under this MPC controller. Moreover, the proposed MPC controller has simultaneous tracking and point stabilization capability. Simulation results are presented to verify the validity of the proposed control algorithms and demonstrate the performance of the proposed controllers. 
Note  Dissertation 
Rights  © Oklahoma Agricultural and Mechanical Board of Regents 
Transcript  MODEL PREDICTIVE CONTROL OF NONHOLONOMIC MOBILE ROBOTS By FENG XIE Bachelor of Science Zhejiang University Hangzhou, CHINA 1997 Master of Science Zhejiang University Hangzhou, CHINA 2000 Oklahoma State University Stillwater, USA 2004 Submitted to the Faculty of the Graduate College of the Oklahoma State University in partial fulfillment of the requirements for the Degree of DOCTOR OF PHILOSOPHY December, 2007 MODEL PREDICTIVE CONTROL OF NONHOLONOMIC MOBILE ROBOTS Dissertation Approved: Dr. Martin Hagan Committee Chair Dr. Rafael Fierro Thesis Adviser Dr. James R. Whiteley Dr. Weihua Sheng Dr. A. Gordon Emslie Dean of the Graduate College ii PREFACE Cooperative decision and control for mobile robot teams have been of great interest in the control community. A lot of effort has been put into this area. Groups of mobile robots working cooperatively lead to many advantages in a variety of critical applications. With the expectation that unmanned mobile robots can perform key roles in future civilian or military missions, the research on mobile robot control is likely to increase rapidly in the near future. The objective of this dissertation is to use model predictive control (MPC) to coordinate the motion of nonholonomic mobile robots. Specifically, we consider the formation control of a group of mobile robots and trajectory tracking and point stabilization of nonholonomic vehicles. The formation control problem is addressed by a Lyapunovbased nonlinear controller design for unmanned aerial vehicles (UAV) and in the context of MPC for nonholonomic mobile robots. For the UAV formation problem, a twolayered hierarchical control scheme is presented. Assuming that an autopilot operating in holding mode controls the UAV dynamics at the lowlayer, a simplified nonholonomic model is constructed for the highlayer formation controller design. With the dynamic extension technique, three different nonlinear formation controllers are proposed. While the first two controllers, a feedback linearization controller and a sliding mode controller, assume full states information of the leader, the third robust controller only requires the knowledge of leader’s position. By eliminating the requirement of leader’s velocity and acceleration information, the robust controller reduces the intervehicle communication overhead and increases the reliability of the overall system. Stability properties of the controllers are proven using Lyapunov iii theory. As for the nonholonomic mobile robot formation problem, we proposed a dualmode MPC formation control algorithm. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. For the choice of stabilizing terminal controller, a comparison between an inputoutput feedback linearization controller and a robust formation controller is given. We proposed a novel firststate contractive MPC (FSCMPC) approach for the problem of trajectory tracking and point stabilization of nonholonomic mobile robots. In the literature, most of the existing controllers address the trajectory tracking problem by assuming that the trajectory needed to be track is continuously excited by a reference robot. When the reference robot stops or even moves backward, most of the controller will fail. As for the point stabilization problem, discontinuous feedback controllers and timevarying algorithms are mostly found since by Brockett’s theorem, smooth timeinvariant feedback control laws do not exist. In addition, only a few controllers can handle the tracking and stabilization problems in the same control structure. In the literature, most of the stabilizing MPC methods address the stability by adding terminal state penalties in the performance index and/or imposing constraints on the terminal state at the end of the prediction horizon. The stability of the FSCMPC algorithm is guaranteed by adding a contractive constraint on the first state at the beginning of the prediction horizon. With this firststate contractive constraint, the proposed FSCMPC algorithm for nonholonomic mobile robot motion control achieves: (i) exponential stability, (ii) the ability to track a trajectory moving backward, and (iii) the capability of simultaneous tracking and point stabilization. Simulation results are presented to verify the validity of the proposed control algorithms and demonstrate the performance of the proposed controllers. iv ACKNOWLEDGMENTS The journey to a Ph.D. degree is not easy. During this long journey, I have had the pleasure of working with several professors and colleagues, receiving help from them and learning a lot from them. I would like to express my gratitude to all of them. I would like to thank my advisor, Dr. Rafael Fierro for his guidance, support, and enthusiasm for research. I am grateful for the freedom and flexibility he gave me for doing research in model predictive control in which I am especially interested. Thanks to Dr. James R. Whiteley for leading me into the research of model predictive control and being on my committee. Also, thanks to Dr. Martin Hagan for being the chair of my committee, and Dr. Weihua Sheng for being on my committee. Your help has been instrumental in completing the work in this dissertation. I would like to thank Dr. Ximing (Tong) Zhang for his friendship and help on the controller presented in Chapter 4. Also many thanks to current and past members of the MARHES lab: James, Brent, Carlo, Yuan (Eric), Chris, Lorne, Justin, Kenny, and Jos´e. You made my life in Stillwater more exciting. Especially, I would like to thank my beautiful wife, Yan and my parents. Without your supporting and encouraging, I would not be able to finish this work. Last but not least, I gratefully acknowledge the sources of financial support. This work is supported in part by the National Science Foundation (NSF) grant CAREER #0348637 and by the U.S. Army Research Office under grant DAAD190310142 (through the University of Oklahoma). v TABLE OF CONTENTS Chapter Page 1 Introduction 1 1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.2 Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.3 Statement of Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.4 Dissertation Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2 Nonholonomic Mobile Robots and Formations 12 2.1 Nonholonomic Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . 12 2.1.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 2.1.2 Brockett’s Theorem . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.1.3 Nonholonomic Mobile Robot Control . . . . . . . . . . . . . . . . 18 2.2 Formations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.2.1 Shapes and Positions . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.2.2 Formation Control Strategies . . . . . . . . . . . . . . . . . . . . . 22 2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 3 Model Predictive Control 25 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 3.2 MPC Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.3 MPC Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.4 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 vi 4 Nonlinear Formation Control of Unmanned Aerial Vehicles 36 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 4.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.2.1 Aircraft Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.2.2 LeaderFollowing Formation . . . . . . . . . . . . . . . . . . . . . 42 4.3 Nonlinear Formation Control . . . . . . . . . . . . . . . . . . . . . . . . . 44 4.3.1 Feedback Linearization . . . . . . . . . . . . . . . . . . . . . . . . 44 4.3.2 Sliding Mode Control . . . . . . . . . . . . . . . . . . . . . . . . 46 4.3.3 Robust Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.4 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 4.4.1 Formation under Feedback Linearization Controller . . . . . . . . . 53 4.4.2 Formation under Sliding Mode Controller . . . . . . . . . . . . . . 54 4.4.3 Formation under Robust Controller . . . . . . . . . . . . . . . . . 55 4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 5 Dualmode Model Predictive Formation Control 64 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 5.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 5.2.1 Vehicle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 5.2.2 Formation and Formation Control Graph . . . . . . . . . . . . . . 66 5.3 Controllers for MultiRobot Coordination . . . . . . . . . . . . . . . . . . 67 5.3.1 Formation Error . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 5.3.2 Dualmode MPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 5.3.3 Terminal Controller . . . . . . . . . . . . . . . . . . . . . . . . . . 72 5.4 Stability Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 5.4.1 Dualmode MPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 5.4.2 InputOutput Feedback Linearization Controller . . . . . . . . . . . 76 5.4.3 Robust Formation Controller . . . . . . . . . . . . . . . . . . . . . 77 vii 5.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 5.5.1 TrackingStabilizingTracking . . . . . . . . . . . . . . . . . . . . 78 5.5.2 Follow a Leader Moving Backward . . . . . . . . . . . . . . . . . 78 5.5.3 Control of Chain Formation . . . . . . . . . . . . . . . . . . . . . 78 5.5.4 Control of Triangle Formation . . . . . . . . . . . . . . . . . . . . 79 5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 6 FisrtState Contractive Model Predictive Control of Nonholonomic Mobile Robots 88 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 6.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 6.2.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 6.2.2 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . 90 6.2.3 Point Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . 92 6.3 FirstState Contractive MPC . . . . . . . . . . . . . . . . . . . . . . . . . 93 6.4 Stability Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 6.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 6.5.1 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . 99 6.5.2 Point Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . 101 6.5.3 Simultaneous Tracking and Stabilization . . . . . . . . . . . . . . 101 6.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 7 FSCMPC Formation Control 119 7.1 Formation Error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 7.2 FSCMPC Formation Controller . . . . . . . . . . . . . . . . . . . . . . . 120 7.3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 7.3.1 Reconfiguration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 7.3.2 Obstacle Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . 124 viii 7.3.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 7.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 8 Conclusions 135 8.1 Summary of Main Results . . . . . . . . . . . . . . . . . . . . . . . . . . 135 8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 BIBLIOGRAPHY 138 ix LIST OF FIGURES Figure Page 1.1 MARHES TXT1 robot truck. . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Unmanned aerial robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.3 Underwater vehicle by ECA HYTEC of France. . . . . . . . . . . . . . . . 2 1.4 Three robots move an object cooperatively. . . . . . . . . . . . . . . . . . 4 1.5 Birds flock. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.6 Formation flight  Thunderbirds. . . . . . . . . . . . . . . . . . . . . . . . 5 2.1 Nonholonomic wheeled mobile robot. . . . . . . . . . . . . . . . . . . . . 13 2.2 Formation shapes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.3 Formation position determination. . . . . . . . . . . . . . . . . . . . . . . 21 3.1 MPC concept. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.2 Basic structure of MPC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.3 Approximate genealogy of industrial MPC algorithms. . . . . . . . . . . . 32 4.1 Earthfixed reference frame. . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.2 Reference frames and aerodynamic angles. . . . . . . . . . . . . . . . . . . 40 4.3 Leaderfollowing formation. . . . . . . . . . . . . . . . . . . . . . . . . . 44 4.4 Trajectories of the UAVs in close formation under the action of C1. . . . . . 57 4.5 Control inputs generated by C1. . . . . . . . . . . . . . . . . . . . . . . . 58 4.6 Formation errors under the action of C1. . . . . . . . . . . . . . . . . . . . 59 4.7 Relative position under the action of C1. . . . . . . . . . . . . . . . . . . . 60 4.8 Trajectories of the UAVs in close formation under the action of C2. . . . . . 60 x 4.9 Control inputs generated by C2. . . . . . . . . . . . . . . . . . . . . . . . 61 4.10 Formation errors under the action of C2. . . . . . . . . . . . . . . . . . . . 61 4.11 Trajectories of the UAVs in close formation under the action of C3. . . . . . 62 4.12 Control inputs generated by C3. . . . . . . . . . . . . . . . . . . . . . . . 62 4.13 Formation errors under the action of C3. . . . . . . . . . . . . . . . . . . . 63 5.1 Formation configuration for two UGVs. . . . . . . . . . . . . . . . . . . . 68 5.2 Terminal constraint of dualmode MPC. . . . . . . . . . . . . . . . . . . . 70 5.3 State trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 5.4 Trajectory of a robot following a reference vehicle which moves forward, stops, and then moves backward according to an MPC controller. . . . . . . 82 5.5 Trajectory of a robot following a reference robot which is moving backward according to the robust formation controller. . . . . . . . . . . . . . . . . . 83 5.6 Five robots in chain formation according to a dualmode MPC with robust formation controller as the terminal controller. . . . . . . . . . . . . . . . . 84 5.7 Linear velocity control inputs of chain formation. . . . . . . . . . . . . . . 85 5.8 Angular velocity control inputs of chain formation. . . . . . . . . . . . . . 85 5.9 Six robots in triangular formation according to a dualmode MPC with robust formation controller as the terminal controller. . . . . . . . . . . . . . 86 5.10 Linear velocity control inputs of triangle formation. . . . . . . . . . . . . . 87 5.11 Angular velocity control inputs triangle formation. . . . . . . . . . . . . . 87 6.1 Trajectory tracking. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91 6.2 Coordinate transformation. . . . . . . . . . . . . . . . . . . . . . . . . . . 93 6.3 Tracking trajectories of three controllers. Dashed: Reference. Solid: FSCMPC. Dotted: Samson. Dashdot: Kanayama. . . . . . . . . . . . . . . . 104 6.4 Control inputs of three controllers. . . . . . . . . . . . . . . . . . . . . . . 105 6.5 Control errors of three controllers. . . . . . . . . . . . . . . . . . . . . . . 106 xi 6.6 Stabilizing trajectories of two controllers. Solid: FSCMPC. Dashed: Aicardi107 6.7 Control inputs of two controllers with [1 0 ¼=2]T as initial posture. . . . . 108 6.8 Control errors of two controllers with [1 0 ¼=2]T as initial posture. . . . . 109 6.9 Control inputs of two controllers with [¡0:5 0:867 ¼=2]T as initial posture. 110 6.10 Control errors of two controllers with [¡0:5 0:867 ¼=2]T as initial posture. 111 6.11 Control inputs of two controllers with [¡0:5 ¡ 0:867 ¼=2]T as initial posture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 6.12 Control errors of two controllers with [¡0:5 ¡ 0:867 ¼=2]T as initial posture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 6.13 Case 1, trajectories of simultaneous tracking and stabilization. Dashed: Reference. Solid: FSCMPC. DashDot: Samson. . . . . . . . . . . . . . . 114 6.14 Case 1, control inputs of two controllers. . . . . . . . . . . . . . . . . . . . 115 6.15 Case 1, control errors of two controllers. . . . . . . . . . . . . . . . . . . . 116 6.16 Case 2, trajectory of simultaneous tracking and stabilization. Dashed: Reference. Solid: FSCMPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 6.17 Case 2, control inputs and errors. . . . . . . . . . . . . . . . . . . . . . . . 118 7.1 Convert formation control to trajectory tracking. . . . . . . . . . . . . . . . 121 7.2 Formation level of five robots. . . . . . . . . . . . . . . . . . . . . . . . . 126 7.3 Position change in formation. . . . . . . . . . . . . . . . . . . . . . . . . . 126 7.4 Reconfiguration during navigation. . . . . . . . . . . . . . . . . . . . . . 128 7.5 Control inputs of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . . 129 7.6 Relative position of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . 129 7.7 Control inputs of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . . 130 7.8 Relative position of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . 130 7.9 Obstacle avoidance during navigation  three robots. . . . . . . . . . . . . 131 7.10 Control inputs of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . . 132 7.11 Relative position of Robot 2. . . . . . . . . . . . . . . . . . . . . . . . . . 132 xii 7.12 Control inputs of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . . 133 7.13 Relative position of Robot 3. . . . . . . . . . . . . . . . . . . . . . . . . . 133 7.14 Obstacle avoidance during navigation  five robots. . . . . . . . . . . . . . 134 xiii LIST OF TABLES Table Page 4.1 Euler angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 6.1 The integral of norm squared actual control inputs for tracking. . . . . . . . 100 6.2 The integral of norm squared actual control inputs for stabilization. . . . . . 102 xiv CHAPTER 1 Introduction From the literal meaning, mobile robots are robots which can move around in their environment and are not fixed to one physical location. In recent years, the research and development of mobile robots are very active, mostly because of their better potential than other kinds of robots in replacing human beings in civilian and military applications. By the environment in which they move, mobile robots can be classified into: ground robots (Figure 1.1), aerospace robots (Figure 1.21), and underwater robots (Figure 1.32). Figure 1.1: MARHES TXT1 robot truck. 1http://www4.army.mil 2http://www.infotechfrance.com/london/upload/photo_9273.jpg 1 Figure 1.2: Unmanned aerial robot. Figure 1.3: Underwater vehicle by ECA HYTEC of France. Due to recent substantial developments in electronics and computing, it is now possible to find onboard embedded computers which have more computing power than the super computers available a few years ago. Exchanging information between mobile robots, such as unmanned aerial/ground vehicles (UAV/UGV) distributed over an area, is now possible by means of offtheshelf adhoc wireless network devices. In addition, there are various small size, light weight sensing devices on the market ranging from laser range sensors to color CCD cameras. As a result, by exploiting current technology, one can 2 build a group of relatively small UAVs/UGVs each having satisfactory capabilities within a reasonable budget. The challenge here lies in designing control algorithms for mobile robots to perform complex tasks within dynamic environments. 1.1 Motivation Cooperative decision and control of mobile robot teams have been of great interest in civilian and military applications, as these teams are expected to be capable of performing many key roles in future civilian or battlefield missions. A lot of effort has been put into this area and the use of mobile robots is likely to increase rapidly in the near future. Applications for cooperative control of multirobot systems include [1]: Formation control, cooperative classification and surveillance, cooperative attack and rendezvous, environmental sampling, distributed aperture observing, intelligent highways, and air traffic control. In application, groups of mobile robots working cooperatively lead to many advantages. Multirobot systems are expected to outperform singlerobot systems in function, fault tolerance, flexibility, size and cost [1] [2]. A short summary describing some important multirobot capabilities is given below. ² Distribution: Multiple mobile robots can work simultaneously at different positions in the space. For example, during a surveillance task, the target can be monitored from different angles by a group of UAVs/UGVs distributed over the area. This will provide more detailed information of the target. ² Multitasking: Usually, a task can be decomposed into several subtasks which are capable of being handled at the same time. Parallel UAV/UGV operation can finish the task much faster than a single UAV/UGV can do. For example, during forestfire monitoring, the time for scanning can be reduced in half with two UAVs working side by side. 3 ² Fault tolerance: In a multirobot group, robots’ functions can overlap. Therefore, if one robot malfunctions, its functionality can be easily substituted by other robots. The whole group’s functionality will not be affected by individual’s failure. This increases the robustness of the system, which is critical in dangerous missions. ² Flexibility: The functionality of a group robots can be easily changed by combining different robots with different capabilities or enhanced by adding new robots. ² costeffectiveness: Design a versatile robot which is capable of handling different tasks sometime might not feasible due to the limitation of robot size and payloads. However, with several robots each has simple functions, a costeffective robot group can be built without losing the capability of different tasks handling. Figure 1.43 shows three mobile robots moving an object cooperatively. Figure 1.4: Three robots move an object cooperatively. Formation control was inspired by the emergent selforganization observed in nature, like birds flocking and fish schooling, see Figure 1.54. Each animal in a herd benefits by maximizing the chance of detecting predators or food and minimizing its encounters with predators. Teams of UAVs moving in formations with precisely defined geometries lead to many advantages, such as energy saving when the vortex forces are taken into account, see 3http://db.tohoku.ac.jp/whois/photo_resact/000000000841.tmp.d.jpg 4http://alanwhite.info/images/Birds_in_formation.JPG 4 Figure 1.6 5. Several experimental studies have verified the energy saved when flying in close formations [3]. In addition, formation control allows for intelligent leaders and single agent planning while followers can focus on other tasks. Leaderfollowing is a common approach to build formations of multivehicle systems. The challenge here lies in designing a formation controller that is computationally simple, yet robust. Figure 1.5: Birds flock. Figure 1.6: Formation flight  Thunderbirds. A nonholonomic model (e.g., unicycle) is commonly adopted to describe vehicle’s kine 5http://www.skyflash.com 5 matics in mobile robot motion coordination. Therefore, fundamental control problems, trajectory tracking and point stabilization of nonholonomic mobile robots, are inevitably encountered. During the past decades, those problems have received a lot of attention. For interested readers, the book [4] is a good starting point for understanding nonholonomic systems. Though numerous control algorithms can be found in the existing literature, the controller design is still challenging. For trajectory tracking, most of the controllers will fail when face a trajectory moving backward. However, a backward trajectory is common in leaderfollowing formation when the leader tries to avoid some obstacles. For point stabilization, according to Brockett’s theorem [5], a smooth timeinvariant feedback control law does not exist. Therefore, the controller design is more challenging. Most of the control design for point stabilization problem can be classified into two categories: (i) Discontinuous feedback laws, and (ii) timevarying algorithms. However, timevarying controllers are reported with slow convergence rates and the discontinuous controller design is complex. In addition, most of the existing approaches do not consider input constraints. Since general cooperative objectives can be formulated into optimal control problems, optimizationbased techniques are suited to multirobot cooperative control. Model predictive control (MPC) or receding horizon control (RHC) in particular is an optimizationbased approach. It has gained more and more attention in the control community. In addition, the inherent ability of MPC to handle constrained systems makes it a promising technique for cooperative control, especially for multivehicle formation control. With recent substantial developments in computing and solver techniques, realtime model predictive control of fast updating system can be found in literature. 6 1.2 Objective The objective of this dissertation is to use model predictive control (MPC) to coordinate the motion of nonholonomic mobile robots. Specifically, we consider the formation control of a group of vehicles (UAVs/UGVs) and the trajectory tracking and point stabilization of nonholonomic mobile robots. 1.3 Statement of Contributions The contributions of this dissertation can be summarized as follows. ² A robust formation controller is developed for the leaderfollowing formation of UAVs. With the assumption that an autopilot operating in holding mode at the lowlayer, we present a twolayered hierarchical control scheme which allows a team of UAVs to perform complex navigation tasks under limited intervehicle communication. Specifically, the robust control law eliminates the requirement of leader’s velocity and acceleration information, which reduces the communication overhead (Published [6]). ² A dualmode MPC algorithm that allows a team of mobile robots to navigate in formations is developed. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed (Published [7]). ² A firststate contractive model predictive control (FSCMPC) algorithm is developed for the trajectory tracking and point stabilization problems of nonholonomic mobile robots. The stability of the proposed MPC scheme is guaranteed by adding a firststate contractive constraint and the controller is exponentially stable. The convergence is faster and no terminal region calculation is required. Tracking a trajectory 7 moving backward is no longer a problem under this MPC controller. Moreover, the proposed MPC controller has simultaneous tracking and point stabilization capability (Submitted [8]). 1.4 Dissertation Outline The dissertation begins in Chapter 2 with a brief introduction of nonholonomic mobile robots. The kinematic model is developed and the Brockett theorem is introduced in Section 2.1.1 and 2.1.2. Then a short literature on nonholonomic mobile robots control of trajectory tracking and point stabilization is given in Section 2.1.3. Since formation control is one of the dissertation objectives, the shape and position of a formation are introduced in Section 2.2.1. Then in Section 2.2.2, different formation control approaches, such as leaderfollowing, virtual structure, behavior based and graph theory based approaches are reviewed. Chapter 3 introduces the model predictive control. The strategy of MPC is qualitatively depicted in Section 3.2. Though in industry, finite impulse response (FIR) or finite step response (FSR) models are used, MPC is always formulated based on statespace models in academia and literature. Generic MPC formulations that admits a statespace model, constraints and a quadratic performance index function are shown in Section 3.3. In detail, we give a discretetime formulation for linear timeinvariant systems and a continuoustime formulation for nonlinear systems. A short literature review is given in Section 3.4. Since in MPC schemes, the control sequence is obtained by solving a finite optimal control problem, the stability is not automatically guaranteed. Some discussion of the stability of MPC are given in Section 3.4. The key idea is to prove the monotonicity of the performance index function and use it as a Lyapunov function. For linear systems, if the openloop system is stable, the monotonicity of the performance index function is easily guaranteed if one of the the weighting matrices is choosing by solving a Lyapunov equation. Generally, by 8 adding a terminal state equality constraint to the optimal control problem, the stability of the closedloop system can be proven. As for nonlinear systems, the terminal state equality approach is still working theoretically. However, since the optimization control problem becomes nonlinear programming, finding the global optimum is computational expensive. Three approaches, the dualmode, the quasiinfinite and the contractive algorithms, are introduced in the second half of Section 3.4. In Chapter 4, we consider the formation control problem of UAVs. Lyapunovbased nonlinear controller design techniques are applied in this chapter. We start with a brief introduction of aircraft dynamics in Section 4.2. Based on the assumption that an autopilot running in the holding mode is used as a lowlevel controller, the lateral and longitudinal movements of a UAV can be decoupled and a simplified nonholonomic model of the aircraft is constructed. With this simplified model, the leaderfollowing formation if defined later in this section. We proposed three nonlinear formation controllers in Section 4.3. The first one is a feedback linearization controller 4.3.1. The dynamic extension method is applied to design the controller. With this feedback linearization controller, we reduce the nonlinear error system into a linear system. Though theocratically sound, this technique is practical only under the assumption of a perfect plant model. Therefore, we propose a sliding mode controller in Section 4.3.2. The stability proof is also given in this section. Since a generic sliding vector function is used in the control design, a variety of available sliding vector functions can be substituted in to reduce the chattering and to achieve satisfactory performance. In Section 4.3.3, we propose a robust formation controller. Compare to the first two controllers, which assume full knowledge of leader aircraft’s states, this robust controller eliminates the requirement of leader’s velocity and acceleration information. Therefore, communication overhead can be reduced. A detailed stability proof can be found in this section. Finally, the performance of the proposed controllers is examined by simulations in Section 4.4. The formation control problem is addressed again in the context of MPC in Chapter 5. 9 In Section 5.1, formation control by MPC literature is reviewed. We propose that it is more convenient to put the vehicles’s nonholonomic constraints inside the MPC framework. In Section 5.2, we redefine the formation with graph theory. Then a dualmode MPC formation controller is propose in Section 5.3. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. A detailed proof is given in Section 5.4. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. For the choice of stabilizing terminal controller, a comparison between an inputoutput feedback linearization controller and a robust formation controller is given in Section 5.4.2 and 5.4.3. Finally, simulation results are presented in Section 5.5 and concluding remarks are given in Section 5.6. In Chapter 6, we consider using MPC to control nonholonomic mobil robots. Since a nonholonomic model is commonly adopted to describe vehicle’s kinematics in mobile robot motion coordination, fundamental control problems, trajectory tracking and point stabilization of nonholonomic mobile robots need further investigation. In the literature, most of the existing controllers address the trajectory tracking problem by assuming that the trajectory needed to be track is continuously excited by a reference robot. When the reference robot stops or even moves backward, most of the controller will fail. As for the point stabilization problem, discontinuous feedback controllers and timevarying algorithms are mostly found since by Brockett’s theorem, smooth timeinvariant feedback control laws do not exist. In addition, only a few controllers can handle the tracking and stabilization problems in the same control structure. In this chapter, we proposed a novel MPC approach for the control of nonholonomic mobile robots. Most stabilizing MPC methods address stability by adding terminal state penalties in the performance index and/or imposing constraints on the terminal state at the end of the prediction horizon. In the MPC algorithm we proposed, its stability is guarantees by adding a contractive constraint on the first state at the beginning of the prediction horizon. With this firststate contractive constraint, the pro 10 posed MPC algorithm achieves: (i) Exponential stability, (ii) the ability to track a trajectory moving backward, and (iii) the capability of simultaneous tracking and point stabilization. Chapter 6 is organized as follows. In Section 6.2, we introduce the robot kinematic model and the trajectory tracking and point stabilization problems of a nonholonomic mobile robot. A firststate contractive MPC algorithm is proposed in Section 6.3. Stability results of the proposed algorithm can be found in Section 6.4. In Section 6.5, simulation results are presented to show the effectiveness of our method. Finally, a summary is given in Section 6.6. In Chapter 7, the FSCMPC algorithm is extended to multirobot formations. With the capability of simultaneous tracking and point stabilization, the FSCMPC controller can achieve some practical formations without any special treatments. Simulation results are provided. Finally, in Chapter 8, we summarize the main results of this dissertation and outline the future work. 11 CHAPTER 2 Nonholonomic Mobile Robots and Formations 2.1 Nonholonomic Mobile Robots A nonholonomic mobile robot is a robot can only move in the direction normal to the axis of the driving wheels. Most of ground mobile robots (wheeled), part of aerial and underwater vehicles which move under some specific conditions can be considered as nonholonomic mobile robots. Nonholonomic mobile robots have received a lot of attention in the past decades because of their ability to work in large application domains, such as: (i) Transportation, (ii) planetary exploration, (iii) surveillance, (iv) security inspection, (v) military targets tracking and attack, and (vi) humanmachineinterfaces for people with mobility deficiency, to mention a few. Due to the wide range of applications, the research of nonholonomic mobile robots is multidisciplinary and has many directions. As the dissertation objective stated in Section 1.2, we only consider the formation control of a group of vehicles and the trajectory tracking and point stabilization of nonholonomic mobile robots. 12 2.1.1 Kinematic Model The kinematic model for a wheeled mobile robot (assumed to be of the unicycle type) under the nonholonomic constraint of pure rolling and nonslipping is considered throughout this dissertation q_ = S(q)u; (2.1) where q(t); q_(t) 2 R3 are defined as q = [x y µ]; q_ = [x_ y_ µ_]: x(t) and y(t) are the position of the center of mass of the wheeled mobile robot in a Cartesian coordinate frame. µ(t) 2 R1 denote the orientation of the robot (see Figure 2.1). x_ (t) and y_(t) are the Cartesian components of the linear velocity v 2 R1 and µ_(t) 2 R1 denotes the angular velocity. q v X Y O Figure 2.1: Nonholonomic wheeled mobile robot. The matrix S(q) 2 R3£2 is defined as follows S(q) = 2 66664 cos µ 0 sin µ 0 0 1 3 77775 ; (2.2) 13 and the velocity vector u(t) 2 R2 is defined as u = 2 64 v ! 3 75 ; (2.3) where v and ! are the linear linear and angular velocity, respectively. The pure rolling and nonslipping constraint means the robot can only move in the direction normal to the axis of the driving wheels. Mathematically, this constraint can be expressed as x_ sin µ ¡ y_ cos µ = 0: (2.4) A detailed analytical study of the kinematics of wheeled mobile robots (includes other types such as tricycle and carlike) can be found in [9]. For nonlinear systems, linear approximations can be the first step for analysis and control design. As we know, if the tangent linearized system is controllable, then the original nonlinear system is at least locally controllable and feedback stabilizable. By linearizing the system (2.1) about the equilibrium point (q = 0; u = 0), we have q_ = 2 66664 1 0 0 0 0 1 3 77775 2 64v ! 3 75 : (2.5) Clearly, by examining the rank of the controllability matrix C = 2 66664 1 0 0 0 0 1 3 77775 ; (2.6) the linear system is not controllable. For driftless nonlinear systems in the form z_ = Xm i=1 gi(z)ui; (2.7) 14 where z 2 Rn and u 2 Rm, a sufficient condition (accessibility rank condition) for controllability is that, for any z, the dimension of the involutive closure of the distribution generated by the vector fields gi is equal to n [10], that is dim finv¢g = n; ¢ ´ span fgig: (2.8) For the system (2.1), we have n = 3, vector fields g1 = 2 66664 cos µ sin µ 0 3 77775 ; g2 = 2 66664 0 0 1 3 77775 ; and rankfg1; g2; [g1; g2]g = rank 2 66664 cos µ 0 sin µ sin µ 0 ¡cos µ 0 1 0 3 77775 = 3; (2.9) where [g1; g2] is the Lie bracket of g1 and g2 [g1; g2] = @g2 @p g1 ¡ @g1 @p g2: (2.10) Therefore, dim finv¢g = n and the system is controllable. However, for nonlinear systems, the existence of smooth timeinvariant state feedback control laws cannot be implied from the controllability. This will be discussed in Section 2.1.2. For control design, with a change of state coordinates, the model equations of the robot can be transformed into a simpler form. The following change of coordinates [11] 2 66664 x1 x2 x3 3 77775 = 2 66664 0 0 1 cos µ sin µ 0 sin µ ¡cos µ 0 3 77775 2 66664 x y µ 3 77775 ; (2.11) and the change of inputs u1 = !; u2 = v ¡ !x3; (2.12) 15 transform the system (2.1) into x_ 1 = u1; x_ 2 = u2; (2.13) x_ 3 = x2u1: This system belongs to a general class of systems called chained system [12] which has the form x_ 1 = u1; x_ 2 = u2; x_ 3 = x2u1; (2.14) ... x_ n = xn¡1u1: 2.1.2 Brockett’s Theorem The problem of smooth timeinvariant state feedback stabilization can be defined as follows. Definition 2.1 Find a state feedback u = k(q), where k(q) is a smooth function of q, such that the closedloop system q_ = S(q)k(q) = f(q); (2.15) is asymptotically stable. However, as mentioned earlier, a controllable nonlinear system does not mean that it can be stabilized by a smooth timeinvariant feedback control law. A general theorem on necessary conditions for smooth feedback stabilization of nonlinear systems is given by Brockett in [13]. 16 Theorem 2.1 Consider the nonlinear system x_ = f(x; u) with f(x0; 0) = 0 and f(¢; ¢) continuously differentiable in a neighborhood of (x0; 0), necessary conditions for the existence of a continuously differentiable control law for asymptotically stabilizing (x0; 0) are: 1. The linearized system has no uncontrollable modes associated with eigenvalues with positive real part, 2. There exists a neighborhood N of (x0; 0) such that for each » 2 N there exists a control u»(t) defined for all t > 0 that drives the solution of x_ = f(x; u») from the point x = » at t = 0 to x = x0 at t = 1, 3. The mapping ° : N £ Rm ! Rn, N a neighborhood of the origin, defined by ° : (x; u) ! f(x; u) should be onto an open set of the origin. Details and the proof of this theorem can be found in [14]. In the particular case of driftless systems, a corollary to Brockett’s theorem is the following [10]. Corollary 2.1 Consider a driftless system of the form q_ = Xm i=1 gi(q)ui; z 2 Rn u 2 Rm; m · n; (2.16) where the gi are smooth vector fields. If the vectors gi(0) are linear independent, i.e. rank[g1(0); g2(0); ¢ ¢ ¢ ; gm(0)] = m; (2.17) then a solution to the stabilization problem defined in Definition 2.1 exists if and only if m = n. Since Corollary 2.1 is not satisfied in the system (2.1) (n = 3, m = 2), stabilizing smooth timeinvariant feedback laws u = k(q) do not exist for the nonholonomic mobile robot. 17 2.1.3 Nonholonomic Mobile Robot Control In this section, a brief literature review focusing on the trajectory tracking and point stabilization problems of nonholonomic mobile robots is given. A detailed summary of developments in control of nonholonomic systems can be found in [15]. The trajectory tracking problem focuses on stabilizing robots to a timevarying trajectory. Nonlinear feedback controllers are mostly found in the literature. Early results on local controllers can be found in [16, 10] using Lyapunov direct method. The problem is globally solved in [17] by nonlinear feedback. Dynamic feedback linearization is applied in [18, 19, 20]. Other techniques include approximate linearization [21], sliding mode control [22, 23] and backstepping [24, 25, 26]. A recursive technique for trajectory tracking of nonholonomic systems in chained form is derived from the backstepping paradigm [27]. However, a major restriction is that, the tracked linear velocity and angular velocity must not converge to zero, which means the tracked trajectory must be continuously excited. This restriction makes it impossible for a single controller to handle the tracking problem and point stabilization problem simultaneously. Consequently, the range of applications of the above mentioned controllers is limited. In addition, according to the authors in [28], the nonlinear internal dynamics of the closedloop system under output feedback linearization controllers exhibit unstable properties when robots track a trajectory moving backward. To overcome this issue, fullstate tracking techniques are explored in [29]. Model predictive controllers are reported in [30] for trajectory tracking. The MPC approach is based on the quasiinfinite algorithm proposed by authors in [31]. The point stabilization problem, which considers stabilizing robots to a final goal posture, is more challenging. As pointed out in Section 2.1.2, a smooth timeinvariant feedback control law does not exist according to Brockett’s theorem. Most of the control algorithms for point stabilization can be classified into three categories: (i) Smooth timevarying algorithms, (ii) discontinuous or piecewise smooth feedback laws, and (iii) middle strategies (discontinuous and timevarying). Smooth timevarying stabilization is pioneered by Sam 18 son [11, 32, 33] and explored by other authors in [34, 35, 4, 36]. For discontinuous feedback controllers, see [37, 38, 10, 39, 40, 41, 42]. Middle strategies include [43]. As pointed out by authors in [40], timevarying control laws are extremely complex and only for a special class of nonholonomic systems a general strategy is available. Moreover, timevarying control laws produce very slow convergence and are intrinsically oscillating. A comparative experimental study of timevarying and discontinuous controllers for nonholonomic mobile robots is reported in [44]. Other techniques, such as dynamic feedback linearization [20], model predictive control [45], adaptive [46], neural networks [47], and backstepping [24] are also found in the literature. Interesting results are given in [48] and [49]. With a special choice of the system state variables in polarlike coordinates, smooth feedback control laws can be globally stable. Since in those polarlike coordinates, the state is not defined at the origin, Brockett’s Theorem does not hold anymore and a smooth timeinvariant state feedback law for global asymptotic stability is not prevented by Brocketts negative result. The problem of design controllers which can be used for both tracking and stabilization tasks has been explicitly addressed in [50] and [26]. Other controllers with simultaneous tracking and stabilization capabilities can be found in [51, 52, 20, 30]. Most of the controllers mentioned above only consider the kinematics of the vehicle. For the sake of handling dynamics, backstepping techniques are commonly used. The steps can be: (i) Design velocity controllers for the kinematic system (all the control algorithms mentioned above can be used), (ii) design a feedback velocityfollowing control law such that the robot’s velocities asymptotically converge to the velocity inputs generated by the first controller, and (iii) calculate the actual toques by a computedtorque feedback controller with the second control signal as inputs. See [25], [26] for trajectory tracking, [53] for point stabilization and [24] for both. 19 2.2 Formations 2.2.1 Shapes and Positions A formation describes the specific relationship among robots in a group. Shape and position are the two important characteristics of a formation. Figure 2.2 shows some of the common formation shapes in consideration, such as line, column, diamond, and wedge [54]. (a) Line. (b) Column. (c) Diamond. (d) Wedge. Figure 2.2: Formation shapes. Besides the shape, each robot must have a specific position in the formation. Three 20 techniques for formation position determination have been identified in [54]. They are unitcenterreferenced, leaderreferenced and neighborreferenced (see Figure 2.3). In a unitcenterreferenced position, each robot maintains its desired relative position to a unitcenter point which is the average of the x and y positions of all the robots involved in the formation. In a leaderreferenced position, each robot (except the leader) maintains its desired relative position to a leader robot. In a neighborreferenced position, each robot maintains its desired relative position with respect to one other predetermined robot. (a) Unitcenterreferenced. (b) Leaderreferenced. (c) Neighborreferenced. Figure 2.3: Formation position determination. 21 2.2.2 Formation Control Strategies Due to the attention received by formation control research, a lot of control strategies have been proposed. Approaches can be classified into leaderfollowing, virtual structure, behavior based, and graph theory based. The leaderfollowing formation control is a important method [55, 56, 57, 58, 59, 60, 61]. In this approach, one or some mobile robots are designated as leaders which take care of the assignment of followers’ relative positions and the global mission objective, such as trajectory tracking and obstacle avoidance. Simplicity is the major advantage of this approach since only the leader takes care of the global objective while the formations are guaranteed by individual robot’s control law. However, when a singleleader architecture is used, the whole system will fail if the leader fails (single point of failure). In addition, the full state of the leader must be communicated to each member of the formation. Leader failure includes robot malfunction and communication error. Some attempts have been made to overcome the single point of failure. In [55], the architecture has some leader failure detection mechanisms and redefines the formation control graph according to some predefined strategies. As the number of robots increase, string stability and mesh stability need to be addressed [62, 63]. In the virtual structure approach, the entire formation is treated as a single entity. Desired states for each vehicle in the formation can be specified by placeholders in the virtual structure [64]. Similar ideas include the virtual leader [65], and the formation reference point [66]. With these approaches, it is easy to prescribe the coordinated behavior for the group. In addition, the virtual structure approach can maintain the formation during manoeuvres and a rigid geometric relationship among multiple robots. However, the requirement of the the formation to act as a virtual structure limits the class of potential application. Using virtual structure approach for mobile robots formation control is proposed by Lewis [67] and studied in [68, 69, 70, 71, 72]. The behavior based formation control approach [73, 74, 75, 76, 77, 78] is inspired by 22 formation behaviors in nature, like flocking and schooling. In [79], the author simulates flocks of birds and schools of fish with a simple egocentric behavioral model. In [54], the authors define two steps for formation maintenance: (i) Detectformationposition which determines the robot’s position in formation, and (ii) maintainformation which generates motor commands to direct the robot toward the correct location. In behavioral approaches, the control action for each robot is defined by a weighted average of the control corresponding to each desired behavior. Possible behaviors can be, collision avoidance, obstacle avoidance, goal seeking and formation keeping. The advantage of the behavioral approach is that, when robots have multiple competing objectives, it is easy to derive control laws in a natural way. However, even this approach performs well in simulations and experiments, it is hard to do mathematical analysis of stability and robustness. Recently, graph theory is applied to multiple robot formation control [80, 81, 82, 83, 84, 85]. Formations can be described by graphs treating each robot as a vertex and the relationships (control, sensing, communicationflow) among robots as edges. The communication links among systems are described by Laplacian matrices. The stability of the multirobot system is guaranteed by the stability of each robot. However, the method is limited to linear robot models. In [86], the authors propose two controllers for nonholonomic mobile robots formation using graph theoretical methods. In the first controller, the robot model is transformed to a linear model by dynamic feedback linearization. The second controller, with the aid of the timescaling technique and the properties of Laplacian matrix, overcomes the singularity of the first controller. 2.3 Summary In this chapter, a brief review of nonholonomic mobile robots is given. Then the kinematic model is developed and the Brockett’s theorem is introduced. In addition, a short literature review on nonholonomic mobile robots control of trajectory tracking and point stabiliza 23 tion is given. Since formation control is one of the dissertation objectives, the shape and position of a formation are introduced. Different formation control algorithms, such as leaderfollowing, virtual structure, behavior based and graph theory based approaches are reviewed. 24 CHAPTER 3 Model Predictive Control 3.1 Introduction In the past decades, model predictive control (MPC), also known as receding horizon control (RHC), has received great interest in the control community. As an effective method to solve multivariable constrained control problems, MPC has appeared in industry for more than 20 years and successful applications of MPC can be easily found. The glorious past and present of MPC is due to its abilities of constraint handling, realtime prediction, optimizing and feedback correcting. Different from conventional control which uses precomputed control laws, MPC is a control of the form in which the current control action is obtained by solving a finite horizon optimal control problem online at each sampling instant. The optimization yields an open loop optimal control sequence and the first control of this sequence is applied to the plant. The whole process will repeat at the following sampling instants. Actually, the term Model Predictive Control does not designate a specific control strategy but a large range of control methods which explicitly use a plant model to calculate the control action by minimizing a finite horizon optimal control problem. The ideas appearing in all the predictive control family are: (i) A model of the plant which is used to predict the future response of the system at future time instants (horizon), (ii) the calculation of a 25 sequence of control action minimizing an optimal control problem using system’s current states as the initial condition, and (iii) the receding strategy which involves the application of the first control action at each step, so that the horizon moves towards the future at each instant. Practically, this combination of feedforward and feedback makes MPC to outperform ”passive” feedback control. As a summary, the advantages of MPC can be [87]: ”Generality  the ability to include generic models (linear or nonlinear) and constraints in the optimal control problem; Reconfigurability  the ability to redefine cost functions and constraints as needed to reflect changes in the system and/or environment”. Though MPC has gained great success in process industries, its applications for fast updating systems are dragged by the computational burden. Since an optimal control problem must be solved online, the sampling period needs to be long enough for the calculation. In process industries, usually plants under control are sufficiently ‘slow’ and can be satisfactorily linearized, the computational burden is not that critical. However, in highly nonlinear systems, an optimal solution may not be able to determined within a short sampling period. Sometimes, even a feasible solution may not be possible. Another issue is that, the stability of MPC algorithms is not automatically guaranteed since the control sequence is obtained from solving a finite optimal control problem. The implicit nature of the closedloop system makes the proofs of stability of MPC a complicated job. 3.2 MPC Strategy The strategy of MPC is depicted in Figure 3.1. This figure shows a generic MPC algorithm for a singleinputsingleoutput (SISO) system. At current time, say k, the system’s future response (predicted output) yp(k) on a finite horizon Np, say [k k + Np], is predicted by the system model and the predicted control input up(k), [k k + Nm]. Np is named as the prediction horizon and Nm is named as 26 the control horizon (Nm · Np). Usually, the system’s future response is expected to return to a desired set point s(k) by following a reference trajectory r(k) from the current states. The difference between the predicted output yp(k) and the reference trajectory r(k) is called predicted error. An finite horizon optimal control problem with a performance index that usually penalizes the predicted control input and the predicted error is solved online and an optimal control input u¤(k), [k k + Nm], which minimizes the predicted error is obtained. Only the first element of u¤(k) is implemented to the plant. All the other elements are discarded. Then, at the next time interval k + 1, the whole procedure is repeated. The predicted control input up(k + 1) at time k + 1 can be built by u¤(k) with linear extrapolation. Since the prediction horizon and control horizon move one step further into future at each time interval, MPC is also named as receding horizon control (RHC). In order to implement this strategy, the basic structure shown in Figure 3.2 is used. 3.3 MPC Formulation Though in industry, finite impulse response (FIR) or finite step response (FSR) models are used, MPC is always formulated based on statespace models in academia and literature. Consider the following discretetime linear timeinvariant system: x(k + 1) = Ax(k) + Bu(k); (3.1) where x(k) 2 Rn and u(k) 2 Rm are the state and control input, respectively. A 2 Rn£n and B 2 Rn£m are the state and input matrices, respectively. The MPC implementation can be formulated by introducing the following openloop optimization problem at every time interval k: min u(¢) J(Np;Nm)(xk); (3.2) 27 r(k) s(k) up(k) yp(k) u*(k) Figure 3.1: MPC concept. Model Optimizer Predicted Outputs Reference Trajectory Predicted Errors +  Past Inputs and Outputs Future Inputs Cost Function Constraints Figure 3.2: Basic structure of MPC. 28 subject to x(k + 1) = Ax(k) + Bu(k); xmin · x(k + i) · xmax; i = 1; 2; ¢ ¢ ¢ ;Np; umin · u(k + i) · umax; i = 0; 1; 2; ¢ ¢ ¢ ;Nm: (3.3) The performance index is defined as J(Np;Nm)(xk) = xT (Np)Px(Np) + NXp¡1 i=1 xT (k + i)Qx(k + i) + XNm i=0 uT (k + i)Ru(k + i); (3.4) where P 2 Rn£n, Q 2 Rn£n, and R 2 Rm£m are the weighting matrices, and P = PT > 0, Q = QT > 0, R = RT > 0. Np and Nm denote the length of the prediction horizon and the length of the control horizon, respectively. Usually, Np ¸ Nm. The first term on the right hand side of equation (3.4) is called terminal state penalty, the second term is called state penalty and the third term is called control penalty. Equations (3.1)(3.3) define a quadratic program problem and many algorithms and software packets are available to solve it. When the optimal control sequence u¤ (Np;Nm)(i j x(k)), i = 0; ¢ ¢ ¢ ;Nm1 is obtained, only the first control u¤ (Np;Nm)(0 j x(k)) is applied to the system. The rest of the control sequence is discarded. Then at the next time interval k + 1, x(k + 1) is used as the new initial condition of the optimal problem (Equation (3.2) and the process is repeated. As for nonlinear systems, the concept of MPC remains the same. Consider the following continuoustime nonlinear system, x_ (t) = f(x(t); u(t)); (3.5) where x(t) 2 Rn and u(t) 2 Rm are the state and control input, respectively. Similar to the linear case, the optimization problem can be defined as: min u(¢) JT (x(t); u(¢)); (3.6) 29 subject to x_ (t) = f(x(t); u(t)); xmin · x(s; x(t); t) · xmax; t · s · t + T; umin · u(s) · umax; t · s · t + T: (3.7) The performance index is defined as: JT (x(t); u(¢)) = Z t+T t (k¹x(s; x(t); t)k2 Q + ku(s)k2 R)ds +k¹x(t + T; x(t); t)k2 P : (3.8) In this case, the prediction horizon and the control horizon are the same and equal to T. The generic MPC algorithm can be described as follows, 1. At current time t or k, measure the current state x(t) or x(k) as the initial condition, 2. Solve the finite optimization control problem (3.2) or (3.6) with the initial condition obtained in 1, yielding the optimal control sequence u¤ over the control horizon 3. Apply the first element in control sequence u¤ to the system, the remaining elements of the control sequence is discarded, 4. At time t + ±t or k + 1, repeat from 1. Note that, ±t is the sampling time and k = (t ¡ t0)=±t. 3.4 Literature Review Since the objective of this dissertation is to use model predictive control to coordinate the motion of nonholonomic mobile robots, a review of theoretical results in MPC is now given. Two thorough survey papers [88], [89], which give good reviews on MPC’s past, present and future, stability and optimality, are an excellent starting point for any interested reader in this area. 30 The history of model predictive control is quite different than other control design tools. This technique has its origin from industry before any theoretical foundation. The development of MPC can be traced to the work of Kalman in the early 1960s, which is known as the Linear Quadratic Gaussian (LQG). However, at that time, the industrial process control community either had no exposure to LQG technique or regarded it as impractical. The LQG failed to have a strong impact. This environment led to a more general model based control methodology developing in industry, in which the dynamic optimization problem is solved online at each control execution. The first description of MPC appeared in 1976 [90] and later summarized in [91]. The authors called their approach model predictive heuristic control (MPHC), but the solution software is usually mentioned as IDCOM, an acronym for IDentification and COMmand. The main features of the IDCOM approach are: impulse response model, quadratic performance objective, reference trajectory for future plant output behavior, including input, and output constraints and heuristic iterative optimization algorithm. Engineers at Shell Oil developed their own MPC technology and an unconstrained multivariable control algorithm which they named dynamic matrix control (DMC) was presented in [92] and [93]. The main features of the DMC approach can be summarized as follows: linear step response model, quadratic performance objective, setpoint for future plant output behavior, and leastsquares optimization algorithm. Later, the DMC algorithm was posing into a quadratic program (QP) in which input and output constraints appear explicitly. This modified DMC algorithm is called quadratic DMC (QDMC) [94, 95]. After the publication of papers addressing IDCOM and DMC/QDMC, interest in this filed starts to surge and new algorithms have been developed. Today, MPC applications have made this machinery a multimillion dollar industry. A survey of commercially available MPC technology can be found in [96]. Figure 3.3 [96] shows the approximate genealogy of industrial MPC algorithms. Although the model predictive control formulation seems quite intuitive, the stability is 31 Figure 3.3: Approximate genealogy of industrial MPC algorithms. not automatically guaranteed since the control sequence is obtained from a finite optimal control problem. Without the fine tuning of weighting matrices, the MPC algorithm formulated in (3.2), (3.6) may lead to divergent responses. Therefore, it is not surprising that much effort has been devoted to determine sufficient conditions for stability. As a powerful analysis tool, Lyapunov methods are frequently encountered in MPC literature. Pointed out by the authors in [89], the performance index function is monotonic and it can be used as a Lyapunov function. The stability analysis of MPC has reached a relatively mature stage. A short summary is given in this section. Interested readers are referred to [89], [88] for excellent reviews of the stability properties of MPC. As for the linear system, proofs of stability based on the monotonicity property of the performance index function have been proposed in [97], [98]. To simplify the notation, the prediction horizon and the control horizon are assumed to be equal Np = Nm = N. Then we use JN to replace J(Np;Nm) = JN which is defined in Equation (3.4). The key idea of the monotonicity approach is using the performance index function JN as a Lyapunov function. This means the following inequality of the index function needs 32 to be shown JN(x(k)) ¡ JN(x(k + 1)) ¸ 0 for x 6= 0: Rewriting JN(x(k)) ¡ JN(x(k + 1)) gives: JN(x(k)) ¡ JN(x(k + 1)) = [xT (k)Qx(k) + u¤T N (x(k))Ru¤ N(x(k))] +[JN¡1(x(k + 1)) ¡ JN(x(k + 1)]: (3.9) With the assumption that Q > 0 and R > 0, the first term on the right hand side of Equation (3.9) is positive. However, in general, it is hard to assert whether the second term is nonnegative. Several approaches have been proposed to assure the constantly decrease of the performance index JN. In most cases, if the open loop system is stable, by choosing the weighting matrix P as the solution of the Lyapunov equation [99] ATPA + Q = P; (3.10) JN is nonincreasing and the stability can be guaranteed. In [100], the authors prove that when a terminal state equality constraint x(k +N) = 0 is imposed, the performance index JN is nonincreasing as a function of N. Then stability follows. Another approach is to add a terminal constraint that forces the terminal state to be inside a positively invariant region. The decreasing property of JN can be achieved by introducing a stabilizing local controller u(k + i) = Lx(k + i) for i > N. In this case, the terminal penalty and the positively invariant region need to be defined with respect to the system x(i + 1) = (A + BL)x(i) rather than x(i + 1) = Ax(i). Furthermore, the positive invariance should be defined with the respect to the input and state constraints. The local feedback controller can be chosen by the infinite horizon unconstrained LQR method [101]. 33 As for the nonlinear system, approaches introduced above are the idea underlying nonlinear MPC. With a nonlinear system model used, the optimization control problem to be solved online becomes nonlinear programming. In [97] and [102], the idea of zero terminal constraints for nonlinear MPC is analyzed. The performance index function is employed as a Lyapunov function. To guarantee stability, a global optimum must be found at each time step. Though theoretically, the optimization problem with terminal equality constraint can be solved, the computation for finding the global optimum is very expensive. Even when a feasible solution exists, the convergence to that solution is not guaranteed. A dualmode MPC algorithm is proposed by authors in [103] to deal with both the global optimality and the feasibility problems. A terminal region is introduced to relax the terminal equality constraint. At the end of the prediction horizon, the terminal region must be reached. Inside this region, an asymptotically stabilizing controller is employed. With these modifications, a global optimum is no longer required. A feasible solution at the initial time will guarantee the feasibility at all future time steps. However, the terminal region is hard to calculate except for low order systems. An algorithm called quasiinfinite MPC proposed by authors in [31] can overcome both the global optimization and the feasibility problem without using controller switching. In this method, the performance index function is formulated as JT (x(t); u(¢)) = Z t+T t (k¹x(s; x(t); t)k2 Q + ku(s)k2 R)ds + k¹x(t + T; x(t); t)k2 P : The openloop optimal control problem besomes min u(¢) JT (x(t); u(¢)); subject to ¹x(t + T; x(t); t) 2 : (3.11) A weight matrix P needs to be determined such that the penalty of the terminal state ¹x(t+ T), which is the second term on the right hand side of the performance index, is bounded 34 by the infinite horizon cost after t + T k¹x(t + T; x(t); t)k2 P · Z 1 t+T (k¹x(s; x(t); t)k2 Q + ku(s)k2 R)ds 8¹x(t + T; x(t); t) 2 (3.12) The bound is established by assuming that the nonlinear system is controlled by a linear optimal state feedback controller within the region after t + T. Again, a feasible control sequence solution at time t means feasible solutions in the future and stability of the closedloop system is guaranteed. However, the difficulty of terminal region calculation is not improved by this quasiinfinite MPC method. A contractive MPC idea is proposed in [104] and completed and proven in [105]. A constraint is added to the MPC formulation which forces both the actual and predicted state to contract. With this requirement, the stability can be proven. All the methods introduced above need to solve nonlinear programming problems at each time step. Compare to the linear case, the computational requirement is huge. Intuitively, we could linearize the system. Then with the linearized system, all the methods developed for linear systems can be employed. This kind of approach can be found in [106], [107], and [108]. 3.5 Summary In this chapter, a brief introduction of MPC is given. MPC formulations for linear systems and nonlinear systems are briefly discussed in discretetime and continuestime. Since a finite horizon optimal control problem is solved inside the MPC algorithm, the control law is not guaranteed to be stable. Different stabilizing MPC methods are introduced for linear and nonlinear systems. 35 CHAPTER 4 Nonlinear Formation Control of Unmanned Aerial Vehicles In this chapter, we consider the problem of designing nonlinear formation controllers on a team of unmanned aerial vehicles (UAV) using offtheshelf autopilots. Three nonlinear formation controllers are presented. The first two controllers require knowledge of the leader’s velocity and acceleration. The third controller, on the other hand, does not have such requirements. Under these controllers, the formation of UAVs is proven to be stable. Simulations validate the performance of the proposed controllers. 4.1 Introduction Due to recent developments in electronics and computing, it is now possible to find small size, light weight, powerful embedded computers, wireless network equipments and sensing devices on the market. As a result, by exploiting current technology, one can build a group of relatively small UAVs each having satisfactory capabilities within a reasonable budget. For tasks such as obtaining sensory measurements over a wide area (e.g., forest fire monitoring [109]), multiple UAVs are desirable because they can accomplish the task more efficiently than a single UAV. Interested readers are referred to [110] where a survey 36 of UAV cooperative control is provided. Teams of UAVs moving in formations with precisely defined geometries lead to many advantages, such as energy saving when the vortex forces are taken into account. Several experimental studies have verified the energy saved when flying in close formations [3]. In addition, formation control allows for intelligent leaders and single agent planning while followers can focus on other tasks. Leaderfollowing is a common approach to build formations of multivehicle systems. The challenge here lies in designing a formation controller that is computationally simple, yet robust. In [111], a leaderfollowing approach for formation flight is designed using input/output feedback linearization techniques. Furthermore, in [112] a framework for controlling a group of UAVs is developed. The controller design utilizes input/output dynamic linearization techniques based on a model that included the induced rolling moment generated by the lead aircraft over the wing aircraft. In [113], formation controllers are designed to maintain an optimal longitudinal separation needed to achieve the maximal reduction in the induced drag. Authors in [114] develop an interesting experimental testbed to investigate close formation flight. In this chapter, based on a cost effective autopilot1, we propose a twolayer control architecture for practical and robust formation control. In this control scenario, the autopilot provides stable velocities and height tracking on the lowerlevel during the mission flight. On the higherlevel, nonlinear formation controllers ensure that leader and follower UAVs are in formations with desired relative distances and bearing angles. The remaining of the chapter is organized as follows. Section 4.2 gives a brief introduction of aircraft dynamics and the leaderfollowing formation. In Section 4.3, we present three different nonlinear formation controllers. Stability results are also provided in this section. Simulation results are presented in Section 4.4. Finally, concluding remarks are given in Section 4.5. 1A product of Cloud Cap Technology, Inc. 37 4.2 Preliminaries Clearly, unmanned aerial vehicles are aircrafts without pilots. They are either remotely controlled or capable of conducting autonomous operations. Like manned aircrafts, UAVs can be classified by their sizes, types, methods of propulsion and their missions. They may be fixedwing aircrafts or helicopters. To control UAVs, we need to know the position and velocity of the aircraft in the air. This leads to the study of aircraft dynamics. This section gives a brief introduction of aircraft dynamics and the setup of leaderfollowing formation. 4.2.1 Aircraft Dynamics Aircraft motion may only ”make sense” when it is represented in some coordinate systems. Therefore, it is necessary to define appropriate coordinate systems. In this report, all the coordinate systems are righthanded and orthogonal. The earthfixed reference frame, FE: This coordinate system is defined like this: the origin is fixed to an arbitrary point on the surface of the Earth. The xE axis points to North, the yE axis points to East. Consequently the zE axis points to the center of the Earth. Figure 4.1 shows this coordinate system. Figure 4.1: Earthfixed reference frame. 38 The bodyfixed reference frame, FB: In this report, we use the following definition as the bodyfixed reference frame. First, we assume that the aircraft has a plane of symmetry. Then the xB and zB axes lie in that plane of symmetry. The origin is fixed to aircraft’s center of gravity. The xB axis points to the head of the aircraft, the zB axis lies in the plane of symmetry and points downward. The yB axis is determined by the righthanded rule. The stabilityaxis reference frame, FS: We consider the aircraft in steady flight condition so that the relative wind is from a constant direction as seen from the aircraft. The velocity vector Vc of the aircraft is relative to the air mass. Then the projection of this velocity vector into the aircraft plane of symmetry is defined as xS axis. The origin is fixed to aircraft’s center of gravity. The zS axis lies in the aircraft plane of symmetry and points downward. The yB axis is then determined by the righthanded rule. The windaxis reference frame, FW: The windaxis system is defined as follows. The origin is fixed to aircraft’s center of gravity. The xW axis is in the direction of the velocity vector of the aircraft relative to the air. The zW axis lies in the aircraft plane of symmetry and points downward. The yW axis is determined by the righthanded rule. Note that xW axis needs not lie in the plane of symmetry. The bodyfixed frame, the stabilityaxis frame and the windaxis frame are related by two aerodynamic angles. The angle between the xW axis and the xS axis is called sidelip and is denoted by symbol ¯. The angle between the xS axis and the xB axis is called angleof attack and is denoted by symbol ®. Figure 4.2 shows the three different frames and the two angles. A point in the space can be interpreted differently with the respect to different coordinate systems. Pointed out by the Swiss mathematician Leonhard Euler, one reference frame can be placed in alignment with any other reference frame by three successive axisrotations. The angles to rotate the axes are called Euler angles. With Euler angles, the transformation between different coordinate systems is represented by the transformation matrices and the coordinates of a point in different reference frames can be related. In 39 Aircraft plane of symmetry x S z S V c x W x B Figure 4.2: Reference frames and aerodynamic angles. aircraft dynamics, the Euler angles between different reference frames are given special symbols. They are summarized as follows: TF2;F1 µx µy µz TB;E Á µ Ã TW;E ¹ ° Â TB;W 0 ® ¡¯ Table 4.1: Euler angles. The TF2;F1 notation means transformation from reference frame 1 to reference frame 2. For example, TW;E means transformation from the earthfixed reference frame to the windaxis reference frame. With the coordinate system established, one can deduce aircrafts’ equations of motion by applying Newton’s law (see e.g., [115, 116]). We simplify our analysis to an ideal scenario that the UAVs are flying in a wingslevel steadystate and that the angle of attack and sideslip are considerably small, such that they can be ignored. With such simplification, 40 a 6DOF nonlinear model can be set up as follows: x_ = V cos ° cos Â; y_ = V cos ° sin Â; z_ = ¡V sin °; _V = ¡D m ¡ g sin °; ¹_ = p + (q sin ¹ + r cos ¹) tan °; °_ = q cos ¹ ¡ r sin ¹; Â_ = (q sin ¹ + r cos ¹) sec °; !_ = J¡1!^J! + J¡1¿ + L¹; (4.1) where x, y, z are position states in the flat earthfixed initial frame; yaw angle Â, pitch angle ° and roll angle ¹ are attitude states in the windaxis frame; roll rate p, pitch rate q and yaw rate r are angular velocity states in the bodyfixed frame; V is the linear velocity along the flying path; ! = [p q r]T ; J is the inertia matrix; ^! is a skewsymmetric operator; ¿ is the external moment vector; g is the acceleration of gravity; m is the mass of the UAV; D is the drag and ¹L = [¹L p 0 0]T is the rolling moment induced by the wake of the leader aircraft. Usually, an offtheshelf autopilot can provide two basic operational modes: (i) Waypoint tracking mode, and (ii) holding mode. In the first mode, a set of ordered points of interest (POI) can be uploaded into the autopilot before the mission or during the flight. The autopilot generates a path from the current position along these points of interest and provides control commands to the aircraft. In this mode, however, the user cannot precisely control the aircraft’s position except waypoints. In addition, the distance between two successive waypoints must be long enough such that the autopilot system can generate the flight path. As for the holding mode, usually three channels are provided as follows: (1) the Mach hold, (2) the heading turn rate hold, and (3) the altitude hold. The autopilot continuously executes control commands sent to these hold channels. Although, some successful waypointbased formation flight experiments have been reported in the literature 41 (see [117]), it would be more convenient to use the autopilot in holding mode when close formation flight is required, for instance, navigation in urban environments. In this chapter, we investigate the feasibility of using an offtheshelf autopilot [118] in holding mode for the follower UAVs. With an autopilot running in the holding mode, the lateral and longitudinal movements are decoupled [113]. We can write a simplified model of the aircraft as follows: 2 66664 x_ y_ _Ã 3 77775 = 2 66664 cos Ã 0 sin Ã 0 0 1 3 77775 2 64 V ! 3 75 ; (4.2) _V = ¡ 1 ¿v V + 1 ¿v Vc; (4.3) !_ = ¡ 1 ¿! ! + 1 ¿! !c; ¢¢ h = ¡ 1 ¿ha _h ¡ 1 ¿hb hi + 1 ¿hb hc; where x and y represent the positions in Cartesian coordinates, V is the velocity, Ã is the heading angle, and h is the altitude. Vc, !c, and hc are the commands to the Mach hold, headingturnrate hold, and altitude hold channels of the autopilot, respectively. ¿v, ¿!, ¿ha; and ¿hb are known positive constants that depend on the autopilot. We will only use this simplified aircraft model in this chapter. 4.2.2 LeaderFollowing Formation In this section, we set up a kinematic model for an UAV formation flight system. Assuming that the UAVs are flying at a constant altitude, we can consider an operator specified the ith UAV motion ai 2 R2, i 2 f1; 2; :::;Ng given as follows ai = xi^{ + yi^; (4.4) where xi and yi 2 R represent the respective Cartesian coordinates in an earthfixed reference inertial frame FE. Let Fd jk 2 R2 be a desired formation that allows a wing airplane k 42 to follow a leader airplane j given by Fd jk = [ld jk ´d jk]T ; (4.5) where ld jk 2 R+ is the desired relative distance and ´d jk 2 £¼ 2 ; 3¼ 2 ¤ is the desired relative bearing angle as shown in Figure 4.3. Consequently by changing Fd jk we are able to define different formation shapes. Then the actual formation for a wing airplane k to follow a leader airplane j is described by Fjk = [ljk ´jk]T 2 R £ · ¼ 2 ; 3¼ 2 ¸ ; (4.6) in which the relative distance is defined as ljk(t) = kaj ¡ akk2 ; (4.7) where k¢k2 denotes the standard Euclidean norm. The relative bearing is given by ´jk = ¼ + ³ ¡ Ãj ; (4.8) where ³ = arctan 2(yj ¡ yk; xj ¡ xk): Taking the time derivative of equation (4.6) with system model (4.2), we have _F jk = 2 64 _ ljk ´_jk 3 75 = 2 64 Vk cos °jk ¡ Vj cos ´jk 1 ljk (Vj sin ´jk ¡ Vk sin °jk ¡ ljk!j) 3 75 ; (4.9) with °jk , Ãj + ´jk ¡ Ãk. Vk is the linear velocity of the follower: Vj and !j are the linear and angular velocities of the leader. Our control objective is to design a formation controller which drives the wing UAV to track the desired formation Fd jk. To this end, we define the formation error as e = Fd jk ¡ Fjk; (4.10) where e 2 R2. 43 Xj Yj Zj DX DZ +DY XE YE jk ljk yyj Figure 4.3: Leaderfollowing formation. 4.3 Nonlinear Formation Control 4.3.1 Feedback Linearization Taking the derivative of (4.10) with respect to time and combining with (4.9), the following is obtained _ e = _F d jk ¡ _F jk = _F d jk ¡ 2 64cos °jk 0 ¡ 1 ljk sin °jk 0 3 75 2 64 Vk !k 3 75 ¡ 2 64 ¡Vj cos ´jk Vj ljk sin ´jk ¡ !j 3 75 : (4.11) Since !k does not appear, it is obvious that the input matrix of the error system (4.11) is not invertible. Thus we cannot design the control based on (4.11). To facilitate the control design on this system, we first use the dynamic extension method from [119] to render (4.11) into a relative degree system. Differentiating both sides of (4.11) with respect to time and after some algebraic and 44 trigonometric manipulation, we obtain Äe = Ä Fd jk ¡ Ä Fjk = 2 64 ¡cos °jk ¡Vk sin °jk 1 ljk sin °jk ¡Vk ljk cos °jk 3 75 2 64 V_k !k 3 75 + FÄd jk ¡ 2 64 s1 s2 3 75 ; (4.12) where s1 = 1 ljk V 2 k sin2 ° + 1 ljk V 2 j sin2 ´jk ¡ V_j cos ´jk ¡ Vj sin ´jk!j ¡ 2 ljk VjVk sin °jksin´jk; s2 = ¡ 2 l2 jk VjVk sin(°jk + ´jk) + V 2 j l2 jk sin 2´jk + V 2 k l2 jk sin 2°jk + V_j ljk sin ´jk ¡ 1 ljk Vj!j cos ´jk ¡ !_j : (4.13) Define a new formation control input uk , [ _V k !k]T 2 R2 and notate the input matrix as g(¢) = 2 64 ¡cos °jk ¡Vk sin °jk 1 ljk sin °jk ¡Vk ljk cos °jk 3 75 and S = [s1s2]T , we can write the error system in (4.12) as eÄ = g(¢)uk + FÄd jk ¡ S: (4.14) It is not difficult to check that the new input matrix g(¢) is nonsingular under the condition that the flight speed Vk ¸ Vmin > 0 and ljk ¸ lmin > 0, where Vmin is the UAV stalling speed, and lmin is the minimum distance to avoid collision between the two UAVs. Let us define the control input uk as uk = g¡1(¢)(¡FÄd jk + S ¡ Ke ¡ e_); (4.15) where K = Diag[k1 k2], and k1;2 2 R+. 45 Substitute (4.15) into (4.14), we obtain a new error system eÄ = ¡Ke ¡ e_: (4.16) Clearly, we reduce the nonlinear error system (4.14) into a linear error system. With a carefully chosen K, system (4.16) can be stable. Therefore, under the action of control law (4.15), the formation can be kept. 4.3.2 Sliding Mode Control In section 4.3.1, by using the feedback linearization technique, we obtain a stabilizing control law to reduce a nonlinear error system into a linear stable error system. Though theocratically sound, this technique is practical only under the assumption of a perfect plant model. However, a perfect model is not always available. The feedback linearization technique may not achieve acceptable performance in a real world application. Let us consider a sliding mode controller in this section. Rewrite (4.11) into the following form, _ e = _F d jk ¡ g1(¢)Vk ¡ g2(¢)Vj ¡ g3(¢)!j ; (4.17) where the vector fields g1(¢); g2(¢); and g3(¢) are defined as g1(¢) = 2 64 cos °jk ¡ 1 ljk sin °jk 3 75 ; (4.18) g2(¢) = 2 64 ¡cos ´jk 1 ljk sin ´jk 3 75 ; (4.19) g3(¢) = 2 64 0 ¡1 3 75 : (4.20) Differentiating both sides of (4.17) with respect to time and after some algebraic and 46 trigonometric manipulation, we obtain Äe = Ä Fd jk ¡ g_1(¢)Vk ¡ g1(¢) _V k ¡ _ g2(¢)Vj ¡ g2(¢) _V j ¡ g_3(¢)!j ¡ g3(¢)!_ j = Ä Fd jk ¡ · @g1(¢) @°jk °_ jk + @g1(¢) @ljk _ ljk ¸ Vk ¡ g1(¢) _V k ¡ _ g2(¢)Vj ¡ g2(¢) _V j ¡ g3(¢)!_ j = Ä Fd jk ¡ · @g1(¢) @°jk (!j + ´_jk ¡ !k) + @g1(¢) @ljk _ ljk ¸ Vk ¡g1(¢) _V k ¡ _ g2(¢)Vj ¡ g2(¢) _V j ¡ g3(¢)!_ j = Ä Fd jk ¡ · @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¸ Vk ¡ g1(¢) _V k + @g1(¢) @°jk Vk!k ¡g_2(¢)Vj ¡ @g1(¢) @°jk Vk!j ¡ g2(¢) _V j ¡ g3(¢)!_ j : Thus, Äe = Ä Fd jk ¡ · @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¸ Vk + g(¢)uk + f1(¢)$j + f2(¢)$_ j : (4.21) where f1(¢) = · ¡g_2(¢) ¡@g1(¢) @°jk Vk ¸ 2 R2£2; f2(¢) = · ¡g2(¢) ¡g3(¢) ¸ 2 R2£2; also, we define the leader linear and angular velocity vector as $j = [Vj !j ]T 2 R2. As a new approach in this subsection, we assume that the leader’s linear and angular velocity and acceleration vectors $j and $_ j are known to the follower aircraft (by leader’s onboard instruments and the communication link between the leader and followers). Let us design the following filtered signal r = [r1 r2]T 2 R2 r = e_ + Ke; (4.22) where K = 2 64 k1 0 0 k2 3 75 , and k1;2 2 R+ are design gain constants. Differentiating (4.22) with respect to time and substituting in (4.21) produces r_ = ¢¢ F d jk ¡ µ @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¶ Vk + g(¢)uk +f1(¢)$j + f2(¢)$_ j + Ke_: (4.23) 47 Based on the subsequent Lyapunov analysis, the formation control law becomes uk = g¡1(¢) · ¡ Ä Fd jk + µ @g1(¢) @°jk ´_jk + @g1(¢) @ljk _ ljk ¶ Vk ¡f1(¢)$j ¡ f2(¢)$_ j ¡ Ke_ ¡ Á(r) ¸ ; (4.24) where Á(r) 2 R2 is a sliding vector function so that the sliding condition is guaranteed. We now state the main stability result of this section for the proposed formation controller. Theorem 4.1 The control law of (4.24) ensures stable sliding surface dynamics of the system in (4.22) and that all system signals are bounded under closedloop operation and the tracking error is asymptotically stable in the sense that lim t!1 e; e_ = 0: (4.25) Proof: To prove the theorem, let us construct the following nonnegative function V = 1 2 rT r: (4.26) By using the control law (4.24) in equation (4.23), we have r_ = ¡Á(r): (4.27) Now, taking the time derivative of (4.26) and substituting (4.27) yields _V = rT r_ = ¡rTÁ(r): (4.28) The sliding vector function can be, for instance, Á(r) = r + a[sgn(r1) sgn(r2)]T , where a 2 R is a positive constant. Then (4.28) becomes _V = ¡krk2 ¡ a jrj · ¡a jrj : Therefore, V (4.26) is a Lyapunov function and system (4.22) is asymptotically stable, which means lim t!1 r = 0. From (4.22) and with reference to Lemma A.8 of [120], we conclude that lim t!1 e = 0 and lim t!1 e_ = 0. ¥ 48 Remark 4.1 A generic sliding vector function Á(r) is given in the above control design, so a variety of available sliding vector functions can be substituted in to reduce the chattering and to achieve satisfactory performance. 4.3.3 Robust Control The effectiveness of the control law introduced in Section 4.3.2 is based on the assumption, that the leader’s position, attitude, velocity and acceleration information are all known to the follower. Those data can be obtained by onboard instruments (on leader and/or follower vehicles) and intervehicle communications. This is a common assumption in the formation control literature. However, in reality, due to the payload limitation or communication failure (for example, under electronic countermeasures), this assumption might not always hold. Consequently, control laws that assume full knowledge of leader aircraft’s states may not guarantee a desired formation in the presence of communication failures. In [121], a failure detection and identification system based on an interacting multiplemodel Kalman filter approach is proposed. When communication is unaccessible, it can provide accurate state estimation, which are required in formation flights. In [122, 123], graph theory is used to improve the robustness and fault tolerance of formation control when communication fails. A novel solution to a class of problems in feedback stabilization of coupled systems with limited communication is presented in [124]. In this section, we propose a robust nonlinear formation controller which requires no information of the leader’s velocity and acceleration. Similar idea of the controller’s design and analysis can be found in [125]. Specifically, by expanding the second term on the right side of equation (4.23) along with equation (4.9), we can rearrange (4.23) into r_ = FÄd jk ¡ 2 64 1 ljk sin2 °jk 2 l2 jk cos °jk sin °jk 3 75 V 2 k + (4.29) g(¢)uk + f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj + Ke_; 49 where f1(¢); f2(¢); and f3(¢) are functions of Fjk, Vk, and °jk. Note that f1(¢); f2(¢); and f3(¢) are bounded since ljk ¸ lmin > 0 and Vk 2 L1. We make the following assumptions: Assumption 4.1 During the formation flight, the leader UAV is stably tracking some desired trajectories $d j = £ V d j !d j ¤T , $_ d j = h _V d j !_ d j iT 2 R2 with $d j , $_ d j , Ä$d j 2 L1 so that we can assume $j , $_ j , $Ä j 2 L1. Assumption 4.2 All other terms in (4.29) except $j and $_ j are known. Remark 4.2 Using equation (4.3), it is possible to generate a leader UAV trajectory so that Assumption 4.1 holds. The follower control vector uk in (4.23) becomes uk = g¡1(¢) Ã ¡ Ä Fd jk + 2 64 1 ljk sin2 °jk 2 l2 jk cos °jk sin °jk 3 75 V 2 k ¡2Ke_ ¡ K2e ¡ ¯1 sgn(e(t)) ! ; (4.30) where ¯1 is a constant positive control gain. After substituting uk into (4.29), the closedloop system is given by r_ = f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj ¡Kr ¡ ¯1 sgn(e(t)): (4.31) Before stating the main result of this section, we give the following lemma which will be invoked later. Lemma 4.1 Let the auxiliary function ¡(t) 2 R be defined as follows ¡ , rT [f1(¢)$j + f2(¢)u_ j + f3(¢)$jVj ¡ ¯1 sgn(e)] : (4.32) 50 If the control gain ¯1 is selected to satisfy the sufficient condition ¯1 > kf1(¢)$j + f2(¢)$_ j + f3(¢)$jVjk2 + (4.33) k¡1 min °°°° d(f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj) dt °°°° 2 ; where kmin = minfk1; k2g, then Z t t0 ¡(¿ )d¿ 6 ³b; (4.34) where the positive constant ³b 2 R is defined as ³b , ¯1ke(t0)k1 ¡ eT (t0) [f1(t0)$j(t0) + f2(t0)$_ j(t0) + f3(t0)$j(t0)Vj(t0)] (4.35) where the notation k ¢ k1 denotes the L1 norm. Proof: Before giving a formal proof of Lemma 4.1, we first show that Z t 0 sgn(y)y_d¿ = jy(t)j ¡ jy(0)j: (4.36) Since jyj = p y2; (4.37) taking the derivative of p y2 yields d p y2 dt = 1 2 ¡ y2¢¡1 2 2yy_ = y p y2 y_ = y jyj y_ = sgn(y)y_: (4.38) Then integrating both sides, we obtain Z t 0 sgn(y)y_d¿ = Z t 0 d p y2 dt d¿ = p y2jt 0 = jyjjt 0 = jy(t)j ¡ jy(0)j: (4.39) Thus, equation (4.36) holds. To simplify the notations, let us define (t) = f1(t)$j(t) + f2(t)$_ j(t) + f3(t)$j(t)Vj(t): 51 After substituting (4.22) into (4.32) and then integrating ¡(t) in time, we obtain Z t t0 ¡(¿ )d¿ = Z t t0 eTKT (¿ ) ((¿ ) ¡ ¯1 sgn e(¿ )) d¿ + Z t t0 deT (¿ ) d¿ (¿ )d¿ ¡ ¯1 Z t t0 deT (¿ ) d¿ sgn(e(¿ ))d¿ : (4.40) After integrating the second term on the righthand side of (4.40) by parts, and utilizing equation (4.36) for the third term, we have the following simplified expression Z t t0 ¡(¿ )d¿ = Z t t0 eTKT (¿ ) µ (¿ ) ¡ KT¡1 d(¿ ) d¿ ¡ ¯1 sgn(e(¿ )) ¶ d¿ + eT (t)(t) ¡ eT (t0)(t0) ¡ ¯1ke (t) k1 + ¯1ke (t0) k1: (4.41) We can now upper bound the righthand side of (4.41), that is Z t t0 ¡(¿ )d¿ 6 Z t t0 kminke (¿ ) k1 µ k(¿ )k2 + k¡1 min °°°° d(¿ ) d¿ °°°° 2 ¡ ¯1 ¶ d¿ + ke (t) k1 (k(t)k2 ¡ ¯1) + ¯1ke (t0) k1 ¡ eT (t0)(t0) (4.42) From (4.42), it is easy to see that if ¯1 is chosen according to (4.33), then the first and second term on the right hand side are less than zero, then we have Z t t0 ¡(¿ )d¿ 6 ¯1ke (t0) k1 ¡ eT (t0)(t0): (4.43) Clearly, (4.43) is (4.34). ¥ We now state the main stability result for the second controller in the following theorem. Theorem 4.2 The control law of (4.30) ensures that all system signals are bounded under closedloop operation and the tracking error is asymptotically stable in the sense that lim t!1 e(t); e_(t) = 0: (4.44) Proof: Let P(t) 2 R be an auxiliary function as follows P(t) , ³b ¡ Z t t0 ¡(¿ )d¿ > 0; (4.45) 52 where ³b and ¡(t) have been defined in Lemma 4.1. Based on the nonnegativity of P (t), we define a Lyapunov function candidate V by V , 1 2 rT r + P: (4.46) After taking the time derivative of (4.46), we have _V = rT _ r + _P . Then utilize the closedloop dynamics of (4.31) and (4.45), we can obtain the following expression _V = rT [f1(¢)$j + f2(¢)$_ j + f3(¢)$jVj ¡Kr ¡ ¯1 sgn(e(t))] ¡ ¡ (4.47) Rearranging the first term on the right hand side of (4.47) and use the definition of ¡, we get _V = ¡rTKr 6 ¡kminkrk2: (4.48) Therefore, r (t) 2 L1 \ L2 and lim t!1 r(t) = 0. With (4.22) again, it is easy to see e(t); e_(t) 2 L1 \ L2 and lim t!1 e(t); e_(t) = 0. ¥ 4.4 Simulation Results In this section, the performance of proposed controllers is tested by simulations. In order to minimize the chattering of sliding mode controller, instead of sgn(¢), arctan(¢) is used in (4.24) and (4.30). To simplify notation, the control algorithms described in Sections 4.3.1, 4.3.2 and 4.3.3 are referred as C1, C2, and C3 , respectively. 4.4.1 Formation under Feedback Linearization Controller The desired formation in this simulation is ld jk = 100 m and ´d jk = 5 4¼ rad. Total simulation time is 60 seconds. Controller parameter is K = Diag[2; 2]. The leader is given a constant angular velocity command !jc = 0:05 rad/s and a constant velocity commandVjc = 17:5 m/s. This means that the leader moves in a circle. 53 The initial conditions of the leader are xj(0) = yj(0) = 0 m, zj(0) = 1000 m, Ãj(0) = ¼ rad, Vj(0) = 17:5 m/s, and !j(0) = 0 rad/s. For the follower, the initial conditions are xk(0) = 100 m, yk(0) = 200 m, zk(0) = 1000 m, Ãj(0) = ¼ rad, Vk = 20 m/s, and !k(0) = 0 rad/s. Figure 4.4 to Figure 4.7 show the response of the feedback linearization formation controller C1. The trajectory of the 2UAV team under the action of C1 is presented in Figure 4.4. Figure 4.5 and 4.6 depict the control inputs and formation errors when C1 is used. The actual relative position is shown in Figure 4.7. As it can be seen, formation errors converge to zero and the follower UAV is able to maintain the desired relative distance and the bearing angle with respect to the leader UAV. 4.4.2 Formation under Sliding Mode Controller In this simulation, the desired formation is ld jk = 100 m and ´d jk = 5 4¼ rad. Total simulation time is 200 seconds. Controller parameter is K = Diag[2; 2]. The leader is given a constant velocity command Vjc = 17:5 m/s. The angular velocity command !jc is 0 rad/s most of the time and !jc = 0:1 rad/s for time periods [30; 45], [60; 75] and [110; 125]. The initial conditions of the leader are xj(0) = yj(0) = 0 m, zj(0) = 1000 m, Ãj(0) = ¼ rad, Vj(0) = 17:5 m/s, and !j(0) = 0 rad/s. For the follower, the initial conditions are xk(0) = 150 m, yk(0) = 20 m, zk(0) = 900 m, Ãj(0) = ¼ rad, Vk = 20 m/s, and !k(0) = 0 rad/s. Figure 4.8 to Figure 4.10 show the response of the sliding mode formation controller C2. The trajectory of the 2UAV team under the action of C2 is presented in Figure 4.8. Figure 4.9 and 4.10 depict the control inputs and formation errors when C2 is used. As it can be seen, formation errors converge to zero the winairplane is able to maintain the desired relative distance and bearing angle with respect to the leader aircraft. 54 4.4.3 Formation under Robust Controller To compare with the sliding mode formation controller, similar simulation setup is used. In this simulation, the desired formation is ld jk = 100 m and ´d jk = 5 4¼ rad. Total simulation time is 200 seconds. Controller parameters are K = Diag[2; 2] and ¯1 = Diag[5; 5]. The leader is given a constant velocity command Vjc = 17:5 m/s. The angular velocity command !jc is 0 rad/s most of the time and !jc = 0:1 rad/s for time periods [30; 45], [60; 75] and [110; 125]. The initial conditions of the leader are xj(0) = yj(0) = 0 m, zj(0) = 1000 m, Ãj(0) = ¼ rad, Vj(0) = 17:5 m/s, and !j(0) = 0 rad/s. For the follower, the initial conditions are xk(0) = 150 m, yk(0) = 20 m, zk(0) = 900 m, Ãj(0) = ¼ rad, Vk = 20 m/s, and !k(0) = 0 rad/s. Figure 4.11 to Figure 4.13 show the response of the sliding mode formation controller C2. The trajectory of the 2UAV team under the action of C3 is presented in Figure 4.11. It is close to the result of Section 4.4.2. Figure 4.12 and 4.13 depict the control inputs and formation error when C2 is used. Again, the controller C3 can maintain the winairplane at the desired relative distance and bearing angle with respect to the lead aircraft. Remark 4.3 Note that the initial amplitudes of the control inputs under C3 are higher than the ones given by C2. We anticipate this behavior since C3 assumes no information about the velocity and acceleration of the leader aircraft. Nevertheless, the overall performance of the closedloop system under C3 is comparable with the performance under C2. C3 is the preferred controller in case of intervehicle communication failure or when the communication bandwidth is limited. A switchinglogic scheme can be designed so that C2 is used when communication between UAVs is possible and the follower UAV has access to its leader’s velocity and acceleration; otherwise, C3 should be switched in. Stability of the switching formation controller becomes an issue that would require further investigation. This analysis is out of the scope of this dissertation. 55 4.5 Summary In this chapter, we present a twolayered control system that allows a team of UAVs to navigate in leaderfollowing formations. At the lowlayer, an offtheshelf autopilot operating in holding mode stabilizes the UAV. At the higher layer, three stable nonlinear formation controllers are developed. This hierarchical control scheme allows a team of UAVs to perform complex navigation tasks under limited intervehicle communication. Specifically, the third robust control law eliminates the requirement of leader’s velocity and acceleration information, which reduces the communication overhead. 56 −300 −200 −100 0 100 200 300 −500 −400 −300 −200 −100 0 100 200 300 L−F Positions X Y Leader Follower Figure 4.4: Trajectories of the UAVs in close formation under the action of C1. 57 0 10 20 30 40 50 60 0 50 100 150 V command t V command (m/s) 0 10 20 30 40 50 60 −5 0 5 10 15 w command t w command (rad/s) Figure 4.5: Control inputs generated by C1. 58 0 10 20 30 40 50 60 −0.5 0 0.5 h error t h error (rad) 0 10 20 30 40 50 60 −200 0 200 l error t l error (m) 0 10 20 30 40 50 60 −2 0 2 y error t y error (rad) Figure 4.6: Formation errors under the action of C1. 59 0 10 20 30 40 50 60 −2.4 −2.3 −2.2 −2.1 −2 h jk t h jk (rad) 0 10 20 30 40 50 60 50 100 150 200 250 l jk t l jk (m) Figure 4.7: Relative position under the action of C1. −1000 −800 −600 −400 −200 0 200 400 600 −1000 −500 0 500 1000 900 950 1000 1050 x L−F Positions y Leader Follower Figure 4.8: Trajectories of the UAVs in close formation under the action of C2. 60 0 20 40 60 80 100 120 140 160 180 200 10 20 30 40 50 60 V command t V command (m/s) 0 20 40 60 80 100 120 140 160 180 200 −10 −5 0 5 10 w command t Figure 4.9: Control inputs generated by C2. 0 20 40 60 80 100 120 140 160 180 200 −0.5 0 0.5 1 h error t 0 20 40 60 80 100 120 140 160 180 200 −100 −50 0 50 l error t 0 20 40 60 80 100 120 140 160 180 200 −1 0 1 2 y error t y error (rad) Figure 4.10: Formation errors under the action of C2. 61 −1000 −800 −600 −400 −200 0 200 400 600 −800 −600 −400 −200 0 200 400 600 800 1000 980 1000 1020 x L−F Positions y Leader Follower Figure 4.11: Trajectories of the UAVs in close formation under the action of C3. 0 20 40 60 80 100 120 140 160 180 200 0 20 40 60 80 100 120 V command t 0 20 40 60 80 100 120 140 160 180 200 −30 −20 −10 0 10 w command t Figure 4.12: Control inputs generated by C3. 62 0 20 40 60 80 100 120 140 160 180 200 −0.5 0 0.5 1 h error t 0 20 40 60 80 100 120 140 160 180 200 −100 −50 0 50 l error t 0 20 40 60 80 100 120 140 160 180 200 −1 0 1 2 3 y error t y error (rad) Figure 4.13: Formation errors under the action of C3. 63 CHAPTER 5 Dualmode Model Predictive Formation Control In this chapter, we consider using model predictive control (MPC) to solve the problem of controlling a team of mobile robots with nonholonomic constraints to leaderfollowing formations. We propose that it is more convenient to put the nonholonomic constraints inside the model predictive control framework. As the first step of exploration, a dualmode MPC algorithm is developed. The stability of the formation is guaranteed by constraining the terminal state to a terminal region and switching to a stabilizing terminal controller at the boundary of the terminal region. The effectiveness of the method is investigated by numerical simulations. 5.1 Introduction A dynamic network consists of spatially distributed dynamic nodes (e.g., autonomous vehicles, mobile sensors) which are coordinated by common set of goals and possible dynamic interaction between the nodes. There are many applications where a dynamic network may be more suitable than a single vehicle, especially where a distributed system of sensors is advantageous. For example, in searchandrescue operations, deployment of many vehicles 64 over an area can allow for more thorough and faster coverage. Other applications, to mention a few, are environmental monitoring, surveillance and reconnaissance, acquisition and tracking. Yet without coordinating the movement of agents, any advantage of multivehicle deployment may be lost and damaging collisions or interference may occur. One interesting problem in multirobot coordination is how to drive a group of robots to a desired formation. Unmanned ground vehicle (UGV) formations can provide a promising and efficient alternative to existing techniques in a wide range of applications. Many researchers have been working on formation problems, and numerous control algorithms can be found in the literature (see e.g., [126],[127], [128]). Recently, model predictive control (MPC) or receding horizon control (RHC) has gained more and more attention in the control community. The inherent ability of MPC to handle constrained systems makes it a promising technique for cooperative control, especially for multivehicle formation control. Recent work includes [127], [128]. Other applications of MPC, such as controlling nonholonomic mobile robots are described in [30], [45]; for multivehicle coordination are given in [129]. The stability and feasibility of the MPC algorithms become a new challenge (see discussion in [130]). In this chapter, based on previous work [131], [6], we show that it is more convenient to put the vehicles’s nonholonomic constraints inside the MPC framework. Moreover, we present a novel MPC algorithm for mobile robot formations. Since a stabilizing terminal controller is switched in within a specified terminal constraint set, the proposed MPC algorithm is dualmode [103]. With this dualmode MPC implementation, stability is achieved while feasibility is relaxed. For the choice of stabilizing terminal controller, a comparison between an inputoutput feedback linearization controller used in [131] and a robust formation controller used in [6] is given. The rest of the chapter is organized as follows. In Section 5.2, some preliminaries are briefly introduced. A dualmode MPC algorithm are proposed in Section 5.3. Stability results are provided in Section 5.4. Section 5.5 contains simulation results. Finally, con 65 cluding remarks are given in Section 5.6. 5.2 Preliminaries The problem considered in this paper is to drive a team of nonholonomic vehicles to a desired formation. This section describes the model used for the mobile agents and the definition of formation. 5.2.1 Vehicle Model Consider the planar motion of nonholonomic unicyclemodel robots whose kinematics are determined by 2 66664 x_ i y_i µ_i 3 77775 = 2 66664 cos µi 0 sin µi 0 0 1 3 77775 2 64 vi !i 3 75 (5.1) where the subscript i 2 [1; : : : ;N] indicates the ith UGV. (xi; yi) are the Cartesian coordinates of the robot, µi 2 (¡¼; ¼] represents the orientation of the robot with respect to the positive x axis, and vi and !i are linear and angular velocities, respectively. 5.2.2 Formation and Formation Control Graph Definition 5.1 A formation is a network of agents interconnected via their controller specifications that dictate the relationships each agent must maintain with respect to its neighbors. The interconnections between agents are modeled as edges in a directed acyclic graph, labeled by a given relationship [59], [132]. Definition 5.2 A formation control graph G = (V; E;F) is a directed acyclic graph consisting of the following: ² A finite set V = (V1; : : : ; VN) of N vertices and a map assigning each vertex Vi to a control system (5.1). 66 ² An edge set E ½ V £ V of pairwise neighbors encoding the formation between agents. If the ordered pair (Vi; Vj) 2 E, then (Vj ; Vi) =2 E, and (Vk; Vj) =2 E for all k 2 f1; : : : ;Ngni. ² A set of constants F = © Fd ij 2 R¡ £ R ª defining control objectives, or set points, for each node j, such that (Vi; Vj) 2 E for some Vi; Vj 2 V. Consequently, by changing Fd ij , we are able to define different formation shapes for the mobile robot team. 5.3 Controllers for MultiRobot Coordination 5.3.1 Formation Error Let a triplet pi = [xi yi µi]T describe the position and the orientation of the ith mobile robot. Let Fd ij = £ ¢xd ij ¢yd ij ¤T be the desired formation between robots i and j. ¢xd ij 2 R¡ and ¢yd ij 2 R are the desired position for robot j in a local Cartesian reference frame C attached to robot i. Then the actual formation for robotpair i and j is described by Fij = [¢xij ¢yij ]T . Figure 5.1 shows the formation configuration for two UGVs. Let us define the formation error for the jth robot in a robotpair (Ri; Rj) xej = 2 66664 ¢xd ij ¡ ¢xij ¢yd ij ¡ ¢yij µi ¡ µj 3 77775 : (5.2) 5.3.2 Dualmode MPC Model predictive control (MPC), as an effective method to solve multivariable constrained control problems, has been used in industry for more than 20 years. Different from conventional control which uses precomputed control laws, MPC is a technique in which the 67 XE YE i q ij h ij l Robot j Robot i j q j d P ij Dx ij Dy Figure 5.1: Formation configuration for two UGVs. current control action is obtained by solving, at each sampling instance, a finitehorizon optimal control problem. Each optimization yields an openloop optimal control sequence and the first control of this sequence is applied to the plant until the next sampling instance. For a robotpair (Ri; Rj), which has an ordered pair (Vi; Vj) 2 E in the formation control graph G and a set point Fd ij 2 F, a control input uj needs to be determined for robot j. With the assumption that robot i’s current and future control action ^ui are known to robot j, the formationerror system for robot j 2 f1; : : : ;Ng at time tk can be defined as follows xej (k + 1) = f(xej (k); uj(k)); xej (k) 2 X; uj(k) 2 U; (5.3) where f(¢) is continuous at the origin, with f(0; 0) = 0; X ½ R3 contains the origin in its interior; U is a compact subset of R2 containing the origin in its interior. To obtain the current control uj(k) at time tk, where k is a nonnegative integer (k 2 Z¤), a finitehorizon optimal control problem P(xej ; k) := min uj fJH(xej ; k; uj)g; 68 must be solved online. JH(xej ; k; uj) is the performance index and H 2 N is the horizon length (for simplicity, the prediction horizon equals the control horizon in this paper). Q and R are positive definite symmetric matrices. To ensure stability of the MPC algorithm, a terminal equality constraint xej (k+H) = 0 is commonly used. Therefore, ¢xij ! ¢xd ij , ¢yij ! ¢yd ij and µj ! µi. However, an equality constraint usually makes the optimal control problem hard to solve. To balance the stability and feasibility, the terminal equality constraint can be relaxed to a terminal region. The MPC algorithm is only required to drive the error system to the edge of the terminal region. Inside the terminal region, a stabilizing terminal controller is switched in and it drives the error system to the equilibrium point. Such MPC algorithm is dualmode [103]. Now let us define the terminal region Xf , which is a convex compact subset of X containing the origin in its interior. Therefore, we can define a set Xc f , where Xc f [Xf = X and Xc f \ Xf = ?. Inside Xf , a stabilizing terminal controller uTj is employed to drive the system (5.3) back to the origin. Note, the terminal region Xf should be positively invariant for the system xej (k + 1) = f(xej (k); uTj (k)). Methods for constructing Xf , which indeed is the terminal controller’s region of attraction, can be found in [103], [133] (local linear controller case). For a nonlinear controller which has a region of attraction X, such as (5.11), the terminal region can be a ball Br, which contains the origin. The incremental cost in the optimal control problem for robot j 2 f1; : : : ;Ng is defined in the manner of [133] L(xej ; uj) = ¸(xej )L(xej ; uj); (5.4) where ¸(xej ) = 8>< >: 0; if xej 2 Xf 1; otherwise ; and L(xej ; uj) = kxej k2 Q + kujk2 R: (5.5) Clearly, the incremental cost L(¢) is continuous at the origin, with L(0; 0) = 0. 69 Now, for robot j 2 f1; : : : ;Ng, given (Vi; Vj) 2 E and Fd ij 2 F, qi(k) and qj(k), ui(k + H ¡ 1; ¢ ¢ ¢ ; k; k) at any update time instance tk, the optimal control problem of dualmode MPC algorithm is defined P(xej ; k) := min uj fJH(xej ; k; uj)g; (5.6) where JH(xej ; k; uj) = XH m=1 L(xej (k + m; k); uj(k + m ¡ 1; k)); (5.7) subject to xej (k + 1) = f(xej (k); uj(k)); xej (k + H) 2 Xf ; uj(k) 2 U: (5.8) The constraint xej (k + H) 2 Xf requires that the final state of the prediction horizon must reach the edge of the terminal region (see Fig 5.2). f X X xe (k H) j + xe (k) j Figure 5.2: Terminal constraint of dualmode MPC. 70 From the definition of incremental cost, the objective function JH(xej ; k; uj) is greater than or equal to 0 and JH(xej ; k; uj) = 0 only when xej = 0 and uj = 0. The solution of optimal control problem (5.6) is denoted as u¤(k) = u¤j (k +m¡ 1; k). The optimal state trajectory under this control action is xej ¤(k) = xej ¤(k + m; k). The corresponding optimal performance index is J¤H (k) = JH(xej ¤(k + m; k); k; u¤j (k + m ¡ 1; k)) where m 2 [1; : : : ;H]. Now the dualmode model predictive controller for robot j 2 f1; : : : ;Ng is stated in the following algorithm. Algorithm: Data: initial states of robots pi(0), pj(0), H 2 N. Initial: At time instance tk = 0, if xej (0) 2 Xf , switch to the terminal controller uTj for all k such that xej (k) 2 Xf . Else set ^ui(l; k) = 0 and ^uj(l; k) = 0 for all l 2 [k; : : : ; k+H¡1]. Then solve optimal control problem (5.6) for robot j and obtain u¤j (l; k), where l 2 [k; : : : ; k + H ¡ 1]. Set u±j (k) = u¤j (k; k) and apply u±j (k) to the system. Controller: 1. At any time instance tk: (a) Measure current state pj(k) and measure or receive current state pi(k). (b) If xej (k) 2 Xf , switch to the terminal controller uTj for all k such that xej (k) 2 Xf . Set u± k(k) = uTj(k). (c) Else, with ^ui(l; k) and ^uj(l; k) as initial guess, solve optimal control problem (5.6) for robot j and obtain u¤j (l; k), where l 2 [k; : : : ; k+H ¡1]. Set u±j (k) = u¤j (k; k). 2. Over time interval [tk; tk+1): (a) Apply u±j (k) to the system. (b) If xej (k) 2 Xf , set ^uj(l; k + 1) = uTj (l), l 2 [k + 1; : : : ; k + H]. 71 (c) Else, compute ^uj(l; k + 1) as 8>< >: u¤j (l; k) l 2 [k + 1; : : : ; k + H ¡ 1] uTj (k + H) l = k + H (d) Transmit ^uj(¢; k + 1) to all robot n that (vj ; vn) 2 E and receive ^ui(¢; k + 1). 5.3.3 Terminal Controller Many formation controllers can be used as the terminal controller. An inputoutput feedback linearization controller (denoted as Separation Bearing Controller, SBC) developed in [56] is used in our previous paper [131]. Set points, are desired distance ld ij and desired orientation ´d ij relative to the leader. The control law determining uj = [vj !j ]T based on the position of Ri, which stabilizes the position of Rj relative to Ri, is [56] vj = sij cos °ij ¡ lij(bij + !i) sin °ij + vi cos (µi ¡ µj); !j = 1 d ¡ sij sin °ij + lij(bij + !i) cos °ij + vi sin (µi ¡ µj) ¢ ; (5.9) where °ij = ´ij + µi ¡ µj ; sij = k1(ld ij ¡ lij); bij = k2(´d ij ¡ ´ij); (5.10) and k1 and k2 are positive constants. Notice that, the SBC controller requires leader’s velocity information, which may not be available. To overcome this limitation, a robust formation control law, uRj , which stabilizes the formation between robotpair i and j is proposed in [6] uRj = g¡1(¢) Ã2 64 1 lij sin2 °ij 2 l2 ij cos °ij sin °ij 3 75 v2 j ¡2Ke_ij ¡ K2eij ¡ ¯1 sgn(eij) ! ; (5.11) 72 where g(¢) = 2 64 ¡cos °ij ¡vj sin °ij 1 lij sin °ij ¡vj lij cos °ij 3 75 2 R2£2; vj is the linear velocity of robot j, K = 2 64 k1 0 0 k2 3 75 , k1;2 2 R+, and ¯1 are positive constant control gains. Details of this robust formation controller have been explained in Section 4.3.3. 5.4 Stability Results 5.4.1 Dualmode MPC Since inside Xf , a stabilizing terminal controller uTj is used, when the state enters Xf , the error system will converge to the origin according to the stability properties of controller uTj . The stability of the system (5.3) is guaranteed if the state, xej (k), starting from any xej (0) 2 XnXf , reaches Xf within finite time under the dualmode MPC Algorithm (see Fig. 5.3). Assumption 5.1 For the incremental cost L(xej ; uj), there exists a Kfunction ·(¢) such that L(xej ; uj) ¸ ·(kxej k) for all xej 2 XnXf and all uj 2 U. Assumption 5.2 For all xej (k) 2 XnXf , u¤(k) exists. Before presenting the main result of this subsection, we state the following lemma (motivated by [103], [133]) which will be invoked later. Lemma 5.1 Suppose assumptions 5.1, 5.2 are satisfied. Then for all k 2 Z¤ such that both xej (k) and xej (k + 1) are in XnXf , under the dualmode MPC algorithm, the following inequality J¤ H(k + 1) ¡ J¤ H(k) · ¡·(kxej (k + 1)k) (5.12) 73 f X e (0) j x o j u X Tj u Figure 5.3: State trajectory. holds. Proof: By Assumption 5.2, u¤j (k) and u¤j (k + 1) exist, so do the optimal performance index J¤H (k) and J¤H (k + 1). To find J¤H (k + 1), the dualmode MPC algorithm solves the optimal control problem (5.6) from an initial control guess ^uj(l; k+1), l 2 [k + 1; : : : ; k + H], which is constructed from the result of previous optimization on time tk. Obviously, J¤H (k +1) · ^ JH(xej (¢); k + 1; ^uj(l; k + 1)). Therefore, J¤ H(k + 1) ¡ J¤ H(k) · ^ JH(xej (¢); k + 1; ^uj(l; k + 1)) ¡ J¤ H(k): (5.13) 74 Follow the construction of ^uj(l; k+1), which is described in the algorithm given in Section 5.3.2, we have ^ JH(xej (¢); k + 1; ^uj(l; k + 1)) ¡ J¤ H(k) = ¡L(xej ¤(k + 1; k); u¤ j (k; k)) +L(xej (k + H + 1; k); uLj (k + H)): (5.14) Since xej (k + H + 1; k) 2 Xf , we have L(xej (k + H + 1; k); uLj (k + H)) = 0: In addition, assuming the system model is perfect, we have L(xej ¤(k + 1; k); u¤ j (k; k)) = L(xej (k + 1); u¤ j (k; k)): Therefore, J¤ H(k + 1) ¡ J¤ H(k) · ¡L(xej (k + 1); u¤ j (k; k)): (5.15) Clearly, with Assumption 5.1, inequality (5.12) holds. ¥ We now state the main stability result for the proposed dualmode MPC in the following theorem. Theorem 5.1 Let assumptions 5.1 and 5.2 be satisfied. Using the terminal controller (5.11) and the kinematic model (5.1), the dualmode MPC is asymptotically stabilizing with a region of attraction X. Proof: According to the analysis at the beginning of this section, since inside Xf , a stabilizing terminal controller uTj is used, when the state enters Xf , the error system will converge to the origin asymptotically. We only need to prove that from any xej (0) 2 XnXf , under the dualmode model predictive controller, the state will be driven into Xf within finite time. 75 As the definition of Xf , it contains the origin in its interior. There must exist a constant r > 0 such that for all xej (¢) 2 XnXf , we have kxej (¢)k ¸ r. Then, with the definition of Kfunction, inequality ·(kxej (¢)k) ¸ ·(r) (5.16) holds. Suppose that a finite time tk does not exist such that xej (k) 2 Xf . Because of Lemma 5.1, by adding inequality (5.12) from 0 to k, we have J¤ H(k) ¡ J¤ H(0) · ¡ Xk n=0 ·(kxej (n)k) · ¡k minf·(kxej (0)k); : : : ; ·(kxej (k)k)g; (5.17) for all k 2 Z¤. Then according to inequality (5.16), we have ¡k minf·(kxej (0)k); : : : ; ·(kxej (k)k)g · ¡k·(r): (5.18) This means J¤H (k) ¡ J¤H (0) · ¡k·(r); for all k 2 Z¤. (5.19) However, (5.19) implies that J¤H (k) ! ¡1 as k ! 1. This contradicts that JH(k) ¸ 0 for all k 2 Z¤. Therefore, there exists a time tk such that xej (k) 2 Xf . The stabilizing property of the controller follows. ¥ 5.4.2 InputOutput Feedback Linearization Controller Theorem 5.2 Assume that the lead vehicle’s linear velocity along the path g(t) 2 SE(2) is lower bounded i.e., vi ¸ Vmin > 0, its angular velocity is also bounded i.e., k!ik < Wmax, the relative velocity ±v ´ vi¡vj and relative orientation ±µ ´ µi¡µj are bounded by small positive numbers "1, "2, and the initial relative orientations kµi(t0) ¡ µj(t0)k < c1¼, with 0 < c1 < 1. If the control law (5.9) is applied to robot Rj , then the formation is stable, and the system outputs lij , ´ij converge exponentially to the desired values [134]. 76 Remark 5.1 Note that, to guarantee stable behavior of Rj , we would require vi > 0. Otherwise, the internal dynamics µj of Rj may be unstable. Let the orientation error be expressed as e_µ = !i ¡ !j . After incorporating the angular velocity for the follower (5.9), we obtain e_µ = ¡ vi dj sin eµ + »(!i; eµ); (5.20) where »(¢) is a nonvanishing perturbation for the nominal system (equation (5.20) with »(¢) = 0), which is itself (locally) exponentially stable. By using the stability of perturbed systems, it can be shown that system (5.20) is stable when vi > 0. A detailed proof of Theorem 5.2 and explanation of internal dynamics can be found in [134]. 5.4.3 Robust Formation Controller A detailed stability proof can be found in Section 4.3.3. Remark 5.2 Notice that, the robust controller proposed here does not require the leader robot’s velocity information and there are no internal dynamics. This is a big improvement to the SBC controller described in Section 5.3.3. The only limitation here is that the reference robot’s velocity cannot be zero. Otherwise, vj needs to be zero and the inverse of g(¢) cannot be computed. In summary, for the SBC controller, it fails when vi · 0. For the robust controller, it only fails when vi = 0. In addition, this robust controller is globally stable, which means that it has a region of attraction X. This alleviates the difficulty of finding a terminal region. 5.5 Simulation Results The effectiveness of the control algorithms presented in Section 5.3 is investigated by numerical simulations. In the figures, each robot is depicted by an arrow within a circle. The orientation of the robot is shown by the orientation of the arrow. 77 5.5.1 TrackingStabilizingTracking A realistic scenario is illustrated in Figure 5.4. The reference robot 1 first moves forward from position (0; 0). Then it stops for some time and finally starts to move backward. This scenario happens when some algorithms are implemented for the reference robot 1 to avoid obstacles. Usually this trackingstabilizingtracking case is not considered under a single controller approach. To keep the formation, a controller switching is required. However, a simulation shows that this case can be handled within the MPC framework. The desired formation is Fd 12 = [¡20;¡10] and it is achieved by the MPC controller. Note, a conventional MPC controller is used here. 5.5.2 Follow a Leader Moving Backward Figure 5.5 shows the response of robot j following a reference robot i which is moving backward under the robust formation controller. This scenario cannot be handled by the SBC controller since it fails when the leader’s velocity is negative. The desired formation is ¹ Fd ij = £ 100; 5¼ 4 ¤ . Robot i starts from position (0; 0) and moves backward with constant speed vi = ¡17:5. Robot j starts from (150; 200) and moves backward. The formation is achieved by the robust formation controller. 5.5.3 Control of Chain Formation Simulations of five robots in chain formation under the dualmode MPC algorithm are presented in this section. The robust formation controller (5.11) is used as the terminal controller. Total simulation time is 50 seconds. The sample time is set to 0:5 second. Therefore, the total time instance is 100. The prediction horizon is set to H = 6. As shown in Figure 5.6, robot 1 moves independently and robots i, i 2 f2; 3; 4; 5g, each follows robot i ¡ 1 to form a chain of robots. The control action for robot 1 at different time instance is defined 78 by u1(k) = 8 >>>>< >>>>: [20 0]T ; k 2 [0; : : : ; 10] [20 0:2]T ; k 2 [11; : : : ; 70] [20 0]T ; k 2 [71; : : : ; 100] The formations for each robotpair are Fd 12 = Fd 23 = Fd 34 = Fd 45 = [¡20 0]T . The initial conditions for each robot are given as p1(0) = [0 0 ¼]T ; p2(0) = [50 10 ¼]T ; p3(0) = [100 ¡ 10 ¼]T ; p4(0) = [150 10 ¼]T ; p5(0) = [200 ¡ 10 ¼]T : The control constrain set is defined as U = n [V !]T 2 R2 : 15 · V · 50;¡0:3 · ! · 0:3 o : Figure 5.6 shows the formation response. The linear velocity control inputs for robots 2; 3; 4; 5 are shown in Figure 5.7 and the angular velocity control inputs are shown in Figure 5.8. Clearly, all the control inputs satisfy the control constraints. As the desired formation is achieved, the linear velocity control inputs converge to 20 m/s and the angular velocity control inputs converge to 0 rad/s, which are the final control inputs for robot 1. 5.5.4 Control of Triangle Formation A simulation of six robots in triangle formation is presented in this section. The edge set is E = f(V1; V2); (V1; V3); (V2; V4); (V2; V5); (V3; V6)g. Robot 1 moves independently. The total simulation time is 15 seconds. Again, the sample time is set to 0:5 second. Therefore, the total time instance is 30. The prediction horizon is set to H = 6. The control action for 79 robot 1 is u1(k) = [20 0]T for all the k. The set points for each robot are Fd 12 = [¡20 20]T ; Fd 13 = [¡20 ¡ 10]T ; Fd 24 = [¡20 10]T ; Fd 35 = [¡20 ¡ 10]T ; Fd 26 = [¡20 ¡ 10]T : The control constrain set is defined as U = n [V !]T 2 R2 : 15 · V · 40;¡0:2 · ! · 0:2 o . The initial conditions for each robot are given by p1(0) = [0 0 ¼]T ; p2(0) = [50 0 ¼]T ; p3(0) = [70 5 ¼]T ; p4(0) = [100 ¡ 30 ¼]T ; p5(0) = [100 0 ¼]T ; p6(0) = [100 20 ¼]T : The formation response is shown in Figure 5.9. The linear velocity control inputs for robots 2; 3; 4; 5; 6 are shown in Figure 5.10 and the angular velocity control inputs are shown in Figure 5.11. Clearly, all the control inputs satisfy the control constraints. As the desired formation is achieved, the linear velocity control inputs converge to 20 m/s and the angular velocity control inputs converge to 0 rad/s, which are the control inputs for robot 1. 5.6 Summary In this chapter, a dualmode MPC algorithm that allows a team of mobile robots to navigate in formation is developed and proven to be stable. Simulations show the effectiveness of the proposed dualmode MPC algorithm. Additionally, we show that it is more convenient to put the tracking and point stabilizing problems of nonholonomic robots inside the MPC 80 framework. For the choice of stabilizing terminal controller, analysis show that the robust formation controller is better than the SBC controller. 81 −80 −60 −40 −20 0 20 40 −40 −30 −20 −10 0 10 20 30 40 x y 1 2 1 1 2 2 Figure 5.4: Trajectory of a robot following a reference vehicle which moves forward, stops, and then moves backward according to an MPC controller. 82 −200 0 200 400 600 800 1000 1200 1400 1600 −200 0 200 400 600 800 1000 1200 x y i j Figure 5.5: Trajectory of a robot following a reference robot which is moving backward according to the robust formation controller. 83 −400 −300 −200 −100 0 100 200 −250 −200 −150 −100 −50 0 50 100 150 x 1 2 3 4 5 1 2 2 3 3 4 4 5 5 1 Figure 5.6: Five robots in chain formation according to a dualmode MPC with robust formation controller as the terminal controller. 84 0 10 20 30 40 50 60 70 80 90 100 5 10 15 20 25 30 35 40 45 50 55 60 k V (m/s) V2 V3 V4 V5 Vlb Vub Figure 5.7: Linear velocity control inputs of chain formation. 0 10 20 30 40 50 



A 

B 

C 

D 

E 

F 

I 

J 

K 

L 

O 

P 

R 

S 

T 

U 

V 

W 


