三维空间刚体运动4-4：四元数多点连续解析解插值方法：Spicv

序：本篇系列文章参照高翔老师《视觉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. 总述：多点旋转插值的数学方法

到目前为止，插值方法都相当简单，并且基于旋转表示。在原理上，插值方法与旋转形式无关，相反，我们所求的最优插值曲线应该根据旋转空间中所需要的特性来定义。当然，这条最优曲线需用算法编写，并可用任何合理的旋转形式表示。

比如某两个旋转间的插值：最优插值曲线等同于旋转空间中的一条直线，这条最优插值曲线可以在算法上使用欧拉角编写，也可以使用四元数形式的Slerp，而Slerp的优势是表述更简洁，因为四元数无冗余数据，而旋转空间和单位四元数球体之间的表示具有 等价性。

因此，我们将基于旋转空间讨论插值问题，我们将给出基于数学的最优插值曲线的基本要求，目标是为了给出最优插值曲线算法描述，并据此找出符合条件的插值算法，并用四元数形式表示。

2. 插值曲线及其连续性

2.1 插值曲线定义

我们将旋转间的插值定义在 特殊正交群$SO(3)$的旋转空间中，但是如前所述，$SO(3)$与$H_1$是拓扑等价的，因此我们选择在单位四元数空间中定义一般的插值方法：

定义：对于给定的$k$个控制点$q_i\in H_1$和对应的集合$I=[t_1,...,t_k]$，有插值曲线$(\gamma (t):I)\propto H_1$，对于$t_i\in I$，有$\gamma (t_i)\equiv q_i$，同时要求$t_1\le t_2,...,t_{k-1}\le t_k$。

2.2 插值曲线连续性的讨论

我们通常希望插值曲线是“漂亮”的，这个模糊的术语在微分几何中通常意味着平滑。然而，平滑存在几种不同的定义，列举如下：

使$\gamma (t)$是某曲线在$C^n(I,R^n)$上的参数化表示，平滑性可被定义为下列方式：
（1）¹：如果$\gamma (t)\in C^1$，并且$\forall t\in I:{\gamma }^{{}^{\prime }}(t)\ne 0$，那么曲线$\gamma (t)$是平滑的。

（2）²：如果$\gamma (t)\in C^2$，那么曲线$\gamma (t)$是平滑的。

（3）³：如果$\gamma (t)\in C^{\infty }$，那么曲线$\gamma (t)$是平滑的。

以上不同定义对于“漂亮”的表述并不明朗，我们还需要进一步检查平滑性到底需要插值曲线的哪些性质，如下图所示：

上图展示了三个平面上控制点间的插值曲线，其中：a)非连续插值曲线；b)连续插值曲线；c)$C^1$连续插值曲线；d)$C^2$连续插值曲线。

很明显，我们希望曲线必须是连续且可微的，不希望在动态曲线上出现“漏洞”或“中断”，因此我们要求插值曲线至少必须是$C^1$。至于是否需要$C^2$甚至$C^{\infty }$暂不确定。

由于控制点是对称的（双倍覆盖），所以很自然地期望插值曲线也是对称的。从之前平面上展示的曲线明确表明：我们至少需要$C^2$而不是$C^1$，然而平面展示的曲线并不是采用$C^2$的充分条件，因此目前采用几阶连续并不明朗，我们将这一决定推迟到下一篇Sping中。

在$\gamma (t)$平滑性的第一条定义中要求${\gamma }^{{}^{\prime }}(t)\ne 0$，这意味着插值函数不允许包含奇点。现在看来，这似乎是一个合理的要求。然而，某些情况也有可能对插值曲线的速度提出合理但矛盾的要求。例如，考虑一个钟摆的动画，控制点(它们定义钟摆摆动的角度)将位于旋转空间的直线上。然而在端点处，钟摆应该没有速度，这与插值函数中的奇点相对应。因此，我们也将是否允许包含奇点这一推论推迟到下一篇Sping中。

关于平滑性的讨论并没有给我们带来太多的启发。上面的定义甚至无法让我们区分LinEuler、Lerp和Slerp，这些曲线都是$C^2$（但在控制点处是$C^0$）。因此，仅仅描述插值曲线应该属于哪一类连续函数是不够的。

3. 最优插值曲线

3.1 最优插值曲线的引出

我们关于平滑性的考量对插值曲线的定义并没有给出明确的需求，但是我们需要一个客观明确的指标来衡量我们的曲线在旋转空间中的“漂亮”程度。

我们将再次在平面中曲线寻找灵感(见下图)。如前所述，三次曲线通常用于在一系列点之间的插值。三次曲线有很多种，在《三维空间刚体运动4-3：四元数线性插值方法：Squad》中描述的贝塞尔曲线就是一个例子，而最常见的一类曲线是样条曲线。

上图表示平面中的样条插值：a)简单线性插值；b)样条插值曲线。

讨论样条曲线，我们必须像其他严谨的项目使用样条曲线那样，写一点关于造船者的故事。传统上，当造船者想要决定如何塑造一艘船的弯曲部分时，会使用一块有弹性的金属，这块金属被固定在一组铆钉之间。然后，金属部件与铆钉相适应，形成一个有张力的优美柔和的曲线，造船者称这个金属部件为样条⁴。

从物理上看，上面对样条的描述对应于金属片在铆钉约束条件下，达到最小的内部张力的形态。从数学角度讲，金属件使曲率（curvature）最小，从而使曲线更平滑。曲率是指一条（平面上的）曲线在特定点弯曲的程度，它正比于曲线转过的角度，反比于曲线的长度⁵。

在平面中，这个曲线的要求可以描述如下：
给定控制点$(x_i,y_i)\in {\mathbb{R}}^2$，定义曲线$\gamma (t)\in C^2(I,{\mathbb{R}}^2)$，其中$t$是自然参数（如曲线长度或0-1之间的进度比等），并且使$\gamma (t)$经过控制点，同时使表达式$${\int }_I{\Vert {\gamma }^{{}^{\prime \prime }}(t)\Vert }^{2}dt$$最小化，此时曲率平方值最小。

这个简单的公式从微分几何中的一般概念出发，给出了插值曲线的明确定义。因此，我们选择将曲率(或曲率平方)最小的曲线视为最优插值曲线。下面我们将看到，在$H_1$中计算这条曲线并不像在平面上那么简单。

3.2 最优插值曲线在$H_1$的曲率

插值曲线位于四元数空间的超球面$H_1$上，假设$t$为自然参数，那么曲线$\gamma (t)$的曲率通常被定义为$\Vert {\gamma }^{{}^{\prime \prime }}(t)\Vert$。

这里解释一下，上文关于曲率的资料中，具有二阶可导函数的曲线在某点曲率被定义为：$$K(t)=\frac{\Vert {\gamma }^{{}^{\prime \prime }}(t)\Vert }{(1+{\gamma }^{{}^{\prime }}(t{)}^2)\frac{3}{2}}\text{\hspace{1em}(3.1)}$$然而在本篇论文中，曲率的分母被省略，这是因为假设t为自然参数。注意，这里$t$不一定是自然参数，所以上述定义在严格的微分几何意义上是不正确的。当t为自然参数时，参数化的曲线曲率可写作$\kappa (r,t)=\Vert {\gamma }^{{}^{\prime \prime }}(t)\Vert$，而当t为连续值时，对于光滑曲线$\gamma (t)$，微分几何上曲线曲率的正确表达式为：$$\kappa (\gamma ,t)=\Vert \frac{{\gamma }^{{}^{\prime \prime }}(t)}{{\Vert {\gamma }^{{}^{\prime }}(t)\Vert }^2}-\frac{{\gamma }^{{}^{\prime }}(t)({\gamma }^{{}^{\prime \prime }}(t)\cdot {\gamma }^{{}^{\prime }}(t))}{{\Vert {\gamma }^{{}^{\prime }}(t)\Vert }^4}\Vert$$但是这里我们只使用t为自然数的情形。

插值曲线Slerp在四元数单位圆上产生 大弧线，$H_1$上的圆弧在切平面上的投影是一条直线，因此，可以预料大弧线的投影是没有曲率的。如果用$\Vert {\gamma }^{{}^{\prime \prime }}(t)\Vert$来计算曲率，曲率将不是0而是1，即单位球的曲率。因此，我们需要计算的是相对于四元数单位球面的曲率（即大弧线），而不是相对于四元数空间（即直线），我们称这个曲率为局部曲率。

曲面上某条曲线的局部曲率定义是基于微分几何的，曲线中某点的局部曲率可以定义如下：给定曲线上的点，过点做关于曲线的切平面，将一个坐标系统(地图)放置在切平面上， 曲线某点的局部曲率就是曲线投影到坐标系统上的曲线的曲率，这也叫做正切曲率。正切曲率越小，说明越接近测地线，

在微分几何中，大弧线(直线)称为测地线。测地线投影到切面上，就成了一条直线。因此，正如预期的那样，我们看到大弧线没有任何局部弯曲。下面我们开始分析正切曲率，如下图所示：

上图表示曲线$\gamma (t)$的正切向量${\gamma }^{{}^{\prime }}(t)$和正切曲率${\gamma }^{{}^{\prime \prime }}(t)$在平面的分量：a)表示位于四元数单位球面上曲线的 位置向量$\gamma (t)$在正切平面上的投影为${\gamma }^{{}^{\prime }}(t)$；b)表示将向量${\gamma }^{{}^{\prime \prime }}(t)$分解为在正切平面的分量${\gamma }_t^{{}^{\prime \prime }}(t)$和平行于位置矢量的分量${\gamma }_o^{{}^{\prime \prime }}(t)$。

由上图可知，如果曲线$\gamma (t)$位于$H_1$的表面，我们可以将${\gamma }^{{}^{\prime \prime }}(t)$分解为两个部分${\gamma }_t^{{}^{\prime \prime }}(t)$、${\gamma }_o^{{}^{\prime \prime }}(t)$。期望的曲率部分是${\gamma }_t^{{}^{\prime \prime }}(t)$，因为它表示${\gamma }^{{}^{\prime \prime }}(t)$的弯曲程度，它越小，$\gamma (t)$越接近测地线。因此，曲线$\gamma (t)$的局部曲率$\kappa$可以表示为：$$\kappa (r,t)=\Vert {\gamma }_t^{{}^{\prime \prime }}(t)\Vert =\Vert {\gamma }^{{}^{\prime \prime }}(t)-{\gamma }_o^{{}^{\prime \prime }}(t)\Vert$$由于曲线$\gamma (t)$位于$H_1$的表面，${\gamma }_o^{{}^{\prime \prime }}(t)$平行于$\gamma (t)$，因此${\gamma }_o^{{}^{\prime \prime }}(t)$可以表示为${\gamma }^{{}^{\prime \prime }}(t)$到$\gamma (t)$的投影，根据 投影公式有：

$$\begin{array}{rl}{\gamma }_t^{{}^{\prime \prime }}(t)& ={\gamma }^{{}^{\prime \prime }}(t)-{\gamma }_o^{{}^{\prime \prime }}(t)\\ & ={\gamma }^{{}^{\prime \prime }}(t)-\left({\gamma }^{{}^{\prime \prime }}(t)\cdot \frac{\gamma (t)}{\Vert \gamma (t)\Vert }\right)\frac{\gamma (t)}{\Vert \gamma (t)\Vert }\\ & ={\gamma }^{{}^{\prime \prime }}(t)-({\gamma }^{{}^{\prime \prime }}(t)\cdot \gamma (t))\gamma (t)\end{array}$$由此给出局部曲率$\kappa$的定义：给定$\gamma (t)\in C^2(I,H_1)$，局部曲率$\kappa (\gamma ,t)$被定义为：$$\kappa (\gamma ,t)=\Vert {\gamma }^{{}^{\prime \prime }}(t)-({\gamma }^{{}^{\prime \prime }}(t)\cdot \gamma (t))\gamma (t)\Vert \text{\hspace{1em}(3.2)}$$然而，我们感兴趣的不仅仅是插值曲线的形状(曲率)。我们还想要一个“漂亮的”角速度函数，即使角加速度最小化的函数(从物理角度来看，对应的是使能量最小化)。角速度和角加速度分别对应${\gamma }^{{}^{\prime }}(t)$和${\gamma }^{{}^{\prime \prime }}(t)$，在表达式3.2中角加速度是自动包括在内的，因为我们没有重新参数化自然参数，因此我们可以使用表达式3.2来计算曲率。

下面将基于这个结论，分别介绍正切曲率$\kappa (\gamma ,t)$在$H_1$上的连续解析解Spicv（第4章）、连续半解析解（第5章）和离散数字化解Sping（篇幅原因，留至下篇）。

4. 正切曲率$\kappa (\gamma ,t)$在$H_1$上的连续解析解——Spicv

4.1 函数$K(\gamma )$及变分法

我们将$H_1$中的最优插值曲线定义为曲率平方和最小的曲线，但又必须通过控制点的限制，然后再定义$H_1$中曲率的相关表达式。因此，我们得到问题的公式函数$K(\gamma )$如下：

给定控制点$q_1,...,q_N\in H_1$，求$\gamma (t)\in C^2(I,H_1)$，使存在$t_1,...,t_N\in (I)$，满足$\gamma (t_i)=q_i$，并使下列表达式最小化：$$K(\gamma )={\int }_{t_1}^{t_N}{\Vert \kappa (\gamma ,t)\Vert }^{2}dt\text{\hspace{1em}(4.1)}$$这里将求以$\gamma$为变量的曲率问题$\kappa (\gamma ,t)$转换为曲率平方的积分问题$K(\gamma )$，同样以函数$\gamma$为变量。这里$K(\gamma )$求的是整条曲线各个点的曲率平方之和，当它越小时，转弯就越平滑，所耗费的动能也就越少，符合导航线路的基本要求。

使函数的积分最小化的问题叫做变分演算问题，又称变分法(Calculus of Variations)⁶。在这里应用变分法只要记住，求极值应用到的必要条件是一阶导数为零。因此这个方法命名为Spicv(Spherical Interpolation using Calculus of Variations)。

下面，我们概述变分学中解决问题的基本方法。积分函数$K(\gamma )$达到最小值的一个必要条件是$K^{{}^{\prime }}(\gamma )=0$，对于$\delta \in \mathbb{R}$和$\psi \in C^k(I,H_1)$，根据导数定义：$$\underset{\delta \to 0}{\lim }\frac{K(\gamma +\delta \psi )-K(\gamma )}{\delta }=0\text{\hspace{1em}(4.2)}$$上式中，函数$\delta \psi$称为$\gamma$的变量函数，函数$\gamma +\delta \psi$称为比较函数。满足边界条件的函数$\gamma (t)\in C^2(I,H_1)$称为容许函数（admissible function）。这样，我们就得到了关于$\gamma$的条件函数。

4.2 求解$\gamma$

现在我们推导变分问题的解必须满足的要求。因此，我们假定解$\gamma$和比较函数$\gamma +\delta \psi$都是可容许或称可接受的，并且$K(\gamma )$是最小的。

比较函数$\gamma +\delta \psi$若要被容许，前提必须是：$$\psi (t_1)=\psi (t_2)=...=\psi (t_N)=0\text{\hspace{1em}(4.3)}$$此外,由于$\gamma$和$\gamma +\delta \psi$在单位球面上，我们有$\Vert \gamma \Vert =1$和$\Vert \gamma +\delta \psi \Vert =1$。在下面的推导中，可以忽略表达式成分中包含四元数函数这一情况，这是因为没有使用四元数乘法(只使用标量乘积)。

因此，交换性没有问题。因此我们有：$$\begin{array}{rl}1& ={\Vert \gamma +\delta \psi \Vert }^2\\ & ={\Vert \gamma \Vert }^2+{\delta }^2\Vert \psi \Vert +2\delta (\gamma \cdot \psi )\\ & =1+\delta (\delta \Vert \psi \Vert +2\gamma \cdot \psi )\end{array}$$$$\Rightarrow \gamma \cdot \psi =-\frac{\delta }{2}{\Vert \psi \Vert }^2\text{\hspace{1em}(4.4)}$$我们想用关于"导数"的知识来检验$K(\gamma +\delta \psi )-K(\gamma )$：$$\begin{array}{rl}K(\gamma +\delta \psi )-K(\gamma )& ={\int }_{t_1}^{t_N}{\Vert \kappa (\gamma +\delta \psi ,t)\Vert }^2-\Vert \kappa (\gamma ,t)\Vert dt\\ & ={\int }_{t_1}^{t_N}{\Vert (\gamma +\delta \psi {)}^{{}^{\prime \prime }}-((\gamma +\delta \psi {)}^{{}^{\prime \prime }}\cdot (\gamma +\delta \psi ))(\gamma +\delta \psi )\Vert }^2-{\Vert {\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma \Vert }^{2}dt\\ & ={\int }_{t_1}^{t_N}{\Vert {\gamma }^{{}^{\prime \prime }}+\delta {\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma +\delta {\gamma }^{{}^{\prime \prime }}\cdot \psi +\delta \gamma {\psi }^{{}^{\prime \prime }}+{\delta }^2{\psi }^{{}^{\prime \prime }}\cdot \psi )(\gamma +\delta \psi )\Vert }^2-{\Vert {\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma \Vert }^{2}dt\\ & ={\int }_{t_1}^{t_N}{\Vert [{\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ]+\delta [{\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\psi -({\gamma }^{{}^{\prime \prime }}\cdot \psi )\gamma -({\psi }^{{}^{\prime \prime }}\cdot \gamma )\gamma )]+{\delta }^2[...]+{\delta }^3[...]\Vert }^2\\ & -{\Vert {\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma \Vert }^{2}dt\end{array}$$在上述表达式中，我们根据$\delta$的指数收集了多项式。上述推导的目的是求导数。$K(\gamma +\delta \psi )-K(\gamma )$的展开式中，被${\delta }^2$和${\delta }^3$乘的多项式可以被删除，因为在检查极限时，除以$\delta$后，当$\delta$无限接近零时它们就消失了。因此，上面表达式可被写为：$$\begin{array}{rl}K(\gamma +\delta \psi )-K(\gamma )& ={\int }_{t_1}^{t_N}{\Vert A+\delta B\Vert }^2-{\Vert A\Vert }^{2}dt\\ & ={\int }_{t_1}^{t_N}{\delta }^2{\Vert B\Vert }^2+2\delta A\cdot Bdt\end{array}$$这里$A={\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ,B={\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\psi -({\gamma }^{{}^{\prime \prime }}\cdot \psi )\gamma -({\psi }^{{}^{\prime \prime }}\cdot \gamma )\gamma )$。同样省略被${\delta }^2$乘的项，重写表达式为：$$\begin{array}{rl}K(\gamma +\delta \psi )-K(\gamma )& ={\int }_{t_1}^{t_N}2\delta [{\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ][{\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\psi -({\gamma }^{{}^{\prime \prime }}\cdot \psi )\gamma -({\psi }^{{}^{\prime \prime }}\cdot \gamma )\gamma )]dt\\ & =2\delta {\int }_{t_1}^{t_N}{\gamma }^{{}^{\prime \prime }}\cdot {\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )-({\gamma }^{{}^{\prime \prime }}\cdot \psi )({\gamma }^{{}^{\prime \prime }}\cdot \gamma )-(\gamma \cdot {\psi }^{{}^{\prime \prime }})({\gamma }^{{}^{\prime \prime }}\cdot \gamma )-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )(\gamma \cdot {\psi }^{{}^{\prime \prime }})\\ & +({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \gamma )(\gamma \cdot \psi )+({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )(\gamma \cdot \gamma )+({\gamma }^{{}^{\prime \prime }}\cdot \gamma )(\gamma \cdot {\psi }^{{}^{\prime \prime }})(\gamma \cdot \gamma )dt\end{array}$$由于$\gamma$在四元数单位圆$H_1$上，因此$\gamma \cdot \gamma ={\Vert \gamma \Vert }^2=1$，同时将公式（4.4）的结论$\gamma \cdot \psi =-\frac{\delta }{2}{\Vert \psi \Vert }^2$代入上述表达式中，我们得到：$$\begin{array}{rl}K(\gamma +\delta \psi )-K(\gamma )& =2\delta {\int }_{t_1}^{t_N}{\gamma }^{{}^{\prime \prime }}\cdot {\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi +\gamma \cdot {\psi }^{{}^{\prime \prime }})+({\gamma }^{{}^{\prime \prime }}\cdot \gamma {)}^2(-\frac{\delta }{2}{\Vert \psi \Vert }^2)dt\\ & =2\delta {\int }_{t_1}^{t_N}{\gamma }^{{}^{\prime \prime }}\cdot {\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi +\gamma \cdot {\psi }^{{}^{\prime \prime }})dt\end{array}$$同样省略被${\delta }^2$乘的项。经过多次改写，我们将$\delta$分离单独作为一个因素。当除以$\delta$时，这个因子将消失在导数的表达式中。因此，我们现在想分离出包含$\psi$的项，使用分部积分和公式（4.3）的结论，表达式被重写如下：$$\begin{array}{rl}K(\gamma +\delta \psi )-K(\gamma )& =2\delta \sum _{i=1}^{N-1}{L}_i\end{array}$$其中$$\begin{array}{rl}{L}_i& ={\int }_{t_i}^{t_{i+1}}{\gamma }^{{}^{\prime \prime }}\cdot {\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi +\gamma \cdot {\psi }^{{}^{\prime \prime }})dt\\ & ={\int }_{t_i}^{t_{i+1}}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )dt\\ & =[({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime }}{]}_{t_i}^{t_{i+1}}-{\int }_{t_i}^{t_{i+1}}(\frac{d}{dt}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ))\cdot {\psi }^{{}^{\prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )dt\\ & =[({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime }}{]}_{t_i}^{t_{i+1}}-[(\frac{d}{dt}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ))\cdot \psi {]}_{t_i}^{t_{i+1}}\\ & +{\int }_{t_i}^{t_{i+1}}(\frac{d^2}{d{t}^2}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ))\cdot \psi -({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )dt\end{array}$$注意，上面的表达式要求它是四阶可微的。现在我们可以使用$\psi$在所有控制点上为零的公式（4.3），可以看到第二项为零，于是得到:$$\begin{array}{rl}{L}_i& =[({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime }}{]}_{t_i}^{t_{i+1}}+{\int }_{t_i}^{t_{i+1}}(\frac{d^2}{d{t}^2}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ))\cdot \psi -({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )dt\end{array}$$再次考虑整个表达式：$$\begin{array}{rl}K(\gamma +\delta \psi )-K(\gamma )& =2\delta \sum _{i=1}^{N-1}{L}_i\\ & =2\delta \sum _{i=1}^{N-1}[({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime }}{]}_{t_i}^{t_{i+1}}+2\delta \sum _{i=1}^{N-1}{\int }_{t_i}^{t_{i+1}}(\frac{d^2}{d{t}^2}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma ))\cdot \psi -({\gamma }^{{}^{\prime \prime }}\cdot \gamma )({\gamma }^{{}^{\prime \prime }}\cdot \psi )dt\\ & =2\delta [({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime }}{]}_{t_1}^{t_N}+2\delta \sum _{i=1}^{N-1}{\int }_{t_i}^{t_{i+1}}(\frac{d^2}{d{t}^2}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )-({\gamma }^{{}^{\prime \prime }}\cdot \gamma ){\gamma }^{{}^{\prime \prime }})\cdot \psi dt\end{array}$$利用控制点的连续性进行最后改写，我们可以求出“导数”：$$\begin{array}{rl}\underset{\delta \to 0}{\lim }\frac{K(\gamma +\delta \psi )-K(\gamma )}{\delta }& =2[({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\cdot {\psi }^{{}^{\prime }}{]}_{t_1}^{t_N}\\ & +2\sum _{i=1}^{N-1}{\int }_{t_i}^{t_{i+1}}(\frac{d^2}{d{t}^2}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )-({\gamma }^{{}^{\prime \prime }}\cdot \gamma ){\gamma }^{{}^{\prime \prime }})\cdot \psi dt\end{array}\text{\hspace{1em}(4.5)}$$由于$\gamma \cdot \gamma =\Vert \gamma \Vert =1$，所以有：$$0=(1{)}^{{}^{\prime \prime }}=(\gamma \cdot \gamma {)}^{{}^{\prime \prime }}=2{\gamma }^{{}^{\prime \prime }}\cdot \gamma +2{\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}\text{\hspace{1em}(4.6)}$$由于在控制点处对任意$\psi$的导数必须为零（方程4.3），结合方程4.6，我们对解有以下要求：$$\begin{array}{rl}\gamma & \in C^4((I),H_1)\\ 0& ={\gamma }^{{}^{\prime \prime }}(t_1)-({\gamma }^{{}^{\prime \prime }}(t_1)\cdot \gamma (t_1))\gamma (t_1)\\ 0& ={\gamma }^{{}^{\prime \prime }}(t_N)-({\gamma }^{{}^{\prime \prime }}(t_N)\cdot \gamma (t_N))\gamma (t_N)\\ ({\gamma }^{{}^{\prime \prime }}\cdot \gamma ){\gamma }^{{}^{\prime \prime }}& =\frac{d^2}{d{t}^2}({\gamma }^{{}^{\prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma )\gamma )\\ & ={\gamma }^{{}^{\prime \prime \prime \prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma {)}^{{}^{\prime \prime }}\gamma -2({\gamma }^{{}^{\prime \prime }}\cdot \gamma {)}^{{}^{\prime }}{\gamma }^{{}^{\prime }}-({\gamma }^{{}^{\prime \prime }}\cdot \gamma ){\gamma }^{{}^{\prime \prime }}\\ \Rightarrow 0& ={\gamma }^{{}^{\prime \prime \prime \prime }}+({\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}{)}^{{}^{\prime \prime }}\gamma +2({\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}{)}^{{}^{\prime }}{\gamma }^{{}^{\prime }}+2({\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}){\gamma }^{{}^{\prime \prime }}\end{array}$$其中第二个和第三个要求等价于端点处的局部曲率为零。

综上所述，现在我们可以找到一组微分方程，当它有解时就能找到理想的最优插值曲线的解析解：
给定控制点$q_1,...,q_N\in H_1$，插值曲线$\gamma \in C^4(I,H_1)$，对于$t_i\in I$有$\gamma (t_i)=q_i$，当$\gamma$满足以下要求时，将使${\int }_{t_1}^{t_N}{\Vert \kappa (h)\Vert }^{2}dt$最小化：

1. $\kappa (t_1)=0$
2. $\kappa (t_N)=0$
3. 对于每个间隔$t_i<t<t_{i+1}$有：$${\gamma }^{{}^{\prime \prime \prime \prime }}+({\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}{)}^{{}^{\prime \prime }}\gamma +2({\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}{)}^{{}^{\prime }}{\gamma }^{{}^{\prime }}+2({\gamma }^{{}^{\prime }}\cdot {\gamma }^{{}^{\prime }}){\gamma }^{{}^{\prime \prime }}=0$$

现在“所有”剩下的工作就是用给定的边界值来解决上述四阶函数微分方程。不幸的是，这在现有的数学知识中是不可能的，也就是说，这个四阶函数微分方程无解，这由Jorgen Sand（哥本哈根大学计算机科学学院系统方程求解专业副教授）和Gerd Grubb（哥本哈根大学数学学院微分方程专业教授）确认过。

5. 正切曲率$\kappa (\gamma ,t)$在$H_1$上的连续半解析解

上边是理想最优插值曲线在不可解微分方程上的严格数学推导。然而，我们可以再次在平面中寻找灵感：假设对应的微分方程是${\gamma }^{{}^{\prime \prime \prime \prime }}$=0，即曲线的四阶导数在控制点之间必须为零，这对应于三阶曲线(样条)。如果解被约束为三阶曲线，则方程组求解起来会比较容易。

因此，我们可以限制所求插值曲线属于哪一类函数，例如，这可以是一种三次样条。在此限制下，优化问题得以解决。

根据曲线类定义的选择，这种寻找解决方案的策略将给出与$Squad$相类似的结果。为了使此系列博客保持在可控制范围内，我们将不再进一步探讨连续半解析解方法。

本文总结：本节虽未找到具体解，但重点是引入了最优插值的求解理念，并给出连续解析解的详细证明过程，然后限于篇幅，只是对连续半解析解进行介绍，感兴趣的童鞋可以自行求证。Spicv虽然限于当前数学水平没有求出解，但是打开了我们探索解决未知问题的思路，同时为Sping做铺垫。下一篇将介绍最优插值曲线的离散数值解Sping的公式、求解和图像演示。

本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。

参考文献：

1. 《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. Quaternions, Interpolation and Animation
3. 变分法入门介绍
4. 什么是曲率和曲率半径？
5. [Madsen, 1991] Tage Gutmann Madsen. Course notes for Mathematics 1MA (calculus). Matematisk Notetryk, Institute of Mathematics, University of Copenhagen, Denmark, Copenhagen, 1991.
6. [Schwarz, 1989] H. R. Schwarz. Numerical Analysis, A Comprehensive Introduction. John Wiley & Sons, Chicester, 1989.
7. [Jakobsen, 1993] Hans Plesner Jakobsen. Course notes for Mathematics 3GE (differential geometry). Matematisk Notetryk, Institute of Mathematics, University of Copenhagen, Denmark, Copenhagen, 1993.

---

1. 请参考 [Madsen, 1991]。 ↩︎

2. 请参考[Schwarz, 1989]。 ↩︎

3. 请参考[Jakobsen, 1993]。 ↩︎

4. 关于样条详细知识也请参考《三维空间刚体运动4-3：四元数线性插值方法：Squad》中的讲解。 ↩︎

5. 关于曲率的知识请参考《什么是曲率和曲率半径》。 ↩︎

6. 关于变分法请参考文章《变分法入门介绍》，因此文已经介绍得比较好，所以这里不再赘述。 ↩︎