旋翼無人機的兩類主要算法

先說一個旋翼類無人機系統的算法主要有兩類：姿態檢測算法、姿態控制算法。

姿態控制、被控對象（即四旋翼無人機）、姿態檢測三個部分構成一個閉環控制系統。被控對象的模型是由其物理系統決定，我們設計無人機的算法就是設計姿態控制算法、姿態檢測算法。

1:姿態檢測算法：姿態的三個自由度可以用歐拉角表示，也可以用四元數表示。姿態檢測算法的作用就是將加速度計、陀螺儀等傳感器的測量值解算成姿態，進而作為系統的反饋量。常用的姿態檢測算法有卡爾曼濾波、互補濾波等。

2:姿態控制算法：控制飛行器姿態的三個自由度，以給定姿態與姿態檢測算法得出的姿態偏差作為輸入，被控對象模型的輸入量作為輸出（例如姿態增量），從而達到控制飛行器姿態的作用。最常用的就是PID控制及其各種PID擴展（分段、模糊等）、高端點的有自適應控制。當然，姿態控制算法里面又常用角速度、角度雙閉環控制，所以常常有PD外環+PID內環等等。

無人機的穩定決定一切

造出一架合格的無人機并不需要花里胡哨的app，穩定決定一切。而穩定又是靠傳感器決定的，傳感器不準怎么辦？當然是靠濾波算法。

加速度計、陀螺儀等傳感器的數據，在現實中，是很“臟”的，也就是說被各種噪聲污染得很厲害。要從大量噪聲中過濾出真實的傳感器數據，這就在姿態測算里涉及到濾波。

大疆的濾波算法就是一個非常核心的競爭力。因此無人機公司的核心產品的核心濾波算法，全公司也就只有那么一兩個人知道，他們平時坐在研發的一個小角落里，深藏不露。

卡爾曼濾波

網上廣為流傳著幾篇關于卡爾曼濾波的科普文章，但是都夾雜著一堆復雜的公式，不知道你看后是一種什么樣的感覺，我看時，看的我如墜云霧里。我希望能看到一篇沒有復雜數學公式的文章，卻一直沒找到。

于是我想寫一篇，講講自己對卡爾曼濾波的淺顯理解。

我覺得卡爾曼濾波算法本質上是一個遞推反饋算法。它分兩部分：時間更新方程和測量狀態更新方程。其中，前者負責遞推，后者負責反饋（將先驗估計和新的測量變量結合，以構造改進后的后驗估計）。

時間更新方程可視為預估方程，測量更新方程可視為校正方程。

這個是預估方程：

x表示狀態變量；左上角帶“—”的量表示先驗；“ ^ ”表示估計。

“先驗”與“后驗”是相對的倆概念，“先驗”的意思就是僅僅用之前的狀態來估計出來的；而“后驗”是在我們進行了測量，知道了測量變量 z 之后的。也就是說，“先驗”與“后驗”的區別就是有沒有進行測量。

預估方程可以這么理解：因為卡爾曼濾波算法的前提條件是：系統是線性的。所以即使不用測量，我們也可以根據“線性”這個特征得到一個先驗狀態值。

這個是校正方程：

中間的那個式子做一下整理就容易理解了：把先驗狀態變量提出來放到一塊兒。

那這個校正方程就可以這么理解了：有了測量值之后，我們便有了兩個可用的量了，即測量值z和先驗狀態值，那我們就根據這兩個量之前的表現來各自給他們分配一個權重（之前表現越好的量，其權重就越高），這個權重就是卡爾曼增益。那么什么是表現好表現不好呢，表現好意思就是測量結果穩定，方差很小，表現不好就是估計值或觀測值不穩定、方差很大。

最后再付一個圖基本就差不多了。

講到這兒我覺得基本就可以了，最起碼在以后生疏之后再看一遍這篇文章，基本就能回想起卡爾曼。

要不最后再說說卡爾曼濾波算法能干嘛吧，我覺得基本上的應用有兩個，一個是測量，一個是預測。

一架飛機在天上飛，我們想知道這架飛行在任一時刻所處的位置、速度等狀態參數，那我們就得用測量裝置進行測量，這樣問題就來了。因為測量裝置存在隨機干擾，所以它測得的結果中就會夾雜著大量隨機干擾，這時測量裝置返回來的狀態參數是不靠譜的。

那怎么辦呢？這就用到了卡爾曼濾波算法，因為卡爾曼濾波的核心是“預測+校正”，所以在測量上，我們用到的就是校正出來后驗狀態變量。

卡爾曼在博士畢業的時候拿著卡爾曼濾波算法作為他畢業論文的主題，唉……差距啊，他這么吊，也不知道他爸媽知不知道。

看到這兒你可能還有點蒙，我們再來通俗的舉例子說明。

假設你有兩個傳感器，測的是同一個信號。可是它們每次的讀數都不太一樣，怎么辦？

取平均。

再假設你知道其中貴的那個傳感器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？

加權平均。

怎么加權？假設兩個傳感器的誤差都符合正態分布，假設你知道這兩個正態分布的方差，用這兩個方差值，（此處省略若干數學公式），你可以得到一個“最優”的權重。

接下來，重點來了：假設你只有一個傳感器，但是你還有一個數學模型。模型可以幫你算出一個值，但也不是那么準。怎么辦？

把模型算出來的值，和傳感器測出的值，（就像兩個傳感器那樣），取加權平均。

OK，最后一點說明：你的模型其實只是一個步長的，也就是說，知道x(k)，我可以求x(k+1)。問題是x(k)是多少呢？

答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權平均之后的那個、對x在k時刻的最佳估計值。

于是迭代也有了。

這就是卡爾曼濾波。

知道一點卡爾曼濾波比較好

作為一個無人機從業者，或許你不是搞算法的、或許你也不是搞飛控的，你可能只是做結構的、業務的，這都沒有關系。但要作為一個資深的無人機從業者，對于無人機的來龍去脈，技術等方面的全面了解，筆者認為是有必要的。

其實，"算法"到最后最是數學問題,整理再多,不會用全白搭！就是網上很多的源碼直接抄，用在你的代碼里，你不明白原理，還是白搭！

總結

以上是生活随笔為你收集整理的作为资深的无人机从业者，卡尔曼滤波你不能不知道 通俗易懂的来说卡尔曼滤波的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。