機械臂運動控制——三維空間剛體運動描述

一、旋轉矩陣

在機器人運動的過程當中，我們通常會設定一個慣性座標系（或者叫世界座標系），姑且認爲這個座標系是固定不動的。例如：,,是固定不動的世界座標系，,,​​是機器人座標系。存在一個向量，在世界座標系下的座標是​​，在移動機器人座標系下的座標是​​，通常情況下，我們通過傳感器已知移動機器人座標系統下的座標，來求在世界座標系下的座標

爲了求​​，我們必須知道機器人座標系,,​​相對與世界座標,,​​做了哪些變換。我們定義世界座標系經過變換矩陣之後得到機器人座標系（這可以通過計算里程和IMU的數據進行測量出來）（這也就說明了爲什麼在機器人剛剛啓動的時候odom和base_link座標系必須是重合的，不然沒有辦法計算旋轉矩陣），另外一般情況下，移動機器人運動是一個剛體運動，也就是說機器人的形狀和大小不會因爲座標系不同而改變，這種變換叫做歐式變換。一個歐式變換可以由旋轉和平移兩個部分組成。首先我們考慮旋轉問題，假設在世界座標系下的單位正交基，在移動機器人座標系下的單位正交基，那麼，根據向量的模可知：

                                                          

因此，，我們將記做旋轉矩陣，因此上面的表達式可以簡化爲。接下來是平移部分，假設平移部分是經過平移向量後得到，那麼可以得到。所以通過旋轉矩陣和平移向量，我們可以描述從世界座標系到移動機器人座標系的座標變換。但是這種表達方式存在一個問題，對於連續的位置變換，例如機器人座標系是隨着時間在不斷變換的，上面這種表達方式並不是一個線性的表達方式，假設我們經歷了兩次變換,和,且滿足：從到的變換，從到的變換，那麼從到的變換是.並不是我們希望的的形式(然後我們採用齊次座標的方式進行表達，詳細的部分參考李羣李代數).

 

二、歐拉角

旋轉本身就是一個很直觀的現象。歐拉角可以提供一種非常直觀的方式。他利用3個分離的轉角，把一次旋轉分解成3次繞不同的軸進行旋轉。例如先繞x軸旋轉，再繞y軸旋轉，最後繞z軸旋轉，這樣就得到一個xyz軸的旋轉。在歐拉角中一個常用的是“航偏-俯仰-翻滾”（yaw-pitch-roll）。可以簡單記憶rpy-xyz。其中roll-對應着繞x軸旋轉後的翻滾角。Pitch對應着繞y軸旋轉後的俯仰值，yawd對應着繞z軸旋轉後的航偏值。那麼旋轉部分就可以通過roll-pitch-yaw這三個量來描述。

在使用歐拉角這種表達方式的時候，會存在萬象鎖的問題。也就是一旦旋轉pitch爲90度，就會導致第一次旋轉和第三次轉換等價，丟失了一個表示維度。萬象鎖現象如下圖所示

 

三、四元數

旋轉矩陣用9個量來描述3自由度的旋轉，具有冗餘性；歐拉角雖然用3個量來描述3自由度的旋轉，但是具有萬向鎖的問題，因此我們選擇用四元數，（ROS當中描述轉向的都是採用的四元數）。一個四元數擁有一個實部和三個虛部組成。

三個虛部滿足以下關係

寫成矩陣的樣子就是，其中，從歐拉角到四元數的公式：

從四元數轉化到歐拉角公式

 

四、不同運動描述轉換的程序實現

C++（轉自https://blog.csdn.net/zhuoyueljl/article/details/70789472）

//歐拉角轉換到四元數
Eigen::Quaterniond euler2Quaternion(double roll, double pitch, double yaw)  
{  
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());  
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());  
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());  
    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;  
    return q;  
}

//四元數轉換到歐拉角
  Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)  
{  
    Eigen::Quaterniond q;  
    q.x() = x;  
    q.y() = y;  
    q.z() = z;  
    q.w() = w;   
    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
    return euler;  
}

//旋轉矩陣轉換到四元數
  Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)  
{  
    Eigen::Quaterniond q = Eigen::Quaterniond(R); 
    q.normalize();  
    return q;  
}  

//四元數轉換到旋轉矩陣
Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
{  
    Eigen::Quaterniond q;  
    q.x() = x;  
    q.y() = y;  
    q.z() = z;  
    q.w() = w;    
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();  
    return R;  
}  

//歐拉角轉換到旋轉矩陣
Eigen::Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
    Eigen::Matrix3d R = q.matrix();
    return R;
}

//旋轉矩陣轉換到歐拉角
Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)
{
    Eigen::Matrix3d m;
    m = R;
    Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);
    return euler;
}

 

參考

https://www.cnblogs.com/21207-iHome/p/6894128.html

https://blog.csdn.net/zhuoyueljl/article/details/70789472

中國大學MOOC———《機器人操作系統入門》