無人機(jī)飛控三大算法：捷聯(lián)式慣性導(dǎo)航系統(tǒng)、卡爾曼濾波算法、飛行控制PID算法。

一、捷聯(lián)式慣性導(dǎo)航系統(tǒng)

說到導(dǎo)航，不得不說GPS，他是接受衛(wèi)星發(fā)送的信號計算出自身位置的，但是當(dāng)GPS設(shè)備上方被遮擋后，GPS設(shè)備無法定位了。比如在室內(nèi)、隧道內(nèi)、地下等場所，基本收不到GPS信號。

語錄：任何一款有缺點的產(chǎn)品，必然成就了另一款能克服其缺點的產(chǎn)品。

另一種導(dǎo)航方式是不依賴外界信息的，這種導(dǎo)航叫做慣性導(dǎo)航。

那什么是慣性導(dǎo)航呢？他就是利用載體上的加速度計、陀螺儀這兩種慣性遠(yuǎn)見，去分別測出飛行器的角運動信息和線運動信息，與初始姿態(tài)、初始航向、初始位置一起交給計算模塊，由計算模塊推算出飛機(jī)的姿態(tài)、速度、航向、位置等導(dǎo)航參數(shù)的自主式導(dǎo)航方法。

（精益求精：陀螺儀是怎么通過角運動信息再經(jīng)過計算模塊計算得出姿態(tài)的呢？同理加速度計又是怎樣通過線運動信息再經(jīng)過計算模塊計算出速度的呢？）

另外慣性導(dǎo)航系統(tǒng)分為平臺式慣性導(dǎo)航和捷聯(lián)式慣性導(dǎo)航。

慣性導(dǎo)航系統(tǒng)分為平臺式慣性導(dǎo)航和捷聯(lián)式慣性導(dǎo)航。

早期的慣性導(dǎo)航系統(tǒng)都是平臺式的，平臺式慣導(dǎo)有實體的物理平臺，陀螺儀和加速度計置于由陀螺穩(wěn)定的平臺上，該平臺跟蹤導(dǎo)航坐標(biāo)系，以實現(xiàn)速度和位置解算，姿態(tài)數(shù)據(jù)直接取自于平臺的環(huán)架。

優(yōu)點：直接模擬導(dǎo)航坐標(biāo)系，計算比較簡單；能隔離載體的角運動，系統(tǒng)精度高。

缺點：結(jié)構(gòu)復(fù)雜，體積大，制作成本高。

還有另一種捷聯(lián)式慣性導(dǎo)航，捷聯(lián)的英文原義是“捆綁”的意思。因此捷聯(lián)式慣性導(dǎo)航也就是將慣性測量元件，包括陀螺儀和加速度計，直接裝在需要姿態(tài)、速度、航向等導(dǎo)航信息的主體上，用計算機(jī)的測量信號變換為導(dǎo)航參數(shù)。

優(yōu)點是沒有平臺，架構(gòu)簡單，體積小，維護(hù)方便。

缺點：慣性元件直接裝在載體上，環(huán)境惡劣，對元件要求較高；坐標(biāo)變換中計算量大。

總體來看，捷聯(lián)慣導(dǎo)比平臺式慣導(dǎo)優(yōu)勢明顯。

在1969年，捷聯(lián)慣導(dǎo)系統(tǒng)作為”阿波羅”-13號登月飛船的應(yīng)急備份裝置，在其服務(wù)艙發(fā)生爆炸時將飛船成功地引導(dǎo)到返回地球的軌道上時起到了決定性作用，成為捷聯(lián)式慣導(dǎo)系統(tǒng)發(fā)展中的一個里程碑

二、卡爾曼濾波算法

卡爾曼濾波算法采用信號與噪聲的狀態(tài)空間模型，利用前一時刻地估計值和現(xiàn)時刻地觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)刻的估計值。

卡爾曼濾波算法是卡爾曼等 人在20世紀(jì)60年代提出的一種遞推濾波算法。它的實質(zhì)是以最小均方誤差為估計的最佳準(zhǔn)則，來尋求一套遞推估計的算法。這套算法采用信號與噪聲的狀態(tài)空間模型，利用前一時刻地估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)時刻的估計值，在慣性導(dǎo)航系統(tǒng)中有非常廣泛的應(yīng)用。剛才說的噪聲指的是計算得出的值與實際值的誤差。

那么為什么Kalman濾波會應(yīng)用到慣性導(dǎo)航系統(tǒng)中呢？這主要是因為慣性導(dǎo)航系統(tǒng)的“純慣性”傳感器不足以達(dá)到所需的導(dǎo)航精度，為了補償導(dǎo)航系統(tǒng)的不足，常常使用其他導(dǎo)航設(shè)備來提高導(dǎo)航精度，以減小導(dǎo)航誤差。所以利用Kalman濾波算法，可以將來自慣性導(dǎo)航系統(tǒng)與其他導(dǎo)航裝置的數(shù)據(jù)（如慣性導(dǎo)航系統(tǒng)計算的位置對照GPS接收機(jī)給出的位置信息）加以混合利用，估計和校正未知的慣性導(dǎo)航系統(tǒng)誤差。

卡爾曼濾波算法廣泛應(yīng)用已經(jīng)超過30年，包括機(jī)器人導(dǎo)航，控制， 傳感器數(shù)據(jù)融合甚至軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。

比如，在雷達(dá)中，人們感興趣的是跟蹤目標(biāo)，但目標(biāo)的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標(biāo)的動態(tài)信息,設(shè)法去掉噪聲的影響，得到一個關(guān)于目標(biāo)位置最優(yōu)的估計。這個估計可以是對當(dāng)前目標(biāo)位置的估計(濾波)，也可以是對于將來位置的估計(預(yù)測)，也可以是對過去位置的估計(插值或平滑)。

卡爾曼濾波算法是一個非常復(fù)雜的計算，我們結(jié)合飛行器來簡單的講一下它的計算過程，比如飛行器想知道自己的一個狀態(tài)，這個狀態(tài)可以是姿態(tài)、速度或位置等信息，我們知道飛行器的傳感器是可以得到這些信息的，通過慣性導(dǎo)航的數(shù)學(xué)模型也可以計算出這些信息，但這兩個信息的值與實際值還是有一定的差距的，把這兩個值放在若干數(shù)學(xué)公式里可以得到一個最優(yōu)值，通過這個最優(yōu)值與傳感器和數(shù)學(xué)模型的值進(jìn)行對比，我們可以知道哪個值與最優(yōu)值比較接近，下次的計算我們應(yīng)該較多的參考接近最優(yōu)值的那個值，比如傳感器的值最接近最優(yōu)值，那我們就把傳感器的值使用較大的權(quán)重，數(shù)學(xué)模型得到的值加使用較小的權(quán)重，所得到的這個權(quán)重不是隨便給的，也是通過數(shù)學(xué)公式得也來的。那我下一個時間段再計算的時候這個權(quán)重就要起作用了，傳感器的值和數(shù)學(xué)模型的值會帶著權(quán)重放在數(shù)學(xué)公式里得到最優(yōu)值，然后我們再把這個最優(yōu)值與傳感器和數(shù)學(xué)模型的值進(jìn)行對比，再看一下哪個值與最優(yōu)值接近，如果還是傳感器的值比較接近，我們還是會通過公式生成一個權(quán)重，交給下個時間段的計算使用。通過不斷重復(fù)這樣的計算，我們就可以得到一個相對較優(yōu)的值，這就是卡爾曼濾波算法的大概過程。

三、飛行控制PID算法

PID控制器是一種線性控制器，它主要根據(jù)給定值和實際輸出值構(gòu)成控制偏差，然后利用偏差給出合理的控制量。

目前，人們通過科學(xué)研究獲得了諸多具有優(yōu)異控制效果的算法和理論，但在工程應(yīng)用領(lǐng)域，基于經(jīng)典PID的控制算法仍然是最簡單、最有效的控制方案。

PID控制器是一種線性控制器，它主要根據(jù)給定值和實際輸出值構(gòu)成控制偏差，然后利用偏差給出合理的控制量。

目前主流的幾款開源飛控中，無一例外地都是采用PID控制算法來實現(xiàn)無人機(jī)的姿態(tài)和軌跡控制。

PID里的P是Proportion的首字線，是比例的意思，I是Integral的首字線，是積分的意思，D是Differential的首字母，是微分的意思。

那么PID控制器算法能解決什么問題呢？以多旋翼為例，在沒有控制系統(tǒng)的情況下，直接用信號驅(qū)動電機(jī)帶動螺旋槳旋轉(zhuǎn)產(chǎn)生控制力，會出現(xiàn)動態(tài)響應(yīng)太快，或者太慢，或者控制過沖或者不足的現(xiàn)象，多旋翼根本無法順利完成起飛和懸停動作。為了解決這些問題，就需要在控制系統(tǒng)回路中加入PID控制器算法。在姿態(tài)信息和螺旋槳轉(zhuǎn)速之間建立比例、積分和微分的關(guān)系，通過調(diào)節(jié)各個環(huán)節(jié)的參數(shù)大小，使多旋翼系統(tǒng)控制達(dá)到動態(tài)響應(yīng)迅速、既不過沖、也不欠缺的現(xiàn)象。

下面我們簡單的舉個例子讓大家了解一下PID的作用，我們先以一個自動駕駛的小車來舉例子，為什么用小車呢而不用多旋翼來舉例子呢？因為用小車舉例，可以看到小車的行進(jìn)軌跡，根據(jù)軌跡我們可以很直觀的看到PID對控制的影響，便于理解PID的作用。

我們先講P，比例控制?，F(xiàn)在我們想讓這個小車沿著綠線向前走，我們給P設(shè)置一個固定的值，這個值可以讓離開綠線的小車向綠線的方向行駛，離的越遠(yuǎn)，方向盤打的角度越大，離的越近，方向盤打的角度越小。橙色的箭頭表示小車行進(jìn)的方向

比如這個小車在這個位置，我們設(shè)置了一個中等大小的P值，想要沿著綠線走，在比例控制下路徑是這樣的，因為小車有一定的速度，到達(dá)綠線時因為慣性的原因又向前運動了，然后再根據(jù)中等P值向回打方向盤，當(dāng)?shù)竭_(dá)綠線時，同樣因為慣性的原因沖過了頭，因為小車越接近綠線，方向盤打的越小，所以小車每經(jīng)過一次綠線，它的偏差就越來越小，所以隨著小車多次的往復(fù)運動，就能離綠線越來越近，理論上最終能夠行駛在綠線上面。

如果我們把P值設(shè)置的比較大，它的路徑是這樣的，因為方向盤打的角度比較大，所以小車比中等P值的時候較早的到達(dá)綠線，但同樣因為慣性的原因會多次往返綠線的兩側(cè)，也是一次比一次接近綠線，因為它方向盤角度打的比較大，所以會比中等P值往返的次數(shù)要多，最終經(jīng)過多次往返，理論上它會離綠線越來越近，最終到達(dá)綠線上方。

如果我們把P值設(shè)置的比較小，也就是方向盤打的角度比較小，它的路徑是這樣的，小車會較晚到達(dá)綠線，因為慣性會往返綠線兩側(cè)，但是因為方向盤角度小，小車可以在較少的往復(fù)次數(shù)下接近綠線。

所以我們對比一下設(shè)置這三種P值的結(jié)果，在小車與綠線相同距離的情況下，P值越大，小車的反應(yīng)越快，P值越小，小車的反應(yīng)越慢，所以這三個小車第一次到達(dá)綠線的時間是不一樣的。雖然P值大能夠較快的到達(dá)綠線，但是反應(yīng)比較劇烈，總是因為過快沖過了頭。相反P值小的反應(yīng)比較平緩，但是它反應(yīng)太慢，我們有時候接受不了。

什么樣的P值是合理的呢，就是設(shè)置后，小車的反應(yīng)不是很劇烈，反應(yīng)時間你也能夠接受，那這就是一個相對合理的P值。

那有沒有一種辦法讓它反應(yīng)再快一點，反應(yīng)又不那么劇烈呢，那就要用到接下來我們要講的微分控制了。

我們?yōu)榱瞬蛔屵@個小車沖過頭，我們再給它加一個力，這個力就是D，讓這個力來起一個作用，就是讓小車越接近綠線的時候，接近綠線的速度越慢，小車越遠(yuǎn)離綠線的時候，接近綠線的速度相對較快，這個接近綠線的速度不是小車前進(jìn)的速度，是小車與綠線平行線之間的相對速度。這個D大家可以理解為小車靠近綠線的一個阻力。

假設(shè)我們設(shè)置了一個相對合理的P值，在P值不變的情況下，微分控制中D值的變化會有怎樣的結(jié)果。比如我們設(shè)置了一個比較合適的D值，微分控制(D)讓小車在靠近綠線時，接近綠線的速度比較慢，這樣比例控制（P）就可以很輕松的讓小車到達(dá)綠線上方行駛。

如果D值過大，也就是小車靠近綠線的阻力過大，這樣會讓小車需要比較長的時間才能到達(dá)綠線上方。

如果D值設(shè)置的過小，也就是小車靠近綠線的阻力過小，那微分控制(D)就不會對比例控制產(chǎn)生大的影響，所以雖然小車能夠較快到達(dá)綠線，但小車需要多調(diào)整幾次，在綠線的兩側(cè)往復(fù)幾次后才能到達(dá)綠線上方行駛。

那這樣看來比例控制（P）和微分控制（D）的配合，貌似已經(jīng)很完美了。為什么還要有積分控制（I）呢？

設(shè)置合適的P值和D值，可以讓小車很好的沿著綠線一直走，但路上不是很平坦，會有些坑坑包包，或者其他的一些干擾，路況不好就會讓小車的行進(jìn)路線發(fā)生偏移，比如小車在這里遇到了坑坑包包，它的行進(jìn)路線 就會變成這樣，稍微偏離了綠線一點，因為微分控制（D）讓小車離綠線越近時，靠近綠線的速度越慢，比例控制（P）讓小車在接近綠線時，方向盤又打的比較小，所以小車要走一段路才回到綠線上面。

有沒有辦法讓它更快回到綠線上面呢，所以我們再給它加一個力，這個力就是I，積分控制。我們讓積分控制起這樣的作用：如果P和D的調(diào)節(jié)不是很理想的話，就讓I幫他倆一把，向P的方向上加一個力，這樣可以讓小車更快回到目的路徑。

設(shè)置了I以后，I會根據(jù)誤差和誤差經(jīng)歷的時間進(jìn)行積分，然后決定施加給目標(biāo)方向的力的大小，就能夠讓小車回到目標(biāo)軌跡上。

一個合適的I值，可以讓小車偏離軌跡后，I可以在合適的時間給P一個合適的力，讓小車快速的回到綠線上面；

如果I值過大，積分控制（I）調(diào)整的力就會比較大，它在幫P的時候會用力過猛，會讓小車沖過綠線，下次幫忙的時候還是用力過猛，P表示很無奈，畢竟I也是好心，經(jīng)過幾次調(diào)整后，小車終于回到綠線上面，但我們看小車的軌跡線產(chǎn)生了一定的振蕩；

如果I值過小，積分控制（I）調(diào)整的力就會比較小，就像有一個手無縛雞之力的柔弱小鮮肉一樣，幫不上太大的忙，所以小車回到正確軌跡的時間就會比較長。

說完了PID控制對小車的影響，下面我們再說說PID控制對多旋翼的影響。比如這架多旋翼想要作的是保持機(jī)身的水平平穩(wěn)。

我們先從P開始，如果P值設(shè)置的過大，哪怕機(jī)身有那么一丁點傾斜，飛行器都會用稍大一些的力去調(diào)整，結(jié)果用力過猛，又繼續(xù)向回調(diào)整，這樣飛行器就會頻繁的調(diào)整自己的水平狀態(tài)，導(dǎo)至機(jī)身產(chǎn)生振動。

　　　　　　　　　　　　上圖中指向左側(cè)的箭頭寫的是過大不是過人

如果P值設(shè)置的過小，飛行器的水平調(diào)整就會顯得力不從心，比如空氣中的微風(fēng)讓飛行器發(fā)生了傾斜，飛行器向回調(diào)整的力比較小，所以需要長一點的時間才能調(diào)平，這樣讓我們覺得它反應(yīng)有點慢。

比如我們經(jīng)過多次測試設(shè)置了一個比較合適的P值，可以讓飛行器有一個我們能接受的反應(yīng)時間，但稍有一點過沖，會有一些震蕩，接下來再設(shè)置D值，讓D值消除震蕩，如果D值設(shè)置過大，會讓飛行器恢復(fù)平衡時間過長，反應(yīng)變慢，

如果設(shè)置了一個過小的D值，會導(dǎo)致效果不明顯，飛行器還是會有震蕩。

比如我們又設(shè)置了一個合適的D值，可以讓飛行器反應(yīng)不是很慢，也不會有明顯的震蕩。接下來我們再設(shè)置一下I值。

這里的I我們可以這么理解，I根據(jù)飛行器的反應(yīng)時間，適當(dāng)?shù)膸兔Γ绻w行器反應(yīng)慢了，他就會幫一下，能讓它的反應(yīng)快一點。但是如果值設(shè)置的過大，這個幫忙可能會用力過猛，導(dǎo)致過沖。

如果I值設(shè)置過小，它的幫忙就沒有太大的作用，還是不會讓飛行器反應(yīng)更快。所以設(shè)置一個合適的I值需要多次的測試，讓它的幫忙起作用，又不會用力過猛。

有些人怕麻煩就不設(shè)置I值了，其實也沒有太大的影響，只不過離完美還差那么一點。

PID的設(shè)置是一個非常復(fù)雜的過程，對于一般的使用者來說是非常難的。所以有些廠商也想出了一些辦法。

比如3DR的開源飛控APM和PIXHWAK，咱們看它的調(diào)參軟件中，PID部分有這么多要調(diào)的地方，對于一般的用戶來說是比較有難度的，所以它加了一個自動調(diào)參的功能，就是這個AutoTune[‘?:to?t?n]，可以把遙控器上的一個開關(guān)設(shè)置成自動調(diào)參，找一片開闊地讓飛行器起飛，然后把遙控器上的自動調(diào)參開關(guān)打開，飛控就開始自己控制飛行器進(jìn)行飛行測試，然后根據(jù)測試情況自動設(shè)置一個比較合適的PID參數(shù)。

有些廠商作的就更簡單了，直接在調(diào)參軟件里加了一個感度的配置，這個感度大家可以理解為敏感度 ，數(shù)值越大，敏感度越高，不同的軸距都有不同的推薦感度設(shè)置。

左面的這個是大疆NAZA飛控的調(diào)參軟件，調(diào)參軟件說明書里就有一個不同軸距的推薦感度值，右面的拓攻更簡單，直接選擇軸距，就能自動設(shè)置推薦的感度值。

這些廠商的調(diào)參軟件里一般都會有姿態(tài)感度和基本感度，或者叫穩(wěn)定感度。姿態(tài)感度是指飛行器對遙控指令的敏感程度，基本感度或穩(wěn)定感度是指飛行器對于外界干擾反應(yīng)的敏感程度。這樣就比PID理解起來要簡單的多了。