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