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

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

博主：shenshikexmu

联系方式：[email protected]

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

问题

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

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

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

设:
$\overrightarrow{v_1}$方向上的单位向量为$\overrightarrow{nv_1}$长度为a。于是$\overrightarrow{v_1}=a\cdot\overrightarrow{nv_1}$。
$\overrightarrow{v_2}$方向上的单位向量为$\overrightarrow{nv_2}$长度为b。于是$\overrightarrow{v_2}=b\cdot\overrightarrow{nv_2}$。
$\overrightarrow{u}$已经为单位向量。
那么有如下关系：
$\overrightarrow{v_1}\cdot\overrightarrow{v_2}=ab\cos\theta$
$\overrightarrow{v_1}\times\overrightarrow{v_2}=ab\sin\theta\overrightarrow{u}$

算法1

思路：寻找$\overrightarrow{v_1}$和$\overrightarrow{v_2}$中间的向量$\overrightarrow{half}$，这样$\overrightarrow{v_1}$与$\overrightarrow{half}$的夹角是$\frac{\theta}{2}$，$\overrightarrow{v_1}$与$\overrightarrow{half}$的内积方向与$\overrightarrow{u}$相同。
使$\overrightarrow{half}$变成单位向量。
$\overrightarrow{half}=（\overrightarrow{v_1}+\overrightarrow{v_2}）/norm(\overrightarrow{v_1}+\overrightarrow{v_2})$
于是
$\overrightarrow{nv_1}\cdot\overrightarrow{half}=\cos\frac{\theta}{2}$
$\overrightarrow{nv_1}\times\overrightarrow{half}=\sin\frac{\theta}{2}\overrightarrow{u}$

$q=\cos\frac{\theta}{2}+\sin\frac{\theta}{2}\overrightarrow{u}$变为
$q=\overrightarrow{nv_1}\cdot\overrightarrow{half}+\overrightarrow{nv_1}\times\overrightarrow{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

这个算法需要一些数学推导了，呵呵，看了stackoverflow页面，有了四元数的思想，算法1还是很好理解，这个算法2也是想把之前imu的工作结束掉，花了些时间推导了一下。
$\overrightarrow{v_1}\cdot\overrightarrow{v_2}=ab\cos\theta$
$\overrightarrow{v_1}\times\overrightarrow{v_2}=ab\sin\theta\overrightarrow{u}$
于是
$\overrightarrow{v_1}\cdot\overrightarrow{v_2}+ab=ab（\cos\theta+1）=2ab\cos^2\frac{\theta}{2}$
$\overrightarrow{v_1}\times\overrightarrow{v_2}=ab\sin\theta\overrightarrow{u}=ab\sin\theta\overrightarrow{u}=2ab\sin\frac{\theta}{2}\cos\frac{\theta}{2}\overrightarrow{u}$
向量$[2ab\cos^2\frac{\theta}{2},2ab\sin\frac{\theta}{2}\cos\frac{\theta}{2}\overrightarrow{u}]$ 归一化得到$[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\overrightarrow{u}]$，正是所要计算的四元数$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

---