机械臂末端参数(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