|
|
分享一個不錯的PID調節文檔
/******************** (C) COPYRIGHT 2014 ANO Tech ***************************
* 文件名 ：ANO_FlyControl.cpp
* 描述 ：飛行控制
**********************************************************************************/
include "ANO_FlyControl.h"ANO_FlyControl fc;
/*
先整定內環，后整定外環。
參數整定找最佳，從小到大順序查
先是比例后積分，最后再把微分加
曲線振蕩很頻繁，比例度盤要放大
曲線漂浮繞大灣，比例度盤往小扳
曲線偏離回復慢，積分時間往下降
曲線波動周期長，積分時間再加長
曲線振蕩頻率快，先把微分降下來
動差大來波動慢。微分時間應加長
理想曲線兩個波，前高后低4比1
*/
/*
ROLL和PIT軸向按照以上公式計算PID輸出，但YAW軸比較特殊，因為偏航角法線方向剛好和地球重力平行，
這個方向的角度無法由加速度計直接測得，需要增加一個電子羅盤來替代加速度計。如果不使用羅盤的話，
我們可以單純的通過角速度積分來測得偏航角，缺點是由于積分環節中存在積分漂移，偏航角隨著時間的推移
會偏差越來越大。我們不使用羅盤就沒有比例項，只僅使用微分環節來控制。
*/
ANO_FlyControl::ANO_FlyControl()
{
yawRate = 120;
//重置PID參數
PID_Reset();
}
//重置PID參數
void ANO_FlyControl: ID_Reset(void)
{
//因為YAW角度會漂移，所以參數和ROLL、PITCH不一樣
pid[PIDROLL].set_pid(70, 15, 120, 2000000); //ROLL角度的內環控制系數,20000:積分上限
pid[PIDPITCH].set_pid(70, 30, 120, 2000000);//PITCH角度的內環控制系數
pid[PIDYAW].set_pid(100, 50, 0, 2000000); //YAW角度的內環控制系數
pid[PIDLEVEL].set_pid(280, 0, 0, 0); //外環控制系數
pid[PIDMAG].set_pid(15, 0, 0, 0); //電子羅盤控制系數
}
/*
【掃盲知識】
串級PID：采用的角度P和角速度PID的雙閉環PID算法------>角度的誤差被作為期望輸入到角速度控制器中 （角度的微分就是角速度）
對于本系統則采用了將角度控制與角速度控制級聯的方式組成整個串級 PID 控制器。
串級 PID 算法中，角速度內環占著極為重要的地位。在對四旋翼飛行的物理模型進
行分析后，可以知道造成系統不穩定的物理表現之一就是不穩定的角速度。
因此，若能夠直接對系統的角速度進行較好的閉環控制，必然會改善系統的動態特性
及其穩定性，通常也把角速度內環稱為增穩環節。而角度外環的作用則體現在對四旋翼飛
行器的姿態角的精確控制。
外環：輸入為角度,輸出為角速度
內環：輸入為角速度，輸出為PWM增量
使用串級pid，分為：角度環控制pid環，和角速度控制環穩定環。主調為角度環（外環），副調為角速度環（內環）。
參數整定原則為先內后外，故在整定內環時將外環的PID均設為0
所謂外環就是只是一個P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飛機震蕩。
震蕩的特點是：頻率小、幅度大
*/
/*
【橫滾（Roll）和俯仰（Pitch）的控制算法】
橫滾（Roll）和俯仰（Pitch）的控制算法是一樣的，控制參數也比較接近。
首先得到軸姿態的角度差（angle error），將這個值乘以角度系數p后限幅（限幅必須有，否則劇烈打舵時容易引發震蕩）作為角速度控制器期望值（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輸出。
*/
//飛行器姿態外環控制
void ANO_FlyControl::Attitude_Outter_Loop(void)
{
int32_t errorAngle[2];
Vector3f Gyro_ADC;
//計算角度誤差值, 角度誤差值=期望值-此刻姿態值
//constrain_int32作用：32位整型數限幅，使其控制輸入的最大飛行傾角不大于25度（如果控制量比25度大，飛機早就墜毀了）
//rc.Command[ROLL]：遙控數據 imu.angle.x ：此刻姿態(角度)
//1.得到軸姿態的角度差（errorAngle）
//2.這個角度差值進行限幅(constrain_int32)（正負FLYANGLE_MAX）（限幅必須有，否則劇烈打舵時容易引發震蕩）作為角速度控制器期望值（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;
/*
得到外環PID輸出（角速度的差值）(實質是相當于內環的P比例項)-------->
3.target_rate與陀螺儀得到的當前角速度作差，得到角速度誤差（RateError）乘以kp（外環控制系數 pid[PIDLEVEL]--->(280, 0, 0, 0)）得到給內環的P。
*/
//橫滾roll：外環控制。輸入為角度,輸出為角速度。RateError[ROLL] 作為內環的輸入。
RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x; //Gyro_ADC.x:陀螺儀X軸的值
//俯仰pitch：外環控制。輸入為角度,輸出為角速度。RateError[PITCH] 作為內環的輸入。
RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) - Gyro_ADC.y;//Gyro_ADC.y:陀螺儀Y軸的值
/*
偏航（Yaw）的控制算法和前兩者略有不同，是將打舵量（遙控數據量rc.Command[YAW]）和角度誤差的和作為角速度內環的期望值，
這樣可以獲得更好的動態響應。角速度內環和橫滾與俯仰的控制方法一致，參數（積分限幅值會很小，默認只有萬分之8）上有不同。*/
//航向yaw：外環控制。輸入為角度,輸出為角速度。 RateError[YAW] 作為內環的輸入。
RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z; //Gyro_ADC.z:陀螺儀Z軸的值
}
//飛行器姿態內環控制: 輸入為角速度，輸出為PWM增量
//內環的效果就是：減小 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++)
{
//現象：當油門低于檢查值時積分清零，重新積分
//猜測：這里應該是擔心飛機沒飛起來時就開始有積分，會導致起飛時不穩定
if ((rc.rawData[THROTTLE]) < RC_MINCHECK)
pid.reset_I();
//get_pid函數：return get_p(error) + get_i(error, dt) + get_d(error, dt);-------->這里實際就是一個完整的PID
//PID_INNER_LOOP_TIME：2000us--->0.2ms 積分微分時間，每隔0.2ms操作積分和微分,RateError是外環計算的結果（從外環算出）
//得到內環PID輸出，直接輸出轉為電機控制量
PIDTerm = pid.get_pid(RateError, PID_INNER_LOOP_TIME);
}
//對YAW角繼續處理，加入遙控控制
//在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]);
}
/*
【調節串環PID大概過程（注意修正反向）】
1、估計大概的起飛油門。
2、調整角速度內環參數。
3、將角度外環加上，調整外環參數。
4、橫滾俯仰參數一般可取一致，將飛機解綁，抓在手中測試兩個軸混合控制的效果（注意安全），有問題回到“烤四軸”繼續調整，直至飛機在手中不會抽搐。
5、大概設置偏航參數（不追求動態響應，起飛后頭不偏即可），起飛后再觀察橫滾和俯仰軸向打舵的反應，如有問題回到“烤四軸”。
6、橫滾和俯仰ok以后，再調整偏航軸參數以達到好的動態效果。
*/
/*
【過程詳解】
1、要在飛機的起飛油門基礎上進行PID參數的調整，否則“烤四軸”的時候調試穩定了，飛起來很可能又會晃蕩。
2、內環的參數最為關鍵！理想的內環參數能夠很好地跟隨打舵（角速度控制模式下的打舵）控制量。
在平衡位置附近（正負30度左右），舵量突加，飛機快速響應；舵量回中，飛機立刻停止運動（幾乎沒有回彈和震蕩）。
2.1首先改變程序，將角度外環去掉，將打舵量作為內環的期望（角速度模式，在APM中叫ACRO模式，在大疆中叫手動模式）。
2.2加上P，P太小，不能修正角速度誤差表現為很“軟”傾斜后難以修正，打舵響應也差。P太大，在平衡位置容易震蕩，
打舵回中或給干擾（用手突加干擾）時會震蕩。合適的P能較好的對打舵進行響應，又不太會震蕩，但是舵量回中后會回彈好幾下才能停止（沒有D）。
2.3加上D，D的效果十分明顯，加快打舵響應，最大的作用是能很好地抑制舵量回中后的震蕩，可謂立竿見影。
太大的D會在橫滾俯仰混控時表現出來（盡管在“烤四軸”時的表現可能很好），具體表現是四軸抓在手里推油門會抽搐。
如果這樣，只能回到“烤四軸”降低D，同時P也只能跟著降低。D調整完后可以再次加大P值，以能夠跟隨打舵為判斷標準。
2.4加上I，會發現手感變得柔和了些。由于筆者“烤四軸”的裝置中四軸的重心高于旋轉軸，這決定了在四軸偏離水平位置后
會有重力分量使得四軸會繼續偏離平衡位置。I的作用就可以使得在一定角度范圍內（30度左右）可以修正重力帶來的影響。
表現打舵使得飛機偏離平衡位置，舵量回中后飛機立刻停止轉動，若沒有I或太小，飛機會由于重力繼續轉動。
3、角度外環只有一個參數P。將外環加上（在APM中叫Stabilize模式，在大疆中叫姿態模式）。打舵會對應到期望的角度。
P的參數比較簡單。太小，打舵不靈敏，太大，打舵回中易震蕩。以合適的打舵反應速度為準。
4、至此，烤四軸”效果應該會很好了，但是兩個軸混控的效果如何還不一定，有可能會抽（兩個軸的控制量疊加起來，
特別是較大的D，會引起抽搐）。如果抽了，降低PD的值，I基本不用變。
5、加上偏航的修正參數后（直接給雙環參數，角度外環P和橫滾差不多，內環P比橫滾大些，I和橫滾差不多，D可以先不加），
拿在手上試過修正和打舵方向正確后可以試飛了（試飛很危險！！！！選擇在寬敞、無風的室內，1米的高度（高度太低會有地面效應干擾，
太高不容易看清姿態且容易摔壞），避開人群的地方比較適合，如有意外情況，立刻關閉油門！！！
5.1試飛時主要觀察這么幾個方面的情況，一般經過調整的參數在平衡位置不會大幅度震蕩，需要觀察：
5.1.1在平衡位置有沒有小幅度震蕩（可能是由于機架震動太大導致姿態解算錯誤造成。也可能是角速度內環D的波動過大，
前者可以加強減震措施，傳感器下貼上3M膠，必要時在兩層3M泡沫膠中夾上“減震板”，注意：鐵磁性的減震板會干擾磁力計讀數；
后者可以嘗試降低D項濾波的截止頻率）。
5.1.2觀察打舵響應的速度和舵量回中后飛機的回復速度。
5.1.3各個方向（記得測試右前，左后等方向）大舵量突加輸入并回中時是否會引起震蕩。
如有，嘗試減小內環PD也可能是由于“右前”等混控方向上的舵量太大造成。
6、橫滾和俯仰調好后就可以調整偏航的參數了。合適參數的判斷標準和之前一樣，打舵快速響應，舵量回中飛機立刻停止轉動（參數D的作用）。
至此，雙環PID參數調節完畢！祝爽飛！
*/
|
-
-
匿名pid算法詳解.doc
2019-2-16 22:32 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
46 KB, 下載次數: 55, 下載積分: 黑幣 -5
|
