求解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

$_{Rxyz}=_{Rz}*_{Ry}*_{Rx}$

注意点：

如果我们按照 X->Y ->Z的顺序来选择的话，我们的旋转矩阵要写成：

$_{Rxyz}=_{Rz}*_{Ry}*_{Rx}$

旋转向量（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) * Mreturn 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

缺陷：

①存在奇异性（当 $\theta =0^{\circ} \ or \ \theta =180^{\circ}$ 时？？？）

②不方便直观理解。

欧拉角

定义

绕三轴旋转的角度(α,β,γ)，分别对应(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, zstatic 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;}

欧拉角与旋转向量

欧拉角转旋转向量

① 欧拉角 $\rightarrow$ 旋转矩阵 $\rightarrow$ 旋转向量

② 欧拉角 $\rightarrow$ 四元数 $\rightarrow$ 旋转向量，

//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

旋转向量转欧拉角

① 旋转向量 $\rightarrow$ 旋转矩阵 $\rightarrow$ 欧拉角

//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