姿态插值轨迹在球面上的绘制

clear,clc,close all

theta = pi/3;
x = 1; y = 0; z = 0;
q1 = [cos(theta/2), sin(theta/2)*x, sin(theta/2)*y, sin(theta/2)*z];

theta = pi/4;
x = 1; y = 2; z = 1;
q2 = [cos(theta/2), sin(theta/2)*x, sin(theta/2)*y, sin(theta/2)*z];

theta = pi/2;
x = 0; y = 0; z = 1;
q3 = [cos(theta/2), sin(theta/2)*x, sin(theta/2)*y, sin(theta/2)*z];

% 归一化四元数（确保它们是单位四元数）
q1 = q1 / norm(q1);
q2 = q2 / norm(q2);
q3 = q3 / norm(q3);

% 计算插值参数 t 的范围
t = linspace(0, 1, 100); % 插值参数从 0 到 1，共100个点
q_interp = zeros(length(t), 4);
q_interp2 = q_interp;

% 实现球面插值（Slerp）
for i = 1:length(t)
    q_interp1(i, :) = slerp(q1, q2, t(i));
    q_interp2(i, :) = slerp(q2, q3, t(i));
end

q_xyz1 = q_interp1(:,2:4) .* q_interp1(:,1);
q_xyz2 = q_interp2(:,2:4) .* q_interp2(:,1);

for i = 1:length(t)
    q_xyz1(i,:) = q_xyz1(i,:)  / norm(q_xyz1(i,:) );
    q_xyz2(i,:) = q_xyz2(i,:)  / norm(q_xyz2(i,:) );
end

% 绘制单位球面
figure;
[X, Y, Z] = sphere(50);
surf(X, Y, Z, 'FaceColor', 'none', 'EdgeColor', 'k');
hold on;

% 绘制插值轨迹
plot3(q_xyz1(:,1), q_xyz1(:,2), q_xyz1(:,3), 'c', 'LineWidth', 2);
plot3(q_xyz2(:,1), q_xyz2(:,2), q_xyz2(:,3), 'c', 'LineWidth', 2);

% 绘制三维坐标轴
plot3([0,1.5],[0,0],[0,0],'r', 'LineWidth', 2)
plot3([0,0],[0,1.5],[0,0],'g', 'LineWidth', 2)
plot3([0,0],[0,0],[0,1.5],'b', 'LineWidth', 2)

% 设置图形属性
xlabel('X');
ylabel('Y');
zlabel('Z');
title('Quaternion Slerp on Unit Sphere');
grid on;
axis equal;
view(3);

function q_interp = slerp(q1, q2, t)
    % Slerp (球面线性插值)
    % 确保q1和q2是单位四元数
    if norm(q1) ~= 1 || norm(q2) ~= 1
        error('q1 和 q2 必须是单位四元数');
    end
    
    % 计算夹角和插值系数
    cos_theta = dot(q1(1:3), q2(1:3)); % 实部不参与计算夹角，只考虑虚部
    if cos_theta < 0
        q2 = -q2; % 如果点乘结果为负，则反转q2的方向以保持角度在0到π之间
        cos_theta = -cos_theta;
    end
    theta_0 = acos(cos_theta); % 夹角θ0
    sin_theta_0 = sin(theta_0); % sinθ0
    
    % 插值计算
    if sin_theta_0 == 0 % 如果两个四元数相同或几乎相同，则直接返回一个插值点
        q_interp = (1 - t) * q1 + t * q2;
    else
        s0 = sin((1 - t) * theta_0) / sin_theta_0;
        s1 = sin(t * theta_0) / sin_theta_0;
        q_interp = s0 * q1 + s1 * q2;
    end
end