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)^22*(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)
參考: