ros要学习的四元数初探

ros要学习的四元数初探

把四元数看做一个标量和一个3维向量的组合。实部w表示标量，虚部表示向量标记为V或三个单独的分量（x,y,z）,则四元数可以记为[ w, V]或[ w，（x,y,z）]也可以计为[V,w]或[（x,y,z）,w]。

任意一个四元数可以表示为：
q=[x,y,z,w]=xi+yj+zk+w
其中，虚部规则
i^2=j^2=k^2=ijk=−1, 
ij=k,  ji=-k， 
jk=i， kj=-i，
ki=j， ik=-j，

注意ij=k，但是ji≠k。四元数的长度（模长）表示为∥q∥=sqrt(x^2+y^2+z^2+w^2)，通常将四元数规一化(Normalized)，即：


q=[x/∥q∥,  y/∥q∥,  z/∥q∥;  w/∥q∥]
四元数本质上是复数（Complex Number），复数就存在一个共轭的概念。若两个复数的实部相等，虚部互为相反数，则称这两个复数互为共轭（Conjugate），q的共轭复数q∗表示为：q∗=[−x,−y,−z,w]


四元数的逆

q^−1=q∗/∥q∥^2
单位四元数摸的平方==1，它的逆：
q^−1=q∗

四元数间的运算，遵循复数间运算法则。


乘法
乘法是把qa=[sa,va]=[sa,xa,ya,za]
的每一项与qb=[sb,vb]=[sb,xb,yb,zb]每项相乘，最后相加，虚部要按照虚部规则进行：
qa qb=sa sb  −  xa xb  −  ya yb  −  za zb
+(sa xb  +  xa sb  +  ya zb  −  za yb)i
+(sa yb  −  xa zb  +  ya sb  +  za xb)j
+(sa zb  +  xa yb  −  xb ya  +  za sb)k

虽然稍为复杂，但形式上也是整齐有序的。如果写成向量形式并利用内外积运算，该表达会更加简洁：

qa qb=[sa sb−va⋅vb+ sa vb+sb va+va×vb]

加深记忆换个代号再重复一遍
例如有q1=[x1,y1,z1,w1];q2=[x2,y2,z2,w2]
则q1 * q2 =
(w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2) i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2) j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2) k
由于q = w + x i + y j + z k中可以分为纯量w与向量x i + y j + z k，所以为了方便表示，将q表示为(S, V)，其中S表示纯量w，V表示向量x i + y j + z k，所以四元数乘法又可以表示为：
q1 * q2 = (S1 + V1)*(S2 + V2) = S1*S2 - V1.V2 + V1XV2 + S1*V2 + S2*V1






用四元数表示旋转


三维空间上任意的伸缩旋转变换可用四元数的乘积来表示。
如果已知一个三维空间的伸缩旋转的转轴u=[α, β, γ]、旋转角度θ和伸缩比例 T，来求相应的四元数.
旋转的轴是(α, β, γ)已经归一化，即（ α^2 + β^2 + γ^2 = 1），
Q=sqrt（T） [cos(θ/2); α sin(θ/2), β sin(θ/2), γ sin(θ/2)]
一般T==1时，即放缩比例为1时
Q = [cos(θ/2); α sin(θ/2), β sin(θ/2), γ sin(θ/2)]


相反，给定一个四元数Q=[x,y,z,w],∥q∥=1，求出它表示的旋转轴和角度，如下所示：


u=(x/sqrt(1−w^2),  y/sqrt(1−w^2),  z/sqrt(1−w^2) )
θ=2⋅acos(w)
其中，若w=±1，则q表示绕着任意轴旋转0度。


设存在一个向量u=(ux,uy,uz)和一个四元数q=[v,θ],∥q∥=1,v=(α, β, γ)，把向量u进行q旋转变换，得到新向量u1，可以表示为：
u1=q[ux,uy,uz,0]q∗
u1=[α, β, γ,θ][ux,uy,uz,0][-α, -β, -γ,θ]

若向量u2进行四元数q的变换后得到向量u，则u2（u对q的逆变换）可以表示为：

u2=q∗[ux,uy,uz,0]q

u2=[-α, -β, -γ,θ][ux,uy,uz,0][α, β, γ,θ]

四元数的插值


这里的插值指的是球面线性插值。

设t是一个在0到1之间的变量。我们想要基于t求Q1到Q2之间插值后四元数Q。它的公式是：

Q3  = (sin((1-t)A)/sin(A))Q1 + (sin((tA)/sin(A))Q2)
Q = Q3/|Q3|，即单位化



四元数到旋转矩阵的转换


设四元数q=q0+q1i+q2j+q3k
，对应的旋转矩阵R

为：

R=

   |1−2q2^2−2q3^2   2q1q2+2q0q3   2q1q3−2q0q2|
   |2q1q2−2q0q3   1−2q1^2−2q3^2   2q2q3+2q0q1|
   |2q1q3+2q0q2   2q2q3−2q0q1   1−2q1^2−2q2^2|


反之，由旋转矩阵到四元数的转换如下。假设矩阵为R={mij},i,j∈[1,2,3]
，其对应的四元数q由下式给出：
q0=sqrt(tr(R)+1)/2,
q1=(m23−m32)/4q0,
q2=(m31−m13)/4q0,
q3=(m12−m21)/4q0

值得一提的是，由于q和−q
表示同一个旋转，事实上一个R的四元数表示并不是惟一的,存在其他三种与上式类似的计算方式。实际编程中，当q0接近0时，其余三个分量会非常大，导致解不稳定，此时会考虑使用剩下的几种方式计算。


欧拉角到四元数：


给定一个欧拉旋转(X, Y, Z)（即分别绕x轴、y轴和z轴旋转X、Y、Z度），则对应的四元数为：


x = sin(Y/2)sin(Z/2)cos(X/2)+cos(Y/2)cos(Z/2)sin(X/2)
y = sin(Y/2)cos(Z/2)cos(X/2)+cos(Y/2)sin(Z/2)sin(X/2)
z = cos(Y/2)sin(Z/2)cos(X/2)-sin(Y/2)cos(Z/2)sin(X/2)
w = cos(Y/2)cos(Z/2)cos(X/2)-sin(Y/2)sin(Z/2)sin(X/2)
q = ((x, y, z), w)




参考
http://www.twinklingstar.cn/2016/2772/orientation/
http://blog.csdn.net/ideallic/article/details/52650907
http://blog.csdn.net/carmazhao/article/details/39990523
http://blog.csdn.net/candycat1992/article/details/41254799
http://blog.sina.com.cn/s/blog_557d254601018dfv.html
http://blog.csdn.net/silangquan/article/details/39008903