四元数表示向量V1到V2的旋转的两种算法

版权声明：本文为博主原创文章，未经博主允许不得转载。

博主：shenshikexmu

联系方式：[e-mail]

本文的算法来源于* 的回答finding quaternion representing the rotation from one vector to another

问题

如下图，三维空间中的向量 v1→ $\stackrel{\to }{v_1}$绕着单位向量 u→ $\stackrel{\to }{u}$旋转 θ $\theta$角后，形成 v2→ $\stackrel{\to }{v_2}$。已知 v1→ $\stackrel{\to }{v_1}$和 v2→ $\stackrel{\to }{v_2}$求出代表向量间旋转的四元数。

当知道单位向量 u→ $\stackrel{\to }{u}$和 θ $\theta$角时，这个四元数表示起来很简单。
q=cosθ2+sinθ2u→ $q=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}\stackrel{\to }{u}$
也就是：
q0=cosθ2 $q_0=\cos \frac{\theta }{2}$
q1=sinθ2u→.x $q_1=\sin \frac{\theta }{2}\stackrel{\to }{u}.x$
q2=sinθ2u→.y $q_2=\sin \frac{\theta }{2}\stackrel{\to }{u}.y$
q3=sinθ2u→.z $q_3=\sin \frac{\theta }{2}\stackrel{\to }{u}.z$

在 v1→ $\stackrel{\to }{v_1}$和 v2→ $\stackrel{\to }{v_2}$已知的条件下，角 θ $\theta$可以利用 v1→ $\stackrel{\to }{v_1}$和 v2→ $\stackrel{\to }{v_2}$内积，也就是 ⋅ $\cdot$乘计算出来，向量 u→ $\stackrel{\to }{u}$可以利用 v1→ $\stackrel{\to }{v_1}$和 v2→ $\stackrel{\to }{v_2}$外积，也就是 × $\times$乘计算出来。

设:
v1→ $\stackrel{\to }{v_1}$方向上的单位向量为 nv1−→− $\stackrel{\to }{n{v}_1}$长度为a。于是 v1→=a⋅nv1−→− $\stackrel{\to }{v_1}=a\cdot \stackrel{\to }{n{v}_1}$。
v2→ $\stackrel{\to }{v_2}$方向上的单位向量为 nv2−→− $\stackrel{\to }{n{v}_2}$长度为b。于是 v2→=b⋅nv2−→− $\stackrel{\to }{v_2}=b\cdot \stackrel{\to }{n{v}_2}$。
u→ $\stackrel{\to }{u}$已经为单位向量。
那么有如下关系：
v1→⋅v2→=abcosθ $\stackrel{\to }{v_1}\cdot \stackrel{\to }{v_2}=ab\cos \theta$
v1→×v2→=absinθu→ $\stackrel{\to }{v_1}\times \stackrel{\to }{v_2}=ab\sin \theta \stackrel{\to }{u}$

算法1

思路：寻找 v1→ $\stackrel{\to }{v_1}$和 v2→ $\stackrel{\to }{v_2}$中间的向量 half−→− $\stackrel{\to }{half}$，这样 v1→ $\stackrel{\to }{v_1}$与 half−→− $\stackrel{\to }{half}$的夹角是 θ2 $\frac{\theta }{2}$， v1→ $\stackrel{\to }{v_1}$与 half−→− $\stackrel{\to }{half}$的内积方向与 u→ $\stackrel{\to }{u}$相同。
使 half−→− $\stackrel{\to }{half}$变成单位向量。
half−→−=（v1→+v2→）/norm(v1→+v2→) $\stackrel{\to }{half}=（\stackrel{\to }{v_1}+\stackrel{\to }{v_2}）/norm(\stackrel{\to }{v_1}+\stackrel{\to }{v_2})$
于是
nv1−→−⋅half−→−=cosθ2 $\stackrel{\to }{n{v}_1}\cdot \stackrel{\to }{half}=\cos \frac{\theta }{2}$
nv1−→−×half−→−=sinθ2u→ $\stackrel{\to }{n{v}_1}\times \stackrel{\to }{half}=\sin \frac{\theta }{2}\stackrel{\to }{u}$

q=cosθ2+sinθ2u→ $q=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}\stackrel{\to }{u}$变为
q=nv1−→−⋅half−→−+nv1−→−×half−→− $q=\stackrel{\to }{n{v}_1}\cdot \stackrel{\to }{half}+\stackrel{\to }{n{v}_1}\times \stackrel{\to }{half}$

function [q] = qUtoV(v1, v2) 
%Finding quaternion representing the rotation from one vector to another

nv1 = v1/norm(v1);
nv2 = v2/norm(v2);

if norm(nv1+nv2)==0
    q = [0, [1,0,0]];
else
    half = (nv1 + nv2)/norm(nv1 + nv2);
    q = [nv1*half',cross(nv1, half)]; end end

算法2

这个算法需要一些数学推导了，呵呵，看了*页面，有了四元数的思想，算法1还是很好理解，这个算法2也是想把之前imu的工作结束掉，花了些时间推导了一下。
v1→⋅v2→=abcosθ $\stackrel{\to }{v_1}\cdot \stackrel{\to }{v_2}=ab\cos \theta$
v1→×v2→=absinθu→ $\stackrel{\to }{v_1}\times \stackrel{\to }{v_2}=ab\sin \theta \stackrel{\to }{u}$
于是
v1→⋅v2→+ab=ab（cosθ+1）=2abcos2θ2 $\stackrel{\to }{v_1}\cdot \stackrel{\to }{v_2}+ab=ab（\cos \theta +1）=2ab{\cos }^2\frac{\theta }{2}$
v1→×v2→=absinθu→=absinθu→=2absinθ2cosθ2u→ $\stackrel{\to }{v_1}\times \stackrel{\to }{v_2}=ab\sin \theta \stackrel{\to }{u}=ab\sin \theta \stackrel{\to }{u}=2ab\sin \frac{\theta }{2}\cos \frac{\theta }{2}\stackrel{\to }{u}$
向量 [2abcos2θ2,2absinθ2cosθ2u→] $[2ab{\cos }^2\frac{\theta }{2},2ab\sin \frac{\theta }{2}\cos \frac{\theta }{2}\stackrel{\to }{u}]$ 归一化得到 [cosθ2,sinθ2u→] $[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\stackrel{\to }{u}]$，正是所要计算的四元数 q $q$。

function [q] = qUtoV2(v1, v2) 
%Finding quaternion representing the rotation from one vector to another

nv1 = v1/norm(v1);
nv2 = v2/norm(v2);

if norm(nv1+nv2)==0
    q = [0, [1,0,0]];
else

    q = [norm(nv1)*norm(nv2)+nv1*nv2',cross(nv1, nv2)]; q=q/norm(q); end end

---