Robust Terminal Attitude Navigation forAutonomous Vehicles

Pubudu N. PathiranaSchool of Engineering and Technology, Deakin University

Victoria, Australia 3217, Email: pubudu@deakin.edu.au

Abstract— Here we define the terminal attitude of the pursuerwith respect to a target and present a LQR and H∞ controlapproach to solving the problem of pursuer achieving a desiredterminal attack/approach angle. The intercept or engagementcriteria is defined in terms of both minimizing the miss distanceand controlling the pursuer’s body attitude with respect to thetarget at the terminal point. This approach in comparison to pre-vious approaches consider the relativistic approach of the pursuerwith respect to the target as opposed the absolute velocities ofthe two dynamic bodies, and have possible applications rangingfrom autonomous vehicle entry in to a mother craft to nossleengagements in on-flight refuelling or even in precision missileguidance. Here we also suitably formulate the H∞ control ideasdirectly applicable to the underlying problem and presents bothstate feedback and output feed back results for the case of finitehorizon and non-zero initial conditions together with a optimalparameter value to achieve a desired terminal characteristic interms of the original weighting parameters.

I. INTRODUCTION

The work presented here introduces the terminal attitudeguidance problem where the control objective is to minimizethe target interceptor miss distances in addition to satisfyingthe terminal attitude of the pursuer with respect to the target.This fundamental problem arises in situations where two spacevehicles have the requirement to engage with definite termi-nal characteristics(distance and approach angle with respectto each other) or a missile hitting a target with a desiredterminal angle where the principle axis of the missile alongwhich the debris fly is aligned with a desired direction withrespect to the target(or on the target). Another instance is inestablishing a physical supply line between two vehicles (i.eon-flight refuelling, loading ammunition etc.). The need forthe terminal attitude guidance problem has been brought aboutas a result of recent advances in space vehicle engagementsand weapon systems and subsystems technologies as well asa shift in guided weapons systems deployment and operationalphilosophies. In the past, due to real-time computing constraintsin implementing sophisticated algorithms on autonomous sys-tems, major simplifications of engagement kinematics model,performance index constraints had to be implemented in orderto render the solution suitable for mechanization of a realsystem. These simplifications led to relatively simple feedbackguidance laws, such as "optimum guidance law" or the "aug-mented proportional navigation with a time-varying(time-to-go)parameter; e.g., see [4], [14], [2], [6]. The precision missileguidance problem was presented in [10], and concerned withachieving a desired terminal angle between the missile and the

target absolute velocities. This is significant in countering endflight defence barriers where the constraints arise from physicalenvironment the interception takes place(minimizing collateraldamage etc). In this work,instead, we consider achieving a de-sired terminal attitude of the pursuer with respect to the target.In other words, we need the pursuer’s relative velocity withrespect to the target to achieve a desired angle and a magnitudeat the terminal point of the trajectory while minimizing themiss distance. This is important in general for autonomousspace vehicle engagements. Also here we introduce closed formsolutions for the overall controller that optimize the terminalobjectives. For simplicity we discard autopilot dynamics andassume the both objects in the pursuit are point-wise. Firstly, asin [10], we formulate the terminal attitude guidance problem asa linear-quadratic optimal control problem and as a H∞ controlproblem side by side for comparison purposes. The respectivecontrollers can be obtained by the resulting Riccati differentialequations. However, the optimal control approach produces thecontroller for the case of non-maneuvering targets. Moreover,two further significant shortcomings in the optimal controlapproach are, all the states of the pursuit system are preciselyknown and the initial conditions are known. However, in mostpractical situations, not all the states are available for mea-surement and even the ones available are subject to noise anduncertainties. Further, the state of the target should be knownfor the knowledge of the initial condition of the overall system.In most applications, not only is this not available, but canbe subject to a large uncertainty. In other words, the terminalattitude guidance problem is an output feedback problem. Oneof the most significant recent advances in the area of controlsystems was the theory of H∞ control; e.g., [3], [1], [12]. Theuse of H∞ control methods has provided an important toolfor the synthesis of robustly stable output feedback controlsystems; e.g., see [9], [11], [7]. In this paper, we show thatthe H∞ control theory when suitably modified provides aneffective framework for the terminal attitude guidance problem.Our computer simulations prove that in the terminal attitudeguidance problem with disturbances, the H∞ control guidancelaw gives a much better performance than the linear quadraticoptimal guidance law. Further this disturbance attenuation ap-proach compensates for the measurement uncertainty and alsothe uncertainty in the initial condition ensuring robustness.

II. TARGET INTERCEPTOR KINEMATIC MODEL

In order to develop terminal attitude guidance laws, as in[10] engagement kinematics are defined in terms of relativestates between the pursuers and the target. For the sake ofsimplicity consider the two dimensional missile target motion.The absolute position of missile and target are [x1

M x2M ]′ and

[x1T x2

T ]′ respectively with velocities [v1M v2

M ]′ and [v1T v2

T ]′

respectively. The control input i.e the acceleration vectors aregiven as [a1

M a2M ]′ and [a1

T a2T ]′ Then the missile target system

in the state space form can be written as : Firstly, as we needto control the missile terminal velocity

VT/E

VM/T

Pursuer

Target

β

Fig. II.1. Engagement scenario for terminal attitude guidance

with respect to the target, we use the model in [10].

Define the state vector as x =

x1

x2

x3

x4

=

v1M − v1

T

v2M − v2

T

x1M − x1

T

x2M − x2

T

Then

the state space model becomes

x = Ax + B1u + B2w (II.1)

where

A =

0 0 0 00 0 0 01 0 0 00 1 0 0

, B1 =

1 00 10 00 0

,

B2 =

−1 00 −10 00 0

III. OPTIMAL CONTROL APPROACH

In this section, we suppose that the plant is described by thefollowing linear differential equation

x(t) = Ax(t) + B1u(t) (III.1)

where x(t) ∈ Rn is the state, u(t) ∈ Rm is the control input.We assume that the initial condition of the system is given,

x(0) = x0 (III.2)

where x0 ∈ Rn is a given vector.With this system let us associate the performance index

J [x(·), u(·)] := 12 (x(T ) − h)′XT (x(T ) − h)

+ α2

∫ T

0‖u(t)‖2dt. (III.3)

Here XT ≥ 0 is a given matrix, h ∈ Rn is a given vector, andα > 0 is a given constant.

The linear quadratic optimal control problem can be formu-lated as follows:

To find the minimum of the functional (III.3) over the setof all [x(·), u(·)] ∈ L2[0, T ] satisfying the equations (III.1) and(III.2),

J [x(·), u(·)] → min . (III.4)

Introduce the following Riccati differential equation

−S(t) = A′S(t) + S(t)A − 1αS(t)B1B

′1S(t),

S(T ) = XT . (III.5)

Furthermore, introduce the following equations

−r(t) = (A − 1α

B1B′1S(t))′r(t), r(T ) = XT h, (III.6)

uopt(t) = − 1α

B′1S(t)xopt(t) +

1α

B′1r(t), (III.7)

−g(t) = − 12α

r(t)′B1B′1r(t), g(T ) =

12h′XT h. (III.8)

Now we are in a position to state the following theorem.

theorem 1. Consider the linear quadratic optimal controlproblem (III.1), (III.2), (III.3), (III.4). Then, for any x0, h,XT ≥ 0 and α > 0, the following statements hold:

(i) The minimum in the linear quadratic optimal controlproblem (III.4) is achieved.

(ii) The Riccati differential equation (III.5) has a uniquesolution on the time interval [0, T ].

(iii) The optimal control law [xopt(·), uopt(·)] is given by theequations (III.5), (III.6), (III.7).

(iv) The optimal cost in the problem (III.4) is

12x′

0S(0)x0 − x′0r(0) + g(0)

where g(·) is defined by (III.8).

Proof: See [5]. � .

IV. H∞ APPROACH

The H∞ control problem was originally introduced byZames in 1981 [13] and has subsequently played a major role inthe area of robust control theory.Consider a linear time varying

system defined on the finite-time interval [0, T ] as follows:

x(t) = A(t)x(t) + B1(t)u(t) + B2(t)w(t) (IV.1)

y(t) = C1(t)x(t) + D1v(t) (IV.2)

z(t) =[

C(t)0

]x(t) +

[0

D2(t)

]u(t) (IV.3)

where x ∈ Rn is the state, u(t) ∈ R

m is the control input,y ∈ R

p is the measurement vector, w ∈ Rq and v ∈ R

p areinput disturbance vectors, respectively and z is the controlledoutput vector. A(·), B1(·), B2(·), C1(·), C2(·),D1(·) and D2(·)are real, bounded, piecewise continuous matrix functions. Asin [8], if we define the measurement history up to t as :Yt ={y(s), 0 ≥ s ≥ t}, the admissible control is restricted to be afunction of only Yt. If

⊔denote the set of admissible controls

and⊔

l �⊔

is the subset of linear functions of Yt. The H∞

tracking controller that we need to use in this paper is that acontrol law u(·) ∈ ⊔

l over the horizon[0, T ] using the availablemeasurement y(·) and the given desired terminal state h. Thecontroller required to reduce the worst case effects of the initialstate x0, the disturbance signal w, and the measurement noisev on the controlled output z. Hence, we consider the followingperformance index.

J(h, u, v, w, x0) =

‖x(T ) − h‖2QT

+∫ T

0‖x‖2

Q + ‖u‖2R dt

−γ2[‖x(0) − µ‖2

P0+

∫ T

0‖w‖2

W + ‖v‖2V dt

](IV.4)

where γ > 0 is a given scalar which indicates the level ofof the tracking performance of the controlled system and µ isa constant to be determined in order for the terminal attitudeproblem to have a solution for a given γ. P0,W and V arepositive definite matrices and QT is a non negative definitematrix. Also Q = C

′C with both D1 and R = C

′1C are non

singular. The following results present necessary and sufficientconditions for the solvability of a corresponding H∞ controlproblem with non-zero initial conditions and a desired terminalstate. These necessary and sufficient conditions are stated interms of certain differential Riccati equations.

(1) Finite Horizon State Feedback H∞ Control with Non-ZeroInitial Conditions and Desired Terminal State Via DynamicGame Approach.

theorem 2. Consider the system IV.2 for the case in whichthe full state is available for the measurement; i.e., y = x.QT ≥ 0 and P0 > 0 be given matrices. Then the followingstatements are equivalent.

a) There exists a unique symmetric matrix S(t), t ∈ [0, T ]such that

−S(t) = A′S(t) + S(t)A

−S(t)[B1R

−1B′1 − 1

γ2 B2W−1B

′2

]S(t) + C

′C,

S(T ) = QT and S(0) > γ2P0 (IV.5)

If condition a holds, then the H∞ tracking for a terminal

state h is solvable with the control law

u∗(t) = −R−1B′1S(t)x(t) − R−1B

′1θ(t) (IV.6)

with θ(t) ∈ [0, T ] satisfying

θ(t) =[S(t)

(B1R

−1B′1 − 1

γ2 B2W−1B

′2

)− A

′]θ(t)

θ(T ) = −QT h

b) If J∗(h) = J(h, u∗, v∗, w∗, x∗(0)) with "*" denote theoptimal value

J∗(h) = 12‖x0‖2

[S(0)−γ2P0]+ x

′0θ(0) + 1

2h′QT h

− 12

∫ T

0θ′(t)

[B1R

−1B′1 − 1

γ2 B2W−1B

′2

]θ(t)dt (IV.7)

Proof: This can easily be proved with simple exten-sions to the work in [3], [1].

(2) Finite Horizon Output Feedback H∞ control with non-zeroinitial condition and desired terminal state via Dynamicgame approach Introduce the following Riccati differentialequations :

−Π = A′Π + ΠA − Π(B1R

−1B′1

−γ−2B2W−1B

′2)Π + Q, Π(T ) = QT . (IV.8)

P = PA′+ AP + P

(γ−2Q − C

′1V

−1C1

)P

+B2W−1B

′2, P (0) = P−1

0 . ,(IV.9)

Now we can state the output feedback version that isnecessary for our application.

theorem 3. Consider the linear time varying system IV.1with the appropriate assumptions on P0,W and V . Thenthe following are equivalent.

(a) There exists an admissible output feedback controlleru(·) ∈ ⊔

l that minimizes IV.4.(b) The following conditions are met

(i) The exits a solution Π(t) to the RDE IV.8.(ii) There exists a symmetric matrix function P (t) >

0 for all t ∈ [0, T ] such that IV.9 is satisfied.(iii) I − γ−2P (t)Π(t) > 0 for all t ∈ [0, T ]If the above conditions are met, a suitable control law

is given by

˙x =(A + γ−2PQ − PC

′1V

−1C1

)x + B1u

+PC′1V

−1y, x(0) = µ

u∗(t) = Kxx + Kθθ

whereKx = −R−1B

′1Π

(I − γ−2PΠ

)−1,

Kθ = −R−1B′1

Lx = 1γ2 W−1B

′2Π

(I − γ−2PΠ

)−1,

Lθ = 1γ2 W−1B

′2

and−θ = Π

(I − γ−2PΠ

)−1 (A′+ γ−2(QP

+PC′1V

−1C1P ) − B1R−1B

′1)θ,

θ(T ) = QT

[I − γ−2P (T )QT

]−1h (IV.10)

(c) The optimal cost is given by

J∗ = 12

∥∥∥x′0

∥∥∥Π(0)(I−γ−2P−1

0 Π(0))−1 + θ′(0)x0

+h′QT h − 1

2

∫ T

0θ′(B1R

−1B′1 − γ−2ΓV Γ′

)θ dt

where V = D1V D′1, Γ = PC

′1V

−1 (IV.11)

and this is achieved when

w∗(t) = Lxx + Lθθ

x∗(0) =(I − γ−2P−1

0 Π(0))−1

+ γ−2P−10 θ(0)

Proof: This is a simple extensions to the work in [3],[8].

Now we state the following H∞ norm for the statement ofthe robust - disturbance attenuating terminal attitude controller.Here we set C = 0 for simplicity and conformity with LQRcase.

Js � supx(0)∈Rn\0,w,v∈L2[0,T ]

‖x(T ) − h‖2QT

+R T

0‖z‖2 dt

‖x(0) − µ‖2P0

+R T

0

`‖w‖2W + ‖v‖2

V

´dt

(IV.12)where the supremum is taken over all w,v ∈ L2[0, T ], x(0) ∈ Rn

such that

‖x(0) − µ‖2P0

+R T

0

`‖w‖2W + ‖v‖2

V

´> 0

The question we ask is, is there a finite value for µ such that Js < γ2.

theorem 4. The controller given in theorem 3 with the appropriateassumptions therein achieves the bound IV.12 for the linear timevarying system IV.1 for µ = −S−1(0)θ(0).

Proof As in [8], the disturbance attenuation problem has a solutionif the following necessary and sufficient condition is satisfied : Takingthe optimal cost from 3,

h′ST h +

R T

0θ′Mθ dt < 0

where

M = γ−2ΓV −1Γ′ − B1R

−1B′1

Noticing h′ST h = θ

′(T )S−1

T θ

θ′(T )S−1

T θ(T ) − θ′(0)S−1(0)θ(0)

=R T

0θ′[− `

MS + A´S−1 + S−1(SA + A

′S + SMS)S−1

−S−1(SM + A′)]θ = 0

(IV.13)

Then the

J(u∗, v∗, w∗, x∗(0)) = µ′S(0)µ + 2θ

′(0)µ + θ

′(0)S−1(0)θ(0)

This means that µ = −S−1(0)θ(0) gives the minimum and J∗ = 0.See [3], [8], for more details � .

V. CLOSING VELOCITY FOR ATTACK/APPROACH ANGLE

IMPROVEMENT

If the closing velocity is arbitrary, and only the miss dis-tance and the approach angle is only concerned, then we giveand explicit expression approach for the technique used in [10].By taking h = [cos β sin β 0 0]

′for the 2D case and h =

[cos β cos φ sin β cos φ sin φ 0 0 0]′for the 3D case, then h = ch.

A. Optimal c

Here we find the optimal value for the c as far as the performanceindex is concerned in both LQR and H∞ control frame work. Foran arbitrary target/mother craft maneuver, we use the c = co. If thetarget is optimally maneuvering, we can use c = cH

s or cHh for state

or output feedback case respectively.1) Optimal Control: The optimal cost for the optimal controller

from theorem 1 is :

Jopt = 12x

′0S(0)x0 − x

′0r(0) + 1

2h

′XT h

− 12α

R T

0r′(t)B1B

′1r(t) dt. (V.1)

By taking f(t) = exp− R Tt

1α

S(τ)B1B′1−A

′dτ XT we notice

∂∂h

R T

0r′(t)B1B

′1(t)r(t) dt =

“R T

0f

′(t)B1B

′1f(t) dt

”h (V.2)

∂∂h

r(0) = f′(0). (V.3)

Taking

∂∂c

Jopt = h′ h

−f′(0)x0 +

“XT − 1

α

R T

0f

′(t)B1B

′1f(t) dt

”h

i

The minimum is achieved when

c = cO = h′f′(0)x0

h′(XT − 1

α

R T0 f

′(t)B1B

′1f(t) dt)h

(V.4)

withh′

2

“2XT − 1

α

R T

0f

′(t)B1B

′1f(t) dt

”h > 0 (V.5)

2) H∞ Control: For the proposed H∞ controller, we use thesame co as in the LQR case. If the target is maneuvering optimallythen we can use the relevant optimal constant(c) to optimize theperformance. If optimal c=cH

s for the state feedback case, by takingΥ(t) = S(t)Γ(t) − A

′with Γ(t) =

hB1R

−1B′1 − 1

γ2 B2W−1B

′2

i

we can write

θ(0) = − exp− R T0 Υ(τ) dτ QT h (V.6)

and

θ(t) = expR t0 Υ(τ) dτ θ(0) = − exp− R T

t Υ(τ) dτ QT h. (V.7)

Assuming f(t) = − exp− R Tt Υ(τ) dτ QT

∂∂h

R T

0θ′(t)Γ(t)θ(t) dt = 2

“R T

0f

′(t)Γ(t)f(t) dt

”h (V.8)

∂∂h

θ(0) = f′(0). (V.9)

Now we find

c = cHs = −h

′f′(0)x0

h′(QT −R T

0 f′(t)Γf(t) dt)h

(V.10)

with

h′ “

QT − R T

0f

′(t)Γf(t) dt

”h > 0 (V.11)

Similarly for the output feedback case, the optimal value of c:cHo

can be stated by letting :

Υo(t) = Π(I − γ−2PΠ)−1Γ0(t)

where Γo(t) =hB1R

−1B′1 − 1

γ2 ΓV Γ′+ QP

i− A

′and defining

fo(t) = − exp− R Tt Υo(τ) dτ QT . That is

c = cHo =

−h′f′o(0)x0

h′(QT −R T

0 f′o(t)Γofo(t) dt)h

(V.12)

with

h′ “

QT − R T

0f

′o(t)Γof(t) dt

”h > 0 (V.13)

VI. SIMULATIONS AND CONCLUSIONS

Two body pursuit was simulated for the case parameters indi-cated in table VI for a target maneuver in the form of w =Amp [sin ωt sin ωt]′, with ω ∈ [0, 2]rad/s. The mean squared errorin achieved and desired final state for range of values in c is givenin figure VI.1 for the LQR and H∞ state feedback case togetherwith miss distances and optimal performance index are given in figureVI.2 and in VI.3 respectively. If the closing velocity can be used toimprove on the actual attack angle (i.e similar to the case of precisionguidance), by using the same optimal value c for both LQR and H∞

controller, the miss distances and attack angles are given in figure VI.4and in VI.5. The simulation results justify the theoretical inferences ofthe superiority of the robust control and also the effective techniqueof terminal attitude homing of the pursuer.

Parameter Description Value

T Simulation time 12sTs Time step 0.1β Desired approach angle 25o

α weighting on the control input 150γ Upper bound for H∞ norm 8.17

Amp Amplitude of the maneuver 0.1

TABLE VI.1

SIMULATION PARAMETERS

REFERENCES

[1] T. Basar and P. Bernhard, H∞-optimal control and related minimaxdesign problems: A dynamic game approach, Birkhäuser, Boston, 1991.

[2] J.Z. Ben-Asher and I. Yaesh, Advances in missile guidance theory, AIAA,Inc., Washington, DC, 1998.

[3] J. C. Doyle, K. Glover, P. P. Khargonekar, and B. Francis, State-space solutions to the standard H2 and H∞ control problems, IEEETransactions on Automatic Control 34 (1989), no. 8, 831–847.

[4] P. Garnell and D.J. East, Guided weapon control systems, Pregamon,Oxford, 1977.

[5] F. L. Lewis, Optimal control, Wiley, New York, 1986.

−200 −150 −100 −50 0 50 100 150 2000

50

100

150

200

250

C

Me

an

sq

ua

rre

d E

rro

r in

D

esi

red

an

d A

chie

ved

te

rmin

al s

tate

LQRH

inf

Fig. VI.1. Mean squared error for varying magnitudes of the desired terminalstate vetor

−2000 −1000 0 1000 2000 3000 40000

50

100

150

200

250

C

Mis

s D

ista

nce

LQRH

infC

LQR = 14 = C

Hinf

Fig. VI.2. Miss distance variations against c

[6] C.F. Lin, Modern navigation, guidance and control processing - vol. ii,Prentice Hall, Englewood Cliffs, NJ, 1991.

[7] I.R. Petersen, V.A. Ugrinovskii, and A.V. Savkin, Robust control designusing h∞ methods, Springer-Verlag, London, 2000.

[8] I. Rhee and J.L. Speyer, A game theoretic approach to a finite-time dis-turbance attenuation problem, IEEE Transactions on Automatic Control36 (1991), no. 9, 1021–1032.

[9] A. V. Savkin and I. R. Petersen, A connection between H∞ control andthe absolute stabilizability of uncertain systems, Systems and ControlLetters 23 (1994), no. 3, 197–203.

[10] A.V. Savkin, P.N. Pathirana, and F.A.Faruqi, The problem of precisionmissile guidance : Lqr and h∞ control framework, IEEE Transactionson Aerospace and Electronic Systems 39 (2003), no. 3, 901–910.

[11] A.V. Savkin and I.R. Petersen, Robust control with a terminal stateconstraint, Automatica 32 (1996), no. 7, 1001–1005.

[12] A.A. Stoorvogel, The H∞ control problem, Prentice Hall, New York,1992.

[13] G. Zames, Feedback and optimal sensitivity: Model reference trans-

−2000 −1000 0 1000 2000 3000 4000−2

−1

0

1

2

3

4

5

6

7

8x 10

6

C

J optim

al

LQR

Hinf

CLQR

= 14 = CHinf

Fig. VI.3. Variation of the optimal cost function Jopt against c

−2000 −1000 0 1000 2000 3000 4000−100

−80

−60

−40

−20

0

20

40

60

80

100

C

Te

rmin

al A

tta

ck a

ng

le

LQRH

inf

Desired Attack Angle(25o)C

LQR = 14 = C

Hinf

Fig. VI.4. Attack angle variation against c

formations, multiplicative seminorms, and approximate inverses, IEEETransactions on Automatic Control 26 (1981), 301–320.

[14] P. Zarchan, Tactical and strategic missile guidance, AIAA, Inc., Wash-ington, DC, 1994.

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 225

30

35

40

45

50

55

60

Maneuver frequency (rad/sec)

Mis

s D

ista

nce

LQRH

inf

Fig. VI.5. Miss distance variation against maneuver frequency

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 222

24

26

28

30

32

34

36

38

40

Maneuver frequency (rad/sec)

Atta

ck a

ngle

(Deg

rees

) LQRH

infDesired Attack Angle

Fig. VI.6. Attack angle variation against target maneuver frequency