SLAM笔记(八)-再谈四元数

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/Kevin_cc98/article/details/79551513

在二维空间中,我们用复数表示某点坐标,此时可以用加法表达点的移动,用乘法(乘以一个复数)表示点绕原点的旋转。

在三维空间中,我们无法无法用三维的“超级复合数”来表示点的移动和旋转。这也是四元数发明者汉姆尔顿(爱尔兰数学家)曾苦恼的地方。后来他想:为什么要坚持3位数表达,不用四位数来表达三维空间的移动和旋转呢?经过一系列证明,现在我们已经知道可以用四元数来表示三维空间的旋转,同理还存在八元数、十六元数等;但越往上就越会失去一些特性,比如四元数失去了交换律。

1.四元数简介

四元数分为向量和旋转角度θ\theta。如果用三个数来表达,则不可避免会出现奇异情况(如欧拉角);而旋转矩阵有9个分量,浪费空间;四元数刚好无奇异表达,又节约空间。

四元数分为实部和虚部:
q=[q0+q1i+q2j+q3k]q = [q_0+q_1i+q_2j+q_3k] (2)
常写作:[q0,q1,q2,q3][q_0,q_1,q_2,q_3]
有时也将四元数用一个标量q0q_0,一个向量[q1,q2,q3][q_1,q_2,q_3] 来表示。

注: 高翔的博客中用实部在前、虚部在后的写法,而很多文章中采用虚部在前,实部在后的方式。但它们都对应于唯一的表达式(2),也就是这对四元数运算,如旋转公式qpq1qpq^{-1}无影响。

假设绕空间中某一向量n=[nx,ny,nz]Tn = [n_x,n_y,n_z]^T 旋转θ\theta,则四元数q=[q0,q1,q2,q3]q = [q_0,q_1,q_2,q_3]式子为:
q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2](1)q = [cos\frac{\theta}{2}, n_xsin\frac{\theta}{2}, n_ysin\frac{\theta}{2}, n_zsin \frac{\theta}{2}] (1)
由上,我们也可以反过来通过任一四元数q=[q0,q1,q2,q3]q = [q_0,q_1,q_2,q_3]来计算:夹角θ\theta和向量n
从这儿也可以看出四元数与旋转向量的联系和区别:
旋转向量由三个量表示,
注:
(1)处为单位四元数模为1,四元数θ\theta加上2π2\pi 则正负相反,因此任一选择可以由q或它的共轭-q表示;如θ=0\theta = 0,q = [1,0,0,0]或[-1,0,0,0]。

四元数可以不为单位四元数,非单位四元数与单位四元数的区别在于 nx,ny,nzn_x,n_y,n_z 进行一定等比例缩放。

2.四元数运算(可先看3再回头看2)

i 加减法 对应元素相加减
ii 乘法qa,qbq_a,q_b的乘法即qaq_a**每一个元素(带虚部)**与qbq_b每一个元素分别进行复数域的相乘(也就是有16次两两相乘):
注意:
ii = jj = k*k = -1;
ij = k, ji = -k;
jk = i, kj = -i;
ki = j, ik = -j;

iii 求模
q=sqrt(q02+q12+q22+q32)||q|| = sqrt(q_0^2+q_1^2+q_2^2+q_3^2)
iv 共轭(可以视为表示同一个三维空间上的旋转)
q=(q0,q1,q2,q3)q^* = (q_0,-q_1,-q_2,-q_3)
v. 求逆
q1=qq2q^{-1} = \frac{q^*}{||q||^2}
通常四元数为单位四元数,所以逆即为共轭
q1=q=(q0,q1,q2,q3)q^{-1} = q*=(q_0,-q_1,-q_2,-q_3)
四元数的逆物理意义表示什么?
vi 点乘
即不考虑虚部i,j,k,只是对应元素相乘:
qa.qb=qa0qb0+qa1qb1+qa2qb2+qa3qb3q_a.*q_b = q_{a0}q_{b0}+q_{a1}q_{b1}+q_{a2}q_{b2}+q_{a3}q_{b3}
注意:四元数不满足交换律,即$pq \neq qp $

3.为什么要有四元数

我们知道用欧拉角或者方向角(roll, pitch,yaw)可以表示旋转,比如,
记录旋转轴顺序,
加上角度就是一个完整的欧拉角表示方式:zxy>θρϕzxy->\theta \rho \phi

这在直观上也很好理解,为什么还要多此一举引入四元数?

大家可能会想到因为欧拉角表示导致的万向锁。
万向锁只是方向角在实际物理实现上会出现的一个问题,理论上
要规避万向节死锁,选择合适的旋转顺序就行了

但问题是:决定顺序也要花费判定成本

用方向角表示旋转,虽然直观,但没有考虑到歧义,以及计算和存储的需求

所谓歧义,即我们一般是先知道欧拉角再去计算唯一的旋转(旋转向量、旋转矩阵、四元数等能唯一表示三维旋转的方式),但给定一个三维旋转,我们却有至少两种欧拉角表示方式。

消除歧义
欧拉角的选取顺序有(x,y x,x,z,x)等
6种,可见选取顺序是a,b,a这样的顺序,也就是绕a轴旋转某角度后,绕新生成的b轴旋转一个角度,最后绕两次旋转以后的a轴再旋转一个角度

为了消除歧义,我们让旋转一次到位,即绕某一旋转轴旋n转θ\theta,这即是旋转向量:nθn\theta

但如果要将旋转构建成一个群,则三维是满足不了群的幺元、封闭性、結合律、有逆元的性质的,所以我们尝试用四维空间来表示三维旋转,即四元数。

本质上,四元数表示的三维旋转是四维空间的一个子集:当以(0,x,y,z)形式表达三维点时,即可参数化四元数。

优化:存储和计算
用欧拉角来计算旋转,如果用常规的李群表示,大致如下:
这里写图片描述

由以上可以看出:存储上和存储上,至少需要存储六组数据(三个角的cos,sin值)。
而四元数的旋转直接相互运算,包括插值(球面插值),都是非常快的,详如下节。

4.物理上的四元数运算

i.三维点p经过某旋转(以四元数表达)后的三维点位置

三维空间的点可以用虚四元数表示:p = [0,x,y,z];

将点p绕着向量n(四元数旋转轴)旋转角度θ\theta的,旋转后的点/向量为:
p=qpq1p' = qpq^{-1}
可以证明得到的p’的维度中第一位是0.

现场落地测试下:对点p = [1,1,0]绕z轴进行90度顺时针旋转(q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]=[2/2,0,0,2/2]q = [cos\frac{\theta}{2}, n_xsin\frac{\theta}{2}, n_ysin\frac{\theta}{2}, n_zsin \frac{\theta}{2}] = [\sqrt 2/2, 0, 0, \sqrt2/2])
转换后的四元数结果为?

答案为:[0, -i, j, 0].

可以认为四元数表征的是四维空间。如果要描述旋转,则必须是单位四元数,或者说单位四元数所代表的球面能正好描述三维旋转。

如果只是左乘一个单位四元数,右边什么都不乘,那么我们得到的是四维旋转的一个子集,这个子集并不能保证结果限制在三维超平面上。如果只右乘,不左乘也是一样一样的。

将某三维点绕四元数p旋转,直接用两次四元数乘积,消耗比较大,p=qpq1p' = qpq^{-1}的实现一般采用更快的方法:

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.四元数表征旋转,当我们需要比较两个旋转谁的旋转幅度大时,比较θ\theta即可,或在归一化成单位四元数后,比较第q0q_0的大小即可。

b.先经过p1旋转,再经过p2旋转,则总共旋转(类似球面最短路径):
ptotal=p1p2p_{total} = p_1p_2
注意此处是四元数的乘法,而非四元素的点积任意两个四元数总是可以组合成一个
新的旋转

比如先绕着x轴旋转60度,再绕着(1,1,1)旋转30度,可以合二为一,用绕着某轴旋转某度来表示。
c. 计算从旋转状态p1到达旋转状态p2的变换ptransp_{trans}(也是一个四元数)
ptrans=p11p2p_{trans}= p_1^{-1}*p_2
四元数的逆表示一个反向的旋转,任意两个四元数总是可以组合成一个新的旋转(四元数的表达中不存在万向锁)。

为了计算从旋转状态p1到达旋转状态p2所需的最小角度θ\theta,只需计算p11p2p1^{-1}*p2的实部,实部等于cos(θ/2)cos(\theta /2)。因此在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.四元数与旋转向量

旋转向量的表示:θ[nx,ny,nz\theta [n_x,n_y,n_z,其中n为单位矩阵:
在上文已经知道旋转向量到四元数[q0,q1,q2,q3][q_0,q_1,q_2,q_3]的转换法,则:
θ=2acos(q0)\theta = 2\cdot acos(q_0)
[nx,ny,nz]=[q1,q2,q3]/sin(θ2)[n_x,n_y,n_z] = [q_1,q_2,q_3]/sin(\frac{\theta}{2})

3.2.四元数与旋转矩阵

有了四元数,我们可以算出n与θ\theta,然后根据罗格斯变换再算出选择矩阵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函数定义为: slerp(q0,q1,t)=q0(q01q1)tslerp(q0,q1,t) = q0(q0^{-1}q1)^t。其代码实现为:

	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类

展开阅读全文

没有更多推荐了,返回首页