2020-10-13 四元数用法（不讲原理，只讲计算规则）

本文不讲四元数的定义，直接讲操作实例和代码。

对象：刚体姿态

坐标系定义：

1. 本体坐标系 $B$，
2. 参考坐标系 $R$，
3. 惯性坐标系 $I$

涉及四元数定义：
$\mathbf{q}=\left[\begin{array}{l}{q}_0\\ {\mathbf{q}}_v\end{array}\right]$表示$R\to B$的四元数（也就是 $B$ 相对于 $R$），
${\mathbf{q}}^{\prime }=\left[\begin{array}{l}{q}_0^{\prime }\\ {\mathbf{q}}_v^{\prime }\end{array}\right]$表示$I\to R$的四元数（也就是 $R$ 相对于 $I$），
${\mathbf{q}}^{\prime \prime }=\left[\begin{array}{l}{q}_0^{\prime \prime }\\ {\mathbf{q}}_v^{\prime \prime }\end{array}\right]$表示$I\to B$的四元数（也就是 $B$ 相对于 $I$）。

定义：
$\mathbf{q}$ 的逆表示为 $\bar{\mathbf{q}}=\left[\begin{array}{r}{q}_0\\ -{\mathbf{q}}_v\end{array}\right]$

四元数乘法（从左向右）：

【$I\to B$的四元数】 = 【$I\to R$的四元数】 $\otimes$ 【$R\to B$的四元数】
$${\mathbf{q}}^{\prime \prime }={\mathbf{q}}^{\prime }\otimes \mathbf{q}=\left[\begin{array}{l}{q}_0^{\prime }\ast q_0-{\mathbf{q}}_v^{\prime }\cdot {\mathbf{q}}_v\\ q_0^{\prime }\ast {\mathbf{q}}_v+q_0\ast {\mathbf{q}}_v^{\prime }+{\mathbf{q}}_v^{\prime }\times {\mathbf{q}}_v\end{array}\right]$$

【$R\to B$的四元数】 = 【$R\to I$的四元数】 $\otimes$ 【$I\to B$的四元数】
$$\mathbf{q}={\bar{\mathbf{q}}}^{\prime }\otimes {\mathbf{q}}^{\prime \prime }=\left[\begin{array}{l}{q}_0^{\prime }\ast q_0^{\prime \prime }+{\mathbf{q}}_v^{\prime }\cdot {\mathbf{q}}_v^{\prime \prime }\\ q_0^{\prime }\ast {\mathbf{q}}_v^{\prime \prime }-q_0^{\prime \prime }\ast {\mathbf{q}}_v^{\prime }-{\mathbf{q}}_v^{\prime }\times {\mathbf{q}}_v^{\prime \prime }\end{array}\right]$$

相应四元数旋转矩阵（从右向左）：

【$I\to B$的旋转矩阵】 = 【$R\to B$的旋转矩阵】 $\times$ 【$I\to R$的旋转矩阵】
$$\mathbf{A}({\mathbf{q}}^{\prime \prime })=\mathbf{A}(\mathbf{q})\mathbf{A}({\mathbf{q}}^{\prime })$$

其中
$$\begin{array}{l}\mathbf{A}(\mathbf{q})=\left(q_0^2-{\mathbf{q}}_v^{\mathrm{T}}{\mathbf{q}}_v\right)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\mathrm{T}}-2q_0\left[{\mathbf{q}}_v\times \right]\\ =\left[\begin{array}{lll}2\left(q_0^2+q_1^2\right)-1& 2\left(q_1{q}_2+q_0{q}_3\right)& 2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_0^2+q_2^2\right)-1& 2\left(q_2{q}_3+q_0{q}_1\right)\\ 2\left(q_1{q}_3+q_0{q}_2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)& 2\left(q_0^2+q_3^2\right)-1\end{array}\right]\end{array}$$
或者
$$\mathbf{A}(\mathbf{q})=\mathbf{I}-2q_0\left[{\mathbf{q}}_v\times \right]+2\left[{\mathbf{q}}_v\times \right]\left[{\mathbf{q}}_v\times \right]$$
和
$$\begin{array}{r}\left[{\mathbf{q}}_v\times \right]=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]\end{array}$$

Matlab代码

function L = TransMatrix(quat)
quat_0=quat(1);quat_1=quat(2);quat_2=quat(3);quat_3=quat(4);
L = [2*(quat_0^2+quat_1^2)-1  2*(quat_1*quat_2+quat_0*quat_3)  2*(quat_1*quat_3-quat_0*quat_2); ...
    2*(quat_1*quat_2-quat_0*quat_3) 2*(quat_0^2+quat_2^2)-1 2*(quat_2*quat_3+quat_0*quat_1); ...
    2*(quat_1*quat_3+quat_0*quat_2) 2*(quat_2*quat_3-quat_0*quat_1) 2*(quat_0^2+quat_3^2)-1];
end

四元数的一些性质：

1. ${\mathbf{q}}_v{\mathbf{q}}_v^{\mathrm{T}}-{\mathbf{q}}_v^{\mathrm{T}}{\mathbf{q}}_v\mathbf{I}=\left[{\mathbf{q}}_v\times \right]\left[{\mathbf{q}}_v\times \right]$

2. 旋转矩阵$\mathbf{A}(\mathbf{q})$的特征值是：$2q_0^2-1-2q_0{\left(q_0^2-1\right)}^{\frac{1}{2}}$，$2q_0^2-1+2q_0{\left(q_0^2-1\right)}^{\frac{1}{2}}$，和$1$.

3. 对于$\mathbf{q}=\left[\begin{array}{l}{q}_0\\ {\mathbf{q}}_v\end{array}\right]$，当$q_0=0$时，则有${\mathbf{q}}_v^T{\mathbf{q}}_v=1$，也可以推出$${\mathbf{q}}_v{\mathbf{q}}_v^T-{\mathbf{q}}_v^{\times }{\mathbf{q}}_v^{\times }=\mathbf{I}$$

4. $\left[\begin{array}{r}{q}_0\\ {\mathbf{q}}_v\end{array}\right]$和$\left[\begin{array}{r}-q_0\\ -{\mathbf{q}}_v\end{array}\right]$表示同样的姿态，$\left[\begin{array}{r}-q_0\\ {\mathbf{q}}_v\end{array}\right]$和$\left[\begin{array}{r}{q}_0\\ -{\mathbf{q}}_v\end{array}\right]$表示同样的姿态。

5. 四元数的代数性质：${\mathbf{q}}_a^T({\mathbf{q}}_b\otimes {\mathbf{q}}_c)={\mathbf{q}}_c^T({\mathbf{q}}_b^{\ast }\otimes {\mathbf{q}}_a)={\mathbf{q}}_b^T({\mathbf{q}}_a\otimes {\mathbf{q}}_c^{\ast })$
   
   $\text{Vec}({\mathbf{b}}^{\ast }\otimes \mathbf{a})=-\text{Vec}({\mathbf{a}}^{\ast }\otimes \mathbf{b})$

6. $$\text{ for any }\mathbf{x}\in {\mathbf{R}}^3,\mathbf{A}\in {\mathbf{R}}^{3\times 3}\text{, and }\mathbf{R}\in \mathrm{S}\mathrm{O}\text{ (3) }$$
   $$\begin{array}{rl}& \mathrm{tr}\left(\mathbf{A}(\mathbf{x}{)}^{\wedge }\right)=\frac{1}{2}\mathrm{tr}\left[(\mathbf{x}{)}^{\wedge }\left(\mathbf{A}-{\mathbf{A}}^{\mathrm{T}}\right)\right]=-{\mathbf{x}}^{\mathrm{T}}{\left(\mathbf{A}-{\mathbf{A}}^{\mathrm{T}}\right)}^{\vee }\\ & (\mathbf{x}{)}^{\wedge }\mathbf{A}+{\mathbf{A}}^{\mathrm{T}}(\mathbf{x}{)}^{\wedge }={\left[\left(\mathrm{tr}(\mathbf{A}){\mathbf{I}}_3-\mathbf{A}\right)\mathbf{x}\right]}^{\wedge }\\ & \mathbf{R}(\mathbf{x}{)}^{\wedge }{\mathbf{R}}^{\mathrm{T}}=(\mathbf{R}\mathbf{x}{)}^{\wedge }\end{array}$$

${}^{\wedge }$也就是上面的叉乘算子，${}^{\vee }$是其逆算子