技术背景
在前面几篇跟SETTLE约束算法相关的文章（1, 2, 3）中，都涉及到了大量的向量旋转的问题--通过一个旋转矩阵，给定三个空间上的欧拉角
，将指定的向量绕对应轴进行旋转操作。而本文主要就阐述这些旋转操作中，有可能面临到的一个重要问题--万向节死锁问题（Gimbal Lock）。
一般大家觉得用图像化的方式来展示问题会显得更加的直观，但是这里我们准备直接用公式来陈述一下这个问题，也许会更直接。首先我们知道几个熟悉的旋转矩阵：
`$R_Y(alpha)=left(
begin{matrix}
cosalpha && 0 && sinalpha
0 && 1 && 0
-sinalpha && 0 && cosalpha
end{matrix}
right)
R_X(beta)=left(
begin{matrix}
1 && 0 && 0
0 && cosbeta && -sinbeta
0 && sinbeta && cosbeta
end{matrix}
right)
R_Z(gamma)=left(
begin{matrix}
cosgamma && -singamma && 0
singamma && cosgamma && 0
0 && 0 && 1
end{matrix}
right)
$`
这些旋转矩阵分别表示绕
轴旋转指定的角度的操作。如果我们将这些旋转操作按照顺序作用在一个向量上，由于矩阵运算的结合律，我们可以得到这样一个等效的操作矩阵：
`$R(alpha, beta, gamma)=R_Y(alpha)R_X(beta)R_Z(gamma)=left(
begin{matrix}
cosalpha cosgamma sinalpha sinbeta singamma&-cosalpha singamma sinalpha sinbeta cosgamma& sinalpha cosbeta
cosbeta singamma&cosbeta cosgamma&-sinbeta
-sinalpha cosgamma cosalpha sinbeta singamma&sinalpha singamma cosalpha sinbeta cosgamma&cosalpha cosbeta
end{matrix}
right)
$`
为了简化写法，我们可以定义：
`$c_1=cosalpha=cos(Y), s_1=sinalpha=sin(Y)
c_2=cosbeta=cos(X), s_2=sinbeta=sin(X)
c_3=cosgamma=cos(Z), s_3=singamma=sin(Z)
$`
那么就有：
`$R(alpha, beta, gamma)=left(
begin{matrix}
c_1c_3 s_1s_2s_3&-c_1s_3 s_1s_2c_3& s_1c_2
c_2s_3&c_2c_3&-s_2
-s_1c_3 c_1s_2s_3&s_1s_3 c_1s_2c_3&c_1c_2
end{matrix}
right)
$`
其实按照不同的顺序进行旋转的话，得到的结果会有6种可能性：
，但是这里一般人为的规定使用
顺规，也就是先绕
轴旋转，再绕
轴旋转，最后再绕
轴旋转的顺序。
如果这里假定
，也就是绕
轴旋转的角度是90度，那么得到的新的旋转矩阵为：
`$R(alpha, frac{pi}{2}, gamma)=R_Y(alpha)R_X(frac{pi}{2})R_Z(gamma)=left(
begin{matrix}
c_1c_3 s_1s_3&-c_1s_3 s_1c_3&0
0&0&-1
-s_1c_3 c_1s_3&s_1s_3 c_1c_3&0
end{matrix}
right)
$`
再回顾下三角函数公式：
`$sin(apm b)=sin(a)cos(b)pm sin(b)cos(a)
cos(apm b)=cos(a)cos(b)mp sin(a)sin(b)
$`
代入可得：
`$R(alpha, frac{pi}{2}, gamma)=left(
begin{matrix}
cos(alpha-gamma)&sin(alpha-gamma)&0
0&0&-1
-sin(alpha-gamma)&cos(alpha-gamma)&0
end{matrix}
right)
$`
类似地：
`$R(alpha, -frac{pi}{2}, gamma)=left(
begin{matrix}
cos(alpha gamma)&-sin(alpha gamma)&0
0&0&1
-sin(alpha gamma)&-cos(alpha gamma)&0
end{matrix}
right)
$`
到这里，端倪初现，按照上面的这个公式，改变
和改变
的值，得到的效果是一样的。换句话说，如果在一个欧拉角的旋转矩阵中，在
轴的方向将其旋转了90度之后，接下来不论是绕
轴旋转，还是绕
轴旋转，得到的效果是一样的。那么，本来应该是三个方向的自由度，现在变成了两个，这就是问题所在。
SETTLE约束算法的局限性
这里我们先回顾一下SETTLE算法中的一个环节，这个环节中涉及到了旋转矩阵的使用。首先是给定一个三角形
：
然后将其按照这样的一个顺序进行旋转操作：
旋转之后可以得到新的三角形
。按照SETTLE: An Analytical Version of the SHAKE and RATTLE Algorithm for Rigid Water Models这篇文章的推导，我们可以得到所需的三个旋转的欧拉角的计算公式为（这里有一个需要注意的问题，SETTLE文章里面的
三个参数跟我们前面提到的旋转欧拉角的意义不同，在这篇文章中分别用
来表示绕
轴的旋转）：
`$begin{align*}
phi&=arcsinleft(frac{Z'_{a_3}}{r_a}right)
psi&=arcsinleft(frac{Z'{b_3}-Z'{c_3}}{2r_ccosphi}right)
theta&=arcsinleft(frac{gamma}{sqrt{alpha^2 beta^2}}right)-arctanleft(frac{beta}{alpha}right)
end{align*}
$`
其中
、
和
的取值如下：
`$begin{align*}
alpha&=-rccospsi(X'{b0}-X'{c0}) (-r_bcosphi-r_csinpsi sinphi)(Y'{b0}-Y'{a0}) (-r_bcosphi r_csinpsi sinphi)(Y'{c0}-Y'{a_0})
beta&=-rccospsi(Y'{c0}-Y'{b0}) (-r_bcosphi-r_csinpsi sinphi)(X'{b0}-X'{a0}) (-r_bcosphi r_csinpsi sinphi)(X'{c0}-X'{a_0})
gamma&=Y'{b_1}(X'{b0}-X'{a0})-X'{b1}(Y'{b0}-Y'{a0}) Y'{c1}(X'{c0}-X'{a0})-X'{c1}(Y'{c0}-Y'{a_0})
end{align*}
$`
那么在这个计算公式中，如果我们取
轴的旋转角度为90度，也就是：
`$phi=pmfrac{pi}{2}
$`
此时，由于
，在计算
的时候，就会直接出现报错，SETTLE算法无法继续执行。那么在其他类似的欧拉角求解的场景下，一样有可能出现类似的问题，导致无法正常计算欧拉角。并且，如果给定欧拉角，那么从原始的向量到终点的向量是一一对应的，但是如果给定两个向量，其对应的旋转矩阵不是唯一的。或者说，两个向量之间变换的轨迹不是唯一的。
Gimbal平衡环架中的死锁
在参考链接2中，作者自己画了一个平衡环架用于表示欧拉角死锁的问题。非常的直观，本章节中的图片都来自于参考链接2。首先我们给定一个这样的Gimbal：
其中每一个颜色的环都可以绕对应轴进行旋转，比如绕绿轴的旋转：
绕红轴的旋转：
绕蓝轴的旋转：
有了这三个轴的话，不论怎么旋转，都可以保持中间那根棍子的位置和朝向不变，达到一个稳定的效果。这个三轴稳定器已经被大量应用在航空航海领域，而后面要讲的四元数的概念，其实也是Hamilton在一次坐船的过程中提出来的一个概念。但是有这样一个特殊的场景，有可能会破坏这个稳定结构。假设我们先把红色这个圈，旋转90度到跟蓝色圈共面的位置：
那么接下来我们就会发现，原本在三个轴向的旋转下都可以保持稳定的棍子，居然摇晃起来了：
这就是有名的万向节死锁，死锁出现的时候，原本三个方向旋转的自由度，变成了两个，就失去了一个自由度。接下来我们可以看一看，有没有什么方案可以解决万向节死锁的问题。
四元数（Quaternion）的定义
应该说，四元数的出现，并不是为了解决欧拉角死锁的问题。但是在后来长期的应用中，人们发现了四元数在几何旋转表示中的独特优势。在蛋白质折叠软件AlphaFold2和MEGAProtein中都使用到了四元数，用于表征分子结构的三维旋转。关于四元数的介绍材料，可以参考一下参考链接3、4，分别是英文版和中文翻译版。
要理解四元数，至少需要了解复数，可以把四元数当做是复数的一个推广形式。一个复数是由实部和虚部组成，很多时候我们也会将其写成指数形式或者是向量形式：
`$z=x i y=frac{1}{sqrt{x^2 y^2}}e^{i arctan(frac{y}{x})}=left[
begin{matrix}
xy
end{matrix}
right]
$`
其中
是实数，
称为复数
的实部，
称为复数
的虚部，
是虚数单位。正因为复数可以被看做是一个矢量，所以我们才会想到复数跟向量旋转之间的联系，比如我们有一个单位长度的复数
，那么就有以下的复数乘法关系：
`$z'z=(costheta i sintheta)(x i y)=xcostheta iycostheta ixsintheta-ysintheta=left[
begin{matrix}
xcostheta-ysintheta
ycostheta xsintheta
end{matrix}
right]=left[
begin{matrix}
costheta&-sintheta
sintheta&costheta
end{matrix}
right]left[
begin{matrix}
xy
end{matrix}
right]
$`
如此一来，我们就得到了一个二维向量的旋转矩阵：
`$R_2(theta)=left[
begin{matrix}
costheta&-sintheta
sintheta&costheta
end{matrix}
right]
$`
有了复数的概念基础，我们可以将其扩展成一个四元数：
`$z=x iy_i jy_j ky_k=left[
begin{matrix}
xy_iy_jy_k
end{matrix}
right]
$`
比较特殊的，相比于复数的定义有一些补充定义：
`$i^2=j^2=k^2=ijk=-1
ij=k,jk=i,ki=j
ji=-k,kj=-i,ik=-j
$`
这些关系，其实很容易让我们联想到向量的叉乘。类似于单位长度复数的定义，这里我们可以定义一个单位长度的四元数：
`$begin{align*}
q&=(cosalpha j sinalpha)(cosbeta i sinbeta)(cosgamma k singamma)
&=(cosalpha cosbeta cosgamma sinalpha sinbeta singamma) i(cosalpha sinbeta cosgamma sinalpha cosbeta singamma) j(sinalpha cosbeta cosgamma-cosalpha sinbeta singamma) k(cosalpha cosbeta singamma-sinalpha sinbeta cosgamma)
&=(c_1c_2c_3 s_1s_2s_3) i(c_1s_2c_3 s_1c_2s_3) j(s_1c_2c_3-c_1s_2s_3) k(c_1c_2s_3-s_1s_2c_3)
&=s ix jy kz
end{align*}
$`
这里我们假定：
`$s=c_1c_2c_3 s_1s_2s_3
x=c_1s_2c_3 s_1c_2s_3
y=s_1c_2c_3-c_1s_2s_3
z=c_1c_2s_3-s_1s_2c_3
$`
因为我们要使得
是一个单位四元数，因此有：
`$s^2 x^2 y^2 z^2=1
$`
四元数与旋转矩阵
四元数与复数的旋转矩阵略有不同，给定一个单位四元数
和一个三维空间向量（纯四元数，即
）：
，那么向量
关于
的旋转被定义为：
。这里
，是
的共轭四元数。对于一个单位四元数而言，
。那么我们可以计算一下对应的旋转矩阵
：
`$begin{align*}
R_qtextbf{v}&=qtextbf{v}q^{*}
&=(s ix jy kz)left[
v_ix v_jy v_kz i(v_is-v_jz v_ky) j(v_iz v_js-v_kx) k(-v_iy v_jx v_ks)
right]
&=left[
begin{matrix}
s^2 x^2-y^2-z^2&&2xy-2sz&&2xz 2sy
2sz 2xy&&s^2-x^2 y^2-z^2&&2yz-2sx
2xz-2sy&&2sx 2yz&&s^2-x^2-y^2 z^2
end{matrix}
right]left[
begin{matrix}
v_iv_jv_k
end{matrix}
right]
end{align*}
$`
即，四元数所代表的旋转矩阵为：
`$begin{align*}
R(alpha,beta,gamma)&=left[
begin{matrix}
s^2 x^2-y^2-z^2&&2xy-2sz&&2xz 2sy
2sz 2xy&&s^2-x^2 y^2-z^2&&2yz-2sx
2xz-2sy&&2sx 2yz&&s^2-x^2-y^2 z^2
end{matrix}
right]
&=left[
begin{matrix}
cos2alpha cos2gamma sin2alpha sin2beta sin2gamma&&sin2alpha sin2beta cos2gamma-cos2alpha sin2gamma&&sin2alpha cos2beta
cos2beta sin2gamma&&cos2beta cos2gamma&&-sin2beta
cos2alpha sin2beta sin2gamma-sin2alpha cos2gamma&&sin2alpha sin2gamma cos2alpha sin2beta cos2gamma&&cos2alpha cos2beta
end{matrix}
right]
end{align*}
$`
可以发现这里得到的旋转矩阵跟前面的
顺规结果是一致的。但有一点不同的是，在这个结果中我们还发现了一个比较重要的变换：所有的角度在旋转矩阵中都翻了1倍。这里给了我们一个重要启示：在使用四元数对向量进行旋转时，需要先把角度除以2。
四元数Python代码实现
在Python的符号计算库Sympy中，已经支持了四元数的相关运算：
代码语言：javascript复制from sympy import *
from sympy.algebras.quaternion import Quaternion
s, x, y, z = symbols('s x y z', real=True, positive=True)
a, b, g = symbols('a b g')
vi, vj, vk = symbols('v_i v_j v_k')
tvi, tvj, tvk = symbols('t_i, t_j, t_k')
# 定义三个轴的旋转四元数
qa = Quaternion(cos(a), 0, sin(a), 0)
qb = Quaternion(cos(b), sin(b), 0, 0)
qg = Quaternion(cos(g), 0, 0, sin(g))
# 按顺序合并旋转四元数为一个单位四元数
q = qa.mul(qb.mul(qg))
print ('The unit quaternion q is: {}'.format(pretty(q)))
# The quaternion q is: (sin(a)*sin(b)*sin(g) cos(a)*cos(b)*cos(g)) (sin(a)*sin(g)*cos(b) sin(b)*cos(a)*cos(g))*i
# (sin(a)*cos(b)*cos(g) - sin(b)*sin(g)*cos(a))*j (-sin(a)*sin(b)*cos(g) sin(g)*cos(a)*cos(b))*k
# 定义一个普通四元数
q = Quaternion(s, x, y, z)
print ('The normal quaternion q is: {}'.format(pretty(q)))
# The normal quaternion q is: s x*i y*j z*k
# 四元数求共轭
cq = conjugate(q)
print ('The conjugate quaternion cq is: {}'.format(pretty(cq)))
# 定义一个纯四元数
v = Quaternion(0, vi, vj, vk)
print ('The pure quaternion v is: {}'.format(pretty(v)))
# The pure quaternion v is: 0 v_i*i v_j*j v_k*k
# 计算四元数的旋转qvq*
tv = simplify(q.mul(v.mul(cq)))
print ('Transformed pure quaternion tv is: {}'.format(pretty(tv)))
# Transformed pure quaternion tv is: 0
# (s*(s*v_i - v_j*z v_k*y) x*(v_i*x v_j*y v_k*z) y*(s*v_k - v_i*y v_j*x) - z*(s*v_j v_i*z - v_k*x))*i
# (s*(s*v_j v_i*z - v_k*x) - x*(s*v_k - v_i*y v_j*x) y*(v_i*x v_j*y v_k*z) z*(s*v_i - v_j*z v_k*y))*j
# (s*(s*v_k - v_i*y v_j*x) x*(s*v_j v_i*z - v_k*x) - y*(s*v_i - v_j*z v_k*y) z*(v_i*x v_j*y v_k*z))*k那么有了Sympy这样强大的四元数处理的工具，可以节省我们很多手算的时间。
四元数是否解决了Gimbal Lock问题？
在最前面的章节中，我们讲Gimbal Lock欧拉角死锁问题时，提到了一个比较重要的点：在特定条件下（如绕一个指定的轴旋转90度），两个空间向量中间可以对应无穷多个欧拉角的组合。这个现象也体现出了，用欧拉角的方式无法一一对应的表示每两个空间向量之间的旋转关系。因此我们需要找到一个新的表示方法，基于新的表示方法，不仅要使得每一组输入参数对应一个特定的旋转，还需要使得给定任意的两个空间向量，所计算得到的参数要是唯一的，这样才能构成一一对应的映射关系。我们先介绍一下从两个空间向量
中去计算四元数的方法，再结合一些实际案例，看看是否会出现欧拉角旋转矩阵中出现的奇点。
假设两个向量之间的夹角为
，则对应的旋转四元数可以表示为：
`$textbf{u}=textbf{v}_1timestextbf{v}_2
costheta=frac{textbf{v}_1cdottextbf{v}_2}{|textbf{v}_1||textbf{v}_2|}
q=cosfrac{theta}{2} i sinfrac{theta}{2}textbf{u}cdot i j sinfrac{theta}{2}textbf{u}cdot j k sinfrac{theta}{2}textbf{u}cdot k
$`
案例重现
那么如果我们使用前面提到的欧拉角奇点的案例，假定一个空间向量
，也就是处于三维坐标系的X轴正方向上的一个单位向量，我们给它设计一个路径：先绕Z轴旋转
的角度，然后绕X轴旋转90度，最后再绕Y轴旋转
的角度，那么对应的四元数为：
`$begin{align*}
q&=(cosfrac{alpha}{2}-j sinfrac{alpha}{2})(frac{sqrt{2}}{2} ifrac{sqrt{2}}{2})(cosfrac{gamma}{2} k sinfrac{gamma}{2})
&=frac{sqrt{2}}{2}left[
cos(frac{alpha}{2} frac{gamma}{2}) i cos(frac{alpha}{2} frac{gamma}{2})-j sin(frac{alpha}{2} frac{gamma}{2}) k sin(frac{alpha}{2} frac{gamma}{2})
right]
end{align*}
$`
因为在四元数中我们并不关心具体每个旋转角度的值，我们只需要四元数的四个元素就可以了，所以这里令
，则有：
`$q=frac{sqrt{2}}{2}(x ix-jy ky)
x^2 y^2=1
$`
使用Hamilton Product作用在
上可以得到：
`$textbf{v}_1=qtextbf{v}_0q^{*}=i(x^2-y^2) 2kxy
$`
为了简化计算量，假定我们得到的结果是
，在三维坐标系下，这个矢量位于
轴的正方向上，也就是：
`$textbf{v}_1=qtextbf{v}_0q^{*}=i(x^2-y^2) 2kxy=k
$`
则有对应关系：
`$left{
begin{matrix}
x^2-y^2=0
x^2 y^2=1
xy=frac{1}{2}
end{matrix}
right.
$`
那么得到两组解：
`$left{
begin{matrix}
x=frac{sqrt{2}}{2}, y=frac{sqrt{2}}{2}
x=-frac{sqrt{2}}{2}, y=-frac{sqrt{2}}{2}
end{matrix}
right.
$`
也就是说，这个旋转过程有两个可能对应的四元数：
`$left{
begin{matrix}
q_1=frac{sqrt{2}}{2} frac{sqrt{2}}{2}i-frac{sqrt{2}}{2}j frac{sqrt{2}}{2}k
q_2=-frac{sqrt{2}}{2}-frac{sqrt{2}}{2}i frac{sqrt{2}}{2}j-frac{sqrt{2}}{2}k
end{matrix}
right.
$`
而在这个计算过程中，我们依然发现，如果使用欧拉角来进行旋转的话，只能计算出
的值，但是法对其进行求解，而四元数则不存在这样的奇点。
案例拓展
在上面这个案例中，我们其实是已经给定了一个轨迹所对应的四元数。而如果只是知道起始点
和终点
，其实我们也可以用上述计算旋转四元数的方法来进行求解：
`$textbf{u}=itimes k=-j
costheta=icdot k=0
$`
这就可以计算得四元数：
`$begin{align*}
q&=cosfrac{theta}{2} i sinfrac{theta}{2}textbf{u}cdot i j sinfrac{theta}{2}textbf{u}cdot j k sinfrac{theta}{2}textbf{u}cdot k
&=frac{sqrt{2}}{2}-frac{sqrt{2}}{2}j
end{align*}
$`
虽然这个计算过程很容易，但是我们依然可以使用sympy来验算一下：
代码语言：javascript复制In [1]: from sympy import *
In [2]: from sympy.algebras.quaternion import Quaternion
In [3]: import numpy as np
In [4]: q=Quaternion(np.sqrt(2)/2,0,-np.sqrt(2)/2,0)
In [5]: v=Quaternion(0,1,0,0)
In [6]: q.mul(v.mul(conjugate(q)))
Out[6]: 0 0*i 0*j 1.0*k类似的，我们也可以验证一下上一个模块中提到的指定轨迹和起始点与终点所得到的旋转四元数：
代码语言：javascript复制In [7]: q=Quaternion(np.sqrt(2)/2,np.sqrt(2)/2,-np.sqrt(2)/2,np.sqrt(2)/2)/np.sqrt(2)
In [8]: q.mul(v.mul(conjugate(q)))
Out[8]: 0 0*i 0*j 1.0*k
In [9]: q=-1*Quaternion(np.sqrt(2)/2,np.sqrt(2)/2,-np.sqrt(2)/2,np.sqrt(2)/2)/np.sqrt(2)
In [10]: q.mul(v.mul(conjugate(q)))
Out[10]: 0 0*i 0*j 1.0*k总结概要
本文通过两个案例——旋转矩阵和分子动力学模拟中的SETTLE约束算法，介绍了一下Gimbal Lock问题，简单来说就是，用旋转矩阵去表示三维空间的向量旋转有可能会遇到奇点，使得三维空间原本的三个自由度在某一个点变成两个自由度。而使用我们这里所介绍的四元数Quaternion则不会有这样的问题，同时本文也介绍了一些四元数的基本运算法则和sympy中的代码实现。后续会再单独写三篇博客介绍一下四元数的具体运算细则、四元数在SETTLE算法中的应用以及四元数的物理含义等，敬请期待。
版权声明
本文首发链接为：https://cloud.tencent.com/developer/article/2147861
作者ID：DechinPhy
更多原著文章请参考：https://www.cnblogs.com/dechinphy/
打赏专用链接：https://www.cnblogs.com/dechinphy/gallery/image/379634.html
腾讯云专栏同步：https://cloud.tencent.com/developer/column/91958
CSDN同步链接：https://blog.csdn.net/baidu_37157624?spm=1008.2028.3001.5343
51CTO同步链接：https://blog.51cto.com/u_15561675
参考链接
- https://matthew-brett.github.io/transforms3d/gimbal_lock.html
- https://blog.csdn.net/AndrewFan/article/details/60981437
- https://www.3dgep.com/understanding-quaternions/
- https://www.qiujiawei.com/understanding-quaternions/
- https://zhuanlan.zhihu.com/p/45404840#:~:text=这个四元数构造的大概思路就是把 四元数的旋转操作写成矩阵形式 （注：给定一个用于旋转的单位四元数 textbf q=w+x textbf i+y textbf,p'= textbf q textbf p textbf q^-1 ）。
- https://blog.csdn.net/shenshikexmu/article/details/70991286
