三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping

序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：

1. 旋转矩阵和变换矩阵；
2. 旋转向量表示旋转；
3. 欧拉角表示旋转；
4. 四元数包括以下部分：
   4-1. 四元数表示变换；
   4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
   4-3. 四元数多点插值方法：Squad；
   4-4. 四元数多点连续解析解插值方法：Spicv；
   4-5. 四元数多点离散数值解插值方法：Sping。
5. 实践：SLAM中显示机器人运动轨迹及相机位姿。

摘要：四元数插值四个方法Slerp、Squad、Spicv和Sping既复杂又很重要，为了详细阐述，故每个方法独立成一篇博文讲解。没有插值的四元数是没有灵魂的，插值的重要性不言而喻。Slerp是经典的两点间一阶连续可导插值方法，Squad方法在Slerp的基础上实现多点间的一阶连续可导，Spicv是多点间连续解析解插值方法，而Sping则是多点间离散数值解插值方法，更适合复杂曲线，此博文也是国内首篇介绍Spicv和Sping的中文资料。

1. 正切曲率$\kappa (\gamma ,t)$在$H_1$上的离散数值解——Sping

由《四元数多点连续解析解插值方法：Spicv》可知：虽然连续解析解过程推导严密，但遗憾的是，我们不能给出最优插值曲线的解析表达式。因此，我们将尝试提出一个用数值方法来解决问题的离散数值解版本。

首先我们把这个问题写成等价的离散形式，然后尝试梯度下降解决这个新版本的问题。由于插值算法使用数值解并用到梯度，所以命名为Sping(Spherical Interpolation using Nmerical Gradient descent)。

注释：原论文将此算法命名为Spring，但这个名字太普遍，容易混淆，所以笔者将其精简为Sping，更容易记忆，也减少重名。

1.1 离散版解决方案

现在回顾一下曲率平方的积分$K(\gamma )$的概念：给定控制点$Q_1,...,Q_k\in H_1$，求$\gamma (t)\in C^2(I,H_1)$，使存在$t_1,...,t_k\in (I)$，满足$\gamma (t_i)=Q_i$，并使下列表达式最小化：$$K(\gamma )={\int }_{t_1}^{t_N}{\Vert \kappa (\gamma ,t)\Vert }^{2}dt\text{\hspace{1em}(1.1)}$$据此在离散版本中进行改写，我们将尝试解决以下问题：
给定控制点$Q_1,...,Q_k\in H_1$，寻找$q_1,...,q_N\in H_1(N\ge k)$，对于$t=1,...,k$有$q_{i_t}=Q_t$，并使下列表达式最小化：$$E=\sum _{i=1}^{N}l(q_i){\Vert \tilde{\kappa }(q_i)\Vert }^2\text{\hspace{1em}(1.2)}$$可见积分被求和代替了，我们积分区间的参数宽度称为$l(q_i)$，它可以表示为第$i$个四元数前后间隔内参数宽度的中值：$$l(q_i)=\frac{\Vert q_i-q_{i-1}\Vert +\Vert q_i-q_{i+1}\Vert }{2}\text{\hspace{1em}(1.3)}$$在$l(q_i)$的近似中，$q_i$和$q_{i-1}$之间的参数化距离的另一种度量方法是角度${\theta }_i$，它须满足$\cos {\theta }_i=q_i\cdot q_{i+1}$。
方程$(1.2)$中另一个参数$\tilde{\kappa }$，它表示局部曲率的离散版：$$\tilde{k}(q_i)=q_i^{{}^{\prime \prime }}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{q}_i\text{\hspace{1em}(1.4)}$$请注意，在定义局部曲率时，分母$q_i\cdot q_i$没有被忽略(如上篇中方程$(3.1)$)。这是因为插值的四元数不一定为单位四元数，此部分请参考第2.1节。因此分母的值一般不等于1，所以它不能被省略。

在方程$(1.4)$中，使用的是插值曲线的离散近似的二阶导数。二阶导数的一个很好的中心近似是¹：$$q_i^{{}^{\prime \prime }}=\frac{q_{i-1}-2q_i+q_{i+1}}{l(q_i{)}^2}\text{\hspace{1em}(1.5)}$$

1.2 梯度下降法求解

本节尝试求解方程$(1.2)$，它可以用梯度下降法最小化。一般来说，梯度下降法可以描述如下：将被最小化的函数图像(通常称为能量函数)视为有坡度的丘陵，其中的函数值是每组坐标上的丘陵高度，函数在某点的梯度表示它最陡的上坡方向。梯度下降是基于对解的初始估计，从最初的估计开始计算梯度，并在梯度的相反方向上迈出一小步(即下山)，从而产生一个新的点。这个过程在新的点上不断重复，直到函数值不再通过走一步变得更小，在这个过程中，梯度下降法产生一个近似的局部最小值。梯度下降理论的思想大致如此，不再做详细解释，但是我们将对推导方法进行足够详细的描述，以便那些在该领域没有前置知识的读者仍然能理解其推导过程。

当使用如梯度下降法的数值近似方法时，通常需要对解空间的进行限制；相反，如果解位于期望解空间之外，则会添加一个使解付出更大代价的附加项。据此我们寻找一个函数，它可以确定离散版本的插值曲线是否在$H_1$中，因此此函数是由单位四元数组成的，可通过下式实现：$$g(q)=q\cdot q-1\text{\hspace{1em}(1.6)}$$其中：$H_1=\{q\in H|g(q)=0\}$，即当$g(q)=0$时，$q$是单位四元数。确定四元数是否为单位四元数的方法，可以与能量函数$E$结合成新的能量函数$F$：$$F=\sum _{i=1}^{N}l(q_i){\Vert \widetilde{\kappa (q_i)}\Vert }^2+cg(q_i{)}^2\text{\hspace{1em}(1.7)}$$假设$c\in \mathbb{R}$为相匹配的实数，当$E$在某处取最小值时，能量函数$F$也近似有一个最小值，在这里所有的$q_i$为近似单位四元数。

我们想用梯度下降法找到方程$(1.7)$中$F$的最小值，因此我们必须求出梯度。下面，我们将$q_{i,x}$记为离散插值曲线第$i$个四元数中的第$x$坐标，其中$x\in 1,2,3,4$，将四元数视为四维向量，因此梯度是$4N$维的。$F$关于每个坐标的偏导可以写成：$$\frac{\partial F}{\partial q_{i,x}}=\frac{\partial }{\partial q_{i,x}}\left(\sum _{j=1}^{N}l(q_j){\Vert \tilde{\kappa }(q_j)\Vert }^2+cg(q_j{)}^2\right)$$在方程$(1.3),(1.4),(1.5)$中，$q_i$存在于算法中的$l(q_{i-1}),l(q_i),l(q_{i+1}),\tilde{\kappa }(q_{i-1}),\tilde{\kappa }(q_i),\tilde{\kappa }(q_{i+1})$，因此相应的项必然出现在偏导中，因此有：$$\begin{array}{rl}\frac{\partial F}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(l(q_{i-1}){\Vert \tilde{\kappa }(q_{i-1})\Vert }^2+l(q_i){\Vert \tilde{\kappa }(q_i)\Vert }^2+l(q_{i+1}){\Vert \tilde{\kappa }(q_{i+1})\Vert }^2+cg(q_i{)}^2\right)\\ & =\frac{\partial }{\partial q_{i,x}}\left(l(q_{i-1})\tilde{\kappa }(q_{i-1})\cdot \tilde{\kappa }(q_{i-1})+l(q_i)\tilde{\kappa }(q_i)\cdot \tilde{\kappa }(q_i)+l(q_{i+1})\tilde{\kappa }(q_{i+1})\cdot \tilde{\kappa }(q_{i+1})+cg(q_i{)}^2\right)\\ & =\frac{\partial l(q_{i-1})}{\partial q_{i,x}}\tilde{\kappa }(q_{i-1})\cdot \tilde{\kappa }(q_{i-1})+\frac{\partial l(q_i)}{\partial q_{i,x}}\tilde{\kappa }(q_i)\cdot \tilde{\kappa }(q_i)+\frac{\partial l(q_{i+1})}{\partial q_{i,x}}\tilde{\kappa }(q_{i+1})\cdot \tilde{\kappa }(q_{i+1})+\\ & 2l(q_{i-1})\tilde{\kappa }(q_{i-1})\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}+2l(q_i)\tilde{\kappa }(q_i)\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}}+2l(q_{i+1})\tilde{\kappa }(q_{i+1})\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}+\\ & 2cg(q_i)\frac{\partial g(q_i)}{\partial q_{i,x}}\end{array}\text{\hspace{1em}(1.8)}$$下面我们将推导方程$(1.8)$中子表达式的偏导数。
我们引入符号：$1_x$，它表示某个向量在第$x$坐标是1，在其他坐标是0。现在我们可以求出$g(q_i),l(q_{i-1}),l(q_i),l(q_{i+1}),q_{i-1}^{{}^{\prime \prime }},q_i^{{}^{\prime \prime }},q_{i+1}^{{}^{\prime \prime }}$以及$\tilde{\kappa }(q_{i-1}),\tilde{\kappa }(q_i),\tilde{\kappa }(q_{i+1})$的偏导了：$$\begin{array}{rl}\frac{\partial g(q_i)}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}(q_i\cdot q_i-1)\\ & =2q_i\cdot \frac{\partial q_i}{\partial q_{i,x}}=2q_{i,x}\end{array}\text{\hspace{1em}(1.9)}$$$$\begin{array}{rl}\frac{\partial l(q_{i-1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{\Vert q_{i-1}-q_{i-2}\Vert +\Vert q_{i-1}-q_i\Vert }{2}\\ & =\frac{1}{2}\frac{\partial }{\partial q_{i,x}}\left(\sqrt{(q_{i-1}-q_{i-2})\cdot (q_{i-1}-q_{i-2})}+\sqrt{(q_{i-1}-q_i)\cdot (q_{i-1}-q_i)}\right)\\ & =\frac{1}{2}\left(-\frac{1_x\cdot (q_{i-1}-q_i)}{\sqrt{(q_{i-1}-q_i)\cdot (q_{i-1}-q_i)}}\right)\\ & =\frac{1_x\cdot (q_i-q_{i-1})}{2\Vert q_{i-1}-q_i\Vert }\end{array}\text{\hspace{1em}(1.10)}$$$$\begin{array}{rl}\frac{\partial l(q_i)}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{\Vert q_i-q_{i-1}\Vert +\Vert q_i-q_{i+1}\Vert }{2}\\ & =\frac{1}{2}\frac{\partial }{\partial q_{i,x}}\left(\sqrt{(q_i-q_{i-1})\cdot (q_i-q_{i-1})}+\sqrt{(q_i-q_{i+1})\cdot (q_i-q_{i+1})}\right)\\ & =\frac{1}{2}\left(\frac{1_x\cdot (q_i-q_{i-1})}{\sqrt{(q_i-q_{i-1})\cdot (q_i-q_{i-1})}}+\frac{1_x\cdot (q_i-q_{i+1})}{\sqrt{(q_i-q_{i+1})\cdot (q_i-q_{i+1})}}\right)\\ & =\frac{1_x\cdot (q_i-q_{i-1})}{2\Vert q_i-q_{i-1}\Vert }+\frac{1_x\cdot (q_i-q_{i+1})}{2\Vert q_{i+1}-q_i\Vert }\end{array}\text{\hspace{1em}(1.11)}$$$$\begin{array}{rl}\frac{\partial l(q_{i+1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{\Vert q_{i+1}-q_i\Vert +\Vert q_{i+1}-q_{i+2}\Vert }{2}\\ & =\frac{1}{2}\frac{\partial }{\partial q_{i,x}}\left(\sqrt{(q_{i+1}-q_i)\cdot (q_{i+1}-q_i)}+\sqrt{(q_{i+1}-q_{i+2})\cdot (q_{i+1}-q_{i+2})}\right)\\ & =\frac{1}{2}\left(-\frac{1_x\cdot (q_{i+1}-q_i)}{\sqrt{(q_{i+1}-q_i)\cdot (q_{i+1}-q_i)}}\right)\\ & =\frac{1_x\cdot (q_i-q_{i+1})}{2\Vert q_{i+1}-q_i\Vert }\end{array}\text{\hspace{1em}(1.12)}$$$$\begin{array}{rl}\frac{\partial q_{i-1}^{{}^{\prime \prime }}}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{q_{i-2}-2q_{i-1}+q_i}{l(q_{i-1}{)}^2}\\ & =\frac{l(q_{i-1}{)}^2\frac{\partial }{\partial q_{i,x}}(q_{i-2}-2q_{i-1}+q_i)-(q_{i-2}-2q_{i-1}+q_i)\frac{\partial }{\partial q_{i,x}}l(q_{i-1}{)}^2}{l(q_{i-1}{)}^4}\\ & =\frac{l(q_{i-1}{)}^2\frac{\partial }{\partial q_{i,x}}{q}_i-(q_{i-2}-2q_{i-1}+q_i)2l(q_{i-1})\frac{\partial }{\partial q_{i,x}}l(q_{i-1})}{l(q_{i-1}{)}^4}\\ & =\frac{l(q_{i-1}{)}^2{1}_x-2(q_{i-2}-2q_{i-1}+q_i)l(q_{i-1})\frac{\partial }{\partial q_{i,x}}l(q_{i-1})}{l(q_{i-1}{)}^4}\end{array}\text{\hspace{1em}(1.13)}$$$$\begin{array}{rl}\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{q_{i-1}-2q_i+q_{i+1}}{l(q_i{)}^2}\\ & =\frac{l(q_i{)}^2\frac{\partial }{\partial q_{i,x}}(q_{i-1}-2q_i+q_{i+1})-(q_{i-1}-2q_i+q_{i+1})\frac{\partial }{\partial q_{i,x}}l(q_i{)}^2}{l(q_i{)}^4}\\ & =\frac{l(q_i{)}^2\frac{\partial }{\partial q_{i,x}}(-2q_i)-(q_{i-1}-2q_i+q_{i+1})2l(q_i)\frac{\partial }{\partial q_{i,x}}l(q_i)}{l(q_i{)}^4}\\ & =-\frac{2l(q_i{)}^2{1}_x+2(q_{i-1}-2q_i+q_{i+1})l(q_i)\frac{\partial }{\partial q_{i,x}}l(q_i)}{l(q_i{)}^4}\end{array}\text{\hspace{1em}(1.14)}$$$$\begin{array}{rl}\frac{\partial q_{i+1}^{{}^{\prime \prime }}}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\frac{q_i-2q_{i+1}+q_{i+2}}{l(q_{i+1}{)}^2}\\ & =\frac{l(q_{i+1}{)}^2\frac{\partial }{\partial q_{i,x}}(q_i-2q_{i+1}+q_{i+2})-(q_i-2q_{i+1}+q_{i+2})\frac{\partial }{\partial q_{i,x}}l(q_{i+1}{)}^2}{l(q_{i+1}{)}^4}\\ & =\frac{l(q_{i+1}{)}^2\frac{\partial }{\partial q_{i,x}}{q}_i-(q_i-2q_{i+1}+q_{i+2})2l(q_{i+1})\frac{\partial }{\partial q_{i,x}}l(q_{i+1})}{l(q_{i+1}{)}^4}\\ & =\frac{2l(q_{i+1}{)}^2{1}_x-2(q_i-2q_{i+1}+q_{i+2})l(q_{i+1})\frac{\partial }{\partial q_{i,x}}l(q_{i+1})}{l(q_{i+1}{)}^4}\end{array}\text{\hspace{1em}(1.15)}$$$$\begin{array}{rl}\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(q_{i-1}^{{}^{\prime \prime }}-\frac{q_{i-1}^{{}^{\prime \prime }}\cdot q_{i-1}}{q_{i-1}\cdot q_{i-1}}{q}_{i-1}\right)\\ & =\frac{\partial q_{i-1}^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{\frac{\partial q_{i-1}^{{}^{\prime \prime }}}{\partial q_{i,x}}{q}_{i-1}}{q_{i-1}\cdot q_{i-1}}{q}_{i-1}\end{array}\text{\hspace{1em}(1.16)}$$$$\begin{array}{rl}\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(q_i^{{}^{\prime \prime }}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{q}_i\right)\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}\frac{\partial q_i}{\partial q_{i,x}}-\frac{\partial }{\partial q_{i,x}}\left(\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\frac{\partial }{\partial q_{i,x}}\left(\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\left(\frac{\frac{\partial q_i^{{}^{\prime \prime }}\cdot q_i}{\partial q_{i,x}}{q}_i\cdot q_i-\frac{\partial q_i\cdot q_i}{\partial q_{i,x}}{q}_i^{{}^{\prime \prime }}\cdot q_i}{(q_i\cdot q_i{)}^2}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\left(\frac{\left(\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}\cdot q_i+q_i^{{}^{\prime \prime }}\cdot \frac{\partial q_i}{\partial q_{i,x}}\right)q_i\cdot q_i-2\left(q_i\cdot \frac{\partial q_i}{\partial q_{i,x}}\right)q_i^{{}^{\prime \prime }}\cdot q_i}{(q_i\cdot q_i{)}^2}\right)q_i\\ & =\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{q_i^{{}^{\prime \prime }}\cdot q_i}{q_i\cdot q_i}{1}_x-\left(\frac{\left(\frac{\partial q_i^{{}^{\prime \prime }}}{\partial q_{i,x}}\cdot q_i+q_i^{{}^{\prime \prime }}\cdot 1_x\right)q_i\cdot q_i-2\left(q_i\cdot 1_x\right)q_i^{{}^{\prime \prime }}\cdot q_i}{(q_i\cdot q_i{)}^2}\right)q_i\end{array}\text{\hspace{1em}(1.17)}$$$$\begin{array}{rl}\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}& =\frac{\partial }{\partial q_{i,x}}\left(q_{i+1}^{{}^{\prime \prime }}-\frac{q_{i+1}^{{}^{\prime \prime }}\cdot q_{i+1}}{q_{i+1}\cdot q_{i+1}}{q}_{i+1}\right)\\ & =\frac{\partial q_{i+1}^{{}^{\prime \prime }}}{\partial q_{i,x}}-\frac{\frac{\partial q_{i+1}^{{}^{\prime \prime }}}{\partial q_{i,x}}{q}_{i+1}}{q_{i+1}\cdot q_{i+1}}{q}_{i+1}\end{array}\text{\hspace{1em}(1.18)}$$
现在我们对梯度中的所有项都有了简单且长的表达式。我们回到梯度方程$(1.8)$，可看到其所有组成项均已导出，将方程$(1.4)$中$\tilde{\kappa }(q_{i-1}),\tilde{\kappa }(q_i),\tilde{\kappa }(q_{i+1})$的导出项和偏导数方程$(1.16),(1.17),(1.18)$中$\frac{\partial \tilde{\kappa }(q_{i-1})}{\partial q_{i,x}},\frac{\partial \tilde{\kappa }(q_i)}{\partial q_{i,x}},\frac{\partial \tilde{\kappa }(q_{i+1})}{\partial q_{i,x}}$的方程代入梯度方程$(1.8)$，即可显式计算出梯度。
下面我们看算法的具体步骤。

1.3 Sping算法步骤

上面我们推导出了Sping算法中梯度的显式离散表达式，现在我们可以根据梯度下降法给出Sping算法的详细步骤：

1. 初始化
   给出一个合理的初始估计值${\mathbf{q}}^0=(q_1,...,q_N)$。容易想到的是使用已介绍过的方法$Slerp$或$Squad$。初始估计值越好，方法收敛越快。
2. 迭代
   根据公式$(1.8)$的推导，计算出梯度：$$▽F=((\frac{\partial F}{\partial q_{1,1}},\frac{\partial F}{\partial q_{1,2}},\frac{\partial F}{\partial q_{1,3}},\frac{\partial F}{\partial q_{1,4}}),...,(\frac{\partial F}{\partial q_{N,1}},\frac{\partial F}{\partial q_{N,2}},\frac{\partial F}{\partial q_{N,3}},\frac{\partial F}{\partial q_{N,4}}))$$注：所有关键帧梯度初始估计值均设为零。
   对初始估计值：${\mathbf{q}}^{\mathbf{i}+1}={\mathbf{q}}^{\mathbf{i}}-e▽F$进行迭代，其中$e$定义为步长。
   或者:在初始估计值上执行迭代：${\mathbf{q}}^{\mathbf{i}+1}={\mathbf{q}}^{\mathbf{i}}-e\frac{▽F}{\Vert ▽F\Vert }$。该方法可以提供更大的数值稳定性。
3. 终止条件
   重复第二步，直到达到预定的终止条件。终止条件可以使用迭代次数，总能量$F$，与控制点数量有关的能量$F$，梯度的大小，或者更高级的策略。

这就是算法的基本步骤，下面我们看一下算法中对范数的改进及算法的扩展。

2. 优化策略——计算Sping插值范数的替代方法及算法扩展

2.1 计算Sping插值范数的三种替代方法

如上所述，我们希望插值曲线位于单位四元数的$H_1$空间，这是通过使用方程$(1.7)$中的惩罚函数$g(q_i)$做到的。但是很难确定方程$(1.7)$中惩罚函数的权重因子$c\in \mathbb{R}$ 多大才能与$E$“相匹配”。理论上，$c$必须是无穷大，才能保证$q_i\in H_1$，有几种方法可以从不同角度解决这个问题，下面分别介绍：

1. 投影
   在解空间内和解空间外的点之间有一个简单且定义明确的连接，即投影。如前所述，通过原点的直线上的所有四元数都执行相同的旋转。因此，通过对生成的四元数进行归一化，可以简单地将解估计值投影到解空间中。这可以在每次迭代中执行一次投影，也可以在最后一次迭代之后再投影。
2. 拉格朗日算子²
   惩罚函数$g(q_i)$也可以用拉格朗日算子${\lambda }_i$来引入，方法如下：$$F=E+\sum _{i=1}^N{\lambda }_{i}g(q_i)$$方程$(1.1)$的解是$F$的一个奇异点。然而奇异点并不是最小值，而是一个鞍点。因此梯度下降法对于此组合四元数$F$仍然适用，需要注意的是梯度上升必须在辅助变量${\lambda }_i$上执行。
   通过在方程$(1.7)$中加入惩罚函数和拉格朗日算法，该方法变得更具有数值鲁棒性，收敛速度更快。该方法的细节可以在附录中的[Platt & Barr, 1988]和[Barr et al.,1992]中找到。
3. 极坐标
   在上述的求解方法中，我们重新表述了问题，使解空间上的限制被整合到最小化的能量函数中（加入$g(q_i)$）。我们还可以重新定义对解空间的限制，降低最小化表达式的复杂度（投影）。这也可以通过用极坐标来表示四元数实现，此时四元数被写为：$q=[r,(\rho ,\theta ,\varphi )]$，其中$r$是半径，$(\rho ,\theta ,\varphi )$是三个必要的旋转角度。
   当使用这种表示方式时，可以很容易地保持在解空间的限制内$H_1$上求解。这可以通过不对半径坐标$r$执行梯度下降来实现，使该坐标保持恒定值1，这样就保持了解空间的限制。

从理论上看，我们认为对生成的四元数进行归一化是“作弊”行为，但也不是不可用。在上述表示中，极坐标将确保插值曲线停留在单位四元数的$H_1$空间中；使用拉格朗日算子将确保一个更鲁棒和更快的收敛算法。为了使算法尽可能简单，我们不会再进一步探索上述可能性，即我们将不再进一步讨论这三种替代方法中的任何一种。

2.2 角速度惩罚项

一般认为，梯度下降法是一种很容易推广的方法。但是对于插值曲线的预期性质，必须可以简单地描述为某些函数的零交叉或恒定状态。例如，我们可能想要确保整个插值曲线的角速度是恒定的，这可以使用另一个惩罚函数得到：$$w(q_i)=\Vert q_{i-1}-q_i\Vert -\Vert q_i-q_{i+1}\Vert \text{\hspace{1em}(2.1)}$$可以看到，惩罚函数$w(q_i)$随上步长$q_{i-1}$与下步长$q_{i+1}$之差而增加。该惩罚项可以简单地引入到算法中，比如将其引入到方程$(1.7)$的原始能量函数中： F = ∑ i = 1 N l ( q i ) ∥ κ ( q i ) ~ ∥ 2 + c g g ( q i ) 2 + c w w ( q i ) 2 (2.2)