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: δrc = kr (rc − r)
– Servo dynamics Hr =
3.33
s+3.33
added to rudder δra = Hr δrc
– System:
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
Gδrcr
1.618s3 + 0.7761s2 + 0.03007s + 0.1883
=− 5
s + 3.967s4 + 3.06s3 + 3.642s2 + 1.71s + 0.01223
0
3
3.5
2.5
2
1.5
0.5
1
−0.5
0.985
−1
0.94
−1.5
−2
0.86
−3.5
0.76
−3
−2.5
0.64
−2
−1.5
0.5
0.34
−1
0.16
−0.5
0
Real Axis
Figure 2: Lateral pole­zero map Gδrc 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)
Lateral autopilot: r to δr with k>0
Lateral autopilot: r to δr with k<0
2
2
1.5
1.5
1
0.5
0.25
1
1
0.5
0.75
0.5
0.5
0.25
Imaginary Axis
Imaginary Axis
0.5
0
0.25
−0.5
0.5
−0.5
1
−1
0.25
0.75
0.5
0.25
−1.5
−2
−2
0.5
1
0
0.75
−1
0.25
0.75
0.5
0.25
−1.5
−1.5
−1
−0.5
0
Real Axis
0.5
1
1.5
2
−2
−3.5
−3
−2.5
−2
−1.5
−1
−0.5
0
Real Axis
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: δrc = kr (rc − Hw (s)r) where Hw (s) =
“washout” filter.
τs
τ s+1
is the
Washout filter with τ=4.2
0
w
|H (s)|
10
−1
10
−2
10
−2
−1
10
10
0
Freq (rad/sec)
1
10
10
Figure 4: Washout filter with τ = 4.2
• New control picture
δa
-
p- 1
s
φ-
r- 1
s
ψ-
Glat (s)
rc
– 6
-
kr
δrc-
Hr (s)
-
Hw (s) Fall 2004
16.333 10–4
Lateral autopilot: r to rudder WITH washout filter
1.5
0.25
1
0.5
0.75
Imaginary Axis
0.5
1
0
0.5
0.25
−0.5
0.75
0.5
−1
−1.5
−2
0.25
−1.5
−1
−0.5
0
0.5
1
Real Axis
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
Response r
1
Without Washout
With Washout
0.5
0
−0.5
0
5
10
15
Time
20
Control δr
1
25
30
Without Washout
With Washout
0.5
0
−0.5
0
5
10
15
Time
20
25
30
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 φ
tan φ =
U0ψ̇
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:
ψ̇ =
φd =
U0
U0
(ψd − ψ)
ψ̇ =
g
τ1 g
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
�
�
ṗ = Lpp + Lδa δa
Ixx
� ¨
I
φ − Lpφ̇ = Lδa δa
xx
φ˙ = p
which gives
φ
Lδa
=
� s−L )
δa s(Ixx
p
• For the design, add the aileron servo dynamics
1
Ha(s) =
, δaa = Ha(s)δac
0.15s + 1
• PD controller δac = −kφ(sγ + 1) + kφφd, adds zero at s = −1/γ
– Pick γ = 2/3
Root Locus
10
8
0.52
10
0.38
0.28
0.2
0.12
0.06
8
0.68
6
6
4
4
0.88
2
Imaginary Axis
2
0
−2
−4
2
0.88
4
−6
−8
−10
−7
6
0.68
8
0.38
0.52
−6
−5
0.28
−4
−3
0.2
−2
0.12
−1
0.06
10 0
Real Axis
Figure 7: Root Locus for roll loop – closed Loop poles for Kp = −20, Kφ = −30
Fall 2004
16.333 10–9
Reponse to initial roll of 15 degs
0.3
φ
p
0.2
0.1
0
−0.1
−0.2
−0.3
−0.4
0
1
2
3
5
6
7
8
9
10
Reponse to initial roll of 15 degs
−3
10
4
x 10
ψ
r
β
8
6
4
2
0
−2
0
1
2
3
4
5
6
7
8
9
Figure 8: Roll response to an initial roll offset
10
Fall 2004
16.333 10–10
Reponse to step input φc=15 degs (0.26 rad)
0.3
φ
p
0.25
0.2
0.15
0.1
0.05
0
−0.05
0
5
10
15
Time (sec)
20
25
30
Reponse to step input φc=15 degs (0.26 rad)
0.35
ψ
r
β
0.3
0.25
0.2
0.15
0.1
0.05
0
−0.05
−0.1
0
5
10
15
Time (sec)
20
25
30
Figure 9: Roll response to a step command for φc . Note the adverse yaw effects.
Fall 2004
16.333 10–11
Heading Autopilot
• Putting the pieces together we get the following autopilot controller
Kp
φ
– ?
+6
φd
–
Kφ
-
δc
? a-
Ha (s)
-
-
p
1
s
φ
-
Glat (s)
0
–
-
δrc-
Kr
Hs (s)
r- 1
s
-
6
ψ-
τs
τs + 1
U0
τ1 g
–
6
+ ψ
d
• Last step: analyze effect of closing the ψ → φd loop
Heading Command Loop τ1=12 sec
0.68
0.8
1
0.5
0.25
0.88
0.94
Imaginary Axis
0.5
0.975
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
−1
0.88
0.8
−2
−1.5
0.68
−1
0.5
0.25
−0.5
0
0.5
1
1.5
2
Real Axis
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.6
φ
p
0.5
0.4
0.3
0.2
0.1
0
−0.1
0
5
10
15
20
25
Time (sec)
30
35
40
45
0.3
50
ψ
r
β
ψc
0.25
0.2
0.15
0.1
0.05
0
−0.05
−0.1
0
5
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 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
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.
Psi_ref
2*pi/180
Signal
Generator
-K-
Clock
time
psi_ref
desired path
path offset
-5000
U0
psi
Psi_d
Saturation
Banking mix
-K-
JNr(s)
JDr(s)
0
Rudder Dynamics
JDa(s)
Constant
Gain
Kphi
JNa(s)
Aileron Dynamics
desired path
Mux
Dwash(s)
Nwash(s)
Washout FIlter
Lateral
syslat2
Kp
-K-
all data
p
phi
psi
all data r
emu
v
Banking mix1
-K-
Mux
ac3out
1
s
Integrator
ye
ye
desired
To Workspace
Path1
Fall 2004
16.333 10–15
Figure 13: Simulink implementation – run ac3.m first
4
Fall 2004
16.333 10–16
x 10
−1.5
Y
ref
Y
a/c
−1
Path Y
e
−0.5
0
0.5
1
1.5
0
100
200
300
400
500
600
700
800
900
1000
Figure 14: Path following for ground track controller
0.6
Φ
Φc
0.4
Φ and Φc
0.2
0
−0.2
−0.4
−0.6
−0.8
0
100
200
300
400
500
600
700
800
900
1000
Figure 15: Roll command following for ground track controller
3
Ψ
Ψc
2
Ψ and Ψc
1
0
−1
−2
−3
0
100
200
300
400
500
600
700
800
900
1000
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.
2
– Lateral Acceleration Command – determined by ascmd = 2 VL1 sin η
aircraft
V
aS
η
L1
cmd
desired path
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
16.333 10–18
∆ψ =
as
∆t
V
reference point
: velocity direction change
due to the acceleration
L1
∆s=V∆t
η
Figure 18: One Time Step
Vehicle Position
Reference Point
L1
50
40
Y
30
Desired Path
20
10
At some point, acceleration sign changes
→ Smooth convergence to desired path
0
−10
0
20
40
X
60
80
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 10–19
• Figure 20 defines the notation used for a linearization.
η
V
2
η
d
as
cmd
L1
η1
desired flight path
η1
reference point
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
2
ascmd = 2
V
V
sin η ≈ 2
L1
L1
�
�
V
d˙ + d
L1
(1)
– 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
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
-
Kφ
–
δc
? a-
Ha (s)
-
-
p
1
s
φ
-
Glat (s)
0
–
-
Kr
δrc-
Hs (s)
r- 1
s
-
6
ψ-
τ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
4
x 10
veh
Path
2
1.5
1
xe
0.5
0
−0.5
−1
−1.5
−2
0
1
2
Ye
3
4
5
4
x 10
Figure 21: Simulation #1: path. Turn radius R ≈ V 2 /(g tan φ)
φ
φ
30
d
20
φ and φd
10
0
−10
−20
−30
0
500
1000
time
1500
2000
Figure 22: simulation #1: bank angle. Limited to 30 degs here.
Fall 2004
16.333 10–22
5
3.5
x 10
veh
Path
3
2.5
xe
2
1.5
1
0.5
0
−2000
0
2000
4000
6000
Ye
8000
10000
12000
14000
φ
φ
30
d
20
φ and φd
10
0
−10
−20
−30
0
200
400
600
time
800
Figure 23: simulation #2
1000
1200
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
Desired Path
Position Trajectory
190
200
180
100
North [m]
200
0
170
230
−100
210
−200
220
−300
160
150
−400
−300
141
−200
−100
0
100
East [m]
200
300
400
Figure 24: Flight Data of MINI ­ Trajectory Following
500
90
400
Desired Path
Position Trajectory
170
North [m]
300
200
150
76
110
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 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
300
40
200
0
O
100
M
−100
0
−100
−200
−200
−300
−300
−400
−300
−200
120 [sec]
200
NORTH [m]
NORTH [m]
100
[sec]
−100
0
100
EAST [m]
200
300
−400
−300
400
M
O
−200
−100
⇓
300
400
300
70
200
[sec]
200
O
90
[sec]
M
100
0
−100
⇒
O
−200
NORTH [m]
100
NORTH [m]
200
⇑
300
0
−100
−200
M
−300
−400
−300
0
100
EAST [m]
−300
−200
−100
0
100
EAST [m]
200
300
400
−400
−300
−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
% 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=[];
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
33
34
35
36
37
40
41
42
43
44
45
48
49
50
51
52
55
56
kk=1;
while (~isempty(iii)) & (kk< length(t)­1)
kk=kk+1;
aim_point=path(iii,:);
57
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));
58
59
60
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);
61
62
63
64
65
67
68
69
70
72
73
74
75
76
ii=find((xe­path(:,1)).^2+(ye­path(:,2)).^2 < L1^2);
iii=max(ii);
77
78
79
end
80
81
82
84
85
86
87
88
89
91
92
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
93
94
95
96
97
98
% find the point on the path L1 ahead
ii=find((xe­path(:,1)).^2+(ye­path(:,2)).^2 < L1^2);
iii=max(ii);
%
%
%
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;
71
90
% 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=[];
46
47
54
83
% 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
38
39
53
66
jcase=1;
% 2 path cases considered
if jcase==1
t=[0:5*dt:2500]’;
omp=.0025;
path=24000*[sin(omp*t) (1­cos(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
31
32
16.333 10–26
99
100
101
102
103
104
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