matlab練習程序（羅德里格斯變換）

給出空間中一個點，將該點繞向量(k1 k2 k3)旋轉一定角度theta，從而得到新點，這種操作能夠利用羅德里格斯變換來處理。

變換公式如下：

其中theta為旋轉度數。

K為由點所繞的單位向量構成的矩陣。

比如所繞向量為(k1 k2 k3)，則矩陣K表示如下：

公式中R即為進行這種變換的旋轉矩陣。

同樣，用四元數也能夠完成這種旋轉操作。

下面給出了用四元數和羅德里格斯變換對同一個點做旋轉的例子，互相驗證一下。

matlab代碼如下：

clear all;
close all;
clc;

p=rand(1,3);        %待旋轉點

quat = eul2quat([0.6,0.4,0.5]);     %利用歐拉角構造四元數
q_quat = quaternion(quat);
p_quat = quaternion([0 p]);
re_quat= q_quat*p_quat*q_quat';     %利用四元數旋轉
[w,a,b,c] = parts(re_quat);         %得到四元數中四個數

theta = acos(quat(1))*2;            %得到旋轉弧度和所環繞的向量
kx = quat(2)/sin(theta/2);
ky = quat(3)/sin(theta/2);
kz = quat(4)/sin(theta/2);

K=[0 -kz ky;
   kz 0 -kx;
   -ky kx 0];
R = eye(3) + sin(theta)*K + (1- cos(theta))*K*K;    %羅德里格斯公式
p1= R*p';                           %利用羅德里格斯公式計算的旋轉后的點

dr = HelperDrawRotation;
figure;
dr.draw3DOrientation(gca,[quat(2:4)],theta*180/pi);

hold on;
plot3(p(1),p(2),p(3),'bo')
plot3(p1(1),p1(2),p1(3),'r*')
plot3(a,b,c,'go');
axis equal;

其中的HelperDrawRotation是matlab幫助中的例子，我就不貼了，可以點擊這里下載。

變換效果如下：

利用四元數和羅德里格斯變換將藍圈變換到綠圈(紅星)上，結果是一致的。

注：這里matlab版本為2019a，2018a之前版本可能沒有quaternion這個函數。