姿态解算方法（1）_ 一种互补滤波方法

姿态解算方法（1）_ 一种互补滤波方法

本文内容主要是参考《An efficient orientation filter for inertial and inertial/magnetic sensor arrays 》此篇论文，由于其在普通飞控中的广泛运用，所以这里简单记录学习感受以期后续详细思索。当然也是因为这篇比较易懂，正好可以锻炼一下我捉襟见肘的表述能力。写到这里可能已经开始被骂了。 几个较好的博文链接如下：

（1）http://blog.csdn.net/Gen_Ye/article/details/52522721?locationNum=10

（2）http://www.geek-workshop.com/thread-10172-1-1.html

这是在博客园的第一篇博文，想来自己一直攒在电脑里的东西，不仅容易被弄没了，也没办法被大家纠错批评，故 也就不在乎好坏把想到的都写下来。

言归正传，首先，需要了解下四元数的知识，这在维基（3）和（1）中都有很好的解释了，这里为了争取篇幅讲述算法核心部分就不 再赘述。

（3） https://en.wikipedia.org/wiki/Quaternion

 

角加速度的事

那么进入关于姿态确定的部分。三轴陀螺仪模块可以测量传感器所在平面的角加速度，一般来说MPU6050模块返回的是 三个角速度值。如果初始姿态是已知的，时间的计量又是准确的，那么就可以积分出姿态了。可惜的是角速度的测量准确度实在是不能积分出一个准确的位置值。 不过这里还是说一下这个积分过程。

$\begin{array}{l} {}^s\omega = \left[ {\begin{array}{*{20}{c}} 0&{{\omega _x}}&{{\omega _y}}&{{\omega _z}} \end{array}} \right]\\ {}_E^S\dot q = \frac{1}{2}{}_E^S\hat q \otimes {}^S\omega \end{array}$

${}^s\omega$ 为传感器获得的角速度值，再次提醒这里的值可能是不准确的。

${}_E^S\hat q$是表示传感器相对于地面坐标系的SO（3）变换四元数的估计，这里用于表示传感器姿态的估计。$ \otimes $ 是四元数的一种运算方式，在上文的参考文献中可以找得到的。

差分的形式可以表示为： 

 

$\begin{array}{l}\delta {}_E^S{q_{\omega ,t}} = \frac{1}{2}{}_E^S{{\hat q}_{t - 1}} \otimes {}^S{\omega _t}\\{}_E^S{q_{\omega ,t}} = {}_E^S{{\hat q}_{t - 1}} + \delta {}_E^S{q_{\omega ,t}}\Delta t\end{array}$

 

$\Delta t$是两次预测之间的时间差，准确的说是传感器上一次数据和这一次数据获取时间差。

其他的部分和连续的形式对应就不多说了。

 

基于场方向观测的姿态求解 

 首先把姿态观测问题看做是一个 求 尽可能满足 观测结果 的 姿态 的优化问题。 

 文章里用的是梯度下降法，也算是非常常见的寻优方法，在文中说明了用二次何塞因矩阵进行变步长的优化求解是没有必要且费时的。这里我们挂起不谈。

 首先，我们从传感器获得的是加速度方向的数据（一般来说先走低通滤波器走一遍），以及磁场强度方向的数据，都是3元素向量。通常认为加速度计只在测量重力加速度（加了滤波来说还是比较可信的不过会使得熊增加延时），具体是不是我也不知道，实践的实在有限，导致只能先相信着再说。 

好的，既然需要优化，就先给出目标函数。这个目标函数也是给得简单地触目惊心，就是使得误差最小，论文里那个目标函数写的是不够严谨，现在

$\begin{array}{l}\mathop {\min }\limits_{{}_E^S\hat q \in Q} \left| {f\left( {{}_E^S\hat q,{}^E\hat d,{}^S\hat s} \right)} \right|\\f\left( {{}_E^S\hat q,{}^E\hat d,{}^S\hat s} \right) = {}_E^S{{\hat q}^ * } \otimes {}^E\hat d \otimes {}_E^S\hat q - {}^S\hat s\end{array}$

注意这里误差的定义是向量形式的，优化函数也是有着三个对应的量。其中${{}^E\hat d}$表示原本的场向量，在地面坐标系上。 ${}^S\hat s$ 是在传感器坐标系上测得的对应的场向量。${}_E^S{{\hat q}^ * } \otimes {}^E\hat d \otimes {}_E^S\hat q$表述的是四元数的坐标变换的方法。

简要的梯度下降求法如下所述。

${}_E^S{q_{k + 1}} = {}_E^S{\hat q_k} - \mu \frac{{\nabla {\bf{f}}}}{{\left\| {\nabla {\bf{f}}} \right\|}}$

不难得知所谓的 ${\bf{f}}$是根据磁场和重力场的局部方向确定的。$\mu$的具体值可以进行对步长的调整，文中给出了

$\mu  = \alpha \left\| {{}_E^S{{\dot q}_{\omega ,t}}} \right\|\Delta t,\alpha  > 1$

 $\left\| {{}_E^S{{\dot q}_{\omega ,t}}} \right\|$较小时，意味着系统变化并不剧烈，所以也就可以有着较小的步长。需要指出的是，作为一个快速的算法，不会进行过多次的迭代，同时也可以对${}_E^S$进行设置使得其去除某些分量。值得注意的是虽然某个地方的重力 和地磁是短期不变的，但是我们不一定要老老实实的按照NED坐标系(https://en.wikipedia.org/wiki/North_east_down)来构造，加个前置旋转可能会使优化什么的简单些。也许吧。

 

滤波器的融合 

上面两个部分讲了从以不同的方法获得传感器姿态的过程。然而做姿态解算的初衷就是又快又好地获得传感器姿态。

这里的融合就是一个加权平均。

$\begin{array}{l}{}_E^S{q_t} = {\gamma _t}{}_E^S{q_{\nabla ,t}} + \left( {1 - {\gamma _t}} \right){}_E^S{q_{\omega ,t}},0 \le {\gamma _t} \le 1\\{\gamma _t} = \frac{\beta }{{\frac{{{\mu _t}}}{{\Delta t}} + \beta }}\end{array}$

 ${}_E^S{q_{\nabla ,t}}$是我们通过寻优在那用磁场方向和重力方向观测来的，而${}_E^S{q_{\omega ,t}}$是通过陀螺仪模块测得的角速度吭哧吭哧积分出来的。

一般情况下$\beta$会设置得较小，而${{\mu _t}}$较大，也就是说系统的姿态还变化较为显著。这时简化上述公式得到

$\begin{array}{l}{}_E^S{q_t} = {}_E^S{{\hat q}_{t - 1}} + {}_E^S\dot q\Delta t\\{}_E^S{{\dot q}_t} = {}_E^S{{\dot q}_{\omega ,t}} - \beta \frac{{\nabla f}}{{\left\| {\nabla f} \right\|}}\end{array}$

至于由于铁磁性物质导致的磁场的失效是比较难于更正的，其误差也会被引入计算而无法直接更正。

这里还需要说明的是一个关于陀螺仪模块的漂移。

$\begin{array}{l}
{}^S{\omega _{\varepsilon ,t}} = 2{}_E^S\hat q_{t - 1}^ * \otimes \frac{{\nabla {\bf{f}}}}{{\left\| {\nabla {\bf{f}}} \right\|}}\\
{}^S{\omega _{b,t}} = \zeta \sum\limits_t {{}^S\left( {{\omega _{\varepsilon ,t}}\Delta t} \right)}
\end{array}$

${}^S{\omega _{b,t}}$表示具体的角速度漂移值，可以看到这是为了防止由于角速度一直在变动导致梯度下降法求得的估计值一直在最优值某一边，第二式的积分便是为了消除这种类似稳态误差的存在。

 

 这一篇就先告一段落吧，有不足和希望细化的大家及时提出来，互相学习。