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 Glat(s) δr 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 c feedback on r to the rudder: δr = kr(rc − r) 3.33 a c – Servo dynamics Hr = s+3.33 added to rudder δr= Hrδ r – System: 1.618s3 + 0.7761s2 + 0.03007s + 0.1883 G c = − δrr s5 + 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
3.5 3 2.5 2 1.5 1 0.5 0 Imaginary Axis
−0.5 0.985
−1
0.94 −1.5
0.86 0.76 0.64 0.5 0.34 0.16 −2 −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0 Real Axis
c Figure 2: Lateral polezero map Gδr 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 δ with k>0 Lateral autopilot: r to δ with k<0 r r 2 2
1.5 1.5
1 1 0.25 1 0.25 0.5 0.5
0.75 0.75 0.5 0.5 0.5 0.25
1 0.5 0.25 0 0 Imaginary Axis 0.25 Imaginary Axis
−0.5 0.5 −0.5 0.75 0.75
0.5 0.5 −1 0.25 1 −1 0.25
−1.5 −1.5
−2 −2 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0 Real Axis Real Axis
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.
c τ s • New yaw damper: δr = kr(rc − Hw(s)r) where Hw(s) = τ s+1 is the “washout” filter.
Washout filter with τ=4.2
0 10 (s)| w |H −1 10
−2 10 −2 −1 0 1 10 10 10 10 Freq (rad/sec) Figure 4: Washout filter with τ = 4.2
• New control picture
δa p φ - - 1 - s Glat(s) c rc δ r ψ - r- - - 1 - kr Hr(s) – 6 s
Hw(s) Fall 2004 16.333 10–4
Lateral autopilot: r to rudder WITH washout filter 1.5
1 0.25 0.5
0.75
0.5
1 0.5 0.25 0 Imaginary Axis
−0.5
0.75
0.5 −1 0.25
−1.5 −2 −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
1 Without Washout With Washout
0.5
Response r 0
−0.5 0 5 10 15 20 25 30 Time
1 Without Washout With Washout
r 0.5 δ Control 0
−0.5 0 5 10 15 20 25 30 Time
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 φ U ψ˙ tan φ = 0 g • Since typically φ � 1, we have U ψ˙ φ ≈ 0 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 ˙ U0 φd = ψ = (ψd − ψ) g τ1g 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� p˙ = L p + L δ � xx p δa a I� φ¨ − L φ˙ = L δ φ˙ = p xx p δa a which gives φ Lδa = � δa s(Ixxs − Lp) • For the design, add the aileron servo dynamics 1 H (s) = , δa = H (s)δc a 0.15s + 1 a a a c • PD controller δa = −kφ(sγ + 1) + kφφd, adds zero at s = −1/γ – Pick γ = 2/3 Root Locus 10 10 0.52 0.38 0.28 0.2 0.12 0.06
8 8 0.68
6 6
4 4 0.88
2 2
0 Imaginary Axis −2 2
0.88 −4 4
−6 6
0.68 −8 8
0.52 0.38 0.28 0.2 0.12 0.06 −10 −7 −6 −5 −4 −3 −2 −1 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 4 5 6 7 8 9 10
−3 x 10 Reponse to initial roll of 15 degs 10 ψ r β
8
6
4
2
0
−2 0 1 2 3 4 5 6 7 8 9 10 Figure 8: Roll response to an initial roll offset Fall 2004 16.333 10–10
Reponse to step input φ =15 degs (0.26 rad) c 0.3 φ p
0.25
0.2
0.15
0.1
0.05
0
−0.05 0 5 10 15 20 25 30 Time (sec)
Reponse to step input φ =15 degs (0.26 rad) c 0.35 ψ r 0.3 β
0.25
0.2
0.15
0.1
0.05
0
−0.05
−0.1 0 5 10 15 20 25 30 Time (sec)
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 φ – – δc φ ? - a-? - - 1 - Kφ Ha(s) + 6 p s G (s) φd lat 0 δc r ψ - r - - - 1 - Kr Hs(s) – 6 s
τs τs + 1 U – 0 6 τ1g + ψd
• Last step: analyze effect of closing the ψ → φd loop
Heading Command Loop τ =12 sec 1
0.8 0.68 0.5 0.25
0.88 1
0.94
0.5 0.975
0.994
2 1.75 1.5 1.25 1 0.75 0.5 0.25 0
Imaginary Axis 0.994
0.975 −0.5
0.94
−1 0.88
0.8 0.68 0.5 0.25
−2 −1.5 −1 −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 • 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.6 φ p
0.5
0.4
0.3
0.2
0.1
0
−0.1 0 5 10 15 20 25 30 35 40 45 50 Time (sec) 0.3 ψ r β 0.25 ψ c
0.2
0.15
0.1
0.05
0
−0.05
−0.1 0 5 10 15 20 25 30 35 40 45 50 Time (sec)
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) τdU0 τdU0 which includes a feedback on the aircraft inertial Y position. ⇒ Use this as the input ψd. Fall 2004 16.333 10–15 Path1 desired ye ye ac3out s 1 To Workspace Integrator Mux -K- Banking mix1 phi v p r psi u em all data all data -K- Kp Nwash(s) Dwash(s) syslat2 Lateral Washout FIlter x u M JNr(s) JDr(s) JNa(s) JDa(s) desired path Aileron Dynamics Rudder Dynamics Gain 0 Kphi Constant Saturation -K- Banking mix psi Psi_d -5000 U0 path offset psi_ref desired path time -K- Clock Psi_ref Signal 2*pi/180 Generator
Figure 13: Simulink implementation – run ac3.m first 4 x 10 Fall 2004 −1.5 16.333 10–16 Y ref Y a/c
−1
−0.5 e 0 Path Y
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
0.2
0 c Φ and Φ −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
1 c Ψ 0 and Ψ
−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. V 2 – Lateral Acceleration Command – determined by as = 2 sin η cmd L1
aircraft V η L a 1 Scmd
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.
1Sanghyuk 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 ∆t V reference point : velocity direction change due to the acceleration L1 ∆s=V∆t η Figure 18: One Time Step Vehicle Position Reference Point 50 L 1
40
Desired Path 30
Y 20
10
0 At some point, acceleration sign changes → Smooth convergence to desired path −10 0 20 40 60 80 X 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. η 2 V η L d 1 reference point as η cmd 1 η 1 desired flight path 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 2 � � V V ˙ V ascmd = 2 sin η ≈ 2 d + d (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 L − mg W ΔC = ≡ (n − 1) L QS QS where n ≡ L/W is the load factor.
– Assume that L cos φ = W , then L sin φ = mas, so that a tan φ = s g
• We can use this to develop φd that we apply to the roll controller.
Kp φ – – δc φ ? - ? a- - - 1 - Kφ Ha(s) + 6 p s G (s) φd lat 0 δc r ψ - r- - - 1 - Kr Hs(s) – 6 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
4 x 10 veh Path 2
1.5
1
0.5 e
x 0
−0.5
−1
−1.5
−2
0 1 2 3 4 5 Y 4 e x 10 Figure 21: Simulation #1: path. Turn radius R ≈ V 2/(g tan φ)
30 φ φ d
20
10 d φ 0 and φ
−10
−20
−30
0 500 1000 1500 2000 time Figure 22: simulation #1: bank angle. Limited to 30 degs here. Fall 2004 16.333 10–22 5 x 10 3.5 veh Path
3
2.5
2 e x
1.5
1
0.5
0 −2000 0 2000 4000 6000 8000 10000 12000 14000 Y e
30 φ φ d
20
10 d φ 0 and φ
−10
−20
−30
0 200 400 600 800 1000 1200 time 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 Desired Path 190 Position Trajectory 200 180
100 200
0
170
North [m] 230 −100
210 −200
220
−300 160
150 141 −400 −300 −200 −100 0 100 200 300 400 East [m] Figure 24: Flight Data of MINI Trajectory Following 500 Desired Path Position Trajectory 90 400 170
300 150
200 110 76 North [m]
100
0
130
−100 −700 −600 −500 −400 −300 −200 −100 East [m] 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 300
200 40 [sec] 200 120 [sec]
100 O 100
0 M 0
−100 −100
NORTH [m] NORTH [m] M O
−200 −200
−300 −300
−400 −400 −300 −200 −100 0 100 200 300 400 −300 −200 −100 0 100 200 300 400 EAST [m] EAST [m] ⇓ ⇑ 300 300
200 70 [sec] 200 90 [sec] O M
100 100
0 0
−100 −100 NORTH [m] ⇒ NORTH [m]
−200 O −200 M
−300 −300
−400 −400 −300 −200 −100 0 100 200 300 400 −300 −200 −100 0 100 200 300 400 EAST [m] EAST [m] Figure 26: Flight Data Rendezvous Trajectories of OHS and Mini (O:OHS, M:Mini) Fall 2004 16.333 10–26
1 % newr.m 53 kk=1; 2 % Analyze tracking algorithm by Park et al 54 while (~isempty(iii)) & (kk< length(t)1) 3 % AIAA GNC 2004 55 kk=kk+1; 4 % 56 aim_point=path(iii,:); 5 % Assumes that ac3.m has been run to generate syscl 57 6 % Jonathan How 58 xedot=U0*cos(X(5))X(1)*cos(X(4))*sin(X(5)); 7 % MIT 16.333 Fall 2004 59 yedot=U0*sin(X(5))+X(1)*cos(X(4))*cos(X(5)); 8 % 60 9 % 61 v1=[xedot yedot]’; 10 close all 62 v2=[aim_point(1)xe aim_point(2)ye]’; 11 dt=1; % time step for the simulation 63 v1=v1/norm(v1); 12 U0=235.9; 64 v2=v2/norm(v2); 13 path=[]; 65 [v1 v2]; 14 66 temp=cross([v1;0],[v2;0]); 15 jcase=1; 67 eta=acos(v1’*v2)*sign(temp(3)); 16 % 2 path cases considered 68 phi_d=atan(2*U0^2/L1*sin(eta)/9.81); 17 if jcase==1 69 phi_d=max(min(phi_d,philim*pi/180),philim*pi/180); 18 t=[0:5*dt:2500]’; 70 19 omp=.0025; 71 store=[store;[t(kk) X’ xe ye phi_d v2’]]; 20 path=24000*[sin(omp*t) (1cos(omp*t))]; 72 % propagate forward a step 21 xe=0;ye=1500; 73 X=Ad*X+Bd*phi_d; 22 X=[.1 0 0 0*pi/180 0*pi/180 0 0 0]’; 74 xe=xe+xedot*dt; 23 else 75 ye=ye+yedot*dt; 24 t=[0:dt:1350]’; 76 25 path(:,1)=U0*t; 77 ii=find((xepath(:,1)).^2+(yepath(:,2)).^2 < L1^2); 26 omp=.005; 78 iii=max(ii); 27 path(:,2)=500*(cos(2*pi*omp*t)+1).*exp(.002*t); 79 end 28 xe=0;ye=1000; 80 29 X=[.1 0 0 15*pi/180 15*pi/180 0 0 0]’; 81 figure(1);clf 30 end 82 plot(store(:,11),store(:,10),’m’); 31 83 hold on;plot(path(:,2),path(:,1),’g’); 32 % Discretize the dynamics with time step dt 84 legend(’veh’,’Path’);xlabel(’Y_e’); 33 % system has the inner yaw and roll loops closed 85 ylabel(’x_e’);setlines(2);hold off 34 [A,B,C,D]=ssdata(syscl); 86 if jcase==1 35 syscld=c2d(ss(A,B,C,D),dt); 87 axis(’square’);axis(’equal’) 36 [Ad,Bd,Cd,Dd]=ssdata(syscld); 88 print depsc park_1; jpdf(’park_1’) 37 Bd=Bd(:,1);Dd=Dd(:,1); % only need first input 89 else 38 90 orient tall 39 % bank angle limit 91 print depsc park_1a; jpdf(’park_1a’) 40 philim=30; 92 end 41 % 93 42 %inputs are phi_d and 0 94 figure(2);clf 43 %state x=[v p r phi Psi xx xx xx] 95 plot(store(:,1),store(:,[5 12])*180/pi); 44 L1=2000; % look ahead distance 96 axis([0 t(kk) philim*1.1 philim*1.1]) 45 store=[]; 97 xlabel(’time’);ylabel(’\phi and \phi_d’); 46 98 legend(’\phi’,’\phi_d’);setlines(2) 47 % find the point on the path L1 ahead 99 if jcase==1 48 ii=find((xepath(:,1)).^2+(yepath(:,2)).^2 < L1^2); 100 print depsc park_2; jpdf(’park_2’) 49 iii=max(ii); 101 else 50 % 102 print depsc park_2a; jpdf(’park_2a’) 51 % 103 end 52 % 104 return