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