Guidance Navigation and Control Algorithms for High Dynamics Vehicles

(1)

TESIS DOCTORAL

Guidance Navigation and Control

Algorithms for High Dynamics Vehicles

Autor:

Ra´

ul de Celis Fern´

andez

Director:

Luis Cadarso Morga

Programa de Doctorado en TECNOLOG´IAS DE LA INFORMACI ´ON Y LAS COMUNICACIONES

Escuela Internacional de Doctorado

(2)

(3)

(4)

(5)

Abstract

Accuracy and precision are the cornerstone for ballistic projectiles from the earliest days of

this discipline. In the beginnings, impact point precision in artillery devices deteriorated

when range was extended, particularly for ballistic artillery rockets and shells, which are

not propelled except during the launch. Later, inertial navigation and guidance systems

were introduced and precision was unlinked from range increases. In the last thirty

years, hybridization between inertial systems and GNSS devices has improved precision

enormously.

Unfortunately, during the last stages of flight, inertial and GNSS methods (hybridized

or not) feature big errors in attitude and position determination. Low cost devices, which

are precise on terminal guidance and do not feature accumulative error, such as quadrant

photo-detector, seem to be appropriate to be included in the guidance systems. Hybrid

algorithms, which combine GNSSs, IMUs and photo-detectors, are required to implement

these novel techniques.

The acceleration autopilot with a rate loop is the most commonly implemented

au-topilot, which has been extensively applied to high-performance missiles. Nevertheless,

for high speed spinning rockets, the design of the guidance and control modules is a

chal-lenging task because the rapid spinning of the body creates a heavy coupling between the

normal and lateral rocket dynamics.

Hybridized measurements are implemented in modified proportional navigation law

(6)

par-ticularized for a high spinning ballistic rocket, has been developed to perform simulations

to prove the accuracy of the presented algorithms.

The research process developed to obtain the final results implied the following steps:

1. The development of a flight model in order to simulate the dynamics of a highly

spinning rocket which features a decoupled fuse.

2. The development of a novel 3D guidance law, based on a modular rotatory force, for

gyroscopically stabilized artillery rockets (i.e., spin rates in the hundreds of rotations

per second during the launch), which is derived from proportional navigation.

3. A model for a quadrant photo-detector based on a real-time area intersection

algo-rithm was developed and the subsequent development of a novel algoalgo-rithm which

improves the precision of spot center determination for a Semi-active Laser quadrant

detector in the terminal guidance of artillery rockets.

4. The integration of this photo-detector spot center determination algorithm and the

hybridization with GNSS/IMU in order to improve the precision of the line of sight.

5. The development of an algorithm, based on an estimation method in order to obtain

the gravity and velocity vectors in a different pair of triads, which aims at avoiding

gyroscopes for attitude determination.

6. The integration of these attitude determination methods together with the aid of

filtering techniques, into the previously photo-detector, GNSS, IMU, and control

rotatory force, developed algorithms.

Finally, nonlinear simulations based on ballistic rocket launches were performed to

demonstrate the applicability of the proposed solution for flight navigation, guidance and

control, for ballistic rocket terminal guidance.

(7)

(8)

(9)

Resumen

La exactitud y la precisi´on han sido la piedra angular de los proyectiles bal´ısticos desde los

primeros d´ıas de esta disciplina. En los comienzos, la precisi´on del punto de impacto en los

dispositivos de artiller´ıa se deterioraba a mediada que el alcance del proyectil se extend´ıa,

especialmente para aquellos cohetes y proyectiles bal´ısticos que no est´an propulsados

excepto en el lanzamiento. Posteriormente, fueron introducidas la navegaci´on inercial y

los sistemas de guiado y consecuentemente la precisi´on se deslig´o del alcance. En los

´

ultimos treinta a˜nos, la hibridaci´on entre dispositivos GNSS y los sistemas inerciales ha

aumentado la precisi´on enormemente.

Desafortunadamente, durante las ´ultimas etapas del vuelo, los m´etodos inerciales y

GNSS (hibridizados o no) desencadenan grandes errores en la determinaci´on tanto de la

posici´on como de la actitud. Dispositivos de bajo coste, que pueden ser precisos durante

el guiado terminal y que no est´an sometidos a errores acumulativos, tales como los

foto-detectores de cuadrante, parecen ser adecuados para ser incluidos dentro de los sistemas de

guiado. Se requiere, por tanto, el desarrollo de algoritmos de hibridaci´on que combinen

sensores de GNSS, IMUs y foto-detectores, para la implementaci´on de estas novedosas

t´ecnicas.

Por otra parte, el autopiloto giro-acelerométrico, es el tipo de autopiloto más común en

estos dispositivos, y ha sido ampliamente implementado en misiles de altas prestaciones.

Sin embargo, para cohetes con altas velocidades de rotación, el diseño de los módulos de

(10)

alto acoplamiento entre la din´amica normal y lateral del cohete.

Se presenta como solución un método en el que una hibridación de medidas obtenidas

de los sensores son introducidas en una ley de navegaci´on proporcional en la que act´ua

una fuerza rotatoria. Para validar la precisi´on de los algoritmos implementados se ha

desarrollado un modelo no lineal de mec´anica de vuelo, particularizado para un cohete

bal´ıstico de alta rotaci´on.

El proceso de investigación se desarrolló siguiendo los pasos mostrados a continuación:

1. Desarrollo de un modelo de mec´anica de vuelo que simula el comportamiento f´ısico

de un cohete bal´ıstico de alta rotaci´on con una espoleta desacoplada.

2. Desarrollo de una novedosa ley de guiado tridimensional, basada en una fuerza

rotatoria modulable, para cohetes de artiller´ıa estabilizados girosc´opicamente (tasas

de rotaci´on de cientos de revoluciones por segundo en el lanzamiento), derivada de

la navegaci´on proporcional.

3. Realizaci´on de un modelo para un foto-detector de cuadrante basado en un m´etodo

de intersección de áreas, calculado en tiempo real, que desembocó en el consecuente

desarrollo de un algoritmo para la mejora de la precisi´on en la determinaci´on del

centro de la huella de un l´aser en un foto-detector de cuadrante para el guiado de

cohetes bal´ısticos de artiller´ıa

4. La integración e hibridación del foto-detector y el algoritmo de determinación de la

posici´on del centro, junto con sensores GNSS e IMUs con el objetivo de mejorar la

precisi´on en la determinaci´on de la l´ınea de mira.

5. El desarrollo de un algoritmo, basado en la estimaci´on del vector gravedad y el

vector velocidad en una pareja de triedros diferentes, con el objetivo de eliminar los

gir´oscopos en la determinaci´on de la actitud del veh´ıculo.

(11)

6. La integración de esos métodos de determinación de actitud, con la ayuda de técnicas

de filtrado, en el desarrollo previo de algoritmos h´ıbridos del foto-detector, GNSS e

IMU y la fuerza rotatoria de control.

Finalmente, se realizaron simulaciones no lineales basadas en lanzamientos de cohetes

bal´ısticos para demostrar la aplicabilidad de la soluci´on propuesta para la navegaci´on,

control y gu´ıado terminal del cohete bal´ıstico.

(12)

(13)

Acknowledgments

First, I would like to thank my research adviser, Professor Luis Cadarso Morga. This

research would have not been possible without his advice and contributions. He was

always available to discuss the research related topics.

I am fortunate to have worked with the researchers in ’Instituto Tecnologico La

Mara-nosa’ at INTA and for the help, data and solid modeling they provided in the very early

stages of this research.

Special thanks to Manuel Fuentes Gonzalez, who helped me in the first stages of the

development of the non-linear simulation model.

I have to thank the Department of Signal Theory, Communications, Telematic systems

and Computation from the Higher Technical School of Telecommunications Engineering

from Universidad Rey Juan Carlos, Higher Technical School of Telecommunications

En-gineering from Universidad Rey Juan Carlos, the Universidad Rey Juan Carlos and the

European Institute For Aviation Training And Accreditation from Universidad Rey Juan

Carlos for their support.

I also want to thank my friends who have suggested me minor and/or major changes

to this dissertation.

Finally, my deep and sincere gratitude to my family for their continuous and

unpar-alleled love, help and support. I am grateful to Johanna for always being there and for

putting up with me all these years. I am forever indebted to my parents for giving me the

(14)

me to explore new directions in life and seek my own destiny. This journey would not

have been possible if not for them, and I dedicate this milestone to them.

(15)

List of Figures

4.i Thrust curve depending of flight time. . . 29

4.ii Aerodynamic coefficients for the rocket under study depending of the Mach

number. . . 30

4.iii Reference systems for a 140 mm axisymmetric rocket with wrap around

fins and a roll-decoupled fuse. . . 31

4.iv A ballistic trajectory for an initial θ of 40 degrees. . . 35

4.v Initial elevation angle and ACorr depending on target distance to launch

point. . . 36

4.vi Controller scheme. . . 38

4.vii Example Trajectory. . . 39

4.viii Response of the rocket to different step commands in the control variable. . 40

4.ix Complete dispersion area of all the flights. . . 41

4.x Impact point dispersion patterns for ballistic and controlled flights for a

target distance onf 18500 m. . . 43

4.xi Impact point dispersion patterns for ballistic and controlled flights for a

4.xii Impact point dispersion patterns for ballistic and controlled flights for a

(24)

4.xiii Impact point dispersion patterns for ballistic and controlled flights for a

4.xiv Impact point dispersion patterns for ballistic and controlled flights for a

4.xv Histogram of number of impacts for each distance to the target. . . 48

5.i Standard (left) and cross (right) quadrant detector configuration. . . 56

5.ii Photo-detector Scheme . . . 57

5.iii Intensity registered under foggy laboratory conditions tests . . . 57

5.iv Error for Non-dimensional Intensity registered under foggy laboratory

con-ditions vs proposed area method . . . 58

5.v Illuminated area for spot-detector ratio of 0.25. . . 61

5.vi Illuminated area for spot-detector ratio of 0.50. . . 62

5.vii Illuminated area for spot-detector ratio of 0.75. . . 63

5.viii Illuminated area for spot-detector ratio of 1.00. . . 64

5.ix Photo-detector spot coordinates geometric composition . . . 65

5.x Transformation developed by method 1 . . . 66

5.xi Transformation developed by method 2 . . . 67

5.xii Transformation developed by method 3 . . . 68

5.xiii Transformation developed by method 4 . . . 69

5.xiv Example of trajectory and grid of 81 impact points . . . 73

5.xv Detector footprint for a shot angle of 30◦ with a spot-detector ratio of 0.25. 74 5.xvi Detector footprint for a shot angle of 30◦ with a spot-detector ratio of 0.50. 75

5.xviiDetector footprint for a shot angle of 30◦ with a spot-detector ratio of 0.75. 76

5.xviiiDetector footprint for a shot angle of 30◦ with a spot-detector ratio of 1.00. 77

5.xix Detector footprint for a shot angle of 45◦ with a spot-detector ratio of 0.25. 78

5.xx Detector footprint for a shot angle of 45◦ with a spot-detector ratio of 0.50. 79

(25)

5.xxi Detector footprint for a shot angle of 45◦ with a spot-detector ratio of 0.75. 80

5.xxiiDetector footprint for a shot angle of 45◦ with a spot-detector ratio of 1.00. 81

5.xxiiiDetector footprint for a shot angle of 60◦ with a spot-detector ratio of 0.25. 82

5.xxivDetector footprint for a shot angle of 60◦ with a spot-detector ratio of 0.50. 83

5.xxvDetector footprint for a shot angle of 60◦ with a spot-detector ratio of 0.75. 84

5.xxviDetector footprint for a shot angle of 60◦ with a spot-detector ratio of 1.00. 85

6.i 140 mm axisymmetric spinning rocket with wrap around fins, decoupled 2

by 2, a roll-decoupled fuse and its actuation force. . . 91

6.ii Reference systems. . . 93

6.iii Ballistic trajectories for initial pitch angles (θ0) of 20, 30 and 45 degrees. . 94

6.iv Incidence aerodynamic speed decomposition, local angle of attack (αi), and

fin deflection (δi). . . 95

6.v dax(M) and dlat scheme. . . 97

6.vi Quadrant photo-detector configuration used. . . 99

6.vii Results for hybridization algorithm. . . 103

6.viii Control system scheme. . . 107

6.ix Ballistic shots for 20◦, 30◦ and 45◦ initial pitch angles. . . 110

6.x Detailed shots for different algorithms. . . 111

7.i Trajectories for initial pitch shot angle of 15◦, 45◦ and 75◦ and 8 different azimuths. . . 128

7.ii Comparison between estimated and real magnitudes for Euler Angles for a

shot angle of 45◦ and azimuths of 0◦ (left), 45◦ (center) and 90◦ (right). . . 130

7.iii Comparison between estimated and real magnitudes for gravity vector for

(26)

7.iv Comparison between estimated and real magnitudes for aircraft angular

velocity for a shot angle of 45◦ and azimuths of 0◦ (left), 45◦ (center) and 90◦ (right). . . 132 7.v Representation of euler angles RMSE along the whole trajectory for each

shot angle and for the 8 azimuths tested. . . 135

7.vi Representation of gravity vector RMSE along the whole trajectory for each

shot angle and for the 8 azimuths tested. . . 136

7.vii Representation of zB angular velocity vector RMSE along the whole

tra-jectory for each shot angle and for the 8 azimuths tested. . . 137

7.viii Scheme of UAV modeled for Simulations. . . 140

7.ix Trajectories 1 (top-left), 2 (top-right), 3 (center-left), 4 (center-right), 5

(bottom-left) and 6 (bottom-right) . . . 143

7.x Comparison between estimated and real magnitudes for Euler Angles for

an inclination path angle of 5◦ and trajectories 1 (left), 3 (center) and 5 (right). . . 145

7.xi Comparison between estimated and real magnitudes for gravity vector for

an inclination path angle of 5◦ and trajectories 1 (left), 3 (center) and 5 (right). . . 146

7.xii Comparison between estimated and real magnitudes for aircraft angular

ve-locity for an inclination path angle of 5◦ and trajectories 1 (left), 3 (center) and 5 (right). . . 147

7.xiii Representation of Euler angles RMSE along the whole trajectory for each

inclination path angle and for the 6 trajectories tested. . . 150

7.xiv Representation of gravity vector RMSE along the whole trajectory for each

inclination path angle and for the 6 trajectories tested. . . 151

7.xv Representation of angular velocity vector RMSE along the whole trajectory

for each inclination path angle and for the 6 trajectories tested. . . 152

(27)

(28)

(29)

List of Tables

4.I Representative data of the studied rocket. . . 29

4.II Monte Carlo initial condition distribution parameters. . . 41

4.III The CEP for each of the targets and for ballistic and controlled flights. . . 42

5.I Relationship angles between ideal and calculated spot center position . . . 71

5.II Correlations between ideal and calculated radial components . . . 72

5.III Quadratic error for the combination of each shot angle, spot-detector ratio

and methods . . . 86

6.I 140 mm axisymmetric spinning rocket aerodynamic coefficients. . . 92

6.II Interpolation between measured radial distance, rquad, and real radial

dis-tance, rc. . . 99

6.III Nominal trajectories’ parameters. . . 109

6.IV Monte carlo simulation parameters. . . 109

6.V Values for the constants on each flight phase. . . 110

6.VI Circular Error Probable in different cases. . . 112

7.I Root Mean Squared Errors along the whole trajectory for a set of shot

angles and for the 8 azimuths tested and calculation of a representative

mean parameter. . . 138

(30)

7.III Root Mean Squared Errors along the whole trajectory for a set of

incli-nation path angles and for the 6 trajectories tested and calculation of a

representative mean parameter. . . 153

8.I 140 mm axisymmetric rocket main parameters versus time. . . 160

8.II Nominal trajectories’ parameters. . . 167

8.III Monte carlo simulation parameters. . . 168

8.IV Values for the constants on each flight phase. . . 169

8.V Circle error probable for the different cases. . . 171

(31)

(32)

(33)

Part I

(34)

(35)

Chapter 1 GNC Algorithms for High Rotating

Speed Artillery Vehicles

This chapter gives a brief overview about the role that GNC Algorithms research play in

High Rotating Speed Artillery Vehicles.

1.1 Introduction

Artillery is the set of weapons of war designed to fire large projectiles over long distances

using an explosive charge or a rocket as a driving element. By extension, the military unit

that manages them is also called artillery. Every artillery piece has a firearm, a metal

tube of a certain caliber and length and a frame where it rests.

The invention of gunpowder - together with that of another artifact closely linked to

the former - the cannon - would constitute the first milestone that would begin the history

of artillery, well differentiated from the history of mere siege devices. In Europe, there

are several references in the 14th century to the use of primitive artillery pieces by the

Arabs at the site of Baza, and it is known that the army of Alfonso XI used it in 1312 at

(36)

is evidence of the use of a cannon that used stone balls as ammunition. The ammunition

used until the seventeenth century consisted usually of balls of stone or metal, suitable for

breaking down walls or attacking ships at sea, but with very little effect on the infantry or

cavalry, other than scaring the horses. They were very dangerous weapons to use which,

often, the nobles preferred to take to the battlefield mainly to intimidate.

During the seventeenth century the artillery did not change too much, since it remained

a dangerous tool. These weapons were a hindrance for the generals, who often had to

use two trios of horses to carry them. This is why the guns would remain during the

eighteenth century as a weapon to disorganize enemy troops, rather than a weapon of

major destruction. Shortly after the Napoleonic wars, the howitzer appeared, a weapon

similar to the cannon but that allows for the first time what is called indirect shot in a

primitive form, that is, to attack positions that, being in the line of reach, are hidden by

elements of the land, walls, etc. thanks to that it allows inclinations of 45 deg or more.

In the second half of the nineteenth century, the artillery undergoes a revolution,

indi-rect shooting is generalized by topographic maps thanks to the improvement of shooting

control, using observers who have the position to beat in sight and that by telephone or

radio are providing to the command of the artillery the information to correct the shot.

These and not others, are the first attempts at improvement in the accuracy of artillery

weapons. In World War I, and thanks to the control of the recoil and the improvement of

the propulsion loads, artillery bombings are made at distances of more than 20 km and

even special cannons are mounted on railway rails that can bombard cities To 100 km of

distance, although the wear of the pieces is enormous and one must be changing the cane

continuously in this case.

From World War II until today, the main innovations have been the use of computers

to give a rapid calculation of the trajectory, including the embedded computing devices

as the subject that competes the development of this thesis, whereas before there was

to make several test shots and correct them, using observers if the target was at a great

(37)

distance.

In World War II rocket artillery appears, although it had previously been used in very

primitive forms, for example, in China since the thirteenth century, in India against the

British in the eighteenth century or Paraguay in the nineteenth century in its war against

the Triple Alliance. The British adopted the Congreve rocket as an incendiary weapon

and for its psychological effect on enemy rather than physical capabilities against the

infantry, at least at these times. In the nineteenth century rocket engineering continued

to study and improved especially so that after the launch maintained a regular trajectory

and increase its destructive capacity. Even in World War I, aviation rockets were used in

a limited way.

Rocket, unlike the missile, lacks a guidance system after its launch. It is used as

a weapon of saturation, to completely destroy an area, with heads of high explosive,

incendiary. For this purpose, several rockets are mounted on a rail or pipe guidance

system, and the whole assembly, on a moving vehicle or platform which is pointed at the

area to be destroyed and simultaneously fired by an electrical system. The classic Russian

Katiusha rockets of World War II, launched from platforms mounted on trucks are still

used today in modern versions, and they showed their potential by destroying a certain

range of firing. Even armies such as the United States, which for decades despised the

use of rockets as a crude weapon of antiquated armies, have in recent years incorporated

vehicles that allow them to launch a certain amount of rockets to saturate an area. It is

also that in recent years, the concept of asymmetric warfare has imposed cost reduction

and the consequent adaptation of these saturation rockets by intelligent rockets, equipped

(38)

1.2 Navigation, Guidance and Control

The purpose of a Guidance, Navigation and Control (GNC) system is to make the

ve-hicle move following a certain reference path, which meets certain requirements. These

requirements can be of different nature: impact on a target, arrival at a certain point,

orbit around a point, etc. To achieve this goal, the automatic GNC systems perform three

fundamental functions, which correspond to their acronyms:

- Guidance.

- Navigation.

- Control.

The guidance function, whose purpose is to calculate a reference state vector such that

the trajectory complies with what is required.

The navigation function is responsible for measuring or estimating the vehicle’s state

vector. Depending on the type of trajectory required, it will be necessary to know certain

kinematic variables of the vehicle in order to know the deviations that the actual trajectory

is undergoing with respect to the one of reference required and to be able to calculate the

necessary actions to approach them. Depending on the specific case, it will be necessary to

know the position, speed, acceleration or combinations of the components of the previous

ones. This function is developed by various devices, such as:

- Gyroscopes.

- Accelerometers.

- GPS Systems.

- Terrain Reference Systems (TRN or TFN).

- Doppler navigation systems.

- ...

The control function, which aims to make the state vector itself evolve so that it

approaches the reference. The control function commands these actions based on the

(39)

deviation that exists, at any moment, between the state vector and the reference vector.

The execution of these control actions will be carried out by the actuators available on

(40)

(41)

Chapter 2 Thesis Outline

This dissertation is divided in five main parts. The first part presents an introduction

to the the role that GNC Algorithms research play in High Rotating Speed Artillery

Vehicles; then, the thesis outline and contributions are presented. The second part of

this dissertation presents the guidance and control methods for high dynamic rotating

artillery rockets, including a dissertation on the mathematical model employed and the

presentation of the rotating force control method. The third part presents the algorithms

for spot-center determination in a GNSS/IMU laser quadrant photo-detector and its

hy-bridization techniques for artillery rocket terminal-guidance. The forth part, describes a

novel aircraft attitude determination algorithm which aims on avoiding gyroscopes and

the related hybridization techniques for artillery rocket terminal-guidance. Finally, the

fourth part enumerates the conclusions of the research presented in this dissertation and

the future research.

In the following sections, we briefly outline the contents of the chapters in this

(42)

2.1 Chapter 3: Thesis Contributions

This chapter briefly describes the main contributions to the literature of the research

presented in this dissertation.

2.2 Chapter 4: Flight Dynamics, Navigation,

Guid-ance and Control for High Dynamic Rotating

Ar-tillery Rockets

The acceleration autopilot with a rate loop is the most commonly implemented autopilot,

which has been extensively applied to high-performance missiles. However, for spinning

rockets, the design of the guidance and control modules is a challenging task because

the rapid spinning of the body creates a heavy coupling between the normal and lateral

rocket dynamics. Nonlinear modeling of the rocket dynamics, control design as well as

guidance algorithms are performed and discrete-time guidance and control algorithms for

the terminal phase, which is based in proportional navigation, are performed. Finally,

complete nonlinear simulations based on realistic scenarios are developed to demonstrate

the robustness of the proposed solution with respect to uncertain launch, environment

and rocket conditions. The performance of the proposed navigation, guidance and control

system for a high-spin rocket leads to significant reductions in impact point dispersion.

(43)

2.3 Chapter 5: Algorithms for Spot-Center

Determi-nation in Semi-Active Laser-Detector for

Terminal-Guidance

Precision of guided projectiles is dependent both on the precision of the measurement

system used for location determination and the precision in setting the coordinates of the

target. Development of algorithms for low-cost high-precision terminal guidance systems

is a cornerstone in research in this field. Semi Active Laser Kits (SAL), and particularly

quadrant detector devices, have been developed in order to improve precision in guided

weapons. Photo-detection system can be functionally divided into two main parts:

sens-ing and processsens-ing. The sensed signal is processed to estimate the spot coordinates, i.e.,

the laser footprint, and to obtain the needed information for the navigation and guidance

algorithms. Using an interpolation algorithm based on the four electrical intensities

ob-tained in a semi active laser quadrant photo-detector, laser footprint center estimation

is improved for artillery applications. The electrical intensities that real sensor provides

under laboratory conditions are compared to a mathematical model based on area

in-tersection calculations in order to simulate the intensities on real flights. From these

intensities, four different processing algorithms are tested for different spot sizes so as to

obtain correlation between real spot center position and estimated position calculated by

algorithms. Finally, an example illustrating a nonlinear real-life projectile is employed

to compare real and calculated laser footprints in order to select the best algorithm for

(44)

2.4 Chapter 6: GNSS/IMU Laser Quadrant

Detec-tor Hybridization Techniques for Artillery Rocket

Guidance

In the past, ballistic rockets impact point precision deteriorated at the same time as

range was extended, specially for those rockets which were non-propelled and non-guided

amid the greater part of their trajectories. Once that inertial and GNSS navigation and

guidance systems were introduced, precision was unlinked from range increments. The

fundamental issue from these inertial and GNSS strategies (hybridized or not) is the

enormous errors on attitude and position determination during last phases of flight as

the movement is governed by aerodynamic forces and moments. Consequently, the

aero-dynamic forces model has a deeply nonlinear character. Choosing another kind of low

cost sensors, independent of accumulative errors and precise on terminal guidance, for

example, quadrant photo-detector semi-active laser, is crucially essential. Hybridization

nonlinear algorithms, such as extended Kalman filter, joining measurements from sensors

such as Global Navigation Satellite System (GNSS), Inertial Measurement Units (IMUs)

and photo-detectors are described in this paper to be utilized on modified proportional

navigation techniques and novel control methods. The results are tested on rocket

non-linear flight simulations in order to prove accuracy of proposed algorithms.

(45)

2.5 Chapter 7: Aircraft Attitude Determination

Al-gorithms through Accelerometers, GNSS Sensors

and Gravity Vector Estimator

Aircraft and spacecraft navigation precision is dependent on the measurement system for

position and attitude determination. Rotation of an aircraft can be determined

measur-ing two vectors in two different reference systems. Velocity vector can be determined

in inertial reference frame from a GNSS-based sensor and by integrating the

accelera-tion measurements in body reference frame. Estimating gravity vector in both reference

frames, and combining with velocity vector, determines rotation of the body. A new

approach for gravity vector estimation is presented, and employed in an attitude

deter-mination algorithm. Nonlinear simulations demonstrate that, using GNSS sensors and

strap-down accelerometers, aircraft attitude determination is precise, especially in

ballis-tic projectiles, allowing for substitution of precise attitude determination devices, which

are usually expensive and forced to bear high solicitations as for instance G forces.

2.6 Chapter 8: Hybridized Attitude Determination

Techniques to Improve Ballistic Projectile

Navi-gation, Guidance and Control

Precise rotation determination is an expensive task in aircraft, as it is usually determined

by strap-down sensors such as fiber optic gyros or MEMS. Particularly in ballistic

projec-tiles, these gyro determination devices increase their price as they need to bear enormous

accelerations during the initial stages but not during the ballistic flight. A new approach to

(46)

At-titude determination methods and gravity vector estimation method, is presented in this

paper. Measurements of accelerometers, GNSS-sensors and Semi-Active photo-detectors

are hybridized to get such a result. The attitude determination method, avoiding the use

of gyroscopes, measures pairs of vectors, i.e., gravity, velocity and line of sight vectors, in

a pair of reference systems, i.e., body fixed and north-east-down reference frames.

Grav-ity vector estimation is based on flight mechanics of a ballistic projectile, but it may be

extrapolated to any aircraft, and later employed in an attitude determination algorithm.

Modified proportional navigation techniques and previously developed control methods

are employed during flight. The presented approach is tested on realistic non-linear flight

simulations to prove accuracy of proposed algorithms.

2.7 Chapter 9: Thesis Conclusions

This chapter presents the main conclusions of this dissertation for each chapter in a

separate way.

2.8 Chapter 10: Future Research

This chapter presents future research it is going to be embarked in the near future.

(47)

Chapter 3 Thesis Contributions

In the following, they are described the major contributions of this thesis to the existing

literature. they are also detailed the contributions of each chapter in a separate way.

3.1 Chapter 4: Flight Dynamics, Navigation,

Guid-ance and Control for High Dynamic Rotating

Ar-tillery Rockets

The main contributions of this chapter are the development of a flight model in order

to simulate the dynamics of a highly spinning rocket which features a decoupled fuse.

The The previous model is employed in the development of a novel 3D guidance law for

gyroscopically stabilized artillery rockets (i.e., spin rates in the hundreds of rotations per

second), which is derived from proportional navigation. This novel control law allows

the development of a simple but effective and robust single-input single-output controller,

which is able to handle the heavy coupling between the normal and lateral rocket

dy-namics. This new concept allows simple and effective control algorithms which enables

(48)

in this chapter on a 140 mm rocket equipped with a rotating force mechanism and

non-controlled thrust. The rotating force mechanism consists of a decoupled fuse from the aft

part of the rocket. The main advantage of the overall setup is that, on the one hand, it

maintains the inherent dynamic stability properties of a rapidly spinning body due to the

aft part, while at the same time, the front part, remains easy to be fit to any unguided

rocket, hence transforming it into a guided one. Nonlinear simulations based on

realis-tic scenarios are performed to demonstrate the robustness of the proposed solution with

respect to uncertain launch, environment and rocket conditions. The chapter proceeds

as follows. First, the nonlinear rocket dynamic model is defined. Second, an integrated

navigation and guidance approach is presented, followed by design of a controller.

Ex-ample of ballistic and controlled flight simulation results are presented, which analyze

performance.

3.2 Chapter 5: Algorithms for Spot-Center

Determi-nation in Semi-Active Laser-Detector for

Terminal-Guidance

The main contribution of this chapter is the development of an algorithm which improves

the precision of spot center determination for a SAL quadrant detector for terminal

guid-ance of artillery rockets. Also, a model for the quadrant detector based on real-time area

intersection algorithm is developed.

Relevant parameters which influence sensor precision and performance are determined

and studied. Also, sensitivity analysis is performed on some of them. It is shown that

the transformation from the real to the estimated spot footprint made by the methods is

conformal. Based on this, an interpolation algorithm is proposed to improve the

perfor-mances.

(49)

Nonlinear simulations based on ballistic rocket launches are performed to obtain this

ideal spot position and compare it to the position obtained from the interpolation

algo-rithms, and to demonstrate the applicability of the proposed solution for artillery final

flight stages guidance.

3.3 Chapter 6: GNSS/IMU Laser Quadrant

Detec-tor Hybridization Techniques for Artillery Rocket

Guidance

The main contribution of this chapter is the development and integration of a novel

algorithm, based on an Extended Kalman Filter (EKF) hybridization between GNSS/IMU

and semi-active laser quadrant photo-detectors, which improves the precision of line of

sight (the vector between rocket center of masses and target) determination during the

terminal guidance of ballistic rockets, and in consequence the precision on impact point.

Laser quadrant photo-detector footprint centroid calculation is developed based on a

cubic spline interpolation. Note that the advantage of such a combined system over the

individual GNSS/IMU, even when the error of GNSS/IMU is reduced to extremely small

quantity by virtue of some technique, is the ability to avoid jamming and also impact on

points in a vertical plane.

The algorithm is based on based on an EKF hybridization which determines the

ter-minal line of sight to be used on a modified proportional navigation law and on a rotatory

control technique. The proposed control approach is based on a robust double-input

double-output controller. This controller is able to handle the heavy coupling between

the normal and lateral rocket nonlinear dynamics. The use of a flight mechanics model,

which takes in account the non-linearity in aerodynamic forces and moments, with the

(50)

spinning rocket, demonstrates the precision and applicability of these algorithms under

uncertain launch, environment and rocket conditions.

3.4 Chapter 7: Aircraft Attitude Determination

Al-gorithms through Accelerometers, GNSS Sensors

and Gravity Vector Estimator

The main contributions of this chapter are twofold. First, the development of a novel

algorithm which aims at avoiding gyroscopes for attitude determination. The central

idea is to decrease attitude sensors costs and even to improve attitude determination

by applying filtering techniques, especially for artillery device and UAV applications,

where high acceleration conditions increase the cost of precise attitude determination

sensors such as gyroscopes or low cost requirements are imposed, respectively. Second, the

development of an estimation method in order to obtain the gravity vector in body axes.

This estimator is motivated by the need of having two vectors expressed in two different

triads in order to determine attitude changes. Flight nonlinear simulations are performed

to determine real attitude and compare it to the estimated one. The applicability of the

proposed solution for aircraft flight navigation, guidance and control, or for ballistic rocket

terminal guidance, where attack and side-slip angles or total angle of yaw are relatively

small, is also demonstrated.

(51)

3.5 Chapter 8: Hybridized Attitude Determination

Techniques to Improve Ballistic Projectile

Navi-gation, Guidance and Control

The main contributions of this chapter are the development of a new algorithm which

substitutes gyroscopes in favor of lower cost sensors for attitude determination and the

employment of an estimation method in order to obtain the gravity vector in body axes,

which is only based on the aerodynamic parameters of the cell and the measurements

provided by accelerometers. The objective is to get simplicity in attitude sensors and

even to increase precision by applying filtering techniques, especially for artillery device

purposes, where high solicitation acceleration conditions increase the price of precise

at-titude determination devices such as gyroscopes. In order to get high precision at impact

points, multiple sensors are employed and a hybridization algorithm is employed so as to

to handle information. Mixing inaccurate signals, e.g., from GNSS and accelerometers,

and precise signals, e.g., from semi-active laser quadrant detector, enables the

determina-tion of a high fidelity line of sight. Non-linear flight simuladetermina-tions are performed in order to

prove the applicability of the proposed approach for ballistic rocket navigation, guidance

(52)

(53)

Part II

GUIDANCE AND CONTROL FOR

HIGH DYNAMIC ROTATING

(54)

(55)

Chapter 4 Flight Dynamics, Navigation,

Guidance and Control for High

Dynamic Rotating Artillery Rockets

4.1 Introduction

Precision has always been recognized as an important attribute of weapon development.

One of the greatest advantages of the precision weapon is the confidence that it can

minimize ”‘collateral damage”’. In addition, using force in some circumstances, would

be either unacceptable or call into question the viability of continued military action in

absence of precision weapons (Hamilton, 1995). A precision-guided munition (PGM) is

a guided munition intended to precisely hit a specific target, and to minimize collateral

damage. Because the damage effects of explosive weapons decrease with distance, even

modest improvements in accuracy enable a target to be attacked with fewer or smaller

bombs. The precision of these weapons is dependent both on the precision of the

measure-ment system used for location determination and the precision in setting the coordinates

(56)

is accurate. If the targeting information is accurate, satellite-guided weapons (including

inertial navigation in the event of signal loss) are significantly more likely to achieve a

successful strike in any given weather conditions than any other type of precision-guided

munition. Development of low-cost navigation, guidance and control technologies for

unguided rockets is a unique engineering challenge. Over the past several decades,

nu-merous solutions have been proposed, primarily for large artillery projectiles or for slowly

rolling airframes. Guided projectiles may be divided into three categories in terms of

the control mechanisms employed: aerodynamic surfaces (Morrison, Amberntson, 1977),

(Rogers,Costello, 2010), jet thrusters (Burchett,Costello, 2002), (Lloyd,Brown, 1979), and

inertial loads (Murphy, 1981), (Ollerenshaw, Costello, 1981). Guided projectile concepts

involving aerodynamic control surfaces can be divided into two categories: fin-stabilized

and spin-stabilized. Spin-stabilized guided projectiles are generally equipped with a

roll-decoupled trajectory correction fuse, designed to provide trajectory correction, while at

the same time they rely on the high spin rate of the aft part for airframe stability. But

the high spin rate creates an important coupling between the normal and lateral axes of

the body, which makes the dynamic characteristic rather complex. For such projectiles,

previous work has proven that flight instabilities occur for spin-stabilized projectiles

ma-neuvering perpendicular to the gravity field when the control effectiveness is sufficiently

high (Lloyd,Brown, 1979). In (Lin et al., 2000) it is stated that in a guidance system,

it is important to choose a suitable guidance law and navigation constant.

Investiga-tion and comparison of the system behavior of guidance laws under different navigaInvestiga-tion

constants is developed. In (Tyan, 2015) it is analyzed the capture region of the general

ideal proportional navigation guidance law. In (He et al., 2015) it is presented a new

longitudinal autopilot to address the finite-time tracking problem. In (Yeh, 2010) it is

addressed a nonlinear terminal guidance/autopilot controller with pulse-type control

in-puts. In (Wang et al., 2016) a three dimensional integrated guidance and control law with

impact angle constraint, is developed, using the dynamic surface control and extended

(57)

state observer techniques. In (Mohammadi et al., 2016) a decrement of coupling effects

of a tactical missile by designing a robust autopilot for its roll channel is developed. In

(Chen, 2016) a description of an application of a new terminal guidance law for missiles in

three-dimensional (3D) environment during the terminal phase is performed. A guidance

and control strategy for a class of 2D trajectory correction fuse with fixed canards for

a spinning projectile is developed in (Yi et al., 2015). Correction control mechanism is

researched through studying the deviation motion and impact point deviation prediction

based on perturbation theory. In (Creagh, Mee, 2010) it is detailed the design and

simu-lation of an attitude guidance and control scheme for a spinning aerospace vehicle. Two

single-input/single-output controllers are used, which in turn issue flap deflection

com-mands. In (Li et al., 2012) it is noticed that the stable region of the design parameters

for the autopilot shrinks significantly under the spinning condition. It is also observed

that the stable region for design parameters is further narrowed when an integrator is

introduced into the acceleration loop while the steady-state accuracy is dramatically

im-proved. In (Zhou et al., 2013) it is also observed that an unexpected and unstable coning

motion occur for spinning missiles after burnout. To address this unstable motion, the

governing equation of the coning motion, and the dynamics of the fin actuators under

the associated hinge moment is derived. The necessary and sufficient conditions of the

coning motion stability are then analytically derived and further validated through

non-linear six degrees-of-freedom simulations for a representative scenario. A discrete-time,

proportional-derivative navigation guidance law, for the terminal phase of an engagement,

with emphasis on the effect of a digital implementation is proposed in (Lechevin,

Rab-bath, 2012). In (Theodoulis et al., 2013) it is presented a complete design, concerning

the guidance and autopilot modules for a class of spin-stabilized fin-controlled projectiles.

The proposed concept is composed of two sections: the rapidly spinning aft part contains

(58)

4.2 Rocket Flight Dynamic Model

This section describes the nonlinear flight dynamic model used in this study, including

dy-namics and aerodydy-namics, and control actuation. An example rocket concept is described,

followed by a description of the mathematical model for each relevant component.

4.2.1 Rocket

The guidance and control formulation proposed in this study applies to a 140mm

axisym-metric rocket with wrap around stabilizing fins. This system features a supersonic launch

and a rocket spin rate of approximately 150 Hz. The maneuver mechanism is placed at

the front part of the spin stabilized rocket, with a roll-decoupled fuse. It consists of a

section composed of fixed canard surfaces in order to generate a rotating control force

and its associated moment. This mechanism consists of a rotary motor linked to the front

section (see 4.iii). The most representative data of the rocket are shown in 4.I. The thrust

curve depending on flight time is shown in 4.i. The propellant mass decreases according

to (4.1).

m(t) =m0−

Z t

0

T(τ)

2348.2dτ (4.1)

Where m(t) is rocket mass function of time,m0is rocket initial mass and T(t) is thrust modulus function of time.

These curves have been fitted using data from actual shots. Note that once the rocket

has been launched there is no control in the thrust force. Finally, all the aerodynamic

coefficients for the rocket under study are acquired by numerical computing and tested

against actual shots. Their variation with the Mach number is shown in 4.ii.

(59)

Table 4.I Representative data of the studied rocket.

Parameter

Value

Maximum thrust

29160 N

Burn-out time

2.70 s

Initial mass

77.40 kg

Propellant mass

21.60 kg

I

x0

0.36 kg

m

2

I

y0

35.63 kg

m

2

X

CG0

1.30 m

Caliber

0.14 m

(60)

Figure 4.ii Aerodynamic coefficients for the rocket under study depending of the Mach number.

4.2.2 Flight Dynamic Model

Three axes systems are defined in order to express forces and moments: earth axes, body

axes, and working axes. Earth axes are defined by sub index e. xe pointing north, ze

perpendicular to xe and pointing nadir, and ye forming a clockwise trihedral. Working

axes are defined by sub index w. xw pointing to the target, yw perpendicular to xw and

pointing zenith, and zw forming a clockwise trihedral. Body axes are defined by sub

index b. xb pointing forward and contained in the plane of symmetry of the rocket, zb

perpendicular to xb pointing down and contained in the plane of symmetry of the rocket,

and yb forming a clockwise trihedral. The origin of body axes is located at the center of

mass of the rocket and they are severely coupled to the roll-decoupled fuse. Earth, body

and working axes systems are illustrated in 4.iii.

Total forces and moments of the rocket are given by (4.2) and (4.3), respectively.

−−→

Fext=

− →

D +−→L +−M→+−→P +−→T +−W→+−→C , (4.2)

(61)

Figure 4.iii Reference systems for a 140 mm axisymmetric rocket with wrap around fins and a roll-decoupled fuse.

where −→D is drag force, −→L is lift force, −M→ is magnus force, −→P is pitch damping force,

− →

T is thrust force, −W→ is weight force and −→C is Coriolis force.

−−→

Mext=

− →

O +−→PM +

−−→

MM +

− →

S , (4.3)

where −→O is overturn moment, −→PM is pitch damping moment,

−−→

MM is magnus moment

and −→S is spin damping moment.

Rocket forces in working axes include contributions from drag, lift, magnus, pitch

damping, thrust, weight and Coriolis forces, which can be described by the following

(4.4), (4.5), (4.6), (4.7), (4.8), (4.9) and (4.10):

− →

D =−π

8d 2_{ρ C}

D0 +CD_α2α

2

k−→vwk−v→w (4.4)

− →

L =−π

8d 2_{ρ C}

Lα·α+CL_α3α

2

k−v→wk2−x→w−(x−→w· −v→w)−v→w

(4.5)

− →

M =−π

8d 3_ρCmf

Ix

−→

Lw· −x→w

(62)

− →

P = π

8d 3_ρCN q

Iy

k−→vwk2

−→

Lw× −x→w

(4.7)

− →

T =T(t)−x→w (4.8)

−→

W =m−g→w (4.9)

− →

C =−2m−→Ω × −v→w, (4.10)

Where d is rocket caliber, ρis air density,CD0 is drag force linear coefficient,CD_α2 is

drag force square coefficient,α is total angle of attack,CLα is lift force linear coefficient,

CL_α3 is lift force cubic coefficient, Cmf is magnus force coefficient,

−→

Lw is rocket angular

momentum expressed in working axes, Ix and Iy are rocket inertia moments in body

axes, CN q is pitch damping force coefficient, −x→w is rocket nose pointing vector expressed

in working axes, −g→w is gravity vector in working axes,

− →

Ω is earth angular speed vector,

and −v→w is rocket velocity expressed in working axes.

Likewise, rocket moments in working axes include contributions from overturning,

pitch damping, Magnus, and spin damping moments and can be described by the following

(4.11), (4.12), (4.13) and (4.14):

− →

O = π

8d 3

ρ CMα +CM_α3α

2

k−v→wk2 (v−→w× −x→w) (4.11)

−→

PM = π

8d 3_ρ

Iy

CMqk− →

vwk

−→

Lw−

−→

Lw· −x→w

₋_→

xw

, (4.12)

−−→

MM =−

π 8d 4_ρ Ix Cmm −→

Lw · −x→w

((−v→w· −x→w)−x→w)− −→vw

(4.13)

(63)

− → S = π 8d 4_ρ Ix

Cspink−v→wk

−→

Lw · −x→w

₋_→

xw, (4.14)

Where CMα is overturning moment linear coefficient, CM_α3 is overturning moment

cubic coefficient, CMq is pitch damping moment coefficient, Cmm is magnus moment co-efficient and Cspin is spin damping moment coefficient.

(4.15) and (4.16) model the control forces and moments in body axes, respectively.

−→

CF = ρSexpkvwk 2

4 CN(α)

      0

cos(φc−φ)

−sin(φc−φ)

      (4.15) −−→

CM =−x−→cgb × −→

CF , (4.16)

where−→CF is control force, −−→CM is control moment,Sexp is reference surface for fins,

CN(α) is fin normal force coefficient, φc is control force angle, φ is roll angle and −x−→cgb is the center of gravity position expressed in body axes.

In order to solve the motion of the rocket a body reference frame, which is coupled

to the fuse, is used. Because, the fuse is uncoupled from the rear part and the rear part

spins at high rates, it is required to model this spinning motion in order to account for

rear Magnus force and moment and gyroscopic effects. The spin rate of the rear part of

the rocket can be modeled as shown in (4.17), note that initial spin speed is modeled as

an impulse which correlates to experimental data.

pr =−

Z

200δ(t0)−

π

8d 4_ρ

Ix

Cspink−→vbkk

−→

Lb· −→xb

₋_→

xbk

dt, (4.17)

whereδ(t0) is dirac’s delta,

− →

Lb is rocket angular momentum expressed in body axes,−→vb

is rocket velocity expressed in body axes and−→xb is rocket nose pointing vector expressed

(64)

It is assumed that fuse mass is negligible, which involves no appreciable reactions are

involved between fuse and aft part. Taking this in account, aft effect can be expressed

as an extra addition of external forces and moments that can be expressed by equations

(4.18), (4.19) and (4.20). This simplification is obtained from a development of Euler

equations, −−→Fext = dm

− →_v b dt + − →

ωb ×m−→vb on where gyroscopic contributions of aft part are

computed separately and moved to the left part of expression.

−→

Mr =−

π

8d 3

ρCmf Ix

−→

Lb· −→xb

(−→xb × −→vb) (4.18)

−−→

MM r =− π 8d 4_ρ Ix Cmm −→

Lb· −→xb

((−→vb · −→xb)−→xb)− −→vb

(4.19)

−→

Gr =−

     

Ix 0 0

0 Iy 0

0 0 Iy

      d dt      

pr+p

q r       + − →

i −→j −→k

p q r

Ix(pr+p) Iyq Iyr

, (4.20)

Where −→Mr is magnus force of the rotating part of the rocket,

−−→

MM r is magnus moment

of the rotating part of the rocket, −G→r is gyroscopic moment of the rotating part of the

rocket, p, q and r are angular speed components of the rocket and pr is angular speed

of the rotating part of the rocket.

Given the force and moment models above, the equations of motion for the rocket are

formulated using a Newton-Euler approach. The inertial, flat-Earth coordinate system

(denoted by frame e) and the body-fixed coordinate system b are related by Euler roll φ,

pitchθ, and yaw ψ angles.

−−→

Fext+

−→

Mr =

dm−→vb

dt +

− →

ωb ×m−→vb (4.21)

(65)

−−→

Mext+

−−→

MM r +

−→

Gr =

d−→Lb

dt +

− →

ωb ×

− →

Lb (4.22)

The equations of motion given by (4.21) and sumMaft are integrated forward in time

using a fixed time step Runge-Kutta of fourth order to obtain a single flight trajectory.

Note that control action is input to this system through specification of the roll angle of the

uncoupled fuse, φc. The result for a non-controlled trajectory, i.e., a ballistic trajectory,

calculated by this model is shown in 4.iv for an initialθ of 40 degrees.

Figure 4.iv A ballistic trajectory for an initialθ of 40 degrees.

4.3 Navigation and Guidance Law

In order to determine the elevation angle of the shot and to correct lateral deviations

produced by Coriolis, Magnus and Gyroscopic effects depending on target location a set

of non-controlled simulations were done and an initial Azimuth is added toAZ0 in order to obtain the initial Azimuth: AZinitial=AZ0+ACorr . Initial elevation angle and ACorr

depending on target distance to launch point are showed in 4.v.

After this correction, navigation and guidance is provided by a modified proportional

law defined in (4.23) and (4.24). Guidance is activated if and only if the variableGN CAct

takes value 1; it takes value 1 if flight time is greater than 5 seconds andθ ≤-15 degrees.

(66)

Figure 4.v Initial elevation angle andACorr depending on target distance to launch point.

ballistic flight because only small lateral deviations are pretended to be corrected. Errors

to be introduced in the controller are defined by the following equations:

θerr =−N ·

d dt      

−Xp1w −Xp2w −Xp3w

      d dt  atan  

Xp2w

q

X2

p1w +X 2

p3w



 

 (4.23)

ψerr =atan

Xp3w

Xp1w

−atan

xw3

xw1

, (4.24) where,      

Xp1w

Xp2w

Xp3w

     

=−−−−−→XT argetw− −→

Xw, (4.25)

where Xp1w, Xp2w, Xp3w are the line of sight vector components expressed in working axes,−−−−−→XT argetw is target position expressed in working axes,

−→

Xwis rocket position expressed

in working axes, θerr is pitch error, ψerr is yaw error and N is Proportional Navigation

Law Constant.

(67)

4.4 Control System

Control is processed by a double loop feedback system, which uses accelerations and

angu-lar speeds in body axes. The inner loop is only used as a system of stability augmentation.

The control angle for the rotating force is defined in (4.26), taking Pitch and Yaw errors

as inputs. 4.vi shows the philosophy of the controller. It has three main inputs: the

accel-eration of the rocket, the pitch error and the yaw error. Roughly speaking, the controller

calculates the needed pointing angle of the aerodynamic force calculating the arc-tangent

of the quotient of the pitch and yaw error. This gives an angle at which the aerodynamic

force, in the yb-zb plane, must point to reach the target. However, the gyroscopic effect

due to the spinning part of the rocket makes the response difficult to govern, i.e.,

impos-ing a φc = 90 degrees will not make the rocket to respond upwards. Therefore, it is also

required to measure the acceleration of the rocket, without accounting for gravity, and

make zero the difference between the angle that forms the projection of the aerodynamic

force in the yb-zb plane with yb and φc .

φc =GN CAct

h

KP

h

atanacczb

accyb

−atan θerr−C1·θ

C2(ψerr−C1ψ)

i

+

+KI

R h

atanacczb

accyb

C2(ψerr−C1ψ)

i

dt+

+KD_dtd

h

atanacczb

accyb

C2(ψerr−C1ψ)

i

C2(ψerr−C1ψ)

i

(C1 = 0.01, C2 = 100)

(4.26)

where accxb, accyb, acczb are rocket accelerations expressed in body axes, θ is pitch

angle,ψ is yaw angle, GN CAct is activation parameter for GNC,KP is PID proportional

constant,KI is PID Integral Constant, KD is PID derivative constant.

In order to develop the discrete-time guidance and control algorithms for the terminal

phase the following process is utilized: Model-In-the-Loop (MIL), Software-In-the-loop

(68)

Figure 4.vi Controller scheme.

all the models in Sections 4.2, 4.3, and 4.4 are implemented and tested; here, continuous

time is used for all the components of the system. Once the MIL stage is successfully

accomplished, the autopilot, that is, the models in Sections III and IV are enabled for

discrete time and tested together with the model of the plant in Section II (i.e., SIL stage).

Then, in the PIL stage, the autopilot is loaded in the final hardware (e.g., processor or

FPGA) and tested again together with the model of the plant. Finally, a gyroscopic

table of three degrees of freedom is employed to perform tests with real hardware such as

processor and sensors (i.e., HIL tests).

4.5 Simulation Results

Simulation results are presented using the nonlinear flight dynamic model to demonstrate

closed-loop performance of the presented navigation, guidance and control novel approach

and contrast performance with ballistic flight. MATLAB/Simulink R2015a was employed

Guidance Navigation and Control Algorithms for High Dynamics Vehicles

TESIS DOCTORAL