PID應用詳解
閱讀目錄
1.PID介紹及原理
2.常用四軸的兩種PID算法講解(單環PID、串級PID)
3.常用PID算法的C語言實現
5.常用的四軸飛行器PID算法
PID介紹及原理
PID介紹
在工業應用中PID及其衍生算法是應用最廣泛的算法之一，是當之無愧的萬能算法，也是最經典的算法。
現在已經演變出很多智能的算法，如蟻群，神經網絡等，感興趣可以看一下劉金琨老師的《先進pid控制》
PID算法的一般形式：
通過誤差信號控制被控量，而控制器本身就是比例、積分、微分三個環節的加和。這里我們規定（在t時刻）：
1.輸入量為rin(t);
2.輸出量為rout(t);
3.偏差量為err(t)=rin(t)-rout(t);
pid的控制規律為：偏差控制被控制量
1.說明一下反饋控制的原理，通過上面的框圖不難看出，PID控制其實是對偏差的控制過程;
2.如果偏差為0,則比例環節不起作用，只有存在偏差時，比例環節才起作用。
3.積分環節主要是用來消除靜差，所謂靜差，就是系統穩定后輸出值和設定值之間的差值，積分環節實際上就是偏差累計的過程，把累計的誤差加到原有系統上以抵消系統造成的靜差。
4.而微分信號則反應了偏差信號的變化規律，或者說是變化趨勢，根據偏差信號的變化趨勢來進行超前調節，從而增加了系統的快速性。
PID原理普及
1、 對自動控制系統的基本要求：穩、准、快：
- 穩定性（P和I降低系統穩定性，D提高系統穩定性）：在平衡狀態下，系統受到某個干擾后，經過一段時間其被控量可以達到某一穩定狀態；
- 准確性（P和I提高穩態精度，D無作用）：系統處於穩態時，其穩態誤差；
- 快速性（P和D提高響應速度，I降低響應速度）：系統對動態響應的要求。一般由過渡時間的長短來衡量。
2、 穩定性：當系統處於平衡狀態時，受到某一干擾作用后，如果系統輸出能夠恢復到原來的穩態值，那么系統就是穩定的；否則，系統不穩定。
3、 動態特性（暫態特性，由於系統慣性引起）：系統突加給定量（或者負載突然變化）時，其系統輸出的動態響應曲線。延遲時間、上升時間、峰值時間、調節時間、超調量和振盪次數。
通常：
- 上升時間和峰值時間用來評價系統的響應速度；
- 超調量用來評價系統的阻尼程度；
- 調節時間同時反應響應速度和阻尼程度；
4、 穩態特性：在參考信號輸出下，經過無窮時間，其系統輸出與參考信號的誤差。影響因素：系統結構、參數和輸入量的形式等
5、 比例（P）控制規律：具有P控制的系統，其穩態誤差可通過P控制器的增益Kp來調整：Kp越大，穩態誤差越小；反之，穩態誤差越大。但是Kp越大，其系統的穩定性會降低。
由上式可知，控制器的輸出m(t)與輸入誤差信號e(t)成比例關系，偏差減小的速度取決於比例系數Kp：Kp越大，偏差減小的越快，但是很容易引起振盪（尤其是在前向通道中存在較大的時滯環節時）；Kp減小，發生振盪的可能性小，但是調節速度變慢。單純的P控制無法消除穩態誤差，所以必須要引入積分I控制。原因：（R為參考輸入信號，Kv為開環增益）
當參考輸入信號R不為0時，其穩態誤差只能趨近於0，不能等於0。因為開環增益Kv不為0。
6、 比例微分（PD）控制規律：可以反應輸入信號的變化趨勢，具有某種預見性，可為系統引進一個有效的早期修正信號，以增加系統的阻尼程度，而從提高系統的穩定性。（tao為微分時間常數）
如果系統中存在較大時滯的環節，則輸出變化總是落后於當前誤差的變化，解決的方法就是使抑制誤差的作用變化“超前”，增強系統的穩定性。
7、 積分（I）控制規律：由於采用了積分環節，若當前誤差e(t)為0，則其輸出信號m(t)有可能是一個不為0的常量。需要注意的是，引入積分環節，可以提到系統型別，使得系統可以跟蹤更高階次的輸入信號，以消除穩態誤差。
8、 比例積分（PI）控制規律：在保證系統穩定的前提下，引入PI控制器可以提高它的穩態控制質量，消除其穩態誤差。（ti為積分時間常數）
積分調節可以消除靜差，但有滯后現象，比例調節沒有滯后現象，但存在靜差。 PI調節就是綜合P、I兩種調節的優點，利用P調節快速抵消干擾的影響，同時利用I調節消除殘差。
9、 比例積分微分（PID）控制規律：除了積分環節提高了系統型別，微分環節提高了系統的動態性能。
觀察PID的公式可以發現：Kp乘以誤差e(t)，用以消除當前誤差；積分項系數Ki乘以誤差e(t)的積分，用於消除歷史誤差積累，可以達到無差調節；微分項系數Kd乘以誤差e(t)的微分，用於消除誤差變化，也就是保證誤差恆定不變。由此可見，P控制是一個調節系統中的核心，用於消除系統的當前誤差，然后，I控制為了消除P控制余留的靜態誤差而輔助存在，對於D控制，所占的權重最少，只是為了增強系統穩定性，增加系統阻尼程度，修改PI曲線使得超調更少而輔助存在。10、P控制對系統性能的影響：
- 開環增益越大，穩態誤差減小（無法消除，屬於有差調節）
- 過渡時間縮短
- 穩定程度變差
11、I控制對系統性能的影響：
- 消除系統穩態誤差（能夠消除靜態誤差，屬於無差調節）
- 穩定程度變
12、D控制對系統性能的影響：
- 減小超調量
- 減小調節時間（與P控制相比較而言）
- 增強系統穩定性
- 增加系統阻尼程
13、PD控制對系統性能的影響：
- 減小調節時間
- 減小超調量
- 增大系統阻尼，增強系統穩定性
- 增加高頻干
14、PI控制對系統性能的影響：
- 提高系統型別，減少系統穩態誤差
- 增強系統抗高頻干擾能力
- 調節時間增大
15、P調節、I調節降低系統穩定性 D調節增強系統穩定性 所以PI調節器的P比P調節器的P要小一些，PD調節器的P比P調節器的P要大一些16、位置式PID表達式（數字PID）：
P(n)為第n次輸出，e(n)為第n次偏差值，Ts為系統采用周期，Ti為積分時間常數，Td為微分時間常數
16、消除隨機干擾的措施：
- 幾個采樣時刻的采樣值求平均后代替本次的采樣值
- 微分項的四點中心差分(e(n)-e(n-3)+3e(n-1)-3e(n-2))*1/(6Ts)
- 矩形積分改為梯形積分
常用四軸的兩種PID算法講解(單環PID、串級PID)
單環PID
這里主要講解的PID算法屬於一種線性控制器，這種控制器被廣泛應用於四軸上。要控制四軸，顯而易見的是控制它的角度，那么最簡單，同時也是最容易想到的一種控制策略就是角度單環PID控制器，系統框圖如圖所示：
偽代碼：
串級PID
上述角度單環PID控制算法僅僅考慮了飛行器的角度信息，如果想增加飛行器的穩定性(增加阻尼)並提高它的控制品質，我們可以進一步的控制它的角速度，於是角度/角速度-串級PID控制算法應運而生。在這里，相信大多數朋友已經初步了解了角度單環PID的原理，但是依舊無法理解串級PID究竟有什么不同。其實很簡單：它就是兩個PID控制算法，只不過把他們串起來了(更精確的說是套起來)。那這么做有什么用？答案是，它增強了系統的抗干擾性(也就是增強穩定性)，因為有兩個控制器控制飛行器，它會比單個控制器控制更多的變量，使得飛行器的適應能力更強。為了更為清晰的講解串級PID，這里筆者依舊畫出串級PID的原理框圖，如圖所示：
偽代碼：
關於如何整定單環PID與串級PID的問題，請原諒筆者的能力有限，無法給出標准而可靠的整定流程，這里我給出三個鏈接，第一個為阿莫論壇的一位同學的單環PID整定現象與思考，個人覺得參考價值很大；第二、三兩個分別為APM與PX4的串級PID整定現象說明，大家可以參考他們的網頁說明
而筆者在整定串級PID時的經驗則是：先整定內環PID，再整定外環P。
內環P：從小到大，拉動四軸越來越困難，越來越感覺到四軸在抵抗你的拉動；到比較大的數值時，四軸自己會高頻震動，肉眼可見，此時拉扯它，它會快速的振盪幾下，過幾秒鍾后穩定；繼續增大，不用加人為干擾，自己發散翻機。特別注意：只有內環P的時候，四軸會緩慢的往一個方向下掉，這屬於正常現象。這就是系統角速度靜差。
內環I：前述PID原理可以看出，積分只是用來消除靜差，因此積分項系數個人覺得沒必要弄的很大，因為這樣做會降低系統穩定性。從小到大，四軸會定在一個位置不動，不再往下掉；繼續增加I的值，四軸會不穩定，拉扯一下會自己發散。特別注意：增加I的值，四軸的定角度能力很強，拉動他比較困難，似乎像是在釘釘子一樣，但是一旦有強干擾，它就會發散。這是由於積分項太大，拉動一下積分速度快，給 的補償非常大，因此很難拉動，給人一種很穩定的錯覺。
內環D：這里的微分項D為標准的PID原理下的微分項，即本次誤差-上次誤差。在角速度環中的微分就是角加速度，原本四軸的震動就比較強烈，引起陀螺的值變化較大，此時做微分就更容易引入噪聲。因此一般在這里可以適當做一些滑動濾波或者IIR濾波。從小到大，飛機的性能沒有多大改變，只是回中的時候更加平穩；繼續增加D的值，可以肉眼看到四軸在平衡位置高頻震動(或者聽到電機發出滋滋的聲音)。前述已經說明D項屬於輔助性項，因此如果機架的震動較大，D項可以忽略不加。
外環P：當內環PID全部整定完成后，飛機已經可以穩定在某一位置而不動了。此時內環P，從小到大，可以明顯看到飛機從傾斜位置慢慢回中，用手拉扯它然后放手，它會慢速回中，達到平衡位置；繼續增大P的值，用遙控器給不同的角度給定，可以看到飛機跟蹤的速度和響應越來越快；繼續增加P的值，飛機變得十分敏感，機動性能越來越強，有發散的趨勢。
常用PID算法的C語言實現
位置型PID的C語言實現：
第一步：定義PID變量結構體，代碼如下：
struct _pid{
float SetSpeed; //定義設定值
float ActualSpeed; //定義實際值
float err; //定義偏差值
float err_last; //定義上一個偏差值
float Kp,Ki,Kd; //定義比例、積分、微分系數
float voltage; //定義電壓值（控制執行器的變量）
float integral; //定義積分值
}pid;
控制算法中所需要用到的參數在一個結構體中統一定義，方便后面的使用。
第二部：初始化變量，代碼如下：
void PID_init(){
printf("PID_init begin \n");
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.voltage=0.0;
pid.integral=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
printf("PID_init end \n");
}
統一初始化變量，尤其是Kp,Ki,Kd三個參數，調試過程當中，對於要求的控制效果，可以通過調節這三個量直接進行調節。
第三步：編寫控制算法，代碼如下：
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
pid.integral+=pid.err;//位置式pid是對積分的持續累加，容易造成積分飽和，是系統過調
pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
注意：這里用了最基本的算法實現形式，沒有考慮死區問題，沒有設定上下限，只是對公式的一種直接的實現，后面的介紹當中還會逐漸的對此改進。
到此為止，PID的基本實現部分就初步完成了。下面是測試代碼：
int main(){
printf("System begin \n");
PID_init();
int count=0;
while(count<1000)
{
float speed=PID_realize(200.0);
printf("%f\n",speed);
count++;
}
return 0;
}
增量型PID的C語言實現：
實現過程仍然是分為定義變量、初始化變量、實現控制算法函數、算法測試四個部分，
#include<stdio.h>
#include<stdlib.h>
struct _pid{
float SetSpeed; //定義設定值
float ActualSpeed; //定義實際值
float err; //定義偏差值
float err_next; //定義上一個偏差值
float err_last; //定義最上前的偏差值
float Kp,Ki,Kd; //定義比例、積分、微分系數
}pid;
void PID_init(){
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.err_next=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
}
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
float incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);//只和前后三次的誤差值有關，也方便計算
pid.ActualSpeed+=incrementSpeed;
pid.err_last=pid.err_next;
pid.err_next=pid.err;
return pid.ActualSpeed;
}
int main(){
PID_init();
int count=0;
while(count<1000)
{
float speed=PID_realize(200.0);
printf("%f\n",speed);
count++;
}
return 0;
}
積分分離的PID控制算法C語言實現：
在普通PID控制中，引入積分環節的目的，主要是為了消除靜差，提高控制精度。但是在啟動、結束或大幅度增減設定時，短時間內系統輸出有很大的偏差，會造成PID運算的積分積累，導致控制量超過執行機構可能允許的最大動作范圍對應極限控制量，從而引起較大的超調，甚至是震盪，這是絕對不允許的。
為了克服這一問題，引入了積分分離的概念，其基本思路是當被控量與設定值偏差較大時，取消積分作用; 當被控量接近給定值時，引入積分控制，以消除靜差，提高精度。其具體實現代碼如下：
pid.Kp=0.2;
pid.Ki=0.04;
pid.Kd=0.2; //初始化過程
if(abs(pid.err)>200)
{
index=0;
}else{
index=1;
pid.integral+=pid.err;
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
//算法具體實現過程可參考上面的
抗積分飽和的PID控制算法C語言實現：
所謂的積分飽和現象是指如果系統存在一個方向的偏差，PID控制器的輸出由於積分作用的不斷累加而加大，從而導致執行機構達到極限位置，若控制器輸出U(k)繼續增大，執行器開度不可能再增大，此時計算機輸出控制量超出了正常運行范圍而進入飽和區。一旦系統出現反向偏差，u(k)逐漸從飽和區退出。進入飽和區越深則退出飽和區時間越長。在這段時間里，執行機構仍然停留在極限位置而不隨偏差反向而立即做出相應的改變，這時系統就像失控一樣，造成控制性能惡化，這種現象稱為積分飽和現象或積分失控現象。
防止積分飽和的方法之一就是抗積分飽和法，該方法的思路是在計算u(k)時，首先判斷上一時刻的控制量u(k-1)是否已經超出了極限范圍： 如果u(k-1)>umax，則只累加負偏差; 如果u(k-1)<umin，則只累加正偏差。從而避免控制量長時間停留在飽和區。直接貼出代碼，不懂的看看前面幾節的介紹。
float PID_realize(float speed){
int index;
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
if(pid.ActualSpeed>pid.umax) //灰色底色表示抗積分飽和的實現
{
if(abs(pid.err)>200) //藍色標注為積分分離過程
{
index=0;
}else{
index=1;
if(pid.err<0)
{//如果超上限要嘛加負值要嘛就不加了，免得進入飽和區
pid.integral+=pid.err;
}
}
}else if(pid.ActualSpeed<pid.umin){
if(abs(pid.err)>200) //積分分離過程
{
index=0;
}else{
index=1;
if(pid.err>0)
{//如果超下限要嘛加正值要嘛就不加了免得進入飽和區
pid.integral+=pid.err;
}
}
}else{
if(abs(pid.err)>200) //積分分離過程
{
index=0;
}else{
index=1;
pid.integral+=pid.err;
}
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
變積分的PID控制算法C語言實現：
變積分PID可以看成是積分分離的PID算法的更一般的形式。在普通的PID控制算法中，由於積分系數ki是常數，所以在整個控制過程中，積分增量是不變的。但是，系統對於積分項的要求是，系統偏差大時，積分作用應該減弱甚至是全無，而在偏差小時，則應該加強。積分系數取大了會產生超調，甚至積分飽和，取小了又不能短時間內消除靜差。因此，根據系統的偏差大小改變積分速度是有必要的。
變積分PID的基本思想是設法改變積分項的累加速度，使其與偏差大小相對應：偏差越大，積分越慢; 偏差越小，積分越快。
這里給積分系數前加上一個比例值index：
當abs(err)<180時，index=1;
當180<abs(err)<200時，index=（200-abs(err)）/20;
當abs(err)>200時，index=0;
最終的比例環節的比例系數值為ki*index;
float PID_realize(float speed){
float index;
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
if(abs(pid.err)>200) //變積分過程
{
index=0.0;
}else if(abs(pid.err)<180){
index=1.0;
pid.integral+=pid.err;
}else{
index=(200-abs(pid.err))/20;
pid.integral+=pid.err;
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
常用的四軸飛行器PID算法
四軸飛行器常用PID組合：
1.三角函數直接解算歐拉角+互補濾波+單級PID版本
效果：簡單暴力，但是補濾波效果差，單級PID響應慢，打舵跟隨效應差。
2.三角函數直接解算歐拉角+卡爾曼濾波+單級PID版本
效果：卡爾曼濾波噪聲偏大，滯后略微嚴重，單級PID難操作，打舵響應慢，跟隨效應差。不過比較適合初學四軸的人，難度比四元數加串級PID版本低，易於理解。
3.四元數姿態解算+互補濾波（德國開源四軸）+串級PID版本
效果：四元數難理解，基於PI控制的互補濾波不適合非專業人員，PID參數較單級PID參數難調。打舵響應極佳，穩定程度高，易於操作，是目前四軸的主流算法
常用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*[e(n)-e(n-1)]
D后面的當前誤差減前次誤差也可以直接使用陀螺儀的數據代替，原理一樣。
單級PID整定方法參見我寫的第一篇帖子！
串級PID：
單極PID適合線性系統，當輸出量和被控制量呈線性關系時單極PID能獲得較好的效果，但是四軸不是線性系統，現代學者認為，四軸通常可以簡化為一個二階阻尼系統。為什么四軸不是線性系統呢？首先，輸出的電壓和電機轉速不是呈正比的，其次，螺旋槳轉速和升力是平方倍關系，故單極PID在四軸上很難取得很好效果，能飛，但是不好飛。
為了解決這個問題，我們提出了串級PID這個解決方法。
串級PID就是兩個PID串在一起，分為內環和外環PID。在此，我們使用內環PID控制，外環PI控制。
單極PID輸入的是期望角度，反饋的是角度數據，串級PID中外環輸入反饋的也是角度數據，內環輸入反饋的便是角速度數據。通俗來講，內環就是你希望將四軸以多少度每秒的速度運動，然后他給你糾正過來，外環就是根據角度偏差告訴內環你該以多少度一秒運動。這樣，即使外環數據劇烈變化，四軸的效果也不會顯得很僵硬。
在內環中，PID三個數據作用分別是：P（將四軸從偏差角速度糾正回期望角速度）D（抑制系統運動）I（消除角速度控制靜差）
外環PI中，兩個數據的作用是：P（將四軸從偏差角度糾正回期望角度）I（消除角度控制靜差）
refer：
https://blog.csdn.net/gen_ye/article/details/52534609
http://bbs.elecfans.com/jishu_1615869_1_1.html
https://blog.csdn.net/mish84/article/details/51062165
更多閱讀：
位置式PID與增量式PID的區別：http://www.elecfans.com/dianzichangshi/20171124585738.html
卡爾曼濾波的原理說明，附源代碼 ：http://bbs.elecfans.com/jishu_484128_1_1.html
【開源】分享一個經典的串級PID算法，附源代碼 ：http://bbs.elecfans.com/jishu_486485_1_1.html
教你在單片機上套公式實現卡爾曼濾波器 ；http://bbs.elecfans.com/jishu_484132_1_1.html
四元數互補濾波(目前匿名等四軸也是用這個解算方)：http://bbs.elecfans.com/jishu_4757431_1.html