專利名稱：高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)及方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)及控制方法， 屬于高分辨率航空對(duì)地觀測(cè)系統(tǒng)領(lǐng)域，可用于承載比大、穩(wěn)定精度要求高的三軸慣性穩(wěn)定平臺(tái)的獨(dú)立、自主控制，特別適合于輕高分辨率航空遙感慣性穩(wěn)定平臺(tái)。
背景技術(shù)：
慣性穩(wěn)定平臺(tái)是實(shí)現(xiàn)高分辨率對(duì)地觀測(cè)的必要設(shè)備，它可以有效隔離飛行平臺(tái)的擾動(dòng)及非理想姿態(tài)運(yùn)動(dòng)，使觀測(cè)載荷視軸指向和航向始終保持慣性空間穩(wěn)定。目前，國(guó)外代表性產(chǎn)品為瑞士 Leica公司的PAV30和PAV80，而國(guó)內(nèi)相關(guān)研究剛剛起步，無成熟產(chǎn)品。現(xiàn)有的航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)僅僅處于實(shí)驗(yàn)階段，專利“一種輕量化快響應(yīng)航空遙感慣性穩(wěn)定平臺(tái)控制系統(tǒng)” O01110155357. 3)，填補(bǔ)了這方面的空白，但是存在以下三方面不足之處。第一，控制方案采用雙環(huán)路從屬控制，無電流環(huán)控制，不能克服電源電壓、負(fù)載波動(dòng)對(duì)系統(tǒng)造成的影響以及電流過大的問題，同時(shí)電機(jī)控制轉(zhuǎn)矩脈動(dòng)較大而且無法進(jìn)行抑制；第二，系統(tǒng)控制精度較低，無法滿足實(shí)際航測(cè)的需要，同時(shí)帶載能力差， 無法應(yīng)用于大負(fù)載場(chǎng)合；第三，控制單元采用DSP+FPGA方案，而FPGA僅僅用于編碼器信號(hào)采集和PWM信號(hào)生成，不但增加了成本、功耗，同時(shí)FPGA需要占用DSP寶貴的總線資源，不利于控制算法執(zhí)行的實(shí)時(shí)性與快速性，而且對(duì)FPGA來說也是資源的浪費(fèi)；第四，現(xiàn)有的工作模式仍然不夠完善，在實(shí)際應(yīng)用中還有很多不便性，首先它沒有手動(dòng)模式，不能解決任意手控的問題，其次組合模式下航向姿態(tài)設(shè)定值仍然需要上位機(jī)頻繁發(fā)送指令，既繁瑣又不利于實(shí)際操作。

發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足，提供一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)及控制方法，達(dá)到具有高精度、大負(fù)載能力并提高了實(shí)際操作的便捷性。本發(fā)明的技術(shù)解決方案為一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)慣性器件及編碼器包括速率陀螺、編碼器、加速度計(jì)及POS ；速率陀螺與陀螺信號(hào)采集電路相連，輸出電壓信號(hào)，用于測(cè)量框架相對(duì)于慣性空間的角速率；編碼器與編碼器信號(hào)采集電路相連，輸出差分形式的二進(jìn)制代碼，用于測(cè)量框架之間的相對(duì)轉(zhuǎn)角；加速度計(jì)與加計(jì)信號(hào)采集電路相連，輸出電流信號(hào)，用于測(cè)量框架相對(duì)于地理水平系的轉(zhuǎn)角；POS與 POS信號(hào)采集電路相連，輸出串行二進(jìn)制代碼，用于測(cè)量框架相對(duì)于慣性空間的姿態(tài)角；信號(hào)采集單元包括陀螺信號(hào)采集電路、編碼器信號(hào)采集電路、加計(jì)信號(hào)采集電路以及POS信號(hào)采集電路；其中，陀螺信號(hào)采集電路用于采集速率陀螺輸出的電壓信號(hào)，首先將電壓信號(hào)進(jìn)行放大，再經(jīng)過V/I轉(zhuǎn)換，輸出到接口電路的I/V轉(zhuǎn)換電路III ；編碼器信號(hào)采集電路用于采集編碼器輸出的二進(jìn)制差分信號(hào)，首先將差分信號(hào)轉(zhuǎn)化為單路信號(hào)，再經(jīng)過高速光耦隔離，輸出到接口電路的數(shù)據(jù)選擇電路；加計(jì)信號(hào)采集電路用于采集加速度計(jì)輸出的電流信號(hào)，首先將電流信號(hào)進(jìn)行放大，再經(jīng)過RC濾波，輸出到接口電路的I/V轉(zhuǎn)換電路I ；POS信號(hào)采集電路將POS輸出的串行二進(jìn)制數(shù)據(jù)經(jīng)過高速光耦隔離、TVS浪涌防護(hù)輸出到接口電路的RS232電平轉(zhuǎn)換電路；接口電路包括I/V轉(zhuǎn)換電路I、I/V轉(zhuǎn)換電路II、I/V轉(zhuǎn)換電路III、數(shù)據(jù)選擇電路、模數(shù)轉(zhuǎn)換電路、RS232電平轉(zhuǎn)換電路以及電平轉(zhuǎn)換芯片I。其中，I/V轉(zhuǎn)換電路I輸入為加計(jì)信號(hào)采集電路，將加計(jì)電流信號(hào)轉(zhuǎn)化為電壓信號(hào)，并疊加偏置電壓，輸出到模數(shù)轉(zhuǎn)換電路；I/V轉(zhuǎn)換電路II輸入為V/I轉(zhuǎn)換電路，將線圈電流信號(hào)轉(zhuǎn)化為電壓信號(hào)，并疊加偏置電壓，輸出到DSP控制單元的內(nèi)部ADC寄存器，供DSP讀??；I/V轉(zhuǎn)換電路III輸入為陀螺信號(hào)采集電路，將陀螺電流信號(hào)轉(zhuǎn)化為電壓信號(hào)，并疊加偏置電壓，輸出到DSP控制單元的內(nèi)部 ADC寄存器，供DSP讀取；數(shù)據(jù)選擇電路輸入為編碼器信號(hào)采集電路，進(jìn)行編碼器通道的選擇以及時(shí)鐘、數(shù)據(jù)信號(hào)的傳輸，輸出到電平轉(zhuǎn)換芯片I ；模數(shù)轉(zhuǎn)換電路輸入為ΙΛ轉(zhuǎn)換電路 I，將電壓信號(hào)轉(zhuǎn)化為數(shù)字量，輸出到電平轉(zhuǎn)換芯片I ；RS232電平轉(zhuǎn)換電路輸入為POS信號(hào)采集電路，將232電平信號(hào)轉(zhuǎn)化為TTL電平信號(hào)，輸出到DSP控制單元的內(nèi)部UART寄存器， 供DSP讀??；電平轉(zhuǎn)換芯片I輸入為數(shù)據(jù)選擇電路、模數(shù)轉(zhuǎn)換電路，將5V電壓轉(zhuǎn)換為3. 3V 電壓，輸出到DSP控制單元的內(nèi)部寄存器；DSP控制單元通過內(nèi)部寄存器讀取各傳感器數(shù)據(jù)；執(zhí)行控制算法；生成PWM信號(hào)控制電機(jī)動(dòng)作；所述DSP控制單元包括DSP和控制算法；所述控制算法實(shí)現(xiàn)自主模式、手動(dòng)模式與組合模式三種工作模式，所述三個(gè)模式由三相開關(guān)進(jìn)行手動(dòng)選擇；在自主模式下，橫滾框和俯仰框保持慣性穩(wěn)定，方位框無控制；控制方式為三環(huán)控制，即位置環(huán)、速率環(huán)、電流環(huán)；橫滾框和俯仰框的控制如下首先DSP讀取MCBSP寄存器中寄存的實(shí)時(shí)采集到的加速度計(jì)的信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與ADC寄存器中寄存的實(shí)時(shí)采集到的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC 寄存器中寄存的電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元驅(qū)動(dòng)電機(jī)動(dòng)作，無需換向控制；在手動(dòng)模式下，橫滾框和俯仰框在手動(dòng)按鈕控制下以指定速率旋轉(zhuǎn)，方位框無控制；DSP通過監(jiān)測(cè)橫滾框和俯仰框的正轉(zhuǎn)和反轉(zhuǎn)四個(gè)手動(dòng)按鈕對(duì)這兩框進(jìn)行獨(dú)立控制；控制方式為雙環(huán)控制，即速率環(huán)和電流環(huán)；橫滾框或俯仰框控制如下首先DSP將轉(zhuǎn)速率設(shè)定值與ADC寄存器中寄存的實(shí)時(shí)采集到的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM 功率驅(qū)動(dòng)單元橫滾或俯仰驅(qū)動(dòng)電機(jī)以指定速率旋轉(zhuǎn)，無需換向控制；在組合模式下，橫滾框、俯仰框和方位框依據(jù)姿態(tài)設(shè)定值都保持慣性穩(wěn)定狀態(tài)，控制方式為三環(huán)控制，即位置環(huán)、速率環(huán)、電流環(huán)；對(duì)于橫滾框或俯仰框控制分別為首先DSP 讀取UART寄存器中寄存的實(shí)時(shí)采集到的POS橫滾或俯仰姿態(tài)信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與ADC寄存器中寄存的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元驅(qū)動(dòng)橫滾電機(jī)或俯仰電機(jī)動(dòng)作，無需換向控制；對(duì)于方位框，姿態(tài)的設(shè)定值并不恒定，需要根據(jù)航跡進(jìn)行自主設(shè)定，然后再進(jìn)行三環(huán)控制，控制為首先DSP將定時(shí)寄存器寄存的自主設(shè)定的方位姿態(tài)的設(shè)定值與UART寄存器中寄存的POS反饋得到的方位姿態(tài)值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與ADC寄存器中寄存的實(shí)時(shí)采集的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量， 完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元驅(qū)動(dòng)方位電機(jī)動(dòng)作，無需換向控制。所述方位框的姿態(tài)根據(jù)航跡進(jìn)行自主設(shè)定過程如下首先DSP讀取UART寄存器中寄存的POS經(jīng)緯度以及方位姿態(tài)信息，將經(jīng)緯度信息與航攝區(qū)經(jīng)緯度信息進(jìn)行匹配判斷是否進(jìn)入攝區(qū)，如果不匹配，則將方位姿態(tài)的設(shè)定值設(shè)定為當(dāng)前飛機(jī)機(jī)頭姿態(tài)方向或者飛機(jī)飛行速度方向，如果匹配，則將方位姿態(tài)的設(shè)定值設(shè)定為航線姿態(tài)方向，其中，航線姿態(tài)方向根據(jù)航跡事先設(shè)定，而飛機(jī)機(jī)頭方向和飛行速度方向?qū)崟r(shí)在線獲取；DSP通過內(nèi)部定時(shí)器功能來完成上述方位姿態(tài)設(shè)定過程，此后根據(jù)方位姿態(tài)設(shè)定值便可以進(jìn)行三環(huán)控制。一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制方法，所述控制方法有自主模式、手動(dòng)模式與組合模式三種工作模式，所述三個(gè)模式由三相開關(guān)進(jìn)行手動(dòng)選擇；在控制方式上采用三環(huán)路復(fù)合控制，即位置環(huán)、速率環(huán)及電流環(huán)；最外環(huán)為位置環(huán)，在自主模式下采用加速度計(jì)作為位置反饋元件；在手動(dòng)模式下無位置環(huán)；在組合模式下采用POS作為位置反饋元件；中間環(huán)為速率環(huán)，采用速率陀螺作為速率反饋元件；最內(nèi)環(huán)為電流環(huán)，采用電流傳感器作為電流反饋元件；具體控制如下在自主模式下，橫滾框和俯仰框保持慣性穩(wěn)定，方位框無控制，橫滾框或俯仰框控制過程為首先DSP通過外擴(kuò)模數(shù)轉(zhuǎn)換電路采集加速度計(jì)信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路及接口電路實(shí)時(shí)采集到的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元C3)驅(qū)動(dòng)橫滾或俯仰電機(jī)動(dòng)作，無需換向控制；在手動(dòng)模式下，橫滾框和俯仰框在手動(dòng)按鈕控制下以指定速率旋轉(zhuǎn)，方位框無控制；通過監(jiān)測(cè)橫滾框和俯仰框的正轉(zhuǎn)和反轉(zhuǎn)四個(gè)手動(dòng)按鈕對(duì)這兩框進(jìn)行獨(dú)立控制，首先 DSP將轉(zhuǎn)速率設(shè)定值與陀螺信號(hào)采集電路及接口電路實(shí)時(shí)采集到的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元驅(qū)動(dòng)橫滾或俯仰電機(jī)以指定速率旋轉(zhuǎn)，無需換向控制；在組合模式下，橫滾框、俯仰框和方位框依據(jù)姿態(tài)設(shè)定值都保持慣性穩(wěn)定狀態(tài)，對(duì)于橫滾框和俯仰框，姿態(tài)設(shè)定值均為恒定值，首先DSP通過RS232電平轉(zhuǎn)換電路采集POS橫滾或俯仰姿態(tài)信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路及接口電路實(shí)時(shí)采集到的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元驅(qū)動(dòng)橫滾電機(jī)或俯仰電機(jī)動(dòng)作，無需換向控制；對(duì)于方位框，姿態(tài)設(shè)定值并不恒定，而是需要根據(jù)航跡進(jìn)行自主設(shè)定，然后再進(jìn)行三環(huán)控制，實(shí)現(xiàn)為首先DSP 將該方位姿態(tài)設(shè)定值與POS反饋得到的方位姿態(tài)值作差，通過PID控制器生成速率設(shè)定值， 完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路以及接口電路實(shí)時(shí)采集到的速率陀螺信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元驅(qū)動(dòng)方位電機(jī)動(dòng)作，無需換向控制。本發(fā)明的工作原理本發(fā)明旨在實(shí)現(xiàn)三軸慣性穩(wěn)定平臺(tái)水平方向(橫滾、俯仰兩個(gè)方向)始終保持對(duì)地垂直指向，航向能夠跟蹤指令變化保持方位不變?？刂扑惴▽?shí)現(xiàn)自主模式、手動(dòng)模式與組合模式三種工作模式，所述三個(gè)模式由三相開關(guān)進(jìn)行手動(dòng)選擇。本發(fā)明控制方式采用三環(huán)路(電流環(huán)、速率環(huán)、位置環(huán))復(fù)合控制方案，如圖2所示為基于DSP 的單框架三環(huán)復(fù)合控制算法框圖，控制器僅采用DSP方案而摒棄DSP+FPGA方案，以節(jié)約成本降低功耗。其中，位置環(huán)以POS或者加速度計(jì)為負(fù)反饋，速率環(huán)以速率陀螺為負(fù)反饋，電流環(huán)以電流傳感器為負(fù)反饋。本發(fā)明正是通過DSP實(shí)現(xiàn)了這三個(gè)環(huán)路并完善控制算法，完成了控制系統(tǒng)高精度、大負(fù)載能力并提高了實(shí)際操作的便捷性。下面針對(duì)這三個(gè)環(huán)路進(jìn)行詳細(xì)介紹。位置環(huán)本發(fā)明通過DSP內(nèi)部定時(shí)器功能實(shí)現(xiàn)了姿態(tài)的自主設(shè)定，完善了系統(tǒng)工作模式手動(dòng)模式、自主模式和組合模式。根據(jù)圖1所示，手動(dòng)模式無位置環(huán)。組合工作模式下，POS測(cè)量出的框架姿態(tài)角信息經(jīng)過采集單元轉(zhuǎn)化為TTL電平讀入到DSP內(nèi)部UART寄存器，DSP將姿態(tài)設(shè)定值與其作差，經(jīng)過位置補(bǔ)償器算法生成速率設(shè)定值，完成位置閉環(huán)。同樣，獨(dú)立工作模式下，加計(jì)信號(hào)經(jīng)過采集單元輸出到I/V轉(zhuǎn)換電路，經(jīng)過高精度模數(shù)轉(zhuǎn)換化模擬量為數(shù)字量，再經(jīng)過電平轉(zhuǎn)換芯片化5V為3. 3V電壓，輸出到DSP內(nèi)部MCBSP寄存器， DSP將姿態(tài)設(shè)定值與其作差，再經(jīng)過位置補(bǔ)償器算法生成速率設(shè)定值，完成位置閉環(huán)。速率環(huán)速率陀螺采用精度較高的光纖速率陀螺VGA941而摒棄精度較低的MEMS 速率陀螺，它可以快速敏感框架角速率，根據(jù)圖1所示，其角速率信息經(jīng)過采集單元化電壓信號(hào)為電流信號(hào)，加強(qiáng)了信號(hào)平滑性和抗干擾能力，再經(jīng)過I/V轉(zhuǎn)換后輸出到DSP內(nèi)部ADC 寄存器，DSP將速率設(shè)定值與其作差，經(jīng)過速率補(bǔ)償器算法后生成電流設(shè)定值，完成速率閉環(huán)。速率環(huán)提高了系統(tǒng)的響應(yīng)速度和抗外界干擾能力，對(duì)控制精度的提高有很大作用。電流環(huán)本發(fā)明采用電流環(huán)作為最內(nèi)環(huán)，可以抑制電源電壓、負(fù)載波動(dòng)對(duì)系統(tǒng)造成的影響以及可以對(duì)電流進(jìn)行限幅，同時(shí)提高電機(jī)轉(zhuǎn)矩線性度并抑制其脈動(dòng)，進(jìn)一步提高系統(tǒng)控制精度；在大力矩電機(jī)的配合下，電流環(huán)控制可以提高力矩剛度和帶載能力。根據(jù)圖1 所示，電流傳感器測(cè)量出電機(jī)線圈電流信息經(jīng)過v/ 轉(zhuǎn)換后輸出到I/V轉(zhuǎn)換電路，供DSP內(nèi)部ADC寄存器讀取，DSP將電流設(shè)定值與其作差，經(jīng)過電流補(bǔ)償器算法后生成PWM控制量， 完成電流閉環(huán)。PWM信號(hào)經(jīng)過電平轉(zhuǎn)換芯片輸出到PWM功率驅(qū)動(dòng)單元，經(jīng)過高速光耦隔離、 邏輯取反、MOSFET功率驅(qū)動(dòng)、H全橋換能電路后，輸出到直流力矩電機(jī)兩端，控制電機(jī)帶動(dòng)穩(wěn)定平臺(tái)旋轉(zhuǎn)。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于(1)本發(fā)明采用三環(huán)路復(fù)合控制方案，即位置環(huán)、速率環(huán)與電流環(huán)，位置環(huán)采用加
8速度計(jì)或者POS作為反饋元件，速率環(huán)采用速率陀螺作為反饋元件，電流環(huán)采用電流傳感 器作為反饋元件。其中，電流環(huán)為最內(nèi)環(huán)，以電機(jī)線圈電流信號(hào)為負(fù)反饋，將電源電壓、負(fù)載 波動(dòng)等因素全部包含在電流環(huán)內(nèi)，從而抑制其對(duì)系統(tǒng)的影響；同吋，通過給電流設(shè)定值進(jìn)行 限幅可以解決電流過大的問題；另外，直流力矩電機(jī)輸出轉(zhuǎn)矩與電機(jī)電流成正比，電流環(huán)可 以實(shí)現(xiàn)轉(zhuǎn)矩控制，對(duì)轉(zhuǎn)矩的脈動(dòng)有很好的抑制能力。(2)本發(fā)明提高了系統(tǒng)的控制精度和帶載能力。本發(fā)明作為速率環(huán)路反饋元件，同 時(shí)增加了陀螺信號(hào)采集以及接ロ電路，加強(qiáng)了信號(hào)抗干擾能力，從而提高了速率敏感精度 和平滑性，進(jìn)ー步提高了響應(yīng)速度和控制精度；另外，本發(fā)明的電流環(huán)提高了轉(zhuǎn)矩控制線性 度并加大了系統(tǒng)的カ矩剛度，對(duì)于系統(tǒng)抗干擾能力和控制精度都有較大提高。同吋，本發(fā)明 采用可輸出大力矩的直流力矩電機(jī)作為執(zhí)行元件，在本發(fā)明配合下可以適當(dāng)發(fā)揮出最大能 力，所以帶載能力有較大幅度提高。(3)本發(fā)明具有三種工作模式，増加了手動(dòng)模式，可以在不影響系統(tǒng)正常工作情況 下，實(shí)現(xiàn)平臺(tái)各框架的手動(dòng)控制；因?yàn)楹綔y(cè)過程中，經(jīng)常需要人為的轉(zhuǎn)動(dòng)載荷到相應(yīng)位置， 所以必須給平臺(tái)増加手控功能，可以實(shí)現(xiàn)三軸姿態(tài)的任意手動(dòng)控制。同吋，本發(fā)明實(shí)現(xiàn)了組 合模式下無需航向姿態(tài)指令可自主設(shè)定的功能，便于實(shí)際操作的便捷性。因?yàn)楹綔y(cè)時(shí)間很 短，來不及接收指令，而且飛機(jī)上人為操作有限，所以必須自主完成姿態(tài)跟蹤任務(wù)。(4)本發(fā)明采用一片DSP即實(shí)現(xiàn)了傳感器數(shù)據(jù)采集、控制算法執(zhí)行與PWM信號(hào)生 成，節(jié)約成本(FPGA以及外圍電路較昂貴)，降低功耗(FPGA功耗較大)。而且本發(fā)明控制 系統(tǒng)通過DSP集成的寄存器中斷功能來完成各子程序的執(zhí)行，通過中斷優(yōu)先級(jí)寄存器進(jìn)行 排序，保證各任務(wù)互不干擾并實(shí)現(xiàn)時(shí)鐘同歩，程序執(zhí)行過程充分占用總線資源，空閑時(shí)仍處 于系統(tǒng)監(jiān)測(cè)狀態(tài)，保證系統(tǒng)運(yùn)行的實(shí)時(shí)性與快速性。


圖1為本發(fā)明控制系統(tǒng)結(jié)構(gòu)框圖；圖2為本發(fā)明的單框架三環(huán)復(fù)合控制算法框圖；圖3為本發(fā)明方法的工作模式實(shí)現(xiàn)框圖；圖4為本發(fā)明的組合模式方位姿態(tài)設(shè)定值自主設(shè)定流程圖；圖5為本發(fā)明的陀螺信號(hào)采集電路圖；圖6為本發(fā)明的編碼器信號(hào)采集電路圖；圖7為本發(fā)明的加計(jì)信號(hào)采集電路圖；圖8為本發(fā)明的POS信號(hào)采集電路圖；圖9為本發(fā)明的I/V轉(zhuǎn)換電路圖；圖10為本發(fā)明的數(shù)據(jù)選擇電路圖；圖11為本發(fā)明的高精度模數(shù)轉(zhuǎn)換電路圖；圖12為本發(fā)明的RS232電平轉(zhuǎn)換電路圖；圖13為本發(fā)明的電流傳感器和V/I轉(zhuǎn)換電路圖；圖14為本發(fā)明的PWM功率驅(qū)動(dòng)單元電路圖；圖15為本發(fā)明的DSP芯片以及外圍電路圖；圖16為本發(fā)明的軟件系統(tǒng)主循環(huán)流程圖17為本發(fā)明的軟件系統(tǒng)定時(shí)器中斷流程圖；圖18為本發(fā)明的軟件系統(tǒng)SPI中斷流程圖。
具體實(shí)施例方式如圖1所示為本發(fā)明的控制系統(tǒng)結(jié)構(gòu)框圖，主要由DSP控制單元1、PWM功率驅(qū)動(dòng)單元3、接口電路20、信號(hào)采集單元15、慣性器件及編碼器10等組成。其中，DSP選擇TI公司浮點(diǎn)型芯片TMS320F28335，它具備32位浮點(diǎn)處理單元、 150MHZ的高速數(shù)據(jù)處理能力、多達(dá)18路的PWM輸出、12位16通道ADC等特性，該系統(tǒng)利用 28335豐富的寄存器資源，與接口電路相連讀取各傳感器數(shù)據(jù)并生成PWM控制量驅(qū)動(dòng)電機(jī)動(dòng)作。在PWM功率驅(qū)動(dòng)單元3中PWM控制量經(jīng)過高速光耦隔離4、邏輯取反5送入到功率驅(qū)動(dòng)單元6以提高輸出功率，作為H橋電機(jī)換能電路7的MOSFET柵極信號(hào)實(shí)現(xiàn)全橋換能控制。如圖1所示，電流傳感器8測(cè)量出電機(jī)線圈電流，連至V/I轉(zhuǎn)換電路9，輸出電流信號(hào)到接口電路20的I/V轉(zhuǎn)換電路2，化電流為電壓信號(hào)，并疊加偏置電壓輸出到DSP控制單元1的內(nèi)部ADC寄存器，作為控制算法中電流環(huán)閉環(huán)負(fù)反饋值。速率陀螺11測(cè)量出框架角速率，輸出到信號(hào)采集單元15的陀螺信號(hào)采集電路16，經(jīng)過電壓放大、V/I轉(zhuǎn)換后輸出到接口電路20的I/V轉(zhuǎn)換電路III，化電流信號(hào)為電壓信號(hào)，并疊加偏置電壓輸出到DSP控制單元1的內(nèi)部ADC寄存器，作為控制算法中速率閉環(huán)負(fù)反饋值。編碼器12測(cè)量出框架相對(duì)轉(zhuǎn)角，輸出到信號(hào)采集單元15的編碼器信號(hào)采集電路17，經(jīng)過差分信號(hào)轉(zhuǎn)單路信號(hào)，輸出到接口電路20的數(shù)據(jù)選擇電路24，進(jìn)行3路編碼器的分時(shí)選通，連至電平轉(zhuǎn)換芯片1，化5V 電壓為3. 3V，輸出到DSP控制單元1的內(nèi)部SPI寄存器，以供讀取。加速度計(jì)13測(cè)量出框架相對(duì)水平系轉(zhuǎn)角，輸出到信號(hào)采集單元15的加計(jì)信號(hào)采集電路18，經(jīng)過電流放大，濾波， 輸出到接口電路20的I/V轉(zhuǎn)換電路1，進(jìn)行電流轉(zhuǎn)電壓，并連至模數(shù)轉(zhuǎn)換電路25，化模型量為數(shù)字量，然后連至電平轉(zhuǎn)換芯片I，化5V電壓為3. 3V，最后輸出到DSP控制單元1的內(nèi)部 MCBSP寄存器，作為控制算法中獨(dú)立工作模式下位置閉環(huán)負(fù)反饋值。P0S14測(cè)量出框架姿態(tài)角，輸出到信號(hào)采集單元15的POS信號(hào)采集電路19，經(jīng)過高速光耦隔離、TVS浪涌防護(hù)，輸出到接口電路20的RS232電平轉(zhuǎn)換電路26，將232電平轉(zhuǎn)化為TTL電平，輸出到DSP控制單元1的內(nèi)部UART寄存器，作為控制算法中組合工作模式下位置閉環(huán)負(fù)反饋值。如圖2所示，本發(fā)明的DSP控制單元1 完成以下三方面任務(wù)通過內(nèi)部寄存器讀取各傳感器數(shù)據(jù)；執(zhí)行控制算法；生成PWM信號(hào)控制電機(jī)動(dòng)作；這些都由DSP獨(dú)立完成，無需FPGA的參與。讀取各傳感器數(shù)據(jù)DSP通過內(nèi)部ADC寄存器完成電機(jī)線圈電流的高速采樣，電流傳感器8串聯(lián)在電機(jī)兩端，輸出正比于線圈電流的電壓信號(hào)，通過V/I轉(zhuǎn)換9化電壓為電流信號(hào)進(jìn)行傳輸，再通過I/V轉(zhuǎn)換1122連接到DSP的ADC輸入引腳，供DSP讀??；DSP通過內(nèi)部SPI寄存器與數(shù)據(jù)選擇器結(jié)合完成三路編碼器信號(hào)的高速分時(shí)采樣，無需FPGA參與，指令發(fā)送與數(shù)據(jù)接收一體化進(jìn)行，保證了時(shí)鐘的同步；編碼器12通過信號(hào)采集電路17輸出至數(shù)據(jù)選擇器24，并通過電平轉(zhuǎn)換芯片I 27連接到DSP的SPI寄存器，供DSP讀取；DSP并不直接利用內(nèi)部ADC寄存器讀取加速度計(jì)數(shù)據(jù)，而是外擴(kuò)位數(shù)更高的模數(shù)轉(zhuǎn)換芯片來完成加計(jì)數(shù)據(jù)的高精準(zhǔn)采樣；加速度計(jì)13通過信號(hào)采集電路18輸出至I/V轉(zhuǎn)換電路I 21與模數(shù)轉(zhuǎn)換電路25相連，再通過電平轉(zhuǎn)換芯片I 27連接到DSP的MCBSP寄存器，供DSP讀取。如圖3所示，DSP在控制算法上實(shí)現(xiàn)了以下工作模式自主模式、手動(dòng)模式與組合模式，這三個(gè)模式由三相開關(guān)進(jìn)行手動(dòng)選擇，狀態(tài)1為自主模式，狀態(tài)2為手動(dòng)模式，狀態(tài)3 為組合模式；在控制方式上采用三環(huán)路復(fù)合控制(如圖2所示)，即位置環(huán)、速率環(huán)及電流環(huán)。最外環(huán)為位置環(huán)，在自主模式下采用加速度計(jì)13作為位 置反饋元件，在手動(dòng)模式下無位置環(huán)，在組合模式下采用P0S14作為位置反饋元件；中間環(huán)為速率環(huán)，采用速率陀螺11作為速率反饋元件；最內(nèi)環(huán)為電流環(huán)，采用電流傳感器8作為電流反饋元件；如圖3所示，在自主模式下，橫滾框和俯仰框保持慣性穩(wěn)定，方位框無控制，具體實(shí)現(xiàn)方式為三環(huán)控制，即位置環(huán)、速率環(huán)、電流環(huán)。圖3以橫滾框?yàn)槔M(jìn)行說明，橫滾框姿態(tài)設(shè)定值為0，首先DSP通過外擴(kuò)模數(shù)轉(zhuǎn)換電路25采集橫滾的加速度計(jì)13信息，將其與0值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路16及接口電路20實(shí)時(shí)采集到的橫滾的速率陀螺11信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器8實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元3驅(qū)動(dòng)橫滾電機(jī)動(dòng)作，無需換向控制。同理，俯仰框也是上述同樣的控制。在手動(dòng)模式下，橫滾框和俯仰框在手動(dòng)按鈕控制下以指定速率旋轉(zhuǎn)，方位框無控制。DSP通過監(jiān)測(cè)四個(gè)手動(dòng)按鈕對(duì)這兩框進(jìn)行獨(dú)立控制，具體實(shí)現(xiàn)方式為雙環(huán)控制，即速率環(huán)和電流環(huán)，圖3以橫滾框正轉(zhuǎn)為例進(jìn)行說明。首先DSP將橫滾框正轉(zhuǎn)速率設(shè)定值與陀螺信號(hào)采集電路(16)及接口電路20實(shí)時(shí)采集到的速率陀螺11信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器8實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元3驅(qū)動(dòng)橫滾電機(jī)以指定速率旋轉(zhuǎn)，無需換向控制；在組合模式下，橫滾框、俯仰框和方位框依據(jù)姿態(tài)設(shè)定值都保持慣性穩(wěn)定狀態(tài)，具體實(shí)現(xiàn)方式為三環(huán)控制，即位置環(huán)、速率環(huán)、電流環(huán)。對(duì)于橫滾框和俯仰框，姿態(tài)設(shè)定值均為 0，首先DSP通過RS232電平轉(zhuǎn)換電路26采集P0S14橫滾和俯仰姿態(tài)信息，將其與0值作差， 通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路 16及接口電路20實(shí)時(shí)采集到的速率陀螺11信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器8實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元3驅(qū)動(dòng)橫滾電機(jī)和俯仰電機(jī)動(dòng)作，無需換向控制。對(duì)于方位框，姿態(tài)設(shè)定值并不恒定，而是需要根據(jù)航跡進(jìn)行自主設(shè)定，如圖4所示。首先DSP通過RS232電平轉(zhuǎn)換電路26采集P0S14經(jīng)緯度以及方位姿態(tài)信息，將經(jīng)緯度信息與航攝區(qū)經(jīng)緯度信息進(jìn)行匹配判斷是否進(jìn)入攝區(qū)，如果不匹配，則將方位姿態(tài)設(shè)定值設(shè)定為當(dāng)前飛機(jī)機(jī)頭姿態(tài)方向或者飛機(jī)飛行速度方向，如果匹配，則將方位姿態(tài)設(shè)定值設(shè)定為航線姿態(tài)方向，其中，航線姿態(tài)方向根據(jù)航跡事先設(shè)定， 而飛機(jī)機(jī)頭方向和飛行速度方向?qū)崟r(shí)在線獲取；DSP通過內(nèi)部定時(shí)器功能來完成上述方位姿態(tài)設(shè)定過程，此后根據(jù)方位姿態(tài)設(shè)定值便可以進(jìn)行三環(huán)控制。如圖3所示，首先DSP將該方位姿態(tài)設(shè)定值與P0S14反饋得到的方位姿態(tài)值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路16以及接口電路20實(shí)時(shí)采集到的速率陀螺11信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器8實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元3驅(qū)動(dòng)方位電機(jī)動(dòng)作，無需換向控制。如圖5所示為本發(fā)明的陀螺信號(hào)采集電路16。速率陀螺11采用光纖速率陀螺 VGA941，連接方式如圖P2所示，輸入+5V電源、地，輸出電壓Gyro_out、Gy ro_Return經(jīng)過 AD620差動(dòng)放大器放大10倍左右，輸出端通過電容濾波后進(jìn)入AD0P07高精密運(yùn)放進(jìn)行V/I 轉(zhuǎn)換。該采集電路輸出端為電流信號(hào)Gyro+、Gyro-，連至接口電路的I/V轉(zhuǎn)換電路3。如圖6所示為本發(fā)明的編碼器信號(hào)采集電路17。編碼器12采用絕對(duì)式光電碼盤， 連接方式如圖Pl所示，輸入+5V電源、地以及差分時(shí)鐘信號(hào)Encoder_clk+、Encoder_clk-， 輸出差分?jǐn)?shù)據(jù)信號(hào)EnCOder_data+、Encoder_data-0差分時(shí)鐘信號(hào)與差分?jǐn)?shù)據(jù)信號(hào)通過圖示的MAX490芯片轉(zhuǎn)化為單路的時(shí)鐘信號(hào)與數(shù)據(jù)信號(hào)Encoder_clk、Encoder_data。同時(shí)差分信號(hào)之間有120歐姆電阻相連以避免信號(hào)串?dāng)_。該采集電路輸出端為二進(jìn)制信號(hào) Encoder_clk、Encoder_data，分別連至接口電路的電平轉(zhuǎn)換芯片1和數(shù)據(jù)選擇電路24。如圖7所示為本發(fā)明的加計(jì)信號(hào)采集電路18。加速度計(jì)13采用石英撓性加速度計(jì)JN-06M，連接方式如圖Pl所示，輸入士 15V電源、地，輸出電流信號(hào)Acc_0ut。該電流信號(hào)輸入到0P284組成的電流放大電路放大7倍左右，電容Cl、C3用于濾波。該采集電路輸出端為電流信號(hào)ACC+、ACC-，連至接口電路的I/V轉(zhuǎn)換電路1。如圖8所示為本發(fā)明的POS信號(hào)采集電路19。POS輸出的二進(jìn)制信息連至圖6的 P0S_DATA端，光耦采用集成芯片HCPL0720，延遲時(shí)間僅有40ns，輸出端為0PT0_DATA，并力 TVS 二極管進(jìn)行浪涌防護(hù)。該采集電路輸出端為0PT0_DATA，連至接口電路的RS232電平轉(zhuǎn)換電路26。如圖9所示為本發(fā)明的I/V轉(zhuǎn)換電路。如圖1所示接口電路20包括三組I/V轉(zhuǎn)換電路Ι/ν轉(zhuǎn)換電路I、I/V轉(zhuǎn)換電路II、I/V轉(zhuǎn)換電路III。它們均采用高精密差動(dòng)運(yùn)放 AD620進(jìn)行電流轉(zhuǎn)電壓，電流信號(hào)經(jīng)過圖中的輸入正流入，從輸入負(fù)流出，經(jīng)過電阻R2轉(zhuǎn)化為電壓信號(hào)，放大倍數(shù)取決于電阻R1。同時(shí)將雙向信號(hào)通過疊加基準(zhǔn)電壓轉(zhuǎn)化為單向信號(hào)， I/V轉(zhuǎn)換電路1的基準(zhǔn)電壓為2. 048V, I/V轉(zhuǎn)換電路II和I/V轉(zhuǎn)換電路III的基準(zhǔn)電壓為 1.024V。I/V轉(zhuǎn)換電路1輸入為加計(jì)信號(hào)采集電路18，輸出連至模數(shù)轉(zhuǎn)換電路25 ；I/V轉(zhuǎn)換電路2輸入為V/I轉(zhuǎn)換電路9，輸出連至DSP控制單元1的ADC寄存器；I/V轉(zhuǎn)換電路III 的輸入為陀螺信號(hào)采集電路16，輸出連至DSP控制單元1的內(nèi)部ADC寄存器。如圖10所示為本發(fā)明的數(shù)據(jù)選擇電路24。數(shù)據(jù)選擇器采用8選1選擇器74151， 使能端為￡，通過三個(gè)控制端SO、Si、S2進(jìn)行8路信號(hào)IO 17的選通，輸出為Z和z。數(shù)據(jù)選擇電路24的輸入為編碼器信號(hào)采集電路17的EnCOder_data引腳，三路編碼器信號(hào)分別連至74151的10、II、12，輸出Encoder_DATAOUT和使能端Encoder_CS#連至電平轉(zhuǎn)換芯片 1。電平轉(zhuǎn)換芯片1化5V電壓為3. 3V，連至DSP控制單元1的內(nèi)部SPI寄存器。如圖11所示為本發(fā)明的模數(shù)轉(zhuǎn)換電路25。模數(shù)轉(zhuǎn)換器采用16位模數(shù)轉(zhuǎn)換芯片 ADS8345，它有8路模擬輸入CH0 CH7，串行二進(jìn)制端有5路信號(hào)SCLK_8345、CS#_8345、 DIN_8345、BUSY_8345、D0UT_8345。它的輸入為I/V轉(zhuǎn)換電路I輸出的電壓信號(hào)，分別連至 ADS8345的CHO CH3端口，輸出連至電平轉(zhuǎn)換芯片I。電平轉(zhuǎn)換芯片I化5V電壓為3. 3V, 連至DSP控制單元1的內(nèi)部MCBSP寄存器。如圖12所示為本發(fā)明的RS232電平轉(zhuǎn)換電路26。采用集成芯片MAX3232進(jìn)行232電平轉(zhuǎn)TTL電平。它的輸入為POS信號(hào)采集電路19，連至MAX232的第8引腳，輸出為DSP_ SCIRXDB，連至DSP控制單元1的內(nèi)部UART寄存器。 如圖13所示為本發(fā)明的電流傳感器8和V/I轉(zhuǎn)換電路9。電流傳感器采用集成霍爾電流傳感器ACS712，它具有上升時(shí)間快、帶寬高、體積小等優(yōu)勢(shì)。它的電流輸入端I_IN 和I_0UT分別連至H橋電機(jī)換能電路7的電機(jī)輸出端。ACS712輸出等比于電流的電壓值， 連至V/I轉(zhuǎn)換電路9進(jìn)行電壓轉(zhuǎn)電流。V/I轉(zhuǎn)換電路采用AD620進(jìn)行差動(dòng)放大，放大倍數(shù)取決于R13和R14 ；采用高精密運(yùn)放AD0P07進(jìn)行電壓轉(zhuǎn)電流，C45用于電容濾波。V/I轉(zhuǎn)換電路輸出為M_Current+、M_Current-，連至接口電路20的I/V轉(zhuǎn)換電路II。如圖14所示為本發(fā)明的PWM功率驅(qū)動(dòng)單元3。DSP控制單元1通過內(nèi)部PWM寄存器產(chǎn)生雙極性PWM信號(hào)，經(jīng)過電平轉(zhuǎn)換芯片164245后化3. 3V電壓為5V，輸出到該控制電路。經(jīng)過HCPL0720高速光耦隔離、74HC04邏輯取反、HIP2101功率放大后作為高速開關(guān)管 MOSFET的柵極驅(qū)動(dòng)信號(hào)，從而實(shí)現(xiàn)全橋式換能電路的控制。它的輸出端連至電機(jī)兩端，同時(shí)串聯(lián)電流傳感器8。如圖15所示為本發(fā)明的DSP芯片以及外圍電路。它主要包括內(nèi)部寄存器引腳PWM 寄存器、ADC寄存器、定時(shí)器寄存器、SPI寄存器、MCBSP寄存器、UART寄存器；AD輸入端口 ADCAO ADCA7、ADCBO ADCB7 ；晶振、JTAG 端口 ^^、TMS、TDI、TDO、EMUO、EMUl 和復(fù)位弓丨腳電源引腳3. 3VU.8V以及地。如圖16所示為本發(fā)明的軟件系統(tǒng)主循環(huán)流程圖。系統(tǒng)上電后，DSP首先進(jìn)行初始化，初始化結(jié)束后進(jìn)入主循環(huán)。沒有中斷申請(qǐng)時(shí)，DSP讀取鍵盤狀態(tài)，執(zhí)行鍵盤指令有鎖緊申請(qǐng)，DSP將相應(yīng)鎖緊標(biāo)志置位，否則復(fù)位標(biāo)志；DSP根據(jù)狀態(tài)指令確定系統(tǒng)工作模式手動(dòng)控制、遠(yuǎn)程控制、本地調(diào)平。有中斷申請(qǐng)而且DSP不在執(zhí)行中斷程序時(shí)，響應(yīng)該中斷請(qǐng)求；當(dāng)有中斷請(qǐng)求而且DSP正在執(zhí)行中斷程序時(shí)，等待中斷執(zhí)行結(jié)束，DSP再予以響應(yīng)。如圖17所示為本發(fā)明的軟件系統(tǒng)定時(shí)器中斷流程圖。本中斷子程序通過Ims定時(shí)器和不同的計(jì)數(shù)值來實(shí)現(xiàn)2ms、IOms和IOOms的定時(shí)功能。DSP初始化結(jié)束后，定時(shí)器中斷使能開啟，當(dāng)Ims定時(shí)結(jié)束，而且此時(shí)DSP不在執(zhí)行其它中斷函數(shù)時(shí)，進(jìn)入定時(shí)器中斷子程序。首先分別給2ms、IOms和IOOms計(jì)數(shù)值增1，然后各自判斷定時(shí)是否結(jié)束，若未結(jié)束則下一次進(jìn)入中斷時(shí)繼續(xù)增1判斷，若結(jié)束則執(zhí)行相應(yīng)子程序，并清零相應(yīng)定時(shí)計(jì)數(shù)值。結(jié)束中斷前，首先復(fù)位中斷標(biāo)志。其中，2ms定時(shí)子程序完成碼盤數(shù)據(jù)采樣，IOms定時(shí)子程序完成加計(jì)數(shù)據(jù)采樣和姿態(tài)自主設(shè)定以實(shí)現(xiàn)位置閉環(huán)，IOOms定時(shí)子程序完成數(shù)據(jù)的發(fā)送。如圖18所示為本發(fā)明的軟件系統(tǒng)SPI中斷流程圖。本中斷子程序用于讀取編碼器數(shù)據(jù)。DSP初始化結(jié)束后，SPI中斷使能開啟。如圖15所示定時(shí)器中斷每2ms向編碼器發(fā)送控制字和時(shí)鐘信號(hào)供編碼器進(jìn)行數(shù)據(jù)轉(zhuǎn)化，轉(zhuǎn)化順序?yàn)橄萖軸編碼器、后Y軸編碼器， 再Z軸編碼器。當(dāng)SPI中斷標(biāo)志置位，而且DSP不在執(zhí)行其它中斷函數(shù)時(shí)，進(jìn)入SPI中斷子程序。首先給中斷計(jì)數(shù)值增1，然后判斷該計(jì)數(shù)值如果等于1，則開啟X軸通道，讀取X軸編碼器數(shù)據(jù)，之后復(fù)位中斷標(biāo)志，中斷結(jié)束；如果等于2，則開啟Y軸通道，讀取Y軸編碼器數(shù)據(jù)，之后復(fù)位中斷標(biāo)志，中斷結(jié)束；如果等于3，則開啟Z軸通道，讀取Z軸編碼器數(shù)據(jù)，之后清零中斷計(jì)數(shù)值，然后復(fù)位中斷標(biāo)志，中斷結(jié)束。
權(quán)利要求
1. 一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)，其特征在于包括慣性器件及編碼器(10)、信號(hào)采集單元(15)、接口電路O0)、DSP控制單元(1)和PWM功率驅(qū)動(dòng)單元 ⑶，其中慣性器件及編碼器(10)包括速率陀螺(11)、編碼器(12)、加速度計(jì)(13)及P0S(14)； 速率陀螺(11)與陀螺信號(hào)采集電路(16)相連，輸出電壓信號(hào)，用于測(cè)量框架相對(duì)于慣性空間的角速率；編碼器(1 與編碼器信號(hào)采集電路(17)相連，輸出差分形式的二進(jìn)制代碼， 用于測(cè)量框架之間的相對(duì)轉(zhuǎn)角；加速度計(jì)(1 與加計(jì)信號(hào)采集電路(18)相連，輸出電流信號(hào)，用于測(cè)量框架相對(duì)于地理水平系的轉(zhuǎn)角；POS (14)與POS信號(hào)采集電路(19)相連，輸出串行二進(jìn)制代碼，用于測(cè)量框架相對(duì)于慣性空間的姿態(tài)角；信號(hào)采集單元(1 包括陀螺信號(hào)采集電路(16)、編碼器信號(hào)采集電路(17)、加計(jì)信號(hào)采集電路(18)、P0S信號(hào)采集電路(19)及電流傳感器(8)；其中，陀螺信號(hào)采集電路(16) 用于采集速率陀螺(11)輸出的電壓信號(hào)，首先將電壓信號(hào)進(jìn)行放大，再經(jīng)過V/I轉(zhuǎn)換，輸出到接口電路00)的I/V轉(zhuǎn)換電路III (23)；編碼器信號(hào)采集電路(17)用于采集編碼器 (12)輸出的二進(jìn)制差分信號(hào)，首先將差分信號(hào)轉(zhuǎn)化為單路信號(hào)，再經(jīng)過高速光耦隔離，輸出到接口電路00)的數(shù)據(jù)選擇電路04)；加計(jì)信號(hào)采集電路(18)用于采集加速度計(jì)(13)輸出的電流信號(hào)，首先將電流信號(hào)進(jìn)行放大，再經(jīng)過RC濾波，輸出到接口電路OO)的I/V轉(zhuǎn)換電路K21) ；POS信號(hào)采集電路(19)將P0S(14)輸出的串行二進(jìn)制數(shù)據(jù)經(jīng)過高速光耦隔離、TVS浪涌防護(hù)輸出到接口電路OO)的RS232電平轉(zhuǎn)換電路06)；電流傳感器(8)串聯(lián)在電機(jī)兩端，輸出正比于線圈電流的電壓信號(hào)，通過V/I轉(zhuǎn)換(9)化電壓為電流信號(hào)；接口電路OO)包括I/V轉(zhuǎn)換電路K21)、I/V轉(zhuǎn)換電路IK22)、I/V轉(zhuǎn)換電路111(23)、 數(shù)據(jù)選擇電路04)、模數(shù)轉(zhuǎn)換電路05)、RS232電平轉(zhuǎn)換電路06)及電平轉(zhuǎn)換芯片I、2Τ)； 其中，I/V轉(zhuǎn)換電路1(21)將加計(jì)信號(hào)采集電路(18)輸出的加計(jì)電流信號(hào)轉(zhuǎn)化為電壓信號(hào)， 并疊加偏置電壓，輸出到模數(shù)轉(zhuǎn)換電路(25)，再通過電平轉(zhuǎn)換芯片I (XT)連接到DSP控制單元(1)內(nèi)部的MCBSP寄存器，供DSP讀??；I/V轉(zhuǎn)換電路II 02)將V/I轉(zhuǎn)換電路(9)輸出的線圈電流信號(hào)轉(zhuǎn)化為電壓信號(hào)，并疊加偏置電壓，輸出到DSP控制單元(1)的內(nèi)部ADC寄存器；I/V轉(zhuǎn)換電路111(23)將陀螺信號(hào)采集電路(16)輸出的陀螺電流信號(hào)轉(zhuǎn)化為電壓信號(hào)，并疊加偏置電壓，輸出到DSP控制單元(1)的內(nèi)部ADC寄存器，供DSP讀?。粩?shù)據(jù)選擇電路04)根據(jù)編碼器信號(hào)采集電路(17)輸出的信號(hào)，進(jìn)行編碼器通道的選擇及時(shí)鐘、數(shù)據(jù)信號(hào)的傳輸，輸出到電平轉(zhuǎn)換芯片K27)；模數(shù)轉(zhuǎn)換電路05)將I/V轉(zhuǎn)換電路K21)輸出的電壓信號(hào)轉(zhuǎn)化為數(shù)字量，輸出到電平轉(zhuǎn)換芯片I 07) ；RS232電平轉(zhuǎn)換電路06)將POS信號(hào)采集電路(19)輸出的232電平信號(hào)轉(zhuǎn)化為TTL電平信號(hào)，輸出到DSP控制單元(1)的內(nèi)部 UART寄存器，供DSP讀?。浑娖睫D(zhuǎn)換芯片I 07)將數(shù)據(jù)選擇電路04)和模數(shù)轉(zhuǎn)換電路05) 輸出的5V電壓轉(zhuǎn)換為3. 3V電壓，輸出到DSP控制單元(1)的內(nèi)部SPI寄存器；DSP控制單元(1)通過內(nèi)部寄存器讀取各傳感器數(shù)據(jù)；執(zhí)行控制算法；生成PWM信號(hào)控制電機(jī)動(dòng)作；所述DSP控制單元(1)包括DSP和控制算法；所述控制算法實(shí)現(xiàn)自主模式、 手動(dòng)模式與組合模式三種工作模式，所述三個(gè)模式由三相開關(guān)進(jìn)行手動(dòng)選擇；在自主模式下，橫滾框和俯仰框保持慣性穩(wěn)定，方位框無控制；控制方式為三環(huán)控制， 即位置環(huán)、速率環(huán)、電流環(huán)；橫滾框和俯仰框的控制如下首先DSP讀取MCB SP寄存器中寄存的實(shí)時(shí)采集到的加速度計(jì)(13)的信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與ADC寄存器中寄存的實(shí)時(shí)采集到的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器⑶實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元(3)驅(qū)動(dòng)電機(jī)動(dòng)作，無需換向控制；在手動(dòng)模式下，橫滾框和俯仰框在手動(dòng)按鈕控制下以指定速率旋轉(zhuǎn)，方位框無控制； DSP通過監(jiān)測(cè)橫滾框和俯仰框的正轉(zhuǎn)和反轉(zhuǎn)四個(gè)手動(dòng)按鈕對(duì)這兩框進(jìn)行獨(dú)立控制；控制方式為雙環(huán)控制，即速率環(huán)和電流環(huán)；橫滾框或俯仰框控制如下首先DSP將轉(zhuǎn)速率設(shè)定值與 ADC寄存器中寄存的實(shí)時(shí)采集到的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器(8)實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到 PWM功率驅(qū)動(dòng)單元C3)橫滾或俯仰驅(qū)動(dòng)電機(jī)以指定速率旋轉(zhuǎn)，無需換向控制；在組合模式下，橫滾框、俯仰框和方位框依據(jù)姿態(tài)設(shè)定值都保持慣性穩(wěn)定狀態(tài)，控制方式為三環(huán)控制，即位置環(huán)、速率環(huán)、電流環(huán)；對(duì)于橫滾框或俯仰框控制分別為首先DSP讀取 UART寄存器中寄存的實(shí)時(shí)采集到的P0S(14)橫滾或俯仰姿態(tài)信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與ADC寄存器中寄存的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器⑶實(shí)時(shí)采集到的電機(jī)電流信息作差，通過 PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元(3)驅(qū)動(dòng)橫滾電機(jī)或俯仰電機(jī)動(dòng)作，無需換向控制；對(duì)于方位框，姿態(tài)的設(shè)定值并不恒定，需要根據(jù)航跡進(jìn)行自主設(shè)定，然后再進(jìn)行三環(huán)控制，控制為首先DSP將定時(shí)寄存器寄存的自主設(shè)定的方位姿態(tài)的設(shè)定值與UART寄存器中寄存的POS (14)反饋得到的方位姿態(tài)值作差，通過PID 控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與ADC寄存器中寄存的實(shí)時(shí)采集的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與ADC寄存器中寄存的電流傳感器⑶實(shí)時(shí)采集到的電機(jī)電流信息作差，通過 PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元(3)驅(qū)動(dòng)方位電機(jī)動(dòng)作，無需換向控制。
2.根據(jù)權(quán)利要求1所述的高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)，其特征在于所述方位框的姿態(tài)根據(jù)航跡進(jìn)行自主設(shè)定過程如下首先DSP讀取UART寄存器中寄存的POS (14)經(jīng)緯度以及方位姿態(tài)信息，將經(jīng)緯度信息與航攝區(qū)經(jīng)緯度信息進(jìn)行匹配判斷是否進(jìn)入攝區(qū)，如果不匹配，則將方位姿態(tài)的設(shè)定值設(shè)定為當(dāng)前飛機(jī)機(jī)頭姿態(tài)方向或者飛機(jī)飛行速度方向，如果匹配，則將方位姿態(tài)的設(shè)定值設(shè)定為航線姿態(tài)方向，其中，航線姿態(tài)方向根據(jù)航跡事先設(shè)定，而飛機(jī)機(jī)頭方向和飛行速度方向?qū)崟r(shí)在線獲??；DSP通過內(nèi)部定時(shí)器功能來完成上述方位姿態(tài)設(shè)定過程，此后根據(jù)方位姿態(tài)設(shè)定值便可以進(jìn)行三環(huán)控制。
3.一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制方法，其特征在于所述控制方法有自主模式、手動(dòng)模式與組合模式三種工作模式，所述三個(gè)模式由三相開關(guān)進(jìn)行手動(dòng)選擇；在控制方式上采用三環(huán)路復(fù)合控制，即位置環(huán)、速率環(huán)及電流環(huán)；最外環(huán)為位置環(huán)，在自主模式下采用加速度計(jì)(1 作為位置反饋元件；在手動(dòng)模式下無位置環(huán)；在組合模式下采用P0S(14)作為位置反饋元件；中間環(huán)為速率環(huán)，采用速率陀螺(11)作為速率反饋元件；最內(nèi)環(huán)為電流環(huán)，采用電流傳感器( 作為電流反饋元件；具體控制如下在自主模式下，橫滾框和俯仰框保持慣性穩(wěn)定，方位框無控制，橫滾框或俯仰框控制過程為首先DSP通過外擴(kuò)模數(shù)轉(zhuǎn)換電路05)采集加速度計(jì)(13)信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路(16)及接口電路OO)實(shí)時(shí)采集到的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器(8)實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元(3)驅(qū)動(dòng)橫滾或俯仰電機(jī)動(dòng)作，無需換向控制；在手動(dòng)模式下，橫滾框和俯仰框在手動(dòng)按鈕控制下以指定速率旋轉(zhuǎn)，方位框無控制；通過監(jiān)測(cè)橫滾框和俯仰框的正轉(zhuǎn)和反轉(zhuǎn)四個(gè)手動(dòng)按鈕對(duì)這兩框進(jìn)行獨(dú)立控制，首先DSP將轉(zhuǎn)速率設(shè)定值與陀螺信號(hào)采集電路(16)及接口電路OO)實(shí)時(shí)采集到的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器⑶實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)； PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元C3)驅(qū)動(dòng)橫滾或俯仰電機(jī)以指定速率旋轉(zhuǎn)，無需換向控制；在組合模式下，橫滾框、俯仰框和方位框依據(jù)姿態(tài)設(shè)定值都保持慣性穩(wěn)定狀態(tài)，對(duì)于橫滾框和俯仰框，姿態(tài)設(shè)定值均為恒定值，首先DSP通過RS232電平轉(zhuǎn)換電路06)采集 POS (14)橫滾或俯仰姿態(tài)信息，將其與姿態(tài)設(shè)定值作差，通過PID控制器生成速率設(shè)定值， 完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路(16)及接口電路OO)實(shí)時(shí)采集到的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器(8)實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM 控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元C3)驅(qū)動(dòng)橫滾電機(jī)或俯仰電機(jī)動(dòng)作，無需換向控制；對(duì)于方位框，姿態(tài)設(shè)定值并不恒定，而是需要根據(jù)航跡進(jìn)行自主設(shè)定，然后再進(jìn)行三環(huán)控制，實(shí)現(xiàn)為首先DSP將該方位姿態(tài)設(shè)定值與POS(14)反饋得到的方位姿態(tài)值作差，通過PID控制器生成速率設(shè)定值，完成位置閉環(huán)；然后將速率設(shè)定值與陀螺信號(hào)采集電路(16)以及接口電路OO)實(shí)時(shí)采集到的速率陀螺(11)信息進(jìn)行作差，通過PID控制器生成電流設(shè)定值，完成速率閉環(huán)；然后將電流設(shè)定值與電流傳感器(8)實(shí)時(shí)采集到的電機(jī)電流信息作差，通過PID控制器生成PWM控制量，完成電流閉環(huán)；PWM信號(hào)輸出到PWM功率驅(qū)動(dòng)單元(3)驅(qū)動(dòng)方位電機(jī)動(dòng)作，無需換向控制。
全文摘要
一種高精度大負(fù)載航空遙感三軸慣性穩(wěn)定平臺(tái)控制系統(tǒng)及方法，包括DSP控制單元、PWM功率驅(qū)動(dòng)單元、接口電路、信號(hào)采集單元、慣性器件及編碼器、直流力矩電機(jī)。該系統(tǒng)通過信號(hào)采集單元獲取各框架相對(duì)于慣性空間的姿態(tài)角和角速率、線圈電流、框架之間的相對(duì)轉(zhuǎn)角，并通過接口電路送入到DSP內(nèi)部寄存器供讀取，DSP將其作為信號(hào)輸入，通過三環(huán)路(電流環(huán)、速率環(huán)及位置環(huán))復(fù)合控制算法生成PWM控制量，PWM信號(hào)經(jīng)功率驅(qū)動(dòng)單元送入到H橋直流電機(jī)換能電路，驅(qū)動(dòng)直流力矩電機(jī)跟隨參考指令的變化，從而實(shí)現(xiàn)此慣性穩(wěn)定平臺(tái)的主動(dòng)控制。本發(fā)明提高了系統(tǒng)控制精度和帶載能力，完善了控制算法，提高了平臺(tái)工作模式的多樣性與智能化。
文檔編號(hào)G05D1/08GK102436262SQ20111028674
公開日2012年5月2日 申請(qǐng)日期2011年9月26日 優(yōu)先權(quán)日2011年9月26日
發(fā)明者房建成, 李樹勝, 趙巖, 鐘麥英 申請(qǐng)人:北京航空航天大學(xué)