【姿态解算初体验】（三）四元数姿态表示总结

简介

最近学习EKF，在看到四元数的坐标转换时碰到点疑问，最后慢慢也弄明白其使用方法，现做总结。
四元数使用中无非三种转换情况：
1、欧拉角 —— 四元数 之间互相转换；
2、旋转矩阵 —— 四元数 之间互相转换；
3、轴角 —— 四元数 之间互相转换，其中轴角转四元数用的最多。
PS：以下 N或e表示NED坐标系(N系)， B或b表示Body机体坐标系(B系)

用法一：欧拉角——四元数

转换公式可以很容易查到，分为以下几种情况：

1、Euler2Quat：

最原始的公式

小角度假设前提下，无穷小化简后得到公式：
Euler2Quat = 【1 0.5*（roll，pitch，yaw）】
具体推到方法只要把sin(x)等价为x（二阶无穷小等价为0），cos(x)等价为1，就可以得到上述公式

2、Euler 2 Vect 2 Quat：

欧拉角旋转先转换为轴角，再由轴角转换为欧拉角，这种方式往往在不能小角度假设时使用，转换方式如下：
假设得到的转角为Euler = 【R ， P，Y】，单位化得到旋转轴Vect，取向量的模得到旋转角θ，再通过下面的公式得到四元数：
Quat = 【cos(θ/2) Vect * sin(θ/2)】
注意代入的角度是θ/2，可以看出，如果使用小角度假设，等价无穷小后就和Euler2Quat = 【1 0.5*（roll，pitch，yaw）】这个公式相同了。
个人认为Euler 2 Vect 2 Quat这种方式可以用在IMU数据读取频率小的时候，得到的Δ角度比较大。

3、Quat 2 Euler：

这种方式往往就是需要从估计器中读取姿态角时需要的了，转换公式如下：

用法二：旋转矩阵——四元数

最开始，一直想不通为什么要用这么多种姿态表示方式，一直用四元数不就得了。后来发现，在进行同一向量在两个坐标系（N系和B系）间转换时，如果采用四元数计算，好像计算复杂度比旋转矩阵还要高，而且还需要把三维的向量在第一位补零成四维才可以进行计算，那不如就先转换成旋转矩阵，然后用旋转矩阵去乘。

1、Quat 2 R

因此最常用的转换是四元数 转 旋转矩阵，过程如下：

特别注意：可以看到，旋转矩阵R的上下角标和四元数q是相反的，这是因为
表示的是坐标系的旋转，也就是把e系旋转到b系，而biaoshi表示的是向量的旋转，也即把一个在B系下表示的向量转换到在e系下表示。这也就是转换后上下角标要反过来了。

2、R 2 Quat

说实话到现在好像还没见过那个地方有这个转换，好像不是很常用，公式如下：
假设R为：

则四元数为：

Q的符号确定如下：

用法三：轴角 —— 四元数

这个转换可能是最常用的一种，但是尤其需要区别出，这个轴角表示的是旋转了向量，还是旋转了坐标系

旋转坐标系Vect 2 Quat：

这种方式其实就是和上面“用法一”提到的Euler 2 Vect 2 Quat内容相同。就不再多赘述了。

旋转向量Vect 2 Quat：

这种方式用的最多的应该就是求姿态误差了，也是用的最多的一种。
举例如下：
在控制飞机自稳时，我们往往会优先保证飞机roll和pitch角为0，这个时候也就是加速度计为【0，0，n】的时候，这可以看成一个目标向量。
假设当前的加速度计数值为【a , b , c】，这就是当前的向量。
其实这两个向量的方向是相同的（如果飞机处于匀速或静止状态）。数值不同是同一个向量在不同坐标系下表示的结果。
【0，0，n】显然是N系坐标系下的表示，【a , b , c】是B系下的表示，如果把【a , b , c】的数值放到N系上，就有了两个方向不同的向量，其中【a,b,c】是倾斜的与NED的D方向有一定的夹角，这个夹角的值β = arctan（Ah ，-Az）
其中Ah = sqrt（a^2 + b^2）, Az= c
有了这两个向量，我们可以找出怎么把向量【a,b,c】旋转到【0，0，n】，得到这个旋转，我们就能知道怎么把飞机旋转到水平位置了。现在我们已经知道旋转角了，其实就是β = arctan（Ah ，-Az），而旋转轴只需要通过向量的叉乘【a,b,c】x【0，0，n】便可以得到。
现在我们有了旋转轴和旋转角，也就是轴角，就可以通过公式**Quat = 【cos(θ/2) Vect * sin(θ/2)】**得到这个旋转的四元数。
好了，这里注意，这个四元数表示的是怎样的坐标系旋转呢？
根据上面提到的，现在旋转的是向量。我们是将倾斜向量旋转到与NED的D方向重合，使飞机水平。
那么得到的四元数表示的坐标系旋转就应为，把水平坐标系旋转到倾斜的坐标系。
因此，通过这种方式求误差角时，要格外注意四元数所表示的是坐标系从哪到哪的旋转

Quat 2 Vect：

目前为止，好像这种转换用的也少，也是需要格外注意以下，得到的转换后得到的轴角是旋转坐标系还是向量，不能搞反了。
转换公式可以通过Quat 2 Euler 2 Vect获得。

总结

1、四元数与欧拉角的转换较为简单，一般都表示坐标系的旋转；
2、旋转矩阵与四元数转换时，需要交换上下角标；
3、轴角与四元数的转换要格外注意轴角是用来旋转的向量还是坐标系，如果是坐标系则跟1相同，如果是向量则跟2相同。

好了，这些乱七八糟的姿态表示和旋转求误差什么的总算理清楚了