Sensor-based Control and Localization of Autonomous Vehicles in ... [PDF]

UNIVERSIDADE DE LISBOA ´ INSTITUTO SUPERIOR TECNICO

Sensor-based Control and Localization of Autonomous Vehicles in Unknown Environments Bruno João Nogueira Guerreiro Supervisor: Doctor Carlos Jorge Ferreira Silvestre

Thesis approved in public session to obtain the PhD Degree in Electrical and Computer Engineering Jury final classification: Pass with Distinction Jury Chairperson: Chairman of the IST Scientific Board Members of the committee: Doctor Giuseppe Casalino Doctor Urbano José Correia Nunes Doctor João José dos Santos Sentieiro Doctor Carlos Jorge Ferreira Silvestre Doctor Paulo Jorge Coelho Ramalho Oliveira

2013

UNIVERSIDADE DE LISBOA ´ INSTITUTO SUPERIOR TECNICO

Sensor-based Control and Localization of Autonomous Vehicles in Unknown Environments Bruno João Nogueira Guerreiro

Supervisor: Doctor Carlos Jorge Ferreira Silvestre

Thesis approved in public session to obtain the PhD Degree in Electrical and Computer Engineering Jury final classification: Pass with Distinction Jury Chairperson: Chairman of the IST Scientific Board Members of the committee: Doctor Giuseppe Casalino Full Professor, University of Genova, Italy Doctor Urbano José Correia Nunes Full Professor, Universidade de Coimbra Doctor João José dos Santos Sentieiro Full Professor, Instituto Superior Técnico, Universidade de Lisboa Doctor Carlos Jorge Ferreira Silvestre Associate Professor, Instituto Superior Técnico, Universidade de Lisboa Doctor Paulo Jorge Coelho Ramalho Oliveira Associate Professor, Instituto Superior Técnico, Universidade de Lisboa

Funding Institution: Fundaça˜ o para a Ciência e a Tecnologia

2013

© Copyright, Bruno João Nogueira Guerreiro, all rights reserved. Permissions under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License may be available by contacting the author.

“Everyone who enjoys thinks that the principal thing to the tree is the fruit, but in point of fact the principal thing to it is the seed. ” – Friedrich Nietzsche, Human, All Too Human, Part II.

Abstract This dissertation addresses several problems related to the control and localization of autonomous vehicles, in particular autonomous helicopters, considering unknown environments or scenarios where the traditional methods cannot be applied. Firstly, the formulation and validation of a nonlinear model-based predictive control (NMPC) strategy is proposed, enabling trajectory tracking and terrain avoidance for autonomous vehicles. This control strategy is applied to achieve effective terrain avoidance for autonomous helicopters operating in unknown environments and is further used to tackle the problem of trajectory tracking control for autonomous surface craft (ASC) under constant unknown ocean currents. Being an essential part to this control strategy, the nonlinear model of each vehicle and its derivatives are presented in detail. The problem of trajectory tracking control for autonomous helicopters is also addressed, particularly in operation scenarios where only a relative position is available, based on the measurements provided by laser detection and ranging (LADAR) systems. A nonlinear LADARbased kinematic model is formulated, and a trajectory-dependent error space is defined and represented by a linear parameter varying (LPV) system, enabling the synthesis problem to be stated as a continuous-time H2 control problem, which is solved and implemented within the scope of gain-scheduling control theory. As the precision of LADARs on board autonomous vehicles plays a relevant role in the problems addressed in this dissertation, two calibration methods for the determination of the attitude installation bias are proposed, using optimization tools for Riemannian manifolds. Whereas the first calibration algorithm assumes the existence of a known calibration surface, the second algorithm solves the calibration problem automatically, in the sense that it does not require any prior knowledge on the trajectories of the vehicle or the terrain where the calibration procedure takes place. Finally, this dissertation presents the design, analysis, and experimental validation of a novel sensor-based simultaneous localization and mapping (SLAM) filter with globally asymptotically stable (GAS) error dynamics, with application to uninhabited aerial vehicles (UAVs). Furthermore, an online earth-fixed trajectory and map estimation algorithm is developed using a computationally efficient and numerically robust methodology.

Keywords: unknown environments; autonomous helicopters; laser sensors; trajectory tracking; model predictive control; sensor-based control and localization; calibration; simultaneous localization and mapping; observability; global asymptotic stability. vii

Resumo Esta dissertaça˜ o aborda problemas relacionados com o controlo e a localizaça˜ o de ve´ıculos ´ ´ autonomos, em particular de helicopteros não tripulados, considerando cenários em que o meio envolvente e´ desconhecido ou não permite o uso de métodos tradicionais. Primeiramente, e´ apresentada a formulaça˜ o e validaça˜ o de uma estratégia de controlo preditivo não-linear baseado em modelos para o controlo de movimento e desvio de obstáculos. ´ ´ A estratégia de controlo e´ aplicada a helicopteros autonomos para o controlo com desvio de ´ obstáculos e ainda a ve´ıculos mar´ıtimos de superf´ıcie para o seguimento rigoroso de trajetorias sob a aça˜ o de correntes oceânicas constantes. Sendo imprescind´ıveis para a aplicaça˜ o desta estratégia de controlo, os modelos não-lineares destes ve´ıculos bem como as suas derivadas anal´ıticas são apresentados em detalhe. ´ ´ E´ também abordado o problema do controlo de helicopteros autonomos para o seguimento ´ rigoroso de trajetorias, em cenários de operaça˜ o em que existe a possibilidade de apenas estarem acess´ıveis medidas de posiça˜ o em relaça˜ o a infra-estruturas presentes no meio envolvente. O problema de controlo e´ formulado no aˆ mbito da teoria dos ganhos comutados, desenvolvendo um modelo cinemático baseado em dados laser e definido um novo espaço de erro para o ´ seguimento de trajetorias no espaço do sensor, permitindo assim a s´ıntese de controladores para um conjunto predefinido de regiões de operaça˜ o. Dado o papel preponderante que os sensores laser assumem nos problemas abordados nesta dissertaça˜ o, são ainda propostos dois algoritmos de calibraça˜ o de erros de montagem ´ de sensores laser a bordo de ve´ıculos autonomos. Enquanto que no primeiro algoritmo de calibraça˜ o proposto se pressupõe o conhecimento do terreno onde e´ feita a calibraça˜ o, no segundo algoritmo propõe-se uma calibraça˜ o automática, no sentido em que não e´ assumido ´ nenhum conhecimento a` priori a respeito da trajetoria descrita pelo ve´ıculo ou do terreno onde e´ feita a manobra de calibraça˜ o. ´ Por ultimo, e´ apresentada a formulaça˜ o, análise e validaça˜ o de um novo filtro no espaço do ´ sensor, para localizaça˜ o e mapeamento simultâneos com aplicaça˜ o a ve´ıculos aéreos autonomos, ´ em que a dinâmica do erro de estimaça˜ o demonstra estabilidade global assimptotica. E´ ainda ´ desenvolvido um algoritmo eficiente e numericamente robusto para a estimaça˜ o da trajetoria e do mapa no referencial inercial.

´ ´ Palavras-chave: ambientes desconhecidos; helicopteros autonomos; sensores laser; seguimento ´ de trajetorias; controlo preditivo; controlo e localizaça˜ o baseados em sensores; calibraça˜ o; ´ localizaça˜ o e mapeamento simultâneos; observabilidade; estabilidade global assimptotica. ix

To my family.

Acknowledgments My first words of utmost gratitude go to my scientific advisor, Professor Carlos Silvestre, for his thoughtful guidance, outstanding support, words of encouragement, and friendship. His energy, exceptional intuition, together with a solid scientific knowledge, were fundamental for guiding my work in the right direction, for which I will be forever grateful. I also thank him for the opportunity to be a part of his research team, always fostering high quality standards and a deep concern for the well-being of each individual. I would also like to express my gratitude to the members of my steering committee, Prof. Guiseppe Casalino and Prof. Paulo Oliveira, who provided me with invaluable feedback, greatly contributing to the improvement of this dissertation. My deepest gratitude is also addressed, once again to Prof. Paulo Oliveira, as well as to ´ Prof. Antonio Pascoal, Rita Cunha, Pedro Batista, José Maria Vasconcelos, and Duarte Antunes, for the fruitful discussions and collaboration, which were instrumental for some of the results presented in this dissertation. I also acknowledge the kindness of Prof. Naira Hovakimyan as well as Prof. Chengyu Cao and their hard working and outstanding research group which welcomed me at Virginia Tech in an early stage of my PhD. It was a remarkable collaboration effort and an experience that I will always cherish. I would also like to thank all the present and past members of the DSOR group, which always demonstrated the kindest and most inclusive concern for its members, creating the right environment for work, critical thinking, and long lasting friendship. I would like to express my gratitude to Bruno Cardeira, Luis Sebastião, Manuel Rufino, André Oliveira, Bruno Gomes, and José Tojeira, not only for their friendship and great discussions over lunch, but also for their herculean efforts in the development and testing of all the prototype vehicles. I also thank Pedro Serra, David Cabecinhas, Marco Morgado, Paulo Rosa, João Botelho, João Almeida, Sergio Brás, Tiago Gaspar, and all those of the new generation, for their friendship and support during these years. My final words of gratitude are addressed to my father, my mother, and my sister, who always provided me with support and encouragement throughout these years no matter what sacrifices they had to endure, and to Hélia, for her unconditional love and endless support.

Financial support: This work was partially supported by the FCT project [PEst-OE/EEI/LA0009/2011], project AIRTICI from ADI through the POS Conhecimento Program that includes FEDER funds, and by project AMMAIA, PTDC/HIS-ARQ/103227/2008. The author was partially supported by the PhD student grant SFRH/BD/21781/2005 from the Portuguese FCT POCTI programme. xiii

Contents Abstract

vii

Resumo

ix

Acknowledgments

xiii

Contents

xv

List of Figures

xix

List of Tables

xxiii

List of Theorems

xxv

Notation

xxvii

Acronyms

xxxiii

1 Introduction

1

1.1 Historical Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

1.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

1.3 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

1.4 Summary of Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

1.5 Dissertation Outline and Published Work . . . . . . . . . . . . . . . . . . . . . .

8

2 Helicopter Model

11

2.1 Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

11

2.2 Rigid-body Dynamics and Equilibrium Set . . . . . . . . . . . . . . . . . . . . . .

12

2.3 Helicopter Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

13

2.3.1

Main Rotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

15

2.3.2

The Other Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

20

2.4 Equilibrium Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

22

2.5 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

23

3 Terrain Avoidance NMPC

25

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

25

3.2 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

28

3.2.1

Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

28

3.2.2

State and Input Saturation Constraint . . . . . . . . . . . . . . . . . . . .

30 xv

Contents 3.2.3

Unconstrained Optimization Problem . . . . . . . . . . . . . . . . . . . .

31

3.2.4

Step Size Computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

33

3.2.5

Stability Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

34

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft . . . . . . . . . . . . . . .

35

3.3.1

Model Intrinsic Input Saturation . . . . . . . . . . . . . . . . . . . . . . .

35

3.3.2

Terrain Avoidance Constraint . . . . . . . . . . . . . . . . . . . . . . . . .

36

3.3.3

Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

38

3.4 Trajectory Tracking NMPC for ASCs . . . . . . . . . . . . . . . . . . . . . . . . .

46

3.4.1

Catamaran Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

46

3.4.2

Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

50

3.4.3

Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

56


60

4 Sensor-based Control

63

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

63

4.2 Trajectory Tracking Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . .

65

4.3 Sensor-based Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

66

4.3.1

Pier Geometry and Center Position . . . . . . . . . . . . . . . . . . . . . .

67

4.3.2

Ground Measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

68

4.3.3

LADAR Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

69

4.3.4

Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

70

4.4 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

72

4.4.1

Synthesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

72

4.4.2

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

75

4.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

78

4.5.1

Absolute Position Control . . . . . . . . . . . . . . . . . . . . . . . . . . .

79

4.5.2

Sensor-based Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

80


90

5 Automatic LADAR Calibration 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

93 93

5.1.1

Relevant Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

94

5.1.2

Proposed Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

95

5.2 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

97

5.2.1

3-D Point Reconstruction . . . . . . . . . . . . . . . . . . . . . . . . . . . .

97

5.2.2

Cloud-to-Surface Comparison . . . . . . . . . . . . . . . . . . . . . . . . .

98

5.2.3

Calibration Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

5.3 Optimization Using Euler Angles Parameterization . . . . . . . . . . . . . . . . . 101 5.3.1 xvi

Descent Direction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

Contents 5.3.2

Line Search

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

5.4 Optimization on Riemannian Manifolds . . . . . . . . . . . . . . . . . . . . . . . 103 5.4.1

Descent Direction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

5.4.2

Iterative Line Search . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107

5.4.3

Closed-form Line Search . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107

5.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 5.5.1

Comparative Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109

5.5.2

Automatic Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112

5.6 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115 5.7 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 6 Sensor-based SLAM

123

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123 6.1.1

Sensor-based SLAM Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 124

6.1.2

Online Earth-Fixed Trajectory and Map Estimation . . . . . . . . . . . . . 125

6.1.3

Chapter Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126

6.2 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 6.2.1

Nominal Nonlinear System Dynamics . . . . . . . . . . . . . . . . . . . . 127

6.2.2

Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130

6.3 Observability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130 6.3.1

Preliminary Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

6.3.2

Main Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133

6.3.3

Convergence Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141

6.4 SLAM Filter Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143 6.4.1

Prediction Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143

6.4.2

Landmark Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144

6.4.3

Update Step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144

6.4.4

Basic Loop Closing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

6.5 Earth-Fixed Trajectory and Map Estimation . . . . . . . . . . . . . . . . . . . . . 146 6.5.1

Procrustes Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146

6.5.2

Trajectory and Map Estimation . . . . . . . . . . . . . . . . . . . . . . . . 151

6.5.3

Algorithm Validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153

6.6 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156 6.6.1

Filter Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159

6.6.2

Localization, Mapping, and Consistency . . . . . . . . . . . . . . . . . . . 162

6.7 Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170 xvii

Contents 7 Conclusions and Future Work

173

7.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173 7.2 Directions for Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174 A Helicopter Model Derivatives

177

A.1 Rigid-body Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178 A.2 Main Rotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180 A.2.1 Downwash . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182 A.2.2 Blade Pitching Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183 A.2.3 Blade Flapping Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 184 A.2.4 Force . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185 A.2.5 Moment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189 A.3 Tail Rotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191 A.3.1 Downwash . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193 A.3.2 Force . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193 A.3.3 Moment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194 B Catamaran Model Derivatives

197

B.1 First-order Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 197 B.1.1 Error-space Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 197 B.1.2 Rigid-body Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198 B.1.3 Catamaran Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198 B.1.4 Intrinsic Input Saturations . . . . . . . . . . . . . . . . . . . . . . . . . . . 199 B.2 Second-order Derivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 B.2.1 Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 B.2.2 Catamaran Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202 C Closed-form Step Size Parameters

205

D Proof of UCO Theorem for SLAM

207

Bibliography

211

List of Publications

223

xviii

List of Figures 1.1 Ktesibios water clock. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2

1.2 Helicopter robotic vehicle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

1.3 Inspection of viaducts using robotic helicopter. . . . . . . . . . . . . . . . . . . .

5

1.4 Inspection of power lines using robotic helicopter. . . . . . . . . . . . . . . . . .

6

1.5 3-D reconstruction of power line. . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.1 Helicopter model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

14

2.2 Helicopter main components. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

15

2.3 Helicopter blades motion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

16

2.4 Helicopter blade rotation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

16

3.1 Saturation penalty function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

31

3.2 One-dimensional terrain avoidance penalty function. . . . . . . . . . . . . . . . .

37

3.3 Terrain Avoidance NMPC: position and attitude. . . . . . . . . . . . . . . . . . .

40

3.4 Terrain Avoidance NMPC: velocity and actuation. . . . . . . . . . . . . . . . . . .

41

3.5 Terrain Avoidance NMPC: 3-D trajectory.

. . . . . . . . . . . . . . . . . . . . . .

42

3.6 Terrain avoidance NMPC: actuation saturation test. . . . . . . . . . . . . . . . . .

44

3.7 Terrain avoidance NMPC: velocities saturation test. . . . . . . . . . . . . . . . . .

45

3.8 The Delfim ASC during sea trials. . . . . . . . . . . . . . . . . . . . . . . . . . . .

46

3.9 Smooth saturation functions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

51

3.10 LQR vs. MPC controller implementation. . . . . . . . . . . . . . . . . . . . . . .

52

3.11 Simulation results: trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

53

3.12 Simulation results: generalized position error and velocity. . . . . . . . . . . . .

54

3.13 Simulation results: actuation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

55

3.14 Simulation results: CPU time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

55

3.15 TT NMPC sea trials: 2-D trajectory. . . . . . . . . . . . . . . . . . . . . . . . . . .

57

3.16 TT NMPC sea trials: position error and velocity. . . . . . . . . . . . . . . . . . . .

58

3.17 TT NMPC sea trials: actuation n. . . . . . . . . . . . . . . . . . . . . . . . . . . .

59

3.18 TT NMPC sea trials: CPU time. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

59

4.1 Laser measurements intersecting the pier. . . . . . . . . . . . . . . . . . . . . . .

68

4.2 Helicopter model with LADAR kinematics. . . . . . . . . . . . . . . . . . . . . .

70

4.3 Synthesis model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

76

4.4 Implementation setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

78

4.5 Automatic industrial chimney inspection (absolute position). . . . . . . . . . . .

81 xix

List of Figures 4.6 3-D trajectory tracking: position and attitude errors. . . . . . . . . . . . . . . . .

82

4.7 3-D trajectory tracking: actuation and wind velocity. . . . . . . . . . . . . . . . .

83

4.8 3-D trajectory tracking: linear and angular velocities. . . . . . . . . . . . . . . . .

84

4.9 3-D trajectory tracking: transitions between regions. . . . . . . . . . . . . . . . .

85

4.10 Projections in 2-D space of the sensor-based regions of operation. . . . . . . . . .

86

4.11 Automatic bridge pier inspection, using sensor-based control. . . . . . . . . . . .

87

4.12 3-D sensor-based trajectory tracking: position and attitude. . . . . . . . . . . . .

88

4.13 3-D sensor-based trajectory tracking: actuation and wind velocity. . . . . . . . .

89

4.14 3-D sensor-based trajectory tracking: linear and angular velocities. . . . . . . . .

91

4.15 3-D sensor-based trajectory tracking: transitions between regions. . . . . . . . .

92

5.1 LADAR acquisition coordinate frames. . . . . . . . . . . . . . . . . . . . . . . . .

98

5.2 Point-to-plane distance metric. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

99

5.3 Wolfe rule. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 5.4 Projection of the extrinsic gradient. . . . . . . . . . . . . . . . . . . . . . . . . . . 105 5.5 Comparative calibration example. . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 5.6 Automatic calibration example. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110 5.7 Automatic calibration: errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 5.8 Automatic calibration: failure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114 5.9 Automatic calibration: CPU time . . . . . . . . . . . . . . . . . . . . . . . . . . . 114 5.10 Unmanned helicopter for LADAR data acquisition. . . . . . . . . . . . . . . . . . 115 5.11 Terrain acquired for calibration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 5.12 Automatic calibration experimental data analysis. . . . . . . . . . . . . . . . . . 118 5.13 Automatic calibration experimental cloud data. . . . . . . . . . . . . . . . . . . . 120 5.14 Automatic calibration experimental surface data. . . . . . . . . . . . . . . . . . . 121 6.1 Instrumented quadrotor. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 6.2 Diagram of the overall SLAM algorithm. . . . . . . . . . . . . . . . . . . . . . . . 152 6.3 Translation uncertainty 95% confidence bounds. . . . . . . . . . . . . . . . . . . 155 6.4 Map and trajectory in the earth-fixed reference frame. . . . . . . . . . . . . . . . 158 6.5 Convergence of the sensor-based filter variables. . . . . . . . . . . . . . . . . . . 160 6.6 Vehicle-related state variables. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161 6.7 NIS associations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162 6.8 Time evolution of the number of landmarks. . . . . . . . . . . . . . . . . . . . . . 163 6.9 Map and trajectory in the sensor-based frame, {H}. . . . . . . . . . . . . . . . . . 164 6.10 Maps and trajectories in the earth-fixed frame, {E}. . . . . . . . . . . . . . . . . . 165 6.11 Vehicle pose uncertainty in the earth-fixed frame. . . . . . . . . . . . . . . . . . . 167 6.12 Heading of the vehicle relative to the earth-fixed frame. . . . . . . . . . . . . . . 168 6.13 Detailed view of the map and trajectory in {H}. . . . . . . . . . . . . . . . . . . . 169 xx

List of Figures 6.14 Detailed view of the map and trajectory in {E}, using OptETM. . . . . . . . . . . 170

xxi

List of Tables 2.1 Helicopter parameters: rigid body. . . . . . . . . . . . . . . . . . . . . . . . . . .

15

2.2 Helicopter parameters: main rotor. . . . . . . . . . . . . . . . . . . . . . . . . . .

20

2.3 Helicopter parameters: Bell-Hiller. . . . . . . . . . . . . . . . . . . . . . . . . . .

21

2.4 Helicopter parameters: other components. . . . . . . . . . . . . . . . . . . . . . .

22

3.1 Terrain Avoidance NMPC: iterations and computation times. . . . . . . . . . . .

39

3.2 Terrain Avoidance NMPC: relative improvements. . . . . . . . . . . . . . . . . .

39

3.3 DELFIM Catamaran Parameters. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

48

4.1 Operating regions for the absolute position controller. . . . . . . . . . . . . . . .

79

5.1 Summary of simulation results for the comparative calibration. . . . . . . . . . . 111 5.2 Summary of experimental results. . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 6.1 Performance of the algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154 6.2 Covariance Likelihood Ratio Tests. . . . . . . . . . . . . . . . . . . . . . . . . . . 155

xxiii

List of Theorems 4.1 Assumption . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

67

4.2 Assumption . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

67

4.3 Assumption . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

67

4.1 Remark . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

71

4.1 Definition (Quadratic stability, [BGFB94]) . . . . . . . . . . . . . . . . . . . . . .

72

4.2 Definition (Affine polytopic LPV system) . . . . . . . . . . . . . . . . . . . . . . .

72

4.1 Proposition ([SW00, Proposition 1.19]) . . . . . . . . . . . . . . . . . . . . . . . .

73

4.2 Theorem (Polytopic stability) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

73

6.1 Definition (Observability) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132 6.2 Definition (Observability Gramian and transition matrix) . . . . . . . . . . . . . 132 6.3 Definition (Uniform complete observability, [SD82, Definition VI.3]) . . . . . . . 133 6.1 Lemma ([BSO11d, Lemma 1]) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 6.2 Proposition ([BSO11a, Proposition 4.2]) . . . . . . . . . . . . . . . . . . . . . . . 133 6.1 Assumption (Different and nonzero landmarks) . . . . . . . . . . . . . . . . . . . 134 6.3 Theorem (Observability) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134 6.4 Theorem (Nonlinear system) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135 6.5 Theorem (Sufficient and necessary) . . . . . . . . . . . . . . . . . . . . . . . . . . 138 6.2 Assumption (Bounds on landmarks) . . . . . . . . . . . . . . . . . . . . . . . . . 140 6.6 Theorem (Uniform complete observability) . . . . . . . . . . . . . . . . . . . . . 140 6.7 Theorem (Global asymptotic stability) . . . . . . . . . . . . . . . . . . . . . . . . 141

xxv

Notation N set of natural numbers. Z set of integer numbers. C set of complex numbers. R set of real numbers. Rn set of n-dimensional vectors with real entries. Rn×m set of n × m matrices with real entries. a, A, α, Γ scalar variable or constant. α a,α vector variable or constant. ai element i of a vector a ∈ Rn . A,ΓΓ matrix variable or constant. aij element of a matrix A ∈ Rn×m , located at row i and column j. 0n×m matrix of zeros with dimensions n × m. 1n×m matrix of ones with dimensions n × m. In identity matrix of dimensions n × n. xxvii

Notation 0 matrix or vector of zeros, with dimensions obvious from context. 1 matrix or vector of ones, with dimensions obvious from context. I identity matrix with dimensions obvious from context. (.)T transpose operator of a vector or matrix. (.)H conjugate transpose operator of a vector or matrix. (.)−1 matrix inverse operator. sign (.) signum function of the elements of its argument, possibly a vector or matrix. |.| absolute value operator, for scalar arguments. |A| matrix determinant operator, A ∈ Rn×n . λmax (.) maximum eigenvalue of the argument matrix. λmin (.) maximum eigenvalue of the argument matrix. tr (.) trace of a matrix operator. vec (.) vectorization of a matrix operator, such that, if vec (A) = a with A ∈ Rn×m , then a ∈ Rn m×1 . ⊗ Kronecker matrix product operator. A0 positive definite matrix, in the sense that xT A x > 0 for all x , 0; a positive semidefinite matrix is denoted as A 0. xxviii

Notation A≺0 negative definite matrix, in the sense that xT A x < 0 for all x , 0; a negative semidefinite matrix is denoted as A 0. AB matrix A is greater than matrix B, in the sense that A − B 0 is positive definite; similar definitions can be inferred for A ≺ B, A B, and A B. svd (.) singular value decomposition of the argument matrix, such that, svd (A) = U D VT , where D is a diagonal matrix whereas U and V are unitary matrices. diag(A1 , . . . , An ) block diagonal matrix composed of matrix elements A1 to An . symm (A) = 12 (A + AT ) symmetric component of a square matrix. skew (A) = 21 (A − AT ) skew-symmetric component of a square matrix. S(a) " # 0 −a skew-symmetric matrix function, such that, if a = a ∈ R then S(a) = , and if a ∈ R3 a 0    0 −a3 a2    0 −a1  and can be used to represent the cross product between two then S(a) =  a3   −a2 a1 0 vectors as S(a)b = a × b, where b ∈ R3 too. S−1 (A) skew-symmetric matrix inverse function, such that, S−1 (A) = a ∈ R, if A is a 2 × 2 skewsymmetric matrix, and S−1 (A) = a ∈ R3 , if A is a 3 × 3 skew-symmetric matrix. co (.) convex hull operator. ⊕ union of two sets operator. (.)⊥ orthogonal component operator. (.)> tangent component operator. := quantity is defined as. xxix

Notation ' space is identified with. h., .i internal product operator or canonical metric of a given space. .×. external product operator. h.i expectation operator for stochastic variables. Σ ab covariance matrix between two generic stochastic vectors a, b ∈ Rn , defined by Σ a b = h(a − hai)(b − hbi)T i, and denoted by Σ a if a = b. M(n, R) , Rn×n set of n × n matrices with real entries. O(n) group of n × n orthogonal matrices, O(n) = {U ∈ M(n, R) : U UT = In }. SO(n) special orthogonal group or group of n × n proper rotation matrices, SO(n) = {R ∈ M(n, R) : R RT = In , |R| = 1}. SE(n) special Euclidean group of rigid body transformations in n-dimensional space, where each element (R, p) is composed of a proper rotation matrix and a translation vector, such that SE(n) = {R, p : R ∈ SO(n), p ∈ Rn }. K(n) set of n × n skew-symmetric matrices with real entries, K(n) = {K ∈ M(n, R) : K = −KT }. {I} inertial reference frame. {E} local earth-fixed reference frame. {B} body-fixed reference frame, rigidly attached to the vehicle, with origin not necessarily at the vehicle’s center of mass. {H} horizontally projected body-fixed reference frame. xxx

Notation {CM} body-fixed reference frame, with origin at the vehicle’s center of mass. {L} LADAR-fixed reference frame, rigidly attached to the LADAR sensor and with origin at the laser’s firing point and z-axis indicating the zero scanning angle, which is defined as a rotation about the y-axis. {LB} LASER beam reference frame, with origin at the firing point, y-axis collinear with that of frame {L}, and z-axis oriented opposite to the direction of the laser beam. A B

R

rotation matrix in SO(n) from reference frame {B} to reference frame {A}, with n obvious from the context. Rx (.) rotation matrix in SO(3) about the x-axis of the argument angle. Ry (.) rotation matrix in SO(3) about the y-axis of the argument angle. Rz (.) rotation matrix in SO(3) about the z-axis of the argument angle. kakp =

P n

p i=1 |ai |

p1

p-norm operator of a vector a ∈ Rn , where the subscript p is omitted whenever clear from context, and for p = ∞ it can be seen that kak∞ = maxi=1 |ai |. kAkp =

P P p p1 n m i=1 j=1 aij

p-norm operator of a matrix A ∈ Rn×m , where the subscript p is omitted whenever clear from context; for p = 2 yields the well-known Frobenius norm kAk2F = tr AH A . kukLp =

R ∞ 0

ku(t)kp

p1

Lp -norm operator, where u(t) is a piecewise continuous function mapping [0, ∞) into Rn , for which the Lp -norm exists and is finite; for p = ∞ it can be seen that kukL∞ = supt≥0 ku(t)k. Lnp set of piecewise continuous functions mapping [0, ∞) into Rn , for which the Lp -norm exists and is finite. kTzw kH2 H2 -norm (also called L2 -gain or L2 /L∞ induced norm) of the transfer function operator Tzw : Ln2 → Lm ∞ , from the input signal w(t) to the output signal z(t). xxxi

Notation kTzw kH∞ H∞ -norm (also called L∞ -gain or L2 /L2 induced norm) of the transfer function operator Tzw : Ln2 → Lm 2 , from the input signal w(t) to the output signal z(t).

xxxii

Acronyms 2-D two-dimensional. 61, 107, 117–120, 122, 146, 152, 155, 162, 164, 165, 224 3-D three-dimensional. 4, 5, 8, 13, 16, 18, 36, 72, 79, 80, 86, 111, 114, 118–120, 122–124, 147, 159, 204, 222, 224, 225 ASC autonomous surface craft. 10, 11, 33, 36, 37, 60, 63, 64, 66, 68, 69, 72–74, 77, 221, 223 C C programming language. 51, 68 C++ C++ programming language. 73 CCDA combined constrained data association. 158 CORS continuously operating reference station. 147 CPU central processing unit. 35, 52, 72, 74, 77, 140, 141, 146, 221 ECEF earth-centered earth-fixed. 15 ECI earth-centered inertial. 15 EKF extended Kalman filter. 158, 159, 162, 199, 216, 219, 224 GAS globally asymptotically stable. 10, 13, 157, 171, 177, 181, 183, 216, 224 GPC generalized predictive control. 34 xxxiii

Acronyms GPS global positioning system. 5, 8, 12, 13, 72, 79, 80, 84, 107, 111, 114, 118, 147, 149, 155, 157, 224 ICP iterative closest point. 119 IMU inertial measurement unit. 3, 163, 164, 184, 213 INS inertial navigation system. 5, 8, 12, 79, 80, 84, 118, 155 ISR Institute for Systems and Robotics. 5, 72, 146, 202 IST Instituto Superior Técnico. 5, 72, 146 JCBB joint compatibility branch and bound. 158, 186–188, 202 LADAR laser detection and ranging system. 8, 10, 12, 13, 77, 79, 80, 82, 84, 85, 87, 88, 114, 117–120, 122–124, 146, 147, 152, 155, 222 LMI linear matrix inequality. 12, 79, 81, 82, 90, 91, 93, 95 LPV linear parameter varying. 10, 12, 79, 81, 82, 90–92 LQR linear quadratic regulator. 66, 69, 72 LTV linear time-varying. 13, 14, 100, 157, 159, 168, 170–174, 176, 177, 179–181, 183, 216 ML maximum likelihood. 121 MPC model predictive control. 33–35, 47, 49, 56, 66 xxxiv

Acronyms NED north-east-down. 16 NIS normalized innovation square. 206 NMPC nonlinear model predictive control. 10, 11, 33–40, 43, 47, 49, 57, 60, 66, 68, 69, 72–74, 77, 221–223, 251 PSD power spectral density. 20 RMS root mean square. 140–143, 146 SDG statistical discrete gust. 20 SLAM simultaneous localization and mapping. 10, 13, 157–160, 162–165, 167, 184, 187, 188, 195–197, 199, 202, 204, 206, 208, 210, 213, 216, 219, 222–224 UAV unmanned aerial vehicle. 3, 4, 10, 12, 13, 80, 117, 118, 122, 157, 202, 219 UCO uniformly completely observable. 14, 179, 180 UDP user datagram protocol. 73 UGAS uniform global asymptotic stability. 181

xxxv

1 Introduction The constant technological developments in the automotive, sensor, and computer industries are empowering the development of autonomous vehicles as highly versatile tools for civilian applications, ranging from driverless cars running in crowded streets and highways to the widespread use of the so-called drones in the motion picture industry, in agriculture, or even as a hobby.

1.1

Historical Notes

The development of autonomous vehicles is intrinsically related to the concepts of feedback control and estimation theory. One of the oldest records of the use of feedback mechanical systems dates back to the 3rd century B.C., when the Greek inventor and mathematician Ktesibios developed a more automated and accurate water clock, as the preceding water clocks had a nonlinear effect as the tank level dropped [LMV92]. As described in [Pol06, Sundials and Water Clocks], [Ree20a], [Ree20b], and [Lan78], Ktesibios used a floating valve that could regulate the inflow of water to a tank is such a way that this new water clock was able to stabilize the outflow of water to the final tank, as illustrated in Figure 1.1. This device was then used for accurate time measurement, adding several mechanisms to automate the draining of the tank at the end of each day, such as the use of a siphon. Nevertheless, one of the first records of a more theoretical analysis of a mechanical feedback system is [Max68], addressing the (in)stability of mechanical steam engine governors, which paved the way to a myriad of other significant contributions to control theory in the following decades, most notably [Rou77, Lya92, Nyq32, PN62]. The observation of the sun and constellations of stars to infer position and heading are among the most ancient techniques for localization and navigation [Tay71, Bow02], which are still presently in use for spacecraft navigation [Qua63, XWL10]. Alternatives to this ancient 1

Chapter 1: Introduction

(a) Featuring the regulating valve.

(b) Featuring the emptying mechanism.

Figure 1.1: Ktesibios water clock (from [Ree20b]).

strategy of navigation include odometry-based mechanisms, such as the south-pointing chariot of 12th century China [Lew92], the (mechanical) gyroscope, and, more recently, the inertial measurement unit (IMU) sensors, that ubiquitously equip any modern vehicle or smartphone. The theoretical roots related to these problems date back to the development of the least squares method around the 1800s, credited to Carl Friedrich Gauss [Sti81], the evolution of the probability and measure theories, which preceded the proposal of the Wiener filter, published in [Wie64], and the most celebrated Kalman filter, attributed to [Kal60, KB61, Swe59]. Together with nonlinear filtering techniques, the latter filter is still one of the core elements of the research and development efforts in localization and navigation. It is argued that the history of unmanned aerial vehicles (UAVs) started with the use of balloons by the Austrian military, in 1849, equipped with electromagnetic devices to drop bombs over the city of Venice, Italy. These balloons eventually evolved throughout the subsequent military conflicts into remotely operated airships, building on the “teleautomaton” boat demonstrated by Nikola Tesla [Jon04], and later into full-size remotely operated airplanes and helicopters. Although the underlying concepts were devised many years before, the rise of computers and the significant developments in control and estimation theory, as well as in the practical application of these developments, have led to increasingly higher degrees of autonomy regarding the use of UAVs. The intrinsically low-budget nature of non-military UAVs delayed their use until the end of the 20th century. Nonetheless, the democratization 2

1.2 Motivation

Figure 1.2: Helicopter robotic vehicle.

and miniaturization of consumer-grade computers is said to be carrying out a revolution in the civil aviation airspace with the inclusion of the commercial use of these vehicles. In fact, there are ongoing efforts to develop legislation towards this goal in both Europe and United States [Com12, DI13], which is expected to foster the already vibrant industry and research ecosystems.

1.2

Motivation

Within the scope of UAVs, autonomous rotorcraft have been steadily growing as a major topic of research. They have the potential to perform high precision tasks in challenging and uncertain operation scenarios as new sensor technology and increasingly powerful computational systems are available. Missions like aerial surveillance, automatic infrastructure inspection, or threedimensional (3-D) surface mapping in unknown environments, demand highly adaptable autonomous vehicles that can meet low altitude and hover flight requirements. Reliable navigation and positioning of UAVs are fundamental for any autonomous mission, particularly in unknown environments where absolute positioning systems are absent or unreliable. The motivation for this dissertation arises from the use of autonomous rotorcraft for automatic inspection of critical infrastructures and buildings, such as bridges, electric power lines, power plants, dams, construction areas, etc. Two examples of the use of these vehicles for infrastructure inspection are presented in Figures 1.3 and 1.4, which result from the AIRTICI project, with the scientific coordination of IST/ISR. 3


The structural components of large infrastructures, such as the deck and piers of a bridge or the walls of an industrial chimney, are affected in their strength and durability by age, aggressive environment, and steadily increasing operation demands, especially in places where design, construction, or maintenance errors have occurred [PH08, CIC06]. It is essential for these large infrastructures to be subject to regular maintenance inspections and procedures in order to enable early detection of any defect which may cause unacceptable serviceability or safety risks, as well as serious maintenance requirements. Otherwise, the consequences include complex remedial or maintenance actions as well as the loss of structural, environmental, or public safety integrity. The maintenance cost and risk reduction stemming from the replacement of the standard procedures and the improved detection of structural defects, confirm the future use of automatic structure inspection tools, for which autonomous helicopters are natural platforms that can carry multiple sensors, such as multi-spectral cameras, laser, sonar, or radar sensor systems.

In the considered operation scenarios, a helicopter is expected to follow complex 3-D trajectories, such as helices around bridge piers or power plant chimneys, and if necessary, hover at exact locations to monitor particular characteristics and elements that may require maintenance or repair. However, near these structures, the widely used global positioning system (GPS) signal may be unreliable or completely inexistent, whereas the electromagnetic interference and the existence of ferromagnetic materials may degrade any magnetometer measurement, which is usually an essential part of an inertial navigation system (INS), to the point of becoming unusable. This is of special importance as these dynamically unstable vehicles have to work as close as possible to the inspection target. Therefore, the use of sensor-based control and aided navigation techniques, as proposed throughout this dissertation, aims at solving this problem in such a way that these (possibly unreliable) sensors are made redundant.

Laser detection and ranging systems (LADARs) technology is nowadays widely used by many industrial fields and by the robotics and the remote sensing research communities. The development of airborne laser ranging sensors started in the 1970s in North America, mainly for topographic applications. Later, with the development of affordable INS and GPS units, other applications captured the attention of the research community, such as monitoring ice sheets [KTM+ 95] or measuring canopy heights [NO02]. The robotics research community is nowadays employing autonomous vehicles equipped with LADARs to perform automatic acquisition and 3-D reconstruction of terrain, buildings, or large infrastructures, as shown in Figure 1.5, as well as using this information to safely navigate through unknown environments [TDH03, MM03]. For all these applications the accuracy of the acquired data is essential. 4

1.2 Motivation

(a) During aerial inspection.

(b) High definition image acquired on board.

Figure 1.3: Inspection of viaducts using robotic helicopter, under the scope of project AIRTICI.

5


(a) During aerial inspection.

(b) High definition image acquired on board.

Figure 1.4: Inspection of electric power lines using robotic helicopter, under the scope of project AIRTICI.

6

1.3 Objectives

Figure 1.5: 3-D reconstruction of electric power lines using airborne LADAR and navigation data, acquired using a robotic helicopter, under the scope of project AIRTICI.

1.3

Objectives

With a clear drive for the practical application of the proposed strategies, this dissertation is not bound to a single research subject. Instead, several control strategies, calibration methods, and an estimation filter are proposed to tackle the problems emerging from the application itself. Thus, the fundamental objective of this dissertation is to provide effective algorithms that address the problems related to the control and localization of autonomous helicopters, and autonomous systems in general, when the surrounding environment is unknown or when traditional methods cannot be used due to the unavailability of important sensors. As mentioned above, the envisioned application scenarios include the inspection of large infrastructures, which invariably impose restrictions to the use of GPS and earth magnetic field measurements, either for localization, navigation, or control.

1.4

Summary of Contributions

Noting that the detailed contributions are addressed in the introduction of each chapter, whereas the specific publications resulting from each chapter are provided in the next section, the main contributions of this dissertation are: • the formulation of a nonlinear model predictive control (NMPC) approach for the motion control with terrain avoidance for autonomous vehicles, using simple and well established optimization techniques, validated through simulation results for terrain avoidance control 7

Chapter 1: Introduction of autonomous helicopters, and through both simulation and experimental results for the case of trajectory tracking control for autonomous surface craft (ASC) under constant ocean currents; • the development of trajectory tracking controllers with H2 performance guarantees for polytopic linear parameter varying (LPV) approximations of the system, using an absolute position error dynamics and formulating a novel sensor-based error dynamics for scenarios where only relative position measurements based on LADARs are available; • the development and validation, through simulation and experimental data, of two calibration methods for the determination of the attitude installation bias, using Riemannian optimization tools to solve the calibration problems within the group of proper rotation matrices, formulating: i) a comparative calibration algorithm, which assumes the existence of a known calibration surface, and ii) a novel automatic calibration algorithm, which does not require any prior knowledge on the trajectories of the vehicle or the terrain where the calibration mission is performed; • the design, analysis, and experimental validation of a novel sensor-based simultaneous localization and mapping (SLAM) filter with globally asymptotically stable (GAS) error dynamics, with application to UAVs, as well as the development of an online earth-fixed trajectory and map estimation algorithm with computationally efficient and closed-form solution, with uncertainty characterization. As the motivation for the problems addressed in this dissertation suggest, the author has also been deeply involved in projects related to the application of aerial vehicles for inspection missions, such as projects AIRTICI, AMMAIA, and HELICIM, as well as project MEDIRES, which included the use of sonar and laser devices for the inspection of rubble mound breakwaters.

1.5

Dissertation Outline and Published Work

In this dissertation, the notation and most acronyms are introduced the first time they are used. Nonetheless, the notation summary and a list of acronyms can be found before this introductory chapter. To distinguish between the author’s published work and the remaining literature, the dissertation features two bibliography styles, using only numbers for the former and alphanumeric codes for the latter, combining the initials of the co-authors and the year of publication. Chapter 2 summarizes the helicopter nonlinear dynamic model, which is used for the design and validation of the two control approaches proposed in this dissertation, including the main rotor blade pitching and flapping motions, as well as the contributions of each component of the helicopter for the resulting external forces and moments. The derivatives of the helicopter 8

1.5 Dissertation Outline and Published Work model are also presented in Appendix A, where further details about the model expressions are also provided. Chapter 3 describes a NMPC approach for the design of integrated guidance, control, and terrain avoidance for autonomous vehicles. A simple NMPC formulation is used to adequately address the problem of controlling nonlinear and highly coupled dynamic systems along a desired trajectory while preventing input or state saturations. The control strategy is firstly applied for the terrain avoidance control of autonomous helicopters, where an additional terrain constraint is defined and comprehensive simulation results are provided. A second application features the trajectory tracking control of ASCs under the effect of constant ocean currents, for which a model of a catamaran is presented and its derivatives computed in Appendix B. Both simulation and experimental results of sea trials are presented for the latter application. This work was presented in [4, 6], and extended results were also published in [12] and conditionally accepted for possible publication in [14]. Chapter 4 addresses the trajectory tracking control problem of autonomous rotorcraft in operation scenarios where either an absolute position solution is available or only a relative position solution based on LADARs is possible. The proposed control approach relies on the definition of a trajectory-dependent error space to express the dynamic model of the vehicle, and adopts a LPV representation with piecewise affine dependence on the parameters to model the error dynamics over a set of predefined operating regions. The synthesis problem is stated as a continuous-time H2 control problem, solved using linear matrix inequalities (LMIs) and implemented within the scope of gain-scheduling control theory. For the operation scenario where an absolute attitude and position solution is available, usually based on GPS and INS measurements, the full dynamics and rigid body kinematics are considered for the definition of a trajectory-dependent error space. A novel sensor-based formulation of the vehicle’s kinematics is proposed for the case where the operation of the autonomous vehicle can only rely on a relative measurement of position, namely based on LADAR measurements, and a trajectory-dependent error space is defined to express the dynamic model of the vehicle and the sensor-based kinematics. The effectiveness of the proposed controllers is assessed in a realistic simulation environment using the full nonlinear model of a small-scale helicopter and strong wind disturbances. This work was presented in [2, 7], and field trials with a model scale helicopter are in preparation for the validation of the proposed controllers. The article presented in [2], at the 17th IFAC Symposium in Automatic Control in Aerospace, Toulouse, France, was granted the “Best Student Paper Award”. Chapter 5 proposes two estimation algorithms for the determination of attitude installation matrix of LADARs mounted on board UAVs: a comparative calibration algorithm assumes the existence of a known calibration surface; whereas an automatic calibration algorithm does not require any prior knowledge on the trajectories of the vehicle or the terrain where the calibration mission is performed. The algorithms rely on the minimization of the errors between 9

Chapter 1: Introduction the measured data sets and a representation of the known calibration surface or, alternatively, the errors between the acquired data sets, by comparing each data set with a surface representation of the others. The resulting error functionals are minimized using two optimization techniques: estimating the vector of Euler angles using standard nonlinear optimization tools or using optimization tools for Riemannian manifolds for the direct estimation of the attitude installation matrix on the group of special orthogonal matrices SO(3). The proposed techniques are extensively tested and compared using simulated and experimental LADAR data sets under realistic acquisition conditions. Preliminary versions of this work were presented in [3, 9], and extended results have been submitted for possible publication in [16]. Chapter 6 presents the design, observability conditions, convergence analysis, and experimental validation of a sensor-based filter for SLAM, with GAS error dynamics, which has straightforward application to UAVs in GPS-denied environments. The SLAM problem is formulated in a sensor-based framework and modified in such a way that the underlying system structure can be regarded as linear time-varying (LTV) for observability and filter design purposes, from which a Kalman filter with GAS error dynamics follows naturally. An online earth-fixed trajectory and map estimation algorithm is also developed using a computationally efficient and numerically robust technique. This algorithm follows from the estimation solution provided by the SLAM filter, by formulating an optimization problem with a closed-form solution, which is statistically characterized resorting to perturbation theory. The validation, performance, and consistency evaluation of the proposed sensor-based SLAM filter and the earth-fixed trajectory and map estimation method are successfully assessed with experimental data. A preliminary version of this work was presented in [10] and [11], a more focused paper on the sensor-based filter is published in [13], whereas preliminary work on the generalization of this approach to 3-D has been presented in [18] and in [17]. Finally, Chapter 7 provides refined concluding remarks and broader directions of further investigation. There are also four appendices in this dissertation, which provide the derivatives of the helicopter and catamaran dynamic models, respectively, in Appendices A and B, the computation of a closed-form step size to be used with the calibration algorithm on SO(n), in Appendix C, and the proof that the sensor-based SLAM system dynamics regarded as LTV are uniformly completely observable (UCO), in Appendix D.

10

2 Helicopter Model This chapter introduces the modeling concepts used throughout this dissertation, defining the principal coordinate frames necessary to deduce the rigid-body kinematics, the Newton-Euler dynamic equations and equilibrium set, the helicopter model, and the helicopter equilibrium trajectories.

2.1

Coordinate Frames

In order to properly define the rigid-body dynamics, an appropriate set of coordinate frames must be defined. Firstly, consider {I} as an inertial frame of reference, sometimes defined as a frame where the laws of physics take their simplest form [LL60]. For the problems considered in this dissertation, it is assumed that the mass center of the earth can be considered as the origin of an inertial frame of reference, aligning the z-axis with the rotation axis, pointing north, and the x-axis aligned with the vernal equinox1 , pointing in the direction of the Sun. This frame is usually denoted as the earth-centered inertial (ECI) frame of reference. As the inertial frame {I} does not rotate along with the earth’s rotation motion, it may be convenient for some problems to use earth-centered earth-fixed (ECEF) coordinates, which can be defined similarly to the ECI coordinates, considering the x-axis aligned with the international reference meridian, however. Alternatively, an earth-fixed reference frame denoted by {E} can be defined using a local tangent plane to the earth’s surface or reference ellipsoid at a convenient location. Among the conventions commonly used for the definition of this type of frame, the north-east-down (NED) convention is considered, in which the x-axis points north, the y-axis points east, and the z-axis points towards the earth’s center. An additional coordinate frame is necessary to describe the relative motion of the vehicle: the body-fixed frame {B}. This frame has its origin at a fixed location on the body of the vehicle, usually assumed at the vehicle’s center of mass, x-axis pointing forward, z-axis pointing down, 1 The intersection of the earth’s orbit plane and the earth’s equatorial plane is called the vernal equinox.

11

Chapter 2: Helicopter Model and y-axis pointing right. Nonetheless, if the origin of {B} is not at the vehicle’s center of mass, an equivalently oriented frame with origin at the vehicle’s center of mass, {CM}, can be defined. Further details on the definition of these frames and other considerations about geodetic coordinates can be found in [GWA01, Gro07, Car09].

2.2

Rigid-body Dynamics and Equilibrium Set

The helicopter is modeled as a rigid-body driven by forces and moments applied to the helicopter’s center of mass that include the contribution of the main rotor, tail rotor, fuselage, horizontal tail plane, vertical tail fin, and gravity. Denoting SE(3) := R3 × SO(3) as the special Euclidean group in 3-D space, consider that (E pB , EB R) ∈ SE(3) denotes the configuration of {B} relative to {E} and that the rotation matrix EB R h iT can also be parameterized by the ZYX Euler angles λ B = φB θB ψB , θB ∈ [− π2 , π2 ], φB , ψB ∈ R. This parameterization can be readily defined by using E B

R = Rz (ψB ) Ry (θB ) Rx (φB ) ,

where the notation Rx (.), for instance, denotes the basic rotation matrix defined by the rotation of a given angle about the x-axis. In addition, let the linear and angular velocities of {B} relative h iT h iT to {E}, expressed in {B}, be given by vB = u v w and ω B = p q r , respectively. Then, the kinematic equations of motion of a generic rigid body can be written as  E E  λ B ) vB   p˙ B = B R(λ   ˙  λ B = Q(φB , θB )ω ωB or, alternatively, considering the rotation matrix EB R ∈ SO(3), as (E p˙ B = EB R vB E ˙ E ωB) B R = B R S(ω where the operator S(.) denotes the skew-symmetric matrix such that S(a) b represents the cross product a × b, for some a, b ∈ R3 , and the matrix Q relates the vehicle angular velocity with the time derivative of the Euler angles, being defined by   1 sin(φB ) tan(θB ) cos(φB ) tan(θB )     . cos(φB ) − sin(φB Q(φB , θB ) = 0   0 sin(φB )/ cos(θB ) cos(φB )/ cos(θB ) The dynamic motion of a rigid body is described by the Newton-Euler differential equations [Fea08, Hah02], which, considering that {B} and {CM} are not coincident and explicitly showing the dependence on the earth’s gravity force, can be written as " # " #" # " # ω B ) (S(ω ω B ) B pCM ) fext + m gCM m I3 −m S(B pCM ) v˙ CM m S(ω = + , ω B ) IB ω B next m S(B pCM ) IB ω˙ B S(ω where m is the mass of the body, the tensor of inertia about frame {B} is defined as IB = ICM − m S2 (B pCM ), the position of the center of mass described in {B} is denoted by B pCM , and the 12

2.3 Helicopter Dynamics total external forces and moments acting on the rigid body are respectively denoted by fext and next , excluding the earth’s gravity acting on the body, gCM . Conversely, when the origin of the body-fixed frame {B} is coincident with the center of mass, the inertia matrix is redefined as IB = ICM and the Newton-Euler equations become " # " #" # " # ω B ) vB fext + m gB m I3 0 v˙ B m S(ω = + . ω B ) IB ω B next 0 IB ω˙ B S(ω The dynamic equilibrium of a rigid body requires that the sum of the external forces and the inertial forces acting on the body are zero. This implies that the linear and angular velocities can assume non-zero values while the accelerations are required to be zero. Therefore, the dynamic equilibrium set of a general rigid body can be defined as RB ω B , fext , next , gB ) ∈ R15 : fext + m gB − m S(ω ω B ) vB = 0 ∧ next − S(ω ω B ) IB ω B = 0} . Xeq := {(vB ,ω

In general, the external forces and moments are functions of custom control variables, of the linear and angular velocities of the vehicle, and in many cases of its linear and angular accelerations. In addition, the gravitational force vector is, indeed, a function of the pose of the vehicle, as a different attitude of the vehicle imply a different perception of the gravitational field, as seen from frame {B}, and some models might also consider the variation of this field with the position of the vehicle.

2.3

Helicopter Dynamics

Since the birth of the first usable manned helicopters, arguably during the 1930s, many engineers and researchers have dedicated their lives to the study of rotary-wing aircraft. As the main source of lift and control authority in any helicopter, the rotary-wing aerodynamics has been the subject of extensive study, along with the performance and flight characteristics of the several types and configurations of rotorcraft [SK84, BDB01]. The stability and control analysis, both theoretical and experimental, can also be found in works such as [Pro95] and [Pad96]. In comparison with manned helicopters, small-scale helicopters demonstrate several fundamental differences that justify the design of models especially tailored for these vehicles. These differences include the use of the Bell-Hiller stabilization mechanism, hingeless blades in terms of flapping, higher rotor speeds, and also a substantially higher thrust-to-weight ratio, which enables the helicopter to perform acrobatic 3-D maneuvers. The formal identification, modeling, and control of small-scale helicopters can be traced back to [FOY84, MvNB94, ZvN96], where the authors mainly use frequency domain linear identification tools to obtain an experimental model of the angular dynamics of the vehicle. Another approach is to use physics first principles modeling for the basic dynamic equations of motions, and resort to linear identification tools to obtain models for specific flight conditions. This approach was followed by the authors of [MTK00, Met01, Met03, KT04], where a the full model of a small-scale helicopter is obtained for hover and cruise flight conditions, including an identified linear model of the Bell-Hiller 13

Chapter 2: Helicopter Model m gB

Gravity Rigid-Body

vw uB

fext

λB

vB

Helicopter Components n ext

Dynamics

Kinematics ωB

E

pB

Figure 2.1: Helicopter model.

stabilizing bar. Similar approaches were also used in [Wei94, WG94], [GMF01, Gav03], and [LMK02, La 03], considering more complex first principles modeling, however, maintaining an identified linear dynamics for the Bell-Hiller stabilizing bar. The model presented in this chapter, which was firstly derived in [Cun02], further analyzed in [1], and extended in [Cun07], was one of the first models to use first principles modeling of the Bell-Hiller stabilizing bar. Recently, several authors have also proposed full nonlinear models of small-scale helicopters based on first principles modeling, such as [Bis08] and [Bar12, BL13], and several books on small-scale helicopters can be found in [CCL11] and [RV11], covering the platform and avionics design, modeling and identification, control and simulation, etc. Building on the rigid-body dynamics introduced above, the helicopter nonlinear dynamics can be introduced by considering that the external forces and moments are functions of the control vector, uB , the linear and angular velocity of the vehicle, and the wind velocity vector, vw , described in {B}. Assuming that the gravitational field is fairly constant with the position of the vehicle and its yaw angle, the dynamics and kinematics state-space equations of a helicopter can be written as    ω B ) vB + m−1 fext (vB ,ω ω B , uB , vw ) + gB (φB , θB ) v˙ B = −S(ω      −1  ω B )IB ω B + next (vB ,ω ω B , uB , vw )]  ω˙ B = IB [−S(ω   E  λB ) vB p˙ B = EB R (λ      ˙  λ B = Q (φB , θB ) ω B h The input vector uB = θc0

θc1c

θc1s

θc0t

iT

(2.1a) (2.1b) (2.1c) (2.1d)

comprises the main rotor collective, longitudinal

cyclic, and lateral cyclic blade pitch angle commands, as well as the tail rotor collective blade pitch angle command, respectively. Depending on the complexity of model considered for the blade flap and pitch motions, additional state variables may be required. The wind disturbance input vw can be modeled as a random process with a predefined power spectral density (PSD), such as the von Karman turbulence model, or using the statistical discrete gust (SDG) approach, essentially employed to cater for more structured disturbances, as detailed in [Pad96]. Considering the main components of a typical helicopter, as shown in 14

2.3 Helicopter Dynamics

Figure 2.2: Helicopter main components. Table 2.1: Vario Xtreme helicopter model parameters: rigid body and environmental.

Parameter g ρair m Ixx Iyy Izz

Value 9.81 1.225 4.671 0.0226 0.1898 0.1768

Description Gravity acceleration [m/s2 ] Air density [kg/m3 ] Helicopter’s Mass [kg] Inertia tensor’s component along x axis [kg m2 ] Inertia tensor’s component along y axis [kg m2 ] Inertia tensor’s component along z axis [kg m2 ]

Figure 2.2, the external force and moment vectors, fext and next , can be further decomposed as fext = fmr + ftr + ff us + ftp + ff n and next = nmr + ntr + nf us + ntp + nf n , where the subscripts mr, tr, f us, tp and f n stand for main rotor, tail rotor, fuselage, horizontal tail plane and vertical tail fin, respectively. The rigid body and environmental model parameters, specific of the Vario Xtreme model-scale helicopter are presented in Table 2.1, whereas the derivatives of the proposed model are provided in Appendix A.

2.3.1

Main Rotor

Not only the main rotor is the dominant system in a helicopter, providing lift and enabling the control of the helicopter’s position, orientation and velocity, it is also the most complex one. The motion of each blade can be described by three different degrees of freedom: pitch, flap, and lag, which are depicted in Figure 2.3. For small-scale helicopters, it is usual to assume that the overall influence of the lag, twist and bending of the blades are negligible for low velocity flight conditions. Further consider two additional frames introduced to appropriately present the main rotor expressions: {hw} – Hub/Wind frame. Non-rotating frame, with its origin at the hub, x-axis aligned 15

Chapter 2: Helicopter Model

Flap

Lag

Pitch

(a) Blade pitch.

(b) Blade flap and lag.

Figure 2.3: Helicopter blades motion.

ym= p 2

ym ym= 0 ym w vw ym= p

W

ym= 3 p 2 Figure 2.4: Helicopter blade rotation.

with the component of the helicopter linear velocity relative to the fluid and coincident with the hub plane, and z-axis aligned with the main rotor shaft;

{b} – Blade frame. Coordinate frame attached to a blade, describing rotation, flapping, and pitching motions. The y-axis is aligned with the blade chord.

The main rotor blade azimuth angle ψm is depicted in Figure 2.4, together with the angle ψmw that defines the rotation between frames {B} and {hw}. 16

2.3 Helicopter Dynamics 2.3.1.1

Blade Pitching Motion

Most of the helicopter maneuvering capabilities rely on a mechanism of the main rotor called swashplate, which is responsible for applying a different blade pitch angle at each azimuth of the blade. Let θm (ψm ) be the pitch angle applied to any main rotor blade that have the azimuth ψm , it can be approximated by θm (ψm ) = θc0 + θ1c cos ψm + θ1s sin ψm , where the main rotor cyclic angles, θ1c and θ1s , result from the combination of the Bell-Hiller stabilizing bar (usually called flybar mechanism), the servo-actuation, and fluid velocities at the hub. To define these angles, consider a compact notation where h iT θ 1 = θ1c θ1s , h iT θ c1 = θc1c θc1s ,

h iT ¯ B = p¯ q¯ , ω h iT λ = µz − λ0 λ1c λ1s ,

the variables µ and µz are the normalized x and z-components of the hub linear velocity in frame {hw}, p¯ and q¯ are the normalized x and y-components of the angular velocity expressed in the same frame, whereas Ω, γf , η2 , c1 and c2 denote model parameters (defined in Tables 2.2 and 2.3, as well as in [1, Cun02]). Thus, the blade pitch motion with flybar can be found to be defined by the differential equation θ 1 = Ω2 Bθ (µ)θ θ c1 + Bω ω¯ B + Bλ (µ)λ λ , θ¨ 1 + Ω Aθ˙ θ˙ 1 + Ω2 Aθ (µ)θ where " Aθ˙ =

γf 8

−2

2

#

γf 8

,

Aθ (µ) =

γf 8

"

0 −1 − 12 η2 µ2

1 − 12 η2 µ2 0

# ,

" # γf 1 (1 + c1 ) + 21 (3 − c1 ) η2 µ2 0 Bθ (µ) = , 0 8 c2 − (1 + c1 ) − 12 (1 + c1 ) η2 µ2 # " γf " # γf 1 2 η2 µ 0 −1 1 −2 0 0 8 Bω = , and Bλ (µ) = . γ 0 1 0 c2 −2 − 8f 0 0 8 c2 2.3.1.2

Induced Downwash

As result of the thrust force generated at the hub, due to the rotating motion of the blades and the blade pitch angle, the air is accelerated downwards generating a flow field, usually called induced downwash, that can be approximated by the vertical component for low forward velocities. The downwash can be decomposed in Fourier Series in a similar fashion to that of the blade pitch angle, resulting in λ(ψm ) = λ0 + rm (λ1c cos ψm + λ1s sin ψm ) ,

17

Chapter 2: Helicopter Model with rm expressing the rotor radius integration variable. As the computation of the induced downwash depends on the trust generated by the rotor and vice-versa, an iterative method is usually applied to find good estimates of the collective and cyclic components of this variable, λ0 , λ1c , and λ1s . Alternatively, their value for stationary hover flight can be used as a fair approximation for low velocity flight conditions. A simplified expression for the collective downwash can be given by a s λ0 = − 0 + 16

r

a0 s 16

2 +

a0 s 1 1 θ0 + µz , 4 3 2

whereas the longitudinal cyclic downwash is given by r  λ −µ 2   λ −µ 0 z  1 + − λ0 0µ z , forward climb λ   µ  0r . λ1c =   λ −µ 2   λ0 −µz 0 z   λ0 1 + µ + λ0 µ , forward descent It is noted that these downwash expressions are defined at the {hw} frame, which is aligned with the wind velocity. This implies that the value of λ1s is negligible and, thus, its computation is omitted. 2.3.1.3

Blade Flapping Motion

Also a consequence of the rotation and feathering (blade pitching) motions, the blades describe flap and lag motions, roughly characterized by pulling up and backwards, respectively, the tip of the blade. In this model, the lag motion is neglected and the flap steady-state motion is considered, resulting in θ 01 + B2 (µ) ω¯ B + B3 (µ)λ λ] , β = A−1 0 (µ) [B1 (µ)θ h defining θ 01 = θc0

θ1c θ1s

iT

and the remaining matrices as

 2  0 0  λβ   γ γ 1 2 2 ,  λβ − 1 A0 (µ) =  6 µ  8 1+ 2 µ    γ 0 − 8 1 − 12 µ2 λ2β − 1    4 1 + µ2 0    3 γ   1 2  B1 (µ) =  0 1+ 2 µ 0  , 8 8  0 1 + 32 µ2 3µ  γ  12 µ 0   γ   , B2 (µ) =  2 8    γ −2 8 and  4  0 − 23 µ   3 γ  B3 (µ) =  0 −1 0  , 8  2µ 0 −1 where λ2β and γ denote model parameters defined in Table 2.2. 18

2.3 Helicopter Dynamics 2.3.1.4

Forces and moments at the hub

The main rotor contribution to the total force and moment acting on the helicopter, described on the body frame {B}, can be computed using fmr = Bhw R hw f and nmr = Bhw R hw n + B phw × fmr , whereas the forces and moments at the rotor hub are the sum of each blade contribution described in the hub/wind frame {hw}, that is hw

f=

nb X

hw b

R(ψmi ) b f(ψmi )

(2.2)

hw b

R(ψmi ) b n(ψmi ) ,

(2.3)

i=1

and hw

n=

nb X i=1

where nb is the number of blades, ψmi is the azimuth angle of blade i, while b f(ψmi ) and b n(ψmi ) denote, respectively, the force and moment of a blade at the azimuth ψmi . The total force and moment acting on a blade result from the combination of the aerodynamic and inertial forces and moments acting on that blade, as evidenced in Z Rm b b f= faero (rm )drm − b finertial 0

and b

Z

Rm

n= 0

b

naero (rm )drm − b ninertial ,

(2.4)

considering that Rm is the rotor radius, b faero and b finertial are, respectively, the aerodynamic and inertial forces acting on each blade, whereas b naero and b ninertial denote, respectively, the aerodynamic and inertial moments acting on each blade. It can also be seen that, for hingeless rotors, Equation (2.4) can be approximated by  0   b  k β n ≈  h i R Rβm  0 0 1 0 b naero (rm )drm

    ,  

where kβ is the blade stiffness. Thus, by considering a Fourier Series decomposition of b f and b n, it is possible to rewrite (2.2) and (2.3) as      Z − Z22s   β0  −Y1s  −Z1c − 22c − Z0 n n      hw Z2s fmr ≈ b −Y1c  + b  Z1s − Z22c + Z0  β1c  2 2   2    2 Z0 β1s 0 0 0 19

Chapter 2: Helicopter Model Table 2.2: Vario Xtreme helicopter model parameters: main rotor.

Parameter nb Ω Rm γ Iβ kβ

Value 2 162.9 0.66 3.2813 0.0267 56.095

Description Main rotor’s number of blades Idle main rotor speed [rad/s] Rotor radius [m] Lock number Flap moment of inertia [kg m2 ] Center-spring rotor stiffness [N m/rad]

λ2β

1.079

Sβ

0.1929

Stiffness number, Sβ =

a0 s s1 s2 δ0 δ1 δ2 c hR xCM

6.0831 0.0598 289.72 191.21 0.0060 0.0036 0.1406 0.062 0.2422 0

Blade lift curve slope [1/rad] Downwash constant Force constant Moment constant Blade profile drag coefficient Blade profile drag coefficient Blade profile drag coefficient Blade chord [m] Height of rotor above fuselage reference point [m] x distance from center of mass to fuselage ref. point [m]

kβ 2 βΩ

Flap frequency ratio, λ2β = 1 + I 8 kβ γ Iβ Ω2

and

hw

nmr

   −N1c −N0 − N22c  0   n   ≈ nb  0  + b  N1s −kβ + N22s   2  N0 0 0

  −kβ − N22s   β0     N0 − N22c  β1c  ,  β  0 1s

where Y(.) , Z(.) , and N(.) are the resulting components of the Fourier Series decomposition of the h iT h iT respective elements of b f := X Y Z and b n := L M N , considering that the third and higher order terms are negligible, the rotor rotation velocity is constant, and that the inertial components are also negligible. These expressions and their derivatives are provided in Sections A.2.4 and A.2.5. Furthermore, the main rotor model parameters, specific of the Vario Xtreme model-scale helicopter are presented in Tables 2.2 and 2.3.

2.3.2

The Other Components

The tail rotor, placed in the tail boom in order to counteract the moment generated by the rotation of the main rotor, is responsible for the yaw angle stabilization and control. To model this component we can use all the theory used for the main rotor, neglecting the flapping and pitching motions, however, as this rotor has no Bell-Hiller mechanism and is too small to have relevant blade dynamic motions. Therefore, the tail rotor contributions to the total force and 20

2.3 Helicopter Dynamics Table 2.3: Vario Xtreme helicopter model parameters: Bell-Hiller stabilizer.

Parameter γf lθ lδ lβf l1 l2 c1 c2 c3 η2

Value 1.8085 0.03 0.025 0.045 0.0180 0.0150 0.666(6) 1.466(6) 4.0511 2.2541

Description Flybar lock number Bell-Hiller parameter [m] idem idem idem idem idem idem idem idem

moment can be approximated by    0    ftr ≈ Btr R tr f ' −nbt Z0t    0 and    0    ntr = Btr R tr n + B ptr × ftr ≈ −nbt N0t  + B ptr × ftr ,   0 where nbt is the number of blades of the tail rotor, Z0t and N0t are the relevant force and moment components produced by the tail rotor (as defined in Sections A.3.2 and A.3.3), and the subscript tr

stands for the tail rotor hub frame {tr}. Accurate modeling of the aerodynamic forces and moments generated by the flow surround-

ing the helicopter fuselage is a demanding task. In this model, they are considered as functions of the module of the mean flow velocity vf us , the incidence angle αf us and the sideslip angle βf us , resulting in   SXf CXf αf us , βf us  1 ff us = ρair vf2us  SY f CY f αf us , βf us 2  SZf CZf αf us , βf us

     

and   0  1  nf us ≈ ρair vf2us  VMf CMf αf us , βf us  2 VN f CN f αf us , βf us

    .  

The horizontal tail plane and vertical tail fin are modeled as normal wings, as functions of the incidence angle and sideslip angle, respectively, since they are usually orthogonal. Further details on the complete expressions and deductions can be found in Appendix A and in [1, Cun02], whereas the remaining model parameters are presented in Table 2.4. 21

Chapter 2: Helicopter Model Table 2.4: Vario Xtreme helicopter model parameters: tail rotor and other components.

Parameter nbt a0t st s1t s2t δ0t ct Rt htr ltr Ωt SXf SY f SZf SMf SN f

2.4

Value 2 6.5606 0.0598 17.192 2.1833 0.0051 0.0255 0.127 0.07 0.89 733.05 0.0231 0.0766 0.0634 0.00887 0.01072

Description Tail rotor’s number of blades Tail rotor lift curve slope [1/rad] Tail rotor’s downwash constant Tail rotor’s force constant Tail rotor’s moment constant Tail rotor profile drag coefficient Tail rotor blade chord [m] Tail rotor radius [m] Height of tail rotor hub above fuselage ref. point [m] Distance of tail rotor hub to fuselage ref. point [m] Idle tail rotor rotation speed [rad/s] Fuselage frontal projection area [m2 ] Fuselage side projection area [m2 ] Fuselage top projection area [m2 ] Fuselage volume for pitch [m3 ] Fuselage volume for yaw [m3 ]

Equilibrium Trajectories

This section introduces the concepts of equilibrium set, generalized trimming trajectories, and trimming trajectories for the helicopter model presented above. A comprehensive study of these issues for the case of autonomous underwater vehicles can be found in [Sil00] and references therein. Recalling the state-space equations defined in (2.1), which describe the motion of a helicopter, and assuming a constant earth acceleration vector in frame {E}, that is, h iT gE = 0 0 g , such that gB (φB , θB ) = EB RT gE = RTx (φB ) RTy (θB ) gE , the dynamic equilibrium set of this type of vehicles can be defined as heli ω B , uB , φB , θB ) ∈ R12 : fext (vB ,ω ω B , uB ) + gB (φB , θB ) − m S(ω ω B ) vB = 0 ∧ Xeq := {(vB ,ω

ω B , uB ) − S(ω ω B ) IB ω B = 0 } , next (vB ,ω where the dependence on the wind velocity is omitted, as its value is usually unknown and considered as an exogenous perturbation. g

A generalized trimming or equilibrium trajectory of a vehicle, TC , corresponds to the set of state and input values for which the dynamic component of the state remains in the equilibrium set, i.e., g

heli ω C , uC , E pBC (.),λ λC (.)) ∈ R15 : (vC ,ω ω C , uC , φC , θC ) ∈ Xeq TC := {(vC ,ω },

where the notation (.) in E pBC (.) and λ C (.) denotes that these variables are allowed to be a function of time and vC , ω C , uC , φC , and θC denote the constant trimming values of the linear velocity, angular velocity, input vector, roll angle, and pitch angle, respectively. Moreover, a trimming 22

2.5 Concluding Remarks or equilibrium trajectory denotes the kinematic component of the state which results from a generalized trimming trajectory, that is, g

g

ω C , uC , E pBC (.),λ λC (.)) ∈ TC } . TC := {(E pBC (.), ψB (.)) ∈ R4 : ∃TC , (vC ,ω This definitions implies that the yaw angle, ψB (.), can change without violating the equilibrium condition. Nonetheless, it can be seen that it must satisfy    0   0  = Q(φ , θ )ω   C C ωC , ˙  ψC thus, yielding a constant yaw rate, ψ˙ C . The set of achievable trimming or equilibrium trajectories, E, is defined by the collection of all the trimming trajectories TC that the vehicle can perform. As shown in [Sil00], this set can be described, for the considered type of vehicles, by      0  VC cos(γC ) cos(ψ˙ C t + ψCT )     λ˙ C =  0  , E p˙ C =  VC cos(γC ) sin(ψ˙ C t + ψCT )  , ˙    ψC −VC sin(γC ) where VC = kvC k denotes the norm of the linear body velocity, γC the flight-path angle, and ψCT stands for the angular difference between the helicopter desired heading, ψC , and the tangent vector to the equilibrium trajectory, ψT . Therefore, the set of achievable helicopter trimming trajectories, E, can be adequately parameterized by the vector h iT ξ = VC γC ψ˙ C ψCT .

(2.5)

It is also possible to see that these trajectories correspond to straight lines, horizontal circles, and z-aligned helices described by the vehicle with arbitrary, but constant, linear velocity and relative yaw angle.

2.5

Concluding Remarks

Together with the detailed expressions and derivatives provided in Appendix A, this chapter paves the way for the use of the helicopter model in several of the following chapters, by presenting a highly nonlinear and complex model especially tailored for small-scale helicopters. This model is a simplified version of a more comprehensive model derived from physics first principles, which was presented in [Cun02] and further analyzed for simplification in [1]. Nonetheless, it still describes complex, yet fundamental, dynamic components of these vehicles, including the Bell-Hiller mechanism, the coupling between the tail and main rotor, the influence of the cyclic downwash, and the influence of the wind velocity, which is of paramount importance for testing the effectiveness of any control strategy.

23

3

Terrain Avoidance Nonlinear Model Predictive Control

This chapter describes a nonlinear model predictive control (NMPC) approach for the design of integrated guidance, control, and terrain avoidance for autonomous vehicles. A simple NMPC formulation is used to adequately address the problem of controlling nonlinear and highly coupled dynamic systems along a desired trajectory while preventing input or state saturations and, possibly, avoiding collisions with the terrain. The control strategy is firstly applied for the terrain avoidance control of autonomous helicopters, where an additional terrain constraint is added and extensive simulation results are provided. A second application features the trajectory tracking control of autonomous surface craft (ASC) under the effect of constant ocean currents, for which both simulation and experimental results of sea trials are presented.

3.1

Introduction

The discrete model predictive control (MPC) formulation amounts to solving at each sampling instant a discrete finite horizon optimal control problem, subject to input and state constraints, yielding a sequence of optimal control actions, and applying the first element of this sequence to the plant. When MPC techniques are based on nonlinear models, non-quadratic cost-functionals, and/or general nonlinear constraints, they are referred to as nonlinear model predictive control (NMPC). The basic concepts inherent to MPC techniques can be traced back to the 1960s, when the connections between minimum time optimal control and linear programming were recognized in [ZW62] and the moving horizon approach was introduced [Pro63]. Interestingly enough, the first algorithms closely related to the present form of MPC were developed and implemented in the context of the process industry during the 1970s and 1980s, such as model predictive heuristic control [RRTP78], dynamic matrix control [CR80], model algorithmic control [MRE+ 82] and internal model control [GM82]. The main focus of the first MPC techniques was the performance achieved in real petro-chemical and process industry applications, gaining popularity due to its 25

Chapter 3: Terrain Avoidance NMPC natural ability to cope with constraints, nonlinearities and uncertainties. These methods did not ensure stability a priori and required fine tuning of the design parameters, using Monte Carlo simulations to attest for the desired behavior of the overall closed loop system. With few results during the 1980s [CS82, KG88], it was in the 1990s that the research community devoted considerable effort to the stability analysis of either MPC techniques. The main approach was to change the optimization problem, by adding a terminal equality constraint, terminal constraint set, and/or terminal cost functional, yielding a plethora of slightly different methods. Such methods can be found in [RM93, MM93, AB95] and also in [CA98, dNMMS00, JYH01, Mag02], where the main focus is the suppression of the terminal constraint. A closely related method, developed in the adaptive control literature, is the generalized predictive control (GPC), which considers input/output models with no access to the system state. The stability of GPC methods was also addressed during the same period, and [MLZ90] was one of the first results to appear. Some recent results focus on explicit solutions of the MPC optimization problem [BMVD02, GKJ08] or on numerical methods for fast online solution of large scale NMPC problems [ZB09]. As for MPC applications, the vast majority is documented in the process industry, where there is a strong economic drive to push the systems toward their limits of operation. The models used in this kind of applications are inherently nonlinear, constrained and with very large time constants, allowing for the use of large sampling times [GPM89, QB03]. With the availability of increasingly faster CPU capabilities in the last decade, MPC techniques are being considered for the control of faster systems. For instance, the control of autonomous vehicles using MPC, as documented in [SB00, KSS02, SKS03, KB06], is progressively revealing promising results. Further details on the theoretical developments and applications of MPC techniques can be found in [GPM89, MRRS00, FIAF03, MRA09]. In this chapter a simple NMPC methodology is proposed in order to simultaneously meet the conflicting control objectives of trajectory tracking, input or state saturations, and terrain avoidance. As in the NMPC approach the optimal control problem is solved on-line, it is straightforward to add state and input saturation constraints as penalty functions to the cost functional, whereas the vehicle dynamic model constraint can be readily incorporated in the cost functional using Lagrange multipliers. An alternative formulation for the input saturation is proposed, incorporating this information in the nonlinear model of the vehicle, and used instead of, or together with, the optimization problem saturation constraints. This improved model allows for the optimization algorithm to provide valid control actions, even without using constraints in the cost functional. The resulting unconstrained optimization problem is then numerically solved using the gradient or quasi-Newton methods to compute the search direction and using the Wolfe conditions for the line search algorithm to solve the step size optimization subproblem [NW99]. The first application of the NMPC strategy presented in this chapter is the terrain avoidance 26

3.1 Introduction control of autonomous helicopters, without assuming any prior knowledge on the terrain. Related work using different control methodologies in the context of terrain following for autonomous helicopters, can be found in [PSC06]. The authors use gain-scheduling control techniques to control and guide the vehicle along a path, in such a way that it is possible to incorporate preview information to achieve terrain following. In [LS04], an MPC-based nap-ofearth flight trajectory optimization for a helicopter is designed, resorting to an input-output mapping of control functions and resultant system trajectories, while in [KSS02] and [YSK+ 09] obstacle avoidance NMPC techniques are applied in the field of autonomous helicopters and ground vehicles, respectively. To enforce the terrain avoidance, a virtual repulsive field is defined around the helicopter such that any obstacle within its range is weighted in the optimal control cost functional, guiding the vehicle trajectory away from collisions. This penalty functional is based on the exponential function instead of the usual infinite barrier function, which in certain cases may demonstrate undesirable behaviors. This part of the chapter considers the helicopter nonlinear dynamic model already presented in Chapter 2, which was derived from first-principles and is especially suited for model-scale helicopters [1, Cun02]. The control algorithm relies on a simplified version of the referred model to compute the sequence of state vectors given a sequence of input vectors and, to be effectively used in the optimization algorithm, the respective simplified model derivatives are computed and presented in Appendix A. The second application is the problem of trajectory tracking control of ASCs, which is particularly relevant when performing time-critical missions. Trajectory tracking controllers are traditionally based on a two-step design methodology: a fast inner loop that stabilizes the vehicle’s attitude and, using a time-scale separation criterion, a slower outer loop that relies on the kinematic equations of the vehicle and converts the tracking errors into inner loop commands. An integrated approach to the design of inner-outer loop control structures for autonomous vehicles moving in 3-D space was proposed in [KPHS98, SPK02]. The application of similar linearization and gain-scheduling techniques can be found in [GSPC07], featuring the control of the DelfimX ASC. Other approaches to the control of these vehicles include nonlinear or adaptive control techniques, as demonstrated in [EPA00, DJP04]. The vehicle model adopted in this part of the chapter is a catamaran ASC nonlinear dynamic model derived from first physics principles [Pra02, Fos94], and the respective derivatives are also presented in Appendix B. The model captures the influence of ocean currents to better evaluate the performance of the controller under realistic scenarios. The key contributions of this chapter are: i) the use of simple and well established optimization techniques to solve the terrain avoidance and the trajectory tracking NMPC problems for complex nonlinear vehicle models; ii) the comparison of the gradient method with fixed step size and the quasi-Newton method with line search, providing indicators that the later optimization approach is a simple yet effective way to reduce the computation burden inherent 27

Chapter 3: Terrain Avoidance NMPC to NMPC techniques; iii) the implementation of intrinsic saturations within the vehicle model, simplifying the controller design by removing the need to include input constraints in the optimization problem; iv) the validation of the terrain avoidance NMPC for autonomous helicopters in simulation environment; and finally, v) the validation of the trajectory tracking NMPC for ASC under the effect of constant ocean currents, both in simulation and experimentally. The chapter is organized as follows. Section 3.2 formulates the general NMPC problem, describing the optimization algorithm as well as the saturation and model constraints. The terrain avoidance NMPC for autonomous rotorcraft is presented in Section 3.3, featuring the intrinsic saturations and the terrain avoidance penalty function definitions, as well as the simulation results. Section 3.4 presents the trajectory tracking NMPC for ASCs, including the catamaran nonlinear model and error space definition, as well as both the simulation and experimental results. Finally, some concluding remarks and directions for future work are provided in Section 3.5.

3.2

Controller Design

To address the two conflicting objectives of trajectory tracking and terrain avoidance proposed in this chapter, the NMPC problem is formulated as a discrete-time open-loop optimal control problem with finite horizon, subject to the discrete nonlinear dynamic model equations, and generic constraints that can be included in the problem as penalty functions, such as the state and input saturation constraints and the terrain avoidance constraint. This section formulates the NMPC problem for a generic discrete-time nonlinear model, and presents the algorithm used in the remainder of the chapter.

3.2.1

Problem Formulation

Consider the generic continuous-time nonlinear dynamic model given by x˙ c (t) = fc (xc (t), uc (t)) . In the NMPC approach used in this chapter it is necessary to find a discrete-time representation of this model. There are several methods available to obtain a discrete-time representation of the equations of motion of the vehicle, featuring different complexity and integration errors, from which the simplest is to use the forward Euler discretization, resulting in the difference equation xc ((k + 1)Ts ) = xc (kTs ) + Ts fc (xc (kTs ), uc (kTs )) , where Ts denotes the sampling interval. The previous equation can be rewritten using a compact notation as xck+1 = fd xck , uck .

28

3.2 Controller Design The intensive computational requirements of NMPC techniques discard the possibility of neglecting the processing time in comparison with the sampling interval. A fair assumption is to consider that the time needed for the computation of the control law is smaller than the sampling interval. In recent literature, some new algorithms are proposed to tackle this problem, such as the advanced step algorithm described in [ZB09]. For simplicity, the classic approach is used in this chapter, which considers a delay of exactly one sample period Ts , assuming that the instant when a new control action is applied coincides with the measurement of the state variables in the subsequent sampling instant. To accomplish this, the model is augmented with an extra delay state yielding the new state vector xk = [xTck xTuk ]T , the input vector is defined as uk = uck , and the new model function can be denoted as # " fd xck , xuk f (xk , uk ) = . uck Therefore, the vehicle dynamics can be modeled as a discrete-time state-space equation xk+1 = f (xk , uk ) ,

(3.1)

with state xk ∈ X and input uk ∈ U , where X ⊂ Rnx and U ⊂ Rnu denote the admissible sets of the state and control vectors, respectively. At each instant of time, the NMPC algorithm uses the nonlinear model of the vehicle and the measured state to predict the evolution of the system within a predefined time horizon. For the sake of simplicity, and without loss of generality, each instant k is considered to be the initial instant of the prediction horizon, so that throughout this section xk+i and uk+i are respectively denoted as xi and ui . Let N be the prediction horizon of the control problem, U = {u0 , . . . , uN −1 } the sequence of control inputs, and X = {x0 , . . . , xN } the sequence of state vectors generated by that control sequence. Note that the state sequence is a function of the initial state vector and the control sequence, i. e. X = X(x0 , U). The saturation constraints of the state and input sequences are defined by the conditions X ∈ XN and U ∈ UN , considering the sets XN = {X : xi ∈ X , ∀i=0,...,N } and UN = {U : ui ∈ U , ∀i=0,...,N −1 }. Using (3.1) and denoting the model function as fi := f(xi , ui ), the model constraint for a horizon of N steps ahead can be written as    f0 − x1    ..  = 0 fM (X, U) =  .    fN −1 − xN and any additional state constraints, which can later be reformulated into penalty methods, are also considered as fC (X) = 0. Thus, the NMPC optimization problem can be defined as U∗ = arg min J0

(3.2a)

s.t. X ∈ XN , U ∈ UN

(3.2b)

fM (X, U) = 0

(3.2c)

fC (X) = 0

(3.2d)

U

29

Chapter 3: Terrain Avoidance NMPC where J0 = J(X, U) = FN + Fi = F(xi ) =

N −1 X

Li ,

i=0 1 ¯ i )T 2 (xi − x

Li = L(xi , ui ) =

1 2

P (xi − x¯ i ) , h i (xi − x¯ i )T Q (xi − x¯ i ) + (ui − u¯ i )T R (ui − u¯ i )

noting that P, Q, and R are symmetric positive definite matrices, whereas the known reference state and input vector sequences are respectively denoted as x¯ i for all i = 0, . . . , N and u¯ i for all i = 0, . . . , N − 1. Additionally, the constraint defined in (3.2d) accounts for any additional constraints, such as that of the terrain avoidance penalty function defined in Section 3.3.2. In brief, the NMPC objective is to find, at each instant, the optimal control sequence U∗ with horizon N , such that the distance to the full-state trajectory and the control effort are minimized through the cost functional J0 , without violating the state and input constraints defined in (3.2b), while considering other mission specific goals, such as keeping the vehicle within a safety distance from the terrain. The constrained optimization problem presented above can be simply reformulated as an unconstrained optimization problem and the optimal solution can be estimated using gradient methods. While constraint (3.2c) is eliminated using Lagrange multipliers, constraints (3.2b) and (3.2d) are incorporated in the cost functional resorting to penalty methods. The following subsections describe the saturation and model constraints, as well as their inclusion in the unconstrained optimization cost functional.

3.2.2

State and Input Saturation Constraint

The saturation constraints defined in (3.2b) are included in the optimization problem to complement the intrinsic input saturation that will be described in Section 3.3.1, thus enabling the definition of mission specific bounds for both the state and input vectors. These constraints can be incorporated in the cost functional as a penalty function FS (x, u) , which is zero-valued for x ∈ X and u ∈ U and behaves as a quadratic function outside these sets. Let the i-th component of a vector a ∈ Rn be denoted as ai . Assuming that the admissible sets for state and input vectors are, respectively, given by X = {x ∈ Rnx : xj ≤ xj ≤ xj ∀j=1,...,nx } and U = {u ∈ Rnu : u l ≤ ul ≤ u l ∀l=1,...,nu } , the penalty saturation function can be defined as FS (x, u) =

nx X j=1

30

fS (xj ) +

nu X l=1

fS (ul ) ,

3.2 Controller Design

fS(a) a ¡

a acenter ¡ arange

a

fS’(a) Figure 3.1: Saturation penalty function.

where, for some generic scalar variable a, fS (a) = 12 h2 (|a − acenter | − arange ) wa , wa is a positive scalar weight chosen for a, acenter := (a + a)/2, arange := a − acenter , and    a h(a) =   0

, if a > 0 . , otherwise

An illustration of this type of function is provided in Figure 3.1. The derivatives of this constraint function are computed analytically using the derivative of the generic function fS (a), given by d fS (a) = sign (a − acenter ) h(|a − acenter | − arange ) wa , da where sign (.) denotes the signum function of its argument. Although this penalty function is not twice continuously differentiable, the signum function can be easily approximated by a smooth function, such as sign (a) ≈ tanh(c a) for some c 1.

3.2.3

Unconstrained Optimization Problem

Adding the state and input saturation constraint defined above to the optimization cost functional, as well as any other penalty function FC (xi ) based on fC (X), the new optimization problem can be written as U∗ = arg min J¯0

(3.3a)

s.t. fM (X, U) = 0

(3.3b)

U

where ¯ U) = F¯N + J¯0 = J(X,

N −1 X

L¯ i ,

i=0

¯ i ) = Fi + FS (xi , 0) + FC (xi ) , F¯i = F(x ¯ i , ui ) = Li + FS (xi , ui ) + FC (xi ) . L¯ i = L(x 31

Chapter 3: Terrain Avoidance NMPC The model constraint (3.3b) is solved by resorting to the elimination method using Lagrange λ1 , . . . ,λ λN } and the Hamiltonian multipliers. Introducing the Lagrange multiplier sequence Λ = {λ T Hi = H (xi , ui ) = L¯ i + λ fi , the cost functional J¯0 can be rewritten as i+1

J¯0 = F¯N − λ TN xN +

N −1 h X

i Hi − λ Ti xi + H0 .

i=1

For a fixed initial state x0 , the first order condition of optimality yields ∂ J¯0 ∂ Hi = − λ i = 0 , ∀i=1,...,N −1 , ∂xi ∂xi ∂ J¯0 ∂ F¯N = − λN = 0 , ∂xN ∂xN ∂ J¯0 ∂ Hi = = 0 , ∀i=0,...,N −1 , ∂ui ∂ui where

∂ Hi ∂ui

=

∂ L¯ i ∂ui

∂ fi + λ Ti+1 ∂u and i

∂ Hi ∂xi

=

∂ L¯ i ∂xi

(3.4) (3.5) (3.6)

∂ fi + λ Ti+1 ∂x . Using (3.4) and (3.5) to define the Lagrange i

multipliers as λN =

∂ F¯N ∂xN

and λ i =

∂ Hi , ∂xi

∀i=1,...,N −1 ,

the first order conditions of optimality reduce to (3.6). Therefore, an iterative algorithm based on gradient methods can be applied to estimate the optimal control sequence, U∗ , updating the current control sequence estimate at each iteration according to ∆, U ← U + s∆

(3.7)

where the step size is denoted by s and the search direction by ∆ . This optimization procedure is summarized in Algorithm 1. Algorithm 1 Minimization algorithm for the NMPC unconstrained problem. Require: Current state x0 1: Initialize U and X 2: repeat 3: Compute {λ nλ∂ iH} ,oi = N , . . . , 1 4: Compute ∂u i , i = 0, . . . , N − 1 i 5: Compute the search direction ∆ 6: Compute the step size s using Wolfe conditions ∆ 7: Update the control sequence using U ← U + s∆ 8: Update the state sequence X using xi+1 = f (xi , ui ) for all i = 0, . . . , N − 1 ¯ 9: Update

the

cost function J0 and its derivatives ¯ 10: until ∇J0 ≤ ε 11: Let U∗ = U be the final estimate 12: Set the next initial solution to U = {u∗1 , . . . , u∗N −2 , u∗N −1 , u∗N −1 } 13: Apply input vector u∗0 to the system 32

3.2 Controller Design Consider that each sequence can be implemented as a single column vector, which for the h iT case of the search direction sequence results in ∆ = δ T0 · · · δ TN −1 ∈ Rnu (N −1)×1 . Thus, the search direction can be obtained using the quasi-Newton method, yielding ∆ = −D ∇H , where ∇H is the sequence of Hamiltonian derivatives

∂ Hi , ∂ui

for all i = 0, . . . , N − 1 , and D is either

the inverse matrix of the second-order derivative of the Hamiltonian sequence, or an estimate of this matrix, obtained by using a quasi-Newton algorithm. Considering that the superscript (.)− denotes the argument variable in the previous iteration of the optimization algorithm, the quasi-Newton algorithm computes D = D− + with p := U − U− , q := ∇H − ∇H− , v :=

p p T D− q q T D− + + ξ τ v vT , pT q qT D− q p pT q

−

D− q τ ,

τ := qT D− q and 0 ≤ ξ ≤ 1 is a parameter of the

algorithm, as detailed in [NW99].

3.2.4

Step Size Computation

The line search optimization subproblem can be numerically solved using the Wolfe rule. This approach guarantees a decrease of the cost functional, as the well-known Armijo rule does, and ensures reasonable progress by ruling out unacceptably short steps [NW99]. Let the updated control sequence defined in (3.7) be denoted as U+ , which is a function of the previous control sequence, the search direction, and the step size. The step size optimization subproblem can be defined as s∗ = arg min φ(s) , s≥0

with φ(s) = J¯0+ := J¯ (X+ , U+ ) ∆ where (.)+ denotes the result of updating a variable using the step size s, resulting in U+ = U + s∆ ∆). Moreover, the derivative of the line search cost function is given by and X+ = X(x0 , U + s∆ d φ(s) d J¯0+ φ (s) := = . ds ds 0

Considering the definition of the cost to go at a given prediction instant i as J¯i = F¯N +

N −1 X

L¯ l ,

l=i

for all i = 0, . . . , N − 1, and J¯N = F¯N , the derivative of the line search cost functional is given by + d J¯i+1 d J¯i+ ∂ L¯ +i ∂ L¯ +i ζ δ = , i+ i+ ds ds ∂x+i T ∂u+i T

33

Chapter 3: Terrain Avoidance NMPC for all i = N − 1, . . . , 0, with d J¯N+ ∂ L¯ +N ζN , = ds ∂x+N T ∂ f+i−1 ∂ f+i−1 d x+i ζi = = ζ + δ i−1 , i−1 ds ∂x+ T ∂u+ T i−1

i−1

for all i = 1, . . . , N , and ζ 0 = 0. For the Wolfe line search algorithm, let µ0 = λ φ0 (0) and µ(s) = φ(0) + σ φ0 (0) s, where σ and λ are parameters of the line search algorithm. The Wolfe conditions classify a step size according to the sets A = s > 0 : φ(s) ≤ µ(s) ∧ φ0 (s) ≥ µ0 R = s > 0 : φ(s) > µ(s) L = s > 0 : φ(s) ≤ µ(s) ∧ φ0 (s) < µ0 that define the acceptable, the right unacceptable, and the left unacceptable step sizes, respectively. The procedure that finds an acceptable step size, i.e., an estimate of the optimal step size, is given in Algorithm 2. Algorithm 2 Wolfe rule step size computation Require: Search direction ∆ 1: Set i ← 0 2: Initialize si > 0, l = 0, and r = +∞ 3: Compute φ(si ) and φ0 (si ) 4: while si < A do 5: if si ∈ R then 6: Set r = si 7: else {si ∈ L} 8: Set l = si 9: end if 10: if d = +∞ then 11: Choose si+1 > si 12: else 13: Choose si+1 ∈ (l, r) 14: end if 15: Set i ← i + 1 16: Compute φ(si ) and φ0 (si ) 17: end while 18: return Set s∗ ← si

3.2.5

Stability Discussion

Although a formal stability analysis of the proposed NMPC methodology is not provided in this chapter, it is noted that significant work on related approaches is available in the literature 34

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft (see [MRRS00], [MNMS01], and references therein). From the literature, it can be concluded that the stability of the proposed algorithm relies on the choice of the horizon N , the parameter matrices P, Q, and R, as well as the terminal cost function. There are three key ingredients to achieve stability of a nonlinear system using NMPC algorithms. They have to do with the choice of: i) the terminal set, Xf ⊂ X ; ii) the terminal ¯ N ). control law, κ f (x); and iii) the terminal cost function, already introduced before as F(x Considering the nonlinear system linearization about an equilibrium point, a stabilizing and optimal infinite horizon control law can be derived using well-known design techniques, such as LQR, H2 , H∞ , etc. Defining this control law as the terminal control law, κ f (x), many stability analysis strategies define the terminal set, Xf , as a closed set approximation of the largest positively invariant set for which the terminal control law is a stabilizing (suboptimal) control law for the nonlinear system. Then, through an appropriate definition of the terminal cost ¯ N ) and the choice of the prediction horizon, it must be guaranteed that the system function F(x converges to the terminal set, which, by definition, ensures convergence to the equilibrium point if the terminal control law is used. Although necessary for the formal convergence analysis, most NMPC techniques do not require the explicit use of the terminal set or the terminal control law for the computation of the NMPC control law.

3.3

Terrain Avoidance NMPC for Autonomous Rotorcraft

This section presents a terrain avoidance NMPC methodology for autonomous helicopters, using the helicopter model introduced in Chapter 2 and defining a repulsive field around the helicopter that penalizes the distance to the closest terrain point. The derivatives of the helicopter nonlinear model, presented in Appendix A, are used to compute the search directions and step size of the optimization algorithm described in this chapter.

3.3.1

Model Intrinsic Input Saturation

Within the MPC literature, input saturation constraints are included in the optimization problem to guarantee that the resulting control sequence is always admissible. However, when using NMPC these and other complex physical constraints can be defined implicitly in the nonlinear model of the plant, instead of explicitly in the optimization problem. The idea is to separate between fundamental control saturations, corresponding to physical limitations of the platform or model, and mission dependent control saturations, where the former are made intrinsic to the nonlinear model and the latter are defined as an additional constraint to the optimization problem. This procedure simplifies the optimization problem and yields a self-contained model of the plant that is valid for any inputs. Consider the helicopter model (2.1), and let the new input u¯ B be defined as a smoothly saturated function of the regular input uB , so that the dynamic equation is now given by x˙ B = fB (xB , u¯ B (uB )) . 35

Chapter 3: Terrain Avoidance NMPC In brief, considering that uB ∈ U , the procedure described below defines the new saturated input vector as u¯ B ∈ Rnu , simplifying the optimization problem formulation. ¯ = The saturation functions are derived from a basic function a(a)

a , 1+|a|

applying translation

and scaling operations both to the function and its derivative, such that inside the bounds a¯ and a are equal and outside the bounds a¯ tends smoothly to the maximum value amax or to the minimum value amin . It is noted that a myriad of other smooth functions could be used to the same end. The generic saturation function is defined as   a , ≤a≤     a−  + , a> ¯ = a(a) , 1+ a−    a−   + a− , a < 1−

with = amax − , = amin + , where 0 < < 1 is a constant, that defines the length of the smooth transition. The derivative of this function is defined as   1 , ≤a≤     1 d a¯   a− 2 , a > = . (1+ )  da   1    1− a− 2 , a < ( ) For simplicity and with a slight abuse of notation, the input vector uB is used hereafter to denote the result of the smooth saturation, u¯ B .

3.3.2

Terrain Avoidance Constraint

The terrain constraint function is designed to enable a collision free trajectory, even if the reference trajectory goes through the terrain. This constraint can be implemented by defining a repulsive field around the origin of the body frame {B}, obtained by weighting exponentially the minimum distance between the helicopter position E pB and the closest point on the terrain, E

pm (E pB ), which is a function of the position of the helicopter. Consider the position error

between these two points to be defined as em (E pB ) = E pm (E pB ) − E pB and let rS be the radius of a safety sphere around the helicopter, such that if eTm em > rS2 then the vehicle is sufficiently distant from the terrain. Consider the discrete-time state vector xi at time instant i, as introduced in Section 3.2.1, that includes the helicopter position E pBi . Then, a function g(xi ) that gives a measure of distance between the terrain and the safety sphere can be defined as g (xi ) = eTm (E pBi ) em (E pBi ) − rS2 , such that g (xi ) = 0 when the terrain touches the safety sphere. Consequently, the terrain avoidance constraint can be included in the optimization problem through (3.2d), by defining the penalty function FC (xi ) = c exp[−g(xi )] , where c > 0 is a scalar weight selected such that, when g(xi ) = 0 the respective value of the terrain constraint, FC (xi ) = c, is adequate to drive the vehicle away from the terrain in the 36

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft 10

8

x 10

100

F (x) T

F (x) T

Safety radius

Safety radius

90

7

80

6 70

5

F (x)

T

FT(x)

60

4

50

40

3

30

2 20

1 10

0 −10

−8

−6

−4

−2

0 x [m]

2

4

6

8

10

0 −10

(a) Global view

−8

−6

−4

−2

0 x [m]

2

4

6

8

10

(b) Detail on safety sphere boundary

Figure 3.2: One-dimensional terrain avoidance penalty function c e−g(x) , with g(x) = x2 − 52 and c = 1.

foreseen operation scenarios. The zero value is forced whenever the distance kem k is greater than a predefined outer radius rO where the influence of the terrain is negligible, that is FC = 0 if kem k > rO , thus avoiding unnecessary computational burden. Note that even if this procedure is not performed, the deviation from the desired trajectory will be negligible, since the terms Li of the cost function will always be dominant when the vehicle is not close to the terrain. The derivative of this terrain constraint is computed using ∂ FC ∂xTi

= −2 c eTm exp[−g(xi )]

∂ em ∂xTi

and ∂ em ∂xTi where the derivatives

∂ em ∂E pTB

= 03×3 03×3

∂ em ∂E pTB

i

03×(5+nu )

T ,

for all i = k, . . . , k+N are computed numerically, based on the available i

terrain information. The values used throughout this section for the constants described earlier are c = 1, rO = 10m, and rS = 5m. An example of a one-dimension version of this function can be seen in Figure 3.2. This type of function has the advantage of being defined for all x ∈ Rnx , while the ubiquitous infinity barrier type of constraint functions, applied to this safety sphere concept, may lead to problems during initialization or when avoiding the terrain. For instance, for takeoff maneuvers, the vehicle would be initialized with the terrain inside the safety sphere and the proposed terrain constraint alone would drive its trajectory away from the terrain, whereas the infinity barrier constraint is not coherently defined for such a situation and could have the opposite effect, keeping the vehicle close to the terrain. 37

Chapter 3: Terrain Avoidance NMPC

3.3.3

Simulation Results

The terrain avoidance NMPC algorithm presented in this section is designed to provide low altitude flight capabilities to unmanned rotorcraft, such as the helicopter shown in Figure 1.2, even in operation scenarios where the predefined trajectory collides with the terrain. The simulation results presented in this section are two-fold. Firstly, a terrain avoidance simulation is presented to demonstrate the capabilities of the algorithm in collision avoidance situations and to compare the performance of each optimization algorithm. Secondly, a shorter simulation is presented to illustrate particular aspects of this NMPC, such as the natural previewing ability of MPC strategies or the activation of intrinsic saturations and saturation constraints included in the optimization problem. As the problem of terrain acquisition and representation is not the main focus of this chapter, it is assumed that the terrain is represented by an elevation function and that this information enters the control algorithm in the computation of the helicopter-terrain distance. In the results presented hereafter, the helicopter model described in Chapter 2, parameterized for the Vario X-treme model scale helicopter, is used to close the simulation loop as the plant model. Nonetheless, a simplified yet highly nonlinear version of this model is used in the control algorithm. The simulations were carried out in Matlab® /Simulink® with C mex-functions, using an Intel® Core™2 T9550 at 2.66GHz with 3GB of RAM running Ubuntu operative system. 3.3.3.1

Terrain Avoidance

The terrain used in the first simulation resembles a winding water stream and the reference trajectory comprises three sections: 1) hovering flight at the initial position; 2) forward flight trajectory with constant speed kvk = 2ms−1 ; and 3) hovering flight at the final position. The sample time is Ts = 0.02s and the horizon is N = 50 sample times, or equivalently 1 second. This horizon is sufficiently large to allow for the algorithm to predict impacts with the terrain and change the helicopter trajectory to avoid it. The precision of the solution is determined by the algorithm stop condition given by

∇J¯(j) |U

0

< 10−3 ,

∇J¯(0) |U

0 (j) where J¯0 denotes the value of the cost functional at the optimization iteration j. Considering a

first order main rotor blade pitching dynamics, the final state vector is defined as h xk = vTBk ω TBk

pTBk λ TBk θ T1k

uTBk−1

iT

Thus, the cost functional matrices used in the simulation are R = diag (150, 300, 300, 150) , Q = diag (I3 , 3 I2 , 10, 5 I3 , I2 , 10, I2 , R) , 38

.

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft Table 3.1: Terrain Avoidance NMPC: iterations and computation times.

Method Grad-FS Grad-LS qNewton-LS

avg(nit ) 1129.9 136.3 20.6

avg(tCP U ) 41.4 s 12.5 s 4.6 s

max(tCP U ) 84.5 s 46.3 s 23.4 s

Table 3.2: Terrain Avoidance NMPC: relative improvements in number of iterations and CPU time between the algorithms.

Reduction from Grad-FS to Grad-LS Reduction from Grad-LS to qNewton-LS Reduction from Grad-FS to qNewton-LS

avg(nit ) 87.9 % 84.9 % 98.2 %

avg(tCP U ) 69.7 % 63.0 % 88.8 %

max(tCP U ) 45.3 % 49.4 % 72.3 %

and the P matrix is computed as the steady-state solution of the discrete-time algebraic Riccati equation using matrices Q and R, for the helicopter model linearized around hover. The operator diag(A1 , . . . , An ) stands for the block diagonal matrix formed by the arguments, and In is the n × n identity matrix. The simulation results presented in Figures 3.3 to 3.5, feature the time evolution of the position, Euler angles, linear velocity, actuation and the 3-D trajectory described by the helicopter. These results include the values for each of the three optimization algorithms: Grad-FS: gradient method with fixed step; Grad-LS: gradient method with line search; and qNewton-LS: quasi-Newton method with line search. For the Grad-FS algorithm, the value s = 10−6 was chosen for the step size so that the algorithm converges at each instant of the desired simulation with a good rate of convergence. Since the optimization problem and the termination tolerance are the same for each of these algorithms, the performance in terms of minimization of the cost function should be the same, apart from numerical issues. Note that the cost function minimization includes several terms: trajectory following, terrain avoidance, control effort, and saturations. It can be seen in the simulation plots that all variables of the three algorithms are overlapping, which indicates that they have identical minimization performance. The improvements introduced by the different methods are presented in Tables 3.1 and 3.2, regarding the average computation time avg(tCP U ), the maximum computation time max(tCP U ), and the average number of iterations avg(nit ). For instance, the reduction percentage of the 39


100

x [m]

50 0 −50 −100

10

20

30

40

50

60

70

80

90

100

110

10

20

30

40

50

60

70

80

90

100

110

20

y [m]

10 0 −10 −20

−30

z [m]

−32 reference Grad−FS Grad−LS qNewton−LS

−34 −36 10

20

30

40

50 60 Time[s]

70

80

90

100

110

(a) Position and reference

φ [rad]

−0.02 −0.04 −0.06 10

20

30

40

50

60

70

80

90

100

110

10

20

30

40

50

60

70

80

90

100

110

θ [rad]

0.1 0.05 0 −0.05 −0.1

ψ [rad]

0 −0.2 reference Grad−FS Grad−LS qNewton−LS

−0.4 −0.6 10

20

30

40

50 60 Time[s]

70

80

90

(b) Euler angles and reference

Figure 3.3: Terrain Avoidance NMPC: position and attitude.

40

100

110

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft

u [m/s]

2 1 0 10

20

30

40

50

60

70

80

90

100

110

10

20

30

40

50

60

70

80

90

100

110

v [m/s]

1 0.5 0

w [m/s]

0.5 reference Grad−FS Grad−LS qNewton−LS

0 −0.5 10

20

30

40

50 60 Time[s]

70

80

90

100

110

(a) Linear velocities and reference

θc [rad]

0.06

0

0.055 0.05

10

20

30

40

50

60

70

80

90

100

110

10

20

30

40

50

60

70

80

90

100

110

10

20

30

40

50

60

70

80

90

100

110

−3 x 10

10

1c

θc [rad]

15

5

−3 x 10 5

θ

c

1s

[rad]

0 −5 −10

0.08 reference Grad−FS Grad−LS qNewton−LS

0.075

0t

θc [rad]

−15

0.07 10

20

30

40

50

60

70

80

90

100

110

Time[s]

(b) Actuation

Figure 3.4: Terrain Avoidance NMPC: velocity and actuation.

41


−100 −80 −60

40 −40 −Z

30 −20 20 0 10

20 reference

−100

40 −80

MPC −60 60

−40 −20 0

80

20 40

X 60

100

80 100

−Y

(a) 3D Trajectory: Global View 1

reference MPC

−60 −40 −20 0 20 60

40

40 20

60

0 −20

80

−40 −60

100

−80 −100

X

−Y

(b) 3D Trajectory: Global View 2

reference MPC

(c) 3D Trajectory: Detail View

Figure 3.5: Terrain Avoidance NMPC: 3-D trajectory.

42

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft average CPU time from algorithm A to B is computed using ReductionA→B (tCP U ) = 100

A B avg(tCP U ) − avg(tCP U ) A avg(tCP U)

%.

From this data it is evident that using the line search algorithm and the quasi-Newton method is more advantageous than using the traditional gradient method with a fixed step. According to these results, the control algorithm solution combines the two conflicting objectives of trajectory tracking and effective terrain avoidance, redirecting the vehicle to avoid the terrain while keeping the shortest possible distance with respect to the reference trajectory. Notwithstanding these improvements in computation times, the algorithms are not yet prepared for a realtime implementation that is compatible with the high sample rate nature of the envisioned platform (50Hz). For this reason, future work will focus on implementation efficiency, further simplification of the helicopter model used within the MPC algorithm, and redefining the terrain representation. With the steadily increasing computational power, new generation processors and algorithm refinements will make these algorithms usable in the near future. The presented terrain avoidance methodology is meant to be the core of a larger system, which would include a supervisor and a reference trajectory planner. Since this optimal control strategy with terrain avoidance can lead to terrain local minima, the global system should detect that the vehicle is approaching a local minimum and redesign the reference trajectory to redirect the vehicle to a different path. 3.3.3.2

Saturation Results

The second simulation results intend to shed some light on particular aspects of this NMPC implementation, which are not clear in a normal operation scenario such as that of the previous simulation results. The simulation is performed in the same conditions as the previous one, but the trajectory is changed to feature an abrupt change in the attitude of the vehicle: 1) hovering flight at the initial position facing north; 2) forward flight trajectory with constant speed kvk = 4 m s−1 facing west. The simulation data comprises three different runs with: NoSat: no saturations; IntrSat: intrinsic saturations in the inputs; and OPSat: optimization problem input and state saturation constraints. The input saturation limits, used for both the IntrSat and the OPSat, are h iT umin = 0.0 −0.1 −0.015 0.0 rad and h iT umax = 0.065 0.1 0.015 0.3 rad . 43

Chapter 3: Terrain Avoidance NMPC θc [rad]

0.07

0

0.06 0.05 0.04 1

2

3

4

5

6

7

8

9

10

1

2

3

4

5

6

7

8

9

10

1

2

3

4

5

6

7

8

9

10

1c

θc [rad]

0.025 0.02 0.015 0.01 0.005

−3

0t

θc [rad]

1s

θc [rad]

x 10 0 −10 −20

0.1

reference NoSat IntrSat OPSat

0.08 0.06 1

2

3

4

5

6

7

8

9

10

Time[s]

Figure 3.6: Terrain avoidance NMPC: actuation saturation test with i) no saturations (NoSat), ii) intrinsic saturations in the inputs (IntrSat), and iii) optimization problem saturation constraints (OPSat).

For the state saturation used in OPSat, all variables will remain within the bounds, except for the yaw rate, for which the absolute value is saturated at 0.4 rad s−1 . The bounds considered for this simulation results are more restrictive than the real bounds used in the terrain avoidance simulation in order to show the behavior of the saturation strategies. It can be observed in Figures 3.6 and 3.7, that both saturation strategies, IntrSat and OPSat, are effectively keeping the main rotor collective and lateral cyclic controls within the bounds. Moreover, the state saturation in the optimization problem is also constraining the yaw rate absolute value below 0.4 rad s−1 . It can also be seen that the control algorithm starts changing the control effort N steps ahead of the change in the reference trajectory, demonstrating the preview ability of this NMPC strategy. As discussed above, the objective of having intrinsic saturations is to make sure that the physical limitations of the platform and the numerical limitations of the model are always taken into account in the form of input saturations within the model function, even if there are no saturation constraints in the optimization problem.

44

3.3 Terrain Avoidance NMPC for Autonomous Rotorcraft

u [m/s]

4 3 2 1 0

1

2

3

4

5

6

7

8

9

10

1

2

3

4

5

6

7

8

9

10

0.5 v [m/s]

0 −0.5 −1 −1.5

w [m/s]

0 −0.5 reference NoSat IntrSat OPSat

−1 −1.5 1

2

3

4

5 Time[s]

6

7

8

9

10

(a) Linear velocity and reference

p [rad/s]

0.1 0 −0.1 −0.2 −0.3 1

2

3

4

5

6

7

8

9

10

1

2

3

4

5

6

7

8

9

10

q [rad/s]

0.1 0 −0.1 −0.2

r [rad/s]

0 −0.2 reference NoSat IntrSat OPSat

−0.4 −0.6 −0.8 1

2

3

4

5 Time[s]

6

7

8

9

10

(b) Angular velocity and reference

Figure 3.7: Terrain avoidance NMPC: velocities saturation test with i) no saturations (NoSat), ii) intrinsic saturations in the inputs (IntrSat), and iii) optimization problem saturation constraints (OPSat).

45


Figure 3.8: The Delfim ASC during sea trials.

3.4

Trajectory Tracking NMPC for ASCs

Motivated by the computational challenges posed by the real-time implementation of NMPC for autonomous helicopters, this section describes a solution to the problem of trajectory tracking control of autonomous surface craft (ASC), which are platforms with lower requirements in terms of actuation frequency, in the presence of constant ocean currents. Simulation results, obtained with a nonlinear dynamic model of a prototype ASC, show that the NMPC strategy adopted yields good performance in the presence of constant currents. Experimental results are also provided to validate the real-time implementation of the NMPC techniques for ASCs.

3.4.1

Catamaran Model

The dynamic model used to describe the behavior of an ASC in the horizontal plane, such as the Delfim catamaran depicted in Figure 3.8, considers that the prototype vehicle considered has two hulls, two propellers driven by electrical motors, and a submerged torpedo-shaped sensor container, attached to the vehicle by a central wing-shaped structure. An in-depth presentation of this model and a description of the catamaran surface craft can be found in [Pra02]. Consider the earth-fixed coordinate frame {E} and the body-fixed coordinate frame {B}, previously defined in Chapter 2, which are herein considered to be defined in two-dimensional (2-D) space. Let also the generalized variables for the horizontal motion mode be given by h iT ν= u v r , 46

h η = E xB

E

yB ψ B

iT

,

h τ= X Y

N

iT

,

3.4 Trajectory Tracking NMPC for ASCs where ν represents the generalized velocity, η the generalized position, and τ denotes the generalized force vector comprising the external force components in the horizontal plane, X and Y , and also the external moment about the vertical direction, N . With this notation, the kinematic equations of the vehicle are given by η )ν ν, η˙ = J(η η ) contains the rotation matrix from {B} to {E}, with where J(η   "E # cos ψB − sin ψB 0    R 0 η) = B J(η =  sin ψB cos ψB 0 . 0 1   0 0 1 The generalized equation of motion for the vehicle dynamics can be written as ν )ν ν = τ (ν˙ ,ν ν , n) , M ν˙ + C(ν where M is the 2-D rigid body inertia matrix and C is the matrix of Coriolis and centripetal terms, respectively defined as   m 0 0    M =  0 m 0    0 0 Iz

and

   0 −m r 0   ν ) = m r 0 0 , C(ν   0 0 0

The generalized force τ includes the added mass terms, the hydrodynamic forces and moments acting on the body, as well as the forces and moments generated by the propellers as functions of the generalized velocity vector ν and the actuation vector n = [nc nd ]T . The symbols nc and nd stand for the common and differential modes of the propellers’ speed of rotation, respectively. A model that captures the effect of constant currents on the ASC dynamics can be obtained by rewriting the above equations in terms of the vehicle’s velocity relative to the fluid. It is assumed η )−1 ν f , that the generalized velocity results from the sum of two components, that is, ν = ν r + J(η where ν r is the vehicle’s generalized velocity with respect to the fluid expressed in the body frame {B} and ν f is the fluid generalized velocity described in the earth-fixed frame {E}. Notice that the third element of ν f is defined as zero, as the fluid is assumed to be irrotational. To obtain the new dynamic equations in terms of ν r instead of ν , first note that the generalized force τ can be decomposed as ν r , n) = M2 ν˙ r + τ 1 (ν ν r , n) , τ (ν˙ r ,ν where M2 is a constant parameter matrix defined by   0  Xu˙ 0   M2 =  0 Yv˙ Yr˙    0 Nv˙ Nr˙ Simple algebraic manipulations show that the kinematic and dynamic equations of motion of the vehicle are given by   η )ν νr + νf   η˙ = J(η   ν˙ r = M−1 ν r , n) , 3 τ 2 (ν

(3.8) (3.9) 47

Chapter 3: Terrain Avoidance NMPC Table 3.3: DELFIM Catamaran Parameters.

Parameter Xu Xu 2 Xu 3 Xv 2 Xr 2 Xvr Xur 2 Xunc Xrnd Xnc nc Xnd nd Xu˙ xcc xc m

Value -0.48 -23.11 -7.60 504.55 38.49 407.11 -12.15 -27.25 19.89 186.98 186.98 -21.79 -0.13 0.14 398

Parameter Yuv Yur Yr 3 u Yvvcc Yvvc Yvr Yrr Yvnc Yrnc Yv˙ Yr˙ Nv˙ Nr˙ Nnc nd Nvnc

Value -925.98 227.55 345.43 -121.15 -581.18 -339.02 -735.67 -1.55 2.72 -608.09 -27.59 -2.04 -364.45 -27.30 2.72

Parameter Nuv Nur Nr Nu 2 r Nr 3 Nr 3 /u Nv 2 ru Nvr 2 u Nrr Nvr Nvvcc Nvvc Nund Nrnc Iz

Value -322.47 -419.36 -0.26 -12.15 -2.16 132.52 -4746.30 609.10 -1764.20 -47.46 49.38 -81.37 19.89 -19.31 326

where M3 = M − M2 is a full rank matrix, ν r , n) := −C(ν ν r )ν ν r + τ 1 (ν ν r , n) , τ 2 (ν and the components of the generalized force τ 2 = [X2 Y2 N2 ]T are defined as X2 = m r vr + Xu ur + Xu 2 ur2 + Xu 3 ur3 + Xv 2 vr2 + Xr 2 r 2 + Xur 2 ur r 2 + Xvr vr r + Xunc ur nc + Xrnd r nd + Xn2c n2c + Xn2 n2d d

3

Y2 = −m r ur + Yuv ur vr + Yur ur r + Yr 3 /u r /ur + Yvvcc |vr + r xcc | (vr + r xcc ) + Yvvc |vr + r xc | (vr + r xc ) + Yvr vr |r| + Yrr r |r| + Yvnc vr nc + Yrnc r nc N2 = Nuv ur vr + Nur ur r + Nr 3 /u r 3 /ur + Nv2r/u vr |vr | r/ur + Nvr2/u vr r |r|/ur + Nr r + Nru 2 r ur2 + Nr 3 r 3 + Nvvcc |vr + r xcc | (vr + r xcc ) + Nvvc |vr + r xc | (vr + r xc ) + Nvr vr |r| + Nrr r |r| + Nvnc vr nc + Nrnc r nc + Nund ur nd + Nnc nd nc nd . The remaining undefined quantities are model parameters, defined in Table 3.3. Further details on catamaran models and general modeling of hydrodynamic effects can be found in [Pra02], [Fos94], and references therein, whereas the derivatives of this model are presented in Appendix B. 3.4.1.1

Generalized Error Dynamics

The concepts of trimming trajectories for the ASC model, a generalized error space to describe the vehicle’s motion about trimming trajectories in the absence of current, and the dynamics of the vehicle in the new error space are provided hereafter. Consider the equations of motion presented in (3.8) and (3.9) without the effect of constant currents (that is, ν f = 0), yielding ν r = ν . For the type of vehicle considered in this section, the 48

3.4 Trajectory Tracking NMPC for ASCs equilibrium set is defined as Cata ν , n) ∈ R5 : τ 2 (ν ν , n) = 0} . Xeq := {(ν

and a trimming trajectory as η C (.) ∈ R3 : τ 2 (ν ν C , nC ) = 0} . TCCata := {η It can be shown that the only possible trimming trajectories for a catamaran are straight lines and circumferences described by the vehicle at constant speed, which can be fully described p by the parameter vector ξ = [VC rC ]T where VC = uC2 + vC2 . Therefore, the set of achievable trimming trajectories, E, can be fully parameterized by the parameter vector ξ . The generalized error vector, xe , between the vehicle state and the desired trajectory is now introduced as    ν −νC ν e      −1 η (η xe = η e  =  J R (η ) η − η C ) t    xi Π η dt 0 xy e

    , 

h i where Πxy denotes the projection matrix Πxy = I2 02×1 . The integral term, xi , is introduced to naturally yield integral action in the trajectory tracking control law that will be designed later. In the new coordinates, the error dynamics take the form  ν˙ e = ν˙      η e )ν ν C − Q(ν ν )η ηe , η˙ = ν − J−1 (η    e   x˙ i = Π xy η e

(3.10) (3.11) (3.12)

ν ) = S([0 0 r]T ) and S(a) stands for the skew-symmetric matrix that verifies S(a) b = a×b, where Q(ν for a, b ∈ R3 . Using the definition of the error-space state vector, the ASC error model described in the previous equation can be rewritten as x˙ e = fe (xe , ne ) . It is also possible to show that the linearization of the error dynamics about ν e = 0 and ne = 0 is time invariant, with ne := n − nC (see [SPK02] for further details). In most practical mission scenarios involving ASCs, the only available velocity measurements are those of the velocity of the vehicle relative to the fluid, provided by a Doppler, which can be dramatically different from the earth-fixed velocity in the presence of ocean currents. As mentioned later in this section, the implementation of this error space uses the velocity relative to the fluid ν r , that is, ν e ' ν r − ν C . This standard inconsistency emerges in the form of a disturbance to be rejected by the controller and stresses the importance of testing the control strategy in the presence of ocean currents. 49

Chapter 3: Terrain Avoidance NMPC 3.4.1.2

Intrinsic Input Saturation Constraints

The error space model introduced above is used in the optimization problem, defined in Section 3.2, to find the optimal error input sequence that drives the error dynamics towards zero. Since only the error states and inputs are used in the optimization problem, the definition of input constraints on the actual vehicle inputs could be achieved either by including additional variables in the optimization problem, or by using variable constraints for the error inputs at each sampling instant along the prediction horizon. Alternatively, the strategy described in Section 3.3.1 naturally yields saturation of the actual vehicle inputs, as even complex physical constraints on the common and differential inputs can be easily incorporated in the nonlinear design model of this vehicle. Let the new inputs n¯ = [n¯ c n¯ d ]T be defined as smoothly saturated functions of the regular ν r , n(n)). ¯ inputs n = [nc nd ]T , so that the dynamic equations are now given by ν˙ r = M−1 For 3 τ 2 (ν the type of vehicle considered in this section, it is necessary to impose a minimum value for the common mode input nc , and also bounds for the differential input, which depend on the value of the common mode input. The saturation function of the common mode input is defined as   nc , c ≤ nc ≤ c     n − c c c + n − , nc > c , n¯ c (nc ) =  1+ c c    nc −c  c + nc −c , nc < c 1−

with c = ncmax − and c = ncmin + , where 0 < < 1 is a constant (typically 0.01), that defines the length of the smooth transition. The saturation of the differential input is given by the function n¯ d (n¯ c , nd ), obtained using the same approach presented above. Without loss of generality, it is considered that the motors can only generate positive thrust. Thus, considering also that d := n¯ c , d := n¯ c (1 − ), and d := −n¯ c (1 − ), the differential mode saturation function is defined as

  n    d n −   d + dnd −dd n¯ d (n¯ c , nd ) =   1+ d   nd −   d + nd −dd 1−

d

, −d ≤ nd ≤ d , nd > d

.

, nd < d

It is noted that the saturated differential input depends also on the saturated common mode input, as a result of the physical limitations of the vehicle. One example of a saturation functions is shown in Figure 3.9. In brief, considering that n ∈ N ⊂ Rnn , the procedure described above defines the new saturated input vector as n¯ ∈ Rnn , which avoids the need to augment the optimization problem variables with the actual inputs or defining variable saturation limits at each sampling instant.

3.4.2

Simulation Results

The real-time implementation of the NMPC methodology presented in Section 3.2 using the trajectory tracking error space formulated in Section 3.4.1.1 has the potential to yield a reliable 50

3.4 Trajectory Tracking NMPC for ASCs

2

SSat(nc)

1.5

1

0.5

0 −3

−2

−1

0 nc

1

2

3

(a) Common mode saturation function n¯ c (nc ).

2

SSat(nd)

1 0 3 −1

2 1

−2 −3

0

−2 −1

−1 0 −2

1 2 3

−3

nd

nc

(b) Differential mode saturation function n¯ d (nc , nd ).

Figure 3.9: Smooth saturation functions: illustrative example using the saturation parameters = 0.1, ncmax = 2 rad/s, and ncmin = 0 rad/s.

51


(a) LQR controller

(b) MPC controller

Figure 3.10: LQR vs. MPC controller implementation.

control system that can meet the physical constraints of the ASC, even in the presence of ocean currents. In this section, the performance of the NMPC controller introduced above is evaluated in simulation by drawing a comparison with the results obtained with a basic linear quadratic regulator (LQR) gain switching methodology, similar to the control approach using H2 synthesis proposed in Chapter 4. The implementation of the LQR and NMPC controllers is depicted in Figure 3.10, where it is possible to notice the major differences between these two methods. The main disadvantage of the LQR gain switching methodology, as well as of H2 , H∞ , and other gain switching/scheduling techniques, is the need for prior computation of the gain matrices for a limited number of operating points (or regions), increasing the sensitivity of the closed-loop system to parametric uncertainties or external disturbances, unless they are accounted for in the design process. Given that the actual state measurement is used to infer the operating point of the vehicle at each instant of time, the selection of the proper gain assumes a relevant role in the performance of this controller, and failing to select the adequate gain may lead to instability. In the MPC algorithm there is no a priori computation since it relies on the full nonlinear model of the vehicle. Additionally, the MPC algorithm is able to cope with the physical limitations of the vehicle, such as actuation saturations, and also with additional state or actuation constraints that may be specific to the mission at hand. In the results presented hereafter, the ASC nonlinear model described in Section 3.4.1 is parameterized for the DelfimX Catamaran and used both in the NMPC control algorithm and for plant simulation purposes. The simulations were carried out in a Intel® Core™ i72640M processor, using Matlab® /Simulink® with C mex-functions. The reference trajectory was 52

3.4 Trajectory Tracking NMPC for ASCs −60 −50 Ref LQR MPC LQR curr MPC curr vf [dm/s]

y [m]

−40 −30 −20 −10 0 0

20

40

60

80 x [m]

100

120

140

160

Figure 3.11: Simulation results: trajectory.

selected to illustrate the behavior of the control algorithms in extreme conditions, which include discontinuities in the reference velocities and a nonzero initial position error. This trajectory is composed of three different sections: 1. a straight line (Vc = 1.2 m/s and rc = 0 deg/s), to be tracked between η c = 03×1 and η c = [72 0 0]0 , with initial conditions ν 0 = [0.6 0 0]0 and η 0 = [0 − 1 − π/4]0 ; 2. one fourth of a circumference turning to port side (Vc = 1.5 m/s and rc = −3 deg/s); 3. one circumference and a half turning to starboard (Vc = 1.2 m/s and rc = 2 deg/s). The sampling time is Ts = 0.2 s and the prediction horizon is N = 50 sampling times, or equivalently, 10 seconds. The precision of the solution is determined by the algorithm stop (j) (j−1) conditions, which include |J¯ − J¯ | < 10−2 . k

k

Two different scenarios were simulated in order to highlight the major differences between the LQR and NMPC controllers: i) trajectory tracking without current; and ii) trajectory tracking with a constant current (uf = −0.1 m/s and vf = 0.2 m/s). The simulation results of these two scenarios are presented in Figures 3.11 to 3.14, that show the trajectories described by the the ASC using the NMPC and LQR controllers, as well as the time evolution of the generalized position error, the generalized velocity, the actuation, the computation time and iterations. It was assumed that the velocity measurement available for feedback was relative to the fluid and not to the earth-fixed frame. It can be seen that both control methodologies achieve the tracking objective, with or without constant current. However, the LQR method presents larger excursions in actuation both at 53


xe(t) [m]

1 0 −1 0

50

100

150

200

250

300

350

0

50

100

150

200

250

300

350

ye(t) [m]

4 2 0 −2 −4

ψe(t) [rad]

0.5 0

LQR MPC LQR curr MPC curr

−0.5 0

50

100

150

200

250

300

350

Time[s]

u(t) [m/s]

(a) Generalized position error

2 1.5 1 0

50

100

150

200

250

300

350

0

50

100

150

200

250

300

350

v(t) [m/s]

0.04 0.02 0 −0.02 −0.04

r(t) [rad/s]

0.2 0.1

Ref LQR MPC LQR curr MPC curr

0 −0.1 0

50

100

150

200

250

300

Time[s]

(b) Generalized velocity

Figure 3.12: Simulation results: generalized position error and velocity.

54

350


25

nc(t) [rad/s]

20 15 10 5 0

50

100

150

200

250

300

350

60

nd(t) [rad/s]

40 20 0 −20 LQR MPC LQR curr MPC curr

−40 −60 −80 0

50

100

150

200

250

300

350

Time[s]

Figure 3.13: Simulation results: actuation.

0.18 MPC MPC curr

0.16

CPU time per iteration [s]

0.14

0.12

0.1

0.08

0.06

0.04

0.02

0

50

100

150

200

250

300

350

Time[s]

Figure 3.14: Simulation results: CPU time.

55

Chapter 3: Terrain Avoidance NMPC the initial stage and during the transitions between sections, which generally translate into larger position errors. Moreover, the control effort demanded by the LQR method is far greater than that of the NMPC, and it even violates the conditions for valid operation (by having |nd | > nc ). The major limitation of the NMPC method is the computation time needed to determine the next control action, which should be smaller than the sampling time Ts = 0.2 s. For the specified simulations, this threshold was never exceeded, as shown in Figure 3.14, noting that the average and maximum CPU times obtained in the absence of currents were 0.010 s and 0.060 s, respectively, whereas in the presence of a constant current these values increased to 0.018 s and 0.183 s. The largest CPU times are obtained during the initial instants and during the transitions between parts of the reference trajectory, where the initial conditions of the optimization algorithm are not close to the optimal solution. This fact indicates that the maximum CPU time can be reduced in real mission scenarios by appropriate choice of initial conditions and the generation of smoother reference signals. In summary, these results indicate that the proposed NMPC control algorithm is adequate for real time implementation, demonstrating the ability to track a demanding trajectory while rejecting constant current disturbances and keeping the actuation signals within their limits of operation.

3.4.3

Experimental Results

This section contains the results of experimental tests with a prototype ASC aimed at validating the performance of the trajectory tracking NMPC methodology presented in this chapter. The results show that the control strategy is reliable and meets the physical constraints of the ASC, even in the presence of ocean currents. The vehicle used for these sea trials is the Delfim catamaran, shown in Figure 3.8, which was designed, built, and instrumented by IST/ISR. The vehicle is equipped with a MEMSENSE nanoIMU, which provides 3-D measurements of acceleration, angular rates, and magnetic field, as well as a GPS unit working in differential mode. The position, attitude, and velocities of the vehicle are computed using well-known Kalman filtering techniques, as detailed in [VCS+ 11]. In order to enable real-time implementation of the NMPC strategy, the controller is implemented in a dedicated onboard computer, hereafter denoted as the NMPC computer. This computer communicates through UDP with a central computer (a PC104), responsible for acquiring data from sensors, sending this data to the NMPC computer, receiving the result of the controller, and changing the actuation values of each motor of the ASC accordingly. The onboard NMPC computer features an Intel® Core™2 T9550 processor, with 4GB of memory, and both computers have Ubuntu 12.04 operative system with the Robotics Operative System (ROS) software framework, where a C++ implementation of the NMPC controller was included as an additional node to the overall system. The ASC Delfim was tested in the Lisbon Oceanarium lake, in Portugal, using the trajectory tracking NMPC algorithm proposed in this section. During 56

3.4 Trajectory Tracking NMPC for ASCs −60 −50

Y

−40 ref MPC MPC int

−30 −20 −10 0 0

20

40

60

80

100

120

140

160

X

Figure 3.15: Trajectory tracking NMPC sea trials data: 2-D trajectory.

the tests, the wind velocity varied between 5 km/h and 15 km/h and the wave amplitude was not significant. The reference trajectory, sampling time, and prediction horizon used in real-time sea trials are exactly the same as those used in the previous simulation results. The main goals of presenting these experimental results are twofold: to validate the proposed NMPC strategy for real-time control of ASCs and to evaluate the performance of different configurations of the error-space model. In particular, two different error-space vectors are considered: 1) the complete error-space vector, as defined in Equation (3.4.1.1), which includes position integral action; and 2) the error-space vector without integral action, defined as iT h xe = ν Te η Te . The results from the described sea-trials are presented in Figures 3.15 to 3.18, that show the trajectories described by the ASC, as well as the time evolution of the generalized position errors, generalized velocity, actuation, and computation time. It can be seen that both algorithms could successfully control the catamaran along the desired trajectory.

The analysis of these results shows that the maximum position error is lower than 1.21 m, when using integral action, and below 0.85 m without integral action, mainly because the transitions between trimming trajectories are more abrupt when using integral action. Nonetheless, the average error is lower when using integral action, 0.26 m, than when no integral states are considered, 0.45 m. This is the result of demanding more from the NMPC controller, naturally yielding lower steady state errors, even in the presence of disturbances or unmodeled dynamics. Regarding the linear and angular velocities, both algorithms show errors below 0.28 m/s and 8 deg/s, respectively. The CPU time used by the algorithms to compute the next actuation values is shown in Figure 3.18. As this CPU time is limited above by the 57


1 xe(t) [m]

0.5 0 −0.5 −1 50

100

150

200

250

300

350

50

100

150

200

250

300

350

100

150

200 Time[s]

250

300

350

ye(t) [m]

1 0.5 0 −0.5 −1

ψe(t) [rad]

0.2

MPC MPC int

0 −0.2 50

(a) Generalized position error, η e

u(t) [m/s]

1.6 1.4 1.2 1 50

100

150

200

250

300

350

50

100

150

200

250

300

350

250

300

350

v(t) [m/s]

0.2 0.1 0 −0.1

r(t) [rad/s]

−0.2

0.05 0 ref MPC MPC int

−0.05 −0.1 50

100

150

200 Time[s]

(b) Generalized velocity, ν

Figure 3.16: Trajectory tracking NMPC sea trials data: generalized position error and velocity.

58


6

nc(t) [rad/s]

4 2 0 −2 −4 50

100

150

200

250

300

350

3 MPC MPC int

nd(t) [rad/s]

2 1 0 −1 −2 50

100

150

200 Time[s]

250

300

350

Figure 3.17: Trajectory tracking NMPC sea trials data: actuation n.

0.25

MPC MPC int

CPU time per iteration [s]

0.2

0.15

0.1

0.05

50

100

150

200 Time[s]

250

300

350

Figure 3.18: Trajectory tracking NMPC sea trials data: CPU time.

59

Chapter 3: Terrain Avoidance NMPC sampling time, the implemented algorithms are forced to abort the optimization procedure whenever the elapsed time values go beyond 1.25 Ts , maintaining the previous control value, and saving the optimization terminal conditions to enable a better initialization value for the next sampling period. It can be seen that the algorithm that does not use integral action is much faster, with a maximum CPU time of 92 ms. Conversely, the integral action gets the NMPC algorithm close to the CPU limit, as the maximum CPU time is reached during some transitions between trimming trajectories. However, this does not compromise the operation of the vehicle.

3.5

Concluding Remarks

This chapter presented a simple NMPC-based strategy for the control of complex vehicles, with application to the terrain avoidance control of autonomous rotorcraft and the trajectory tracking control of ASCs with constant ocean current disturbances. The underlying NMPC constrained optimization problem was reformulated as an unconstrained optimization problem using penalty methods to accommodate the saturation and terrain constraints, and using Lagrange multipliers to eliminate the vehicle dynamic model constraint. The optimization problem was solved using a simple iterative algorithm that relies on gradient methods to find the search direction and on the Wolfe conditions to find an estimate of the optimal step size. In contrast to the standard approach in NMPC literature, the actuation constraints were also incorporated into the model so that every control action provided by the control algorithm is always valid and the controller design is simplified. In addition to imposing state and input saturation constraints, the proposed solution enforces terrain avoidance by defining a repulsive field around the helicopter that grows exponentially fast as the distance between the vehicle and the closest point on the terrain decreases. The terrain avoidance simulation results were obtained using a simplified nonlinear helicopter model inside the NMPC algorithm and the full model as the real plant, showing that the presented methodology can achieve effective terrain avoidance while steering the vehicle along the desired trajectory. A comparative study of several optimization algorithms is also provided, showing that the quasi-Newton method with line search drastically reduced the number of iterations and CPU time relative to the standard gradient method with fixed-step. For the ASC control problem, a natural trajectory tracking error space is defined and used within the design of the controller. The simulation results show that the presented solution can steer the vehicle along a demanding reference trajectory under the presence of constant currents, and the computation times observed in the simulation also indicate that this methodology is well suited for real time implementation. Experimental results for the real-time control during sea trials were also presented, validating the proposed trajectory tracking NMPC strategy. One direction of future work is to reduce the computational burden of the algorithm. The most critical tasks in terms of CPU time consumption include the helicopter model computations and the algorithm to determine the closest point on the terrain ,which with modern sensors, 60

3.5 Concluding Remarks like LADARs, time-of-flight cameras, sonars, or even the popular Kinect sensor, can be obtained with negligible processing.

61

4

Sensor-based Control of Autonomous Rotorcraft

This chapter addresses the trajectory tracking control problem of autonomous rotorcraft in operation scenarios where either an absolute position solution is available or only a relative position solution based on LADAR sensors is possible. The motivation for the research on this problem arises from the automatic inspection of large infrastructures (such as bridges, power plants, dams, etc), for which the GPS data is usually reliable during the approach to the infrastructure, whereas only relative positioning information might be possible once near the inspection target. The proposed approach relies on the definition of a trajectory-dependent error space to express the dynamic model of the vehicle, and adopts a linear parameter varying (LPV) representation with piecewise affine dependence on the parameters to model the error dynamics over a set of predefined operating regions. The synthesis problem is stated as a continuous-time H2 control problem, solved using LMIs and implemented within the scope of gain-scheduling control theory. For the operation scenario where an absolute attitude and position solution is available, usually based on GPS and INS measurements, the full dynamics and rigid-body kinematics are considered for the definition of a trajectory-dependent error space. Conversely, a new sensorbased nonlinear kinematics model is formulated in 3-D space for the case where the operation of the autonomous vehicle can only rely on a relative measurement of position, namely based on LADAR measurements, and a trajectory-dependent error space is defined to express the dynamic model of the vehicle and the new sensor-based kinematics.

4.1

Introduction

The development of trajectory tracking control systems constitutes both a challenge and a fundamental requirement to accomplish high performance autonomous flight, even without absolute positioning measurements. Two different approaches to the trajectory tracking control 63

Chapter 4: Sensor-based Control problem are considered: Absolute position control It is assumed that a full position and attitude solution, relative to a reference frame is available, usually resulting from the fusion of GPS and INS measurements; Sensor-based control It is assumed that there is no reliable GPS signal, however, both the absolute attitude and the 3-D position of the vehicle relative to an inspection target can be accurately measured resorting to LADARs. The main difference between these two approaches is the definition of the vehicle kinematics, which for the absolute position control are considered to be the usual rigid-body kinematics, whereas the sensor-based control approach relies on the formulation of nonlinear LADAR-based kinematics in order to obtain the position of the vehicle relative to the structure to be inspected. It is considered for the latter case that both a laser scanner and a range only scanner are used in order to get measurements on the horizontal plane and vertical axis, respectively. The use of ranging sensors to obtain vehicle localization in GPS-denied environments is by now an ubiquitous and mandatory technology in mobile robots [LDW91, TFBD01]. However, in the field of UAVs this problem has only been addressed in recent years [HPR08], and mainly assuming full knowledge of the geometry of the world where the UAV has to localize itself. The sensor-based control approach proposed in this chapter does not aim at providing a localization algorithm, but rather to find a simple and reliable control methodology that enables automatic inspection of large infrastructures, under mild geometric assumptions. The proposed control strategies define a trajectory-dependent error space to express the dynamic model of the vehicle as well as its kinematics, either assuming absolute or relative position measurements. The error vector, which should be driven to zero by the trajectory tracking controller, comprises linear and angular velocities, orientation errors relative to the reference trajectory, as well as position errors based on the kinematics equations. One of the most widely used approach to tackle the design of controllers for vehicles with complex and highly nonlinear dynamic models is to resort to gain-scheduling control theory [RS00]. The basic idea is to design a set of simple controllers, each of them limited to a predefined point or region of operation, and switch or interpolate between them to obtain a nonlinear and stabilizing controller for the complete operation envelope of the vehicle. The use of LPV model descriptions together with LMIs for the design of each controller, [GN99, SW00], constitute a powerful tool for tackling difficult nonlinear control problems, such as those addressed in this chapter. Several examples in the literature attest for its level of success, such as [CAGS06, SPK02, Sil00, Ros11] and references therein. In the work presented in this chapter the flight envelope of the helicopter is partitioned into a set of overlapping regions of operation, and for each of these regions an LPV representation 64

4.2 Trajectory Tracking Error Dynamics with piecewise affine dependence on the parameters is considered to accurately model the trajectory tracking error dynamics. By imposing an affine parameter dependence, a continuoustime state feedback controller can be derived in order to guarantee stability and H2 -norm performance bound over a polytopic set of parameters using a finite number of LMIs. Based on this result, a controller is synthesized for each of the operating regions and the overall controller is implemented within the framework of gain-scheduling control theory using the D-methodology to switch between controllers [KPKC95]. The effectiveness of the proposed controllers is assessed in a realistic simulation environment using the full nonlinear model of a small-scale helicopter, presented in Chapter 2. The main contributions of this chapter are: i) the formulation of a trajectory tracking error space dynamics, when absolute position measurements are available; ii) the formulation of a sensor-based trajectory tracking error space dynamics, to be used in scenarios where only relative position measurements based on LADARs are available; iii) the design of trajectory tracking controllers based on existing results on gain-scheduling theory, LPV model representation, and LMI controller design tools extended for polytopic systems; and iv) the validation in a realistic simulation environment of the proposed trajectory tracking controllers. This chapter is organized as follows. Section 4.2 presents a trajectory dependent error space considering that absolute position measurements are available, whereas Section 4.3 proposes the laser kinematics and a new sensor-based trajectory-dependent error space is introduced. The control synthesis results and implementation details are provided in Section 4.4, preceding the simulation results that are presented in Section 4.5. Finally, the conclusions and directions of further work are given in Section 4.6.

4.2

Trajectory Tracking Error Dynamics

This section presents a generalized error space to describe the helicopter’s motion about trimming trajectories, and computes explicitly the helicopter dynamics in the new error space. It is assumed that an accurate estimate of the full state of the vehicle is available, which includes its position relative to an earth-fixed reference frame {E}. Considering the definitions of equilibrium set and trimming trajectory presented in Section 2.4, a generalized error vector defined between the vehicle state and the desired trajectory can be defined using the nonlinear transformation     vB − vC   ve       ωB − ωC ω e     , xe =   = E −1 E E  pe  B RB ( pB − pC )    −1  λB − λ C ) QB (λ λe λB ) and QB = Q(λ λB ) = where it is considered, with a slight abuse of notation, that EB RB = EB R(λ Q(φB , θB ). In addition, let ue = uB − uC and vwe = vw , assuming that there is no wind disturbance for the trimming analysis, i.e. vwc = 0. It can be shown that in the new error coordinate system, 65

Chapter 4: Sensor-based Control the linearization of the rigid-body dynamics (2.1) along an arbitrary trimming trajectory, within the set of all trimming trajectories E, is time invariant (as detailed in [Sil00]). As v˙ C = 0 and ω˙ C = 0 for any trimming trajectory, the nonlinear error dynamics can be expressed as   v˙ e = v˙ B      ω˙ e = ω˙ B ,   ω B ) pe p˙ e = vB − EB R−1 vC − S(ω  e    λ˙ = ω − Q−1 Q ω − d Q−1 Q λ , e B C C B e B dt B

(4.1)

E −1 E λC ), and denoting S(a) as the skew-symmetric matrix repredefining EB R−1 e = B RB B RC , QC = Q (λ

senting the cross product. The linearization of (4.1) about the origin, or equivalently, the linearization of the vehicle dynamic and kinematic equations about a trimming trajectory parameterized by the constant vector ξ ∈ E can be expressed in the generalized error space as   ω e + Aλv δλ λe + Bv δue + Bwv δvw δv˙ e = Avv δve + Aω  v δω   v ω λ  ω e + Aω δλ λe + Bω δue + Bwω δvw  δω˙ e = Aω δve + Aω δω (4.2)   ˙ ω λe δ p = δv − S(ω ) δp − S(v ) δλ  e e C e C    δλ˙ = δω ω e − S(ω ω C ) δλ λe e y ∂ where Ax = ∂y fx (.) are constant matrices for each trimming trajectory, and fx (.) represents the c ω B ,λ λB , uB , vw ) and ω˙ B = fω (vB ,ω ω B ,λ λB , uB , vw ). Introducing the output vector functions v˙ B = fv (vB ,ω ye , the general disturbance input vector we , and rewriting (4.2) in a compact form yields ( ξ ) δxe + Bwe (ξ ξ ) δwe + B(ξ ξ ) δue δx˙ e = Ae (ξ , (4.3) ye = Ce δxe + De δwe + E δue where the dependence of Ae (.), B(.), and Bw e (.) on the trimming trajectory parameter ξ denotes y the evaluation of the matrices Ax = ∂ fx (.) for each trimming condition. Thus, it follows that ∂y

c

there is a linear time invariant plant (4.3), associated with each trimming trajectory ξ ∈ E, for which a linear controller can be designed.

4.3

Sensor-based Error Dynamics

The use of sensor-based controllers is only justified if the accuracy of the relative position measurements is greater than that of the absolute position measurements. This is the case of the close inspection of an infrastructure, as it may occlude the visibility of the GPS satellite constellation and the accuracy of LADAR sensors is increased with the proximity to the infrastructure. At close distance to the inspection target, only a subset of the admissible trimming trajectories may be considered, reducing the sensor-based trimming trajectories to helices with arbitrary vertical velocity and relative orientation, climbing or descending vertical lines, and stationary hover at a given distance from the target. It is also assumed that the inspection targets are approximately cylindrical, which includes many bridge piers, industrial chimneys, wind turbine towers, etc. The LADAR coordinate frame {L} is usually defined when working with this sensors, 66

4.3 Sensor-based Error Dynamics considering that its origin is located at the laser beam firing point. Without loss of generality, the LADAR is assumed to be at the center of mass and aligned with the body-fixed frame, i.e., {L} = {B} = {CM}, as the transformations between these frames are assumed to be known. Nonetheless, one additional frame is considered, denoted as the horizontal body-fixed frame, {H}, such that E pH = E pB denotes the position of frame {H} described in frame {E}, EH R = Rz (ψB ) is the rotation from frame {H} to frame {E}, and HB R = Ry (θB ) Rx (φB ) the rotation from frame {B} to frame {H}. It is assumed that the roll, pitch, and yaw measurements, respectively, φB , θB , and ψB , are readily available. Consider also that at each sampling instant, the horizontally mounted laser scanner provides a set of nL laser data points (ρi , αi ), for i = 1, . . . , nL , where ρi stands for the range distance measurement and αi is the respective bearing. The main assumptions on which the development of the laser space kinematics is based are presented below. The first assumption concerns the measurements that are available for the kinematics computations. Assumption 4.1. There is no access to absolute position measurements. However, accurate estimates of velocities and attitude are provided. Assumption 4.1 implies that the state variables vB , ω B , and λ B can be used. This might be achieved by using an INS unit, that readily provides measurements of acceleration, angular velocities, and of the earth magnetic field vector, which can be used by navigation filters to provide the necessary variables. The next assumption concerns the shape of the inspection targets and reflects one the main applications of this control methodology, which is the automatic inspection of concrete bridge piers. For the sake of simplicity, the inspection targets are referred to as the bridge piers for the remainder of this section. Assumption 4.2. The piers of the bridge are similar to a vertical cylindrical volume. Concrete bridge piers usually have cylindrical volume or at least a rectangular section. This last shape can also be tackled by the proposed methodology as will be discussed later. One further assumption is made about the ground surrounding the inspection area, as stated below. Assumption 4.3. The ground surrounding the inspection area is assumed to be planar and horizontal.

4.3.1

Pier Geometry and Center Position

The main goal after acquiring the laser measurements is to detect invariant points from which we can infer the relative position of the helicopter. As this task is especially daunting in frames {L} or {B}, the original measurements of the LADAR in frame {B}, (B ρi , B αi ), are projected into frame {H} and denoted as (ρi , αi ) The relation between range and angular information in {B} and their projection into {H} can be expressed as 2 2 ρi2 = B ρi2 cos(B αi ) cos(θB ) + sin(B αi ) sin(φB ) sin(θB ) + sin(B αi ) cos(φB ) , 67

Chapter 4: Sensor-based Control

x

½1 ®1

®n y

pctr ½n

Figure 4.1: Laser measurements intersecting the pier.

and tan(αi ) =

sin(B αi ) cos(φB ) . cos(B αi ) cos(θB ) + sin(B αi ) sin(φB ) sin(θB )

It is straightforward to see that the intersection of any vertical cylindrical pier with the xyplane of frame {H} yields a circle. In this way, the laser measurements (ρi , αi ) that represent the pier also represent an arc of a circle. Using basic notions of geometry, it can be seen that, given any point outside the circle and in the same plane, there are only two lines that pass through this point and are tangent to the circle, as shown in Figure 4.1. These lines are approximated by the first and last laser measurement that intersects the pier, provided that the angular resolution of the laser is sufficiently high, which is typically below 0.25 degrees. Therefore, the center of the pier can be estimated using the first and last laser measurements transformed into the {H} frame, respectively (ρ1 , α1 ) and (ρn , αn ), where n is the number of laser measurements that intersect the pier. The expressions for the equivalent range and angle of the center of the pier are given by ρctr =

ρ1 + ρn 1 2 cos( αn −α 2 )

,

αctr =

α1 + αn . 2

The fact that only the first and last measurements of the laser are used to estimate the center of the pier indicates that if its shape is not exactly a cylinder, the algorithm will still be valid. Naturally, the width of the pier measured by the laser will not have a direct relation with the its center, contrary to what is obtained with a cylindrical pier.

4.3.2

Ground Measurement

In order to have a complete 3-D position measurement, an additional altitude sensor is necessary. In the approach presented in this chapter it is assumed that an additional range only laser sensor is placed on board the vehicle in order to acquire the distance to the ground along the z-axis of the sensor frame {B}. For simplicity, and once again without loss of generality, it is assumed that both laser systems are located in the same reference frame, {B} (the laser scanner 68

4.3 Sensor-based Error Dynamics on the xy plane and the range only laser in the z-axis). Considering Assumption 4.3 and the fact that the pitch and roll angles are known, the distance-to-ground measurement in frame {B}, denoted as B zhg , can be easily projected into the z-axis of frame {H} using the expression zhg = B zhg cos φB cos θB . Note that the horizontal ground assumption, which is somehow restrictive, could be easily overcome with the use of an additional laser scanner placed in the yz plane of frame {B}, replacing the range only sensor in the z-axis. In this way, the vehicle could easily estimate a general ground plane and navigate relative to some initial altitude. Alternatively, a pressure sensor could be used to obtain an independent measurement of altitude, irrespectively of the terrain topology.

4.3.3

LADAR Kinematics

The equations of motion for the LADAR kinematics are naturally divided into the horizontal plane and the vertical axis kinematics, corresponding to the two different laser sensors previously referred. Nonetheless, they can be considered as a unique sensor that provides the position of the center of the pier at ground level, which can be expressed as

pctr

  ρctr    = Rz (αctr )  0  , z  hg

with time derivative given by

p˙ ctr

   ρ˙ ctr    = Rz (αctr ) ρctr α˙ ctr  .  z˙ 

(4.4)

hg

Moreover, the same point can be expressed as pctr = HE R(E pctr − E pB ) , and its time derivative, as a function of the vehicle velocities, can be written as p˙ ctr = HE R˙ (E pctr − E pB ) − HE R E p˙ B Πz ω B ) HE R˙ (E pctr − E pB ) − HB R BE R E p˙ B , = −S(Π resulting in Πz ω B )pctr − HB R vB , p˙ ctr = −S(Π

(4.5)

where the skew-symmetric matrix S(a) denotes the cross product matrix operator, S(a) b = a × b, and Πz = diag (0, 0, 1) is the projection matrix onto the z-axis. Note that the operator diag(A1 , . . . , An ) is a block diagonal matrix of its arguments. 69

Chapter 4: Sensor-based Control m gB

Gravity Rigid-Body

vw

fext

vB

Helicopter Components n ext

uB

Dynamics ωB

LADAR Kinematics

λB η

Figure 4.2: Helicopter model with LADAR kinematics.

Combining (4.4) and (4.5), after some algebraic manipulation, in order to obtain the differential equations that define the LADAR-based kinematics results in    ρ˙ ctr  α˙  = r (r) − I (ρ ) RT (α ) H R v ,  ctr  η η ctr ctr B B z  z˙  hg

h iT h i where rη (r) = 0 −r 0 , HB R = Ry (θB ) Rx (φB ) and Iη (a) = diag( 1 1/a 1 ), for all a , 0. Definh iT ing the generalized laser-based position vector as η = ρctr αctr zhg , the laser-based kinematics equation can be rewritten as η˙ = rη (r) − Iη (ρctr ) RTz (αctr ) Ry (θB ) Rx (φB ) vB .

(4.6)

Thus, this laser-based position kinematics equation can be used to replace (2.1c), as depicted in Figure 4.2, and formulate a new trajectory dependent error space to be used in a sensor-based control synthesis.

4.3.4

Error Dynamics

The concept of equilibrium set and trimming trajectories for the helicopter model has already been introduced in Chapter 2, whereas a generalized error space based on absolute position kinematics to describe the helicopter’s motion about trimming trajectories is presented in Section 4.2. In this section, a new parameterization is defined for a subset of the trimming trajectories, suitable for the sensor-based approach, and a novel error space is derived based on the position of the helicopter relative to a detected target. Consider the helicopter equations of motion presented in (2.1a), (2.1b), (2.1d), and the sensor-based position kinematics (4.6), denoting the desired trimming values for the state and input vectors as vC , ω C , η C , λ C , and uC . For the envisioned application, the trimming trajectories of interest are only z-aligned helices and hover, which are characterized by constant values of range and bearing of the pier center, denoted as ρC and αC . This subset of the possible helicopter trimming trajectories, EL ⊂ E, can be described by the parameterization h iT ξ L = ρC αC ψ˙ C vzC , 70

4.3 Sensor-based Error Dynamics where ρC is the desired value of the distance to the pier, αC is the desired azimuth of the pier in frame {H} and vzC is the desired z-component of the velocity vector described in {H}. This parameterization can be mapped onto the more general parameterization already introduced in (2.5), which fully parameterizes the set of achievable helicopter trimming trajectories, E, as detailed in [Cun07]. Remark 4.1. Note that other, more general, parameterizations for the trimming trajectories using the laser-based kinematics are possible. For instance, it is fairly easy to conclude that straight lines aligned with the pier could be parameterized by considering α˙ C = 0, which would result in a constant ρ˙ C . The new sensor-based error vector defined between the sensor-based vehicle state and the commanded trajectory can be defined using the nonlinear transformation    vB − vC  ve       ω   ωB − ωC xLe =  e  =  η −ηC η  e      −1 λB − λ C ) λe QB (λ

     .  

In addition, let ue = uB − uC and vwe = vw , assuming that there is no disturbance at trimming, vwc = 0. It can be shown that, in the new error coordinate system, the linearization of the rigid-body dynamics and sensor-based kinematics given by (2.1a), (2.1b), (2.1d), and (4.6) along a trimming trajectory is time invariant, and that the nonlinear error dynamics can then be expressed as             

v˙ e ω˙ e η˙ e λ˙ e

= = = =

v˙ B ω˙ B , η˙ d −1 ω B − Q−1 B QC ω C − dt QB QB λ e ,

(4.7)

noting that v˙ C = 0 and ω˙ C = 0 for any trimming trajectory, and defining QC as in the previous error space formulation. The linearization of (4.7) about the origin, or equivalently, the linearization of (2.1a), (2.1b), (2.1d), and (4.6) about the trimming trajectory can be expressed in the generalized error space as  ω e + Aλv δλ λe + Bv δue + Bwv δvw δv˙ e = Avv δve + Aω  v δω    v ω λ  ˙ ω e + Aω δλ λe + Bω δue + Bwω δvw  δω e = Aω δve + Aω δω , (4.8) η  v δv + Aω δω λ λ  ˙ ω η δ η = A + A δη  η e e e + Aη δλ e η η  e   ˙ ω e − S(ω ω c ) δλ λe δλ e = δω y ∂ where Ax = ∂y fx (.) are constant matrices for each trimming trajectory, and fx (.) represents the c functions defined by Equations (2.1a), (2.1b), and (4.6), as already mentioned in Section 4.2. Introducing the output vector yLe , the general disturbance input vector we , and rewriting (4.8) in a compact form yields (

δx˙ Le yLe

ξ ) δxLe + Bwe (ξ ξ ) δwe + B(ξ ξ ) δue = ALe (ξ . = Ce δxLe + De δwe + E δue

(4.9) 71

Chapter 4: Sensor-based Control Thus, as in the regular trajectory tracking error dynamics, it follows that there is a linear time invariant plant (4.9), associated with each trimming trajectory ξ ∈ EL , for which a linear controller can be designed.

4.4 4.4.1

Controller Design Synthesis

In this section a LMI approach is used to tackle the continuous-time state feedback H2 synthesis problem for polytopic LPV systems. Consider a LPV system of the form ( ξ )x + Bw (ξ ξ )w + B(ξ ξ )u x˙ = A(ξ ξ ξ ξ z = C(ξ )x + D(ξ )w + E(ξ )u

(4.10)

where x is the state, u is the control input, z denotes the error signal to be controlled, and w denotes the exogenous input signal. The system is parameterized by ξ , which is a possibly j

time-varying parameter vector and belongs to the convex set E j = co(E0 ). The operator co (.) j

ξ 1 , . . . ,ξ ξ nj }, where ξ 1 to ξ nj denotes the convex hull of the elements of the argument set, E0 = {ξ are the vertices of a polytope. It is also noted that the controller synthesis presented in this section will only be valid for a specific operating region, here represented by E j ⊂ E. Applying the static state feedback law given by u = K x to (4.10) results in the closed-loop system given by ( ξ ) := Tzw (ξ

ξ ) x + Bc (ξ ξ)w x˙ = Ac (ξ , ξ ) x + Dc (ξ ξ)w z = Cc (ξ

ξ ) denotes the resulting closed-loop operator from the disturbance input w to the where Tzw (ξ ξ ) = A(ξ ξ ) + B(ξ ξ ) K, Bc (ξ ξ) = performance output z, and the system matrices are defined as Ac (ξ ξ ), Cc (ξ ξ ) = C(ξ ξ ) + E(ξ ξ ) K and Dc (ξ ξ ) = D(ξ ξ ). The closed-loop system can be characterized in Bw (ξ terms of quadratic stability using the following definition, where X 0 denotes that matrix X is positive definite. Definition 4.1 (Quadratic stability, [BGFB94]). The system is said to be quadratically stable if there ξ ) X + X Ac (ξ ξ ) ≺ 0 is satisfied for all ξ ∈ E j . exists a matrix X 0 such that ATc (ξ It can be seen that testing for stability or solving the synthesis problem without any further result, involves an infinite number of LMIs. Thus, several different structures for LPV systems have been proposed which reduce the problem to that of solving a finite number of LMIs. In this section, an affine polytopic description is adopted, which can also be used to model a wide spectrum of systems and, as shown in the results presented in Section 4.5, is an adequate choice for the system at hand. Definition 4.2 (Affine polytopic LPV system). The system (4.10) is said to be a polytopic LPV system if the system matrix " # ξ ) Bw (ξ ξ ) B(ξ ξ) A(ξ ξ) = P(ξ ξ ) D(ξ ξ ) E(ξ ξ) C(ξ 72

4.4 Controller Design ξ ) ∈ co P1 , . . . , Pnr for all ξ ∈ E j , where verifies P(ξ " A Pi = i Ci

Bw i Di

Bi Ei

#

j j ξ 1 , . . . ,ξ ξ nj }, and P(ξ ξ) for all i = 1, . . . , nj . Moreover, if E j is a polytopic set, such as E j = co E0 , E0 = {ξ ξ i ) for all i = 1, . . . , nj , i.e., the vertices of the parameter set can be depends affinely on ξ , then Pi = P(ξ uniquely identified with the vertices of the system. This polytopic structure used with the following lemma, enables the use of a powerful set of results. Proposition 4.1 ([SW00, Proposition 1.19]). Let f : E j → R be a convex function defined on the j

ξ ) ≤ α for all ξ ∈ E j if and only if f(ξ ξ ) ≤ α for all convex set E j = co E0 . Then, for some α ∈ R, f(ξ j

ξ ∈ E0 . Thus, the quadratic stability of an affine polytopic LPV system can be easily established if there j

ξ ) X + X Ac (ξ ξ ) ≺ 0 is satisfied for all ξ ∈ E0 . exists a matrix X 0 such that ATc (ξ The H2 synthesis problem can be described as that of finding a control matrix K that ξ )kH2 . ξ ), denoted by kTzw (ξ stabilizes the closed-loop system and minimizes the H2 -norm of Tzw (ξ ξ ) = 0 in order to guarantee that kTzw (ξ ξ )kH2 is finite for every It is assumed that matrix D(ξ internally stabilizing and strictly proper controller. The following theorem is used for controller design and relies on results available in [GN99] and [SW00], after being rewritten for the case of polytopic LPV systems. In the following, tr (.) denotes the trace of the argument matrix. Theorem 4.2 (Polytopic stability). If there are real matrices X = XT 0, Y 0, and W such that "

# ξ ) X + X AT (ξ ξ ) + B(ξ ξ ) W + WT BT (ξ ξ ) Bw (ξ ξ) A(ξ ≺0 ξ) BTw (ξ −I " # ξ ) X + E(ξ ξ)W Y C(ξ 0 ξ ) + WT ET (ξ ξ) X CT (ξ X tr (Y) < α 2 .

(4.11a) (4.11b) (4.11c)

j

for all ξ ∈ E0 , where the static feedback controller is defined as K = W X−1 , then the closed-loop system is quadratically stable and there exists and upper-bound α for the continuous-time H2 -norm ξ ) for all ξ ∈ E j , i.e., of the closed-loop operator Tzw (ξ ξ )kH2 < α , ∀ξ ξ ∈ Ej . kTzw (ξ

(4.12)

Proof. Using Proposition 4.1 and assuming an affine polytopic LPV system, it is trivial to note j

that satisfying the LMI system for all ξ ∈ E0 is equivalent to satisfying the same system for all ξ ∈ E j . The proof that the LMI system (4.11) implies (4.12) can be obtained from the definition ξ ). Let the transfer function matrix of the closed-loop operator Tzw (ξ ξ ) be of H2 -norm of Tzw (ξ 73

Chapter 4: Sensor-based Control j

denoted as Tizw (s) for some parameter vector ξ i ∈ E0 , and defined as ξ i ) [s I − Ac (ξ ξ i )]−1 Bc (ξ ξ i ) + Dc (ξ ξi) Tizw (s) = Cc (ξ ξ i ) + E(ξ ξ i ) K] [s I − A(ξ ξ i ) − B(ξ ξ i ) K]−1 Bw (ξ ξ i ) + D(ξ ξi) . = [C(ξ Then, the H2 -norm of Tizw (s) is defined as ! Z +∞

2

1 H

Tizw := tr Tizw (jω) Tizw (jω) dω H2 2π −∞ which using Parseval’s theorem can be rewritten as ! Z +∞

2 T

Tizw = tr Hi (t) Hi (t) dt H 2

0

where Hi (t) is the impulse response matrix of Tizw (s). Noting that the impulse response can be defined as ξ i ) eAc (ξξ i ) t Bc (ξ ξi) , Hi (t) = Cc (ξ then Z

2

Tizw = tr H 2

Z

+∞ 0 +∞

= tr 0

T ξ i ) eAc (ξξ i ) t CTc (ξ ξ i ) Cc (ξ ξ i ) eAc (ξξ i ) t Bc (ξ ξ i ) dt BTc (ξ

ξi)e Cc (ξ +∞

Z

! !

ξi)t Ac (ξ

T ξ i ) BTc (ξ ξ i ) eAc (ξξ i ) t CTc (ξ ξ i ) dt Bc (ξ

ξi)t Ac (ξ

T ξ i ) BTc (ξ ξ i ) eAc (ξξ i ) t dt CTc (ξ ξi) Bc (ξ

!

ξi) = tr Cc (ξ e 0 ξ i ) Wctr (ξ ξ i ) CTc (ξ ξi) , = tr Cc (ξ where

Z ξ i ) := Wctr (ξ

+∞

0

T

ξ i ) BTc (ξ ξ i ) eAc (ξξ i ) t dt eAc (ξξ i ) t Bc (ξ

stands for the controllability Grammian, given by the symmetric positive definite solution of the Lyapunov equation ξ i ) Wctr (ξ ξ i ) + Wctr (ξ ξ i ) ATc (ξ ξ i ) + Bc (ξ ξ i ) BTc (ξ ξi) = 0 . Ac (ξ j

It can be easily seen that, for each parameter vector ξ i ∈ E0 , α is an upper bound for the H2 -norm ξ i ) ≺ X such that of the closed-loop operator Tizw if and only if there exists X 0, 0 ≺ Wctr (ξ ξ i ) X + X Ac (ξ ξ i )T + Bc (ξ ξ i ) BTc (ξ ξi) ≺ 0 Ac (ξ

(4.13)

ξ i ) X CTc (ξ ξ i ) < α2. tr Cc (ξ

(4.14)

and

Equation (4.13) can be rewritten as " # ξ i ) X + X ATc (ξ ξ i ) + Bc (ξ ξ i ) BTc (ξ ξi) 0 Ac (ξ ≺0 0 −I

74

4.4 Controller Design that using Schur complements becomes # " ξ i ) X Bc (ξ ξi) ξ i ) X + X ATc (ξ Ac (ξ ≺0, ξi)X BTc (ξ −I or equivalently, introducing the matrix W = K X yields " # ξ i ) X + X AT (ξ ξ i ) + B(ξ ξ i ) W + WT BT (ξ ξ i ) B(ξ ξi) A(ξ ≺0. ξi) BT (ξ −I

(4.15) j

Under the conditions of the theorem, it can be seen that (4.15) is satisfied for all ξ i ∈ E0 and that (4.11b) can be written as "

ξi) Y Cc (ξ T −1 ξi) X Cc (ξ

# 0,

which, applying once again Schur complements, implies that " # ξ i ) X CTc (ξ ξi) 0 Y − Cc (ξ 0 0 X−1 and consequently ξ i ) X Cc (ξ ξ i )T < tr (Y) < α 2 , tr Cc (ξ j

also, for all ξ i ∈ E0 . Thus, (4.14) is implied and the bound on the H2 -norm is established. As (4.13) is implied by the conditions of the theorem, it is trivial to note that ξ i ) X + X Ac (ξ ξ i )T ≺ −Bc (ξ ξ i ) BTc (ξ ξi) 0 , Ac (ξ j

for all ξ i ∈ E0 , implying the quadratic stability of the closed-loop and, thus, concluding the proof. With this result, the optimal solution for the continuous-time H2 control problem is approximated through the minimization of α subject to the LMIs of Theorem 4.2.

4.4.2

Implementation

The linear state feedback controllers defined above are implemented to meet the following design specifications: i) achieve zero steady state error in position and yaw angle; ii) ensure that the actuators are not driven beyond their natural bandwidth, considered to be 10 rad/s; and iii) reject the wind disturbance, modeled using (4.16). Consider the feedback system shown in Figure 4.3, where P is a continuous-time linear model of the helicopter error dynamics for a specific trimming trajectory, and K is a state feedback controller to be designed. The augmented system G, shown within the dashed line, denotes the synthesis model used as a design model for the H2 controller synthesis algorithm. This model is derived from P by appending the depicted dynamics weights, that serve as tuning knobs to meet the desired performance specifications, 75


G ye

I

xi

W1

s

w

W4

vw z

W2

P u

dx e

W3 xi

dx e x x2 x4

K Figure 4.3: Synthesis model.

whereas the variable w denotes a vector of Gaussian white noise with zero mean and unitary intensity, and vw is the wind velocity disturbance. To guarantee that the closed-loop system has zero steady-state error in response to the desired position and yaw angle, the integral of ye is included in the design as a performance output, augmenting the state dynamics with the equation x˙ i = ye . For the case of absolute position h iT control, this integral action performance output is defined as ye = pTe ψe , whereas for the h iT sensor-based position control is given by ye = η Te ψe . To meet the design requirements the weighting function associated with the integral state is chosen as W1 = 0.1 I4 , the dynamic weight associated with the actuation vector u is W2 (s) = diag W20 (s), W21c (s), W21s (s), W20t (s) where 13 10 13 W21c (s) = 10 W20 (s) =

s + 10 s + 13 s + 10 s + 13

,

13 10 10 W20t (s) = 7

W21s (s) =

, and

s + 10 , s + 13 s+7 , s + 10

the state weight is given by W3 = 0.1 I12 , and the dynamic weight associated with the disturbance process is W4 (s) = diag (W4u (s), W4v (s), W4w (s)) where W4(.) (s) represent Von Karman disturbance model transfer functions, as detailed in [Pad96] and references therein. For the presented simulation scenarios, these transfer functions 76

4.4 Controller Design are defined as q

W4u (s) =

2 Lu π V 1 + 1.357 LVu

σu

q

W4v (s) =

1 Lv π V 1 + 2.996 LVv

σv

q

W4w (s) =

, s + 0.199 ( LVu s)2 1 + 2.748 LVv s + 0.34 ( LVv s)2

1 Lw π V 1 + 2.996 LVw

σw

1 + 0.25 LVu s

s + 1.975 ( LVv s)2 + 0.154 ( LVv s)3 1 + 2.748 LVw s + 0.34 ( LVw s)2 s + 1.975 ( LVw s)2 + 0.154 ( LVw s)3

(4.16a)

(4.16b)

,

(4.16c)

,

where σ(.) denotes the turbulence intensity of each component, L(.) denotes the turbulence scale lengths and V is the aircraft speed norm. Thus, the synthesis model, G, can be written as (

ξ )x + Bw (ξ ξ )w + B(ξ ξ )u x˙ = A(ξ , z = Cx + Dw + Eu

where h x = δxTe

xTi

xT2

xT4

iT

∈ R12+4+4+8

is the new state vector, w ∈ R3 is the white noise disturbance vector, u ∈ R4 is the actuation vector, and the design matrices are given by  ξ)  Ae (ξ   Ce ξ ) =  A(ξ  0  0  ξ)  Be (ξ   Ee ξ ) =  B(ξ  Bw2  0

 ξ ) Cw4  0 0 Bwe (ξ  0 0 De Cw4   ,  0 Aw2 0  0 0 Aw4    0   0 W1    , C =  0 0 C w2    W3 0 0

 ξ ) Dw4  Bwe (ξ   De Dw4 ξ ) =  Bw (ξ 0   Bw 4    0   0     0  , E =  Dw2  ,    0 0

     ,  

and D = 0, whereas Awi , Bwi , Cwi , Dwi result from the state space representation of Wi (s). It is also noted that for the sensor-based control approach, the only differences are the substitution ξ ) by ALe (ξ ξ ). of δxe by δxLe and Ae (ξ For the implementation of the overall controller within the framework of the gain scheduling theory, the set of all possible trimming trajectories, E, is partitioned into overlapping regions of operation, E j for j = 1, . . . , nr , such that the union of all operating regions completely covers the set of all possible trimming trajectories, i.e., E 1 ⊕ E 2 ⊕ · · · ⊕ E nr = E. Figure 4.4 depicts the controller implementation scheme based on the D-methodology, comprehensively described in [KPKC95], which moves all integrators to the plant input, and adds differentiators where they are needed to preserve the transfer functions and the stability characteristics of the closed-loop system. This implementation methodology provides several important properties that are worthwhile emphasizing, which include: 77


vw xB

Plant

u

Operating Region

I

s

Ki(s)

s

xe ye

Error Space Reference Trajectory

D-implementation

Figure 4.4: Implementation setup.

Linearization property: at any given trimming point, the linearization of the closed-loop system consisting of the full gain-scheduled controller and the nonlinear plant exhibits the same internal and input-output properties as the closed-loop interconnection of the linearized plant and the corresponding linear controller; Auto-trimming: the integrator block at the output of the controller gain naturally provides the actuation trimming values as well as the trimming values for the state variables that are not required to track reference values; Anti-windup: if it is assumed that D = 0, then the block of integrators is directly at the plant input, enabling the straightforward implementation of anti-windup schemes. Thus, using the D-methodology implementation with the synthesized controllers for each of the considered operating regions allows the controller to switch between regions without having to interpolate between the individual controllers.

4.5

Simulation Results

To perform accurate infrastructure inspection, an autonomous helicopter is required to cover the selected targets of the infrastructure comprehensively in order to detect cracks, corrosion, leaks, or any other indicator that further maintenance procedures are required. Therefore, the required flight envelope is characterized by low speed, high yaw maneuverability, good vertical flight capabilities, and the possibility of describing helices around the infrastructure. This section provides realistic simulation results for the two trajectory tracking controllers proposed in this chapter, obtained in the Matlab® /Simulink® simulation environment using the nonlinear dynamic helicopter model SimModHeli, parameterized for the Vario X-Treme model-scale helicopter, as detailed in Chapter 2, in [1, Cun07], and references therein. In 78

4.5 Simulation Results Table 4.1: Operating regions for the absolute position controller.

Parameter VC ψ˙ C γC ψCT

Intervals [−3, 3] [−11.5, 11.5] [−100, −85]; [−90, −60]; [−65, −35]; [−40, −10]; [−15, 15]; [10, 40]; [35, 65]; [60, 90]; [85, 100] [−190, −160]; [−165, −135]; [−140, −110]; [−115, −85]; [−90, −60]; [−65, −35]; [−40, −10]; [−15, 15]; [10, 40]; [35, 65]; [60, 90]; [85, 115]; [110, 140][135, 165]; [160, 190]

Unit m/s deg/s deg deg

addition to the wind disturbance noise generated using the Von Karman disturbance model, as described in Section 4.4.2 and with covariance matrix defined as diag (0.98, 0.98, 0.5) m2 s−2 , both simulation scenarios presented below resort to a discrete wind gust with amplitude 2.5m/s, rising time of 1s, constant direction in the earth-fixed frame, and applied at time t = 20 s. Each of the two simulation scenarios, absolute position and sensor-based position controllers, was carried twice to enable the comparison of two sets of data: one without any wind disturbance and other considering the previously defined wind disturbance.

4.5.1

Absolute Position Control

Recalling that the set of possible trimming trajectories, E, of the helicopter model with absolute position kinematics, can be parameterized by the vector h i ξ = VC γC ψ˙ C ψCT , the design of the gain switching controller for the absolute position approach considers nr = 225 polytopic regions, E j for all j = 1, . . . , nr , each of them with 24 = 16 vertices. Namely, for parameters VC and ψ˙ C only one interval is considered, while parameters γC and ψCT are respectively partitioned into the 9 and 15 overlapping intervals, as presented in Table 4.1. The state space matrices of the continuous-time system (4.10) are approximated by affine functions on the parameters ξ ∈ E j , for j = 1, . . . , nr , using least squares fitting. The error introduced by this approximation for all the operating regions can be measured by comparing the resulting LTV systems with the original nonlinear system, and it was observed that the average relative error on the matrix entries is always less than 6.49% for this simulation. Note that as the number of regions increase, this error can be further reduced, as is the case of the simulation scenario of Section 4.5.2, where 945 overlapping convex regions were used. This simulation setup is motivated by a specific inspection application, where the vehicle is required to inspect a power plant chimney with realistic wind disturbances, as depicted in Figure 4.5, such that the performance of the overall closed loop system in a real mission scenario can be effectively evaluated. For that purpose the vehicle is required to track a trajectory composed of: 79


1. a straight line moving sideways, with VC = 2 m/s, ψCT = 90 deg and ψ˙ C = γC = 0; 2. a helix keeping the vehicle facing the surface under inspection, with VC = 2 m/s, ψCT = π/2 rad, ψ˙ C = 14 deg/s and γC = 20 deg; 3. stationary hover at a specified point. The results of this trajectory tracking simulation scenario are represented in Figures 4.5-4.9, featuring the position and attitude errors, the control errors, the wind disturbances, the linear and angular velocities as well as their reference values, and finally the transitions between regions of operation. Furthermore, the transitions between straight line, helix, and hover reference trajectories, as well as the moment the wind gust is applied, are also represented in these figures with vertical lines, respectively, at t = 10 s, t = 63 s, and t = 20 s. It can be seen that in the first 10 seconds the vehicle has no problem following the straight line moving sideways, and rejecting the wind disturbance with tracking errors below 2.5 cm in position and 0.17 deg in attitude. The transition from straight line to an ascending helix is also smooth, and the tracking and actuation errors quickly return to negligible values. However, when the wind gust is applied, which has a constant direction in the earth-fixed frame {E}, the actuation applied to the vehicle as to adapt to ensure that the desired trajectory is followed. This out-of-trim behavior can be observed in Figure 4.7(a), and the result is that, even under the influence of a strong wind gust, the tracking errors remain below 18 cm in position and 1.7 deg in attitude. The abrupt transition from the helix trajectory to hover, and also maintaining a stationary hover while under strong wind disturbances, is the most demanding moment of this simulation scenario. Nonetheless, it can be seen that the proposed control system is able to make a smooth transition into hover and cope with the wind disturbances to maintaining it, with tracking errors not greater than 1.55 m in position and 1.95 deg in attitude during the transient part, as well as errors below 4.5 cm in position and 1.6 deg in attitude during the last 10 seconds of simulation. It can also be seen in Figure 4.9, which presents the operating region transitions, that the controller has the most number of transitions concentrated on the final part of the trajectory, where it must maintain a stationary hover. Note also that, despite the high rate of transitions between regions of operation, no sign of performance degradation is observed in these results.

4.5.2

Sensor-based Control

The parameter vector for the sensor-based control approach, previously defined as ξ L = h i ρC αC ψ˙ C vzc , is considered to parameterize a subset of all possible trimming trajecto80

4.5 Simulation Results

40

30

−Z

20

10

0

−10

30

−20

20

−10

10 0

0 −Y

X Figure 4.5: Automatic industrial chimney inspection, using absolute position control.

81


0.4

helix

wind gust

hover

xe [m]

0.2 0 −0.2 −0.4

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

20

30

40 Time[s]

50

60

70

ye [m]

1 0 −1 −2

0.5 p e pe

ze [m]

0

noise

−0.5 −1

0

10

(a) Position error pe 0.3

helix

wind gust

hover

φe [rad]

0.2 0.1 0 −0.1

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

20

30

40 Time[s]

50

60

70

0.1 θe [rad]

0.05 0 −0.05 −0.1

ψe [rad]

0.3 λe

0.2

λe

noise

0.1 0 −0.1

0

10

(b) Euler angles errors λ e

Figure 4.6: 3-D trajectory tracking results: position and attitude errors without wind disturbance (red solid line) and with wind disturbances (blue dashed line).

82


helix

wind gust

hover

0

0

c

θe [rad]

0.1

−0.1

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

20

30

40 Time[s]

50

60

70

1c

0

c

θe [rad]

0.05

−0.05

1s

0

c

θe [rad]

0.05

−0.05

ue ue

0t

0

c

θe [rad]

0.1

noise

−0.1

0

10

(a) Actuation error ue

uwind [m/s]

4 2 0 −2

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

vwind [m/s]

4 2 0 −2

wwind [m/s]

4 2 vw vw

0

noise

−2

0

10

20

30

40 Time[s]

50

60

70

(b) Wind velocity disturbances I vw

Figure 4.7: 3-D trajectory tracking results: actuation errors, without wind disturbance (red solid line) and with wind disturbances (blue dashed line), and wind velocity.

83


0.6

helix

wind gust

hover

u [m/s]

0.4 0.2 0 −0.2

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

20

30

40 Time[s]

50

60

70

1 v [m/s]

0 −1 −2 −3

0.5 v

c

v v

w [m/s]

0

noise

−0.5 −1

0

10

(a) Linear Velocity vB

p [rad/s]

0.6

helix

wind gust

hover

0.4 0.2 0 −0.2

0

10

20

30

40

50

60

70

0

10

20

30

40

50

60

70

20

30

40 Time[s]

50

60

70

0.1 q [rad/s]

0.05 0 −0.05 −0.1

r [rad/s]

0.3 0.2 ωc

0.1

ω ωnoise

0 −0.1

0

10

(b) Angular Velocity ω B

Figure 4.8: 3-D trajectory tracking results: linear and angular velocities without wind disturbance (red solid line), with wind disturbances (blue dashed line), and reference values (green dash-dotted line).

84

4.5 Simulation Results 140

helix

wind gust

hover

120

zone number

100

80

60

40

20 zone zonenoise 0

0

10

20

30

40 Time[s]

50

60

70

Figure 4.9: 3-D trajectory tracking results: transitions between operation regions without wind disturbance (red solid line) and with wind disturbances (blue dashed line).

ries, E. In the sensor-based case, the gain-switching controller design considers nr = 945 overlapping polytopic regions, E j for all j = 1, . . . , nr , with 24 vertices each, and defined partitioning ρC ∈ [4, 15] m into 3 intervals; αC ∈ [−100, 100] deg into 9 intervals; ψ˙ C ∈ [−14, 14] deg/s into 7 intervals; and vzc ∈ [−0.6, 0.6] m/s into 5 intervals. Figure 4.10 shows two projections of the operation regions into 2-D space, featuring the partition of the ρC αC -plane, transformed into the x y-plane for the sake of clarity, as well as the partitioning of the ψ˙ C vz -plane, described c

in the v w-plane. As in the absolute position approach, the state space matrices of the continuous-time system (4.10) are approximated by affine functions on the parameters ξ L ∈ E j , for j = 1, . . . , nr , using least squares fitting. The average relative error on the matrix entries introduced by this approximation for all the operating regions was observed to be always less than 2.19% for this simulation scenario. The specific application addressed in this simulation scenario is the automatic inspection of concrete bridge piers, where the GPS signals are degraded by the occlusion introduced by the bridge deck, to the point of becoming unreliable. Thus, the use of a sensor-based control approach to control the position of the vehicle relative to the piers is of extreme importance to guide the vehicle along the inspection trajectories depicted in Figure 4.11. The vehicle is required to track the trajectory composed of: 1. an ascending helix keeping the vehicle facing the pier, with ρC = 10 m, αC = 0 deg, 85


15

x [m]

10

5

0

−5

10

5

0 −y [m]

−5

−10

−w [m/s]

(a) ρC αC -plane partitioning

0.1 0.05 0 −0.05 −0.1 0.5

0.4

0.3

0.2

0.1

0 −0.1 −0.2 −0.3 −0.4 −0.5 −v [m/s]

(b) ψ˙ C vzc -plane partitioning

Figure 4.10: Projections in 2-D space of the sensor-based regions of operation.

86


45 40 35 30

−Z

25 20 15 10 5 0 −5 40

−40

30 −30

20 10

−20

0

−10

−10 −20

0 −Y

X

Figure 4.11: Automatic bridge pier inspection, using sensor-based control.

ψ˙ C = 11.5 deg/s and vzc = −0.48 m/s; 2. a stationary hover closer to the pier, with ρC = 9 m, αC = ψ˙ C = vzc = 0. The results of the sensor-based trajectory tracking simulation scenario are represented in Figures 4.11-4.15, featuring the laser-based position and attitude errors, the control errors, the wind disturbances, the linear and angular velocities as well as their reference values, and finally the transitions between regions of operation. The moment the wind gust is applied as well as the transition between the ascending helix and stationary hover are also represented in these figures with vertical lines, at t = 20 s and t = 62 s. It can be seen that in the first 20 seconds the vehicle has no problem following the ascending helix and rejecting the wind disturbance when it exists, showing tracking errors of no more than 2 cm in ρctr , 1.2 deg in αctr , and 3 cm in zhg . After the wind gust is applied, the actuation has to adapt to the constant direction of the wind gust while changing the vehicle direction to track the reference trajectory, as can be seen in Figure 4.13(a). Nonetheless, the tracking errors 87


1.5

wind gust

hover

ρe [m]

1 0.5 0 −0.5

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

20

30

40 50 Time[s]

60

70

80

0.3 αe [rad]

0.2 0.1 0 −0.1

0.6 ηe

[m]

0.4

ηe

noise

z

hg

e

0.2 0 −0.2

0

10

(a) Laser-based position error η e 0.15

wind gust

hover

φe [rad]

0.1 0.05 0 −0.05

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

20

30

40 50 Time[s]

60

70

80

0.3 θe [rad]

0.2 0.1 0 −0.1

ψe [rad]

0.6 λe

0.4

λe

noise

0.2 0 −0.2

0

10

(b) Euler angles errors λ e

Figure 4.12: 3-D sensor-based trajectory tracking results: position and attitude without wind disturbance (red solid line) and with wind disturbances (blue dashed line).

88


wind gust

hover

0

0

c

θe [rad]

0.1

−0.1

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

20

30

40 50 Time[s]

60

70

80

1c

0

c

θe [rad]

0.05

−0.05

1s

0

c

θe [rad]

0.05

−0.05

ue ue

0t

0

c

θe [rad]

0.05

noise

−0.05

0

10

(a) Actuation error ue

uwind [m/s]

4 2 0 −2

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

vwind [m/s]

4 2 0 −2

wwind [m/s]

4 2 vw vw

0

noise

−2

0

10

20

30

40 50 Time[s]

60

70

80

(b) Wind velocity disturbances I vw

Figure 4.13: 3-D sensor-based trajectory tracking results: control signals without wind disturbance (red solid line), with wind disturbances (blue dashed line), and wind velocity.

89


remain below 18 cm in ρ, 3.5 deg in α, and 7 cm in zhg , even under the influence of strong wind disturbance. As shown in the figures, the transition from the helix trajectory to stationary hover closer to the pier, which is the most demanding part of the desired trajectory, displays a smooth behavior and the trajectory tracking errors during the transition seem to be slightly smaller than those of the absolute position control approach under similar conditions, showing errors not greater than 1.1 m in ρctr , 8.0 deg in αctr , and 30 cm in zhg . In the operating region transitions, presented in Figure 4.15, it can be seen that this controller has also the most number of transitions concentrated on the final part of the trajectory, where it must maintain a stationary hover. Relative to the absolute position controller, the sensor-based controller shows a greater number of transitions, which can be justified by the fact that the latter controller has a larger number of operating regions. Note also that, despite the high rate of transitions between regions of operation, no sign of performance degradation is observed in the presented results, which is also verified for the absolute position controller presented above.

4.6

Concluding Remarks

This chapter presented the design and performance evaluation of two 3-D trajectory tracking controllers for autonomous helicopters, considering an absolute position solution available for control, such as a GPS unit, and alternatively, a sensor-based control approach. The sensor-based control approach is motivated by the automatic inspection of large infrastructures were the GPS signal is not reliable. One of the major contributions of this chapter is the introduction of a LADAR-based nonlinear kinematics, formulated in 3-D space, and a trajectory-dependent error space defined to express the dynamic model of the vehicle and the sensor-based kinematics. Resorting to a H2 controller design methodology for affine parameter-dependent systems, the proposed technique exploited an error vector that naturally describes the particular dynamic characteristics of the helicopter for a suitable flight envelope. For a given set of operating regions, a nonlinear controller was synthesized and implemented under the scope of gainscheduling control theory, using a piecewise affine parameter-dependent model representation. The effectiveness of the proposed control laws was assessed in a simulation environment with a nonlinear model of the helicopter and realistic mission scenarios. The quality of the obtained results clearly indicates that the proposed methodologies are well suited to be used for automatic inspection of large infrastructures. Future work includes the real-time implementation and performance evaluation of the proposed controllers during automatic inspection operation, concerning the absolute position and the sensor-based controllers, as well as the transition between them.

90

4.6 Concluding Remarks

1

wind gust

hover

u [m/s]

0.5 0 −0.5

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

20

30

40 50 Time[s]

60

70

80

1 v [m/s]

0 −1 −2 −3

0.2 v

c

w [m/s]

0

v v

−0.2

noise

−0.4 −0.6

0

10

(a) Linear Velocity vB

p [rad/s]

0.3

wind gust

hover

0.2 0.1 0 −0.1

0

10

20

30

40

50

60

70

80

0

10

20

30

40

50

60

70

80

20

30

40 50 Time[s]

60

70

80

q [rad/s]

0.2 0.1 0 −0.1 −0.2

r [rad/s]

0.3 0.2 ωc

0.1

ω ωnoise

0 −0.1

0

10

(b) Angular Velocity ω B

Figure 4.14: 3-D sensor-based trajectory tracking results: linear and angular velocities without wind disturbance (red solid line), with wind disturbances (blue dashed line), and reference values (green dash-dotted line).

91


900

wind gust

hover

800

700

zone number

600

500

400

300

200

100 zone zonenoise 0

0

10

20

30

40 50 Time[s]

60

70

80

Figure 4.15: 3-D sensor-based trajectory tracking results: transitions between operation regions without wind disturbance (red solid line) and with wind disturbances (blue dashed line).

92

5 Automatic LADAR Calibration This chapter proposes two estimation algorithms for the determination of attitude installation matrix for 2-D LADAR sensors mounted on board UAVs. While a comparative calibration algorithm assumes the existence of a known calibration surface, an automatic calibration algorithm does not require any prior knowledge on the trajectories of the vehicle or the terrain where the calibration mission is performed. The use of autonomous vehicles equipped with LADARs to conduct fully automatic surveys of terrain, infrastructures, or just to navigate safely in unknown environments, motivates the research on increasingly precise LADAR data acquisition and processing algorithms, for which the determination of the correct attitude installation matrix is critical. The proposed calibration algorithms rely on the minimization of the errors between the measured point cloud and a representation of the known calibration surface or, alternatively, the errors between several acquired point clouds, by comparing each measured point cloud with a surface representation of the others. The resulting optimization problems are addressed using two techniques: i) nonlinear optimization, where the attitude installation rotation matrix is parameterized by the ZYX Euler angles, and ii) optimization on Riemannian manifolds, enabling the estimation of the attitude installation matrix on the group of special orthogonal matrices SO(3). The proposed calibration techniques are extensively validated and compared using both simulated and experimental LADAR data sets, demonstrating their accuracy and performance.

5.1

Introduction

The calibration of 2-D LADARs on board an UAV, capable of complex 3-D motion, is one of the most challenging problems in the extrinsic calibration of LADAR sensors. There is comprehensive work in the literature that is dedicated to the analysis of the intrinsic and extrinsic LADAR error sources, such as in [Sch01, Fil01a] and references therein. Namely, the identified error sources include the attitude and position installation biases, range detection 93

Chapter 5: Automatic LADAR Calibration errors, scanning angle errors, vehicle attitude and position errors, time synchronization errors, etc. While most intrinsic errors can be identified and accounted for with laboratory experiments using the sensor, the installation biases are highly dependent on the vehicle and mounting apparatus. It is argued by these authors that the attitude installation bias, also simply referred to as the mounting bias, is a particularly important source of error in LADAR systems. For instance, an airborne LADAR acquiring terrain elevation 1.2 km above the ground, with 0.5 deg of roll mounting bias will generate points with an error of 10.5 m. Moreover, when working with rough terrain with high slopes, the distortions resulting from poorly calibrated installation bias can become highly nonlinear. This further motivates the necessity of the calibration of these errors, in particular, the attitude installation bias, to meet the desired accuracy requirements.

5.1.1

Relevant Work

Most applications requiring a 3-D reconstruction of the surrounding environment use one or several LADARs installed on board a UAV equipped with an INS/GPS unit, which provide measurements of the relative distances to the terrain and the trajectory of the vehicle, respectively. The most common calibration procedures require particular terrain features and specific vehicle trajectories in order to calibrate a subset of the parameters, as found in [RMW+ 97]. For instance, a standard procedure in the literature for airborne LADAR calibration would be to fly over a known flat surface while performing pitch or roll maneuvers, separately, which would enable the calibration of only these two parameters. Nonetheless, several difficulties render the problem of 2-D LADAR attitude installation calibration special, including the absence of any matching information between the measured point clouds and a known calibration surface, and also the fact that, with each calibration correction step, the reconstructed clouds of points will change their shape in a nonlinear fashion, accordingly to the vehicle trajectory and the terrain. There are two fundamental approaches in the literature to compare two clouds of points when there is no matching information between them. The first approach is to use a point-topoint metric, as in the by now classic iterative closest point (ICP) algorithm [BM92], and assign to each point of one cloud a matching point of the other cloud. An alternative approach is to use a point-to-plane metric and, thus, to measure the closest distance between each point of one cloud to a surface approximation of the other cloud [CGM92]. As the surface information is not taken into account in point-to-point based techniques, they suffer from the inability to slide overlapping clouds to find a better fit between them, demonstrating slower convergence rates than the point-to-plane alternatives [RL01]. To reduce the conservativeness of the calibration procedures of 2-D LADAR sensors, new algorithms were proposed in [HS99, Fil01b], where the measured point cloud is compared with a plane-wise representation of the known calibration surface to obtain the calibration parameters. These authors consider the linearization of the error model to obtain a Gauss-Helmert model and then obtain the least-squares estimate of the installation bias. This approach was refined in [3] 94

5.1 Introduction by considering the full nonlinear model of the reconstruction error and, consequently, resorting to nonlinear optimization techniques to obtain the optimal calibration parameters. More recently, especially with the use of highly expensive 3-D LADARs in ground vehicles, several strategies for LADAR calibration have been proposed that rely on the detection and association of artificial features in the point clouds [UHS07, GS10]. Nonetheless, the major drawback of these techniques is that they make strong assumptions about the environment where the calibration procedure takes place, either assuming complete knowledge of the calibration surface or adding artificial marks to enable feature based association. Within the field of mobile robotics, the calibration of 3-D LADARs onboard ground vehicles has been addressed in [LT10] and [MHN12]. The former uses a point-to-plane metric to define a cost function, based on Euler angles for the attitude installation bias, and a heuristic search in several directions to avoid local minima. The later approach resorts to an entropy-based point cloud quality metric, which allows for the calibration of a 3-D LADAR sensor using several overlapping 3-D scans. Nevertheless, it should be emphasized that both these methods are based on the use of 3-D LADARs and only present results for LADAR data acquired using ground vehicles. An entropy-based point cloud quality metric might be inappropriate for the automatic calibration of 2-D LADARs, as two clouds of points acquired using different trajectories may show very low entropy when considering a small search radius around each point.

5.1.2

Proposed Approach

The algorithms proposed in this chapter can cope with arbitrary 3-D motion of the vehicle and terrain topology, yielding accurate calibration of airborne 2-D LADAR sensors. It is assumed that there is enough information on the calibration terrain and vehicle trajectories to enable the full attitude installation bias calibration. One obvious counter-example would be a flat terrain, thus, with no relevant topological features, for which there would always be one uncalibrated degree of freedom. The proposed methods use a point-to-plane metric to compare each acquired point cloud with either a known calibration surface or a surface approximation of all the remaining acquired point clouds, by measuring the minimum distance of each point to the approximated surface. A full nonlinear model of the calibration errors is also defined, either using Euler angles or rotation matrices, thus avoiding the linearization problems of many approaches in the existing literature. In addition, by using a rotation matrix representation of the attitude installation bias, optimization tools on the special orthogonal group can be used. Two different approaches are considered for the calibration procedures addressed in this chapter: Comparative Calibration: considers arbitrary vehicle trajectories and assumes the existence of a known calibration surface that will be compared with the measured data during the calibration algorithm; Automatic Calibration: no a priori information on the terrain or trajectory is required 95

Chapter 5: Automatic LADAR Calibration and, at least, two measured data sets of the same terrain obtained from different vehicle trajectories are necessary. The former approach is included in this chapter for completeness, as it represents the classical approach to this calibration problem. Regarding the automatic calibration, where a known calibration surface is not required, several acquired overlapping point clouds of the same portion of terrain are compared between each other, while the vehicle preforms arbitrary maneuvers. The fundamental idea supporting the automatic calibration algorithm is that, in an ideal noise free world, only one attitude installation matrix should exist that fully justifies all the acquired point clouds, in which case the cost function would achieve its minimum value. The estimation problem is formulated within the scope of maximum likelihood (ML) theory [Kay93], allowing the formulation of the calibration problem as an optimization problem. The cost function to be minimized is defined by the summation of the errors between each measured data set and either the known calibration surface or a surface approximation of the other sets yielding a weighted scalar cost function. When using the ZYX Euler angles parameterization of the attitude installation bias rotation matrix, which is the standard approach in the literature, basic concepts of nonlinear optimization can be used. For completeness, this chapter includes the use of the gradient and Newton methods to find a search direction, as well as a line search algorithm, in particular the Wolfe rule, to compute the step size [Ber99, NW99]. To take advantage of the use of the rotation matrix parameterization, optimization tools on Riemannian manifolds are devised, enabling the use of gradient and Newton methods to directly estimate the attitude installation matrix on the group of special orthogonal matrices SO(3), [EAS98, AMS07]. This latter optimization approach can also use a modified version of the Wolfe rule to compute the step size along a geodesic of the manifold. Nonetheless, there exists an exact computation of the step size within SO(3), as detailed in [PKK00], which is shown to improve the performance of the algorithm. The proposed calibration techniques are compared using simulated and experimental data sets to assess their performance and limitations in realistic scenarios. The main contributions of this chapter are: (i) the formulation of the attitude installation bias comparative 2-D LADAR calibration problem as a nonlinear optimization problem which is not bind to any specific trajectory or terrain shape; (ii) the formulation of a novel 2-D LADAR automatic calibration problem for vehicles capable of 3-D motion, for which no a priori knowledge on the terrain or trajectories used in the calibration procedure is assumed; (iii) the solution of the optimization problem resorting to a Riemannian optimization framework to solve the problem within the group of proper rotation matrices; (iv) the application of an exact line search algorithm to find the optimal step size for the Riemannian optimization calibration algorithm; (v) the analysis and comparison of the proposed algorithms for several optimization techniques using realistic simulations; and (vi) the validation of the algorithms using experimental field data. The chapter is organized as follows. Section 5.2 introduces the 2-D LADAR calibration 96

5.2 Problem Definition problems. In Section 5.3 the optimization problem is addressed using the ZYX Euler angles parameterization, whereas Section 5.4 introduces the Riemannian optimization framework used to address the same calibration problems. The performance analysis of the proposed calibration algorithms as well as a comparison of the several optimization techniques are detailed in the simulation results presented in Section 5.5. In addition, the experimental results presented in Section 5.6 provide further validation of the automatic calibration approach. Finally, in Section 5.7 the main conclusions and directions for further research are offered.

5.2

Problem Definition

This chapter addresses the problem of finding the best possible estimate of the attitude installation bias of a 2-D LADAR sensor mounted on board a UAV capable of complex 3-D motion. In this section the comparative and the automatic calibration problems are defined. Firstly, the 3-D point reconstruction model is detailed, followed by the definition of a point-to-plane metric, which is then used to introduce the cost functions for the comparative and automatic calibration problems. Finally, the comparative and the automatic calibration algorithms are unified by rewriting them as particular cases of a common optimization problem, so that the same optimization tools can be easily applied.

5.2.1

3-D Point Reconstruction

The point reconstruction model describes the computations required to obtain 3-D points combining the vehicle trajectories and the LADAR raw data. In addition to the earth-fixed reference frame {E} and the body-fixed frame {B}, the following coordinate frames are required: • {L} – LADAR frame. Origin at the laser’s firing point, z-axis indicating the opposite direction of the zero scanning angle, and the y-axis perpendicular to the scanning plane; • {LB} – Laser beam frame. Origin at the laser firing point, y-axis collinear with that of frame {L} and z-axis oriented opposite to the direction of the laser beam. These frames and the relationships between them are depicted in Figure 5.1. A measurement l is defined by the pair (ρl , αl ), denoting the distance between the laser firing point and the laser hit point as ρl , and the angular position of the laser beam as αl , which stands for the angle from {LB} to {L}. Let the rotation from {LB} to {L} be defined as LLB R(αl ) = Ry (αl ), with Ry (.) standing for the rotation matrix about the y axis, and the laser range measurement denoted as h iT r(ρl ) = 0 0 −ρl . Then, the position measurement described in {L}, denoted by pl , can be defined as pl = LLB R(αl ) r(ρl ). Therefore, it can be seen that the expression that transforms the LADAR measurement into the reconstructed 3-D point E pl , expressed in the earth-fixed frame, is given by E

pl = EB Rl (R pl + b) + E pBl ,

(5.1) 97

Chapter 5: Automatic LADAR Calibration

B

z

L

z

B

{B}

y

LB

z

{L} {LB}

b L B

x

E

z

®l

E

pB

l

r(½l )

y = LBy

L

LB

x

x

E

y

{E} E

pl

E

x

Figure 5.1: LADAR acquisition coordinate frames.

where EB Rl is the platform attitude defined by the rotation from {B} to {E}, at the time that measurement l is acquired, and E pBl is the INS/GPS unit position expressed in {E}. The position installation bias is denoted by b ∈ R3 , which describes the laser firing point expressed in {B}, whereas R denotes the rotation matrix that defines the attitude installation bias, which is an element of the group of special orthogonal matrices SO(3). Let M(n, R) = {A : n × n matrix with real entries} and the group of orthogonal matrices be defined as O(n) = {U ∈ M(n, R) : U UT = In }, then, the group of special orthogonal matrices is SO(n) = {R ∈ O(n) : |R| = 1}, where the operator |.| denotes the determinant of the argument matrix. In this chapter, only the attitude installation bias is considered for calibration, noting that the position installation bias, b, can be more accurately measured and its influence in the reconstruction error is not as significant as that of the attitude installation bias. Nonetheless, the optimization algorithms proposed in the next sections can be used to calibrate simultaneously the attitude and the position installation biases with simple changes to the search direction and step size computations.

5.2.2

Cloud-to-Surface Comparison

Due to nature of the available measurements, it should be emphasized that there is no correspondence between the points of the different clouds of data for the calibration problems tackled in this chapter. Thus, the use of techniques that are based on the knowledge of the matching between points of different data sets is not an option. 98

5.2 Problem Definition E

pi

l

eij

n ij

l

l

E

p ¹ij

l

Figure 5.2: Point-to-plane distance metric.

Assume there are nc clouds of measured points, Ci for i = 1, . . . , nc , resulting from different trajectories of the vehicle, such that the intersection of the laser data boundaries of each cloud projected onto the other clouds is not empty. Each cloud is a set of npi laser reconstructed n o points, Ci = E pil for l = 1, . . . , npi , which consists of a nonlinearly distorted sampling with noise of the actual surface S. Each cloud Ci can also be approximated by a piecewise surface Si , consisting of a set of nsi planes. Moreover, the known calibration surface approximation used in the comparative calibration algorithm is denoted as S0 and is also a set of ns0 planes. The comparative calibration, which assumes that a known calibration surface is available, compares each cloud Ci , for all i = 1, . . . , nc , with the calibration surface S0 . Conversely, the proposed solution for the automatic calibration compares each cloud Ci , for all i = 1, . . . , nc , with a surface approximation of all the remaining clouds, Sj , for all j = 1, . . . , nc and j , i.

To compare the points of a cloud Ci with an approximated surface Sj , each point is linked to the closest plane of Sj along its normal vector, thus, defining a point-to-plane metric. Considering the set of planes that define Sj , let the plane associated with the measurement l of Ci be defined by the set of points p ∈ R3 that verify the equation of the plane pT nijl + dijl = 0, where h iT nijl = aijl bijl cijl ∈ R3 is the plane normal unit vector and dijl ∈ R is the distance to the origin. Thus, the point-to-plane distance metric, depicted in Figure 5.2, is represented by the error between each point E pil and the associated plane of the surface. This distance is defined as eijl = E p¯ ijl − E pil , where E p¯ ijl = E pil − nijl Dijl denotes the point in the associated plane closest to E pil (naturally along the normal to the plane, nijl ), and the distance along the normal vector is defined as Dijl = E pTil nijl + dijl . 99


5.2.3

Calibration Problem

Using the reconstruction model (5.1), it is a matter of algebraic manipulation to show that the measurement error introduced above can be written as eijl (R) = Hijl R pil + cijl , where Hijl = −nijl nTijl EB Ril , cijl = Hijl b − nijl nTijl E pBi − dijl nijl , l

and R ∈ SO(3) is the calibration parameter matrix to be estimated, whereas Hijl ∈ M(3, R), pil ∈ R3 , and cijl ∈ R3 are known parameters. Note that this error, which will be used to define the optimization problem, is fundamentally different from that of the well-known Procrustes problem, see [Ume91, GD04] and references therein, as the matrix Hijl depends on the specific measurement l and the association between clouds i and j. Assuming that the probability distribution of the measurement error is Gaussian and defined by p(eijl ) = N 0, σij2l I3 , the calibration problems can be formulated within the scope of the maximum likelihood estimation theory as that of maximizing the reconstruction error probability function, or equivalently, minimizing the log-likelihood function, resulting in the optimization problem R∗ = arg min f (R) . R∈SO(3)

(5.2)

The comparative calibration problem can be formulated considering the errors between each measured point clouds, Ci , and the known calibration surface approximation, S0 , yielding the log-likelihood function fcomp (R) =

npi nc X X i=1 l=1

2

1

ei0l (R)

. 2 2 nc npi σi0l

(5.3)

On the other hand, the automatic calibration problem, where no known calibration surface is available, considers all the errors between each measured set of reconstructed points, Ci , for all i = 1, . . . , nc , and the surface approximation of the remaining clouds, Sj , for all j = 1, . . . , nc and j , i, yielding the log-likelihood function fauto (R) =

npi nc X nc X X i=1 j=1 j,i

2 1

eijl (R)

. 2 2 nc (nc − 1)npi σijl l=1

(5.4)

This latter calibration problem is called automatic calibration due to the fact that a known absolute calibration surface is not necessary to obtain an estimate of the installation parameters. Both these cost functions can be further simplified, by mapping the successive summations into only one, and a new unified cost function can be defined by considering the errors for each 100

5.3 Optimization Using Euler Angles Parameterization combination of cloud-to-surface comparisons, yielding f (R) =

n X i=1

1 kei (R)k2 , 2 n σi2

(5.5)

where ei (R) := Hi R pi + ci and σi are obtained from the previously defined variables ekjl (R) and σkjl , for all k = 1, . . . , nc , j = 0, . . . , nc , j , k, and l = 1, . . . , npi , considering the total number of point-to-plane comparisons, n, and using a simple index map to obtain the global index, i = 1, . . . , n. The uncertainty parameters σi can be used to model different error behaviors, for instance, increasing its value according to the range distance of each point or accounting for the differences in accuracy of several laser sensors. It is fairly simple to obtain the complexity of computing these cost functions, (5.3) and (5.4), regarding the number of clouds, nc , the maximum number of points per cloud, np := max(np1 , . . . , npnc ), and also the maximum number of planes per approximation surface, ns := max(ns0 , ns1 , . . . , nsnc ). To compute the error between a measured point of cloud i and the surface approximation of cloud j, eijl (R), it is necessary to find the corresponding plane of the approximation surface Sj , which has at most ns planes. Thus, the complexity of computing the comparative calibration cost function can be shown to be O(nc np ns ), whereas the complexity of the automatic calibration cost function is given by O(n2c np ns ).

5.3

Optimization Using Euler Angles Parameterization

In this section the optimization problem is reformulated to estimate an Euler angles vector, λ , instead of the rotation matrix R. Let λ ∈ R3 be the ZYX Euler angles parameterization of the λ), then, the optimization problem (5.2) can be written as rotation matrix, i.e. R = R(λ λ) λ ∗ = arg min f (λ λ

with f : R3 → R given by λ) = f (λ

n X i=1

1 λ) pi + ci k2 . kHi R(λ 2 2 n σi

As described in Algorithm 3, the optimal calibration parameter vector, denoted by λ ∗ , is computed using the Gradient and Newton methods, that at each iteration k provide a descent direction, dk ∈ R3 . This descent direction is used to update the next estimate λ k+1 by solving a minimization subproblem, usually called line search, to find the optimal step size, denoted as tk , along the direction dk .

5.3.1

Descent Direction

To compute the descent direction dk , the most widely used method is the gradient method, for λ)|λ k . It can be seen that the which dk is computed using the gradient of the cost function, ∇f (λ 101

Chapter 5: Automatic LADAR Calibration Algorithm 3 Minimization using Euler angles parameterization. Require: Initial estimate λ 0 1: Initialize k = 0 and λ k 2: repeat 3: Compute descent direction dk 4: Compute the step size by solving the minimization subproblem: λ k + t k dk ) tk∗ = arg min f (λ tk ≥0

∗ Compute next

parameter estimate: λ k+1 = λ k + tk dk 6: until ∇f |λ k+1 < 7: return λ ∗ ← λ k+1 as the optimal parameter

5:

descent direction for the gradient method is given by λ)|λ k dk = −∇f (λ where the gradient is computed using λ)|λ = ∇f (λ

h

λ) ∂ f (λ ∂λ1

λ) ∂ f (λ ∂λ2

λ) ∂ f (λ ∂λ3

iT

and n

λ) X 1 T λ) ∂ f (λ ∂ R(λ = ei Hi pi . 2 ∂λj ∂λj n σi i=1

λ)|λ k , yielding faster convergence The Newton method uses the Hessian matrix, denoted by ∇2 f (λ near the optimum value. The descent direction for the Newton method is defined by −1 λ)|λ k λ)|λ k , dk = − ∇2 f (λ ∇f (λ where the Hessian matrix is given by ( 2 ) λ) ∂2 f (λ ∂ f λ)|λ = ∇ f (λ = ∂λj ∂λl λ ∂λ λT ∂λ 2

and " # n X T λ) λ) λ) ∂2 f ∂2 R(λ ∂ R(λ 1 T T ∂ R (λ T = e Hi p + pi Hi Hi pi . ∂λj ∂λl ∂λj ∂λl i ∂λj ∂λl n σi2 i i=1

5.3.2

Line Search

The step size optimization subproblem in Algorithm 3 is numerically solved using the Wolfe λk + tk dk ) and conditions [NW99]. Consider the function φ : R → R defined by φ(tk ) = f (λ derivative given by φ0 (tk ) = ∇T f |λ k +tk dk dk and let also µ(tk ) = φ(0) + σ φ0 (0) tk and µ0 = λ φ0 (0), 102

5.4 Optimization on Riemannian Manifolds

Á(t) t

L

A

R

Figure 5.3: Wolfe rule.

where σ and λ are parameters of the algorithm. The Wolfe rule classifies the step size according to the sets A = tk > 0 : φ(tk ) ≤ µ(tk ) ∧ φ0 (tk ) ≥ µ0 R = tk > 0 : φ(tk ) > µ(tk ) L = tk > 0 : φ(tk ) ≤ µ(tk ) ∧ φ0 (tk ) < µ0 that respectively define the acceptable, the right unacceptable and the left unacceptable step sizes as depicted in Figure 5.3. As detailed in Algorithm 2 from Chapter 3, the line search algorithm consists of finding an acceptable step size that estimates the optimal step size.

5.4

Optimization on Riemannian Manifolds

This section introduces the optimization methodology on Riemannian manifolds. Since the rotation matrix R is an element of the group of special orthogonal matrices SO(3), which is an embedded submanifold of M(3, R), the optimization tools adopted in this section are based on simple exercises of Riemannian geometry theory and allow for the minimization of the loglikelihood function directly on the manifold of special orthogonal matrices SO(3). The concepts of intrinsic gradient and Hessian derived for SO(3) produce descent search directions in the manifold and the cost function is minimized along geodesics in SO(3) to obtain an estimate of the optimal step size. A comprehensive introduction to the subject and applications with orthogonality constraints can be found in [EAS98, AMS07]. Considering the unified log-likelihood function f : SO(3) → R given in (5.5), the optimization problem reduces to the one already defined in (5.2) and the optimal value R∗ can be computed using the gradient or the Newton methods generalized to manifolds. As described in Algorithm 4, given the current parameter estimate Rk , at iteration k, these methods compute a descent direction in the intrinsic tangent space, dk ∈ T Rk SO(3), allowing to obtain the new estimate Rk+1 by solving a minimization subproblem along the geodesic of the manifold, also 103

Chapter 5: Automatic LADAR Calibration denoted as the line search. A geodesic along the manifold is defined as γ dk (t) ∈ SO(3), with initial conditions γ dk (0) = Rk and γ˙ dk (0) = dk . The accuracy of the solution is determined by the constant and the norm is determined using a metric in the parameter space SO(3). Algorithm 4 Minimization on SO(3). Require: Initial estimate R0 1: Initialize k = 0 and Rk 2: repeat 3: Compute descent direction dk 4: Compute the step size by solving the minimization subproblem: tk∗ = arg min f γ dk (t) t≥0

∗ Compute next

parameter estimate: Rk+1 = γ dk (tk ) 6: until ∇f | Rk+1 < 7: return R∗ ← Rk+1 as the optimal parameter

5:

The definition of the canonical metric for a given rotation matrix R ∈ SO(3) is inherited from the canonical metric in the Euclidean space M(3, R). While the tangent space of M(3, R) is identified with T R M(3, R) ' M(3, R) and represented by the usual gradient, the tangent space of SO(3) at point R is identified by T R SO(3) ' RK(3) = {R K : K ∈ K(3)}, where K(n) is the set of n × n skew-symmetric matrices with real entries. To define the canonical metric in SO(3), let two tangent vectors {δδ 1 ,δδ 2 } ∈ T R SO(3), which are identified with δ 1 ' R K1 and δ 2 ' R K2 , with {K1 , K2 } ∈ K(3), then hδδ 1 ,δδ 2 i = tr δ T1 δ 2 , where h., .i denotes the internal product and tr (.) stands for the trace of a matrix.

5.4.1

Descent Direction

To improve the accuracy of the descent direction estimate, this section provides generalizations for manifolds of the gradient and Newton methods. The derivation of the gradient and the Hessian of the log-likelihood function are described below specifically for the SO(3) manifold. 5.4.1.1

Gradient Method

The log-likelihood function (5.5) can be generalized to M(3, R) by defining the smooth function fˆ : M(3, R) → R such that fˆ|SO(3) = f . The tangent space on M(3, R) is characterized as the direct sum of two tangent spaces complementary to SO(3), that is ⊥ T R M(3, R) = T R SO(3) ⊕ T R SO(3) , where the operator ⊕ stands for the direct sum of two sets and (.)⊥ is the orthogonal complement of its argument. The smooth vector field defined by the extrinsic gradient gradfˆ| R ∈ T R M(3, R) 104


-rf^|R -rf |R SO(3)

Figure 5.4: Projection of the extrinsic gradient.

is decomposed as the sum of its tangent and orthogonal components > ⊥ gradfˆ| R = gradfˆ| R + gradfˆ| R and is identified with the usual gradient in M(3, R), that is " # ∂f ∂f ˆ ˆ gradf | R ' ∇f | R := , := ∂R ∂rij i,j∈{1,2,3} which, for the proposed calibration cost function, is given by ∇fˆ| R =

n X 1 HT (Hi R pi + ci ) pTi . 2 i n σi i=1

Hence, the intrinsic gradient ∇f | R ∈ RK(3) is obtained by the projection of the extrinsic gradient on the tangent space T R SO(3), i.e. ∇f | R = (∇fˆ| R )> , as illustrated in Figure 5.4. The projection of the extrinsic gradient results from an optimization problem with closed-form solution given by

2 ∇f | R = R arg min

∇fˆ| R − R K

K∈K

2 = R arg min

skew RT ∇fˆ| R − K

K∈K(3) = R skew RT ∇fˆ| R , where skew (A) = 12 (A − AT ) is the skew-symmetric component of a square matrix A. Thus, at each iteration k, the intrinsic gradient direction used in the optimization algorithm is dk = −∇f | Rk = −Rk skew RTk ∇fˆ| Rk . 5.4.1.2

Newton Method

The Newton method uses the second-order properties of the log-likelihood function to compute the descent direction, resulting in increased convergence rates near the optimal value when 105

Chapter 5: Automatic LADAR Calibration compared with those of the gradient method. Nonetheless, it should be noted that this algorithm is harder to implement, requires more memory, and may yield non-descent directions, in which case the implementation of the algorithm must use the gradient direction instead. ˆ Y} ˆ ∈ Given two tangent vectors {X, Y} ∈ T R SO(3) and the correspondent extension {X, T R M(3, R), the intrinsic Hessian is given by compensating the extrinsic Hessian using ˆ Y ˆ + Π R (X, Y) fˆ , Hessf (X, Y) = Hessfˆ X, where the second fundamental form Π R : T R SO(3) × T R SO(3) → (T R SO(3))⊥ is a differentiable local vector field on M(3, R) normal to SO(3). The extrinsic Hessian is identified by the usual second-order derivative in Euclidean spaces ˆ Y ˆ = vec (X)T ∇2 fˆ| R vec (Y) , Hessfˆ X, ∇2 fˆ| R =

∂2 fˆ ∂vec (R) ∂vec (R)T

,

ˆ ' X ∈ M(3, R), Y ˆ ' Y ∈ M(3, R), the vec (.) operator is the vectorization of a matrix, and where X it is a matter of algebraic manipulation to see that the extrinsic second-order derivative for the calibration cost function is given by ∇2 fˆ| R =

n X 1 p pT ⊗ HTi Hi , 2 i i n σ i i=1

where ⊗ denotes the Kronecker product operator. Regarding the application of the second fundamental form to fˆ, it can be seen that Π R (X, Y) fˆ = −hR symm(XT Y), ∇fˆ| R i , where symm (A) = 12 (A + AT ) is the symmetric component of a square matrix A. The Newton method search direction at iteration k is the unique tangent vector dk ∈ T R SO(3) that satisfies Hessf (X, dk ) = −hX, dk i for all X ∈ T R SO(3). Let ET R SO(3) = {E1 , E2 , . . . , Em } be an orthonormal basis for T R SO(3), then the Newton direction coordinates zik in the basis ET R SO(3) , P such that dk = m i=1 zik Ei , are computed by solving the linear system Ahess zk = bhess , h where zk = z1k

· · · zmk

iT

is the unknown parameter vector, h iT bhess = − hE1 , ∇f | Rk i · · · hEm , ∇f | Rk i , and    Hessf (E1 , E1 ) · · · Hessf (E1 , Em )    .. .. ..  . Ahess =  . . .    Hessf (Em , E1 ) · · · Hessf (Em , Em )

1 ω i ), for all i = 1, 2, 3, where The orthonormal basis used in this work is given by Ei = kω ω i k R S(ω h iT h iT h iT ω1 = 1 0 0 , ω 2 = 0 1 0 , and ω3 = 0 0 1 .

The skew-symmetric matrix S(a1 ) ∈ K(3) stands for the cross product operator, such that S(a1 ) a2 = a1 × a2 , for a1 , a2 ∈ R3 . 106


5.4.2

Iterative Line Search

A geodesic is defined as the curve in the manifold with zero acceleration, and is fully characterized by its initial position and velocity conditions, respectively, γ dk (0) and γ 0d (0). In the k

particular case of SO(3), the geodesic γ dk : I → SO(3) is defined as T

γ dk (t) = Rk e Rk dk t , where I is an interval in R, dk ∈ Rk K(3) identifies the tangent vector, and the initial conditions are given by γ dk (0) = Rk and γ 0d (0) = dk . Differentiation of the geodesic function, relative to the k

step variable, t, yields d γ dk (t) T = dk e R k d k t . dt The step size optimization subproblem of Algorithm 4 is numerically solved using the Wolfe γ 0dk (t) :=

conditions [NW99], generalized to line search on geodesics. To this end, consider the line search cost function along geodesics φ : R → R defined as φ(t) = f γ dk (t) with derivative given by φ0 (t) = ∇T f |γ d

k

0 (t) γ dk (t) .

The remaining of the algorithm is in every aspect identical to that of the line search using the Wolfe rules described in Section 5.3.2.

5.4.3

Closed-form Line Search

Taking advantage of the periodicity of the line search cost function along geodesics, a closedform optimal solution for line search problem can be found by simply determining the roots of a fourth order polynomial, as detailed in [PKK00] for a similar cost function. It is noted, however, that if the position installation bias is considered for calibration the direct application of this closed-form line search would not be possible, as the objective function whould have a nonperiodic component. Given a search direction dk , computed using either the gradient or the Newton methods for SO(3), the line search optimization subproblem is solved by minimizing φ(t) = f γ dk (t) . It is a matter of algebraic manipulation to show that the line search cost function can be written as n X 1 Ωt T T −Ω Ωt T Ωt φ(t) = tr M e R N R e M + 2 W R e M + C , i k i k i i i i k 2 n σi2 i=1

(5.6)

1 ω ) with S(ω ω ) = RTk dk . As Ω has ω k S(ω kω + Ω sin t + Ω 2 (1 − cos t), can be readily applied to

where Mi = pi , Ni = HTi Hi , Wi = cTi Hi , Ci = cTi ci and Ω = unit length, the Rodrigues’ formula, eΩ t = I

(5.6). Substituting this formula and simplifying, it can be seen that φ(t) = k1 + k2 sin t + k3 cos t + k4 sin 2 t + k5 cos 2 t ,

(5.7)

where kj , j = 1, . . . , 5 are constant scalars defined in Appendix C. The optimal value for the step size can be found by considering the first-order condition of optimality, φ(t)0 = 0, which yields k2 cos t − k3 sin t + 2 k4 cos 2 t − 2 k5 sin 2 t = 0 , 107


4

x 10

Z [m]

3 2 1

16

16 14

14 12

12 4

x 10

10

10

4

x 10

8

8 6

6 4

4 Y [m]

2

2

X [m]

Figure 5.5: Comparative calibration example: calibration surface S0 , trajectory (magenta), calibrated (green) and uncalibrated (yellow) laser beams

reducing the optimization subproblem to that of finding the values of t ∈ [0, 2 π) for which the previous condition is satisfied. The first step is to make a trigonometric half-angle substitution, x = tan 2t , reducing the first-order condition of optimality to a fourth order polynomial in x, b4 x4 + b3 x3 + b2 x2 + b1 x + b0 = 0 , where bi , for all i = 0, . . . , 4 are constant scalars, also defined in Appendix C. After finding the roots of this quartic polynomial, which is a standard procedure described in [AS72], the optimal value of t is the real root for which the cost function is minimal. As this closed-form step size computation algorithm does not guarantee, by itself, a decrease in the cost function, the Wolfe rule described in previous section is used whenever a higher cost is detected. This behavior may occur near the optimum, as a consequence of having noise in the acquired data.

5.5

Simulation Results

To assess the performance of the proposed algorithms, a digital elevation map is used to define the simulated terrain actual surface, S, which is approximated by a set of planes to define the known calibration surface, S0 . This can be seen in the example of the comparative calibration shown in Figure 5.5, where the platform trajectory, the calibrated points, and the uncalibrated points using the initial condition, are overlaid on the known calibration surface. An automatic 108

5.5 Simulation Results calibration example is also shown in Figure 5.6, featuring the two different trajectories and the approximated surfaces, S1 and S2 , which were obtained from two clouds of simulated laser data, C1 and C2 . Naturally, it can be seen that the uncalibrated surfaces are very distant from each other, Figure 5.6(a), whereas the calibrated surfaces are virtually coincident, Figure 5.6(b). This section compares the accuracy and performance of the different optimization approaches and calibration algorithms introduced before, resorting to extensive Monte Carlo runs. To this end, the distance to the optimal value is defined in SO(3) and the solution of the Euler λ∗ ) ∈ SO(3). Let Rtrue , R∗ ∈ SO(3) be, reangle optimization approach λ ∗ is evaluated as R∗ = R(λ spectively, the true rotation matrix and the optimal rotation matrix obtained using the proposed ˜ = RTtrue R∗ ∈ SO(3) be the error rotation matrix. The performance of algorithms, and let also R ˜ to I as each method can be evaluated by defining the distance from R

˜

R

= arccos SO(3)

1 2 (tr

RTtrue R∗ − 1) .

The initial conditions for all the Monte Carlo runs presented in this section were computed using a uniform distribution between ±30 deg for each component of the ZYX Euler angle λ0 ). The true value for all the trials is defined vector λ 0 yielding the rotation matrix R0 = R(λ λtrue ), obtained from the ZYX Euler angles vector λ true = by the rotation matrix, Rtrue = R(λ h iT 5.73 2.86 −2.29 deg. The stopping criterion used was

∇f | Rk

< 10−6

∇f | R0

and all the simulations were performed using the Matlab® simulation environment on an Intel® Xeon™E5645 processor at 2.4 GHz with Ubuntu operative system. Although both gradient and Newton search directions were introduced for each optimization approach, the best convergence rates were obtained using the Newton direction, and for that reason, the results presented in this chapter always use the Newton search direction.

5.5.1

Comparative Calibration

Considering the comparative calibration algorithm, three different configurations of optimization methods and line search algorithms are tested: NLW: nonlinear optimization using Euler angles parameterization and Wolfe rule; SOW: optimization on SO(3) using Wolfe rule; and SOC: optimization on SO(3) using closed-form step size. These configurations will be compared in the first part of this section in order to highlight the advantages and disadvantages of each of them. Firstly, these three methods are tested using a computer generated noise-free data set, for which the objective function is smooth and well 109


i

i

Iteration 1: Si, pk and pins

k

4

x 10 3.5 3

Z [m]

2.5 2 1.5 1 0.5 1.5

13 1.4

12 1.3

11 1.2

5

x 10

10 1.1

4

x 10

9 1

8 0.9

Y [m]

7 0.8

X [m]

(a) Before Calibration Iteration 13: Si, pik and piins

k

4

x 10 3.5

Z [m]

3 2.5 2 1.5 1.4

1.5

1.3

1.4 1.2

1.3 5

x 10

1.1

1.2 1.1 0.9

1 0.8

0.9 Y [m]

5

x 10

1

0.7

X [m]

(b) After Calibration: S1 , S2 , trajectories and laser beams

Figure 5.6: Automatic calibration example: approximated surfaces S1 and S2 , trajectories, and laser beams

110

5.5 Simulation Results Table 5.1: Summary of simulation results for the comparative calibration.

Average CPU Time [s] Average Number of Iterations Failure [%] RMS error [deg] Maximum error [deg]

NLW 49.6 95.8 6.8 7.4 5.8 5.5 < 10−6 1.82 < 10−5 4.48

Method SOW SOC 53.2 40.4 99.5 46.4 6.9 5.9 7.8 6.8 2.0 0.0 1.3 0.0 −6 < 10 < 10−6 1.11 0.93 −5 < 10 < 10−5 1.56 1.33

Noise No Yes No Yes No Yes No Yes No Yes

behaved near the optimal value, as the acquired points can be made to be coincident to the calibration surface S0 . In addition, the methods are tested with another data set, for which the acquired range data is corrupted with additive white Gaussian noise with zero mean and standard deviation of approximately 10% of the maximum height variation of the simulation terrain. The objective of these simulation scenarios is to see the influence of the imperfections when the actual surface S is approximated by the control surface S0 and also when the acquired points cannot be made coincident with the calibration surface. The experience consisted of 1000 Monte Carlo runs with random initial conditions, and for each initial condition all methods were tested with and without noise addition. Considering the objective function complexity analysis detailed in Section 5.2.3, there are three main parameters involved: the number of point clouds, nc ; the maximum number of points per cloud, np ; and the maximum number of planes used to approximate each cloud by a planewise surface, ns . For the comparative calibration under analysis, there is only one point cloud, nc = 1, with np = 800 points, and the surface approximation procedure considers a maximum of ns = 240 planes. Table 5.1 presents several statistical indicators of the simulation results for the comparative calibration algorithm: the average computation time, the number of iterations, the percentage of failure, and, most importantly, the RMS and maximum errors, which are computed from the distance errors in SO(3) between the obtained solutions and the true rotation matrix. The computational efficiency of the methods can be observed through the average CPU time and number of iterations. While NLW and SOW have similar values for these indicators, it is evident that the Riemannian method with closed-form step size (SOC) shows a significant decrease in both indicators, particularly in the CPU time, either with or without noisy data. When the data is corrupted by noise, there is also an increase of nearly 30% on the average CPU time of all the methods relative to the case where no noise is considered, which can be attributed, for instance, to the increased difficulty of the line search procedure. 111

Chapter 5: Automatic LADAR Calibration The use of the percentage of failure is justified by the fact that the problem at hand is highly nonlinear and nonconvex, as the control surface has discontinuities and even the actual surface may introduce local minima. It can be noticed that, while the first two methods (NLW and SOW) have failure rates of less than 6%, the method that resorts to the closed-form step size (SOC) seems to be less sensitive to these hazards for the presented Monte Carlo simulations. The performance of the comparative calibration algorithm and the three optimization methods can be inferred using the RMS and maximum errors for the case where the data is corrupted by noise. Regarding the simulations without noise, the maximum error is always less than 3.0 × 10−6 deg. The main trend to consider in these indicators is that the optimization in SO(3) yields smaller errors than the optimization using Euler angles (NLW), showing a decrease of, at least, 39% in the RMS error and 65% in the maximum error. Besides having smaller average CPU time, the SOC method also proves to be the most accurate method, with a RMS error of 0.93 deg and a maximum error of 1.33 deg. Therefore, it is fair to say that the Riemannian optimization using a closed-form step size is more efficient and has the best performance, consistently presenting the best qualities in the analyzed aspects: computation time, number of iterations, less prone to failures and most accurate estimates in the presence of noise.

5.5.2

Automatic Calibration

The aim of the simulation results presented in this section is to validate and analyze the properties of the automatic calibration algorithm, as well as to demonstrate that there is a relation between the number of planes considered for the surface approximation and the accuracy of the solution that can be achieved by the optimization method. Noting that the previous results show evidence that the optimization on SO(3) has a higher accuracy than the optimization using Euler angles, the presented results only consider the following two configurations for the automatic calibration: AutoSOW: automatic calibration on SO(3) with Wolfe rule; and AutoSOC: automatic calibration on SO(3) with closed-form step size. The results consist of Monte Carlo runs with random and uniformly distributed initial conditions, as detailed for the comparative calibration results. Without loss of generality, only two clouds of points are considered, nc = 2, and the surface approximation is based on a xy-plane uniform sampling grid for each cloud of laser points, finding the best z-coordinate value based on the surrounding laser points. A set of planes is then generated to be used by the calibration method in each iteration as a surface approximation of that point cloud. To establish the relation between the maximum number of planes and the solution accuracy, several surface approximation scenarios are considered, ranging from ns = 50 to ns = 2450 planes. Figures 5.9, 5.8, and 5.7 present the main statistical indicators of the simulation 112

5.5 Simulation Results 8 1.8 7

AutoSOW AutoSOC max AutoSOW max AutoSOC

1.6 1.4

6

1.2 1

rms/max [deg]

5 0.8 0.6

4

0.4 0.2

3

100

150

200

250

300

350

400

450

500

550

2

1

0

0

500

1000

1500

2000

2500

ns

Figure 5.7: Automatic calibration analysis: RMS and maximum errors

described above: the average computation time, the percentage of failure, and also the RMS and maximum errors. Each point in these plots represents the results of 150 or 100 Monte Carlo runs, respectively, for ns ≤ 800 or for ns > 800. The first striking evidence provided by these results is that AutoSOW and AutoSOC have similar performance if the maximum number of planes ns is greater than 500. It is also visible that the AutoSOC method consistently yields better accuracy in terms of RMS and maximum errors than the AutoSOW method, which seems to perform marginally better only when ns = 2450. Regarding the percentage of failure, the AutoSOC method shows a much greater immunity to local minima than the AutoSOW method, when ns is smaller than 200 planes, which achieves a value lower that 10% even for ns as low as 70 planes. Conversely, when ns is greater than 200 planes the AutoSOW methods is consistently less prone to local minima. The main conclusion emerging from these simulation results is that the most appropriate choice of the parameter ns might not be the highest possible in terms of computational power, as the most accurate solutions are obtained for ns ∈ [200, 300], where both methods were able to present maximum errors below 0.5 deg and RMS errors below 0.2 deg. Furthermore, for any value of ns above 125, both methods show RMS errors below 1 deg while having automatic calibration CPU times as low as 8 minutes. These results indicate that the proposed automatic 113

Chapter 5: Automatic LADAR Calibration 40 AutoSOW AutoSOC

15 35

14 13 12

30 11

failure [%]

10 9

25

8 7 20

6 50

100

150

200

250

300

350

400

450

15

10

5

0

500

1000

1500

2000

2500

ns

Figure 5.8: Automatic calibration analysis: percentage of failure 120

average CPU time [min]

100

80

60

40

20 AutoSOW AutoSOC 0

0

500

1000

1500

2000

ns

Figure 5.9: Automatic calibration analysis: average CPU time

114

2500

5.6 Experimental Results

Figure 5.10: Unmanned helicopter used for airborne LADAR data acquisition for the calibration experiment.

calibration methods can provide accurate estimates of the attitude installation matrix of 2-D LADARs. The evaluation of the performance differences between the comparative and the automatic calibration approaches is somehow unfair and difficult, as the former considers a known calibration surface in order to compare the acquired data. It is also impossible for the latter approach to have a completely error free simulation, as each measured cloud of points is approximated by a set of planes and will always have approximation errors. Nonetheless, the accuracy of the comparative calibration, when noise is considered, can be confronted with the accuracy of the automatic calibration methods. Thus, it is noted that the accuracy of the latter methods, when ns is above 125, is always below that of the former methods, for which the RMS error values are 1.114 deg and 0.935 deg for the SOW and SOC methods, respectively. Regarding the much higher computational times, there are two factors that explain this difference: (i) the increased number of point-to-plane comparisons and (ii) the higher surface approximation accuracy.

5.6


This section presents the validation of the automatic calibration algorithms using experimental data acquired using a custom unmanned helicopter shown in Figure 5.10, which is property of IST/ISR. The worst case scenario for the proposed calibration algorithm is a flat terrain, as it 115


Figure 5.11: Airborne image of terrain acquired for calibration, from which two overlapping patches with approximate dimensions of 90 × 30 m were chosen for the calibration experiment.

would be impossible to solve the calibration problem independently of the vehicle trajectories. Conversely, the calibration will achieve better performance if the environment is rougher and contains non-symmetrical 3-D features, as long as an appropriate surface approximation algorithm is considered. The terrain acquired for calibration is shown in Figure 5.11, where the small slope variations of the terrain as well as the presence of vegetation and a wall can be noticed, thus, increasing the difficulty of the calibration procedure. The vehicle describes two different and overlapping trajectories, as shown in Figures 5.13(c) and 5.14(b), and is equipped with a Memsense nanoIMU, an Ashtech GPS receiver, and a Hokuyo UTM-30LX laser scanner. In addition to the available continuously operating reference station (CORS), that can be used in post-processing for higher GPS data accuracy, an additional reference station was setup in the field where the LADAR data was acquired. The measurements provided by the GPS and IMU sensors can be used to obtain the pose of the vehicle at each time instant, resorting to well-known attitude and positioning filters, such as [BSO11a] and [BSO11c], which, together with the laser measurements, can be used to obtain two clouds of points and test the calibration algorithms. While the performance of the considered attitude filter, in similar conditions to those used in this experimental results, is reported to be 0.125 degrees in terms of mean angle error, the post-processed GPS data has been reported to have a standard deviation below 2 centimeters 116


when the base station is not more than 5 km away from the vehicle, which was assured during the data acquisition. The available specifications for the Hokuyo laser sensor used in these experiments indicate that the range measurement standard deviation is 1 cm from 0.1 to 10 m and 3 cm from 10 to 30 m, it has 0.25 deg of angular resolution, and a laser footprint of approximately 20 cm at a distance of 20 meters. The experimental results presented in this section are focused on the automatic calibration algorithms, AutoSOW and AutoSOC, as it is extremely difficult to have a known calibration surface, necessary for the comparative calibration algorithms (NLW, SOW, and SOC). As it is equally demanding to obtain accurate enough ground truth attitude installation matrix, these results do not intend to show that the optimization solutions are close to some true angular installation bias. Instead, a statistically consistent measurement of the performance can be obtained through extensive Monte Carlo runs for random initial conditions, showing that the solutions provided by the proposed methods have a low standard deviation. Exploiting this experimental analysis direction, the presented experimental results consider nc = 2 point clouds and several scenarios for the remaining parameters of the computational complexity, np and ns . In particular, a subsampling procedure is applied to the two acquired point clouds, which have a total of 42834 and 53899 points, respectively, so that the maximum number of points per cloud is limited to a value on the interval np ∈ [1000, 4000] points. This experimental results use the same computational capabilities as in the simulation results presented in the previous section, and the initial conditions were computed using a uniform distribution between ±30 deg for each component of the ZYX Euler angles. For instance, the h iT average solution is found to be λavg = 1.65 −2.64 −2.66 deg, for the AutoSOC algorithm with 4000 points per cloud and ns = 800, which was computed from the average rotation matrix in SO(3) obtained with the method described in [SWR10]. The statistical indicators of performance for the two automatic calibration algorithms, considering several scenarios of accuracy, are presented in Figure 5.12 and, particularly for the case when np = 4000, in Table 5.2. One key conclusions drawn from the analysis of these experimental results is the fact that the standard deviation of the calibration solutions drops as the values of np and ns are increased, either using SOW or SOC algorithms, achieving less than 0.75 deg for np = 4000 and ns = 2450. In addition, the AutoSOC algorithm, which uses the closed-form step size, achieves higher accuracy in terms of standard deviation than the one using only the Wolfe rule (AutoSOW), echoing the simulation results presented before. The down size of AutoSOC in these experimental results seems to be a consistently larger average number of iterations, which directly implies a rise in the computational time. It is also interesting to note that the increase of np = 3000 to np = 4000, when ns = 800, results in a mild decrease on the computational time of both algorithms, which might indicate 117


2.5

std [deg]

2

1.5

1

0.5

4000 3500 3000

0 2500 500

2000

1000 1500

1500 2000 2500

1000

np

n

s

(a) Standard deviation.

12 10

time [h]

8 6 1000 4

1500 2000

2

2500

0

3000

2500 2000

3500

1500 1000 500

4000

np

ns

(b) Average CPU time.

Figure 5.12: Automatic calibration experimental data analysis. The AutoSOW algorithm is represented using dark surface edges and the AutoSOC is represented with interpolated colors for the surface edges. The actual points used are represented with blue circles and green squares, respectively, for the AutoSOW and for the AutoSOC algorithms.

118

5.7 Concluding Remarks Table 5.2: Summary of experimental results for the automatic calibration with np = 4000.

ns Avg. CPU Time [hour] Avg. Number of Iterations Percentage of Failure [%] Solution standard deviation [deg] Solution maximum deviation [deg] Number of runs

200 1.7 2.0 11.0 15.0 5.3 4.7 1.79 1.68 16.33 16.31 150

800 4.1 4.4 11.2 12.6 6.0 3.0 1.35 0.94 2.30 1.92 100

2450 11.8 12.6 11.7 13.2 8.0 6.0 0.75 0.75 2.63 1.22 50

Method AutoSOW AutoSOC AutoSOW AutoSOC AutoSOW AutoSOC AutoSOW AutoSOC AutoSOW AutoSOC

that the lower values are not adequate for a good convergence rate of the optimization algorithm. The fact that the maximum deviation of the AutoSOW algorithm increases when changing from ns = 800 to ns = 2450, for np = 4000, is another example of unexpected behavior. The reasoning behind this may be twofold: i) having ns and np too close to each other and ii) the previously supported evidence that the AutoSOW algorithm may be more prone to local minima than the AutoSOC algorithm for certain choices of ns . The reconstructed point clouds and their approximation surfaces used in the algorithms are presented, respectively, in Figures 5.13 and 5.14, before and after the calibration. It can be seen that without calibration there is a considerable error between the two clouds of points, Figure 5.13(a), which is mitigated using the calibrated attitude installation bias found by the proposed algorithms, Figure 5.13(b).

5.7

Concluding Remarks

This chapter addressed the problem of 2-D LADAR calibration on board aerial vehicles by building on a comparative calibration algorithm and proposing a novel automatic calibration algorithm, resorting to numerical optimization methods to estimate the attitude mounting bias rotation matrix R ∈ SO(3). While for the comparative calibration algorithm this is achieved by minimizing the distance between a cloud of acquired LADAR points and a known calibration surface, the automatic calibration algorithm minimizes the distances between several clouds of acquired LADAR points, assuming no a priori knowledge of the terrain. The latter algorithm is one of the major contributions of this chapter, as it suppresses the use of a known calibration surface, representing the true terrain to which all measured points are compared. To solve these estimation problems, two numerical optimization approaches were used: i) nonlinear optimization using the ZYX Euler angles parameterization of the attitude bias installation matrix, λ), and ii) a Riemannian optimization framework for the group of special orthogonal R = R(λ 119


(a) Before Calibration: a detail of the errors between clouds of points.

(b) After Calibration: consistent reconstruction of both clouds of points.

(c) After Calibration: overview.

Figure 5.13: Automatic calibration experimental data: C1 in red, C2 in green, and respective vehicle trajectories.

120

5.7 Concluding Remarks

−8

−6

Z [m]

−4

−2

0

2

90

85

80

75

70

65

60

55

25

20

15

10

5

0

Y [m] X [m]

(a) Before Calibration detailed view.

−8 Z [m]

−6 −4 −2 0 20

2 90

85

80

10 75

70

65

60

55

0 Y [m]

X [m]

(b) After Calibration.

Figure 5.14: Automatic calibration experimental data: plane-wise surface representation of the two point clouds (using a gray scale depending on the height of each surface plane), vehicle trajectories (in magenta) and a subset of the laser beams used in the presented results (in dashed yellow). To better distinguish the two different surfaces, one is represented using dark edges for each plane and the other is represented with interpolated colors for the edges, depending on the face color of each plane.

121

Chapter 5: Automatic LADAR Calibration matrices, SO(3). Both approaches were tested using the Newton direction, the Wolfe rule for the line search subproblem and, for the case of the optimization in SO(3), a closed-form solution for the computation of the step size was also used. The performance and limitations of the comparative and the automatic calibration algorithms, as well as the different numerical optimization approaches, were tested in Matlab® simulation environment and with experimental data, from where it is possible to conclude that all methods are able to find good estimates for the 2-D LADAR calibration problem, even with real life and noisy airborne data. The optimization on SO(3) with the closed-form step size computation displays some immunity to local minima and higher accuracy estimates. Additionally, the accuracy of the estimated parameters obtained using the automatic calibration algorithm is shown to increase with the number of planes used to describe the surface approximation of each cloud of laser measured points, as well as with the maximum number of points per cloud. Future directions of research include the convergence analysis of the algorithm and considering nonuniform surface approximations in order to decrease the computational time, as the same approximation error might be achieved with less planes. Further work is also needed in order to address the computational complexity of the algorithm and to include other sources of reconstruction errors, such as the time delay between laser acquisition and the INS/GPS data, or the range measurement error.

122

6

Sensor-based Simultaneous Localization and Mapping

This chapter presents the design, observability conditions, convergence analysis, and experimental validation of a sensor-based filter for SLAM, with GAS error dynamics. This filter has straightforward application to UAVs in GPS-denied environments, using acceleration and angular rate inertial measurements, a laser scanning device, and an altitude sensor. The SLAM problem is formulated in a sensor-based framework and modified in such a way that the underlying system structure can be regarded as LTV for observability and filter design purposes, from which a linear Kalman filter with GAS error dynamics follows naturally. Furthermore, an online earth-fixed trajectory and map estimation algorithm is developed using a computationally efficient and numerically robust methodology. This algorithm follows from the estimation solution provided by the sensor-based SLAM filter, by formulating an optimization problem with a closed-form solution. The uncertainty description of this solution is also derived resorting to perturbation theory, both for the earth-fixed trajectory and map. The performance and consistency validation of the proposed sensor-based SLAM filter and the earth-fixed trajectory and map estimation method are successfully assessed with real data, acquired indoors using a quadrotor instrumented platform.

6.1

Introduction

Over the past decades the research community has devoted tremendous effort in the field of probabilistic SLAM. The seminal works that established the statistical foundations for describing the relationships between landmarks and their correlations include [SC86], [DW88], and [SSC90]. Further research showed that a full and consistent solution to this problem would require all the vehicle pose and map variables to be considered in a single, and potentially large, state vector. This requirement made the problem intrinsically nonlinear and one of the technical solutions emerging from this challenge was to use the much celebrated extended Kalman filter (EKF) [CDW96, Cso97]. A detailed survey and tutorial on SLAM can be found 123

Chapter 6: Sensor-based SLAM in [DWB06], [BDW06], [TBF05], and references therein, which include a significant number of successful implementations of SLAM algorithms in real world scenarios. The association of measured and state landmarks is one of the major sources of inconsistency in SLAM algorithms and considerable effort has been put into this issue. Several strategies have emerged, namely, the simplistic nearest neighbor (NN) validation gating, joint compatibility branch and bound (JCBB) [NT01], combined constrained data association (CCDA) [Bai02], and other strategies, such as multi-hypothesis data association or particle filters, that imply changes or a different algorithm. A general proof of global convergence for the EKF, to the best of the author’s knowledge, is yet to be found. Nonetheless, there are some notable SLAM convergence results [DNDW+ 01, HD07], which usually assume that the linearized system matrices are evaluated at the ideal values of the state variables. This linearization may lead to inconsistency as it was first pointed out in [JU01] and subsequently acknowledged and discussed in [BNG+ 06], [CNT04], and [HD07]. In order to minimize the inconsistency problems induced by the linearization, some algorithms have been proposed, such as the robocentric map joining algorithm [CMCTN07], or the first-estimates Jacobian EKF [HMR09]. The robocentric map joining approach bears resemblance with the method proposed in this chapter, in the sense that the filtering process takes place in the sensor or vehicle coordinate space. In [BJ09], the authors resort to stochastic stability concepts to provide results concerning the boundedness and convergence of the expected estimation error of an EKF-based polar SLAM for a unicycle model in the presence of noise, when there is access to both linear and angular velocity measurements.

6.1.1

Sensor-based SLAM Filter

The first contribution of this chapter is the design, analysis, and experimental validation of a novel sensor-based SLAM filter, which can be used in structured 3-D environments if the altitude is included in the system dynamics, as part of an integrated SLAM algorithm, that: i) has globally asymptotically stable error dynamics; ii) resorts to the linear and angular motion kinematics, which are exact; iii) can be generalized into full 3-D SLAM, if 3-D landmarks are available; iv) builds on the well-established LTV Kalman filtering theory; and v) explicitly estimates the rate gyro bias, merging low bandwidth landmark observations with high bandwidth rate gyro measurements. The result of this filter is the solution of the SLAM problem in the vehicle frame, where the vehicle pose is deterministic, as it is simply that of the vehicle frame, and the positions of the landmarks rotate and translate according to the vehicle motion, in a similar fashion to what happens in [CMCTN07]. An essential feature of the proposed filter is the modification of the nominal nonlinear system dynamics such that it can be regarded as LTV for observability and convergence analysis purposes, even though it still is intrinsically nonlinear. Conversely, the system dynamics described in [CMCTN07] include the full odometry of the vehicle in the state vector, rendering 124

6.1 Introduction the use of the observability analysis strategy proposed hereafter non-trivial. It must also be stressed that the system dynamics used in the design and analysis of the proposed Kalman filter are exact and no approximations or linearizations are performed. The proposed solution builds on previous sensor-based navigation filters proposed in [BSO10, BSO11a, BSO11c]. Regarding the work presented in [BJ09], the main contribution of this chapter is that the observability results are global and constructive, in the sense that both necessary and sufficient conditions for observability are derived, with clear physical interpretation, and a system structure naturally appears for observer design purposes, yielding globally asymptotically stable error dynamics. Furthermore, the proposed formulation enables the solution of the SLAM problem even in the absence of linear velocity measurements, which are not usually available for aerial platforms. If available, these measurements could be used in the proposed filter to further improve the quality of the obtained estimates.

6.1.2

Online Earth-Fixed Trajectory and Map Estimation

The online estimation of the vehicle trajectory is achieved by comparing the sensor-based map provided by the SLAM filter against the previous estimate of the environment map in the earth-fixed frame. With the obtained translation and rotation between the vehicle and the earth-fixed frames, the earth-fixed position of the landmarks as well as their uncertainty can be computed. The problem of extracting the orthogonal transformation that maps one set of points into a second set of points, with known associations between them, is usually called the orthogonal Procrustes problem and it can be traced back to the work presented in [Sch66]. The generalization for translation, rotation, and scaling can be found in [AHB87] and [Ume91], where each landmark is equally weighted in the optimization problem. The statistical characterization of the Procrustes analysis using perturbation theory, as well as the usage of different weights for each landmark, usually denoted as the extended orthogonal Procrustes problem, can be found in [Sib78, Sib79, Goo91]. Within the field of medical imaging, [FW01] also resorts to perturbation theory to present a statistical characterization of a target position, considering small rotations, isotropic uncertainty, and equal weights for each landmark. More recently, the work presented in [WLFP08] extends these results for anisotropic uncertainty in the components of the landmark space. This is achieved by considering the same covariance matrix for all landmarks, which may weigh each component of the landmark space independently. The online earth-fixed trajectory and map estimation algorithm proposed in this chapter builds on the results presented in [FW01] and [WLFP08] by considering arbitrary rotations, individual weights, and arbitrary covariance matrix for each landmark and between them. This algorithm adds to the contributions of this chapter: vi) a novel formulation with closed-form solution for the problem of obtaining the transformation from the sensor frame to the earthfixed frame, yielding the earth-fixed trajectory of the vehicle and map of the landmarks; vii) the 125

Chapter 6: Sensor-based SLAM

Figure 6.1: Instrumented quadrotor.

uncertainty characterization of the obtained trajectory and map, which resorts to perturbation theory, using individual covariance matrices for each landmark; and viii) the validation in terms of performance and consistency of both the SLAM filter and the earth-fixed trajectory and map using real data, acquired using the quadrotor platform depicted in Figure 6.1, driven along a 60 m path. A preliminary version of this work can be found in [10], and [11], and a more focused paper on the sensor-based filter is published in [13].

6.1.3

Chapter Structure

The chapter is organized as follows. Section 6.2 introduces the problems at hand, including the sensor-based paradigm, the nominal system dynamics and the earth-fixed trajectory and map estimation problem. A constructive observability analysis is carried out in Section 6.3 and Section 6.4 details the design of the sensor-based SLAM filter. In Section 6.5, the optimization-based online earth-fixed trajectory and map estimation algorithm is derived, and the experimental results using an instrumented quadrotor platform are presented and discussed in Section 6.6. Finally, the conclusions and directions for future work are provided in Section 6.7.

6.2

Problem statement

The traditional EKF-based SLAM filtering solution models the estimated features and vehicle pose in an absolute coordinate frame, usually denoted as the earth-fixed reference frame {E}, while the measurements are obtained in body-fixed coordinates {B}, i.e., in the frame rigidly 126

6.2 Problem statement attached to the vehicle. Even in 2-D space, the dynamic equations of this system require a representation of the attitude of the vehicle to transform each measured landmark from the sensor-space into the reference frame using, for instance, rotation matrices, quaternions, or Euler angles. Thus, the system equations are usually linearized to compute the state pseudocovariance matrix, potentially resulting in inconsistency or even failure of the SLAM filter, as pointed out in [JU01] and [CNT04], mainly when the linearization condition is systematically far from the actual attitude of the vehicle. Moreover, the employment of attitude representations necessarily carries pitfalls such as singularities, topological limitations for achieving continuous global stabilization, double covering, slow convergence near unstable equilibrium points, or unwinding phenomena, depending on the parameterization, as detailed in [BB00] and [CSM11]. A remarkable and successful effort to reduce the inconsistency related to these linearizations was proposed in [CMCTN07], with the redefinition of the SLAM dynamics into a vehicle centered representation, which was shown to have better consistency properties both in simulated and real world environments. Nonetheless, this approach still considers the estimation of the incremental vehicle pose (driven by the odometry) by including these variables in the state vector. The earth-fixed reference frame is also included in the mentioned work as an unobservable state, which is integrated in open-loop using odometry estimates of the vehicle motion, and is used afterwards to transform the map and vehicle trajectory back into the reference frame. The concept of the sensor-based SLAM approach that is presented in this chapter is the design of the filter directly in the space of the sensors, taking into account the original noise characteristics of each sensor. At each instant, the vehicle can localize itself trivially, at the origin of the robocentric frame, and have a local representation of the environment map using landmarks. Nonetheless, the filter avoids the use of any representation of the pose of the vehicle both in the measurement equation and in the state equations concerning the representation of the environment. Moreover, as defined in Section 6.2.2 and derived in Section 6.5, the full pose of the vehicle in the earth-fixed frame can also be computed at each sampling instant, by using exclusively the history of the sensor-based map of the environment. It is noted that the proposed technique is in-line with, and complements, the so-called sensor-based control paradigm, as the quantities in the sensor frame, required for control purposes, are readily estimated by the filter presented in the sequel.

6.2.1

Nominal Nonlinear System Dynamics

Recall that {E} denotes the earth-fixed reference frame, {B} the body-fixed frame, and EB R(t) ∈ SO(3) the rotation matrix from {B} to {E}. Let also E pB (t) ∈ R3 denote the position of the origin of {B}, described in {E}, and vB (t) ∈ R3 the velocity of the vehicle relative to {E}, expressed in {B}. The linear and angular motion kinematics of the vehicle are given by (E

p˙ B (t) = EB R(t) vB (t) , E ˙ E ω B (t)] B R(t) = B R(t) S[ω 127

Chapter 6: Sensor-based SLAM ωB) where ω B (t) ∈ R3 is the angular velocity of the vehicle relative to {E}, expressed in {B}, and S(ω ω B )d is the cross product ω B × d, for some d ∈ R3 . is the skew-symmetric matrix such that S(ω Consider also the ZYX-Euler angles which can be used to decompose the rotation matrix EB R(t) as EB R(t) = Rz (ψB (t)) Ry (θB (t)) Rx (φB (t)), where φB (t), θB (t), and ψB (t) denote the roll, pitch, and yaw angles, whereas Rx (.), Ry (.), and Rz (.) denote the rotation matrices about the x, y, and z axes, respectively. An IMU is required for most navigation systems and it usually contains, at least, two triads of orthogonally mounted accelerometers and rate gyros, and in some cases it features an additional triad of orthogonally mounted magnetometers. Assuming that the IMU is aligned with {B}, the rate gyros provide angular velocity measurements, ω Bg (t) ∈ R3 , corrupted with bias and noise, as given by ω Bg (t) = ω B (t) + bω (t) + nω (t) , where bω (t) ∈ R3 denotes the bias of the tri-axial rate gyro, which is slowly time-varying, and nω (t) ∈ R3 corresponds to the rate gyro noise. As the rate gyro measurements will be used in the system dynamics, the estimation of the measurement bias will be of paramount importance for the consistency of the filter. When navigating inside buildings or close to large infrastructures, like bridges or electrical power lines, the measurements provided by the magnetometer are prone to large errors and bias due to the presence of ferromagnetic materials or electromagnetic noise. Nonetheless, using standard filtering techniques (see for instance [BSO11b] and references therein), the roll and pitch angles can be obtained using only rate gyros and accelerometer measurements, enabling the definition of a horizontal body-fixed frame {H}, such that E pH (t) = E pB (t) denotes the position of frame {H} described in frame {E}, E H

R(t) = Rz (ψB (t)) the rotation from frame {H} to frame {E}, and HB R(t) = Ry (θB (t)) Rx (φB (t)) the

rotation from {B} to {H}. Using the rotation matrix HB R(t) and the known pose configuration of the sensor relative to frame {B}, the measurements of a laser scanner mounted horizontally in the vehicle can be projected into {H} and the respective measurement noise covariance matrix can account for the estimation errors of HB R(t). In this way, the laser measurements represent a horizontal profile of the environment, from which the position of the vehicle and a map of the environment in 2-D can be obtained using a SLAM algorithm. The underlying assumption is that the environment is fairly structured along the vertical direction, that is, with limited variation of features for different altitudes, which include indoor environments, outdoor fields with trees, piers, or conventional buildings. It is also assumed that the vehicle is not too close to the lower and upper boundaries of the environment, which inside a building would translate into not being too close to the ground nor too close to the ceiling. As the SLAM algorithm herewith derived provides a 2-D position and map independently of the altitude position, for the sake of clarity and with a slight abuse of notation, it is considered in the remainder of this chapter that all vectors are in 2-D and all rotation matrices belong to SO(2), unless otherwise stated. 128

6.2 Problem statement Consider that each landmark that is detected can be represented by a 2-D position and some auxiliary characteristics depending on the type of feature. For instance, a wall corner can be represented by the position of the edge and two line segments, providing not only a position measurement but also one or two direction readings. Considering the case of just a single direction associated with a landmark, the position and the direction associated with the i-th landmark in {H}, denoted respectively by pi (t) ∈ R2 and di (t) ∈ R2 , satisfy  E T E E    pi (t) = H R (t) ( pi − pH (t)) ,    di (t) = E RT (t) E di H

where E pi ∈ R2 and E di ∈ R2 denote the 2-D position and direction associated with the landmark, described in the reference frame {E} and considered to be static. In the sensor-based framework, their kinematics can be written as  h i   p˙ i (t) = − rg (t) − br (t) S pi (t) − v(t)   h i ,     d˙ i (t) = − rg (t) − br (t) S di (t)

(6.1) "

# 0 −a where S := S(1) and S(a) for some a ∈ R denotes the 2-D skew-symmetric matrix S(a) = , a 0 rg (t) ∈ R is the z-component of rate gyro measurement expressed in {H}, br (t) ∈ R is the corresponding rate gyro bias, and v(t) ∈ R2 denotes the linear velocity of the vehicle relative to {E}, expressed in {H}. These equations describe the linear and angular motion kinematics for each landmark, which are exact. It is also noted that, because the bias term is expressed in {H}, it is not constant. Instead, its derivative depends on the roll, pitch, and respective angular velocities, which can all be obtained using a complementary filter with globally asymptotically stable error dynamics based on the acceleration and rate gyro readings, see [BSO11b] for details. In the remainder of the chapter it is considered, without loss of generality for observer design purposes, b˙ r (t) = 0. In the final design, an additional input is naturally considered to model the exact bias dynamics. In a SLAM state-space formulation, the full state vector, here denoted as xF (t) ∈ RnF , can be decomposed into vehicle specific variables, xV (t) ∈ RnV , and landmark variables, xM (t) ∈ RnM . In the proposed formulation, the vehicle state vector is composed by the horizontal linear velocity, v(t) ∈ R2 , and the angular velocity bias, br (t) ∈ R, all described in {H}, yielding h iT xV (t) = vT (t) br (t) . Notice that this vehicle state vector does not consider any information about the position and attitude of the vehicle relative to the reference frame. Each landmark vector, denoted as mi (t) ∈ Rni , can feature the position of a landmark and up h iT to two directions, e.g., to represent a corner landmark it would be mi (t) = pTi (t) dTai (t) dTbi (t) . The set of landmarks, I , considered in the state vector can be arranged in such a way that the first m landmarks denote the ones that are visible/observed and the next p are the nonvisible/unobserved landmarks, respectively, IO = {1, . . . , m} and IU = {m + 1, . . . , m + p}, yielding h iT a natural decomposition of the landmark state vector such that xM (t) = xTO (t) xTU (t) , where xO (t) = {mi (t)} ∈ RnO , for all i ∈ IO , and xU (t) = {mi (t)} ∈ RnU , for all i ∈ IU . 129

Chapter 6: Sensor-based SLAM With the above introduction and considering that the linear velocity and the rate gyro bias are slowly time-varying, the complete system dynamics in {H} can now be written as   x˙ (t) = 0    V ˙ i (t) = AMVi (mi (t)) xV (t) + AMi (t) mi (t), ∀i∈I , m     y (t) = m (t) , ∀ i

i

(6.2)

i∈IO

where, considering landmarks with only one direction, it is a matter of algebraic manipulation to obtain "

# −I2 S pi (t) AMVi (mi (t)) = 02×2 S di (t) and AMi (t) = −rg (t) S2 , denoting Sn as the block diagonal matrix formed using n times the matrix S, in this case S2 = diag (S, S). For the case where there are two directions associated with the i-th landmark, the above matrices would be defined as AMi (t) = −rg (t) S3 and    −I2 S pi (t)    AMVi (mi (t)) = 02×2 S dai (t) .   02×2 S dbi (t) The nominal system dynamics (6.2) assumes that the vehicle has a constant linear velocity and that the angular velocity measurement bias is also constant. However, by opening the filter tolerance to model uncertainty, using higher disturbance noise covariance values, it is possible to track these slowly time-varying variables.

6.2.2

Problem Statement

The first problem considered in this chapter is that of designing a filter with globally asymptotically stable error dynamics for the nominal nonlinear system (6.2), considering also additive system disturbances and sensor noise. Sections 6.3 and 6.4 are intended to tackle this problem, presenting the observability analysis, convergence properties, and the SLAM filter design. The second problem is to obtain, in real time, a statistically consistent vehicle trajectory and map of the surrounding environment, described in the earth-fixed frame {E}, using exclusively the information provided by the sensor-based SLAM filter. The problem is then to find the position and attitude of the vehicle as well as the landmarks in the earth-fixed frame, respectively given by E pH (t) ∈ R2 , EH R(t) ∈ SO(2), and E mi (t) for each state landmark i. This second problem is addressed in Section 6.5, where an optimization problem is proposed for which a closed-form solution can be found, and the uncertainty of the body to earth-fixed transformation and the earth-fixed landmarks are characterized through their mean and covariance matrices.

6.3

Observability Analysis

The main focus of this section is to obtain the necessary and sufficient conditions for the observability of the proposed formulation of the SLAM problem, with a clear physical interpretation. 130

6.3 Observability Analysis In addition, the analysis is constructive in the sense that a Kalman filter can be readily applied, identifying the nonlinear system with a particular linear time-varying system, yielding globally asymptotically stable error dynamics. The observability analysis considered in this chapter is deterministic, as the dynamic model is considered noise-free for analysis purposes. Nonetheless, the experimental results presented in Section 6.6 provide evidence that the system is, indeed, convergent in the presence of noise. For observability and filter design purposes, the non-visible features are not considered, as they are simply propagated in open-loop and are evidently unobservable. However, the observability analysis presented in this chapter shows that this fact has no influence in the observability and convergence of both the visible landmarks and the variables related to the vehicle dynamics, v and br . Discarding the non-visible landmarks, the reduced state vector h iT h iT is x(t) = xTV (t) xTO (t) ∈ Rnx , recalling that xO (t) = mT1 (t) · · · mTm (t) ∈ RnO denotes the h iT vector of visible landmarks, whereas the output vector is y(t) = yT1 (t) · · · yTm (t) ∈ Rny , h iT with each landmark output given by yi (t) = yTpi (t) yTdi (t) for a position landmark with one associated direction. Thus, it is possible to rewrite the nominal nonlinear system (6.2) as ( ˙ = A(t, xO (t)) x(t) x(t) , (6.3) y(t) = C x(t) where "

# 0nV ×nV 0nV ×nO ∈ Rnx ×nx , A(t, xO (t)) := AMV (xO (t)) AM (t) AM (t) := diag AM1 (t), . . . , AMm (t) ∈ RnO ×nO , h iT AMV (xO (t)) := ATMV1 (m1 (t)) . . . ATMVm (mm (t)) , h and C := 0nO ×nV

i InO ∈ RnO ×nx . Consider the previous nonlinear system rewritten as (

˙ = A(t, y(t)) x(t) x(t) . y(t) = C x(t)

(6.4)

Assuming that the output signal y(t) is known for all t ≥ t0 , it can be seen that this dynamic system can be regarded as a linear time-varying (LTV) system, even though it still is, in fact, a nonlinear system. As the system matrix A(t, y(t)) depends on the system output, y(t), it can be considered as a time varying matrix, A(t) = A(t, y(t)). However, this is not a problem for observability analysis and observer design purposes, as the output is available and can be considered as a known function of t. The same approach has been pursued in [BSO11d], where further details on preliminary results can be found. In the remaining of this analysis, when referring to the nonlinear system (6.4) regarded as LTV, or in short, the LTV system (6.4), it is implied that the output signal is considered a known function of t, and thus the system matrix A(t, y(t)) is also a known matrix function of t. In order to simplify the analysis, let Rg (t) ∈ SO(2) be a rotation matrix such that R˙ g (t) = rg (t)Rg (t)S and consider the Lyapunov state transformation [Bro70], z(t) := T(t)x(t), which 131

Chapter 6: Sensor-based SLAM preserves stability and observability properties, with T(t) = diag InV , TM (t) , TM (t) = diag (T1 (t), . . . , Tm (t)) , and Ti (t) = diag Rg (t), Rg (t) ∈ Rni ×ni , with ni = 4, considering a position landmark with one associated direction. It is a matter of computation to show that the new nonlinear system is given by (

which can also be rewritten as

(

˙ = A (t, xO (t)) z(t) z(t) , y(t) = C (t) z(t)

(6.5)

˙ = A (t, y(t)) z(t) z(t) , y(t) = C (t) z(t)

(6.6)

where " A(t, y(t)) =

# 0nV ×nV 0nV ×nO ∈ Rnx ×nx , A MV (y(t)) 0nO ×nO

h iT A MV (y(t)) = A TMV1 (y1 (t)) . . . A TMVm (ym (t)) ∈ RnO ×nV , A MVi (yi (t)) = Ti (t) AMVi (yi (t)) ∈ Rni ×ni , and C (t) = C TT (t).

6.3.1

Preliminary Results

Consider the following definitions of observability, observability Gramian, system transition matrix, and uniform complete observability. For further details on these definitions, the reader is referred to [Rug95, Bro70, SA68]. h i Definition 6.1 (Observability). The system (6.3) is observable on T = t0 , tf if any initial state x(t0 ) is uniquely determined by the corresponding output [y(t) , t ∈ T ]. Definition 6.2 (Observability Gramian and transition matrix). Given the input u(t) and output y(t), for all t ∈ T , of the general system ( ˙ = A(t, u(t), y(t))z(t) + B (t)u(t) z(t) , y(t) = C (t)z(t)

(6.7)

A(t, u(t), y(t)),C C (t)), denoted as W tf , t0 , is the observability Gramian associated with the pair (A defined as Z W tf , t0 =

tf

t0

φ T (τ, t0 ) C T (τ)C C (τ)φ φ (τ, t0 ) dτ

where it is possible to compute the transition matrix Zτ φ (τ, t0 ) = I + A (σ1 , u(σ1 ), y(σ1 )) dσ1 t0 τ

Z +

t0

+ ... 132

A (σ1 , u(σ1 ), y(σ1 ))

Z

σ1 t0

A (σ2 , u(σ2 ), y(σ2 )) dσ2 dσ1

6.3 Observability Analysis associated with A (t, u(t), y(t)). Definition 6.3 (Uniform complete observability, [SD82, Definition VI.3]). The system (6.7), regarded as LTV, is called uniformly completely observable if there exist positive constants δ, α1 , and α2 such that α1 I ≤ W (t + δ, t) ≤ α2 I

(6.8)

for all t ≥ t0 . Considering the above definition, it is easy to see that the right side of (6.8) is always satisfied when the system matrices A(t) and C (t) are norm-bounded. This is the case of the system under study in this section and, therefore, only the existence of the lower bound is addressed in the theoretical considerations provided below. The following lemma provides the basis for the observability analysis of this type of systems. Lemma 6.1 ([BSO11d, Lemma 1]). Given the system input, u(t), and output, y(t), for all t ∈ T = h i A(t, u(t), y(t)),C C (t)) on T is t0 , tf , if the observability Gramian W t0 , tf associated with the pair (A invertible, then system (6.7) is observable on T , in the sense that the initial condition z(t0 ) is uniquely defined. Also, the following proposition is used throughout the proof of Theorem 6.6. Proposition 6.2 ([BSO11a, Proposition 4.2]). Let g(t1 , t2 ) : T × T ⊂ R2 → Rn be a continuous and h i i-times continuously differentiable function on T := t0 , tf , and T := tf − t0 > 0 such that ∂ g (t0 , τ) ∂i−1 g (t0 , τ) = ... = g (t0 , t0 ) = = 0. ∂τ τ=t0 ∂τ i−1 τ=t0

i+1

∂ g(t ,τ) Further assume that

∂τ i+10

≤ C for all t ∈ T . If there exists a positive constant α > 0 and

τ=t

∂i g(t0 ,τ)

a time instant t ∈ T such that ∂τ i

≥ α then there exists 0 < δ ≤ T and a positive constant τ=t β > 0 such that kg(t0 , t0 + δ)k ≥ β.

6.3.2

Main Results

The first result presented in this chapter establishes sufficient conditions for observability of the nonlinear system (6.4), regarded as LTV. The remaining of the observability analysis will follow by establishing that a GAS observer for system (6.4) regarded as LTV is also a GAS observer for the nominal nonlinear system, which enables the design of such an observer considering only the system regarded as LTV. Furthermore, it is also shown that the proposed conditions for observability are both sufficient and necessary, and that the system regarded as LTV is uniformly completely observable. This last result is the foundation of the design of the proposed GAS observer. Prior to the first result, a realistic assumption about the detected landmarks is presented, which basically states that all detected position landmarks are different and have nonzero norm, 133

Chapter 6: Sensor-based SLAM and that any direction also has nonzero norm. This is the case when using a laser scanner, since the origin is the laser firing point and it is impossible to have two laser points of the same scan with exactly the same bearing. Assumption 6.1 (Different and nonzero landmarks). For any detected landmark i ∈ IO , its position and any associated direction satisfies ypi (t) , 0 and ydi (t) , 0 for all t ≥ t0 . Furthermore, any two detected position landmarks i, j, ∈ IO , with i , j, satisfy ypi (t) , ypj (t) for all t ≥ t0 . Theorem 6.3 (Observability). Suppose that Assumption 6.1 holds. Then, the nonlinear system (6.4) h i regarded as LTV is observable on T := t0 , tf if there exists a time instant t1 ∈ T such that there exist, at least, either: (i) two visible position landmarks, yp1 (t1 ) and yp2 (t1 ); (ii) one visible position landmark yp1 (t1 ) with, at least, one associated direction yd1 (t1 ); (iii) one visible position landmark yp1 (t1 ) with nonzero derivative, y˙ p1 (t1 ) , 0. Proof. As the Lyapunov transformation T(t) preserves observability, this proof establishes sufficient conditions of observability for the nonlinear system (6.6) regarded as LTV, which are also valid for the nominal nonlinear system (6.4), also regarded as LTV. Using Lemma 6.1, system (6.6), regarded as LTV, is observable on T if the observability Gramian associated with A(t, y(t)),C C (t)) on T is invertible. The proof follows by establishing that this is the case. the pair (A The transition matrix associated with A (t, y(t)) is given by " # InV 0 φ (t, t0 ) = , φ MV (t, t0 ) InO where h iT φ MV (t, t0 ) = φ TMV1 (t, t0 ) · · · φ TMVm (t, t0 ) , Zt φ MVi (t, t0 ) = Ti (σ ) AMVi (σ ) dσ , t0

and # −I2 S ypi (σ ) AMVi (σ ) = , 02×2 S ydi (σ ) "

if it is considered that every landmark has an associated direction. Consider the vector c := h iT h iT h iT h iT cTV cTM ∈ Rnx , where cV := cTv cb , cM := cT1 . . . cTm , cb ∈ R, cv ∈ R2 , and ci = cTpi cTdi , with cpi , cdi ∈ R2 . If W t0 , tf denotes the observability Gramian associated with the pair h i A(t, y(t)),C C (t)) on t0 , tf , it is a matter of computation to show that (A Z cT W t 0 , t f c = 134

tf t0

kf (t0 , τ)k2 dτ,

6.3 Observability Analysis where

Z f (t0 , τ) = cM +

τ

TM (σ ) AMV (σ ) dσ cV , t0

h iT for all τ ∈ T , with AMV (σ ) = ATMV1 (σ ) · · · ATMVm (σ ) . The first derivative of the function f (t0 , τ) is simply given by ∂ f (t , τ) = TM (τ) AMV (τ) cV , ∂τ 0 where, for the limit points τ = t0 and τ = tf , with a slight abuse of notation, the derivative is considered to be, respectively, the right and left derivatives of f. The second derivative is given by ∂2 ˙ M (τ) AMV (τ) + TM (τ) A ˙ MV (τ) cV , (t f , τ) = T 0 ∂τ 2 where the first derivative of the components of AMV (τ) is " # d AMVi (τ) 02×2 S y˙ pi (τ) ˙ = AMVi (τ) = . 02×2 S y˙ di (τ) dτ Suppose now that the observability Gramian W t0 , tf is not invertible. Then, there exists a unit vector c such that cT W (t0 , t) c = 0 for all t ∈ T , which in turn implies that f (t0 , t) = 0 and

(6.9) ∂ f (t0 , t) = 0 ∂t

for all t ∈ T . In particular,

for t = t0 , this immediately implies that ci = 0 ∀i=1,...,m . Then, under conditions (i) or (ii) of the theorem, the only solution of

∂ f (t0 , t) = 0 ∂t

for all t ∈ T is cv = 0 and cb = 0, as the result of

Assumption 6.1. Moreover, under condition (iii), it can be seen that

∂ f (t0 , t) = 0 ∂t

for all t ∈ T is

equivalent to −cv + S yp1 (t) cb = 0 , for all t ∈ T , and taking the time derivative on both sides it becomes S y˙ p1 (t) cb = 0 , for all t ∈ T . In particular, for t = t1 , the only solution is cb = 0 and, thus, the former equation implies that cv = 0. Either way, this contradicts the existence of a unit vector c such that (6.9) holds for all t ∈ T . As such, the observability Gramian W t0 , tf is invertible on T and, from Lemma 6.1, the nonlinear system (6.6), regarded as LTV, is observable on T , thus concluding the proof of the theorem. With these sufficient conditions for observability, a practical result on the design of an observer for the nominal nonlinear system is presented in the following theorem. Theorem 6.4 (Nonlinear system). If the conditions of Theorem 6.3 hold, then: (i) the initial state of both the nonlinear system (6.4), regarded as LTV, and the nominal nonlinear system (6.3) are the same and uniquely determined; 135

Chapter 6: Sensor-based SLAM (ii) a state observer with uniformly globally asymptotically stable error dynamics for the LTV system is also a state observer for the underlying nonlinear system, with uniformly globally asymptotically stable error dynamics. Proof. As in the previous theorem, the nonlinear system (6.6) is considered for simplicity of analysis, noting that the Lyapunov transformation preserves all the properties provided in this proof for system (6.4). Consider the initial condition for the nonlinear system (6.6), regarded as LTV, given by h iT z¯ (t0 ) = v¯ T (t0 ) b¯ r (t0 ) z¯ T1 (t0 ) . . . z¯ Tm (t0 ) , h iT where z¯ i (t) = z¯ Tpi (t) z¯ Tdi (t) , for landmarks with one associated direction. It is also noted that y(t) = C (t) z(t) implies that z¯ pi = Rg (t) ypi (t) and z¯ di = Rg (t) ydi (t), for all t ∈ T . Then, it can be seen that the output of the nonlinear system (6.6), regarded as LTV, as a function of the initial φ (t, t0 ) z¯ (t0 ), yielding conditions is defined by y(t) = C (t)φ Zt h i T T ypi (t) = Rg (t) z¯ pi (t0 ) + Rg (t) Rg (σ ) S ypi (σ ) b¯ r (t0 ) − v¯ (t0 ) dσ t0

=

RTg (t) Rg (t0 ) ypi (t0 ) + RTg (t)

Z

t t0

h i Rg (σ ) S ypi (σ ) b¯ r (t0 ) − v¯ (t0 ) dσ ,

(6.10)

for all t ∈ T and i ∈ IO , noting that a similar expression can be derived for the output of each landmark direction. Multiplying both sides by Rg (t) and rearranging the terms, (6.10) becomes Zt Rg (t) ypi (t) − Rg (t0 ) ypi (t0 ) = Rg (σ ) −¯v (t0 ) + S ypi (σ )b¯ r (t0 ) dσ t0

and differentiation of both sides yields R˙ g (t) ypi (t) + Rg (t) y˙ pi (t) = Rg (t) −¯v (t0 ) + S ypi (t) b¯ r (t0 ) , which can be further simplified recalling that R˙ g (t) = rg (t) Rg (t) S, y˙ pi (t) = −[rg (t) − b¯ r (t0 )] S ypi (t) − v¯ (t0 ) , and differentiating both sides once more results in the second order derivative y¨ pi (t) = −[rg (t) − b¯ r (t0 )] S y˙ pi (t) − r˙g (t) S ypi (t) . Using the same procedure for the landmark directions, it can be seen that y˙ di (t) = −[rg (t) − b¯ r (t0 )] S ydi (t) , and y¨ di (t) = −[rg (t) − b¯ r (t0 )] S y˙ di (t) − r˙g (t) S ydi (t) . Consider now the initial condition for the nominal nonlinear system (6.3) given by h i x(t0 ) = v(t0 ) br (t0 ) xT1 (t0 ) . . . xTm (t0 ) , 136

6.3 Observability Analysis h iT where xi (t) = pTi (t) dTi (t) . It can be seen that the output of the nonlinear system as a function of the initial condition is given by y(t) = C x(t), yielding Z t ypi (t) = pi (t0 ) − [rg (σ ) − br (σ )]S pi (σ ) + v(σ ) dσ t0

and Z ydi (t) = di (t0 ) −

t t0

[rg (σ ) − br (σ )]S di (σ )dσ ,

for all t ∈ T and i ∈ IO . Noting that v(t) and br (t) are constant, the first order derivatives of the above expressions are given by y˙ pi (t) = −[rg (t) − br (t0 )]S pi (t) − v(t0 ) and y˙ di (t) = −[rg (t) − br (t0 )]S dt (t) , for all t ∈ T and i ∈ IO . It is relevant to note that the above expressions imply y˙ pi (t) = p˙ i (t) and y˙ d (t) = d˙ i (t), also for all t ∈ T and i ∈ IO . Furthermore, the second order derivatives are given by i

y¨ pi (t) = −[rg (t) − br (t0 )]S p˙ i (t) − r˙g (t)S pi (t) and y¨ di (t) = −[rg (t) − br (t0 )]S d˙ i (t) − r˙g (t)S di (t) , for all t ∈ T and i ∈ IO . In particular, comparing the first order derivatives of the output of the LTV system and the output the nominal nonlinear system for t = t1 , noting that ypi (t1 ) = pi (t1 ) and ydi (t1 ) = di (t1 ) for all i ∈ IO , it can be seen that 0 = [br (t0 ) − b¯ r (t0 )]S ypi (t1 ) − [v(t0 ) − v¯ (t0 )] and 0 = [br (t0 ) − b¯ r (t0 )]S ydi (t1 ) , for all i ∈ IO . The first condition of Theorem 6.3 yields ( 0 = [b¯ r (t0 ) − br (t0 )]S yp1 (t1 ) + [v(t0 ) − v¯ (t0 )] , 0 = [b¯ r (t0 ) − br (t0 )]S yp2 (t1 ) + [v(t0 ) − v¯ (t0 )] whereas the second condition yields ( 0 = [b¯ r (t0 ) − br (t0 )]S yp1 (t1 ) + [v(t0 ) − v¯ (t0 )] . 0 = [b¯ r (t0 ) − br (t0 )]S yd1 (t1 ) 137

Chapter 6: Sensor-based SLAM Thus, the only solution for the set of equations derived for each condition is b¯ r (t0 ) = br (t0 ) and v¯ (t0 ) = v(t0 ). For the third condition, it can be seen that the comparison of the first and second order derivatives for t = t1 yields (

0 = [b¯ r (t0 ) − br (t0 )]S yp1 (t1 ) + [v(t0 ) − v¯ (t0 )] , 0 = [b¯ r (t0 ) − br (t0 )]S y˙ p1 (t1 )

which, for y˙ p1 (t1 ) , 0, implies that the only solution for the above system of equations is also b¯ r (t0 ) = br (t0 ) and v¯ (t0 ) = v(t0 ). It is noted that the state of the nonlinear system (6.4) is related through the Lyapunov transformation T(t) with the state of the nonlinear system (6.6). Therefore, under the conditions of Theorem 6.3, the state of the nonlinear system (6.4), regarded as LTV, and the state of the nominal nonlinear system (6.3) are the same and uniquely defined, provided the knowledge about the evolution of y(t) and rg (t) for all t ≥ t0 . This concludes the proof of the first part of the theorem. The second part of the theorem follows from this conclusion. The estimation error of an observer designed for the LTV system with GAS error dynamics converges to zero, which means that its estimates asymptotically approach the true state. But as the true state of the LTV system matches that of the nominal nonlinear system, this means that the observer for the LTV system is also an observer for the nominal nonlinear system, with GAS error dynamics. The next result shows that there is no conservativeness whatsoever in the conditions of Theorem 6.3, as they are necessary and sufficient for the observability of the nominal nonlinear system. While the first two conditions of Theorem 6.3 are two well-known examples of how to observe the pose of a vehicle in 2-D, the importance of the third condition arises when proving necessity in the following result. Indeed, when only one landmark is visible and the vehicle is moving exactly in a circle around that landmark with the same relative bearing, the system becomes unobservable. Conversely, if either the relative bearing or the distance to that landmark varies through time, the pose of the vehicle is observable. Theorem 6.5 (Sufficient and necessary). The nonlinear system (6.3) is observable on T = [t0 , tf ], if and only if the conditions of Theorem 6.3 hold. Proof. The sufficiency part is readily provided by Theorems 6.3 and 6.4, assuming that the output signal y(t) is known for all t ∈ T . The proof of necessity is achieved by transposition, i.e., if none of the conditions of the theorem are met, then the nonlinear system (6.3) must be unobservable, in the sense that, there exists a system output for which the initial condition is not unique. The proof will proceed towards this goal, noting that this is equivalent to find, at least, two different initial conditions for which the generated output is exactly the same. Recalling the landmark nonlinear dynamic equations defined in (6.1), the output solution 138

6.3 Observability Analysis for the nonlinear system (6.3), as a function of the initial conditions, can be defined as y(t) = p1 (t) Z = p1 (t0 ) + = p1 (t0 ) −

t

p˙ 1 (σ )dσ t0 Z t t0

v(σ ) + [rg (σ ) − br (σ )] S p1 (σ ) dσ ,

for all t ∈ T , when there is only one visible position landmark. The negation of the conditions of the theorem means that either no landmark is visible or there is only one constant visible position landmark, yp1 (t) = yp1 (t0 ), for all t ∈ T . As the case where no landmark is visible clearly renders the system unobservable, the proof proceeds with the analysis of the case where there is only one visible position landmark that remains constant for all t ∈ T . Under this condition, the dynamic constraint yi (t) = xi (t) for all i ∈ IO and t ∈ T , implies that y1 (t) = yp1 (t) = p1 (t) = p1 (t0 ) for all t ∈ T , yielding p˙ 1 (t) = 0 and h i v(t) = − rg (t) − br (t) S p1 (t0 ) , ˙ = 0 and b˙ r (t) = 0 for all t ∈ T , it can for all t ∈ T . As the vehicle dynamics further impose v(t) also be seen that rg (t) = rg (t0 ) for all t ∈ T , yielding the system output solution Z t y(t) = p1 (t0 ) − v(t0 ) + [rg (t0 ) − br (t0 )] S p1 (t0 ) dσ t0

= p1 (t0 ) − (t − t0 ) v(t0 ) + [rg (t0 ) − br (t0 )] S p1 (t0 ) , for all t ∈ T . Consider two different initial conditions for system (6.5) given by h iT xa (t0 ) = vTa (t0 ) br (t0 ) pT1 (t0 ) and h iT xb (t0 ) = vTb (t0 ) α br (t0 ) pT1 (t0 ) , where α , 0 ∈ R and there is only one position landmark, such that the initial velocities are defined as h i va (t0 ) = − rg (t0 ) − br (t0 ) S p1 (t0 ) and h i vb (t0 ) = − rg (t0 ) − α br (t0 ) S p1 (t0 ) . Considering the previously computed output solution expression, it is a matter of algebraic manipulation to show that the output solution of (6.3) for the initial condition xa (t0 ) is given by ya (t) = p1 (t0 ) − (t − t0 ) va (t0 ) + [rg (t0 ) − br (t0 )] S p1 (t0 ) = p1 (t0 ) − (t − t0 ) (va (t0 ) − va (t0 )) , 139

Chapter 6: Sensor-based SLAM for all t ∈ T , whereas the output for the second initial condition, xb (t0 ), is given by yb (t) = p1 (t0 ) − (t − t0 ) vb (t0 ) + [rg (t0 ) − α br (t0 )] S p1 (t0 ) = p1 (t0 ) − (t − t0 ) (vb (t0 ) − vb (t0 )) , for all t ∈ T . From the above expressions it can be seen that ya (t) = yb (t) = p1 (t0 ) for all t ∈ T , meaning that there are at least two different initial conditions for which the output of the system is the same. This implies that the system is unobservable on T . Therefore, if none of the conditions of the theorem are satisfied, then system (6.3) is unobservable, which proves the necessity of those conditions, thus concluding the proof of the theorem. With the previous results, the design of a filter for system (6.4) regarded as LTV, with globally asymptotically stable error dynamics follows naturally with a linear time-varying Kalman filter, provided that the pair (A(t), C(t)) is UCO. A generalization of Assumption 6.1 is also introduced, which considers upper and lower bounds for the norms of each landmark and for the differences between them. This is also a realistic assumption, as in a laser scanner there are upper and lower bounds for both range and bearing measurements. The necessity of this generalization emerges from the need to establish uniform bounds when proving UCO. Assumption 6.2 (Bounds on landmarks). For any detected landmark i ∈ IO , there exist α¯ p > 0,

αpi > 0, α¯ d > 0, and αdi > 0 such that α¯ p ≥

ypi (t)

≥ αpi and α¯ d ≥

ydi (t)

≥ αdi for all t ≥ t0 . Futhermore, for any two detected position landmarks i, j ∈ IO , with i , j, there exist αpij > 0 such that

yp (t) − yp (t)

≥ αp for all t ≥ t0 . i

j

ij

As stronger forms of observability are needed when dealing with LTV systems, the following result establishes the conditions under which the pair (A(t), C(t)) is uniformly completely observable. Theorem 6.6 (Uniform complete observability). Suppose that Assumption 6.2 holds. The pair (A(t), C(t)) is uniformly completely observable if and only if there exist αp˙ > 0 and δ > 0 such that, for all t ≥ t0 , it is possible to choose t1 ∈ Tδ , with Tδ = [t, t + δ], for which there exist, at least, either: (i) two visible position landmarks, yp1 (t1 ) and yp2 (t1 ); (ii) one visible position landmark yp1 (t1 ) with, at least, one associated direction yd1 (t1 );

(iii) one visible position landmark yp1 (t1 ) for which the condition

y˙ p1 (t1 )

≥ αp˙ is satisfied. Proof. The proof of this theorem, provided in the Appendix D, follows similar steps to the proofs of Theorems 6.3 and 6.5, but considering uniform bounds for all t ≥ t0 and intervals [t, t + δ]. The third condition of Theorem 6.6, which is necessary for the observability of the system when only one landmark is available, may be interpreted as a persistent excitation condition. 140

6.3 Observability Analysis

6.3.3

Convergence Analysis

This section provides a summary of the results that can be used to establish the GAS filter error dynamics, or more accurately, uniform global asymptotic stability (UGAS). Consider the continuous-time observer dynamics defined by  T  Θ −1 (t) (y(t) − C(t) xˆ (t)) , xˆ (t0 ) = x0   xˆ˙ (t) = A(t) xˆ (t) + P(t) C (t)Θ   ˙ = P(t) AT (t) + A(t) P(t) − P(t) CT (t)Θ P(t) Θ −1 (t) C(t)P(t) + Ξ (t) , P(t0 ) = P0 and assume Ξ (t) is positive semidefinite, partitioned as Ξv (t)Ξb (t),Ξ Ξ1 (t), . . . ,Ξ Ξm (t)) , Ξ (t) = diag (Ξ with Ξ i (t) 0 for all i ∈ IO , and all remaining elements are positive semidefinite, whereas Θ (t) is a symmetric positive definite matrix. It is noted that for such an observer, matrix P(t) exists and is a symmetric positive semidefinite matrix for all t ≥ t0 . Defining the error variable as x˜ (t) = x(t) − xˆ (t), it can be seen that the observer error dynamics are given by ˜ x˜ (t) , x˜˙ (t) = A(t)

(6.11)

˜ = A(t) − P(t) CT (t)Θ ˜ = A(t) − P(t) C ˜ T (t) C(t), ˜ ˜ = Θ −1 (t)C(t), or equivalently, A(t) where A(t) with C(t) 1

Θ − 2 (t)C(t), and the matrices A(t), C(t), Ξ(t), and Θ (t) are assumed to be norm-bounded. The following theorem is included for completeness, as it is a standard result is design of observers for LTV systems and can be found in the literature with minor variations. Further details can be found in [And71], [Jaz70], [Kha02, Example 8.11], and references therein. Theorem 6.7 (Global asymptotic stability). If the conditions of Theorem 6.6 are met, then the origin of the observer error dynamics (6.11) is a uniformly globally asymptotically stable equilibrium point. Proof. To achieve asymptotic stability, uniform complete controllability, or a weaker form of controllability, is usually required. An alternative, using [And71, Lemma 3.1], is to shown that the controllability Gramian is invertible and, consequently, the matrix P(t) is positive definite. The process is very similar to what is done in the proof of Theorem 6.3, and thus omitted. Therefore, it can be seen that P(t) 0, yielding P−1 (t) 0 and 0 < αmin I ≤ P−1 (t) ≤ αmax I . for all t > t0 . Consider the candidate Lyapunov function V(t, x˜ (t)) = x˜ T (t) P−1 (t) x˜ (t) , with derivative given by " # d P−1 (t) T T −1 −1 ˙ ˜ ˜ V(t, x˜ ) = x˜ (t) A (t) P (t) + P (t) A(t) + x˜ (t) dt h i ˙ P−1 (t) − 2 C ˜ T (t) C(t) ˜ = x˜ T (t) AT (t) P−1 (t) + P−1 (t) A(t) + P−1 (t) P(t) x˜ (t) h i ˜ T (t) C(t) ˜ − P−1 (t)Ξ Ξ(t) P−1 (t) x˜ (t) . = x˜ T (t) −C 141

Chapter 6: Sensor-based SLAM Given the positive definiteness of P−1 (t) for all t > t0 , it can easily be seen that αmin k˜x(t)k2 ≤ V(t, x˜ ) ≤ αmax k˜x(t)k2 , and that h i ˙ x˜ ) = x˜ T (t) −C ˜ T (t) C(t) ˜ − P−1 (t)Ξ Ξ(t) P−1 (t) x˜ (t) V(t, ˜ T (t) C(t) ˜ x˜ (t) ≤ −˜xT (t) C ≤0. Let the transition matrix of (6.11) be denoted as φ˜ (t2 , t1 ) and the solution at time t2 from the initial condition x˜ (t1 ) be defined as x˜ φ (t2 ; t1 , x˜ ) = φ˜ (t2 , t1 ) x˜ (t1 ) . Then, it can be seen that Z V(t + δ, x˜ φ (t + δ; t, x˜ )) − V(t, x˜ ) =

t+δ

t T

˙ V(τ, x˜ φ (τ; t, x˜ )) dτ Z

≤ −˜x (t)

t+δ

T ˜ T (t) C(t) ˜ φ˜ (t + δ, t) dτ x˜ (t) φ˜ (t + δ, t) C

t

˜ (t + δ, t) x˜ (t) . = −˜xT (t) W ˜ (t + δ, t) is the observability Gramian of the pair (A(t), ˜ ˜ where W C(t)). As the pair (A(t), C(t)) is uniformly completely observable and the matrices Θ (t) and P(t) are positive definite for all ˜ ˜ ˜ t > t0 , it can easily be seen that the pairs (A(t), C(t)) and (A(t), C(t)) are uniformly completely observable, yielding ∗ ˜ (t + δ, t) x˜ (t) ≤ α ∗ k˜x(t)k2 , 0 < αmin k˜x(t)k2 ≤ x˜ T (t) W max ∗ ∗ where αmin and αmax are positive constants and, consequently, ∗ V(t + δ, x˜ φ (t + δ; t, x˜ )) − V(t, x˜ ) ≤ −αmin k˜x(t)k2 ≤ −

∗ αmin V(t, x˜ ) . αmax

Thus, noting that all the conditions of [Kha02, Theorem 8.5] are satisfied globally, the proposed filter is uniformly globally asymptotically stable, which concludes the proof of the theorem. This concludes the convergence analysis that, in fact, is a direct consequence of the observability results presented before. A summary of the steps taken in this section include: 1. the nonlinear system is rewritten in such a way that it can be regarded as LTV for observability analysis purposes; 2. the sufficient and necessary conditions for the observability of the nonlinear system are derived; 142

6.4 SLAM Filter Design 3. an observer with GAS error dynamics for the LTV system is also an observer for the nonlinear system with GAS error dynamics; 4. necessary and sufficient conditions for the uniform complete observability of the system, regarded as LTV, are derived; and 5. a continuous-time observer for the LTV system is derived and its error dynamics are proved to be GAS.

6.4

SLAM Filter Design

The design of the sensor-based SLAM filter is presented in this section. As detailed in the previous section, system (6.3), regarded as LTV, is uniformly completely observable under the conditions of Theorem 6.6. This is an important result that leads naturally to the design of a Kalman filter with globally asymptotically stable error dynamics, noting that other filtering alternatives could be devised, e.g. a H∞ filter. The majority of sensors available today and the processing units used in robotic vehicles are usually sample-based or even intrinsically digital. To profit from those sensors as well as the low cost and light weighted embedded computers that can be installed on board small platforms, a discrete-time version of the SLAM algorithm is required. Without loss of generality, let Ts denote the sampling period of the synchronized IMU and laser scanner, noting that a multi-rate implementation of the proposed filter can be devised. Considering that xk := x(tk ), Ak := A(tk ), and Ck := C(tk ), with tk = t0 + k Ts , k ∈ N0 , and t0 as the initial time, the forward Euler discretization of the system dynamics (6.3) yields    xk+1 = Fk xk + ξ k ,   yk+1 = Hk+1 xk+1 + θ k+1

(6.12)

where Fk := Inx +Ts Ak and Hk := Ck . System (6.12) includes system disturbance and measurement noise, where the vectors ξ k ∈ Rnx and θ k ∈ Rny are zero-mean discrete white Gaussian noise, ξ k ξ Tl i = Ξk δk−l and hθ θ k θ Tl i = Θ k δk−l , respectively, where δk denotes the discrete-time Dirac with hξ delta function and h.i stands for the expectation operator. It is noted that the dimensions of the discrete system (6.12) may change dynamically with the number of visible landmarks obtained in each new laser measurement.

6.4.1

Prediction Step

The resulting discrete-time Kalman filter equations for the above system are standard [KB61, Gel74, Jaz70]. Nonetheless, system (6.12) does not account for the non-visible landmarks xUk , which have to be propagated in open-loop using the nonlinear equations defined in (6.2). Using h iT the full system state vector xFk := xTk xTUk , the prediction equations are   xˆ = FFk|k xˆ Fk|k   Fk+1|k ,   Σ Fk+1|k = Fˆ Fk|k Σ Fk|k Fˆ TF + Ξ Fk k|k 143

Chapter 6: Sensor-based SLAM where xˆ Fk|k denotes the estimated full state vector, the disturbance covariance matrix is defined ΞUk , whereas the full state covariance matrix Σ Fk|k can be decomposed in as Ξ Fk = diag Ξ k ,Ξ vissible, non-visible, and crossed terms, respectively, Σ k|k , Σ Uk|k , and Σ U Ok|k as # Σ k|k Σ TU Ok|k = , Σ U Ok|k Σ Uk|k "

Σ Fk|k

and the full transition matrix FFk|k is defined as " FFk|k =

# 0nx ×nU , FUk|k

Fk FU Ok|k

with FUk|k = InU − Ts [rmk − bˆ rk|k ] SnU and

FU Ok|k

  FU V1k|k   . =  ..  FU Vn U

k|k

 02×nx  MO   ..  .  .  02×n  xMO

To compute the covariance of the non-visible landmarks it is also necessary to use the matrix Fˆ Fk|k =

6.4.2

∂ FFk|k xˆ Fk|k ∂ˆxFk|k

.

Landmark Detection

Whenever a new laser profile measurement is obtained, the raw values are projected into the horizontal frame {H} using the roll and pitch information provided by an external attitude filter. The horizontal laser profile is fed to a landmark detection algorithm, where the data is processed into clusters and a robust line detection strategy is implemented, which builds on the basic Split and Merge algorithm, see [PH74], using a reduced space Hough transform parameter fitting for each line, in a similar way to the algorithm proposed in [FMCV10]. Afterwards, a corner identification procedure is used to obtain the desired point landmarks. This detection algorithm can provide point landmarks with up to two associated line segments, which may be included in the filtering process as direction landmarks. This is the converse case of having line landmarks with two end points.

6.4.3

Update Step

The association between measured landmarks and existing state landmarks is implemented resorting to the JCBB algorithm, see [NT01] for details. In short, this algorithm searches the tree of possible landmark association pairs such that all the associations are jointly compatible in a probabilistic sense. To avoid computational complexity pitfals intrinsic to such a tree search 144

6.4 SLAM Filter Design algorithm, the JCBB algorithm may be stopped if a predefined number of jointly compatible associations are found, and a simpler strategy, such as the nearest neighbour, is used for the remaining unassociated measured landmarks. It is noted that the corner detection algorithm and the landmark association algorithm may be replaced by others available, as they are completely independent of the filtering technique proposed in this chapter. The sets of visible and nonvisible landmarks, respectively, IOk and IUk , are redefined at each new laser measurement, based on landmark associations provided by the JCBB algorithm. In addition, this algorithm naturally provides the innovation vector and respective covariance matrix,     ν k+1 = yk+1 − Hk+1 xˆ Fk+1|k .   Σ ν = Hk+1 Σ F HTk+1 + Θ k+1 k+1 k+1|k

(6.13)

Thus, for the update step, it is just a matter of applying the standard discrete time-varying Kalman filter update equations, given by     xˆ k+1|k+1 = xˆ k+1|k + Kk+1 ν k+1 ,   Σ k+1|k+1 = Σ k+1|k − Kk+1 Hk+1 Σ k+1|k

(6.14)

where the Kalman gain is defined as Kk+1 = Σ k+1|k HTk+1 Σ −1 νk+1 .

6.4.4

Basic Loop Closing

The loop closing problem is not the main focus of this chapter, but rather a demonstration that the proposed filter provides consistent estimates that will enable any loop closing technique to perform efficiently in real world scenarios. Any available strategy can be used, such as the partitioning of the total map into smaller statistically independent maps, accounting for the proper joining and merging between them using a common set of landmarks, in a similar fashion to what is proposed in [TNNL02] and [CMCTN07]. One of the main drawbacks of the sensor-based approach to SLAM is the computational complexity resulting from having to update all the landmarks. Indeed, the use of the submap strategy allows the definition of an upper bound for the number of landmarks in the current state vector, thus limiting the computational complexity. A basic algorithm is described below and implemented for completeness of the experimental results presented in Section 6.6. The main difficulty when closing a loop is to obtain a coherent map, as older landmarks will tend to have large uncertainty and wrong associations are very likely to happen during the JCBB procedure of the update step, especially for environments with a dense population of landmarks. The basic idea is then to use only a subset of the state landmarks for the current update step association algorithm, Icur , where only the landmarks that were most recently visible to the vehicle are used. When revisiting an area, this procedure will generate duplicates of older landmarks, which are defined as the set Iold . Therefore, a rather 145

Chapter 6: Sensor-based SLAM naive loop closing algorithm is to search periodically for possible landmark associations between the sets Icur and Iold , triggering a loop closure if a minimum number of jointly compatible associations are available. The set of state landmarks is then partitioned into three sets: i) current landmarks Icur , ii) old landmarks Iold , and iii) the landmarks in between the former two sets, Igap , which are neither used in the current update step JCBB association algorithm nor in the loop closing association algorithm. Consider the noise free association between landmark i ∈ Icur and landmark j ∈ Iold , using ˆ ik|k − m ˆ jk|k = Hijk xˆ Fk|k . The respective noise covariance matrix is Θ ijk := 0 the output vector zijk := m and the measurement vector is defined as yijk := 0. Thus the innovation vector and respective covariance matrix, defined earlier in (6.13), can be adapted for the loop closing association of these landmarks, yielding     ν ijk = −Hijk xˆ Fk|k .   Σ ν = Hij Σ F Hij k+1 k k k|k A loop closure takes place if a significant number of valid loop closing associations are jointly compatible. This test can be computed using a modified version of the JCBB algorithm adapted to the above loop closing innovation expressions. With the joint innovation vector and covariance matrix provided by this algorithm, the standard Kalman filter update equations (6.14) can be used to constraint the two landmarks to be equal and fully correlated, enabling the removal of one of them from the filter state.

6.5

Earth-Fixed Trajectory and Map Estimation

In the previous sections a SLAM filter was proposed to estimate the landmark locations as well as the linear velocity and rate gyro bias, all described in vehicle or sensor frame, {H}. With this sensor-based approach, the vehicle pose is trivially given by the coordinate frame itself. However, in many applications it is desirable to obtain an earth-fixed map and trajectory of the vehicle, which are yet to be determined. For this reason, this section is focused on formulating and solving the underlying optimization problem to estimate, in real-time, both the vehicle trajectory and the map described in the earth-fixed reference frame {E}. This section presents the formulation, closed form solution, and uncertainty characterization in Subsection 6.5.1, which is followed by the earth-fixed trajectory and map estimation algorithm proposed in Subsection 6.5.2 and its validation through extensive Monte Carlo simulations in Subsection 6.5.3.

6.5.1

Procrustes Problem

This subsection presents the formulation and closed-form solution of the extended orthogonal Procrustes problem, as well as the uncertainty characterization of the obtained translation and rotation. The problem formulation features individual weights for each pair of points, and the uncertainty characterization proposes the use of individual covariance matrices for each point and considers that the solution can be an arbitrary rotation matrix. 146

6.5 Earth-Fixed Trajectory and Map Estimation 6.5.1.1

Optimization Problem and Closed-form Solution

Let ei ∈ R2 denote the error function between the position of the i-th landmark in an arbitrary frame {A}, ai ∈ R2 , and its respective position in some other frame {B}, bi ∈ R2 , rotated and translated into frame {A}, yielding ei := ai − R bi − p. The transformation from {A} to {B}, denoted as (p, R) , can be found by solving the optimization problem (R∗ , p∗ ) = arg

min

R∈SO(2),p∈R2

G(R, p) ,

(6.15)

where G(R, p) =

X i∈J

1 n σi2

kei k2 ,

each weight σi > 0 models the landmark uncertainty, and the set of landmarks used to find this transformation is denoted as J = {j1 , . . . , jn }. The above functional can be described using matrix notation as

2

Σe−1/2

, G(R, p) = n1

(Y − R X − p 1T )Σ where the vector 1 ∈ Rn×1 is a vector of ones and the matrices Y, X ∈ R2×n are, respectively, the concatenation of the landmark vectors described in {A} and {B} frames, ai and bi , for all i ∈ J . The weight matrix Σ e is a diagonal matrix defined as Σ e = diag(σ12 , . . . , σn2 ), where the diagonal elements are conservatively defined as Σ b i RT , σi2 := λmax Σ ai + λmax Σ bi ≥ λmax Σ ai + RΣ denoting λmax (.) as the maximum eigenvalue of the argument matrix. With this weight matrix Σ e , which ideally should be a covariance matrix, it is possible to use landmarks with different degrees of uncertainty without compromising the accuracy of the solution. The optimization problem just described has a closed-form, numerically robust, and computationally efficient solution, based on the results derived in [Ume91] and [Goo91]. Consider the 1 −1 T −1 and W := 1T Σ −1 1. Using the singular value matrix B := X W YT , with W := Σ −1 e − W Σe 1 1 Σe e T T decomposition svd B = U D V , it can be seen that the optimal rotation matrix is given by

R∗ = U∗ V∗T ,

(6.16)

where U∗ := U diag(1, |U|) and V∗ := V diag(1, |V|) are proper rotation matrices. Once the optimal rotation matrix solution is determined, the optimal translation is given by p∗ = 6.5.1.2

1 W

Σ−1 (Y − R∗ X)Σ e 1 .

(6.17)

Uncertainty Characterization

The goal of this section is to compute accurate approximations of the uncertainty associated with the extended Procrustes problem solution obtained above. The study of the statistical properties of the orthogonal Procrustes problem can be traced back to [Sib79] and [Goo91], 147

Chapter 6: Sensor-based SLAM which used perturbation theory to find the approximate distribution of the cost functional G(R, p), assuming Σ −1 = I, small rotations, and the same covariance matrix for each landmark. e These results have been extensively applied to the field of point-based medical image registration problems, as detailed in [FW01] and [WLFP08]. The analysis presented hereafter is based on the aforementioned works and aims at providing approximate uncertainty descriptions for the transformation parameters, R and p. This uncertainty characterization is achieved while considering arbitrary rotations and translations and using individual weights as well as individual covariance matrices for each landmark. The error models used for the known landmarks, in the context of perturbation theory, are defined as (0)

(1)

(0)

(1)

bi = bi + bi + O(2 ) , ai = ai + ai + O(2 ) , for all i ∈ J , where is the smallness parameter and the notation O(m ) stands for the remaining (0)

(0)

terms of order m or higher. The zero order terms are deterministic, i. e., hbi i = bi (0)

and

(0)

hai i = ai , whereas the first order terms are assumed to be Gaussian distributed with zero (1)

(1) T

mean and covariance matrices defined by Σbij = hbi bj

(1) (1) T

i and Σaij = hai aj

i, respectively, for

all i, j ∈ J . Note that each of these clouds have cross covariance terms between their landmarks, as suggested in the above covariance matrices expressions. The error model for the translation of the origin of frame {B} described in frame {A} is assumed to have the regular structure p = p(0) + p(1) + O(2 ) .

(6.18)

Conversely, the rotation matrix from {B} to {A} is assumed to have the special structure R = exp [S(Ω)] R(0) = [I + S(Ω) + O(2 )] R(0) ,

(6.19)

anticipating that Ω ∈ R will denote the rotation error and R(0) the true rotation matrix. The objective is then to obtain the expressions and properties of p(0) , p(1) , R(0) , and Ω, together with their expected values and covariance matrices. 6.5.1.3

Rotation Uncertainty

Firstly, the restriction of the rotation matrix R to the special orthogonal group SO(2) yields the equations RT R = I and |R| = 1. Considering the rotation matrix error model (6.19), it follows that R(0) ∈ SO(2) and that S(Ω) = −ST (Ω) is skew-symmetric. Secondly, from the derivation of the solution for the optimization problem (6.15), it is possible to conclude that skew (B R) = 0, where the operator skew (.) denotes the skew-symmetric component of the argument matrix. By using (6.19) and neglecting second and higher order terms, it can be seen that skew (B(0) R(0) ) = 0 and skew B(0) S[Ω] R(0) = −skew B(1) R(0) , 148

(6.20)

6.5 Earth-Fixed Trajectory and Map Estimation where B(0) = X(0) W Y(0) T and B(1) = X(0) W Y(1) T + X(1) W Y(0) T . As the true rotation matrix R(0) is unknown, a possible approximation is to use the optimal rotation computed before such that R(0) = R∗ , with which (6.20) can be used to find an expression for the rotation error Ω and its uncertainty description. It can be seen that, for the 2-D landmark case, the rotation error is given by Ω=

CΩ1 S−1 [−skew (B(1) R(0) )] := , −1 (0) (0) CΩ0 S [skew (B S R )]

where CΩ1 , CΩ0 ∈ R, and the operator S−1 (.) denotes the inverse function of S(a). Considering the summation expansion of B(1) R(0) and B(0) S R(0) , in terms of the position of the landmarks in {A} and {B} frames, and defining (1) T

B1ij = bi

(0) T

B0ij = aj

T

(1) T

ST R(0) aj + aj (0)

(0)

R(0) S bi ,

(0)

R(0) bi ,

where the identity S−1 (b aT − a bT ) = bT S a = aT ST b, for some generic vectors a, b ∈ R2 , was used for simplicity of presentation. Thus, it can be seen that XX X 1 1 1 CΩ1 = B 2 B1ii − W σ σ 2 σ 2 1ij i∈J

i

i∈J

i

j∈J

j

and CΩ0 =

X i∈J

1 B σi2 0ii

− W1

XX i∈J

j∈J

1 B σi2 σj2 0ij

.

From these expressions it is straightforward to see that the rotation error has zero mean, as hΩi = hCΩ1 i/CΩ0 = 0, and that its variance is given by Σ Ω = hΩ2 i = hCΩ21 i/CΩ20  X X X  1 1 1 1 hB B i− 1 = hB B i− 1ij 1ll W  1ii 1ll W C2 σ 2 σ 2  σ2 σ2 i,l∈J

Ω0

i

l

j∈J

j

  X   1 hB B i+ 1 . hB B i   2 1 1 1 1 W ii ij lm lm σj m 

m∈J

j∈J

Assuming that the landmarks in {A} and {B} are independent at time instant k, it can be seen that (0) T

hB1ij B1lm i = aj

T

(0) T

Σbij ST R(0) a(0) R(0) SΣ m + bi

T (0) ST R(0) Σ aij R(0) S bl .

The cross covariance terms between the rotation error and the position of landmarks, necessary for the remaining uncertainty, can be defined as   (1)  haj CΩ1 i 1 X 1  (1) 1 X 1 (1) (1) T  hal B1ii i − Σ al Ω = Σ Ωal = hal Ωi = = ha B i  1 l 2  2 ij CΩ0 CΩ0 i∈J σi  W j∈J σj and Σ bl Ω = Σ TΩbl

  (1) X X   hb C i 1 1 1 1  j Ω1 (1) (1) hb(1) B i − = hbl Ωi = = hbl B1ij i , 1ii l 2  2  CΩ0 CΩ0 i∈J σi W j∈J σj 149

Chapter 6: Sensor-based SLAM where (1)

(1)

(1) T

i ST R(0) aj + hal aj

(1)

(1)

(1) T

i ST R(0) aj + hbl aj

hal B1ij i = hal bi

T

(0)

T

(0)

(1)

(1) T

(0)

(0)

i R(0) S bi ≈ Σ alj R(0) S bi

and hbl B1ij i = hbl bi

(1)

(1) T

T

i R(0) S bi ≈ Σ bli ST R(0) aj , (0)

(0)

considering that the landmarks from different frames at time instant k are independent, as previously mentioned. Note also that these cross covariance terms only exist for the landmarks used in the optimization problem, i. e. for all i, j, l ∈ J . 6.5.1.4

Translation Uncertainty

The solution for the translation term (6.17) can be rewritten as X 1 (a − Rbi ) . p = W1 σ2 i i∈J

(6.21)

i

Substituting the error models (6.18) and (6.19) in (6.21) and neglecting second and higher order terms it can be seen that p(0) =

1 W

X i∈J

1 σi2

(0) (0) ai − R(0) bi

1 σi2

(1) (1) (0) ai − R(0) bi − S(Ω) R(0) bi .

and p(1) =

1 W

X i∈J

Thus, it is straightforward to notice that p(0) is deterministic, while p(1) has zero mean and its covariance matrix is approximately given by 1 X 1 (0) (0) T (0) (0) (0) T (0) T T Σ p = hp(1) p(1) T i ≈ 2 Σ Σ Σ + Ω S R bi bj R S , aij + R bij R W i,j∈J σi2 σj2 where the cross covariance terms between the rotation error and the landmarks were neglected. The cross covariance terms between the translation and the position of the landmarks in {A} and {B} are defined as T

1 X 1 (0) Σ ail + S R(0) bi Σ Ω al 2 W i∈J σi

T

1 X 1 (0) (0) (0) R Σ + S R b Σ , i b Ω b il l W i∈J σi2

(1) Σ pal = Σ Tal p = hp(1) al i =

and (1) Σ pbl = Σ Tbl p = hp(1) bl i =

for all l ∈ J , considering once more that Σ ai bj = 0. The cross covariance relative to the rotation error is given by Σ pΩ = Σ TΩp = hp(1) Ωi =

1 X 1 (0) (0) (0) Σ + R Σ + Σ S R b . i bi Ω Ω W i∈J σi2 ai Ω

As in the previous cross covariances, note that these only exist for the landmarks used in the optimization problem, i. e. for all i, l ∈ J . 150

6.5 Earth-Fixed Trajectory and Map Estimation

6.5.2

Trajectory and Map Estimation

This subsection formulates the underlying optimization problem of estimating, in real-time, both the vehicle trajectory and map described in the earth-fixed frame, denoted for the remaining of this chapter as the earth-fixed trajectory and map (ETM) estimation algorithm. 6.5.2.1

Problem Formulation and Solution

The fundamental idea is to use the known landmarks to formulate an optimization problem with a closed-form solution to enable the real-time estimation of the transformation from {H} to {E} at each time instant k, which is defined by the position and orientation of the vehicle in {E}, ˆ k ∈ SO(2). This can be accomplished in several ways, respectively denoted as E pˆ H ∈ R2 and EH R k

however, the computation of the resulting transformation uncertainty depends highly on the choice of the error that is minimized. The proposed algorithm resorts to the solution of the extended Procrustes problem presented in Section 6.5.1, to obtain the transformation between the sensor and earth-fixed frames at each time instant, thus yielding the vehicle trajectory in the earth-fixed frame. However, this implies that there is already a known earth-fixed map of landmarks. For the initialization of the ETM algorithm, it can be assumed that earth-fixed and sensor frames are coincident, thus yielding the same landmark map for both {E} and {H}. Assuming that the earth-fixed landmarks considered for the ETM algorithm are static and that the vehicle navigates through the environment maintaining visibility with some of the previously visible sensor-based landmarks, it is always possible to estimate the pose of the vehicle, using the Procrustes solution, and initialize any new landmark into the earth-fixed map by using the current vehicle pose estimate. Furthermore, it is also possible to update the current estimate of ˆ k are each landmark of the earth-fixed map. In general, at a given time instant k, if E pˆ H and EH R k

known, the update for each earth-fixed landmark estimate can be computed using E E E ˆ    pˆ ik = pˆ Hk + H Rk pˆ ik , ∀i∈I .   ˆ k dˆ i E dˆ i = EH R k k

(6.22)

A diagram of the overall structure of the proposed SLAM algorithm is presented in Figure 6.2. Essentially, the innovation resulting from matching the detected landmarks is used in the Kalman filter to provide the updated filter state, comprising the vehicle-related variables and the sensor-based map of the environment. Based on this map and using the previously computed earth-fixed map estimate, the proposed algorithm provides new estimates of the vehicle pose and landmarks described in the earth-fixed frame. The error function between the previous estimate of the earth-fixed position of the i-th landmark, E pˆ ik−1 , and the associated current estimate in the sensor frame, pˆ ik , rotated and translated into the earth-fixed frame, can be defined as ˆ k pˆ i − E pˆ H . eik := E pˆ ik−1 − EH R k k 151

Chapter 6: Sensor-based SLAM Laser

Landmark Altimeter Detection and JCBB

ν k , Σν k IO , IU

xˆ Vk|k

Kalman Filter

E

xˆ Mk|k

Procrustes Problem Ex ˆ

xˆ Mk|k−1

Mk−1

Sensor-based SLAM Filter

pˆ Hk

E ˆ H Rk

Earth-fixed Map

Ex ˆ

Mk

z−1

Earth-fixed Trajectory and Map Estimation (OptETM)

Figure 6.2: Diagram of the overall SLAM algorithm.

Thus, the transformation from {H} to {E} can be found by solving the optimization problem (6.15), considering the cost functional ˆ k , E pˆ H ) = G(EH R k

X i∈J

1 n σi2

2

eik

and using the set of landmarks J = {j1 , . . . , jn } ⊆ I , which are obtained from the sensor-based filter described above, choosing the least uncertain landmarks among all the state landmarks. As the closed-form solution of this optimization problem is provided in Subsection 6.5.1 by (6.16) and (6.17), the trajectory of the vehicle in the earth-fixed frame can be readily computed, ˆ k ) for all k ≥ k0 , together with the respective uncertainty characterization. denoted by (E pˆ H , EH R k

Furthermore, the earth-fixed landmark update equation, (6.22), enables the real-time estimaˆ ik }, for all i ∈ Ik . This optimization based algorithm, tion of the earth-fixed map, E xˆ Mk = {E m together with the earth-fixed map uncertainty described below is hereafter denoted by OptETM algorithm.

6.5.2.2

Alternative Trajectory Estimation

In a similar fashion to the approach presented in [CMCTN07], an alternative approach is to include the earth-fixed frame in the sensor-based SLAM filter by augmenting the state with its position and orientation, which will always be unobservable. In the SLAM formulation presented in this chapter the inclusion of the position, H pE , is trivial and can be seen as a new position landmark. As for the orientation, the state can be augmented to include an angle representing the relative orientation between the earth-fixed frame and the sensor frame, ψB , which is considered to be known at initialization. Note that the motion kinematics of this angle is given by ψ˙ B (t) = −rg (t) + br (t), where rg (t) is the rate gyro measurement. Thus, one can ˆ k = Rz (ψˆB ), and the position estimate of trivially obtain the rotation matrix estimate, using EH R k ˆ k H pˆ E . Using the sensor frame described in the earth-fixed frame, which is given by E pˆ H = −EH R k

this approach, the Kalman filter provides the estimates and respective covariance matrices for the position and rotation of the earth-fixed frame described in the body frame, which can be used to obtain the earth-fixed vehicle trajectory and map, with their respective covariances. This algorithm is hereafter denoted as AugETM. 152

6.5 Earth-Fixed Trajectory and Map Estimation 6.5.2.3

Earth-Fixed Map Uncertainty

Considering the earth-fixed landmark update (6.22), using the new earth-fixed landmark position error model defined by E

(0)

(1)

pˆ ik = E pˆ ik + E pˆ ik + O(2 ) ,

for all i ∈ I , and neglecting second and higher order terms, it is possible to write E

(0) (0) ˆ (0) ˆ (0) pˆ ik = E pˆ Hk + EH R ik , k p

E

(1) (1) E ˆ (0) (0) ˆ (0) ˆ (1) ˆ ik . pˆ ik = E pˆ Hk + EH R ik + S(Ω) H Rk p k p (1)

(0)

Thus, it can be seen that E pˆ ik is deterministic, hE pˆ ik i = 0 for all i ∈ I , and the updated covariance matrix between earth-fixed landmarks i, j ∈ I can be defined as T

(1) (1) Σ E pˆ ij = hE pˆ ik E pˆ jk i k

ˆ (0) ≈ Σ E pˆ H + EH R k Σ pˆ ij k

+ Σ E pˆ H

k

k

Ω

(0) pˆ jk

T

E H

E H

T T E (0) T T ˆ (0) ˆ (0) ˆ ˆ (0) ˆ (0) R + Σ Ω S EH R S ik p jk k k p H Rk

T T T ˆ (0) ˆ (0) ˆ (0) R S + S EH R ik Σ E pˆ k k p

Hk Ω

.

It is noted that the cross covariance terms between the landmarks and the position of the vehicle were neglected and that similar expressions can be obtained for the new landmark directions. Furthermore, each landmark estimate and respective covariance matrix are updated only if the trace of the current covariance matrix is smaller than that of the previous estimate. Nonetheless, this practical approach does not hurt neither the sensor-based SLAM filter properties nor the uncertainty computation of the earth-fixed quantities described above.

6.5.3

Algorithm Validation

As the earth-fixed trajectory and map algorithm presented in this section is intended to work for any rotation and translation, to validate the proposed technique, a Monte Carlo simulation with N = 1000 samples was performed for each of M = 500 random configurations. In each of these configurations, the rigid-body transformation is uniformly distributed in each component of the translation vector within the interval [−10, 10] m, the rotation angle in [−π, π] radians, and the locations of the landmarks are also uniformly distributed within [0, 10] m in each component. The covariance matrices used to generate the normally distributed location errors of the body and earth-fixed landmarks are respectively defined by Σ pˆ i = diag(0.01, 0.03)2 and Σ E pˆ i = diag(0.05, 0.005)2 for half the landmarks, by Σ pˆ i = diag(0.03, 0.01)2 and Σ E pˆ i = diag(0.05, 0.05)2 for the remaining half, and setting the cross covariance between landmarks to zero. The performance of the algorithm was tested for different number of landmarks, n ∈ {5, 7, 10, 15, 20, 30, 50}, and the summary of the results can be found in Tables 6.1 and 6.2. 153

Chapter 6: Sensor-based SLAM Table 6.1: Performance of the algorithm in terms of the distance to the true values (mean ± std).

n 2 5 7 10 15 20 30 50

d(EH R) [deg] d(E pH ) [%] avg[d(E pˆ i )] [%] -0.444 ± 7.04 12.8 ± 13.6 7.2 ± 7.9 -0.378 ± 3.84 6.4 ± 9.3 5.6 ± 7.9 -0.027 ± 3.36 5.9 ± 9.1 5.5 ± 7.6 0.035 ± 2.92 4.7 ± 4.9 5.3 ± 7.5 0.115 ± 1.78 3.6 ± 4.3 5.1 ± 9.1 0.028 ± 1.52 3.3 ± 4.0 4.7 ± 7.8 -0.032 ± 1.21 2.7 ± 3.7 4.7 ± 9.3 0.031 ± 0.94 2.2 ± 4.1 4.3 ± 6.5

The mean and standard deviation of the differences between the results obtained using the proposed OptETM algorithm and the true values is shown in Table 6.1. The absolute angular difference between the rotation matrices EH Rtrue and EH Ropt is defined as d(EH R) := S−1 [log(EH Ropt T EH Rtrue )] , where the superscript opt is used to denote the quantities obtained using the proposed OptETM algorithm and the uncertainty computations derived above, whereas the superscript true denotes the respective true values. The relative difference for the vehicle position can be defined as

opt

E true

d(E pH ) := 100

E pH − E ptrue / p H H and a similar expression can be applied to obtain the relative difference for the positions of the earth-fixed landmarks, d(E pˆ i ). For this latter relative difference, an average over the number of landmarks n, denoted as avg[.], is considered in order to obtain a comparable scalar value. The meaningful comparison between two different models, usually denoted as the null model and the alternate model, is a difficult problem which is frequently performed using likelihood ratio tests. In short, the likelihood ratio expresses how many times more likely the data is explained under one model than the other, by obtaining the probability distribution of the test, assuming the null model to be true (null hypothesis). Thus, the null hypothesis is rejected if its probability is below a desired significance value, typically 0.01 or 0.05. However, a central limitation for the usage of these tests is the assumption that the stochastic variable under analysis is Gaussian distributed. Indeed, and as a consequence of the considered arbitrary rotations, the vehicle and landmark positions in the earth-fixed frame tend to be Gaussian distributed as the number of landmarks increases, but fail to be so for a reduced number of landmarks. Figure 6.3 depicts the results obtained for different numbers of landmarks, using a configuration of landmarks inline in the x-axis, no translation, and a 180 deg rotation. It can be seen that for n smaller than 5, the 95% confidence bound of the Monte Carlo simulation histogram is far from an ellipse. This nonlinearity affecting the results is intrinsic to the problem of translating and rotating a map arbitrarily between coordinate frames, and is also found in EKF-based 154

6.5 Earth-Fixed Trajectory and Map Estimation 0.6

1.5

0.8

1

0.6 0.6

0.8

0.6

0.4

1

0.6

0.4

0.4

0.2

0.2

0

0

0.4

0.2

0.4 0.5

0.2 y [m]

0.2

0

0

0

−0.2

−0.2

−0.2

0

−0.2

−0.2

−0.4

−0.4

−0.6

−0.6

−0.4

−0.5

−0.4

−0.4

−0.6

−0.6

−1

−0.8

−0.6 −0.8

−1

−0 0..02 02

(e) n = 6

(f) n = 7

.0 0. 4 02

(d) n = 5

.0 6 0. 04

05

.1 −0

(b) n = 3

−0

(a) n = 2

0.

0

2

0

−0 .

−0.4 −0.2 x [m]

−0

−1.5

(c) n = 4

Figure 6.3: Translation uncertainty 95% confidence bounds for a difficult case: Monte Carlo simulation histogram (green dashed line) and covariance matrix resulting from the optimization algorithm and respective uncertainty approximation (red dash-dotted line). Table 6.2: Covariance Likelihood Ratio Tests.

n HΩ [%] H p [%] HE pˆ i [%] dλmin (E pˆ i ) 5 7 10 15 20 30 50

79 86 87 93 94 96 99

47 55 60 79 81 90 96

0.3 1.5 4.0 12.1 16.0 32.4 45.0

19.9 ± 30.3 7.6 ± 25.8 2.1 ± 24.1 0.5 ± 17.3 1.0 ± 19.3 -0.1 ± 14.5 -0.0 ± 13.6

dλmax (E pˆ i ) HΩ100 [%] H p100 [%] HE100 [%] pˆ i

37.9 ± 10.6 31.4 ± 8.1 24.9 ± 6.7 18.4 ± 5.5 15.2 ± 6.9 10.1 ± 4.9 6.4 ± 4.7

95 96 97 99 98 99 99

91 91 94 96 98 98 99

38 60 72 83 85 89 91

SLAM algorithms. By proposing this algorithm, it is argued that it may be possible to obtain less uncertain, yet consistent, earth-fixed trajectory and landmark map. With this approach, all the control, decision, and loop-closing procedures can be made in the sensor-based frame, minimizing the effects of nonlinearities in the consistency of the filter, whereas the earth-fixed trajectory and map estimation is used for completeness of results, noting that it is usually possible to have n always larger than 10. Table 6.2 summarizes the likelihood ratio tests for the comparison between the sample sim sim covariance matrices resulting from the Monte Carlo simulations, Σ sim Ω , Σ E p , and Σ E pˆ , and H

i

the covariance matrices resulting from the OptETM algorithm and respective uncertainty opt

opt

opt

approximation, Σ Ω , Σ E p , and Σ E pˆ . The values shown in the table for these tests denote the H

i

155

Chapter 6: Sensor-based SLAM percentage of successful tests among the M different configurations. For the scalar case of the opt

null hypothesis HΩ : Σ sim Ω = Σ Ω , the ratio is defined as ΣΩ ∼ χN2 ∗ N ∗ Σ sim Ω /Σ opt

which is asymptotically χ2 distributed with N ∗ := N − 1 degrees of freedom and, thus, can be compared with a predefined significance level, which in the presented results is considered to opt

opt

sim be 0.01. For the multivariable cases of the null hypotheses H p : Σ E p = Σ sim E p and HE pˆ i : Σ E pˆ = Σ E pˆ , H

H

i

i

for all i ∈ J , the specific likelihood ratio test can be found in [And84, Section 10.8]. The likelihood ratio for H p is defined as e λ1 = N∗ ∗

21 p N ∗ 1 N ∗ − 1 trBsim Σ opt −1 opt −1 2 Ep Ep 2 Bsim H H , E p ΣE p e H H

sim ∗ where Bsim E p = N Σ E p , and the comparison with the significance level is achieved by noting that H

H

−2 log λ∗1 is asymptotically χ2 distributed with q = 1/2 p (p + 1) degrees of freedom, for E pH ∈ Rp (in the presented case, p = 2 and q = 3). For the hypothesis HE pˆ i , for all i ∈ J , a similar likelihood ratio test can be easily inferred from the previous case. In addition to these tests, Table 6.2 also provides the maximum and minimum eigenvalue relative differences between the earth-fixed landmark position covariance matrices, which are defined as opt

E

dλmax ( pˆ i ) =

ΣE pˆ ) − λmax (Σ Σsim λmax (Σ E pˆ ) i

i

Σsim λmax (Σ E pˆ )

100

i

and proceeding in a similar fashion for the minimum eigenvalue relative difference. These values are presented in the form mean±std. As mentioned before, the Gaussian assumption may not be valid when dealing with an arbitrary rotation EH R, which together with the fact that the ratio expressions and the degrees of freedom of their approximate distributions depend on the number of Monte Carlo samples, the tests are expected to fail more pronouncedly when the number of Monte Carlo samples increase. For this reason an additional set of likelihood ratio tests are presented using a smaller number of samples, N = 100, and denoted as H∗100 . From the values shown in these tables, it can be concluded that the optimization algorithm yields accurate estimates of the earth-fixed map and vehicle trajectory, even with just a few landmarks available. In terms of the uncertainty characterization, it can be seen that for more than 10 landmarks the obtained covariance matrices are a good approximation. Nonetheless, the uncertainty values seem to be conservative for a small number of landmarks, especially the covariance of the earth-fixed map landmarks, as it is possible to observe in Table 6.2 by the larger eigenvalue differences of the algorithm covariance estimates relative to the Monte Carlo simulation covariance matrix estimates.

6.6


This section describes the experimental setup and presents the results for the proposed sensorbased SLAM algorithm, using position-only landmarks for simplicity of presentation. These 156

6.6 Experimental Results results intend to demonstrate the convergence, performance, and consistency properties of the overall SLAM algorithm, both in the sensor-based and earth-fixed frames. Recalling the diagram of the overall structure of the proposed SLAM algorithm presented in Figure 6.2, note that the laser measurements are used to detect the visible landmarks and associate them with the predicted state landmarks using the JCBB algorithm. The resulting innovation, which may also include the measured altitude information, is used in the Kalman filter to provide the updated filter state, comprising the vehicle-related variables and the sensor-based map of the environment. Based on this map and using the previous earth-fixed map estimate, the algorithm provides new estimates of the vehicle pose and landmarks described in the earth-fixed frame. An instrumented quadrotor was hand-driven along a path of about 60 meters in an indoor environment with a loop, as shown in Figure 6.4, at an average speed of 0.4 m/s. The trajectory described by the vehicle starts near the middle and circulates counterclockwise until some of the first landmarks detected are once again visible, at the lower right corner. This custom built quadrotor UAV, property of the ISR, is equipped with a MEMSENS nanoIMU, a Maxbotix XL sonar for altitude measurements, and a Hokuyo UTM-30LX laser scanning device that provides horizontal profiles of the surroundings. For completeness, the altitude of the vehicle, z(t) and its vertical velocity, w(t) are also included in the system dynamics, enabling a 3-D trajectory estimation without affecting any of the properties of the proposed sensor-based SLAM filter. Thus, the new vehicle-related h iT state vector is defined as xV (t) = z(t) vT (t) w(t) br (t) . The covariance matrix for the zero-mean white Gaussian disturbance affecting the vehicle-related state variables is defined as ΞV = Ts diag σz2 , σv2 I3 , σb2r , with σz = 0.04 m, σv = 0.1 m/s, σbr = 5.7 × 10−5 deg/s, and the covariance matrix of the disturbance affecting each landmark position is given by Ξ pi = Ts σp2 I2 , σp = 0.05 m, for all i ∈ Ik . The altitude measurement is also considered to be corrupted by zero-mean Gaussian noise with variance Θz = σθ2z , σθz = 0.03 m, whereas the landmark position measurement noise covariance matrix is given as a function of ρim and αim , which are the correspondent range and bearing coordinates of the detected landmark pim , and also function of the angular and range standard deviation values, σα = 3.75 deg, σρ1 = 0.15 m, and σρ2 = 0.25 m. Thus, the landmark position measurement noise covariance matrix is defined as Θ pim = σρ2i Mαim + σα2 m

d Mαim dα

assuming a small angle approximation such that Mα = Rα diag(1, 0) RTα , where Rα is the 2-D rotation matrix obtained from an angle α. The range measurement noise of the laser sensor depends on the distance to the detected target, yielding σρim = σρ1 if ρim ≤ 10 m and σρim = σρ2 if ρim > 10 m. It is noted that the above implicit linearization, always performed at the measured values, is just a way to properly approximate the sensor noise and does not affect any of the SLAM filter properties. 157


−12

−10

−8

−6

y [m]

−4

−2

0

2

4 Trajectory Laser profile Curr. LMs Old LMs Gap LMs

6

−10

−8

−6

−4

−2

0

2

4

6

x [m]

Figure 6.4: Map and trajectory in the earth-fixed reference frame, obtained using an optimization-based trajectory and map estimation algorithm, with the floor blueprint in the background.

158


6.6.1

Filter Results

When analyzing the convergence properties of any navigation filter, one of the main goals is to observe a decreasing uncertainty in all variables. This can be seen in Figure 6.5(a), where the uncertainty of all the vehicle-related variables decreases over time. Furthermore, it can be seen that the uncertainty of each landmark decreases whenever it is visible and increases otherwise, as shown in Figure 6.5(b). The proposed SLAM filter also provides estimates of the linear motion quantities of the vehicle, shown in Figure 6.6, along with the remaining vehicle-related variables, without resorting to standard odometry sensors, which are not available for aerial vehicles. These results also show that the constant velocity assumption can be relaxed with the appropriate choice of the filter parameters, in particular σv . Although there is an underlying trade-off, as the velocity uncertainty will also propagate to the uncertainty of the non-visible landmarks, the presented results indicate that the filter was able to cope with acceleration peaks of 4 m/s2 , without introducing too much conservativeness in the uncertainty of the non-visible landmarks. A basic measure of consistency of a SLAM algorithm, when ground truth data is not available, is the maximum normalized innovation square (NIS) value at each time instant, considering all the landmark associations. As the NIS value for each landmark association is χ2 distributed with nm = dim(mi ) degrees of freedom, it can be compared, for instance, with the 95% confidence threshold such that NISik = ν ik Σ νi ν ik ≤ χn2m ,95% . k

This comparison is presented in Figure 6.7, along with an average value, where it can be seen that the maximum value seldomly approaches the threshold and it is mostly concentrated below χn2m = 2, which might indicate some degree of conservativeness of the filter parameters. Figure 6.8 provides the evolution of the number of landmarks: i) in the state vector; ii) being visible; iii) being used for the optimization problem of the online earth-fixed trajectory and map estimation; and also iv) the number of landmark associations being considered for loop closure, including the minimum threshold to enable the loop closure procedure. From the number of state landmarks, it can be observed that a basic landmark management is done to avoid spurious landmarks due to bad detection. The criteria to delete a landmark is: a) not visible for a long time while it should be in the field of view; b) intermittently visible in the past, less than an acceptable number of times; and c) maximum number of landmarks supported is reached, in which case the non-visible landmark with the highest uncertainty is deleted. This last case is highly undesirable, as it implies the loss of important information, and it does not occur in the experiments provided in this chapter. Recalling the conditions of Theorem 6.3, the number of visible landmarks is always above two, with some exceptions where there 159


0.2

σz [m] σu [m/s]

0.18

σ [m/s] v

0.16

σw [m/s]

0.14

σbr [deg/s]

σ

0.12 0.1 0.08 0.06 0.04 0.02 0

5

10 time [s]

15

20

(a) Standard deviation of the vehicle-related variables: altitude, linear velocity and gyro rate bias.

0.8

i

x

σ [m]

0.6 0.4 0.2 0

50

100 time [s]

150

200

0.1

i

σx [m]

0.15

0.05 0

5

10

15

20

time [s] (b) Standard Deviation of the first 15 landmark positions to be visible in the sensor-based frame (x-coordinate only). Top: complete time evolution; Bottom: first 25 seconds.

Figure 6.5: Uncertainty convergence of the sensor-based SLAM filter variables.

160

v [m]

u [m]


0.6 0.4 0.2 0 −0.2 −0.4 0

50

100 time [s]

150

200

0.4 0.2 0 −0.2 −0.4 0

50

100 time [s]

150

200

50

100 time [s]

150

200

w [m]

0.2 0 −0.2 0

(a) Linear Velocity

br [deg/s]

0.2 0 −0.2 0

50

100 time [s]

150

200

50

100 time [s]

150

200

0.6

z [m]

0.55 0.5 0.45 0.4 0.35 0

(b) Rate Gyro Bias (top) and Altitude (bottom)

Figure 6.6: Time evolution of the vehicle-related state variables (solid red), with 2 σ bounds (dash-dotted black).

161


6

5

χ2 NIS

4

3

95% conf max avg

2

1

0

1150

1200

1250 time [s]

1300

Figure 6.7: NIS association: maximum (red), average (blue) and 95% confidence threshold (dashed-dotted black).

was only one visible landmark while the vehicle was moving. Although this is an intrinsic characteristic of the surrounding environment, provided a landmark detection algorithm and respective choice of parameters, it shows that the conditions for observability and convergence of the filter are satisfied in the presented real-world experiments. In addition, it is noted that the number of landmarks used in the optimization problem is consistently greater than 5, with just one exception around t = 185 s.

6.6.2

Localization, Mapping, and Consistency

The outcome of a SLAM algorithm is twofold: i) self-localization; and ii) map construction of the environment. In sensor-based SLAM, the vehicle is deterministically localized at the origin and aligned with the sensor frame. As for the mapping, the proposed SLAM filter provides at each instant a map of the environment in the sensor frame, with consistent uncertainty estimates. Nevertheless, Section 6.5 proposes and validates an optimization-based real-time earth-fixed trajectory and map estimation algorithm (OptETM), and also describes the traditional approach of augmenting the sensor-based SLAM filter with the pose of the earth-fixed frame, the AugETM algorithm. Using either one of these methods, the trajectory of the vehicle and a consistent map in the earth-fixed frame can be obtained. The sensor-based map and both the earth-fixed maps computed using the OptETM and AugETM algorithms are presented in Figures 6.9 and 6.10, featuring the final results of the experimental trial. The landmarks and their respective 95% 162


# of landmarks

150

100

in state visible in opt

50

0 0

50

100 time [s]

150

200

100 time [s]

150

200

# of landmarks

30

20

10

0 0

visible in opt loop min loop 50

Figure 6.8: Time evolution of the number of landmarks. Top: number of landmarks in the filter state (solid red), the number of visible landmarks (dash-dotted green), noting that the latter is always less than the former, and the number of landmarks used in optimization-based earth-fixed trajectory and map estimation (dotted dark blue); Bottom: excludes the number of landmarks in the state to provide better detail on the other and shows the number of landmark associations available for loop closure (magenta) and minimum loop closing threshold (dashed black horizontal line).

163


−10

y [m]

−5

0

5

Trajectory Laser profile Curr. LMs Old LMs Gap LMs

10

−10

−5

0

5

x [m] Figure 6.9: Map and trajectory in the sensor-based frame, {H}. This figure shows the current laser profile (in gray), the landmarks in Icur (in magenta crosses), as well as the landmarks in Iold and Igap (respectively, in light-blue squares and yellow diamonds) along with their 95% confidence bounds. The earth-fixed trajectory of the vehicle obtained using OptETM is also shown for completeness (in dashed green).

164


−10

−8

−6

−4

y [m]

−2

0

2

4

6 Trajectory Laser profile Curr. LMs Old LMs Gap LMs

8

−10

−8

−6

−4

−2 x [m]

0

2

4

6

(a) Earth-fixed frame, {E}, using OptETM algorithm.

−10

y [m]

−5

0

5


10 −12

−10

−8

−6

−4

−2

0

2

4

6

x [m]

(b) Earth-fixed frame, {E}, using AugETM algorithm.

Figure 6.10: Maps and trajectories in the earth-fixed frame, {E}.

165


confidence (or 2 σ ) bounds are shown in three different styles (solid magenta, dashed yellow, and dash-dotted light blue), denoting respectively the three sets of landmarks used for the loop closing step, Icur , Igap , and Iold . For the earth-fixed maps, the trajectory of the vehicle and the respective 95% confidence bounds are also shown in dash-dotted green for OptETM and in dashed dark blue for AugETM, whereas the laser sensor readings are shown in solid light gray. For completeness, the earth-fixed reference frame, which was added to the sensor-based SLAM filter as a non-visible landmark, is shown in the sensor-based map (axes shown in red and green, position and uncertainty in dark blue). This additional non-visible landmark does not interfere with the observability or convergence analysis, as its state is not used by any other state or measurement equation. The main aspects to retain from these intricate figures are the quality of the both sensor-based and earth-fixed maps generated by the algorithm and the smaller landmark uncertainty of the earth-fixed map generated using the OptITM, when compared with the one generated using the AugITM approach. In the sensor-based framework, as there is no vehicle localization uncertainty and the full map rotates and translates when the vehicle moves, the uncertainty of the non-visible landmarks increases with time, thus enabling consistent estimation of the sensor-based map. Conversely, in the earth-fixed map the landmarks are assumed static and, therefore, the landmark position uncertainty is always non-increasing, whereas the vehicle pose uncertainty may increase indefinitely unless a loop closure scenario occurs. The time evolution of the orientation and position uncertainties, using both OptETM and AugETM approaches, are presented in Figure 6.11. It can be seen that the pose uncertainty computed with AugETM increases even when the vehicle is immobilized (first 50 seconds), whereas the pose uncertainty computed by the OptETM algorithm remains constant. The standard state augmentation method is based on the open-loop integration of the state of the sensor-based filter, noting that the velocity disturbance noise, necessary for the consistency of the sensor-based filter, is also integrated to obtain the earth-fixed frame position. Conversely, the OptETM algorithm uses the best landmark position estimates in both {B} and {E} frames to compute new estimates of the earth-fixed map and vehicle pose. The result is notorious when comparing the earth-fixed landmark uncertainties arising from each method in Figures 6.10(a) and 6.10(b). The heading of the vehicle is also shown in Figure 6.12, where it can be seen that both transformations provide coherent results and, notably, that the effects of the magnetic distortions inside buildings can be devastating. Note that the heading solution provided by the external attitude filter is utterly wrong, which is a consequence of relying on the IMU magnetometer readings while indoors. The practical validation of the consistency of SLAM algorithms occurs in a scenario where 166


4.5 4

ψaug ψ

opt

3.5

σψ [deg]

3 2.5 2 1.5 1 0.5 0 0

50

100 time [s]

150

200

150

200

(a) Heading Uncertainty

0.8 0.7

xaug xopt

0.6

yopt

0.4

σ

LP

pI

[m]

0.5

yaug

0.3 0.2 0.1 0 0

50

100 time [s] (b) Position Uncertainty

Figure 6.11: Vehicle pose uncertainty in the earth-fixed frame, using the augmented state approach AugETM (in solid light blue and dotted dark blue) and using the optimization-based approach OptETM (in dashed green and dash-dotted yellow).

167


150

mag aug opt

heading [deg]

100 50 0 −50 −100 −150 0

50

100 time [s]

150

200

Figure 6.12: Heading of the vehicle relative to the earth-fixed frame, with 2σ confidence bounds, provided by: an external attitude filter based on magnetometer readings (solid red); the augmented state approach AugETM (dashed dark blue); and the optimization approach OptETM (dash-dotted green).

the vehicle explores new terrain and at some point returns to a previously visited area, possibly from a different perspective than before. In the experimental results presented in this chapter, a loop is closed only three times, the first of which occurs at t = 193.4s, as depicted in Figure 6.8. When the vehicle turns at each corner of the building, some older landmarks are observed again, yielding a similar effect to that of a loop closure, but is not considered here as such. To be consistent, the filter uncertainty must allow the association of the currently visible landmarks and the ones observed when the same area was previously visited. As a consequence of this association, and subsequent update of the state variables, the pose and associated landmarks position uncertainties are reduced to values similar to those when the area was first visited, see Figure 6.11(a). The moments just before and right after the loop closure are captured in Figures 6.13 and 6.14, where a detailed version of the maps of Figures 6.9 and 6.10(a) is presented. The landmark associations between the sets Icur and Iold are shown in black and the fused landmarks positions and uncertainty bounds obtained after the loop closure are also depicted in black. The successive loop closures that occur in this experiment lead to the consistent correction of the environment map, in particular those landmarks closer to the current position of the vehicle. Although the revisited landmarks suffer a great decrease in uncertainty and their position is corrected, it can also be observed in Figure 6.9 that the position of the nearby non-visible 168


−2

−1.5

−1

y [m]

−0.5

0

0.5

1 Trajectory Laser profile Curr. LMs Old LMs Loop LMs

1.5

2 −1

−0.5

0

0.5

1

1.5 x [m]

2

2.5

3

3.5

4

(a) Before loop closure.

−2

−1.5

−1

y [m]

−0.5

0

0.5

1 Trajectory Laser profile Curr. LMs Old LMs Loop LMs

1.5

2 −1

−0.5

0

0.5

1

1.5 x [m]

2

2.5

3

3.5

4

(b) After loop closure.

Figure 6.13: Detailed view of the map and trajectory in {H}, showing the 95% confidence bounds of landmarks in Icur (in magenta), 95% confidence bounds of landmarks in Iold that are being considered for loop closing (in light-blue), and the landmark loop closing associations and resulting 95% confidence bounds (in black). The sensor-based trajectory of the vehicle is also shown for completeness (in dashed green).

169

−2

−2

−1.5

−1.5

−1

−1

−0.5

−0.5

0

0

y [m]

y [m]


0.5

0.5

1

1

1.5

1.5

2

2

2.5

2.5 Trajectory Laser profile Curr. LMs Old LMs Gap LMs

3

3

3.5

4

4.5 x [m]

5

5.5

(a) Before loop closure.


3

6

3

3.5

4

4.5 x [m]

5

5.5

6

(b) After loop closure.

Figure 6.14: Detailed view of the map and trajectory in {E}, using OptETM algorithm.

landmarks is also corrected accordingly. This is a consequence of the cross correlation terms in the state covariance matrix, which are responsible for the propagation of recent innovations to the non-visible landmarks. It is also noted that for loop closing purposes, only the information provided by the sensor-based filter is used. Nevertheless, the results indicate that the earth-fixed map provided by the proposed OptETM algorithm is also consistent.

6.7

Concluding Remarks

This chapter presented the design, analysis, and experimental performance evaluation of a novel GAS sensor-based SLAM filter with real-time earth-fixed trajectory and map estimation, using a closed-form, computationally efficient, and numerically robust optimization-based algorithm. As opposed to traditional EKF-SLAM algorithms, the proposed method avoids the attitude representation of the vehicle in the filter state, enabling the modification of the nominal nonlinear dynamics into a structure that can be regarded as LTV for analysis and filter design and ultimately, the error dynamics result GAS. The filter does not rely on any odometry sensor, but rather on angular rate measurements, having the byproduct of estimating the linear velocity and the rate gyro bias in the vehicle frame. Building on the sensor-based map provided by the filter, the earth-fixed trajectory and map estimation algorithm computes the vehicle attitude, position, and the environment map 170

6.7 Concluding Remarks described in the earth-fixed frame. This algorithm resorts to the optimal closed-form solution of the problem of estimating the body to earth transformation from the landmark position estimates in both frames, which is ideal for real-time applications. Furthermore, an improved uncertainty characterization of the earth-fixed trajectory and map is proposed and validated through extensive Monte Carlo simulations. The performance and consistency of the proposed SLAM filter and the earth-fixed trajectory and map estimation algorithm are validated in a real world indoors environment. The sensorbased filter shows the reduction of the uncertainty in every state variable but the non-visible landmarks and produces a consistent map that is used to close a 60 meter long loop. On the other hand, the online earth-fixed trajectory and map estimation algorithm provides accurate and consistent estimates of the earth-fixed variables. Future work includes the implementation of a more general loop closure method such as the division of the total map into submaps, the real-time optimized implementation and testing of the algorithm in UAVs, and the comparison in terms of performance and consistency of the proposed method with the state-of-the-art EKF-SLAM algorithms. Generalizing these results for bearing only or range only measurements as well as the fusion of accelerometer measurements are also great topics for further research. Concerning the earth-fixed trajectory and map estimation algorithm, further effort can be put into a better statistical characterization of the estimated quantities, notwithstanding the fact that the proposed statistics seem to provide already a very good approximation.

171

7 Conclusions and Future Work This dissertation addressed several problems within the scope of control and localization of autonomous vehicles, as well as the modeling of helicopters and their equilibrium trajectories. Although for each of these problems the detailed concluding remarks were provided at the end of each chapter, the main conclusions are summarized below, bearing in mind a global perspective.

7.1

Conclusions

Chapter 3 presented a NMPC-based strategy for the control of complex vehicles, with application to the terrain avoidance control of autonomous rotorcraft and the trajectory tracking control of ASCs with constant ocean current disturbances. Beyond the accuracy and performance validation of this strategy for terrain avoidance, a comparative study of several standard optimization algorithms was also presented, showing that the quasi-Newton method with line search drastically reduces the number of iterations and CPU time relative to the standard gradient method with fixed-step. Regarding the application to ASC, where a natural trajectory tracking error space is defined and used within the design of the controller, the simulation results show that the presented solution can steer the vehicle along a demanding reference trajectory under the presence of constant currents, and the computation times observed in the simulation also indicate that this methodology is well suited for real time implementation. This is confirmed by the experimental sea trials results of real-time control of an ASC, thus validating the proposed NMPC strategy. Chapter 4 introduced the design and performance evaluation of two 3-D trajectory tracking controllers for autonomous helicopters, considering that an absolute position solution is available for control and, alternatively, a sensor-based control approach. Resorting to an H2 controller design methodology for affine parameter-dependent systems, the proposed technique exploited an error vector that naturally describes the particular dynamic characteristics of 173

Chapter 7: Conclusions and Future Work the helicopter for a suitable flight envelope. For a given set of operating regions, a nonlinear controller was synthesized and implemented under the scope of gain-scheduling control theory, using a piecewise affine parameter-dependent model representation. The effectiveness of the proposed control laws was assessed in simulation environment with a nonlinear model of the helicopter and realistic mission scenarios. The quality of the presented results clearly indicates that the proposed methodologies are well suited to be used for automatic inspection of large infrastructures. Chapter 5 formulated and solved the problem of LADAR calibration by proposing a comparative calibration algorithm and an automatic calibration algorithm, resorting to numerical optimization methods to estimate the attitude mounting bias rotation matrix. The performance and limitations of the comparative and automatic calibration algorithms, as well as the different optimization approaches, were extensively tested in simulation environment and with experimental data, showing that all methods are able to find accurate estimates for the LADAR calibration problem. The optimization on SO(3) with the closed-form step size computation displays immunity to local minima, smaller estimation errors, and faster average computation time. Additionally, the accuracy of the estimated parameters using the automatic calibration is shown to increase with the number of planes used to describe surface approximation of each cloud of laser measured points, as well as with the number of points of each cloud used for calibration. Finally, Chapter 6 proposed a novel sensor-based approach for the SLAM estimation problem. This algorithm features a sensor-based Kalman filter, which is shown to have globally asymptotically stable error dynamics, and a real-time earth-fixed trajectory and map estimation, using a closed-form, computationally efficient, and numerically robust optimization-based algorithm. The performance and consistency of the proposed SLAM filter and of the earth-fixed trajectory and map estimation algorithm are validated in a real world indoors environment. The sensor-based filter shows the reduction of the uncertainty in every state variable but the non-visible landmarks while producing a consistent map that is used to close a 60 meter long loop. On the other hand, the algorithm also provides an accurate estimate of the earth-fixed trajectory and map.

7.2

Directions for Future Work

In each of the presented methods, future work considerations were given concerning some of the aspects that can be improved within the algorithms or regarding the approach itself. Conversely, this section intends to provide major research directions that may not be as directly related to the algorithms or even to the specific approaches used. Terrain Avoidance NMPC:

The experimental results for the NMPC control of ASCs, which is

a less demanding platform in terms of actuation requirements than an autonomous helicopter, 174

7.2 Directions for Future Work together with the constant technological advancements in terms of processing capability, indicate that the real-time implementation of the terrain avoidance NMPC is a reasonable direction for future work. As noted in [ADHK04], for some vehicles the use of trajectory tracking may impose performance bounds on the controlled system. As some of these bounds are not present when using path-following, and bearing in mind mission scenarios with no trajectory time-critical requirements, a direction for future research is the formulation of a path-following error space, as well as the possibility of switching between the two modes online. Sensor-based control: The use of alternative controller synthesis is one direction of future work, as this chapter is relatively independent of the control synthesis strategy used. As in the formulation of the sensor-based kinematics some geometric constraints were assumed, one interesting direction of research is the generalization of the formulation of the laser kinematics for generic bridge piers and ground level terrain, either by changing the detection algorithm or the kinematics itself. The combination of both the absolute position and the sensor-based control approaches into an integrated controller, guaranteeing a smooth and stabilizing transition between them, is also a very promising research direction. One further direction of research is the formulation of the problems tackled in Chapters 3 and 4 in a unified framework, allowing a vehicle to track a desired trajectory coherently and as precisely as possible, while avoiding unforeseen conflicts with terrain and obstacles. LADAR Calibration: One major direction of research is the convergence analysis of the Riemannian optimization algorithm, which is made difficult by the discontinuous nature of the optimization problem, nonetheless, under mild assumptions, the SO(3) manifold demonstrates extremely useful properties that may contribute to this end. By considering new and alternative ways of formulating the error distances between the several clouds of points, the underlying optimization problem might also become easier to tackle with respect to the convergence analysis. Another direction of research is the characterization of the necessary conditions regarding the terrain and trajectory topology to ensure the feasibility of the calibration procedure, so that the identifiability of the unknown calibration parameters is guaranteed, and also which are the performance bounds of the proposed estimator. Sensor-based SLAM: The proposed SLAM algorithm paves the way for several generalizations and applications to related problems, including: i) the generalization of the filter for full 3-D environments, for which new necessary and sufficient observability conditions and convergence have to be addressed; ii) the fusion of acceleration, GPS, and landmark information while avoiding the use of an EKF, so that the GAS convergence properties may be preserved; and iii) the use of the proposed SLAM algorithm for 2-D and altitude, reformulated in such a way that it admits the reconstruction or mapping of 3-D structures by combining and fusing environment information from different altitudes. The earth-fixed trajectory and map estimation algorithm 175

Chapter 7: Conclusions and Future Work can also be further improved by its straightforward generalization to 3-D, by considering also directions within the formulation of the optimization problem, and by improving the statistical characterization of the obtained solutions.

176

A Helicopter Model Derivatives The dynamic equations of the helicopter nonlinear model, defined in (2.1), can be denoted in compact notation as x˙ B = fB (xB , uB )

where, for the simplified helicopter model considering first orther blade pitching dynamics, the state and input vectors are defined as

   vB   ω B    xB = E pB     λ B    θ1

and

   θ0    θ  uB =  c1c  , θc1s    θ0t

whereas the model function is given by

  ω B )vB + m−1 fext (xB , uB ) + gB (λ λB ) −S(ω   −I−1 [S(ω ω B )IB ω B ) + next (xB , uB )]   B   E  . λ B ) vB fB (xB , uB ) =  B R (λ    λB ) ω B Q (λ    fθ1 (xB , uB ) Given the state equation above, the objective of this section is to compute the partial derivatives

∂ fB (xB , uB ) T

∂xB

and

∂ fB (xB , uB ) ∂uTB

.

177

Chapter A: Helicopter Model Derivatives

A.1

Rigid-body Dynamics

Considering the rigid-body dynamics, the partial derivatives of the helicopter model can be rewriten as  ∂ v˙ B ∂ v˙ B  ∂vT ω TB ∂ω   ∂ ω˙ BB ∂ ω˙ B   ∂vTB ω TB ∂ω ∂ fB (xB , uB )  E λB ) 03×3 =  B R (λ  ∂xTB   03×3 Q (λ λB )   ∂ θ˙ ∂ θ˙ 1 1  T T ωB ∂ω

∂vB

03×3

∂ v˙ B λTB ∂λ

03×3

03×3

03×3 03×3 02×3

∂ E p˙ B λTB ∂λ ∂ λ˙ B λTB ∂λ

02×3

∂ vB θ T1 ∂θ ωB ∂ω θ T1 ∂θ

03×2 03×2 ∂ θ˙ 1 θ T1 ∂θ

              

and

 ∂ v˙ B   ∂uTB  ∂ ω˙ B  T ∂ fB (xB , uB )  ∂uB =   03×4 ∂uTB   03×4  ˙  ∂ θ 1 T ∂uB

where ∂ v˙ B ∂vTB

=

∂ ∂vTB

ω B ) vB + m−1 fext (xB , uB ) −S(ω

ω B ) + m−1 = −S(ω ∂ v˙ B ∂ = T ωB ω TB ∂ω ∂ω ∂ = ω TB ∂ω

∂ fext (xB , uB ) ∂vTB

,

ω B )vB + m−1 fext (xB , uB ) −S(ω ω B + m−1 fext (xB , uB ) S(vB )ω

= S(vB ) + m−1

∂ fext (xB , uB ) , ω TB ∂ω

λB ) ∂ v˙ B ∂ gB (λ = T λB λTB ∂λ ∂λ λ B ) gE ∂ E RT (λ = B λTB ∂λ λB ) gE )Q−1 (λ λB ) , = S(EB RT (λ h iT with gE = 0 0 g and g is defined in Table 2.1, whereas ∂ v˙ B ∂uTB ∂ ω˙ B ∂vTB

∂ ω˙ B ω TB ∂ω

178

= m−1

∂ fext (xB , uB ) ∂uTB

,

∂ −1 −1 ω −I (S(ω )I ω ) + I n (x , u ) B B B ext B B B B ∂vTB ∂ next (xB , uB ) = I−1 , B ∂vTB ∂ −1 −1 ω = −I (S(ω )I ω ) + I n (x , u ) B B B ext B B B B ω TB ∂ω ! ∂ next (xB , uB ) −1 ∂ (IB ω B ) × ω B = IB + ω TB ω TB ∂ω ∂ω ! ∂ next (xB , uB ) −1 ω B ) IB + , = IB S(IB ω B ) − S(ω ω TB ∂ω =

            

A.1 Rigid-body Dynamics noting that ω ∂ (IB ω B ) × ω B ∂ IB ω B ∂ω ω B ) TB = × ω B + IB S(ω T T ωB ωB ωB ∂ω ∂ω ∂ω ω ∂ω ∂I ω ω B ) TB ω B ) B T B + IB S(ω = −S(ω ωB ωB ∂ω ∂ω ω B ) IB + S(IB ω B ) , = −S(ω and also ∂ ω˙ B

= I−1 B

∂ next (xB , uB )

∂uTB ∂uTB ∂ v˙ B ∂ fext (xB , uB ) = , θ T1 θ T1 ∂θ ∂θ ∂ θ˙ 1 ∂ fθ1 (xB , uB ) = , ω TB ω TB ∂ω ∂ω

λB ) ω B ∂ λ˙ B ∂ Q (λ = , T λB λTB ∂λ ∂λ ∂ θ˙ 1 ∂ fθ1 (xB , uB ) = , ∂vTB ∂vTB ∂ θ˙ 1 ∂ fθ1 (xB , uB ) = , ∂uTB ∂uTB

λB ) vB ∂ E p˙ B ∂ EB R (λ = , T λB λTB ∂λ ∂λ ∂ ω˙ B ∂ next (xB , uB ) = , θ T1 θ T1 ∂θ ∂θ ∂ θ˙ 1 ∂ fθ1 (xB , uB ) = , θ T1 θ T1 ∂θ ∂θ

,

where, for some generic vector a ∈ R3 λB ) a ∂ EB R (λ = λTB ∂λ λB ) a h ∂ Q (λ = λTB ∂λ

λB ) ∂ EB R(λ a ∂φB λB ) ∂ Q(λ a ∂φB

λB ) ∂ EB R(λ a ∂θB λB ) ∂ Q(λ a ∂θB

λB ) ∂ EB R(λ a ∂ψB λB ) ∂ Q(λ a ∂ψB

i

.

Since the rotation matrix from body to earth-fixed frames is defined by the ZYX Euler angles, i. λB ) = Rz (ψB ) Ry (θB ) Rx (φB ), it can be seen that e., EB R (λ λB ) ∂ Rx (φB ) ∂ EB R(λ = Rz (ψB ) Ry (θB ) , ∂φB ∂φB ∂ Ry (θB ) λB ) ∂ EB R(λ = Rz (ψB ) Rx (φB ) , ∂θB ∂θB λB ) ∂ Rz (ψB ) ∂ EB R(λ = Ry (θB ) Rx (φB ) . ∂ψB ∂ψB Moreover, the partial derivatives of Matrix Q are given by   0 cos φB tan θB − sin φB tan θB λB )  ∂ Q(λ − sin φB − cos φB =  0 ∂φB cos φB sin φ  0 − cos θB cos θB B   sin φB cos φB  0  2 2 cos θB cos θB  λB )  ∂ Q(λ  ,  =  0 0 0  ∂θB  0 sin φB sin θB − cos φB sin θB  cos θ cos θ B

    ,  

B

λB ) ∂ Q(λ = 03×3 . ∂ψB Considering that the simplified external force and moment vectors, f = fmr + ftr and n = h iT nmr + ntr , and using the vectors ζ 1 = η 1 = vB ω B θ 1 uB , the derivatives relative to these variables, that is, ∂ fmr , ζ1 ∂ζ

∂ ftr , ζ1 ∂ζ

∂ nmr , ζ1 ∂ζ

∂ ntr , ζ1 ∂ζ

∂ fθ1 ξ1 ∂ξ

will be defined hereafter. 179

Chapter A: Helicopter Model Derivatives

A.2

Main Rotor

The first derivatives that need to be computed within the main rotor expressions are the transformations between frames {B}, {mr} and {hw}, noting that frame {mr} is located at the origin of {hw} and aligned with {B}. Considering that B mr B

R = I3 ,

pmr = B phw , B B vmr = mr B R (vB + ω B × pmr ) = vB + ω B × phw ,

ωB = ω B , ω mr = mr B Rω and   cos ψmw hw  − sin ψ mw mr R = Rz −ψmw =   0 mr R = hw mr R B R , vhub1 cos ψmw = q 2 2 vhub + vhub 1 2

sin ψmw cos ψmw 0

    , 

0 0 1

hw B

vhw = hw mr R vmr

vhub2 sin ψmw = q , 2 2 vhub + v hub2 1

,

ω mr , ω hw = hw mr Rω

,

in the rest of this main rotor section some partial derivatives will be needed and are given below. Considering that ∂ vmr T

∂vB ω mr ∂ω ∂vTB

∂ vmr B = −mr B R S( pmr ) , ω TB ∂ω ω mr mr ∂ω = B R = I3 , ω TB ∂ω

= mr B R = I3 , = 03×3 ,

∂ v¯ hw ∂ = (Ω Rm )−1 T T ∂vmr ∂vmr

hw mr

R vmr

 

hw

 ∂ Ra = (Ω Rm )−1  mr T ∂vmr a=vmr

∂ v¯ hw = 03×3 , ω Tmr ∂ω hw ∂ ω¯ hw −1 ∂ mr R a =Ω , ∂vTmr ∂vTmr a=ω ω mr ∂ ω¯ hw = Ω−1 hw mr R , ω Tmr ∂ω hw ∂ hw ∂ mr R mr R a a = ∂vhub1 ∂vTmr

∂ hw mr R ∂vhub2

a

∂ hw mr R ∂vhub3

a

,

 ∂ cos ψ mw   ∂vhubi T T mr B hw ∂ R ∂ mr R ∂ hw R  = = hw =  − ∂ sin ψmw ∂vhubi ∂vhubi ∂vhubi ∂vhubi   0

∂ sin ψmw ∂vhubi ∂ cos ψmw ∂vhubi

0 v2

 0    0  ,  0

∂ cos ψmw vhub1 ∂ hub2 = =q , q ∂vhub1 ∂vhub1 2 2 2 2 2 2 vhub1 + vhub2 vhub1 + vhub2 (vhub1 + vhub2 ) 180

  + R , hw mr

A.2 Main Rotor ∂ cos ψmw vhub1 vhub1 vhub2 ∂ = = −q , q ∂vhub2 ∂vhub2 2 2 2 2 2 2 vhub1 + vhub2 vhub1 + vhub2 (vhub1 + vhub2 ) ∂ sin ψmw vhub2 vhub1 vhub2 ∂ = = −q , q ∂vhub1 ∂vhub1 2 2 2 2 2 2 vhub1 + vhub2 vhub1 + vhub2 (vhub1 + vhub2 ) 2 vhub ∂ sin ψmw vhub2 ∂ 1 =q , = q ∂vhub2 ∂vhub2 2 2 2 2 2 2 vhub1 + vhub2 vhub1 + vhub2 (vhub1 + vhub2 )

∂ sin ψmw ∂ cos ψmw = =0, ∂vhub3 ∂vhub3 then      Defining hwθ c1 =

h

∂ v¯ hw ω TB ∂ω ∂ ω¯ hw ω TB ∂ω

∂ v¯ hw ∂vTB ∂ ω¯ hw ∂vTB

hw θ c1c

hw θ c1s

iT

       =  

, θ c1 =

mr T hw mr T = hw T

=

∂ v¯ hw ω Tmr ∂ω ∂ ω¯ hw ω Tmr ∂ω

∂ v¯ hw ∂vTmr ∂ ω¯ hw ∂vTmr

h

θc1c

"

cos ψmw sin ψmw

θc1s

      

∂ vmr ∂vTB ω mr ∂ω ∂vTB

∂ vmr ω TB ∂ω ω mr ∂ω ω TB ∂ω

    . 

iT

, the transformation # − sin ψmw , cos ψmw

its derivatives hw ∂ hw ∂ hw ∂ hw ∂ mr T mr T a mr T mr T a a a , = ∂vhub1 ∂vhub2 ∂vhub3 ∂vTmr T ∂ mr ∂ hw hw T a mr T a = ∂vTmr ∂vTmr T hw T hw T ∂ mr T ∂ T ∂ hw T mr = a ∂v a ∂vmr a , ∂vhub1 hub2 hub3  ∂ cos ψ  ∂ sin ψ mw  − ∂v mw  ∂ hw T  ∂v mr hubi hubi  , =  ∂ cos ψmw   ∂vhubi  ∂ sin ψmw ∂vhubi

∂vhubi

θ c1 and hwθ 1 = hw θ 1 , the remaining derivatives left to be defined and knowing that hwθ c1 = hw mr Tθ mr Tθ are given by ∂ θ0 ∂uTB ∂ hwθ c1 ∂uTB ∂ hwθ c1 ∂vTB ∂ hwθ c1

= = =

h

1 0 0 0

i

θ c1 ∂ hwθ c1 ∂θ θ Tc1 ∂θ

= hw mr T T

∂uB

"

0 1 0 0 0 0 1 0

# ,

θ c1 ∂ vmr ∂ hw mr Tθ , T ∂vmr ∂vTB

θ c1 ∂ vmr ∂ hw mr Tθ , ∂vTmr ∂ω ω TB ω TB ∂ω θ ∂ vmr ∂ hwθ 1 ∂ hw Tθ = mr T 1 , T ∂vmr ∂vTB ∂vB θ ∂ vmr ∂ hwθ 1 ∂ hw Tθ = mr T 1 , T ∂vmr ∂ω ωB ω TB ∂ω =

181

Chapter A: Helicopter Model Derivatives ∂ hwθ 1 hw = mr T . θ T1 ∂θ

A.2.1

Downwash

As the collective downwash expression for the simplified model is given by r a0 s a0 s 2 a0 s 1 1 λ0 = − + + θ0 + µz , 16 16 4 3 2 the respective partial derivatives are given by # " 2 − 21 ∂ λ0 a s a s a s 1 1 0 0 0 = 0 0 + 4 3 θ0 + 2 µz 16 16 ∂¯vThw !− 12 ∂ λ 0 a0 s a0 s 2 a0 s 1 1 = , + θ + µ ∂θ0 24 16 4 3 0 2 z Likewise, noting that the longitudinal cyclic downwash is given by r  2    λ0 1 + λ0 −µz − λ0 λ0 −µz , forward climb   µ µ  r λ1c =  ,  λ −µ 2   λ −µ 0 z 0 z   λ0 1 + µ + λ0 µ , forward descent its partial derivatives are given by r  λ −µ 2   λ −µ ∂ 0 z  λ0 T 1 + − λ0 ∂¯v∂T 0µ z   µ ∂ λ1c  ∂¯vhw hw r = λ −µ 2  ∂¯vThw   λ −µ ∂ 0 z   λ0 T 1 + + λ0 ∂¯v∂T 0µ z µ ∂¯v hw

and ∂ λ1c ∂λ0

, forward climb , , forward descent

hw

r  r λ −µ 2 λ −µ 2 λ −µ   ∂ ∂ λ0 −µz 0 z  1+ µ + λ0 ∂λ 1 + 0µ z − 0µ z − λ0 ∂λ   µ  r 0 0 r =  λ −µ 2 λ −µ 2 λ −µ   ∂ ∂ λ0 −µz   1 + 0µ z + λ0 ∂λ 1 + 0µ z + 0µ z + λ0 ∂λ µ 0

0

, f. c. , , f. d.

where s ∂ ∂¯vThw

λ 0 − µz 1+ µ

!2

 λ0 −µz !  !2 −1/2 λ0 − µz   λ0 − µz  1  ∂ µ = 2  1 +   2  ∂¯vThw µ µ =

s ∂ ∂λ0

λ 0 − µz 1+ µ ∂

!2

λ0 −µz µ T

∂¯vhw ∂

λ0 −µz µ

∂λ0 182

=

= =

∂

λ0 −µz µ T

∂¯vhw ∂

λ0 −µz µ

∂λ0 h

−

1 . µ

λ0 −µz µ2

! !2 −1/2   λ − µ 0 z 1 +    µ ! !2 −1/2 λ0 − µz  λ0 − µz  1 +  µ µ λ0 − µz µ

0 − µ1

i

A.2 Main Rotor

A.2.2

Blade Pitching Dynamics

Firstly, it is noted that the first order blade pitch dynamics are given by hw ˙ hw hw ¯ λ θ 1 = Ω A−1 −A (µ) θ + B (µ) θ + B ω + B (µ)λ , θ 1 θ c1 ω hw λ θ˙ T hw ˙ θ˙ 1 = fθ1 (xB , uB ) = hw θ1 , mr T h iT h iT ˙ where hwθ c1 = hw θc1c hw θc1s , λ = µz − λ0 λ1c λ1s and hwθ˙ 1 = hw mr T θ 1 . The derivatives of

ω B , uB ) can be easily computed considering the vectors fθ1 (vB ,ω h iT ξ 1 = vB ω B θ 1 uB h iT ξ 2 = v¯ hw ω¯ hw hwθ 1 λ0 hwθ c1 h iT ¯ hw hw θ 1 λ0 λ1c hw θ c1 ξ 3 = v¯ hw ω , resulting in ∂ fθ1 ξ T1 ∂ξ ∂ hwθ˙

1

ξ T1 ∂ξ where it can be seen that T ∂ hw mr T a = ξ T1 a=hwθ˙ ∂ξ 1 =

hw ˙ T ∂ hw mr T a hw T ∂ θ 1 + T = , mr ξ T1 a=hwθ˙ ξ T1 ∂ξ ∂ξ 1

∂ hw fθ1 ξ T3 ∂ξ

=

T ∂ hw mr T a ∂vTB

T ∂ hw mr T a ω TB ∂ω

T ∂ hw mr T a ∂ vmr T ∂vmr ∂vTB

 ∂ v¯ hw  ∂vT  B  ∂ ω¯ hw   ∂vTB ξ 2  ∂ hwθ 1 ∂ξ = T ξ T1  ∂ λ0∂v∂B v¯ hw ∂ξ  T  ∂¯vhw ∂vTB  hw  ∂ θ c1

∂ hwθ˙ 1 = ξ T3 ∂ξ

∂ hw θ˙ 1 ∂¯vThw

T ∂ hw mr T a θ T1 ∂θ

03×3 I3 02×3 01×3 01×3

03×2 03×2 ∂ hw θ 1 θ T1 ∂θ

01×2 02×2

03×2 03×1 03×2 03×1 I2 02×1 01×2 1 ∂ λ1c 01×2 ∂λ 0

02×3 02×2 02×1 ∂ hw θ˙ 1 ∂ω¯ Thw

T ∂ hw mr T a ∂uTB

T ∂ hw mr T a ∂ vmr T ω TB ∂vmr ∂ω

∂ v¯ hw ω TB ∂ω ∂ ω¯ hw ω TB ∂ω ∂ hw θ 1 ω TB ∂ω ∂ λ0 ∂ v¯ hw ω TB ∂¯vThw ∂ω hw ∂ θ c1 ω TB ∂ω

∂vTB

  I3  03×3  ξ 3 02×3 ∂ξ = ξ T2 0∂1×3 ∂ξ  λ1c  ∂¯vT  hw 02×3 and

ξ 3 ∂ξ ξ2 ∂ξ , T ξ 2 ∂ξ ξ T1 ∂ξ

∂ hw θ˙ 1 ∂hw θ T1

a=hw θ˙ 1

02×2 02×4 , a=hw θ˙ 1  03×4   03×4    02×4  ,  ∂ λ 0 ∂ θ0   T  ∂θ0 ∂uB   ∂ hw θ c1   ∂uTB

 03×2   03×2   02×2   01×2  ,  01×2   I2

∂ hw θ˙ 1 ∂λ0

∂ hw θ˙ 1 ∂λ1c

∂ hw θ˙ 1 ∂hw θ Tc1

.

The elements of this last derivative are defined below. The partial derivative relative to the linear velocity vector is given by hw ∂ hwθ˙ 1 ˙ = ∂∂hwθv¯ 1 T 1 ∂¯vhw

02×1

∂ hw θ˙ 1 ∂hw v¯3

=

∂ hw θ˙ 1 ∂µ

02×1

∂ hw θ˙ 1 ∂µz

, 183

Chapter A: Helicopter Model Derivatives where ! ∂ Aθ (µ) hw ∂ Bθ (µ) hw ∂ Bλ (µ) ∂ hw θ˙ 1 −1 = Ω Aθ˙ − θ1 θ c1 + λ ∂µ ∂µ ∂µ ∂µ ∂ Bλ (µ) = Ω A−1 λ, θ˙ ∂µ λ ∂λ ∂ hwθ˙ 1 = Ω A−1 , ˙ Bλ (µ) θ ∂µz ∂µz with

λ ∂λ ∂µz

h iT = 1 0 0 , and ∂ Bλ (µ) ∂ γf = ∂µ ∂µ 8

"

2 η2 µ 0 0 0 1 0

#!

" =

γf 4

η2 0 0 0 0 0

# .

The rest of the partial derivatives are given by ∂ hwθ˙ 1 = Ω A−1 Bω , θ˙ ∂ω¯ Thw λ ∂ hwθ˙ 1 ∂λ = Ω A−1 , Bλ (µ) θ˙ ∂λ1c ∂λ1c where

A.2.3

λ ∂λ ∂λ0

=

h

iT

−1 0 0

and

λ ∂λ ∂λ1c

∂ hwθ˙ 1 = −Ω A−1 Aθ , θ˙ ∂hwθ T1 ∂ hwθ˙ 1 = Ω A−1 Bθ , T θ˙ hw ∂ θ c1 =

h

0 1 0

iT

λ ∂ hwθ˙ 1 ∂λ = Ω A−1 , ˙ Bλ (µ) θ ∂λ0 ∂λ0

.

Blade Flapping Dynamics

Recall that the simplified steady state flapping motion expression is given by hw

where hwθ =

h

θ0

hwθ

hw ω hw + Bλ (µ)λ λ , β = A−1 β (µ) Bθ (µ) θ + Bω (µ)ω

iT 1

and λ is already defined in the blade pitching dynamic derivatives

section. The linear velocity derivatives of the flapping motion are given by hw hw hw hw ∂ hwβ = ∂∂hw v¯β 03×1 ∂∂hw v¯β = ∂∂µβ 03×1 ∂∂µ β , z 1 3 ∂¯vhw hw ∂ β ∂ hw ω λ = A−1 (µ) B (µ) θ + B (µ)ω + B (µ)λ θ w hw λ ∂µ ∂µ β =

∂ A−1 β (µ) ∂µ

+ A−1 β (µ)

ω hw + Bλ (µ)λ λ Bθ (µ) hwθ + Bw (µ)ω

! ∂ Bθ (µ) hw ∂ Bw (µ) ∂ Bλ (µ) θ+ ω hw + λ ∂µ ∂µ ∂µ

λ ∂ hwβ ∂λ = A−1 β (µ) Bλ (µ) ∂µz ∂µz 184

A.2 Main Rotor where

∂ A−1 β (µ) ∂µ

∂ Bθ (µ) ∂µ ∂ Bw (µ) ∂µ ∂ Bλ (µ) ∂µ λ ∂λ ∂µz

 S 2 +1  γ β 0  8 Sβ +1  4 ∂ 1   − γ3 µ Sβ 8 S = ∂µ Sβ2 + 1  8 Sβ +1 γ β 4  8  −γ 3µ γ 8 Sβ +1     1 0 0   0 ∂ γ     0 1 0  =  0 =   γ  ∂µ 8  8 µ 0 1 3 3    0 0 0  ∂    2 γ 0  = 03×3 , =  ∂µ  γ 8 8 −2 0    4 0 0     3 ∂ γ     0 −1 0  =  =    ∂µ 8  2 µ 0 −1      µz − λ0   1  ∂      λ1c  =  0  . =     ∂µz  0 λ1s

   0    4   1  − γ 3 Sβ   − γ8  = 2 S +1  Sβ + 1  8 4β    − γ S3 +1 8 8 β γ Sβ  0 0   0 0  ,  0 0 0

0 0 γ 4

 0 0   0 0  ,   0 0 

 0 0   0 0  ,  0 0

The remaining derivatives are given by ∂ hwβ = A−1 β (µ) Bω (µ) , ∂ω¯ hw ∂ hwβ ∂θ0 ∂ hwβ ∂hwθ 1 ∂ hwβ ∂λ0 ∂ hw β ∂λ1c

A.2.4

   1    −1  0  = A−1 = A (µ) B (µ) (µ) B (µ) θ θ β β   ∂θ0 0   0 ∂ hwθ  −1  1 = A−1 (µ) B (µ) = A (µ) B (µ) θ θ β β  hw ∂ θ1 0    −1   0  , = A−1 (µ) B (µ)   λ β   0    0   1  . = A−1 (µ) B (µ)   λ β   0 ∂ hwθ

, 0 0 1

    , 

Force

Noting that the main rotor force is defined by

ω B ) hw fmr (xB , uB ) , fmr (xB , uB ) = Bhw R(vB ,ω 185

Chapter A: Helicopter Model Derivatives and its derivative is denoted by

∂ fmr ζ1 ∂ζ

hw ∂ Bhw R a + Bhw R ∂ ∂vfTmr ∂vTB hw B a= fmr ∂ Bhw R a ∂ hw fmr B + hw R ∂ω ω TB hw ω TB ∂ω a= fmr hw ∂ fmr B hw R ∂θ θ T1 ∂ hw fmr B hw R ∂uT B

 T             =          

∂ fmr ∂vTB ∂ fmr ω TB ∂ω ∂ fmr θ T1 ∂θ ∂ fmr ∂uTB

      =     

T        ,      

where ∂ Bhw R a ∂vTB

=

∂ mr hw R a ∂vTB

=

∂ mr hw R a ∂ vmr ∂vTmr ∂vTB

and ∂ Bhw R a ∂ mr ∂ mr hw R a hw R a ∂ vmr = = T T T ∂v ωB ωB ω TB ∂ω ∂ω mr ∂ω it can be seen that these derivatives are completely defined in the preceding sections. Thus, only the derivatives of hw fmr need to be found, for which the following vectors are considered iT

ζ1 =

h

vB ω B θ 1 uB

ζ2 =

h

¯ hw θ0 v¯ hw ω

hw θ

1

λ0

ζ3 =

h

v¯ hw ω¯ hw θ0

hwθ

1

λ0 λ1c

ζ4 =

h

v¯ hw ω¯ hw θ0

hwθ

1

λ0 λ1c

iT iT hwβ

It can be seen that ζ 4 ∂ζ ζ 3 ∂ζ ζ2 ∂ hw fmr ∂ hw fmr ∂ζ = , ζ ζ ζ ζ ζ ∂ζ 1 ∂ζ 4 ∂ζ 3 ∂ζ 2 ∂ζ 1 where       ζ 2  ∂ζ = ζ 1  ∂ζ          ζ 3  ∂ζ = ζ 2  ∂ζ    186

∂ v¯ hw ∂vTB ∂ ω¯ hw ∂vTB

∂ v¯ hw ω TB ∂ω ∂ ω¯ hw ω TB ∂ω

03×2

03×4

03×2

03×4

01×3

01×3

01×2

∂ hw θ 1 ∂vTB ∂ λ0 ∂ v¯ hw ∂¯vThw ∂vTB

∂ hw θ 1 ω TB ∂ω ∂ λ0 ∂ v¯ hw ω TB ∂¯vThw ∂ω

∂ hw θ 1 θ T1 ∂θ

∂ θ0 ∂uTB

I3 03×3 01×3 02×3 01×3 ∂ λ1c ∂¯vThw

03×3 03×1 I3 03×1 01×3 1 02×3 02×1 01×3 0 01×3 0

02×4 ∂ λ 0 ∂ θ0 ∂θ0 ∂uTB

01×2

03×2 03×1 03×2 03×1 01×2 0 I2 02×1 01×2 1 ∂ λ1c 01×2 ∂λ 0

        ,     

         ,      

iT

.

A.2 Main Rotor       ζ 4  ∂ζ = ζ 3  ∂ζ     

I3 03×3 01×3 02×3 01×3 01×3

03×3 I3 01×3 02×3 01×3 01×3 ∂ hw β ∂ω¯ Thw

∂ hw β ∂¯vThw

03×1 03×1 1 02×1 0 0 ∂ hw β ∂θ0

03×2 03×2 01×2 I2 01×2 01×2

∂ hw β ∂hw θ T1

03×1 03×1 0 02×1 1 0

03×1 03×1 0 02×1 0 1

∂ hw β ∂λ0

∂ hw β ∂λ1c

         ,      

and ∂ hw fmr = ζ4 ∂ζ

∂ hw fmr ∂¯vThw

∂ hw fmr ∂ω¯ Thw

∂ hw fmr ∂θ0

∂ hw fmr ∂hw θ T1

∂ hw fmr ∂λ0

∂ hw fmr ∂λ1c

∂ hw fmr ∂hw β T

.

The force vector at frame {hw} is defined by hw

fmr = nb Af 0 + nb Af 12 hwβ ,

where  1  − 2 Y1s  Af 0 =  − 12 Y1c  Z0  1  − 2 Z1c  Af 12 =  12 Z1s  0

    ,  − 12 Z0 − 14 Z2c 1 4 Z2s 0

− 14 Z2s 1 1 2 Z0 − 4 Z2c 0

    , 

and nb denotes the number of blades of the main rotor. Its derivatives are given by ∂ Af 0 ∂ Af 12 hwβ ∂ hw fmr = n + n b b ∂hwβ T ∂hwβ T ∂hwβ T ∂ Af 12 a ∂ Af 0 = nb hw T + nb hw T + nb Af 12 ∂ β ∂ β a=hwβ and ∂ Af 0 ∂ Af 12 hwβ ∂ hw fmr = n + n ,, b b ∂bT ∂bT ∂bT for some vector b , hwβ , with ∂ Af 12 a ∂bT ∂ Af 0 ∂bT

∂ Af 12 ∂b1

a  1 ∂Y  − 2 ∂b1sT  =  − 21 ∂∂bY1cT  ∂ Z0 =

∂bT

···     ,  

∂ Af 12 ∂bnb

a

,

and ∂ Af 12 ∂bi

 1 ∂Z  − 2 ∂b1c  i =  1 ∂ Z1s  2 ∂bi 0

− 12

∂ Z0 Z2c − 14 ∂∂b ∂bi i 1 ∂ Z2s 4 ∂bi

0

∂ Z2s ∂bi 1 ∂ Z0 1 ∂ Z2c − 2 ∂bi 4 ∂bi

− 14

0

    .   187

Chapter A: Helicopter Model Derivatives Therefore, knowing that 1 1 hw hw a1c + (µz − λ0 ) θ1c , θ + µz − λ0 Y1c = −s1 a0 3 0 2 1 1 hw Y1s = −s1 a0 θ0 + µz − λ0 a1s + (µz − λ0 ) hw θ1s + µ (µz − λ0 ) θ0 + s1 δ0 µ , 3 2 s1 a0 2 θ + µz − λ0 , Z0 = − 2 3 0 and Z1c = Z1s = Z2c = Z2s = 0 , with µ = hw v¯1 , µz = hw v¯3 , a1c = hw ω¯ 2 − λ1c − hw β1s and a1s = hw ω¯ 1 + hw β1c , then the derivatives of these force components are given by ∂ Y1c T

∂¯vhw ∂ Y1c ∂ω¯ Thw ∂ Y1c ∂θ0 ∂ Y1c ∂hwθ T1 ∂ Y1c ∂λ0 ∂ Y1c ∂λ1c ∂ Y1c ∂hwβ T ∂ Y1s ∂¯vThw ∂ Y1s ∂ω¯ Thw ∂ Y1s ∂θ0 ∂ Y1s ∂hw θ T1 ∂ Y1s ∂λ0 ∂ Y1s ∂λ1c ∂ Y1s ∂hwβ T ∂ Z0

= −s1 a0

h

= −s1 a0

h

0

hw a

1c

1 3 θ0 + µz

+ 12 hw θ1c

i

i

,

− λ0 0

,

s1 a0 hw a1c , 3 i h = −s1 a0 12 (µz − λ0 ) 0 , =−

1 hw θ1c , 2 1 = s1 a0 θ0 + µz − λ0 , 3 h i = s1 a0 0 0 13 θ0 + µz − λ0 ,

= s1 a0

= −s1

h

hw

a1c +

a0 (µz − λ0 ) θ0 + δ0 0 a0

= −s1 a0

h

1 3 θ0 + µz

− λ0 0 0

i

,

1 hw a1s + µ (µz − λ0 ) , 3 h i = −s1 a0 0 21 (µz − λ0 ) ,

= −s1 a0

= s1 a0

hw

a1s +

h

0

1 hw θ1s + µ θ0 , 2

=0, = −s1 a0 =−

1 3 θ0 + µz

i s1 a0 h 0 0 1 , 2

∂¯vThw ∂ Z0 = 01×3 , ∂ω¯ Thw 188

0 0

− λ0 0

i

,

hw a

1s

+ 12 hw θ1s + µ θ0

i

,

A.2 Main Rotor ∂ Z0 ∂θ0 ∂ Z0 ∂hwθ T1 ∂ Z0 ∂λ0 ∂ Z0 ∂λ1c ∂ Z0 ∂hwβ T

=−

s1 a0 , 3

= 01×2 , =

s1 a0 , 2

=0, = 01×3 ,

concluding the computation of the main rotor force derivatives.

A.2.5

Moment

As the main rotor moment is defined by ω B ) hw nmr (xB , uB ) + S(B phw )fmr (xB , uB ) , nmr (xB , uB ) = Bhw R(vB ,ω its derivative is denoted by

∂ nmr ζ1 ∂ζ

      =     

∂ nmr ∂vTB ∂ nmr ω TB ∂ω ∂ nmr ∂hw θ T1 ∂ nmr ∂uTB

∂ Bhw R a ∂ hw nmr B + R + S(B phw ) ∂∂vfmrT T hw ∂vB ∂vTB hw B a= nmr B hw ∂ hw R a nmr fmr + Bhw R ∂ ∂ω + S(B phw ) ∂∂ω ω TB hw ω TB ω TB ∂ω a= nmr ∂ hw nmr ∂ fmr B B hw R ∂hw θ T + S( phw ) ∂θ θ T1 1 ∂ hw nmr ∂ fmr B B + S( phw ) ∂uT hw R ∂uT B B

 T             =          

T        ,      

from where only the derivatives of hw nmr remain to be found. Considering the same process of change of variables used in the previous section, using the vectors ζ 1 , ζ 2 , ζ 3 and ζ 4 that yield the same partial derivatives

ζ 2 ∂ζ ζ3 ∂ζ ζ 1 , ∂ζ ζ2 ∂ζ

and

ζ4 ∂ζ ζ3 , ∂ζ

it can be seen that

ζ 4 ∂ζ ζ 3 ∂ζ ζ2 ∂ hw nmr ∂ hw nmr ∂ζ = , ζ1 ζ 4 ∂ζ ζ 3 ∂ζ ζ 2 ∂ζ ζ1 ∂ζ ∂ζ where

hw ∂ hw nmr ∂ nmr ∂ hw nmr ∂ hw nmr = T T ∂θ0 ∂¯vhw ∂ω¯ hw ζ4 ∂ζ The moment vector at frame {hw} is defined by hw

∂ hw nmr ∂hw θ T1

∂ hw nmr ∂λ0

∂ hw nmr ∂λ1c

∂ hw nmr ∂hw β T

.

nmr = nb An0 + nb An12 hwβ ,

where    An0 =      An12 =  

0 0 N0

    , 

− 12 N1c − 12 N0 − 14 N2c − 21 kβ − 14 N2s 1 − 12 kβ + 14 N2s 12 N0 − 14 N2c 2 N1s 0 0 0

    ,  189

Chapter A: Helicopter Model Derivatives and nb is the number of blades of the rotor. Its derivatives are given by ∂ An12 hwβ ∂ An0 ∂ hw nmr = n + n b b ∂hwβ T ∂hwβ T ∂hwβ T ∂ An0 ∂ An12 a + nb An12 , = nb hw T + nb hw T ∂ β ∂ β a=hwβ and ∂ An12 hwβ ∂ hw nmr ∂ An0 = n + n , b b ∂bT ∂bT ∂bT for some vector b , hwβ , with ∂ An12 a = ∂bT

  ∂ An0   =  ∂bT

∂ An12 ∂b1

0 0 ∂ N0 ∂bT

a ···     , 

∂ An12 ∂bnb

a

,

and ∂ An12 ∂bi

 1 ∂N  − 2 ∂b1c  i =  1 ∂ N1s  2 ∂bi 0

− 12

∂ N0 N2c − 14 ∂∂b ∂bi i 1 ∂ N2s 4 ∂bi

0

∂ N2s ∂bi 1 ∂ N0 1 ∂ N2c − 2 ∂bi 4 ∂bi

− 14

0

    .  

Therefore, knowing that 1 1 1 N0 = −s2 a0 (µz − λ0 ) θ0 + (µz − λ0 )2 + s2 δ0 , 3 2 4 1 1 1 hw hw hw N1c = −s2 a0 a1c θ0 + (µz − λ0 ) θ1c + 2 a1c − µ θ0 + µ (µz − λ0 ) β0 , 4 3 3 1 1 hw 1 hw hw N1s = −s2 a0 a1s θ0 + (µz − λ0 ) θ1s + 2 a1s + µ (µz − λ0 ) θ0 , 4 3 2 N2c = N2s = 0 , with µ = hw v¯1 , µz = hw v¯3 , a1c = hw ω¯ 2 − λ1c − hw β1s and a1s = hw ω¯ 1 + hw β1c , the derivatives of these variables are given by ∂ N0 ∂¯vThw ∂ N0 ∂ω¯ Thw ∂ N0 ∂θ0 ∂ N0 ∂hwθ T1 ∂ N0 ∂λ0 ∂ N0 ∂λ1c 190

h = −s2 a0 0 0

1 3 θ0 + µz

= 01×3 , =−

s2 a0 (µz − λ0 ) , 3

= 01×2 , = s2 a0 =0,

1 θ + µz − λ0 , 3 0

− λ0

i

,

A.3 Tail Rotor ∂ N0 = 01×3 , ∂hwβ T h i ∂ N1c 1 1 hw hw a = −s a − θ + µ − λ β 0 θ + 2 − µ β 2 0 0 z 0 0 1c 1c 0 3 3 ∂¯vThw h i ∂ N1c = −s2 a0 0 14 θ0 + 23 (µz − λ0 ) 0 , T ¯ hw ∂ω 1 hw 1 ∂ N1c = −s2 a0 a1c − µ β0 , ∂θ0 4 3 h i ∂ N1c 1 = −s a , (µ ) − λ 0 2 0 z 0 3 ∂hwθ T1 1 hw ∂ N1c = s2 a0 θ1c + 2 hw a1c − µ β0 , ∂λ0 3 ∂ N1c 1 2 = s2 a0 θ + (µ − λ0 ) , ∂λ1c 4 0 3 z h i ∂ N1c = s2 a0 13 µ θ0 + µ (µz − λ0 ) 0 41 θ0 + 23 (µz − λ0 ) , hw T ∂ β h i ∂ N1s 1 hw 1 1 hw a ) (µ θ 0 − λ θ + 2 + = −s a µ θ , 0 z 0 1s 1s 2 0 0 2 3 2 ∂¯vThw i h ∂ N1s 1 2 , = −s a ) (µ 0 0 − λ θ + 2 0 z 0 0 4 3 ∂ω¯ Thw ∂ N1s 1 1 hw = −s2 a0 a1s + µ (µz − λ0 ) , ∂θ0 4 2 i h ∂ N1s 1 , = −s a (µ ) − λ 0 2 0 z 0 3 ∂hwθ T1 1 ∂ N1s 1 hw hw = s2 a0 θ1s + 2 a1s + µ θ0 , ∂λ0 3 2 ∂ N1s =0, ∂λ1c h i ∂ N1s 1 2 = −s a , (µ ) 0 θ + − λ 0 2 0 0 z 0 4 3 ∂hw β T concluding the computation of the main rotor moment derivatives.

A.3

Tail Rotor

The first step in the computation of the tail rotor derivatives are those of the transformations between frames {B}, {tr} and {tw}, which are similar to those of the transformation between frames {B}, {mr} and {hw}. Considering that    1 0 0   tr  0 0 −1  ,  BR =    0 1 0 vtr = trB R (vB + ω B × B ptr ) , ωB , ω tr = trB Rω 191

Chapter A: Helicopter Model Derivatives and   cos ψtw tw  − sin ψ tw tr R = Rz −ψtw =   0

sin ψtw cos ψtw 0

0 0 1

    , 

tr R = Btw RT = tw tr R B R , vtr2 vtr1 , sin ψtw = q , cos ψtw = q vtr21 + vtr22 vtr21 + vtr22 tw B

vtw = tw tr R vtr

,

ω tr , ω tw = tw tr Rω

consider the following derivatives, necessary for the remaining of the tail rotor derivatives, ∂ vtr T

∂vB ω tr ∂ω

= trB R

,

∂ vtr = −trB R S(B ptr ) , ω TB ∂ω ω tr tr ∂ω , = BR , ω TB ∂ω

= 03×3 ∂vTB    ∂ tw  ∂ v¯ tw tr R a −1 ∂ tw −1  tw   (tr R vtr ) = (Ω Rt )  + tr R , = (Ω Rt ) T T T ∂vtr ∂vtr ∂vtr a=vtr tw ∂ v¯ tw ∂ ω¯ tw ∂ Ra ∂ ω¯ tw = 03×3 , = Ω−1 tr T , = Ω−1 tw tr R , T T T ω tr ω ∂ω ∂vtr ∂vtr a=ω ∂ω tr ω tr tw ∂ tw ∂ tr R ∂ tw ∂ tw tr R a tr R tr R a a a = , ∂vtr1 ∂vtr2 ∂vtr3 ∂vTtr   ∂ cos ψ ∂ sin ψtw tw   0  tr tr T T   tr B ∂ vi ∂ vi ∂ R ∂ R ∂ tw R   tw tw tr  ∂ sin ψtw ∂ cos ψtw = = =  , − 0  tr tr tr tr tr   ∂ vi ∂ vi ∂ vi ∂ vi ∂ vi   0 0 0 vtr22 ∂ cos ψtw vtr1 ∂ =q , = q ∂vtr1 ∂vtr1 v 2 + v 2 2 + v 2 (v 2 + v 2 ) v tr1 tr2 tr1 tr2 tr1 tr2 ∂ cos ψtw vtr1 vtr1 vtr2 ∂ = = −q , q ∂vtr2 ∂vtr2 v 2 + v 2 vtr21 + vtr22 (vtr21 + vtr22 ) tr1 tr2 ∂ sin ψtw vtr2 vtr1 vtr2 ∂ = = −q , q ∂vtr1 ∂vtr1 v 2 + v 2 2 + v 2 (v 2 + v 2 ) v tr1 tr2 tr1 tr2 tr1 tr2 vtr21 ∂ sin ψtw vtr2 ∂ , = =q q ∂vtr2 ∂vtr2 v 2 + v 2 2 + v 2 (v 2 + v 2 ) v tr1 tr2 tr1 tr2 tr1 tr2 ∂ sin ψtw ∂ cos ψtw = =0, ∂vtr3 ∂vtr3 resulting in      192

∂ v¯ tw ∂vTB ∂ ω¯ tw ∂vTB

∂ v¯ tw ω TB ∂ω ∂ ω¯ tw ω TB ∂ω

       =   

∂ v¯ tw ∂vTtr ∂ ω¯ tw ∂vTtr

∂ v¯ tw ω Ttr ∂ω ∂ ω¯ tw ω Ttr ∂ω

       

∂ vtr ∂vTB ω tr ∂ω ∂vTB

∂ vtr ω TB ∂ω ω tr ∂ω ω TB ∂ω

    , 

A.3 Tail Rotor where ψtw defines the rotation between that defines the rotation between frames {tr} and {tw}, whereas ∂ θ0t ∂uTB

A.3.1

=

h

i

0 0 0 1

.

Downwash

As the tail rotor collective downwash expression for the simplified model is given by r a0t st a0t st 2 a0t st 1 1 λ0t = − + + θ0t + µzt . 16 16 4 3 2 the respective partial derivatives are given by # " ∂ λ 0t − 21 a0t st a0t st 2 a0t st 1 1 = 0 0 + 4 3 θ0t + 2 µzt 16 16 ∂¯vThw !− 12 ∂ λ0t a0t st a0t st 2 a0t st 1 1 = θ + µ , + ∂θ0t 24 16 4 3 0t 2 zt

A.3.2

Force

Noting that the tail rotor force is defined as ω B ) tw ftr (xB , uB ) , ftr (xB , uB ) = Btw R(vB ,ω its derivative is  ∂ ftr T B tr R  T    ∂vB     ∂ ftr  ∂ ftr  B R = =  ∂ω tr  T   ζ 1  ω B  ∂ζ   ∂ ftrT   ∂u B

 ∂ tr ∂ tw ftr T B tw R a  + R T T tw  ∂vB a=tw f ∂vB  tr  tr tw ∂ tw R a ∂ f B tr   + R T T  tw ω B a=tw f ωB  ∂ω ∂ω  tr  ∂ tw ftr B  tw R ∂uT B

,

where ∂ trtw R a ∂vTB

=

∂ trtw R a ∂vTB

=

∂ trtw R a ∂ vtr ∂vTtr ∂vTB

and ∂ trtw R a ∂ trtw R a ∂ trtw R a ∂ vtr = = ∂vTtr ∂ω ω TB ω TB ω TB ∂ω ∂ω are already defined in the preceding sections. Thus, only the derivatives of

tw f tr

remain to be found. Considering the same process of

change of variables, using the vectors h iT τ 1 = vB ω B uB and h τ 2 = v¯ tw ω¯ tw θ0t

λ0t

iT

, 193

Chapter A: Helicopter Model Derivatives it can be seen that ∂ tw ftr ∂ tw ftr ∂ττ 2 = , ∂ττ 1 ∂ττ 2 ∂ττ 1 where      ∂ττ 2  =  ∂ττ 1    

∂ v¯ tw ∂vTB ∂ ω¯ tw ∂vTB

∂ v¯ tw ω TB ∂ω ∂ ω¯ tw ω TB ∂ω

01×3

01×3

∂ λ0t ∂ v¯ tw ∂¯vTtw ∂vTB

∂ λ0t ∂ v¯ tw ω TB ∂¯vTtw ∂ω

03×4 03×4 ∂ θ 0t ∂uTB ∂ λ0t ∂ θ0t ∂θ0t ∂uTB

       ,    

and ∂ tw ftr = ∂ττ 2

∂ tw ftr ∂¯vTtw

∂ tw ftr ∂ω¯ Ttw

∂ tw ftr ∂θ0t

∂ tw ftr ∂λ0t

.

The force vector at frame {tw} is defined by    tw ftr =  

0 0 2 Z0t

   =  

0 0

    , 

and its derivatives are given by ∂ tw ftr ∂bT

2

∂ Z 0t ∂bT

    .  

Therefore, knowing that s1t a0t 2 Z0t = − θ + µzt − λ0t 2 3 0t with µzt = tw v¯3 , the derivatives are given by ∂ Z 0t ∂¯vTtw ∂ Z 0t ∂θ0t

i s1t a0t h ∂ Z 0t 0 0 1 = 01×3 , 2 ∂ω¯ Ttw s1 a0 ∂ Z0t s1t a0t =− t t , = , 3 ∂λ0t 2

=−

finalizing the computation of the main rotor force derivatives.

A.3.3

Moment

Recalling that the tail rotor moment is defined by ω B ) tw ntr (xB , uB ) + B ptr × ftr (xB , uB ) , ntr (xB , uB ) = Btw R(vB ,ω its derivative is denoted by

∂ ntr ∂ττ 1

194

    =   

∂ ntr ∂vTB ∂ ntr ω TB ∂ω ∂ ntr ∂uTB

T  B  tr R       =  Btr R      

∂ tr ∂ tw ntr B tw R a + R + S(B ptr ) ∂∂vftrT T tw ∂vB a=tw n ∂vTB B tr ∂ tr ∂ tw ntr ∂ ftr B B tw R a + tw R ∂ω + S( ptr ) ∂ω ω TB a=tw n ω TB ω TB ∂ω tr tw ∂ ntr ∂ ftr B + S(B ptr ) ∂u T tw R ∂uT B B

T     ,   

A.3 Tail Rotor from where only the derivatives of tw ntr remain to be found. Considering the same process of change of variables used in the previous section, using the vectors τ 1 and τ 2 that yield the same partial derivative

∂ττ 2 , ∂ττ 1

it can be seen that ∂ tw ntr ∂ tw ntr ∂ττ 2 = , ∂ττ 1 ∂ττ 2 ∂ττ 1

where

∂ tw ntr = ∂ττ 2

∂ tw ntr ∂¯vTtw

∂ tw ntr ∂ω¯ Ttw

The moment vector at frame {tw} is defined by    tw ntr =  

∂ tw ntr ∂θ0t

∂ tw ntr ∂λ0t

.

    

0 0 2 N0 t

and its derivatives are given by    =  

∂ tw ntr ∂bT

0 0 2

∂ N0t ∂bT

    .  

Therefore, knowing that N0t = −s2t a0t

1 1 1 (µzt − λ0t ) θ0t + (µzt − λ0t )2 + s2t δ0t , 3 2 4

with µzt = tw v¯3 , the derivatives of this variable is given by ∂ N0t

h = −s2t a0t 0 0

1 3 θ0t

∂¯vTtw ∂ N0t s2 a0 = − t t (µzt − λ0t ) , ∂θ0t 3

∂ N 0t ∂λ0t

i

∂ N 0t

= 01×3 , ∂ω¯ Ttw 1 = s2t a0t θ0t + µzt − λ0t , 3

+ µzt − λ0t

,

which concludes the computation of the tail rotor moment derivatives.

195

B Catamaran Model Derivatives Having defined the catamaran model in the Section 3.4.1, the computation of the analytical derivatives of the model function assumes an important role in the computation of equilibrium points and in real-time implementation of the NMPC algorithms.

B.1 B.1.1

First-order Derivatives Error-space Dynamics

Considering Equation (3.1) for the particular case of the catamaran error-space model, defined in Equations (3.10), (3.11), and (3.12), and dropping the subscript k in the remaining of this section for the sake of clarity, the state and input derivatives are given by  ∂f   ∂ fe ∂ fd    Td ∂f  ∂xe  = I + Ts ∂xTe ∂xTn  = ∂xT 02×9 02×2   02×9 # " ∂f 09×2 = I2 ∂uT

 ∂ fe  Ts ∂n T  e xn   ,  02×2

with  ∂ ν˙  Te  ∂νν e ∂ fe  ∂ η˙ e =  ν T e ∂xTe  ∂ν  ∂ x˙ i ν Te ∂ν

∂ ν˙ e η Te ∂η ∂ η˙ e η Te ∂η ∂ x˙ i η Te ∂η

∂ ν˙ e   ∂ ν˙   ∂νν T ∂xTi    ∂ η˙ e ∂ η˙ e   T  ∂xi   =  ∂νν Te   ∂ x˙ i   03×3 T ∂xi



 03×3 03×3   ∂ η˙ e  0  T 3×3 ηe ∂η  I3 03×3

and  ∂ ν˙ e   ∂nT   ∂ ν˙      ∂ fe  ∂ η˙ee   ∂nT   T  = 03×2  . =  e    ∂nTe  ∂n ∂ x˙ i   03×2 ∂nTe

197

Chapter B: Catamaran Model Derivatives

B.1.2

Rigid-body Kinematics

The derivatives of the catamaran error dynamics rigid-body kinematics are given by ∂ η˙ e

∂ T νe ν Te ∂ν ∂ν ∂ = ν Te ∂ν ∂ = ν Te ∂ν =

η e )ν ν rc − Q(ν ν )η ηe ν − J −1 (η ν e + ν rc )η ηe ν e + ν rc − Q(ν ν e + ν rc )η ηe ν e − Q(ν

∂ ν e + ν rc )η ηe Q(ν T νe ∂ν ∂ = I3 − T [−(re + rc ) ye − (re + rc ) xe 0]T νe ∂ν   1 0 −ye    = 0 1 xe    0 0 1

= I3 −

and ∂ η˙ e η Te ∂η

B.1.3

∂ η e )ν ν rc − Q(ν ν )η ηe ν − J −1 (η T ηe ∂η ∂ η e )ν ν rc − Q(ν ν e + ν rc ) = − T J −1 (η ηe ∂η # " η e) ∂ J −1 (η ν e + ν rc ) ν rc − Q(ν = − 03×2 ∂ψe   0 re + rc uc sin ψe − vc cos ψe     0 uc cos ψe + vc sin ψe  . = −(re + rc )   0 0 0 =

Catamaran Dynamics

The derivatives of the dynamics of the catamaran model are given by τ2 ∂ ν˙ −1 ∂τ = M 3 νT νT ∂ν ∂ν and ∂ ν˙ ∂ττ 2 = M3−1 , T ∂n ∂n where  ∂X  2 ∂ττ 2  ∂∂u =  Y2 ν T  ∂∂u ∂ν N2 ∂u

 ∂ X2  ∂n ∂ττ 2  ∂ Yc2 = ∂nT  ∂∂nNc 2 ∂nc

198

∂ X2 ∂ X2  ∂v ∂r   ∂ Y2 ∂ Y2    ∂v ∂r   ∂ N2 ∂ N2  ∂v ∂r ∂ X2   ∂nd    ∂ Y2   . ∂nd  ∂ N2   ∂nd

 ,

B.1 First-order Derivatives The derivatives of each element of τ 2 relative to the generalized velocity vector are given by ∂ X2 = Xu + 2 Xu 2 u + 3 Xu 3 u 2 + Xur 2 r 2 + Xunc nc , ∂u ∂ X2 = m r + 2 Xv 2 v + Xvr r , ∂v ∂ X2 = m v + 2 Xr 2 r + 2 Xur 2 u r + Xvr v + Xrnd nd , ∂r ∂ Y2 = −m r + Yuv v + Yur r − Yr 3 /u r 3 /u 2 , ∂u ∂ Y2 = Yuv u + 2 Yvvcc |v + r xcc | + 2 Yvvc |v + r xc | + Yvr |r| + Yvnc nc , ∂v ∂ Y2 = −m u + Yur u + 3 Yr 3 /u r 2 /u + 2 xcc Yvvcc |v + r xcc | + 2 xc Yvvc |v + r xc | ∂r + Yvr v sign (r) + 2 Yrr |r| + Yrnc nc , ∂ N2 = Nuv v + Nur r − Nr 3 /u r 3 /u 2 − Nv2r/u v |v| r/u 2 − Nvr2/u v r |r|/u 2 ∂u + 2 Nru 2 r u + Nund nd , ∂ N2 = Nuv u + 2 Nv2r/u |v| r/u + Nvr2/u r |r|/u + 2 Nvvcc |v + r xcc | + 2 Nvvc |v + r xc | ∂v + Nvr |r| + Nvnc nc , ∂ N2 = Nur u + 3 Nr 3 /u r 2 /u + Nv2r/u v |v|/u + 2 Nvr2/u v |r|/u + Nr + Nru 2 u 2 + 3 Nr 3 r 2 ∂r + 2 xcc Nvvcc |v + r xcc | + 2 xc Nvvc |v + r xc | + Nvr v sign (r) + 2 Nrr |r| + Nrnc nc , whereas the derivatives of the elements of τ 2 relative to the control inputs are given by ∂ X2 = Xrnd r + 2 Xn2 nd , d ∂nd ∂ Y2 =0, ∂nd ∂ N2 = Nund u + Nnc nd nc . ∂nd

∂ X2 = Xunc u + 2 Xn2c nc , ∂nc ∂ Y2 = −Yvnc v + Yrnc r , ∂nc ∂ N2 = Nvnc v + Nrnc r + Nnc nd nd , ∂nc

B.1.4

Intrinsic Input Saturations

Considering the saturation functions introduced in Section 3.4.1.2, the derivative on the saturated inputs relative to the regular inputs is given by  ∂ n¯    c ∂ n¯c   ∂ n¯c ∂ n¯ ∂n ∂n  d   =  ∂nc =  c ∂nT  ∂ n¯d ∂ n¯d   ∂ n¯d ∂ n¯c ∂nc

∂nd

∂n¯ c ∂nc

 0  , ∂ n¯ d   ∂nd

where the common mode input saturation derivative is   , c ≤ nc ≤ c  1  ∂ n¯ c   nc −c −2 , nc < c =  1 + c  ∂nc   −2 n −   1− c c , nc < c c

199

Chapter B: Catamaran Model Derivatives and the derivatives of the differential mode input saturation function are given by    1  ∂ n¯ d   nd −d −2 1 + = d  ∂nd     1 − nd −d −2

, d ≤ nd ≤ d , nd > d , nd < d

d

and    0  ∂ n¯d ∂ n¯ d   = ∂d  ∂n¯ c     ∂∂n¯d d

∂ d ∂n¯ c ∂ d ∂n¯ c

+ +

∂ n¯ d ∂d ∂ n¯ d ∂d

, d ≤ nd ≤ d

∂ d ∂n¯ c ∂ d ∂n¯ c

, nd > d

,

, nd < d

considering that ∂ d = 1− , ∂n¯ c ∂ d = −1 + , ∂n¯ c ∂ d =, ∂n¯ c ∂ n¯ d nd − d n − d = 1+ d ∂d d d

!−2

nd − d ∂ n¯ d nd − d = 1− ∂d d d

!−2

, ,

and  −2  nd −d 2  1 + nd−d ∂ n¯ d   d d =  n − 2 n − −2  ∂d   d d 1 − d d d

B.2

d

, nd > d , nd < d

.

Second-order Derivatives

Consider the notation where f(i) (x, u) indicates the i-th component of the vector function f : nx × nu → nx , for all i = 1, . . . , nx , and A(i,:) denotes the i-th row vector, with dimensions 1 × m, of the generic matrix A ∈ Rn×m .

B.2.1

Error Dynamics

Considering the first-order derivatives presented above, it can be seen that  (i)  ∂2 fe  ∂2 f(i) Ts ∂x ∂xT = 0 ∂x ∂xT  200

, ∀i=1,...,8 , ∀i=9,10

B.2 Second-order Derivatives where  ∂2 (i)  T ∂2 fe e =  ∂x∂e ∂x 2 T ∂x ∂x ∂ne ∂xTe  2 (i)  ∂ ν˙    ∂xe ∂xTe (i)  2  ∂ fe  ∂2 η˙ (i−3) e =  ∂x ∂xTe T ∂xe ∂xe  e   0   2 (i) (i) ∂ ν˙   ∂2 fe  ∂ne ∂nTe =  0 ∂ne ∂nTe 

∂2  (i) ∂xe ∂nTe   fe ∂2   ∂ne ∂nTe ne =xn

 2 (i) ∂ ν˙    ∂xe ∂nTe =  0 ∂xe ∂nTe 

, ∀i=1,...,3



,

, ∀i=1,...,3 , ∀i=4,...,6 , , ∀i=7,8 , ∀i=1,...,3 , ∀i=4,...,8

,

and (i)

∂2 fe

, ∀i=4,...,8

.

Noting that ∂ η˙ e ∂ ν˙ ∂ ν˙ = =0 = η Te ∂η ∂xTi ∂xTi the remaining second-order derivatives of the error-space function are given by νT ∂2 ν˙ (i) ∂ν = νe ∂ν ν e ∂ν ν Te ∂ν νT ∂2 ν˙ (i) ∂ν = νe ∂ν ν e ∂nTe ∂ν

ν ∂2 ν˙ (i) ∂ν ∂2 ν˙ (i) = , ν ∂ν ν T ∂ν ν ∂ν νT ∂ν ∂ν ν Te ∂2 ν˙ (i) ∂ n ∂2 ν˙ (i) , = ν ∂nT ∂nTe ν ∂nT ∂ν ∂ν

(i)

∂2 η˙ e

ν e ∂ν ν Te ∂ν

= 03×3 ,

for all i = 1, . . . , 3, whereas   0 0      0 0          0 1  (i)   ∂2 η˙ e  =  0 0 ν e ∂η η Te  ∂ν   0 0          −1 0     0 3×3

 0  0  0  0  0  0

, i=1

, i=2 , i=3,

and (i)

   03×3 =  d 2 J −1 (ψe ) T  νrc η e ∂η ηe  ∂η dψ 2 ∂2 η˙ e

e

, i = 1, 2 , i=3,

.

201

Chapter B: Catamaran Model Derivatives

B.2.2

Catamaran Dynamics

The second-order derivatives of the catamaran dynamics model can be readily obtained from the first-order derivatives presented above, yielding ∂2 ν˙ (i) ν ∂ν νT ∂ν

=

∂2 ν˙ (i) = ν ∂nT ∂ν

(i) ∂2 M3−1 τ 2 ν ∂ν νT ∂ν (i) ∂2 M3−1 τ 2 ν ∂nT ∂ν

2 (i) −1 (:,i) ∂ τ 2 = M3 , ν ∂ν νT ∂ν 2 (i) −1 (:,i) ∂ τ 2 = M3 , ν ∂nT ∂ν

and ∂2 ν˙ (i) ∂n ∂nT

=

(i) ∂2 M3−1 τ 2 ∂n ∂nT

2 (i) −1 (:,i) ∂ τ 2 = M3 . ∂n ∂nT

for all i = 1, . . . , 3. Therefore, the remaining second-order derivatives to be defined are those of h iT the elements of the generalized force, τ 2 = X2 Y2 N2 , relative to the generalized velocity and input vectors. The second-order derivatives for the first element, X2 , relative to the generalized velocity are given by ∂2 X2 ∂u 2 ∂2 X2 ∂v ∂u ∂ 2 X2 ∂r ∂u ∂2 X2 ∂v 2 ∂2 X2 ∂r ∂v ∂2 X2 ∂r 2

= 2 Xu 2 + 6 Xu 3 u , =0, = 2 Xur 2 r , = 2 Xv 2 , = m + Xvr , = 2 Xr 2 + 2 Xur 2 u ,

whereas the derivatives of the second element, Y2 , are given by ∂2 Y2 ∂u 2 ∂2 Y2 ∂v ∂u ∂2 Y2 ∂r ∂u ∂2 Y2 ∂v 2 ∂2 Y2 ∂r ∂v ∂2 Y2 ∂r 2 202

= 2 Yr 3 /u r 3 /u 3 , = Yuv , = −m + Yur − 3 Yr 3 /u r 2 /u 2 , = 2 Yvvcc sign (v + r xcc ) + 2 Yvvc sign (v + r xc ) , = 2 xcc Yvvcc sign (v + r xcc ) + 2 xc Yvvc sign (v + r xc ) + Yvr sign (r) , 2 = 6 Yr 3 /u r/u + 2 xcc Yvvcc sign (v + r xcc ) + 2 xc2 Yvvc sign (v + r xc ) + 2 Yrr sign (r) ,

B.2 Second-order Derivatives and the derivatives of the the third element, N2 , by ∂2 N2 ∂u 2 ∂2 N2 ∂v ∂u ∂2 N2 ∂r ∂u ∂2 N2 ∂v 2 ∂2 N2 ∂r ∂v

= 2 Nr 3 /u r 3 /u 3 + 2 Nv 2 r/u v |v| r/u 3 + 2 Nvr 2 /u v r |r|/u 3 + 2 Nru 2 r , = Nuv − 2 Nv 2 r/u |v| r/u 2 − 2 Nvr 2 /u r |r|/u 2 , = Nur − 3 Nr 3 /u r 2 /u 2 − Nv 2 r/u v |v|/u 2 − Nvr 2 /u v |r|/u 2 + 2 Nru 2 u , = 2 Nv 2 r/u sign (v) r/u + 2 Nvvcc sign (v + r xcc ) + 2 Nvvc sign (v + r xc ) , = 2 Nv 2 r/u |v|/u + Nvr 2 /u r |r|/u + 2 xcc Nvvcc sign (v + r xcc ) + 2 xc Nvvc sign (v + r xc ) + Nvr sign (r) ,

and ∂2 N2 = 6 Nr 3 /u r/u + 2 Nvr 2 /u v sign (r) /u + 6 Nr 3 r ∂r 2 2 + 2 xcc Nvvcc sign (v + r xcc ) + 2 xc2 Nvvc sign (v + r xc ) + 2 Nrr sign (r) . The second-order derivatives of the elements of generalized force relative to the input vector are given by " 2 Xn2c ∂2 X2 = T 0 ∂n ∂n

# 0 , 2 Xn2

∂2 Y2 = 02×2 , ∂n ∂nT " ∂2 N2 0 = T Nnc nd ∂n ∂n

# Nnc nd , 0

d

and, finally, the second-order cross derivatives are given by " # ∂2 X2 Xunc 0 0 = , 0 0 Xrnd νT ∂n ∂ν " # ∂2 Y2 0 Yvnc Yrnc = , 0 0 0 νT ∂n ∂ν # " ∂2 N2 0 Nvnc Nrnc = . Nund 0 0 νT ∂n ∂ν

203

C Closed-form Step Size Parameters In Section 5.4.3 a closed-form optimal solution for line search problem is found, using the solution of a fourth order polynomial [PKK00]. This appendix provides the undefined constants used in this process. Considering the line search cost function, resulting from the application of the Rodrigues’ formula, defined in (5.7) as φ(t) = k1 + k2 sin t + k3 cos t + k4 sin 2 t + k5 cos 2 t , the constants kj , for all j = 1, . . . , 5, are given by kj =

n X i=1

1 tr Kji , 2 n σi2

where K1i = MTi RTk Ni Rk Mi + Ci + 2 Wi Rk Mi + MTi RTk Ni Rk Ω 2 Mi + MTi Ω 2 RTk Ni R Mi + 2 Wi Rk Ω 2 Mi + MTi Ω 2 RTk Ni Rk Ω 2 Mi − 1/2 MTi Ω RTk Ni Rk Ω Mi + 1/2 MTi Ω2 RTk Ni Rk Ω2 Mi , K2i = MTi RTk Ni Rk Ω Mi − MTi Ω RTk Ni Rk Mi + 2 Wi Rk Ω Mi − MTi Ω RTk Ni Rk Ω 2 Mi + MTi Ω 2 RTk Ni Rk Ω Mi , K3i = −MTi RTk Ni Rk Ω 2 Mi − MTi Ω 2 RTk Ni Rk Mi − 2 Wi Rk Ω 2 Mi − 2 MTi Ω 2 RTk Ni Rk Ω 2 Mi , K4i = 1/2 MTi Ω RTk Ni Rk Ω 2 Mi − 1/2 MTi Ω 2 RTk Ni Rk Ω Mi , and K5i = 1/2 MTi Ω RTk Ni Rk Ω Mi + 1/2 MTi Ω 2 RTk Ni Rk Ω 2 Mi . 205

Chapter C: Closed-form Step Size Parameters Furthermore, the constants bi , for all i = 0, . . . , 4, resulting from the trigonometric half-angle substitution, x = tan 2t , in the first-order condition of optimality b4 x4 + b3 x3 + b2 x2 + b1 x + b0 = 0 , are given by b0 = k2 + 2 k4 , b1 = −2 k3 − 8 k5 , b2 = −12 k4 , b3 = −2 k3 + 8 k5 , b4 = −k2 + 2 k4 .

206

D Proof of UCO Theorem for SLAM The proof of Theorem 6.6 follows similar steps to the proofs of Theorems 6.3 and 6.5, but considering uniform bounds for all t ≥ t0 and intervals [t, t + δ]. Consider the previously defined expressions for the generic unitary vector c, the observability Gramian, as well as for the function f(t, τ) and its derivatives, noting that they all have bounded norm. The sufficiency proof follows by exhaustion, showing that ∃ δ>0 ∀t≥t0 ∀c∈Rnc : cT W (t, t + δ) c ≥ α. α>0

(D.1)

kck=1

To do so, all the possibilities for the components of c are addressed, either establishing that

∂ f(t, τ)

≥ α2∗ for every possible case, for some α1∗ > 0, α2∗ > 0, and τ ∈ Tδ . Then, kf(t, τ)k ≥ α1∗ or ∂τ noting that Proposition 6.2 can be used, condition (D.1) is implied. Firstly, considering that kcM k ≥ αM , for some 1 ≥ αM > 0, it can be seen that kf(t, t)k = kcM k ≥ αM , for all t ≥ t0 , which implies (D.1). Now, consider the converse case where kcM k < αM , noting that this implies that kcV k ≥ αV for some 1 ≥ αV > 0, as kck = 1. Defining the auxiliary function Zτ fV (t, τ) := TM (σ ) AMV (σ ) dσ c∗V t

with

c∗

V

:=

cV , kcV k

such that f (t, τ) = cM + fV (t, τ) kcV k, it can be seen that kf (t, τ)k ≥ max(kcM k , kfV (t, τ)k kcV k) − min(kcM k , kfV (t, τ)k kcV k) ,

for all t ≥ t0 and τ ∈ Tδ . Assuming that kfV (t, t ∗ )k ≥ αfV if kcV k ≥ αV , for some t ∗ ∈ Tδ and αfV > 0, and choosing any αM ≤

αfV αV 2

, it can be seen that kf (t, t ∗ )k ≥ kfV (t, t ∗ )k kcV k − kcM k αf αV ≥ αfV αV − V 2 αfV αV ≥ , 2 207

Chapter D: Proof of UCO Theorem for SLAM which also implies (D.1). Thus, it remains to prove that, whenever kcV k ≥ αV , then there exists t ∗ ∈ Tδ such that kfV (t, t ∗ )k ≥ αfV . Recalling Proposition 6.2 and noting that fV (t, t) = 0, it is sufficient to shown that there exists some αf∗ > 0 such that there exists τ ∈ Tδ for which V

∂ fV (t,τ)

≥ α ∗ . To this end, without loss of generality, consider that c∗ = cV with kcV k = 1, such V fVR ∂τ τ that fV (t, τ) = t TM (σ ) AMV (σ ) dσ cV . Computing the derivative of this function, it is possible to conclude that

2 X

m

A (τ) c

2 ,

∂ fV (t, τ)

= MVi V

∂τ i=1

which, considering conditions (i), (ii), and (iii) of the theorem, results in

2

∂ fV (t, τ)

=

S y (τ) c − c

2 +

S y (τ) c − c

2 , b v p2 b v p1

∂τ

2

∂ fV (t, τ)

=

S y (τ) c − c

2 +

S y (τ) c

2 , d1 b p1 b v

∂τ and

2

∂ fV (t, τ)

=

S y (τ) c − c

2 p1 b v

∂τ Zτ

2 =

S yp1 (t) cb − cv + S y˙ p1 (σ ) dσ cb

, t

respectively, noting that Z yp1 (τ) = yp1 (t) +

t

τ

y˙ p1 (σ ) dσ .

(D.2)

Using the reverse triangular inequality, it can be seen that 2

2

S yp1 (τ) cb − cv

≥

S yp1 (τ) cb

− kcv k

2

=

yp1 (τ)

|cb |2 + kcv k2

− 2

yp1 (τ)

|cb | kcv k Consider the case where |cb | < αb for any 0 < αb ≤ 1, implying that kcv k ≥ αv for some 0 < αv ≤ 1, as kcV k = 1. Choosing αb ≤ 4αα¯v , the previous equation becomes p

2


≥ kcv k kcv k − 2

yp1 (τ)

|cb | ≥ αv αv − 2 α¯ p αb ≥

αv2 , 2

α2 ∂ f (t,τ) 2 implying that

V∂τ

≥ 2v for all three conditions of the theorem. Considering the case where kcv k < αv for any 0 < αv ≤ 1, which also implies that |cb | ≥ αb for some 0 < αb ≤ 1, and choosing αv ≤

αp1 αb 4 ,

it can be seen that

2


≥

yp1 (τ)

|cb |

yp1 (τ)

|cb | − 2 kcv k ≥ αp1 αb αp1 αb − 2 αv ≥

208

αp21 αb2 2

,

2 α 2 α 2 ∂ fV (t,τ)

which also implies that ∂τ

≥ p12 b for all three conditions of the theorem. The only remaining case is when both kcv k ≥ αv and |cb | ≥ αb for some 0 < αb , αv ≤ 1, for which, if

∂ f (t,τ)

S yp1 (τ) cb − cv ≥ α1 for some positive constant α1 > 0, then

V∂τ

≥ α1 for all the conditions

(i), (ii), and (iii) of the theorem, thus implying (D.1). Conversely, if


< α1 , the conditions of the theorem have to be addressed separately. For the first condition of the theorem, Assumption 6.2 implies that the two visible position landmarks are different and

satisfy

yp1 (τ) − yp2 (τ)

≥ αp12 . It is a matter of algebraic manipulation to show that this relations can be rewritten as

αp12 ≤

yp1 (τ) − yp2 (τ)

αp12 αb ≤

S yp1 (τ) cb − S yp2 (τ) cb

αp12 αb ≤

(S yp1 (τ) cb − cv ) − (S yp2 (τ) cb − cv )

α α ≤

S y (τ) c − c

+

S y (τ) c − c

. p12

Then, by choosing α1 ≤

αp12 αb 2 ,

b

b

p1

v

b

p2

v

it can be seen that


≥ αp12 αb − α1 αp αb ≥ 12 , 2

α α ∂ f (t,τ) which implies that

V∂τ

≥ p122 b . For the second condition of the theorem, considering α α α1 ≤ d12 b , it is possible to show that

2

∂ fV (t, τ)

=

S y (τ) c − c

2 +

y (τ)

2 kcb k2 d1 b v p1

∂τ

2

≥


+ αd21 αb2

≥ (α α −

S y (τ) c − c

)2 d1

b

p1

b

v

2

≥ (αd1 αb − α1 ) ≥

αd2 αb2 1

4

.

Rτ Finally, for the third condition of the theorem, the function t y˙ p1 (σ ) dσ is zero for τ = t and that

its derivative, which is simply given by y˙ p1 (τ), satisfies

y˙ p1 (t1 )

≥ αp˙ . Thus,

R using once

again t Proposition 6.2, it can be seen that there exists t2 ∈ Tδ and α2 > 0 such that

2 y˙ p (σ ) dσ

≥ α2 . Choosing α1 ≤

α2 2 ,

t

1

it can be seen that

∂ fV (t, τ)

= ∂τ τ=t2

Z t2

= S yp1 (t) cb − cv + S y˙ p1 (σ ) dσ cb

t

Z

t2

≥

S y˙ p1 (σ ) dσ cb

−

S yp1 (t) cb − cv

t

≥ α2 − α1 α ≥ 2. 2 209

Chapter D: Proof of UCO Theorem for SLAM Therefore, for all kcV k ≥ αV the conditions of the theorem are sufficient to ensure that there exists t ∗ ∈ Tδ such that kfV (t, t ∗ )k ≥ αfV . This concludes the proof for the sufficiency part of the A(t),C C (t)). theorem, for the pair (A The proof of necessity is done by transposition, that is, we want to prove that if none of the A(t),C C (t)) is not uniformly completely conditions of the theorem are satisfied, then the pair (A observable. This means that either no landmark is visible, clearly yielding unobservability, or the only visible position landmark satisfies

∀ δ>0 ∃t≥t0 ∀t ∗ ∈Tδ :

y˙ p1 (t ∗ )

< αp˙ . αp˙ >0

For the latter case, consider that kcM k = 0, whereas 1 > kcv k ≥ αv and 1 > |cb | ≥ αb , for some αb > 0 and αv > 0. It is a matter of algebraic manipulation to show that t+δ

Z

T

c W (t, t + δ) c =

kf (t, τ)k2 dτ

t

2

dτ R (σ ) S y (σ ) c − c dσ g 1 p1 1 b v 1

t t !2 Z t+δ Z τ

S y (σ ) c − c

dσ ≤ dτ p1 1 b v 1 t+δ

Z τ

Z

=

t

t

which, using (D.2) and choosing cv = S yp1 (t0 ) cb and cb such that kck = 1, yields cT W (t, t + δ) c Z t+δ Z τ

Z

S ≤

t

Z

t

t+δ

t σ1

Z τZ

≤ t

< |cb | < < Considering αp2˙ =

10 δ5

2

Z t

αp2˙ Z 2

t+δ

!2

y˙ p1 (σ2 ) dσ2 cb

dσ1 dτ

y˙ (σ )

dσ dσ p1 2 2 1 |cb |

t

Z τZ t

dτ

!2

σ1

t

!2

αp˙ dσ2 dσ1

dτ

(τ − t)4 dτ

t

αp2˙ δ5 10

t t+δ

σ1

.

with > 0, it can be seen that ∀δ>0 ∃t≥t0 ∀c∈Rnc : cT W (t, t + δ) c < , >0

kck=1

A(t),C C (t)) is not uniformly completely observable. Therefore, if the meaning that the pair (A A(t),C C (t)) is uniformly completely observable, the conditions of the theorem must hold. pair (A Noting that the Lyapunov transformation T(t) preserves observability properties, the uniform complete observability of the pair (A(t), C(t)) is achieved under the same conditions as for the A(t),C C (t)), which concludes the proof of the theorem. pair (A 210

Bibliography [AB95] M. Alamir and G. Bornard. Stability of a truncated infinite constrained receding horizon scheme: the general discrete nonlinear case. Automatica, 31(9):1353–1356, 1995. [ADHK04] A. P. Aguiar, D. B. Dacic, J. P. Hespanha, and P. Kokotovic. Path following or reference-tracking? an answer relaxing the limits to performance. In 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles, Lisbon, Portugal, July 2004. [AHB87] K. S. Arun, T. S. Huang, and S. D. Blostein. Least-squares fitting of two 3-D point sets. IEEE Transactions on Pattern Analysis and Machine Intelligence, PAMI-9(5):698 –700, Sept. 1987. [AMS07] P. A. Absil, R. Mahony, and R. Sepulchre. Optimization Algorithms on Matrix Manifolds. Princeton University Press, 2007. [And71] B.D.O. Anderson. Stability properties of Kalman-Bucy filters. Journal of the Franklin Institute, 291(2):137 – 144, 1971. [And84] T.W. Anderson. An Introduction to Multivariate Statistical Analysis. Wiley Series in Probability and Mathematical Statistics. Wiley, 1984. [AS72] Milton Abramowitz and Irene A. Stegun. Handbook of Mathematical Functions with Formulas, Graphs, and Mathematical Tables. Dover Publications, New York, 1972. [Bai02] T. Bailey. Mobile Robot Localisation and Mapping in Extensive Outdoor Environments. PhD thesis, University of Sydney, Australian Center of Field Robotics, 2002. [Bar12] Martin Barczyk. Nonlinear State Estimation and Modeling of a Helicopter UAV. PhD thesis, Department of Electrical and Computer Engineering, University of Alberta, 2012. [BB00] S. Bhat and D. Bernstein. A topological obstruction to continuous global stabilization of rotational motion and the unwinding phenomenon. Systems & Control Letters, 39(1):63–70, 2000. [BDB01] A. R. S. Bramwell, George Done, and David Balmford. Bramwell’s Helicopter Dynamics. Butterworth-Heinemann, Oxford, 2001. [BDW06] T. Bailey and H. Durrant-Whyte. Simultaneous localization and mapping (SLAM): Part II. Robotics Automation Magazine, IEEE, 13(3):108–117, 2006. [Ber99] D. P. Bertsekas. Nonlinear Programming. Athena Scientific, 2nd edition, 1999. [BGFB94] Stephen Boyd, Laurent Ghaoui, Eric Feron, and Venkataramanan Balakrishnan. Linear Matrix Inequality in System and Control Theory, volume 15 of SIAM studies in applied mathematics. Society for Industrial and Applied Mathematics, SIAM, Philadelphia, PA, 1994. [Bis08] Morten Bisgaard. Modeling, Estimation, and Control of Helicopter Slung Load System. PhD thesis, Department of Control Engineering, Aalborg University, 2008. [BJ09] A. N. Bishop and P. Jensfelt. A stochastically stable solution to the problem of robocentric mapping. In Robotics and Automation, 2009. ICRA ’09. IEEE International Conference on, pages 1615–1622, may 2009. 211

Bibliography [BL13] Martin Barczyk and Alan Francis Lynch. Control-oriented modeling of a helicopter UAV with a Bell-Hiller stabilizer mechanism. In Proceedings of the IEEE American Control Conference, Montreal, USA, June 2013. [BM92] P. J. Besl and H. D. McKay. A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 14(2):239 –256, Feb 1992. [BMVD02] A. Bemporad, M. Morari, and E. N. Pistikopoulos V. Dua. The explicit linear quadratic regulator for constrained systems. Automatica, 38(1):3–20, 2002. [BNG+ 06] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot. Consistency of the EKFSLAM algorithm. In Proceedings of the 2006 IEEE/RSJ International Conference Intelligent Robots and Systems, pages 3562–3568, October 2006. [Bow02] Nathaniel Bowditch. The American Practical Navigator: Bowditch. U. S. National Imagery and Mapping Agency, 2002. [Bro70] R. Brockett. Finite Dimensional Linear Systems. Wiley, 1970. [BSO10] P. Batista, C. Silvestre, and P. Oliveira. Optimal position and velocity navigation filters for autonomous vehicles. Elsevier Automatica, 46(4):767–774, April 2010. [BSO11a] P. Batista, C. Silvestre, and P. Oliveira. On the observability of linear motion quantities in navigation systems. Elsevier Systems and Control Letters, 60(2):101– 110, Feb. 2011. [BSO11b] P. Batista, C. Silvestre, and P. Oliveira. Partial attitude and rate gyro bias estimation: Observability analysis, filter design, and performance evaluation. International Journal of Control, 84(5):895–903, May 2011. [BSO11c] P. Batista, C. Silvestre, and P. Oliveira. Sensor-based globally asymptotically stable filters for attitude estimation: Analysis, design, and performance evaluation. IEEE Transactions on Automatic Control, July 2011. accepted for publication. [BSO11d] P. Batista, C. Silvestre, and P. Oliveira. Single range aided navigation and source localization: observability and filter design. Systems & Control Letters, 60(8):665– 673, Aug 2011. [CA98] H. Chen and F. Allgöwer. A quasi-infinite horizon nonlinear model predictive control with guaranteed stability. Automatica, 34(10):1205–1217, 1998. [CAGS06] Rita Cunha, Duarte Antunes, Pedro Gomes, and Carlos Silvestre. A path-following preview controller for autonomous air vehicles. In AIAA Guidance, Navigation and Control Conference, Keystone, CO, August 2006. AIAA. [Car09] Bruno Cardeira. Arquitecturas para navegaça˜ o inercial/GPS com aplicaça˜ o a ´ ve´ıculos autonomos. Master’s thesis, Department of Electrical and Computer Engineering, Instituto Superior Técnico, Lisbon, Portugal, February 2009. in Portuguese. [CCL11] Guowei Cai, Ben M. Chen, and Tong H. Lee. Unmanned rotorcraft systems. In M.J. Grimble and M.A. Johnson, editors, Advances in Industrial Control. Springer, 2011. [CDW96] M. Csorba and H.F. Durrant-Whyte. A new approach to simultaneous localisation and map building. In SPIE Aerosense, 1996. [CGM92] Y. Chen and G G. Medioni. Object modelling by registration of multiple range images. Image and Vision Computing, 10:145–155, April 1992. 212

Bibliography [CIC06] CICIND. Chimney Maintenance Guide. International Committee on Industrial Chimneys (CICIND), January 2006. ´ and J. Neira. Robocentric [CMCTN07] J. A. Castellanos, Ruben Martinez-Cantin, J. D. Tardos, map joining: Improving the consistency of EKF-SLAM. Robotics and Autonomous Systems, 55(1):21–29, January 2007. ´ Limits to the consistency of EKF-based [CNT04] J.A. Castellanos, J. Neira, and J.D. Tardos. SLAM. In Proceedings of the 5th IFAC Symposium on Intelligent Autonomous Vehicles, July 2004. [Com12] European Commission. Towards a european strategy for the development of civil applications of remotely piloted aircraft systems (RPAS). Technical Report SWD(2012) 259 final, September 2012. Available at http://register. consilium.europa.eu/pdf/en/12/st13/st13438.en12.pdf. [CR80] C. R. Cutler and B. L. Ramaker. Dynamic matrix control: a computer control algorithm. In Proceedings of the joint automatic control conference, San Francisco, CA, USA, 1980. [CS82] C. C. Chen and L. Shaw. On receding horizon feedback control. Automatica, 18(3):349–352, 1982. [CSM11] N.A. Chaturvedi, A.K. Sanyal, and N.H. McClamroch. Rigid-body attitude control. IEEE Control Systems, 31(3):30 –51, june 2011. [Cso97] M. Csorba. Simultaneous Localisation and Map Building. PhD thesis, University of Oxford, 1997. [Cun02] Rita Cunha. Modeling and control of an autonomous robotic helicopter. Master’s thesis, Department of Electrical and Computer Engineering, Instituto Superior Técnico, Lisbon, Portugal, May 2002. [Cun07] Rita Cunha. Advanced Motion Control for Autonomous Air Vehicles. PhD thesis, Instituto Superior Técnico, Universidade Técnica de Lisboa, Lisbon, Portugal, June 2007. [DI13] Alissa M. Dolan and Richard M. Thompson II. Integration of drones into domestic airspace: Selected legal issues. Technical Report R42940, U.S. Congressional Research Service, April 2013. Available at http://assets.opencrs.com/rpts/ R42940_20130130.pdf. [DJP04] K. D. Do, Z. P. Jiang, and J. Pan. Robust adaptive path following of underactuated ships. Automatica, 40(6):929–944, June 2004. [DNDW+ 01] G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M. Csobra. A solution to the simultaneous localisation and mapping (SLAM) problem. IEEE Transactions on Robotics and Automation, 17(3):229–241, 2001. [dNMMS00] G. de Nicolao, L. Magnani, L. Magni, and R. Scattolini. A stabilizing receding horizon controller for nonlinear discrete time systems. In American Control Conference, Chicago, Illinois, USA, June 2000. [DW88] H.F. Durrant-Whyte. Uncertain geometry in robotics. IEEE Journal of Robotics and Automation, 4(1):23–31, feb 1988. [DWB06] H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping: Part I. Robotics Automation Magazine, IEEE, 13(2):99–110, 2006. [EAS98] A. Edelman, T. A. Arias, and S. T. Smith. Geometry of algorithms with orthogonality constraints. SIAM Journal on Matrix Analysis and Aplications, 20(2):303–353, 1998. 213

Bibliography [EPA00] P. Encarnaça˜ o, A. Pascoal, and M. Arcak. Path following for autonomous marine craft. In 5th IFAC Conference on Marine Craft Maneuvering and Control, pages 117–122, Aalborg, Denmark, August 2000. [Fea08] Roy Featherstone. Rigid Body Dynamics Algorithms. Springer, 2008. [FIAF03] Rolf Findeisen, Lars Imsland, Frank Allgöwer, and Bjarne A. Foss. State and output feedback nonlinear model predictive control: An overview. Europ. J. Contr., 9, 2003. [Fil01a] S. Filin. Calibration of Airborne and Spaceborne Laser Altimeters Using Natural Surfaces. PhD thesis, The Ohio State University, 2001. [Fil01b] S. Filin. Recovery of systematic biases in laser altimeters using natural surfaces. In International Archives of Photogrametry and Remote Sensing, volume 34, 2001. [FMCV10] Carlos Fernández, Vidal Moreno, Belen Curto, and J. Andres Vicente. Clustering and line detection in laser range measurements. Robotics and Autonomous Systems, 58:720–726, 2010. [Fos94] T. I. Fossen. Guidance and Control of Ocean Vehicles. Wiley, New York, USA, 1994. [FOY84] Katsuhisa Furuta, Yasuhiro Ohyama, and Osamu Yamano. Dynamics of RC helicopter and control. Mathematics and Computers in Simulation, 26(2):148–159, 1984. [FW01] J.M. Fitzpatrick and J.B. West. The distribution of target registration error in rigidbody point-based registration. IEEE Transactions on Medical Imaging, 20(9):917– 927, sept. 2001. [Gav03] V. Gavrilets. Autonomous Acrobatic Maneuvering of Miniature Helicopters. PhD thesis, Massachusetts Institute of Technology, May 2003. [GD04] J. C. Gower and G. B. Dijksterhuis. Procrustes Problems. Oxford University Press, 2004. [Gel74] A. Gelb. Applied Optimal Estimation. MIT Press, May 1974. [GKJ08] Alexandra Grancharova, Jus Kocijan, and Tor A. Johansen. Explicit stochastic predictive control of combustion plants based on gaussian process models. Automatica, 44:1621–1631, 2008. [GM82] Carlos E. Garcia and Manfred Morari. Internal model control. a unifying review and some new results. Industrial and Engineering Chemistry Process Design and Development, 21(2):308–323, April 1982. [GMF01] V. Gavrilets, B. Mettler, and E. Feron. Nonlinear model for a small-size acrobatic helicopter. In Proceedings of the AIAA Guidance, Navigation, and Control Conference and Exhibit, Montreal, Canada, August 2001. [GN99] Laurent Ghaoui and Silviu-Iulian Niculescu. Advances in Linear Matrix Inequality Methods in Control. Society for Industrial and Applied Mathematics, SIAM, Philadelphia, PA, 1999. [Goo91] Colin Goodall. Procrustes methods in the statistical analysis of shape. Journal of the Royal Statistical Society. Series B (Methodological), 53(2):pp. 285–339, 1991. [GPM89] C. E. Garcia, D. M. Prett, and M. Morari. Model predictive control: theory and practice - a survey. Automatica, 25(3):335–348, 1989. [Gro07] Paul D. Groves. Principles of GNSS, Inertial, and Multi-Sensor Integrated Navigation Systems. Artech Print on Demand, 2007. 214

Bibliography [GS10] C. Gao and J.R. Spletzer. On-line calibration of multiple LIDARs on a mobile vehicle platform. In Robotics and Automation (ICRA), 2010 IEEE International Conference on, pages 279–284, 2010. [GSPC07] Pedro Gomes, Carlos Silvestre, Antonio Pascoal, and Rita Cunha. A coastline following preview controller for the DELFIMx vehicle. In 16th International Offshore and Polar Engineering Conference, Lisbon, Portugal, July 2007. [GWA01] M. S. Grewal, L. R. Weill, and A. P. Andrews. Global Positioning Systems, Inertial Navigation, and Integration. Wiley & Sons, 2001. [Hah02] Hubert Hahn. Rigid Body Dynamics of Mechanisms: 1 Theoretical Basis. Springer, 2002. [HD07] Shoudong Huang and Gamini Dissanayake. Convergence and consistency analysis for extended Kalman filter based SLAM. IEEE Transactions on Robotics, 23(5):1036– 1049, oct. 2007. [HMR09] Guoquan Huang, Anastasios Mourikis, and Stergios Roumeliotis. A first-estimates Jacobian EKF for improving SLAM consistency. In Oussama Khatib, Vijay Kumar, and George Pappas, editors, Experimental Robotics, volume 54 of Springer Tracts in Advanced Robotics, pages 373–382. Springer Berlin Heidelberg, 2009. [HPR08] Ruijie He, Sam Prentice, and Nicholas Roy. Planning in information space for a quadrotor helicopter in a GPS-denied environment. In IEEE International Conference on Robotics and Automation, pages 1814–1820, Pasadena, CA, USA, May 2008. [HS99] A. Habib and T. Schenk. A new approach for matching surfaces from laser scanners and optical sensors. In International Archives of Photogrametry and Remote Sensing, volume 32 of Part 3 W14, 1999. [Jaz70] A. Jazwinski. Stochastic Processes and Filtering Theory. Academic Press, Inc., 1970. [Jon04] J. Jonnes. Empires Of Light: Edison, Tesla, Westinghouse, And The Race To Electrify The World. Random House Publishing Group, 2004. [JU01] S. J. Julier and J. K. Uhlmann. A counter example for the theory of simultaneous localization and map building. In Proceedings of the 2001 IEEE International Conference on Robotics and Automation, page 4238–4243, May 2001. [JYH01] Ali Jadbabaie, Jie Yu, and John Hauser. Unconstrained receding-horizon control of nonlinear systems. IEEE Transactions on Automatic Control, 46(5):776–783, May 2001. [Kal60] R. E. Kalman. A new approach to linear filtering and prediction problems. Transactions of the ASME – Journal of Basic Engineering, 82(Series D):35–45, 1960. [Kay93] Steven M. Kay. Fundamentals of Statistical Signal Processing: Estimation Theory. Prentice-Hall, Inc., Upper Saddle River, New Jersey, 1993. [KB61] R. E. Kalman and R. S. Bucy. New results in linear filtering and prediction theory. Transactions of the ASME – Journal of Basic Engineering, 83(1):95–108, 1961. [KB06] Tamás Keviczky and Gary J. Balas. Receding horizon control of an F-16 aircraft: A comparative study. Control Engineering and Practice, 14(9):1023–1033, 2006. [KG88] S. S. Keerthi and E. G. Gilbert. Optimal infinite-horizon feedback laws for a general class of constrained discrete-time systems: Stability and moving-horizon approximations. Journal of Optimization Theory and Applications, 57(2):265–293, May 1988. 215

Bibliography [Kha02] Hassan K. Khalil. Nonlinear Systems. Prentice-Hall, Inc., 3nd edition, 2002. [KPHS98] I. Kaminer, A. Pascoal, E. Hallberg, and C. Silvestre. Trajectory tracking for autonomous vehicles: An integrated approach to guidance and control. Journal of Guidance, Control, and Dynamics, 21(1):29–38, January 1998. ´ Pascoal, Pramod P. Khargonekar, and Edward E. Cole[KPKC95] Isaac Kaminer, Antonio man. A velocity algorithm for the implementation of gain-scheduled controllers. Automatica, 31(8):1185–1191, 1995. [KSS02] H. Kim, D. Shim, and S. Sastry. Nonlinear model predictive tracking control for rotorcraft-based unmanned aerial vehicles. In American Control Conference, volume 5, pages 3576–3581, Anchorage, AK, May 2002. [KT04] S. K. Kim and D. M. Tilbury. Mathematical modeling and experimental identification of an unmanned helicopter robot with flybar dynamics. Journal of Robotic Systems, 21(3):95–116, 2004. [KTM+ 95] W. B. Krabill, R. H. Thomas, C. F. Martin, R. N. Swift, and E. B. Frederick. Accuracy of airborne laser altimetry over the greenland ice sheet. International Journal of Remote Sensing, 16(7):1211–1222, 1995. [La 03] Marco La Civita. Integrated Modeling and Robust Control for Full-Envelope Flight of Robotic Helicopters. PhD thesis, Robotics Institute, Carnegie Mellon University, January 2003. [Lan78] J.G. Landels. Engineering the Ancient World. Ancient culture and society. University of California Press, 1978. [LDW91] John J. Leonard and Hugh F. Durrant-Whyte. Mobile robot localization by tracking geometric beacons. IEEE Transactions on Robotics And Automation, 7(3):376–382, June 1991. [Lew92] F.L. Lewis. Applied Optimal Control & Estimation: Digital Design & Implementation. Prentice Hall, 1992. [LL60] L. D. Landau and E. M. Lifshitz. Course of theoretical physics. In Mechanics. Vol. 1. Pergamon Press, 1960. [LMK02] Marco La Civita, William Messner, and Takeo Kanade. Modeling of small-scale helicopters with integrated first-principles and system-identification techniques. In Proceedings of the 58th Forum of the American Helicopter Society, pages 2505– 2516, June 2002. [LMV92] A.M. Lepschy, G.A. Mian, and U. Viaro. Feedback control in ancient water and mechanical clocks. IEEE Transactions on Education, 35(1):3–10, Feb 1992. [LS04] T. Lapp and L. Singh. Model predictive control based trajectory optimization for nap-of-earth flight including obstacle avoidance. In American Control Conference, pages 891–892, Boston, MA, USA, June 2004. [LT10] J. Levinson and S. Thrun. Unsupervised calibration for multi-beam lasers. In Proceedings of the International Symposium on Experimental Robotics, 2010. [Lya92] A.M. Lyapunov. The General Problem of the Stability of Motion. Control Theory and Applications Series. Tayor & Francis, 1992. [Mag02] L. Magni. On robust tracking with non-linear model predictive control. International Journal of Control, 75(6):399–407, 2002. [Max68] J. Clerk Maxwell. On governors. Procedings of the Royal Society of London, 16:270– 283, 1868. 216

Bibliography [Met01] B. F. Mettler. Modeling Small-scale Unmanned Rotorcraft for Advanced Flight Control Design. PhD thesis, Carnegie Mellon University, Pittsburg, PA, USA, January 2001. [Met03] B. Mettler. Identification Modeling and Characteristics of Miniature Rotorcraft. Springer, 2003. [MHN12] W. Maddern, A. Harrison, and P. Newman. Lost in translation (and rotation): Rapid extrinsic calibration for 2D and 3D LIDARs. In Robotics and Automation (ICRA), 2012 IEEE International Conference on, pages 3096–3102, 2012. [MLZ90] E. Mosca, J. M. Lemos, and J. Zhang. Stabilizing I/O receding horizon control. In Proceedings of the 29th IEEE Conference on Decision and Control, volume 4, pages 2518 – 2523, Honolulu, USA, December 1990. [MM93] H. Michalska and D. Q. Mayne. Robust receding horizon control of constrained nonlinear systems. IEEE Transactions on Automatic Control, 38(11):1623–1633, November 1993. [MM03] R. Madhavan and E. Messina. Iterative registration of 3D LADAR data for autonomous navigation. In Intelligent Vehicles Symposium, 2003. Proceedings., pages 186–191. IEEE, June 2003. [MNMS01] L. Magni, G. De Nicolao, L. Magnani, and R. Scattolini. A stabilizing model-based predictive control algorithm for nonlinear systems. Automatica, 37:1351–1362, 2001. [MRA09] Lalo Magni, Davide Martino Raimondo, and Frank Allgöwer, editors. Nonlinear Model Predictive Control: Towards New Challenging Applications, volume 384 of Lecture Notes in Control and Information Sciences. Springer, 2009. [MRE+ 82] M. Mehra, R. Rouhani, J Eterno, J. Richalet, and A. Rault. Model algorithmic control: review and recent development. In Proceedings of the Eng. Foundation Conf. on Chemical Process Control II, pages 287–310, Sea Island, Georgia, USA, 1982. [MRRS00] D. Mayne, J. Rawlings, C. Rao, and P. Scokaert. Constrained model predictive control: Stability and optimality. Automatica, 36:790–814, 2000. Survey Paper. [MTK00] B. Mettler, Mark Tischler, and Takeo Kanade. System identification of a modelscale helicopter. Technical Report CMU-RI-TR-00-03, Robotics Institute, Pittsburgh, PA, January 2000. [MvNB94] John C. Morris, Michiel van Nieuwstadt, and Pascale Bendotti. Identification and control of a model helicopter in hover. In Proceedings of the American Control Conference, pages 1238–1242, Baltimore, MD, USA, June 1994. [NO02] Erik Naesset and Tonje Okland. Estimating tree height and tree crown properties using airborne scanning laser in a boreal nature reserve. Remote Sensing of Environment, 79(1):105–115, January 2002. ´ Data association in stochastic mapping using the joint [NT01] J. Neira and J.D. Tardos. compatibility test. IEEE Transactions on Robotics and Automation, 17(6):890–897, December 2001. [NW99] Jorge Nocedal and Stephen Wright. Numerical Optimization. Springer Series in Operation Reasearch. Springer, 1999. [Nyq32] H. Nyquist. Regeneration theory. Bell System Technical Journal, 11:126–147, 1932. 217

Bibliography [Pad96] Gareth D. Padfield. Helicopter Flight Dynamics: The Theory and Application of Flying Qualities and Simulation Modeling. AIAA Education Series. AIAA, Washington DC, 1996. [PH74] T. Pavlidis and S.L. Horowitz. Segmentation of plane curves. IEEE Transactions on Computers, C-23:860–870, 1974. [PH08] Gerard Parke and Nigel Hewson, editors. ICE Manual of Bridge Engineering. Institution of Civil Engineers, London, UK, second edition, 2008. [PKK00] F. C. Park, Junggon Kim, and Changdon Kee. Geometric descent algorithms for attitude determination using the global positioning system. AIAA Journal of Guidance, Control and Dynamics, 23(1):26–33, January-February 2000. [PN62] L.S. Pontryagin and L.W. Neustadt. The Mathematical Theory of Optimal Processes. Number 4 in Classics of Soviet Mathematics. Gordon and Breach Science Publishers, 1962. [Pol06] Vitruvius Pollio. De Architectura (Ten Books on Architecture): Book IX. Project Gutenberg, 2006. Written around 15 BC, translated by Morris H. Morgan in 1914, and available at http://www.gutenberg.org/etext/20239. [Pra02] M. Prado. Modeling and control of an autonomous oceanographic vehicle. Master’s thesis, Instituto Superior Técnico, Lisbon, 2002. [Pro63] A. I. Propoi. Use of linear programming methods for synthesizing sample-data automatic systems. Automation and Remote Control, 24:837, 1963. [Pro95] Raymond W. Prouty. Helicopter Performance, Stability, and Control. Krieger, 1995. [PSC06] N. Paulino, C. Silvestre, and R. Cunha. Affine parameter-dependent preview control for rotorcraft terrain following. AIAA Journal of Guidance, Control, and Dynamics, 29(6):1350–1359, 2006. [QB03] S. Joe Qin and Thomas A. Badgwell. A survey of industrial model predictive control technology. Control Engineering and Practice, 11:733–764, 2003. [Qua63] Glenn Quasius. A topological approach to the analysis of star tracking systems. Aerospace and Navigational Electronics, IEEE Transactions on, ANE-10(3):206–212, 1963. [Ree20a] Abraham Rees. The Cyclopaedia; or, Universal Dictionary of Arts, Sciences, and Literature. Number 8. Longman, Hurst, Rees, Orme, & Brown, London, 1820. Available at http://archive.org/details/cyclopaediaoruni08rees. [Ree20b] Abraham Rees. The Cyclopaedia; or, Universal Dictionary of Arts, Sciences, and Literature. Plates. Number 2. Longman, Hurst, Rees, Orme, & Brown, London, 1820. Available at http://archive.org/details/cyclopaediaoru02rees. [RL01] S. Rusinkiewicz and M. Levoy. Efficient variants of the icp algorithm. In Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling, pages 145 –152, 2001. [RM93] James B. Rawlings and Kenneth R. Muske. The stability of constrained receding horizon control. IEEE Transactions on Automatic Control, 38(10):1512–1516, October 1993. [RMW+ 97] J. R. Ridgway, J. Minster, N. Williams, J. L. Bufton, and W. B. Krabill. Airborne laser altimeter survey of long valley, california. Geophysical Journal International, 131(2):267–280, November 1997. 218

Bibliography [Ros11] Paulo Rosa. Multiple-Model Adaptive Control of Uncertain LPV Systems. PhD thesis, Instituto Superior Técnico, 2011. [Rou77] Edward John Routh. Treatise on the stability of a given state of motion, particularly steady motion. Macmillan and co., 1877. [RRTP78] J. Richalet, A. Rault, J. L. Testud, and J. Papon. Model predictive heuristic control: applications to industrial processes. Automatica, 14:413–428, 1978. [RS00] W. J. Rugh and J. S. Shamma. Research on gain scheduling. 36(10):1401–1425, October 2000. Survey Paper.

Automatica,

[Rug95] W. Rugh. Linear System Theory. Prentice-Hall, Inc., 2nd edition, 1995. [RV11] Ioannis A. Raptis and Kimon P. Valavanis. Linear and nonlinear control of smallscale unmanned helicopters. In S.G. Tzafestas, editor, Intelligent Systems, Control, And Automation: Science And Engineering, volume 45. Springer, 2011. [SA68] L. Silverman and B. Anderson. Controllability, observability and stability of linear systems. SIAM Journal on Control, 6(1):121–130, 1968. [SB00] G. Sutton and R. Bitmead. Computational implementation of nonlinear model predictive control to nonlinear submarine. In F. Allgöwer and A. Zheng, editors, Nonlinear Model Predictive Control, Progress in Systems and Control Theory, pages 461–471. Birkhäuser Verlag, Basel-Boston-Berlin, 2000. [SC86] Randall C. Smith and Peter Cheeseman. On the representation and estimation of spatial uncertainty. International Journal of Robotics Research, 5(4):56–68, 1986. [Sch66] Peter Schönemann. A generalized solution of the orthogonal procrustes problem. Psychometrika, 31:1–10, 1966. [Sch01] T. Schenk. Modeling and analysing systematic errors of airborne laser scanners. Technical Notes in Photogrametry 19, Department of Civil and Environmental Engineering and Geodetic Science, The Ohio State University, Columbus, OH., 2001. [SD82] S. Sastry and C. Desoer. The robustness of controllability and observability of linear time-varying systems. IEEE Transactions on Automatic Control, 27(4):933– 939, August 1982. [Sib78] Robin Sibson. Studies in the robustness of multidimensional scaling: Procrustes statistics. Journal of the Royal Statistical Society. Series B (Methodological), 40(2):pp. 234–238, 1978. [Sib79] Robin Sibson. Studies in the robustness of multidimensional scaling: Perturbational analysis of classical scaling. Journal of the Royal Statistical Society. Series B (Methodological), 41(2):pp. 217–229, 1979. [Sil00] Carlos Silvestre. Multi-objective Optimization Theory with Applications to the Integrated Design of Controllers/Plants for Autonomous Vehicles. PhD thesis, Instituto Superior Técnico, Universidade Técnica de Lisboa, Lisbon, Portugal, June 2000. [SK84] W. Z. Stepniewski and C. N. Keys. Rotary-wing Aerodynamics. Dover, 1984. [SKS03] D. Shim, H. Kim, and S. Sastry. Decentralized reflective model predictive control of multiple flying robots in dynamic environment. Conference on Decision and Control, 2003. U.S.A. [SPK02] C. Silvestre, A. Pascoal, and I. Kaminer. On the design of gain-scheduled trajectory tracking contollers. International Journal of Robust and Nonlinear Control, 12(9):797– 839, July 2002. 219

Bibliography [SSC90] Randall C. Smith, Mathew Self, and Peter Cheeseman. Estimating Uncertain Spatial Relationships in Robotics, pages 167–193. Springer-Verlag New York, Inc., New York, NY, USA, 1990. [Sti81] Stephen M. Stigler. Gauss and the invention of least squares. The Annals of Statistics, 9(3):pp. 465–474, 1981. [SW00] Carsten Scherer and Siep Weiland. Lecture notes on linear matrix inequality methods in control. Dutch Institute of Systems and Control, 2000. [Swe59] Peter Swerling. First order error propagation in a stagewise smoothing procedure for satellite observations. Technical report, Santa Monica, CA, 1959. [SWR10] Inna Sharf, Alon Wolf, and M.B. Rubin. Arithmetic and geometric solutions for average rigid-body rotation. Mechanism and Machine Theory, 45(9):1239 – 1251, 2010. [Tay71] E. G. R. Taylor. The Haven-Finding Art: A History of Navigation from Odysseus to Captain Cook. The Bodley Head Ltd, 1971. [TBF05] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics. The MIT Press, 2005. [TDH03] Sebastian Thrun, Mark Diel, and Dirk Hähnel. Scan alignment and 3-D surface modeling with a helicopter platform. In Proceedings of the 4th International Conference on Field and Service Robotics, July 2003. [TFBD01] Sebastian Thrun, Dieter Fox, Wolfram Burgard, and Frank Dellaert. Robust Monte Carlo localization for mobile robots. Artificial Intelligence, 128(1-2):99–141, 2001. ´ José Neira, Paul M. Newman, and John J. Leonard. Robust mapping [TNNL02] Juan D. Tardos, and localization in indoor environments using sonar data. International Journal of Robotics Research, 21:311–330, 2002. [UHS07] J. Underwood, A. Hill, and S. Scheding. Calibration of range sensor pose on mobile platforms. In Intelligent Robots and Systems, 2007. IROS 2007. IEEE/RSJ International Conference on, pages 3866–3871, 2007. [Ume91] S. Umeyama. Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence, 13(4):376 –380, April 1991. [VCS+ 11] J. F. Vasconcelos, B. Cardeira, C. Silvestre, P. Oliveira, and P. Batista. Discrete-time complementary filters for attitude and position estimation: Design, analysis and experimental validation. IEEE Transactions on Control Systems Technology, 19:181 – 198, January 2011. [Wei94] Martin Felix Weilenmann. Robuste Mehrgrössen-Regelung eines Helikopters. PhD thesis, Eidgenössischen Technischen Hochschule (ETH), Zurich, Switzerland, 1994. in German. [WG94] Martin F. Weilenmann and Hans P. Geering. Test bench for rotorcraft hover control. AIAA Journal of Guidance Control and Dynamics, 17(4):729–736, 1994. [Wie64] N. Wiener. Extrapolation, Interpolation, and Smoothing of Stationary Time Series: with Engineering Applications. MIT Press, 1964. [WLFP08] Andrew D. Wiles, Alexander Likholyot, Donald D. Frantz, and Terry M. Peters. A statistical model for point-based target registration error with anisotropic fiducial localizer error. IEEE Transactions on Medical Imaging, 27(3):378–390, March 2008. 220

Bibliography [XWL10] Longxu Xiao, Shihui Wei, and Hongbin Lin. Research on star-tracking correction technology for near-earth flight vehicles. Systems Engineering and Electronics, Journal of, 21(3):485–490, 2010. [YSK+ 09] Y. Yoon, J. Shin, H. Kim, Y. Park, and S. Sastry. Model-predictive active steering and obstacle avoidance for autonomous ground vehicles. Control Engineering and Practice, 17:741–750, 2009. [ZB09] Victor M. Zavala and Lorentz T. Biegler. The advanced-step NMPC controller: optimality, stability, and robustness. Automatica, 45(1):83–93, 2009. [ZvN96] Xiaoyun Zhu and Michiel van Nieuwstadt. The caltech helicopter control experiment. Technical Report CIT-CDS 96-009, California Institute of Technology (CDS), Pasadena, CA, USA, June 1996. [ZW62] L. A. Zadeh and B. H. Whalen. On optimal control and linear programming. IRE Transactions on Automatic Control, 7(4):45–46, July 1962.

221

List of Publications [1] Rita Cunha, Bruno Guerreiro, and Carlos Silvestre. Vario-xtreme helicopter nonlinear model: Complete and simplified expressions. Technical report, Instituto Superior Técnico, Institute for Systems and Robotics, September 2005. [2] B. J. Guerreiro, C. Silvestre, R. Cunha, and D. Antunes. Trajectory tracking H2 controller for autonomous helicopters: an application to industrial chimney inspection. In 17th IFAC Symposium on Automatic Control in Aerospace, June 2007. DOI:10.3182/20070625-5FR-2916.00074. [3] B. Guerreiro, C. Silvestre, P. Oliveira, and J.F. Vasconcelos. Nonlinear and geometric optimization methods for LADAR calibration. In IEEE International Conference on Robotics and Automation, 2008, pages 1406 –1411, May 2008. DOI:10.1109/ROBOT.2008.4543399. [4] B. J. Guerreiro, C. Silvestre, and R. Cunha. Terrain avoidance model predictive control for autonomous rotorcraft. In 17th IFAC World Congress, July 2008. DOI:10.3182/200807065-KR-1001.00186. [5] B.J. Guerreiro, C. Silvestre, R. Cunha, C. Cao, and N. Hovakimyan. L1 adaptive control for autonomous rotorcraft. In American Control Conference, 2009, pages 3250 –3255, June 2009. DOI:10.1109/ACC.2009.5159940. [6] B.J. Guerreiro, C. Silvestre, R. Cunha, and A. Pascoal. Trajectory tracking nonlinear model predictive control for autonomous surface craft. In European Control Conference, August 2009. [7] B. J. Guerreiro, C. Silvestre, and R. Cunha. Laser-based trajectory tracking H2 control of autonomous rotorcraft. In 18th IFAC Symposium on Automatic Control in Aerospace, September 2010. DOI:10.3182/20100906-5-JP-2022.00010. [8] J.F. Vasconcelos, C. Silvestre, P. Oliveira, and B. Guerreiro. Embedded UAV model and laser aiding techniques for inertial navigation systems. Control Engineering Practice, 18(3):262 – 278, 2010. DOI:10.1016/j.conengprac.2009.11.004. [9] B. Guerreiro, C. Silvestre, and P. Oliveira. Automatic LADAR calibration methods using geometric optimization. In 2011 IEEE International Conference on Robotics and Automation (ICRA), pages 969 –974, May 2011. DOI:10.1109/ICRA.2011.5979729. [10] B. J. Guerreiro, P. Batista, C. Silvestre, and P. Oliveira. Sensor-based simultaneous localization and mapping – part I: Gas robocentric filter. In American Control Conference (ACC), 2012, pages 6352 –6357, June 2012. [11] B. J. Guerreiro, P. Batista, C. Silvestre, and P. Oliveira. Sensor-based simultaneous localization and mapping – part II: Online inertial map and trajectory estimation. In American Control Conference (ACC), 2012, pages 6334 –6339, June 2012. [12] B. J. Guerreiro, C. Silvestre, and R. Cunha. Terrain avoidance nonlinear model predictive control for autonomous rotorcraft. Journal of Intelligent & Robotic Systems, 68(1):69 –85, 2012. DOI:10.1007/s10846-012-9669-6. [13] B. J. Guerreiro, P. Batista, C. Silvestre, and P. Oliveira. Globally asymptotically stable sensor-based simultaneous localization and mapping. IEEE Transactions on Robotics, 29(6), 2013. DOI:10.1109/TRO.2013.2273838. [14] B. J. Guerreiro, C. Silvestre, R. Cunha, and A. Pascoal. Trajectory tracking nonlinear model predictive control for autonomous surface craft. IEEE Transactions in Control Systems Technology, 2013. Conditionally accepted. 223

List of Publications [15] B. J. Guerreiro, C. Silvestre, R. Cunha, and A. Pascoal. Trajectory tracking nonlinear model predictive control for autonomous surface craft. In European Control Conference (ECC), pages 3006–3011, July 2013. [16] B. J. Guerreiro, C. Silvestre, and P. Oliveira. Automatic 2-D LADAR geometric calibration of installation bias. Robotics and Autonomous Systems, 2013. submitted. [17] P. Lourenço, B. J. Guerreiro, P. Batista, P. Oliveira, and C. Silvestre. 3-D inertial trajectory and map estimation: building on a gas sensor-based SLAM algorithm. In European Control Conference (ECC), pages 4214–4219, July 2013. [18] P. Lourenço, B. J. Guerreiro, P. Batista, P. Oliveira, and C. Silvestre. Preliminary results on globally asymptotically stable simultaneous localization and mapping in 3-D. In American Control Conference (ACC), pages 3093–3098, June 2013.

224

Sensor-based Control and Localization of Autonomous Vehicles in ... [PDF]

Sensor-based Control and Localization of Autonomous Vehicles in ... [PDF]

Suggest Documents

Sensor-based Control and Localization of Autonomous Vehicles in ...

Cooperative Control of Distributed Autonomous Vehicles in ...

Model Predictive Control of Autonomous Vehicles

Coverage Control of Autonomous Vehicles for Oil

Autonomous Control of Underground Mining Vehicles ... - CiteSeerX

Control engineering of autonomous cognitive vehicles - EUCognition

Autonomous Underwater Vehicles: Modeling, Control ...

Unmanaged Intersection Control for Autonomous Vehicles - IFAAMAS

Adaptive Control for Autonomous Underwater Vehicles

Cross-Track Control for Underactuated Autonomous Vehicles

Autonomous Underwater Vehicles: Modeling, Control ... - EPDF.TIPS

Connected Vehicles and Autonomous Vehicles - Experts.com

Stochastic Predictive Control of Autonomous Vehicles in Uncertain

Autonomous Vehicles - Munich Re [PDF]

A localization approach for autonomous underwater vehicles: A ROS

Road DNA Based Localization for Autonomous Vehicles

Fuzzy Logic Control for Energy Saving in Autonomous Electric Vehicles

integrated vehicle dynamics control in autonomous vehicles: a real ...

Autonomous Vehicles and Autonomous Driving in ... - Springer Link

Autonomous Vehicles and Autonomous Driving in Freight Transport ...

Longitudinal and Lateral Control for Autonomous Ground Vehicles

Autonomous underwater vehicles - CiteSeerX

AUTONOMOUS UNDERWATER VEHICLES - AUVAC

Autonomous Vehicles - Munich Re