四元数姿态计算

本文基本上就是[1]的注解

---

一基本概念

1.1 四元数的定义 Quaternion

A quaternion is a four-dimensional complex number
四元数是一个四维的复数
写成行向量形式的话,q=[q1 q2 q3 q4]

1.2 表示姿态的核心思想

An arbitrary orientation of frame B relative to frame A can be achieved through a rotation of angle θ
around an axis A_rˆ defined in frame A。
任何一个姿态，都可以绕某条旋转轴旋转一定角度实现。

另外一种姿态表示法：旋量，也是基于这个思想的

---

二 姿态计算的相关公式

2.1 基本运算公式

2.1.1 用四元数表示姿态

我们假设一开始没有任何的旋转，姿态是跟参考坐标系重合。
目标的姿态是绕一个旋转轴旋转一定角度，这个旋转的结果可以用下面的公式表示

其中$\theta$ 是旋转过的角度，rx,ry,rz是旋转轴上的单位矢量的分量 。

2.1.2 姿态的逆

是表示坐标系B相对于A的姿态。

如果我们要求A相对于B的姿态，这可以通过四元数的共轭来算

公式(2)表示的四元数，称为公式(1)表示的四元数的共轭

2.1.3 姿态的坐标变换以及四元数乘法

四元数的乘法是用⊗这个符号表示。
如果我们知道B相对于A的姿态，C相对于B的姿态

如果我们要求C相对于A的姿态，就要用到四元数的乘法

2.1.4 向量与四元数

同一个向量v，分别用坐标系A，B来表示：

如果我们知道向量v在A中的表示，以及B相对于A的姿态，就可以用下面的公式求得v在B中的表示

2.1.5 欧拉表示法与四元数

3D姿态是可用一个3x3的矩阵表示的，这个矩阵可以根据姿态对应的四元数计算得到

欧拉表示法是用ψ, θ ， φ 分别表示绕坐标系本身的z,y,x进行旋转得到的姿态。它与四元数的关系如下：

2.2 陀螺仪与四元数

陀螺仪的角速度可以表示成一个四元数的向量形式

世界坐标系相对于传感器坐标系的姿态变化速率的表达式可以写成：

（11）可以写成与时间相关的形式：

如果我们知道处世姿态，就可以用（12）来积分，得到一段时间后的姿态：

2.3 加速度计和四元数

对于重力来说，它在地球坐标系的方向是确定。
加速度计测量的是重力矢量在加速度计坐标系中的方向。
我们可以根据这两个信息，通过上面说的坐标变换计算得到传感器的姿态。
但是直接计算会有个问题，因为我们可以绕另外一个平行的旋转轴旋转一定角度，然后传感器测到的
读数是相同的。这样子的旋转方式是无限多的。

那么为什么不选一个来用就好呢？我们这里讲的是四元数的实现方式，这种实现有个前提：

A quaternion representation requires a complete solution to be found

解决的方法就是通过将问题变成优化问题来找

（14）（15）表示我们要找能让f的值最小的解
其中(15)等式右边减号前面的部分，就是公式(5)
（16）是地球坐标系相对于传感器坐标系的姿态
(17）是地球坐标系描述表示重力方向的向量
(18）是传感器坐标系描述表示重力方向的向量，测量得到

最简单的优化算法就是：gradient descent algorithm

假设地球坐标系相对于传感器坐标系一开始的姿态是：

然后我们用步长u，进行梯度下降迭代，迭代n次以后得到：

梯度的计算公式如下：

其中f是要求解的，J是f的雅可比

f用(5)展开：

对f求偏导可得到雅可比：

以上就是优化问题的全形式。
这里需要注意的是，上面的表达式中，对地球坐标系的建立没有要求，可以随便建。
你可以建一个很奇怪的坐标系，让表示重力方向的向量所有分量都不为0.

这里我们有个可以让表达式更简洁的建坐标方法，就是重力方向指向地球坐标系的正方向，这样子
会其他分量会出现0，可以简化表达式和计算：

2.4 磁力计和四元数

---

reference

[1] 《An efficient orientation filter for inertial and inertial/magnetic sensor arrays》Sebastian O.H. Madgwick

[2] https://blog.csdn.net/wuwuku123/article/details/105699482