四元数表示向量V1到V2的旋转

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

博主：shenshikexmu

联系方式：[e-mail]

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

问题

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

当知道单位向量u→和θ角时，这个四元数表示起来很简单。
q=cosθ2+sinθ2u→
也就是：
q0=cosθ2
q1=sinθ2u→.x
q2=sinθ2u→.y
q3=sinθ2u→.z

在v1→和v2→已知的条件下，角θ可以利用v1→和v2→内积，也就是⋅乘计算出来，向量u→可以利用v1→和v2→外积，也就是×乘计算出来。

设:
v1→方向上的单位向量为nv1−→−长度为a。于是v1→=a⋅nv1−→−。
v2→方向上的单位向量为nv2−→−长度为b。于是v2→=b⋅nv2−→−。
u→已经为单位向量。
那么有如下关系：
v1→⋅v2→=abcosθ
v1→×v2→=absinθu→

算法1

思路：寻找v1→和v2→中间的向量half−→−，这样v1→与half−→−的夹角是θ2，v1→与half−→−的内积方向与u→相同。
使half−→−变成单位向量。
half−→−=（v1→+v2→）/norm(v1→+v2→)
于是
nv1−→−⋅half−→−=cosθ2
nv1−→−×half−→−=sinθ2u→

q=cosθ2+sinθ2u→变为
q=nv1−→−⋅half−→−+nv1−→−×half−→−

function [q] = qUtoV(v1, v2)        %Finding quaternion representing the rotation from one vector to anothernv1 = 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)];endend

算法2

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

function [q] = qUtoV2(v1, v2) %Finding quaternion representing the rotation from one vector to anothernv1 = 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);endend

---