本节假设陀螺是轴对称的,在 系中相对转轴的角速度为 ,而进动角速度,即 系相对于 的角速度)为 (延 方向),那么总角速度为 。显然此时 的大小不变,处于转轴和 轴所在的平面,且相对 系静止。所以角动量定理为
function top_sim
close all;
M = 1; R0 = 1; a= 2;
I0 = 2/5*M*R0^2*eye(3) + M*a^2*[1 0 0; 0 1 0; 0 0 0];
th0 = -pi/3;
q0 = [cos(th0/2); [1; 0; 0]*sin(th0/2)];
w0 = quat2mat(q0)*[0; 0; 97.86];
g = 9.8;
tau = @(q) cross(quat2mat(q)*[0;0;a], M*g*[0,0,-1]);
tmin = 0; tmax = 12.2; Nt = 200;
RelTol = 1e-6;
invI0 = inv(I0);
Y0 = [q0; w0];
f = @(t,Y)ode_fun(t, Y, I0, invI0, tau);
options = odeset('RelTol', RelTol);
[tt, Y] = ode45(f, [tmin,tmax], Y0, options);
t = linspace(tmin, tmax, Nt);
q = zeros(4, Nt); w = zeros(3, Nt);
for i = 1:4
q(i,:) = interp1(tt, Y(:,i), t, 'spline');
end
for i = 1:3
w(i,:) = interp1(tt, Y(:,i+4), t, 'spline');
end
verify(Y, tt, I0, tau);
figure;
P1 = zeros(3, Nt);
for it = 1:Nt
R = quat2mat(q(:,it));
clf; hold on; grid on; axis equal; view(33, 17);
axis([-1, 1, -1, 1, -0.2, 1]*2*a);
P1(:,it) = plot_top(R, R0, a);
title(['t = ' num2str(t(it), '%.3f')]);
plot3([0,w(1,it)],[0,w(2,it)],[0,w(3,it)], 'r');
Tau = tau(q(:,it))/10;
plot3([0,Tau(1)],[0,Tau(2)],[0,Tau(3)], 'c');
plot3(P1(1,1:it), P1(2,1:it), P1(3,1:it), 'm');
xlabel x; ylabel y; zlabel z;
drawnow();
saveas(gcf, [num2str(it) '.png']);
end
end
function dY = ode_fun(~, Y, I0, invI0, tau)
q = Y(1:4); w = Y(5:7);
q = q / norm(q);
R = quat2mat(q);
RT = R';
Tau = tau(q); Tau = Tau(:);
dw = R*invI0*RT*(Tau - cross(w, R*I0*RT*w));
dq = 0.5*quat_mul([0; w], q);
dY = [dq; dw];
end
function out = quat_mul(q1, q2)
s1 = q1(1); v1 = q1(2:4);
s2 = q2(1); v2 = q2(2:4);
out = [s1*s2 - dot(v1,v2); s1*v2 + s2*v1 + cross(v1, v2)];
end
function R = quat2mat(q)
s = q(1); vx = q(2); vy = q(3); vz = q(4);
R = [1 - 2*vy^2 - 2*vz^2, 2*vx*vy - 2*s*vz, 2*vx*vz + 2*s*vy;
2*vx*vy + 2*s*vz, 1 - 2*vx^2 - 2*vz^2, 2*vy*vz - 2*s*vx;
2*vx*vz - 2*s*vy, 2*vy*vz + 2*s*vx, 1 - 2*vx^2 - 2*vy^2];
end
function P1 = plot_top(R, R0, a)
[X,Y,Z] = sphere(10);
X = X*R0; Y = Y*R0; Z = Z*R0; Z = Z + a;
P = R * [X(:)'; Y(:)'; Z(:)'];
P1 = R * [0; 0; 2*a];
X = reshape(P(1,:), size(X));
Y = reshape(P(2,:), size(Y));
Z = reshape(P(3,:), size(Z));
surf(X, Y, Z, 'FaceColor', 'w', 'EdgeColor', 'b'); hold on;
plot3([0, P1(1)], [0, P1(2)], [0, P1(3)], 'LineWidth', 2, 'Color', 'k');
end
function verify(Y, t, I0, tau)
Nt = numel(t);
L = zeros(3, Nt); w = L;
Ek = zeros(1, Nt); W = Ek;
q = Y(:, 1:4);
for it = 1:Nt
w(:,it) = Y(it, 5:7);
R = quat2mat(q(it,:));
L(:, it) = R*I0*R'*w(:,it);
Ek(it) = 0.5*dot(w(:,it), L(:, it));
if (it > 1)
W(it) = W(it-1) + dot(tau(q(it,:)), w(:,it))*(t(it)-t(it-1));
else
W(it) = Ek(it);
end
end
L0 = zeros(3, Nt);
L0(:, 1) = L(:, 1);
for it = 2:Nt
L0(:, it) = L0(:, it-1) + tau(q(it,:)).' *(t(it)-t(it-1));
end
figure;
subplot(5, 1, 1); ylabel E_k; hold on;
plot(t, Ek, 'r'); plot(t, W, 'b--');
for i = 1:3
subplot(5, 1, 1+i);
plot(t, L(i, :), 'r'); hold on;
plot(t, L0(i, :), 'b--');
end
subplot(5, 1, 2); ylabel L_x;
subplot(5, 1, 3); ylabel L_y;
subplot(5, 1, 4); ylabel L_z;
subplot(5, 1, 5);
plot(t, w); hold on;
ylabel 'w_x, w_y, w_z';
legend({'w_x','w_y','w_z'});
xlabel t;
end