多传感器融合定位七-惯性导航解算及误差分析其一

1. 三维运动描述基础知识
2. 三维运动的微分性质

Reference:

文章跳转：

1. 三维运动描述基础知识

1.1 概述

多传感器融合中三维运动的导航信息包含姿态、速度、位置，其中姿态的处理最为复杂，也最为核心。
姿态有三种表示形式：欧拉角、旋转矩阵、四元数，此外还有等效旋转矢量，但它一般在中间计算过程中使用。(等效旋转矢量推导过程中的一些计算，推导会更加简单方便。)
注：这里只介绍基于四元数和旋转矩阵的姿态更新，不介绍基于欧拉角的更新。(我们做姿态更新的时候，不会给出欧拉角的姿态更新和误差形式，欧拉角在实际过程中不会参与这种性质的运算，更多的是给人看的，很直观，但是在公式中会非常复杂。)

1.2 姿态描述方法

1.2.1 欧拉角

欧拉角等同于把姿态绕三次不同轴旋转。
不同的旋转顺序会得到不同的欧拉角，常见的有：
a. 机器人坐标系：xyz分别对应前左上，旋转顺序为z-y-x；
b. 惯性导航坐标系：xyz分别对应右前上，旋转顺序为z-x-y；
c. 另一种惯性导航坐标系：xyz分别对应前右下，旋转顺序为z-y-x。

在这里插入图片描述
万向锁：当载体处在某个姿态时，会产生奇异性问题，导致丢失一个自由度。
不同的旋转顺序下，产生万向锁时所处的姿态不同。下图展示了z-y-x的旋转顺序下的万向锁问题。(最后一个图中的旋转和第一次旋转是一样的，导致丢失了一个自由度)
在这里插入图片描述

为什么不同的坐标系定义下，会选择不同的旋转顺序？
不同坐标系定义下，选择不同旋转顺序的原因：其本质都是按照“航向(yaw)->俯仰(pitch)->横滚(roll)”的顺序旋转，因为此时万向锁出现在俯仰为90°时的情况，而多数载体出现该姿态的几率最小。

需要注意的是，欧拉角有明确的物理意义，不随坐标系定义的不同而改变：
a. 俯仰角：载体抬头为正，低头为负；
b. 横滚角：向右滚为正，向左滚为负；
c. 航向角：机器人中，一般以逆时针旋转为正，顺时针旋转为负。(在与地理系相关的惯性导航中，常以北偏东为正，北偏西为负，遇到时需要注意)

由于欧拉角必然产生奇异性，因此一般只用它做人机交互的显示，而不用来做姿态解算。

1.2.2 旋转矩阵

旋转矩阵是描述旋转的一个三维矩阵，一个真实姿态对应一个唯一的旋转矩阵。
假设旋转前，载体系(b系)的单位正交基为多传感器融合定位七-惯性导航解算及误差分析其一，旋转后对应的单位正交基为
假设在世界坐标系( 系，不随载体的旋转而旋转)下有向量，它在旋转前后两个坐标系中的坐标分别为和，那么有

多传感器融合定位七-惯性导航解算及误差分析其一由此可以得到
其中便是旋转矩阵，在结合机器人模型推导时，记为

从矩阵可以看出，三个基向量都是单位正交基，所以旋转矩阵也就是单位正交矩阵，即行列式为1，且满足 多传感器融合定位七-惯性导航解算及误差分析其一。

优点：
a. 没有奇异性，适合用于解算；

缺点：
a. 用9个元素表示3个自由度，会增加计算复杂度；
b. 为了保持正交性，一般更新完毕后，要重新做正交化。不然连续解算几次，旋转矩阵的性质变了，会遇到奇异性问题。

1.2.3 四元数

四元数是超复数，即 “复数的复数” 。(这个说法很有意思，但超复数不单单指四元数，比如还包含复数的复数的复数。四元数用来表示三维，但为什么不是三元数表示三维？)
若有复数
多传感器融合定位七-惯性导航解算及误差分析其一则复数的复数为(将当作复数的实部，当作复数的虚部)：
若令 , 则有
此即为四元数。
一般四元数的常见表示符号为：
共轭四元数(实部相同，虚部相反)：
四元数的逆：
多传感器融合定位七-惯性导航解算及误差分析其一姿态运算时，四元数为单位四元数(单位四元数即模为一，即)，此时有：
四元数乘法：(注意单位 )
若在四元数乘法中出现三维向量，指的是和三维向量构成的纯虚四元数相乘，比如
多传感器融合定位七-惯性导航解算及误差分析其一乘法结合律
四元数相乘，可以展开为矩阵与向量相乘的形式
其中
其中，符号，同样表示反对称矩阵。(就一个东西)

也可以展开为
多传感器融合定位七-惯性导航解算及误差分析其一其中
由此，可以得出重要性质(后续推导时常用)(可以看出来)：

1.2.4 等效旋转矢量

理解：把旋转当做绕空间一个固定轴转过一个角度。在不引起歧义的情况下可简称为旋转矢量或旋转向量。(将欧拉角的三次旋转等效成了一次旋转)
直接用向量表示，其方向即为转轴方向，对应的单位向量记为它的长度即为转角。
等效旋转矢量的指数形式，可以表示为
多传感器融合定位七-惯性导航解算及误差分析其一上式包含高次幂，为了便于后续计算，需要对高次幂进行化简。
由于反对称矩阵具有以下性质(可自行推导)，
因此，等效旋转矢量的指数函数可以表示如下
(罗德里格斯公式，也写成多传感器融合定位七-惯性导航解算及误差分析其一 )：

1.3 各描述方法之间的关系

1.3.1 欧拉角与旋转矩阵

按照机器人前多传感器融合定位七-惯性导航解算及误差分析其一 -左-上的坐标系定义，并令横滚角为、俯仰角为、航向角为。
a. 欧拉角转旋转矩阵
由于旋转矩阵是按照z-y-x(本质都是按照yaw-pitch-roll旋转)的顺序旋转得来，因此可以表示为
多传感器融合定位七-惯性导航解算及误差分析其一其中，

b. 旋转矩阵转欧拉角

由欧拉角得到的旋转矩阵，其完整形式为：
多传感器融合定位七-惯性导航解算及误差分析其一其中符号：
观察矩阵，可以看出：

1.3.2 旋转矩阵与四元数

它们转换的推导过程较为复杂，此处直接给出结论。
四元数转旋转矩阵(常用)：
多传感器融合定位七-惯性导航解算及误差分析其一旋转矩阵转四元数(不那么常用)：
需要注意的是，这需要满足：
当不满足该条件时，转换步骤比较复杂，此处不讲述。

1.3.3 旋转矩阵与旋转矢量

(和十四讲写的一样，但这里总看着怪怪的)
a. 由旋转矢量计算旋转矩阵
多传感器融合定位七-惯性导航解算及误差分析其一此公式也被称为罗德里格斯公式，并且与旋转矢量的指数运算结果相同。
b. 由旋转矩阵计算旋转矢量
其中，符号表示由反对称矩阵得到对应的矢量。

1.3.4 四元数与旋转矢量

a. 由旋转矢量计算四元数
多传感器融合定位七-惯性导航解算及误差分析其一思考：为什么角度是旋转矢量转角的一半?
三维空间中的一个矢量，使用四元数对它进行旋转，得到新的矢量时，其计算过程为
原因是矢量是三维的，四元数与矢量(对应的纯虚四元数)直接相乘是四维(即实部不为零)，而用上式计算出的结果则能保证永远是三维。
可以理解为两次旋转，第一次转到四维，第二次再转回三维，每次转总角度的一半。