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

时间:2021-07-16 17:19:33

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

博主:shenshikexmu

联系方式:shenshikexmu@163.com

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

问题

如下图,三维空间中的向量 v 1 绕着单位向量 u 旋转 θ 角后,形成 v 2 。已知 v 1 v 2 求出代表向量间旋转的四元数。

四元数表示向量V1到V2的旋转的两种算法
当知道单位向量 u θ 角时,这个四元数表示起来很简单。
q = cos θ 2 + sin θ 2 u
也就是:
q 0 = cos θ 2
q 1 = sin θ 2 u . x
q 2 = sin θ 2 u . y
q 3 = sin θ 2 u . z

v 1 v 2 已知的条件下,角 θ 可以利用 v 1 v 2 内积,也就是 乘计算出来,向量 u 可以利用 v 1 v 2 外积,也就是 × 乘计算出来。

设:
v 1 方向上的单位向量为 n v 1 长度为a。于是 v 1 = a n v 1
v 2 方向上的单位向量为 n v 2 长度为b。于是 v 2 = b n v 2
u 已经为单位向量。
那么有如下关系:
v 1 v 2 = a b cos θ
v 1 × v 2 = a b sin θ u

算法1

四元数表示向量V1到V2的旋转的两种算法
思路:寻找 v 1 v 2 中间的向量 h a l f ,这样 v 1 h a l f 的夹角是 θ 2 v 1 h a l f 的内积方向与 u 相同。
使 h a l f 变成单位向量。
h a l f = v 1 + v 2 / n o r m ( v 1 + v 2 )
于是
n v 1 h a l f = cos θ 2
n v 1 × h a l f = sin θ 2 u

q = cos θ 2 + sin θ 2 u 变为
q = n v 1 h a l f + n v 1 × h a l f

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的工作结束掉,花了些时间推导了一下。
v 1 v 2 = a b cos θ
v 1 × v 2 = a b sin θ u
于是
v 1 v 2 + a b = a b cos θ + 1 = 2 a b cos 2 θ 2
v 1 × v 2 = a b sin θ u = a b sin θ u = 2 a b sin θ 2 cos θ 2 u
向量 [ 2 a b cos 2 θ 2 , 2 a b sin θ 2 cos θ 2 u ] 归一化得到 [ cos θ 2 , sin θ 2 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