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

本文为博主“声时刻”原创文章，未经博主允许不得转载。 联系方式：[email protected]

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

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

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

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

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

##算法1 思路：寻找 v 1 → \overrightarrow{v_1} v1​ ​和 v 2 → \overrightarrow{v_2} v2​ ​中间的向量 h a l f → \overrightarrow{half} half ​，这样 v 1 → \overrightarrow{v_1} v1​ ​与 h a l f → \overrightarrow{half} half ​的夹角是 θ 2 \frac{\theta}{2} 2θ​， v 1 → \overrightarrow{v_1} v1​ ​与 h a l f → \overrightarrow{half} half ​的外积方向与 u → \overrightarrow{u} u 相同。 使 h a l f → \overrightarrow{half} half ​变成单位向量。

h a l f → = （ n v 1 → + n v 2 → ） / n o r m ( n v 1 → + n v 2 → ) \overrightarrow{half}=（\overrightarrow{nv_1}+\overrightarrow{nv_2}）/norm(\overrightarrow{nv_1}+\overrightarrow{nv_2}) half ​=（nv1​ ​+nv2​ ​）/norm(nv1​ ​+nv2​ ​) 于是 n v 1 → ⋅ h a l f → = cos ⁡ θ 2 \overrightarrow{nv_1}\cdot\overrightarrow{half}=\cos\frac{\theta}{2} nv1​ ​⋅half ​=cos2θ​ n v 1 → × h a l f → = sin ⁡ θ 2 u → \overrightarrow{nv_1}\times\overrightarrow{half}=\sin\frac{\theta}{2}\overrightarrow{u} nv1​ ​×half ​=sin2θ​u

q = cos ⁡ θ 2 + sin ⁡ θ 2 u → q=\cos\frac{\theta}{2}+\sin\frac{\theta}{2}\overrightarrow{u} q=cos2θ​+sin2θ​u 变为 q = n v 1 → ⋅ h a l f → + n v 1 → × h a l f → q=\overrightarrow{nv_1}\cdot\overrightarrow{half}+\overrightarrow{nv_1}\times\overrightarrow{half} q=nv1​ ​⋅half ​+nv1​ ​×half ​

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