【机器人学】四元数与旋转矩阵的相互转换

1. 从四元数到旋转矩阵

推导过程利用以下矩阵(该矩阵的推导过程链接),它能计算绕任意轴的旋转:
R ( n , θ ) = [ ( 1 − c o s θ ) n x 2 + c o s θ ( 1 − c o s θ ) n x n y − n z s i n θ ( 1 − c o s θ ) n x n z + n y s i n θ ( 1 − c o s θ ) n x n y + n z s i n θ ( 1 − c o s θ ) n y 2 + c o s θ ( 1 − c o s θ ) n y n z − n x s i n θ ( 1 − c o s θ ) n x n z − n y s i n θ ( 1 − c o s θ ) n y n z + n x s i n θ ( 1 − c o s θ ) n z 2 + c o s θ ] \textbf{R}(\textbf{n},\theta)=\begin{bmatrix} (1-cos\theta)n_x^2+cos\theta & (1-cos\theta)n_xn_y-n_zsin\theta & (1-cos\theta)n_xn_z+n_ysin\theta\\ (1-cos\theta)n_xn_y+n_zsin\theta & (1-cos\theta)n_y^2+cos\theta & (1-cos\theta)n_yn_z-n_xsin\theta\\ (1-cos\theta)n_xn_z-n_ysin\theta & (1-cos\theta)n_yn_z+n_xsin\theta & (1-cos\theta)n_z^2+cos\theta\\ \end{bmatrix} R(n,θ)=(1cosθ)nx2+cosθ(1cosθ)nxny+nzsinθ(1cosθ)nxnznysinθ(1cosθ)nxnynzsinθ(1cosθ)ny2+cosθ(1cosθ)nynz+nxsinθ(1cosθ)nxnz+nysinθ(1cosθ)nynznxsinθ(1cosθ)nz2+cosθ
该矩阵用 n \textbf{n} n θ \theta θ 表示,但其中四元数的分量是:
w = c o s ( θ / 2 ) x = n x s i n ( θ / 2 ) y = n y s i n ( θ / 2 ) z = n z s i n ( θ / 2 ) \begin{aligned} w&=cos(\theta/2)\\ x&=\textbf{n}_xsin(\theta/2)\\ y&=\textbf{n}_ysin(\theta/2)\\ z&=\textbf{n}_zsin(\theta/2)\\ \end{aligned} wxyz=cos(θ/2)=nxsin(θ/2)=nysin(θ/2)=nzsin(θ/2)
将矩阵变形以带入 w , x , y , z w,x,y,z w,x,y,z。首先考虑矩阵对角线上的元素:
m 11 = n x 2 ( 1 − c o s θ ) + c o s θ = n x 2 − n x 2 c o s θ + c o s θ = 1 − 1 + n x 2 − n x 2 c o s θ + c o s θ = 1 − ( 1 − n x 2 + n x 2 c o s θ − c o s θ ) = 1 − ( 1 − c o s θ − n x 2 + n x 2 c o s θ ) = 1 − ( 1 − n x 2 ) ( 1 − c o s θ ) \begin{aligned} m_{11}&= \textbf{n}_x^2(1-cos\theta)+cos\theta\\ &=\textbf{n}_x^2-\textbf{n}_x^2cos\theta+cos\theta\\ &=1-1+\textbf{n}_x^2-\textbf{n}_x^2cos\theta+cos\theta\\ &=1-(1-\textbf{n}_x^2+\textbf{n}_x^2cos\theta-cos\theta)\\ &=1-(1-cos\theta-\textbf{n}_x^2+\textbf{n}_x^2cos\theta)\\ &=1-(1-\textbf{n}_x^2)(1-cos\theta)\\ \end{aligned} m11=nx2(1cosθ)+cosθ=nx2nx2cosθ+cosθ=11+nx2nx2cosθ+cosθ=1(1nx2+nx2cosθcosθ)=1(1cosθnx2+nx2cosθ)=1(1nx2)(1cosθ)
根据三角函数倍角公式有 c o s θ = 1 − 2 s i n 2 ( θ / 2 ) cos\theta=1-2sin^2(\theta/2) cosθ=12sin2(θ/2),带入上式:
m 11 = 1 − ( 1 − n x 2 ) ( 1 − c o s θ ) = 1 − ( 1 − n x 2 ) ( 2 s i n 2 ( θ / 2 ) ) = 1 − 2 s i n 2 ( θ / 2 ) + 2 n x 2 s i n 2 ( θ / 2 ) = 1 − 2 ( 1 − c o s 2 ( θ / 2 ) ) + 2 n x 2 s i n 2 ( θ / 2 ) = 1 − 2 + 2 c o s 2 ( θ / 2 ) ) + 2 n x 2 s i n 2 ( θ / 2 ) = − 1 + 2 c o s 2 ( θ / 2 ) ) + 2 n x 2 s i n 2 ( θ / 2 ) \begin{aligned} m_{11}&=1-(1-\textbf{n}_x^2)(1-cos\theta)\\ &=1-(1-\textbf{n}_x^2)(2sin^2(\theta/2))\\ &=1-2sin^2(\theta/2)+2\textbf{n}_x^2sin^2(\theta/2)\\ &=1-2(1-cos^2(\theta/2))+2\textbf{n}_x^2sin^2(\theta/2)\\ &=1-2+2cos^2(\theta/2))+2\textbf{n}_x^2sin^2(\theta/2)\\ &=-1+2cos^2(\theta/2))+2\textbf{n}_x^2sin^2(\theta/2)\\ \end{aligned} m11=1(1nx2)(1cosθ)=1(1nx2)(2sin2(θ/2))=12sin2(θ/2)+2nx2sin2(θ/2)=12(1cos2(θ/2))+2nx2sin2(θ/2)=12+2cos2(θ/2))+2nx2sin2(θ/2)=1+2cos2(θ/2))+2nx2sin2(θ/2)
带入 w , x w,x w,x即可求得, m 11 = − 1 + 2 w 2 + 2 x 2 m_{11}=-1+2w^2+2x^2 m11=1+2w2+2x2,等价于 m 11 = 1 − 2 y 2 − 2 z 2 m_{11}=1-2y^2-2z^2 m11=12y22z2 m 11 = w 2 + x 2 − y 2 − z 2 m_{11}=w^2+x^2-y^2-z^2 m11=w2+x2y2z2
对角元素 m 22 , m 33 m_{22},m_{33} m22,m33可以用同样的方式求得。

m 12 m_{12} m12为例,使用三角形的倍角公式 s i n θ = 2 s i n ( θ / 2 ) c o s ( θ / 2 ) sin\theta=2sin(\theta/2)cos(\theta/2) sinθ=2sin(θ/2)cos(θ/2) 推导非对角线元素,其他非对角线元素可用类似方式推导:
m 12 = n x n y ( 1 − c o s θ ) − n z s i n θ = n x n y ( 1 − ( 1 − 2 2 s i n ( θ / 2 ) ) ) − n z 2 s i n ( θ / 2 ) c o s ( θ / 2 ) = n x n y ( 2 s i n 2 ( θ / 2 ) ) − 2 n z s i n ( θ / 2 ) c o s ( θ / 2 ) = 2 ( n x s i n ( θ / 2 ) n y s i n ( θ / 2 ) ) − 2 c o s ( θ / 2 ) ( n z s i n ( θ / 2 ) ) = 2 x y − 2 w z \begin{aligned} m_{12}&=\textbf{n}_x\textbf{n}_y(1-cos\theta)-\textbf{n}_zsin\theta\\ &=\textbf{n}_x\textbf{n}_y(1-(1-2^2sin(\theta/2)))-\textbf{n}_z2sin(\theta/2)cos(\theta/2)\\ &=\textbf{n}_x\textbf{n}_y(2sin^2(\theta/2))-2\textbf{n}_zsin(\theta/2)cos(\theta/2)\\ &=2(\textbf{n}_xsin(\theta/2)\textbf{n}_ysin(\theta/2))-2cos(\theta/2)(\textbf{n}_zsin(\theta/2))\\ &=2xy-2wz \end{aligned} m12=nxny(1cosθ)nzsinθ=nxny(1(122sin(θ/2)))nz2sin(θ/2)cos(θ/2)=nxny(2sin2(θ/2))2nzsin(θ/2)cos(θ/2)=2(nxsin(θ/2)nysin(θ/2))2cos(θ/2)(nzsin(θ/2))=2xy2wz
最终推导出的四元数构造的完整旋转矩阵为以下形式:
R ( n , θ ) = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2 x z + 2 w y 2 x y + 2 w z 1 − 2 x 2 − 2 z 2 2 y z − 2 w x 2 x z − 2 w y 2 y z + 2 w x 1 − 2 x 2 − 2 y 2 ] (2) R(n,\theta)= \begin{bmatrix} 1-2y^2-2z^2 & 2xy-2wz & 2xz+2wy \\ 2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx \\ 2xz-2wy & 2yz+2wx & 1-2x^2-2y^2 \\ \end{bmatrix} \tag{2} R(n,θ)=12y22z22xy+2wz2xz2wy2xy2wz12x22z22yz+2wx2xz+2wy2yz2wx12x22y2(2)

2. 从旋转矩阵到四元数

利用公式(2),将对角线元素求和可以得到:
t r ( M ) = m 11 + m 22 + m 33 = ( 1 − 2 y 2 − 2 z 2 ) + ( 1 − 2 x 2 − 2 z 2 ) + ( 1 − 2 x 2 − 2 y 2 ) = 3 − 4 ( x 2 + y 2 + z 2 ) = 3 − 4 ( 1 − w 2 ) = 4 w 2 − 1 \begin{aligned} tr(\textbf{M})&=m11+m22+m33\\ &=(1-2y^2-2z^2)+(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ &=3-4(x^2+y^2+z^2)\\ &=3-4(1-w^2)\\ &=4w^2-1\\ \end{aligned} tr(M)=m11+m22+m33=(12y22z2)+(12x22z2)+(12x22y2)=34(x2+y2+z2)=34(1w2)=4w21
因此可以推导出: w = m 11 + m 22 + m 33 + 1 2 w=\frac{\sqrt{m11+m22+m33+1}}{2} w=2m11+m22+m33+1
通过使轨迹中三个元素中的两个为负,可以用类似的方法求得其他三个元素:
m 11 − m 22 − m 33 = ( 1 − 2 y 2 − 2 z 2 ) − ( 1 − 2 x 2 − 2 z 2 ) − ( 1 − 2 x 2 − 2 y 2 ) = 4 x 2 − 1 − m 11 + m 22 − m 33 = − ( 1 − 2 y 2 − 2 z 2 ) + ( 1 − 2 x 2 − 2 z 2 ) − ( 1 − 2 x 2 − 2 y 2 ) = 4 y 2 − 1 − m 11 − m 22 + m 33 = − ( 1 − 2 y 2 − 2 z 2 ) − ( 1 − 2 x 2 − 2 z 2 ) + ( 1 − 2 x 2 − 2 y 2 ) = 4 z 2 − 1 \begin{aligned} m11-m22-m33&=(1-2y^2-2z^2)-(1-2x^2-2z^2)-(1-2x^2-2y^2)\\ &=4x^2-1\\ -m11+m22-m33&=-(1-2y^2-2z^2)+(1-2x^2-2z^2)-(1-2x^2-2y^2)\\ &=4y^2-1\\ -m11-m22+m33&=-(1-2y^2-2z^2)-(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ &=4z^2-1\\ \end{aligned} m11m22m33m11+m22m33m11m22+m33=(12y22z2)(12x22z2)(12x22y2)=4x21=(12y22z2)+(12x22z2)(12x22y2)=4y21=(12y22z2)(12x22z2)+(12x22y2)=4z21
这种方法是存在一定问题的:平方根的结果总是正值(没有选择是负根还是正根的依据) q q q − q -q q代表相同的方位,因此我们可以任意选择用非负根作为4个分量中的一个并仍能得到正确的四元数,只是不能对四元数的所有4个数都用这种方法。

可以考虑计算相对于对角线的对称位置上元素的和与差:
m 21 + m 12 = ( 2 x y + 2 w z ) + ( 2 x y − 2 w z ) = 4 x y m 21 − m 12 = ( 2 x y + 2 w z ) − ( 2 x y − 2 w z ) = 4 w z m 13 + m 31 = ( 2 x z + 2 w y ) + ( 2 x z − 2 w y ) = 4 x z m 13 − m 31 = ( 2 x z + 2 w y ) − ( 2 x z − 2 w y ) = 4 w y m 32 + m 23 = ( 2 y z + 2 w x ) + ( 2 y z − 2 w x ) = 4 y z m 32 − m 23 = ( 2 y z + 2 w x ) − ( 2 y z − 2 w x ) = 4 w x \begin{aligned} m21+m12&=(2xy+2wz)+(2xy-2wz)=4xy\\ m21-m12&=(2xy+2wz)-(2xy-2wz)=4wz\\ m13+m31&=(2xz+2wy)+(2xz-2wy)=4xz\\ m13-m31&=(2xz+2wy)-(2xz-2wy)=4wy\\ m32+m23&=(2yz+2wx)+(2yz-2wx)=4yz\\ m32-m23&=(2yz+2wx)-(2yz-2wx)=4wx\\ \end{aligned} m21+m12m21m12m13+m31m13m31m32+m23m32m23=(2xy+2wz)+(2xy2wz)=4xy=(2xy+2wz)(2xy2wz)=4wz=(2xz+2wy)+(2xz2wy)=4xz=(2xz+2wy)(2xz2wy)=4wy=(2yz+2wx)+(2yz2wx)=4yz=(2yz+2wx)(2yz2wx)=4wx
因此,一旦用对角线元素和/差的平方根解得了4个值中的一个,就能用以下方法计算其他三个:
w = m 11 + m 22 + m 33 + 1 2    ⟹    x = m 32 − m 23 4 w y = m 13 − m 31 4 w z = m 21 − m 12 4 w x = m 11 − m 22 − m 33 + 1 2    ⟹    w = m 32 − m 23 4 x y = m 21 + m 12 4 x z = m 13 + m 31 4 x y = − m 11 + m 22 − m 33 + 1 2    ⟹    w = m 13 − m 31 4 y x = m 12 + m 21 4 y z = m 23 + m 32 4 y z = − m 11 − m 22 + m 33 + 1 2    ⟹    w = m 21 − m 12 4 z x = m 13 + m 31 4 z y = m 23 + m 32 4 z \begin{aligned} w=\frac{\sqrt{m11+m22+m33+1}}{2}\implies&x=\frac{m32-m23}{4w} \quad\quad y=\frac{m13-m31}{4w} \quad\quad z=\frac{m21-m12}{4w} \\ x=\frac{\sqrt{m11-m22-m33+1}}{2}\implies&w=\frac{m32-m23}{4x} \quad\quad y=\frac{m21+m12}{4x} \quad\quad z=\frac{m13+m31}{4x} \\ y=\frac{\sqrt{-m11+m22-m33+1}}{2}\implies&w=\frac{m13-m31}{4y} \quad\quad x=\frac{m12+m21}{4y} \quad\quad z=\frac{m23+m32}{4y} \\ z=\frac{\sqrt{-m11-m22+m33+1}}{2}\implies&w=\frac{m21-m12}{4z} \quad\quad x=\frac{m13+m31}{4z} \quad\quad y=\frac{m23+m32}{4z} \\ \end{aligned} w=2m11+m22+m33+1 x=2m11m22m33+1 y=2m11+m22m33+1 z=2m11m22+m33+1 x=4wm32m23y=4wm13m31z=4wm21m12w=4xm32m23y=4xm21+m12z=4xm13+m31w=4ym13m31x=4ym12+m21z=4ym23+m32w=4zm21m12x=4zm13+m31y=4zm23+m32
我们可以先计算四个分量中的其中一个,比如 w w w,然后在计算 x , y , z x,y,z x,y,z。这样也会出现问题,如果 w = 0 w=0 w=0,除法就没有意义了,如果 w w w非常小,将会出现数值不稳定。所以我们可以采取的策略是,先判断 w , x , y , z w,x,y,z w,x,y,z中哪个元素最大,用矩阵对角线计算该元素,再计算其他三个。

你可能感兴趣的:(机器人学,运动学,矩阵,线性代数,算法,数学)