matlab转动可视化，基于四元数

实现转动可视化，基于四元数微分方程。

clear

close all

% 四元数解算角速度旋转。

q = [0; 0; 0; 1]; % 假设初始四元数为单位四元数。q4为标量。

% 时间设置

t_start = 0; % 起始时间

t_end = 30; % 结束时间

dt = 0.01; % 时间步长

n_steps = (t_end - t_start) / dt; % 总步数

% 初始化时间和四元数矩阵

t = linspace(t_start, t_end, n_steps);

q_matrix = zeros(4, n_steps);

q_matrix(:, 1) = q;

cla

% 画初始坐标系

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

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

k = [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, k(1), k(2), k(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, k(1), k(2), k(3), 'b--', 'LineWidth', 2);

R_temp=zeros(3,3);

% 龙格库塔法求解四元数微分方程

for ij = 2:n_steps

k1 = dt * quaternion_ode(q);

k2 = dt * quaternion_ode(q + 0.5 * k1);

k3 = dt * quaternion_ode(q + 0.5 * k2);

k4 = dt * quaternion_ode(q + k3);

q = q + (k1 + 2*k2 + 2*k3 + k4) / 6;

q_matrix(:, ij) = q;

q1=q(1);

q2=q(2);

q3=q(3);

q4=q(4);

%四元数转换为姿态矩阵.P146,(5.2-20)

R_temp(1,1)=q1^2-q2^2-q3^2+q4^2;

R_temp(2,2)=-q1^2+q2^2-q3^2+q4^2;

R_temp(3,3)=-q1^2-q2^2+q3^2+q4^2;

R_temp(1,2)=2*(q1*q2+q3*q4);

R_temp(1,3)=2*(q1*q3-q2*q4);

R_temp(2,1)=2*(q1*q2-q3*q4);

R_temp(2,3)=2*(q2*q3+q1*q4);

R_temp(3,1)=2*(q1*q3+q2*q4);

R_temp(3,2)=2*(q2*q3-q1*q4);

R_temp=R_temp';% 注意这是把坐标要投影到惯性系下（参考系下）

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

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

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

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

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

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

i_new = R_temp * i;

j_new = R_temp * j;

k_new = R_temp * k;

% 更改旋转后的新坐标系

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 dq = quaternion_ode(q)

% 定义角速度分量

omega_x = 5; % 可以根据需要修改

omega_y = 0; % 可以根据需要修改

omega_z = 0; % 可以根据需要修改

% 四元数微分方程矩阵

Omega = 0.5 * [0, omega_z, -omega_y, omega_x;

-omega_z, 0, omega_x, omega_y;

omega_y, -omega_x, 0, omega_z;

-omega_x, -omega_y, -omega_z, 0];

% 计算 dq/dt

dq = Omega * q;

end