四元数 旋转矩阵 基础运算

四元数相关运算

四元数 $q={\left[\begin{array}{cccc}{q}_x& q_y& q_z& q_w\end{array}\right]}^T={\left[\begin{array}{cc}{q}_v& q_w\end{array}\right]}^T$ 则
$$p\otimes q=\left[\begin{array}{cccc}{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{array}\right]\left[\begin{array}{c}{q}_x\\ q_y\\ q_z\\ q_w\end{array}\right]=\left[\begin{array}{c}{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\end{array}\right]=\left[\begin{array}{c}{p}_w{q}_w-p_v^T{q}_v\\ p_w{q}_v+q_w{p}_v+p_v\times q_v\end{array}\right]$$
$$p_v\times q_v=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]\times \left[\begin{array}{c}{q}_x\\ q_y\\ q_z\end{array}\right]=\left[\begin{array}{ccc}0& -p_x& p_y\\ p_x& 0& -p_z\\ -p_y& p_z& 0\end{array}\right]\left[\begin{array}{c}{q}_x\\ q_y\\ q_z\end{array}\right]=\left[\begin{array}{c}-p_x{q}_y+p_y{q}_z\\ p_x{q}_x-p_z{q}_z\\ -p_y{q}_x+p_z{q}_y\end{array}\right]$$
结合律：
$$\left(p\otimes q\right)\otimes r=p\otimes \left(q\otimes r\right)$$
分配律：
$$p\otimes \left(q+r\right)=p\otimes q+p\otimes r$$
$$\left(p+ｑ\right)\otimes r=p\otimes r+q\otimes r$$
左乘右乘：
$$q_1\otimes q_2={\left[q_1\right]}_L{q}_2$$
$$q_1\otimes q_2={\left[q_2\right]}_R{q}_1$$
$${\left[q\right]}_L=\left[\begin{array}{cccc}{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{array}\right]=q_{w}I+\left[\begin{array}{cc}{\left[q_v\right]}_{\times }& q_v\\ & \\ -q_v^T& 0\end{array}\right]$$
$${\left[q\right]}_R=\left[\begin{array}{cccc}{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{array}\right]=q_{w}I+\left[\begin{array}{cc}-{\left[q_v\right]}_{\times }& q_v\\ & \\ -q_v^T& 0\end{array}\right]$$
$${\left[a\right]}_{\times }\triangleq \left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$$
$${\left[a\right]}_{\times }^T=-{\left[a\right]}_{\times }$$
$${\left[a\right]}_{\times }b=a\times b,\forall a,b\in \mathbb{R}$$
Finally, since
$$\left(q\otimes x\right)\otimes p={\left[p\right]}_R{\left[q\right]}_{L}x$$
$$q\otimes \left(x\otimes p\right)={\left[q\right]}_L{\left[p\right]}_{R}x$$
$${\left[p\right]}_R{\left[q\right]}_L={\left[q\right]}_L{\left[p\right]}_R$$
四元数的逆：
$$q^{-1}=q^{\ast }q\otimes q^{\ast }=1$$
四元数的导数：
$$\dot{q}=\frac{1}{2}q\otimes \omega$$
指数映射：
$$q=\exp \left(u\varphi /2\right)$$
对数映射：
$$\log \left(q\right)=u\varphi /2$$
两个四元数运算：
$$q_1\otimes q_2$$
旋转操作：
$$q=\cos \left(\varphi /2\right)+u\sin \left(\varphi /2\right)$$
旋转作用：
$$q\otimes x\otimes q^{\ast }$$

旋转矩阵相关运算

$$R_x=\left[\begin{array}{ccc}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{array}\right]$$
$$R_y=\left[\begin{array}{ccc}\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{array}\right]$$
$$R_z=\left[\begin{array}{ccc}\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{array}\right]$$
旋转矩阵的逆：
$$R^{-1}=R^{T}R{R}^{-1}=R{R}^T=Idet\left(R\right)=1$$
旋转矩阵的导数：
$$\dot{R}=R{\left[\omega \right]}_{\times }$$
指数映射：
$$R=\exp \left({\left[u\varphi \right]}_{\times }\right)$$
对数映射：
$$\log \left(R\right)={\left[u\varphi \right]}_{\times }$$
两个旋转矩阵运算：
$$R_1{R}_2$$
旋转操作：
$$R=I+\sin \left(\varphi {\left[u\right]}_{\times }\right)+\left(1-\cos \left(\varphi \right)\right){\left[u\right]}_{\times }^2$$
旋转作用：
$$Rx$$