AHRS基础知识（旋转矩阵(四元数）、向量叉乘、哥氏定理、四元数运算法则）

最新推荐文章于 2024-07-29 11:11:45 发布

置顶 qq_42348833

最新推荐文章于 2024-07-29 11:11:45 发布

阅读量1.6k

点赞数 3

分类专栏： AHRS 文章标签： matlab

本文链接：https://blog.csdn.net/qq_42348833/article/details/106121951

版权

AHRS 专栏收录该内容

4 篇文章 6 订阅

订阅专栏

旋转矩阵：《惯性导航》

所有观点参见秦永元教授的 《惯性导航》书第6页，书中的旋转矩阵为右乘矩阵（坐标变换时候，向量在旋转矩阵右侧，旋转矩阵在向量的左侧：b = R * a）。
以东北天参考系为例则：
旋转的顺序为先绕 Z轴（角度北偏东为正）旋转：
$\boldsymbol{C}_{n}^{1}=\boldsymbol{C}_{\mathrm{g}}^{1}=\left[\begin{array}{ccc} \cos \Psi & -\sin \Psi & 0 \\ \sin \Psi & \cos \Psi & 0 \\ 0 & 0 & 1 \end{array}\right]$
再绕 X轴旋转（Y->Z 为正），最后绕 Y轴旋转（Z->X 为正）。
$\begin{aligned} \boldsymbol{C}_{1}^{b} &=\boldsymbol{C}_{2}^{b} \boldsymbol{C}_{1}^{2}=\left[\begin{array}{ccc} \cos \gamma & 0 & -\sin \gamma \\ 0 & 1 & 0 \\ \sin \gamma & 0 & \cos \gamma \end{array}\right]\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & \sin \theta \\ 0 & -\sin \theta & \cos \theta \end{array}\right] \\ &=\left[\begin{array}{ccc} \cos \gamma & \sin \theta \sin \gamma & -\cos \theta \sin \gamma \\ 0 & \cos \theta & \sin \theta \\ \sin \gamma & -\sin \theta \cos \gamma & \cos \theta \cos \gamma \end{array}\right] \end{aligned}$
n系到b系的旋转矩阵为：
$\begin{aligned} \boldsymbol{C}_{n}^{b} &=\boldsymbol{C}_{1}^{b} \cdot \boldsymbol{C}_{n}^{1} \\ &=\left[\begin{array}{ccc} \cos \gamma_{\cos } \Psi+\sin \gamma_{\sin } \Psi \sin \theta & -\cos \gamma_{\sin } \Psi+\sin \gamma_{\cos } \Psi \sin \theta & -\sin \gamma_{\cos \theta} \\ \sin \Psi \cos \theta & \cos \Psi \cos \theta & \sin \theta \\ \sin \gamma_{\cos } \Psi-\cos \gamma_{\sin } \Psi \sin \theta & -\sin \gamma \sin \Psi-\cos \gamma_{\cos } \Psi \sin \theta & \cos \gamma_{\cos \theta} \end{array}\right] \end{aligned}$
旋转矩阵是一个正交矩阵：
$\boldsymbol{C}_{b}^{n}=\left(\boldsymbol{C}_{n}^{b}\right)^{-1}=\left(\boldsymbol{C}_{n}^{b}\right)^{\mathrm{T}}$
两个坐标系的正反转换可以用旋转矩阵和旋转矩阵的转置表示：
$\boldsymbol{C}_{n}^{b}=\left(\boldsymbol{C}_{b}^{n}\right)^{\mathrm{T}}=\left[\begin{array}{lll} T_{11} & T_{21} & T_{31} \\ T_{12} & T_{22} & T_{32} \\ T_{13} & T_{23} & T_{33} \end{array}\right]$
如果记：
$\boldsymbol{C}_{b}^{n}=\left[\begin{array}{lll} T_{11} & T_{12} & T_{13} \\ T_{21} & T_{22} & T_{23} \\ T_{31} & T_{32} & T_{33} \end{array}\right]$
则：
$\boldsymbol{C}_{n}^{b}=\left(\boldsymbol{C}_{b}^{n}\right)^{\mathrm{T}}=\left[\begin{array}{lll} T_{11} & T_{21} & T_{31} \\ T_{12} & T_{22} & T_{32} \\ T_{13} & T_{23} & T_{33} \end{array}\right]$
所以有（ $\gamma$ 和 $\Psi$ 取值要根据象限来定）：
$\left\{\begin{array}{l} \theta=\arcsin \left(T_{32}\right) \\ \gamma_{\text {主 }}=\arctan \left(-\frac{T_{31}}{T_{33}}\right) \\ \Psi_{\text {主 }}=\arctan \left(\frac{T_{12}}{T_{22}}\right) \end{array}\right.$

$\begin{array}{|c|c|c|} \hline T_{22} & T_{12} & \Psi \\ \hline \rightarrow 0 & + & 90^{\circ} \\ \hline \rightarrow 0 & - & -90^{\circ} \\ \hline+ & + & \Psi_{\text {主 }} \\ \hline+ & - & \Psi_{\text {主 }} \\ \hline- & + & \Psi_{\text {主 }}+180^{\circ} \\ \hline- & - & \Psi_{\text {主 }}-180^{\circ} \\ \hline \end{array}$

$\begin{array}{|c|c|c|} \hline \gamma_{\text {主 }} & T_{\mathrm{3} 3} & \gamma \\ \hline+ & & \\ \hline- & + & \gamma_{\text {主 }} \\ \hline+ & - & \gamma_{\text {主 }}-180^{\circ} \\ \hline- & - & \gamma_{\text {主 }}+180^{\circ} \\ \hline \end{array}$

Matlab 符号计算与计算结果（与书中给出的一致）：

clear all;
clc;
syms cosF sinF cosT sinT cosR sinR;
%% 东北天坐标系 (秦永元《惯性导航》 P6) 

% ① 绕 Z 轴旋转R角，Y->X 为正 (角度北偏东为正)        
C_n_1 = [cosF   -sinF   0;
         sinF   cosF    0;
           0     0      1];
   
% ② 绕 X 轴旋转R角，Y->Z 为正           
C_1_2 = [1      0       0;
         0    cosT   sinT;
         0    -sinT  cosT];
          
% ③ 绕 Y 轴旋转R角，Z->X 为正
C_2_b = [cosR   0    -sinR;
        0       1       0;
        sinR    0    cosR];

C_n_b  = C_2_b*C_1_2*C_n_1;
disp(C_n_b);

旋转矩阵计算结果：

[ cosF*cosR + sinF*sinR*sinT,   cosF*sinR*sinT - cosR*sinF, -cosT*sinR]
[                  cosT*sinF,                    cosF*cosT,       sinT]
[ cosF*sinR - cosR*sinF*sinT, - sinF*sinR - cosF*cosR*sinT,  cosR*cosT]

四元数表示旋转矩阵：《惯性导航》
$\left\{\begin{array}{l} q_{0}=\cos \frac{\theta}{2} \\ q_{1}=l \sin \frac{\theta}{2} \\ q_{2}=m \sin \frac{\theta}{2} \\ q_{3}=n \sin \frac{\theta}{2} \end{array}\right.$
旋转矩阵用四元数可以描述为（转轴为u = { l,m,n },旋转角度为 $\theta$ ）：
$\boldsymbol{C}_{b}^{R}=\left[\begin{array}{ccc} 1-2\left(q_{2}^{2}+q_{3}^{2}\right) & 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) & 1-2\left(q_{1}^{2}+q_{3}^{2}\right) & 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) & 1-2\left(q_{1}^{2}+q_{2}^{2}\right) \end{array}\right]$
同时四元数还可以这样描述旋转矩阵（向量 ${r}^{b}$ 旋转到 ${r}^{R}$ ）：
$\boldsymbol{r}^{R}=\boldsymbol{Q} \otimes \boldsymbol{r}^{b} \otimes \boldsymbol{Q}^{*}$

$\boldsymbol{Q} \otimes \boldsymbol{r}^{b} \otimes \boldsymbol{Q}^{*}=\boldsymbol{M}(\boldsymbol{Q}) \boldsymbol{M}^{\prime}\left(\boldsymbol{Q}^{*}\right)\left[\begin{array}{l} 0 \\ \boldsymbol{r}^{b} \end{array}\right]$

$=\left[\begin{array}{cccc} q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & -q_{3} & q_{2} \\ q_{2} & q_{3} & q_{0} & -q_{1} \\ q_{3} & -q_{2} & q_{1} & q_{0} \end{array}\right]\left[\begin{array}{cccc} q_{0} & q_{1} & q_{2} & q_{3} \\ -q_{1} & q_{0} & -q_{3} & q_{2} \\ -q_{2} & q_{3} & q_{0} & -q_{1} \\ -q_{3} & -q_{2} & q_{1} & q_{0} \end{array}\right]\left[\begin{array}{c} 0 \\ r_{x}^{b} \\ r_{y}^{b} \\ r_{z}^{b} \end{array}\right]$

$=\left[\begin{array}{cccc} \times & 0 & 0 & 0 \\ \times & q_{1}^{2}+q_{0}^{2}-q_{3}^{2}-q_{2}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{1} q_{3}+q_{0} q_{2}\right) \\ \times & 2\left(q_{1} q_{2}+q_{0} q_{3}\right) & q_{2}^{2}-q_{3}^{2}+q_{0}^{2}-q_{1}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ \times & 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{2} q_{3}+q_{0} q_{1}\right) & q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2} \end{array}\right]\left[\begin{array}{c} 0 \\ r_{x}^{b} \\ r_{y}^{b} \\ r_{z}^{b} \end{array}\right]$

向量叉乘：《惯性导航》

（1）向量叉乘
设有：

$\boldsymbol{r}^{n}=\left[\begin{array}{lll} r_{x}^{n} & r_{y}^{n} & r_{z}^{n} \end{array}\right]^{\mathrm{T}}, \boldsymbol{s}^{n}=\left[\begin{array}{lll} s_{x}^{n} & s_{y}^{n} & s_{z}^{n} \end{array}\right]^{\mathrm{T}}, \boldsymbol{t}^{n}=\boldsymbol{r}^{n} \times \boldsymbol{s}^{n}$
则：
$\left[\begin{array}{c} t_{x}^{n} \\ t_{y}^{n} \\ t_{x}^{n} \end{array}\right]=\left[\begin{array}{ccc} 0 & -r_{z}^{n} & r_{y}^{n} \\ r_{z}^{n} & 0 & -r_{x}^{n} \\ -r_{y}^{n} & r_{x}^{n} & 0 \end{array}\right]\left[\begin{array}{c} s_{x}^{n} \\ s_{y}^{n} \\ s_{z}^{n} \end{array}\right]$
其中： $\boldsymbol{t}^{n}=\left[\begin{array}{lll} t_{x}^{n} & t_{y}^{n} & t_{z}^{n} \end{array}\right]^{\mathrm{T}}$

（2）三重矢积：
$\begin{aligned} \boldsymbol{u} \times(\boldsymbol{u} \times \boldsymbol{r}) &=\boldsymbol{u}(\boldsymbol{u} \cdot \boldsymbol{r})-(\boldsymbol{u} \cdot \boldsymbol{u}) \boldsymbol{r} \\ &=(\boldsymbol{r} \cdot \boldsymbol{u}) \boldsymbol{u}-\boldsymbol{r} \end{aligned}$

哥氏定理：《惯性导航》

哥氏定理用于描述绝对变化率与相对变化率间的关系。设有矢量 $r$ ， $m$ 和 $n$ 是两个作相对旋转的坐标系，则哥氏定理可描述为：
$\left.\frac{\mathrm{d} \boldsymbol{r}}{\mathrm{d} t}\right|_{m}=\left.\frac{\mathrm{d} \boldsymbol{r}}{\mathrm{d} t}\right|_{n}+\omega_{m n} \times \boldsymbol{r}$
其中： $\left.\frac{\mathrm{d} \boldsymbol{r}}{\mathrm{d} t}\right|_{m} \text {和}\left.\frac{\mathrm{d} \boldsymbol{r}}{\mathrm{d} t}\right|_{n}$ 是分别在 $m$ 坐标系和 $n$ 坐标系内观察到的 $\boldsymbol{r}$ 的时间变化率 $\omega_{m n}$ 是坐标系 n 相对坐标系 m 的旋转角速度。如果将哥氏定理两边的矢量都向 m 坐标系投影,则有：
$\dot{\boldsymbol{r}}^{m}=\boldsymbol{C}_{n}^{m} \dot{\boldsymbol{r}}^{n}+\omega_{m n}^{m} \times \boldsymbol{r}^{m}$
其中：
$\begin{aligned} \dot{r}^{m} &=\left[\begin{array}{lll} \dot{r}_{x}^{m} & \dot{r}_{y}^{m} & \dot{r}_{z}^{m} \end{array}\right]^{\mathrm{T}} \\ \dot{r}^{n} &=\left[\begin{array}{lll} \dot{r}_{x}^{n} & \dot{r}_{y}^{n} & \dot{r}_{z}^{n} \end{array}\right]^{\mathrm{T}} \end{aligned}$

四元数运算法则：《惯性导航》

矢量式：
$\boldsymbol{Q}=q_{0}+\boldsymbol{q}$
指数式：
$\boldsymbol{Q}=\mathrm{e}^{u \frac{\theta}{2}}$
三角式：
$\boldsymbol{Q}=\cos \frac{\theta}{2}+\boldsymbol{u} \sin \frac{\theta}{2}$
复数式：
$\boldsymbol{Q}=q_{0}+q_{1} \boldsymbol{i}+q_{2} \boldsymbol{j}+q_{3} \boldsymbol{k}$
$\boldsymbol{Q}^{*}=q_{0}-q_{1} \boldsymbol{i}-q_{2} \boldsymbol{j}-q_{3} \boldsymbol{k}$
矩阵式：
$\boldsymbol{Q}=\left[\begin{array}{l} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{array}\right]$
范数：（范数为1的四元数为规范四元数）
$\|\boldsymbol{Q}\|=q_{0}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}$
加减法：
$\begin{array}{l} \boldsymbol{Q}=q_{0}+q_{1} \boldsymbol{i}+q_{2} \boldsymbol{j}+q_{3} \boldsymbol{k} \\ \boldsymbol{P}=p_{0}+p_{1} \boldsymbol{i}+p_{2} \boldsymbol{j}+p_{3} \boldsymbol{k} \end{array}$
$\boldsymbol{Q} \pm \boldsymbol{P}=\left(q_{0} \pm p_{0}\right)+\left(q_{1} \pm p_{1}\right) i+\left(q_{2} \pm p_{2}\right) \boldsymbol{j}+\left(q_{3} \pm p_{3}\right) \boldsymbol{k}$
乘法：
标量乘法： $a Q=a q_{0}+a q_{1} i+a q_{2} j+a q_{3} k$
四元数乘法： $\begin{aligned} \boldsymbol{P} \otimes \boldsymbol{Q}=&\left(p_{0}+p_{1} i+p_{2} \boldsymbol{j}+p_{3} \boldsymbol{k}\right) \otimes\left(q_{0}+q_{1} \boldsymbol{i}+q_{2} \boldsymbol{j}+q_{3} \boldsymbol{k}\right) \\ =&\left(p_{0} q_{0}-p_{1} q_{1}-p_{2} q_{2}-p_{3} q_{3}\right)+\left(p_{0} q_{1}+p_{1} q_{0}+p_{2} q_{3}-p_{3} q_{2}\right) \boldsymbol{i} \\ &+\left(p_{0} q_{2}+p_{2} q_{0}+p_{3} q_{1}-p_{1} q_{3}\right) \boldsymbol{j}+\left(p_{0} q_{3}+p_{3} q_{0}+p_{1} q_{2}-p_{2} q_{1}\right) \boldsymbol{k} \end{aligned}$ $r_{0}+r_{1} i+r_{2} j+r_{3} k$
也可表示为： $\left[\begin{array}{c} r_{0} \\ r_{1} \\ r_{2} \\ r_{3} \end{array}\right]=\left[\begin{array}{cccc} p_{0} & -p_{1} & -p_{2} & -p_{3} \\ p_{1} & p_{0} & -p_{3} & p_{2} \\ p_{2} & p_{3} & p_{0} & -p_{1} \\ p_{3} & -p_{2} & p_{1} & p_{0} \end{array}\right]\left[\begin{array}{c} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{array}\right]=M(P) Q$
$\left[\begin{array}{c} r_{0} \\ r_{1} \\ r_{2} \\ r_{3} \end{array}\right]=\left[\begin{array}{cccc} q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & q_{3} & -q_{2} \\ q_{2} & -q_{3} & q_{0} & q_{1} \\ q_{3} & q_{2} & -q_{1} & q_{0} \end{array}\right]\left[\begin{array}{c} p_{0} \\ p_{1} \\ p_{2} \\ p_{3} \end{array}\right]=M^{\prime}(\boldsymbol{Q}) \boldsymbol{P}$
即是：
$\begin{array}{l} \boldsymbol{P} \otimes \boldsymbol{Q}=\boldsymbol{M}(\boldsymbol{P}) \boldsymbol{Q} \\ \boldsymbol{P} \otimes \boldsymbol{Q}=\boldsymbol{M}^{\prime}(\boldsymbol{Q}) \boldsymbol{P} \end{array}$
分配律和结合律：（不满足交换律）
$\begin{array}{c} \boldsymbol{P} \otimes(\boldsymbol{Q}+\boldsymbol{R})=\boldsymbol{P} \otimes \boldsymbol{Q}+\boldsymbol{P} \otimes \boldsymbol{R} \\ \boldsymbol{P} \otimes \boldsymbol{Q} \otimes \boldsymbol{R}=(\boldsymbol{P} \otimes \boldsymbol{Q}) \otimes \boldsymbol{R}=\boldsymbol{P} \otimes(\boldsymbol{Q} \otimes \boldsymbol{R}) \end{array}$
四元数的逆：
$\begin{aligned} \boldsymbol{P} \otimes \boldsymbol{P}^{*} &=\left(p_{0}+p_{1} \boldsymbol{i}+p_{2} \boldsymbol{j}+p_{3} \boldsymbol{k}\right) \otimes\left(p_{0}-p_{1} \boldsymbol{i}-p_{2} \boldsymbol{j}-p_{3} \boldsymbol{k}\right) \\ &=p_{0}^{2}+p_{1}^{2}+p_{2}^{2}+p_{3}^{2} \\ &=\|\boldsymbol{P}\| \end{aligned}$
则： $\boldsymbol{P}^{-1}=\frac{\boldsymbol{P}^{*}}{\|\boldsymbol{P}\|}$

四元数微分：
$\left[\begin{array}{c} \dot{q}_{0} \\ \dot{q}_{1} \\ \dot{q}_{2} \\ \dot{q}_{3} \end{array}\right]=\left[\begin{array}{cccc} 0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0 \end{array}\right]\left[\begin{array}{c} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{array}\right]$