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
-
G
lat
(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
= k
r
(r
c
? r)
r
3.33
– Servo dynamics H
r
=
s+3.33
added to rudder δ
a
= H
r
δ
c
r r
– System:
1.618s
3
+ 0.7761s
2
+ 0.03007s + 0.1883
G
δ
r
c
r
= ?
s
5
+ 3.967s
4
+ 3.06s
3
+ 3.642s
2
+ 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
Imaginary Axis
c
r
Figure 2: Lateral pole-zero map G
δ r
?
Fall 2004 16.333 10–2
? Note that the gain of the plant is negative (K
plant
< 0), so if k
r
< 0,
then K = K
plant
k
r
> 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 dr with k>0
Real Axis
Imaginary Axis
?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 dr with k<0
Real Axis
Imaginary Axis
Figure 3: Lateral pole-zero map. De?nitely need k
r
< 0
? Root locus with k
r
< 0 looks pretty good as we have authority over
the four poles.
– k
r
= ?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 ?ight modes that require a steady yaw rate
(r
ss
= 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 10–3
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.
? New yaw damper: δ
c
= k
r
(r
c
? H
w
(s)r) where H
w
(s) =
τ s
is the
r
τ s+1
“washout” ?lter.
10?2 10?1 100 10110
?2
10?1
100
Washout filter with t=4.2
|H
w
(s)|
Freq (rad/sec)
Figure 4: Washout ?lter with τ = 4.2
? New control picture
p φ
1
δ
a
- - -
s
G
lat
(s)
δ
c
r ψ
1
r
r
c
- - - - -
H
r
(s)k
r
6
– s
H
w
(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.5
0.25
Lateral autopilot: r to rudder WITH washout filter
Real Axis
Imaginary Axis
Figure 5: Root Locus with the washout ?lter included.
? Zero in H
w
(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 ?
k
r
= ?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
Response r
Without Washout
With Washout
0 5 10 15 20 25 30?0.5
0
0.5
1
Time
Control
d
r
Without Washout
With Washout
Figure 6: Impulse response of closed loop system with and without the Washout
?lter (τ = 4.2). Commanded r
c
= 0, and both have (δ
r
)
ss
= 0, but without the
?lter, r
ss
= 0, whereas with it, r
ss
= 0.
? For direct comparison with and without the ?lter, applied impulse as
r
c
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 e?ort 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 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 mU
0
ψ
˙
is?
along the body z-axis
– Summing in the body y-axis direction gives mu
0
ψ
˙
cos φ = mg sin φ
U
0
ψ
˙
tan φ =
g
? Since typically φ ? 1, we have
U
0
ψ
˙
φ ≈
g
gives the desired bank angle for a speci?ed 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 ?ltering it.
– Assume that the desired heading is known ψ
d
and we want ψ to
follow ψ
d
relatively slowly
– Choose dynamics τ
1
ψ
˙
+ψ = ψ
d
ψ 1
= ?
ψ
d
τ
1
s + 1
with τ
1
=15-20sec depending on the vehicle and the goals.
– A low pass ?lter that eliminates the higher frequency noise.
? Filtered heading angle satis?es
1
ψ
˙
= (ψ
d
? ψ)
τ
1
which we can use to create the desired bank angle:
U
0
φ
d
= ψ
˙
=
U
0
(ψ
d
? ψ)
g τ
1
g
?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
Imaginary Axis
?
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
? φ)? k
p
p
? To design k
φ
and k
p
, can just use the approximation of the roll mode
I
?
I
?
¨
˙
xx
p˙ = L
p
p +L
δ
a
δ
a
xx
φ ? L
p
φ
˙
= L
δ
a
δ
a
φ = p
which gives
φ L
δ
a
=
δ
a
s(I
?
xx
s ? L
p
)
? For the design, add the aileron servo dynamics
1
H
a
(s) = , δ
a
= H
a
(s)δ
c
a
0.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 K
p
= ?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.3
Reponse to initial roll of 15 degs
f
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
y
r
b
Figure 8: Roll response to an initial roll o?set
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 fc=15 degs (0.26 rad)
f
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 fc=15 degs (0.26 rad)
y
r
b
Figure 9: Roll response to a step command for φ
c
. Note the adverse yaw e?ects.
?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 t1=12 sec
Real Axis
Imaginary Axis
Fall 2004 16.333 10–11
Heading Autopilot
? Putting the pieces together we get the following autopilot controller
K
p
φ
?
6
–
+
-
K
φ
–
H
a
(s)
-?
δ
c
a
- -
p
1
s
-
φ
φ
d
G
lat
(s)
–
0
6
-
K
r
H
s
(s)
-
δ
c
r
- -
r
1
s
-
ψ
τs
τs + 1
U
0
–
τ
1
g
6
+
ψ
d
? Last step: analyze e?ect of closing the ψ → φ
d
loop
Figure 10: Heading loop root locus. Closed loop poles for gain U
0
/(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
φ
, K
p
, τ
1
, τ , and K
r
? 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)
f
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)
y
r
by
c
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 10–14
Ground Track Control
? Consider scenario shown - use this to determine desired heading ψ
d
Figure 12: Heading de?nitions
– Are initially at (0,0), moving along X
e
(ψ
0
= 0)
– Want to be at (0,1000) moving along dashed line angled at ψ
ref
? Separation distance d = Y
ref
?Y
a/c
(d
0
= 1000)
– Desired inertial y position - y position of aircraft
d
˙
= U
0
sin(ψ
ref
?ψ) ≈U
0
(ψ
ref
?ψ)
? Want to smoothly decrease d to zero, use a ?lter so that
d
˙
=
1
d τ
d
= 30 sec?
τ
d
1
d = U
0
(ψ
ref
?ψ)??
τ
d
1 1
= ψ = ψ
ref
+ d = ψ
ref
+ (Y
ref
?Y
a/c
)?
τ
d
U
0
τ
d
U
0
which includes a feedback on the aircraft inertial Y position.
? Use this as the input ψ
d
.
Fall 2004 16.333 10–15
-5000
path offset
Nwash(s)
Dwash(s)
Washout FIlter
ac3out
To Workspace
Signal
Generator
Saturation
JNr(s)
JDr(s)
Rudder Dynamics
2*pi/180
Psi_ref
Path1
desired
ye
Mux
Muxsyslat2
Lateral
-K-
Kp
1
s
Integrator
-K-
U0
Kphi
Gain
emu
0
Constant
Clock
-K-
Banking mix1
-K-
Banking mix
JNa(s)
JDa(s)
Aileron Dynamics
all data
all datar
p
phi
v
psi
psi
time
desired path
desired path
psi_ref
Psi_d
ye
Figure 13: Simulink implementation – run ac3.m ?rst
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
Path Y
e
YrefY
a/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
F
and
F
c
FF
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
Y
and
Y
c
YY
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 summer
1
? 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 (L
1
) forward of the vehicle – Figure 17.
– Lateral Acceleration Command – determined by a
s
cmd
= 2
V
2
sin η
L
1
R
η
V
R
L
1
desired path
reference point
2η
a
S
cmd
aircraft
Figure 17: Diagram for Guidance Logic
? Direction of acceleration depends on sign of angle between the L
1
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 L
1
line segment.
1
Sanghyuk Park, John Deyst, and Jonathan How, “A New Nonlinear Guidance Logic for Trajectory Tracking,” AIAA GNC 2004.
Fall 2004 16.333 10–18
η
?ψ =
a
s
V
?t
?s=V?t
L
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 Position
Reference Point
L
1
Δt V L
1
Figure 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 de?nes the notation used for a linearization.
d
reference point
desired flight path
η
1
η
2
η
L
1
V
η
1
s
cmd
a
Figure 20: Linear Model for Straight-line Following Case
– L
1
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
≈
L
1
V
? Combining the above gives
V
2
a
s
cmd
= 2 sin η ≈ 2
V
d
˙
+
V
d (1)
L
1
L
1
L
1
– Linearization yields a PD controller for the cross-track error.
– Ratio of V and separation distance L
1
is an important factor in
determining the proportional and derivative controller gains.
– Key points: NL form works signi?cantly 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
su?cient lift to balance weight, even though banked at angle φ.
– Requires that the vehicle speed up and/or change to a larger α.
Lift increment
W
ΔC
L
=
L ? mg
≡ (n ? 1)
QS QS
where n ≡ L/W is the load factor.
– Assume that L cos φ = W , then L sin φ = ma
s
, so that
a
s
tan φ =
g
? We can use this to develop φ
d
that we apply to the roll controller.
K
p
φ
δ
c
– – φ
1
a
? - ? - - - -
H
a
(s)K
φ
6
+ p s
G
lat
(s)
φ
d
δ
c
0 r ψ
1
r
- - - - -
H
s
(s)K
r
6
– s
τ s
τ s + 1
Some simulations shown ?
– Works well – hardest part is determining where to place the ref-
erence point, which is L
1
ahead along the path.
– Recall that L
1
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
e
veh
Path
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
f
and
f
d
ff
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 10
5
Ye
x
e
veh
Path
0 200 400 600 800 1000 1200
?30
?20
?10
0
10
20
30
time
f
and
f
d
ffd
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 ?ight velocity of about 25 m/s, the choice of L
1
=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 2-dimensional 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 16.333 10–24
?300 ?200 ?100 0 100 200 300 400
?400
?300
?200
?100
0
100
200
300
East [m]
North [m]
141
150
160
170
180
190
200
210
220
230
Desired Path
Position 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]
North [m]
76
90
110
130
150
170
Desired Path
Position 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 con?guration of tight formation ?ight.
– 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 ?ight path, with no knowl-
edge of the Mini vehicle’s location.
– Mini vehicle schedules its ?ight path and performs formation ?ight
by receiving position information from the OHS Parent.
?300?200?1000 100200300400?400
?300
?200
?100
0
100
200
300
O
M
EAST [m]
NORTH [m]
40 [sec]
?300?200?1000 100200300400?400
?300
?200
?100
0
100
200
300
OM
EAST [m]
NORTH [m]
120[sec]
? ?
?300?200?1000 100200300400?400
?300
?200
?100
0
100
200
300
O M
EAST [m]
NORTH [m]
70 [sec]
?
?300?200?1000 100200300400?400
?300
?200
?100
0
100
200
300
OM
EAST [m]
NORTH [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