Aircraft Lateral Autopilots - MIT OpenCourseWare · PDF fileAircraft Lateral Autopilots •...

27
Lecture # 12 Aircraft Lateral Autopilots Multi-loop closure Heading Control: linear Heading Control: nonlinear

Transcript of Aircraft Lateral Autopilots - MIT OpenCourseWare · PDF fileAircraft Lateral Autopilots •...

Lecture # 12

Aircraft Lateral Autopilots

• Multi­loop closure

Heading Control: linear

Heading Control: nonlinear

Fall 2004 16.333 10–1

Lateral Autopilots

• We can stabilize/modify the lateral dynamics using a variety of dif­

ferent feedback architectures. δa - -

p 1 -φ

s

δr -

Glat(s)

-r 1

s

• Look for good sensor/actuator pairings to achieve desired behavior.

• Example: Yaw damper

– Can improve the damping on the Dutch­roll mode by adding a feedback on r to the rudder: δc = kr(rc − r)r

3.33 – Servo dynamics Hr = s+3.33 added to rudder δa = Hrδc

r r

– System:

1.618s3 + 0.7761s2 + 0.03007s + 0.1883 Gδr

cr = −s5 + 3.967s4 + 3.06s3 + 3.642s2 + 1.71s + 0.01223

−3.5 −3 −2.5 −2 −1.5 −1 −0.5 0−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

0.94

0.86 0.160.340.50.640.76

3.5

0.76

0.985

0.160.340.50.64

1

0.86

0.94

0.985

0.53 1.522.5

Lateral autopilot: r to rudder

Real Axis

Imag

inar

y A

xis

cr

Figure 2: Lateral pole­zero map Gδ r

Fall 2004 16.333 10–2

• 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)

−2 −1.5 −1 −0.5 0 0.5 1 1.5 2−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

1

0.50.25

0.25

0.25

0.75

0.5

0.5

1

0.75

0.25

0.5

Lateral autopilot: r to δr with k>0

Real Axis

Imag

inar

y A

xis

−3.5 −3 −2.5 −2 −1.5 −1 −0.5 0−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

1

0.25

0.250.5

0.5

0.75

0.25

0.5

0.75

Lateral autopilot: r to δr with k<0

Real Axis

Imag

inar

y A

xis

Figure 3: Lateral pole­zero map. Definitely 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 Dutch­roll damping and spiral/roll modes have combined into a damped oscillation.

• Yaw damper looks great, but this implementation has a problem.

– There are various flight modes that require a steady yaw rate (rss = 0). For example, steady turning flight.

– 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 10–3

Yaw Damper: Part 2

• Can avoid this problem to some extent by filtering 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.

• New yaw damper: δc = kr(rc − Hw(s)r) where Hw(s) = τ s is the r τ s+1 “washout” filter.

10−2 10−1 100 10110−2

10−1

100

Washout filter with τ=4.2

|Hw

(s)|

Freq (rad/sec)

Figure 4: Washout filter with τ = 4.2

• New control picture

p φ1δa - - -

s Glat(s)

δc r ψ1rrc - - - - -Hr(s)kr6– s

�Hw(s)

Fall 2004 16.333 10–4

−2 −1.5 −1 −0.5 0 0.5 1−1.5

−1

−0.5

0

0.5

1

1.5

0.250.5

0.5

0.75

0.75

0.25

1

0.50.25

Lateral autopilot: r to rudder WITH washout filter

Real Axis

Imag

inar

y A

xis

Figure 5: Root Locus with the washout filter 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 16.333 10–5

0 5 10 15 20 25 30−0.5

0

0.5

1

Time

Res

pons

e r

Without WashoutWith Washout

0 5 10 15 20 25 30−0.5

0

0.5

1

Time

Con

trol δ

r

Without WashoutWith Washout

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

• For direct comparison with and without the filter, applied impulse as rc to both closed­loop 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 effort is being applied to correct the motion.

• But only the one with the washout filter produces a zero control input even though the there is a steady turn ⇒ the controller will not try to fight a commanded steady turn.

Fall 2004 16.333 10–6

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 z­axis

– Summing in the body y­axis direction gives mu0ψ cos φ = mg sin φ

U0ψtan φ =

g

• Since typically φ � 1, we have

U0ψφ ≈

g

gives the desired bank angle for a specified turn rate.

Fall 2004 16.333 10–7

Problem now is that ψ tends to be a noisy signal to base out bank • angle on, so we generate a smoother signal by filtering 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=15­20sec depending on the vehicle and the goals.

– A low pass filter that eliminates the higher frequency noise.

• Filtered heading angle satisfies

1 ψ = (ψd − ψ)

τ1

which we can use to create the desired bank angle:

U0φd = ψ =

U0 (ψd − ψ)

g τ1g

−7 −6 −5 −4 −3 −2 −1 0−10

−8

−6

−4

−2

0

2

4

6

8

10

0.88

0.68

0.52 0.38 0.28 0.060.120.2

0.68

0.38 0.28 0.2 0.12 0.06

8

0.88

10

0.52

6

2

4

6

8

10

2

4

Root Locus

Real Axis

Imag

inar

y A

xis

Fall 2004 16.333 10–8

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

I �I � ¨

˙xxp = Lpp + Lδa δa

xxφ − Lpφ = Lδa δaφ = p

which gives φ Lδa = δa s(I �xxs − Lp)

• For the design, add the aileron servo dynamics

1 Ha(s) = , δa = Ha(s)δ

c a0.15s + 1 a

• PD controller δc = −kφ(sγ + 1) + kφφd, adds zero at s = −1/γa

– Pick γ = 2/3

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

Fall 2004 16.333 10–9

0 1 2 3 4 5 6 7 8 9 10−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3Reponse to initial roll of 15 degs

φp

0 1 2 3 4 5 6 7 8 9 10−2

0

2

4

6

8

10x 10−3 Reponse to initial roll of 15 degs

ψrβ

Figure 8: Roll response to an initial roll offset

Fall 2004 16.333 10–10

0 5 10 15 20 25 30−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

Time (sec)

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

φp

0 5 10 15 20 25 30−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Time (sec)

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

ψrβ

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

−2 −1.5 −1 −0.5 0 0.5 1 1.5 2

−1

−0.5

0

0.5

1

0.68 0.5 0.25

2

0.975

0.94

0.88

0.8

0.68 0.25

0.994

1.5

0.50.8

11.75

0.88

0.50.751.25 0.25

0.94

0.975

0.994

Heading Command Loop τ1=12 sec

Real Axis

Imag

inar

y A

xis

Fall 2004 16.333 10–11

Heading Autopilot

• Putting the pieces together we get the following autopilot controller

Kp

φ

? 6

+ - Kφ

– Ha(s)-?

δc a - -

p 1 s

φd Glat(s)

0 6

- Kr Hs(s)-δc r - -

r 1 s

τs �

τs + 1 U0 � �

τ1g 6+ ψd

• Last step: analyze effect of closing the ψ → φd loop

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

Fall 2004 16.333 10–12

• 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

• Multi­loop 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 10–13

0 5 10 15 20 25 30 35 40 45 50−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

Time (sec)

φp

0 5 10 15 20 25 30 35 40 45 50−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

Time (sec)

ψrβψ

c

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

Fall 2004 16.333 10–14

Ground Track Control

• Consider scenario shown ­ use this to determine desired heading ψd

Figure 12: Heading definitions

– 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 filter so that

d = 1 d τd = 30 sec−

τd 1 d = U0(ψref − ψ)⇒ −

τd 1 1

= ψ = ψref + d = ψref + (Yref − Ya/c)⇒τdU0 τdU0

which includes a feedback on the aircraft inertial Y position.⇒ Use this as the input ψd.

Fall 2004 16.333 10–15

-500

0

path

offs

et

Nw

ash(

s)

Dw

ash(

s)

Was

hout

FIlt

er

ac3o

ut

To

Wor

kspa

ce

Sig

nal

Gen

erat

or

Sat

urat

ion

JNr(

s)

JDr(

s)

Rud

der

Dyn

amic

s

2*pi

/180

Psi

_ref

Pat

h1

desi

red

ye

Mux

Mux

sysl

at2

Late

ral

-K-

Kp

1 s

Inte

grat

or

-K-

U0

Kph

i

Gai

nem

u

0

Con

stan

t

Clo

ck

-K-

Ban

king

mix

1

-K-

Ban

king

mix

JNa(

s)

JDa(

s)

Aile

ron

Dyn

amic

s

all d

ata

all d

ata

rp

phi

v

psi

psi

time

desi

red

path

desi

red

path

psi_

ref

Psi

_d

ye

Figure 13: Simulink implementation – run ac3.m first

Fall 2004

0 100 200 300 400 500 600 700 800 900 1000

−1.5

−1

−0.5

0

0.5

1

1.5

x 104

Pat

h Y e

Yref

Ya/c

16.333 10–16

Figure 14: Path following for ground track controller

0 100 200 300 400 500 600 700 800 900 1000−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

Φ a

nd Φ

c

ΦΦ

c

Figure 15: Roll command following for ground track controller

0 100 200 300 400 500 600 700 800 900 1000−3

−2

−1

0

1

2

3

Ψ a

nd Ψ

c

ΨΨ

c

Figure 16: Heading following for ground track controller

Fall 2004 16.333 10–17

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 = 2V 2 sin ηL1

R

ηV

R

L1

desired path reference point

aScmd

aircraft

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.

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

Fall 2004 16.333 10–18

η

∆ψ = asV

∆t

∆s=V∆tL

1

: velocity direction change due to the acceleration

reference point

Figure 18: One Time Step

0 20 40 60 80−10

0

10

20

30

40

50

X

Y

Desired Path

At some point, acceleration sign changes

→ Smooth convergence to desired path

Vehicle PositionReference PointL

1

Δt V L1Figure 19: Step by Step ( =1, =10, and =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 10–19

• Figure 20 defines the notation used for a linearization.

d reference point

desired flight path

η1

η2

ηL1

V

η1s

cmda

Figure 20: Linear Model for Straight­line Following Case

– L1 is the distance from the vehicle to the reference point.

– d is the cross­track error

– V is the vehicle nominal speed.

• Assuming η is small sin η ≈ η = η1 + η2 and

d dη1 ≈ , η2 ≈

L1 V

• Combining the above gives

V 2

ascmd = 2 sin η ≈ 2 V

d + Vd (1)

L1 L1 L1

– Linearization yields a PD controller for the cross­track 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 significantly better than a PD and is much more tolerant to winds disturbances.

Fall 2004 16.333 10–20

Implementation

• Can implement this command by assuming that vehicle maintains sufficient lift to balance weight, even though banked at angle φ.

– Requires that the vehicle speed up and/or change to a larger α. Lift increment

W ΔCL =

L − mg ≡ (n − 1)QS QS

where n ≡ L/W is the load factor.

– Assume that L cos φ = W , then L sin φ = mas, so thatas

tan φ = g

• We can use this to develop φd that we apply to the roll controller.

Kp

φδc– – φ1a? - ? - - - -Ha(s)Kφ6+ p s

Glat(s)φd δc0 r ψ1r- - - - -Hs(s)Kr6– 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 16.333 10–21

0 1 2 3 4 5

x 104

−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

x 104

Ye

x evehPath

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

0 500 1000 1500 2000

−30

−20

−10

0

10

20

30

time

φ an

d φ d

φφ

d

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

Fall 2004 16.333 10–22

−2000 0 2000 4000 6000 8000 10000 12000 140000

0.5

1

1.5

2

2.5

3

3.5x 105

Ye

x e

vehPath

0 200 400 600 800 1000 1200

−30

−20

−10

0

10

20

30

time

φ and φ d

φφ

d

Figure 23: simulation #2

Fall 2004 16.333 10–23

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 flight 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 flight data for the Mini vehicle using the guidance logic in the lateral dynamics.

– Plot shows the 2­dimensional trajectory of the Mini vehicle (–) with a commanded desired trajectory (­ ­).

– Small numbers along the trajectory are the flight times recorded in the onboard avionics.

– Lateral displacement between the vehicle and the desired path within ±2 meters for the 75% of its flight time and within ±3 meters for 96% of the flight time

.

• A similar flight 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 flight time and within ±3 meters for 97% of the flight time.

Fall 2004 16.333 10–24

−300 −200 −100 0 100 200 300 400−400

−300

−200

−100

0

100

200

300

East [m]

Nor

th [m

]

141150

160

170

180

190

200

210

220

230

Desired PathPosition Trajectory

Figure 24: Flight Data of MINI ­ Trajectory Following

−700 −600 −500 −400 −300 −200 −100−100

0

100

200

300

400

500

East [m]

Nor

th [m

]

76

90

110

130

150

170

Desired PathPosition Trajectory

Figure 25: Flight Data of OHS Parent ­ Trajectory Following

Fall 2004 16.333 10–25

• Then demonstrated rendezvous from any arbitrary initial positions to a configuration of tight formation flight.

– Figures 26 show the positions of the Parent and the Mini in the north­east 2­D map every 10 seconds.

– OHS Parent vehicle follows the circular flight path, with no knowl­

edge of the Mini vehicle’s location.

– Mini vehicle schedules its flight path and performs formation flight by receiving position information from the OHS Parent.

−300 −200 −100 0 100 200 300 400−400

−300

−200

−100

0

100

200

300

O

M

EAST [m]

NO

RTH

[m]

40 [sec]

−300 −200 −100 0 100 200 300 400−400

−300

−200

−100

0

100

200

300

OM

EAST [m]

NO

RTH

[m]

120 [sec]

⇓ ⇑

−300 −200 −100 0 100 200 300 400−400

−300

−200

−100

0

100

200

300

OM

EAST [m]

NO

RTH

[m]

70 [sec]

−300 −200 −100 0 100 200 300 400−400

−300

−200

−100

0

100

200

300

OM

EAST [m]

NO

RTH

[m]

90 [sec]

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

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

Fall 2004 16.333 10–26

% newr.m 53 kk=1; % Analyze tracking algorithm by Park et al 54 while (~isempty(iii)) & (kk< length(t)­1) % AIAA GNC 2004 55 kk=kk+1; % 56 aim_point=path(iii,:); % Assumes that ac3.m has been run to generate syscl 57

% Jonathan How 58 xedot=U0*cos(X(5))­X(1)*cos(X(4))*sin(X(5)); % MIT 16.333 Fall 2004 59 yedot=U0*sin(X(5))+X(1)*cos(X(4))*cos(X(5)); % 60

% 61 v1=[xedot yedot]’; close all 62 v2=[aim_point(1)­xe aim_point(2)­ye]’; dt=1; % time step for the simulation 63 v1=v1/norm(v1); U0=235.9; 64 v2=v2/norm(v2); path=[]; 65 [v1 v2];

66 temp=cross([v1;0],[v2;0]); jcase=1; 67 eta=acos(v1’*v2)*sign(temp(3)); % 2 path cases considered 68 phi_d=atan(2*U0^2/L1*sin(eta)/9.81); if jcase==1 69 phi_d=max(min(phi_d,philim*pi/180),­philim*pi/180);

t=[0:5*dt:2500]’; 70

omp=.0025; 71 store=[store;[t(kk) X’ xe ye phi_d v2’]]; path=24000*[sin(omp*t) (1­cos(omp*t))]; 72 % propagate forward a step xe=0;ye=1500; 73 X=Ad*X+Bd*phi_d; X=[.1 0 0 0*pi/180 0*pi/180 0 0 0]’; 74 xe=xe+xedot*dt;

else 75 ye=ye+yedot*dt; t=[0:dt:1350]’; 76

path(:,1)=U0*t; 77 ii=find((xe­path(:,1)).^2+(ye­path(:,2)).^2 < L1^2); omp=.005; 78 iii=max(ii); path(:,2)=500*(­cos(2*pi*omp*t)+1).*exp(.002*t); 79 end xe=0;ye=­1000; 80

X=[.1 0 0 ­15*pi/180 ­15*pi/180 0 0 0]’; 81 figure(1);clf end 82 plot(store(:,11),store(:,10),’m’);

83 hold on;plot(path(:,2),path(:,1),’g’); % Discretize the dynamics with time step dt 84 legend(’veh’,’Path’);xlabel(’Y_e’); % system has the inner yaw and roll loops closed 85 ylabel(’x_e’);setlines(2);hold off [A,B,C,D]=ssdata(syscl); 86 if jcase==1 syscld=c2d(ss(A,B,C,D),dt); 87 axis(’square’);axis(’equal’) [Ad,Bd,Cd,Dd]=ssdata(syscld); 88 print ­depsc park_1; jpdf(’park_1’) Bd=Bd(:,1);Dd=Dd(:,1); % only need first input 89 else

90 orient tall % bank angle limit 91 print ­depsc park_1a; jpdf(’park_1a’) philim=30; 92 end % 93

%inputs are phi_d and 0 94 figure(2);clf %state x=[v p r phi Psi xx xx xx] 95 plot(store(:,1),store(:,[5 12])*180/pi); L1=2000; % look ahead distance 96 axis([0 t(kk) ­philim*1.1 philim*1.1]) store=[]; 97 xlabel(’time’);ylabel(’\phi and \phi_d’);

98 legend(’\phi’,’\phi_d’);setlines(2) % find the point on the path L1 ahead 99 if jcase==1 ii=find((xe­path(:,1)).^2+(ye­path(:,2)).^2 < L1^2); 100 print ­depsc park_2; jpdf(’park_2’) iii=max(ii); 101 else % 102 print ­depsc park_2a; jpdf(’park_2a’) % 103 end % 104 return