aircraft lateral autopilots - mit opencourseware · pdf fileaircraft lateral autopilots •...
Post on 07-Mar-2018
230 Views
Preview:
TRANSCRIPT
Lecture # 12
Aircraft Lateral Autopilots
• Multiloop 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 Dutchroll 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 polezero 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 polezero 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 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 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 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 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 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 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=1520sec 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
• 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 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
2η
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 vector, 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 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
V 2
ascmd = 2 sin η ≈ 2 V
d + Vd (1)
L1 L1 L1
– 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 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 reference 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 2dimensional 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 northeast 2D 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) (1cos(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((xepath(:,1)).^2+(yepath(:,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((xepath(:,1)).^2+(yepath(:,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
top related