Professional Documents
Culture Documents
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
Fall 2004 16.333 101
Lateral Autopilots
1.5
0.94
0.985
0.5
Imaginary Axis
0.5
0.985
0.94
1.5
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
0.25 1
1 1 0.25
0.5 0.5
0.75 0.75
0.5
0.5 0.5
0.25
Imaginary Axis
Imaginary Axis
0 1 0.5 0.25
0
0.25
0.5 0.5
0.5
0.75 0.75
0.5 0.5
1 0.25 1 0.25
1
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
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.
0
10
|H (s)|
w
1
10
2
10
2 1 0 1
10 10 10 10
Freq (rad/sec)
Hw (s)
Fall 2004 16.333 104
1 0.25
0.5
0.75
0.5
Imaginary Axis
1 0.5 0.25
0
0.5
0.75
0.5
1 0.25
1.5
2 1.5 1 0.5 0 0.5 1
Real Axis
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 105
1
Without Washout
With Washout
0.5
Response r
0.5
0 5 10 15 20 25 30
Time
1
Without Washout
With Washout
0.5
Control r
0.5
0 5 10 15 20 25 30
Time
Figure 6: Impulse response of closed loop system with and without the Washout
lter ( = 4.2). Commanded rc = 0, and both have (r )ss = 0, but without the
0.
lter, rss = 0, whereas with it, rss =
For direct comparison with and without the lter, 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 eort 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 106
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
=
d 1s + 1
with 1=1520sec depending on the vehicle and the goals.
A low pass lter that eliminates the higher frequency noise.
Roll Control
which gives
La
= sL )
a s(Ixx p
8
8
0.68
6
6
4
4
0.88
2
2
Imaginary Axis
2
2
0.88
4
4
6
6
0.68
8
8
Figure 7: Root Locus for roll loop closed Loop poles for Kp = 20, K = 30
Fall 2004 16.333 109
0.2
0.1
0.1
0.2
0.3
0.4
0 1 2 3 4 5 6 7 8 9 10
x 10
3 Reponse to initial roll of 15 degs
10
r
2
0 1 2 3 4 5 6 7 8 9 10
0.25
0.2
0.15
0.1
0.05
0.05
0 5 10 15 20 25 30
Time (sec)
0.25
0.2
0.15
0.1
0.05
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 eects.
Fall 2004 16.333 1011
Heading Autopilot
Putting the pieces together we get the following autopilot controller
Kp
? c
? a-
1
- K Ha (s) - - -
+6 p s
d Glat (s)
0 rc- r- 1 -
- Kr Hs (s) -
6 s
s
s + 1
U0
1 g + 6
d
0.88
1
0.94
0.5
0.975
0.994
Imaginary Axis
0.994
0.975
0.5
0.94
1
0.88
Figure 10: Heading loop root locus. Closed loop poles for gain U0 /(g1 ). 8th order
system, but RL fairly well behaved.
Fall 2004 16.333 1012
0.6
p
0.5
0.4
0.3
0.2
0.1
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.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 .
Mux ac3out
To Workspace
desired path
time
U0
Clock
Signal
Generator
ye
16.333 1015
4
Fall 2004 1.5
x 10
Y
16.333 1016
ref
Y
a/c
0.5
e
Path Y
0
0.5
1.5
0 100 200 300 400 500 600 700 800 900 1000
0.4
0.2
0
and c
0.2
0.4
0.6
0.8
0 100 200 300 400 500 600 700 800 900 1000
1
and c
3
0 100 200 300 400 500 600 700 800 900 1000
Alternative Strategy
aircraft
V
aS L1
cmd
as
= t
V reference point
: velocity direction change
due to the acceleration
L1
s=Vt
Figure 18: One Time Step
Vehicle Position
Reference Point
50 L1
40
Desired Path
30
20
Y
10
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 1019
L1
d as 1 reference point
cmd 1
desired flight path
Implementation
as
tan =
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
d Glat (s)
0 rc- r- 1 -
- Kr Hs (s) -
6 s
s
s + 1
4
x 10
veh
Path
2
1.5
0.5
0
xe
0.5
1.5
0 1 2 3 4 5
Ye x 10
4
30
d
20
10
and d
10
20
30
5
x 10
3.5
veh
Path
2.5
2
xe
1.5
0.5
0
2000 0 2000 4000 6000 8000 10000 12000 14000
Ye
30
d
20
10
and d
10
20
30
Flight Test
Figure 24 shows the ight 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 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 1024
300
Desired Path
190 Position Trajectory
200
180
100
200
0
North [m]
170
230
100
210
200
220
300 160
150
141
400
300 200 100 0 100 200 300 400
East [m]
500
Desired Path
Position Trajectory
90
400
170
300 150
North [m]
200 110 76
100
0
130
100
700 600 500 400 300 200 100
East [m]
Figure 25: Flight Data of OHS Parent Trajectory Following
100 O 100
NORTH [m]
NORTH [m]
0 M 0
100 100
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
100 100
NORTH [m]
NORTH [m]
0 0
100 100
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)