機械臂末端參數(x y z rx ry rz)

對於六軸機械臂而言，在控制器端一般會有六個參數表徵當前機械臂末端的狀態(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而言，能夠經過歐拉角轉換成旋轉矩陣，使用opencv以及eigen程序實現以下：

1. opencv實現

/**
opencv實現歐拉角計算對應的旋轉矩陣
cv::Vec3d &theta 必須是弧度值 ： 
cv::Vec3d eular(0.2*M_PI, 0.3*M_PI, 0.4*M_PI);
**/
cv::Mat eulerAnglesToRotationMatrix(cv::Vec3d &theta)
{
	// 計算旋轉矩陣的X份量
	cv::Mat R_x = (cv::Mat_<double>(3, 3) <<
		1, 0, 0,
		0, cos(theta[0]), -sin(theta[0]),
		0, sin(theta[0]), cos(theta[0])
		);

	// 計算旋轉矩陣的Y份量
	cv::Mat R_y = (cv::Mat_<double>(3, 3) <<
		cos(theta[1]), 0, sin(theta[1]),
		0, 1, 0,
		-sin(theta[1]), 0, cos(theta[1])
		);

	// 計算旋轉矩陣的Z份量
	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);

	// 合併
	cv::Mat R = R_z * R_y * R_x;
	return R;
}

2. eigen實現

#include <Eigen/Core>
#include <Eigen/Geometry>


//使用Eigen實現歐拉角到旋轉矩陣的轉換

Eigen::Matrix3d rotation;
Eigen::Vector3d eular_angle(0.2*M_PI, 0.3*M_PI, 0.4*M_PI);

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(M_PI / 4, Eigen::Vector3d(0, 1, 0));     //沿 Z 軸旋轉 45 度

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();                // 雖然稱爲3d，實質上是4＊4的矩陣
T.rotate(rotation_matrix);                                     // 按照rotation_vector進行旋轉
T.pretranslate(Eigen::Vector3d(1, 3, 4));                     // 把平移向量設成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() << endl;

假設機械臂末端座標原點在基座標系下的座標值爲(134, 324, 45, 123, 45, 126)，能夠計算出t=[134, 324, 45], R的值以下：

cv::Vec3d eular(123*M_PI/180, 45*M_PI/180, 126*M_PI/180);

R = eulerAnglesToRotationMatrix(eular);

假設在機械臂末端座標系下有座標值( 12, 4 , 45)，那麼在基座標系下的座標值計算以下：

R*([12, 4 , 45].transpose()) + t