姿態解算方法（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}}$表示具體的角速度漂移值，可以看到這是為了防止由於角速度一直在變動導致梯度下降法求得的估計值一直在最優值某一邊，第二式的積分便是為了消除這種類似穩態誤差的存在。

 

 這一篇就先告一段落吧，有不足和希望細化的大家及時提出來，互相學習。