如何求解将M_start旋转至M_end矩阵的xyz轴旋转角a、b、c

我有一个包含xyz坐标的6×3起始矩阵M_start，以及一个同维度的目标坐标矩阵M_end。需要求解绕x、y、z轴的旋转角a、b、c，满足旋转矩阵等式：Rz(c) * Ry(b) * Rx(a) * M_start = M_end（其中Rx、Ry、Rz分别是绕x、y、z轴的旋转矩阵）。

我尝试用维基百科提供的3×3旋转矩阵RzRyRx推导得到18个方程，选取其中3个后用SymPy的solve()和nonlinsolve()求解，但solve()耗时极久，nonlinsolve()返回的ConditionSet结果难以理解。返回结果示例如下：

ConditionSet((a, b, c), Eq((2.02015sin(c) - 0.708642cos(c))cos(b) + 0.6677sin(b) - 1.0154, 0) & Eq(-0.1967*((sin(c) - 1.122cos(c))sin(b) - 0.6853cos(b))sin(a) - 0.1967(1.122sin(c) + cos(c))cos(a) - 0.0285, 0) & Eq(0.2201sin(a)sin(c) + 0.1967sin(a)cos(c) - 0.1967sin(b)*sin(c)cos(a) + 0.2207sin(b)*cos(a)cos(c) + 0.1348cos(a)*cos(b) - 0.323, 0), ProductSet(Complexes, Complexes, Complexes)

我的代码如下：

from sympy import symbols, cos, sin, Eq, solve, nonlinsolve

a, b, c = symbols('a b c')
# 方程定义
eq1 = Eq(0.2201*sin(a)*sin(c) + 0.1967*sin(a)*cos(c) - 0.1967*sin(b)*sin(c)*cos(a) + 0.2207*sin(b)*cos(a)*cos(c) + 0.1348*cos(a)*cos(b), 0.323)
eq2 = Eq(-0.1967*(cos(a)*(cos(c) + 1.122*sin(c)) + sin(a)*(-0.6853*cos(b) + sin(b)*(-1.122*cos(c) + sin(c)))), 0.0285)
eq3 = Eq(0.6677*sin(b) + cos(b)*(-0.708642*cos(c) + 2.02015*sin(c)), 1.0154)

solution = nonlinsolve((eq1,eq2,eq3), (a, b, c))

print(solution)

请问是否有其他求解a、b、c的方法，或是我的方法存在错误？

当前方法的问题 #

直接用符号求解带浮点数系数的非线性三角函数方程组，本身就属于计算复杂度极高的问题：

• SymPy的符号求解器更适合处理精确的符号表达式，面对含浮点数的非线性方程组时，不仅耗时久，返回的ConditionSet是符号解的集合表示，对实际工程应用来说几乎无法直接使用。
• 手动选取3个方程可能遗漏约束，或引入冗余，进一步增加求解难度。

替代方法 #

1. 先求整体旋转矩阵，再分解为欧拉角 #

这是最常用且高效的方法，步骤如下：

• 计算整体旋转矩阵R：
  因为R * M_start = M_end，当M_start列满秩（至少3个点不共线）时，可通过最小二乘法求解R：

  import numpy as np
  
  # 假设M_start和M_end是numpy数组（6×3）
  M_start = ...
  M_end = ...
  # 计算最小二乘旋转矩阵
  H = M_start.T @ M_end
  U, S, Vt = np.linalg.svd(H)
  R = Vt.T @ U.T
  # 确保旋转矩阵是正交且行列式为1（避免反射）
  if np.linalg.det(R) < 0:
      Vt[2,:] *= -1
      R = Vt.T @ U.T
  

• 将R分解为Z-Y-X欧拉角：
  对应R = Rz(c) * Ry(b) * Rx(a)的顺序，分解公式如下：
  • 俯仰角b = np.arcsin(R[2, 0])，或b = np.pi - np.arcsin(R[2, 0])（两个可能的解分支）
  • 当np.cos(b) ≠ 0时：

    a = np.arctan2(-R[2, 1], R[2, 2])
    c = np.arctan2(-R[1, 0], R[0, 0])
    

  • 当np.cos(b) = 0时（欧拉角奇异性，万向锁），需要特殊处理：此时a + c为定值，可根据实际需求选择其中一个角度固定为0求解另一个。

2. 使用数值求解器替代符号求解 #

如果一定要直接解方程组，用数值优化方法更实用，比如SciPy的least_squares（适合处理残差最小化问题）：

import numpy as np
from scipy.optimize import least_squares

def residuals(x):
    a, b, c = x
    # 计算每个方程的残差（左边减右边）
    res1 = 0.2201*np.sin(a)*np.sin(c) + 0.1967*np.sin(a)*np.cos(c) - \
           0.1967*np.sin(b)*np.sin(c)*np.cos(a) + 0.2207*np.sin(b)*np.cos(a)*np.cos(c) + \
           0.1348*np.cos(a)*np.cos(b) - 0.323
    res2 = -0.1967*(np.cos(a)*(np.cos(c) + 1.122*np.sin(c)) + \
           np.sin(a)*(-0.6853*np.cos(b) + np.sin(b)*(-1.122*np.cos(c) + np.sin(c)))) - 0.0285
    res3 = 0.6677*np.sin(b) + np.cos(b)*(-0.708642*np.cos(c) + 2.02015*np.sin(c)) - 1.0154
    return [res1, res2, res3]

# 初始猜测值（可根据实际情况调整）
x0 = np.array([0.0, 0.0, 0.0])
# 求解
result = least_squares(residuals, x0)

if result.success:
    a_sol, b_sol, c_sol = result.x
    print(f"求解成功：a={a_sol:.4f}, b={b_sol:.4f}, c={c_sol:.4f}")
else:
    print("求解失败：", result.message)

数值求解会给出满足精度要求的实数解，适合工程场景。

3. 检查方程推导的正确性 #

• 旋转矩阵顺序：确认Rz(c)*Ry(b)*Rx(a)的乘法顺序是否和你推导方程时一致，欧拉角的顺序（内旋/外旋）很容易搞反，是常见错误来源。
• 系数精度：推导方程时建议先用符号变量完成推导，最后再代入浮点数，避免中间步骤的精度损失。

内容的提问来源于stack exchange，提问作者Emil Karaev