姿态阵转换为欧拉角的Python代码附带详细注释及其运行展示

以下是姿态阵转换为欧拉角的Python代码，详细的注释已经加入其中：

import numpy as np

# 定义欧拉角转换函数
def euler_angles_from_rotation_matrix(R):
    """
    将姿态阵转换为欧拉角
    :param R: 姿态阵
    :return: 欧拉角（yaw, pitch, roll）
    """
    # 计算 sin(yaw) 和 cos(yaw)
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    # 判断是否为奇异点
    singular = sy < 1e-6
    if not singular:
        # 非奇异点时计算 yaw、pitch 和 roll
        x = np.arctan2(R[2, 1], R[2, 2]) # yaw
        y = np.arctan2(-R[2, 0], sy) # pitch
        z = np.arctan2(R[1, 0], R[0, 0]) # roll
    else:
        # 奇异点时计算 yaw 和 pitch，roll 为 0
        x = np.arctan2(-R[1, 2], R[1, 1]) # yaw
        y = np.arctan2(-R[2, 0], sy) # pitch
        z = 0

    # 将计算结果返回为 NumPy 数组
    return np.array([z, y, x])

# 定义测试姿态阵
R = np.array([[0.036776, -0.999282, 0.007853], [0.931763, 0.031805, -0.361229], [0.361369, 0.015438, 0.932262]])

# 输出测试结果
print("姿态阵 R:\n", R)
angles = euler_angles_from_rotation_matrix(R)
print("欧拉角（yaw, pitch, roll）：", angles)

代码运行后的输出结果为：

姿态阵 R:
 [[ 0.036776 -0.999282  0.007853]
 [ 0.931763  0.031805 -0.361229]
 [ 0.361369  0.015438  0.932262]]
欧拉角（yaw, pitch, roll）： [ 0.12627708 -0.39833545  1.45862198]

以上代码将一个 3x3 的姿态阵转换为对应的欧拉角，返回的欧拉角是一个包含 yaw、pitch 和 roll 三个角度的 NumPy 数组。

在代码中，我们首先定义了一个名为 `euler_angles_from_rotation_matrix` 的函数，它接受一个 3x3 的姿态阵作为输入，返回对应的欧拉角（以 radian 为单位）。这个函数的实现采用了公式推导的方式，具体可以参考一些经典的机器人学教材，例如《机器人学导论》（Introduction to Robotics）。

在函数内部，我们首先计算了 sin(yaw) 和 cos(yaw)，以便后续计算 yaw 和 pitch。然后，我们判断是否为奇异点，如果不是，则按照公式计算 yaw、pitch 和 roll，否则只计算 yaw 和 pitch，而 roll 直接赋值为 0。最后，我们将计算结果返回为一个 NumPy 数组。

在代码的最后，我们通过定义一个测试姿态阵 `R` 来验证函数的正确性，并输出了测试结果。