旋轉矩陣和歐拉角相互轉換及代碼實現(八十九)

一、相關概念介紹

1.1. 旋轉矩陣與旋轉向量

旋轉矩陣（Rotation Matrix）

旋轉向量（Rotation Vector）

旋轉向量到旋轉矩陣的轉換是通過羅德裡格斯公式（Rodrigues's Formula），轉換公式如下：

其中，  為旋轉軸單位向量，  為旋轉角度，  為向量到反對稱矩陣的運算符。

旋轉矩陣到旋轉向量的轉換過程如下：

可得轉角  ：

對於轉角  ，旋轉軸上的向量在旋轉後不發生改變：

因此，轉軸  是矩陣  特徵值 1 對應的特徵向量。

#include <vector>#include <Eigen/Core>#include <Eigen/Geometry>#include <opencv2/opencv.hpp>
// 1. Eigen 實現

// 旋轉向量轉旋轉矩陣
Eigen::Vector3d rvec (r_x, r_y, r_z);
double n_norm = rvec.norm();
Eigen::AngleAxisd rotation_vector (n_norm, rvec/n_norm);
Eigen::Matrix3d rotm;
rotm = rotation_vector.toRotationMatrix();

// 旋轉矩陣轉旋轉向量
Eigen::Matrix3d rotation_matrix;
rotation_matrix << x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

// 2. OpenCV 實現

// 旋轉向量轉旋轉矩陣
cv::Mat rvec = (cv::Mat_<double>(3,1) << r_x, r_y, r_z);
cv::Mat rotm;
cv::Rodrigues(rvec, rotm);

// 旋轉矩陣轉旋轉向量
cv::Mat rvec;
cv::Mat rotm = (cv::Mat_<double>(3,3) << x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22);
cv::Rodrigues(rotm, rvec);

1.2. 歐拉角

旋轉矩陣和旋轉向量描述旋轉時非常不直觀，而歐拉角提供了一種比較直觀的表示法：將旋轉分解成3次繞不同軸的旋轉。由於分解方式有多種，歐拉角也存在不同的定義方法，大致可分為兩類：

比較常用的一種歐拉角為 RPY（「偏航 - 俯仰 - 翻轉」/ Yaw - Pitch - Roll），等價於 ZYX 軸的旋轉：

繞物體的 Z 軸旋轉，得到偏航角 Yaw；

繞旋轉後的 Y 軸旋轉，得到俯仰角 Pitch；

繞旋轉後的 X 軸旋轉，得到翻轉角 Roll。

歐拉角雖然比較直觀，但存在萬向節死鎖（Gimbal Lock）問題：在俯仰角為正負90° 時，第一次旋轉和第三次旋轉將使用一個軸，使得系統丟失一個自由度，會發生奇異。因此，歐拉角不適於插值和迭代。

1.3. 四元數

四元數（Quaternion）既是緊湊的，又沒有奇異性，缺點是不夠直觀，運算比較複雜。

四元數  有一個實部和三個虛部，如下所示：

三個虛部  滿足以下關係式：

（1）四元數與旋轉向量

（2）四元數與旋轉矩陣

或者：

二、歐拉角轉換成旋轉矩陣

對於六軸機械臂而言，在控制器端通常會有六個參數表徵當前機械臂末端的狀態(x, y, z, rx, ry, rz)，其中(x, y, z)表示以機械臂的基座為原點(0, 0, 0)的OXYZ坐標系下機械臂工具的坐標值，其中地面為xoy面，垂直方向為z軸，而(rx, ry, rz)表示末端工具分別圍繞基座的x、y以及z軸旋轉的角度，即歐拉角(eular)。

此時我們定義機械臂末端存在另一個坐標系O'X'Y『Z'，可以知道從OXYZ坐標系經過Rt的坐標變換會得到O'X'YZ'。
其中t=[x, y, z], 為一個3*1的列向量。
對於旋轉矩陣R而言，可以通過歐拉角轉換成旋轉矩陣，

Euler 角 ψ, θ, 和 φ

橫滾角 ψ

俯仰角 θ

航向角 φ

旋轉角 R

根據 Euler 角 ψ, θ, 和 φ 計算旋轉角 R

使用opencv以及eigen程序實現如下：

2.1. opencv實現

Mat eulerAnglesToRotationMatrix(Vec3d &theta){            Mat R_x = (Mat_<double>(3, 3) <<    1, 0, 0,    0, cos(theta[0]), -sin(theta[0]),    0, sin(theta[0]), cos(theta[0])    );
        Mat R_y = (Mat_<double>(3, 3) <<    cos(theta[1]), 0, sin(theta[1]),    0, 1, 0,    -sin(theta[1]), 0, cos(theta[1])    );
        cv::Mat R_z = (cv::Mat_<double>(3, 3) <<    cos(theta[2]), -sin(theta[2]), 0,    sin(theta[2]), cos(theta[2]), 0,    0, 0, 1);
    Mat R = R_z * R_y * R_x;    return R;}
2.2. eigen實現
  cout << "R = " << endl << " " << Eigen::Vector3d::UnitZ() << endl << endl;  cout << "R = " << endl << " " << Eigen::Vector3d::UnitY() << endl << endl;  cout << "R = " << endl << " " << Eigen::Vector3d::UnitX() << endl << endl;
    Matrix3d rotation;  Vector3d eular_angle(123 * PI / 180, 45 * PI / 180, 126 * PI / 180);
  rotation = Eigen::AngleAxisd(eular_angle[2], Eigen::Vector3d::UnitZ())  * Eigen::AngleAxisd(eular_angle[1], Eigen::Vector3d::UnitY())  * Eigen::AngleAxisd(eular_angle[0], Eigen::Vector3d::UnitX());  cout << rotation << endl;
通常我們會將R和t組合成一個齊次坐標，形式如下：
r11    r12    r13    t1
r21    r22    r23    t2
r31    r32    r33    t3
0        0        0      1
使用eigen實現如下：
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();    Eigen::AngleAxisd rotation_vector(PI / 4, Eigen::Vector3d(0, 1, 0));       Eigen::Isometry3d T = Eigen::Isometry3d::Identity();                  T.rotate(rotation_matrix);                                       T.pretranslate(Eigen::Vector3d(1, 3, 4));                       cout << "Transform matrix = " << T.matrix() << endl;
假設機械臂末端坐標原點在基坐標系下的坐標值為(134, 324, 45, 123, 45, 126)，可以計算出t=[134, 324, 45], R的值如下：
Mat R; Vec3d eular(123 *  PI/ 180, 45 * PI / 180, 126 * PI / 180);  R = eulerAnglesToRotationMatrix(eular);
假設在機械臂末端坐標系下有坐標值( 12, 4 , 45)，那麼在基坐標系下的坐標值計算如下：
R*([12, 4 , 45].transpose()) + t

三、從旋轉矩陣計算歐拉角

旋轉矩陣和歐拉角之間的正向轉換關係比較好推理，而逆向變換就顯得不是那麼容易了。這篇博客介紹由旋轉矩陣計算歐拉角的方法，參考了一篇Paper：Computing Euler angles from a rotation matrix。
3.1MATLAB實現
function eulerAngles = rotationMatrix2eulerAngles(R)
% eulerAngles = rotationMatrix2eulerAngles(R)%% This function returns the rotation angles in degrees about the x, y and z axis for a% given rotation matrix% % Copyright : This code is written by david zhao from SCUT,1257650237@qq,com. The code%              may be used, modified and distributed for research purposes with%              acknowledgement of the author and inclusion this copyright information.%% Disclaimer : This code is provided as is without any warrantly.
if abs(R(3,1)) ~= 1    theta1 = -asin(R(3,1));    theta2 = pi - theta1;    psi1 = atan2(R(3,2)/cos(theta1), R(3,3)/cos(theta1));    psi2 = atan2(R(3,2)/cos(theta2), R(3,3)/cos(theta2));    pfi1 = atan2(R(2,1)/cos(theta1), R(1,1)/cos(theta1));    pfi2 = atan2(R(2,1)/cos(theta2), R(1,1)/cos(theta2));    theta = theta1; % could be any one of the two    psi = psi1;    pfi = pfi1;else    phi = 0;    delta = atan2(R(1,2), R(1,3));    if R(3,1) == -1        theta = pi/2;        psi = phi + delta;    else        theta = -pi/2;        psi = -phi + delta;    endend
%psi is along x-axis.theta is along y-axis...pfi is along z%axis% eulerAngles = [psi theta pfi]; %for rad;eulerAngles = [psi*180/pi theta*180/pi pfi*180/pi]; %for degree;
測試：
R =
    -0.4156    0.0920    0.9048
    0.5720    0.7999    0.1814
   -0.7071    0.5930   -0.3851
eulerAngles =
  122.9994   44.9995  126.0012
R =
   -0.4156    0.0920    0.9049
    0.5721    0.7999    0.1814
   -0.7071    0.5930   -0.3851
eulerAngles =
  122.9999   45.0000  125.9999
3.2 C++實現
需要math.h，矩陣R的類型，用的Eigen庫，如果你不用這個直接換成二維數組vector<vector<float>>就可以，完全不影響。
std::vector<float> computeEularAngles(Eigen::Matrix4f& R, bool israd){  std::vector<float> result(3, 0);  const float pi = 3.14159265397932384626433;  float theta = 0, psi = 0, pfi = 0;  if (abs(R(2, 0)) < 1 - FLT_MIN || abs(R(2, 0)) > 1 + FLT_MIN){     float theta1 = -asin(R(2, 0));    float theta2 = pi - theta1;    float psi1 = atan2(R(2,1)/cos(theta1), R(2,2)/cos(theta1));    float psi2 = atan2(R(2,0)/cos(theta2), R(2,2)/cos(theta2));    float pfi1 = atan2(R(1,0)/cos(theta1), R(0,0)/cos(theta1));    float pfi2 = atan2(R(1,0)/cos(theta2), R(0,0)/cos(theta2));    theta = theta1;    psi = psi1;    pfi = pfi1;  } else{    float phi = 0;    float delta = atan2(R(0,1), R(0,2));    if (R(2, 0) > -1 - FLT_MIN && R(2, 0) < -1 + FLT_MIN){       theta = pi / 2;      psi = phi + delta;    } else{      theta = -pi / 2;      psi = -phi + delta;    }  }
    if (israd){     result[0] = psi;    result[1] = theta;    result[2] = pfi;  } else{    result[0] = psi * 180 / pi;    result[1] = theta * 180 / pi;    result[2] = pfi * 180 / pi;  }  return result;}
Matrix4f R(4,4);R << -0.4156, 0.0920, 0.9048, 1,0.5720, 0.7999, 0.1814, 1,-0.7071, 0.5930, -0.3851, 1,0, 0, 0, 1;cout << "R=" << endl << R << endl;bool RIGHT =false;computeEularAngles(R, RIGHT);    
測試：
參考文獻
【1】https://zhuanlan.zhihu.com/p/93563218
【2】blog.csdn.net/yuzhongch
【3】http://nghiaho.com/?page_id=846
【4】http://www.soi.city.ac.uk/~sbbh653/publications/euler.pdf
【5】
https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2012/07/euler-angles.pdf
推薦：機器人實時糾偏系統（一）
在C++中調用Matlab函數（二十八）
機器人手眼標定MATLAB及C++實現
機器人位姿運算及Eigen的使用（三十）
OpenCV與Eigen矩陣運算（三十一）
VS中數據讀寫及OpenCV擬合（三十二）
VS2013配置OpenGL庫（三十三）
曲線擬合/插值（三十四）
曲線擬合繪製濾波及機器人平移（三十五）
DX200操作要領—示教1（三十六）
直接打開與平移變換（三十七）
PAM與鏡像平移變換（三十八）
修改與編輯程序（三十九）
YRC1000 宏程序命令（四十）
程序編輯與試運行（四十一）
程序編輯與再現（四十二）
再現（四十三）
程序管理（四十四）
便捷功能（四十五）
便捷功能（四十六）
橢圓擬合（四十七）
RANSAC直線擬合（四十八）
讀寫CSV文件類（四十九）
RANSAC直線擬合（五十）
法向量、旋轉矩陣計算（五十一）
機器人手眼標定與變量設置調試（五十二）
20200715調試記錄（五十三）
20200717調試記錄（五十四）
C++~RANSAC擬合圓/隨機數產生（五十五）
安川機器人外部軸協調（五十六）
安川機器人Motoplus編程（五十七）
安川機器人Motoplus編程（五十八）
安川機器人Motoplus編程（五十九）
安川機器人Motoplus編程（六十）
安川機器人Motoplus編程（六十一）
機器人調試（六十二）
機器人調試（六十三）
機器人調試（六十四）
機器人調試（六十五）
機器人調試（六十六）
機器人調試（六十七）
高斯卷積的可分離性（六十八）
機器人焊縫識別及軌跡規劃研究（六十九）
安川機器人同步設置與檢驗（七十）
OpenCV+Eigen上位機程序移植（七十一）
機器人調試（七十二）
雷射焊縫跟蹤技術（七十三）
工業機器人編程技術及趨勢（七十四）
20201130記錄（七十五）
結構相似性(SSIM)原理及其實現（七十六）
機器人調試（七十七）
20201209調試記錄（七十八）
機器人調試（七十九）
機器人調試（八十）
機器人調試（八十一）
KUKA 機器人系統（八十二）
KUKA機器人運動控制（八十三）
KUKA機器人運行（八十四）
KUKA機器人運行（八十五）
B樣條曲線擬合原理及代碼實現（八十六）
卡爾曼濾波及其代碼實現（八十七）
執行KUKA機器人程序（八十八）