13、四軸飛行器匿名pid算法詳解

對串級PID和單級PID的理解(基于匿名微型六軸)/?caicaizl/********************(C)COPYRIGHT2014ANOTech***************************

*文件名：ANO_FlyControl.cpp

*描述：飛行控制

**********************************************************************************/

include"ANO_FlyControl.h"ANO_FlyControlfc;

/*

先整定內環(huán)，后整定外環(huán)。

參數(shù)整定找最佳，從小到大順序查

先是比例后積分，最后再把微分加

曲線振蕩很頻繁，比例度盤要放大

曲線漂浮繞大灣，比例度盤往小扳

曲線偏離回復慢，積分時間往下降

曲線波動周期長，積分時間再加長

曲線振蕩頻率快，先把微分降下來

動差大來波動慢。微分時間應加長

理想曲線兩個波，前高后低4比1

*/

/*

ROLL和PIT軸向按照以上公式計算PID輸出，但YAW軸比較特殊，因為偏航角法線方向剛好和地球重力平行，

這個方向的角度無法由加速度計直接測得，需要增加一個電子羅盤來替代加速度計。如果不使用羅盤的話，

我們可以單純的通過角速度積分來測得偏航角，缺點是由于積分環(huán)節(jié)中存在積分漂移，偏航角隨著時間的推移

會偏差越來越大。我們不使用羅盤就沒有比例項，只僅使用微分環(huán)節(jié)來控制。

*/

ANO_FlyControl::ANO_FlyControl()

{

yawRate=120;

//重置PID參數(shù)

PID_Reset();

}

//重置PID參數(shù)

voidANO_FlyControl:ID_Reset(void)

{

//因為YAW角度會漂移，所以參數(shù)和ROLL、PITCH不一樣

pid[PIDROLL].set_pid(70,15,120,2000000);//ROLL角度的內環(huán)控制系數(shù),20000:積分上限

pid[PIDPITCH].set_pid(70,30,120,2000000);//PITCH角度的內環(huán)控制系數(shù)

pid[PIDYAW].set_pid(100,50,0,2000000);//YAW角度的內環(huán)控制系數(shù)

pid[PIDLEVEL].set_pid(280,0,0,0);//外環(huán)控制系數(shù)

pid[PIDMAG].set_pid(15,0,0,0);//電子羅盤控制系數(shù)

}

/*

【掃盲知識】

串級PID：采用的角度P和角速度PID的雙閉環(huán)PID算法------>角度的誤差被作為期望輸入到角速度控制器中（角度的微分就是角速度）

對于本系統(tǒng)則采用了將角度控制與角速度控制級聯(lián)的方式組成整個串級PID控制器。

串級PID算法中，角速度內環(huán)占著極為重要的地位。在對四旋翼飛行的物理模型進

行分析后，可以知道造成系統(tǒng)不穩(wěn)定的物理表現(xiàn)之一就是不穩(wěn)定的角速度。

因此，若能夠直接對系統(tǒng)的角速度進行較好的閉環(huán)控制，必然會改善系統(tǒng)的動態(tài)特性

及其穩(wěn)定性，通常也把角速度內環(huán)稱為增穩(wěn)環(huán)節(jié)。而角度外環(huán)的作用則體現(xiàn)在對四旋翼飛

行器的姿態(tài)角的精確控制。

外環(huán)：輸入為角度,輸出為角速度

內環(huán)：輸入為角速度，輸出為PWM增量

使用串級pid，分為：角度環(huán)控制pid環(huán)，和角速度控制環(huán)穩(wěn)定環(huán)。主調為角度環(huán)（外環(huán)），副調為角速度環(huán)（內環(huán)）。

參數(shù)整定原則為先內后外，故在整定內環(huán)時將外環(huán)的PID均設為0

所謂外環(huán)就是只是一個P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飛機震蕩。

震蕩的特點是：頻率小、幅度大

*/

/*

【橫滾（Roll）和俯仰（Pitch）的控制算法】

橫滾（Roll）和俯仰（Pitch）的控制算法是一樣的，控制參數(shù)也比較接近。

首先得到軸姿態(tài)的角度差（angleerror），將這個值乘以角度系數(shù)p后限幅（限幅必須有，否則劇烈打舵時容易引發(fā)震蕩）作為角速度控制器期望值（target_rate）。target_rate與陀螺儀得到的當前角速度作差，得到角速度誤差（rate_error）乘以kp得到P。在I值小于限幅值（這個值大概在5%油門）或者rate_error與i值異號時將rate_error累加到I中。前后兩次rate_error的差作為D項，值得注意的是加需要入20hz（也可以采用其它合適頻率）濾波，以避免震蕩。將P,I,D三者相加并限幅（50%油門）得到最終PID輸出。

*/

//串環(huán)PID調節(jié)詳情參見：/super_mic...36723

//飛行器姿態(tài)外環(huán)控制

voidANO_FlyControl::Attitude_Outter_Loop(void)

{

int32_terrorAngle[2];

Vector3fGyro_ADC;

//計算角度誤差值,角度誤差值=期望值-此刻姿態(tài)值

//constrain_int32作用：32位整型數(shù)限幅，使其控制輸入的最大飛行傾角不大于25度（如果控制量比25度大，飛機早就墜毀了）

//rc.Command[ROLL]：遙控數(shù)據(jù)imu.angle.x：此刻姿態(tài)(角度)

//1.得到軸姿態(tài)的角度差（errorAngle）

//2.這個角度差值進行限幅(constrain_int32)（正負FLYANGLE_MAX）（限幅必須有，否則劇烈打舵時容易引發(fā)震蕩）作為角速度控制器期望值（target_rate）

errorAngle[ROLL]=constrain_int32((rc.Command[ROLL]*2),-((int)FLYANGLE_MAX),+FLYANGLE_MAX)-imu.angle.x*10;

errorAngle[PITCH]=constrain_int32((rc.Command[PITCH]*2),-((int)FLYANGLE_MAX),+FLYANGLE_MAX)-imu.angle.y*10;

//獲取此時陀螺儀上的角速度，取角速度的四次平均值

Gyro_ADC=mpu6050.Get_Gyro()/4;

/*

得到外環(huán)PID輸出（角速度的差值）(實質是相當于內環(huán)的P比例項)-------->

3.target_rate與陀螺儀得到的當前角速度作差，得到角速度誤差（RateError）乘以kp（外環(huán)控制系數(shù)pid[PIDLEVEL]--->(280,0,0,0)）得到給內環(huán)的P。

*/

//橫滾roll：外環(huán)控制。輸入為角度,輸出為角速度。RateError[ROLL]作為內環(huán)的輸入。

RateError[ROLL]=pid[PIDLEVEL].get_p(errorAngle[ROLL])-Gyro_ADC.x;//Gyro_ADC.x:陀螺儀X軸的值

//俯仰pitch：外環(huán)控制。輸入為角度,輸出為角速度。RateError[PITCH]作為內環(huán)的輸入。

RateError[PITCH]=pid[PIDLEVEL].get_p(errorAngle[PITCH])-Gyro_ADC.y;//Gyro_ADC.y:陀螺儀Y軸的值

/*

偏航（Yaw）的控制算法和前兩者略有不同，是將打舵量（遙控數(shù)據(jù)量rc.Command[YAW]）和角度誤差的和作為角速度內環(huán)的期望值，

這樣可以獲得更好的動態(tài)響應。角速度內環(huán)和橫滾與俯仰的控制方法一致，參數(shù)（積分限幅值會很小，默認只有萬分之8）上有不同。*/

//航向yaw：外環(huán)控制。輸入為角度,輸出為角速度。RateError[YAW]作為內環(huán)的輸入。

RateError[YAW]=((int32_t)(yawRate)*rc.Command[YAW])/32-Gyro_ADC.z;//Gyro_ADC.z:陀螺儀Z軸的值

}

//飛行器姿態(tài)內環(huán)控制:輸入為角速度，輸出為PWM增量

//內環(huán)的效果就是：減小P比例控制帶來的震蕩

voidANO_FlyControl::Attitude_Inner_Loop(void)

{

int32_tPIDTerm[3];

//注意這里是i的值是0到2

//PIDROLL、PIDPITCH、PIDYAW是枚舉類型，也就是0、1、2，也就是下面的pid

、PIDTerm就是3組PID

for(u8i=0;i<3;i++)

{

//現(xiàn)象：當油門低于檢查值時積分清零，重新積分

//猜測：這里應該是擔心飛機沒飛起來時就開始有積分，會導致起飛時不穩(wěn)定

if((rc.rawData[THROTTLE])<RC_MINCHECK)

pid.reset_I();

//get_pid函數(shù)：returnget_p(error)+get_i(error,dt)+get_d(error,dt);-------->這里實際就是一個完整的PID

//PID_INNER_LOOP_TIME：2000us--->0.2ms積分微分時間，每隔0.2ms操作積分和微分,RateError是外環(huán)計算的結果（從外環(huán)算出）

//得到內環(huán)PID輸出，直接輸出轉為電機控制量

PIDTerm

=pid.get_pid(RateError,PID_INNER_LOOP_TIME);

}

//對YAW角繼續(xù)處理，加入遙控控制

//在I值小于限幅值（這個值大概在5%油門）或者rate_error與i值異號時將rate_error累加到I中。

PIDTerm[YAW]=-constrain_int32(PIDTerm[YAW],-300-abs(rc.Command[YAW]),+300+abs(rc.Command[YAW]));

//PID輸出轉為電機控制量

motor.writeMotor(rc.Command[THROTTLE],PIDTerm[ROLL],PIDTerm[PITCH],PIDTerm[YAW]);

}

/*

【調節(jié)串環(huán)PID大概過程（注意修正反向）】

1、估計大概的起飛油門。

2、調整角速度內環(huán)參數(shù)。

3、將角度外環(huán)加上，調整外環(huán)參數(shù)。

4、橫滾俯仰參數(shù)一般可取一致，將飛機解綁，抓在手中測試兩個軸混合控制的效果（注意安全），有問題回到“烤四軸”繼續(xù)調整，直至飛機在手中不會抽搐。

5、大概設置偏航參數(shù)（不追求動態(tài)響應，起飛后頭不偏即可），起飛后再觀察橫滾和俯仰軸向打舵的反應，如有問題回到“烤四軸”。

6、橫滾和俯仰ok以后，再調整偏航軸參數(shù)以達到好的動態(tài)效果。

*/

/*

【過程詳解】

1、要在飛機的起飛油門基礎上進行PID參數(shù)的調整，否則“烤四軸”的時候調試穩(wěn)定了，飛起來很可能又會晃蕩。

2、內環(huán)的參數(shù)最為關鍵！理想的內環(huán)參數(shù)能夠很好地跟隨打舵（角速度控制模式下的打舵）控制量。

在平衡位置附近（正負30度左右），舵量突加，飛機快速響應；舵量回中，飛機立刻停止運動（幾乎沒有回彈和震蕩）。

2.1首先改變程序，將角度外環(huán)去掉，將打舵量作為內環(huán)的期望（角速度模式，在APM中叫ACRO模式，在大疆中叫手動模式）。

2.2加上P，P太小，不能修正角速度誤差表現(xiàn)為很“軟”傾斜后難以修正，打舵響應也差。P太大，在平衡位置容易震蕩，

打舵回中或給干擾（用手突加干擾）時會震蕩。合適的P能較好的對打舵進行響應，又不太會震蕩，但是舵量回中后會回彈好幾下才能停止（沒有D）。

2.3加上D，D的效果十分明顯，加快打舵響應，最大的作用是能很好地抑制舵量回中后的震蕩，可謂立竿見影。

太大的D會在橫滾俯仰混控時表現(xiàn)出來（盡管在“烤四軸”時的表現(xiàn)可能很好），具體表現(xiàn)是四軸抓在手里推油門會抽搐。

如果這樣，只能回到“烤四軸”降低D，同時P也只能跟著降低。D調整完后可以再次加大P值，以能夠跟隨打舵為判斷標準。

2.4加上I，會發(fā)現(xiàn)手感變得柔和了些。由于筆者“烤四軸”的裝置中四軸的重心高于旋轉軸，這決定了在四軸偏離水平位置后

會有重力分量使得四軸會繼續(xù)偏離平衡位置。I的作用就可以使得在一定角度范圍內（30度左右）可以修正重力帶來的影響。

表現(xiàn)打舵使得飛機偏離平衡位置，舵量回中后飛機立刻停止轉動，若沒有I或太小，飛機會由于重力繼續(xù)轉動。

3、角度外環(huán)只有一個參數(shù)P。將外環(huán)加上（在APM中叫Stabilize模式，在大疆中叫姿態(tài)模式）。打舵會對應到期望的角度。

P的參數(shù)比較簡單。太小，打舵不靈敏，太大，打舵回中易震蕩。以合適的打舵反應速度為準。

4、至此，烤四軸”效果應該會很好了，但是兩個軸混控的效果如何還不一定，有可能會抽（兩個軸的控制量疊加起來，

特別是較大的D，會引起抽搐）。如果抽了，降低PD的值，I基本不用變。

5、加上偏航的修正參數(shù)后（直接給雙環(huán)參數(shù)，角度外環(huán)P和橫滾差不多，內環(huán)P比橫滾大些，I和橫滾差不多，D可以先不加），

拿在手上試過修正和打舵方向正確后可以試飛了（試飛很危險！?。?！選擇在寬敞、無風的室內，1米的高度（高度太低會有地面效應干擾，

太高不容易看清姿態(tài)且容易摔壞），避開人群的地方比較適合，如有意外情況，立刻關閉油門?。?！

5.1試飛時主要觀察這么幾個方面的情況，一般經過調整的參數(shù)在平衡位置不會大幅度震蕩，需要觀察：

5.1.1在平衡位置有沒有小幅度震蕩（可能是由于機架震動太大導致姿態(tài)解算錯誤造成