SLAM笔记(八)-再谈四元数
在二维空间中,我们用复数表示某点坐标,此时可以用加法表达点的移动,用乘法(乘以一个复数)表示点绕原点的旋转。
在三维空间中,我们无法无法用三维的“超级复合数”来表示点的移动和旋转。这也是四元数发明者汉姆尔顿(爱尔兰数学家)曾苦恼的地方。后来他想:为什么要坚持3位数表达,不用四位数来表达三维空间的移动和旋转呢?经过一系列证明,现在我们已经知道八元数,十六元数等来做比八维、十六维更高维上的事情;但越往上就越会失去一些特性,比如四元数失去了交换律。
1.四元数简介
四元数分为向量和旋转角度。如果用三个数来表达,则不可避免会出现奇异情况(如欧拉角);而旋转矩阵有9个分量,浪费空间;四元数刚好无奇异表达,又节约空间。
四元数分为实部和虚部:
(2)
常写作:
有时也将四元数用一个标量,一个向量 来表示。
注:高翔的博客中用实部在前,虚部在后,但很多文章中采用虚部在前,实部在后的方式。但都对应于唯一的表达式(2),也就是这对四元数运算,如旋转公式无影响。
假设绕空间中某一向量 旋转,则四元数式子为:
由上,我们也可以反过来通过任一四元数来计算:夹角和向量n。
从这儿也可以看出四元数与旋转向量的联系和区别:
旋转向量由三个量表示,
注:
(1)处为单位四元数,模为1,四元数加上 则正负相反,因此任一选择可以由q或它的共轭-q表示;如,q = [1,0,0,0]或[-1,0,0,0]。
四元数可以不为单位四元数,非单位四元数与单位四元数的区别在于 进行一定等比例缩放。
2.四元数运算(可先看3再回头看2)
i 加减法 对应元素相加减
ii 乘法的乘法即每一个元素(带虚部)与每一个元素分别进行复数域的相乘(也就是有16次两两相乘):
注意:
i*i = j*j = k*k = -1;
ij = k, ji = -k;
jk = i, kj = -i;
ki = j, ik = -j;
iii 求模
iv 共轭(可以视为表示同一个三维空间上的旋转)
v. 求逆
通常四元数为单位四元数,所以逆即为共轭:
四元数的逆物理意义表示什么?
vi 点乘
即不考虑虚部i,j,k,只是对应元素相乘:
注意:四元数不满足交换律,即
3.为什么要有四元数
我们知道用欧拉角或者方向角(roll, pitch,yaw)可以表示旋转,比如,
记录旋转轴顺序,
加上角度就是一个完整的欧拉角表示方式:

这在直观上也很好理解,为什么还要多此一举引入四元数?
大家可能会想到因为欧拉角表示导致的万向锁。
万向锁只是方向角在实际物理实现上会出现的一个问题,理论上
要规避万向节死锁,选择合适的旋转顺序就行了。
只是:决定顺序也要花费判定成本。
用方向角表示旋转,虽然直观,但没有考虑到歧义,以及计算和存储的需求。
所谓歧义,即我们一般是先知道欧拉角再去计算唯一的旋转(旋转向量、旋转矩阵、四元数等能唯一表示三维旋转的方式),但给定一个三维旋转,我们却有至少两种欧拉角表示方式。
消除歧义:
欧拉角的选取顺序有(x,y x,x,z,x)等
6种,可见选取顺序是a,b,a这样的顺序,也就是绕a轴旋转某角度后,绕新生成的b轴旋转一个角度,最后绕两次旋转以后的a轴再旋转一个角度
为了消除歧义,我们让旋转一次到位,即绕某一旋转轴旋n转,这即是旋转向量:
但如果要将旋转构建成一个群,则三维是满足不了群的幺元、封闭性、結合律、有逆元的性质的,所以我们尝试用四维空间来表示三维旋转,即四元数。
即用三维空间表示三维旋转。
本质上,四元数表示的三维旋转是四维空间的一个子集:当以(0,x,y,z)形式表达三维点时,即可参数化四元数。
优化:存储和计算
用欧拉角来计算旋转,如果用常规的李群表示,大致如下:
由以上可以看出:存储上和存储上,至少需要存储六组数据(三个角的cos,sin值)。
而四元数的旋转直接相互运算,包括插值(球面插值),都是非常快的,详如下节。
4.物理上的四元数运算
i.三维点p经过某旋转(以四元数表达)后的三维点位置
三维空间的点可以用虚四元数表示:p = [0,x,y,z];
将点p绕着向量n(四元数旋转轴)旋转角度的,旋转后的点/向量为:
可以证明得到的p’的维度中第一位是0.
现场落地测试下:对点p = [1,1,0]绕z轴进行90度顺时针旋转()
转换后的四元数结果为?
答案为:[0, -i, j, 0].
可以认为四元数表征的是四维空间。如果要描述旋转,则必须是单位四元数,或者说单位四元数所代表的球面能正好描述三维旋转。
如果只是左乘一个单位四元数,右边什么都不乘,那么我们得到的是四维旋转的一个子集,这个子集并不能保证结果限制在三维超平面上。如果只右乘,不左乘也是一样一样的。
将某三维点绕四元数p旋转,直接用两次四元数乘积,消耗比较大,的实现一般采用更快的方法:
void rotate_vector_by_quaternion(const Vector3& v, const Quaternion& q, Vector3& vprime)
{
// Extract the vector part of the quaternion
Vector3 u(q.x, q.y, q.z);
// Extract the scalar part of the quaternion
float s = q.w;
// Do the math
vprime = 2.0f * dot(u, v) * u
+ (s*s - dot(u, u)) * v
+ 2.0f * s * cross(u, v);
}
ii.四元数之间的旋转
a.四元数表征旋转,当我们需要比较两个旋转谁的旋转幅度大时,比较即可,或在归一化成单位四元数后,比较第的大小即可。
b.先经过p1旋转,再经过p2旋转,则总共旋转(类似球面最短路径):
注意此处是四元数的乘法,而非四元素的点积。任意两个四元数总是可以组合成一个新的旋转。
比如先绕着x轴旋转60度,再绕着(1,1,1)旋转30度,可以合二为一,用绕着某轴旋转某度来表示。
c. 计算从旋转状态p1到达旋转状态p2的变换(也是一个四元数):
四元数的逆表示一个反向的旋转,任意两个四元数总是可以组合成一个新的旋转(四元数的表达中不存在万向锁)。
为了计算从旋转状态p1到达旋转状态p2所需的最小角度,只需计算的实部,实部等于。因此在Unity中计算两个四元数lhs与rhs之间的旋转最小角度可为:
float Quaternion::Angle(const Quaternion &lhs, const Quaternion &rhs)
{
float cos_theta = Dot(lhs, rhs);
// if B is on opposite hemisphere from A, use -B instead
if (cos_theta < 0.f)
{
cos_theta = -cos_theta;
}
float theta = acos(cos_theta);
return 2 * Mathf::Rad2Deg * theta;
}
d.两个四元数的余弦角度,无具体物理含义,既不能表征谁的旋转幅度大,也不能表征各自旋转轴的关系,但在球面插值时会有用(参见部分4)。
3.四元数、旋转矩阵、旋转向量、欧拉角的相互转换
3.1.四元数与旋转向量
旋转向量的表示:,其中n为单位矩阵:
在上文已经知道旋转向量到四元数的转换法,则:
3.2.四元数与旋转矩阵
有了四元数,我们可以算出n与,然后根据罗格斯变换再算出选择矩阵R,也可以直接算R(此处为单位四元数):
如果四元数非单位四元数,则:
由于q和−q表示同一个旋转,对同一个旋转矩阵R,对应4种q。存在其他三种与上式类似的计算方式。实际编程中,当q0接近0时,其余三个分量会非常大,导致解不稳定,此时会考虑使用剩下的三种种方式之一计算。具体推导可参见该链接
3.3 从四元数到欧拉角
相互转实现可参考:
用C++实现一个Quaternion类
4.四元数插值
在两个四元数的插值操作称为slerp,即球面线性插值(Spherical Linear Interpolation)。slerp运算非常有用,可以避免欧拉角插值的所有问题(如万向锁)。
设开始与结束的四元数为q0,q1,插值变量设为t,t在[0, 1]之间变化 。则slerp函数定义为:
Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b, float t)
{
float cos_theta = Dot(a, b);
// if B is on opposite hemisphere from A, use -B instead
float sign;
if (cos_theta < 0.f){
cos_theta = -cos_theta;
sign = -1.f;
}
else sign = 1.f;
float c1, c2;
if (cos_theta > 1.f - Mathf::EPSILON) { // if q2 is (within precision limits) the same as q1,
// just linear interpolate between A and B.
c2 = t;
c1 = 1.f - t;
}
else
{
// faster than table-based :
//const float theta = myacos(cos_theta);
float theta = acos(cos_theta);
float sin_theta = sin(theta);
float t_theta = t*theta;
float inv_sin_theta = 1.f / sin_theta;
c2 = sin(t_theta) * inv_sin_theta;
c1 = sin(theta - t_theta) * inv_sin_theta;
}
c2 *= sign; // or c1 *= sign
// just affects the overrall sign of the output
// interpolate
return Quaternion(a.x * c1 + b.x * c2, a.y * c1 + b.y * c2, a.z * c1 + b.z * c2, a.w * c1 + b.w * c2);
}
参考小结:
【Numberphile数字狂】神奇四元数 @柚子
Understanding Quaternions 中文翻译《理解四元数》
知乎:如何形象地理解四元数?
用C++实现一个Quaternion类