四元数 旋转矩阵 基础运算

文章目录

    • 四元数相关运算
    • 旋转矩阵相关运算

四元数相关运算

四元数 q = [ q x q y q z q w ] T = [ q v q w ] T q=\begin{bmatrix}q_x & q_y & q_z & q_w\end{bmatrix}^T = \begin{bmatrix}q_v & q_w\end{bmatrix}^T q=[qxqyqzqw]T=[qvqw]T
p ⊗ q = [ p w − p z p y p x p z p w − p x p y − p y p x p w p z − p x − p y − p z p w ] [ q x q y q z q w ] = [ p w q x − p z q y + p y q z + p x q w p z q x + p w q y − p x q z + p y q w − p y q x + p x q y + p w q z + p z q w − p x q x − p y q y − p z q z + p w q w ] = [ p w q w − p v T q v p w q v + q w p v + p v × q v ] p \otimes q = \begin{bmatrix} p_w & -p_z & p_y & p_x \\ p_z & p_w & -p_x & p_y \\ -p_y & p_x & p_w & p_z \\ -p_x & -p_y & -p_z & p_w \end{bmatrix}\begin{bmatrix}q_x \\ q_y \\ q_z \\ q_w\end{bmatrix} = \begin{bmatrix} p_wq_x - p_zq_y + p_yq_z + p_xq_w \\ p_zq_x + p_wq_y - p_xq_z + p_yq_w \\ -p_yq_x + p_xq_y + p_wq_z + p_zq_w \\ -p_xq_x - p_yq_y - p_zq_z + p_wq_w \end{bmatrix} = \begin{bmatrix} p_wq_w - p_v^Tq_v \\ p_wq_v + q_wp_v + p_v \times q_v \end{bmatrix} pq=pwpzpypxpzpwpxpypypxpwpzpxpypzpwqxqyqzqw=pwqxpzqy+pyqz+pxqwpzqx+pwqypxqz+pyqwpyqx+pxqy+pwqz+pzqwpxqxpyqypzqz+pwqw=[pwqwpvTqvpwqv+qwpv+pv×qv]
p v × q v = [ p x p y p z ] × [ q x q y q z ] = [ 0 − p x p y p x 0 − p z − p y p z 0 ] [ q x q y q z ] = [ − p x q y + p y q z p x q x − p z q z − p y q x + p z q y ] p_v \times q_v = \begin{bmatrix}p_x \\ p_y \\ p_z\end{bmatrix} \times \begin{bmatrix} q_x \\ q_y \\ q_z \end{bmatrix} = \begin{bmatrix}0 & -p_x & p_y \\ p_x & 0 & -p_z \\ -p_y & p_z & 0\end{bmatrix}\begin{bmatrix}q_x \\ q_y \\ q_z\end{bmatrix} = \begin{bmatrix} -p_xq_y + p_yq_z \\ p_xq_x - p_zq_z \\ -p_yq_x + p_zq_y \end{bmatrix} pv×qv=pxpypz×qxqyqz=0pxpypx0pzpypz0qxqyqz=pxqy+pyqzpxqxpzqzpyqx+pzqy
结合律:
( p ⊗ q ) ⊗ r = p ⊗ ( q ⊗ r ) \left(p \otimes q\right) \otimes r = p \otimes \left(q \otimes r\right) (pq)r=p(qr)
分配律:
p ⊗ ( q + r ) = p ⊗ q + p ⊗ r p \otimes \left(q + r\right) = p \otimes q + p \otimes r p(q+r)=pq+pr
( p + q ) ⊗ r = p ⊗ r + q ⊗ r \left(p +q\right) \otimes r = p \otimes r + q \otimes r (p+)r=pr+qr
左乘右乘:
q 1 ⊗ q 2 = [ q 1 ] L q 2 q_1 \otimes q_2 = \left[q_1\right]_Lq_2 q1q2=[q1]Lq2
q 1 ⊗ q 2 = [ q 2 ] R q 1 q_1 \otimes q_2 = \left[q_2\right]_Rq_1 q1q2=[q2]Rq1
[ q ] L = [ q w − q z q y q x q z q w − q x q y − q y q x q w q z − q x − q y − q z q w ] = q w I + [ [ q v ] × q v − q v T 0 ] \left[q\right]_L = \begin{bmatrix} q_w & -q_z & q_y & q_x \\ q_z & q_w & -q_x & q_y \\ -q_y & q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix} = q_wI + \begin{bmatrix} \left[q_v\right]_{\times} & q_v \\ \\ -q_v^{T} & 0 \end{bmatrix} [q]L=qwqzqyqxqzqwqxqyqyqxqwqzqxqyqzqw=qwI+[qv]×qvTqv0
[ q ] R = [ q w q z − q y q x − q z q w q x q y q y − q x q w q z − q x − q y − q z q w ] = q w I + [ − [ q v ] × q v − q v T 0 ] \left[q\right]_R = \begin{bmatrix} q_w & q_z & -q_y & q_x \\ -q_z & q_w & q_x & q_y \\ q_y & -q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix} = q_wI + \begin{bmatrix}-\left[q_v\right]_{\times} & q_v \\ \\ -q_v^T & 0\end{bmatrix} [q]R=qwqzqyqxqzqwqxqyqyqxqwqzqxqyqzqw=qwI+[qv]×qvTqv0
[ a ] × ≜ [ 0 − a z a y a z 0 − a x − a y a x 0 ] \left[a\right]_{\times} \triangleq \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix} [a]×0azayaz0axayax0
[ a ] × T = − [ a ] × \left[a\right]_{\times}^T = -\left[a\right]_{\times} [a]×T=[a]×
[ a ] × b = a × b ,      ∀ a , b ∈ R \left[a\right]_{\times}b = a \times b, \ \ \ \ \forall a, b \in \mathbb{R} [a]×b=a×b,    a,bR
Finally, since
( q ⊗ x ) ⊗ p = [ p ] R [ q ] L x \left(q \otimes x\right) \otimes p = \left[p\right]_R\left[q\right]_L x (qx)p=[p]R[q]Lx
q ⊗ ( x ⊗ p ) = [ q ] L [ p ] R x q\otimes\left(x\otimes p\right) = \left[q\right]_L\left[p\right]_Rx q(xp)=[q]L[p]Rx
[ p ] R [ q ] L = [ q ] L [ p ] R \left[p\right]_R\left[q\right]_L = \left[q\right]_L\left[p\right]_R [p]R[q]L=[q]L[p]R
四元数的逆:
q − 1 = q ∗         q ⊗ q ∗ = 1 q^{-1} = q^{*} \ \ \ \ \ \ \ q \otimes q^* = 1 q1=q       qq=1
四元数的导数:
q ˙ = 1 2 q ⊗ ω \dot{q} = \frac{1}{2}q \otimes \omega q˙=21qω
指数映射:
q = exp ⁡ ( u ϕ / 2 ) q = \exp\left(u \phi / 2\right) q=exp(uϕ/2)
对数映射:
log ⁡ ( q ) = u ϕ / 2 \log\left(q\right) = u \phi / 2 log(q)=uϕ/2
两个四元数运算:
q 1 ⊗ q 2 q_1 \otimes q_2 q1q2
旋转操作:
q = cos ⁡ ( ϕ / 2 ) + u sin ⁡ ( ϕ / 2 ) q = \cos \left(\phi / 2\right) + u \sin \left(\phi / 2\right) q=cos(ϕ/2)+usin(ϕ/2)
旋转作用:
q ⊗ x ⊗ q ∗ q \otimes x \otimes q^* qxq

旋转矩阵相关运算

R x = [ 1 0 0 0 cos ⁡ ( θ x ) − sin ⁡ ( θ x ) 0 sin ⁡ ( θ x ) cos ⁡ ( θ x ) ] R_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) \\ 0 & \sin\left(\theta_x\right) & \cos\left(\theta_x\right) \end{bmatrix} Rx=1000cos(θx)sin(θx)0sin(θx)cos(θx)
R y = [ cos ⁡ ( θ x ) 0 sin ⁡ ( θ x ) 0 1 0 − sin ⁡ ( θ x ) 0 cos ⁡ ( θ x ) ] R_y = \begin{bmatrix} \cos\left(\theta_x\right) & 0 & \sin\left(\theta_x\right) \\ 0 & 1 & 0 \\ -\sin\left(\theta_x\right) & 0 & \cos\left(\theta_x\right) \end{bmatrix} Ry=cos(θx)0sin(θx)010sin(θx)0cos(θx)
R z = [ cos ⁡ ( θ x ) − sin ⁡ ( θ x ) 0 sin ⁡ ( θ x ) cos ⁡ ( θ x ) 0 0 0 1 ] R_z = \begin{bmatrix} \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) & 0 \\ \sin\left(\theta_x\right) & \cos\left(\theta_x\right) & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz=cos(θx)sin(θx)0sin(θx)cos(θx)0001
旋转矩阵的逆:
R − 1 = R T       R R − 1 = R R T = I      d e t ( R ) = 1 R^{-1} = R^T \ \ \ \ \ RR^{-1} = RR^T = I \ \ \ \ det\left(R\right) = 1 R1=RT     RR1=RRT=I    det(R)=1
旋转矩阵的导数:
R ˙ = R [ ω ] × \dot{R} = R\left[\omega\right]_{\times} R˙=R[ω]×
指数映射:
R = exp ⁡ ( [ u ϕ ] × ) R = \exp\left(\left[u \phi\right]_{\times}\right) R=exp([uϕ]×)
对数映射:
log ⁡ ( R ) = [ u ϕ ] × \log\left(R\right) = \left[u \phi\right]_{\times} log(R)=[uϕ]×
两个旋转矩阵运算:
R 1 R 2 R_1 R_2 R1R2
旋转操作:
R = I + sin ⁡ ( ϕ [ u ] × ) + ( 1 − cos ⁡ ( ϕ ) ) [ u ] × 2 R = I + \sin\left(\phi \left[u\right]_{\times}\right) + \left(1 - \cos\left(\phi\right)\right)\left[u\right]_{\times}^2 R=I+sin(ϕ[u]×)+(1cos(ϕ))[u]×2
旋转作用:
R x Rx Rx

你可能感兴趣的:(SLAM)