Lecture # 12 Aircraft Lateral Autopilots Multiloop closure Heading Control: linear Heading Control: nonlinear

Fall 2004

16.333 101

Lateral Autopilots

We can stabilize/modify the lateral dynamics using a variety of dif ferent feedback architectures.
a
-

p- 1 s Glat (s) r 1 s

r
-

Look for good sensor/actuator pairings to achieve desired behavior. Example: Yaw damper Can improve the damping on the Dutchroll mode by adding a c feedback on r to the rudder: r = kr (rc r) Servo dynamics Hr = System:
c Gr r

3.33 s+3.33

a c added to rudder r = Hr r

1.618s3 + 0.7761s2 + 0.03007s + 0.1883 = 5 s + 3.967s4 + 3.06s3 + 3.642s2 + 1.71s + 0.01223

Lateral autopilot: r to rudder
2 0.86 0.76 0.64 0.5 0.34 0.16 1.5 0.94 1 0.985 0.5 Imaginary Axis

3.5

2.5

1.5

0.5

0.5 0.985 1 0.94 1.5

0.86 3.5 3 2.5

0.76 2

0.64 1.5 Real Axis

0.5 1

0.34 0.5

0.16 0

c Figure 2: Lateral polezero map Gr r

Fall 2004

16.333 102

Note that the gain of the plant is negative (Kplant < 0), so if kr < 0, then K = Kplantkr > 0, so must draw a 180 locus (neg feedback)
Lateral autopilot: r to r with k>0
2
2

Lateral autopilot: r to r with k<0

1.5

1.5

1 0.75 0.5 Imaginary Axis

0.5

0.25

1 0.75

0.5

0.25

0.5
Imaginary Axis

0.5

0.25 0 0.25 0.5 0.75 1 0.5 0.25 1 0.5

0.5

0.25

0.5 0.75 1 0.5 0.25

1.5

1.5

2 2

1.5

0.5

0 Real Axis

0.5

1.5

3.5

2.5

2 Real Axis

1.5

0.5

Figure 3: Lateral polezero map. Denitely need kr < 0

Root locus with kr < 0 looks pretty good as we have authority over the four poles. kr = 1.6 results in a large increase in the Dutchroll damping and spiral/roll modes have combined into a damped oscillation.

Yaw damper looks great, but this implementation has a problem. There are various ight modes that require a steady yaw rate (rss = 0). For example, steady turning ight. Our current yaw damper would not allow this to happen it would create the rudder inputs necessary to cancel out the motion !! Exact opposite of what we want to have happen, which is to damp out any oscillations about the steady turn.

Fall 2004

16.333 103

Yaw Damper: Part 2

Can avoid this problem to some extent by ltering the feedback signal. Feedback only a high pass version of the r signal. High pass cuts out the low frequency content in the signal steady state value of r would not be fed back to the controller.
c New yaw damper: r = kr (rc Hw (s)r) where Hw (s) = washout lter.
Washout filter with =4.2

s s+1

is the

10

|H (s)|

10

10

10

10

Freq (rad/sec)

10

10

Figure 4: Washout lter with = 4.2

New control picture

a
-

p- 1 s Glat (s) r- 1 s

rc 6

kr

c r -

Hr (s)

Hw (s) 

Fall 2004
Lateral autopilot: r to rudder WITH washout filter 1.5

16.333 104

1 0.5 0.75 0.5

0.25

Imaginary Axis

0.5

0.25

0.5 0.75 0.5 1 0.25

1.5 2

1.5

0.5 Real Axis

0.5

Figure 5: Root Locus with the washout lter included.

Zero in Hw (s) traps a pole near the origin, but it is slow enough that it can be controlled by the pilot. Obviously has changed the closed loop pole locations ( ), but kr = 1.6 still seems to give a well damped response.

Fall 2004
1

16.333 105

Without Washout With Washout

Response r

0.5

0.5

10

15 Time

20

25

30

Without Washout With Washout

Control r

0.5

0.5

10

15 Time

20

25

30

Figure 6: Impulse response of closed loop system with and without the Washout lter ( = 4.2). Commanded rc = 0, and both have (r )ss = 0, but without the lter, rss = 0, whereas with it, rss = 0.

For direct comparison with and without the lter, applied impulse as rc to both closedloop systems and then calculated r and r . Bottom plot shows that control signal quickly converges to zero in both cases, i.e., no more control eort is being applied to correct the motion. But only the one with the washout lter produces a zero control input even though the there is a steady turn the controller will not try to ght a commanded steady turn.

Fall 2004

16.333 106

Heading Autopilot Design

So now have the yaw damper added correctly and want to control the heading . Need to bank the aircraft to accomplish this. Result is a coordinated turn with angular rate

Aircraft banked to angle so that vector sum of mg and mU0 is along the body zaxis Summing in the body yaxis direction gives mu0 cos = mg sin U0 tan = g Since typically 1, we have U0 g gives the desired bank angle for a specied turn rate.

Fall 2004

16.333 107

Problem now is that tends to be a noisy signal to base out bank angle on, so we generate a smoother signal by ltering it. Assume that the desired heading is known d and we want to follow d relatively slowly Choose dynamics 1 + = d
1
= d 1s + 1

with 1=1520sec depending on the vehicle and the goals. A low pass lter that eliminates the higher frequency noise.

Filtered heading angle satises 1 = (d ) 1 which we can use to create the desired bank angle: d = U0 U0 (d ) = g 1 g

Fall 2004

16.333 108

Roll Control

Given this desired bank angle, we need a roll controller to ensure that the vehicle tracks it accurately. Aileron is best actuator to use: a = k(d ) kpp To design k and kp, can just use the approximation of the roll mode Ixxp = Lpp + La a Ixx Lp = La a =p which gives La = a s(Ixxs Lp) For the design, add the aileron servo dynamics 1 a c Ha(s) = , a = Ha(s)a 0.15s + 1
c PD controller a = k(s + 1) + kd, adds zero at s = 1/

Pick = 2/3
Root Locus 10 0.52 0.38 0.28 0.2 0.12 0.06 8 0.68 6 10 8

0.88

2 Imaginary Axis

2 0.88

6 0.68

8 0.52 6 5 0.38 4 Real Axis 3 0.28 0.2 2 0.12 1 0.06 10 0

10 7

Figure 7: Root Locus for roll loop closed Loop poles for Kp = 20, K = 30

Fall 2004

16.333 109

0.3

Reponse to initial roll of 15 degs

p

0.2

0.1

0.1

0.2

0.3

0.4

0
3

10

10

x 10

Reponse to initial roll of 15 degs

r

10

Figure 8: Roll response to an initial roll oset

Fall 2004

16.333 1010

0.3

Reponse to step input c=15 degs (0.26 rad)

p

0.25

0.2

0.15

0.1

0.05

0.05

10

15 Time (sec)

20

25

30

0.35

Reponse to step input c=15 degs (0.26 rad)

r

0.3

0.25

0.2

0.15

0.1

0.05

0.05

0.1

10

15 Time (sec)

20

25

30

Figure 9: Roll response to a step command for c . Note the adverse yaw eects.

Fall 2004

16.333 1011

Heading Autopilot
Putting the pieces together we get the following autopilot controller
Kp ? +6 d 0 K c ? a c r -

Ha (s)

p Glat (s)

1 s

Kr

Hs (s)

r- 1 s

s  s + 1 U0 1 g


+ d
Heading Command Loop 1=12 sec
0.8 1 0.88 0.68 0.5 0.25

 6

Last step: analyze eect of closing the d loop

0.94 0.5

0.975

Imaginary Axis

0.994 2 0 1.75 1.5 1.25 1 0.75 0.5 0.25

0.994

0.5

0.975

0.94

0.88 0.8 0.68 1 0.5 0.5 0.25 0 Real Axis 0.5 1 1.5 2

1.5

Figure 10: Heading loop root locus. Closed loop poles for gain U0 /(g1 ). 8th order system, but RL fairly well behaved.

Fall 2004

16.333 1012

Measure with a vertical gyro and with a directional gyro Add a limiter to d or else we can get some very large bank angles

Design variables are K, Kp, 1, , and Kr Multiloop closure must be done carefully Must choose the loop gains carefully so that each one is slower than the one inside can lead to slow overall response

Analysis on fully coupled system might show that the controllers designed on subsystem models interact with other modes (poles) several iterations might be required

Now need a way to specify d

Fall 2004

16.333 1013

0.6

0.5

0.4

0.3

0.2

0.1

0.1

10

15

20

25 Time (sec)

30

35

40

45

50

0.3

0.25

r c

0.2

0.15

0.1

0.05

0.05

0.1

10

15

20

25 Time (sec)

30

35

40

45

50

Figure 11: Response to 15 deg step in c . Note the bank angle is approximately 30
degs, which is about the maximum allowed. Decreasing 1 tends to increase max .

Fall 2004

16.333 1014

Ground Track Control

Consider scenario shown use this to determine desired heading d

Figure 12: Heading denitions

Are initially at (0,0), moving along Xe (0 = 0) Want to be at (0,1000) moving along dashed line angled at ref Separation distance d = Yref Ya/c (d0 = 1000) Desired inertial y position y position of aircraft d = U0 sin(ref ) U0(ref ) Want to smoothly decrease d to zero, use a lter so that 1 d = d d = 30 sec d 1 d = U0(ref ) d 1 1 = = ref + d = ref + (Yref Ya/c) d U0 dU0
which includes a feedback on the aircraft inertial Y position.
Use this as the input d.

Fall 2004

Mux

ac3out To Workspace

time

desired path

U0

Clock

Signal Generator
all data

2*pi/180

Psi_ref

-5000

desired ye Path1

path offset -KAileron Dynamics

Psi_d

desired path

psi_ref

Kp
v p

-KKphi JDa(s) Rudder Dynamics JNr(s) JDr(s) Mux Gain 0 Constant Saturation Banking mix

JNa(s)

syslat2 Lateral Washout FIlter

all data r phi

emu

-Kpsi

psi

-KNwash(s) Dwash(s) Banking mix1

1 s Integrator

Figure 13: Simulink implementation run ac3.m rst

ye

16.333 1015

Fall 2004

1.5

x 10

Y ref Y

16.333 1016

a/c

0.5

Path Y

0.5

1.5

100

200

300

400

500

600

700

800

900

1000

Figure 14: Path following for ground track controller

0.6 c

0.4

0.2

and c

0.2

0.4

0.6

0.8

100

200

300

400

500

600

700

800

900

1000

Figure 15: Roll command following for ground track controller

3 c

and c

100

200

300

400

500

600

700

800

900

1000

Figure 16: Heading following for ground track controller

Fall 2004

16.333 1017

Alternative Strategy

Sanghyuk Park developed an alternative tracking algorithm that he presented this past summer1 Guidance logic selects a reference point on the desired trajectory and uses it to generate a lateral acceleration command. Selection of Reference Point Reference point is on the desired path at a distance (L1) forward of the vehicle Figure 17. Lateral Acceleration Command determined by ascmd = 2 V 1 sin L
aircraft V
2

aS
desired path

cmd

L1

reference point R R 2

Figure 17: Diagram for Guidance Logic

Direction of acceleration depends on sign of angle between the L1 line segment and the vehicle velocity vector. If selected reference point to the right of the vehicle velocity vec tor, then the vehicle will be commanded to accelerate to the right (see Figure 17) vehicle will tend to align its velocity direction with the direction of the L1 line segment.
1 Sanghyuk

Park, John Deyst, and Jonathan How, A New Nonlinear Guidance Logic for Trajectory Tracking, AIAA GNC 2004.

Fall 2004
= as t V L1

16.333 1018

reference point

: velocity direction change due to the acceleration

s=Vt

Figure 18: One Time Step

50 40 30 20 10 0 10 At some point, acceleration sign changes Smooth convergence to desired path 0 20 40 X 60 80 Y Desired Path Vehicle Position Reference Point L1

Figure 19: Step by Step (t=1, V =10, and L1 =40)

Figure 18 shows evolution of the guidance logic in one time step and Figure 19 shows the trajectory of the vehicle over several time steps. Vehicle initially starts from a location far away from the desired path, and eventually converges to it. If vehicle far from the desired path, then the guidance logic rotates the vehicle so that its velocity direction approaches the desired path at a large angle. If vehicle close to the desired path, then the guidance logic rotates the vehicle so its velocity direction approaches the desired path at a small angle.

Fall 2004

16.333 1019

Figure 20 denes the notation used for a linearization.

d as
cmd

1
desired flight path

L1

reference point

Figure 20: Linear Model for Straightline Following Case

L1 is the distance from the vehicle to the reference point. d is the crosstrack error V is the vehicle nominal speed.

Assuming is small sin = 1 + 2 and d d 1 , 2 L1 V Combining the above gives ascmd = 2 V V sin 2 L1 L1
2

V d+ d L1

(1)

Linearization yields a PD controller for the crosstrack error. Ratio of V and separation distance L1 is an important factor in determining the proportional and derivative controller gains. Key points: NL form works signicantly better than a PD and is much more tolerant to winds disturbances.

Fall 2004

16.333 1020

Implementation

Can implement this command by assuming that vehicle maintains sucient lift to balance weight, even though banked at angle . Requires that the vehicle speed up and/or change to a larger . Lift increment W L mg (n 1) CL = QS QS
where n L/W is the load factor.
Assume that L cos = W , then L sin = mas, so that
as
tan = g We can use this to develop d that we apply to the roll controller.
Kp

? +6 d 0 K c ? a c r -

Ha (s)

p Glat (s)

1 s

Kr

Hs (s)

r- 1 s

s  s + 1

Some simulations shown Works well hardest part is determining where to place the ref erence point, which is L1 ahead along the path. Recall that L1 acts like a gain in the controller making it too small can drive the aircraft unstable.

Fall 2004
x 10
4

16.333 1021

veh Path

2 1.5 1 0.5 0 0.5 1 1.5 2

xe

Ye

5 x 10
4

Figure 21: Simulation #1: path. Turn radius R V 2 /(g tan )

30
d

20

10

and d

10

20

30 0 500 1000 time 1500 2000

Figure 22: simulation #1: bank angle. Limited to 30 degs here.

Fall 2004
3.5 x 10
5

16.333 1022

veh Path

2.5

xe 1.5 1 0.5 0 2000

2000

4000

6000 Ye

8000

10000

12000

14000

30

20

10

and d

10

20

30 0 200 400 600 time 800 1000 1200

Figure 23: simulation #2

Fall 2004

16.333 1023

Flight Test

Guidance algorithm implemented and tested with two UAVs [Parent Child Unmanned Air Vehicle (PCUAV) project by Prof. Deyst] Required lateral acceleration achieved using bank angle control Nominal ight velocity of about 25 m/s, the choice of L1=150 m results in the associated crossover frequency at 0.4 rad/s.

Figure 24 shows the ight data for the Mini vehicle using the guidance logic in the lateral dynamics. Plot shows the 2dimensional trajectory of the Mini vehicle () with a commanded desired trajectory ( ). Small numbers along the trajectory are the ight times recorded in the onboard avionics. Lateral displacement between the vehicle and the desired path within 2 meters for the 75% of its ight time and within 3 meters for 96% of the ight time .

A similar ight test was performed for the OHS Parent (see Figure 25) After the transient period, the trajectory of the vehicle followed the commanded path within 2 meters for the 78% of its ight time and within 3 meters for 97% of the ight time.

Fall 2004
300
190

16.333 1024

Desired Path Position Trajectory

180

200

100
200

North [m]

0
170

100
210

230

200
220

300
150 141

160

400 300

200

100

0 100 East [m]

200

300

400

Figure 24: Flight Data of MINI Trajectory Following

500
90

Desired Path Position Trajectory

400

170

300 North [m]

150

200

110

76

100

0
130

100 700

600

500

400 East [m]

300

200

100

Figure 25: Flight Data of OHS Parent Trajectory Following

Fall 2004

16.333 1025

Then demonstrated rendezvous from any arbitrary initial positions to a conguration of tight formation ight. Figures 26 show the positions of the Parent and the Mini in the northeast 2D map every 10 seconds. OHS Parent vehicle follows the circular ight path, with no knowl edge of the Mini vehicles location. Mini vehicle schedules its ight path and performs formation ight by receiving position information from the OHS Parent.
300 200 100 NORTH [m] 0 100 200 300 400 300
O

300 40 [sec] 200 100 NORTH [m] 0 100 200 300 400 300 120 [sec]

M O

200

100

0 100 EAST [m]

200

300

400

200

100

0 100 EAST [m]

200

300

400

300 200 100 NORTH [m] 0 100 200 300 400 300
O M

300 70 [sec] 200 100 NORTH [m] 0 100 200 300 400 300 90
M

[sec]

200

100

0 100 EAST [m]

200

300

400

200

100

0 100 EAST [m]

200

300

400

Figure 26: Flight Data Rendezvous Trajectories of OHS and Mini (O:OHS, M:Mini)

Fall 2004
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52

16.333 1026

53 54 55 56 57 58 59 60 61 62 63 64 65 66

% newr.m % Analyze tracking algorithm by Park et al % AIAA GNC 2004 % % Assumes that ac3.m has been run to generate syscl % Jonathan How % MIT 16.333 Fall 2004 % % close all dt=1; % time step for the simulation U0=235.9; path=[]; jcase=1; % 2 path cases considered if jcase==1 t=[0:5*dt:2500]; omp=.0025; path=24000*[sin(omp*t) (1cos(omp*t))]; xe=0;ye=1500; X=[.1 0 0 0*pi/180 0*pi/180 0 0 0]; else t=[0:dt:1350]; path(:,1)=U0*t; omp=.005; path(:,2)=500*(cos(2*pi*omp*t)+1).*exp(.002*t); xe=0;ye=1000; X=[.1 0 0 15*pi/180 15*pi/180 0 0 0]; end % Discretize the dynamics with time step dt % system has the inner yaw and roll loops closed [A,B,C,D]=ssdata(syscl); syscld=c2d(ss(A,B,C,D),dt); [Ad,Bd,Cd,Dd]=ssdata(syscld); Bd=Bd(:,1);Dd=Dd(:,1); % only need first input % bank angle limit philim=30; % %inputs are phi_d and 0 %state x=[v p r phi Psi xx xx xx] L1=2000; % look ahead distance store=[]; % find the point on the path L1 ahead ii=find((xepath(:,1)).^2+(yepath(:,2)).^2 < L1^2); iii=max(ii); % % %

kk=1; while (~isempty(iii)) & (kk< length(t)1) kk=kk+1; aim_point=path(iii,:); xedot=U0*cos(X(5))X(1)*cos(X(4))*sin(X(5)); yedot=U0*sin(X(5))+X(1)*cos(X(4))*cos(X(5)); v1=[xedot yedot]; v2=[aim_point(1)xe aim_point(2)ye]; v1=v1/norm(v1); v2=v2/norm(v2); [v1 v2]; temp=cross([v1;0],[v2;0]); eta=acos(v1*v2)*sign(temp(3)); phi_d=atan(2*U0^2/L1*sin(eta)/9.81); phi_d=max(min(phi_d,philim*pi/180),philim*pi/180); store=[store;[t(kk) X xe ye phi_d v2]]; % propagate forward a step X=Ad*X+Bd*phi_d; xe=xe+xedot*dt; ye=ye+yedot*dt; ii=find((xepath(:,1)).^2+(yepath(:,2)).^2 < L1^2); iii=max(ii); end figure(1);clf plot(store(:,11),store(:,10),m); hold on;plot(path(:,2),path(:,1),g); legend(veh,Path);xlabel(Y_e); ylabel(x_e);setlines(2);hold off if jcase==1 axis(square);axis(equal) print depsc park_1; jpdf(park_1) else orient tall print depsc park_1a; jpdf(park_1a) end figure(2);clf plot(store(:,1),store(:,[5 12])*180/pi); axis([0 t(kk) philim*1.1 philim*1.1]) xlabel(time);ylabel(\phi and \phi_d); legend(\phi,\phi_d);setlines(2) if jcase==1 print depsc park_2; jpdf(park_2) else print depsc park_2a; jpdf(park_2a) end return

67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104