16.333: Lecture #12

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 Figure 8: Roll response to an initial roll offset 9 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 τ d U0 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