《電子工程師項目教學與訓練》課件第6章

第6章消旋控制器設計

6.1引言6.2消旋控制器設計任務書

6.3設計方案6.4硬件電路設計6.5軟件設計6.6程序清單

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

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

2.設計要求

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

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

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

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

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

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

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

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

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

(7)基礎時鐘模塊：包括一個數(shù)控振蕩器(DCO)和2個晶體振蕩器。CPU可以用DCO工作，也可以用晶振時鐘信號運行。

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

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

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

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

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

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

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

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

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

PWM波形的生成是利用了MSP430F149定時器的復位/置位模式來實現(xiàn)的。如圖6-9所示，對復位/置位模式，當TimerB的計數(shù)值小于TBCCR1時輸出高電平；大于TBCCR1時輸出低電平。配合計數(shù)器時鐘的選擇，配置TBCCR0的值來設定PWM波形的頻率，修改TBCCR1的值來設定其占空比。圖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輸出模式為“復位/置位”模式

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}; //采樣結果

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標志

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

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

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

ADC12CTL0|=REFON;//內(nèi)部參考電壓設為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)核的時鐘源為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輸出模式為“復位/置位”模式

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; //啟動轉(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;

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