概述

1. 以下公式默认为右手坐标系；
2. ZYZ通常可以避免死解情况，因此在六轴末端解算时常被用到；
3. 参考文章

1、XYZ旋转

XYZ旋转一般是绕固定轴旋转(外旋)，旋转矩阵的构成为：RzRyRx。
MATLAB代码：

function R = RotationZYX(xyz)
X = [1, 0, 0; ...
    0, cos(xyz(1)), -sin(xyz(1)); ...
    0, sin(xyz(1)), cos(xyz(1))];
Y = [cos(xyz(2)), 0, sin(xyz(2)); ...
    0, 1, 0; ...
    -sin(xyz(2)), 0, cos(xyz(2))];
Z = [cos(xyz(3)), -sin(xyz(3)), 0; ...
    sin(xyz(3)), cos(xyz(3)), 0; ...
    0, 0, 1];
R = Z * Y * X;
end

C++代码：

cv::Mat EulerToMat(cv::Point3f Euler)
{
	float alpha = Euler.x / 180 * CV_PI;
	float belta = Euler.y / 180 * CV_PI;
	float gamma = Euler.z / 180 * CV_PI;

	cv::Mat Rx = (cv::Mat_<float>(3, 3) << 
			1, 0, 0, 
			0, cos(alpha), -sin(alpha), 
			0, sin(alpha), cos(alpha));

	cv::Mat Ry = (cv::Mat_<float>(3, 3) <<
			cos(belta), 0, sin(belta),
			0, 1, 0,
			-sin(belta), 0, cos(belta)
			);

	cv::Mat Rz = (cv::Mat_<float>(3, 3) <<
			cos(gamma), -sin(gamma), 0,
			sin(gamma), cos(gamma), 0,
			0, 0, 1
			);

	return Rz * Ry * Rx;
}

2、ZYZ旋转

XYZ旋转一般是绕自身轴旋转(内旋)，旋转矩阵的构成为：RxRyRz。

MATLAB代码：

syms a b c
Ra=[cos(a) -sin(a) 0; sin(a) cos(a) 0; 0 0 1];
Rb=[cos(b) 0 sin(b); 0 1 0;-sin(b) 0 cos(b)];
Rc=[cos(c) -sin(c) 0;sin(c) cos(c) 0;0 0 1];
Rzyz=Ra*Rb*Rc

% 定义姿态旋转角度
angle1 = deg2rad(-8.668);  % 绕z轴旋转角度
angle2 = deg2rad(146.282);   % 绕y轴旋转角度
angle3 = deg2rad(175.557);  % 绕新z轴旋转角度

r11 = cos(angle1) * cos(angle2) * cos(angle3) - sin(angle1) * sin(angle3); 
r12 = -cos(angle1) * cos(angle2) * sin(angle3) - sin(angle1) * cos(angle3); 
r13 = cos(angle1) * sin(angle2);
r21 = sin(angle1) * cos(angle2) * cos(angle3) + cos(angle1) * sin(angle3); 
r22 = -sin(angle1) * cos(angle2) * sin(angle3) + cos(angle1) * cos(angle3); 
r23 = sin(angle1) * sin(angle2);
r31 = -sin(angle2) * cos(angle3); 
r32 = sin(angle2) * sin(angle3); 
r33 = cos(angle2);

% 姿态旋转矩阵
ZYZ_matrix = [r11 r12 r13; r21 r22 r23; r31 r32 r33];

C++代码：

float angle1 = config.ReadFloat("Parmeter", "angleA", 0.0);
float angle2 = config.ReadFloat("Parmeter", "angleE", 0.0);
float angle3 = config.ReadFloat("Parmeter", "angleR", 0.0);
angle1 = angle1 * CV_PI / 180;
angle2 = angle2 * CV_PI / 180;
angle3 = angle3 * CV_PI / 180;

float r11 = cos(angle1) * cos(angle2) * cos(angle3) - sin(angle1) * sin(angle3);
float r12 = -cos(angle1) * cos(angle2) * sin(angle3) - sin(angle1) * cos(angle3);
float r13 = cos(angle1) * sin(angle2);
float r21 = sin(angle1) * cos(angle2) * cos(angle3) + cos(angle1) * sin(angle3);
float r22 = -sin(angle1) * cos(angle2) * sin(angle3) + cos(angle1) * cos(angle3);
float r23 = sin(angle1) * sin(angle2);
float r31 = -sin(angle2) * cos(angle3);
float r32 = sin(angle2) * sin(angle3);
float r33 = cos(angle2);
cv::Mat ZYZ_matrix = (cv::Mat_<float>(3, 3) << r11, r12, r13, r21, r22, r23, r31, r32, r33);