matlab 角度转四元数_matlab练习程序（求向量间的旋转矩阵与四元数）

问题是这样，如果我们知道两个向量v1和v2，计算从v1转到v2的旋转矩阵和四元数，由于旋转矩阵和四元数可以互转，所以我们先计算四元数。

我们可以认为v1绕着向量u旋转θ ​

角度到v2，u垂直于v1-v2平面。

四元数q可以表示为 cos(

所以我们求出u和θ/2即可，

u等于v1与v2的叉积，不要忘了单位化；

θ/2用向量夹角公式就能求。

clear all;

close all;

clc;

v1=[1 2 3];

v2=[4 5 6];

%转为单位向量

nv1 = v1/norm(v1);

nv2 = v2/norm(v2);

if norm(nv1+nv2)==0

q = [0 0 0 0];

else

u = cross(nv1,nv2);

u = u/norm(u);

theta = acos(sum(nv1.*nv2))/2;

q = [cos(theta) sin(theta)*u];

end

%由四元数构造旋转矩阵

R=[2*q(1).^2-1+2*q(2)^2 2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)-q(1)*q(3));

2*(q(2)*q(3)-q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)+q(1)*q(2));

2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)-q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];

s = nv1*R;

%显示结果

v2

s*norm(v2)

参考：