《電子工程師項(xiàng)目教學(xué)與訓(xùn)練》課件第6章

第6章消旋控制器設(shè)計(jì)

6.1引言6.2消旋控制器設(shè)計(jì)任務(wù)書

6.3設(shè)計(jì)方案6.4硬件電路設(shè)計(jì)6.5軟件設(shè)計(jì)6.6程序清單

6.1引言飛行器在飛行中，機(jī)體產(chǎn)生旋轉(zhuǎn)，使其上攜帶的相機(jī)也隨之旋轉(zhuǎn)，造成拍攝到的圖像疊加了額外的旋轉(zhuǎn)，妨礙了后續(xù)的圖像處理。為了獲得穩(wěn)定的圖像，需要隔離機(jī)體旋轉(zhuǎn)對(duì)相機(jī)姿態(tài)的耦合影響，實(shí)現(xiàn)對(duì)目標(biāo)圖像的消旋。本節(jié)設(shè)計(jì)了隔離機(jī)體旋轉(zhuǎn)的消旋控制器。該控制器通過測量機(jī)體旋轉(zhuǎn)的運(yùn)動(dòng)參數(shù)，控制消旋平臺(tái)及相機(jī)反向旋轉(zhuǎn)，從而使相機(jī)姿態(tài)不受機(jī)體旋轉(zhuǎn)的影響。本節(jié)內(nèi)容主要涉及到A/D轉(zhuǎn)換、PWM波生產(chǎn)、直流電機(jī)驅(qū)動(dòng)、數(shù)字濾波、閉環(huán)控制系統(tǒng)和數(shù)字PID算法等方面的內(nèi)容。6.2消旋控制器設(shè)計(jì)任務(wù)書

1.設(shè)計(jì)任務(wù)消旋系統(tǒng)的基本結(jié)構(gòu)如圖6-1所示。圖中，直流有刷電機(jī)的轉(zhuǎn)子與相機(jī)鏡頭和陀螺儀直接軸連，當(dāng)機(jī)體產(chǎn)生旋轉(zhuǎn)時(shí)，電機(jī)帶動(dòng)鏡頭做反向旋轉(zhuǎn)實(shí)現(xiàn)消旋效果。陀螺儀為系統(tǒng)中的測角速度元件，其信號(hào)表征相機(jī)鏡頭旋轉(zhuǎn)的角速度，即機(jī)體轉(zhuǎn)速與電機(jī)轉(zhuǎn)速之差?，F(xiàn)設(shè)計(jì)并制作一消旋控制器，要求該控制器能根據(jù)陀螺儀的信號(hào)，通過控制算法的運(yùn)算，輸出一定占空比的PWM波驅(qū)動(dòng)直流電機(jī)，使鏡頭反向旋轉(zhuǎn)以抵消機(jī)體的旋轉(zhuǎn)，實(shí)現(xiàn)消旋效果。圖6-1消旋系統(tǒng)結(jié)構(gòu)示意圖

2.設(shè)計(jì)要求

1)對(duì)陀螺儀輸出信號(hào)的采樣和處理系統(tǒng)中使用的陀螺儀為單軸角速率陀螺，用來測量相機(jī)鏡頭旋轉(zhuǎn)的角速率。陀螺儀的量程為±20°/s，其輸出為一電壓信號(hào)，該信號(hào)與被測角速度間呈圖6-2所示的線性關(guān)系。陀螺儀的零位電壓值(即陀螺在靜止時(shí)的輸出電壓)為2.5V。按設(shè)計(jì)要求，對(duì)陀螺儀的輸出信號(hào)要進(jìn)行A/D采樣，按陀螺儀的刻度因數(shù)，要求采樣的精度大于10mV。圖6-2陀螺儀信號(hào)關(guān)系圖

2)控制算法的實(shí)現(xiàn)和輸出PWM波形本設(shè)計(jì)中采用數(shù)字PID算法，要求實(shí)現(xiàn)較高的控制精度。當(dāng)機(jī)體轉(zhuǎn)速為720°/s時(shí)，鏡頭的轉(zhuǎn)速小于5°/s。另外，當(dāng)機(jī)體有加/減速動(dòng)作時(shí)，要求鏡頭能盡快響應(yīng)該變化，實(shí)現(xiàn)較理想的消旋效果。控制算法的運(yùn)算結(jié)果通過量程轉(zhuǎn)換將轉(zhuǎn)變?yōu)镻WM波形的占空比，該P(yáng)WM波的頻率為8kHz。

3)驅(qū)動(dòng)直流有刷電機(jī)系統(tǒng)通過對(duì)PWM波形進(jìn)行功率放大，驅(qū)動(dòng)直流有刷電機(jī)。該電機(jī)驅(qū)動(dòng)器要求外加電源，包括邏輯部分電源和繞組供電電源，驅(qū)動(dòng)器邏輯部分輸入電壓5V±10%，電機(jī)繞組驅(qū)動(dòng)輸入電壓范圍為12～48V。6.3設(shè)計(jì)方案消旋控制器主要由三部分組成：處理器、功放驅(qū)動(dòng)電路和陀螺儀，三者形成了一個(gè)閉環(huán)控制系統(tǒng)，實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)速的自動(dòng)調(diào)速。其原理框圖如圖6-3所示。處理器選用了TI公司的16位微控制器MSP430F149，該芯片上集成有12位A/D轉(zhuǎn)換器、16位定時(shí)器等資源，可以大大簡化系統(tǒng)的電路設(shè)計(jì)。驅(qū)動(dòng)電路采用ST公司生產(chǎn)L298芯片，該芯片為內(nèi)含兩個(gè)H橋的高電壓大電流全橋式驅(qū)動(dòng)器。其最高工作電壓可達(dá)46V，瞬間峰值電流可達(dá)3A，持續(xù)工作電流為2A。圖6-3消旋控制器原理框圖6.4硬件電路設(shè)計(jì)6.4.1主處理器及其外圍電路設(shè)計(jì)此次設(shè)計(jì)中主控制器選擇使用MSP430F149。該單片機(jī)是TI公司生產(chǎn)的一款FLASH型超低功耗16位單片機(jī)，它集處理能力強(qiáng)、運(yùn)算速度快、功耗低等優(yōu)點(diǎn)于一身；內(nèi)部預(yù)設(shè)了JTAG模塊，使得每一個(gè)單片機(jī)芯片都具有完整的在線調(diào)試功能，而不必使用復(fù)雜的仿真調(diào)試工具；采用了精簡指令集，只有27條核心指令，指令周期可達(dá)125ns。該系列單片機(jī)不但在指令的執(zhí)行速度上取得了優(yōu)勢，其內(nèi)部的附加資源也比傳統(tǒng)的MCS-51豐富了許多，其主要特點(diǎn)如下：

(1)超低功耗：運(yùn)行在1MHz時(shí)鐘條件下時(shí)，工作電流視工作模式不同為0.1～400μA。

(2)工作電壓：1.8～3.6V。

(3)工作頻率范圍：32768Hz～8MHz。

(4)用戶應(yīng)用程序空間：60KB、48KB、32KB、16KB、8KB、4KB、1KB。

(5)片上集成最高可達(dá)2KBRAM。

(6)?6個(gè)8位通用I/O端口(48個(gè))：每一端口的所有引腳可以單獨(dú)配置，具有控制I/O方向、輸出、輸入的能力。P1、P2口具有中斷功能，每一個(gè)引腳都可以單獨(dú)選擇中斷觸發(fā)沿、單獨(dú)允許中斷。

(7)基礎(chǔ)時(shí)鐘模塊：包括一個(gè)數(shù)控振蕩器(DCO)和2個(gè)晶體振蕩器。CPU可以用DCO工作，也可以用晶振時(shí)鐘信號(hào)運(yùn)行。

(8)看門狗定時(shí)器：保證程序發(fā)生問題時(shí)使受控系統(tǒng)重新啟動(dòng)。應(yīng)用程序中如果不需要此功能，則可以把它當(dāng)作普通定時(shí)器，當(dāng)選定時(shí)間到達(dá)后將產(chǎn)生中斷。

(9)帶有3個(gè)捕獲、比較寄存器的16位定時(shí)器Timer_A3和帶有7個(gè)捕獲、比較寄存器的16位定時(shí)器Timer_B7：可以支持同時(shí)進(jìn)行的多種時(shí)序控制、多個(gè)捕獲、比較功能、多種輸出波形(PWM波形)，也可以是上述功能的組合。具有中斷能力，每個(gè)捕獲、比較寄存器還可以硬件方式實(shí)現(xiàn)串行通信(如UART方式)。

(10)模擬比較器：可實(shí)現(xiàn)模擬電壓比較。

(11)具有8路12位精度的A/D轉(zhuǎn)換模塊。

(12)?UART接口：支持兩種不同的串行協(xié)議，即通用異步協(xié)議(UART協(xié)議)和同步協(xié)議(SPI協(xié)議)。

(13)硬件乘法器：可以明顯提高程序的運(yùn)行速度，大大減輕CPU的程序開銷。

(14)工作溫度范圍：-40～+85℃。

(15)封裝：PDIP-28，SOP-28，PDIP-20，SOP-20，PLCC-32，TSSOP-20。圖6-4給出了MSP430F149及其外圍電路。系統(tǒng)采用了外部晶振(8MHz)；復(fù)位電路采用了普通的按鍵復(fù)位電路。JP2接口為14針JTAG 接口，可以利用它下載和在線調(diào)試程序。陀螺儀的輸出信號(hào)由接口JP1引入，經(jīng)電阻分壓后接入MSP430F149的A4通道進(jìn)行采樣。管腳P4.1為TimerB的輸出管腳，輸出PWM波形。管腳P5.0輸出L298的使能信號(hào)(高電平有效)。圖6-4主處理器及其外圍電路6.4.2功率驅(qū)動(dòng)電路設(shè)計(jì)功放驅(qū)動(dòng)電路的核心是L298芯片，該芯片內(nèi)含兩個(gè)H橋，其原理框圖如圖6-5所示。由圖6-5可見，每個(gè)H橋的下側(cè)橋臂晶體管發(fā)射極連在一起，其輸出腳(1和15)用來連接電流檢測電阻。9腳接邏輯控制部分的電源，常用+5V。4腳為電機(jī)驅(qū)動(dòng)電源VS。5、7、10、12腳輸入標(biāo)準(zhǔn)TTL邏輯點(diǎn)評(píng)信號(hào)，用來控制H橋的開與關(guān)。6、11腳則為使能控制端。圖6-6所示為由L298構(gòu)成的電機(jī)驅(qū)動(dòng)電路，其中電機(jī)驅(qū)動(dòng)電源為?+28V。首先將單片機(jī)輸出的PWM波形通過與非門芯片HEF4093B變?yōu)閮陕贩聪嗟腜WM波(In1、In2)接入L298的輸入端，然后將L298的兩組H橋并聯(lián)使用，這樣可以使電機(jī)驅(qū)動(dòng)電流達(dá)到3.5A。圖6-5L298原理框圖圖6-6電機(jī)驅(qū)動(dòng)電路6.5軟件設(shè)計(jì)6.5.1系統(tǒng)程序結(jié)構(gòu)系統(tǒng)的程序結(jié)構(gòu)如圖6-7所示。系統(tǒng)上電之后先進(jìn)行初始化操作，?主要包括時(shí)鐘初始化、變量的初始化、A/D和Timer的配置。進(jìn)入主程序后，首先對(duì)陀螺儀的輸出信號(hào)啟動(dòng)A/D轉(zhuǎn)換，得到采樣結(jié)果后進(jìn)行數(shù)字濾波；然后計(jì)算偏差，進(jìn)入數(shù)字PID算法；根據(jù)其計(jì)算結(jié)果，經(jīng)量程轉(zhuǎn)換變?yōu)檎伎毡戎?，然后配置TimerB，使其在P4.1管腳上輸出該占空比的PWM波，之后程序再次啟動(dòng)A/D轉(zhuǎn)換，循環(huán)運(yùn)行以上步驟。這樣，程序可根據(jù)陀螺儀測得的鏡頭轉(zhuǎn)速，不斷地調(diào)整所輸出PWM波的占空比，再由L298驅(qū)動(dòng)達(dá)到控制電機(jī)轉(zhuǎn)速使之抵消外部旋轉(zhuǎn)的目的。圖6-7系統(tǒng)程序結(jié)構(gòu)框圖6.5.2數(shù)字濾波在本設(shè)計(jì)中，主程序首先要對(duì)陀螺儀的輸出值進(jìn)行采樣，由于各種干擾的存在，如果僅采樣一次，就無法確定結(jié)果是否可信。因此，在軟件設(shè)計(jì)上采用數(shù)字濾波的方法來減少干擾，提高采樣的可靠性。設(shè)計(jì)中采用中值濾波和算數(shù)平均濾波相結(jié)合的方法，具體的做法是：在一個(gè)控制周期內(nèi)(即主程序循環(huán)一次)對(duì)被測量連續(xù)采樣7次，將采樣結(jié)果排序去除最大和最小值，然后求剩余5個(gè)采樣結(jié)果的算術(shù)平均值，將其作為最終的采樣結(jié)果輸出。這部分程序結(jié)構(gòu)比較簡單，請(qǐng)讀者閱讀程序清單。6.5.3數(shù)字PID算法

PID控制器是一種線性控制器，它根據(jù)給定值r(t)與實(shí)際輸出值c(t)構(gòu)成控制偏差e(t)=r(t)-c(t)將偏差的比例(P)、積分(I)和微分(D)通過線性組合構(gòu)成控制量，對(duì)被控對(duì)象進(jìn)行控制，故稱PID控制器。在計(jì)算機(jī)控制中，采用的是對(duì)積分、微分項(xiàng)進(jìn)行離散化處理后的數(shù)字PID算法。關(guān)于數(shù)字PID算法及其常用的改進(jìn)方法，本節(jié)中不再做詳細(xì)的討論，請(qǐng)參閱相關(guān)參考書。在本設(shè)計(jì)中，為了得到較好的控制特性，采用增量式PID控制，并加入了積分分離、和遇限削弱積分環(huán)節(jié)。其程序框圖見圖6-8，具體的程序設(shè)計(jì)詳見程序清單。圖6-8系統(tǒng)流程圖6.5.4PWM波形的生成

PWM波形的生成是利用了MSP430F149定時(shí)器的復(fù)位/置位模式來實(shí)現(xiàn)的。如圖6-9所示，對(duì)復(fù)位/置位模式，當(dāng)TimerB的計(jì)數(shù)值小于TBCCR1時(shí)輸出高電平；大于TBCCR1時(shí)輸出低電平。配合計(jì)數(shù)器時(shí)鐘的選擇，配置TBCCR0的值來設(shè)定PWM波形的頻率，修改TBCCR1的值來設(shè)定其占空比。圖6-9PWM波形輸出示意圖下面給出了生成PWM的程序：

voidInit_TimerB(void)

{

P4DIR|=BIT1; //P4.1為輸出

P4SEL|=BIT1; //選擇P4.1為TB1管腳

TBCTL=TBSSEL1+TBCLR; //選擇SMCLK，清除TAR

TBCCR0=1000;

//SMCLK的頻率為8MHz,PWM頻率為8KHz

TBCCTL1=OUTMOD_7; //CCR1輸出模式為“復(fù)位/置位”模式

TBCCR1=500; //PWM占空比為500/1000=50%

TBCTL|=MC0;

//增記數(shù)模式

return;

}6.6程序清單

#include<MSP430X14X.h>

voidInit_CLK(void);

voidInit_ADC(void);

voidInit_TimerB(void);

intDutyCycle,i,max,min,PWM; //DutyCycle為占空比值

intSampling[7]={0,0,0,0,0,0,0}; //采樣結(jié)果

longinttemp1;

floatKp,Ki,Kd,Input,Feedback,Error,Error_1,U_PID,U_P,U_I,U_D,U_I_1;

//Kp,Ki,Kd為PID控制參數(shù)

//Error,Error_1分別為偏差和上一控制周期的偏差//U_PID,U_P,U_I,U_D,U_I_1分別為總控制量、比例控制量、積分控制量、微分控制量和上一//控制周期的積分控制量//---------------------------------------------------voidInit_Variable(void)//變量初始化{DutyCycle=2048;temp1=0;Kp=0.035;Ki=0.0085;Kd=0.001;Input=2048.0;Feedback=0.0;Error=0.0;Error_1=0.0;U_PID=0.0;U_P=0.0;U_I=0.0;U_D=0.0;U_I_1=0.0;}//---------------------------------------------------------------------------voidInit_CLK(void){unsignedinti;//將寄存器的內(nèi)容清零

//XT2震蕩器開啟

//LFTX1工作在低頻模式

//ACLK的分頻因子為1BCSCTL1=0X00;

do{//清除OSCFault標(biāo)志

IFG1&=～OFIFG;for(i=0x20;i>0;i--);}while((IFG1&OFIFG)==OFIFG);BCSCTL2=0X00; //MCLK的時(shí)鐘源為XT2CLK:8MHz，分頻因子為0BCSCTL2+=SELM1+DIVM_0; //SMCLK的時(shí)鐘源為XT2CLK:8MHz，分頻因子為0BCSCTL2+=SELS+DIVS_0;return;}//------------------------------------------------------------------------------voidInit_ADC(void){//設(shè)置P6.0為模擬輸入通道

P6SEL=0X07;//設(shè)置ENC為0，從而修改ADC12寄存器的值

ADC12CTL0&=～(ENC);//打開內(nèi)部參考電壓

ADC12CTL0|=REFON;//內(nèi)部參考電壓設(shè)為2.5VADC12CTL0|=REF2_5V;//轉(zhuǎn)換的起始地址為：ADCMEM4ADC12CTL1|=CSTARTADD_4; //輸入通道為A4ADC12MCTL4=INCH_4+EOS;ADC12CTL0|=ADC12ON;ADC12CTL0|=MSC;

//轉(zhuǎn)換模式為：單通道、單次轉(zhuǎn)換

ADC12CTL1|=CONSEQ_0;

ADC12CTL1|=ADC12SSEL_3;

//選擇轉(zhuǎn)換內(nèi)核的時(shí)鐘源為SMCLKADC12CTL1|=ADC12DIV_3;

//分頻因子為4ADC12CTL1|=(SHP);//使能ADC轉(zhuǎn)換

ADC12CTL0|=ENC; return;}//----------------------------------------------------------------------voidInit_TimerB(void){//P4.1為輸出

P4DIR|=BIT1;

//選擇P4.1為TB1管腳

P4SEL|=BIT1;//選擇SMCLK，清除TARTBCTL=TBSSEL1+TBCLR;//CCR0中斷允許

//PWM頻率為8000HzTBCCR0=1000;//CCR1輸出模式為“復(fù)位/置位”模式

TBCCTL1=OUTMOD_7;TBCCR1=500;//增記數(shù)模式

TBCTL|=MC0; return;}voidmain(void){

WDTCTL=WDTPW+WDTHOLD;

Init_CLK();Init_TimerB();Init_ADC();Init_Variable();

while(1){DutyCycle=0;temp1=0;

//-------------------------------------數(shù)據(jù)采樣-----------------------------------for(i=0;i<7;i++){ADC12CTL0|=ENC+ADC12SC; //啟動(dòng)轉(zhuǎn)換

while((ADC12IFG&ADC12BUSY)==0); //判斷轉(zhuǎn)換是否完成

Sampling[i]=ADC12MEM4;temp1=temp1+Sampling[i];}//------------------------------------數(shù)字濾波-----------------------------------------max=Sampling[0];min=Sampling[0];for(i=1;i<7;i++){if(Sampling[i]>max)max=Sampling[i];elseif(Sampling[i]<min)min=Sampling[i];}

Feedback=(temp1-max-min)/5;

//----------------------