Проблемы с расчетом крена, тангажа и рыскания для маркера ArUco на манипуляторе робота с 6 степенями свободы ⇐ Python

[Anonymous]

Мне нужна помощь в определении углов крена, тангажа и рыскания для маркера ArUco, установленного на манипуляторе робота с 6 степенями свободы (назовем этот маркер M2).
Я оцениваю положение M2 относительно контрольного маркера, расположенного на базе робота (M1). Для этого я вычисляю относительное вращение как:

Код: Выделить всё

def rotate_to_base(home_R, R):
return home_R.T @ R

Из-за механической конструкции робота движение M2 ограничено:

• Первое вращение: вокруг вертикальной оси (ось Y в моем случае)
• Второе вращение: вдоль одной оси плоскости маркера (ось Z в моем случае)

Чтобы извлечь углов, я вывел формулы для рыскания (Y) и тангажа (Z), умножив матрицы вращения в порядке Y * Z, а затем выделив углы из полученной матрицы:

Код: Выделить всё

theta_y_deg = np.degrees(np.arctan2(R[0,2], R[2,2]))
theta_z_deg = np.degrees(np.arctan2(R[1,0], R[1,1]))

Этот ручной подход дает результаты, наиболее близкие к моим ожиданиям.
Однако во время тестирования я заметил проблему:

• Вращение вокруг оси Y также влияет на угол оси Z
• Расхождение составляет около 5–10 градусов, что кажется слишком большим, чтобы быть шум
• Эта проблема видна на моих тестовых изображениях на моем Google Диске:
  
  Просмотр изображений на Google Диске

Я также пробовал использовать извлечение угла Эйлера в SciPy:

Код: Выделить всё

r = Rot.from_matrix(R)
angles = r.as_euler('xzy', degrees=True)  # extrinsic
angles2 = r.as_euler('yzx', degrees=True)  # intrinsic

Но они дают значительно худшие результаты, чем ручной метод, и возвращают вращения вокруг всех трех осей. Одна ось должна быть лишней, поскольку робот не может вращаться вокруг нее из-за механических ограничений.
Мой вопрос:

Что я делаю неправильно при расчете углов крена и тангажа?

Почему вращение вокруг одной оси влияет на другую в моем текущем подходе?
Ниже приведена соответствующая часть моего кода:

Код: Выделить всё

global_rvec = rvecs[i]
global_tvec = tvecs[i]

global_R, _ = cv2.Rodrigues(global_rvec)
R = rotate_to_base(home_R, global_R)

rvec, _ = cv2.Rodrigues(global_R)

theta_y_deg = np.degrees(np.arctan2(R[0,2], R[2,2]))
theta_z_deg = np.degrees(np.arctan2(R[1,0], R[1,1]))

# Alternative approaches
r = Rot.from_matrix(R)
angles = r.as_euler('xzy', degrees=True)
angles2 = r.as_euler('yzx', degrees=True)

lines = [
f"ID {ids[i][0]}",
f"X={tvec[0][0]:.3f} Y={tvec[0][1]:.3f} Z={tvec[0][2]:.3f} m",
f"rot angles X {angles[0]:.1f}, rot Y {angles[1]:.1f}, rot Z {angles[2]:.1f} deg",
f"rot angles2 X {angles2[0]:.1f}, rot Y {angles2[1]:.1f}, rot Z {angles2[2]:.1f} deg",
f"rot manual Y {theta_y_deg:.1f}, rot Z {theta_z_deg:.1f} deg"
]

Заранее благодарим за любую помощь!