II四旋翼無人機(jī)的抗風(fēng)實(shí)驗(yàn)設(shè)計(jì)案例目錄TOC\o"1-3"\h\u32278四旋翼無人機(jī)的抗風(fēng)實(shí)驗(yàn)設(shè)計(jì)案例 1322161.1MiniFly的硬件資源介紹 1294891.2四旋翼無人機(jī)抗風(fēng)姿態(tài)的參數(shù)設(shè)計(jì) 2182991.2.1角度測(cè)量值的獲取 3140971.2.2角度環(huán)ADRC控制器 4180791.2.3角度環(huán)PID控制器 5209791.2.4角速度環(huán)PID控制器 6171971.2.5高度環(huán)PID控制器 7128801.3電機(jī)的控制 8264001.3.1脈沖位置調(diào)制（PWM） 886971.3.2PWM控制代碼設(shè)計(jì) 914711.4實(shí)驗(yàn)測(cè)試 11319801.4.1寫入程序 11208951.4.2抗風(fēng)實(shí)驗(yàn) 11為了驗(yàn)證PID算法和ADRC算法的有效性，采用了由正點(diǎn)原子開發(fā)的ATK-MiniFly微型四旋翼無人機(jī)作為實(shí)驗(yàn)對(duì)象進(jìn)行飛控平穩(wěn)的實(shí)驗(yàn)。1.1MiniFly的硬件資源介紹圖5-1MiniFly的系統(tǒng)框架從MiniFly系統(tǒng)的框架可以看出，四旋翼無人機(jī)通過一顆九軸傳感器MPU9250來獲取實(shí)際數(shù)據(jù)值，如加速計(jì)值、陀螺計(jì)值和氣壓值大?。徊捎肧TM32F411CEU06作為主控芯片，在此芯片上可以完成電機(jī)驅(qū)動(dòng)、數(shù)據(jù)計(jì)算、姿態(tài)控制等功能；為了提高四旋翼無人機(jī)的可開發(fā)性，MiniFly專門設(shè)置了一個(gè)擴(kuò)展接口，使得四旋翼無人機(jī)可以搭載外設(shè)設(shè)備。1.2四旋翼無人機(jī)抗風(fēng)姿態(tài)的參數(shù)設(shè)計(jì)四旋翼無人機(jī)在飛行過程中受到風(fēng)的干擾，傳感器通過獲得的加計(jì)數(shù)據(jù)、氣壓數(shù)據(jù)、陀螺數(shù)據(jù)[16]（捕獲的數(shù)據(jù)中已已將風(fēng)的干擾考慮進(jìn)去）作為實(shí)際的測(cè)量值，輸入設(shè)計(jì)的四旋翼無人機(jī)PID控制器完成控制所用。圖5-2姿態(tài)控制算法流程圖控制算法流程圖中PID控制器主要控制四旋翼無人機(jī)的位置，ADRC控制器主要控制四旋翼無人機(jī)的姿態(tài)。通過Z軸高度PID控制器完成高度控制；利用ADRC控制器完成中橫滾角、俯仰角的姿態(tài)控制，利用PID控制器控制四旋翼無人機(jī)的偏航角。設(shè)計(jì)的四旋翼無人機(jī)的PID控制代碼如下：while(1){vTaskDelayUntil(&lastWakeTime,MAIN_LOOP_LT);//獲得周期延遲信息 sensorsAcquire(&sensorData,tick);//獲得氣壓數(shù)據(jù) stateEstimator(&state,&sensorData,tick); //姿態(tài)角的解算 commanderGetWeizhi(&weizhi,&state);//目標(biāo)數(shù)據(jù)和飛行模式的設(shè)定flyerFlipCheck(&weizhi,&KZ,&state,tick); //翻滾檢測(cè)anomalDetec(&sensorData,&state,&KZ); //異常檢測(cè) stateKZ(&KZ,&sensorData,&state,&weizhi,tick);//ADRC與PID控制NLKZ(&KZ); //電機(jī)控制 tick++;}四旋翼無人機(jī)獲取氣壓數(shù)據(jù)和姿態(tài)角數(shù)據(jù)后在stateKZ下完成PID控制與ADRC控制，控制結(jié)果輸入NLKZ控制電機(jī)的輸出。1.2.1角度測(cè)量值的獲取傳感器MPU9250獲得加計(jì)數(shù)據(jù)和陀螺數(shù)據(jù)的實(shí)際測(cè)量值[17]（橫滾角、俯仰角、偏航角）后，利用四元數(shù)和歐拉角的關(guān)系進(jìn)行姿態(tài)角編碼。第一步：HD.x=HD.x*JD;HD.y=HD.y*JD;HD.z=HD.z*JD;其中HD表示為弧度，這里JD表示π180接著將四元數(shù)換算成方向余弦中的第三行第三個(gè)元素：DWH=KPF(JSD.x*JSD.x+JSD.y*JSD.y+JSD.z*JSD.z);JSD.x*=DANWEIHUA;JSD.y*=DANWEIHUA;JSD.z*=DANWEIHUA;其中JSD表示在三個(gè)軸上的加速度，這里運(yùn)用了KPF函數(shù)（平方開根號(hào)）進(jìn)行和加速度的單位化，得到單位化后的加速計(jì)測(cè)量值。ZLZ=2*(a1*a3-a0*a2);ZLZ=2*(a0*a1+a2*a3);ZLZ=a0s-a1s-a2s+a3s; 此處得出陀螺儀數(shù)據(jù)得到的重力向量，稱之為機(jī)體坐標(biāo)系下的Z方向向量。第二步：x=(JSD.y*ZLZ-JSD.z*ZLZ);y=(JSD.z*ZLZ-JSD.z*ZLZ);z=(JSD.x*ZLZ-JSD.z*ZLZ);單位化后的加速計(jì)測(cè)量值與在機(jī)體坐標(biāo)系下的重力加速計(jì)方向的差值用x、y、z表示，其大小與陀螺積分的誤差成正比，可以用來糾正陀螺。xLeiJI+=Ki*x*lt;yLeiJI+=Ki*y*lt;zLeiJI+=Ki*z*lt;Int是積分誤差累積計(jì)算，其中在初始條件下設(shè)置積分誤差為0。JSD.x+=Kp*x+xLeiJI;JSD.y+=Kp*y+yLeiJI;JSD.z+=Kp*z+zLeiJI;有了積分誤差累積，得到了新的陀螺儀角速度。第三步：a0+=(-a1*JSD.x-a2*JSD.y-a3*JSD.z)*BANT;a1+=(a0*JSD.x+a2*JSD.z-a3*JSD.y)*BANT;a2+=(a0*JSD.y-a1*JSD.z+a3*JSD.x)*BANT;a3+=(a0*JSD.z+a1*JSD.y-a2*JSD.x)*BANT;運(yùn)用畢卡算法計(jì)算出四元數(shù)的微分方程，其中BANT表示測(cè)量的半周期值，大小為BANT=0.5f*lt。第四步：利用四元數(shù)方向余弦矩陣與歐拉角的轉(zhuǎn)換關(guān)系，把四元數(shù)轉(zhuǎn)換成歐拉角。R11=a0s+a1s-a2s-a3s; R21=2*(a1*a2+a0*a3);ZLZ=2*(a1*a3-a0*a2);ZLZ=2*(a0*a1+a2*a3);ZLZ=a0s-a1s-a2s+a3s;JD.pitch=-asinf(ZLZ)*JIAODU;JD.roll=atan2f(ZLZ,ZLZ)*JIAODU;JD.yaw=atan2f(R21,R11)*JIAODU; 利用正余弦函數(shù)得到輸出的橫滾角、俯仰角、偏航角。1.2.2角度環(huán)ADRC控制器 角度環(huán)ADRC控制器可以得到期望角速度，其輸入是期望角度與姿態(tài)角度的差值（期望角度是在進(jìn)行飛行控制時(shí)通過遙控器輸入給四旋翼無人機(jī)的數(shù)據(jù)）。在設(shè)計(jì)自抗擾ADRC控制器時(shí)，首先安排了過渡過程，輸入為期望給定，由TD微分跟蹤器得到過渡的期望信號(hào)v1和過渡期望微分信號(hào)v2；接著設(shè)計(jì)擴(kuò)張狀態(tài)觀測(cè)器，它是由系統(tǒng)的輸出值作為反饋量，通過擴(kuò)張狀態(tài)觀測(cè)器后得到狀態(tài)信號(hào)Z1、狀態(tài)速度信號(hào)Z2、狀態(tài)加速度信號(hào)，最后經(jīng)過非線性反饋后得到需要的輸出。設(shè)計(jì)的ADRC角度環(huán)控制器的代碼如下：float

ADRC(JD*JD,JD*QWJD,JD*SCQWJD){v1

=

v1

+

h*v2;v2

=

v2

+

h*fst(v1-QWJD+JD,v2,r,h);

e

=

z1

-

y;z1

=

z1

+

h*(z2-belta01*e);z2

=

z2

+

h*(z3-belta02*fal(e,0.5,delta)+b*u);z3

=

z3

+

h*(-belta03*fal(e,0.25,delta));

e1

=

v1

-

v1;e2

=

v2

-

z2;

u0

=

belta1*fal(e1,alpha1,delta)

+

belta2*fal(e2,alpha2,delta);

SCQWJD=

u0

-

z3/b;return

SCQWJD;}代碼中QWJD-SCQWJD表示ADRC控制算法的輸入；v1、v2為微分跟蹤器的輸出；h表示濾波因子（即系統(tǒng)調(diào)用步長(zhǎng)）；e表示誤差值大?。粃1表示跟蹤反饋值；z2表示跟蹤反饋值的積分；z3表示跟蹤系統(tǒng)的總擾動(dòng)；e1、e2作為非線性反饋的輸入；u0為非線性反饋的輸出；輸出結(jié)果用SCQWJD表示。其中定義了2個(gè)函數(shù)，分別是fst函數(shù)和fal函數(shù)。函數(shù)公式為：fst=fal=通過ADRC角度環(huán)控制器后可以得到橫滾角和俯仰角的期望角速度。1.2.3角度環(huán)PID控制器偏航角的期望角速度只需用PID控制器就可以實(shí)現(xiàn)：VoidJDPID(ZITAI*SJJD,JD*QWJD,JD*SCQWJD {floatJDerroryaw=QWJD->yaw-SJJD->yaw;if(JDerroryaw>180.0f)JDerroryaw-=360.0f;elseif(JDerroryaw<-180.0)JDerroryaw+=360.0f;SCQWJD->yaw=pidGX(&pidJDyaw,JDerroryaw);}代碼中定義了一個(gè)新函數(shù)pidGX，它的作用是進(jìn)行姿態(tài)角的更新，其數(shù)學(xué)公式為：ut通過ut設(shè)計(jì)JDerroryaw是為了使偏航角的范圍限制在[-π，+π]范圍內(nèi)。1.2.4角速度環(huán)PID控制器角速度環(huán)PID控制器可以計(jì)算期望角速度與測(cè)量角速度間的差值，并與已知參數(shù)INT16_MAX進(jìn)行比較，若差值大小在參數(shù)范圍內(nèi)，就認(rèn)為此時(shí)的角速度值近似滿足期望值。其中已知參數(shù)INT16_MAX的獲得方法是經(jīng)過仿真模擬后，為了防止調(diào)整量過大難以控制而得到。voidQWJSDPID(Zzb3f*SJJSD,JD*QWJSU,KZ*SH) {SH->roll=pidSCXZ(pidGX(&pidJSDRoll,QWJD->roll-SJRate->x));SH->pitch=pidSCXZ(pidGX(&pidJSDPitch,QWJD->pitch-SJRate->y));SH->yaw=pidSCXZ(pidGX(&pidJSDYaw,QWJD->yaw-SJRate->z));}角速度環(huán)PID的設(shè)計(jì)僅僅起到了一個(gè)比較的作用，確保了角速度在合理范圍內(nèi)，比較函數(shù)pidSCXZ表示為：staticinlineint16_tpidSCXZ(floatin){if(in>INT16_MAX)//INT16_MAX=32767returnINT16_MAX;elseif(in<-INT16_MAX)return-INT16_MAX;elsereturn(int16_t)in;}若差值大小在[-32768，+32767]的范圍內(nèi)，就認(rèn)定是滿足四旋翼無人機(jī)飛行的姿態(tài)角速度值。1.2.5高度環(huán)PID控制器設(shè)計(jì)PID控制系統(tǒng)的目的在于控制四旋翼無人機(jī)電機(jī)，而控制電機(jī)的輸入為姿態(tài)角的角速度值與實(shí)際電機(jī)值的和，為此設(shè)計(jì)高度環(huán)PID控制器以獲得實(shí)際電機(jī)值。voidGDHPID(float*DJ,constJD*state,constWEIZHI*weizhi){ newDJ=DJ_SCALE*PidZ(&posPid.pidVZ,state->position.z,weizhi,POS_UPDATE_LT);*DJ=newDJ+posPid.DJBase; }實(shí)際的電機(jī)值（*DJ）=newDJ+電機(jī)基準(zhǔn)值。電機(jī)基準(zhǔn)值表示為四旋翼無人機(jī)在空中定點(diǎn)飛行時(shí)（即Z軸上的合力矩為0）電機(jī)的功率大小，newDJ表示用于其他動(dòng)力上的電機(jī)值。為了得到newDJ和電機(jī)基準(zhǔn)值，下面設(shè)計(jì)函數(shù)PidZ和函數(shù)detecZL：staticfloatPidZ(pidZzb_t*zzb,floatshuru,constWEIZHI*weizhi,floatlt){if(zzb->XQMS==false&&weizhi->DQMS==true){positionResetAllPID();zzb->weizhi=shuru+START_HIRHT;posPid.DJBase=XZDJBase(configParam.DJBase);}zzb->XQMS=weizhi->DQMS;if(weizhi->DQMS==true){zzb->weizhi+=weizhi->velocity.z*lt;SC=pidGX(&zzb->pid,zzb->weizhi-shuru);}returnSC;}PidZ函數(shù)表示：如果前一模式是手動(dòng)模式而當(dāng)前模式是定高模式，四旋翼無人機(jī)中有關(guān)PID的定高數(shù)據(jù)就會(huì)初始化狀態(tài)，默認(rèn)為初始化狀態(tài)為高度期望值（高度期望值=輸入值+START_HIRHT），當(dāng)前模式也會(huì)變成為上一模式。如果當(dāng)前模式是定高模式，可得高度期望值=上一狀態(tài)的高度期望值+電機(jī)Z軸上速度的積分，最后利用PID更新函數(shù)，將高度期望值與輸入值的差值作為更新函數(shù)的輸入值，得到需要的電機(jī)控制量。detecZL函數(shù)定義如下：四旋翼無人機(jī)在定高模式下會(huì)有一個(gè)速度值，根據(jù)機(jī)械能守恒可知，此時(shí)在Z軸方向上合力為0，則可以說明此時(shí)的電機(jī)值可以讓四旋翼無人機(jī)接近懸停[20]，也就得到了電機(jī)的基準(zhǔn)值。1.3電機(jī)的控制PWM控制器捕獲到姿態(tài)角速度值和實(shí)際電機(jī)值后，計(jì)算出每個(gè)電機(jī)所需的功率，通過電機(jī)功率的改變進(jìn)而達(dá)到姿態(tài)穩(wěn)定的目的。1.3.1脈沖位置調(diào)制（PWM）PWM簡(jiǎn)稱為脈寬調(diào)制，實(shí)際在強(qiáng)調(diào)占空比，即高電平占據(jù)一個(gè)周期的比例。圖5-4PWM占空比利用占空比可以實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)速的改變，對(duì)于直流電機(jī)來說，電機(jī)輸出端的引腳是高電平電機(jī)就可以轉(zhuǎn)動(dòng)。在STM32F411芯片上在通過引腳15、引腳21、引腳42和引腳43來控制電機(jī)，這四個(gè)引腳對(duì)應(yīng)通用計(jì)時(shí)器中的TIM2和TIM4。通用寄存器PWM中含有一個(gè)邊沿檢測(cè)器，而邊沿檢測(cè)器可以檢測(cè)到上升沿的到來和下降沿的到來，那么通過捕獲一個(gè)周期上升沿到達(dá)的時(shí)間和下降沿到達(dá)的時(shí)間，即時(shí)間的差值就是高電平持續(xù)的時(shí)間，高電平持續(xù)的時(shí)間與周期時(shí)間的比值就是占空比。1.3.2PWM控制代碼設(shè)計(jì)PWM控制包含輸入和捕獲和控制輸出，由于4個(gè)電機(jī)分別是在TIM4的通道2和通道1下作用于引腳43和引腳42，在TIM2的通道3和通道1下控制引腳15和引腳21。通過控制引腳高電平的占空比就可以實(shí)現(xiàn)對(duì)電機(jī)的控制。第一步：進(jìn)行電機(jī)的初始化使得四個(gè)引腳的復(fù)用功能控制在TIM4和TIM2通道上，另外設(shè)計(jì)PWM模式控制在1模式下（即高電平有效），初始化引腳上的比較值，使得TIM4和TIM2使能后開始捕獲數(shù)據(jù)。第二步：voidNLKZ(KZ*KZ) {s16Q=KZ->roll/2.0f;s16P=KZ->pitch/2.0f;DJPWM.m1=XZDJ(KZ->DJ-Q-P+KZ->yaw);DJPWM.m2=XZDJ(KZ->DJ-Q+P-KZ->yaw);DJPWM.m3=XZDJ(KZ->DJ+Q+P+KZ->yaw);DJPWM.m4=XZDJ(KZ->DJ+Q-P-KZ->yaw); if(DJSetEnable){DJPWM=DJPWMSet;}DJsSetZKB(DJ_M1,DJPWM.m1); DJsSetZKB(DJ_M2,DJPWM.m2);DJsSetZKB(DJ_M3,DJPWM.m3);DJsSetZKB(DJ_M4,DJPWM.m4);}由此函數(shù)可以得到電機(jī)的輸出功率，PWM初始化后開始捕獲數(shù)據(jù)（包括姿態(tài)的控制量和電機(jī)的控制量），并設(shè)計(jì)函數(shù)DJPWM=KZ->DJ-r-p+KZ->，此函數(shù)的結(jié)果作為PWM的輸出，即每個(gè)電機(jī)的功率輸出值大小，為了保證電機(jī)的輸出功率滿足飛行條件，設(shè)計(jì)了XZDJ函數(shù)對(duì)電機(jī)功率大小進(jìn)行限制。在DJPWM函數(shù)生效后，設(shè)計(jì)DJsSetZKB（以DJPWM作為輸入）來實(shí)現(xiàn)四個(gè)電機(jī)的占空比輸出。設(shè)計(jì)的代碼為：voidDJsSetZKB(u32ID,u16iDJ){if(isInit){u16ZKB=iDJ;#ifdefENABLE_DJ_BAT_COMPENSATED floatDJ=((float)iDJ/65536.0f)*60;floatDY=-0.0006239f*DJ*DJ+0.088f*DJ;floatTGDY=pmGetBatteryVoltage();floatBFB=DY/TGDY;BFB=BFB>1.0f?1.0f:BFB;ZKB=BFB*UINT16_MAX;DJ_ZKBs[ID]=ZKB; #endifswitch(ID){case0: TIM_KZBJ2(TIM4,ZKBToCCRx(ZKB));bre