通过不同坐标系下的同一向量，求解旋转矩阵

问题：
已知坐标系A经过两次旋转（先绕x轴旋转，再绕y轴旋转）得到坐标系B，且已知在坐标系A中的一个向量 a，在坐标系B中对应的向量为 b，那么如何求出从A到B的旋转矩阵？
这个问题在slam中还是算常见的，用于imu通过重力方向估计姿态，并且假设yaw角为0。

---

一、问题分析

• 坐标系A → 坐标系B的变换是通过两次旋转实现的：

  1. 绕坐标系A的x轴旋转角度为 α
  2. 绕坐标系A的y轴旋转角度为 β

• 坐标系的变换可以表示为一个旋转矩阵 $R=R_y(\beta )\cdot R_x(\alpha )$

• 已知向量 a（在A中）和对应的 b（在B中），即：
  $$\mathbf{a}=R\cdot \mathbf{b}$$
  或
  $$\mathbf{B}=R\cdot \mathbf{A}$$

• 我们的目标是：从a和b出发，解析地求出旋转矩阵 R

---

二、旋转矩阵定义

标准旋转矩阵如下：

• 绕x轴旋转α：
  $$R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right]$$

• 绕y轴旋转β：
  $$R_y(\beta )=\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right]$$

所以总旋转矩阵为：
$$R=R_y(\beta )R_x(\alpha )$$

---

三、已知向量求旋转矩阵

给定：

• 向量 $\mathbf{a}=[a_x,a_y,a_z{]}^T$
• 向量 $\mathbf{b}=[b_x,b_y,b_z{]}^T$
• 且满足 $\mathbf{a}=R\mathbf{b}$

我们要求解的是：
$$R=R_y(\beta )R_x(\alpha )$$

这个是一个旋转估计问题，但只给一个向量对 $(\mathbf{a},\mathbf{b})$ 是不足以唯一确定一个三维旋转的（需要至少两个不共线的向量对）。不过我们有先验知识：旋转是先绕x轴，再绕y轴完成的。

我们可以利用这个结构来求解。

---

四、解析解推导

1. 令：

$$R=R_y(\beta )R_x(\alpha )$$

展开后得到：
$$R=\left[\begin{array}{ccc}\cos \beta & \sin \beta \sin \alpha & \sin \beta \cos \alpha \\ 0& \cos \alpha & -\sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{array}\right]$$

2. 由 $\mathbf{a}=R\mathbf{b}$，即：

$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{ccc}\cos \beta & \sin \beta \sin \alpha & \sin \beta \cos \alpha \\ 0& \cos \alpha & -\sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{array}\right]\cdot \left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]$$

写成三个方程：

$$\{\begin{array}{l}{a}_x=b_x\cos \beta +b_y\sin \beta \sin \alpha +b_z\sin \beta \cos \alpha \\ a_y=b_y\cos \alpha -b_z\sin \alpha \\ a_z=-b_x\sin \beta +b_y\cos \beta \sin \alpha +b_z\cos \beta \cos \alpha \end{array}$$

---

五、求解思路

我们要求的是 $\alpha$ 和 $\beta$，可以利用第2个方程直接解出 $\alpha$，再代入其他方程求 $\beta$。

步骤1：从 $a_y=b_y\cos \alpha -b_z\sin \alpha$ 解出 $\alpha$

这是一个标准的正弦余弦线性组合：

$$a_y=b_y\cos \alpha -b_z\sin \alpha$$

这可以写成：
$$a_y=\sqrt{b_y^2+b_z^2}\cdot \cos (\alpha +\theta )$$
其中 $\tan \theta =\frac{b_z}{b_y}$

当$a_y=0$时（这与重力方向一致的），更简单的方式是使用反正切函数：

令：
$$\tan \alpha =\frac{b_y}{b_z}\Rightarrow \alpha =\arctan 2(b_y,b_z)$$

注意：这个只在 $a_y=0$ 时成立（因为左边是 $a_y$，不是0）。如果 $a_y\ne 0$，那就不能直接解出 $\alpha$。因此，这个方法只适用于 $a_y=0$ 的情况。

---

步骤2：代入 $\alpha$ 到第一个和第三个方程，解出 $\beta$

将 $\alpha$ 代入到：
$$a_x=b_x\cos \beta +b_y\sin \beta \sin \alpha +b_z\sin \beta \cos \alpha$$

整理得：
$$a_x=b_x\cos \beta +\sin \beta (b_y\sin \alpha +b_z\cos \alpha )$$

令：
$$C=b_y\sin \alpha +b_z\cos \alpha$$

则：
$$a_x=b_x\cos \beta +C\sin \beta$$

这是标准的：

$$a_x=A\cos \beta +B\sin \beta =\sqrt{A^2+B^2}\cos (\beta -\theta ),\text{其中 }\theta =\arctan 2(B,A)$$

当$a_x=0$时（这与重力方向一致的），更简单的方式是使用反正切函数：

令：
$$\tan \beta =\frac{-b_x}{C}\Rightarrow \beta =\arctan 2(b_x,-C)$$
(注求角度时候需要分辨清楚负号的位置)

---

六、最终旋转矩阵表达式

一旦求出 $\alpha$ 和 $\beta$，就可以代入下面的旋转矩阵：

$$R=R_y(\beta )R_x(\alpha )=\left[\begin{array}{ccc}\cos \beta & \sin \beta \sin \alpha & \sin \beta \cos \alpha \\ 0& \cos \alpha & -\sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{array}\right]$$

---

七、总结

已知：

• 向量 $\mathbf{a}=(a_x,a_y,a_z)$
• 对应的 $\mathbf{b}=(b_x,b_y,b_z)$
• 坐标系变换顺序：先绕x轴旋转α，再绕y轴旋转β

解法步骤：

1. 计算：
   $$\alpha =\arctan 2(-b_z,b_y)$$
2. 计算中间量：
   $$C=b_y\sin \alpha +b_z\cos \alpha$$
3. 解出：
   $$\beta =\arctan 2(b_x,-C)$$
4. 构造旋转矩阵：
   $$R=R_y(\beta )R_x(\alpha )$$

---

八、注意事项

• 这个方法在 $b_y^2+b_z^2\ne 0$ 时有效
• 如果 $b_y=b_z=0$，则 $\alpha$ 无法唯一确定
• 如果 $a_y\ne 0$或$a_x\ne 0$，则上面的 $\alpha$ 求解方式不适用，需要使用更通用的最小二乘旋转估计方法（如SVD）

---

九、提供对应代码

适用于imu通过重力方向估计姿态，并且假设yaw角为0。因此 $a=[0,0,-9.8]$，$b$是imu测的重力方向

    Eigen::Matrix3d ImuReader::computeRotationMatrix(const Eigen::Vector3d &a, const Eigen::Vector3d &b, double &alpha, double &beta){// Step 1: 计算 alphaif (b.y() == 0 && b.z() == 0){std::cerr << "Error: a_y and a_z cannot both be zero!" << std::endl;exit(EXIT_FAILURE);}alpha = std::atan2(b.y(), b.z()); // 大概是abs（90）// Step 2: 计算中间量 Cdouble ca = std::cos(alpha);double sa = std::sin(alpha);double C = b.y() * sa + b.z() * ca;// Step 3: 计算 betabeta = std::atan2(b.x(), -C);if (std::abs(beta) < 1){beta = std::atan2(-b.x(), C);}// Step 4: 构造旋转矩阵 R = Ry(beta) * Rx(alpha)Eigen::Matrix3d Rx = Eigen::Matrix3d::Identity();Rx.block<2, 2>(1, 1) << std::cos(alpha), -std::sin(alpha), std::sin(alpha), std::cos(alpha);Eigen::Matrix3d Ry = Eigen::Matrix3d::Identity();Ry.block<2, 2>(0, 0) << std::cos(beta), std::sin(beta), -std::sin(beta), std::cos(beta);Ry(0, 2) = std::sin(beta);Ry(2, 0) = -std::sin(beta);Ry(0, 0) = std::cos(beta);Ry(2, 2) = std::cos(beta);return Ry * Rx;}