University of Southern California Dissertations and Theses

Flexible formation configuration for terrain following flight: formation keeping constraints

(USC Thesis Other)

Flexible formation configuration for terrain following flight: formation keeping constraints

PDF

Download a page range

Download transcript

Copy asset link

Request this asset

Transcript (if available)

Content FLEXIBLE FORMATION CONFIGURATION FOR TERRAIN FOLLOWING FLIGHT: FORMATION KEEPING CONSTRAINTS by Simon Latyshev A Dissertation Presented to the FACULTY OF THE USC GRADUATE SCHOOL UNIVERSITY OF SOUTHERN CALIFORNIA In Partial Fulfillment of the Requirements for the Degree DOCTOR OF PHILOSOPHY (AEROSPACE ENGINEERING) May 2013 Copyright 2013 Simon Latyshev ii ACKNOWLEDGMENTS I will be forever grateful to my mentor, Professor Bingen Yang. I express gratitude to Professor Firdaus E. Udwadia and Professor Michael Crowley for their invaluable help and advice. Thank you also to my family and friends, without whom I could not have made it this far. iii TABLE OF CONTENTS Acknowledgments iii List of Tables viii List of Figures ix Abstract xix Nomenclature xx Chapter 1: Introduction 1 1.1 Literature Review 2 1.1.1 Applications of Formation Motion 2 1.1.2 Approaches to Formation Control 5 1.1.2.1 Behavioral Approach 6 1.1.2.2 Virtual Structure Approach 7 1.1.2.3 Leader-Follower Approach 8 1.1.2.4 Synthetic Approach 10 1.1.2.5 Motion Coordination as a Specific Case of Formation Control 11 1.1.3 Centralized vs. Decentralized Strategies for Formation Motion Control 11 1.1.4 Graph Theory for Developing and Analyzing Strategies for Formation Control 14 1.1.4.1 Graph Theory and Formation Recovery from a Fault 14 1.1.4.2 Analysis of the Formation Geometry Feasibility with Graph Theoretic Methods 15 1.1.4.3 Application of Graph Theoretic Methods for Formation Stability Analysis 16 1.1.5 Algorithms and Strategies for the Formation Motion Control 18 1.1.5.1 RHC or MPC Algorithm 18 1.1.5.2 LQR Algorithm 21 1.1.5.3 HOSM Algorithm 22 1.1.5.4 State-Space Control Strategies 22 1.1.5.5 Mode-Based Flight 24 1.1.5.6 Adaptive Controls 25 1.1.6 Formation Motion and Cooperative Localization 26 1.1.7 Formation Motion and Non-GPS Based Navigation 28 1.1.8 Cooperative Navigation and Localization 30 1.1.9 Constrained Motion, the Udwadia-Kalaba Equation, and Formation Control 32 1.1.10 Constraint Based Aircraft Formation Flight Control 34 1.1.11 Simultaneous Localization and Mapping, SLAM, Algorithm 35 iv 1.1.12 Terrain Following Flight of Independent Agents 35 1.2 Contribution 36 1.3 Outline 37 Chapter 2: Terrain Following Formation Flight Problem 40 2.1 Requirements for the Terrain Following Formation Motion Control Algorithm 42 2.2 Problem Formulation 48 2.2.1 Dynamical System and Control Structure 50 2.2.2 Graph Representation of the Formation Structure 54 2.3 Problem Approach 58 Chapter 3: Theory and Background to Constrained Motion, and Terrain Following Flight 61 3.1 Constrained Dynamical Systems and the Udwadia-Kalaba Equation 62 3.2 Constraints and Implementation of Inter-Agent Collision Avoidance 66 3.3 Terrain Following Flight of Independent Agents 68 3.3.1 Ray Based Sensor Model 69 3.3.2 Selection Algorithm for the Terrain Following Navigation Vector 72 3.3.3 Metrics for Evaluating Terrain Following Trajectories 75 3.3.4 Constraints and Equations of Motion for Tracking a Line Defined by Position Point and Navigation Vector 76 3.4 Model Settings and Simulation Examples 80 3.4.1 Examples of Sensor Model and Terrain Following Corridor Volumes 82 3.4.2 Comparison of the Simulation Results Obtained with the Ranges of Model Settings 84 3.4.2.1 Range of Settings for the Desired Navigation Height 84 3.4.2.2 Two Examples with Different Settings for the Duration of a Time Step and Identical Settings for the Ranges of Sensor Cone Width 87 3.4.2.3 Range of Settings for Sensor Range 90 3.5 Conclusion 92 Chapter 4: Rigid vs. Adjustable Formation Geometry 93 4.1 Formation Navigation and Guidance, Navigation Vector of the Virtual Leader 93 4.1.1 Method One, Averaging Navigation Vectors 94 4.1.2 Method Two, Considering Navigation Rays 94 4.1.3 Method Three, Considering Full Set of Sensor Data 96 4.2 Virtual Rigid Body Structure Defined with Positioning Offset Constraints 97 4.3 Adjustable Formation Defined with Heuristic Function Constraints 99 4.3.1 Constraints and Equations of Motion 100 4.3.2 Heuristic Function Design With Terrain Following v Trajectories and Corridor Volumes 101 4.4 Simulation Results and Comparison of Rigid and Adjustable Formations 104 4.4.1 Terrain Collision with Rigid Virtual Body Structure 105 4.4.2 Collision Free Motion with Adjustable Formation Geometry 107 4.4.3 Comparison of Rigid and Adjustable Formations 110 4.5 Conclusion 112 Chapter 5: Flexible Formation Geometry Defined with Planes and Navigation Vector Constraints 114 5.1 Plane Surface Constraint 115 5.2 Full Sets of Constraints and Corresponding Equations of Motion 117 5.3 Metrics for Evaluating Performance of Terrain Following Formations 119 5.4 Example of Leader-Follower Flight and Demonstration of Performance Metrics 122 5.5 Simulation Results for Triangular and Star Formation Configurations 125 5.5.1 Triangular Formations, Modeled with Different Ratios of Stabilization Coefficients 126 5.5.2 Star Formations, Modeled with Different Durations of a Time Step 131 5.6 Examples of Trajectory Profiles Demonstrating Different Methods of Computing Navigation Vector of the Virtual Leader 142 5.7 Conclusion 145 Chapter 6: Hierarchical Formation Structures 146 6.1 Sets of Constraints for Two-Level Formations 146 6.2 Simulation Results 149 6.2.1 Formation Defined with Planes and Navigation Vectors Constraints 155 6.2.2 Formations Defined with Addition of Adjustable Positioning Offset Constraints 158 6.2.3 A Case of Terrain Collision with the Design that Uses Linear Heuristic Function in the Definition of Adjustable Positioning Offsets 162 6.3 Conclusion 165 Chapter 7: Flexible Formation Geometry Defined with Planes and Surfaces of Revolution Constraints 166 7.1 Transformation of System Parameters 167 7.2 Expressions for Surface Constraints 169 7.3 Constraint Equations and Corresponding Equations of Motion 174 7.4 Simulation Results for Triangular Formation Configuration 180 7.5 Conclusion 190 vi Chapter 8: Modeling and Simulation with Maya and Mathematica 192 8.1 Terrain Following Formation Flight Node Graph Model 195 8.2 Examples of Individual Model Components 198 8.3 Simulation Results, Comparison, and Verification 202 8.4 Simulation Results Obtained using USGS DEM Terrain Model 206 8.5 Conclusion 213 Chapter 9: Conclusions 214 Bibliography 218 Appendices Appendix A: Tracking a Line Defined with Point and Navigation Vector, Version LZ 226 Appendix B: Plane and Navigation Vector Constraints, Version PLZ 228 Appendix C: Navigation Vector, Direction, Tests for Individual Agents Modeled with Parabolic, Cone, and Cylindrical Surface Constraints 230 vii LIST OF TABLES Table 5.1: Model settings 126 Table 6.1: Sets of formation constraints 147 Table 6.2: Stabilization coefficients 150 viii LIST OF FIGURES Figure 2.1: Flight control methods, a) Terrain following, b) Formation keeping, c) Combined, terrain following formation flight 40 Figure 2.2: Adherence to safe corridors, a) No, b) Yes 43 Figure 2.3: Formation shape, a) Rigid, b) Flexible 45 Figure 2.4: Multi-level formation 46 Figure 2.5: Triangular formation primitive 50 Figure 2.6: Diagram for the i th formation member 51 Figure 2.7: Data flow diagram 52 Figure 2.8: Structure of a formation primitive 55 Figure 2.9: Evolution of formation structure 55 Figure 2.10: Two-level hierarchical formation structure 56 Figure 2.11: Restructuring corresponding to flocking behavior 57 Figure 2.12: Data flow node graph 58 Figure 3.1: Triangular spring and damper structure 67 Figure 3.2: Sensor model 70 Figure 3.3: Navigation vector and corridor volume 73 Figure 3.4: Data flow diagram for an independent agent 74 Figure 3.5: Terrain models, a) Hills, b) Wave 81 Figure 3.6: Sensor model, a) Distribution of rays within sensor cone, b) Ray tracing to the terrain surface 83 Figure 3.7: Corridor volume, navigation vector, sensor cone, and navigation ray, a) minRay, b) maxRay 83 Figure 3.8: Trajectory, corridors, navigation vectors and rays 83 ix Figure 3.9: Independent agents, a) Trajectories and Hills terrain, b) Trajectories 86 Figure 3.10: Trajectory profiles, a) TF1, b) TF10 86 Figure 3.11: Trajectory deviation from, a) Centerlines of corridors, b) Navigation height 86 Figure 3.12: Trajectories and Hills terrain, a) dt 0.05 s, b) dt 0.025 s 88 Figure 3.13: Trajectory deviation from navigation height , a) dt 0.05 s, b) dt 0.025 s 88 Figure 3.14: Trajectory deviation from centerlines of corridors a) dt 0.05 s, b) dt 0.025 s 89 Figure 3.15: Independent agents, a) Trajectories and Hills terrain, b) Trajectories 91 Figure 3.16: Trajectory profiles, a) TF1, b) TF2 91 Figure 3.17: Trajectory deviation from, a) Centerlines of corridors, b) Navigation height 91 Figure 3.18: Constraint expressions, a) Φ i (t) , b) Ψ i (t) 92 Figure 4.1: Navigation vector of the virtual leader 97 Figure 4.2: Trajectory adjustment 102 Figure 4.3: Initial configuration 105 Figure 4.4: Terrain and trajectories, a) dt 0.05 s b) dt 0.025 s 106 Figure 4.5: Formation shape, a) dt 0.05 s b) dt 0.025 s 106 Figure 4.6: Trajectory profile of FK3, a) dt 0.05 s b) dt 0.025 s 107 Figure 4.7: Trajectory deviation from centerlines of corridors, a) dt 0.05 s b) dt 0.025 s 107 Figure 4.8: Adjustable, a) Terrain and trajectories, b) Formation shape 109 Figure 4.9: Trajectory, a) Deviation from centerlines of corridors for all of the formation members b) Elevation profile of FK3 109 x Figure 4.10: Adjustable offsets magnitude 109 Figure 4.11: Trajectory deviation from navigation height, a) Rigid, b) Adjustable 111 Figure 4.12: Velocity magnitude, a) Rigid, b) Adjustable 111 Figure 4.13: Forces of constraints magnitude, a) Rigid, b) Adjustable 111 Figure 4.14: Constraint expressions Φ i (t) , a) Rigid, b) Adjustable 112 Figure 4.15: Constraint expressions Ψ i (t), a) Rigid, b) Adjustable 112 Figure 5.1: Planes, normal and navigation vectors shown, a) At the time t n b) During the n th time step 116 Figure 5.2: Leader and follower, a) 1 st step, b) 10 steps 123 Figure 5.3: Angle between leader-follower position vector and vector normal to plane 123 Figure 5.4: Angles between consecutive navigation vector of, a) Leader, b) Follower 124 Figure 5.5: Angles between velocity direction and navigation vectors of, a) Leader, b) Follower 124 Figure 5.6: Triangular formation, a) Desired geometry, b) Initial configuration 127 Figure 5.7: Formation Set One, a) Polygons and sensor rays, b) Planes, corridors, and navigation vectors 128 Figure 5.8: Formation shape, a) Set One, b) Set Two 128 Figure 5.9: Trajectory profile of FK3, a) Set One, b) Set Two 129 Figure 5.10: Deviation from centerlines of corridors, a) Set One, b) Set Two 129 Figure 5.11: Angles between consecutive navigation vectors, a) Set One, b) Set Two 130 Figure 5.12: Angles between velocity directions and navigation vectors, a) Set One, b) Set Two 130 xi Figure 5.13: Magnitude of the forces of constraints, a) Set One, b) Set Two 131 Figure 5.14: Star formation, a) Desired geometry, b) Initial configuration 132 Figure 5.15: Planes and corridor volumes, a) Star One, b) Star Two 133 Figure 5.16: Trajectories, a) Star One, b) Star Two 134 Figure 5.17: Formation shapes plotted as lines, a) Star One, b) Star Two 134 Figure 5.18: Distances between positions of the virtual leader and formation members, a) Star One, b) Star Two 135 Figure 5.19: Distances between positions of adjoined formation members, i and i+1, a) Star One, b) Star Two 135 Figure 5.20: Trajectory profile of the virtual leader, a) Star One, b) Star Two 136 Figure 5.21: Trajectory profile of the formation member FK5, a) Star One, b) Star Two 136 Figure 5.22: Trajectory deviation from navigation height, a) Star One, b) Star Two 137 Figure 5.23: Angles between consecutive navigation vectors, a) Star One, b) Star Two 137 Figure 5.24: Deviation from centerlines of corridors, a) Star One, b) Star Two 138 Figure 5.25: Angles between velocity directions and navigation vectors, a) Star One, b) Star Two 139 Figure 5.26: Angles between vectors defined by positions q s (t),q i (t) { } and vectors normal to planes, a) Star One, b) Star Two 139 Figure 5.27: Plane constraint expression ϕ P,i (t), a) Star One, b) Star Two 140 Figure 5.28: Velocity magnitude, a) Star One, b) Star Two 141 Figure 5.29: External acceleration magnitude, a) Star One, b) Star Two 141 Figure 5.30: Forces of constraints magnitude, a) Star One, b) Star Two 141 xii Figure 5.31: Constraint expressions Ψ i (t), a) Star One, b) Star Two 142 Figure 5.32: Method one, averaging navigation vectors, a) Profile, b) Distance 143 Figure 5.33: Method two, considering navigation rays, a) Profile, b) Distance 143 Figure 5.34: Method three, considering full set of sensor data, a) Profile, b) Distance 144 Figure 5.35: Distances between positions of the virtual leader and formation members with, a) Method One, b) Method Two 144 Figure 6.1: Formation structure 147 Figure 6.2: The desired formation configuration, a) Primitive, b) Level- 2 GC’s 152 Figure 6.3: Initial positions and navigation vectors for the 1 st time step 153 Figure 6.4: Sensor rays and navigation vectors for the 1 st time step 154 Figure 6.5: Elevation estimates and navigation rays with navigation method two 154 Figure 6.6: Elevation estimates and navigation rays with navigation method three 154 Figure 6.7: Terrain and trajectories 155 Figure 6.8: Formation shape 156 Figure 6.9: Additional views of formation shape 156 Figure 6.10: Trajectory deviation from navigation height, a) FK’s, b) GC’s 157 Figure 6.11: Formation members, FK’s, a) Deviation from centerlines of corridors, b) Forces of constraints magnitude 157 Figure 6.12: Constraint expressions Ψ i (t), a) FK’s, b) GC’s 158 Figure 6.13: Formation Shape, a) Set Two, b) Set Three 160 xiii Figure 6.14: Additional views of formation shape, a) Set Two, b) Set Three 160 Figure 6.15: Trajectory deviation from navigation height for FK’s, a) Set Two, b) Set Three 161 Figure 6.16: Deviation from centerlines of corridors for FK’s, a) Set Two, b) Set Three 161 Figure 6.17: Forces of constraints magnitude, a) Set Two, b) Set Three 162 Figure 6.18: Terrain and trajectory 163 Figure 6.19: Evolution of formation shape 163 Figure 6.20: Trajectory deviation from navigation height, a) FK’s, b) GC’s 164 Figure 6.21: Formation members, FK ij , a) Deviation from centerlines of corridors, b) Min and max values of magnitude of the forces of constraints 164 Figure 7.1: Plane, surface of revolution, navigation vectors and corridor volume 167 Figure 7.2: Parabolic surface, a) Position and orientation, b) Model 171 Figure 7.3: Cone surface, a) Position and orientation, b) Model 172 Figure 7.4: Cylindrical surface, a) Position and orientation, b) Model 173 Figure 7.5: Plane and parabolic surface, leader-follower, a) 1st step, b) 10 steps 176 Figure 7.6: Plane and cone surface, leader-follower, a) 1st step, b) 10 steps 178 Figure 7.7: Plane and cylindrical surface, leader-follower, a) 1st step, b) 10 steps 180 Figure 7.8: Parabolic surfaces, a) A single surface with corridor volume, b) Planes, paraboloids, and navigation vectors for the first 3 time steps 182 Figure 7.9: Trajectories with, a) Terrain, b) Formation shapes 182 xiv Figure 7.10: Terrain and trajectory elevation profiles, a) FK 1 , b) FK 3 182 Figure 7.11: Distances, a) Between positions of GC and FK i , b) Deviation from constant elevation above terrain surface for formation members FK i 183 Figure 7.12: FK i , a) Angles between normal and planes, b) Deviation of agent trajectories from centerlines of corridor volumes 183 Figure 7.13: Angles between, a) Consecutive navigation vectors, b) Velocity directions and navigation vectors 183 Figure 7.14: Magnitudes of, FK i , a) Velocity, b) Forces of constraints 184 Figure 7.15: Numerical values of expressions, a) Ψ LX for GC, b) Ψ PPR,i for FK i 184 Figure 7.16: Cone surfaces, a) A single surface with corridor volume, b) Planes, cones, and navigation vectors for the first 3 time steps 185 Figure 7.17: Trajectories with, a) Terrain, b) Formation shapes 185 Figure 7.18: Trajectory profiles, a) Deviation from the desired navigation height, b) FK 1 185 Figure 7.19: Angles between, a) Consecutive navigation vectors, b) Velocity directions and navigation vectors 186 Figure 7.20: Formation members FK i , a) Deviation of agent trajectories from centerlines of corridor volumes, b) Magnitudes of forces of constraints 186 Figure 7.21: Cylindrical surfaces, a) A single surface with corridor volume, b) Planes, cylinders, and navigation vectors for FK 3 187 Figure 7.22: Formation member FK 3 , a) Deviation of agent trajectories from centerlines of corridor volumes, b) Trajectory elevation profile 187 Figure 7.23: Angles for FK 3 between, a) Consecutive navigation vectors, b) Projections of consecutive navigation vectors on XY plane 188 xv Figure 7.24: Angles for FK 3 between, a) Velocity directions and navigation vectors, b) Projections of the vectors on XY plane 188 Figure 7.25: Forces of constraints for FK 3 , a) Forces, b) Magnitude 188 Figure 7.26: Actual and desired trajectory segments for 3 time steps 189 Figure 8.1: System architecture diagram 192 Figure 8.2: Formation node graph 195 Figure 8.3: Multi-element sensor model 197 Figure 8.4: Parallel MathKernel processes 197 Figure 8.5: Sensor model 199 Figure 8.6: Alaska terrain 200 Figure 8.7: Initial positions of 2000 agents, a) View 1, b) View 2 201 Figure 8.8: Trajectories of 2000 agents 201 Figure 8.9: Star Two, formation shape after 14 time steps 203 Figure 8.10: Star Two, trajectories, shape, navigation vectors and rays 203 Figure 8.11: Star Two, a) Constraint force magnitude, b) Constraint expressions Ψ i (t) 204 Figure 8.12: Set One, initial configuration 204 Figure 8.13: Set One, evolution of formation shape 205 Figure 8.14: Set One, trajectories 205 Figure 8.15: Set One, a) Constraint force magnitude, b) Constraint expressions Ψ i (t) 205 Figure 8.16: Set I, dt 0.25 s 208 Figure 8.17: Set II, dt 0.025 s 208 Figure 8.18: Set I, trajectories 209 xvi Figure 8.19: Set I, constraint forces a) Magnitude, b) Min, max magnitude 209 Figure 8.20: Set I, values of constraint expressions Ψ i (t) for, a) Formation members, b) Virtual leaders 209 Figure 8.21: Set II, a) Constraint force magnitude, b) Constraint expressions Ψ i (t) 210 Figure 8.22: Terrain and trajectory profiles for FK 11 a) Set I, b) Set II 210 Figure 8.23: Terrain and trajectory profiles for FK 21 a) Set I, b) Set II 211 Figure 8.24: Deviation from the desired navigation height a) Set I, b) Set II 211 Figure 8.25: Distances to centerlines of corridors for formation members a) Set I, b) Set II 211 Figure 8.26: Angles between consecutive navigation vectors a) Set I, b) Set II 212 Figure 8.27: Angles between velocity directions and navigation vectors a) Set I, b) Set II 212 Figure C.1: Navigation, direction, vectors, a) Shown as arrows, b) Rotation angles 234 Figure C.2: Sets of surfaces , a) PR I, b) PR II 235 Figure C.3: Trajectories, a) PR I, b) PR II 236 Figure C.4: Angles between velocity directions and navigation vectors, a) PR I, b) PR II 236 Figure C.5: Angles between projections of velocity directions and navigation vectors on XY plane, a) PR I, b) PR II 236 Figure C.6: Deviation of agent trajectories from centerlines of corridor volumes, a) PR I, b) PR II 237 Figure C.7: Velocity  q i (t) , a) PR I, b) PR II 237 Figure C.8: Velocity magnitude, a) PR I, b) PR II 237 Figure C.9: Forces of constraints F c (t), a) PR I, b) PR II 238 xvii Figure C.10: Magnitude of forces of constraints, a) PR I, b) PR II 238 Figure C.11: Min and Max values of ||F c (t)|| for the i th agent, a) PR I, b) PR II 238 Figure C.12: Values of expressions ψ PR,i (t), a) PR I, b) PR II 239 Figure C.13: Cone surfaces and corridor volume 239 Figure C.14: Trajectories 240 Figure C.15: Angles between, a) Velocity directions and navigation vectors, b) Projections of velocity directions and navigation vectors on XY plane 240 Figure C.16: Deviation of agent trajectories from centerlines of corridor volumes 240 Figure C.17: Velocity, a)  q i (t) , b) Magnitude 241 Figure C.18: Forces of constraints, a) F c (t), b) Magnitude 241 Figure C.19: CN i , values of, a) Min and Max of ||F c (t)|| for the i th agent, b) Expressions ψ CN,i (t) 241 Figure C.20: Cylindrical surfaces and corridor volume 242 Figure C.21: Trajectories 243 Figure C.22: Angles between, a) Velocity directions and navigation vectors, b) Projections of velocity directions and navigation vectors on XY plane 243 Figure C.23: Deviation of agent trajectories from centerlines of corridor volumes 243 Figure C.24: Velocity, a)  q i (t) , b) Magnitude 244 Figure C.25: Forces of constraints, a) F c (t), b) Magnitude 244 Figure C.26: CL i , values of, a) Min and Max of ||F c (t)|| for the i th agent, b) Expressions ψ CL,i (t) 244 xviii ABSTRACT This work suggests a control method for the terrain-following formation motion of a group of communicating autonomous agents. The presented approach centers on defining a suitable set of constraints for formation keeping task that shall be fulfilled while agents are negotiating an unknown terrain toward the predefined goal location. It allows agents to maintain a general geometric formation shape, while allowing each individual formation member freedom of maneuver, required for terrain collision free motion. Formation structure is defined with the use of virtual leader. Formation keeping constraints are defined with plane surfaces, specified relative to position and navigation vector of the virtual leader. Formation navigation and guidance constraints are defined using navigation vectors of formation members and the virtual leader. Alternative designs for the constraints derived with parabolic, cone, and cylindrical surfaces are considered. Formation control is derived using the Udwadia-Kalaba equation, following corresponding approach to the development of control methods for constraint based dynamical systems, including leader-follower systems defined using geometric constraints. Approach to terrain following motion requiring agents to stay within bounds of cylindrical corridor volumes built around their respective navigation vectors is assumed. Individual formation primitives and multi-level, hierarchical, formation structures are considered. Simulations, based on three degrees of freedom nonlinear model of an agent, performed using Mathematica and specifically developed combined Maya-Mathematica modeling and simulation system, demonstrate that a flexible terrain following formation motion is achieved with the presented sets of constraints. xix NOMENCLATURE i subscript indicating the i th agent, formation member s subscript indicating virtual leader n f number of formation members GC,i subscript indicating the i th virtual leader for multi-level formation structures FK,i subscript indicating the i th formation member TF,i subscript indicating the i th independent agent dt duration of a time step q i , q s , q TF,i position vectors  q i ,  q s ,  q TF,i velocity vectors  q i ,  q s ,  q TF,i acceleration vectors Q i , Q s , Q TF,i state matrices t n time at the beginning of the n th time step interval ˆ u i normalized vectors normal to planes ˆ ν i , ˆ ν s , ˆ ν TF,i normalized navigation direction vectors γ cr diameter of a cylindrical safe corridor volume Δ r,i matrix of normalized ray directions Λ r,i matrix of ray-surface intersection points Φ i , Φ s expressions for constraints Ψ i , Ψ s expressions for constraints considering stabilization coefficients 1 CHAPTER 1: INTRODUCTION In this work we consider the problem of controlling a formation of inter- communicating autonomous mobile agents navigating in an unknown environment. The mobile agents use terrain-following navigation, based on reactive collision avoidance methods. Formation motion and inter-agent communication are instrumental in achieving GPS independent navigation. The presented approach includes two major components, terrain-following navigation of individual agents, and formation keeping control using virtual leader approach as a method for specifying formation geometry and implementing formation navigation and guidance. The presented research aims to develop a novel methodology for design of cooperating autonomous systems, where the controlling algorithm considers the effects of imperfect equipment used by the cyber-physical system, and offers the ways to reduce them as well as to make the system less sensitive to sensor errors. Specifically, the presented method of formation control will, (a) lead to development of a robust navigation algorithm for negotiating unknown and dynamically changing environments, where formation motion serves as navigation aid; (b) lead to development of a control system, capable of reducing the effects of disturbances and imperfect equipment; (c) lead to development of a formation control algorithm that yields highly flexible reconfigurable formation, extendable due to reduced string instability, and analyzable by standard control-theoretic methods even though it possesses a strong behavioral component. The proposed technology can be utilized for such applications as search-and-rescue, surveillance, exploration on macro- and micro-scales, and those that build distributed sensor networks in general. 2 Formation control laws are derived using methodology based on the Udwadia- Kalaba approach, introduced by Udwadia and Kalaba [1, 2]. The equations of motion for all of the agents considered in this thesis are derived using the Udwadia-Kalaba equation, [1, 2], which allowed us to explicitly incorporate optimization into the derived control laws because this method adheres with the Gauss Principle of Least Constraint, always minimizing the control force it specifies. Such an approach is consistent with natural systems always conserving energy while choosing where and how to move. The implemented navigation algorithm is based on keeping agent trajectories within bounds of safe corridor volumes, specified over a terrain, rather than following an exact predefined trajectory, as described by Abramova et al. [3]. Terrain acquisition sensor model is consistent with the simplified model of rotating laser rangefinder, LIDAR, considering a discrete sequence of taking sensor reading, modeled as a snapshot based technique of acquiring sensor data. 1.1 Literature Review 1.1.1 Applications of Formation Motion Formation motion is applicable to any type of mobile agents: ground vehicles, satellites, manned and unmanned aircraft, micro-machines, even living creatures such as traveling insects, flocking birds and schooling fish. Nevertheless, the concept of formation motion is most widely used when discussing a flight of either piloted aircraft or UAVs. It is known that an aircraft with a large aspect ratio wings has better overall aerodynamic efficiency because of the smaller induced drag for a given lift. It is possible 3 to achieve similar reduction in drag by flying multiple aircraft in a wing-tip to wing-tip formation due to favorable wake-vortex encounters, as shown by Schumacher and Kumar [4], and by Galzi and Shtessel [5]. Drag reduction causes a decrease in power requirements, thus, resulting in aircraft fuel economy. The power reduction in the rear plane of a two-airplane formation was predicted theoretically. This benefit was estimated to be as high as 19% by the inviscid flow theory, and in excess of that number for larger formations, as described by Seiler et al. [6], Hummel [7], Lissaman and Shollenberger [8]. Wind tunnel tests have also demonstrated the existence of the drag reduction and consequent fuel economy. Experiments showed 15% power reduction in the follower plane of a two-airplane formation, which agrees with the analytical derivations, [6, 7]. In addition to the energy savings formation flight offers a wide spectrum of useful applications for both military and civil use. Formation flight could be used in the future air traffic management systems to deal with the predicted heavy increases in commercial air traffic, plan aircraft trajectories in the face of large weather disturbances, automated airborne conflict detection and resolution between aircraft, and provide for optimal scheduling of aircraft in the closely spaced parallel approaches into airports, as described by Tabuada et al. [9], Guilietti et al. [10], Tomlin et al. [11], Jenkinson et al. [12], Ghosh and Tomlin [13]. Maintaining a formation is of a paramount importance for air-to-air refueling, [10], Bloy et al. [14], Bloy and Joumaa [15], and Pachter et al. [16]. UAV formations are invaluable for use as a distributed sensor network for purposes such as reconnaissance and surveillance, damage assessment, exploration missions, evaluation of topographical and biological changes, monitoring of chemical and biological compounds in the air, [9], Johnson et al. [17], Chen and Ozguner [18], Borrelli et al. [19], and 4 Stipanovich et al. [20]. Because of their innate ability to control long baseline apertures UAV formations could be used to provide low-cost systems with distributed sensing capabilities for a wide spectrum of applications, such as synthetic aperture radar interferometry, [20]. UAV formation could reconfigure itself quickly in the event of point failure, be it inter-vehicular communication failure or loss of a vehicle, and still accomplish the mission, [3, 20]. Formation flight is also used for satellites and spacecraft in deep space. Increased robustness, improved instrument resolution, and reduced overall cost of the systems, are among the advantages of such systems, as described in Beard et al. [21]. NASA and Air Force consider multiple spacecraft approach, as one of technological priorities, as stated in [17], and Leitner et al. [22]. Multiple satellites, satellite clusters, flying in formation in the Earth orbit serve as a distributed sensor network utilized either for synthetic aperture radar interferometry, or as a giant antenna, [20, 21], Asawa et al. [23], Leonard et al. [24], LISA project [25], Min et al. [26], and Tanner et al. [27]. Loose formations of miniature probes operating at orbits around the Earth or the Sun could deliver data invaluable for improving understanding of magnetosphere dynamics, as described in [22]. Multiple spacecraft on an exploration mission in deep space use formation flight for creating the fault-tolerant control system capable of regulating the attitude errors and recovering from faults by reconfiguring the controllers, as stated in Meskin and Khorasani [12]. For an interferometer imaging application, where the effective aperture depends on the collector separation, a formation of multiple spacecraft could provide extremely large apertures, associated with considerable improvement in instrument resolution, as described in Smith 5 and Hadaegh [29]. Precise mechanism of formation control is required for such instrument application, [29]. Another applications of formation motion include coordination of the ground vehicles in future automated highway systems, [9], Desai et al. [30], and Varaiya [31]. Operation of multiple mobile robots is described in [9, 30], Balch and Arkin [32], Tan and Lewis [33]. Examples of navigation by cooperative localization are presented in Ren and Sorensen [34], Spletzer et al. [35], Trawny and Barfoot [36], and Urcola et al. [37]. Dynamics and control of formation motion is a broad field of study, generally divided into the following topics, researched in depth on both conceptual and detailed levels, including (a) communication within the group of objects moving in formation; (b) conceptual design of control laws to achieve motion in formation while minimizing positioning errors and ensuring string stability; (c) design of an actual control system, including controller, observer, and actuators, for each individual member of the group to be moving in formation, comparison of different possible designs and selection of the most suitable solution for a given case of formation motion; (d) mathematical representation of formation geometry and its evolution during formation motion, using graph theory features. The presented work falls into a category of conceptual design of the control laws for formation flight control. 1.1.2 Approaches to Formation Control Among the existing strategies and approaches to formation control, considered as the most suitable for applications of formation flight, are behavioral, virtual structure, 6 leader-follower and synthetic, approaches. Motion coordination strategies could also be viewed as specific cases of formation control. 1.1.2.1 Behavioral Approach Nature provided the first inspiration for a concept of formation motion in the shape of swarming bees, schooling fish, flocking birds, and herding animals. The most famous swarm model, utilizing a distributed behavioral approach to synthesizing motion of swarming particles, is boids introduces by Reynolds [38]. Boids present behavioral model for flocking. Boids behavior is governed by a set of separate components including inter-agent collision avoidance, velocity matching, and flock centering. Boids clearly demonstrate the emergence of coherent group behavior while individual agents only sense their local environment and close neighbors, [32, 38]. The basic idea behind the behavioral approach, as shown on the example of boids, is to prescribe several simplistic desired behaviors for each agent, behavior primitives, and to make the control action of each agent a weighted average of the control for each behavior, [21] and Tabuada et al. [38]. Coordination between mobile agents is achieved through shared knowledge of the relative formation configuration states. The strengths and weaknesses of the behavioral approach are described in [21, 38]. Its major strengths are as follows, (a) intuitive derivation of control strategies when for the cases when agents have multiple competing objectives, (b) existence of explicit feedback to the formation since each agent reacts according to the position of its neighbors. Its primary weakness is that group behavior, emerging from a sum of agent behaviors, which, in their turn represent a weighted sum of the specified behavior primitives for each agent, cannot 7 be explicitly defined. Therefore, behavioral approaches are difficult to analyze mathematically and characteristics of the formation, such as stability, cannot generally be guaranteed, [21, 38]. To allow for analysis by standard graph theoretic methods behavioral approach is often augmented with some other formation control technique. An example of such mixed approach is presented in [32], where behavior primitives are successfully combined with the principles of leader-following strategy, implementing formations of unmanned ground vehicles. 1.1.2.2 Virtual Structure Approach In the virtual structure approach, the entire formation is treated as a single structure, similar to a rigid body, where all points in the object maintain a fixed geometric relationship via a system of physical constraints, and no disturbance can be made to one particle that is not propagated to all particles, as described in [21, 33, 39]. The distance between agent positions defines formation configurations. Constant distance settings result in a structure with strongly enforced rigidity. Such structure is mostly used for implementation of sensor and manipulator systems, [33]. Derivation of formation control consists of several steps, including derivation of dynamics of virtual structure, translating motion of the structure into motion of individual agents, and deriving tracking control for the agents, [21, 39]. The advantage of virtual structure approach is the ease of specifying the desired behavior, while it major weakness is in limitation on types of suitable applications for such systems, as stated in [21, 39]. 8 Examples of using a virtual structure approach to the formation motion are found in [26, 33]. A variety of formations geometries can be maintained while performing precise fault tolerant maneuvers, [33]. Simulation and experiments were conducted to verify the performance of the proposed algorithm for a group of three identical robots, [33]. A satellite cluster, a group of several micro-satellites working as a virtual structure for the purposes of SAR interferometry is described in [26]. For such application, geometric relations between satellites, position on the Earth orbit, must remain unchangeable during motion in order to achieve the acceptable precision of measurements, [26]. 1.1.2.3 Leader-Follower Approach Leader-follower is, by far, the most popular approach for winged aircraft formation flight. That type of formation structure was first noticed in the flight of birds. As the biologists studied V-formation for the prolonged flights of migratory birds, they came to a conclusion that V-formation allows equal or approximately equal distribution of drag savings among formation members, including the leading bird, [8]. Leader- follower formation structure is defined by as set requirements specifying constant or time varying offsets between position and orientation of the leader and formation members, [10, 21, 39]. A technique of designating multiple leaders may be used in order to support chain or tree formation topologies. As notes in [10], maintenance of formation trajectory in leader-follower formation flight is considered as prime responsibility of the leading aircraft. The strength of leader-follower approach is that group behavior is directed by specifying the behavior of a single quantity, the leader. The weaknesses, however, are 9 that; (a) there is no explicit feedback to the formation; if leader moves too fast for the other agents to track, there is usually no way to communicate the problem to the leader; (b) the leader is a single point of failure for the formation. The leader-following strategy for formation control has received significant attention from the researchers. Its applications in various motion-restraining conditions have been studied extensively, and relevant control algorithms were developed and implemented in hardware. The leader-following control systems performed sufficiently well for both aircraft and ground vehicle formations, [4, 5, 10, 16, 17, 20, 27, 30], Li et al. [40], Koo and Shahruz [41]. Nevertheless, such strategy has a very serious inherent limitation, string instability, resulting from amplification of disturbances as they propagate through the formation, or amplification of errors as they propagate through the coupled feedback loops, as described in [6]. The authors showed that constraint on error dynamics within a formation is a fundamental, inherent, characteristic, attributed to the presence of integrator in the control loop, [6]. The authors considered the coupled feedback loops as an abstraction of the leader-follower strategy for a chain of multiple ground vehicles, where position of the i th vehicle is used as the reference for the (i+1) th vehicle, in order to derive the error dynamics. The results presented in [6] imply that leader-follower chain is considered an inherently poor control strategy for formation control, due to string instability. The authors suggest investigating nonlinear control methods, considering intra-vehicle communication, and designing distributed control in order to find more suitable solution, [6]. 10 1.1.2.4 Synthetic Approach In order to achieve better performance by balancing strengths and alleviating weaknesses of behavioral, virtual structure, and leader-following formation control approaches, a synthetic method that combines all three, and is amenable to analysis with accepted control-theoretic methods, may be used. An example of the formation control architecture, based on such synthetic approach is presented in [21]. That architecture is used for implementing a system of multiple spacecraft interferometer, [21]. The design was prompted by the existence of several levels of control in formation flying. At the highest level of abstraction is the dynamic transition from one subtask to another, represented by a discrete-event supervisor. At the next level of abstraction, there is a mechanism to coordinate the motion of each individual spacecraft to synthesize the desired behavior of the constellation, represented by a formation control block. At the lowest level of abstraction, each individual spacecraft needs to be controlled to be consistent with the coordinating mechanism, represented by a local controller. There also is a feedback between all these three levels of abstraction. The article [21] describes the primary formation coordination mechanism as the states of the formation control block and the states of the discrete-event supervisor. The supervisor uses performance vector, computed considering performance variables of each spacecraft, to determine the input to the formation control block, which would be translated into the coordination variable and broadcasted to every spacecraft. 11 1.1.2.5 Motion Coordination as a Specific Case of Formation Control Various applications of autonomous robot teams may not require explicit formations, but rather coordination of motions of multiple robots, performing their respective tasks and interacting in the same environment. Such coordination is aimed at both collision avoidance and maximizing performance efficiency, and could be achieved in a variety of ways depending on the specific application. For example, for the task of warehouse management paths of robots could be pre-planned and coordinated in advance, while for a search and rescue operation emphasis is on real-time collision avoidance. A review of approaches to path planning and motion coordination problem in autonomous robot teams is presented in Parker [42]. Motion coordination between two satellites on elliptical orbits is presented in Lee et al. [43]. The authors [43] derive analytical solutions to the variational equations of completely integrable nonlinear, autonomous dynamical unperturbed two-body system of orbiting satellites. 1.1.3 Centralized vs. Decentralized Strategies for Formation Motion Control The strategies for controlling an aircraft formation could be further divided by the type of formation management, centralized, decentralized, or a combination of both. Another type of classification considers information exchange mode, including point-to- point, broadcast, a synthetic method utilizing either communication mode depending on the control algorithm, or a case where there could be no communication at all between formation members, such as in case of the vision-based formation control as shown in [17]. 12 In the centralized formation management there exists a single formation manager, supervising all the aircraft and managing the topology of the channels used to exchange information among them. This manager can be either on one of the aircraft or ground- based, [10]. The important advantage of the centralized scheme lies in its capability to react to and reach decisions from a potentially greater processing capabilities than achievable by on-board computers, possibly adding a human decision to the automatic system. The major disadvantages of the centralized formation control are (a) large amount of communications, which may be undesirable or even impossible; (b) an unavoidable delay in reacting to a failure. Also, if the formation manager is located on one of the aircraft, and that aircraft experiences failure, the whole mission is lost, even though the other members of formation are in working order and could have continue towards the goal. If the formation manager is ground-based, even a temporary loss of communication puts a mission in dire jeopardy. Due to these serious drawbacks of the centralized control scheme, the decentralized formation management comes into consideration. In a decentralized management scheme each formation member is assigned part of the processing and decision making tasks, [10]. The advantages of decentralized formation controls include (a) reaction times can be minimized; (b) low volume of communications, only inter-aircraft information must be exchanged, except for possible mission updates from the ground control station; (c) low-power or non-radio-based communications such as optical sensors can be used because of the relatively small distances among aircraft; (d) could be successfully used when no communication is available. Nevertheless, the decentralized formation control puts greater demands on the 13 distributed decision-making algorithm. It must produce deterministic results on all the managed aircraft, and conflicting decisions must be avoided for formation safety. An example of using the decentralized formation control in absence of communication is presented in [17]. It shows that in a case where no communication between aircraft is available, passive 2D vision system designed using Extended Kalman Filters for image processing could be used for formation maintenance. The architecture of vision-based formation control is based solely on the leader-following approach. Collision hazards between following aircraft are prevented by careful selection of these chosen relative positions. When static obstacle avoidance is required, the formation is broken and re-assembled after the hazard is passed, [17]. An instance of the control architecture that could accommodate both centralized and decentralized control schemes is described in [21]. Centralized control is possible when high-bandwidth communication is available. Supervisor and control block is positioned at a centralized location and broadcast of the control variable to local control loops of individual spacecraft. Decentralized control scheme would require each spacecraft to instantiate a local copy of the supervisor and control block, and synchronize them. In that case synchronization could be achieved either by infrequent low-bandwidth communication, or estimation of the states of instantiations of the supervisor and control block on the other spacecraft. Decentralized formation control strategy for mobile robots performing manipulation tasks is presented in Desai et al. [44]. The proposed framework combines kinematic and dynamic aspects of a motion planning task, and is scalable for changing number of robots and controllers. 14 A set of decentralized and distributed control architectures that allow changes in formation shape for obstacle avoidance is presented in Monteiro and Bicho [45]. A leader-follower type formation of 4 and 6 non-holonomic robots using non-linear attractor dynamics is modeled, showing how the behavior of each robot is generated by time series of asymptotically stable states, [45]. Flexibility of the proposed method is demonstrated in simulations, where autonomous robot formations that have no previous knowledge of the environment, successfully avoid sensed obstacles while keeping formation. 1.1.4 Graph Theory for Developing and Analyzing Strategies for Formation Control Graph theory is one of the major tools sued for studying formation motion. Graphs are used to represent formation topology, as described in Olfati-Saber and Murray [46], Eren and Morse [47], relationships between different parts of the formation structure as well as inter-vehicle constraints and constraints feasibility [9, 27, 30, 39], error propagation [10, 30], Tanner et al. [48], and describe communication between formation members [10]. The formation control system and structure could also be reflected in a graph, and formation reconfigurations could be viewed as transitions from one control graph to the other [30]. 1.1.4.1 Graph Theory and Formation Recovery from a Fault The use of graph theoretic methods for optimizing the minimum error propagation path within the formation is illustrated in [10]. Graph describes inter-aircraft communication. Nodes of the graph represent aircraft, and the edges are physical 15 communication channels. This communications graph shall be connected in order for the formation to be coherent, and shall also be redundant in order to run formation reconfiguration procedure, node change, in case of transmitter or receiver failures. The node change may involve either switching aircraft positions or moving an aircraft into an empty node. The authors also show optimization of available communication channels under a cost function constraint. The optimization is desired because of the periodic broadcast of fault event information to all aircraft so that everyone is informed of the active nodes and node loss is detected timely, [10]. For the case of limited communication and sensing within the formation a problem of formation reconfiguration and subsequent maintenance after the fault, vehicle loss, occurs could be successfully solved, using graph theoretic methods, as shown in [47]. A method of preserving formation rigidity after loss of an agent is presented, considering that it is desirable to minimize the amount of rearrangements of links. The solution proposed in [47] is shown to be applicable for the formations with a random number of links in both 2D and 3D space. 1.1.4.2 Analysis of the Formation Geometry Feasibility with Graph Theoretic Methods Another common application of graph theoretic methods to formation motion is to analyze feasibility of the formation geometry and formation stability. As discussed in [9, 39] formation feasibility for a typical leader-follower formation depends on the formation geometry, and derive differential geometric conditions that guarantee formation feasibility given the individual agent kinematics. Certain trajectories, while feasible for a 16 single moving agent, may be incompatible with the formation motion. Formations are modeled using control graphs, whose nodes capture the individual agent kinematics, and whose edges represent inter-agent constraints that must be satisfied. Undirected formation graphs are used for systems with centralized formation control, and directed for systems with decentralized one. Similar approach to formation analysis is presented in [30], discussing the control of ground vehicles formation, navigating a terrain with obstacles. Formation control is based on the traditional leader-following approach, where the lead vehicle plans the group trajectory. The controls of the followers are specified using non-isomorphic graph, [30]. Navigating through the obstacles causes the formation to change its shape and, subsequently, its control graph. Formation feasibility can also be viewed from the point of efficient information exchange. A study of formation topology as means for efficient information exchange, using graph theory methods and approaches, is presented in [34]. The ways of overcoming the limitations of communication range and bandwidth, which often present a major problem for large-scale formation control applications, are explored, [34]. 1.1.4.3 Application of Graph Theoretic Methods for Formation Stability Analysis A study that uses graph theory for analyzing formation stability is presented in [27]. The formations considered are of the leader-following type, and are represented by acyclic directed graphs, where vertices are the formation members, and directed edges embody the leader-following relationships. Nonlinear gain estimates are derived capturing how leaders behavior, inputs and disturbances, affect the interconnection errors 17 observed within the formation . The concept of leader-to-formation stability, LFS, which is based on input-to-state stability and its invariance properties under cascading is used in [27]. Analysis of LFS gains allows to assess stability of the formation as a whole and of particular subgroups within the formation. The theoretical conclusions were applied to formations of autonomous non-holonomic ground vehicles. Different formation types such as string of vehicles, cascade, and parallel formations are analyzed. LFS is shown to be a valid analysis tool for investigating the robustness of formation topologies and exploring error bounds for the worst case scenarios. The concept of input-to-state stability, ISS, is considered in [48]. Stability conditions and bounds for the formations, based on leader-following approach, are established. ISS is considered a weaker form of stability than string or mesh stability, which relates leader inputs to formation state errors. Its most important property is preserving ISS in many system interconnections. Thus, it is possible to propagate ISS from the level of basic interconnection to the whole network by exploiting the relevant interconnection properties. Basic interconnections such as 3 agents cascade, 4 agents parallel, 5 agents multiple leader, and 2 agents cyclic are considered. ISS gains are recursively computed for each one. Stability properties of these elementary interconnections are later expressed through the said gains, and are propagated throughout the network in order to establish the global stability bounds, [48]. Analysis of formation stability may call for augmenting graph theoretic methods with the Lyapunov based techniques. This is particularly relevant for dealing with the mobile agents that have nonlinear dynamics or may perform arbitrary rotations and translations. The example of such modified approach is presented in [46], focusing on 18 defining the methods for regaining formation stability after split and rejoin maneuvers and reconfiguration of the information flow within the formation. Three types of stabilization, tracking, problems for dynamic multi-agent system are defined using the notion of graph rigidity and minimally rigid graph as means of identifying the shape variables of a formation. Structural stabilization and navigational stabilization and tracking are achieved for a formation of two agents, this formation is augmented with a new agent that establishes two edges with the two existing agents in the graph that represents the formation. Agents are represented by the nodes of the graph. Each agent performs sensing and communication with all of its immediate neighbors. The authors prove that rigidity of the graph is preserved during node augmentation, thus, could be used for expanding the existing formation without compromising its stability, [46]. 1.1.5 Algorithms and Strategies for the Formation Motion Control A number of algorithms designed for formation control has been introduced. These methods include Receding Horizon and Model Predictive Controls, RHC and MPC, Kothare et al. [49], Lu and Arkun [50], Linear Quadratic Regulation algorithm, LQR, [4, 10, 18], and High Order Sliding Mode Control, HOSM, [5]. Control strategies include state-space control [16, 40, 41], mode based flight [13, 20], and adaptive controls, Kim et al, [51], Sonneveldt et al. [52], Lavretsky and Hovakimyan [53]. 1.1.5.1 RHC or MPC Algorithm Receding Horizon Control or Model Predictive Control gained its popularity as a method of choice for controlling relatively slow dynamical systems such as chemical 19 processes. It has originated as a real time algorithm suitable for solving multivariable constraint problems while accounting for time delays, [50]. RHC solves online an open- loop constrained optimization problem at each time instant, computes optimal control inputs over a fixed number of future time instants, time horizon, and implements only the first element of that optimal control profile, [49, 50]. At the next sampling time the initial condition is updated with the new state of the system, and the process is repeated. The online optimization could be reduced to either linear or quadratic program. Linear optimization technique often involves Linear Matrix Inequalities, LMIs. This optimization could be solved in polynomial time, comparable to that required for evaluation of an analytical solution for that problem, resulting in a feasible online implementation. A number of concepts of the existing robust control theory could be applied within the framework of LMIs, [23, 49, 50] and Kothare et al. [54]. RHC based design technique that aims at robust RHC synthesis while allowing explicit incorporation of the description of plant uncertainty in the problem formulation is discussed in [49]. Inability of a straightforward RHC based control system to explicitly incorporate plant model uncertainty is considered the major drawback of that algorithm. Thus, if there exists a mismatch between the real plant and its model, the said controller does not exhibit robust behavior. One of the methods of dealing with plant uncertainty in RHC considers modification or conversion of minimization to min-max problem, attempting to minimize a worst-case objective function for infinite horizon. The resulting RHC system that robustly stabilizes plants with uncertainty descriptions, expressed in both time and frequency domains, is demonstrated in [49]. 20 The problem of controlling nonlinear system, using RHC algorithm, is considered in [54]. Approximations of nonlinear system using feedback linearization, FL, and linear time-varying, LTV, systems are considered. FL-MPC is found to be sensitive to nonlinear plant model uncertainty. LTV-MPC method implemented using LMIs is considered robust. It is shown that its asymptotic stability can be determined with Lyapunov methods, [49, 54]. Infinite horizon scheduling RHC algorithm for discrete polytopic LPV plants is presented in [50]. The proposed algorithm is termed Quasi-Min-Max MPC. The optimization problems are solved by semi-definite programming involving LMIs. Due to the recent manufacturing advances it has become possible to implement RHC, MPC suitable for systems with fast dynamics, such as MEMS, as described in Bleris and Kothare [55], or formation flight [19, 23, 55], and Keviczky et al. [56]. The development of embedded MPC for micro-chemical system applications is presented in [55]. Embedded hardware implementation of MPC is developed using a high- performance processor, microcontroller, as described in [49, 55]. Formation flight maintenance is the type of control problem where RHC, MPC technique could be applicable due to high degree of nonlinearity of the dynamical systems in question. The boids formation flight, [38], could be viewed as the earliest application of RHC, a special case, where time horizon equals one. A decentralized RHC based control system that could guarantee collision avoidance while fulfilling the constraints imposed by formation flight, is proposed in [19, 56]. The authors represent a dynamical system as a set of decoupled, distinct, independently actuated subsystems. The resulting control scheme consists of distinct RHC controllers, one for each subsystem, 21 where the local control inputs are computed based on the states of the subsystem itself and its neighbors, [19, 56]. 1.1.5.2 LQR Algorithm Linear Quadratic Regulation technique is popular for formation controller design, in particular for the systems where formation flight control is implemented by a two-loop system, [4, 10]. A control system for formation of UAVs where both inner and outer loops are designed on the basis of LQR is demonstrated in [10]. Inner-loop controller, is designed for tracking while outer-loop controller generating a reference path considering the desired formation trajectory. Even though LQR based formation flight controller provides a viable option for formation maintenance, it could be augmented with the other methods and controller types to achieve more robust performance. A system of UAVs in close formation of the leader-following type, where formation control is implemented in a two-loop system, is discussed in [4]. The formation flight is used here for the explicit purpose of aerodynamic drag reduction. LQR technique is used for the outer loop, while inner loop uses adaptive control methods. Such design is selected because it requires only minimal knowledge of the vehicle dynamics, and could successfully achieve tracking in the presence of uncertainties, [4]. An example of using LQR based formation control for a system that considers a hybrid, discrete and continuous, UAV models is shown in [18]. The discrete part is represented by a finite state machine, [18]. The continuous part considers lateral kinematic model, as described in [13, 18, 20]. The formation is divided into triangular 22 interconnected elements with mutual nodes. The hybrid controllers are designed separately for each of these subsystems, and then contracted back and applied to the original interconnected formation. The continuous part of the hybrid controllers, used for trajectory following, is LQR based, [18]. The simulations show that these controllers perform well in formation keeping. Triangular forms are maintained by retaining the distance between neighbor agents preventing collision occurrence. 1.1.5.3 HOSM Algorithm Autopilot design for an autonomous leader-follower close coupled formation flight, using continuous high order sliding mode control is proposed in [5]. HOSM based controller provides tracking for the follower while minimizing effects of drag force. HOSM technique allows to achieve a perfect tracking of the longitudinal and lateral separation commands by providing finite time convergence to zero of the tracking errors, as described in [5]. A relative degree approach is used for the controller design. The provided simulations are based on a full 6-DOF fixed wing aircraft model, developed by Boeing. The performance of HOSM based controller was compared with the results obtained using PID and neural network based adaptive controls and considered satisfactory, [5]. 1.1.5.4 State-Space Control Strategies The traditional state-space control strategies are widely used for the leader- following formation flight. Control problem is defined as formulating a suitable tracking 23 control law, where tracking may pertain to the followers trajectories, put forward by the leader, [41], or followers positions relative to the leader, [16, 20, 40]. Control design of an autopilot for formation flight maneuvers, such as aerial refueling, automatic coordination between leader and followers, and development of a robotic wingman, is discussed in [16]. The authors decompose this control problem into two uncoupled linear single-input two-output dynamic tracking control system design problems, [16]. Leader-following type of UAV formation, where the leader determines the desired, safe and achievable trajectories for all the followers and for itself is presented in [41]. These trajectories are communicated to all the follower UAVs that are expected to lock onto them in finite time and ensure perfect tracking. State-space methodology was used for design and analysis of control laws, [41]. Another approach to an autopilot design for leader-following type formation of multiple UAVs, using state-space methods, is shown in [40]. Flight model considers the nonlinear dynamical characteristics of vortex and external disturbances, arisen from the close formation. The design focuses on the tracking control, considering heading parameters, such as speed, angle, and altitude, [40]. Three robust control schemes to steer the relative tracking error in lateral, longitudinal, and altitude directions to zero asymptotically are developed. The analysis with the methods of Lyapunov stability theory verifies robustness of the proposed control law design, [40]. The design of a decentralized overlapping control for UAVs flying in close formation, using static state feedback, is proposed in [20]. The dynamic model of the formation is represented as an interconnected system with overlapping coupled subsystems, developed using model input-state feedback linearization. It is assumed that 24 a formation member could obtain state information only of the vehicle directly in front of it. Stability of the formation is described in terms distances between UAVs and analyzed considering local Lyapunov stability, [20]. The design of a decentralized nonlinear robust control system for formation flight of UAVs is presented in Singh et al. [57]. Formation considered is of the leader-following type, and the proposed control system takes into consideration the nonlinear forces and moments of the vortex, generated by the lead vehicle, and induced on the trail vehicle. These forces, that are complex functions of relative position coordinates of the UAVs, are considered unknown and unable to be parameterized. Each follower synthesizes its control law, using its own state variables and its relative position with respect to its immediate leader. Each formation member communicates with its immediate leader only, and depends on its own sensors for position measurement, [57]. State-space methods are also applicable for designing a control system for spacecraft formation flying. Meskin and Khorasani [58] investigate development of fault detection and isolation filters for multiple spacecraft in formation in deep space. Control system design for spacecraft formation flight is shown in Folta [59]. The use of improved numeric methods to attain constrained formation geometries and control their dynamical evolution is discussed, using the examples of spacecraft missions supported or developed by NASA, [59]. 1.1.5.5 Mode-Based Flight The mode-based flight was developed for the piloted aircraft on-board flight management systems, and presented in [11, 13]. The main purpose of such systems is to 25 design an autopilot capable of adjusting aircraft trajectories satisfying requirements of maintaining minimal separation distance between aircraft, especially when approaching airports, while accounting for the weather, wind, disturbances. Mode based control is used to guarantee bounded tracking errors. Mode is defined as a trajectory segment obtained with a constant subset of state parameters, for example bank or heading angle, [13]. Within the context of mode-based flight the control problem is hybrid. On one side it involves designing the continuous control laws for the aircraft within each defined flight mode, and on the other side it involves devising a method for sequencing the discrete modes together, [11, 13]. Simulation results show the proposed design provides the desired tracking in the various modes of operation and smooth switching between modes. It is capable of following the prescribed trajectories with zero error, [13]. 1.1.5.6 Adaptive Controls Adaptive controls and neural networks present a promising control strategy for both formation and individual object motion. Integrated guidance and control design for aircraft formation flight using a combination of adaptive output feedback inversion and backstepping techniques is demonstrated in [51]. The control problem is formulated as an adaptive output feedback control for a line-of-sight-based formation flight configuration of a leader and a follower aircraft, where the follower has an onboard camera to see and track down the leader. Adaptive neural networks are trained online with available measurements to compensate for unaccounted, not modeled, nonlinearities in the design process, including uncertainties due to unknown leader aircraft acceleration and the 26 modeling error due to parametric uncertainties in the aircraft aerodynamic derivatives, [51]. Adaptive back stepping flight control law for F-16 using recursive, Lyapunov- based, nonlinear design method that makes use of online parameter estimation laws to deal with parametric uncertainties is discussed in [52]. The main idea of the demonstrated approach is to use some states as virtual controls to control other states. B-spline neural networks are used to parametrize the nonlinear functions so that the update laws can adapt to the unknown parameters, such as uncertain nonlinear aerodynamic forces and moments. Direct model reference adaptive control framework, developed for uncertain multi-input linear dynamical systems in the presence of position limited actuators is presented in [53]. The proposed design is Lyapunov-based and ensures global asymptotic tracking for open-loop stable systems. The authors confirm the theoretical conclusions by performing a simulation, using the linear roll, yaw dynamics of F-16 aircraft. 1.1.6 Formation Motion and Cooperative Localization Formation motion often calls for cooperative localization that could be also viewed as using formation for navigating the environment. One of the examples of cooperative localization is coordination of multiple autonomous robots in cooperative manipulation tasks. An approach where cyber-vision is used for establishing relative position and orientation of the robots, and maintaining formation is proposed in [35]. Ground-based vehicles, robots, are equipped with omnidirectional cameras, transmitters, and identification collars as means to identify each other by using visual imagery. The 27 hybrid formation type, defined as a mix of leader-follower and virtual structure is considered. The described framework is based on two major elements, (a) cooperative scheme for localizing the robots based on visual imagery, (b) a set of control algorithms that allow the robots to maintain shape and size of the prescribed formation. Large teams of robots are decomposed into smaller groups, allowing localizer and controller to be used for these individual groups, considering limited communication, [35]. Terrain navigation by ground-based robots that use each other positioning data instead of orientating by terrain landmarks is explored in [36]. Formation shape is shown to be of importance for efficient navigation. The authors demonstrate an approach to optimizing entire trajectories for a group of mobile robots that use one another as localization beacons. The cost function to be optimized is defined as a measure of localization uncertainty as opposed to distance travelled or time. The classic Extended Kalman Filter approach to robot localization is used, employing a single monolithic filter for the entire group of robots, [36]. Cooperative navigation in a hybrid type formation, virtual structure and leader- follower, of ground-based robots is explored in [37]. The links within the virtual structure formation are defined by spring-damper elements in order to allow the formation to comply with environment shape. Leader-follower structure comes into consideration for designing navigation strategies. The leader plans the trajectories by using a two-level path planner with obstacle avoidance capabilities. Kinodynamic constraints of the robots are taken into account in the context of trajectory generation. The authors use realistic urban scenarios for simulations and experiments with real robots, [37]. 28 Cooperative control of formations of non-holonomic mobile robots equipped with range sensors is presented in Fierro et al. [60]. The derived control laws allow the robots to navigate in a partially unknown environment, controlling their respective positions and orientations with respect to neighboring robots and environment obstacles. The developed coordination protocol automatically switches between the several specified control laws to maintain a particular formation. Two types of trajectory generators, derived from the potential fields theory, are presented considering cases when, (a) each robot plans its own reference trajectory on the basis of the information available to it, (b) robots share information and maintain rigid, virtual structure, formation. Scalability of the proposed method for large formations is demonstrated, [60]. 1.1.7 Formation Motion and Non-GPS Based Navigation Maintenance of the formation geometry requires each mobile agent to compute the distance to its reference, either an immediate neighbor, preceding agent, or an obstacle. Usually a formation member acquires its position from a GPS-based position sensing system and receives, through appropriate communication channels, the other agents positions. Examples of GPS-based technologies for satellite formation control are shown in [23, 26, 28]. GPS-based formation maintenance and navigation control systems are expected to play an important role in the upcoming space missions, [22]. Precision- guided military systems, such as Multiple Launch Rocket Systems, MLRS and GMLRS, rely on embedded GPS navigation combined with internal navigation system, Lockheed Martin [61, 62]. 29 Nevertheless, despite the advances in the development and deployment of robust GPS systems, GPS dependent designs might lead to mission failures if the signal is lost, jammed, or becomes unavailable. For deep space missions, such as interferometric imaging application, GPS is simply unavailable. In that case, navigation processing is expressed in terms of the relative spacecraft positions and absolute spacecraft orientations, [29]. The spacecraft in formation are free flying, and their dynamics are coupled only through the application objectives and measurements of relative spacecraft positions and velocities, [29]. For mobile agents, such as UAVs and missiles, GPS also may become unavailable due to jamming, reception area coverage gaps, and communication faults. Vulnerability of GPS to interference is partially attributed to the fact that it is a low power signal, ONR [63]. Therefore, the development of GPS independent navigation capability is desired, as stated in [63] and Aboutalib et al. [64]. Relative navigation system for an aircraft in formation flight could be beneficial for dealing with GPS outages and short-term communication faults. The approach involving using relative GPS measurements to calibrate the relative internal navigation, INS, positioning system is described in Felter and Wu [65]. The resulting system can tolerate GPS outages because of the high short-term accuracy of the INS. The relative navigation is formulated as a federated filtering problem. Relative measurements from the GPS and range sensors are processed separately to obtain two independent estimates of the common relative INS errors. These estimates are then combined to output a single estimate of the relative INS errors. These errors are then used to correct the relative INS position. An error analysis is carried out for the federated filter in the form of an estimation error covariance for the relative navigation state, [65]. 30 A method of using signals of opportunity for implementing non-GPS dependent navigation systems is described in AFIT [66]. A signal of opportunity is defined as any available radio, television station, or satellite broadcast transmissions. It is noted that such signals are stronger then GPS and could provide reliable navigation aid. An approach involving combining all available sensor and digital elevation map data in order to aid navigation process is designated as All Source Adaptive Fusion, ASAF, as described in [64]. Among advantages of ASAF the core ones could be summarized as, (a) it is fully adaptive to the existing mission environment since it combines in real-time various sources of information as they become available; (b) it provides navigation capabilities in the GPS-denied environment; (c) it supports obstacle avoidance and provides 3D digital terrain mapping in the presence of GPS, [64]. 1.1.8 Cooperative Navigation and Localization Localization is an integral part of any reliable GPS-independent navigation method since in order to follow direction to target a mobile entity needs to be able to determine its position and orientation in the environment by comparing its own sensor readings to the map data. Localization could be absolute or relative. Absolute localization determines the position and orientation of the mobile agent with respect to a map with the predefined world-fixed coordinate system, while relative localization does it with respect to other mobile agents or some other specific objects, Grabowski and Khosla [67]. Relative localization is important for formation maintenance and other cooperative motion problems, as stated in Premvuti and Wang [68] , and Kurazume et al. [69], even though the uncertainty in the estimated absolute position is often unbounded. 31 Localization is, generally, could be viewed as a state estimation problem. Bayesian state estimation methods work with the probability distribution across the entire state space, thus, are computationally expensive, as described in Crespo et al. [70]. The Bayesian estimator is capable of tracking multiple hypotheses, corresponding to peaks in the distribution, until further measurements allow convergence to a single estimate, [70]. Kalman Filter, the most commonly employed optimal Bayesian state estimator, is introduced by Kalman [71]. It generates an optimal state estimate for linear systems where the sensor and process noise have known Gaussian characteristics. Extended Kalman Filter, EKF, that uses Taylor expansion of dynamics of a nonlinear system about its current mean and covariance, is described in Jazwinski [72]. Unscented Kalman Filter, UKF, that models a nonlinear system by propagating the covariance estimate through a set of sample points through the nonlinear system model, is described in Julier and Uhlmann [73]. Federated Filter is a distributed version of Kalman Filter designed to share information, error covariance matrix and standard state vector, among multiple local filters and one master filter, and from their solutions build a total solution, as described in Bingham [74]. Distributed localization approach performing data fusion by using observations from a team of mobile robots is presented in Roumeliotis and Bekey [75]. Multi-agent localization problem is decoupled with state coupling occurring only when relative position and orientation observations become available. Thus, standard Kalman Filter processing can be defined as decentralized and distributed among the agents, [75]. A cooperative navigation system algorithm for a group of communicating robots, based on a Kalman Filter, is presented in Sanderson [76]. The distributed EKF-based 32 localization technique for a team of robots with absence or degradation of GPS signal is presented in Madhavan et al. [77]. Examples of using EKF-based localization for cooperative navigation of communicating autonomous underwater vehicles are presented in Maczka et al. [78] and Zhang et al. [79]. Gauss-Hermite Filter, GHF, is another type of Gaussian filter used for nonlinear systems, as described in Ito and Xiong [80]. GHF- based cooperative navigation method for multi-missile formation of leader-follower type is presented in Xing et al.[81]. Particle Filter is another popular state estimator method, defined using sequential Monte-Carlo technique, avoiding the assumptions of Gaussian noise and linear dynamics by directly sampling the underlying distribution, as described in Doucet et al. [82], and Fox et al. [83, 86]. Particle filters approximate the full Bayesian distribution with a set of particles, each of which represents one estimate of agent position, [84]. Computational load by maintaining only an approximation of the full probability distribution, [83]. Particle filters allow performing global localization, [84]. An example of such system for cooperative global localization is demonstrated in Peasgood [85]. Stochastic estimator-based cooperative concurrent mapping and localization technique that merges sensor and navigation information from multiple autonomous vehicles is presented in Fenwick et al. [86]. 1.1.9 Constrained Motion, the Udwadia-Kalaba Equation, and Formation Control A new approach for deriving equations of motion for constrained dynamical systems is introduced in [1, 2]. The resulting method of deriving equations of motion, known as the Udwadia-Kalaba Equation of Constrained Motion, [1, 2], allows holonomic 33 or non-holonomic, rheonomic or scleronomic, integrable or non-integrable constraints to be used for derivation. Forces of constraint, obtained with this method, are minimized in accordance with Gauss Principle of Least Constraint. The method does not require finding Lagrangian multipliers, using instead the weighted Moore-Penrose inverse of constraint matrix, [1, 2], Udwadia [87], Udwadia and Kalaba [88]. In addition, it does allow functionally dependent constraints to be used, as described in Udwadia [89]. Thus, this method considerably simplifies the task of defining a suitable set of constraints for derivations of equations of motion. Formation of orbiting spacecraft, in conjunction with application of Udwadia- Kalaba Equation of Constraint Motion to derive exact equations that define control forces, is introduced by Lam et al. [90] and Lam [91]. The formation tracks predefined orbit, while satisfying the requirement of maintaining specified inclination. Control forces are defined considering non-uniform gravitational field and rotation of the planetary body, orbited by the spacecraft, [90, 91]. Control method for precise keeping of virtual structure-type rigid formation of two and four spacecraft is presented in [90]. Approaches to control of multi-body spacecraft system, where linear, rotational, and elastic motions of two spacecraft are coupled are presented by Schutte et al. [92]. A generalized approach to modeling multi-body systems, considering the desired number of generalized coordinates and constraints, required for modeling such systems, is introduced by Schutte and Udwadia [101]. The method presented in [101] addresses the problem of dealing with the complexity arising from the structure of such systems, while deriving equations of motion in closed form. Nonlinear control law that allows to precisely maintain a prescribed tumbling trajectory of the dynamical system of two 34 spacecraft, represented as rigid bodies connected by a nonlinear elastic spring, as described in [92]. An analytical method for the formation-keeping problem of multiple satellites, solving the problem in the Hill frame is developed by Cho and Udwadia [93]. Full nonlinear control problem is considered, and closed-form solutions for general reference orbits are derived, [93]. Holonomic formation geometry constraints are considered in [90-93], while addition of non-holonomic constraints to the combined constraint set is demonstrated in [91, 92]. Baumgarte stabilization technique, Baumgarte [100], allowing specifying initial conditions that violate constraints is applied, as described in [90, 93]. 1.1.10 Constraint Based Aircraft Formation Flight Control A method for formation flight control of aircraft by using forces of constraints, deriving equations of motions using method of Lagrange multipliers, is introduced by Zou et al. [94, 95, 97], Zou and Pagilla [96]. Formation control algorithm achieves the formation from arbitrary initial positions of aircraft, maintains formation geometry, and tracks the desired group trajectory that is consistent with constraints. Separation between aircraft or safe distance between agents is maintained, [94-97]. Distributed formation control of multiple aircraft is described in [95-97]. Adaptive control allowing compensating for uncertainty in the drag coefficient, is described in [95]. The developed control algorithm allows seamless addition and removal of formation members by modifications of the control input. That provides for an inherent scalability of the developed control method, [95-97]. 35 1.1.11 Simultaneous Localization and Mapping, SLAM, Algorithm Simultaneous localization and mapping for terrain-aided navigation of UAVs is implemented with the SLAM algorithm, Kim and Sukkarieh, [103, 104]. SLAM implies preforming terrain sensor readings with the specified time intervals, making possible navigation over unknown terrain. SLAM augmented with an existing GPS signal provides improvements in positioning error estimates, resulting in SLAM/GPS/INS algorithm, [103]. The simulations presented in [103] consider sampling rate of internal navigation system at 50 Hz, making the sampling model of INS discrete. Kalman filter is used to real time implementation of airborne internal-SLAM is presented in [104]. It used Extended Kalman Filter for data fusion between internal navigation system and optical sensor. Bryson and Sukkarieh, [105, 106] presented architecture and procedures allowing multiple cooperation UAVs to enhance trajectory path planning based on refined mapping data obtained as a result of data fusion. The architecture presented in [106], active SLAM control, implies decentralized planning performed by each vehicle, based on the common map estimates, obtained by the group. Lee, et al. [107] suggest reducing sensor data fusion complexity by using approximation of the expected terrain topology to a plane, thus, creating a plane constraint from which trajectory adjustment for a UAV could be made. 1.1.12 Terrain Following Flight of Independent Agents The problem of terrain following flight of an autonomous agent is considered in [3]. An algorithm for discrete, time step by time step, navigation of an agent flying over unknown terrain is presented. It is directly related to the problem considered in this 36 thesis. Simulations demonstrate how an agent is able to achieve collision-free terrain following flight towards stationary goal, while relying on sensors modeled as cones of rays, [3]. 1.2 Contribution The presented formation control method is derived using new sets of constraints, specifically designed to achieve flexible formation geometry consistent with the requirements of terrain following flight. In simulation, application of these constraints allow achieving flexible formation, while ensuring collision free motion over unknown terrain. Individual multi-agent formation primitives and hierarchical formation structure designs are presented and their performances are evaluated. A set of constraints, defined using planes and navigation vectors, is found suitable for providing the desired behavior of flexible terrain following formations, while demonstrating robust performance considering a range of model settings and terrain topology. The presented approach allows defining a structured but flexible formation adding to the group of systems suitable for application of SLAM based algorithms, while presenting a new type of terrain following formation design. The presented examples and test cases are intended to illustrate system behavior. The presented formation models are implemented with custom software package that uses Wolfram Mathematica as computation engine, and Autodesk Maya as node based simulation driver, visualizer, and geometry processing tool. In addition to providing connection with state-of-the-art numerical solvers and allowing implementation of system components using symbolic math, it allows computationally 37 efficient simulation of ray based sensors while representing terrain topology as mesh or NURBS surfaces, created based on digital elevation data sets. 1.3 Outline This thesis proceeds as follows, Chapter 1 provides introduction and literature review of related work, as presented above. Chapter 2 presents formulation of terrain following formation flight problem and states set of requirements for the desired formation behavior. It includes description of dynamical systems selected for this study and outlines structure of formation primitives and multi-level, hierarchical formations, stating that a constraint based system design in conjunction with application of the Udwadia-Kalaba approach [1, 2, 87-93] is considered for all of the systems presented in this thesis. Chapter 3 presents theoretical background, providing a brief overview of constrained dynamical systems and the Udwadia-Kalaba equation [1, 2]. In addition, it provides a brief description of independent terrain following agent models, including an overview of navigation algorithm, complete with simulation examples demonstrating main aspect of terrain following system behavior. Chapter 4 describes guidance and navigation methods for the formations considered in this thesis, describing procedures for obtaining navigation vectors for virtual leaders. The main portion of that chapter is devoted to providing justification for selecting flexible, changeable, formation configuration as means of achieving terrain collision free flight. It includes comparison of virtual rigid body with adjustable formation configuration structures. 38 Chapter 5 provides derivation of plane and navigation vector constraints. It describes application of these constraints to terrain following formations, defines corresponding performance metrics, and demonstrates achieving the desired formation behavior. It also demonstrates how different navigation methods defined for a virtual leader, described in Chapter 4, translate into obtaining different trajectory profiles for that agent. Chapter 6 presents an extension of terrain following formation control methods, described in the previous chapters, for constructing multi-level, hierarchical, formations. Different sets of the constraints for two-level formation structures are considered. Their performance is demonstrated and conclusion regarding selecting a suitable design is drawn. Chapter 7 describes formations defined with planes and surfaces of revolution constraints. The presented study considers parabolic, cone, and cylindrical surfaces for derivation of terrain following constraint components. Inherent problems corresponding to selecting such design, and errors associated with model implementation are outlined. The conclusion regarding suitability of thee particular constraints for terrain following formation flight system is drawn, supported by tests performed for individual agents, presented in Appendix C. Chapter 8 describes architecture for the developed Maya-Mathematica modeling and simulation system. It concentrates on those aspects of system design that are directly applicable to modeling and simulation of terrain following formations. Validation and verification of the implementation of model components is demonstrated using direct comparison between simulation results obtained with stand-along Mathematica and 39 newly developed Maya-Mathematica assembly. Simulation results obtained using formation models presented earlier in this thesis, and additional models that include digital elevation model, ArcGRID and USGS DEM, representation of terrain topology, are demonstrated. Chapter 9 states conclusions and discusses provisions for future application of the developed procedures, methods, and designs, presented in this thesis. 40 CHAPTER 2: TERRAIN FOLLOWING FORMATION FLIGHT PROBLEM The objective of the presented work is developing a control method for the terrain following formation flight of a group of communicating autonomous agents. Formation flight is governed by an algorithm implemented as a combination of terrain-following and formation-keeping components, as illustrated in the figure below. a) b) c) Figure 2.1: Flight control methods, a) Terrain following, b) Formation keeping, c) Combined, terrain following formation flight In the context of the proposed research autonomous agents are robots that can perform desired tasks in unstructured environments. As follows from the definition of fully autonomous agents presented in Russel and Norvig [98], and Latombe [99], these robots have the ability to gain information about the environment, work for an extended period without human intervention, move safely throughout its operating environment without human guidance, and adjust its strategies for accomplishing tasks in response to changing surroundings. The proposed control method is designed to support terrain following formation flight of missiles and UAVs. The presented control method is designed to solve a new problem, combination of terrain-following flight at low altitudes with structured flexible formation geometry. It allows using terrain-following formation motion with inter-agent communication for 41 GPS-independent navigation. Distributed airborne sensor networks have distinct formation geometry with reconfiguration as an emergency maneuver, but their guidance and navigation methods do not specifically consider closely following the terrain surface. The presented method utilizes a new concept of specifying formation motion where each agent is moving within a definite space corridor described in [3]. The corridor is defined for every time step by computing bounds on horizontal spread of formation members and their min, max elevation. Such technique differs from the existing approaches that involve maintaining a constant formation shape by following a set of fixed relative positioning constraints or supporting a changeable formation geometry by tracking preplanned trajectories. Design of formation keeping constraints, system modeling, and analysis of formation performance combines traditional and new elements. Formulation of the constraints that ensure formation keeping while every agent is engaged in the terrain following motion is the thrust of this research. System modeling is an integral part of the proposed approach, where the new elements include tracking of a variable trajectory, considering discrete updates of relative positioning and sensor data, while navigation constraints designed for terrain following flight. Benefits using the proposed flight control algorithm include (a) supporting GPS- independent navigation for groups of UAVs or missiles, regardless of terrain data availability prior to commencement of a mission, (b) maintaining formation geometry that is well suited for performing cooperative localization, reducing agents position uncertainty, hence, facilitating observation data fusion and subsequent mapping, (c) providing formation operation that is less susceptible to point failure due to distributed 42 control design, (d) feasibility of defining a multi-level formation control, capable of supporting formation of formations type of structure. These aspects of system design are of concern when such a formation is operating in highly uncertain or unknown environment, for example, the one that is likely to be encountered during search and rescue missions. Potential applications for the proposed control method include surveillance, search and rescue, reconnaissance, and similar operations where formation plays a role of distributed sensor network and flight at lower elevations may be required for better sensing, as well as exploration missions where observation data, provided by individual formation members, needs to be summarized to map an unknown terrain. 2.1 Requirements for the Terrain Following Formation Motion Control Algorithm In order to support the features specified above and provide for practically feasible, computationally efficient implementation, the requirements to the control law design and corresponding specifications for the terrain following formation motion control algorithm are summarized as follows. • Terrain and inter-agent collision avoidance • Formation shape reconfiguration mechanism • Deliberative guidance component • Fully decentralized design • Scalable and extendable in terms of the total number of formation members • Support for adding or removing agents to and from formation • Support a hierarchical formation structure defined with formation primitives 43 • Full consideration of kinematic and dynamic constraints of a mobile agent • Allow real-time or near real-time implementation • Able to account or compensate for imperfect sensor data • Able to benefit from a limited prior knowledge of the terrain topology The following justification of the requirements stated above discusses specific aspects of the algorithm design and corresponding choices for system implementation. Terrain and obstacle collision avoidance is provided by adherence of individual agents trajectories to their safe corridor volumes described in [3]. The safe corridors and their connection with navigation vectors are discussed in greater details in Chapter 3. The terrain following control provides for terrain features collision avoidance, but subsequent application of the formation keeping control may cause an agent to violate its safe corridor as defined by the terrain following method. Therefore, the proposed control method needs to consider additional measures for keeping an agent within its safe corridor. The following figure illustrates a case of terrain collision, associated with violating bounds of safe corridors by an agent. The collision is indicated by a circle with a point, as shown in Fig. 2.2.a. A collision free motion is shown in Fig 2.2.b. Agent trajectories and the resulting formation shapes are different in these two cases. a) b) Figure 2.2: Adherence to safe corridors, a) No, b) Yes 44 Within the study of terrain-following motion it is shown that multiple agents, which employ the same terrain acquisition and sensor data processing method and move towards the same goal position, tend to exhibit flocking behavior. It results in homogenous individual agents attempting to converge to exactly the same path and coalesce, which may lead to inter-agent collisions. Since nothing within the terrain- following control law prevents inter-agent collisions, that task has to be undertaken by the formation keeping control. Moving in formation presents a logical choice for introducing constraint that regulates relative positioning of mobile agents in space. Inter-agent collision avoidance could be provided by considering such techniques as adding a spring-and-dumper system to the model, or by defining constraints in such a way that the resulting formation shape does not change its geometry type, for example, from a triangle to a line segment. The task of providing all of the collision avoidance capabilities are considered in conjunction with the design of formation reconfiguration mechanism discussed below. Formation reconfiguration mechanism is provided to support a flexible formation shape. As follows from the set of requirements described above, no command for an individual agent, formation member, generated by the formation-keeping control yields positions located outside of that agent’s safe corridor. To satisfy that requirement an algorithm that maintains a rigid formation may need to be augmented to allow variable formation shape. For example, a collision avoidance maneuver might consist of stretching a triangular formation into nearly a line or changing formation shape away from an equilateral triangle, while reducing its area, for passing through a canyon and re- acquiring the original formation shape when the obstacle is passed. An example of the 45 latter case is illustrated in the figure below. An agent-terrain collision, indicated by a circle with a point, is shown in Fig. 2.3.a. Evolution of a flexible formation shape, allowing to avoid such collision, is shown in Fig. 2.3.b. a) b) Figure 2.3: Formation shape, a) Rigid, b) Flexible In addition to reactive collision avoidance the system has a deliberative component providing for reaching the goal while eliminating or strongly reducing the probability of getting stuck at local minima. That component also provides for re- acquiring goal direction after an emergency maneuver that caused the deviation from such. Thus, it a part of the formation navigation and guidance mechanism. In order to implement formation guidance, we assume that the direction towards the goal is known, although the exact elevation data of the goal point is not required. The selected formation navigation and guidance method is described in greater detail in Chapter 4. Having a fully decentralized design is important since it will reduce system susceptibility to a point failure. Thus, data processing is performed independently by individual formation members, making all of them of equal worth so that a loss of any agent does not present an insurmountable obstacle for achieving mission objectives. Only some of the results, such as the directions of navigation vectors, are exchanged. Thus, no 46 distributed computations that involve network algorithms, decision-making, and extensive search are to be utilized. Communication with ground control is not considered and not required, removing the necessity of adapting to the inherent time delays. Among the agents themselves there is no single entity responsible for gathering all the data, computing state vectors for every formation member, and broadcasting the information. The algorithm is required to be scalable and extendable, in terms of both the number of individual formation members for a single-level formation structure and the number of formation primitives for hierarchical, multi-level, formations. The resulting design considers arbitrary number of individual agents, while the presented simulations are performed for 3 and 5 member formations. Similarly, while the design does not limit number of agents per formation block, primitive, structures composed of triangular formation blocks are considered. Such multi-level formation is illustrated in the figure below. Figure 2.4: Multi-level formation A reconfigurable formation structure implies support of additions and removals of formation members. The presented simulations focus on the cases of removing an agent from the formation, since this case corresponds to loosing an individual formation 47 member. A case of removing and then adding a formation member might correspond to an agent loosing and then reacquiring communication with the rest of the group. Full consideration of kinematic and dynamic constraints of a mobile agent, ensures that no impossible trajectories are generated. Corresponding models are required to be sufficiently expressive to account for specifics of agent hardware, such as on-board sensors, communication equipment, types of propulsion and actuators. This requirement is directly related to the task of specifying a set of external forces and defining a set of constraints to be used in conjunction with the methodology based on the Udwadia-Kalaba approach, [1, 2, 87-93], and deriving equations of motion using the Udwadia-Kalaba equation, [1, 2]. In regards to real-time or near real-time execution, it is important to note that length of the allowable navigation time step bounds not only the sensors sampling frequency, but computational complexity associated with solving the equations of motion. Thus, both complexity of the equations, corresponding to the complexity of constraints, and runtime performance of numerical solvers, required to support computations, are considered in the design process. Aspects of system modeling, such as the selected types and settings of numerical differential equation solvers suitable for nonlinear equations of motion are the design factors to consider. For example, selecting Runge-Kuttar solvers and specifying the desired difference and interpolation order parameters affect computation precision and bounds of the expected computation runtime. Therefore, their selection presents not only implementation, but also the design choices. 48 Imperfect sensor measurements, in terms of both angle and distance errors of terrain acquisition rangefinder sensors, are accounted for selecting navigation settings such as diameter of safe corridor volumes or the desired navigation height. Therefore, an estimate of potential or expected range of such errors is assumed to be available prior to the mission, and we expect these errors to be bounded. Similarly, a limited prior knowledge of the terrain topology, if such data is available, might be expressed by updating specified navigation settings. The resulting system would be able to accommodate system parameters to the worst case scenario within the environment, for example, ensure avoidance of the largest or the tallest obstacle. 2.2 Problem Formulation The dynamical system that is a subject of this research consists of n f autonomous communicating mobile agents that shall fly in formation toward the predefined stationary goal, while each of the agents performs terrain-following motion. The trajectory generation task is formulated as a problem of constrained motion. At every instance of time each agent is required to be positioned within a collision free cylindrical volume of space, denoted as a safe corridor, as described in [3], rather than be following a preplanned path. The computed navigation direction vector ˆ ν i serves as the axis of safe corridor for the i th formation member, where i=1,..,n f . Its diameter is specified as a predefined constant, denoted as γ cr . Agents are equipped with rangefinder sensors. Sensor measurements are performed with the predefined time intervals, denoted as dt . Value of dt is assumed to 49 be sufficiently large to prevent sensing process from being modeled as continuous, but sufficiently small to allow agents enough time for reacting to terrain obstacles. Navigation vectors ˆ ν i are obtained by processing sensor data, represented by matrices of ray directions and ray-surface intersections, denoted as Δ r,i and Λ r,i respectively. Such mechanism of heading direction updates imposes a discrete navigation algorithm, with n th time step specified as t∈ t n , t n +dt ( ) . Formation structure is defined using virtual leader, a virtual agent that serves as geometrical center of the formation, GC . Desired positions of formation members, denoted as q i (t) for the i th formation member FK i , are defined in relation to position and navigation vector of this agent, denoted as q s (t) and ˆ ν s , respectively. The virtual leader is modeled as an independent terrain following agent. However, it does not have sensors associated with it. Its motion is not required to be bounded by a safe corridor volumes. Therefore, its constraints and the corresponding dynamical model is different from the dynamical model of formation members. Thus, models of the virtual leader and a formation member are not homogeneous, while all of n f formation members are modeled as homogeneous agents. Navigation vector of the virtual leader represents formation guidance. Depending on the selected formation navigation and guidance algorithm, described in Chapter 4, the value of ˆ ν s is obtained considering set of navigation vectors and sensor data of formation member, Δ r,i ,Λ r,i , ˆ ν i { } where i∈ 1,n f " # $ % , considering that direction towards the goal point, denoted as p g is available. The vector ˆ ν s serves as an input into computation of 50 equations of motion of individual formation members, since it is a part of definition of formation keeping constraints. A triangular formation or a triangular formation primitive for multi-level formation structure is illustrated in the following figure. It shows evolution of a flexible formation shape during the n th time step. Positions of independent terrain following agents, denoted as q TF,i (t) for the agent TF i , are provided for the comparison. Positions q TF,i (t n+1 ) specify shape of the group of agents obtained without considering formation maintenance control. Figure 2.5: Triangular formation primitive 2.2.1 Dynamical System and Control Structure The desired control structure consists of two components. The first component, TF, generates control command to achieve the terrain-following motion. The second component, FK, generates control command to fulfill formation-keeping task, using the agent’s state matrix, which resulted from the terrain-following motion, and communicated state matrices of the other formation members. State matrices are denoted as Q s (t) , Q i (t) , and Q TF,i (t) for the virtual leader, formation members, and independent q TF,1 (t n+1 ) q 2 (t n ) q TF,3 (t n+1 ) q s (t n ) q 1 (t n ) q TF,2 (t n+1 ) q 3 (t n ) q s (t n+1 ) q 2 (t n+1 ) q 1 (t n+1 ) q 3 (t n+1 ) Terrain 51 agents, respectively. Feedback loop consists of a block, CL, is responsible for an optional cooperative localization function. However, global cooperative localization functionality is not included in the presented formation models. This thesis concentrates on the second component of the described control, formation maintenance, FK. A control diagram for the i th formation member for the n th time step is shown in the figure below. It corresponds to an adjustable formation system, such as the one presented in Chapter 4. The diagrams describing systems presented in Chapters 5-7 would not include an explicitly defined TF block, instead, a single FK block would be used to represent control defined by the combined set of terrain following and formation keeping constraints. Figure 2.6: Diagram for the i th formation member Agents are modeled as three degrees of freedom particles with constant mass m . Mass matrixes for all of the agents are identical, denoted as M ≡M s ≡M i . They are specified as M =mI 3×3 , where I 3×3 is the identity matrix. Further simplification of the model is achieved by assuming particles of unit mass m=1. The modeling is done in Cartesian coordinates. Positions are specified as q(t)= q x (t) q y (t) q z (t) ! " # $ T . Velocities and accelerations are defined as  q(t)= d dt q(t) and  q(t)= d 2 dt 2 q(t) , respectively. State matrices are defined as Q s (t)= q s (t)  q s (t)  q s (t) [ ] T , Q i (t)= q i (t)  q i (t)  q i (t) [ ] T , and TF FK CL Q s (t) Q i (t n ) Q TF,i (t) Q i (t) 52 Q TF,i (t)= q TF,i (t)  q TF,i (t)  q TF,i (t) ! " # $ T . All of the state variables are modeled as continuous functions of time t . Data flow diagram for the both i th formation member and the virtual leader for the n th time step is shown in the figure below. This diagram is applicable to the description of all of the formation models presented in this thesis. The exact contents of the exchanged data sets depend on selected type of formation navigation algorithm, as stated previously in this section. Terrain topology, as an input into the sensor terrain acquisition block, is represented by surface elevation function f Terrain (x,y) . Figure 2.7: Data flow diagram The resulting force of constraints for the i th formation member is denoted as F i c (t) , where F i c (t)= F x,i c (t) F y,i c (t) F z,i c (t) ! " # $ T . It constitutes the control force, following derivation and methodology, based on the Udwadia-Kalaba approach, presented in [1, 2 , 87-93]. The set of external forces considered for agent models is identical for all of the agents, including the virtual leader. The total external forces acting on the agents are Sensor Model Sensor Terrain Acquisition Navigation Terrain Following Navigation GC EOM GC EOM FK i f Terrain (x,y) Δ r,i Λ r,i ˆ ν i ˆ ν s q s (t n ),  q s (t n ) { } F i c (t) {q j (t n ),Δ r, j ,Λ r, j , ˆ ν j } ∀ j∈ 1,n [ ] ∧ j≠ i Q i (t) Q s (t) q i (t n ) q i (t n ),  q i (t n ) { } 53 denoted as F i e (t) , F s e (t) , and F TF,i e (t) for the agents FK i , GC , and F TF,i e (t) , respectively. Thus, using the subscript notation k∈ i, s, TF i { } to indicate type of an agent, the external forces are F k e (t)= F kx e (t) F ky e (t) F kz e (t) ! " # $ T . The total external force is defined as the sum of the gravity, drag, and thruster forces, denoted as F g , F k d (t), and, F k t (t) , respectively. Gravity force is modeled with constant magnitude g and direction ˆ ν g , specified as ˆ ν g = 0 0 −1 [ ] T . Thus, it is identical for all of the agents, F g ≡F k g (t) , considering constant mass matrices M ≡M k . Drag force is modeled following the definition of nonlinear cubic drag, specified with constant drag coefficients η 1 and η 2 , where η 1 >η 2 >0 . Thruster force is modeled with constant coefficient f t specifying its magnitude. Two types of thruster models are considered, movable and constant thrusters, denoted as F k tm (t) and F k tc (t) , respectively. Definition of a movable thruster model includes constant direction vector ˆ ν k coinciding with the direction of navigation vector of the respective agent for the current time step, thus, ˆ ν k ∈ ˆ ν i , ˆ ν s , ˆ ν TF,i { } . Thruster force F k t (t) is included in the model of external forces. It is not a part of the resulting control force. The control force F k c (t) is associated with additional actuators. Considering these assumptions, the external forces are defined as follows, F k e (t)=F g +F k d (t)+F k t (t) (2.1) F g =mg ˆ ν g (2.2) 54 F k d (t)=−η 1  q k (t)  q k (t) −η 2  q k (t) 3  q k (t) (2.3) F k t (t)= F k tm (t), F k tm (t)= f t ˆ ν k F k tc (t), F k tc (t)= f t  q k (t)  q k (t) ! " # $ # (2.4) 2.2.2 Graph Representation of Formation Structure A leader-follower formation could be represented as a fully connected directed graph. Considering constraint based approach to formation control, a formation structure could be represented by a graph that consists of nodes corresponding to the agents and edges indicating constraints. A change in formation structure, such as adding or removing formation members would correspond to adding or removing nodes and corresponding edges of this graph. For the formation structure described above, a node represents a computation step for obtaining an agent state matrix. Such node is denoted as Ν i or Ν s for the i th formation member or the virtual leader, respectively. A directed edge from node Ν s to Ν i , denoted as Σ i , represents input in the constraint equation, Q s (t) , used for computing Q i (t). Therefore, such graph is directly related to the diagram shown in Fig. 2.6. However, unlike the diagram, the described graph representation indicates just the structure of constraints and corresponding sequence of computations to be undertaken at each time step. Thus, it may be primarily used for evaluating structural and computational complexity imposed by formation design. An acyclic graph indicates decoupled systems of equations for the virtual leader and formation members. A 55 formation primitive with n f formation members is represented by a tree of depth 1, as shown in the figure below. Figure 2.8: Structure of a formation primitive It should be noted that a tree of width 1, representing a formation composed of the virtual leader and a single formation member, is a valid structure. Such case may result from a series of removal operations, representing crashes or total communication loss of the rest of the agents. During the course of this study such cases has been modeled, resulting in triangular formation changing its structure from 3 to 2, and then to a single formation member, as illustrated in the figure below. These cases were associated with insufficient magnitude of stabilization coefficients, resulting in violation of bounds of safe corridor volumes, as illustrated in Fig. 2.2.a. Figure 2.9: Evolution of formation structure For a multi-level, hierarchical, formation structure that includes multiple virtual leaders is represented by a tree of depth k s , for k s levels of hierarchy. An example of such graph, corresponding to a formation similar to the one illustrated in Fig. 2.4, is Ν s Ν 2 …" Ν n f Ν 1 Σ n f Σ 2 Σ 1 Ν s Ν 2 Ν 3 Ν 1 Σ 3 Σ 2 Σ 1 Ν s Ν 3 Ν 1 Σ 3 Σ 1 Ν s Ν 1 Σ 1 56 shown in the figure below. This graph corresponds to a 2-level structure, k s =2 , composed of n s triangular formation primitives. Figure 2.10: Two-level hierarchical formation structure The root node of the tree, shown in the figure above, corresponds to the top-level virtual leader, denoted as Ν s . A node representing a second-level virtual leader and corresponding edge of the graph are denoted as Ν s,2 n i and Σ s,2 n i , respectively, where n i is the index of a formation primitive, with n i ∈ 1,n s [ ] . A leaf node, denoted as Ν i n i , represents i th formation member that belongs to a formation primitive indicated as n i . It shall be noted that avoiding having a tree of considerable depth may be considered as a reasonable choice for formation design. Thus, avoiding having a multitude of hierarchical levels in the attempt to limit effects of string instability, described in [6]. Although the presence of string instability has been proven for a chain of linear systems, the authors of [6] fully expect this effect to be present for nonlinear systems as well, as it was already noted in the previous chapter. One of the potential ways to counteract such an effect might be found in considering different types of constraints associated with virtual leaders belonging to different levels of formation hierarchy, such as the agents Ν s and Ν s,2 2 . Such an approach Σ s,2 n s Ν s,2 1 Ν 2 1 Ν 1 1 Ν 3 1 Ν s,2 n s Ν 2 n s Ν 1 n s Ν 3 n s Ν s … … Σ s,2 1 57 is practically feasible, since a virtual leader does not have to maintain a terrain collision free motion, thus, allowing sufficient room for making the design choices. Another aspect of hierarchical formation design corresponds to allowing or preventing a flocking behavior, such as the one observed in the example of boids, [38]. For the purposes of terrain following formation flight, we try to achieve structured formation motion, thus, tying to avoid an explicit implementation of flocking behavior. To illustrate this point, the following mutable graph structure is presented in the figure below. It shows the process of moving the node Ν 3 1 from one formation primitive to another, using a standard graph operation technique. It directly relates to a formation specifically designed for flocking. Therefore, such types of formation restructuring are not considered for the formations presented in this thesis. Figure 2.11: Restructuring corresponding to flocking behavior Another type of graph representation of a formation structure indicates information flow among agents. It is directly corresponds to data flow diagram, such as the one shown in Fig. 2.7. State matrix and navigation data exchange are associated with two distinct types of directed edges, denoted using symbols Σ and Ε , respectively. Since processed navigation data of formation members is an input in computation of the state matrix of the virtual leader, Q s (t) , and Q s (t) is an input into computations of Q i (t) , the edges Σ and Ε have the opposite direction. In order to clarify the order of data Ν s,2 1 Ν 2 1 Ν 1 1 Ν 3 1 Ν s Ν s,2 2 Ν 2 2 Ν 1 2 Ν 3 2 Ν s,2 1 Ν 2 1 Ν 1 1 Ν 3 1 Ν s Ν s,2 2 Ν 2 2 Ν 1 2 Ν 3 2 58 processing, a method of splitting a node of type Ν i associated with a formation member into two nodes, denoted as Κ i and Ν i FK may be used. Splitting a single node into two in order to build an acyclic graph is one of the standard techniques of modeling node-based systems. This operation applied to a graph representing a formation primitive is shown in the figure below. Figure 2.12: Data flow node graph Similar technique could be applied to produce a data flow node graph for a multi- level formation structure, in which case a virtual leader nodes, Ν s,k n i , other then the root node, would be split and edges corresponding to navigation data of these agents, denoted as Ε s,k n i , would be added. 2.3 Problem Approach Terrain following formation flight, its applications and corresponding set of the requirements, general description of system models, and some of the assumptions regarding sensor and agent models has been described above. A suitable formation structure has been illustrated with a graph and structural reconfiguration options has been discussed. Ν s Ν 2 …" Ν n f Σ n f Σ 2 Σ 1 Ν 1 E 1 E 2 E n f Κ n f Κ 1 …" Ν 2 FK Ν n f FK Ν 1 FK …" Ν s Κ 2 E 1 E 2 E n f Σ 1 Σ 2 Σ n f 59 The problem of achieving structured three dimensional formation shapes is solved by setting constraints on relative positioning of the agents. The agent are allowed to change their relative positions while maintaining specified general formation shape. Since these constraints deal only with formation shape but not direction of motion, they do not define motion of the agents completely. Therefore, the second set of requirements, specified using navigation vectors, is added. These requirements allow deriving the second set of constraints. The resulting full set of constraints uniquely defines motion of formation members at a given moment of time. The design of a formation with adjustable formation geometry, resulting from the direct augmentation of the traditional rigid virtual structure is presented in Chapter 4. Its limitations are described and a search for alternative methods is suggested. A suitable approach to terrain-following formation design is found in defining formation shape by constraints specifying that position of a formation member should belong to selected type of geometric surface. Positions and orientations of the surfaces are defined relative to position and heading direction of the virtual leader. The types of surfaces considered in this thesis involve (a) planes and navigation vectors (b) planes and surfaces of revolution, such as paraboloids, cones, and cylinders. Sets of corresponding constraints and the examples of formation designs are presented in Chapters 5 and 7. A multi-level hierarchical formation designed for the terrain-following flight is presented in Chapter 6. The formation is defined using a hybrid approach combining leader-follower and virtual structure methods. However, due to the flexible formation geometry, the resulting method of formation control might be considered as a case of 60 motion coordination, following the classification provided in the literature review section, Section 1.1.2. The derivations of terrain following and formation keeping constraints and corresponding equations of motion, presented in the following chapters, are based on the Udwadia-Kalaba approach introduced in [1, 2] and further described in [87-93]. More detailed description of some of the aspects of this approach and an example of application of the corresponding methodology to the implementation of constraint-based terrain- following flight system is presented in the subsequent background chapter. 61 CHAPTER 3: THEORY AND BACKGROUND TO CONSTRAINED MOTION, AND TERRAIN FOLLOWING FLIGHT This chapter refers to the theory of constrained motion for nonlinear constraint based dynamical systems. It concentrates on some of the aspects of the methodology of application of the Udwadia-Kalaba approach to system modeling and control design, as described in [1, 2, 87-93]. The method of applying Baumgarte stabilization technique to derivation of constraints to be used in conjunction with the Udwadia-Kalaba equation is described following [90, 93]. The techniques of providing collision avoidance capabilities for a virtual structures are discussed. They include bounding relative positions by constraints, [1, 94, 96], and implementing spring and dumper models, [1, 101]. Although these methods are perfectly suitable to be used in conjunction with the Udwadi-Kalaba approach, due to the requirements to flexible formation geometry, described in the previous chapter, they would have to be augmented with other techniques in order to support the desired formation design. The background to terrain following flight of independent agents consists of the brief description of navigation algorithm defined using notion of safe corridor volumes, as described in [3]. A ray based model of a rangefinder sensors is shown. The procedure and corresponding model for obtaining navigation vectors of independent agents is described. Derivation of the set of constraints and corresponding equations of motion for tracking a line defined by a point and a vector is presented, following [3]. These components are used in the subsequent chapters to define formation models. 62 Agent state variables, sensor, and control system parameters, described in this chapter, are denoted by symbols containing subscript k , indicating the k th agent. The corresponding equations are referred to in the subsequent chapters, where distinct types of agents denoted using different subscripts, such that k∈{i, s, TF i , FK i } . Although this is a background chapter, it includes a section containing simulation results illustrating cases that might be encountered during simulations of terrain following flight, such as sensors loosing terrain or collision of an agent with the terrain surface. These models are implemented based on the system description presented in [3], considering difference in the exact values of system parameters, models of terrain topology, and the exact distribution of rays within a sensor cone. Implementation of all of the programming components used for performing numerical simulations presented in this thesis is also different from [3]. 3.1 Constrained Dynamical Systems and the Udwadia-Kalaba Equation The Udwadia-Kalaba approach allows deriving equations of motion for both linear and nonlinear dynamical systems using the Udwadia-Kalaba Equation of Constrained Motion, [1, 2]. The corresponding methodology of control systems design ensures that the resulting control forces, which are represented by the constraint forces, are always minimized in accordance with the Gauss Principle of Least Constraint [1, 91]. An unconstrained dynamical system of k particles of constant mass, M k ≡M , is defined by the Lagrange equation, where x k (t) denotes a particle position vector and F k e x k ,  x k ,t ( ) denotes the total external force acting on a particle, [1], such that, 63 M x k (t)= F k e x k ,  x k ,t ( ) (3.1)  x k (t)=M −1 F k e x k ,  x k ,t ( ) (3.2) The acceleration  x k (t) represents the acceleration of the unconstrained system, denoted as a k (t) , [1]. The Gaussian quantity, denoted as G(  x k ) , where  x k represents a possible acceleration, is then represented as, G(  x k )= (  x k −a k ) T M (  x k −a k ) (3.3) The Gaussian is then minimized when  x k =a k at a particular instance of time t , thus, a k (t)≡  x k (t) for the unconstrained system, [1]. For a constrained particle system the equation of motion for the k th particle is defined considering the total force represented by the sum of external and constraint forces, F k e (q k ,  q k ,t) and F k c (q k ,  q k ,t) , respectively [1]. With the position vector of the k th particle denoted as q k (t), the equation of motion become, M  q k (t)=F k e (q k ,  q k ,t)+F k c (q k ,  q k ,t) (3.4) The Gaussian, denoted as G(  q k ) where  q k represents a possible acceleration consistent with the constraints, [1], is written as, G(  q k )= (  q k −a k ) T M (  q k −a k ) (3.5) The Udwadia-Kalaba equation defines the acceleration of the constrained system q k (t), thus, defining the constraint force F k c (q k ,  q k ,t) , such that the Gaussian is minimized at every instance of time t , [1], i.e.  q k ≡   q k where   q k =min G(  q k ) ( ) . Therefore, for the control system design, if the control force is defined as the constraint 64 force computed using the Udwadia-Kalaba equation, then it ensures that the optimal control force is found, considering provided set of constraints, [1, 91], i.e. ensuring optimization at every instance of time t . The set of constraints for the k th particle is represented by a 1×n k matrix of constraint expressions denoted as Φ k , and defined as Φ k = ϕ 1 ϕ 2 ϕ nk [ ] T , where n k specifies the number of constraints. With the j th constraint for the k th particle is denoted as ϕ k, j where j=1,2,..,n k , this constraint and the constraint equation obtained from the full set of constraints for the particle, is defined as, [1], ϕ k, j q k ,  q k ,t ( ) = 0 (3.6) A k q k ,  q k ,t ( )  q k (t)= b k q k ,  q k ,t ( ) (3.7) The Udwadia-Kalaba equation is defined as, [1], where Moore-Penrose inverse is denoted with the superscript “+ ”,  q k =a k +M − 1 2 A k .M − 1 2 " # $ % & ' + b k −A k .a k ( ) (3.8) The Udwadia-Kalaba equation allows use nonholonomic , non-integrable, and linearly dependent constraints to be used in derivations of equations of motion, [1, 2], greatly simplifying the task of defining the dynamical system, [91], as discussed in the literature review, in Section 1.1.9. It should be noted that a consistent set of constraints must be provided, i.e. the constraints from this set could not be contradictory to each other, [91]. 65 If the external and constraint forces for the k th particle is denoted as F k e (t) and F k c (t) , respectively, and the external acceleration is defined following Eq. (3.2) as a k (t)=M −1 F k e (t) , then another form of the Udwadia-Kalaba equation, and the equation for the force of constraint, [1], are written as, M  q k =F k e (t)+M 1 2 A k .M − 1 2 " # $ % & ' + b k −A k .a k ( ) (3.9) F k c (t)=M 1 2 A k .M − 1 2 " # $ % & ' + b k −A k .a k ( ) (3.10) These equations could be further simplified by assuming constant diagonal mass matrix, M =mI 3×3 , [1],  q k =a k +A k + b k −A k .a k ( ) (3.11) F k c (t)=mA k + b k −A k .a k ( ) (3.12) Form  q k (t) and a k (t) the force of constraints for a particle is obtained as, F k c (t)=m  q k (t)−a k (t) ( ) (3.13) In order to provide constraints that allow violating initial conditions, which is indicated by ϕ k,s q k ,  q k ,t 0 ( ) ≠ 0 for some constraints s∈[1,..,n k ] , the Baumgarte stabilization technique, [100], could be used, as described in [93]. The new set of constraint expressions specified by 1×n k matrix is denoted as Ψ k , and defined as Ψ k = ψ 1 ψ 2 ψ nk [ ] T . Baumgarte stabilization coefficients could be represented by scalar values µ k, j and λ k, j for an individual expression of constraint ψ k, j , or by n k ×n k 66 diagonal matrices µ k =diag µ k,1 µ k,2  µ k,nk ! " # $ and λ k =diag λ k,1 λ k,2 λ k,nk ! " # $ for the full set Ψ k . Then the constraint equation Eq. (3.7) is obtained based on the full set of constraints Ψ k =0 , ψ k, j =  ϕ k, j +µ k, j  ϕ k, j +λ k, j ϕ k, j (3.14) Ψ k =  Φ k +µ k .  Φ k +λ k .Φ k (3.15) 3.2 Constraints and Implementation of Inter-Agent Collision Avoidance The technique of placing constraints on relative positions or relative distances between the agents, particles, or rigid bodies could provide collision avoidance capabilities for a wide range of multi-particle or multi-body systems, such as the systems described in [1, 92, 93, 94-97, 101]. The most common approach is introducing constraints on relative positions between the agents. With the addition of Baumgarte stabilization technique, they are capable of supporting any realistic, non-contradictory, geometry of a rigid virtual body structure, allowing to start motion of the agents from arbitrary positions, [1, 92, 94-97]. In addition, they could be used as a components of a fully distributed, decentralized control algorithm, [95-97]. For example, in a system of n k particles, for two adjacent, connected, particles i and j , a holonomic constraint requiring keeping constant distance d ij is expressed as, ϕ ij (t)= q i (t)−q j (t) −d ij = 0 (3.16) 67 This constraint could be adjusted to allow maintaining minimal separation distance r ij between the particles, as described in [94-97], the constraint becomes, ϕ ij (t)= q i (t)−q j (t) −d ij q i (t)−q j (t) −r ij = 0 (3.17) Another technique of designing a system representing an adjusted virtual structure involves representing connections between adjacent agents as a spring and dumper systems. An example of using model of a damper and nonlinear model of a spring to represent a connection component is provided in [101], where the Udwadia-Kalaba approach is used to introduce a generalized procedure, theory, of modeling complex multi-body dynamical systems. It should be noted that the method developed in [101] is specifically designed for dealing with modeling complexity far exceeding the one that arises from representing formation structures considered in this thesis. An example illustrating connections of a triangular structure, i=1,2,3, where the connections between particles are modeled using as springs with linear and nonlinear coefficients k Li and k Ni , respectively, and dumpers with linear dumping coefficients c i , is shown in in the figure below, Figure 3.1: Triangular spring and damper structure. q 2 (t) q 1 (t) q 3 (t) k L2 ,k N2 c 1 c 2 c 3 k L1 ,k N1 k L3 ,k N3 q s (t) 68 Although the illustration presented above considers a system of particles, with agent’s position vectors denoted as q i (t) , it is assembled based on the description of the technique for representing a connection component used for modeling a system of rigid bodies presented in [101]. While the constraints described above correspond to well established techniques of virtual structure design, they are considerably limiting flexibility of the resulting structures in terms of being able to accommodate significant changes of geometrical shape. In other words, without implementing explicit steps for updating values of such parameters as the desired separation distances or spring coefficients, during the motion, the range of motion of corresponding structures is predefined. Thus, making the formation design dependent on the numerical procedures and heuristic techniques for estimating suitable values. Therefore, for terrain following formation flight, where the exact terrain topology is unknown, use of other constraints, allowing grater deviation from the virtual rigid body type of structures might be desired. 3.3 Terrain Following Flight of Independent Agents The main points of the algorithm for terrain following flight of independent agents, are already described in the problem formulation Section 2.2. The following section provides the description of sensor model in greater details. Sensor data processing steps for obtaining navigation vectors are outlined. The set of constraints and equations of motion for models of independent terrain following agents 69 are described. It shall be noted that the concepts and rationale behind these constraints and models are the same as described in [3]. 3.3.1 Ray Based Sensor Model Agent models, presented in this thesis, are developed assuming that each of the agents is equipped with a rangefinder sensor modeled as single cone of rays, unlike the model used in [3], where multiple ray cones per agent are considered. Rays are distributed evenly within the cone. Number of rays is specified by parameter n c . Point of origin of rays within the i th cone is defined as p r,i = p rx,i p ry,i p rz,i ! " # $ T . Ray direction vectors and ray lengths are mapped to square n r ×n r matrices Λ r,i and Δ r,i , where n r =2n c +1. Length and direction of an individual ray correspond to matrix elements λ r,i, jk =Λ r,i j,k ( ) and ˆ ν r,i, jk =Δ r,i j,k ( ) , respectively, where j=1,2,..,n r and k=1,2,..,n r . Therefore, an individual ray is described by the set R r,i,jk = p r,i ,λ r,i, jk , ˆ ν r,i, jk { } . Orientation of the cone is defined by cone axis ˆ w r,i = w rx,i w ry,i w rz,i ! " # $ T coinciding with the direction of central ray ˆ w r,i ≡Δ r,i n c +1,n c +1 ( ) . Width of sensor cone is specified by the constant cone angle α r , assuming that α r <180° . Vertical inclination of the cones is specified by the constant angle defined as β r =∠ ˆ w r,i , XY Plane ( ) and bounded by β r + α r 2 <90° , preventing any ray form pointing directly downwards. XY Plane denotes the horizontal plane. Position and orientation of a sensor is defined by the position of the i th cone at the beginning of the n th time step, at the time t n , is specified by positions of the agent q i (t n ) 70 and the goal point p g = p gx p gy p gz ! " # $ T . Thus, the sensor is positioned at the point p r,i =q i (t n ) and oriented towards the point p g along the horizontal projection of the goal vector ˆ ν rg,i xy = v rgx,i xy v rgy,i xy ! " # $ T . Then, horizontal projection of the cone axis coincides with this direction, ˆ w r,i xy ≡ ˆ ν rg,i xy , where ˆ w rg,i xy = w rx,i xy w ry,i xy ! " # $ T . Considering horizontal projections of the points defined as p r,i xy = p rx,i xy p ry,i xy ! " # $ T and p g xy = p gx xy p gy xy ! " # $ T , these normalized direction vectors are defined as, ˆ w r,i xy ≡ ˆ ν rg,i xy = p g xy − p r,i xy p g xy − p r,i xy (3.18) Limit on the maximum value of β r ensures that any sensor ray points toward position of the goal point p g . It shall be noted the direction vectors, ˆ w r,i , ˆ ν rg,i xy , and ˆ ν r,i, jk , specify discrete values, updated at the time t n . It follows from the definition the navigation algorithm, assuming that sensor readings are taken at the beginning of a time step, [3]. The corresponding model is illustrated in the figure below. Figure 3.2: Sensor model. y x z ˆ w r,i p g xy p r,i p r,i xy ˆ w r,i xy p g β r P Line,i ∈ XY Plane α r 71 Terrain surfaces are modeled as continuous functions, where elevation is a function of horizontal projection of a surface point, z= f Terrain (x,y) . For the simulations performed using stand along Mathematica programs, without adding Maya based components, terrain surface models are limited to functions in order to support implementation of ray tracing computations by solving the ray equation directly. It is not the most computationally efficient technique of finding ray surface intersection points, but the one that allows simple and accurate implementation. Sensor terrain acquisition is modeled as tracing sensor rays to the terrain surface. The ray equation, Eq. (3.19), is solved for the ray lengths, denoted as λ r , considering ray directions defined as ˆ ν r,i,jk = ν rx,i,jk ν ry,i,jk ν rz,i,jk ! " # $ T and the limit on the maximum sensor range specified by r max . There could be a set of solutions of the ray equation for a single ray. This set of solutions, λ r ∈ λ r,i,jk,n { } , is associated with several ray surface intersection points. Minimum value of λ r from this set corresponds to the actual ray surface intersection point, λ r,jk . If no intersections are found or ray does not intersect the surface within the range r max , then λ r,jk is set to ∅ , as specified in Eq. (3.20). Results are stored in the matrix Λ r,i with elements specified as Λ r,i (j,k)=λ r, jk . p rx,i +λ r ν rz,i,jk = f Terrain p rx,i +λ r ν rx,i,jk , p ry,i +λ r ν ry,i,jk ( ) 0<λ r ≤ r max " # $ % $ (3.19) λ r,jk =min λ r,i,jk,n { } ( ) , λ r,i,jk,n { } ≠∅ ∅, λ r,i,jk,n { } =∅ # $ % & % (3.20) 72 3.3.2 Selection Algorithm for the Terrain Following Navigation Vector Normalized navigation vector ˆ ν i = ν x,i ν y,i ν z,i ! " # $ T is computed based on the desired constant elevation, denoted as navigation height h n , and selected navigation ray, represented by the set R r,i n = p r,i n , λ r,i n , ˆ ν r,i n { } . Navigation ray is indicated by indices j n ,k n ( ) in the matrices of ray length and directions, such that λ r,i n =Λ r,i j n ,k n ( ) and ˆ ν r,i n =Δ r,i j n ,k n ( ) . It corresponds to the ray surface intersection point denoted as p rs,i n . Two types of the selection algorithms for choosing a navigation ray are considered for models presented in this thesis, denoted as minRay and maxRay . They correspond to selecting a ray with minimum and maximum length, respectively, as defined in Eq. (3.20). If no ray surface intersections are found, λ r,i n =∅ , the value ˆ ν i is set to the default navigation vector denoted as ˆ ν r,i d and defined in Eq. (3.24). In this case, vertical component of ˆ ν i is specified by the value of constant angle α d , defined as α d =∠ ˆ ν r,i d , XY Plane ( ) . Its horizontal components are defined by the direction to the goal point, ˆ ν r,i xy,d = ˆ ν rg,i xy . This is the case when sensors lose terrain and an agent needs to descend to re-acquire it. Such condition might occur due to orientation of the sensor with respect to a particular set of terrain features considering limited sensor range. λ r,i n = min Λ r,i ( ) , ∃Λ r,i j,k ( ) ≠∅∧minRay max Λ r,i ( ) , ∃Λ r,i j,k ( ) ≠∅∧maxRay ∅, Λ r,i j,k ( ) =∅∀ j=1,2,..,n r ∧k=1,2,..,n r ' ( ) ) * ) ) (3.21) 73 ˆ ν r,i n = Δ r,i j n ,k n ( ) , λ r,i n ≠∅ ∅, λ r,i n =∅ $ % & ' & (3.22) p rs,i n = p r,i +λ r,i n ˆ ν r,i n , λ r,i n ≠∅ ∅, λ r,i n =∅ # $ % & % (3.23) ˆ ν i = p rs,i n +h n ˆ z axis − p r,i p rs,i n +h n ˆ z axis − p r,i , p rs,i n ≠∅ ˆ ν r,i d , p rs,i n =∅ $ % & ' & (3.24) Construction of the navigation vector, ˆ v i , and corresponding safe corridor volume, defined by ˆ v i and γ cr , are illustrated in the figure below. Figure 3.3: Navigation vector and corridor volume. A safe corridor is modeled as a cylindrical volume with navigation vector ˆ ν i serving as cylinder axis. Diameter of cylindrical corridor is specified by the constant parameter γ cr . Point of origin for the local coordinate system of a corridor volume is the agent position q i t n ( ) , coinciding with the point of origin of sensor rays p r,i . Length of a γ cr h n ˆ v r,i n y x z p r,i p r,i xy ˆ v i p rs,i n p r,i n 74 cylinder λ cr,i is estimated as a function of the duration of a time step dt and magnitude of agent velocity  q i t n ( ) at the beginning of the time step, such that λ cr,i = dt  q i t n ( ) . Such estimate of λ cr,i is not accurate for a nonlinear system. However, since parameter λ cr,i is not included into definition of constraints, this variable is not directly included in derivations of the equations of motion. It could be, optionally, accounted for in the process of computing the navigation vector, by adjusting the ray selection algorithm described above. However estimated range of values of λ cr,i is considered during the design of the terrain following system, allowing to specify, balance, values of such settings as dt , h n , α r , β r , and n c . Data flow diagram showing sensor and navigation specific parameters for an independent terrain following agent is shown in the figure below. The diagram includes only a partial set of inputs into equations of motion, EOM block, limited to the navigation vectors and initial condition parameters. Figure 3.4: Data flow diagram for an independent agent. Sensor Model Sensor Terrain Acquisition Navigation EOM f Terrain (x,y) Δ r,i Λ r,i ˆ ν i F i c (t) Q i (t) q i (t n ) q i (t n ),  q i (t n ) { } p g n c α r β r r max h n α d SensorType∈ minRay,maxRay { } 75 3.3.3 Metrics for Evaluating Terrain Following Trajectories Considering terrain collision avoidance, settings for the desired height above terrain, and the requirement of keeping agents positions within bounds of their respective corridor volumes, the following values characterizing the resulting trajectories may be observed. Considering the flat Earth model, as follows from using a function in the form of z= f Terrain (x,y) to describe terrain surface, the following metrics for evaluating agents trajectories are defined. Elevation of the trajectory above terrain the i th agent is observed by comparing values of z e,i (t)≡ q z,i (t) with the parameter denoted as z Terrain,i (t) , specifying terrain elevation at the point coinciding with the vertical projection of agent position. Distances between the points of this trajectory profile and a surfaces of constant elevation above terrain, h n,i , is denoted as d e,i (t) , and defined as follows, z Terrain,i (t)= f Terrain q x,i (t), q y,i (t) ( ) (3.25) d e,i (t)=q z,i (t)− z Terrain,i (t)+h n,i ( ) (3.26) Deviation of elevation of the trajectory of the i th agent from the bounds of its safe corridor volumes is observed suing the following metrics. Parameters specifying elevations of the centerline, maximum, and minimum bounds of the corridors, computed using projections of the trajectory points on the surfaces, are denoted as z ce,i (t) , z ce1,i (t) , and z ce,i (t) , respectively, and defined as, z ce,i (t)= z Terrain,i (t)+h n,i (3.27) z ce1,i (t)= z ce,i (t)+ γ cr 2 (3.28) 76 z ce2,i (t)= z ce,i (t)− γ cr 2 (3.29) Distances of between the trajectory points of the i th agent and the centerlines of its safe corridor volumes are denoted as r cr,i (t) . They are computed for each n th trajectory segment, corresponding to the n th time step t∈ t n , t n +dt ( ) . The computations are based on the procedure for computing a distance of the nearest point on a line to a given point, described in Vince [102]. Scaling coefficient c cr >>1.0 is included, since the values λ cr,i are estimates, made at the beginning of a time steps, as follows, r cr,i (t)= q i t ( ) − p r,i ( ) × q i t ( ) − p r,i −c cr λ cr,i ˆ ν i ( ) c cr λ cr,i ˆ ν i (3.30) The values r cr,i (t) serve as the direct metric of adherence of agent trajectory to the bounds of corridor volumes, as terrain following navigation algorithm requires Within any n th time step corridor volume bounds are not violated if and only if, r cr,i (t)< γ cr 2 , t∈ t n , t n +dt ( ) (3.31) The example containing plot of the parameters described above are provided in the examples and results section of this chapter, Section 3.4. 3.3.4 Constraints and Equations of Motion for Tracking a Line Defined by Position Point and Navigation Vector The set of constraints and corresponding equations of motion for the models of dynamical systems of independent terrain following agents are defined using the Udwadia-Kalaba approach, [1, 2], as described in the Section 3.1. The presented 77 derivations follow [3]. The set of expressions for the constraints, denoted Φ L,k (t) , represents the requirement of tracking a line defined by the agent position at the beginning of the time step t∈ t n , t n +dt ( ) , and the direction of navigation vector ˆ ν i , obtained at the time t n . The agent position is denoted as p r,i and defined as p k = p kx p ky p kz ! " # $ T and specified as p k =q k t n ( ) . The navigation vector is denoted as ˆ ν k and defined as ˆ ν k = ν kx ν ky ν kz ! " # $ T . Expressions for constraint Φ L,k (t) include coefficients α vj and β vj , derived from components of the vectors p k and ˆ ν k . Computation of coefficient α vj requires of the components of vector ˆ ν k to have non-zero value, as follows from Eq. (3.32) presented below. Since the value of ˆ ν k remains constant for the duration a time, if three versions of Φ L,k (t) are derived, corresponding to non-zero values of ν kx , ν ky , and ν kz , then it is possible to implement a switch between solving corresponding versions of equations of motion. This switch would be applied at the time t n . It would guarantee that a version corresponding to non-zero value of the component of ˆ ν k is selected for computations. Such implementation technique is feasible since ˆ ν k is a normalized vector, thus, at least one of its components has non-zero value, ∃ν kx ≠0∨ν ky ≠0∨ν kz ≠0 . These versions of Φ L,k (t) are denoted as Φ LX,k (t), Φ LY,k (t), and Φ LZ,k (t) , and defined as follows, Φ L,k (t)= Φ LX,k (t), ν kx ≠ 0 Φ LY,k (t), ν ky ≠ 0 Φ LZ,k (t), ν kz ≠ 0 # $ % & % (3.32) 78 α y1 = ν ky ν kx , α z1 = ν kz ν kx , β y1 = p ky − p kx α y1 , β z1 = p kz − p kx α z1 , ν kx ≠0 α x2 = ν kx ν ky , α z2 = ν kz ν ky , β x2 = p kx − p ky α x2 , β z2 = p kz − p ky α z2 , ν ky ≠0 α x3 = ν kx ν kz , α y3 = ν ky ν kz , β x3 = p kx − p kz α x3 , β y3 = p ky − p kz α y3 , ν kz ≠0 # $ % % % % & % % % % (3.33) Φ LX,k (t)= q ky (t)−β y1 −α y1 q kx (t) q kz (t)−β z1 −α z1 q kx (t) # $ % % & ' ( ( (3.34) Φ LY,k (t)= q kx (t)−β x2 −α x2 q ky (t) q kz (t)−β z2 −α z2 q ky (t) # $ % % & ' ( ( (3.35) Φ LZ,k (t)= q kx (t)−β x3 −α x3 q kz (t) q ky (t)−β y3 −α y3 q kz (t) # $ % % & ' ( ( (3.36) Derivation of constraint equation corresponding to the expression for constraint Φ LX,k (t) is presented below. This version of the constraint is used for the models presented in this thesis. Derivations corresponding to the other two versions Φ LY,k (t) and Φ LZ,k (t) follow the same steps and result in similar forms of the constraint equations, considering different sets of coefficients α vj and β vj , and variables q kx (t),q ky (t) { } and q kx (t),q kz (t) { } , respectively. The constraints Ψ L,k (t)= 0 are defined following Eq. (3.15) with µ k and λ k specified as scalar values, since both components of Φ L,k (t) , ϕ L1,k (t) and ϕ L2,k (t) , describe a single constraint of tracking a parametric line.  Φ LX,k (t)=  q ky (t)−α y1  q kx (t)  q kz (t)−α z1  q kx (t) # $ % % & ' ( ( (3.37) 79  Φ LX,k (t)=  q ky (t)−α y1  q kx (t)  q kz (t)−α z1  q kx (t) # $ % % & ' ( ( (3.38) Ψ LX,k (t)=  q ky (t)−α y1  q kx (t)+µ k  q ky (t)−α y1  q kx (t) ( ) +λ k q ky (t)−β y1 −α y1 q kx (t) ( )  q kz (t)−α z1  q kx (t)+µ k  q kz (t)−α z1  q kx (t) ( ) +λ k q kz (t)−β z1 −α z1 q kx (t) ( ) # $ % % & ' ( ( (3.39) A LX,k = α y1 −1 0 α z1 0 −1 " # $ $ % & ' ' (3.40) b LX,k = µ k  q ky (t)−α y1  q kx (t) ( ) +λ k q ky (t)−β y1 −α y1 q kx (t) ( ) µ k  q kz (t)−α z1  q kx (t) ( ) +λ k q kz (t)−β z1 −α z1 q kx (t) ( ) " # $ $ % & ' ' (3.41) The equations of motion are derived using the Udwadia-Kalaba equation, [1, 2], Eq. (3.8), while a coefficient denoted as c LX is defined in order to simplify the equations. The control forces are then obtained following Eq. (3.13), considering the set of external forces defined in Eq. (2.1-2.4). c LX =1+α y1 2 +α z1 2 (3.42) A LX,k .M − 1 2 " # $ % & ' + = m c LX α y1 α z1 −1−α z1 2 α y1 α z1 α y1 α z1 −1−α y1 2 ( ) * * * * + , - - - - (3.43)  q kx (t)= 1 c LX a kx (t)+α y1 a ky (t)+α z1 a kz (t) +λ k − α y1 2 +α z1 2 ( ) q kx (t)+α y1 q ky (t)−β y1 ( ) +α z1 q kz (t)−β z1 ( ) ( ) +µ k − α y1 2 +α z1 2 ( )  q kx (t)+α y1  q ky (t)+α z1  q kz (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (3.44) 80  q ky (t)= 1 c LX α y1 a kx (t)+α y1 a ky (t)+α z1 a kz (t) ( ) +λ k α y1 q kx (t)+ 1+α z1 2 ( ) β y1 −q ky (t) ( ) +α y1 α z1 q kz (t)−β z1 ( ) ( ) +µ k α y1  q kx (t)− 1+α z1 2 ( )  q ky (t)+α y1 α z1  q kz (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (3.45)  q kz (t)= 1 c LX α z1 a kx (t)+α y1 a ky (t)+α z1 a kz (t) ( ) +λ k α z1 q kx (t)+α y1 α z1 q ky (t)−β y1 ( ) + 1+α y1 2 ( ) β z1 −q kz (t) ( ) ( ) +µ k α z1  q kx (t)+α y1 α z1  q ky (t)− 1+α y1 2 ( )  q kz (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (3.46) 3.4 Model Settings and Simulation Examples The following examples of simulations of terrain following flight of independent agents are provided to illustrate a flight system governed by the selected terrain following navigation algorithm. The presented simulation results demonstrate correlation between ranges of values of the some of system parameters, such as values h n , α r , and dt , specifying the desired navigation height, sensor cone width , and time step settings, respectively. Plots of metrics for evaluation of terrain following flight performance, such as d e,i (t) and r cr,i (t) , are presented. Differences in the resulting trajectories and the magnitudes of the resulting control forces are observed. The models of terrain topology and the settings specifying external forces, base navigation height, and the duration of a time step presented below are identical to corresponding settings used for modeling of formations, presented in the subsequent chapters. Due to the differences in the constraints, an independent agent and a formation member represent different dynamical systems. However, the described metrics for evaluating the terrain following flight performance are applicable to both types of these agents. 81 Elevation functions z= f Terrain x,y ( ) for two models of terrain surfaces, denoted as Hills and Wave, are defined below. These functions are selected in order to specify terrain topology leading to different types of paths and agent trajectories. That is the reason for including positions of the goal points in the specifications of the models described below, f Hills x,y ( ) =a tr cos ω tr1 x ( ) +cos ω tr2 y ( ) ( ) a tr =30.0m, ω tr1 =0.018, ω tr2 =0.022 p g = 1000.0 2000.0 −20.0 " # $ % T (3.47) f Wave x,y ( ) =a tr sin ω tr xy 2 ! " # $ % & −c tr cos ω tr xy 2 ! " # $ % & ! " # $ % & a tr =5.0 m, c tr =2.5, ω tr =0.0001 p g = 1800.0 2000.0 10.0 ( ) * + T (3.48) The model Hills results in agent paths going in-between hills, while moving closer to the sides of the hills. It tests how agents adjust motion in horizontal direction in order to avoid terrain obstacles. The model Wave results in agent paths going directly towards position of the goal, forcing agents to move over surface ridges. Slopes of the ridges increase continuously, along the paths, testing how terrain following system reacts on changes in surface elevation. These two models as shown in the figure below. a) b) Figure 3.5: Terrain models, a) Hills, b) Wave. 82 Base navigation and sensor settings, and the values of parameters specifying the external forces, considering that the model of a movable thruster is selected, are shown below as numbered expressions, in order to be referenced later in this thesis, h n =25.0m,γ cr =10.0m (3.49) n c =5,α r =25°,β r =45°,α d =1.5°, r max =200.0m (3.50) g=9.81m s 2 ,η 1 =0.08,η 2 =0.008, f t =200.0N (3.51) 3.4.1 Examples of Sensor Model and Terrain Following Corridor Volumes The following examples serve as an addition to illustrations of sensor model and construction of a safe corridor volume presented in Fig. 3.2, 3.3. Distribution of rays within sensor cone and rays surface intersections are shown in Fig. 3.6. Safe corridor volumes considering minRay and maxRay selection settings are shown in Fig. 3.7. A sequence of safe corridors, navigation vectors, and navigation rays for 12 simulation steps performed using the equations of motion and external forces defined in Eq. (2.1-4, 3.44-46), selection type specified as maxRay, and the duration of a time step specified as dt =0.05 s, is shown in Fig. 3.8. All of the examples presented below share model settings specified in Eq. (3.47, 3.49-51). 83 a) b) Figure 3.6: Sensor model, a) Distribution of rays within sensor cone, b) Ray tracing to the terrain surface. a) b) Figure 3.7: Corridor volume, navigation vector, sensor cone, and navigation ray, a) minRay, b) maxRay. Figure 3.8: Trajectory, corridors, navigation vectors and rays. 84 3.4.2 Comparison of the Simulation Results Obtained with the Ranges of Model Settings The comparison of agent trajectories and their adherence to the bounds of safe corridor volumes is demonstrated by the following series of examples. They test system performance for ranges of model settings. The rest of the settings are identical for all of the models, as specified in Eq. (3.47, 3.49-51), unless noted otherwise in the description of a specific test. Equations of motion and model of external forces are described in Eq. (2.1-4, 3.44-46). The values of stabilization coefficients specified as µ =6.0 and λ =8.0. The tests for different values of dt correspond to different simulation runs, since current implementation of our modeling and simulation system does not allow setting ranges on values of dt . Each of the simulations is performed for 10 independent agents, representing 10 different numerical values of the system parameters being tested. Total flight time considered for all of these simulations is t max =5.0 s, with time steps indexed starting from 1, such that t 1 =0.0 s. All of these agents share initial conditions specified as q i t 1 ( ) = 100.0 100.0 −4.4711 [ ] T and  q i t 1 ( ) = 42.8085 90.3736 0.212668 [ ] T for the i th agent. 3.4.2.1 Range of Settings for the Desired Navigation Height The following example illustrates how change in values of the desired navigation height above terrain h n affects agent trajectories. The range is specified as h n,i ={20.0, 24.4444, 28.8889, 33.3333, 37.7778, 42.2222, 46.6667, 51.1111, 55.5556, 60.0} meters, 85 where i=1,..,10, such that the difference between the subsequent values is constant Δh n =h n,i+1 −h n,i =const . Navigation type is specified as maxRay and the duration of a time step is specified as dt =0.05 s. Plots corresponding to elevation of agent trajectories above terrain are presented below. Elevation profiles of agent trajectories for the first and the last agents, denoted as TF 1 and TF 10 , respectively, are shown on Fig. 3.10. Model settings for these agents correspond to minimum and maximum values of h n from the range being tested. Trajectory and terrain profiles for the agent are obtained with z Terrain,i (t) as described in Eq. (3.25). Distances of between points of agent trajectories and centerlines of corresponding terrain-following corridor volumes are obtained with r cr,i (t) as described in Eq. (3.30), and shown in Fig. 3.11a, where the horizontal line corresponds to the bounds of the cylindrical safe corridor volumes. Distances between points of trajectory profiles and surfaces of constant elevation above terrain are obtained with d e,i (t) as described in Eq. (3.26). Since the values of h n,i are different for different agents, values of d e,i t 1 ( ) are different at the beginning of the simulation, at the time t 1 , even though the initial conditions for all of the agents are identical, as shown in Fig. 3.11b. Central dashed lines shown in Fig. 3.10 and 3.11.b correspond to the desired elevation of agent trajectory above terrain surface, z ce,i (t) , and its offsets, z ce1,i (t) and z ce,i (t) , representing bounds of terrain-following corridor volumes, as defined in Eq. (3.27-29). 86 a) b) Figure 3.9: Independent agents, a) Trajectories and Hills terrain, b) Trajectories. a) b) Figure 3.10: Trajectory profiles, a) TF1, b) TF10. a) b) Figure 3.11: Trajectory deviation from, a) Centerlines of corridors, b) Navigation height. 87 Comparison of distance plots shown above demonstrates that the requirement of staying within the bounds of safe corridors is satisfied, as follows from Fig. 3.11.a. However, maximum magnitude of deviation of agent trajectory from the desired navigation height is considerably greater then the radius of corridor volumes, max d e,i (t) ( ) > γ cr 2 , as follows from Fig. 3.11.b. Since the algorithm implies performing updates of navigation vectors and corresponding corridor volumes at the beginning of each time step, this effect shows deviation of elevations of corridor volumes, represented by z ce,i (t) , from the desired elevation, h n . It could be viewed as a consequence of having a discrete sensing process, specified with dt>>δt , where δt signifies a small number δt→0 . 3.4.2.2 Two Examples with Different Settings for the Duration of a Time Step and Identical Settings for the Ranges of Sensor Cone Width The following two examples demonstrate how the values of the duration of a time step, dt , affect elevations of the resulting trajectories, considering range of settings specifying sensor cone width, α r , specified as α r,i ={10.0, 15.5556, 21.1111, 26.6667, 32.2222, 37.7778, 43.3333, 48.8889, 54.4444, 60.0} degrees, for where i=1,..,10, with the constant difference between subsequent values Δα r =α r,i+1 −α r,i = const . The values of dt are specified as 0.05 s and 0.025s for the first and the second model, respectively, requiring 100 and 200 time steps to complete a simulation with the total time span of 5.0 seconds. More importantly, different number of time steps corresponds to different number of senor readings and the subsequent updates of 88 navigation vectors, affecting the lengths of individual segments of terrain following corridor volumes. Therefore, the presented simulation results demonstrate how increase in sensor frequency affects the behavior of the presented terrain following flight system, as shown in the figures below. a) b) Figure 3.12: Trajectories and Hills terrain, a) dt 0.05 s, b) dt 0.025 s. a) b) Figure 3.13: Trajectory deviation from navigation height , a) dt 0.05 s, b) dt 0.025 s. 89 a) b) Figure 3.14: Trajectory deviation from centerlines of corridors a) dt 0.05 s, b) dt 0.025 s. Comparison of plots shown in Fig. 3.12a demonstrates that different width of sensor cones results in distinctly different agent paths, considering the selected navigation type specified as minRay. Use of the Hills terrain model, where agents are able to move in between hills, makes this difference more pronounced. However, the difference in paths of the same agent obtained with different values of dt is less significant, as follows from Fig. 3.13. Such behavior could be attributed to the fact that sensor cone width directly affects the range of potential navigation directions, while the value of dt specifies frequency with which sensor readings are obtained and a particular navigation direction is selected. Simulation results shown in Fig. 3.14 demonstrate that the increase in senor frequency defined by value of dt decreases magnitude of deviation of agent trajectories from the centerlines of terrain following corridor volumes. Such correlation between values of dt and r cr,i (t) is expected since agent positions belong to the centerlines at the beginning of each time step, as specified by the procedure of updating position and orientation of safe corridor volumes, described in Section 3.3.2. 90 3.4.2.3 Range of Settings for Sensor Range The following example illustrates how change in sensor ranges r max affect elevation of agent trajectories. The range of values of r max is specified as r max ={40.0, 57.7778, 75.5556, 93.3333, 111.111, 128.889, 146.667, 164.444, 182.222, 200.0} meters, for where i=1,..,10, with the constant difference between subsequent values Δr max = r max,i+1 −r max,i = const . Navigation type is specified as maxRay, while the duration of time step is specified as dt =0.05 s. The results include cases when no ray-surface intersections are found due to limited sensor range. In the cases of loss of terrain by sensors, navigation vector is specified using default navigation angle α d , as described in Section 3.3.2. Loss of terrain during one the steps may lead to acquiring large magnitude of elevation of agent trajectories over terrain during subsequent time step, until sensors reacquire terrain. This behavior is demonstrated by trajectory of the agents TF1 and TF2 shown in Fig. 3.16 and corresponding plots of elevation differences shown in Fig. 3.17.b. Despite large magnitude of elevation of agent trajectories, caused by loss of terrain, the requirement of staying within bounds of terrain-following corridor volumes are satisfied, as follows from the results shown in Fig. 3.17.a. It is satisfied because the corridor volumes are build around navigation vectors. In cases of loss of terrain, the vectors are defined without accounting for terrain topology, since this data is not available. 91 a) b) Figure 3.15: Independent agents, a) Trajectories and Hills terrain, b) Trajectories. a) b) Figure 3.16: Trajectory profiles, a) TF1, b) TF2. a) b) Figure 3.17: Trajectory deviation from, a) Centerlines of corridors, b) Navigation height. 92 The plots shown below present numerical values of constraint expressions, defined in Eq. (3.34, 3.39), obtained during the simulation run described above. The range of values of constraint expression, Ψ i (t), serve as a direct metric of computation precision, following [1]. a) b) Figure 3.18: Constraint expressions, a) Φ i (t) , b) Ψ i (t) . 3.5 Conclusion Simulation results presented in this section illustrate how ranges of navigation and sensor specific parameters affect behavior of terrain following system considering selected navigation algorithm and corresponding model of independent agents. More detailed discussion of terrain following system performance is presented in the subsequent chapters devoted to study of formations of terrain following agents. 93 CHAPTER 4: RIGID VS. ADJUSTABLE FORMATION GEOMETRY The following chapter describes guidance and navigation for terrain following formations, using models of dynamical systems and formations structure described in Section 2.2. Two types of formations are considered, virtual rigid body and adjustable virtual structures. Corresponding constraints are defined and equations of motion are derived. Simulation results are presented and applicability of these formation types to solving the problem of terrain following formation flight is evaluated. 4.1 Formation Navigation and Guidance, Navigation Vector of the Virtual Leader This section describes procedure for obtaining navigation vectors of virtual leader. Navigation vectors of virtual leader, ˆ ν s , define formation guidance. This vector, computed for the n th time step, specifies formation navigation for the duration of this time step t∈ t n , t n +dt ( ) . It serves as an input into computation of equations of motion since ˆ ν s is a part of definition of formation keeping constraints of individual formation members. Three methods of defining navigation vectors for the virtual leader are considered. Depending on the selected navigation type, navigation vectors of formation members ˆ ν i , partial set of sensor data R r,i n = p r,i n ,λ r,i n , ˆ ν r,i n { } , or full set of sensor data R r,i = p r,i ,Λ r,i ,Δ r,i { } obtained by formation members are exchanged. Consideration of sensor data during computations of ˆ ν s is not intended for providing terrain collision free motion for virtual leader. Instead, it is trying to limit the range of deviation of leader trajectory from the surface of constant elevation above terrain. 94 4.1.1 Method One, Averaging Navigation Vectors The first method of computing direction of navigation vector for the virtual leader ˆ ν s averages navigation vectors of formation members ˆ ν i . Since the sensor model ensures that each of the vectors ˆ ν i leads towards position of the goal point, p g , averaging of these vectors produces a direction vector ν s,avg that also leads towards position of p g . With the number of formation members specified as n f , normalized vector ˆ ν s is computed as follows, ν s,avg = 1 n f ˆ ν i i=1 n a ∑ , ˆ ν s = ν s,avg ν s,avg (4.1) This method could be considered as heading-only navigation. Since directions of ˆ ν i account for terrain obstacle avoidance, the resulting vector ˆ ν s reflects this behavior. Relative distances between formation members define dimensions of the formation. However, if they are comparable or greater then the size of the terrain features, hills and canyons, the directions represented by vectors ˆ ν i might differ considerably. This occurs when formation members are flying along different sides of a hill or going over different hills. The issue lies in avoiding having considerably different directions of consecutive navigations vectors ˆ ν s for consecutive navigation steps. 4.1.2 Method Two, Considering Navigation Rays The second method of computing ˆ ν s takes into account elevations of the intersection points of terrain surface with navigation rays, p rs,i n , used by individual formation members to compute their respective navigation vectors ˆ ν i , as defined in Eq. 95 (3.23). Set of partial sensor data, consisting of navigation rays R r,i n = p r,i n , λ r,i n , ˆ ν r,i n { } , is exchanged. Therefore, this method is based on the partial pre-processed navigation and sensor data of formation members. Horizontal projection of ˆ ν s coincides with horizontal projection of averaged navigation vector, specified as ˆ v s,avg xy = v s,avg,x xy v s,avg,y xy ! " # $ T . Only navigation vectors of those formation members, which sensors acquired terrain for the n th time step, are considered for computing ν s,avg . Number of those vectors is denoted as n rs , as defined in Eq. (4.2). Vertical component of vector ˆ ν s is defined based on the estimate of surface elevation h s , desired navigation height h n , and magnitude of velocity of virtual leader at the beginning of time step  q s t n ( ) . Position of virtual leader at the beginning n th time step is defined as p s = p sx p sy p sz ! " # $ T and specified as p s =q s t n ( ) . Optional scaling coefficient d s applied to estimate of trajectory length for a single time step is considered, as specified in Eq. (4.2, 4.3). Limit of maximum vertical inclination of ˆ ν s , specified with the angle α s and defined as α s =∠ ˆ ν s , XY Plane ( ) ≤ 45° . These bounds are specified with the term 2h ds in Eq. (4.3). Limiting vertical inclination of ˆ ν s is an acceptable approach since virtual leader is not required to achieve terrain collision free motion. The procedure for computing ˆ ν s implies assigning a virtual navigation ray, defined by a ray-surface intersection point, p s n = p sx n p sy n p sz n ! " # $ T , and defined in Eq. (4.5). 96 This point corresponds to an estimate of terrain topology for the virtual leader. The rest of the procedure is defined as follows, ν s,avg = 1 n rs ˆ ν i i=1 n f ∑ , h s = 1 n rs p rs,i n i=1 n f ∑ , ∀{i}∃λ r,i n ≠∅ (4.2) h ds = p sz −h s +h n , d ds =max 2h ds , d s  q s t n ( ) dt ( ) (4.3) α s = arcsin h ds d ds ! " # $ % & , r s =d ds cos α s ( ) , ˆ ν s,avg xy = ν s,avg xy ν s,avg xy (4.4) p s n = p sx +r s ν s,avg,x xy p sy +r s ν s,avg,y xy h s ! " # $ % & T (4.5) ν s = p s n + 0 0 h n − p sz " # $ % T , ˆ ν s = ν s ν s (4.6) 4.1.3 Method Three, Considering Full Set of Sensor Data The third method of computing ˆ ν s takes into account full set of sensor data collected by formation members, R r,i = p r,i , Λ r,i ,Δ r,i { } . Computations of ˆ ν s follow the same set of steps as with the method described above, Eq. (4.2-6), with the exception of steps for computing value of surface elevation estimate h s . It is computed based on the full set of ray-surface intersection points {p rs,i,jk }∀{i, j,k}∃λ r,i,jk ≠∅ . With the total number of rays intersecting the terrain surface denoted as n ps , the step for obtaining value of h s is defined as follows, h s = 1 n ps p rsz,i,jk k=1 n r ∑ j=1 n r ∑ i=1 n f ∑ , p rs,i,jk = p r,i +λ r,i,jk ν r,i,jk , ∀{i, j,k}∃λ r,i,jk ≠∅ (4.7) 97 Estimating elevation of a plane surface, defined with the point p s n , for computations of the virtual navigation ray and corresponding vector ˆ ν s , is illustrated in the figure below. This estimation is part of the methods two and three, as described above. Figure 4.1: Navigation vector of the virtual leader. 4.2 Virtual Rigid Body Structure Defined with Positioning Offset Constraints Ideally, we would like to have a virtual-structure type formation, the shape of which does not change during motion. In that case the desired holonomic constraints Φ D,i (t)= 0 specified the desired formation geometry relative to the position and navigation vector of the virtual leader , q s (t) and ˆ ν s , respectively. It is defined with a set of constant positioning offsets Δ o ={Δ o,i }, i=1,..,n f , where Δ o,i = Δ ox,i Δ oy,i Δ oz,i " # $ % T specifies the desired position of i th formation member assuming that virtual leader is positioned at the origin of coordinate system and the direction of navigation vector of virtual leader coincides with ˆ x axes = 1 0 0 [ ] T . h n −h s p s ˆ v s ˆ v s,avg xy p s n q 1 t n ( ) q 2 t n ( ) q 3 t n ( ) y x z 98 Computation of actual offset Δ i =Γ R,i s Δ o,i ( ) for the n th time step consists of rotating Δ o,i using rotation operator denoted as Γ R,i s =Γ R ˆ x axis , ˆ ν s ( ) , where operator Γ R ˆ u, ˆ v ( ) is associated with transformation matrix specifying rotation of vector ˆ u to the direction of vector ˆ v around the origin p o =[0 0 0] T . Values of Γ R,i s are recomputed each time step, as required for updating offsets Δ i = Δ x,i Δ y,i Δ z,i " # $ % T . Such formation geometry could be classified as a virtual rigid body structure, since it specifies fixed relative position offsets for the duration of each time step. Then the constraints Ψ D,i (t)= 0 are defined following Eq. (3.15), considering stabilization coefficients µ i =µ and λ i =λ for i=1,..,n f . The equations of motion are derived using the Udwadia-Kalaba equation, [1, 2], Eq. (3.8). The control forces are obtained following Eq. (3.13), with the set of external forces defined in Eq. (2.1-2.4). Φ D,i (t)=q i (t)−q s (t)−Δ i (4.8)  Φ D,i (t)=  q i (t)−  q s (t) (4.9)  Φ D,i (t)=  q i (t)−  q s (t) (4.10) Ψ D,i (t)=λ q i (t)−q s (t)−Δ i ( ) +µ  q i (t)−  q s (t) ( ) +  q i (t)−  q s (t) (4.11) A D,i =diag −1 −1 −1 " # $ % (4.12) b D,i =λ q i (t)−q s (t)−Δ i ( ) +µ  q i (t)−  q s (t) ( ) −  q s (t) (4.13) A D,i .M − 1 2 " # $ % & ' + = m diag −1 −1 −1 ( ) * + (4.14)  q i (t)=λ q s (t)+Δ i −q i (t) ( ) +µ  q s (t)−  q i (t) ( ) +  q s (t) (4.15) 99 While such approach to defining formation geometry is fit for formation keeping, as a method for implementing terrain following formations, it has an inherent flaw. It does not enforce a constraint requiring an agent to stay inside its safe corridor at all times, as defined in context of terrain-following algorithm. Thus, formulated formation control law may produce a command explicitly requiring an agent to occupy a position outside of its safe corridor, which will potentially lead to collision with terrain features. Therefore, the above constraint for formation members needs to be redefined. 4.3 Adjustable Formation Defined with Heuristic Function Constraints An adjustable formation presented in this section is a modification of rigid virtual body structure. It is implemented using heuristic function constraints. This modification is achieved by adding a continuous time heuristic functions, denoted as ξ i (t) and defined as ξ i (t)= ξ x,i (t) ξ y,i (t) ξ z,i (t) ! " # $ T , to the values of constant offsets Δ i . Heuristic functions ξ i (t) are updated at the beginning of each n th time step, at the time t n . If no adjustment is required for j th formation member for n th time step, then the value of heuristic function for this agent is set to zero ξ j (t)= 0, t∈ t n , t n +dt ( ) . The resulting formation is designed to satisfy the requirement of keeping trajectories of individual formation members within bounds of their respective corridor volumes, as required by the terrain following algorithm. Such formation could be classified as adjustable. However, it is not designed as a reconfigurable formation, in terms of changing formation configuration from one predefined geometric shape to another, for example, from a triangle to a line. The desired formation shape remains constant, defined by a set of positioning offsets Δ o , as described in the previous section. 100 Heuristic functions ξ i (t) are defined as purely numerical constructs. They are implemented as continuous differentiable interpolating functions of time t . Different techniques of specifying values of ξ i (t) could be employed, correspond to different implementations of adjustment heuristic functions. This thesis considers three models of heuristics, defined as linear and nonlinear functions. 4.3.1 Constraints and Equations of Motion Expression for adjustable positioning offset constraint is denoted as Φ DX,i (t) , and defined as an extension Φ D,i (t) by adding heuristic term ξ i (t) . Then the constraints Ψ DX,i (t)= 0 are defined following Eq. (3.15) and corresponding equations of motion are derived with accordance to Udwadia-Kalaba approach [1, 2], Eq.(3.8), and the control forces are obtained with Eq.( 3.13, 2.1-4). Φ DX,i (t)=q i (t)−q s (t)−Δ i −ξ i (t) (4.16)  Φ DX,i (t)=  q i (t)−  q s (t)−  ξ i (t) (4.17)  Φ DX,i (t)=  q i (t)−  q s (t)−  ξ i (t) (4.18) Ψ DX,i (t)=λ q i (t)−q s (t)−Δ i −ξ i (t) ( ) +µ  q i (t)−  q s (t)−  ξ i (t) ( ) +  q i (t)−  q s (t)−  ξ i (t) (4.19) A DX,i =diag −1 −1 −1 " # $ % (4.20) b DX,i =λ q i (t)−q s (t)−Δ i −ξ i (t) ( ) +µ  q i (t)−  q s (t)−  ξ i (t) ( ) −  q s (t)−  ξ i (t) (4.21) A DX,i .M − 1 2 " # $ % & ' + = m diag −1 −1 −1 ( ) * + (4.22)  q i (t)=λ q s (t)+Δ i +ξ i (t)−q i (t) ( ) +µ  q s (t)+  ξ i (t)−  q i (t) ( ) +  q s (t)+  ξ i (t) (4.23) 101 4.3.2 Heuristic Function Design With Terrain Following Trajectories and Corridor Volumes Heuristic functions are designed to serve as direct adjustment to constant offsets of agents position, computed for the n th time step. The direction and magnitude of the required adjustments are estimated based on computed values state matrices Q Δ,i (t) describing unadjusted terrain following motion considering offsets Δ i only. Heuristic ξ i (t) for the i th agent for the n th time step is specified according to the computed deviation of unadjusted trajectory of the agent, defined by q Δ,i (t) , from the bounds of corridor volumes, defined by the point p i =q i t n ( ) and vector ˆ ν i for this time step, where q i t n ( ) ≡q TF,i t n ( ) . This process implies modeling formation with both TF and FK elements as illustrated on diagram shown in Fig. 2.6 and formation structure shown in Fig. 2.5 in the problem formulation Section 2.2, considering notations Q TF,i (t)=Q Δ,i (t) . The corresponding geometric procedure is defined as follows. The deviation at the end of a time step is specified with the offset defined as ξ r,i = ξ rx,i ξ ry,i ξ rz,i ! " # $ T , where ξ r,i = const for the i th agent for the n th time step. The distance, denoted as r d,i , quantifies amount of this deviation. The value of r d,i is specified using the procedure for computing position and distance of the nearest point on a line to a given point, described in [102]. The point of unadjusted trajectory at the end of the n th time step is defined as p d,i =q Δ,i t n+1 ( ) , where t n+1 =t n +dt . Nearest point on centerline of corridor volume is denoted as p q,i and specified using distance λ q,i . Normalized vector ˆ u q,i belongs to a line 102 defined by points p d,i and p q,i . This vector is perpendicular to the centerline, ˆ u q,i ⊥ ˆ ν i . Point denoted as p b,i coincides with intersection of a line defined by p q,i , ˆ u d,i { } and bounds of the corridor volume. If the resulting deviation distance is greater then the radius of the cylinder, r d,i > γ cr 2 , then a non-zero adjustment term ξ r,i is assigned. Positions of these points, unadjusted trajectory, and the desired adjusted trajectory are illustrated in the figure below. The values are computed as follows, λ q,i = ˆ ν i . p d,i − p i ( ) , p q,i = p i +λ q,i ˆ ν i (4.24) ˆ u q,i = p d,i − p q,i p d,i − p q,i , r d,i = p d,i − p q,i , p b,i = p q,i + γ cr 2 ˆ u q,i (4.25) ξ r,i = p d,i − p b,i , r d,i > γ cr 2 ∅, r d,i ≤ γ cr 2 $ % & & ' & & (4.26) Figure 4.2: Trajectory adjustment. Type of interpolation function for ξ i (t) is specified as xiLinear or xiNonlinear, with the parameter specifying interpolation order set to 1 and 3, respectively. The functions are defined using list interpolation method, considering set of vectors denoted p d,i ˆ ν i r d,i p b,i p q,i q Δ,i (t) q i (t) p i γ cr 2 ˆ u q,i λ q,i 103 as Ξ i . This set represents values of positioning offset adjustments along the trajectory of the i th formation member for the n th time step. It is obtained based on value of ξ r,i , considering a set of constant scaling coefficients denoted as c ξ1 , c ξ2 , c ξ3 . The resulting procedure for specifying value of heuristics is defined as follows, Ξ i = c ξ1 ξ r,i , c ξ2 ξ r,i { } , ξ r,i ≠∅∧xiLinear c ξ1 ξ r,i , c ξ2 ξ r,i , c ξ3 ξ r,i ,ξ r,i { } , ξ r,i ≠∅∧xiNonlinear ∅, ξ r,i =∅ % & ' ' ( ' ' (4.27) ξ i (t)= Intepolation Ξ i , t∈ t n , t n +dt ( ) ( ) , Ξ i ≠∅∧xiLinear Intepolation Ξ i , t∈ t n , t n +dt ( ) ( ) , Ξ i ≠∅∧xiNonlinear 0 0 0 & ' ( ) T , Ξ i =∅ * + , , - , , (4.28) It shall be noted that there is a reason to expect relatively poor performance of linear heuristics. It lies in the assumption we made. Since the term ξ i (t) represents an error between the ideal constant and current formation offsets, by doing linear interpolation between its values at the beginning and the end of time step we implicitly assumed that this error grows along the time step and reaches its maximum value at the end of time step. That assumption may not hold true in all cases. It is possible that an agent reaches maximum distance from the safe corridor at some time before the end of time step, and from that time point the error decreases. A set of trajectory points {q Δ,i t k ( ) } where t k ∈ t n , t n +dt [ ] could be considered as an extension to the procedure for specifying values of heuristic functions ξ i (t) presented above. Such extension would consider cases when maximum deviation of unmodified 104 agents trajectory from centerline of corridor volume does not occur at the end of n th time step. 4.4 Simulation Results and Comparison of Rigid and Adjustable Formations This section presents simulation results for terrain-following flight of rigid and flexible formations. Comparison of terrain following flight performance is presented. The simulations are performed for a triangular formation configuration, n f =3. Virtual leader is modeled using equations of motion defined in Eq. (3.44-46). The values of stabilization coefficients for both formation members and the virtual leader are specified as µ i =µ s =6.0 and λ i =λ s =8.0 . External forces are modeled using Eq. (2.1- 4). All of the presented simulation examples share terrain model Hills, position of the goal, external forces, sensor, and navigation settings, as specified in Eq. (3.47, 3.49-51). Navigation type for formation members is specified as minRay, while formation navigation and guidance is achieved using method of computing navigation vector of the virtual leader denoted as method one, averaging navigation vectors, defined with Eq. (4.1). Total flight time is set to t max =5.0 s, with the initial time step indexed as 1 and specified as t 1 =0.0 s. Initial conditions of formation members are specified by matrices Q t 1 ( ) = q 1 t 1 ( ) T q 2 t 1 ( ) T q 3 t 1 ( ) T ! " # $ T and  Q t 1 ( ) =  q 1 t 1 ( ) T  q 2 t 1 ( ) T  q 3 t 1 ( ) T ! " # $ T . Initial conditions of the virtual leader are specified by vectors q s t 1 ( ) and  q s t 1 ( ) . Constant desired positioning offsets of formation members are specified by Δ o = Δ o,1 T Δ o,2 T Δ o,3 T " # $ % T . Initial formation configuration is illustrated in the figure below, and specified as follows, 105 Q(t 1 )= 112.845 166.625 −20.0 191.75 188.625 −20.0 185.437 106.954 −20.0 " # $ $ $ % & ' ' ' ,  Q(t 1 )= 43.5576 90.0152 0.0 40.7483 91.3213 0.0 39.5254 91.8572 0.0 " # $ $ $ % & ' ' ' (4.29) q s t 1 ( ) = 160.0 150.0 −20.0 " # $ % T ,  q s t 1 ( ) = 41.3432 91.0535 0.0 " # $ % T (4.30) Δ o = -4.35779 49.8097 0.0 48.2963 -12.941 0.0 -28.6788 -40.9576 0.0 " # $ $ $ % & ' ' ' (4.31) Figure 4.3: Initial configuration. 4.4.1 Terrain Collision with Rigid Virtual Body Structure The following simulation results support the argument that using rigid virtual body structure for terrain following formation flight might lead to terrain collision of formation members, modeled using equations of motion defined in Eq. (4.15). Two presented examples of triangular formations share all of the settings except the values of dt , set to 0.05 and 0.025 seconds for the first and the second model, respectively. These two models are considered in order to show that terrain collision with rigid formation geometry could not always be avoided by simply increasing sensor sampling frequency by decreasing the value of dt . 106 As follows from the simulation results shown in the figures below, terrain collision for the agent denoted as FK 3 occurs in both cases. The requirement of staying within bounds of terrain-following corridor volumes is violated during several consecutive time steps, with larger duration of a time step, as shown in Fig. 4.7a. However, this requirement is satisfied with smaller time step, except during the step associated with the collision, not included in the plot, as follows from the results shown in Fig. 4.7b. a) b) Figure 4.4: Terrain and trajectories, a) dt 0.05 s b) dt 0.025 s. a) b) Figure 4.5: Formation shape, a) dt 0.05 s b) dt 0.025 s. 107 a) b) Figure 4.6: Trajectory profile of FK3, a) dt 0.05 s b) dt 0.025 s. a) b) Figure 4.7: Trajectory deviation from centerlines of corridors, a) dt 0.05 s b) dt 0.025 s. The results presented above show that the formations change structure from a triangle to a segment after an agent collides with the terrain. Such behavior demonstrates that the requirement to formation design regarding the possibility of removing agents from formation, stated in Section 2.2, is satisfied. 4.4.2 Collision Free Motion with Adjustable Formation Geometry An adjustable formation geometry is modeled using equations of motion for formation members defined in Eq. (4.23). The following example, demonstrating collision free flight of a triangular formation, is modeled with the settings described 108 above, considering duration of a time step dt =0.05 s, heuristic function type xiNonlinear, and the values of coefficients used for obtaining values of ξ i (t) specified as c ξ1 =0.0, c ξ2 =0.75, c ξ3 =1.0. Simulation results shown in the figures below demonstrate that an adjustable formation allows avoiding terrain collision for all of the formation members, while satisfying requirements of keeping agent positions within bounds of terrain following corridor volumes. However, frequent changes in positioning offsets are reflected by the resulting trajectory shapes, follows from formation shape plots shown in Fig.4.8b and trajectory profile of FK 3 shown in Fig. 4.9b. Magnitudes and frequency of occurrence of non-zero adjustment terms ξ i (t) is demonstrated in the Fig. 4.10, showing combined constant and adjustment offsets, Δ i +ξ i (t) . It shall be noted that the magnitude of Δ o,i signifies the desired distance form position of i th formation member to position of the virtual leader. This value is set to 50.0 meters for all of the formation members, as follows from the settings defined in Eq. (4.31). 109 a) b) Figure 4.8: Adjustable, a) Terrain and trajectories, b) Formation shape. a) b) Figure 4.9: Trajectory, a) Deviation from centerlines of corridors for all of the formation members b) Elevation profile of FK3. Figure 4.10: Adjustable offsets magnitude. 110 4.4.3 Comparison of Rigid and Adjustable Formations Formation geometry defined with heuristic offset adjustments does allow avoiding terrain collision of formation members, however, it requires considerably greater control force then a similar model defined as a virtual rigid body structure. Although magnitude of offset adjustments with flexible structure is much smaller then the total offset magnitude, as follows from the results shown in Fig. 4.10, such adjustments translate into sharp change in the direction of motion, velocity, and acceleration of an agent. These, in turn, translates into the requirement of large and abrupt increase in control force, as demonstrated by simulation results shown in the figures below. The duration of a time step is set to 0.05 seconds for both examples shown below. It shall be noted that the corresponding models describe greatly exaggerated flight system, in terms of considerably large value of stabilization coefficients, relatively low desired navigation height combined with high slope values of terrain features. Relatively small diameter of safe corridors is required, as a result. However, the differences in the magnitude of control forces, observed in the plots shown in Fig. 4.13, demonstrate a consistent trend inherent for an adjustable formation defined with the described structure of heuristic functions. Larger magnitude of the control force, which is represented by forces of constraints, places greater demand or more strict requirements, for the actuator design. 111 a) b) Figure 4.11: Trajectory deviation from navigation height, a) Rigid, b) Adjustable. a) b) Figure 4.12: Velocity magnitude, a) Rigid, b) Adjustable. a) b) Figure 4.13: Forces of constraints magnitude, a) Rigid, b) Adjustable. 112 Sharp abrupt changes in agents motion required due to adjustment offset term in the expression of constraints could be observed directly by evaluating plots for the values of Φ D,i (t) and Φ DX,i (t) , shown below. The values of constraint expressions Ψ D,i (t) and Ψ DX,i (t) show that the orders of magnitude of numerical errors, associated with the simulations of rigid and adjustable formations, is comparable. a) b) Figure 4.14: Constraint expressions Φ i (t) , a) Rigid, b) Adjustable. a) b) Figure 4.15: Constraint expressions Ψ i (t) , a) Rigid, b) Adjustable. 4.5 Conclusion Application of rigid and adjustable formations for the design of terrain following flight systems is described and their performance is compared. It was found that the rigid 113 virtual body structure does not result in the desired terrain collision free formation motion. It was also found that although an adjustable formation allows formation members to stay within bounds of their respective safe corridor volumes, it requires considerable magnitude of the control force, making the design of the corresponding flight system potentially unfeasible for practical implementation. However, it shall be noted that the presented simulation results are obtained with an exaggerated settings, considering a purely numerical procedure for specifying heuristic functions used for the positioning offset adjustments. Formation navigation and guidance is implemented with the procedures for computing navigation vector of virtual leader. However, only method one, averaging navigation vectors of formation members is demonstrated with modeling and simulation results presented in this chapter. Moreover, no examples of trajectory profiles for the virtual leader has been shown. These examples are included in the results section of the next chapter, describing the design for achieving a flexible formation geometry. 114 CHAPTER 5: FLEXIBLE FORMATION GEOMETRY DEFINED WITH PLANES AND NAVIGATION VECTOR CONSTRAINTS This chapter describes formation motion for terrain following formation with flexible formation geometry, using model of the dynamical system and formation structure described in Section 2.2. A flexible formation allows formation members to achieve much greater range of relative position changes then an adjustable formation, presented in the previous chapter. Thus, it is expected to provide a more suitable solution for the terrain following flight system design. Formation model is obtained using constraints defined with planes and navigation vectors. The presented method uses two distinct sets of constraints, for terrain following motion and for formation keeping, as described in Chapter 2. The first set consists of the constraints on tracking a line defined by the agent position point and its navigation vector, updated at the beginning of each time step. The second set consists of plane constraint defined in the following section. The desired geometrical shape of the formation is defined with the settings for base vectors normal to planes, specifying orientation of the planes relative to position vector of the virtual leader. Simulation results illustrating motion of an agent considering the constraints described above is provided. Examples of triangular and star formation configurations, with 3 and 5 formation members, respectively, are shown. Performance of these formations is evaluated using metrics applicable to the analysis of terrain following flight, as described in Section 3.3.2. Additional characteristics, specific to the analysis of terrain following formation flight, are defined and used also, as described in the following sections. 115 The section describing simulation results presents comparison of the trajectory paths of the virtual leader, obtained with the tree methods of computation of navigation vectors for this agent, described in the previous chapter. These examples are included in this chapter, since they are using models for flexible formations, thus, allowing to highlight the differences between these methods. 5.1 Plane Surface Constraint The set of formation keeping surface constraints restricts motion of agents to a set of plane surfaces. A plane associated with an agent is defined by the following three factors; position of the virtual leader, q s (t) , position of the i th agent, q i (t) , and vector normal to the plane, defined as ˆ u i = u x,i u y,i u z,i ! " # $ T . Direction of the vector normal to this plane is updated at the beginning of the n th time step, at the time t n . It is specified by a normalized, constant base direction vector, defined as ˆ u o,i = u ox,i u oy,i u oz,i ! " # $ T , and navigation vector of the virtual leader, ˆ ν s . Therefore, the set of these base, reference, direction vectors defines geometrical shape of the formation. Vector ˆ u o,i is specified assuming that the direction of navigation vector of virtual leader coincides with ˆ x axis . Therefore, computation of vector ˆ u i consists of rotation of vector ˆ u o,i relative to the direction of vector ˆ ν s for the n th time step, u i =Γ R,i s ˆ u o,i ( ) and ˆ u i = u i u i , where Γ R,i s =Γ R ˆ x axis , ˆ ν s ( ) . This procedure involves transformation operator Γ R u,v ( ) specifying rotation of vector u into the direction of vector v , around origin point p o . Position of a plane, denoted as PQ Plane , relative to the parameters described 116 above is illustrated in the figure below. Figure 5.1a shows positions of the agents at the beginning of a time step. Figure 5.1b shows orientation of the plane relative to the trajectory of the virtual leader during the n th time step, where t n+1 =t n +dt . a) b) Figure 5.1: Planes, normal and navigation vectors shown, a) At the time t n b) During the n th time step. The plane constraint, denoted as ϕ P,i (t)= 0 , is derived based on the equation of a plane presented in [102]. Then, the plane constraint, ψ P,i (t)= 0 , is defined following Eq. (3.14) with constant stabilization coefficients denoted as µ i and λ i . The plane is defined by two points p,q { }∈PQ Plane and normalized vector normal to a plane, denoted as ˆ u⊥PQ Plane , such that ˆ u⋅ p−q ( ) =0 . Considering notations for the i th agent, such as vector ˆ u= ˆ u i , points p=q i (t) and q=q s (t) , the plane equation and corresponding expressions for constraints become, ˆ u i ⋅ q i (t)−q s (t) ( ) = 0 (5.1) ϕ P,i (t)=u x,i q x,i (t)−q sx (t) ( ) +u y,i q y,i (t)−q sy (t) ( ) +u z,i q z,i (t)−q sz (t) ( ) (5.2)  ϕ P,i (t)=u x,i  q x,i (t)−  q sx (t) ( ) +u y,i  q y,i (t)−  q sy (t) ( ) +u z,i  q z,i (t)−  q sz (t) ( ) (5.3) q s t n ( ) PQ Plane,i q i t n ( ) ˆ u i ˆ ν s ˆ x axis ˆ u o,i q s t n ( ) q i t n ( ) ˆ u i q s t n+1 ( ) q i t n+1 ( ) ˆ u i ˆ u i ˆ u i 117  ϕ P,i (t)=u x,i  q x,i (t)− q sx (t) ( ) +u y,i  q y,i (t)−  q sy (t) ( ) +u z,i  q z,i (t)− q sz (t) ( ) (5.4) ψ P,i (t)=u x,i  q x,i (t)− q sx (t)+µ i  q x,i (t)−  q sx (t) ( ) +λ i q x,i (t)−q sx (t) ( ) ( ) +u y,i  q y,i (t)−  q sy (t)+µ i  q y,i (t)−  q sy (t) ( ) +λ i q y,i (t)−q sy (t) ( ) ( ) +u z,i  q z,i (t)− q sz (t)+µ i  q z,i (t)−  q sz (t) ( ) +λ i q z,i (t)−q sz (t) ( ) ( ) (5.5) 5.2 Full Sets of Constraints and Corresponding Equations of Motion The combined set of constraints for the i th formation member is denoted as Φ PLX,i (t)= 0 . It shall be noted that since three versions of the expressions Φ LX,i , Φ LY,i , and Φ LZ,i are available, as defined in Eq. (3.34-36), three corresponding sets Φ PLX,i , Φ PLY,i , and Φ PLZ,i could be derived. However, the models presented in this chapter are limited to using equations of motion derived with the constraints Ψ PLX,i (t)= 0 corresponding to the expressions Φ PLX,i (t) , defined as follows, Φ PLX,i (t)= ϕ P,i ϕ L1,i ϕ L2,i " # $ $ $ $ % & ' ' ' ' = u x,i q x,i (t)−q sx (t) ( ) +u y,i q y,i (t)−q sy (t) ( ) +u z,i q z,i (t)−q sz (t) ( ) q y,i (t)−β y1 −α y1 q x,i (t) q z,i (t)−β z1 −α z1 q x,i (t) " # $ $ $ $ % & ' ' ' ' (5.6) As follows from the description of Φ PLX,i (t) presented above, the expression Ψ PLX,i (t) is a combination of expressions for the plane surface constraint, ψ P,i (t) , defined above, in Eq. (5.5), and the expressions for constraints on tracking a line defined by the agent’s position and navigation vector, Ψ LX,k (t)= ψ L1,k (t)ψ L2,k (t) " # $ % T , as defined in Eq. (3.39), considering agent’s index k=i. 118 The stabilization coefficients are defined with 3×3 diagonal matrices. However, only two distinct values per each type of the coefficients are specified, µ 1 ,µ 2 { } and λ 1 ,λ 2 { } , since both expressions for the second set of constraints, ψ L1,i (t) and ψ L2,i (t) , are defined with the values specified by µ 2 and λ 2 . Therefore, the matrices are defined as µ i =diag µ 1 µ 2 µ 2 [ ] and λ i =diag λ 1 λ 2 λ 2 [ ] . The equations of motion are derived using the Udwadia-Kalaba equation, [1, 2], Eq. (3.8). The control forces are obtained following Eq. (3.13), considering the set of external forces defined in Eq. (2.1-2.4). Terms denoted as c PLX,i and d PLX,i (t) are used to simplify the expressions, as follows, Ψ PLX,i (t)= ψ P,i (t) ψ L1,i (t) ψ L2,i (t) " # $ $ $ $ % & ' ' ' ' (5.7) A PLX,i = −u x,i −u y,i −u z,i α y1 −1 0 α z1 0 −1 " # $ $ $ $ % & ' ' ' ' (5.8) b PLX,i = b P,i b L1,i b L2,i ! " # # # # $ % & & & & , b P,i =u x,i − q sx (t)+µ 1  q x,i (t)−  q sx (t) ( ) +λ 1 q x,i (t)−q sx (t) ( ) ( ) +u y,i − q sy (t)+µ 1  q y,i (t)−  q sy (t) ( ) +λ 1 q y,i (t)−q sy (t) ( ) ( ) +u z,i − q sz (t)+µ 1  q z,i (t)−  q sz (t) ( ) +λ 1 q z,i (t)−q sz (t) ( ) ( ) b L1,i =µ 2  q y,i (t)−α y1  q x,i (t) ( ) +λ 2 q y,i (t)−β y1 −α y1 q x,i (t) ( ) b L2,i =µ 2  q z,i (t)−α z1  q x,i (t) ( ) +λ 2 q z,i (t)−β z1 −α z1 q x,i (t) ( ) (5.9) 119 c PLX,i =u x,i +u y,i α y1 +u z,i α z1 2 (5.10) d PLX,i (t)=λ 1 u x,i q sx (t)−q x,i (t) ( ) +u y,i q sy (t)−q y,i (t) ( ) +u z,i q sz (t)−q z,i (t) ( ) ( ) +µ 1 u x,i  q sx (t)−  q x,i (t) ( ) +u y,i  q sy (t)−  q y,i (t) ( ) +u z,i  q sz (t)−  q z,i (t) ( ) ( ) +u x,i  q sx (t)+u y,i  q sy (t)+u z,i  q sz (t) (5.11) A PLX,i .M − 1 2 " # $ % & ' + = m c PLX,i −1 u y,i u z,i −α y1 −u x,i −u z,i α z1 u z,i α y1 −α z1 u y,i α z1 −u x,i −u y,i α y1 ( ) * * * * + , - - - - (5.12)  q x,i (t)= 1 c PLX,i d PLX,i (t)−λ 2 u y,i β y1 +u z,i β z1 ( ) +λ 2 − u y,i α y1 +u z,i α z1 ( ) q x,i (t)+u y,i q y,i (t)+u z,i q z,i (t) ( ) +µ 2 − u y,i α y1 +u z,i α z1 ( )  q x,i (t)+u y,i  q y,i (t)+u z,i  q z,i (t) ( ) " # $ $ $ $ % & ' ' ' ' (5.13)  q y,i (t)= 1 c PLX,i α y1 d PLX,i (t)+λ 2 u x,i β y1 +u z,i α z1 β y1 −α y1 β z1 ( ) ( ) +λ 2 u x,i q x,i (t)− u x,i +u z,i α z1 ( ) q y,i (t)+u z,i q z,i (t) ( ) +µ 2 u x,i  q x,i (t)− u x,i +u z,i α z1 ( )  q y,i (t)+u z,i  q z,i (t) ( ) " # $ $ $ $ % & ' ' ' ' (5.14)  q z,i (t)= 1 c PLX,i α z1 d PLX,i (t)+λ 2 u x,i β z1 +u y,i α y1 β z1 −α z1 β y1 ( ) ( ) +λ 2 u x,i q x,i (t)+u y,i q y,i (t)− u x,i +u y,i α y1 ( ) q z,i (t) ( ) +µ 2 u x,i  q x,i (t)+u y,i  q y,i (t)− u x,i +u y,i α y1 ( )  q z,i (t) ( ) " # $ $ $ $ % & ' ' ' ' (5.15) 5.3 Metrics for Evaluating Performance of Terrain Following Formations The following metrics are defined and used for analyzing and evaluating performance of terrain following formation flight. These characteristics are considered in addition to such metrics as d e,i (t) and r cr,i (t) defined in Section 3.3.3. Relative distance between the i th formation member and the virtual leader is denoted as d s,i (t) . Relative distances between the adjoined formation members i and i+1 120 are denoted as d i,i+1 (t) , where the last agent is connected with the first formation member, as follows, d s,i (t)= q s (t)−q i (t) (5.16) d i,i+1 (t)= q i (t)−q i+1 (t) i≠ n f q i (t)−q 1 (t) i= n f # $ % & % (5.17) An indirect metric of satisfying plane constraint could be defined by observing values of angles between vectors normal to planes and vectors connecting positions of the i th formation member and virtual leader. The angles, denoted as α i (t) , are computed for each n th time step t∈ t n , t n +dt ( ) . These values are represented by piecewise continuous functions of time t , since of values of vectors ˆ u i remain constant for the duration of a time step and positions of the agents q s (t) , q i (t) are continuous functions of time t . They are defined as follows, α i (t)=∠ ˆ u i ,q i (t)−q s (t) ( ) (5.18) A metric of satisfying plane constraint could be defined as deviation of values of the angles α i (t) from 90° . If the constraint is satisfied, values of the angles are α i (t)=90° . It should be noted that while such metric has a potential of providing useful insight for the analysis of formation behavior, the direct metrics of satisfying plane constraint are values of expressions for ϕ P,i (t) and ψ P,i (t) themselves, computed following Eq. (5.2, 5.5). 121 Another metric allowing some insight into formation behavior could be defined by observing values of angles between consecutive navigation vectors of the agents. The angles, denoted as β i,n for the i th agent for the n th time step, are defined as, β i,n =∠ ˆ v i,n−1 , ˆ v i,n ( ) (5.19) The angles quantify change in the desired heading of the agents from time step to time step. They are described by discrete, piecewise functions. The values of β i,n remain constant for the duration of the n th time step, since values of navigation vectors are discrete parameters updated at the time t n , at the beginning of the n th time step. Such metric is applicable to the analysis of both types of agents, virtual leader and formation member, with corresponding angles denoted as β s and β i respectively. In the context of terrain-following flight angles β i could be viewed as characteristic describing connection of sensor, navigation settings, and terrain topology. An additional metric applicable to the analysis of performance of terrain following dynamical system could be created by observing values of the angles between velocity direction vectors and navigation vectors. The angles, denoted as γ i (t) for the i th agent for the n th time step are defined as, γ i (t)=∠ ˆ v i ,  q i (t) ( ) (5.20) The angles are described as piecewise continuous functions of time t , since values of navigation vectors are discrete piecewise parameters and velocity direction vectors are represented by continuous functions, with values  q i (t) changing during the time step. 122 5.4 Example of Leader-Follower Flight and Demonstration of Performance Metrics The following example serves as a test case illustrating the derived constraint is presented below. It considers a leader-follower system without sensors or terrain models. It is created for illustrating a connection between virtual leader and a formation member. Thus, updates of navigation vectors is modeled as predefined functions, changing the directions of vectors ˆ ν s and ˆ ν i at each time step. Provided that only several time steps are considered in order to show more detailed plots, they demonstrate changes of parameter values during a time step. Navigation vectors of the leader and the follower are generated using rotation of a base vectors ˆ ν o,s and ˆ ν o,i at specified angles α n,s and α n,i at each n th time step. Duration of a time step is specified as dt =0.05 s, with the total of 10 time steps considered. Equations of motion used in this example are defined in Eq. (3.44- 46) for the leader and in Eq. (5.13-15) for the follower. The rest of the model settings are specified in Eq. (2.1-4, 3.47, 3.49-51). Trajectory plots, presented in the figure below, show orientation of planes, vectors normal to planes ˆ u i , navigation vectors of the leader, ˆ ν s , and the follower, ˆ ν i . It shall be noted that the planes and vectors are plotted at the beginning of each n th time step, at the time t n , unlike the previous illustration shown in Fig. 5.1, demonstrating orientation of planes during a time step. 123 a) b) Figure 5.2: Leader and follower, a) 1 st step, b) 10 steps. The values α 1 (t), corresponding to leader-follower example presented above, are shown in the figure below, as defined in Eq.(5.18), with horizontal dashed line corresponding to the value of 90° . Figure 5.3: Angle between leader-follower position vector and vector normal to plane. Plots of values of the angles β s and β 1 , describing the difference between consecutive navigation vectors of the leader and the follower, are shown in the figure below. The plots demonstrate that these values, computed as defined in Eq. (5.19), remain constant during a time step, as described previously. It shall be noted that in the context of this example, in the absence of terrain in sensor models, the values angles are 124 defined by the procedure for obtaining navigation vectors for the next time step, described previously. a) b) Figure 5.4: Angles between consecutive navigation vector of, a) Leader, b) Follower. Plots of angles γ s (t) and γ 1 (t) corresponding to the leader and the follower, respectively are shown in the figure below. It demonstrates that these values, computed as defined in Eq. (5.20), are changing during a time step, as described previously. a) b) Figure 5.5: Angles between velocity direction and navigation vectors of, a) Leader, b) Follower. 125 5.5 Simulation Results for Triangular and Star Formation Configurations The presented simulation examples demonstrate behavior of terrain following flight system for the formations designed using plane and navigation vector constraints. Correlation of system performance and model settings is evaluated using metrics described in Sections 3.3.3 and 5.3. The analysis is performed for two models of formations, implementing triangular and star formation configurations, with three and five formation members, n f =3 and n f =5, respectively. Equations of motion for the virtual leader are defined in Eq. (3.44-46), with the values of stabilization coefficients µ s =6.0 and λ s =8.0. Formation navigation and guidance is performed using method two, considering navigation rays of formation members, as described in Section 4.1.2. Equations of motion for formation members are defined in Eq. (5.13-15). The terrain models specified as Hills and Wave are defined in Eq. (3.47, 3.48). External forces, sensor and navigation settings, shared among the examples, are specified in Eq. (2.1-4, 3.49-51). The examples of two models for each of the formation configurations are presented. The models of triangular formations are denoted as Set One and Set Two. Comparison of the results obtained with these models demonstrates performance difference for formations modeled with different ratios of values of stabilization coefficients µ and λ . The models of star formations are denoted as Star One and Star Two. The results obtained with these models demonstrate the how the difference in the duration of a time step, dt , affects formation performance. The differences between model settings are summarized in the table below. 126 Example Name Formation Configuration Terrain Model Navigation Type dt , s µ 1 µ 2 λ 1 λ 2 Set One Triangular Hills minRay 0.05 3.0 8.0 5.0 10.0 Set Two 0.05 8.0 5.0 10.0 7.0 Star One Star Wave maxRay 0.1 3.0 8.0 5.0 10.0 Star Two 0.025 3.0 8.0 5.0 10.0 Table 5.1: Model settings. The desired formation geometry is defined with the set of vectors normal to planes, denoted as ˆ u o,i , as described in the previous section. Initial formation configuration is specified by initial positions of formation members and a set of initial vectors normal to planes denoted as ˆ u 1,i for the first time step t 1 =0.0 s. Thus, initial conditions of formation members are defined by matrices U o = ˆ u o,1 T ... ˆ u o,n T ! " # $ T , U 1 = ˆ u 1,1 T ... ˆ u 1,n T ! " # $ T , Q t 1 ( ) = q 1 t 1 ( ) T ... q n t 1 ( ) T ! " # $ T , and  Q t 1 ( ) =  q 1 t 1 ( ) T ...  q n t 1 ( ) T ! " # $ T . Initial conditions for the virtual leader are defined by initial position q s t 1 ( ) and velocity  q s t 1 ( ) . 5.5.1 Triangular Formations, Modeled with Different Ratios of Stabilization Coefficients The presented examples illustrate how change in model parameters specifying values of Baumgarte stabilization coefficients affects behavior the formations. Change in ratios between values µ 1 , µ 2 , and λ 1 , λ 2 allows to demonstrate how different constraints affect overall system performance. The ratios are set to µ 1 <µ 2 , λ 1 <λ 2 for Set One, and µ 1 >µ 2 , λ 1 >λ 2 for Set Two, as described in Table 5.1. 127 Initial conditions of for the virtual leader and formation members, q s t 1 ( ) ,  q s t 1 ( ) , Q t 1 ( ) , and  Q t 1 ( ) , are the same as specified previously, in Eq. (4.29, 4.30). The resulting initial configuration and the desired formation shape, defined with vectors normal to planes, is shown in the figure below and the values are specified as follows, U o = -0.996195 -0.0871557 0.0 0.258819 0.965926 0.0 0.819152 -0.573576 0.0 ! " # # # $ % & & & , U 1 = 0.435576 0.900152 0.0 0.407483 0.913213 0.0 0.395254 0.918572 0.0 ! " # # # $ % & & & (5.21) a) b) Figure 5.6: Triangular formation, a) Desired geometry, b) Initial configuration. Formation structure, obtained with Set One, for the first 5 simulation time steps is demonstrated in the figure below. Triangular formation polygons plotted at the beginning of time steps, agent trajectories, and sensor rays are shown in Fig. 5.7a. Plane surfaces plotted at the beginning of time steps, vectors normal to planes, navigation vectors, and cylindrical corridor volumes together with terrain and agent trajectories are shown in Fig. 5.7b. 128 a) b) Figure 5.7: Formation Set One, a) Polygons and sensor rays, b) Planes, corridors, and navigation vectors. Plots of terrain, trajectories and formation shapes for both examples are shown in the figure below. The results obtained with Set Two show terrain collision of formation member, denoted as FK 3 . The agent exits formation, while the two remaining formation members proceed, as shown in Fig. 5.8b. Since the constraints for individual formation members are defined relative to position and direction of motion of virtual leader, an agent could exit formation without changing models, constraints, of the remaining agents. a) b) Figure 5.8: Formation shape, a) Set One, b) Set Two. 129 Terrain collision of the agent FK 3 , shown above, is associated with considerable magnitude of violation of safe corridor bounds during several consecutive time steps preceding the collision event, as follows from simulation results presented in Fig. 5.10b. Such violation is attributed to insufficient magnitude of stabilization coefficient values µ 2 and λ 2 , considering the rest of model settings, such as the desired navigation height, duration of time step, and parameters specifying external forces acting on the agents. a) b) Figure 5.9: Trajectory profile of FK3, a) Set One, b) Set Two. a) b) Figure 5.10: Deviation from centerlines of corridors, a) Set One, b) Set Two. The spike observed in Fig. 5.10b, described above, is associated with corresponding spike in the values of both angles β 3 and γ 3 , observed for the agent FK 3 130 during the time interval t∈ (2.1, 2.4) seconds, and shown in the figures below. The spike indicates the attempt to compensate for deviation from the desired direction of terrain following navigation vectors. The same effect is demonstrated by observing magnitude of the forces of constraints, as follows from the results obtained with Set Two, as shown in the figures below. The results obtained with Set One are shown for comparison. The plots show data collected for the formation members. The results describing performance of the virtual leader are plotted separately, as shown in the subsequent subsections. a) b) Figure 5.11: Angles between consecutive navigation vectors, a) Set One, b) Set Two. a) b) Figure 5.12: Angles between velocity directions and navigation vectors, a) Set One, b) Set Two. 131 a) b) Figure 5.13: Magnitude of the forces of constraints, a) Set One, b) Set Two. 5.5.2 Star Formations, Modeled with Different Durations of a Time Step The presented examples illustrate how different duration of time step affect behavior of a terrain-following formation. Simulation results are obtained with two examples, denoted Star One and Star Two, corresponding to time step settings specified as dt =0.1s and dt =0.025 s, thus, the entire simulations take 50 and 200 time steps to complete, respectively. As it was previously described, the value of dt specifies sensor frequency and corresponding updates of navigation vectors for both virtual leader and formation members. At the same time, the value of dt defines time window during which terrain following and formation keeping constraints move towards converging to the values specified at the beginning of a time step. The rest of the model settings are as specified in Table 5.1. The values of stabilization coefficients and for both examples specified as µ 1 <µ 2 and λ 1 <λ 2 . Initial conditions and initial formation configuration are illustrated in the figure below. These settings and the desired formation geometry are specified as follows, 132 Q t 1 ( ) = 83.8549 54.4826 10.0 -99.8641 5.21131 10.0 77.7286 -62.9147 10.0 -25.9035 96.5868 10.0 -35.816 -93.366 10.0 ! " # # # # # # $ % & & & & & & ,  Q t 1 ( ) = 66.1516 74.9931 0.0 68.9668 72.4126 0.0 64.0882 76.764 0.0 69.2261 72.1647 0.0 65.9342 75.1843 0.0 ! " # # # # # # $ % & & & & & & (5.22) q s t 1 ( ) = 0.0 0.0 10.0 ! " # $ T ,  q s t 1 ( ) = 66.8965 74.3294 0.0 ! " # $ T (5.23) U o = 0.258819 0.965926 0.0 -0.777146 -0.62932 0.0 0.99863 0.052336 0.0 -0.838671 0.544639 0.0 0.358368 -0.93358 0.0 ! " # # # # # # $ % & & & & & & , U 1 = 0.661516 0.749931 0.0 0.689668 0.724126 0.0 0.640882 0.76764 0.0 0.692261 0.721647 0.0 0.659342 0.751843 0.0 ! " # # # # # # $ % & & & & & & (5.24) a) b) Figure 5.14: Star formation, a) Desired geometry, b) Initial configuration. Simulation results for the total flight time of 5.0 seconds, which corresponds to 3 and 12 time steps for Star One and Star Two examples, respectively, are shown in the figure below. It demonstrates the difference in orientation of constraint planes plotted at the beginning of time steps. Formation shapes are indicated by lines connecting position points of virtual leader and formation members at the beginning of each time step, illustrating evolution of formation geometry, as shown in Fig. 5.17. 133 a) b) Figure 5.15: Planes and corridor volumes, a) Star One, b) Star Two. Trajectories and formation shapes for the full simulation runtime, are shown below. Due to the difference in sensor frequency, specified by dt , simulation results for Star One and Star Two are obtained with different sets of input sensor data, resulting in different trajectory paths and different orientations of planes defining formation geometry. The terrain features, hill slopes, encountered along these paths, are not identical, although the topology of Wave terrain and position of the goal point are specified with the aim at minimizing such difference. Thus, the following comparative analysis of the results, is preformed in terms of evaluating formation behavior, rather then comparing numerical values of performance metrics directly. 134 a) b) Figure 5.16: Trajectories, a) Star One, b) Star Two. a) b) Figure 5.17: Formation shapes plotted as lines, a) Star One, b) Star Two. Bounds on the magnitude of distances between positions of virtual leader and formation members, d s,i (t) , and length of edges for the desired star geometry structure, 135 d i,i+1 (t) , defined in Eq. (5.16, 5.17), are shown in the figures below. The differences between lengths are smaller for the model with greater value of dt . The results demonstrate that the described formation keeping method implies fluctuation of actual lengths in order to provide flexibility for satisfying terrain-following constraints. a) b) Figure 5.18: Distances between positions of the virtual leader and formation members, a) Star One, b) Star Two. a) b) Figure 5.19: Distances between positions of adjoined formation members, i and i+1, a) Star One, b) Star Two. Trajectory and terrain profiles for the virtual leader and one of the formation member, FK 5 , for each of the examples, are shown in the figures below. The distances between points of trajectory profiles and surfaces of constant elevation above terrain for 136 the formation members, d e,i (t) , defined in Eq. (3.26), are shown in Fig. 5.22. Constant elevation at navigation height, h n , corresponds to the value d e =0.0 m. Dashed horizontal lines show values of z ce,i (t) , z ce1,i (t) , and z ce,i (t) , defined in Eq. (3.27-29). These profiles demonstrate that a better fit to the desired navigation height is achieved with smaller values of dt , as it would be expected following the analysis of the performance of individual terrain following agents, presented in Section 3.4.2.2. a) b) Figure 5.20: Trajectory profile of the virtual leader, a) Star One, b) Star Two. a) b) Figure 5.21: Trajectory profile of the formation member FK5, a) Star One, b) Star Two. 137 a) b) Figure 5.22: Trajectory deviation from navigation height, a) Star One, b) Star Two. The results presented above illustrate correlation between steepness of the terrain and the values of deviation of agent trajectories from the desired navigation height. Steeper terrain encountered during time interval t∈[0.0, 2.5] is associated with considerably tighter bounds of d e,i t ( ) , compared to the values for t∈ (2.5, 5.0] . Steepness of the terrain together with sensor settings are directly associated with the values of angles between consecutive navigation vectors,β i , defined in Eq. (5.19). They quantify difference between stets of sensor data obtained during consecutive sensor readings. More frequent sensor readings correspond to lesser magnitude of angles β i , as expected. It is demonstrated by comparison of the results shown in the figure below. a) b) Figure 5.23: Angles between consecutive navigation vectors, a) Star One, b) Star Two. 138 Terrain-following requirement of keeping agent trajectories within bounds of corridor volumes is violated during the second half of the flight for the example Star One, corresponding to larger duration of time step, as follows from the distances r cr,i (t) , defined in Eq. (3.30), as shown in the figure below. The horizontal line r cr =5.0 m shows the extent of bounds of cylindrical corridor volumes. a) b) Figure 5.24: Deviation from centerlines of corridors, a) Star One, b) Star Two. Set of angles between velocity directions and navigation vectors is another metric of system performance applicable to the analysis of agent behavior described above. Values of the angles γ i (t) , defined in Eq. (5.20), shown in the figure below. Distribution of the values γ i (t) among the simulation timeline outline portion of the agents paths corresponding to greater deviation between actual and the desired direction of motion, which, in turn, is correlated with the deviation of agent trajectories from the centerline of terrain-following corridor volumes. 139 a) b) Figure 5.25: Angles between velocity directions and navigation vectors, a) Star One, b) Star Two. Adherence of formation geometry to the desired shape is illustrated by observing values of the angles α i (t) , defined in Eq. (12), shown in Fig. 5.26. Considering lesser value of the stabilization coefficients µ 1 and λ 1 , associated with plane constraint, the system shows the opposite trend from the one described above. Larger value of a time step corresponds to closer fit between the actual values of the angles and the desired value of α =90°, as shown on the figure below. The same observation is supported by the distribution of numerical values of the constraint ϕ P,i (t), defined in Eq. (5.2), shown in Fig. 5.27. a) b) Figure 5.26: Angles between vectors defined by positions q s (t),q i (t) { } and vectors normal to planes, a) Star One, b) Star Two. 140 a) b) Figure 5.27: Plane constraint expression ϕ P,i (t), a) Star One, b) Star Two. These results signify greater deviation of the formation geometry from the desired star shape for the simulation performed with greater number of time steps. Both models require the same star formation shape, defined by the settings ˆ u o,i , however, actual desired configuration implied by the models differ. It is defined by vectors ˆ u i recomputed at each time step depending on the directions of discrete navigation vectors ˆ ν s , which, in turn, depend on the frequency of sensor readings. Magnitudes of velocity, external acceleration, and control forces, represented by the forces of constraints, required for achieving the described formation motion are shown on the figures below. The control forces are represented by the forces of constraints, as described in [1, 93]. Since these values define the design requirements for actuators, the presented data is a critical component in evaluation of system performance. The results show that the maximum magnitude of the force required from the actuators is comparable for both models. However, the model Star One, specified with greater value of dt , would place considerably higher requirements for actuator design in terms of the ability of changing thrust output abruptly. 141 a) b) Figure 5.28: Velocity magnitude, a) Star One, b) Star Two. a) b) Figure 5.29: External acceleration magnitude, a) Star One, b) Star Two. a) b) Figure 5.30: Forces of constraints magnitude, a) Star One, b) Star Two. 142 Following the method of estimating numerical errors described in [1], the order of magnitude of the errors, associated with the presented simulations, is comparable for both examples. This follows from the range of values of Ψ i (t) presented in the figure below. a) b) Figure 5.31: Constraint expressions Ψ i (t) , a) Star One, b) Star Two. 5.6 Examples of Trajectory Profiles Demonstrating Different Methods of Computing Navigation Vector of the Virtual Leader The following section presents examples of trajectory profiles of the virtual leader, obtained with different methods of computing navigation vector of this agent, described in Section 4.1. Illustration of the differences in the resulting trajectories of the virtual leader, is obtained with the Star formation model, described above. The Star model is selected since it represents a flexible formation, defined using plane and navigation vector constraints, instead being based on a rigid virtual body or an adjustable formation structures, described in the previous chapter. A star configuration, involving 5 formation members, is better suited for the demonstration of methods two and three, that consider partial and full sets of sensor data collected by the formation members. A greater number of agents in conjunction with Wave terrain topology allows 143 highlight navigation differences corresponding to surface elevation estimates, preformed by the virtual leader, illustrated in Fig. 4.1. The Star formation is modeled using settings specified in Table 5.1, with value of dt =0.05 s, and navigation type of formation members specified as minRay. These two settings are different from the parameters of models Star One and Star Two, presented previously. Trajectory profiles of the virtual leader, GC, and the distances between trajectory points and surfaces of constant elevation above terrain, d e,s (t) , representing trajectory deviation of GC from the desired navigation height are shown in the figures below. a) b) Figure 5.32: Method one, averaging navigation vectors, a) Profile, b) Distance. a) b) Figure 5.33: Method two, considering navigation rays, a) Profile, b) Distance. 144 a) b) Figure 5.34: Method three, considering full set of sensor data, a) Profile, b) Distance. Consideration of an estimate of terrain elevation for the virtual leader results in considerably closer fit with the surface of constant elevation, as expected, as demonstrated by the results shown in Figures 5.33, 5.34, and Fig. 5.32. That is the reason why the examples, Set and Star, One and Two, use method two for formation navigation. This argument is also supported by observing distances between the virtual leader and formation members obtained with methods one and two, as shown in the figure below. While terrain collision is permitted for the virtual leader, it is not acceptable for the formation members. Thus, the abrupt increase in the distances d s,i (t) shown in Fig. 5.35a for the time frame t∈ (4.5, 5.0] seconds. a) b) Figure 5.35: Distances between positions of the virtual leader and formation members with, a) Method One, b) Method Two. 145 5.7 Conclusion The presented evaluation of simulation results for a flexible formation geometry, achieved with plane navigation vectors constraints for formation members in conjunction with navigation vector constraints for the virtual leader, allows to conclude that these constraints are suitable for the design of a terrain following formation flight system. The preferred model settings include formation navigation methods two and three, involving computing elevation estimates for the virtual leader. Both of the reviewed types of navigation selection algorithms for the individual formation members, minRay and maxRay, are acceptable, considering correlation with expected terrain topology, as discussed with the description of the selected terrain following algorithm. A smaller value of dt , corresponding to more frequent sensor reading and navigation vector updates is preferred, as expected based on the previous evaluation of the performance of terrain following flight of independent agents. 146 CHAPTER 6: HIERARCHICAL FORMATION STRUCTURES This chapter presents design and performance analysis of multi-level, hierarchical, formation structures for the applications to terrain following formation flight. The described structures are designed following specifications and requirements for formation extension, described in the problem formulation Section 2.1 and illustrated in Fig. 2.4. The formation structures are described as trees, graphs, where nodes represent formation members, as described in Section 2.2 and shown in Fig. 2.10. The rational for limiting the depth of the formation hierarchy and avoiding formation restructuring, specific to flocking behavior, is described in Section 2.2.2 and illustrated in Fig. 2.11. Considering these specifications for the desired formation design, the suggested structures are two-level formations, where the top level consists of a single virtual leader agent, denoted as GC, the second level consists of virtual leaders for formation primitives, denoted as GC i , and formation primitives are triangular formation blocks representing actual aircraft, denoted as FK ij for the i th primitive and j th formation member. 6.1 Sets of Constraints for Two-Level Formations The following graph describe sets of constraints used to create and maintain the desired formation configuration. Nodes represent agents, as described above, and edges represent sets of constraints, defined with expressions Ψ k for the k th element, where k∈{GC,GC i ,FK ij } . Three formation designs are considered. They share formation structure, defined with leader follower type of connections between nodes that belong to different levels of the hierarchy. However, the sets of constraints used to implement these 147 connections are different. The types of corresponding expressions of constraints for the connections at the same level of the hierarchy are identical, such that Ψ GC,i ≡Ψ GC Level2 and Ψ FK,ij ≡Ψ FK , as specified in the diagram and the table shown below. Figure 6.1: Formation structure. Constraints Type of Ψ GC Top Type of Ψ GC Level2 Type of Ψ FK Set One Line Ψ LZ Plane and Line Ψ PLZ Plane and Line Ψ PLZ Set Two Adjustable Offset Ψ DX Set Three Adjustable Offset Ψ DX Plane and Line Ψ PLZ Table 6.1: Sets of formation constraints. Constraints and corresponding equations of motion, denoted as DX, LZ, and PLZ for Ψ DX , Ψ LZ , and Ψ PLZ , are defined in Section 4.3.1, Eq. (4.11, 4.15), Appendix A, Eq. (A.4, A.9-11), and Appendix B, Eq. (B.2, B.7-9), respectively. The model denoted as Set One implies using a plane and navigation vector constraints, designed for achieving a flexible formation geometry, at both levels of formation hierarchy GC-GC i and GC i -FK ij , while navigation vector constraint for top level GC represents formation guidance, as described in the table above. Therefore, Set One, presents an example of applying flexible geometry constraints to multi-level formation design. Thus, considerable changes in formation geometry for obstacle GC 1 FK 12 FK 11 FK 13 GC 2 FK 22 FK 21 FK 23 GC Ψ GC Level2 Ψ FK Ψ GC Top Ψ GC Level2 Ψ FK Ψ FK Ψ FK 148 avoidance are expected, as follows from the description and analysis of flexible formations presented in the previous chapter. The model denoted as Set Two uses a set of an adjustable positioning offset constraints to maintain geometry of a triangular primitives, replacing plane and navigation vector constraints at the bottom level of formation hierarchy, GC i -FK ij . Thus, it implies having an adjustable virtual rigid body structure at that level, as follows from the description of adjustable formation geometry presented in Section 4.3, while second- level connections GC-GC i represent flexible formation geometry. It shall be noted that such structural elements consider changes of orientation of the virtual structures, since the positions are defined relative to the direction of navigation vector of the second level virtual leaders, GC i , as follows from the description of positioning offsets presented in Section 4.2. The adjustment terms for positioning offset constraints, ξ i (t) , are defined as heuristic functions, using computation procedure described in Section 4.3.2. They are specified as linear or nonlinear interpolating functions, as defined in Eq. (4.27, 4.28). The model denoted as Set Three uses a set of an adjustable positioning offset constraints at the second level of formation hierarchy, GC-GC i , while formation primitives, GC i -FK ij , are defined with plane and navigation vector constraints. The adjustment components of positioning offsets are defined as described above. It shall be noted that a formation defined using an adjustable positioning offset constraints at both levels of hierarchy, GC-GC i and GC i -FK ij , is not considered. Such structure is expected to have all of the properties of an adjustable virtual rigid body 149 structure, considered as not suitable for a terrain following formation design, as stated in the conclusion of Chapter 4. Therefore, with Set Two and Set Three, the resulting formation geometry is expected to display properties of both formation types, flexible structure and adjustable rigid body. The evolution of formation configuration during the motion is expected to be governed by the geometrical size of formation elements, specified by relative distances between virtual agents GC-GC i and the desired positioning offsets and GC i -FK ij . The offsets GC i -FK ij implicitly specify relative positions among formation members that belong to the same primitive, FK ij -FK ik , where j, k = 1, 2, 3 and j≠ k . 6.2 Simulation Results The presented simulation results demonstrate behavior of hierarchical, two-level, terrain following formation flight systems described in the previous section. The differences between performance of formations designed with different sets of constraints are outlined. It shall be noted analysis of performance of hierarchical formations correlates with the analysis of a single level formations, presented in Sections 3.4.2, 4.4.2, and 5.5. The model settings corresponding to formations denoted as Set One, Two, and Three are specified as follows. Formation structure is shown in Fig. 6.1, with state matrices of the agents denoted as Q GC (t) , Q GC,i (t) , and Q FK,ij (t) for the agents GC, GC i , and FK ij , respectively. Formation constraints and corresponding equations of motion for agent models are shown in Table 6.1, with coefficients for external forces specified in Eq. (3.51). The duration of a time step is specified as dt =0.05 s. Simulation time interval 150 includes 50 steps, t∈[0.0, 2.5] seconds, with initial time specified as t 1 =0.0 s. The values of stabilization coefficients for different types of constraints are shown in the table below. Set Ψ GC Top Ψ GC Level2 Ψ FK µ λ µ 1 µ 2 λ 1 λ 2 µ 1 µ 2 λ 1 λ 2 One 6.0 8.0 6.0 8.0 8.0 10.0 3.0 8.0 5.0 10.0 Two 6.0 8.0 Three 6.0 8.0 3.0 8.0 5.0 10.0 Table 6.2: Stabilization coefficients. Terrain model is specified as Hills, defined in Eq. (3.47), where ω tr,1 = 0.008 , ω tr,2 = 0.012 , α tr =16.0m , and position of the goal point is p g = 1800.0 4000.01.0 [ ] T in meters. Such settings create Hills terrain with the size of a terrain features, hills and canyons, comparable with the desired size of a formation, but smaller then the desired size of an individual formation primitive, as follows from terrain and trajectory plots presented in the following subsections. The desired navigation height and the diameter of terrain following corridor volumes are h n =25.0m and γ cr =10.0m , respectively, as specified in Eq. (3.49). Sensor settings for formation members are specified as n c =4 , α r =30° , β r =40° , α d =1.5° , and r max =200.0m . The selected navigation type for formation members, FK ij , is specified as maxRay. Navigation for both levels of the virtual leaders, GC and GC i , is specified as method two, considering navigation rays of formation members, with scaling coefficient in Eq. (4.3) specified as d s =6.0 . Thus, the selected formation navigation and guidance method considers estimates of terrain elevation for computing navigation vectors of virtual leaders. 151 Initial conditions and settings specifying the desired formation configuration for all of the formation sets are shown below. Relative initial positions of formation members from the same triangular primitive match the respective relative positions defined in Eq. (4.29). The desired formation configuration is defined with the set of the desired positioning offsets, denoted as Δ o , and sets of vectors normal to planes, U o , for the DX, and PLZ constraints, as defined in Section 4.2 and 5.1, respectively. The values of these settings are identical for both of the formation primitives, FK 1j and FK 2j . The values of Δ o,FK are specified in Eq. (4.31), and the values of U o,FK are specified in Eq. (5.21). The settings for the second-level virtual leaders GC 1 and GC 2 assume that the position of the top-level GC coincides with the origin p o = 000 [ ] T and the direction of navigation vector coincides with ˆ x axis = 10 0 [ ] T . The values are specified as follows, Δ o,GC1 = −28.2842 98.9949 0.0 # $ % & T Δ o,GC2 = −28.2842 −98.9949 0.0 # $ % & T (6.1) ˆ u o,GC1 = 0.9615 0.2747 0.0 ! " # $ T ˆ u o,GC2 = 0.9615 −0.2747 0.0 ! " # $ T (6.2) Initial conditions include identical sets of values of initial velocity for agents that belong to triangular formation primitives 1 and 2. Initial positions of formation members are set by applying offsets to the base configuration. Thus, relative initial positions between agents are identical for the primitives 1 and 2. The matrices of initial positions and velocities of n fp formation members of the i th formation primitive, where n fp =3 for a 152 triangular primitive, are defined as Q FK,i t 1 ( ) = q FK,i1 t 1 ( ) T ,...,q FK,in fp t 1 ( ) T ! " # $ T and  Q FK,i t 1 ( ) =  q FK,i1 t 1 ( ) T ,...,  q FK,in fp t 1 ( ) T ! " # $ T . The desired, base, and initial formation configuration are shown in the figures below. The initial conditions are specified as, Q FK,1 t 1 ( ) = 42.84 236.62 10.0 121.75 258.62 10.0 115.43 176.95 10.0 ! " # # # $ % & & & ,  Q FK,1 t 1 ( ) = 42.3066 90.6096 0.7833 40.9274 91.2409 0.7805 40.3225 91.5099 0.7845 ! " # # # $ % & & & (6.3) Q FK,2 t 1 ( ) = 182.84 96.62 10.0 261.75 118.62 10.0 255.43 36.95 10.0 ! " # # # $ % & & & ,  Q FK,2 t 1 ( ) = 38.2749 92.385 0.7869 36.8435 92.9651 0.7844 36.3136 93.1734 0.7884 ! " # # # $ % & & & (6.4) q GC1 t 1 ( ) = 93.34 224.063 10.0 ! " # $ ,  q GC1 t 1 ( ) = 41.1866 91.1242 0.7828 ! " # $ q GC2 t 1 ( ) = 233.34 84.063 10.0 ! " # $ ,  q GC2 t 1 ( ) = 37.1448 92.8451 0.7866 ! " # $ q GC t 1 ( ) = 183.34 174.063 10.0 ! " # $ ,  q GC t 1 ( ) = 38.9229 92.1138 0.7833 ! " # $ (6.5) a) b) Figure 6.2: The desired formation configuration, a) Primitive, b) Level-2 GC’s. 153 Figure 6.3: Initial positions and navigation vectors for the 1 st time step. Sensor rays and corresponding terrain surface estimates for GC’s and navigation rays for all of the agents, obtained for the 1 st time step with the settings described above, are shown in Fig. 6.4 and 6.5. Terrain surface elevation estimate obtained with method three, considering full sets of sensor data collected by formation members, are shown in Fig. 6.6, for comparison. Surface estimates are visualized as horizontal disks, associated with elevation values denoted as h s,i . The results show that consideration of full sets of sensor data, implied by navigation method three, provides a closer fit between the estimated and actual value of surface elevation, as expected. However, both of the methods are applicable to formation modeling since virtual leaders are not required to achieve terrain collision free motion. It shall be noted that the simulation results presented below are obtained with a relatively large duration of a time step, dt . Such settings specify an exaggerated terrain following model, in terms of terrain collision avoidance, required evolution of formation geometry, and corresponding magnitude of the control forces, as follows from the analysis of terrain following flight presented in Section 4.4.3. 154 Figure 6.4: Sensor rays and navigation vectors for the 1 st time step. Figure 6.5: Elevation estimates and navigation rays with navigation method two. Figure 6.6: Elevation estimates and navigation rays with navigation method three. 155 6.2.1 Formation Defined with Planes and Navigation Vectors Constraints The following simulation results present formation flight system defined with Set One constraints, as shown in Table 6.1, considering model settings described above. The resulting trajectories and evolution of formation geometry is shown in the figures below. The results show that the formation members are able to achieve collision free motion using constraints the type of constraints designed for a flexible formation, Ψ PLZ , at both level of formation hierarchy, presented at Fig. 6.1. Figure 6.7: Terrain and trajectories. The following set of figures show different views visualizing the same simulation results. They demonstrate changes of geometrical shape of individual formation primitives, shown as triangles FK 1 and FK 2 , and present the difference in elevation of second level virtual leaders GC 1 and GC 2 . The triangular primitives are oriented along the hill slopes, “hugging” the terrain, as follows from the results shown in Fig. 6.8. The sizes of the formation primitives, defined by relative distances between the agents, change during the motion while retaining the desired triangular shape, defined with the formation configuration settings illustrated in Fig. 6.2, as follows from the results shown in Fig. 6.9. Therefore, it is possible to conclude that terrain following flight of a flexible formation is achieved with this model. 156 Figure 6.8: Formation shape. Figure 6.9: Additional view of formation shape. Plots of deviation of trajectory points for the desired navigation height along trajectory profiles of the agents, d e,FK,ij (t) , d e,GC (t) and d e,GC,i (t) computed as defined in Eq. (3.26), are shown in Fig. 6.10. Dashed horizontal lines correspond to the surfaces of constant elevation above terrain, as defined in Eq. (3.27-29). The results describing deviation of agent trajectories from centerlines of cylindrical terrain following corridor volumes, r cr,FK,ij (t) , computed as defined in Eq. (3.30), are shown in Fig. 6.11a. The horizontal lines represents the bounds of cylindrical volumes. The results demonstrate adherence of trajectories of formation members to the 157 bounds of the corridors, by showing that the curves representing the distances are below the horizontal line r= γ cr 2 for all of the formation members, as described in Eq. (3.31). The corresponding plot of magnitudes of the forces of constraints of formation members is shown in Fig. 6.11b. The values of F FK,ij c (t) represent control forces, as described in [1, 2]. Large magnitude of these values combined with the requirement to change force magnitude abruptly, are attributed to settings a highly exaggerated model, as described in the previous section. a) b) Figure 6.10: Trajectory deviation from navigation height, a) FK’s, b) GC’s. a) b) Figure 6.11: Formation members, FK’s, a) Deviation from centerlines of corridors, b) Forces of constraints magnitude. 158 The following plots of values of expressions of constraints Ψ FK,ij (t) , Ψ GC (t) , and Ψ GC,i (t) are shown in the figure below. These values quantify numerical errors in solving equations of motion for the agents, as follows from the description presented in [1]. The plots show that the order of magnitude of the errors associated with computations for formation members and virtual leaders are comparable. a) b) Figure 6.12: Constraint expressions Ψ i (t) , a) FK’s, b) GC’s. 6.2.2 Formations Defined with Addition of Adjustable Positioning Offset Constraints The following simulation results demonstrate performance of formations defined with Set Two and Set Three constraints, considering using adjustable relative positions offsets constraints, Ψ DX . These are used in the definition of formation primitives GC i - FK ij and the second level GC-GC i connections, for Set Two and Set Three, respectively, as described in Table 6.1. The adjustment heuristics terms, ξ i (t) , are specified as xiNonlinear, defined in Eq. (4.27, 4.28). The values of a set of scaling coefficients c ξ1 , c ξ2 , c ξ3 { } , used to assemble a set of points for interpolation, are specified as {1.2, 1.65, 2,2} and {0.0, 0.75, 159 1.0} for Set Two and Set Three, respectively. Another difference in the models settings are the diameters of cylindrical corridor volumes, γ cr , considered for computations of ξ i (t) , as defined in Eq. (4.25). Set Two uses the adjustment terms in the definition of constraints for formation members, representing actual aircraft, FK ij . Thus, corresponding corridor diameters are set to 10.0 m, as specified in the previous section, in accordance to the requirements of the selected terrain following algorithm. Set Thee uses adjustable positioning offsets for the constraints connecting two levels of virtual leaders, GC and GC i . While motion of these agents is correlated with the terrain topology, virtual leaders are not required to perform collision free flight, as discussed in Section 4.1. Therefore, the diameter of terrain following corridor for Set Three is specified as γ cr =20.0 m. A greater value of γ cr is selected attempting to minimize or avoid having non-zero adjustment terms, providing a smoother trajectory of virtual leaders and avoiding an abrupt increase in the magnitude of acceleration of these agents. Such conditions are associated with a non-zero values of ξ i (t) for some of the time steps, following the comparison of rigid and adjustable formations presented in Section 4.4.3. Evolution of formation shapes for both models are shown in the figures below. Two views for each of the examples are displayed in order to demonstrate changes in elevation and orientation of formation components, together with changes in the shapes of formation primitives. The plots show that (a) flexible geometry constraints placed at GC-CG i connections provide closer fit between terrain surface and trajectories of virtual leaders, as specified for Set Two, as follows from Fig. 6.13, (b) triangular formation 160 primitive maintain the desired shape more closely with the model Set Two, as follows from Fig. 6.14. However, considering the previous example, Set One, it is possible to conclude that better fit with the desired formation shapes, observed with Set Two, is attributed to flexible geometry constraints of GC-CG i connections, instead of an adjustable components at the level GC i -FK ij . a) b) Figure 6.13: Formation Shape, a) Set Two, b) Set Three. a) b) Figure 6.14: Additional views of formation shape, a) Set Two, b) Set Three. 161 While Set Two allows maintaining the desired formation geometry, it is associated with a sharp bends of agent trajectories and corresponding peaks in the magnitude of the forces of constraints, representing the required control forces, as follows from the Figures 6.15a and 6.17a shown below. Such behavior is expected from an adjustable virtual rigid body components, as demonstrated in Chapter 4. Therefore, although the requirements of keeping agent trajectories within the bound of terrain following corridor volumes are maintained for Set Two, it might not be considered as a suitable formation design, due to extremely large demand for the actuators force output. a) b) Figure 6.15: Trajectory deviation from navigation height for FK’s, a) Set Two, b) Set Three. a) b) Figure 6.16: Deviation from centerlines of corridors for FK’s, a) Set Two, b) Set Three. 162 a) b) Figure 6.17: Forces of constraints magnitude, a) Set Two, b) Set Three. The presented simulation results demonstrate the use of an adjustable positions offsets in place of plane and navigation vector constraints at the different levels of formation hierarchy results in a sharply different behavior of the resulting formations. 6.2.3 A Case of Terrain Collision with the Design that Uses Linear Heuristic Function in the Definition of Adjustable Positioning Offsets The following example demonstrates that selecting an unsuitable heuristic function for the definition of adjustable constraints, results in the collisions of formation members with the terrain. The term “unsuitable” refers to inconsistency between type of the function or numerical values of coefficients used to obtain it and the rest of the model settings. The type of the selected heuristics term, ξ i (t) , is specified as xiLinear, defined in Eq. (4.27, 4.28), considering the values of scaling coefficients specified as c ξ1 =0.0 and c ξ2 =1.2. It shall be noted that since ξ i (t) is supposed to compensate for an error between the ideal constant Δ i and current formation offsets, by doing linear interpolation we implicitly assume linear accumulation of this error along the trajectory corresponding to 163 the current, n th , time step, t∈ t n , t n +dt ( ) . That assumption may not hold true in all cases. Thus, selecting a linear type of heuristic function might not be considered as a suitable choice, regardless of the values specified for scaling coefficients. The presented example also illustrates a case when all of the formation member belonging to a triangular primitive, FK 2 , exit the formation, leading to the exit of the corresponding second level virtual leader, GC 2 . The remaining agents continue flight without reducing formation hierarchy, since the top level virtual leader, GC, continues its motion. Trajectories of the agents, changes in formation structure, and evolution of formation shape are shown in the figures below. Figure 6.18: Terrain and trajectory. Figure 6.19: Evolution of formation shape. 164 The differences between elevation above terrain surface of the agent’s trajectory points and the desired navigation height, computed along the trajectory profiles of the agents, are shown in Fig. 6.20. The curves d e,i (t) are discontinued for the agents colliding with the terrain. The distances between trajectory points of the formation members , FK ij , and centerlines of terrain following corridor volumes are shown in Fig. 6.21a. Minimum and maximum bounds of magnitudes of the forces of constraints for the same agents are shown in Fig. 6.21b, where the values corresponding to the formation members FK 11 , FK 12 , FK 13 , FK 21 , FK 22 , FK 23 are denoted with the indices i=1,..,6, respectively. a) b) Figure 6.20: Trajectory deviation from navigation height, a) FK’s, b) GC’s. a) b) Figure 6.21: Formation members, FK ij , a) Deviation from centerlines of corridors, b) Min and max values of magnitude of the forces of constraints. 165 The results shown in Fig. 6.21a demonstrate that a terrain collision of an formation member is associated with violation of the bounds of the safe corridors for several consecutive time steps. Corresponding sharp decline of distances d e,i (t) , shown in Fig. 6.20a, indicates an abrupt drop in elevation of these agents. 6.3 Conclusion The presented structures of multi-level terrain following formations, combined with the simulation results and performance evaluation, allow to conclude that the use of a flexible geometry constraints at all of the levels of formation hierarchy is required to achieve the desired evolution of formation shape while keeping the required magnitude of the forces of constraints of formation members at an acceptable level. The results also show that while use of an adjustable virtual rigid body structures at the level of formation primitive is possible. However, using these structures at the level connecting virtual leaders does not result in maintaining the desired formation geometry. 166 CHAPTER 7: FLEXIBLE FORMATION GEOMETRY DEFINED WITH PLANES AND SURFACES OF REVOLUTION CONSTRAINTS This chapter describes constraint based flexible formation geometry defined with planes and surfaces of revolution, considering parabolic, cone, and cylindrical surfaces. Formation structure and configuration are defined following method for a achieving a flexible formation for the design of terrain following formation flight systems described in Chapter 5, with the difference in the types of terrain following constraints for formation members. This chapter evaluates the technique of using the constraint requiring agents to belong to surfaces of revolution. These surface constraints substitute terrain following constraints on following a line defined with navigation vector, while formation keeping constraints are defined with plane surfaces, designed to maintain flexible formation configuration, as described in Chapter 5. Such structure of the constraints is defined as an attempt to bound motion of formation members to regions of space defined by corridor volumes. Surfaces of revolution are selected since they represent geometric structures that fit within cylindrical volumes. Main axis of the surfaces for the n th time step, coincide with main axis of the corridors for this step. Position and navigation vector of an agent at the beginning of a time step specify position and orientation of a surface. Transformations of input parameters and inverse transformations of the resulting state matrices are performed between global coordinate system of the model, denoted a XYZ, and local coordinate system of a surface for the i th formation member for the current, n th , time step, denoted as xyz i , as illustrated in the figure below. 167 Figure: 7.1 Plane, surface of revolution, navigation vectors and corridor volume. Expressions for surface constraints, defined in xyz i for the i th agent, are denoted as ϕ PR,i (t) , ϕ CN,i (t) , and ϕ CL,i (t) for parabolic, cone, and cylindrical surfaces, respectively. Corresponding expressions derived with application of Baumgarte stabilization technique, following Eq. (3.15), are denoted as ψ PR,i (t), ψ CN,i (t), ψ CL,i (t) . Then, full sets of constraints for formation members are defined with addition of plane constraint, defined with expressions ϕ P,i (t) and ψ P,i (t) , Eq. (5.2-5), considering transformation of the parameters in xyz i . These sets are denoted as Φ PPR,i (t) , Φ PCN,i (t) , Φ PCL,i (t) , and Ψ PPR,i (t) , Ψ PCN,i (t) , Ψ PCL,i (t). 7.1 Transformation of System Parameters Expressions for constraints, corresponding equations of motion, and the resulting state matrices of the i th formation member for the n th time step, Q i (t)≡Q xyz,i (t) , are obtained in xyz i , while state matrix of the virtual leader, Q XYZ,s (t) , and expressions of external acceleration, navigation vectors, and vectors normal to planes are specified in XYZ. ˆ ν s ˆ ν i γ cr q i t n ( ) q s t n ( ) z x y X Y Z 168 Transformations from XYZ to xyz i are performed using operator Γ xyz =Γ Τ xyz ( ) , where Τ xyz ≡Τ XYZ→xyz denotes transformation matrix. Operator Γ xyz represents composition of translation and rotation operators, denoted as Γ xyz TR and Γ xyz RT , respectively. Γ xyz TR is defined by translation of point p XYZ,i =q i t n ( ) into the origin of xyz i . Γ xyz RT is defined by rotation of vector ˆ ν XYZ,i into the direction of ˆ x axis around the origin of xyz i . Transformations from xyz i to XYZ are performed using operator Γ XYZ =Γ Τ XYZ ( ) , where matrix Τ XYZ ≡Τ xyz→XYZ is specified as inverse transform Τ xyz −1 . Since the value of navigation vector ˆ ν i ≡ ˆ ν xyz,i remains constant for the duration of n th time, corresponding transformation matrices Τ xyz,i and Τ XYZ,i are constant for t∈ t n , t n +dt ( ) . Translation and rotation operators are defined as follows, Γ xyz,i TR =Γ TR −p XYZ,i ( ) , Γ xyz,i RT =Γ RT ˆ ν XYZ,i , ˆ x axis ( ) (7.1) Γ xyz,i =Γ xyz,i TR ∪Γ xyz,i RT , Γ XYZ,i =Γ Τ −1 xyz,i ( ) (7.2) Transformations of input parameters include transforming direction vectors for initial velocity and accounting for the directions of gravity and thruster forces. Direction of movable thruster force, F tm,i (t) , is defined by the direction of ˆ ν i . Initial velocity, v o,i , and direction of gravity force, ˆ ν g,i , for the i th formation member for the n th time step are computed as follows, v o,i =Γ xyz,i RT  q XYZ,i (t n )  q XYZ,i (t n ) " # $ $ % & ' ' , v o,i =  q XYZ,i (t n ) v o,i v o,i (7.3) 169 ν g,i =Γ xyz,i RT ˆ ν XYZ,g ( ) , ˆ ν g,i = ν g,i ν g,i (7.4) Constraints defined by planes and surfaces of revolution require transformations of vectors normal to plane ˆ u XYZ,i and elements of state matrix of virtual leader Q XYZ,s (t) from XYZ to xyz i . Position vector is transformed from q XYZ,s (t) to q s (t)≡q xyz,i,s (t) using operator Γ xyz,i . Velocity  q s (t)≡  q xyz,i,s (t) and acceleration  q s (t)≡  q xyz,i,s (t) components are computed using numerical differentiation of q s (t) . This operation is feasible to implement since components of q s (t) are represented by differentiable continuous interpolating functions of time t . Inverse transforms xyz i to XYZ required for obtaining state matrices Q XYZ,i (t) and computing the resulting control forces F XYZ,i c (t) are performed following similar procedure. u i =Γ xyz,i RT ˆ u XYZ,i ( ) , ˆ u i = u i u i (7.5) q s (t)=Γ xyz,i q XYZ,s (t) ( ) ,  q s (t)= ∂ ∂t q s (t),  q s (t)= ∂ ∂t  q s (t) (7.6) q XYZ,i (t)=Γ XYZ,i q i (t) ( ) ,  q XYZ,i (t)= ∂ ∂t q XYZ,i (t),  q XYZ,i (t)= ∂ ∂t  q XYZ,i (t) (7.7) 7.2 Expressions for Surface Constraints Expression for holonomic constraints, ϕ PR,i , ϕ CN,i , and ϕ CL,i , are derived directly from the equations of respective surfaces in standard from, described in [102]. Then, the expressions for the constraints , ψ PR,i , ψ CN,i , and ψ CL,i , are derived considering constant 170 stabilization coefficients µ i and λ i . The expressions are defined in local coordinate system of the surfaces, xyz i , where ˆ x axis is the main, revolution, axis of a surface. Position of the i th formation member at the beginning of the n th time step coincides with the origin of xyz i , denoted as p o = p i ≡q i t n ( ) , since q xyz,i t n ( ) ≡ 0 0 0 [ ] T , as follows from the transformation procedure defined in Eq. (7.1). Apex of an elliptic paraboloid, a cone, and point positioned on the main axis of a cylinder is denoted as p a , where p a ≠ p o , indicating that motion of an agent starts from inside the volume bounded by the respective surface. Considering these specifications, the expressions for surface constraints are defined as follows. Parabolic surface constraint is defined based on the equation of elliptic paraboloid with scaling coefficients in y and z dimensions denoted as a p and b p respectively, Eq. (7.8), [102]. Considering the desired symmetry of the surface with respect to ˆ x axis , a single coefficient, c p , specifying size of elliptic paraboloid, is used. The value of c p is selected considering model parameters, such as diameter and estimated length of cylindrical corridor volumes, γ cr and λ cr,i , respectively. Point p a is moved in the negative direction on ˆ x axis , with the magnitude of this translation specified by constant parameter δ x . Expressions for constraints ϕ PR,i (t)= 0 and ψ PR,i (t)= 0 are derived as follows, x= y 2 a p 2 + z 2 b p 2 , x=c p y 2 +c p z 2 , wherec p = 1 a p 2 , with a p =b p (7.8) x+δ x =c p y 2 +c p z 2 (7.9) 171 ϕ PR,i (t)= q x,i (t)+δ x −c p q y,i (t) 2 +q z,i (t) 2 ( ) (7.10)  ϕ PR,i (t)=  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) (7.11)  ϕ PR,i (t)=  q x,i (t)−2c p  q y,i (t) 2 +  q z,i (t) 2 +q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) (7.12) ψ PR,i (t)=λ i q x,i (t)+δ x −c p q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +µ i  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) −2c p  q y,i (t) 2 +  q z,i (t) 2 ( ) +  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) (7.13) a) b) Figure 7.2: Parabolic surface, a) Position and orientation, b) Model. Cone surface constraint is defined based on the equation of elliptic cone with scaling coefficients in x , y , and z dimensions are specified as a cn , b cn , and c cn respectively, [102]. A single coefficient, c n , is used due to symmetry of the surface with respect to ˆ x axis . Apex of the surface is translated by constant value specified by δ x . Then expressions for constraints ϕ CN,i (t)= 0 and ψ CN,i (t)= 0 are derived using the procedure described above, as follows, x 2 a cn 2 = y 2 b cn 2 + z 2 c cn 2 , x 2 =c n y 2 +c n z 2 , withc n = a cn 2 b cn 2 , whereb cn =c cn (7.14) z x y δ x p a p i ˆ ν i 172 x+δ x ( ) 2 =c n y 2 +c n z 2 (7.15) ϕ CN,i (t)= q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) (7.16)  ϕ CN,i (t)= 2 q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.17)  ϕ CN,i (t)= 2  q x,i (t) 2 −c n  q y,i (t) 2 +  q z,i (t) 2 ( ) ( ) +2 q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.18) ψ CN,i (t)=λ i q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +2µ i q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) +2  q x,i (t) 2 −c n  q y,i (t) 2 +  q z,i (t) 2 ( ) ( ) +2 q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.19) a) b) Figure 7.3: Cone surface, a) Position and orientation, b) Model. Cylindrical surface constraint is derived directly from equation of a cylinder, [102], with the radius denoted as r c . Model of a cylinder used for derivation of constraint ϕ CL,i (t) considers translation of position of main cylinder axis relative to ˆ x axis of xyz i , as illustrated in the figure below. Cylinder axis is moved in the negative direction of ˆ z axis with translation magnitude specified by a constant parameter δ z . This adjustment allows z x y δ x p a p i ˆ ν i 173 avoiding a condition when denominator in equations of motion becomes zero at the beginning of a time step, t n , when p i = p o . With the translation c PCL,i (t n )≠ 0 , as follows from Eq. (7.48) presented in the subsequent subsection. Considering this adjustment, expressions for constraints ϕ CL,i (t)= 0 and ψ CL,i (t)= 0 are derived as follows, y 2 +z 2 =r c 2 (7.20) y 2 + z+δ z ( ) 2 =r c 2 (7.21) ϕ CL,i (t)=q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 (7.22)  ϕ CL,i (t)= 2 q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) (7.23)  ϕ CL,i (t)= 2  q y,i (t) 2 +  q z,i (t) 2 +q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) (7.24) ψ CL,i (t)=λ i q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 ( ) +2µ i q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) +2  q y,i (t) 2 +  q z,i (t) 2 ( ) +2 q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) (7.25) a) b) Figure 7.4: Cylindrical surface, a) Position and orientation, b) Model. z x y δ z p i ˆ ν i p a 174 7.3 Constraint Equations and Corresponding Equations of Motion Constraint equations and equations of motion for combined sets of is derived using the Udwadia-Kalaba approach, [1, 2], as described in Section 3.1, and applied for derivations using plane and navigation vector constraints, presented in Section 5.2. Expressions Ψ PPR,i , Ψ PCN,i , Ψ PCL,i are defined using identical values of stabilization coefficients for all of the formation members, µ i =µ∧λ i =λ∀ i=1,..,n f . The full sets include expression for plane surface constraint, ψ P,i , defined in Eq. (5.5), and parabolic, cone, and cylindrical surface constraints, ψ PR,i , ψ CN,i , ψ CL,i , defined in Eq. (7.13, 7.19, 7.25), respectively. The term b P,i is defined in Eq. (5.9). Equations of motion are derived following Eq. (3.7-15). The examples, shown in the figures below, demonstrate application of the derived equations of motion to a leader-follower system, modeled without terrain and sensors. Navigation vectors for both leader and follower are generated by rotating a base direction vectors by predefined angles, as described in Section 5.4 presenting a similar test for plane and navigation vector constraints. The following equations of motion, corresponding to plane and parabolic surface constraints, are derived using term denoted as c PPR,i (t) , d PPR,i (t) , and f P,i (t) . Φ PPR,i (t)= u x,i q x,i (t)−q sx (t) ( ) +u y,i q y,i (t)−q sy (t) ( ) +u z,i q z,i (t)−q sz (t) ( ) q x,i (t)+δ x −c p q y,i (t) 2 +q z,i (t) 2 ( ) # $ % % % & ' ( ( ( (7.26) Ψ PPR,i (t)= ψ P,i (t) ψ PR,i (t) " # $ $ % & ' ' ,withµ i = µ 1 0 0 µ 2 " # $ $ % & ' ' ,λ i = λ 1 0 0 λ 2 " # $ $ % & ' ' (7.27) 175 A PPR,i = −u x,i −u y,i −u z,i −1 2c p q y,i (t) 2c p q z,i (t) " # $ $ % & ' ' , b PPR,i = b P,i b PR,i " # $ $ % & ' ' , where b PR,i =λ 2 q x,i (t)+δ x −c p q y,i (t) 2 +q z,i (t) 2 ( ) ( ) −2c p  q y,i (t) 2 +  q z,i (t) 2 ( ) +µ 2  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.28) c PPR,i (t)=u y,i 2 +u z,i 2 +4c p u x,i u z,i q z,i (t)+u y,i q y,i (t) ( ) +4c p 2 u x,i 2 +u z,i 2 ( ) q y,i (t) 2 + u x,i 2 +u y,i 2 ( ) q z,i (t) 2 −2u y,i u z,i q y,i (t)q z,i (t) ( ) (7.29) A PPR,i .M − 1 2 " # $ % & ' + = m c PPR,i (t) a 11 a 12 a 21 a 22 a 31 a 32 ( ) * * * * + , - - - - a 11 =−2c p u y,i q y,i (t)+u z,i q z,i (t) ( ) +2c p u x,i q y,i (t) 2 +q z,i (t) 2 ( ) ( ) a 12 =−2c p u x,i u y,i q y,i (t)+u z,i q z,i (t) ( ) −q y,i (t) 2 −q z,i (t) 2 a 21 =−u y,i −2c p u x,i q y,i (t)+2c p u y,i q z,i (t) 2 −u z,i q y,i (t)q z,i (t) ( ) ( ) a 22 =u x,i u y,i +2c p u x,i 2 +u z,i 2 ( ) q y,i (t)−u y,i u z,i q z,i (t) ( ) a 31 =−u z,i −2c p u x,i q z,i (t)+2c p u z,i q y,i (t) 2 −u y,i q y,i (t)q z,i (t) ( ) ( ) a 32 =u x,i u z,i +2c p u x,i 2 +u y,i 2 ( ) q z,i (t)−u y,i u z,i q y,i (t) ( ) (7.30) d PPR,i (t)=a x,i (t)−2c p a y,i (t)q y,i (t)+a z,i (t)q z,i (t) ( ) +λ 2 δ x +q x,i (t)+c p q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +2c p  q y,i (t) 2 +  q z,i (t) 2 ( ) +µ 2  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.31) f P,i (t)=u x,i a x,i (t)−  q sx (t) ( ) +u y,i a y,i (t)−  q sy (t) ( ) +u z,i a z,i (t)−  q sz (t) ( ) +λ 1 u x,i q x,i (t)−q sx (t) ( ) +u y,i q y,i (t)−q sy (t) ( ) +u z,i q z,i (t)−q sz (t) ( ) ( ) +µ 1 u x,i  q x,i (t)−  q sx (t) ( ) +u y,i  q y,i (t)−  q sy (t) ( ) +u z,i  q z,i (t)−  q sz (t) ( ) ( ) (7.32)  q x,i (t)=a x,i (t)− 1 c PPR,i (t) d PPR,i (t) u y,i 2 +u z,i 2 +2c p u x,i u y,i q y,i (t)+u z,i q z,i (t) ( ) ( ) +2c p f P,i (t) u y,i q y,i (t)+u z,i q z,i (t) ( ) +2c p u x,i q y,i (t) 2 +q z,i (t) 2 ( ) " # $ $ % & ' ' " # $ $ $ $ % & ' ' ' ' (7.33) 176  q y,i (t)=a y,i (t)+ 1 c PPR,i (t) d PPR,i (t) u x,i u y,i +2c p u x,i 2 +u z,i 2 ( ) q y,i (t)−u y,i u z,i q z,i (t) ( ) ( ) +f P,i (t) u y,i +2c p u x,i q y,i (t) ( ) −4c p 2 u z,i q y,i (t)q z,i (t)−u y,i q z,i (t) 2 ( ) " # $ $ % & ' ' " # $ $ $ $ $ % & ' ' ' ' ' (7.34)  q z,i (t)=a z,i (t)+ 1 c PPR,i (t) d PPR,i (t) u x,i u z,i +2c p u x,i 2 +u y,i 2 ( ) q z,i (t)−u y,i u z,i q y,i (t) ( ) ( ) +f P,i (t) u z,i +2c p u x,i q z,i (t) ( ) −4c p 2 u y,i q y,i (t)q z,i (t)−u z,i q y,i (t) 2 ( ) " # $ $ % & ' ' " # $ $ $ $ $ % & ' ' ' ' ' (7.35) a) b) Figure 7.5: Plane and parabolic surface, leader-follower, a) 1 st step, b) 10 steps. The equations corresponding to plane and cone surface constraints are derived using terms denoted as c PCN,i (t) , d PCN,i (t) , and f P,i (t) . Coefficients µ i and λ i are defined in Eq. (7.27), while the term f P,i (t) is defined in Eq. (7.32). Φ PCN,i (t)= u x,i q x,i (t)−q sx (t) ( ) +u y,i q y,i (t)−q sy (t) ( ) +u z,i q z,i (t)−q sz (t) ( ) q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) # $ % % % & ' ( ( ( (7.36) Ψ PCN,i (t)= ψ P,i (t) ψ CN,i (t) " # $ $ % & ' ' (7.37) 177 A PCN,i = −u x,i −u y,i −u z,i −2 q x,i (t)+δ x ( ) 2c n q y,i (t) 2c n q z,i (t) ! " # # $ % & & ,b PCN,i = b P,i b CN,i ! " # # $ % & & ,where b CN,i =λ i q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +2  q x,i (t) 2 −c n  q y,i (t) 2 +  q z,i (t) 2 ( ) ( ) +2µ i q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.38) c PCN,i (t)= u y,i 2 +u z,i 2 ( ) q x,i (t) 2 +δ x ( ) +c n 2 u x,i 2 +u z,i 2 ( ) q y,i (t) 2 + u x,i 2 +u y,i 2 ( ) q z,i (t) 2 ( ) +2c n δ x u x,i u y,i q y,i (t)+u z,i q z,i (t) ( ) −c n u y,i u z,i q y,i (t)q z,i (t) ( ) (7.39) A PCN,i .M − 1 2 " # $ % & ' + = m c PCN,i (t) a 11 a 12 a 21 a 22 a 31 a 32 ( ) * * * * + , - - - - a 11 =−c n q x,i (t)+δ x ( ) u y,i q y,i (t)+u z,i q z,i (t) ( ) +c n u x,i q y,i (t) 2 +q z,i (t) 2 ( ) ( ) a 12 =− 1 2 q x,i (t)+δ x ( ) q y,i (t) 2 +q z,i (t) 2 ( ) +c n u x,i u y,i q y,i (t)+u z,i q z,i (t) ( ) ( ) a 21 =− u y,i q x,i (t)+δ x ( ) 2 +c n u x,i q y,i (t) q x,i (t)+δ x ( ) −c n 2 q z,i (t) u z,i q y,i (t)−u y,i q z,i (t) ( ) ( ) a 22 = 1 2 u x,i u y,i q x,i (t)+δ x ( ) +c n q y,i (t) q y,i (t) 2 +q z,i (t) 2 ( ) −c n u y,i u z,i q z,i (t) ( ) a 31 =−u z,i q x,i (t)+δ x ( ) 2 +c n 2 q y,i (t) 2 ( ) −c n q z,i (t) u x,i q x,i (t)+δ x ( ) −c n u y,i q y,i ( ) a 32 = 1 2 u z,i u x,i q x,i (t)+δ x ( ) −c n u y,i q y,i (t) ( ) ( ) +c n q z,i (t) q x,i (t) 2 +q z,i (t) 2 ( ) (7.40) d PCN,i (t)= 2 a x,i (t) q x,i (t)+δ x ( ) −c n a y,i (t)q y,i (t)+a z,i (t)q z,i (t) ( ) ( ) +λ 2 q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +2  q x,i (t) 2 −c n  q y,i (t) 2 +  q z,i (t) 2 ( ) ( ) +2µ 2 q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (7.41)  q x,i (t)=a x,i (t)− 1 2c PCN,i (t) d PCN,i (t) q x,i (t)+δ x ( ) q y,i (t) 2 +q z,i (t) 2 ( ) +2c n u x,i u y,i q y,i (t)+u z,i q z,i (t) ( ) " # $ $ % & ' ' +2c n f P,i (t) q x,i (t)+δ x ( ) u y,i q y,i (t)+u z,i q z,i (t) ( ) +c n u x,i q y,i (t) 2 +q z,i (t) 2 ( ) " # $ $ % & ' ' " # $ $ $ $ $ $ % & ' ' ' ' ' ' (7.42) 178  q y,i (t)=a y,i (t)+ 1 2c PCN,i (t) d PCN,i (t) u x,i u y,i q x,i (t)+δ x ( ) +c n u x,i 2 +u z,i 2 ( ) q y,i (t)−u y,i u z,i q z,i (t) ( ) " # $ $ % & ' ' −2f P,i (t) u y,i q x,i (t)+δ x ( ) 2 +c n u x,i q y,i (t) q x,i (t)+δ x ( ) −c n 2 q z,i (t) u z,i q y,i (t)+u y,i q z,i (t) ( ) " # $ $ $ $ % & ' ' ' ' " # $ $ $ $ $ $ $ $ % & ' ' ' ' ' ' ' ' (7.43)  q z,i (t)=a z,i (t)+ 1 2c PCN,i (t) d PCN,i (t) u x,i u y,i q x,i (t)+δ x ( ) −c n u y,i u z,i q y,i (t)− u x,i 2 +u y,i 2 ( ) q z,i (t) ( ) " # $ $ % & ' ' −2f P,i (t) u z,i q x,i (t)+δ x ( ) 2 +c n u x,i q z,i (t) q x,i (t)+δ x ( ) +c n 2 q y,i (t) u z,i q y,i (t)−u y,i q z,i (t) ( ) " # $ $ $ $ % & ' ' ' ' " # $ $ $ $ $ $ $ $ % & ' ' ' ' ' ' ' ' (7.44) a) b) Figure 7.6: Plane and cone surface, leader-follower, a) 1 st step, b) 10 steps. The equations corresponding to plane and cylindrical surface constraints are derived, using terms denoted as c PCL,i (t) , d PCL,i (t) , while the term f P,i (t) is defined in Eq. (7.32). 179 Φ PCL,i (t)= u x,i q x,i (t)−q sx (t) ( ) +u y,i q y,i (t)−q sy (t) ( ) +u z,i q z,i (t)−q sz (t) ( ) q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 # $ % % % & ' ( ( ( (7.45) Ψ PCL,i (t)= ψ P,i (t) ψ CL,i (t) " # $ $ % & ' ' (7.46) A PCL,i = −u x,i −u y,i −u z,i 0 −2q y,i (t) −2 q z,i (t)+δ z ( ) ! " # # $ % & & ,b PCL,i = b P,i b CL,i ! " # # $ % & & ,where b CL,i =λ i q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 ( ) +2  q y,i (t) 2 +  q z,i (t) 2 +µ i q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) ( ) (7.47) c PCL,i (t)= u x,i 2 +u z,i 2 ( ) q y,i (t) 2 −2u y,i u z,i q y,i (t) q z,i (t)+δ z ( ) + u x,i 2 +u y,i 2 ( ) q z,i (t)+δ z ( ) 2 (7.48) A PCL,i .M − 1 2 " # $ % & ' + = m c PCL,i (t) a 11 a 12 a 21 a 22 a 31 a 32 ( ) * * * * + , - - - - a 11 =−u x,i q y,i (t) 2 + q z,i (t)+δ z ( ) 2 ( ) a 12 = 1 2 u x,i u y,i q y,i (t)+u z,i q z,i (t)+δ z ( ) ( ) ( ) a 21 =u z,i q y,i (t) q z,i (t)+δ z ( ) −u y,i q z,i (t)+δ z ( ) 2 a 22 =− 1 2 u x,i 2 +u z,i 2 ( ) q y,i (t)−u y,i u z,i q z,i (t)+δ z ( ) ( ) a 31 =−u z,i q y,i (t) 2 +u y,i q y,i (t) q z,i (t)+δ z ( ) a 32 = 1 2 u y,i u z,i q y,i (t)− u x,i 2 +u y,i 2 ( ) q z,i (t)+δ z ( ) ( ) (7.49) d PCL,i (t)= 2 a y,i (t)q y,i (t)+a z,i (t) q z,i (t)+δ z ( ) ( ) +λ 2 q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 ( ) +2  q y,i (t) 2 +  q z,i (t) 2 ( ) +2µ 2 q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) (7.50) 180  q x,i (t)=a x,i (t)− 1 2c PCL,i (t) 2d PCL,i (t)u x,i q y,i (t) 2 + q z,i (t)+δ z ( ) 2 ( ) −f P,i (t)u x,i u y,i q y,i (t)+u z,i q z,i (t)+δ z ( ) ( ) " # $ $ $ % & ' ' ' (7.51)  q y,i (t)=a y,i (t)+ 1 2c PCL,i (t) 2d PCL,i (t) u z,i q y,i (t) q z,i (t)+δ z ( ) −u y,i q z,i (t)+δ z ( ) 2 ( ) −f P,i (t) u x,i 2 +u z,i 2 ( ) q y,i (t)−u y,i u z,i q z,i (t)+δ z ( ) ( ) " # $ $ $ % & ' ' ' (7.52)  q z,i (t)=a z,i (t)− 1 2c PCL,i (t) 2d PCL,i (t) u z,i q y,i (t) 2 −u y,i q y,i (t) q z,i (t)+δ z ( ) ( ) −f P,i (t) u y,i u z,i q y,i (t)− u x,i 2 +u z,i 2 ( ) q z,i (t)+δ z ( ) ( ) " # $ $ % & ' ' (7.53) a) b) Figure 7.7: Plane and cylindrical surface, leader-follower, a) 1 st step, b) 10 steps. 7.4 Simulation Results for Triangular Formation Configuration The presented simulation results are obtained for flexible formation models considering triangular formation configuration. Model settings correspond to the set denoted as Set One, as described in Section 5.5, Table 5.1. This model uses Hills terrain, considering minRay navigation settings for the formation members. Parameters specific to surfaces of revolution constraints, defining size of the surfaces, c p , c n , r c , and magnitude of apex translation, δ x , δ z , are added to model 181 settings. Their values are specified as c p =0.28 and δ x = 0.2 m, c n =1.5 and δ x =1.0 m, r c =4.8 m and δ z =0.2 m for parabolic, cone, and cylindrical surfaces respectively. Those values are selected considering the diameter of cylindrical safe corridor volumes γ cr =10.0 m, duration of a time step dt =0.05 s, and the expected magnitude of initial velocities estimated as 100.0 m/s 2 . Dimensions of the surfaces relative to the expected size of safe corridors are illustrated with the plots for each of the examples, as shown in the figures below. Simulation of terrain following formation flight for triangular formation defined with planes and parabolic surface constraints demonstrate encountering too stiff system condition for formation member FK 3 , associated with steep β 3 and γ 3 angles, as follows from the simulation results shown in Fig. 7.11-13. It occurs during simulation time step t 54 ∈ (2.65, 2.7) s at the time t=2.67 s. It shall be noted that a “too stiff” condition reported by Mathematica numerical solver, NDSolve, designates a case when the solver estimates that the duration of internal time step, required achieving the desired numerical precision is effectively zero, [120]. In application to solving equations of motion for formation members, this condition translates into extremely large magnitude of the resulting forces of constraints. In such case, our modeling and simulation system, developed for simulation formation flight, reports this condition and stops further computations for the corresponding formation member. This agent exits formation, while the remaining agents proceed, as follows from trajectory plots shown in Fig 7.9 and elevation profile for FK 3 shown in Fig. 7.10b. 182 a) b) Figure 7.8: Parabolic surfaces, a) A single surface with corridor volume, b) Planes, paraboloids, and navigation vectors for the first 3 time steps. a) b) Figure 7.9: Trajectories with, a) Terrain, b) Formation shapes. a) b) Figure 7.10: Terrain and trajectory elevation profiles, a) FK 1 , b) FK 3 . 183 a) b) Figure 7.11: Distances, a) Between positions of GC and FK i , b) Deviation from constant elevation above terrain surface for formation members FK i . a) b) Figure 7.12: FK i , a) Angles between normal and planes, b) Deviation of agent trajectories from centerlines of corridor volumes. a) b) Figure 7.13: Angles between, a) Consecutive navigation vectors, b) Velocity directions and navigation vectors. 184 a) b) Figure 7.14: Magnitudes of, FK i , a) Velocity, b) Forces of constraints. a) b) Figure 7.15: Numerical values of expressions, a) Ψ LX for GC, b) Ψ PPR,i for FK i . Simulation results obtained with plane and cone surfaces constraints demonstrate that a stiffness condition, similar to the one described above, is encountered for formation member FK 1 , as follows from the results shown in Fig. 7.17-18. It occurs during simulation time step t 76 ∈ (3.8, 3.85) s at the time t=3.8 s. Such condition is associated with a spike in β 1 angles, and corresponding spike in the required control forces, as follows from the results shown in Fig. 7.19a, 7.20b. As follows from the results shown in Fig. 7.20a, the requirement of maintain adherence to the bounds of safe corridor volumes is not satisfied. At the same time, exceedingly large magnitude of forces of constraints are required for all of the formation members, not only for FK 1 , as shown in Fig. 7.20b. 185 a) b) Figure 7.16: Cone surfaces, a) A single surface with corridor volume, b) Planes, cones, and navigation vectors for the first 3 time steps. a) b) Figure 7.17: Trajectories with, a) Terrain, b) Formation shapes. a) b) Figure 7.18: Trajectory profiles, a) Deviation from the desired navigation height, b) FK 1 . 186 a) b) Figure 7.19: Angles between, a) Consecutive navigation vectors, b) Velocity directions and navigation vectors. a) b) Figure 7.20: Formation members FK i , a) Deviation of agent trajectories from centerlines of corridor volumes, b) Magnitudes of forces of constraints. Unlike the previous two examples, use of plane and cylinder surface constraints with the triangular formation model described above, results in encountering stiff condition for all of the formation members. Moreover, for the agents FK 1 and FK 2 such condition occurs immediately, at the beginning of the simulation, time t 1 =0.0 s while for the agent FK 3 the constraint forces reach an extremely large magnitude at the initial time t 1 . In addition, the results indicate that although magnitudes of angles β 3 and γ 3 are comparable with the ones obtained with previously described models, there is a 187 distinctive spike in the values of angles, β XY,3 and γ XY,3 , between horizontal projections of corresponding vectors, as shown in Fig. 7.23b, 7.24b. These results indicate that use of plane and cylindrical surface constraint, as defined in Eq. (7.45-46), results in the system model that is highly sensitive to deviation of initial conditions from the values consistent with the constraints ϕ CL,i , Φ PCL,i . The results for only the first 7 simulation steps are shown blow, since all of the agents exit the formation within the first 5 steps, as shown in the figures below. a) b) Figure 7.21: Cylindrical surfaces, a) A single surface with corridor volume, b) Planes, cylinders, and navigation vectors for FK 3 . a) b) Figure 7.22: Formation member FK 3 , a) Deviation of agent trajectories from centerlines of corridor volumes, b) Trajectory elevation profile. 188 a) b) Figure 7.23: Angles for FK 3 between, a) Consecutive navigation vectors, b) Projections of consecutive navigation vectors on XY plane. a) b) Figure 7.24: Angles for FK 3 between, a) Velocity directions and navigation vectors, b) Projections of the vectors on XY plane. a) b) Figure 7.25 Forces of constraints for FK 3 , a) Forces, b) Magnitude. 189 The presented results indicate that considered formations are highly sensitive to the values of initial conditions and directions of navigation vectors. They also indicate extremely large magnitude of the resulting forces of constraints. This is observed for all of the considered surface types, i.e. parabolic, cone, and cylindrical surfaces. The problem arises due to changes in orientation between consecutive surfaces. A considerable difference between orientation of the main axis of a surface, ˆ x axis of xyz i , and the direction of initial velocity,  q i t n ( ) , results. Magnitude of this difference is quantified by the values of angles γ i (t)=∠ ˆ ν xyz,i ,  q xyz,i t n ( ) ( ) , where ˆ x axis ≡ ˆ ν xyz,i , as shown in Fig. 7.13b, 7.19b, 7.24. An illustration of such transition case for a system modeled using parabolic surface constraints is presented in the figure below. It shows that trajectory segments for time steps 2 and 3 that does not provide a smooth transition, at the time t 3 . In simulation, it would result in a spike in the angles γ t 3 ( ) , corresponding spike in F c t 3 ( ) , or an encounter of a stiff condition by numerical solver. The following figure also indicates the difference between the actual and the desired path for the third trajectory segment. Figure 7.26: Actual and desired trajectory segments for 3 time steps. ˆ ν i,1 q i t 1 ( ) x 1 z 1 y 1 ˆ ν i,2 q i t 2 ( ) z 2 y 2 x 2 ˆ ν i,3 q i t 3 ( ) x 3 z 3 y 3 Trajectory Segments Actual Desired X Y Z 190 Therefore, it is possible to conclude that plane and surfaces of revolution constraints, presented in this chapter, results in system models that are highly sensitive to initial conditions at the beginning of each n th time interval. This aspect of system performance is inherent to selecting of the described surfaces of revolution as means for satisfying terrain following requirements. Further verification of sensitivity of the considered surfaces of revolution constraints to specified size of the surfaces and changes in the direction of navigation vectors are presented in Appendix C. The tests are presented in an appendix since they are conducted using models of independent agents, not multi agent formations or single leader-follower systems. Test results are obtained for ranges of surface sizes, represented by rages of scaling coefficients c p , c n , and radius r c . The description provided in the appendix also includes derivations of equations of motion for the agents and description of the procedure for generation directions of consecutive navigation vectors used for the tests. 7.5 Conclusion Flexible formations modeled with planes and parabolic, cone, and cylindrical surfaces constraints demonstrate considerable degree of sensitivity to specified size of the surfaces, combined with the sampling frequency, and other model parameters. Therefore, it is possible to conclude that they could not be considered as a suitable solution for the design of flexible terrain following formation flight systems. Simulation results presented in the previous subsection and test results presented in Appendix C support such conclusion. 191 However, it shall be noted that constraints defined using cylindrical surfaces provide, or guarantee, adherence to the bounds of safe corridor volumes, considering large magnitude of stabilization coefficients, as follows from both simulation examples and test results. It should also be noted that stiffness condition and/or numerical precision problems arising from application of this type of constraint to agent models, could be attributed to implementation of transformation procedure using direct numerical differentiation, as discussed in the appendix. The tests indicate an inherent contradiction in described system design, applicable to all three types of the surfaces. Larger size of the surfaces are required in order to decrease magnitude of the required control forces. At the same time, closer fit between surface points and a line defined by navigation vector, coinciding with the main axes of the surfaces, is required in order to support motion of the agents towards the goal. 192 CHAPTER 8: MODELING AND SIMULATION WITH MAYA AND MATHEMATICA This chapter describes system architecture and presents simulation results obtained with modeling and simulation tool implemented using Autodesk Maya, [108], and Wolfram Mathematica, [109]. The system contains state of the art modeling, simulation, analysis, and visualization components, attributed to the respective tools. It allows adding new custom modules implemented as Mathematica packages and Maya plug-ins. The resulting system architecture diagram shown in the figure below. MathKernel is the computational engine and Mathematica is the front end of Wolfram Mathematica. IM2Link denotes the set of our newly developed components, where IMath and ILink indicate Mathematica and Maya based parts, respectively. Figure 8.1: System architecture diagram. MEL and MX file types, indicated in the above diagram, support input/output functionalities of IM2Link. MEL is an editor and a scripting language, developed by Autodesk, for describing Maya models. MX is a data format, implemented by Wolfram Research, accessible within Mathematica. The results obtained with IM2Link are stored Maya OpenMaya API MathKernel MathLink API Mathematica Workspace IM2Link ILink Maya Scenes Model Scripts MEL Simulation Results MX Results Notebook Functions Data IMath 193 as MX files. The data is read, evaluated, and plotted in the standard Mathematica notebook workspace. Therefore, the numerical results could be further processed by Mathematica functions. Availability of an extensive visualization capabilities, such as displaying trajectories, navigation vectors, and sensor rays, is an additional benefit for analyzing system behavior and presenting the results. Implementation of IM2Link uses MathLink C API of Wolfram Mathematica, [117]. It uses C++ components of OpenMaya API, [118], including Maya base classes and functions supporting OpenGL based visualization. Data input/output functionality, available with IM2Link, is provided by an extensive set of dedicated functions of both Maya and Mathematica. The most notable of these data formats include 3D CAD files, DEM and ArcGRID terrain data sets. Connecting Maya with MathKernel directly presents a novel approach to the assembly of modeling and simulation systems. Despite the existence of a number of extremely capable modeling and simulation software tools, such as Simulink, [110], MapleSim, [111], Wolfram SystemModeler, [109], and Autodesk Inventor, [113], no single tool provides all of the features necessary for performing a large-scale simulations involving computationally extensive geometry interactions and continuous time dynamics at the same time. It shall be noted that any of the tools mentioned above is fully capable of supporting node based models combining geometrical and dynamical components, as demonstrated on the examples of terrain following formation models presented in this thesis, developed using stand along Mathematica. In addition, it is possible to use external solvers with any of these tools. 194 For example, connectivity between Simulink and MapleSim is supported by MapleSim Connector package, [114]. Another example is Mathematica Symbolic Toolbox for MATLAB developed by Barrows, [115]. A collection of connectivity solutions together with related tutorials and documentation is maintained by Linking Technology section of the Wolfram Library Archive, [116]. These solutions provide access to symbolic math, 3D visualization, and node-based modeling capabilities in addition to continuous time dynamics. However, interactions between 3D geometry models, implemented using spatial division techniques, comparable to the complexity of modeling features available in Maya, are unavailable with the existing techniques. The key features of the combined Maya-Mathematica system are computation efficiency of the resulting simulator, availability of extensive set of functions available for the development of problem specific components, and relative simplicity of setting models, running simulations, and evaluating results. However, computational complexity is the main factor that prompted development of a new modeling and simulation solution. Validation and verification of newly developed problem specific components and design models is performed by comparing numerical simulation results obtained with Maya-Mathematica and stand along Mathematica. Availability and relative simplicity of performing such comparison is a considerable advantage of the suggested simulation and modeling architecture. In the context of terrain following flight system design, extensive geometry modeling is required for representing terrain surfaces. Extensive ray tracing computations are required for sensor modeling. Support of continuous time dynamics is required for the 195 models of terrain following agents. Capability of specifying a node-based model structure is highly desirable for representing interconnections between system components. 8.1 Terrain Following Formation Flight Node Graph Model The set of functions and modeling components developed with IM2Link for simulation of terrain following formation flight systems includes terrain elevation data processing, models of ray sensors, data models and solver nodes for representing particles, external forces, navigation computations, and equations of motion. The resulting node graph of a formation primitive is shown in the figure below. It is limited to indicating IM2Link computational and input data processing nodes. Custom visualization nodes for displaying trajectories, rays, and navigation vectors are not shown, in order to reduce graph complexity. The diagram corresponds to the data flow and formation structure as described in the Problem Formulation Section 2.2 and illustrated in Fig. 2.7 and 2.8. Figure 8.2: Formation node graph. Terrain Ray Trace Navigation FK i Navigation GC Mesh Sensor FK i Particle FK i Particle GC EOM GC Goal Time F Gravity F Drag F Thruster F Total Standard Maya node Constant data Expression Node Multiple nodes EOM FK i 196 As it follows from the above diagram, a single instance a Ray Trace node, is connected with all of the sensor nodes. This node creates a single instance of spatial subdivision structure for terrain mesh, allowing performing ray tracing to large terrain surface models efficiently. Maya standard Time node serves only as a frame counter for invoking node evaluations. Simulation time steps are represented as t∈ t n , t n +dt ( ) , specified in Particle and EOM settings. Continuous time data, stored as Particle state vectors, constraint forces, and external acceleration components, are implemented using interpolation functions resulting from processing of solutions of equations of motion, obtained with Mathematica NDSolve. Each of the Maya IM2Link components, indicated in the diagram, includes a corresponding set of Mathematica functions, as follows from the description of system architecture illustrated in Fig. 8.1. It shall be noted that Maya node graph is a directed acyclic graph, DAG. However, the above diagram contains a loop between Particle and EOM nodes. Internal structure of a Particle node consists of two distinct components evaluated separately, within the same time step. This method could be viewed as an application of a node splitting technique, such as the one described in Section 2.2.2 and illustrated in Fig. 2.12. In order to reduce model complexity, a version of Maya-Mathematica tool assembly, denoted as INDY Link, is developed. It reduces number of Maya nodes and amount of function calls made through Link-Math interface by implementing nodes corresponding to a set of modeling elements, such as particles, sensors, navigation, and equation of motion components. This modeling solution could be considered an application of multi-particle implementation technique, allowing reduction of simulation runtime. It also simplifies implementation of model settings and analysis functionality in 197 Mathematica by providing access to multi-element data structures, such as an array of particle state vectors. INDY Link implements problem specific components. Thus, a collection of independent terrain following agents and a formation are modeled using different node types. Node connection diagram, shown in Fig. 8.3, illustrates application of this technique to modeling of sensor components of formation members. Another method of improving simulation runtime performance is implementing parallel Mathematica computations, [117], by connecting INDY Link with several, m k , MathKernel processes, denoted as MK j , where j=1,…, m k , as illustrated in Fig. 8.4. In combination with multi-element modeling described above, this parallel processing technique provides an additional benefit of simplifying model settings. Figure 8.3: Multi-element sensor model. Figure 8.4: Parallel MathKernel processes. ILink Sensor FK i INDY Link Sensor FK, i=1,…, n f IMath Sensor FK i INDY Math Sensor FK, i=1,…, n f Node Multiple nodes Connection Multi-connections Maya Maya-MathKernel Maya INDY Link MathKernel MK j Simulation Results MX j 198 8.2 Examples of Individual Model Components The following are the examples of terrain surface and sensor models. These parts of terrain following formation flight system models require availability of an efficient geometry processing in order to support larger scale simulations. Terrain is represented by either digital elevation models, DEM, or surfaces defined with mathematical expressions. With IM2Link, surfaces defined as Mathematica symbolic expressions have corresponding mesh and NURBS representation in Maya environment. The ability to specify equivalent models is required for performing direct comparison between Maya- Mathematica and stand along Mathematica simulation results. However, symbolic math and mesh or NURBS surface models are close but not identical. Magnitude of deviation between the results of the respective ray trace computations corresponds to point sampling rate used during creation of surface models, where closer fit is associated with finer point grids, as expected. Terrain surface models are created in IM2Link by processing USGS DEM or ArcGRID data sets using standard set of Mathematica Geographic Data and Geodesy components, [119]. A terrain model is created based on raw elevation data, not mapping and projection results. Conversions between {Latitude, Longitude, Elevation}, LLE, {East, North, Up}, ENU, and the local Cartesian {X, Y, Z}, XYZ, coordinate systems of terrain models are implemented with a set of Geodesy functions. The Earth representation as the geoid and corresponding settings specified within datum elements of DEM data sets are considered. 199 Sensor model in conjunction with a terrain surface, modeled as combination of functions z= f s x,y ( ) , is shown in the figure below. Sensor and navigation model settings follow the description presented in Section 3.3.1 and 3.3.2 with navigation type specified as maxRay, desired navigation height h n =20.0 m, size of sensor ray matrix is 11x11. Width and orientation of sensor cone defined by α r =30° and β r =60° , respectively. Positions of the agent and goal point are specified as p s ={-3.0, -6.0, 18.0} m and p g ={20.0, 20.0, 10.0} m. Figure 8.5: Sensor model. Model of Alaska terrain with latitude, longitude coordinates of the corners of corresponding map specified as (N: 61.0, W:-146.5) and (S: 60.0, E: -146.0) is shown in the figure below. The model is created with ArcGRID data from National Elevation Dataset, NED, 1 Arc Second. The resulting Maya model is generated from 1800x1801 point grid, with the origin of local XYZ coordinate system of the model coinciding with position of the central point of the grid matrix. Scaling coefficient for points in the local XYZ coordinate system of the model is specified as 0.01. A portion of Alaska terrain, which is relatively close to the North Pole, is selected for this example since the shape of the resulting model demonstrates results of conversion from LLE to XYZ coordinates. 200 Figure 8.6: Alaska terrain. Multi-element modeling capability of INDY Link is demonstrated with the example of 2000 individual terrain following agents. The terrain model is Los Angeles West, created with USGS DEM data set, including 1201x1201 point grid, as shown in the figures below. The presented simulation is limited to 18 simulation steps considering dt =1.0 s. Initial positions of the agents are evenly spaced within a volume specified by min and max corners, p min ={-4000.0, -4000.0, 520.0} and p max ={4010.0, 4010.0, 2030.0} m, respectively. Initial velocity of the agents are specified as {47.5511, 64.3339, -0.188747 } m/s. Each of the agents is equipped with 5x5 ray rangefinder sensor with the maximum sensor range of 1020.0 m. A significant portion of terrain model is considered during ray surface intersection computations with model settings specified above. However, a significant portion of the sensors belonging to the agents, initially positioned at higher elevations, do not acquire terrain due to limited sensor range. It results in near horizontal motion of the agents, which is a default behavior defined with downward inclination of navigation vector, 201 specified as α d =1.5° . The resulting separation of agents trajectory paths is visible in Fig. 8.8. The values of the position points are scaled by INDY Link using scaling coefficient specified for the terrain, 0.01. Therefore, all of the geometry computations in Maya are performed using scaled coordinates, while all of the inputs into computations of equations of motion in Mathematica use absolute coordinates, with the positions specified in meters. a) b) Figure 8.7: Initial positions of 2000 agents, a) View 1, b) View 2. Figure 8.8: Trajectories of 2000 agents. 202 8.3 Simulation Results, Comparison, and Verification Comparison and verification for Maya-Mathematica modeling and simulation system components is achieved by direct comparison between simulation results obtained with IM2Link and stand along Mathematica. Visualization capabilities of Maya serves as a design aid for the development of terrain following system. Maya workspace provide proportional view of modeled objects, such as terrain surface and ray cones, complete with a full set of standard display features, such as interactive object selection, zoom, and transparency settings for volumes and surfaces. Another advantage of using Maya for visualization is its capacity for drawing large number of arbitrary shapes, such as arrows for rays and navigation vectors, directly in the workspace. IM2Link includes dedicated display nodes that access positioning and direction data directly from MathKernel data structures, thus, providing designers with an additional verification capability. Custom display nodes include problem-specific user interface/attributes, such as length of navigation vector arrow or colors of sensor rays. Runtime performance of these display functions and their drawing capacity is attributed to implementation using OpenGL, as Maya allows. The following examples correspond to Star Two flexible formation with settings specified in Section 5.1 and Set One hierarchical formation with structure and settings specified in Section 6.1. Thus, the simulation results shown in the figures below correspond to the results presented in Sections 5.5.2 and 6.2.1. Shape of a flexible formation and sets sensor rays for the current time step, together with the records of navigation vectors and rays for the example Star Two is 203 shown in the figures below. The results for the first 14 time steps and the full simulation time frame are shown in Fig. 8.9 and 8.10, respectively. Plots of the magnitude of constraint, control, forces and values of constraint expressions are shown in Fig. 8.11. This data corresponds to the results shown in Fig. 5.30.b and 5.31.b. It shall be noted that there is a difference between the maximum values of expressions Ψ i (t) , 1.5x10 -8 and 2x10 -10 , obtained with IM2Link and stand along Mathematica. It may be attributed to expected difference between ray tracing results obtained with expression and mesh model of the terrain surface, as described earlier in this chapter. Figure 8.9: Star Two, formation shape after 14 time steps. Figure 8.10: Star Two, trajectories, shape, navigation vectors and rays. 204 a) b) Figure 8.11: Star Two, a) Constraint force magnitude, b) Constraint expressions Ψ i (t) . Initial estimates of terrain surface elevation, evolution of formation shapes, and results obtained using hierarchical flexible formation model, Set One, specified in Section 6.1, are shown in the figures below. Elevation estimates are shown in Fig. 8.12, these results correspond to the ones shown in Fig 6.5. The plots of constraint force magnitude and numerical values of the constraint expressions Ψ i (t), shown in Fig. 8.15, match the results presented in Fig. 6.11b and 6.12a. It may indicate that with less steep slopes of the terrain the difference between ray tracing results obtained with expression based and mesh based numerical computations diminishes, allowing sufficiently close match between the results obtained with IM2Link and stand along Mathematica. Figure 8.12: Set One, initial configuration. 205 Figure 8.13: Set One, evolution of formation shape. Figure 8.14: Set One, trajectories. a) b) Figure 8.15: Set One, a) Constraint force magnitude, b) Constraint expressions Ψ i (t) . 206 8.4 Simulation Results Obtained using USGS DEM Terrain Model Simulations performed with USGS DEM terrain model are available with Maya- Mathematica based modeler and simulator assembly, since our current modeling system does not support ray tracing to large Mathematica meshes suitable for representing a terrain surface. Therefore, these simulations are performed using IM2Link. The results are evaluated, considering that verification of individual model components is performed, as described in the previous section. In addition to using realistic terrain model, our modeling and simulation tool allows performing simulation involving considerably larger number of time steps, due to efficient implementation of ray tracing in Maya. Thus, system models involving much smaller values of dt combined with greater total simulation time t max may be explored. Simulation results for the models with time step settings approximating continuous sensor sampling rate, dt<<1.0 could be obtained. However, the amount of memory for accumulation and storage of the resulting data will increase proportionally, since each time step generates a set of interpolating functions representing state variables for a continuous time segment. These aspects of system performace are demonstrated with the result for two- level, hierarchical, terrain following formation flight model, shown in the figures below. The terrain model is created based on the part of USGS DEM Los Angeles West data set. This portion of the terrain surface is modeled as 100x100 points mesh, with maximum resolution that the original DEM data set allows. Initial positions of the agents are computed from XY projections on the terrain surface, while Z component is computed as constant elevation of 24.0 m above the surface. Formation structure and the rest of the 207 model settings for the agents, with the exception of the duration of a time step and simulation time frame, are as specified for the example Set One, described in Section 6.2, and used in the previous section for illustrating verification capabilities of IM2Link. Models based on Set One with the values of dt specified as 0.25 s and 0.025 s are denotes as Set I and Set II, respectively. Total simulation time frame is specified as 20.0 s for both models, resulting in simulation run with 80 and 800 time steps, respectively. Formation shapes, trajectories, navigation vectors and navigation rays are shown in Figures 8.16 and 8.17. The results demonstrate avoiding terrain obstacles, hills, with corresponding changes in formation configuration, expected from a flexible formation. Plots of magnitudes forces of constraints, representing control forces of formation members for Set I, and Min, Max of these values, per agent, are shown in Fig. 8.19. Numerical values of expressions of constraints Ψ i (t) , representing numerical errors in computations of equations of motion for both formation members and virtual leaders for Set I are shown in Fig. 8.20. Corresponding plots for formation members of Set II are shown in Fig. 8.21. The comparison of the results indicate that greater frequency of sensor reading, represented by Set II results in smoother distribution of the required control forces. However, selection algorithm for navigation vector results in considerable changes of navigation directions, regardless of the frequency of sensor reading. These are the cases when a new terrain feature, a hill or a canyon, appear within detection range of agents sensors. Another observation of corresponds to formation navigation and guidance, represented by computing navigation vectors of virtual leaders. The exact elevation of the 208 trajectories for the virtual leaders differ for different values of dt , since this value is a parameter for estimation of terrain surface elevation. Figure 8.16: Set I, dt 0.25 s. Figure 8.17: Set II, dt 0.025 s. 209 Figure 8.18: Set I, trajectories. a) b) Figure 8.19: Set I, constraint forces a) Magnitude, b) Min, max magnitude. a) b) Figure 8.20: Set I, values of constraint expressions Ψ i (t) for, a) Formation members, b) Virtual leaders. 210 a) b) Figure 8.21: Set II, a) Constraint force magnitude, b) Constraint expressions Ψ i (t) . Comparison of trajectory, terrain profiles, and metrics of terrain following flight performance, obtained with Set I and Set II, demonstrate that spikes in values of angles β i and γ i (t) occur with both examples, as follows from the results shown in the figures below. These spikes correspond to changes in the direction of consecutive navigation vectors of formation members, which depend on a selection type for navigation algorithm, such as maxRay, sensor settings, and terrain topology, as described in the previous chapters. Such behavior is attributed to cases when new terrain features appear within sensor range, as it was already stated above. a) b) Figure 8.22: Terrain and trajectory profiles for FK 11 a) Set I, b) Set II. 211 a) b) Figure 8.23: Terrain and trajectory profiles for FK 21 a) Set I, b) Set II. a) b) Figure 8.24: Deviation from the desired navigation height a) Set I, b) Set II. a) b) Figure 8.25: Distances to centerlines of corridors for formation members a) Set I, b) Set II. 212 a) b) Figure 8.26: Angles between consecutive navigation vectors a) Set I, b) Set II. a) b) Figure 8.27: Angles between velocity directions and navigation vectors a) Set I, b) Set II. It shall be noted that simulation for Set II requires 325.6 MB of runtime memory for Maya and 541.5 MB for MathKernel, both considering records of state matrices, external acceleration, and expressions of constraints. Data output for Set II, saved as MX Mathematica files, amounts to 120.15 MB for state matrices and 241.8 MB for forces of constraints, external acceleration, and expressions of constraints for all 9 agents, 3 virtual leaders and 6 formation members. The total amount of rays primarily affects simulation runtime. It is not the most considerable factor for the runtime memory consumption. Number of rays does not affect size of the output data, since ray data records are not stored by the presented models. 213 8.5 Conclusion Modeling and simulation system based on Maya and Mathematica engines does provide a practical advantage in creating models of complex systems, requiring both continuous time dynamics and extensive, precise, geometric computations. It is shown that such tool could be implemented making use of node based modeling with Maya and parallel processing with multiple MathKernels with Mathematica. It was shown that the suggested assembly allows validation of simulation results and verification of software implementation of an individual system components by performing a direct comparison between simulation results obtained with IM2Link and stand-along Mathematica. The new system is capable of implementing components not currently available in the standard Mathematica, such as large terrain meshes. At the same time, Mathematica does provide an extensive sets of problem or field specific functionalities, such as the Geodesy package, not currently available as the standard Maya functionality. Dramatic runtime improvements and availability of data output in Mathematica format are shown to be important features of the new system, while visualization capabilities and interactive parameter editing, provided by Maya workspace, serve as additional benefits. Therefore, it is possible to conclude that combination of these two software tools provides a unique, capable, simulation and modeling solution, because, not despite, of the inherent differences of their respective software design and implementation. 214 CHAPTER 9: CONCLUSIONS We have presented a new flexible formation design, defined using formation keeping constraints are specifically developed for terrain following flight of fast moving aircraft/UAVs at low altitudes. Changeable formation geometry is required not only for maintenance of the desired type of formation shape, but also for providing terrain collision avoidance by maneuvering around terrain obstacles, as simulation results demonstrate. The sets of constraints constituting system design, are applicable to both individual formation primitives and multi-level, hierarchical formations. The suggested implementation of the presented formation maintenance method, technique, could be considered significantly different from the existing approaches, developed for individual agents and groups of aircraft, rotorcraft, UAVs. The new terrain following formation system uses principle of reactive obstacle avoidance utilizing a simplified selection-type algorithm for sensor data processing. Navigation procedures does not converge a considerable computational complexity for sensor data processing, as required by a number of well researched sensor fusion techniques. Formation maintenance, supported by newly developed sets of constraints, is performed without relying on AI, Artificial Intelligence, type procedures for specifying optimal or updating desired formation configuration. At the same time, the presented formation design is specifically developed for supporting terrain following flight, thus, providing obstacle avoidance in an environment where slopes of the terrain features, either hills or canyons, are naturally limited, and range of sizes of these features could be estimated, compared with the desired or expected formation size, and taking into consideration for specifying system, model, 215 parameters. Therefore, in its current state, the newly developed system is not intended, modeled, or tested for operation in an urban environment of for flight inside corridor, waypoints grid, or labyrinth types of topologies. In other words, by considering a new, but narrowly defined problem, we were able to develop suitable sets of constraints for prototype of formation system that presents an exact match between the desired capabilities and terrain following performance, demonstrated in simulation. The design is represented by full sets of formation constraints, combining formation-keeping and terrain-following components. Formation-keeping control law is derived using the Udwadia-Kalaba approach and corresponding the Udwadia-Kalaba equation, [1, 2, 87-93]. By presenting comparison of terrain following flight performance between rigid virtual body and adjustable formation structures, we demonstrated that the development of a flexible formation configuration is required for achieving collision-free motion, in regards to terrain feature obstacle avoidance. We presented a set of formation keeping constraints, defined with planes and navigation vectors, for formation members, used in conjunction with navigation vector constraints for the virtual leader. Simulation results illustrating application of corresponding formation-keeping control law to the models of triangular and star formation configurations, in combination with representative Hills and Wave terrain surface topologies, are presented. The results demonstrate critical importance of selecting suitable values of stabilization coefficients in order to ensure adherence of agent trajectories to the bounds of terrain following corridors. A smaller value of the duration of a time step, corresponding to more frequent sensor reading and navigation vector updates is preferred, as expected. The values of performance metrics, describing 216 connection between terrain topology, sensor, and agent model settings are presented. The results demonstrate that such a formation is suitable for the design of a flexible terrain following formation flight system. We presented extension of the developed method to assembly of multi-level, hierarchical, terrain following formation structures. The presented structures contain two levels of virtual leaders and composed of individual formation primitives. The analysis of simulation results for the models defined using different sets of constraints, allows to conclude that selecting of plane and navigation vectors constraints allows retaining the desired flexible formation behavior for hierarchical formations. We also modeled and studied performance of formations implemented using planes and surfaces of revolution constraints. Application of constraints defined based on parametric equations of parabolic, cone, and cylindrical surfaces were studied. However, the developed models show an inherent performance and/or precision problems, associated with such designs, were found. In addition, selecting paraboloids and cones may imply contradiction between the desire to minimize relative size of the surfaces in order to support motion in the desired navigation direction and the necessity to specify larger surface sizes in order to reduce magnitude of the required control forces. Therefore, among the constraints and corresponding control methods, presented in this thesis, application of plane and navigation vector constraints, derived in Chapter 5, remains as the most robust choice for the design of flexible terrain following formation flight systems. In order to enhance modeling and simulation capabilities of available tools, a new architecture using Autodesk Maya and Wolfram Mathematica were developed and 217 corresponding software implemented. Such combination allows using symbolic math of Mathematica and provides access to extensive geometry modeling and processing capabilities of Maya. For terrain following formation models this system simplifies settings of numerical solvers for equations of motions and allows performing computationally efficient ray trace based simulation of sensor in conjunction with terrain models set based on ArcGRID or USGS DEM data. Mathematica data post processing capabilities and Maya node based model structure, and state of the art visualization capabilities serve as additional benefits available by implementation of this system. Future research efforts may benefit from the developed designs by considering various sets of surface based constraints and/or multi-level formation structures. A study of surfaces of revolution constraints might serve as a basis for trying model design utilizing shell like structure of multiple surfaces of the same type, specified with a range of scaling coefficients. An agent would then switch constraints from one surface to another within the same time step, in an attempt of minimizing constraint forces while maintaining the desired direction of motion. The developed Maya-Mathematica modeling and simulation system is suitable not only for support of formation simulations, presented in this thesis, but for a wide use in the general area of simulating multi-element nonlinear dynamical systems modeled in conjunction with complex geometry components. Parallel processing, already implemented with the use of multiple MathKernels, opens possibility for running efficient simulations utilizing full power of multiple processors, on computers ranging from multicore laptops to large scale servers. 218 BIBLIOGRAPHY [1] Udwadia, F. E., and Kalaba, R. E., “Analytical Dynamics: a New Approach”, Cambridge University Press, 1996. [2] Udwadia, F. E., and Kalaba, R. E., “Equations of Motion for Mechanical Systems,” Journal of Aerospace Engineering, Vol. 9, 1996, pp. 64-69. [3] Abramova, I., Latyshev, S., and Yang, B., “Terrain-Following Motion of an Autonomous Agent as Means of Motion Planning in the Unknown Environment”, AIAA Guidance Navigation and Control Conference, AIAA, 2012. [4] C. J. Schumacher, R. Kumar, "Adaptive Control of UAVs in Close-Coupled Formation Flight", Proceedings of the American Control Conference, June 2000, pp. 849-853 [5] D. Galzi, Y. Shtessel, "Closed-Coupled Formation Flight Control Using Quasi- Continuous High-Order Sliding-Mode", Proceedings of the 2007 American Control Conference, July 2007, pp. 1799-1804 [6] P. Seiler, A. Pant, J.K. Hedrick, "String Instabilities in Formation Flight: Limitations Due to Integral Constraints", Journal of Dynamic Systems, Measurement, and Control, Vol. 126, December 2004, pp. 873-879 [7] D. Hummel, "Formation Flight as an Energy-Savings Mechanism", Israel J. Zoology, 1995, Vol. 41, pp. 261-278 [8] P. B. S. Lissaman, C. S. Shollenberger, "Formation Flight of Birds", Science, 1970, Vol. 168, pp. 1003-1005 [9] P. Tabuada, G. J. Pappas, P. Lima, "Feasible Formation of Multi-Agent Systems", Proceedings of the American Control Conference, June 2001, pp. 56-61. [10] F. Guilietti, L. Pollini, M. Innocenti, "Autonomous Formation Flight", IEEE Control Systems Magazine, December 2000, pp. 34-44. [11] C. Tomlin, G. J. Pappas, S. Sastry, "Conflict Resolution for Air Traffic Management: A Study in Mulit-agent Hybrid Systems", IEEE Transactions on Automatic Control, Vol. 43, no. 4, April 1998, pp. 509-521. [12] L. R. Jenkinson, R. E. Caves, D. P. Rhodes, "Automatic Formation Flight: a Preliminary Investigation into the Civil Operation", Department of Aeronautical and Automotive Engineering and Transport Studies, Loughborough Univ. of Technology, Loughborough, UK, 1995 [13] R. Ghosh, C. Tomlin, "Nonlinear Inverse Dynamic Control for Mode-based Flight", Proceedings of the AIAA Guidance, Navigation, and Control Conference, August 2000 [14] A. W. Bloy, M. G. West, K. A. Lea, M. Joumaa, "Lateral Aerodynamic Interference Between Tanker and Receiver in Air-to-Air Refueling", J. Aircraft, Vol. 30, 1993, pp. 705-710 [15] A. W. Bloy, M. Joumaa, "Lateral and Directional Stability and Control in Air-to- Air Refueling", J. Aerospace Eng., Vol. 209, 1995, pp. 299-305 [16] M. Pachter, J. J. D'Azzo, J. L. Dargan, "Automatic Formation Flight Control", Journal of Guidance, Control, and Dynamics, Vol. 17, no. 6, 1994, pp. 1380- 1383. 219 [17] E. Johnson, A. Calise, R. Sattigeri, Y. Watanabe, V. Madyastha, "Approaches to Vision-Based Formation Control", Proceedings of 43rd IEEE Conference on Decision and Control, December 2004, pp. 1643-1648 [18] Qi Chen, U. Ozguner, "A Hybrid System Model and Overlapping Decomposition for Vehicle Flight Formation Control", Proceedings of the 42nd IEEE Conference on Decision and Control, December 2003, pp. 516-521 [19] F. Borrelli, T. Keviczky, G. J. Balas, "Collision-free UAV Formation Flight Using Decentralized Optimization and Invariant Sets", Proceedings of 43rd IEEE Conference on Decision and Control, December 2004, pp. 1099-1104 [20] D. Stipanovich, G. Inhalan, R. Teo, C. J. Tomlin, "Decentralized Overlapping Control of a Formation of Unmanned Aerial Vehicles", Automatica, Vol. 40, no. 8, 2004, pp. 1285-1296 [21] R. W. Beard, J. Lawton, F. Y. Hadaegh, "A Coordination Architecture for Spacecraft Formation Control", IEEE Transaction on Control Systems Technology, 2000 [22] J. Leitner, F. Bauer, J. How, M. Moreau, R. Carpenter, D. Folta, "GPS World - Formation Flight in Space: Distributed Spacecraft Systems Develop New GPS Capabilities", February 2002, retrieved from http://www.gpsworld.com [23] S. Asawa, T. Nagashio, T. Kida, "Formation Flight of Spacecraft in Earth Orbit via MPC", Proceedings of SICE-ICASE International Joint Conference, October 2006, pp. 2207-2211 [24] C. Leonard, W. Hollister, E. Bergmann, "TechSat21 - Space Missions Using Collaborating Constellations of Satellites", Proceedings of AIAA/US Small Satellite Conference, 1998 [25] LISA Project Office, Laser Interferometer Space Antenna, NASA, retrieved 2012 from http://lisa.nasa.gov [26] G. Min, Z. Chuanqu, Z. Xiaoling, H. Shunji, "Analysis of the Virtual Baseline of Cluster SAR Satellites", Proceedings of IGARSS '04, Geoscience and Remote Sensing Symposium, IEEE International, September 2004, pp. 500-502 [27] H. G. Tanner, G. J. Pappas, V. Kumar, "Leader-to-Formation Stability", IEEE Transaction on Robotics and Automation, Vol, 20, No. 3, June 2004, pp. 443-455 [28] N. Meskin, K. Khorasani, "Fault Detection and Isolation of Actuator Faults in Spacecraft Formation Flight", Proceedings of the 45th IEEE Conference on Decision & Control, December 2006, pp. 1159-1164 [29] R. S. Smith, F. Y. Hadaegh, "Control of Deep-space Formation-flying Spacecraft; Relative Sensing and Switched Information", Journal of Guidance, Control, and Dynamics, Vol. 28, no. 1, 2005, pp. 106-114 [30] J. Desai, V. Kumar, J. Ostrowski, "Control of Changes in Formation of Multi- robot Teams", Proceedings of the International Conference on Robotics and Automation, 1998 [31] P. Varaiya, "Smart Cars on Smart Roads: Problems of Control", IEEE Transactions on Automatic Control, Vol. 38, no. 2, 1993, pp. 195-207 [32] T. Balch, R. Arkin, "Behavior-based Formation Control for Multi-robot Systems", IEEE Transaction on Robotics and Automation, Vol, 14, no. 12, 1998 220 [33] K. H. Tan, M. A. Lewis, "Virtual Structures for High Precision Cooperative Mobile Robot Control", Autonomous Robots, Vol. 4, no. 4, October 1997, pp. 387-403 [34] Wei Ren, N. Sorensen, “Distributed Coordination Architecture for Multi-Robot Formation Control”, Robotics and Autonomous Systems, Elsevier, 2007 [35] J. Spletzer, A.K. Das, R. Fierro, C. J. Taylor, V. Kumar, J.P. Ostrowski, “Cooperative Localization and Control for Multi-Robot Manipulation”, Proceedings of IEEE/RSJ International Conference on Intelligence in Robotics and Autonomous Systems, Maui, HI, USA, October 2001 [36] N. Trawny, T. Barfoot, “Optimized Motion Strategies for Cooperative Localization of Mobile Robots”, Proceedings of IEEE International Conference on Robotics and Automation, New Orleans, LA, USA, April 2004 [37] P. Urcola, L. Riazuelo, M. T. Lazaro, L. Montano, “Cooperative Navigation Using Environment Compliant Robot Formations”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008, pp. 2789-2794 [38] C. W. Reynolds, "Flocks, Herds, and Schools: a Distributed Behavioral Model", Computer Graphics, Vol. 21, no. 4, 1987, pp. 71-87 [39] P. Tabuada, G. J. Pappas, P. Lima, "Motion Feasibility of Multi-Agent Formations", IEEE Transactions on Robotics, Vol. 21, No. 3, June 2005, pp. 387- 392. [40] Bin Li, X. H. Liao, Z. Sun, Y. H. Li, Y. D. Song, "Robust Autopilot for Close Formation Flight of Multi-UAVs", Proceedings of the 38th Southeastern Symposium on System Theory, March 2006, pp. 294-298 [41] T. J. Koo, S. M. Shahruz, "Formation of a Group of Unmanned Aerial Vehicles (UAVs)", Proceedings of the American Control Conference, 2001 [42] L. E. Parker, “Path Planning and Motion Coordination in Multiple Mobile Robot Teams”, in “Encyclopedia of Complexity and System Science”, edited by R. Meyers, Springer, 2009 [43] D. Lee, J.E. Cochran, J.H. Jo, “Solutions to the Variational Equations for Relative Motion of Satellites”, AIAA, Journal of Guidance, Control, and Dynamics, Vol. 30, No. 3, May-June 2007, pp. 669-678 [44] J. Desai, T. Sugar, V. Kumar, J. Ostrowski, “A Novel Framework for Kinematic and Dynamic Planning for a Formation of Mobile Robots”, 2002, retrieved from ftp://ftp.cis.upenn.edu/pub/jpo/Preprints/formation_IJRR02.pdf [45] S. Monteiro, E. Bicho, “Robot Formations Generated by Non-Linear Attractor Dynamics”, Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2003, Vol. 2, pp. 2016-2022 [46] R. Olfati-Saber, R. M. Murray, "Distributed Structural Stabilization and Tracking for Formations of Dynamic Multiagents", Proceedings of the 41st IEEE Conference on Decision & Control, December 2002, pp. 209-215 [47] P. N. B. T. Eren, A. S. Morse, "Closing Rank in Vehicle Formations Based on Rigidity", Proceedings of the 41st IEEE Conference on Decision & Control, December 2002, pp. 2959-2964 [48] H. Tanner, V. Kumar, G. Pappas, "Stability Properties of Interconnected Vehicles", Proceedings of the 15th International Symposium on Mathematical Theory of Networks and Systems, August 2002 221 [49] M. V. Kothare, V. Balakrishnan, M. Morari, "Robust Constrained Model Predictive Control using Linear Matrix Inequalities", Automatica, Vol, 32, no. 10, 1996, pp. 1361-1379 [50] Y. Lu, Y. Arkun, "Quasi-Min-Max MPC Algorithms for LPV Systems", Automatica, Vol, 36, 2000, pp. 527-540 [51] B. Soo Kim, A. J. Calise, R. J. Sattigeri, “Adaptive, Integrated Guidance and Control Design for Line-of-Sight-Based Formation Flight”, AIAA, Journal of Guidance, Control, and Dynamics, Vol. 30, No. 5, September-October 2007, pp.703-712. [52] L. Sonneveldt, Q. P. Chu, J. A. Mulder, “Nonlinear Flight Control Design Using Constrained Adaptive Backstepping”, AIAA, Journal of Guidance, Control, and Dynamics, Vol. 30, No. 2, March-April 2007, pp. 322-336 [53] E. Lavretsky, N. Hovakimyan, “Stable Adaptation in the Presence of Actuator Constraints with Flight Control applications”, AIAA, Journal of Guidance, Control, and Dynamics, Vol. 30, No. 2, March-April 2007, pp. 337-345 [54] M. V. Kothare, V. Nevistic, M. Morari, "Robust Constrained Model Predictive Control for Nonlinear Systems: a Comparative Study", Proceedings of the 34th Conference on Decision & Control, Vol. 3, Dec. 1995, pp. 2884-2885 [55] L. G. Bleris, M.V. Kothare, "Real-time Implementation of Model Predictive Control", Proceedings of 2005 American Control Conference, Vol. 6, June 2005, pp. 4166-4171 [56] T. Keviczky, F. Borelli, G. J. Balas, "A Study on Decentralized Receding Horizon Control for Decoupled Systems", Proceedings of American Control Conference, 2004 [57] S. N. Singh, Rong Zhang, P. Chandler, S. Banda, "Decentralized Nonlinear Robust Control of UAVs in Close Formation", International Journal of Robust and Nonlinear Control, Vol. 13, Issue 11, March 2003, pp. 1057-1078 [58] N. Meskin, K. Khorasani, "Fault Detection and Isolation of Actuator Faults in Spacecraft Formation Flight", Proceedings of the 45th IEEE Conference on Decision & Control, December 2006, pp. 1159-1164 [59] D. Folta, "Formation Flying Design and Applications in Weak Stability Boundary Regions", Annals of the New York Academy of Sciences, Astrodynamics, Space Missions, and Chaos, Volume 1017, published May 2004, retrieved from http://www.annalsnyas.org/cgi/content/abstract/1017/1/95 [60] R. Fierro, P. Song, A. Das, V. Kumar, “Cooperative Control of Robot Formations”, in “Cooperative Control and Optimization”, edited by R. Murphey and P. M. Pardalos, Kluwer Academic Publishers, 2002, pp. 73-93 [61] Lockheed Martin, “Multiple Launch Rocket System (MLRS M270A1)”, retrieved on August 2012 from http://www.lockheedmartin.com/us/products/MLRSM270A1.html [62] Lockheed Martin, “GMLRS Rocket System Evolving to Meet Battlefield Demands”, retrieved on August 2012 from http://www.lockheedmartin.com/us/news/trade-shows/singapore/sas-stories/sas- gmlrs.html [63] Office of Naval Research, ONR, Broad Agency Announcement ONR 07-038, "Navigation and Timekeeping Technology", 2007 222 [64] O. Aboutalib, B. Awalt, A. Fung, B. Thai, J. Leibs, T. J. Klausutis, R. Wehling, M. Iames, "All Source Adaptive Fusion for Aided Navigation in Non-GPS Environment", Proceedings of SPIE, Vol. 6575, 657509, AFRL-MN-EG-TP- 2007-7416, March 2007 [65] S. Felter, N. E. Wu, "A Relative Navigation System for Formation Flight", IEEE Transactions on Aerospace and Electronic Systems, Vol. 33, No. 3, July 1997, pp. 958-967 [66] Air Force Institute of Technology, AFIT, "AFIT Research Brings Us One Step Closer to Automated Air Refueling", Air Force Institute of Technology News, November 2005, retrieved from http://www.afit.edu/pa/news.cfm?vwk=65 [67] R. Grabowski, P. Khosla, “Localization techniques for a team of small robots”, Proceedings of IEEE International Conference on Intelligent Robots and Systems, 2001 [68] S. Premvuti, J. Wang, “Relative Position Localizing system for Multiple Autonomous Robots in Distributed Robotic System: System Design and Simulation”, Robotics and Autonomous Systems, Vol 18, 1996, pp. 319-326 [69] R. Kurazume, S. Hirose, S. Nagata, N. Sashida, “Study on Cooperative Positioning System”, Proceedings of the IEEE International Conference on Robotics and Automation, Vol 2, 1996, pp. 1421-1426 [70] M. A. Crespo, J. M. Caas, V. Matellan, “Comparing Bayesian and Monte-Carlo Localization for a Robot with Local Vision”, Proceedings of the 2003 Conference of the Spanish Association for Artificial Intelligence, CAEPIA, 2003, pp. 171–174 [71] R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems”, Transactions of ASME - Journal of Basic Engineering, 82 (Series D): 35-45, 1960 [72] A. H. Jazwinski, “Stochastic Processes and Filtering Theory”, New York: Academic Press, 1970 [73] S. Julier, J. Uhlmann, “A New Extension of the Kalman Filter to Nonlinear Systems”, Proceedings of Int. Symposium on Aerospace/Defense Sensing, Simulation and Controls, Vol. 3068, 1997, pp. 182-193 [74] J. K. Bingham, "Vision-Aided Cooperative Navigation for Multiple Unmanned Vehicles", Thesis, AFIT/GE/ENG, Wright-Patterson AFB, Ohio, 2009 [75] S. Roumeliotis, G. Bekey, “Distributed Multi-robot Localization”, Proceedings of Distributed Autonomous Robotic Systems, Springer-Verlag, 2000, pp. 179-188 [76] A. C. Sanderson, "A distributed algorithm for cooperative navigation among multiple mobile robots", Advanced Robotics, Volume 12, Number 4, 1997, pp. 335-349(15) [77] R. Madhavan, K. Fregene, L. E. Parker, "Distributed Heterogeneous Outdoor Multi-robot Localization", Proceedings of IEEE International Conference on Engineering of Complex Computer Systems, 2002 [78] D. K. Maczka, A. S. Gadre, D. J. Stilwell, "Implementation of a Cooperative Navigation Algorithm on a Platoon of Autonomous Underwater Vehicles", OCEANS 2007, Sept. 29 2007-Oct. 4 2007, pp. 1-6 [79] Li-Chuan Zhang, De-Min Xu, Ming-Yong Liu, Wei-Sheng Yan, "Cooperative Navigation and Localization for Multiple UUVs", Journal of Marine Science and Application, Vol. 8, No. 3, 2009, pp. 216-221 223 [80] K. Ito, K. Xiong, “Gaussian Filters for Nonlinear Filtering Problems”, IEEE Transactions on Automatic Control, 45, 5:910–927, 2000 [81] Liu Xing, Peng Chen, Mu Xiaomin, Du Yang, Wu Sentang, "An Approach for Cooperative Navigation of Multi-Missiles Based on Wireless Datalink", Proceedings of Control and Decision Conference, 2009, pp. 2269-2272 [82] A. Doucet, N. de Freitas, N. Gordon, editors, “Sequential Monte Carlo Methods in Practice”, Springer, 1st ed., 2001 [83] D. Fox, W. Burgard, F. Dellaert, S. Thrun, “Monte-Carlo Localization: Efficient Position Estimation for Mobile Robots”, Proceedings of the National Conference on Artificial Intelligence AAAI/IAAI, 1999, pp. 343-349 [84] D. Fox, W. Burgard, H. Kruppa, S. Thrun, “A probabilistic approach to collaborative multi-robot localization”, Autonomous Robots, Vol. 8(3), 2000, pp. 325 – 344 [85] M. Peasgood, "Cooperative Navigation for Teams of Mobile Robots", Thesis, Department of Mechanical Engineering, University of Waterloo, Ontario, Canada, 2007 [86] J. W. Fenwick, P. M. Newman, J. J. Leonard, "Cooperative Concurrent Mapping and Localization", Proceedings of IEEE Int. Conf. on Engineering of Complex Computer Systems, Vol. 2, 2002, pp. 1810-1817 [87] F. E. Udwadia, “Equations of Motion for Mechanical Systems: a Unified Approach”, International Journal of Nonlinear Mechanics, Vol 31, 1997, pp. 951-958 [88] F. E. Udwadia, R. E. Kalaba, “A Unified approach for the Recursive Determination of Generalized Inverses”, Computers and Mathematics with Applications, Vol. 37(1), 1999, pp. 125-130 [89] F. E. Udwadia, “On Constrained Motion”, Applied Mathematics and Computation, Vol. 164, 2005, pp. 313-320 [90] Lam, T., Udwadia, F. E., and Schutte, A. D., “Precision Formation Flight of Spacecraft in Non-Uniform Gravity Fields,” Proceedings of ASCE “Earth & Space 2006” Conference, NASA/JPL, 2006. [91] Lam, T., “New Approach to Mission Design Based on the Fundamental Equations of Motion,” ASCE Journal of Aerospace Engineering, Vol. 19, 2006, pp. 59-67. [92] Schutte, A. D., Udwadia, F. E., and Lam, T., “Nonlinear Dynamics and Control of a Dumbbell Spacecraft System,” Proceedings of ASCE “Earth & Space 2008” Conference, 2008 [93] Cho, H., and Udwadia, F. E., “Explicit Solution to the Full Nonlinear Problem for Satellite Formation-Keeping,” Acta Astronautica, Vol. 67, 2010, pp. 369-387 [94] Zou, Y., Pagilla, P. R., and Misawa, E., “Formation of a Group of Vehicles with Full Information Using Constraint Forces,” ASME Journal of Dynamic Systems, Measurement, and Control, Vol. 129, 2007, pp. 654-661 [95] Zou, Y., Pagilla, P. R., and Ratliff, R. T, “Distributed Formation Control of Multiple Aircraft Using Constraint Forces,” Proceedings of 2008 American Control Conference, June 2008, pp. 644-649 [96] Zou, Y., and Pagilla, P. R., “Distributed Constraint Force Approach for Coordination of Multiple Mobile Robots,” Journal of Intelligent Robotic Systems, Vol. 56:5-21, 2009, pp. 5-21 224 [97] Zou, Y., Pagilla, P. R., and Ratliff, R. T, “Distributed Formation Flight Control Using Constraint Forces,” Journal of Guidance, Control, and Dynamics, Vol. 32, No. 1, 2009, pp. 112-120. [98] S. Russel, P. Norvig, "Artificial Intelligence: a Modern Approach", Prentice-Hall, 1995 [99] J. C. Latombe, "Robot Motion Planning", Kluwer Academic Publishers, Springer, 1991 [100] J. Baumgarte, “Stabilization of constraints and integrals of motion in dynamical systems”, Computer Methods in Applied Mechanics and Engineering, Vol. 1, 1972, pp. 1–16 [101] Schutte, A. D., Udwadia, F. E., “New Approach to the Modeling of Complex Multibody Dynamical Systems”, Journal of Applied Mechanics, ASME, 2011 [102] Vince, J., “Geometry for Computer Graphics: Formulae, Examples and Proofs”, Springer-Verlag, 2005 [103] J. Kim, S. Sukkarieh, “6DoF SLAM aided GNSS/INS Navigation in GNSS Denied and Unknown Environments”, Journal of Global Positioning Systems, Vol. 4, No. 1-2: 120-128, 2005 [104] J. Kim, S. Sukkarieh, “Real-time implementation of airborne inertial-SLAM”, Robotics and Autonomous Systems, 55, 62–71, Elsevier, 2007 [105] M. Bryson and S. Sukkarieh, “Co-operative Localisation and Mapping for Multiple UAVs in Unknown Environments”, Aerospace Conference, IEEEAC, IEEE, 2007 [106] M. Bryson and S. Sukkarieh, “Architectures for Cooperative Airborne Simultaneous Localisation and Mapping”, Journal of Intelligent and Robotics Systems, Special Issue on Airborne SLAM, Springer, 2009 [107] G. H. Lee, F. Fraundorfer, and M. Pollefeys, “MAV Visual SLAM with Plane Constraint”, IEEE International Conference on Robotics and Automation, IEEE, 2011 [108] Autodesk Maya 2013, usa.autodesk.com/maya, Autodesk Inc., 2012 [109] Wolfram Mathematica 8, www.wolfram.com/mathematica, Wolfram Research Inc., 2012 [110] Simulink, “Simulation and Model-Based Design”, www.mathworks.com/products/simulink, The MathWorks Inc., 2012 [111] MapleSim 6, “Advanced System-Level Modeling”, www.maplesoft.com/products/maplesim, Maplesoft, a division of Waterloo Maple Inc., 2012 [112] Wolfram SystemModeler, “A New Era of Integrated Design Optimization”, www.wolfram.com/system-modeler, Wolfram Research Inc., 2012 [113] Autodesk Inventor, “3D CAD and Simulation for Mechanical Design”, usa.autodesk.com/autodesk-inventor, Autodesk Inc., 2012 [114] MapleSim Connector, “Getting Started with the MapleSim Connector”, retrieved from www.maplesoft.com/products/toolboxes/maplesim_connectivity, Maplesoft, a division of Waterloo Maple Inc., 2011 [115] B. Barrows, “Mathematica Symbolic Toolbox for MATLAB--Version 2.0”, library.wolfram.com/infocenter/MathSource/5344, Wolfram Library Archive, 2007 225 [116] Mathematica Technology, “Linking Technology”, library.wolfram.com/infocenter/BySubject/Mathematica/LinkingTechnology, Wolfram Library Archive, 2012 [117] Mathematica, “MathLink and External Program Communication”, reference.wolfram.com/mathematica/tutorial/MathLinkAndExternalProgramCom municationOverview.html, Wolfram Research Inc., 2012 [118] Autodesk Maya, “Autodesk Maya API White Paper”, images.autodesk.com/adsk/files/maya_api_whitepaper.pdf, Autodesk Inc., 2012 [119] Mathematica Documentaiton, “Geographic Data”, reference.wolfram.com/mathematica/guide/GeographicData.html, Wolfram Research Inc., 2012 [120] Mathematica Documentaiton, “Stiffness Detection”, reference.wolfram.com/mathematica/tutorial/NDSolveStiffnessTest.html, Wolfram Research Inc., 2012 226 APPENDIX A: TRACKING A LINE DEFINED WITH POINT AND NAVIGATION VECTOR, VERSION LZ This appendix includes definition of constraints and corresponding derivation of equations of motion for tracking a line defined with position point and navigation vector. The expression for line constraints, denoted as the version LZ, Φ LZ,k (t) , is defined in Eq. (3.36). They are specified assuming that z component of normalized navigation vector ˆ ν k is not zero, ν kz ≠0 , as defined in Eq. (3.32). Position and orientation of the line is specified with coefficient α x3 , α y3 , β x3 , and β y3 , defined in Eq. (3.33). The constraint Ψ LZ,k (t)= 0 is defined following Eq. (3.15). Corresponding equations of motion are derived using the Udwadia-Kalaba equation, [1, 2], Eq. (3.8). The derivations presented below, for the version LZ, follow the procedure used to obtain equations of motion for the version LX, described in Section 3.3.4. Φ LZ,k (t)= ϕ LZ1,k (t) ϕ LZ2,k (t) " # $ $ % & ' ' = q kx (t)−β x3 −α x3 q kz (t) q ky (t)−β y3 −α y3 q kz (t) " # $ $ % & ' ' (A.1)  Φ LZ,k (t)=  ϕ LZ1,k (t)  ϕ LZ2,k (t) " # $ $ % & ' ' =  q kx (t)−α x3  q kz (t)  q ky (t)−α y3  q kz (t) " # $ $ % & ' ' (A.2)  Φ LZ,k (t)=  ϕ LZ1,k (t)  ϕ LZ2,k (t) " # $ $ % & ' ' =  q kx (t)−α x3  q kz (t)  q ky (t)−α y3  q kz (t) " # $ $ % & ' ' (A.3) Ψ LZ,k (t)= ψ LZ1,k (t) ψ LZ2,k (t) " # $ $ % & ' ' ψ LZ1,k (t)=  q kx (t)−α x3  q kz (t)+µ k  q kx (t)−α x3  q kz (t) ( ) +λ k q kx (t)−β x3 −α x3 q kz (t) ( ) ψ LZ2,k (t)=  q ky (t)−α y3  q kz (t)+µ k  q ky (t)−α y3  q kz (t) ( ) +λ k q ky (t)−β y3 −α y3 q kz (t) ( ) (A.4) 227 A LZ,k = −1 0 α x3 0 −1 α y3 " # $ $ % & ' ' (A.5) b LZ,k = b LZ1,k b LZ2,k ! " # # $ % & & = µ k  q kx (t)−α x3  q kz (t) ( ) +λ k q kx (t)−β x3 −α x3 q kz (t) ( ) µ k  q ky (t)−α y3  q kz (t) ( ) +λ k q ky (t)−β y3 −α y3 q kz (t) ( ) ! " # # $ % & & (A.6) c LZ =1+α x3 2 +α y3 2 (A.7) A LZ,k .M − 1 2 " # $ % & ' + = m c LZ −1−α y3 2 α x3 α y3 α x3 α y3 −1−α x3 2 α x3 α y3 ( ) * * * * + , - - - - (A.8)  q kx (t)= 1 c LZ a kx (t)α x3 2 +a ky (t)α x3 α y3 +a kz (t)α x3 +λ k 1+α y3 2 ( ) β x3 −q kx (t) ( ) +α x3 α y3 q ky (t)−β y3 ( ) +α x3 q kz (t) ( ) +µ k − 1+α y3 2 ( )  q kx (t)+α x3 α y3  q ky (t)+α x3  q kz (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (A.9)  q ky (t)= 1 c LZ a kx (t)α x3 α y3 +a ky (t)α y3 2 +a kz (t)α y3 +λ k α x3 α y3 q kx (t)−β x3 ( ) + 1+α x3 2 ( ) β y3 −q ky (t) ( ) +α y3 q kz (t) ( ) +µ k α x3 α y3  q kx (t)− 1+α x3 2 ( )  q ky (t)+α y3  q kz (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (A.10)  q kz (t)=a kz (t)+ 1 c LZ a kx (t)α x3 +a ky (t)α y3 −a kz (t) α x3 2 +α y3 2 ( ) +λ k α x3 q kx (t)−β x3 ( ) +α y3 q ky (t)−β y3 ( ) − α x3 2 +α y3 2 ( ) q kz (t) ( ) +µ k α x3  q kx (t)+α y3  q ky (t)− α x3 2 +α y3 2 ( )  q kz (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (A.11) 228 APPENDIX B: PLANE AND NAVIGATION VECTOR CONSTRAINTS, VERSION PLZ This appendix includes definition of formation keeping plane and navigation vector constraints, denoted as Ψ PLZ,i , and corresponding derivation of equations of motion. The constraints are defined using version LZ of line, navigation vector, constraints, Φ LZ,k , defined in Appendix A. The constraint Ψ PLZ,i (t)= 0 is defined following Eq. (3.15). Corresponding equations of motion are derived using the Udwadia-Kalaba equation, [1, 2], Eq. (3.8), following the procedure used to obtain equations of motion for the version PLX, as described in Sections 5.1 and 5.2, using Eq. (5.4-6, 5.9). Φ PLZ,i = ϕ P,i ϕ LZ1,i ϕ LZ2,i " # $ $ $ $ % & ' ' ' ' ,  Φ PLZ,i =  ϕ P,i  ϕ LZ1,i  ϕ LZ2,i " # $ $ $ $ % & ' ' ' ' ,  Φ PLZ,i =  ϕ P,i  ϕ LZ1,i  ϕ LZ2,i " # $ $ $ $ % & ' ' ' ' (B.1) Ψ PLZ,i = ψ P,i ψ LZ1,i ψ LZ2,i " # $ $ $ $ % & ' ' ' ' , b PLZ,i = b P,i b LZ1,i b LZ2,i " # $ $ $ $ % & ' ' ' ' µ i =diag µ 1 µ 2 µ 2 [ ] , λ i =diag λ 1 λ 2 λ 2 [ ] (B.2) A PLZ,i = −u x,i −u y,i −u z,i −1 0 α x3 0 −1 α y3 " # $ $ $ $ % & ' ' ' ' (B.3) c PLZ,i =u x,i α x3 +u y,i α y3 +u z,i (B.4) 229 A PLZ,i .M − 1 2 " # $ % & ' + = m c PLZ,i −α x3 −u y,i α y3 −u z,i u y,i α x3 −α y3 u x,i α y3 −u x,i α x3 −u z,i −1 u x,i u y,i ( ) * * * * + , - - - - (B.5) d PLZ,i (t)=λ 1 u x,i q sx (t)−q x,i (t) ( ) +u y,i q sy (t)−q y,i (t) ( ) +u z,i q sz (t)−q z,i (t) ( ) ( ) +µ 1 u x,i  q sx (t)−  q x,i (t) ( ) +u y,i  q sy (t)−  q y,i (t) ( ) +u z,i  q sz (t)−  q z,i (t) ( ) ( ) +u x,i  q sx (t)+u y,i  q sy (t)+u z,i  q sz (t) (B.6)  q kx (t)= 1 c PLZ α x3 d PLZ,i (t)+λ 2 u y,i α y3 β x3 −α x3 β y3 ( ) +u z,i β x3 ( ) +λ 2 − u y,i α y3 +u z,i ( ) q kx (t)+u y,i α x3 q ky (t)+u z,i α x3 q kz (t) ( ) +µ 2 − u y,i α y3 +u z,i ( )  q kx (t)+u y,i α x3  q ky (t)+u z,i α x3  q kz (t) ( ) " # $ $ $ $ % & ' ' ' ' (B.7)  q ky (t)= 1 c PLZ α y3 d PLZ,i (t)+λ 2 u xi α x3 β y3 −α y3 β x3 ( ) +u z,i β y3 ( ) +λ 2 u x,i α y3 q kx (t)− u x,i α x3 +u z,i ( ) q ky (t)+u z,i α y3 q kz (t) ( ) +µ 2 u x,i α y3  q kx (t)− u x,i α x3 +u z,i ( )  q ky (t)+u z,i α y3  q kz (t) ( ) " # $ $ $ $ % & ' ' ' ' (B.8)  q kz (t)= 1 c PLZ d PLZ,i (t)−λ 2 u x,i β x3 +u y,i β y3 ( ) +λ 2 u x,i q kx (t)+u y,i q ky (t)− u x,i α x3 +u y,i α y3 ( ) q kz (t) ( ) +µ 2 u x,i  q kx (t)+u y,i  q ky (t)− u x,i α x3 +u y,i α y3 ( )  q kz (t) ( ) " # $ $ $ $ % & ' ' ' ' (B.9) 230 APPENDIX C: NAVIGATION VECTOR, DIRECTION, TESTS FOR INDIVIDUAL AGENTS MODELED WITH PARABOLIC, CONE, AND CYLINDRICAL SURFACE CONSTRAINTS In order to study systems modeled using surfaces of revolution constraints, the following tests were conducted. The tests are performed for sets of independent agents, without considering formation, terrain, or sensor components. They could be considered as direction, heading, tests. The results provide additional substantiation, verification, of already stated conclusion regarding sensitivity of such systems to scaling coefficients of the surfaces and changes in surface orientation, governed by changes in the direction of navigation vectors. Equations of motion for independent agents are derivations are based on individual constraints, ψ PR,i (t)= 0 , ψ CN,i (t)= 0 , and ψ CL,i (t)= 0 , defined in Eq. (7.13, 7.19, 7.25) for parabolic, cone, and cylindrical surfaces respectively, as described in Section 7.2. Constraint equations and corresponding equations of motion derived using the Udwadia-Kalaba approach, and the corresponding Udwadia-Kalaba equation, [1, 2], Eq. (3.8). The equations of motion are derived in local coordinate systems of the surfaces, xyz i , for the i th agent, as described in Section 7.2. Values of stabilization coefficients are assumed to be constant for all of the agents, µ i =µ and λ i =λ . Equations of Motion with Parabolic Surface Constraint Constraint equation is derived following Eq. (3.7), with A PR,i and b PR,i defined as, A PR,i = −1 2c p q y,i (t) 2c p q z,i (t) " # $ % & ' (C.1) 231 b PR,i =λ q x,i (t)+δ x −c p q y,i (t) 2 +q z,i (t) 2 ( ) ( ) −2c p  q y,i (t) 2 +  q z,i (t) 2 ( ) +µ  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (C.2) Equations of motion are derived with acceleration of unconstrained system denoted as a i (t) . The terms denoted as c PR,i (t) and d PR,i (t) are defined in order to simplify the resulting equations. c PR,i (t)=1+4c p q y,i (t) 2 +q z,i (t) 2 ( ) (C.3) A PR,i .M − 1 2 " # $ % & ' + = m c PR,i (t) −1 2c p q y,i (t) 2c p q z,i (t) ( ) * * * * + , - - - - (C.4) d PR,i (t)= 1 c PR,i (t) a x,i (t)−2c p a y,i (t)q y,i (t)+a z,i (t)q z,i (t)+  q y,i (t) 2 +  q z,i (t) 2 ( ) +λ δ x +q x,i (t)−c p q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +µ  q x,i (t)−2c p q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (C.5)  q x,i (t)=a x,i (t)−d PR,i (t)  q y,i (t)=a y,i (t)+2c p q y,i (t)d PR,i (t)  q z,i (t)=a z,i (t)+2c p q z,i (t)d PR,i (t) (C.6) Equations of Motion with Cone Surface Constraint Constraint equation is derived with A CN,i and b CN,i defined as, A CN,i = −2 q x,i (t)+δ x ( ) 2c n q y,i (t) 2c n q z,i (t) " # $ % & ' (C.7) b CN,i =λ q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +2  q x,i (t) 2 −c n  q y,i (t) 2 +  q z,i (t) 2 ( ) ( ) +2µ q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) (C.8) 232 Equations of motion are derived using terms denoted as c CN,i (t) and d CN,i (t) , defined as follows, c CN,i (t)= 2 δ x 2 +2δ x q x,i (t)+q x,i (t) 2 +c n 2 q y,i (t) 2 +q z,i (t) 2 ( ) ( ) (C.9) A CN,i .M − 1 2 " # $ % & ' + = m c CN,i (t) −q x,i (t)−δ x c n q y,i (t) c n q z,i (t) ( ) * * * * + , - - - - (C.10) d CN,i (t)= 1 c CN,i (t) 2 a x,i (t) q x,i (t)+δ x ( ) −c n a y,i (t)q y,i (t)+a z,i (t)q z,i (t) ( ) ( ) +λ q x,i (t)+δ x ( ) 2 −c n q y,i (t) 2 +q z,i (t) 2 ( ) ( ) +2  q x,i (t) 2 −c n  q y,i (t) 2 +  q z,i (t) 2 ( ) ( ) +2µ q x,i (t)+δ x ( )  q x,i (t)−c n q y,i (t) q y,i (t)+q z,i (t) q z,i (t) ( ) ( ) " # $ $ $ $ $ $ $ % & ' ' ' ' ' ' ' (C.11)  q x,i (t)=a x,i (t)− q x,i (t)+δ x ( ) d CN,i (t)  q y,i (t)=a y,i (t)+c n q y,i (t)d CN,i (t)  q z,i (t)=a z,i (t)+c n q z,i (t)d CN,i (t) (C.12) Equations of Motion with Cylindrical Surface Constraint Constraint equation is derived with A CL,i and b CL,i defined as, A CL,i = 0 −2q y,i (t) −2 q z,i (t)+δ z ( ) " # $ % & ' (C.13) b CL,i =λ q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 ( ) +2  q y,i (t) 2 +  q z,i (t) 2 +µ q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) ( ) (C.14) Equations of motion are derived using terms denoted as c CL,i (t) and d CL,i (t) , defined as follows, 233 c CL,i (t)= 2 q y,i (t) 2 + q z,i (t)+δ z ( ) 2 ( ) (C.15) A CL,i .M − 1 2 " # $ % & ' + = m c CL,i (t) 0 −q y,i (t) −q z,i (t)−δ z ( ) * * * * + , - - - - (C.16) d CL,i (t)= 1 c CL,i (t) 2 a y,i (t)q y,i (t)+a z,i (t) q z,i (t)+δ z ( ) ( ) ( ) +λ q y,i (t) 2 + q z,i (t)+δ z ( ) 2 −r c 2 ( ) +2  q y,i (t) 2 +  q z,i (t) 2 ( ) +2µ q y,i (t) q y,i (t)+ q z,i (t)+δ z ( )  q z,i (t) ( ) " # $ $ $ $ $ % & ' ' ' ' ' (C.17)  q x,i (t)=a x,i (t)  q y,i (t)=a y,i (t)−q y,i (t)d CL,i (t)  q z,i (t)=a z,i (t)− q z,i (t)+δ z ( ) d CL,i (t) (C.18) Model Settings for Navigation Vector, Direction, Tests Models of dynamical systems used for the tests correspond to equations of motion presented above, with values of stabilization coefficients specified as µ =6.0 and λ =8.0, duration of a time step dt =0.05 s, while the test sequence includes n max =50 time steps. Initial conditions for all of the agents are set to q i (t 1 )= p o =[0 0 0] T and  q i (t 1 )=v o =[70.7107 70.7107 0.0] T , such that v o v o = ˆ ν i,1 , where t 1 =0.0 s. The set of external forces, the desired diameter of safe corridors, magnitude of translation of apex for paraboloids and cones, and translation of the main axis of cylinders are set to the values specified in Section 7.3. The tests do not imply following a predefined trajectory, instead they consider generating a sequence of navigation, direction, vectors, ˆ ν i , such that the angles between 234 consecutive navigation vectors remain constant for all of the agents, for all of the time steps, β i =β=∠ ˆ ν i,n , ˆ ν i,n+1 ( ) =const . Navigation vectors for the n th time step are computed by rotating base vector ˆ ν b = 1 2 , 1 2 , 0 ! " # $ % & T by the angle α n in the vertical plane specified by x=y, such that α n =∠ ˆ ν i,n , ˆ x axis ( ) . Absolute value of angles α n is bounded by a constant parameter α max , such that α n ≤α max , where 0<α max <90° . Constant coefficient ω a corresponds to specified frequency of change in values α n , as follows from Eq. (C.19). The selected values, specified ω a =6 and α max =45.0°, result in constant value of β = 7.5° . It shall be noted that since the base vector is rotated in a vertical plane, the angles between horizontal components of the vectors, β XY , remain zero. The resulting directions of navigation vectors, illustrated by arrows, and values of rotation angles are shown in the figures below. α n = 2α max π arcsin sin n−1 ( ) π 2ω a " # $ % & ' " # $ $ % & ' ' ,wheren=1,2,..,n max (C.19) a) b) Figure C.1: Navigation, direction, vectors, a) Shown as arrows, b) Rotation angles. 235 Test Results for Ranges of Parabolic Surfaces The following results are obtained for agents modeled using equations of motion derived with parabolic surface constraint, defined in Eq. (C.6). The agents are divided into two groups, denoted PR I and PR II, corresponding to two ranges of sizes of the surfaces, defined by scaling coefficients c p . Each set contains 20 agents, denoted as PR I i and PR II i , with indices i=1,..,20. The ranges, c p,PRI,i ∈ [0.3, 0.4] and c p,PRII,i ∈ [0.4, 0.8], specify progression of sizes of parabolic surfaces. The values are distributed evenly within a range, such that c p,PRI ={0.3, 0.305263, 0.310526, 0.315789, 0.321053, 0.326316, 0.331579, 0.336842, 0.342105, 0.347368, 0.352632, 0.357895, 0.363158, 0.368421, 0.373684, 0.378947, 0.384211, 0.389474, 0.394737, 0.4} and c p,PRII ={0.4, 0.421053, 0.442105, 0.463158, 0.484211, 0.505263, 0.526316, 0.547368, 0.568421, 0.589474, 0.610526, 0.631579, 0.652632, 0.673684, 0.694737, 0.715789, 0.736842, 0.757895, 0.778947, 0.8}. The surfaces, agent trajectories, and the rest of test results for both sets are shown in the figures below. a) b) Figure C.2: Sets of surfaces , a) PR I, b) PR II. 236 a) b) Figure C.3: Trajectories, a) PR I, b) PR II. a) b) Figure C.4: Angles between velocity directions and navigation vectors, a) PR I, b) PR II. a) b) Figure C.5: Angles between projections of velocity directions and navigation vectors on XY plane, a) PR I, b) PR II. 237 a) b) Figure C.6: Deviation of agent trajectories from centerlines of corridor volumes, a) PR I, b) PR II. a) b) Figure C.7: Velocity  q i (t) , a) PR I, b) PR II. a) b) Figure C.8: Velocity magnitude, a) PR I, b) PR II. 238 a) b) Figure C.9: Forces of constraints F c (t), a) PR I, b) PR II. a) b) Figure C.10: Magnitude of forces of constraints, a) PR I, b) PR II. a) b) Figure C.11: Min and Max values of ||F c (t)|| for the i th agent, a) PR I, b) PR II. 239 a) b) Figure C.12: Values of expressions ψ PR,i (t), a) PR I, b) PR II. Test Results for Ranges of Cone Surfaces The following results are obtained for agents modeled using equations of motion derived with cone surface constraint, defined in Eq. (C.12). The test set consists of 20 agents, denoted as CN i , with i=1,..,20. The set represents a range of cone sizes, defined by scaling coefficient c n . The values within the range are c n,CN,i ∈ [1.0, 10.0], where c n,CN ={1.0, 1.47368, 1.94737, 2.42105, 2.89474, 3.36842, 3.84211, 4.31579, 4.78947, 5.26316, 5.73684, 6.21053, 6.68421, 7.15789, 7.63158, 8.10526, 8.57895, 9.05263, 9.52632, 10.0}. The cones, agent trajectories, and the rest of test results are shown in the figures below. Figure C.13: Cone surfaces and corridor volume. 240 Figure C.14: Trajectories. a) b) Figure C.15: Angles between, a) Velocity directions and navigation vectors, b) Projections of velocity directions and navigation vectors on XY plane. Figure C.16: Deviation of agent trajectories from centerlines of corridor volumes. 241 a) b) Figure C.17: Velocity, a)  q i (t) , b) Magnitude. a) b) Figure C.18: Forces of constraints, a) F c (t), b) Magnitude. a) b) Figure C.19: CN i , values of, a) Min and Max of ||F c (t)|| for the i th agent, b) Expressions ψ CN,i (t) . 242 Test Results for Ranges of Cylindrical Surfaces The following results are obtained for agents modeled using equations of motion derived with cylindrical surface constraint, defined in Eq. (C.18). The test set consists of 20 agents, denoted as CL i , with i=1,..,20. The set represents a range of cylinder sizes, defined by radius r c . The values within the range are r c,CL,i ∈ [4.0, 18.0], where r c,CL ={4.0, 4.73684, 5.47368, 6.21053, 6.94737, 7.68421, 8.42105, 9.15789, 9.89474, 10.6316, 11.3684, 12.1053, 12.8421, 13.5789, 14.3158, 15.0526, 15.7895, 16.5263, 17.2632, 18.0}. It shall be noted that a too stiff condition is detected by the numerical solver for the first 12 agents in the set, with radius r c,i ∈ [4.0, 12.1053] where i=1,..,12. This condition occurred during time steps, t n ∈ [0.15, 1.55] where n=3,..,30, as follows from the results shown in the figures below. Figure C.20: Cylindrical surfaces and corridor volume. 243 Figure C.21: Trajectories. a) b) Figure C.22: Angles between, a) Velocity directions and navigation vectors, b) Projections of velocity directions and navigation vectors on XY plane. Figure C.23: Deviation of agent trajectories from centerlines of corridor volumes. 244 a) b) Figure C.24: Velocity, a)  q i (t) , b) Magnitude. a) b) Figure C.25: Forces of constraints, a) F c (t), b) Magnitude. a) b) Figure C.26: CL i , values of, a) Min and Max of ||F c (t)|| for the i th agent, b) Expressions ψ CL,i (t) . 245 Evaluation of Test Results The test results demonstrate that the models stetting specifying narrower surfaces, with surface points closer to the main axis, result in considerably greater magnitude of forces of constraints. This trend is directly observed for paraboloids and cones, Fig. C.11 and C.19a. It is also present for the cylinders, Fig. C.26a, considering too stiff system condition detected for the set of cylinders with smaller radius. These results support a claim that the presented surface constraints, in conjunction with model setting implying relatively large values of stabilization coefficients, β angles, and high time step frequency, might be considered highly sensitive to the exact size of the surfaces. This, in turn, makes it difficult to use surfaces of revolution constraints as means of providing direction of motion for the agents. However, such design does capable of satisfying the requirement of keeping agent trajectories within the bounds of corridor volumes, as follows from the results shown in Fig. C.6, C.16, and C.23. Another observation, made based on the presented test results, is high degree of sensitivity of the considered constraints to numerical errors, encountered during simulation. In addition to the error directly observed with the values of expressions for the constraints, ψ PR,i (t), ψ CN,i (t), and ψ CL,i (t) , shown in Fig. C.12, C.19b, and C 26.b, there is an indication of errors associated with internal transformation procedure XYZ to/from xyz i , present for all types of these surfaces. Although XY projection components for both initial velocity and navigation direction vectors is zero, the trajectories, velocities, and angles γ i (t) , has non-zero components γ XY,i (t) , as demonstrated by plots shown in Fig. C.5, C.15b, and C.22.b. It is especially pronounced for the models of 246 cylindrical surfaces, and could be considered as a purely implementation issue or aspect of the developed modeling and simulation system. Since current implementation includes direct numerical differentiation for transforming velocity and acceleration components, XYZ to/from xyz i , computational precision achieved with currently used solvers, considering specified interpolation order and other settings, might not be sufficient to support direct numerical differentiation with necessary precision. Application of different types of numerical solvers, changes in solver settings, or changes in the definition of transformation procedure, used for the models, might be desired.

Abstract (if available)

Abstract This work suggests a control method for the terrain-following formation motion of a group of communicating autonomous agents. The presented approach centers on defining a suitable set of constraints for formation keeping task that shall be fulfilled while agents are negotiating an unknown terrain toward the predefined goal location. It allows agents to maintain a general geometric formation shape, while allowing each individual formation member freedom of maneuver, required for terrain collision free motion. Formation structure is defined with the use of virtual leader. Formation keeping constraints are defined with plane surfaces, specified relative to position and navigation vector of the virtual leader. Formation navigation and guidance constraints are defined using navigation vectors of formation members and the virtual leader. Alternative designs for the constraints derived with parabolic, cone, and cylindrical surfaces are considered. Formation control is derived using the Udwadia-Kalaba equation, following corresponding approach to the development of control methods for constraint based dynamical systems, including leader-follower systems defined using geometric constraints. Approach to terrain following motion requiring agents to stay within bounds of cylindrical corridor volumes built around their respective navigation vectors is assumed. Individual formation primitives and multi-level, hierarchical, formation structures are considered. Simulations, based on three degrees of freedom nonlinear model of an agent, performed using Mathematica and specifically developed combined Maya-Mathematica modeling and simulation system, demonstrate that a flexible terrain following formation motion is achieved with the presented sets of constraints.

Linked assets

University of Southern California Dissertations and Theses

Conceptually similar

PDF

Terrain-following motion of an autonomous agent as means of motion planning in the unknown environment

PDF

New approaches to satellite formation-keeping and the inverse problem of the calculus of variations

PDF

Dynamic modeling and simulations of rigid-flexible coupled systems using quaternion dynamics

PDF

Control of spacecraft with flexible structures using pulse-modulated thrusters

PDF

Application of the fundamental equation to celestial mechanics and astrodynamics

PDF

Optimal design, nonlinear analysis and shape control of deployable mesh reflectors

PDF

On the synthesis of dynamics and control for complex multibody systems

PDF

Accelerated Runge-Kutta methods and non-slip rolling

PDF

On the synthesis of controls for general nonlinear constrained mechanical systems

PDF

Dynamic analysis and control of one-dimensional distributed parameter systems

PDF

Control of two-wheel mobile platform with application to power wheelchairs

PDF

New approaches in modeling and control of dynamical systems

PDF

Optimal guidance trajectories for proximity maneuvering and close approach with a tumbling resident space object under high fidelity J₂ and quadratic drag perturbation model

PDF

Dynamic social structuring in cellular self-organizing systems

$The effect of Xᵤ and T_{theta_2} on autonomous in-flight refueling: a global Hawk RQ-4A approach$

PDF

The effect of Xᵤ and T_{theta_2} on autonomous in-flight refueling: a global Hawk RQ-4A approach

PDF

An approach to experimentally based modeling and simulation of human motion

PDF

Building cellular self-organizing system (CSO): a behavior regulation based approach

PDF

An energy method for earthquake resistant design of RC structures

PDF

An analytical dynamics approach to the control of mechanical systems

PDF

Controlled and uncontrolled motion in the circular, restricted three-body problem: dynamically-natural spacecraft formations

Asset Metadata

Creator Latyshev, Simon (author)

Core Title Flexible formation configuration for terrain following flight: formation keeping constraints

School Viterbi School of Engineering

Degree Doctor of Philosophy

Degree Program Aerospace Engineering

Publication Date 01/23/2013

Defense Date 12/12/2012

Publisher University of Southern California (original), University of Southern California. Libraries (digital)

Tag constraints,dynamics,formation,Modeling,OAI-PMH Harvest,simulation,terrain

Language English

Contributor Electronically uploaded by the author (provenance)

Advisor Yang, Bingen (Ben) (committee chair), Jin, Yan (committee member), Udwadia, Firdaus E. (committee member), Wang, Chunming (committee member)

Creator Email latyshev@usc.edu,simonLTT@yahoo.com

Permanent Link (DOI) https://doi.org/10.25549/usctheses-c3-128932

Unique identifier UC11291365

Identifier usctheses-c3-128932 (legacy record id)

Legacy Identifier etd-LatyshevSi-1405.pdf

Dmrecord 128932

Document Type Dissertation

Rights Latyshev, Simon

Type texts

Source University of Southern California (contributing entity), University of Southern California Dissertations and Theses (collection)

Access Conditions The author retains rights to his/her dissertation, thesis or other graduate work according to U.S. copyright law. Electronic access is being provided by the USC Libraries in agreement with the a...

Repository Name University of Southern California Digital Library

Repository Location USC Digital Library, University of Southern California, University Park Campus MC 2810, 3434 South Grand Avenue, 2nd Floor, Los Angeles, California 90089-2810, USA