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

本文为博主“声时刻”原创文章，未经博主允许不得转载。
联系方式：shenshikexmu@163.com

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

##问题
如下图，三维空间中的向量$\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{n{v}_1}$长度为a。于是$\overrightarrow{v_1}=a\cdot \overrightarrow{n{v}_1}$。
$\overrightarrow{v_2}$方向上的单位向量为$\overrightarrow{n{v}_2}$长度为b。于是$\overrightarrow{v_2}=b\cdot \overrightarrow{n{v}_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{n{v}_1}+\overrightarrow{n{v}_2}）/norm(\overrightarrow{n{v}_1}+\overrightarrow{n{v}_2})$
于是
$\overrightarrow{n{v}_1}\cdot \overrightarrow{half}=\cos \frac{\theta }{2}$
$\overrightarrow{n{v}_1}\times \overrightarrow{half}=\sin \frac{\theta }{2}\overrightarrow{u}$

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

---