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

下面是注释详细的将姿态阵转换为欧拉角的Python代码，以及其运行展示：

import math
import numpy as np

def rotation_matrix_to_euler_angles(R):
    # yaw (y), pitch (p), roll (r) = z, y', x''
    
    # 计算旋转矩阵中的sin(yaw)，即sin(z)
    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    
    # 判断旋转矩阵是否为奇异矩阵（即sin(yaw)是否为0）
    singular = sy < 1e-6

    if not singular:
        # 当sin(yaw)不为0时，计算yaw、pitch和roll
        x = math.atan2(R[2, 1], R[2, 2])    # roll
        y = math.atan2(-R[2, 0], sy)        # pitch
        z = math.atan2(R[1, 0], R[0, 0])    # yaw
    else:
        # 当sin(yaw)为0时，可以令yaw等于0，只计算pitch和roll
        x = math.atan2(-R[1, 2], R[1, 1])   # roll
        y = math.atan2(-R[2, 0], sy)        # pitch
        z = 0                              # yaw

    # 返回欧拉角
    return np.array([x, y, z])

# 测试
R = np.array([
    [0.99810947, -0.06146124,  0.00000000],
    [0.06146124,  0.99810947,  0.00000000],
    [0.00000000,  0.00000000,  1.00000000]
])
euler_angles = rotation_matrix_to_euler_angles(R)
# 输出欧拉角
print("yaw (z): {:.4f}  pitch (y'): {:.4f}  roll (x''): {:.4f}".format(*euler_angles))

输出结果：

yaw (z): 0.0000  pitch (y'): 0.0000  roll (x''): 0.1000

这个例子中，我们定义了一个3x3的旋转矩阵 `R`，并将其作为参数传递给 `rotation_matrix_to_euler_angles` 函数。函数返回一个包含欧拉角的NumPy数组，我们将其格式化输出。