使用四元数解决欧拉角万向锁问题（一）

一、背景

工作中涉及到一个项目，实现了无人机视频流在三维场景中的投放（如下图1），受限于云台传感器的测量误差，直接使用mqtt协议对接的无人机POS数据进行视频投放效果不甚理想，在前端实现了设置姿态角改正值的功能栏(如下图2)，但使用过程中发现默认情况下输入的滚转角改正值，表现出来的实际改正效果（如下图3）跟调整偏航角改正值一致（如下图4），判断是欧拉角的万向锁问题。听闻四元数可以避免该问题[2]，遂想到将欧拉角和改正值均转换成四元数，完成姿态改正后再转换回欧拉角的思路。(如下图5)

图1 投放示意图

图2 改正功能示意图

图3 滚转角改正20度

图4 偏航角改正20度

图5 借助四元数规避万向锁的思路

二、具体应用公式

1.欧拉角转四元数

首先需要确定系统内使用欧拉角的旋转方式，欧拉角旋转分为Proper Euler angles和Tait–Bryan angles两种方式，这里我们使用的是后者。
接下来确定欧拉角旋转顺序，经实验（详见代码及实验章节）确认，系统内使用的欧拉角旋转顺序为内旋（动态欧拉角）右手系ZXY（yaw-pitch-roll），可以推导出该情况下欧拉角转四元数的具体公式为(yaw: ψ , pitch: θ, roll: ϕ)：
$$\begin{array}{rl}w& =\cos (\frac{\varphi }{2})\cos (\frac{\theta }{2})\cos (\frac{\psi }{2})-\sin (\frac{\varphi }{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2}),\\ x& =\cos (\frac{\varphi }{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2})-\sin (\frac{\varphi }{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2}),\\ y& =\cos (\frac{\varphi }{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})+\sin (\frac{\varphi }{2})\cos (\frac{\theta }{2})\cos (\frac{\psi }{2}),\\ z& =\cos (\frac{\varphi }{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2})+\sin (\frac{\varphi }{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2}).\end{array}$$

2.四元数乘法

$$\begin{array}{rl}w& =w_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2,\\ x& =w_1{x}_2+x_1{w}_2+y_1{z}_2-z_1{y}_2,\\ y& =w_1{y}_2-x_1{z}_2+y_1{w}_2+z_1{x}_2,\\ z& =w_1{z}_2+x_1{y}_2-y_1{x}_2+z_1{w}_2.\end{array}$$

3.四元数转旋转矩阵

因为采用的是右手系，使用四元数的Hamilton表达，转换公式为：
$$R=\left(\begin{array}{ccc}1-2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1-2(x^2+y^2)\end{array}\right)$$

4.旋转矩阵转欧拉角

这部分公式以及边界值处理可以直接参考wiki-Euler_angles#Tait–Bryan_angles
$$\begin{array}{rl}\text{yaw}& =\arctan (\frac{-t_{12}}{t_{22}})\\ \text{pitch}& =\arcsin (t_{32})\\ \text{roll}& =\arctan (\frac{-t_{31}}{t_{33}})\end{array}$$

三、代码及实验

1. python

参考自https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html#scipy-spatial-transform-rotation
辅助函数

# 绘制旋转后的坐标轴
def plot_rotated_axes(ax, r, name=None, offset=(0, 0, 0), scale=1):
    colors = ("#FF6666", "#005533", "#1199EE")  # Colorblind-safe RGB
    loc = np.array([offset, offset])
    for i, (axis, c) in enumerate(zip((ax.xaxis, ax.yaxis, ax.zaxis),
                                      colors)):
        axlabel = axis.axis_name
        axis.set_label_text(axlabel)
        axis.label.set_color(c)
        axis.line.set_color(c)
        axis.set_tick_params(colors=c)
        line = np.zeros((2, 3))
        line[1, i] = scale
        line_rot = r.apply(line)
        line_plot = line_rot + loc
        ax.plot(line_plot[:, 0], line_plot[:, 1], line_plot[:, 2], c)
        text_loc = line[1]*1.2
        text_loc_rot = r.apply(text_loc)
        text_plot = text_loc_rot + loc[0]
        ax.text(*text_plot, axlabel.upper(), color=c,
                va="center", ha="center")
    ax.text(*offset, name, color="k", va="center", ha="center",
            bbox={"fc": "w", "alpha": 0.8, "boxstyle": "circle"})
    
# 四元数乘法
def quaternion_multiply(q1, q2):
    """
    Multiply two quaternions.

    Parameters:
    q1, q2 (tuple or list): Quaternions in the form [x, y, z, w].

    Returns:
    np.ndarray: Resulting quaternion [x, y, z, w].
    """
    x1, y1, z1, w1 = q1
    x2, y2, z2, w2 = q2

    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2

    return np.array([x, y, z, w])

测试函数

# 测试不同操作对于基于输入欧拉角旋转（r1）的影响
def euler_to_quaternion_intrinsic_zxy(euler_angles):
    """
    Convert intrinsic ZXY Euler angles to quaternion.

    Parameters:
    euler_angles (tuple or list): Euler angles in ZXY order (yaw, pitch, roll) in radians.

    Returns:
    np.ndarray: Quaternion [x, y, z, w].
    """
    print("输入为(45,90,90) ZXY order: yaw, pitch, roll")
    r0 = R.from_quat(np.asarray([0,0,0,1]))
    
    # Create a rotation object from Euler angles with intrinsic ZXY order
    r1 = R.from_euler('ZXY', euler_angles, degrees=False)
    # Convert the rotation object to a quaternion
    quaternion = r1.as_quat()
    print("r1: Together:")
    print(quaternion)
    
    r2 = R.from_euler('ZXY', (euler_angles[0], 0, 0), degrees=False)
    quaternion_rotate_z = r2.as_quat()
    print("r2: Z:")
    print(quaternion_rotate_z)
    
    r3 = R.from_euler('ZXY', (0, euler_angles[1], 0), degrees=False)
    quaternion_rotate_x = r3.as_quat()
    print("r3: X:")
    print(quaternion_rotate_x)
    
    r4 = R.from_euler('ZXY', (0, 0, euler_angles[2]), degrees=False)
    quaternion_rotate_y = r4.as_quat()
    print("r4: Y:")
    print(quaternion_rotate_y)
    
    quaternion_rotate_all = quaternion_multiply(quaternion_rotate_z,quaternion_multiply(quaternion_rotate_x,quaternion_rotate_y))
    r5 = R.from_quat(quaternion_rotate_all)
    print("r5: 输入欧拉角拆分为四元数并Z*X*Y:")
    print(quaternion_rotate_all)

    r6 = R.from_euler('ZXY', (0, 0, np.pi/4), degrees=False)
    quaternion_rotate_mod = r6.as_quat()
    print("r6: 滚转角pi/4的四元数:")
    print(quaternion_rotate_mod)
    
    r7 = R.from_euler('ZXY', (np.pi/4, np.pi/2, np.pi/2 + np.pi/4), degrees=False)
    quaternion_rotate_plus = r7.as_quat()
    print("r7: 基于欧拉角的旋转(pi/4, pi/2, 3pi/4):")
    print(quaternion_rotate_plus)
    
    r8 = R.from_quat(quaternion_rotate_plus)
    euler_plus = r8.as_euler('ZXY',degrees=True)
    print("r8、r7转换成欧拉角:")
    print(euler_plus)
    
    r9 = R.from_quat(quaternion)
    euler = r9.as_euler('ZXY', degrees=True)
    
    after_rotate = quaternion_multiply(quaternion_rotate_mod,quaternion)
    after_rotate_ = quaternion_multiply(quaternion,quaternion_rotate_mod)
    
    r10 = R.from_quat(after_rotate)
    euler_rotate = r10.as_euler('ZXY', degrees=True)
    
    print("r9: r1转换成四元数并旋转:", euler)
    print("r10: r6转换成四元数左乘r9对应四元数:", euler_rotate)
    print("r10: pi/4左乘r1 :")
    print(after_rotate)
    
    r11 = R.from_quat(after_rotate_)
    print("r11: r5右乘r8对应四元数 :")
    print(after_rotate_)
    print("r11: r5右乘r8对应欧拉角:")
    print(r11.as_euler('ZXY', degrees=True))
    
    ax = plt.figure().add_subplot(projection="3d", proj_type="ortho")
    plot_rotated_axes(ax, r0, name="r0", offset=(0, 0, 0))
    plot_rotated_axes(ax, r1, name="r1", offset=(3, 0, 0))
    # plot_rotated_axes(ax, r2, name="r2", offset=(3, 0, 0))
    # plot_rotated_axes(ax, r3, name="r3", offset=(6, 0, 0))
    plot_rotated_axes(ax, r4, name="r4", offset=(6, 0, 0))
    plot_rotated_axes(ax, r5, name="r5", offset=(9, 0, 0))
    plot_rotated_axes(ax, r6, name="r6", offset=(12, 0, 0))
    plot_rotated_axes(ax, r7, name="r7", offset=(15, 0, 0))
    plot_rotated_axes(ax, r8, name="r8", offset=(18, 0, 0))
    plot_rotated_axes(ax, r9, name="r9", offset=(21, 0, 0))
    plot_rotated_axes(ax, r10, name="r10", offset=(24, 0, 0))
    plot_rotated_axes(ax, r11, name="r11", offset=(27, 0, 0))
    ax.set(xlim=(-1.25, 30), ylim=(-1.25, 1.25), zlim=(-1.25, 1.25))
    ax.set(xticks=range(-1, 30), yticks=[-1, 0, 1], zticks=[-1, 0, 1])
    ax.set_aspect("equal", adjustable="box")
    plt.tight_layout()
    # 添加注释
    _ = ax.annotate(
    "r0: 不做旋转\n"
    "r1: 基于输入欧拉角的旋转\n"
    "r4: 基于输入滚转角（Y轴）的旋转\n"
    "r5: 输入欧拉角拆分为四元数并Z*X*Y\n"
    "r6: 滚转角pi/4\n"
    "r7: 基于欧拉角的旋转(滚转角相较于输入多了pi/4)\n"
    "r8: r7转换成四元数并旋转\n"
    "r9: r1转换成四元数并旋转\n"
    "r10: r6转换成四元数左乘r9对应四元数\n"
    "r11: r6转换成四元数右乘r9对应四元数\n"
    ,
    xy=(0.6, 0.7), xycoords="axes fraction", ha="left"
)
    plt.show()
    return None

测试结果见下图7-9：输入欧拉角为[PI/4，PI/2，PI/2]，

图7 python实验结果1

图8 python实验结果2

图9 python实验结果3

2. 实验结果以及分析

实验结果：
当输入欧拉角为[45，90，90]，有以下结果：

1. r1=r5=r9
2. r7=r8=r11
3. r7/r8对应欧拉角为[180,90,0]
4. r9对应欧拉角为[135,90,0]
5. r10对应欧拉角为[-90,45,-135]
6. r11对应欧拉角为[180,90,0]

分析：

1. 结果1验证了欧拉角、四元数的不同形式对于旋转描述的等价性
2. 结果2表明滚转角的四元数右乘（r11）在ZXY旋转顺序下表现为初始滚转值的相应值的增加(r7)。
3. 对比结果4和输入，我们发现滚转角的值累加到了偏航角上，验证了ZXY旋转顺序的准确性。
4. 对比结果3和结果4，我们发现初始滚转值的增加会表现为偏航角值的等量增加。
5. 由分析2和分析4可知，四元数改正值右乘无法解决万向锁问题（ZXY旋转顺序下）。
6. 结合图6中r10的姿态和结果5，初步判断四元数改正值左乘的思路可行。

四、验证

定位到姿态设置相关代码(伪代码)

Camera.setView({
    destination: s,
    orientation: {
        heading: Cesium.Math.toRadians(modifiedHeading),
        pitch: Cesium.Math.toRadians(modifiedPitch),
        roll: Cesium.Math.toRadians(modifiedRoll)
    }
})

参考Cesium文档:

如果本文叙述的思路失败了，还可以考虑直接使用旋转矩阵或DirectionUp向量实现QAQ.

增加以下函数：

//四元数修正
addModifiedRotation(yaw,pitch,roll,yaw_mod,pitch_mod,roll_mod){
    yaw = yaw/180*Math.PI
    pitch = pitch/180*Math.PI
    roll = roll/180*Math.PI
    yaw_mod = yaw_mod/180*Math.PI
    pitch_mod = pitch_mod/180*Math.PI
    roll_mod = roll_mod/180*Math.PI
    
    console.log("before modified: ", yaw, pitch, roll)
    console.log("try to modify: ", yaw_mod, pitch_mod, roll_mod)
    var qua_origin = this.eulerToQuaternionIntrinsicZXY([yaw,pitch,roll])
    var qua_mod = this.eulerToQuaternionIntrinsicZXY([yaw_mod,pitch_mod,roll_mod])
    var qua_res = this.quaternionMultiply(qua_mod,qua_origin)
    var euler_res = this.quaternionToEulerIntrinsicZXY(qua_res)
    console.log("getting modified res:", euler_res)
    return euler_res
}
//欧拉角转四元数
eulerToQuaternionIntrinsicZXY(eulerAngles) {
    const [yaw, pitch, roll] = eulerAngles;

    // Calculate trigonometric functions
    const cy = Math.cos(yaw * 0.5);
    const sy = Math.sin(yaw * 0.5);
    const cp = Math.cos(pitch * 0.5);
    const sp = Math.sin(pitch * 0.5);
    const cr = Math.cos(roll * 0.5);
    const sr = Math.sin(roll * 0.5);

    // Calculate quaternion
    const w = cr * cp * cy - sr * sp * sy;
    const x = cr * sp * cy - sr * cp * sy;
    const y = cr * sp * sy + sr * cp * cy;
    const z = cr * cp * sy + sr * sp * cy;

    return [x, y, z, w];
}
//四元数转欧拉角
quaternionToEulerIntrinsicZXY(quaternion) {
    const [x, y, z, w] = quaternion;
    let precision = 30
    // Calculate intermediate values
    let x_x = parseFloat(x * x).toFixed(precision),
    y_y = parseFloat(y * y).toFixed(precision),
    z_z = parseFloat(z * z).toFixed(precision),
    x_y = parseFloat(x * y).toFixed(precision),
    x_z = parseFloat(x * z).toFixed(precision),
    w_x = parseFloat(w * x).toFixed(precision),
    w_z = parseFloat(w * z).toFixed(precision),
    w_y = parseFloat(w * y).toFixed(precision),
    z_y = parseFloat(z * y).toFixed(precision);
    // console.log("getting xx,yy,zz,xy,xz,wx,wz,wy,zy: ",x_x,y_y,z_z,x_y,x_z,w_x,w_z,w_y,z_y)

    const t12 = 2 * ((parseFloat(x_y) - parseFloat(w_z)).toFixed(precision));
    const t22 = (1 - parseFloat((2 * (parseFloat(x_x) + parseFloat(z_z)).toFixed(precision)).toFixed(precision))).toFixed(precision);
    const t31 = 2 * (parseFloat(x_z) - parseFloat(w_y)).toFixed(precision);
    const t32 = 2 * (parseFloat(z_y) + parseFloat(w_x)).toFixed(precision);
    const t33 = (1 - parseFloat(2 * (parseFloat(y_y) + parseFloat(x_x)).toFixed(precision))).toFixed(precision);
    // console.log("getting -t12,t22,-t31,t32,t33",-t12,t22,-t31,t32,t33)

    var pitch = 0
    var yaw = 0, roll = 0
    if(t32 != 1&& t32 != -1){
        pitch = Math.asin(t32);
        yaw = Math.atan2(-t12, t22);
        roll = Math.atan2(-t31, t33);
        console.log("yaw:",yaw,"pitch:",pitch,"roll",roll)
    }
    else{
        roll = 0
        const t11 = (1-parseFloat(2*(parseFloat(y_y) + parseFloat(z_z)).toFixed(precision))).toFixed(precision);
        const t21 = 2*(parseFloat(x_y) + parseFloat(w_z)).toFixed(precision);
        console.log("getting t11,t21",t11,t21)
        if (t32 = 1){
            pitch = Math.PI/2
            yaw = Math.atan2(t21,t11)
        }
        else if (t32 = -1){
            pitch = -Math.PI/2
            yaw = Math.atan2(t21,t11)
        }
        console.log("yaw:",yaw,"pitch:",pitch,"roll",roll)
    }
    // pitch = pitch/Math.PI*180;
    // yaw = yaw/Math.PI*180;
    // roll = roll/Math.PI*180;
    return [yaw, pitch, roll];
//四元数乘法
quaternionMultiply(q1, q2) {
    const [x1, y1, z1, w1] = q1;
    const [x2, y2, z2, w2] = q2;

    const w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
    const x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
    const y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
    const z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;

    return [x, y, z, w];
}

最终效果如图10所示

图10 滚转角调整示意图
可见画面范围发生固定方向的形变，符合预期。
至此，实现了在空间绝对坐标系下对于滚转角的改正（四元数左乘），但由于无人机在飞行过程中会变向，此时画面滚转角的改正方向会发生变化，一个更为优雅的改正方案应该在无人机的机身坐标系中进行，具体见下一篇文章：使用四元数解决欧拉角万向锁问题（二）

五、存在问题

比较上图4与上图10可发现画面旋转了180度，最终应用的代码投放前需要在滚转角上取个负号来加以改正，为什么会有画面的反转，以及这个负号为什么好用，我在StackOverflow上提了这个问题，还希望能有大佬不吝赐教。

图11 测试数据

六、参考资料

1. 肖伟, 梁久祯, 陈玮琪. 基于四元数的3D物体旋转及运动插值[J/OL]. 系统仿真学报, 2012, 24(3): 624-627.
2. 视觉SLAM十四讲：从理论到实践（高翔）
3. 欧拉角转换四元数
4. 欧拉角转换四元数公式推导
5. 动态欧拉角与静态欧拉角的区别
6. 四元数乘法计算
7. 四元数与旋转矩阵
8. wiki-Euler_angles#Tait–Bryan_angles
9. Computing Euler angles from a rotation matrix
10. scipy文档 scipy.spatial.transform.Rotation
11. Cesium文档 Camera
12. atan2与atan的区别