惯性导航姿态解算原理流程

默寫無言

已于 2024-04-16 23:08:22 修改

阅读量1.3k

点赞数 9

文章标签：算法

于 2024-04-02 11:24:04 首次发布

本文链接：https://blog.csdn.net/hjkdahfads/article/details/136858037

版权

一：惯导概念混乱

相信大家和我一样，刚入门惯导就接触到各种惯导相关的术语和概念：各种坐标系，坐标系转换，坐标转换，姿态矩阵，姿态更新，欧拉角、方向余弦矩阵、四元数等等，各种概念看了个遍，好像懂了，稍微一结合串起来具体理解姿态解算过程就是各种感觉绕，各种感觉懵。

好不容易绕出来一点点，给大家介绍一下一些关于姿态解算原理尤其是对于欧拉角、方向余弦矩阵、四元数在姿态转换表达中的作用的比较浅显的个人理解，不一定都正确，欢迎参考指正。

二：姿态解算原理流程

姿态解算的整体流程主要包括获取惯性传感器数据、姿态估计(更新)、姿态提取、姿态控制。

获取惯性传感器数据：在运载体上，我们将惯性器件（陀螺仪和加速度计等）与之固连，在运载体的运动过程中，惯性器件会对运载体三轴（x，y，z）的角速度和加速度信息进行测量，这也就是我们获得的最原始的惯导数据，我们通过这些数据去进行姿态估计，求取姿态矩阵。

姿态估计（更新）：分析数据时我们发现，惯性传感器测量得到的原始的惯导数据是载体坐标系（b系）下的，而我们需要解算的运载体姿态信息是导航/地理坐标系（n/g系）下的，（在求解惯性导航参数过程中，导航坐标系通常选用地理坐标系），这就需要我们从b系下变换到g系，坐标变换计算过程中就会涉及到从b系到g系的坐标变换矩阵C，也称为姿态（矩）阵，在b系下利用坐标表示的一个三维矢量左乘坐标变换矩阵C即可得到其在g系下的坐标表示。

而姿态矩阵的表示通常有三种方式：欧拉角、方向余弦矩阵以及四元数，这也就对应着我们传统的三种姿态更新算法；

1）欧拉角

欧拉角是三个一组的角参数广义坐标，十分适用于人的理解。根据坐标轴绕其轴旋转顺序的不同，共计12种定义方式（欧拉角定义并不要求必须绕每个轴都旋转一次）。描述运载体姿态的一组欧拉角我们称之为姿态角，包括航向角（yaw）、俯仰角（pitch）、横滚角（row）。

我们将绕一根轴的旋转称为基本旋转，两坐标系任何复杂的角关系都可以看做有限次基本旋转的复合，那么两坐标系的姿态变换矩阵就是按顺序排列的基本旋转矩阵的连续左乘。我们利用欧拉角来描述基本旋转矩阵，由此可以利用欧拉角进一步描述姿态矩阵。

2）方向余弦矩阵

对于两个坐标系（如b系和i系）的转换，我们分别选取b系和i系的标准正交基，将b系中各个方向的单位向量分解成i系中各个方向的单位向量的线性组合，将其改写为矩阵形式：

$\begin {bmatrix} \mathbf{i}_b&\mathbf{j}_b&\mathbf{k}_b \end{bmatrix} = \begin {bmatrix} \mathbf{i}_i&\mathbf{j}_i&\mathbf{k}_i \end{bmatrix} \begin{bmatrix} \mathbf{i}_b \cdot \mathbf{i}_i & \mathbf{j}_b \cdot \mathbf{i}_i & \mathbf{k}_b \cdot \mathbf{i}_i \\ \mathbf{i}_b \cdot \mathbf{j}_i & \mathbf{j}_b \cdot \mathbf{j}_i & \mathbf{k}_b \cdot \mathbf{j}_i \\ \mathbf{i}_b \cdot \mathbf{k}_i & \mathbf{j}_b \cdot \mathbf{k}_i & \mathbf{k}_b \cdot \mathbf{k}_i \\ \end{bmatrix} =\begin {bmatrix} \mathbf{i}_i&\mathbf{j}_i&\mathbf{k}_i \end{bmatrix}\mathbf{P}$

P即为i系到b系的坐标系变换矩阵，P中的每一个元素均表示两套坐标系相应坐标轴之间夹角的余弦值，即称之为方向余弦矩阵。

姿态变换矩阵和方向余弦矩阵之间有什么关系？方向余弦矩阵是姿态变换矩阵的一种描述方式，当我们采取计算不同坐标系标准正交基之间的关系这种研究方式时，得到的方向余弦矩阵就是姿态变换矩阵。

3）四元数

三维空间中的任意旋转，除了可以分解为有限次基础旋转的叠加外，还可以等效于绕三维空间中某个轴旋转某个角度，那么全部的旋转信息就全部浓缩在旋转轴和旋转角度中。

这里我们指出单位四元数的三角函数形式表达式：

$\mathbf{Q}=\mathbf{q}_{0}+\mathbf{q}_{v}=cos(\frac{\phi}{2}) + \mathbf{u}\sin(\frac{\phi}{2})$

其中u为单位长度的三维矢量；

这种形式很好的囊括了旋转轴和旋转角度信息，我们可以利用其来方便的表达等效旋转。

在罗德里格旋转公式中，同样描述了这样一种基于等效旋转轴和旋转角度的旋转前后的矢量关系：

$\mathbf{r}^{'} =[\mathbf{I}+\sin\phi(\mathbf{u}\times)+(1-\cos\phi)(\mathbf{u}\times)^2]\mathbf{r}=\mathbf{D}\mathbf{r}$

$\mathbf{D}=\mathbf{I}+\sin\phi(\mathbf{u}\times)+(1-\cos\phi)(\mathbf{u}\times)^2$

其中D即为转动前后的两个矢量所需要左乘的姿态变换矩阵，将四元数Q拆分为实数部分和虚数部分，即可建立四元数与姿态变换矩阵的关系。

姿态提取：姿态解算的最终目的是获取实时的运载体姿态信息，这就需要我们实时进行姿态更新，也就是通过惯性器件的输出对姿态矩阵进行实时的计算，从姿态矩阵中即可提取出我们所需的运载体姿态信息（姿态角）。

姿态控制：根据运载体姿态信息，利用姿态控制系统按要求调整运载体姿态，这里不做过多赘述。

最后补充一些惯导坐标系转换变来变去的一些疑问和理解：

地心惯性坐标系以地球质心为圆心，以地球的自转轴和恒星确定坐标轴方向，不随地球转动，这里所说的以恒星确定坐标轴方向即为ox轴指向春分点，说白了就是这个点不在地球上，所以这个坐标轴不会随着地球转动，在这个坐标系下才建立的位置、速度、加速度等之间的数学关系。而另一个叫做地球坐标系也叫地心地固坐标系的坐标系它的ox轴指向本初子午线，而这条线在地球上，会随着地球自转转动，因此这个坐标系也是随着地球转动的。

将坐标转换后的姿态、速度信息通过积分等操作得到导航（地理）坐标系下位置变化信息，地理坐标系相对于地球（地心地固）坐标系的关系可用运载体的地理坐标表示，即经纬度和椭球高度。也就是说把载体系下的惯导数据转换到导航（地理）系，积一下就是简陋版的经纬啦，这也可以算是整个导航的简化流程。

参考：惯性导航-秦永元

捷联惯导算法与组合导航原理-严恭敏

姿态解算基础：欧拉角、方向余弦、四元数_四元素姿态矩阵-CSDN博客写太好啦，感谢感谢，绕不清楚了再看总能绕出来点，但过会又绕进去了哈哈哈.