求解AX=XB 方法
一、简介
一文浅谈旋转变换：旋转矩阵、旋转向量、欧拉角、四元数-CSDN博客
在机器人学、计算机视觉和几何学中，经常会遇到求解矩阵方程 AX=XB 的问题。这种方程通常出现在坐标系变换、手眼标定（Hand-Eye Calibration）等场景中。理解如何求解这个方程对于正确建立坐标系之间的关系至关重要。本文将详细讲解 AX=XB的求解方法，包括其几何意义、数学推导以及具体的求解步骤。
二、方程 AX=XB的几何意义
矩阵 A, B, X 的结构
通常，AA, BB, XX 是齐次变换矩阵（Homogeneous Transformation Matrices），形式如下：
三、旋转的表示方式
分别是 1 、旋转矩阵
2、欧拉角
3、旋转向量（Rotation Vector）
旋转矩阵
- 用 9 个量描述旋转的3个自由度，有冗余；
- 9 个量是有约束的：必须是正交矩阵，且行列式为 1
注意点：
如果我们按照 X->Y ->Z的顺序来选择的话，我们的旋转矩阵要写成：
旋转向量（Rotation Vector）
- 任意的旋转都可以用一个旋转轴和绕轴的旋转角来描述，简称“轴角”（Axis-Angle）；
- 旋转向量，是一个三维向量，其方向与旋转轴一致，长度等于旋转角；
刚体绕旋转轴旋转
r表示的是旋转轴的方向，r的长度表示刚体绕轴旋转的角度。
旋转向量转旋转矩阵：
import numpy as np
def rodrigues_rotation(r, theta):
# n旋转轴[3x1]
# theta为旋转角度
# 旋转是过原点的，n是旋转轴
r = np.array(r).reshape(3, 1)
rx, ry, rz = r[:, 0]
M = np.array([
[0, -rz, ry],
[rz, 0, -rx],
[-ry, rx, 0]
])
R = np.eye(4)
R[:3, :3] = np.cos(theta) * np.eye(3) + \
(1 - np.cos(theta)) * r @ r.T + \
np.sin(theta) * M
return R 
旋转矩阵转旋转向量
J = (R - R.T) / 2
t = [-J[1, 2], J[0, 2], -J[0, 1]]
t_norm = np.linalg.norm(t)
theta = np.math.asin(t_norm)
angle = theta / np.pi * 180
r = t / t_norm
v = r * theta缺陷：
①存在奇异性（当时？？？）
②不方便直观理解。
欧拉角
定义
绕三轴旋转的角度(α,β,γ)，分别对应(x,y,z)三个坐标轴。也可用 yaw-偏航、pitch-俯仰、roll-横滚 表示（旋转方向默认按右手螺旋方向，即从旋转轴正半轴向坐标原点看，逆时针方向为正，顺时针方向为负。）
分类
旋转顺序。一般有两类排序系列，一类是X-Z-X系列（经典欧拉角），一类是X-Z-Y系列（泰特布莱恩角）：
Proper Euler angles: X-Z-X, X-Y-X, Y-X-Y, Y-Z-Y, Z-Y-Z, Z-X-Z
Tait–Bryan angles: X-Z-Y, X-Y-Z, Y-X-Z, Y-Z-X, Z-Y-X, Z-X-Y（SLAM中常使用）
转换
外旋的旋转矩阵，是按照旋转次序，依次左乘各轴分量。内旋的旋转矩阵，是按照旋转次序，依次右乘各轴分量。
欧拉角与旋转矩阵
欧拉角->旋转矩阵
def eulerAnglesToRotationMatrix(theta) :
R_x = np.array([[1, 0, 0 ],
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
[0, math.sin(theta[0]), math.cos(theta[0]) ]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
[0, 1, 0 ],
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
return R旋转矩阵->欧拉角
def rotation_to_euler_radian(R):
x = np.math.atan2(R[2,1] , R[2,2])
y = np.math.atan2(-R[2,0], np.linalg.norm([R[2, 1], R[2, 2]]))
z = np.math.atan2(R[1,0], R[0,0])
return x, y, z
static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)
{
Eigen::Vector3d n = R.col(0);
Eigen::Vector3d o = R.col(1);
Eigen::Vector3d a = R.col(2);
Eigen::Vector3d ypr(3);
double y = atan2(n(1), n(0));
double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
ypr(0) = y;
ypr(1) = p;
ypr(2) = r;
return ypr / M_PI * 180.0;
} 
欧拉角与旋转向量
欧拉角转旋转向量
① 欧拉角 旋转矩阵
旋转向量
② 欧拉角 四元数
旋转向量，
//code
{
Eigen::AngleAxisd E2V(Eigen::AngleAxisd(gamma, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitY()));
std::cout << "Euler Angles to Axis-Angle: " << E2V.axis().norm() << std::endl << "angle=" << E2V.angle() << ", axis=" << E2V.axis().transpose() << std::endl << std::endl;
}
//print
Euler Angles to Axis-Angle: 1.000000000
angle=1.817567592, axis=-0.015311407 0.009690003 0.999835819旋转向量转欧拉角
① 旋转向量 旋转矩阵
欧拉角
//code
{
const auto V2E_zxy = V2R.eulerAngles(2, 0, 1);
const Eigen::Vector3d V2E(V2E_zxy(1), V2E_zxy(2), V2E_zxy(0));
std::cout << "Axis-Angle to Euler Angles: " << std::endl << V2E.transpose() << std::endl << std::endl;
}
//print
Axis-Angle to Euler Angles:
-0.002792527 0.028448866 1.817448093缺陷
四元数
定义
四元数（Quaternion）既是紧凑的，又没有奇异性，又无奇异性,且无论是求逆、串联等操作，相比矩阵更加高效,缺点是不够直观。
四元数 q 有一个实部和三个虚部，如下所示：
推导：
转换
缺陷
四、解方程-Tais法
分解旋转和平移部分
AX=XB
A, B, X 都是变换矩阵，那么A, B, X 表示为
注意：
我们需要先求解 旋转Rx ，然后求解平移Tx