前言

姿态解算过程涉及到两个坐标系，一个是运载体的机体坐标系，该坐标系与运载体固连，当运载体转动的时候，这个坐标系也跟着转动，我们假设运载体的坐标系为b系。另外一个是地理坐标系，即东北天坐标系，X轴指向东，Y轴指向北，Z轴指向天，这个坐标系也称为导航坐标系，我们假设这个坐标系为n系。

所谓的姿态解算，就是求出当前的机体坐标系b相对于地理坐标系n的变化。由于n系和b 系均为直角坐标系，各轴之间始终保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时，可理解成刚体的定点转动。而刚体定点转动这个变化可以通过一个变换矩阵来表示，这个矩阵包含了刚体的所有姿态信息。

可以这么理解这个变换矩阵：对于机体坐标系中的一个向量，当它左乘这个变换矩阵之后，就能得出这个向量在地理坐标系中的大小。由b系到n系的坐标变换短阵也称为运载体的姿态矩阵，而我们姿态解算的目标就是根据惯性器件的输出实时计算出这个姿态矩阵，从而实时获取当前的偏航、俯仰角、翻滚角。

陀螺仪和加速度计分别用来测量运载体的角运动信息和线运动信息，根据陀螺仪和加速度计的输出，我们就能实时跟新这个姿态矩阵。

本篇文档将从这一基本思想出发，主要介绍姿态更新的四元数算法。

变换矩阵的三种表示方法：欧拉角，余弦矩阵，四元数

1、欧拉角：欧拉角是很直观的，一个物体的旋转，可以分解到三个坐标轴上的旋转。这三个旋转角度就是欧拉角。在惯性系统中一般把这三个角度分别称为roll，pitch，yaw。用欧拉角来描述物体的旋转不光需要有角度，还需要有旋转顺序，一般旋转顺序是先yaw后pitch，再roll反应到坐标轴上就是先绕Z轴旋转，再绕X轴旋转，最后绕Y轴旋转。需要注意的是 yaw pitch roll 都是对应的固定的参考系（地理坐标系），每次新的姿态坐标系都是由地理坐标系通过欧拉角旋转得到的。这样我们就用欧拉角表示了物体的姿态。如右下图我们就得到了使用欧拉角表示的坐标变换矩阵。

2、余弦矩阵：方向余弦表示一个向量的方向（姿态）我们可以用他在参考坐标系（地理坐标系）各个轴向的夹角的余弦来表示（及在各个轴的投影）。类似的一个坐标系可以看成是3个向量组成，所以三个向量分别在坐标轴上的投影可以用来表示一个坐标系与参考坐标系的关系。这总共9个方向余弦组成了一个三阶矩阵。在本体坐标系下的 i, j, k在全局坐标系(I,J,K)下有一套完整的表示，我们可以将这些向量组成一个方便的矩阵，其中cos(I,i) = |I||i| cos(I,i) = I.i，即|i|为单位向量i的范数(长度)，cos(I,i)为向量I与i形成的夹角余弦：

事实上方向余弦和欧拉角没有本质区别，因为方向余弦实际上就是用欧拉角表示的。这里就不再赘述了。

3、四元数：欧拉角微分方程中包含了大量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”。四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。因此姿态更新更多的使用的是四元数更新算法。

思维导图

四元数简介

（摘自《惯性导航》秦永元著）

四元数与姿态矩阵间的关系

（摘自《惯性导航》秦永元著）

四元数微分方程

（摘自《惯性导航》秦永元著）

四元数微分方程

这样我们就可以得到四元数Q关于角速度向量的实时更新的微分方程

从理论上来看，角速度信息由陀螺仪提供，因此我们只需要获得角速度信息就可以进行联捷惯导的计算。

但是在实际中，我们还需要考虑到，陀螺仪自身的动静态误差的补偿，以及来自地球的位置速率和自转速率的影响。

所以在实际应用中，也需要加速度计和磁力计来对这些误差进行补偿。

四元数微分方程求解法

毕卡求解法：

捷联陀螺的输出一般是采样时间间隔内的角增量，为了避免噪声的微分放大，应直接用角增量来确定四元数。毕卡算法就是由角增量计算四元数的常用算法。

上一节介绍了四元数的微分方程，如下：

龙格-库塔法（Runge-Kutta）

对于泰勒级数而言，虽然高精度，但是计算复杂，而龙格-库塔法则能大大改善泰勒公式的这一缺陷。

这是一种在工程上应用广泛的高精度单步算法。由于此算法精度高，采取措施对误差进行抑制，所以其实现原理也较复杂。该算法是构建在数学支持的基础之上的。
假设有一个矩阵微分方程为：

当初是条件已知，那么它的一阶解为：

那么套用到四元数中，可有：

展开得：

将其写成代码为：

一阶龙格-库塔法是用初始端点的斜率来近似T时间段内的斜率，而二阶龙格-库塔法是使用初始端点和末尾端点的平均斜率来近似T时间段内的斜率。

则

其中

那么将其解套用到四元数中：

其中

当然，经典的龙格-库塔方法是四阶的。也就是在[xi,xi+1][xi,xi+1]上用四个点处的斜率加权平均作为平均斜率k∗的近似值，构成一系列四阶龙格-库塔公式。

代码实现

这部分可以看一下我的上一篇博客《基于icm20609姿态更新计算的四元数算法》

https://blog.csdn.net/zhangyufeikk/article/details/94391858

本文感谢以下大佬的贴子，获益匪浅：

https://wenku.baidu.com/view/218c876d856a561253d36f2a.html

https://wenku.baidu.com/view/b2f5ac27a5e9856a561260ce.html

从原理到代码梳理姿态更新计算的四元数算法相关推荐

捷联惯导基础知识之一（姿态更新的毕卡算法、龙格库塔算法、及精确数值解法）
一.介绍旋转矢量以及由旋转矢量更新四元数等效旋转矢量微分方程(Bortz方程): 经过理论和近似推导: 单位四元数与旋转矢量关系: 单位四元数与三角函数的关系: 四元数更新方程:利用旋转矢量,考虑了 ...
深度强化学习-D3QN算法原理与代码
Dueling Double Deep Q Network(D3QN)算法结合了Double DQN和Dueling DQN算法的思想,进一步提升了算法的性能.如果对Doubel DQN和Duelin ...
深度强化学习-Double DQN算法原理与代码
深度强化学习-Double DQN算法原理与代码引言 1 DDQN算法简介 2 DDQN算法原理 3 DDQN算法伪代码 4 仿真验证引言 Double Deep Q Network(DDQN)是 ...
协议crc计算_CRC校验原理及代码实现（一）
网上的有关CRC的原理和代码实现的文章,数不胜数,但比较全的好像还没看到:此文是集百家之智慧,加之自己的使用经验和理解汇聚此文,让你快速理解和使用CRC,当然不足之处欢迎评论区吐槽. 1. CRC是什 ...
深度图像转换为点云数据计算原理及代码实现
深度图像转换为点云数据计算原理及代码实现 1.开发环境 2. 深度图转点云计算原理 3.代码实现 3.1 头文件Depth_TO_PointCloud.h 3.2Depth_TO_PointCloud ...
CRC16 Modbus计算原理与代码实现
1.CRC16 Modbus计算原理 1) 预置 1 个 16 位的寄存器为十六进制FFFF(即全为 1) , 称此寄存器为 CRC寄存器. 2) 把第一个 8 位二进制数据 (通信信息帧的第一个字节 ...
视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
搞懂 Vision Transformer 原理和代码，看这篇技术综述就够了
↑ 点击蓝字关注极市平台作者丨科技猛兽编辑丨极市平台极市导读本文对Vision Transformer的原理和代码进行了非常全面详细的解读,一切从Self-attention开始.Trans ...
LOAM 原理及代码实现介绍
文章目录 LOAM 原理及代码实现介绍 LOAM技术点 LOAM整体框架退化问题代码结构 LOAM 原理及代码实现介绍 paper:<Lidar Odometry and Mapping i ...

从原理到代码梳理姿态更新计算的四元数算法

前言

变换矩阵的三种表示方法：欧拉角，余弦矩阵，四元数

思维导图

四元数简介

四元数与姿态矩阵间的关系

四元数微分方程

四元数微分方程求解法

毕卡求解法：

龙格-库塔法（Runge-Kutta）

代码实现

从原理到代码梳理姿态更新计算的四元数算法相关推荐

最新文章

热门文章

从原理到代码 梳理姿态更新计算的四元数算法

前言

变换矩阵的三种表示方法：欧拉角，余弦矩阵，四元数

思维导图

四元数简介

四元数与姿态矩阵间的关系

四元数微分方程

四元数微分方程求解法

毕卡求解法：

龙格-库塔法（Runge-Kutta）

代码实现

从原理到代码 梳理姿态更新计算的四元数算法相关推荐

最新文章

热门文章

从原理到代码梳理姿态更新计算的四元数算法

从原理到代码梳理姿态更新计算的四元数算法相关推荐