输入转动角速度在体坐标系下的分量即可。复制粘贴到matlab即可运行
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', 2);
hold on;
axis([-1,1,-1,1,-1,1]);
quiver3(0, 0, 0, j(1), j(2), j(3), 'g', 'LineWidth', 2);
quiver3(0, 0, 0, ks(1), ks(2), ks(3), 'b', 'LineWidth', 2);
% 旋转后的坐标系初始状态
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);
% 龙格库塔法迭代
for ij = 1:N-1
% 计算k值
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.01)%暂留实现动画过程
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