陀螺章动效应

陀螺的章动特性，图中展示的是参考坐标系与本体坐标系的关系。

clear

cla

%转动速度在体坐标系的分量

% wx=15;

% wy=20;

% wz=10;

%

% w=[wx,wy,wz];

%初始角度

psi0 = 0;

phi0 = 0;

theta0 =0;

% 画初始坐标系

i = [1; 0; 0]; % X轴

j = [0; 1; 0]; % Y轴

ks = [0; 0; 1]; % Z轴

quiver3(0, 0, 0, i(1), i(2), i(3), 'r--', 'LineWidth', 0.1);

hold on;

xlim([-1,1]); ylim([-1,1]); zlim([-1,1]); view([1 1 0.3]); grid on;

% axis([-1,1,-1,1,-1,1]);

quiver3(0, 0, 0, j(1), j(2), j(3), 'g--', 'LineWidth', 0.1);

quiver3(0, 0, 0, ks(1), ks(2), ks(3), 'b--', 'LineWidth', 0.1);

% 旋转后的坐标系初始状态

a1=quiver3(0, 0, 0, i(1), i(2), i(3), 'r', 'LineWidth', 2);

a2=quiver3(0, 0, 0, j(1), j(2), j(3), 'g', 'LineWidth', 2);

a3=quiver3(0, 0, 0, ks(1), ks(2), ks(3), 'b', 'LineWidth', 2);

init=[psi0,phi0,theta0];

t0 = 0; % 初始时间

tf = 80; % 结束时间

dt = 0.001; % 时间步长

% 初始化变量

t = t0:dt:tf;

N = length(t);

angle=zeros(N,3);

angle(1,:)=init;

k=zeros(N,3);

w_s=100*pi/3;

Omega_n=(10-1)*w_s;

w0=300;

% 龙格库塔法迭代

for ij = 1:N-1

% 计算k值

wx=w0*cos(Omega_n*0.001*ij+0.01);

wy=w0*sin(Omega_n*0.001*ij+0.01);

wz=w_s;

w=[wx,wy,wz];

k1 = dt * att_mov(angle(ij,:),w);

k(ij,:)=k1;

k2 = dt * att_mov(angle(ij,:)+0.5*k1, w);

k3 = dt * att_mov(angle(ij,:)+0.5*k2, w);

k4 = dt * att_mov(angle(ij,:)+k3, w);

angle(ij+1, :) = angle(ij, :) + (1/6)*(k1 + 2*k2 + 2*k3 + k4);%

% 定义旋转矩阵:

theta3= angle(ij+1,1);

theta1= angle(ij+1,2);

theta2= angle(ij+1,3);

Rz = [cos(theta3), sin(theta3), 0;

-sin(theta3), cos(theta3), 0;

0, 0, 1];

Rx = [1, 0, 0;

0, cos(theta1), sin(theta1);

0, -sin(theta1), cos(theta1)];

Ry = [cos(theta2), 0, -sin(theta2);

0, 1, 0;

sin(theta2), 0, cos(theta2)];

% 组合旋转矩阵（注意旋转的顺序）

R = Rz'* Rx' *Ry' ;

% 旋转后的新坐标系基向量在新坐标系的投影

i = [1; 0; 0]; % X轴

j = [0; 1; 0]; % Y轴

ks = [0; 0; 1]; % Z轴

% 计算旋转后的新坐标系基向量在 原坐标系的投影（坐标）。

% （为了和原来的坐标系进行视觉对比！）

i_new = R * i;

j_new = R * j;

k_new = R * ks;

% 更改旋转后的新坐标系

a1.UData=i_new(1);

a1.VData=i_new(2);

a1.WData=i_new(3);

a2.UData=j_new(1);

a2.VData=j_new(2);

a2.WData=j_new(3);

a3.UData=k_new(1);

a3.VData=k_new(2);

a3.WData=k_new(3);

% 设置图形属性

grid on;

xlabel('X');

ylabel('Y');

zlabel('Z');

title('3-1-2 Rotation Sequence of Coordinate System');

legend({'Original X', 'Original Y', 'Original Z', 'Rotated X', 'Rotated Y', 'Rotated Z'}, 'Location', 'northeastoutside');

hold off;

pause(0.1)%暂留实现动画过程

end

function d_angle=att_mov(angle,w)

% 注意：注意角度单位

phi=angle(2);

theta=angle(3);

wx=w(1);

wy=w(2);

wz=w(3);

d_psi=(1/cos(phi))*(-wx*sin(theta)+wz*cos(theta));

d_phi=(1/cos(phi))*(wx*cos(theta)*cos(phi)+wz*sin(theta)*cos(phi));

d_theta=(1/cos(phi))*(wx*sin(theta)*sin(phi)+wy*cos(phi)-wz*cos(theta)*sin(phi));

d_angle=[d_psi,d_phi,d_theta];

end