旋转矩阵转成四元数的python代码

以下是将旋转矩阵转换为四元数的Python代码：

import numpy as np

def rotation_matrix_to_quaternion(R):
    qw = np.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) / 2
    qx = (R[2, 1] - R[1, 2]) / (4 * qw)
    qy = (R[0, 2] - R[2, 0]) / (4 * qw)
    qz = (R[1, 0] - R[0, 1]) / (4 * qw)
    return np.array([qw, qx, qy, qz])

# 示例代码
R = np.array([
    [0.99810947, -0.06146124,  0.00000000],
    [0.06146124,  0.99810947,  0.00000000],
    [0.00000000,  0.00000000,  1.00000000]
])
q = rotation_matrix_to_quaternion(R)
print(q)

其中，参数 `R` 是一个 $3 \times 3$ 的旋转矩阵，返回值 `q` 是一个四元数，表示与旋转矩阵等效的旋转。