無人機飛控三大算法:捷聯式慣性導航系統、卡爾曼濾波算法、飛行控制PID算法。
一、捷聯式慣性導航系統
說到導航,不得不說GPS,他是接受衛星發送的信號計算出自身位置的,但是當GPS設備上方被遮擋后,GPS設備無法定位了。比如在室內、隧道內、地下等場所,基本收不到GPS信號。
語錄:任何一款有缺點的產品,必然成就了另一款能克服其缺點的產品。
另一種導航方式是不依賴外界信息的,這種導航叫做慣性導航。
那什么是慣性導航呢?他就是利用載體上的加速度計、陀螺儀這兩種慣性遠見,去分別測出飛行器的角運動信息和線運動信息,與初始姿態、初始航向、初始位置一起交給計算模塊,由計算模塊推算出飛機的姿態、速度、航向、位置等導航參數的自主式導航方法。
(精益求精:陀螺儀是怎么通過角運動信息再經過計算模塊計算得出姿態的呢?同理加速度計又是怎樣通過線運動信息再經過計算模塊計算出速度的呢?)
另外慣性導航系統分為平台式慣性導航和捷聯式慣性導航。
慣性導航系統分為平台式慣性導航和捷聯式慣性導航。
早期的慣性導航系統都是平台式的,平台式慣導有實體的物理平台,陀螺儀和加速度計置於由陀螺穩定的平台上,該平台跟蹤導航坐標系,以實現速度和位置解算,姿態數據直接取自於平台的環架。
優點:直接模擬導航坐標系,計算比較簡單;能隔離載體的角運動,系統精度高。
缺點:結構復雜,體積大,制作成本高。
還有另一種捷聯式慣性導航,捷聯的英文原義是“捆綁”的意思。因此捷聯式慣性導航也就是將慣性測量元件,包括陀螺儀和加速度計,直接裝在需要姿態、速度、航向等導航信息的主體上,用計算機的測量信號變換為導航參數。
優點是沒有平台,架構簡單,體積小,維護方便。
缺點:慣性元件直接裝在載體上,環境惡劣,對元件要求較高;坐標變換中計算量大。
總體來看,捷聯慣導比平台式慣導優勢明顯。
在1969年,捷聯慣導系統作為"阿波羅"-13號登月飛船的應急備份裝置,在其服務艙發生爆炸時將飛船成功地引導到返回地球的軌道上時起到了決定性作用,成為捷聯式慣導系統發展中的一個里程碑
二、卡爾曼濾波算法
卡爾曼濾波算法采用信號與噪聲的狀態空間模型,利用前一時刻地估計值和現時刻地觀測值來更新對狀態變量的估計,求出現刻的估計值。
卡爾曼濾波算法是卡爾曼等 人在20世紀60年代提出的一種遞推濾波算法。它的實質是以最小均方誤差為估計的最佳准則,來尋求一套遞推估計的算法。這套算法采用信號與噪聲的狀態空間模型,利用前一時刻地估計值和現時刻的觀測值來更新對狀態變量的估計,求出現時刻的估計值,在慣性導航系統中有非常廣泛的應用。剛才說的噪聲指的是計算得出的值與實際值的誤差。
那么為什么Kalman濾波會應用到慣性導航系統中呢?這主要是因為慣性導航系統的“純慣性”傳感器不足以達到所需的導航精度,為了補償導航系統的不足,常常使用其他導航設備來提高導航精度,以減小導航誤差。所以利用Kalman濾波算法,可以將來自慣性導航系統與其他導航裝置的數據(如慣性導航系統計算的位置對照GPS接收機給出的位置信息)加以混合利用,估計和校正未知的慣性導航系統誤差。
卡爾曼濾波算法廣泛應用已經超過30年,包括機器人導航,控制, 傳感器數據融合甚至軍事方面的雷達系統以及導彈追蹤等等。
比如,在雷達中,人們感興趣的是跟蹤目標,但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態信息,設法去掉噪聲的影響,得到一個關於目標位置最優的估計。這個估計可以是對當前目標位置的估計(濾波),也可以是對於將來位置的估計(預測),也可以是對過去位置的估計(插值或平滑)。
卡爾曼濾波算法是一個非常復雜的計算,我們結合飛行器來簡單的講一下它的計算過程,比如飛行器想知道自己的一個狀態,這個狀態可以是姿態、速度或位置等信息,我們知道飛行器的傳感器是可以得到這些信息的,通過慣性導航的數學模型也可以計算出這些信息,但這兩個信息的值與實際值還是有一定的差距的,把這兩個值放在若干數學公式里可以得到一個最優值,通過這個最優值與傳感器和數學模型的值進行對比,我們可以知道哪個值與最優值比較接近,下次的計算我們應該較多的參考接近最優值的那個值,比如傳感器的值最接近最優值,那我們就把傳感器的值使用較大的權重,數學模型得到的值加使用較小的權重,所得到的這個權重不是隨便給的,也是通過數學公式得也來的。那我下一個時間段再計算的時候這個權重就要起作用了,傳感器的值和數學模型的值會帶着權重放在數學公式里得到最優值,然后我們再把這個最優值與傳感器和數學模型的值進行對比,再看一下哪個值與最優值接近,如果還是傳感器的值比較接近,我們還是會通過公式生成一個權重,交給下個時間段的計算使用。通過不斷重復這樣的計算,我們就可以得到一個相對較優的值,這就是卡爾曼濾波算法的大概過程。
三、飛行控制PID算法
PID控制器是一種線性控制器,它主要根據給定值和實際輸出值構成控制偏差,然后利用偏差給出合理的控制量。
目前,人們通過科學研究獲得了諸多具有優異控制效果的算法和理論,但在工程應用領域,基於經典PID的控制算法仍然是最簡單、最有效的控制方案。
PID控制器是一種線性控制器,它主要根據給定值和實際輸出值構成控制偏差,然后利用偏差給出合理的控制量。
目前主流的幾款開源飛控中,無一例外地都是采用PID控制算法來實現無人機的姿態和軌跡控制。
PID里的P是Proportion的首字線,是比例的意思,I是Integral的首字線,是積分的意思,D是Differential的首字母,是微分的意思。
那么PID控制器算法能解決什么問題呢?以多旋翼為例,在沒有控制系統的情況下,直接用信號驅動電機帶動螺旋槳旋轉產生控制力,會出現動態響應太快,或者太慢,或者控制過沖或者不足的現象,多旋翼根本無法順利完成起飛和懸停動作。為了解決這些問題,就需要在控制系統回路中加入PID控制器算法。在姿態信息和螺旋槳轉速之間建立比例、積分和微分的關系,通過調節各個環節的參數大小,使多旋翼系統控制達到動態響應迅速、既不過沖、也不欠缺的現象。
下面我們簡單的舉個例子讓大家了解一下PID的作用,我們先以一個自動駕駛的小車來舉例子,為什么用小車呢而不用多旋翼來舉例子呢?因為用小車舉例,可以看到小車的行進軌跡,根據軌跡我們可以很直觀的看到PID對控制的影響,便於理解PID的作用。
我們先講P,比例控制。現在我們想讓這個小車沿着綠線向前走,我們給P設置一個固定的值,這個值可以讓離開綠線的小車向綠線的方向行駛,離的越遠,方向盤打的角度越大,離的越近,方向盤打的角度越小。橙色的箭頭表示小車行進的方向
比如這個小車在這個位置,我們設置了一個中等大小的P值,想要沿着綠線走,在比例控制下路徑是這樣的,因為小車有一定的速度,到達綠線時因為慣性的原因又向前運動了,然后再根據中等P值向回打方向盤,當到達綠線時,同樣因為慣性的原因沖過了頭,因為小車越接近綠線,方向盤打的越小,所以小車每經過一次綠線,它的偏差就越來越小,所以隨着小車多次的往復運動,就能離綠線越來越近,理論上最終能夠行駛在綠線上面。
如果我們把P值設置的比較大,它的路徑是這樣的,因為方向盤打的角度比較大,所以小車比中等P值的時候較早的到達綠線,但同樣因為慣性的原因會多次往返綠線的兩側,也是一次比一次接近綠線,因為它方向盤角度打的比較大,所以會比中等P值往返的次數要多,最終經過多次往返,理論上它會離綠線越來越近,最終到達綠線上方。
如果我們把P值設置的比較小,也就是方向盤打的角度比較小,它的路徑是這樣的,小車會較晚到達綠線,因為慣性會往返綠線兩側,但是因為方向盤角度小,小車可以在較少的往復次數下接近綠線。
所以我們對比一下設置這三種P值的結果,在小車與綠線相同距離的情況下,P值越大,小車的反應越快,P值越小,小車的反應越慢,所以這三個小車第一次到達綠線的時間是不一樣的。雖然P值大能夠較快的到達綠線,但是反應比較劇烈,總是因為過快沖過了頭。相反P值小的反應比較平緩,但是它反應太慢,我們有時候接受不了。
什么樣的P值是合理的呢,就是設置后,小車的反應不是很劇烈,反應時間你也能夠接受,那這就是一個相對合理的P值。
那有沒有一種辦法讓它反應再快一點,反應又不那么劇烈呢,那就要用到接下來我們要講的微分控制了。
我們為了不讓這個小車沖過頭,我們再給它加一個力,這個力就是D,讓這個力來起一個作用,就是讓小車越接近綠線的時候,接近綠線的速度越慢,小車越遠離綠線的時候,接近綠線的速度相對較快,這個接近綠線的速度不是小車前進的速度,是小車與綠線平行線之間的相對速度。這個D大家可以理解為小車靠近綠線的一個阻力。
假設我們設置了一個相對合理的P值,在P值不變的情況下,微分控制中D值的變化會有怎樣的結果。比如我們設置了一個比較合適的D值,微分控制(D)讓小車在靠近綠線時,接近綠線的速度比較慢,這樣比例控制(P)就可以很輕松的讓小車到達綠線上方行駛。
如果D值過大,也就是小車靠近綠線的阻力過大,這樣會讓小車需要比較長的時間才能到達綠線上方。
如果D值設置的過小,也就是小車靠近綠線的阻力過小,那微分控制(D)就不會對比例控制產生大的影響,所以雖然小車能夠較快到達綠線,但小車需要多調整幾次,在綠線的兩側往復幾次后才能到達綠線上方行駛。
那這樣看來比例控制(P)和微分控制(D)的配合,貌似已經很完美了。為什么還要有積分控制(I)呢?
設置合適的P值和D值,可以讓小車很好的沿着綠線一直走,但路上不是很平坦,會有些坑坑包包,或者其他的一些干擾,路況不好就會讓小車的行進路線發生偏移,比如小車在這里遇到了坑坑包包,它的行進路線 就會變成這樣,稍微偏離了綠線一點,因為微分控制(D)讓小車離綠線越近時,靠近綠線的速度越慢,比例控制(P)讓小車在接近綠線時,方向盤又打的比較小,所以小車要走一段路才回到綠線上面。
有沒有辦法讓它更快回到綠線上面呢,所以我們再給它加一個力,這個力就是I,積分控制。我們讓積分控制起這樣的作用:如果P和D的調節不是很理想的話,就讓I幫他倆一把,向P的方向上加一個力,這樣可以讓小車更快回到目的路徑。
設置了I以后,I會根據誤差和誤差經歷的時間進行積分,然后決定施加給目標方向的力的大小,就能夠讓小車回到目標軌跡上。
一個合適的I值,可以讓小車偏離軌跡后,I可以在合適的時間給P一個合適的力,讓小車快速的回到綠線上面;
如果I值過大,積分控制(I)調整的力就會比較大,它在幫P的時候會用力過猛,會讓小車沖過綠線,下次幫忙的時候還是用力過猛,P表示很無奈,畢竟I也是好心,經過幾次調整后,小車終於回到綠線上面,但我們看小車的軌跡線產生了一定的振盪;
如果I值過小,積分控制(I)調整的力就會比較小,就像有一個手無縛雞之力的柔弱小鮮肉一樣,幫不上太大的忙,所以小車回到正確軌跡的時間就會比較長。
說完了PID控制對小車的影響,下面我們再說說PID控制對多旋翼的影響。比如這架多旋翼想要作的是保持機身的水平平穩。
我們先從P開始,如果P值設置的過大,哪怕機身有那么一丁點傾斜,飛行器都會用稍大一些的力去調整,結果用力過猛,又繼續向回調整,這樣飛行器就會頻繁的調整自己的水平狀態,導至機身產生振動。
上圖中指向左側的箭頭寫的是過大不是過人
如果P值設置的過小,飛行器的水平調整就會顯得力不從心,比如空氣中的微風讓飛行器發生了傾斜,飛行器向回調整的力比較小,所以需要長一點的時間才能調平,這樣讓我們覺得它反應有點慢。
比如我們經過多次測試設置了一個比較合適的P值,可以讓飛行器有一個我們能接受的反應時間,但稍有一點過沖,會有一些震盪,接下來再設置D值,讓D值消除震盪,如果D值設置過大,會讓飛行器恢復平衡時間過長,反應變慢,
如果設置了一個過小的D值,會導致效果不明顯,飛行器還是會有震盪。
比如我們又設置了一個合適的D值,可以讓飛行器反應不是很慢,也不會有明顯的震盪。接下來我們再設置一下I值。
這里的I我們可以這么理解,I根據飛行器的反應時間,適當的幫忙,如果飛行器反應慢了,他就會幫一下,能讓它的反應快一點。但是如果值設置的過大,這個幫忙可能會用力過猛,導致過沖。
如果I值設置過小,它的幫忙就沒有太大的作用,還是不會讓飛行器反應更快。所以設置一個合適的I值需要多次的測試,讓它的幫忙起作用,又不會用力過猛。
有些人怕麻煩就不設置I值了,其實也沒有太大的影響,只不過離完美還差那么一點。
PID的設置是一個非常復雜的過程,對於一般的使用者來說是非常難的。所以有些廠商也想出了一些辦法。
比如3DR的開源飛控APM和PIXHWAK,咱們看它的調參軟件中,PID部分有這么多要調的地方,對於一般的用戶來說是比較有難度的,所以它加了一個自動調參的功能,就是這個AutoTune[‘ɔ:toʊtən],可以把遙控器上的一個開關設置成自動調參,找一片開闊地讓飛行器起飛,然后把遙控器上的自動調參開關打開,飛控就開始自己控制飛行器進行飛行測試,然后根據測試情況自動設置一個比較合適的PID參數。
有些廠商作的就更簡單了,直接在調參軟件里加了一個感度的配置,這個感度大家可以理解為敏感度 ,數值越大,敏感度越高,不同的軸距都有不同的推薦感度設置。
左面的這個是大疆NAZA飛控的調參軟件,調參軟件說明書里就有一個不同軸距的推薦感度值,右面的拓攻更簡單,直接選擇軸距,就能自動設置推薦的感度值。
這些廠商的調參軟件里一般都會有姿態感度和基本感度,或者叫穩定感度。姿態感度是指飛行器對遙控指令的敏感程度,基本感度或穩定感度是指飛行器對於外界干擾反應的敏感程度。這樣就比PID理解起來要簡單的多了。