基于加速度计与磁力计的姿态解算方法(加计补偿偏航)

附上转载文章链接
加速度计实时输出机体坐标系下的三轴线加速度，磁力计实时输出机体坐标系下的三轴地磁强度，加速度计能解算出俯仰角与横滚角，由磁力计计算出航向角，两者相互配合可以解算三个姿态角信息。

1.计算roll和pitch
当多旋翼无人机在地理坐标系中静止时， 加速度计的量测输出为：
$a_n=[00g{]}^T$
其中g 为重力加速度。

当多旋翼无人机处于任意姿态时，加速度计在机体坐标系中的测量输出为：
$a_b=[a_x{a}_y{a}_z{]}^T$

我们知道地理坐标系的速度可以通过旋转矩阵得到机体坐标系下的数据：
$$\begin{gathered} \left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]={\text{C}}_b^n\left[\begin{array}{c}0\\ 0\\ g\end{array}\right] \\ =\left[\begin{array}{ccc}cos\psi cos\theta & sin\psi cos\theta & -sin\theta \\ cos\psi sin\theta sin\gamma -sin\psi cos\gamma & sin\psi sin\theta sin\gamma +cos\psi cos\gamma & cos\theta sin\gamma \\ cos\psi sin\theta cos\gamma +sin\psi sin\gamma & sin\psi sin\theta cos\gamma -cos\psi sin\gamma & cos\theta cos\gamma \end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right] \\ =\left[\begin{array}{c}-gsin\theta \\ gcos\theta sin\gamma \\ gcos\theta cos\gamma \end{array}\right] \end{gathered}$$

所以：
$$\begin{gathered} \theta =arcsin(-\frac{a_x}{g}) \\ \gamma =arctan(\frac{a_y}{a_z}) \end{gathered}$$
2.计算yaw
多旋翼无人机在地理坐标系（ 导航坐标系） 下磁感应强度表示为：
${\text{m}}_n=[m_x^n{m}_y^n{m}_z^n{]}^T$
地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面（即地理坐标系的xy平面）平行的分量和一个与当地水平面（即地理坐标系的xy平面）垂直的分量。

如果保持电子罗盘和当地的水平面平行，那么罗盘中磁力计的三个轴就和这三个分量对应起来，即罗盘水平状态下，测量的三个轴数据就是机体系下的地磁分量，因为此时罗盘所在的坐标系的x，y，z轴和机体系以及地理坐标系的指向是一致的。

对水平方向的两个分量来说，他们的矢量和总是指向磁北的，所以我们只需要水平的磁力分量即可算出传感器x轴与磁北的夹角：
${\psi }_m=arctan(\frac{m_y^n}{m_x^n})$
如果我们认为磁北是0度，那我们就可以得到当前的偏航角。

但是我们不能保证当前的机体绝对水平，即机体系和地理坐标系的指向并不一致，所以我们需要把机体下的传感器数据转换到水平面（即地理坐标系下）。

怎么转换呢？当然是使用旋转矩阵啦。但是我们只需要旋转roll和pitch。

所以令旋转矩阵，yaw为0，即yaw不旋转。根据旋转顺序ZYX可得：
${\text{C}}_b^{n^{\prime }}=\left[\begin{array}{ccc}cos\theta & sin\theta sin\gamma & sin\theta cos\gamma \\ 0& cos\gamma & -sin\gamma \\ -sin\theta & cos\theta sin\gamma & cos\theta cos\gamma \end{array}\right]$
磁力计的输出是在机体坐标系下的磁感应强度，表示为:
${\text{m}}_b=[m_x^b{m}_y^b{m}_z^b{]}^T$
可知 $m_n$与 $m_b$关系式为：
${\text{m}}_n={\text{C}}_b^{n\prime }{\text{m}}_b$
带入已知条件可以得:
$$\begin{gathered} \left[\begin{array}{c}{m}_x^n\\ m_y^n\\ m_z^n\end{array}\right]={\text{C}}_b^{n\prime }\left[\begin{array}{c}{m}_x^b\\ m_y^b\\ m_z^b\end{array}\right] \\ =\left[\begin{array}{ccc}cos\theta & sin\theta sin\gamma & sin\theta cos\gamma \\ 0& cos\gamma & -sin\gamma \\ -sin\theta & cos\theta sin\gamma & cos\theta cos\gamma \end{array}\right]\left[\begin{array}{c}{m}_x^b\\ m_y^b\\ m_z^b\end{array}\right] \\ =\left[\begin{array}{c}{m}_x^b\ast cos\theta +m_y^b\ast sin\theta \ast sin\gamma +m_z^b\ast sin\theta \ast cos\gamma \\ m_y^b\ast cos\gamma -m_z^b\ast sin\gamma \\ -m_x^b\ast sin\theta -m_y^b\ast sin\gamma \ast cos\theta +m_z^b\ast cos\theta \ast cos\gamma \end{array}\right] \\ =\left[\begin{array}{c}{m}_x^b\ast cos(pitch)+m_y^b\ast sin(pitch)\ast sin(roll)+m_z^b\ast sin(pitch)\ast cos(roll)\\ m_y^b\ast cos(roll)-m_z^b\ast sin(roll)\\ m_x^b\ast sin(pitch)-m_y^b\ast sin(roll)\ast cos(pitch)+m_z^b\ast cos(pitch)\ast cos(roll)\end{array}\right] \end{gathered}$$

通过这个旋转，我们就把机体坐标系的数据，变成了地理坐标系下的数据，也就是机体水平的情况下，磁力计测量的数据值。

注意z轴的正负（原始数据是z轴朝上为正）
$m_x^n=m_x^{b}cos\theta +m_y^{b}sin\theta sin\gamma +m_z^{b}sin\theta cos\gamma$
$m_y^n=m_y^{b}cos\gamma -m_z^{b}sin\gamma$

用同样的方法求得x轴与磁北的夹角：
${\psi }_m=arctan(\frac{m_y^n}{m_x^n})$
磁力计所解算的航向角是机体坐标x轴相对于磁北而言的，而真北与磁北之间存在一个磁偏角$\Delta \psi$。

所以机体坐标纵轴相对于真北的航向角为：
$\psi ={\psi }_m+\Delta \psi$

这里有几个隐藏添加条件要注意：

1.我们使用的是zyx顺序旋转。

2.传感器x轴，对齐机体x轴；传感器y轴，对齐机体y轴时yaw为0度。

3.加计和陀螺仪的坐标系是完全重合的。

提问：计算出来的夹角是正还是负？

和机体坐标系有关，如果z轴朝下，那么机体的旋转正方向为顺时针，那么得到的偏航角就是负的。