AOE 5204: Homework Assignment 4 Due: Wednesday, 9/21 in class. Extended to Friday 9/23 Online Students:

Transcription

1 AOE 5204: Homework Assignment 4 Due: Wednesday, 9/21 in class. Extended to Friday 9/23 Online Students: (cdhall@vt.edu) by 5 PM Suppose F b is initially aligned with F i and at t = 0 begins to rotate with angular velocity ω bi = e t [ sin tî1 + sin 2tî2 + sin 3tî3]. Use numerical integration to obtain the body-frame attitude for the time interval from t = 0 to t = 2π seconds. Do the numerical integration using Euler angles as well as quaternions. Use appropriate graphs and sample calculations to convince yourself (and me) that the two approaches give the same results. Solution: The initial conditions are based on the statement that F b is initially aligned with F i, which implies that the initial rotation matrix is R bi = 1 (the 3 3 identity matrix). Furthermore, R bi = 1 θ = 0. The initial condition for the quaternion, q, is simply q = [ ] T, which is evident from the relationship given by q 4 = ± trace R q = 1 R 23 R 32 R 31 R 13 4q 4 R 12 R 21 So, we have the necessary initial conditions to integrate the differential equations. The differential equations are θ = S 1 (θ)ω bi b q = Q( q)ω bi b The right-hand side of the θ equation depends on θ and ω, whereas the right-hand side of the q equation depends on q and ω. In both cases, ω is the angular velocity of the body frame with respect to the inertial frame, expressed in the body frame. You are given the angular velocity expressed in the inertial frame, so it must be rotated into the body frame, using R bi. The rotation matrix R bi can be computed either using the Euler angles or using the quaternion. The Euler angle approach We need the rotation matrix R bi and the kinematics differential equation matrix S(θ), and its inverse. All three matrices are obtained in straightforward fashion as: R bi = S = c 1 c 3 s 1 c 2 s 3 s 1 c 3 + c 1 c 2 s 3 s 2 s 3 c 1 s 3 s 1 c 2 c 3 s 1 s 3 + c 1 c 2 c 3 s 2 c 3 s 1 s 2 c 1 s 2 c 2 s 2 s 3 c 3 0 s 2 c 3 s 3 0 c

2 S 1 = s 3 /s 2 c 3 /s 2 0 c 3 s 3 0 c 2 s 3 /s 2 c 2 c 3 /s 2 1 Obtaining these three matrices for a given Euler angle sequence is something you should be able to do. An Essential Observation: The matrix S 1 has a singularity at θ 2 = 0. This singularity occurs at θ 2 = 0 for all six of the symmetric Euler angle sequences, whereas an equivalent singularity occurs at θ 2 = π/2 or 3π/2 for the six asymmetric sequences. Integration of the kinematics differential equation for the Euler angles is impossible for the given initial conditions! This observation is one of the main points of this homework assignment: I want you to have firsthand experience with the difficulty associated with the use of Euler angles in practical applications. For this homework, I suggested tweaking the initial condition on θ 2 slightly to avoid the singularity. Of course, that tweak changes the value of R(0), which in turn changes the value of q for the other half of the assignment. A right-hand side file for integrating the θ differential equations is % RHS for Homework problem % Name this file HW4RHS.m function thetadot = HW4RHS(t,theta) % the kinematics matrix S(theta): s2 = sin(theta(2)); % compute these first c2 = cos(theta(2)); s3 = sin(theta(3)); c3 = cos(theta(3)); S = [s2*s3 c3 0 ; s2*c3 -s3 0 ; c2 0 1]; % the rotation matrix (uses 3 simple functions I wrote) R = R3(theta(3))*R1(theta(2))*R3(theta(1)); % the angular velocity in Fi w = exp(-t).*[sin(t) sin(2*t) sin(3*t)] ; % the right-hand side thetadot = S\(R*w); With some appropriately defined initial conditions, theta0, and a tspan matrix, we can integrate the equations using this Matlab statement: [t,theta]=ode45( HW4RHS,tspan,theta0); The result of this line of Matlab code is an n 1 matrix of t values and an n 3 matrix of θ values. The complete code is given below. A plot of the Euler angles for a tweak of θ 2 = 0.1 is given in Fig.??. 2

3 Figure 1: Euler Angles The quaternion approach For the quaternions, we do not have the singularity problem. The differential equations are simple, and here is a right-hand side file: % qbar RHS for Homework problem % name this file HW4RHSq.m function qbardot = HW4RHSq(t,qbar) qbar = qbar/norm(qbar); % good idea to renormalize % the kinematics matrix Q(qbar) Q = 0.5*[cr(qbar(1:3))+qbar(4)*eye(3); -qbar(1:3) ]; % the rotation matrix (uses a simple function I wrote) R = q2r(qbar); % the angular velocity in Fi w = exp(-t).*[sin(t) sin(2*t) sin(3*t)] ; % the right-hand side qbardot = Q*R*w; With this file in place, the line of Matlab code to perform the integration is simply [t,qbar] = ode45( HW4RHSq,tspan,qbar0,options); The result of the integration is an n 1 matrix of t values and an n 4 matrix of q values. 3

4 Figure 2: Quaternions The complete code is given below. A plot of the quaternions for the same tweak as used for the Euler angles is given in Fig.??. The Comparison There are numerous ways to compare the two sets of results. Obviously, looking at the two graphs is not sufficient. One way would be to begin with the quaternion solution, compute a rotation matrix at each time step, extract the Euler angles, and compare the result with the Euler angles obtained in the first integration. The extreme tediousness of the extraction will be left as an exercise. Another approach would be to begin with the Euler angles solution, compute a rotation matrix at each time step, extract the quaternion, and compare the result with the quaternions obtained in the second integration. This approach is easy enough and is what is accomplished in the Matlab code (included at the end of the solution). A graph of the differences between the integrated quaternions and the quaternions obtained using this method is given in Fig.??. The differences are on the order of 10 8, which suggests a strong agreement between the two approaches. Another approach is to compute two rotation matrices for each time step, using the Euler angles solution to compute R θ, and using the quaternion solution to compute R q. If the two approaches were absolutely identical, then the two series of rotation matrices would be absolutely identical, and hence R T θ R q = 1 at each time step. However, due to numerical issues, the two results will not be exactly equal, and the matrix multiplication will not give 1, but rather, an error matrix: R T θ R q = R e 4

5 Figure 3: Quaternion Comparison Clearly R e is a rotation matrix, and if the numerical results are correct, it should be near the identity matrix. One way to quantify how near is to extract the Euler angle, Φ from R e, as it is a measure of the error between the two integration methods. Recall that Φ = cos 1 [ 1 2 (trace R 1) ] Carrying out this computation and making a plot of Φ vs. t yields the graph in Fig.??. The angle remains small, which is another indication of a strong equivalence between the two approaches to integrating the attitude kinematics. % Driver file for Homework 4 % calls HW4RHS and HW4RHSq, % the right-hand side files for the Euler angles % and the quaternions, respectively clear all % Euler angle part first % initial conditions theta0 = [0 0 0] ; % we know this won t work theta0(2) = 1e-1; % this "tweak" can be changed % will need the initial rotation matrix for computing % the initial quaternion R0 = R3(theta0(3))*R1(theta0(2))*R3(theta0(1)); 5

6 Figure 4: Euler Angle Φ n = 100; % this way we ll have a specific number of integration points tspan = linspace(0,2*pi,n); options = odeset( reltol,1e-8, abstol,1e-8); [t,theta]=ode45( HW4RHS,tspan,theta0,options); % now do the quaternion part % to make the initial conditions the same, need to extract % from rotation matrix % (actually I have a function called "r2q" to do this, % but I ll spell it out q4 = sqrt(1+trace(r0))/2; q = [R0(2,3)-R0(3,2); R0(3,1)-R0(1,3); R0(1,2)-R0(2,1)]/(4*q4); qbar0 = [q;q4]; [t,qbar] = ode45( HW4RHSq,tspan,qbar0,options); % we have done the integration, and we can plot the results % plot the euler angles figure(1); handle = plot(t,theta(:,1), -,t,theta(:,2), :,t,theta(:,3),.- ); set(handle, linewidth,1.5); xlabel( t, fontsize,18); ylabel( \theta, fontsize,18); legend( \theta_1, \theta_2, \theta_3 ); set(gca, fontsize,18, linewidth,1.5); print -deps HW4EulerAngles 6

7 % plot the quaternions figure(2); handle = plot(t,qbar(:,1), -,t,qbar(:,2), :,... t,qbar(:,3),.-,t,qbar(:,4), -- ); set(handle, linewidth,1.5); xlabel( t, fontsize,18); ylabel( qbar, fontsize,18); legend( q_1, q_2, q_3, q_4 ); set(gca, fontsize,18, linewidth,1.5); print -deps HW4quaternions % now we need to make some comparisons % use the thetas to calculate R, then extract qbar % compare with the integrated qbar qdiff = zeros(size(qbar)); % where we ll save the difference for i=1:n R = R3(theta(i,3))*R1(theta(i,2))*R3(theta(i,1)); qb = r2q(r); %using that simple function % deal with that +/- issue if qb(4)*qbar(i,4)<0 qb=-qb; end qdiff(i,:) = qb -qbar(i,:); end figure(3); handle = plot(t,qdiff(:,1), -,t,qdiff(:,2), :,... t,qdiff(:,3),.-,t,qdiff(:,4), -- ); set(handle, linewidth,1.5); xlabel( t, fontsize,18); ylabel( qbar difference, fontsize,18); legend( q_1, q_2, q_3, q_4 ); set(gca, fontsize,18, linewidth,1.5); print -deps HW4qdiff % another way to compare phidiff = zeros(n,1); % where we ll save the Euler angle for i=1:n Rt = R3(theta(i,3))*R1(theta(i,2))*R3(theta(i,1)); Rq = q2r(qbar(i,:)); Re = Rt *Rq; [a,phidiff(i)] = r2aphi(re); % another simple function end figure(4); handle = plot(t,phidiff); 7

8 set(handle, linewidth,1.5); xlabel( t, fontsize,18); ylabel( Euler angle difference, fontsize,18); set(gca, fontsize,18, linewidth,1.5); print -deps HW4phidiff 8