5種常用的四軸飛行器PID算法講解集合

1. 三角函數直接解算歐拉角+互補濾波+單級PID版本
效果：簡單暴力，但是補濾波效果差，單級PID響應慢，打舵跟隨效應差。
2. 三角函數直接解算歐拉角+卡爾曼濾波+單級PID版本
效果：卡爾曼濾波噪聲偏大，滯后略微嚴重，單級PID難操作，打舵響應慢，跟隨效應差。不過比較適合初學四軸的人，難度比四元數加串級PID版本低，易於理解。

3.四元數姿態解算+互補濾波（德國開源四軸）+串級PID版本
效果：四元數難理解，基於PI控制的互補濾波不適合非專業人員，PID參數較單級PID參數難調。打舵響應極佳，穩定程度高，易於操作，是目前四軸的主流算法
附帶程序下載：

現在分享5中最常用的PID算法

三角函數直接解算：加速度計輸出的數值為多少個g，就是多少倍加速度，在物體在靜止時會受到1個g的重力加速度，如果加速度計水平放置，那么Z軸數據就是1，XY軸就是0，原理不懂的話不用看了，好好讀完高中再來研究四軸。當加速度計XY軸產生偏轉時，XYZ軸的數值會產生變動，此時根據反三角函數即可解算出當前的角度。公式：AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*57.2957795f;AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*57.2957795f;后面的數值是180/PI 目的是弧度轉角度這種方法簡單實用但是不是太准確，因為不光重力能產生加速度，運動也能產生加速度，當運動產生的加速度引入時，就會干擾解算精度，因此對運動的還原性較差。此外，網上的版本多是用反正弦函數解算的，那樣其實是錯的，假使我要解算物體在XZ坐標軸所構成的平面的角度時朝Y軸偏移了一點，那么解算出來的值就是錯的，原因自己畫圖想想吧。
互補濾波：當我們用三角函數直接解算出姿態后，需要對其經行濾波以及和陀螺儀的數據進行融合。因為加速度是很容易受外界干擾的，一個手機開了震動模式放在水平面上，實際角度是0度，但是解算出來的值是在0度正負某個范圍內呈均勻分布的，這樣的值顯然不適合使用，因此需要陀螺儀的幫組。陀螺儀輸出的數據是多少度一秒，對這個數據積分就可以算出系統偏轉過的角度。陀螺儀受震動影響小，故短時間內可以信任它，但是陀螺儀會有溫飄，其誤差是隨着溫度而改變的，陀螺儀出廠后還會存在一定的靜差，而且積分也有誤差，故長時間不能信任陀螺儀。由於加速度計長時間來說值得信任，故可以用互補濾波來融合二者的優點，消減二者的缺點。公式：Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;陀螺儀數據正負號根據自己需要而改變在上式中我們可以看出互補濾波是由兩個小式子相加得到的，小式子前有一個系數，二者相加為1，我們可以理解這兩個數是我們對加速度計和陀螺儀的信任度，你信任哪個的程度大點，哪個的權值就相應的變大，其輸出數據在最終結果中占的比重也越大。前面一個式子是用陀螺儀積分計算角度，后面一個式子是用加速度計解算出的角度，當加速度計比重很小時就可以壓制加速度噪聲，也就是進行了低通濾波，前式也就是對陀螺儀進行了高通濾波。從公式中不難得出互補濾波的原理，陀螺儀占的比重偏大，短時間內以陀螺儀數據為准，加速度計占的比重較小，長時間內以加速度計來校准角度數據。一般情況下來講，陀螺儀比加速度計的比值取值為0.95：0.05或0.98：0.02。當然也可以根據實際情況降低陀螺儀的比重。
單極PID：當你知道系統當前狀態和期望狀態后，如何將系統從當前狀態調整到期望狀態是個問題，在此我們可以用PID進行調整，PID分為位置式和增量式，位置式適合舵機等系統，在此使用的是增量式。公式：PID=P*e(n)+I*[(e(n)+e(n-1)+...+e(0)]+D*D后面的當前誤差減前次誤差也可以直接使用陀螺儀的數據代替，原理一樣。單級PID整定方法參見我寫的第一篇帖子！
串級PID：單極PID適合線性系統，當輸出量和被控制量呈線性關系時單極PID能獲得較好的效果，但是四軸不是線性系統，現代學者認為，四軸通常可以簡化為一個二階阻尼系統。為什么四軸不是線性系統呢？首先，輸出的電壓和電機轉速不是呈正比的，其次，螺旋槳轉速和升力是平方倍關系，故單極PID在四軸上很難取得很好效果，能飛，但是不好飛。為了解決這個問題，我們提出了串級PID這個解決方法。串級PID就是兩個PID串在一起，分為內環和外環PID。在此，我們使用內環PID控制，外環PI控制。單極PID輸入的是期望角度，反饋的是角度數據，串級PID中外環輸入反饋的也是角度數據，內環輸入反饋的便是角速度數據。通俗來講，內環就是你希望將四軸以多少度每秒的速度運動，然后他給你糾正過來，外環就是根據角度偏差告訴內環你該以多少度一秒運動。這樣，即使外環數據劇烈變化，四軸的效果也不會顯得很僵硬。在內環中，PID三個數據作用分別是：P（將四軸從偏差角速度糾正回期望角速度）D（抑制系統運動）I（消除角速度控制靜差）外環PI中，兩個數據的作用是：P（將四軸從偏差角度糾正回期望角度）I（消除角度控制靜差）整定方法：1，將內外環PID都歸0，適當增加內環的P，調整P至四軸從正面朝上自然轉動到正面朝下時能感受到阻力，且沒有抖動，有抖動就應減小P，當P減小到無抖動或者輕微抖動時即可。2，讓內環的D慢慢增加，到你用手能明顯感受到轉動四軸產生排斥外力的阻力即可，D能抑制P產生的振盪，但是D過大也會導致高頻振盪，調整D至系統無振盪且能抑制外界的力即可。3，給內環一點點I，注意的是I的積分要在油門開啟后才開始，油門關閉就清0，且必須有積分限幅。I推薦取越小越好，我取的是0.01，I取大了會導致系統振盪。4，將內環P減半，將外環P調至內環的50-70倍，根據系統產生的高頻振盪降低內環的D，直至高頻振盪消除即可。5，給外環一點點I，同3.6，根據實際情況對參數進行優化調整，調整過程中要注意區分各個參數的作用，時刻記住，P是回復力，大了會低頻振盪，D是抑制力，大了會高頻振盪，I是靜差消除力，越小越好，大了會產生振盪。
壇子里面分享的串級PID算法:
【開源】分享自己寫的串級PID算法
http://bbs.elecfans.com/jishu_486485_1_1.html
卡爾曼濾波：參見資料包內的 新手平衡小車的卡爾曼濾波算法總結.doc 寫的很好，不想重復了，說白了就是5個公式，用協方差矩陣對加速度計的值進行線性擬合再融合。不過后來了解到卡爾曼並不是個很好的選擇，也還是會存在一定的噪聲，除非用擴展卡爾曼。
附上壇子里面一個講的比較好的卡爾曼濾波：
很形象的！卡爾曼濾波的原理說明，附源代碼
http://bbs.elecfans.com/jishu_484128_1_1.html

教你在單片機上套公式實現卡爾曼濾波器
http://bbs.elecfans.com/jishu_484132_1_1.html

四元數互補濾波：我也不懂，來自於德國的開源四軸，目前匿名等四軸也是用這個解算方法。壇子里面找到的：MikroKopter:來自德國的開源四軸飛行器項目http://bbs.elecfans.com/jishu_4757431_1.html