哈工大惯技术(导航原理)大作业.doc

Assignment ofInertial Technology惯性技术作业My Chinese NameMy Student ID15S004001Autumn 2015Assignment 1: 2-DOF response simulation A 2-DOF gyro is subjected to a sinusoidal torque with amplitude of 4 g.cm and frequency of 10 Hz along its outer ring axis. The angular moment of its rotor is 10000 g.cm/s, and the angular inertias in its equatorial plane are both 4 g.cm/s2. Please simulate the response of the gyro within 1 second, and present whatever you can discover or confirm from the result. In this simulation, we are going to discuss the response of a 2-DOF gyro to sinusoidal torque input. According to the transfer function of the 2-DOF gyro, the outputs can be expressed as: The original system transfer function is a 2-input, 2-output coupling system. But the given input only exists one input, we can treat the system as 2 separate FIFO system As a consequence, we can establish the block diagram of the system in simulink in Fig 1.1.Substitute the parameters into the system and input, then we have the input signals as follow: Then the inverse Laplace transform of the output equals the response of the gyro in time domain as follows: Fig 1.1 The block diagram of the system in simulink And the simulation results in time domain within 1 second are shown in the follwing pictures. Fig1.2 is the the output of outer ring . Fig1.3 is the output of inner ring . Fig1.4 is the trajectory of 2-DOF gyros response to sinusoidal input. As we can see from the Fig1.3,there are obvious sawtooth wave in the output of the inner ring. Its a unexpected phenomenon in my original theoretical analysis. Fig1.2 The output of inner ring Fig 1.3 The output of outer ring I believe the sawtooth wave is caused by the nutation. For the frequency of the nutation is obtained as , which is far higher than the frequency of the applied sinusoidal torque , namely . Fig 1.4 Trajectory of 2-DOF gyros response to sinusoidal inputThe trajectory of 2-DOF gyros response to sinusoidal input are shown in Fig1.4. As we can see, its coupling of X and Y channel scope output. The overall shape is an ellipse, which is not perfect for there are so many sawteeth on the top of it.Note that the major axis of ellipse is in the direction of the forced procession, amplitude of which is , whereas the minor axis is in the direction of the torsion spring effects, with amplitude .The nutation components are much smaller than that of the forced vibration, which can be eliminated to get the clear static response. To prove it, we eliminate the effects of the nutation namely the quadratic term in the denominator and get Fig 1.5, which is a perfect ellipse. We can conclude that when input to the 2-DOF gyro is sinusoidal torque, the gyro will do an ellipse conical pendulum as a static response, including procession and the torsion spring effects, together with a high-frequency vibration as the dynamic response. Fig 1.5 Trajectory of the gyros response without nutationAssignment 2: Single-axis INS simulation2.1 description of the problemA magnetic levitation train is being tested along a track running north-south. It first accelerates and then cruises at a constant speed. Onboard is a single-axis platform INS, working in the way described by the courseware of Unit 5: Basic problems of INS. The motion information and Earth parameters are shown in table 2-1, and the possible error sources are shown in Table2-2. Fig2.1 The sketch map of the single-axis INS problemYou are asked to simulate the operation of the INS within 10,000 seconds, and investigate, first one by one and then altogether, the impact of these error sources on the performance of the INS.The block diagram in the courseware might be of some help. However, there lurks an inconspicuous error, which you have to correct before you can obtain reasonable results. There are one core relevant formula, to get the specific form of its solution, we should substitute the unknown parameters. Firstly, the input signal is accelerometer of the platform, and the velocity of the platform is the integration of the acceleration. The acceleration along Yp may contains two parts:When accelerometer errors are concerned, the output of accelerometer will be:When gyro errors concerned:Only is unknown: And the reference block diagram and simulink block diagram are as follows in Fig2.2, Fig2.3. There is a small fault in the reference block, which is that the sign of the marked add operation should be positive instead of negative. The results of the simulation are shown in Fig 2.4 to Fig 2.13. Fig2.2 The reference block diagram in the courseware(unrectified)Fig2.3 The simulink block diagram for Assignment 22.2 results and analysis of the problem Fig.2.4 real acceleration,velocity and displacement output without error sources Fig2.5 position bias when only accelerometer scale factor error existsFig2.6 position bias output when only gyro scale factor error exists Fig2.7 position bias when only acceleromete bias error existsFig2.8 position bias output when only initial velocity error exists Fig2.9 position bias output when only initial position error exists Fig2.10 position bias output bias when only gyro bias errorexists Fig2.11 position bias output when only initial platform misalignment angle exists Fig2.12 output considering all the error sources Fig2.13 position bias output considering all error sourcesAs we can see in the above simulation results, if there is no error we can navigate the trains motion correctly, which comes from north to the south as shown in Fig2.4, beginning with an constant acceleration within 60 seconds then cruises at a constant speed, approximately 65 m/s. However, the situation will change a lot when different errors put into the simulation. The initial position error effects least as Fig.2.8, for this error doesnt enter into the closed loop and it wont influence the iterative process. The position bias is constant and can be negligible.In the second case, when the accelerometer scale factor error exists, , as shown in Fig2.5, the result are stable and almost accurate, the position bias is a sinusoidal output. So it is with the accelerometer bias error situation, , in Fig2.7, the initial velocity error, in Fig2.9, and the initial platform misalignment angle, , in Fig2.11. However, the influence degrees of the different factors are not in the same magnitude. The accelerometer scale factor influences the least with magnitude of 5, then the initial velocity larger magnitude of 8, and the accelerometer bias magnitude of 25. The influence of the initial platform misalignment angle is much more significant with a magnitude of 150. All the navigation bias in the second kind case is sinusoidal, which means theyre limited and negligible as time passes by. In the third case, such as the gyro scale factor error situation, , in Fig2.6, and the gyro bias error, results in Fig2.10, effects the most significant, the trajectory of the navigation disvergence accumulated as time goes. The position bias is a combination of sinusoidal signal and ramp signal. They also show that the longitudinal and distance errors resulted from gyro drifts are not convergent in time. It means the errors in the gyroscope do most harm to our navigation. And due to the significant influence of the gyro bias errors and the gyroscope scale factor error, results considering all the error sources disverge, and the navigation position of the motion will be away from the real motion after a enough long time, as shown in Fig2.10. The gyro bias error is the most significant effect factor of all errors. By the time of 10000s, it has reaches 1600m, and its nearly the quantity of the position bias considering all error sources. Through contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias. Impression of the Whole simulation experimentThrough contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias, and the the gyro bias or the drift error do most harm to our navigation. So it is a must for us to weaken or eliminate it anyway. In spite of all the disadvantages discussed above, the INS still shows us a relatively accurate results of single-axis navigation.Assignment 3: SINS simulation3.1 Task descriptionA missile equipped with SINS is initially at the position of 46o NL and 123 o EL, stationary on a launch pad. Three gyros, GX, GY, GZ, and three accelerometers, AX, AY, AZ, are installed along the axes Xb, Yb, Zb of its body frame respectively.Case 1: stationary testThe body frame of the missile initially coincides with the geographical frame, as shown in the figure, with its pitching axis Xb pointing to the east, rolling axis Yb to the north, and azimuth axis Zb upward. Then the body of the missile is made to rotate in 3 steps:(1) -22o around Xb(2) 78o around Yb(3) 16o around Zb Fig 3.1 Introduction to assignment 3After that, the body of the missile stops rotating. You are required to compute the final outputs of the three accelerometers on the missile, using quaternion and ignoring the device errors. It is known that the magnitude of gravity acceleration is g = 9.8m/s2.Case 2: flight testInitially, the missile is stationary on the launch pad, 400m above the sea level. Its rolling axis is vertical up,and its pitching axis is to the east. Then the missile is fired up. The outputs of the gyros and accelerometers are both pulse numbers. Each gyro pulse is an angular increment of 0.01arc-sec, and each accelerometer pulse is 1e-7g, with g =9.8m/s2. The gyro output frequency is 200Hz, and the accelerometers is 10Hz. The outputs of the gyros and accelerometers within 1315s are stored in a MATLAB data file named imu.mat, containing matrices gm of 263000 3 and am of 13150 3 respectively. The format of the data is as shown in the tables, with 10 rows of each matrix selected. Each row represents the outputs of the type of sensors at each sampling time.The Earth can be seen as an ideal sphere, with radius 6371.00km and spinning rate 7.292 10-5 rad/s, The errors of the sensors are ignored, so is the effect of height on the magnitude of gravity. The outputs of the gyros are to be integrated every 0.005s. The rotation of the geographical frame is to be updated every 0.1s, so are the velocities and positions of the missile.You are required to:(1) compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the missile.(2) compute the total horizontal distance traveled by the missile.(3) draw the latitude-versus-longitude trajectory of the missile, with horizontal longitude axis.(4) draw the curve of the height of the missile, with horizontal time axis. Fig 3.2 simplified navigation algorithm for SINS3.2Procedure code3.2.1 Sub function code：quaternion multiply code:function q1=quml(q1,q2);lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);q1=lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm*q2;endquaternion inversion code:function qni =qinv(q)q(1)=q(1);q(2)=-q(2);q(3)=-q(3);q(4)=-q(4);qni=q;end3.2.2case1 DCM algorithm：function ans11cz=cos(-22/180*pi) sin(-22/180*pi) 0 ; -sin(-22/180*pi) cos(-22/180*pi) 0; 0 0 1;%The third rotation DCM cx=1 0 0 ; 0 cos(78/180*pi) sin(78/180*pi); 0 -sin(78/180*pi) cos(78/180*pi);%The first rotation DCM cy=cos(-16/180*pi) 0 -sin(-16/180*pi); 0 1 0; sin(-16/180*pi) 0 cos(-16/180*pi);%The second rotation DCM A=cz*cy*cx*0;0;-9.8 end3.2.3case1 quaternion algorithm：function ans12g=0;0;0;-9.8;q1=cos(-11/180*pi);sin(-11/180*pi);0;0; %The first rotation quaternion q2=cos(39/180*pi);0;sin(39/180*pi);0; %The second rotation quaternionq3=cos(-8/180*pi);0;0;-sin(-8/180*pi); %The third rotation quaternion r=quml(q1,q2); %call the quaternion multiplication subfunctionq=quml(r,q3);P1=q(1) q(2) q(3) q(4); -q(2) q(1) q(4) -q(3); -q(3) -q(4) q(1) q(2); -q(4) q(3) -q(2) q(1);P2=q(1) -q(2) -q(3) -q(4); q(2) q(1) q(4) -q(3); q(3) -q(4) q(1) q(2); q(4) q(3) -q(2) q(1);P=P1*P2; gn=P*g;gn=gn(2:4)end3.2.4 case2 SINS quaternion algorithm code：function ans2 clc; clear; %parameters initializing：T=0.005; K=13150; R=6371000;%radius of earthwE=7.292*10(-5);%spinning rate of earth Q=zeros(4, 263001);%quaternion matrix initializing longitude=zeros(1,13151);latitude=zeros(1,13151);H=zeros(1,13151);%altitude matrixQ(:,1)=cos(45/180*pi);-sin(45/180*pi);0;0 ; % initial quaternion longitude(1)=123;%initial longitude latitude(1)=46;% initial latitude H(1)=400;%initial altitude length=0;g=9.8;vE = zeros(1,13151); %eastern velocity vN = zeros(1,13151); %northern velocity vH = zeros(1,13151); %upward velocity vE(1)=0; vN(1)=0; vH(1)=0; load imu.mat %data loading %main calculation sectionfor N=1:K q1=zeros(4,11); q1(:,1)=Q(:,N); for n=1:20 % Attitude iteration wx=0.01/(3600*180)*pi*gm(N-1)*10+n,1); % Angle increment wy=0.01/(3600*180)*pi*gm(N-1)*10+n,2); wz=0.01/(3600*180)*pi*gm(N-1)*10+n,3); w=wx,wy,wz; normw=norm(w); % Norm calculation W=0,-w(1),-w(2),-w(3); w(1),0,w(3),-w(2); w(2),-w(3),0,w(1); w(3),w(2),-w(1),0; I=eye(4); S=1/2-normw2/48; C=1-normw2/8; q1(:,n+1)=(C*I+S*W)*q1(:,n); Q(:,N+1)=q1(:,n+1); end WE=-vN(N)/R; % rotational angular velocity component of a geographic coordinate system WN=vE(N)/R+wE*cos(latitude(N)/180*pi); WH=vE(N)/R*tan(latitude(N)/180*pi)+wE*sin(latitude(N)/180*pi);attitude=WE,WN,WH*T; %correction of the quaternion by updating the rotation of geographic coordinate normattitude=norm(attitude); e=attitude/normattitude; QG=cos(normattitude/2);sin(normattitude/2)*e; Q(:,N+1)=quml(qinv(QG),Q(:,N+1); fx=1e-7*9.8*am(N,1); %specific force measured by accelerometer fy=1e-7*9.8*am(N,2); fz=1e-7*9.8*am(N,3); Fb=fx fy fz; F=quml(Q(:,N+1),quml(0;Fb,qinv(Q(:,N+1); %The specific force is decomposed into geographic coordinate system. FE(N)=F(2);FN(N)=F(3);FU(N)=F(4); %calculate the velocity of the vehicle:VED(N)=FE(N)+vE(N)*vN(N)/R*tan(latitude(N)/180*pi)-(vE(N)/R+2*wE*cos(latitude(N)/180*pi)*vH(N)+2*vN(N)*wE*sin(latitude(N)/180*pi); VND(N)=FN(N)-2*vE(N)*wE*sin(latitude(N)/180*pi)-vE(N)*vE(N)/R*tan(latitude(N)/180*pi)-vN(N)*vH(N)/R; VUD(N)=FU(N)+2*vE(N)*wE*cos(latitude(N)/180*pi)+(vE(N)2+vN(N)2)/R-g; %integration and get the relative velocity of vehicle: vE(N+1)=VED(N)*T+vE(N); vN(N+1)=VND(N)*T+vN(N); vH(N+1)=VUD(N)*T+vH(N); % in