四元数球面线性插值在无人机视频融合中的应用

一、背景

项目需要将无人机画面实时动态地投放于三维场景，受限于大疆上云API的POS(Position and Orientation System)数据采样频率0.5hz，投放后的视频画面卡顿错位相对严重，考虑使用插值算法对POS数据进行平滑，以优化视频投放后的融合效果。

位置插值方面使用一般线性插值，但姿态方面的插值需要额外考量：
大疆上云API接入的POS数据使用“ZXY”（yaw-pitch-roll）内旋的欧拉角进行表达，当无人机投放画面时，俯仰角pitch一般在-90度附近，这时会出现万向锁问题。通过将欧拉角变换到四元数，并实现球面线性插值（Slerp)，可以做到平滑均匀的中间姿态补充。

二、具体应用公式

1.球面线性插值

$$\text{SLERP}({\mathbf{q}}_1,{\mathbf{q}}_2;t)=\frac{\sin ((1-t)\theta )}{\sin (\theta )}{\mathbf{q}}_1+\frac{\sin (t\theta )}{\sin (\theta )}{\mathbf{q}}_2$$

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}$$

三、代码及实验

1. python代码

# -*- coding: utf-8 -*-
# @Author: 27987
# @Date: 2024-7-5 16:04
# @Last Modified by: 27987
# @Last Modified time: 2024-7-5 16:04

import numpy as np
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
import matplotlib.pyplot as plt

plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号


# 绘制旋转后的坐标轴
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"})


class Quaternion:
    def __init__(self, w, x, y, z):
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    def __mul__(self, other):
        if isinstance(other, Quaternion):
            return Quaternion(
                self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
                self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
                self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
                self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
            )
        elif isinstance(other, (int, float, np.float64)):
            return Quaternion(self.w * other, self.x * other, self.y * other, self.z * other)

    def __rmul__(self, other):
        if isinstance(other, Quaternion):
            return Quaternion(
                self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
                self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
                self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
                self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
            )
        elif isinstance(other, (int, float, np.float64)):
            print(Quaternion(self.w * other, self.x * other, self.y * other, self.z * other))
            return Quaternion(self.w * other, self.x * other, self.y * other, self.z * other)

    def __neg__(self):
        return Quaternion(-self.w, -self.x, -self.y, -self.z)

    def __sub__(self, other):
        return self.__add__(-other)

    def __add__(self, other):
        return Quaternion(
            self.w + other.w,
            self.x + other.x,
            self.y + other.y,
            self.z + other.z
        )

    def __repr__(self):
        return f"Quaternion(w={self.w}, x={self.x}, y={self.y}, z={self.z})"

    def normalize(self):
        norm = np.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)
        self.w /= norm
        self.x /= norm
        self.y /= norm
        self.z /= norm
        return self
    def dot(self, q2):
        return self.w * q2.w + self.x * q2.x + self.y * q2.y + self.z * q2.z

    @staticmethod
    def slerp(q1, q2, t):
        # 点乘：|a||b|cos(theta); |a|=|b|=1
        dot = q1.dot(q2)
        # 四元数q和其负数−q代表相同的旋转,当cos为负数取反以找到球面最短路径
        if dot < 0.0:
            q1 = -q1
            dot = -dot
        # 夹角过小退化成线性插值
        if dot > 0.9995:
            dis = (q2 - q1)
            result = q1 + (t * dis)
            result.normalize()
            return result

        theta_0 = np.arccos(dot)
        theta = theta_0 * t
        theta_ = theta_0 * (1-t)
        # q2 = q2 - (q1 * dot)


        q = q1 * (np.sin(theta_) / np.sin(theta_0)) + q2 * (np.sin(theta) / np.sin(theta_0))
        print("the length of q:",q.normalize())
        return q
    
r = R.from_euler("ZXY",(-0,-90.30000000000001,0),degrees=True)
q = r.as_quat()
r_ = R.from_euler("ZXY",(-40,-60,64),degrees=True)
q_ = r_.as_quat()
# Example usage:
q1 = Quaternion(q[3], q[0], q[1], q[2])
q2 = Quaternion(q_[3], q_[0], q_[1], q_[2])
print(q,q_)
print("""""""""""")
t = 0.32999999999999996
r0 = R.from_quat([q1.x, q1.y, q1.z, q1.w])
r1 = R.from_quat([q2.x, q2.y, q2.z, q2.w])
interpolated_q = Quaternion.slerp(q1, q2, t)
r2 = R.from_quat([interpolated_q.x, interpolated_q.y, interpolated_q.z, interpolated_q.w])
print("inter",interpolated_q)
interpolated_q = Quaternion.slerp(q1, q2, 0.6)
r3 = R.from_quat([interpolated_q.x, interpolated_q.y, interpolated_q.z, interpolated_q.w])
interpolated_q = Quaternion.slerp(q1, q2, 0.9)
r4 = R.from_quat([interpolated_q.x, interpolated_q.y, interpolated_q.z, interpolated_q.w])

def sci_slerp():
    r = R.from_quat([q,q_])
    slerp = Slerp([0,1],r)
    print(slerp(0.3).as_quat())
    r5 = R.from_quat(slerp(0.3).as_quat())

    print(slerp(0.6).as_quat())
    r6 = R.from_quat(slerp(0.6).as_quat())

    print(slerp(0.9).as_quat())
    r7 = R.from_quat(slerp(0.9).as_quat())

    show_plot(r5,r6,r7)
def show_plot(r5,r6,r7):
    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=(6, 0, 0))
    plot_rotated_axes(ax, r3, name="r3", offset=(9, 0, 0))
    plot_rotated_axes(ax, r4, name="r4", offset=(12, 0, 0))
    plot_rotated_axes(ax, r5, name="r5", offset=(15, 0, 0))
    plot_rotated_axes(ax, r6, name="r6", offset=(18, 0, 0))
    plot_rotated_axes(ax, r7, name="r7", offset=(21, 0, 0))
    ax.set(xlim=(-1.25, 24), ylim=(-1.25, 1.25), zlim=(-1.25, 1.25))
    ax.set(xticks=range(-1, 24), yticks=[-1, 0, 1], zticks=[-1, 0, 1])
    ax.set_aspect("equal", adjustable="box")
    plt.tight_layout()
    # 添加注释
    _ = ax.annotate(
        "r0: (-0,-90.30000000000001,0)\n"
        "r1: (-40,-60,64)\n"
        "r2: 0.32999999999999996手动插值\n"
        "r3: 0.6手动插值\n"
        "r4: 0.9手动插值\n"
        "r5: 0.3 scipy slerp\n"
        "r6: 0.6 scipy slerp\n"
        "r7: 0.9 scipy slerp\n"
        ,
        xy=(0.6, 0.7), xycoords="axes fraction", ha="left"
    )
    plt.show()
if __name__ == "__main__":
    sci_slerp()

2. 实验结果以及分析

实验结果

1. r3 = r6
2. r4 = r7
   分析
   实现了scipy中slerp球面线性插值算法

四、验证

JavaSript实现：

class Quaternion{
    constructor(w, x, y, z){
        this.w = w
        this.x = x
        this.y = y
        this.z = z
    }
    quaMultiply(q1, q2){
        return new Quaternion(
            q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
            q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
            q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
            q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
        )
    }
    numMultiply(num,quaternion){
        return new Quaternion(quaternion.w * num, quaternion.x * num, quaternion.y * num, quaternion.z * num)
    }
    neg(){
        this.w = -this.w
        this.x = -this.x
        this.y = -this.y
        this.z = -this.z
        return this
    }
    add(q1,q2){
        return new Quaternion(q1.w+q2.w, q1.x+q2.x, q1.y+q2.y, q1.z+q2.z)
    }
    subtract(q1,q2){
        return this.add(q1,q2.neg())
    }
    normalize(){
        var norm = Math.sqrt(this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z)
        this.w /= norm
        this.x /= norm
        this.y /= norm
        this.z /= norm
        return this
    }
    dot(q){
        return this.w * q.w + this.x * q.x + this.y * q.y + this.z * q.z
    }
    slerp(q2, t){
        // 点乘：|a||b|cos(theta); |a|=|b|=1
        let dot = this.dot(q2)
        // 四元数q和其负数−q代表相同的旋转,当cos为负数取反以找到球面最短路径
        if (dot < 0.0){
            console.log("q1: ", this)
            this.neg()
            console.log("neg q1: ", this)
            dot = -dot
        }
        // 夹角过小退化成线性插值
        if (dot > 0.9995){
            let dis = this.subtract(q2, this),
            result = this.add(this, this.numMultiply(t, dis))
            result.normalize()
            return result
        }

        let theta_0 = Math.acos(dot)
        let theta = theta_0 * t
        let theta_ = theta_0 * (1-t)
        // q2 = q2 - (q1 * dot)
        let res = this.add(this.numMultiply((Math.sin(theta_) / Math.sin(theta_0)), this), this.numMultiply((Math.sin(theta) / Math.sin(theta_0)), q2))
        console.log("t, res: ", t, res)
        return res
    }
}

最终效果如下：

四、参考资料

1. 视觉SLAM十四讲：从理论到实践（高翔）
2. 欧拉角转换四元数
3. 欧拉角转换四元数公式推导
4. 动态欧拉角与静态欧拉角的区别
5. 四元数乘法计算
6. 球面线性插值
7. 大疆上云API
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的区别