姿态插补算法

前言

姿态空间插值的一个主要问题是姿态空间的三个自由度是相互耦合的，而不像位置空间一样三个自由度完全解耦正交。所以，直接对欧拉角三个角度分别插值，可能会出现错误的结果。

姿态空间是一个 SO(3)群。当确定姿态空间的空间属性，我们就可以通过数学方法来定义它的空间插值。

Serp 插值可以认为是最短路径插值，类似于位置空间的直线插值。但直线(Slerp)插值只能保证一阶连续，过渡点处的角速度方向会发生突变。

Lerp插值

线性插值(Lerp/Linear Interpolation)，即沿着一条直线（也就是圆上的一个弦）进行插值，此种插值方式所得结果并非单位四元数(只有单位四元数才能表示旋转)。
${\mathrm{q}}_{\mathrm{t}}=\mathrm{Lerp}({\mathrm{q}}_0,{\mathrm{q}}_1,\mathrm{t})=(1-\mathrm{t}){\mathrm{q}}_0+\mathrm{t}{\mathrm{q}}_1$

Slerp插值

球面线性插值(SphericalLinearInterpolation)对每一对四元数使用Slerp插值，虽然能够保证每两个四元数之间的角速度是固定的，但是角速度会在切换插值的四元数时出现断点，或者说在切换点不可导．
Slerp球面线性插值是对角度本身进行线性插值，适用于插补角度不接近0度的情况。

Slerp插值公式：
$\mathrm{Slerp}(\mathrm{t};{\mathrm{q}}_0,{\mathrm{q}}_1)=\frac{{\mathrm{q}}_0\sin \left[(1-\mathrm{t})\theta \right]+{\mathrm{q}}_1\sin (\mathrm{t}\theta )}{\sin \theta }(\mathrm{t}\in [0,1])$

对于单位四元数：
$\mathrm{q}=\cos (\theta )+\hat{\mathrm{u}}\sin (\theta )$
其中$\hat{\mathrm{u}}$为单位三维矢量，$\hat{\mathrm{u}}\hat{\mathrm{u}}=-1$。

对于单位四元数，该公式可写为：
$\mathrm{Slerp}(\mathrm{t};{\mathrm{q}}_0,{\mathrm{q}}_1)={\mathrm{q}}_0({\mathrm{q}}_0^{-1}{\mathrm{q}}_1{)}^{\mathrm{t}}$

四元数有定理：
$\begin{array}{c}\exp (\hat{\mathrm{u}}\theta )=\cos (\theta )+\hat{\mathrm{u}}\sin (\theta )\\ {\mathrm{q}}^{\mathrm{t}}=(\cos (\theta )+\hat{\mathrm{u}}\sin (\theta ){)}^{\mathrm{t}}=\exp (\hat{\mathrm{u}}\theta \mathrm{t})\\ \log (\mathrm{q})=\log (\cos (\theta )+\hat{\mathrm{u}}\sin (\theta ))=\log (\exp (\hat{\mathrm{u}}\theta ))=\hat{\mathrm{u}}\theta \end{array}$

根据上述定理，可以推得：
$({\mathrm{q}}_0^{-1}{\mathrm{q}}_1{)}^{\mathrm{t}}=\exp (\hat{\mathrm{u}}\theta \mathrm{t})=\exp (\log ({\mathrm{q}}_0^{-1}{\mathrm{q}}_1)\ast \mathrm{t})$

Slerp插值可写为
$\mathrm{Slerp}(\mathrm{t};{\mathrm{q}}_0,{\mathrm{q}}_1)={\mathrm{q}}_0\exp (\log ({\mathrm{q}}_0^{-1}{\mathrm{q}}_1)\ast \mathrm{t})$

以下4个Slerp公式相等：

如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。为了解决这个问题，先测试点积的结果，当结果是负值时，将2个四元数的其中一个取反（并不会改变它代表的朝向）。而经过这一步操作，可以保证这个旋转走的是最短路径。

当$q_1$和$q_2$的夹角θ差非常小时会导致sinθ→0，这时除法可能会出现问题。为了避免这样的问题，当θ非常小时可以使用简单的线性插值代替（当θ → 0时，sinθ ≈ θ，因此方程退化为线性方程：slerp(p, q, t) = (1−t)p + tq

Nlerp正规化线性插值

正规化线性插值（Normalized LinearInterpolation)，是对线性插值的改进，即将线性插值除以其模长，将其转化为一个单位四元数。这种插补算法适用于插补角度接近0度的情况。
${\mathrm{q}}_{\mathrm{t}}=\mathrm{Lerp}({\mathrm{q}}_0,{\mathrm{q}}_1,\mathrm{t})=\Vert (1-\mathrm{t}$