哈工大-导航原理作业

Assignments of Inertial Navigation 《惯性导航》作业

Autumn 2017

Assignment 1: Coordinate transformation for 3D animation

figure 1.1 Interface for rotating animation control of missile

Attached is a group of MATLAB programs for 3D animation control of a missile. Initially, the body frame of the missile coincides with the local geographical frame, with its pitching axis X pointing to the east, rolling axis Y to the north, and heading axis Z to the sky, as shown in figure 1.1 which is the controlling in-terface produced by running the program main.m.

The three coordinates of each vertex of the 3D model occupies a row in the matrix VTX, The color property and surface information of the model are defined by matrices VTXcolor and faceM respectively. These matrices are loaded from the data file missiledata.mat.

Input an angle (in degree) in the "Rotation Angle" box, and click one of the rotation buttons ("Heading Ro-tation", "Pitching Rotation", or "Rolling Rotation"), then it is expected that the missile will rotate from its current attitude for the input angle around the chosen axis of its current body.

The animation is to be achieved by the program redraw.m which redraws the missile every once it rotates for one degree, using patch command. During each step of the animation, the current rotation angle of the missile relative to its pre-animation attitude has been generated and stored in one of the variables head, pitch and roll, with only one of them being nonzero. Before each redrawing, you need to re-calculate the coordinates of the missile in VTX resolved in the geographical frame, instead of keeping them the same as their values at the very beginning (VTX0). That is, in the program redraw.m, you need to replace the command line VTX=VTX0with your own codes for re-calculating VTX. Elsewhere, additional codes also might be required to make it work.

Please rewrite the program redraw.m, or others if necessary, so that successive rotating animations can be achieved, and explain the rationale behind your rewriting.

一、 任务分析

本作业需要实现的功能为实现火箭的连续旋转。老师已经为我们写了main 程序、restore 程序和火箭模型，我们需要补充redraw 程序，加入坐标变换矩阵，通过火箭模型各点的坐标变换实现火箭的旋转。

二、 坐标变换矩阵

设OEN ξ为定坐标系，有一转动坐标系000OX Y Z ，在起始时刻两坐标系重合，经过绕相应轴旋转之后，转到它的新位置OXYZ 。称绕三个轴的转动角度ψ、θ、?为欧拉角。按三次转动顺序列写方向余弦矩阵，以欧拉角的形式利用矩阵进行坐标变换，三个不同自由度上的角度转动分别对应一个变换矩阵，最终的坐标为初始坐标按顺序与变换矩阵相乘得到的结果。

可写出三个坐标变换矩阵分别为

可以得到坐标系OXYZ 和坐标系OEN ξ的变换关系为

cos 0sin 10

0cos sin 00100cos sin sin cos 0sin 0cos 0sin cos 0

1X E Y N Z ?

?ψ

ψθθψψ

??θθζ-????????????????????=-?????

???????????????-??????????

再进行整理得

三、运行结果

Heading rotation 45°：

Patching rotation -45°：

Rolling rotation -45°：

四、程序代码

main.m:

clear;

fg=figure('position',[200 20 1100 600], ...

'color',[0.945 0.619 0.761]);

set(gca, 'position',[0.13 0.02 0.775 0.97], ...

'xlim', [-2.6 2.6], ...

'ylim', [-2.6 2.6], ...

'DataAspectRatio',[1 1 1], ...

'DataAspectRatioMode','Manual', ...

'CameraViewAngle',6, ...

'CameraViewAngleMode','Manual'); axis off

hold on

uicontrol('style','text','string','Rotation Angle:','unit','normal','pos',[0.01 0.9 0.09

0.04]);

rotah=uicontrol('style','edit','unit','normal', 'pos',[0.11 0.9 0.05 0.04]);

uicontrol('style','pushbutton','string','Heading Rotation', 'unit','normal','pos',[0.88 0.9 0.11 0.05],'call','rotsq=1; redraw;');

uicontrol('style','pushbutton','string','Pitching Rotation','unit','normal','pos',[0.88 0.8 0.11 0.05],'call','rotsq=2; redraw;');

uicontrol('style','pushbutton','string','Rolling Rotation', 'unit','normal','pos',[0.88 0.7 0.11 0.05],'call','rotsq=3; redraw;');

uicontrol('style','pushbutton','string','Reset', 'unit','normal', 'pos',[0.88 0.6 0.11 0.05],'call','restore;');

xx=[-2.2 0 0

2.2 0 0];

yy=[ 0 -2.2 0

0 2.2 0];

zz=[ 0 0 -2.2

0 0 2.2];

plot3(xx,yy,zz,'b'); % drawing axes

text(2.4, 0, 0, 'East', 'color','green'); % marking axes

text( 0, 2.4, 0, 'North','color','green');

text( 0, 0, 2.4, 'Sky', 'color','green');

view(3);

lhd =camlight(315,20);

lhd1=light('position',[90 90 -40]);

load missiledata; % containing VTX, faceM, VTXcolor

VTX0=VTX; % VTX0 store the initial coordinates of the missile model

% VTX will change as the missile rotates

Cb0_f_geo=eye(3); % DCM from geographical frame to body frame at the start of each rotation.

phd=patch('Vertices',VTX, ...

'Faces', faceM, ...

'FaceVertexCData', VTXcolor, ...

'FaceColor', 'flat', ...

'EdgeColor', 'flat', ...

'FaceLighting', 'Gouraud', ...

'EdgeLighting', 'Gouraud');

redraw.m:

rotangle=str2num( get(rotah,'string') );

head=0; pitch=0; roll=0;

astep=sign(rotangle);%判断角度正负

for i=0:astep:rotangle %实现连续转动效果

if rotsq==1, head =i; % if around heading axis

elseif rotsq==2, pitch=i; % if around pitching axis

elseif rotsq==3, roll =i; % if around rolling axis

end

%将角度转换为弧度

head = head/180*pi;

pitch = pitch/180*pi;

roll = roll/180*pi;

%矩阵变换

Cb0_f_geoH = [cos(head), -sin(head), 0; sin(head), cos(head), 0; 0, 0, 1];

Cb0_f_geoP = [1, 0, 0; 0, cos(pitch), -sin(pitch); 0, sin(pitch), cos(pitch)];

Cb0_f_geoR = [cos(roll), 0, sin(roll); 0, 1, 0; -sin(roll), 0, cos(roll)];

R = Cb0_f_geo * Cb0_f_geoH*Cb0_f_geoP*Cb0_f_geoR;

VTX=(VTX0 * R');%计算出旋转后的坐标

delete(phd);

phd=patch('Vertices',VTX, 'Faces', faceM, 'FaceVertexCData', VTXcolor, 'FaceColor', 'flat','Edgecolor','flat', ...

'FaceLighting','Gouraud','Edgelighting','Gouraud');%重新绘制火箭模型pause(0.02);

end

Cb0_f_geo = R;%记录上一次旋转矩阵以便进行下一次转动