惯导机械编排——姿态更新

学习代码的小赵

于 2024-08-02 14:16:19 发布

阅读量1.6k

点赞数 29

分类专栏：惯性导航文章标签：笔记

本文链接：https://blog.csdn.net/JIEAOGUOFEN/article/details/140869298

版权

惯性导航专栏收录该内容

2 篇文章 0 订阅

订阅专栏

在这里借CSDN这个平台记录一下姿态更新过程中遇到的所有问题，并对整个姿态更新流程做一个梳理和总结，学习课件均来自i2nav牛小骥老师团队整理，感谢陈起金老师和各位师兄提供的帮助。资料链接：

武汉大学多源智能导航实验室 (i2nav.com)http://www.i2nav.com/index/newList_zw?newskind_id=13a8654e060c40c69e5f3d4c13069078

一、姿态更新概述

姿态更新，顾名思义就是根据前一时刻的姿态和陀螺的输出的当前历元对应的测量值，来计算当前的姿态。姿态更新的推算可以通过方向余弦矩阵或者四元数来实现，但是笔者认为方向余弦矩阵更易于理解，所以在我的文章中均采用方向余弦矩阵的形式来实现姿态的更新。而姿态变化的表达均通过等效旋转矢量来表示。

首先在进行整个更新过程的讲解之前，我们首先要明确我们整个流程的最终任务和目标是啥，可以利用的条件有哪些：

已知量：

上一历元的姿态： $\mathbf{C}_b^n(t_{k-1})$ ；

当前历元和上一历元陀螺输出的角增量测量值： $\Delta\boldsymbol{\theta}_k\text{,}\Delta\boldsymbol{\theta}_{k-1}$

求解目标：

当前历元的姿态： $\mathbf{C}_b^n(t_{k})$

二、预备知识

为了扫清大家的疑惑，在这里需要大家了解一些前景知识：

（1）首先就是在拥有上一历元的姿态的情况下，方向余弦矩阵 $\mathbf{C}_b^n(t_{k-1})$ 怎么去求解？

在已知上一历元的姿态 $(\phi ,\theta, \psi )$ 的情况之下，我们可以很轻易地获得那个历元的位置坐标由 $n$ 系转到 $b$ 系的方向余弦矩阵 $\mathbf{C}_n^b(t_{k-1})$ ， $\mathbf{C}_n^b(t_{k-1})$ 的具体求解思路可以参照我的另一篇文章：

导航过程中的坐标系转换（n系转b系）-CSDN博客

而 $\mathbf{C}_b^n(t_{k-1})=\mathbf{C}_b^n(t_{k-1})^T$ 。

当然也可以直接通过公式获得：

$\mathbf{C}_b^n(t_{k-1})=\begin{bmatrix}\cos\theta\cos\psi&-\cos\phi\sin\psi+\sin\phi\sin\theta\cos\psi&\sin\phi\sin\psi+\cos\phi\sin\theta\cos\psi\\\cos\theta\sin\psi&\cos\phi\cos\psi+\sin\phi\sin\theta\sin\psi&-\sin\phi\cos\psi+\cos\phi\sin\theta\sin\psi\\-\sin\theta&\sin\phi\cos\theta&\cos\phi\cos\theta\end{bmatrix}$

（2）欧拉角和等效旋转矢量不是一个概念，他们两个所代表的意义完全不一样，不能简单地认为 $\left[\phi ,\theta, \psi \right]^T$ 就是等效旋转矢量，也不能能认为等效旋转矢量的三个量的大小就是旋转角度，这个理解是非常严重错误的。

一个坐标系到另一个坐标系的变换可以通过多次转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。该矢量称作等效旋转矢量（rotation vector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小。又称为轴角（axis-angle）。

（3）欧拉角转方向余弦矩阵可以通过三次转动的旋转矩阵累乘来实现，而等效旋转矢量是如何转化为对应的方向余弦矩阵的呢？

等效旋转矢量转化为对应的方向余弦矩阵是通过Rodrigues 旋转公式来实现的：

$\mathbf{C}_{b}^{R}=\mathbf{I}-\dfrac{\sin\left\|\boldsymbol{\phi}\right\|}{\left\|\boldsymbol{\phi}\right\|}(\boldsymbol{\phi}\times)+\dfrac{1-\cos\left\|\boldsymbol{\phi}\right\|}{\left\|\boldsymbol{\phi}\right\|^{2}}(\boldsymbol{\phi}\times)\left(\boldsymbol{\phi}\times\right)$

上式中， $\boldsymbol{\phi}$ 表示由 $b$ 系转到 $R$ 系的等效旋转矢量， $\mathbf{C}_{b}^{R}$ 则为对应 $b$ 系转到 $R$ 系的反向余弦矩阵。

三、姿态更新方法

3.1 姿态更新公式

为了更清楚地表达转化关系，在这里将 $\mathbf{C}_b^n(t_{k-1})$ 写作： $\mathbf{C}_{b(k-1)}^{n(k-1)}$ ，将 $\mathbf{C}_b^n(t_{k})$ 写作： $\mathbf{C}_{b(k)}^{n(k)}$ 。

要想建立起 $\mathbf{C}_{b(k)}^{n(k)}$ 和 $\mathbf{C}_{b(k-1)}^{n(k-1)}$ 之间的关系，则可以通过如下连乘运算实现：

$\mathbf{C}_{b(k)}^{n(k)}=\mathbf{C}_{n(k-1)}^{n(k)}\mathbf{C}_{b(k-1)}^{n(k-1)}\mathbf{C}_{b(k)}^{b(k-1)}$

上式中 $\mathbf{C}_{n(k-1)}^{n(k)}$ 是 $n$ 系由 $t_{k-1}$ 到 $t_k$ 的姿态变化矩阵， $\mathbf{C}_{b(k)}^{b(k-1)}$ 是 $b$ 系由 $t_k$ 到 $t_{k-1}$ 的姿态变化矩阵。二者均可以利用Rodrigues 旋转公式将对应的等效旋转矢量转化为对应的的姿态变化矩阵，方式如下：

$\begin{cases} \mathbf{C}_{b(k)}^{b(k-1)}=\mathbf{I}+\dfrac{\sin\lVert\boldsymbol{\phi}_k\rVert}{\lVert\boldsymbol{\phi}_k\rVert}\left(\boldsymbol{\phi}_k\times\right)+\dfrac{1-\cos\lVert\boldsymbol{\phi}_k\rVert}{\lVert\boldsymbol{\phi}_k\rVert^2}\left(\boldsymbol{\phi}_k\times\right)\left(\boldsymbol{\phi}_k\times\right)\\\mathbf{C}_{n(k-1)}^{n(k)}=\mathbf{I}-\dfrac{\sin\lVert\boldsymbol{\zeta}_k\rVert}{\lVert\boldsymbol{\zeta}_k\rVert}\left(\boldsymbol{\zeta}_k\times\right)+\dfrac{1-\cos\lVert\boldsymbol{\zeta}_k\rVert}{\lVert\boldsymbol{\zeta}_k\rVert^2}\left(\boldsymbol{\zeta}_k\times\right)\left(\boldsymbol{\zeta}_k\times\right) \end{cases}$

在这里需要注意的是单位阵后面的那个符号变化，因为我们直接求解的等效旋转矢量 $\boldsymbol{\zeta}_k$ 和 $\boldsymbol{\phi}_k$ 都是相对于 $n$ 系和 $b$ 系下由时间 $t_k$ 到 $t_{k-1}$ 的，但是 $\mathbf{C}_{b(k)}^{b(k-1)}$ 是 $b$ 系由 $t_k$ 到 $t_{k-1}$ 的姿态变化矩阵，所以带入的时候需要反号。

3.2 等效旋转矢量求解

$\boldsymbol{\phi}_k$ 代表载体在 $b$ 系下由时间 $t_{k-1}$ 到 $t_k$ 的的等效旋转矢量，这个矢量是通过对陀螺在当前历元和上一历元陀螺输出的角增量测量值： $\Delta\boldsymbol{\theta}_k\text{,}\Delta\boldsymbol{\theta}_{k-1}$ 进行处理获得，中间利用了Bortz 公式和对旋转角速度的双子样假设，具体求解流程可以看这篇文章有详细的讲解：

惯导机械编排——等效旋转矢量公式推导-CSDN博客

而 $\boldsymbol{\zeta}_k$ 表示 $n$ 系变化对应的等效旋转矢量，表示 $n$ 系从 $t_{k-1}$ 时刻的状态 $n(k-1)$ 绕向量 $\boldsymbol{\zeta}_k$ 正转角度 $\lVert\boldsymbol{\zeta}_k\rVert$ 后达到 $t_{k}$ 时刻的状态 $n(k)$ ，计算方式如下：

$\begin{aligned} \zeta_{k}&=\int_{t_{k-1}}^{t_{k}}\left[\boldsymbol{\omega}_{ie}^{n}(t)+\boldsymbol{\omega}_{en}^{n}(t)\right]dt\\&\approx\left[\boldsymbol{\omega}_{ie}^{n}(t_{k-1})+\boldsymbol{\omega}_{en}^{n}(t_{k-1})\right]\Delta t \end{aligned}$

其中 $\boldsymbol{\omega}^n_{ie}(t_{k-1})$ 是由地球自转角速度在载体所在位置的 $n$ 系投影（分解到北-东-地方向）：

$\boldsymbol{\omega}_{ie}^n=\begin{bmatrix}\omega_e\cos\varphi&0&-\omega_e\sin\varphi\end{bmatrix}^\mathrm{T}$

而 $\boldsymbol{\omega}^n_{en}(t_{k-1})$ 表示由物体自身运动所引起的角速度在 $n$ 系下的投影：

$\boldsymbol\omega_{en}^n=\begin{bmatrix}v_E/(R_N+h)\\-v_N/(R_M+h)\\-v_E\tan\varphi/(R_N+h)\end{bmatrix}$

其中

$\begin{cases} R_{M}=\dfrac{a(1-e^{2})}{\sqrt{(1-e^{2}\sin^{2}\varphi)^{3}}}\\R_{N}=\dfrac{a}{\sqrt{1-e^{2}\sin^{2}\varphi}} \end{cases}$

式中 $\varphi$ 为纬度（单位：rad）， $v_E$ ， $v_N$ 分别为东向和北向速度。 $R_M$ 和 $R_N$ 分别为子午圈半径和卯酉圈半径。 $a$ 为地球椭球长半轴， $e$ 为地球椭球模型第一偏心率，对于WGS84椭球， $a=6378137.0 m$ ， $e^2 =0.00669437999013$ ， $\omega_e=7.292115×10^{-5} rad/s$ 。