標題: 對串級PID和單級PID的理解(基于匿名微型六軸) [打印本頁]
作者: tanyize    時間: 2019-2-16 22:32
標題: 對串級PID和單級PID的理解(基于匿名微型六軸)
分享一個不錯的PID調(diào)節(jié)文檔
/******************** (C) COPYRIGHT 2014 ANO Tech ***************************
* 文件名 ：ANO_FlyControl.cpp
* 描述 ：飛行控制
**********************************************************************************/
include "ANO_FlyControl.h"ANO_FlyControl fc;

/*
先整定內(nèi)環(huán)，后整定外環(huán)。
參數(shù)整定找最佳，從小到大順序查

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

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

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

曲線偏離回復(fù)慢，積分時間往下降

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

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

動差大來波動慢。微分時間應(yīng)加長

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

/*
ROLL和PIT軸向按照以上公式計算PID輸出，但YAW軸比較特殊，因為偏航角法線方向剛好和地球重力平行，
這個方向的角度無法由加速度計直接測得，需要增加一個電子羅盤來替代加速度計。如果不使用羅盤的話，
我們可以單純的通過角速度積分來測得偏航角，缺點是由于積分環(huán)節(jié)中存在積分漂移，偏航角隨著時間的推移
會偏差越來越大。我們不使用羅盤就沒有比例項，只僅使用微分環(huán)節(jié)來控制。
*/

ANO_FlyControl::ANO_FlyControl()
{
yawRate = 120;
//重置PID參數(shù)
PID_Reset();
}

//重置PID參數(shù)
void ANO_FlyControl: ID_Reset(void)
{
//因為YAW角度會漂移，所以參數(shù)和ROLL、PITCH不一樣
pid[PIDROLL].set_pid(70, 15, 120, 2000000); //ROLL角度的內(nèi)環(huán)控制系數(shù),20000:積分上限
pid[PIDPITCH].set_pid(70, 30, 120, 2000000);//PITCH角度的內(nèi)環(huán)控制系數(shù)

pid[PIDYAW].set_pid(100, 50, 0, 2000000); //YAW角度的內(nèi)環(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 算法中，角速度內(nèi)環(huán)占著極為重要的地位。在對四旋翼飛行的物理模型進
行分析后，可以知道造成系統(tǒng)不穩(wěn)定的物理表現(xiàn)之一就是不穩(wěn)定的角速度。
因此，若能夠直接對系統(tǒng)的角速度進行較好的閉環(huán)控制，必然會改善系統(tǒng)的動態(tài)特性
及其穩(wěn)定性，通常也把角速度內(nèi)環(huán)稱為增穩(wěn)環(huán)節(jié)。而角度外環(huán)的作用則體現(xiàn)在對四旋翼飛
行器的姿態(tài)角的精確控制。
外環(huán)：輸入為角度,輸出為角速度
內(nèi)環(huán)：輸入為角速度，輸出為PWM增量
使用串級pid，分為：角度環(huán)控制pid環(huán)，和角速度控制環(huán)穩(wěn)定環(huán)。主調(diào)為角度環(huán)（外環(huán)），副調(diào)為角速度環(huán)（內(nèi)環(huán)）。
參數(shù)整定原則為先內(nèi)后外，故在整定內(nèi)環(huán)時將外環(huán)的PID均設(shè)為0
所謂外環(huán)就是只是一個P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飛機震蕩。
震蕩的特點是：頻率小、幅度大
*/

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

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


//飛行器姿態(tài)外環(huán)控制
void ANO_FlyControl::Attitude_Outter_Loop(void)
{
int32_t errorAngle[2];
Vector3f Gyro_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輸出（角速度的差值）(實質(zhì)是相當(dāng)于內(nèi)環(huán)的P比例項)-------->
3.target_rate與陀螺儀得到的當(dāng)前角速度作差，得到角速度誤差（RateError）乘以kp（外環(huán)控制系數(shù) pid[PIDLEVEL]--->(280, 0, 0, 0)）得到給內(nèi)環(huán)的P。
*/

//橫滾roll：外環(huán)控制。輸入為角度,輸出為角速度。RateError[ROLL] 作為內(nèi)環(huán)的輸入。
RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x; //Gyro_ADC.x:陀螺儀X軸的值

//俯仰pitch：外環(huán)控制。輸入為角度,輸出為角速度。RateError[PITCH] 作為內(nèi)環(huán)的輸入。
RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) - Gyro_ADC.y;//Gyro_ADC.y:陀螺儀Y軸的值

/*
偏航（Yaw）的控制算法和前兩者略有不同，是將打舵量（遙控數(shù)據(jù)量rc.Command[YAW]）和角度誤差的和作為角速度內(nèi)環(huán)的期望值，
這樣可以獲得更好的動態(tài)響應(yīng)。角速度內(nèi)環(huán)和橫滾與俯仰的控制方法一致，參數(shù)（積分限幅值會很小，默認只有萬分之8）上有不同。*/

//航向yaw：外環(huán)控制。輸入為角度,輸出為角速度。 RateError[YAW] 作為內(nèi)環(huán)的輸入。
RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z; //Gyro_ADC.z:陀螺儀Z軸的值

}

//飛行器姿態(tài)內(nèi)環(huán)控制: 輸入為角速度，輸出為PWM增量
//內(nèi)環(huán)的效果就是：減小 P比例控制帶來的震蕩
void ANO_FlyControl::Attitude_Inner_Loop(void)
{
int32_t PIDTerm[3];

//注意這里是i的值是0到2
//PIDROLL、PIDPITCH、PIDYAW是枚舉類型，也就是0、1、2，也就是下面的pid 、PIDTerm就是3組PID
for(u8 i=0; i<3;i++)
{
//現(xiàn)象：當(dāng)油門低于檢查值時積分清零，重新積分

//猜測：這里應(yīng)該是擔(dān)心飛機沒飛起來時就開始有積分，會導(dǎo)致起飛時不穩(wěn)定
if ((rc.rawData[THROTTLE]) < RC_MINCHECK)

pid.reset_I();

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

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

//得到內(nèi)環(huán)PID輸出，直接輸出轉(zhuǎn)為電機控制量
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輸出轉(zhuǎn)為電機控制量
motor.writeMotor(rc.Command[THROTTLE], PIDTerm[ROLL], PIDTerm[PITCH], PIDTerm[YAW]);
}

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

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

2、調(diào)整角速度內(nèi)環(huán)參數(shù)。

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

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

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

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

*/

/*
【過程詳解】

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

2、內(nèi)環(huán)的參數(shù)最為關(guān)鍵！理想的內(nèi)環(huán)參數(shù)能夠很好地跟隨打舵（角速度控制模式下的打舵）控制量。
在平衡位置附近（正負30度左右），舵量突加，飛機快速響應(yīng)；舵量回中，飛機立刻停止運動（幾乎沒有回彈和震蕩）。
2.1首先改變程序，將角度外環(huán)去掉，將打舵量作為內(nèi)環(huán)的期望（角速度模式，在APM中叫ACRO模式，在大疆中叫手動模式）。
2.2加上P，P太小，不能修正角速度誤差表現(xiàn)為很“軟”傾斜后難以修正，打舵響應(yīng)也差。P太大，在平衡位置容易震蕩，
打舵回中或給干擾（用手突加干擾）時會震蕩。合適的P能較好的對打舵進行響應(yīng)，又不太會震蕩，但是舵量回中后會回彈好幾下才能停止（沒有D）。
2.3加上D，D的效果十分明顯，加快打舵響應(yīng)，最大的作用是能很好地抑制舵量回中后的震蕩，可謂立竿見影。
太大的D會在橫滾俯仰混控時表現(xiàn)出來（盡管在“烤四軸”時的表現(xiàn)可能很好），具體表現(xiàn)是四軸抓在手里推油門會抽搐。
如果這樣，只能回到“烤四軸”降低D，同時P也只能跟著降低。D調(diào)整完后可以再次加大P值，以能夠跟隨打舵為判斷標準。
2.4加上I，會發(fā)現(xiàn)手感變得柔和了些。由于筆者“烤四軸”的裝置中四軸的重心高于旋轉(zhuǎn)軸，這決定了在四軸偏離水平位置后
會有重力分量使得四軸會繼續(xù)偏離平衡位置。I的作用就可以使得在一定角度范圍內(nèi)（30度左右）可以修正重力帶來的影響。
表現(xiàn)打舵使得飛機偏離平衡位置，舵量回中后飛機立刻停止轉(zhuǎn)動，若沒有I或太小，飛機會由于重力繼續(xù)轉(zhuǎn)動。

3、角度外環(huán)只有一個參數(shù)P。將外環(huán)加上（在APM中叫Stabilize模式，在大疆中叫姿態(tài)模式）。打舵會對應(yīng)到期望的角度。
P的參數(shù)比較簡單。太小，打舵不靈敏，太大，打舵回中易震蕩。以合適的打舵反應(yīng)速度為準。

4、至此，烤四軸”效果應(yīng)該會很好了，但是兩個軸混控的效果如何還不一定，有可能會抽（兩個軸的控制量疊加起來，
特別是較大的D，會引起抽搐）。如果抽了，降低PD的值，I基本不用變。

5、加上偏航的修正參數(shù)后（直接給雙環(huán)參數(shù)，角度外環(huán)P和橫滾差不多，內(nèi)環(huán)P比橫滾大些，I和橫滾差不多，D可以先不加），
拿在手上試過修正和打舵方向正確后可以試飛了（試飛很危險！�。�！選擇在寬敞、無風(fēng)的室內(nèi)，1米的高度（高度太低會有地面效應(yīng)干擾，
太高不容易看清姿態(tài)且容易摔壞），避開人群的地方比較適合，如有意外情況，立刻關(guān)閉油門�。�！
5.1試飛時主要觀察這么幾個方面的情況，一般經(jīng)過調(diào)整的參數(shù)在平衡位置不會大幅度震蕩，需要觀察：
5.1.1在平衡位置有沒有小幅度震蕩（可能是由于機架震動太大導(dǎo)致姿態(tài)解算錯誤造成。也可能是角速度內(nèi)環(huán)D的波動過大，
前者可以加強減震措施，傳感器下貼上3M膠，必要時在兩層3M泡沫膠中夾上“減震板”，注意：鐵磁性的減震板會干擾磁力計讀數(shù)；
后者可以嘗試降低D項濾波的截止頻率）。
5.1.2觀察打舵響應(yīng)的速度和舵量回中后飛機的回復(fù)速度。
5.1.3各個方向（記得測試右前，左后等方向）大舵量突加輸入并回中時是否會引起震蕩。
如有，嘗試減小內(nèi)環(huán)PD也可能是由于“右前”等混控方向上的舵量太大造成。

6、橫滾和俯仰調(diào)好后就可以調(diào)整偏航的參數(shù)了。合適參數(shù)的判斷標準和之前一樣，打舵快速響應(yīng)，舵量回中飛機立刻停止轉(zhuǎn)動（參數(shù)D的作用）。

至此，雙環(huán)PID參數(shù)調(diào)節(jié)完畢！祝爽飛！
*/


匿名pid算法詳解.doc

46 KB, 下載次數(shù): 55, 下載積分: 黑幣 -5

作者: wdliming    時間: 2019-9-3 13:14
謝謝分享



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1