基于dsp和fpga的無人直升機(jī)飛行控制系統(tǒng)及方法
【專利摘要】一種基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)及方法，由無人直升機(jī)機(jī)體、機(jī)載舵機(jī)組和無線電接收機(jī)構(gòu)成的模型直升機(jī)分別連接機(jī)載控制平臺(tái)和地面站監(jiān)控平臺(tái)，機(jī)載控制平臺(tái)有機(jī)載下位機(jī)和分別與機(jī)載下位機(jī)相連的姿態(tài)測(cè)量模塊、經(jīng)緯度測(cè)量模塊和機(jī)載上位機(jī)，以及與機(jī)載下位機(jī)相連的氣壓傳感器，地面站監(jiān)控平臺(tái)設(shè)置有與無線電接收機(jī)無線連接的遙控器和與機(jī)載上位機(jī)相連的地面站PC。方法有用于FPGA的信息采集、命令字接收、舵機(jī)驅(qū)動(dòng)和飛行數(shù)據(jù)上傳，用于DSP的姿態(tài)、經(jīng)緯度和高度，轉(zhuǎn)發(fā)飛行數(shù)據(jù)和命令字，實(shí)現(xiàn)飛行數(shù)據(jù)存儲(chǔ)，飛行數(shù)據(jù)的接收和顯示，以及向機(jī)載平臺(tái)發(fā)布飛行命令。本發(fā)明使無人直升機(jī)具有了自主飛行能力：自主調(diào)節(jié)姿態(tài)和位置。
【專利說明】基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)及方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種無人直升機(jī)飛行控制系統(tǒng)。特別是涉及一種基于DSP和FPGA的 無人直升機(jī)飛行控制系統(tǒng)及方法。

【背景技術(shù)】
[0002] 與固定翼飛機(jī)相比，直升機(jī)具有起飛著陸場(chǎng)地小、可垂直起降、空中懸停、使用靈 活等一系列優(yōu)點(diǎn)。作為直升機(jī)的一種特例，無人直升機(jī)還具有無人員傷亡、體積小、造價(jià)低、 戰(zhàn)場(chǎng)生存能力高等有人駕駛直升機(jī)無法比擬的優(yōu)越性。它的這些獨(dú)特性，使其在民用和軍 事方面均有著廣泛的應(yīng)用。
[0003] 在民用方面，主要的應(yīng)用領(lǐng)域有：空中交通管制、大地測(cè)量、氣象觀測(cè)、城市環(huán)境控 制、地質(zhì)勘測(cè)、森林防火、人工降雨、超高壓輸電線路巡檢和通信中繼等。
[0004] 在軍事方面，主要應(yīng)用領(lǐng)域有：戰(zhàn)場(chǎng)偵察、通信中繼、通信情報(bào)偵察、軍事測(cè)繪、探 測(cè)戰(zhàn)場(chǎng)上大規(guī)模殺傷性武器（有害物質(zhì)）、與其他空中平臺(tái)協(xié)同作戰(zhàn)、與衛(wèi)星或預(yù)警機(jī)組成 多/雙基地雷達(dá)預(yù)警/偵察系統(tǒng)、信息戰(zhàn)、電子干擾及對(duì)敵防空兵壓制等。
[0005] 無人直升機(jī)的應(yīng)用前提是自主飛行能力。所以，設(shè)計(jì)一個(gè)使無人直升機(jī)具有自主 飛行能力的無人直升機(jī)飛行控制系統(tǒng)是有意義的。


【發(fā)明內(nèi)容】

[0006] 本發(fā)明所要解決的技術(shù)問題是，提供一種能夠增強(qiáng)系統(tǒng)的可靠性和靈活性的基于 DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)及方法。
[0007] 本發(fā)明所采用的技術(shù)方案是：一種基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)， 包括有由無人直升機(jī)機(jī)體、機(jī)載舵機(jī)組和無線電接收機(jī)構(gòu)成的模型直升機(jī)，所述的模型直 升機(jī)分別連接機(jī)載控制平臺(tái)和地面站監(jiān)控平臺(tái)，其中，所述的機(jī)載控制平臺(tái)包括有機(jī)載下 位機(jī)和分別通過RS232接口與所述的機(jī)載下位機(jī)相連的姿態(tài)測(cè)量模塊、經(jīng)緯度測(cè)量模塊和 機(jī)載上位機(jī)，以及通過SPI接口與所述的機(jī)載下位機(jī)相連的氣壓傳感器，所述的地面站監(jiān) 控平臺(tái)設(shè)置有與所述的無線電接收機(jī)無線連接的遙控器和與所述的機(jī)載上位機(jī)相連的地 面站PC。
[0008] 所述的機(jī)載下位機(jī)包括有：現(xiàn)場(chǎng)可編程門陣列模塊，分別與所述的現(xiàn)場(chǎng)可編程門 陣列模塊相連的數(shù)字信號(hào)處理模塊、RS232協(xié)議電平轉(zhuǎn)換模塊、現(xiàn)場(chǎng)可編程門陣列程序存儲(chǔ) 模塊、多路三態(tài)緩沖模塊，其中，所述的現(xiàn)場(chǎng)可編程門陣列模塊通過SPI接口連接所述的氣 壓傳感器，所述的RS232協(xié)議電平轉(zhuǎn)換模塊通過RS232接口分別連接姿態(tài)測(cè)量模塊、經(jīng)緯度 測(cè)量模塊和機(jī)載上位機(jī)，所述的多路三態(tài)緩沖模塊通過PWM輸出通道連接機(jī)載舵機(jī)組和通 過PWM輸入通道連接無線電接收機(jī)。
[0009] 一種基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方法，包括有機(jī)載下位 機(jī)、機(jī)載上位機(jī)和地面站PC三部分的控制方法，其中，所述的機(jī)載下位機(jī)控制方法包括有 用于FPGA的信息采集、命令字接收、舵機(jī)驅(qū)動(dòng)和飛行數(shù)據(jù)上傳的控制方法，以及用于DSP 的姿態(tài)、經(jīng)緯度和高度的控制方法；所述機(jī)載上位機(jī)控制方法包括有轉(zhuǎn)發(fā)飛行數(shù)據(jù)和命令 字，實(shí)現(xiàn)飛行數(shù)據(jù)的存儲(chǔ)；所述的地面站PC控制方法包括飛行數(shù)據(jù)的接收和顯示，以及向 機(jī)載平臺(tái)發(fā)布飛行命令。
[0010] 所述的機(jī)載下位機(jī)控制方法中的信息采集是指：FPGA通過串口采集MU和GPS測(cè) 量的姿態(tài)和經(jīng)緯度，通過SPI接口采集氣壓傳感器測(cè)量的大氣壓和溫度，然后發(fā)起中斷通 知DSP讀取采集信息；所述的機(jī)載下位機(jī)控制方法中的命令字接收是指：FPGA通過串口接 收并處理機(jī)載上位機(jī)轉(zhuǎn)發(fā)的命令字；所述的機(jī)載下位機(jī)控制方法中的舵機(jī)驅(qū)動(dòng)是指：FPGA 根據(jù)飛行模式產(chǎn)生驅(qū)動(dòng)舵機(jī)組的PWM信號(hào)；所述的機(jī)載下位機(jī)控制方法中的飛行數(shù)據(jù)上傳 是指：FPGA通過串口將飛行數(shù)據(jù)上傳給機(jī)載上位機(jī)。
[0011] 所述的通過SPI接口采集氣壓傳感器測(cè)量的大氣壓和溫度，包括如下步驟：
[0012] 1)復(fù)位：FPGA向設(shè)置在無人直升機(jī)上的氣壓傳感器發(fā)送復(fù)位命令，延時(shí)3ms后進(jìn) 入下一步驟；
[0013] 2)校正參數(shù)讀取：FPGA向氣壓傳感器發(fā)送校正參數(shù)讀取命令，然后FPGA讀取氣壓 傳感器中的校正參數(shù)，并存入內(nèi)部寄存器供DSP讀?。?br> [0014] 3)主循環(huán)：FPGA先向氣壓傳感器發(fā)送大氣壓轉(zhuǎn)換命令，延時(shí)9ms后讀取氣壓傳感 器中的大氣壓，并存入內(nèi)部寄存器，再向氣壓傳感器發(fā)送溫度轉(zhuǎn)換命令，延時(shí)9ms后讀取氣 壓傳感器中的溫度，并存入內(nèi)部寄存器，然后，將FPGA大氣壓中斷標(biāo)志位置1，F(xiàn)PGA發(fā)起大 氣壓中斷通知DSP讀取大氣壓測(cè)量值和溫度測(cè)量值；
[0015] 4) FPGA延時(shí)Is的采樣周期后返回步驟3)。
[0016] 所述的命令字接收包括接收飛行模式切換命令字、姿態(tài)給定命令字和位置給定命 令字，具體包括如下步驟：
[0017] 1)接收命令字第一個(gè)字節(jié)；
[0018] 2)判斷所接收字節(jié)的高4位是否都為0,若是，說明正在接收的命令字為飛行模式 切換命令字，將所接收字節(jié)的低4位賦給飛行模式寄存器，上傳飛行模式返回步驟1);若不 是，說明正在接收的命令字為姿態(tài)給定命令字或位置給定命令字，進(jìn)入下一步驟；
[0019] 3)接收命令字后四個(gè)字節(jié)；
[0020] 4)根據(jù)命令字類型將姿態(tài)給定值或者位置給定值存入FPGA內(nèi)特定的寄存器供 DSP讀取，然后返回步驟1)。
[0021] 所述的舵機(jī)驅(qū)動(dòng)是選用FPGA中5路PWM信號(hào)輸出通道的前兩路PWM信號(hào)輸出通 道驅(qū)動(dòng)機(jī)載舵機(jī)組中的總距舵機(jī)和油門舵機(jī)，后三路PWM信號(hào)輸出通道驅(qū)動(dòng)橫滾舵機(jī)、俯 仰舵機(jī)和偏航舵機(jī)，具體包括如下步驟 ：
[0022] 1)將FPGA中的5個(gè)計(jì)數(shù)寄存器和5個(gè)初值寄存器的初始值設(shè)定為1500,重裝載 寄存器和周期寄存器的初始值設(shè)定為〇 ;
[0023] 2)判斷是否經(jīng)過一微秒的計(jì)數(shù)周期，是則將計(jì)數(shù)寄存器中非零的寄存器減1，周 期寄存器加1后進(jìn)入下一步驟，否則直接進(jìn)入下一步驟；
[0024] 3)判斷DSP是否將重裝載寄存器設(shè)為OxFF或者周期寄存器是否大于28000,是則 說明DSP更新了初值寄存器或者本周期超過28 了毫秒，將計(jì)數(shù)寄存器中的計(jì)數(shù)初值裝載至 計(jì)數(shù)寄存器，將重裝載寄存器和周期寄存器設(shè)為〇,產(chǎn)生下個(gè)周期的PWM信號(hào)后進(jìn)入下一 步；否則直接進(jìn)入下一步；
[0025] 4)由飛行模式寄存器判斷飛行模式，飛行模式寄存器等于0為手動(dòng)模式，進(jìn)入步 驟5);飛行模式寄存器等于1為姿態(tài)模式，進(jìn)入步驟6);飛行模式寄存器等于2為位置模 式，進(jìn)入步驟7);
[0026] 5)將與5個(gè)計(jì)數(shù)寄存器相對(duì)應(yīng)的5路PWM信號(hào)輸入通道接收的信號(hào)輸出在同樣與 5個(gè)計(jì)數(shù)寄存器相對(duì)應(yīng)的5路PWM信號(hào)輸出通道上后返回步驟2);
[0027] 6)將前兩路PWM信號(hào)輸入通道接收的信號(hào)輸出在前兩路PWM信號(hào)輸出通道上；5 個(gè)計(jì)數(shù)寄存器中的后三個(gè)計(jì)數(shù)寄存器中若有非零的計(jì)數(shù)寄存器，則與所述的非零的計(jì)數(shù)寄 存器相對(duì)應(yīng)的PWM信號(hào)輸出通道輸出高電平1，否則輸出低電平0后返回步驟2);
[0028] 7) 5個(gè)計(jì)數(shù)寄存器中若有非零的計(jì)數(shù)寄存器，則與所述的非零的計(jì)數(shù)寄存器相對(duì) 應(yīng)的PWM信號(hào)輸出通道輸出高電平1，否則輸出低電平〇后返回步驟2)。
[0029] 所述的DSP的姿態(tài)控制方法包括如下步驟：
[0030] 1)從FPGA讀取姿態(tài)給定值、姿態(tài)測(cè)量值和飛行模式；
[0031] 2)判斷當(dāng)前飛行模式是否為姿態(tài)模式或者位置模式，是則進(jìn)入下一步，否者直接 結(jié)束；
[0032] 3)將橫滾角給定值、俯仰角給定值和偏航角給定值依次與橫滾角測(cè)量值、俯仰角 測(cè)量值和偏航角測(cè)量值相減，得橫滾角偏差roll_error、俯仰角偏差pitch_error和偏航 角偏差 yaw_error ;
[0033] 4)計(jì)算橫滾舵機(jī)控制量增量Λ u_rol 1、俯仰舵機(jī)控制量增量Λ u_pitch和偏航舵 機(jī)控制量增量Au_yaw :
[0034]

【權(quán)利要求】
1. 一種基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)，包括有由無人直升機(jī)機(jī)體（11)、 機(jī)載舵機(jī)組（12)和無線電接收機(jī)（13)構(gòu)成的模型直升機(jī)（1)，其特征在于，所述的模型直 升機(jī)（1)分別連接機(jī)載控制平臺(tái)（2)和地面站監(jiān)控平臺(tái)（3)，其中，所述的機(jī)載控制平臺(tái) (2)包括有機(jī)載下位機(jī)（21)和分別通過RS232接口與所述的機(jī)載下位機(jī)（21)相連的姿態(tài) 測(cè)量模塊（22)、經(jīng)緯度測(cè)量模塊（23)和機(jī)載上位機(jī)（25)，以及通過SPI接口與所述的機(jī)載 下位機(jī)（21)相連的氣壓傳感器（24)，所述的地面站監(jiān)控平臺(tái)（3)設(shè)置有與所述的無線電接 收機(jī)（13)無線連接的遙控器（31)和與所述的機(jī)載上位機(jī)（25)相連的地面站PC(32)。
2. 根據(jù)權(quán)利要求1所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)，其特征在于， 所述的機(jī)載下位機(jī)（21)包括有：現(xiàn)場(chǎng)可編程門陣列模塊（211)，分別與所述的現(xiàn)場(chǎng)可編程 門陣列模塊（211)相連的數(shù)字信號(hào)處理模塊（212)、RS232協(xié)議電平轉(zhuǎn)換模塊（213)、現(xiàn)場(chǎng) 可編程門陣列程序存儲(chǔ)模塊（215)、多路三態(tài)緩沖模塊（218)，其中，所述的現(xiàn)場(chǎng)可編程門 陣列模塊（211)通過SPI接口連接所述的氣壓傳感器（24)，所述的RS232協(xié)議電平轉(zhuǎn)換模 塊（213)通過RS232接口分別連接姿態(tài)測(cè)量模塊（22)、經(jīng)緯度測(cè)量模塊（23)和機(jī)載上位機(jī) (25)，所述的多路三態(tài)緩沖模塊（218)通過PWM輸出通道連接機(jī)載舵機(jī)組（12)和通過PWM 輸入通道連接無線電接收機(jī)（13)。
3. -種用于權(quán)利要求1所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方 法，其特征在于，包括有機(jī)載下位機(jī)、機(jī)載上位機(jī)和地面站PC三部分的控制方法，其中，所 述的機(jī)載下位機(jī)控制方法包括有用于FPGA的信息采集、命令字接收、舵機(jī)驅(qū)動(dòng)和飛行數(shù)據(jù) 上傳的控制方法，以及用于DSP的姿態(tài)、經(jīng)緯度和高度的控制方法；所述機(jī)載上位機(jī)控制方 法包括有轉(zhuǎn)發(fā)飛行數(shù)據(jù)和命令字，實(shí)現(xiàn)飛行數(shù)據(jù)的存儲(chǔ)；所述的地面站PC控制方法包括飛 行數(shù)據(jù)的接收和顯示，以及向機(jī)載平臺(tái)發(fā)布飛行命令。
4. 根據(jù)權(quán)利要求3所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方法， 其特征在于，所述的機(jī)載下位機(jī)控制方法中的信息采集是指：FPGA通過串口采集MU和 GPS測(cè)量的姿態(tài)和經(jīng)緯度，通過SPI接口采集氣壓傳感器測(cè)量的大氣壓和溫度，然后發(fā)起中 斷通知DSP讀取采集信息；所述的機(jī)載下位機(jī)控制方法中的命令字接收是指：FPGA通過串 口接收并處理機(jī)載上位機(jī)轉(zhuǎn)發(fā)的命令字；所述的機(jī)載下位機(jī)控制方法中的舵機(jī)驅(qū)動(dòng)是指： FPGA根據(jù)飛行模式產(chǎn)生驅(qū)動(dòng)舵機(jī)組的PWM信號(hào)；所述的機(jī)載下位機(jī)控制方法中的飛行數(shù)據(jù) 上傳是指：FPGA通過串口將飛行數(shù)據(jù)上傳給機(jī)載上位機(jī)。
5. 根據(jù)權(quán)利要求4所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方法，其 特征在于，所述的通過SPI接口采集氣壓傳感器測(cè)量的大氣壓和溫度，包括如下步驟： 1) 復(fù)位：FPGA向設(shè)置在無人直升機(jī)上的氣壓傳感器發(fā)送復(fù)位命令，延時(shí)3ms后進(jìn)入下 一步驟； 2) 校正參數(shù)讀?。篎PGA向氣壓傳感器發(fā)送校正參數(shù)讀取命令，然后FPGA讀取氣壓傳感 器中的校正參數(shù)，并存入內(nèi)部寄存器供DSP讀??； 3) 主循環(huán)：FPGA先向氣壓傳感器發(fā)送大氣壓轉(zhuǎn)換命令，延時(shí)9ms后讀取氣壓傳感器中 的大氣壓，并存入內(nèi)部寄存器，再向氣壓傳感器發(fā)送溫度轉(zhuǎn)換命令，延時(shí)9ms后讀取氣壓傳 感器中的溫度，并存入內(nèi)部寄存器，然后，將FPGA大氣壓中斷標(biāo)志位置1，F(xiàn)PGA發(fā)起大氣壓 中斷通知DSP讀取大氣壓測(cè)量值和溫度測(cè)量值； 4. FPGA延時(shí)Is的采樣周期后返回步驟3)。
6. 根據(jù)權(quán)利要求3或4所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方 法，其特征在于，所述的命令字接收包括接收飛行模式切換命令字、姿態(tài)給定命令字和位置 給定命令字，具體包括如下步驟： 1) 接收命令字第一個(gè)字節(jié)； 2) 判斷所接收字節(jié)的高4位是否都為0,若是，說明正在接收的命令字為飛行模式切換 命令字，將所接收字節(jié)的低4位賦給飛行模式寄存器，上傳飛行模式返回步驟1);若不是， 說明正在接收的命令字為姿態(tài)給定命令字或位置給定命令字，進(jìn)入下一步驟； 3) 接收命令字后四個(gè)字節(jié)； 4) 根據(jù)命令字類型將姿態(tài)給定值或者位置給定值存入FPGA內(nèi)特定的寄存器供DSP讀 取，然后返回步驟1)。
7. 根據(jù)權(quán)利要求3或4所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方 法，其特征在于，所述的舵機(jī)驅(qū)動(dòng)是選用FPGA中5路PWM信號(hào)輸出通道的前兩路PWM信號(hào) 輸出通道驅(qū)動(dòng)機(jī)載舵機(jī)組中的總距舵機(jī)和油門舵機(jī)，后三路PWM信號(hào)輸出通道驅(qū)動(dòng)橫滾舵 機(jī)、俯仰舵機(jī)和偏航舵機(jī)，具體包括如下步驟： 1) 將FPGA中的5個(gè)計(jì)數(shù)寄存器和5個(gè)初值寄存器的初始值設(shè)定為1500,重裝載寄存 器和周期寄存器的初始值設(shè)定為〇 ; 2) 判斷是否經(jīng)過一微秒的計(jì)數(shù)周期，是則將計(jì)數(shù)寄存器中非零的寄存器減1，周期寄 存器加1后進(jìn)入下一步驟，否則直接進(jìn)入下一步驟； 3) 判斷DSP是否將重裝載寄存器設(shè)為OxFF或者周期寄存器是否大于28000,是則說明 DSP更新了初值寄存器或者本周期超過28 了毫秒，將計(jì)數(shù)寄存器中的計(jì)數(shù)初值裝載至計(jì)數(shù) 寄存器，將重裝載寄存器和周期寄存器設(shè)為〇,產(chǎn)生下個(gè)周期的PWM信號(hào)后進(jìn)入下一步；否 則直接進(jìn)入下一步； 4) 由飛行模式寄存器判斷飛行模式，飛行模式寄存器等于0為手動(dòng)模式，進(jìn)入步驟5); 飛行模式寄存器等于1為姿態(tài)模式，進(jìn)入步驟6);飛行模式寄存器等于2為位置模式，進(jìn)入 步驟7); 5) 將與5個(gè)計(jì)數(shù)寄存器相對(duì)應(yīng)的5路PWM信號(hào)輸入通道接收的信號(hào)輸出在同樣與5個(gè) 計(jì)數(shù)寄存器相對(duì)應(yīng)的5路PWM信號(hào)輸出通道上后返回步驟2); 6) 將前兩路PWM信號(hào)輸入通道接收的信號(hào)輸出在前兩路PWM信號(hào)輸出通道上；5個(gè)計(jì) 數(shù)寄存器中的后三個(gè)計(jì)數(shù)寄存器中若有非零的計(jì)數(shù)寄存器，則與所述的非零的計(jì)數(shù)寄存器 相對(duì)應(yīng)的PWM信號(hào)輸出通道輸出高電平1，否則輸出低電平0后返回步驟2); 7) 5個(gè)計(jì)數(shù)寄存器中若有非零的計(jì)數(shù)寄存器，則與所述的非零的計(jì)數(shù)寄存器相對(duì)應(yīng)的 PWM信號(hào)輸出通道輸出高電平1，否則輸出低電平〇后返回步驟2)。
8. 根據(jù)權(quán)利要求3所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方法，其 特征在于，所述的DSP的姿態(tài)控制方法包括如下步驟： 1) 從FPGA讀取姿態(tài)給定值、姿態(tài)測(cè)量值和飛行模式； 2) 判斷當(dāng)前飛行模式是否為姿態(tài)模式或者位置模式，是則進(jìn)入下一步，否者直接結(jié) 束； 3) 將橫滾角給定值、俯仰角給定值和偏航角給定值依次與橫滾角測(cè)量值、俯仰角測(cè)量 值和偏航角測(cè)量值相減，得橫滾角偏差roll_error、俯仰角偏差pitch_error和偏航角偏 差 yaw_error ; 4) 計(jì)算橫滾舵機(jī)控制量增量Ai^roll、俯仰舵機(jī)控制量增量△u+pitch和偏航舵機(jī)控 制量增量Au_yaw :
其中roll_error(k)、pitch_error(k)和yaw_error(k)是本次姿態(tài)控制周期計(jì)算出 的橫滾角偏差、俯仰角偏差和偏航角偏差，roll_error(k-l)、pitch_error(k-l)和yaw_ error (k-1)是上次姿態(tài)控制周期計(jì)算出的橫滾角偏差、俯仰角偏差和偏航角偏差，ro 1 l_kp 和roll_ki是橫滾角控制的比例增益和積分增益，pitch_kp和pitch_ki是俯仰角控制的 比例增益和積分增益，yaw_kp和yaw_ki是偏航角控制的比例增益和積分增益，它們的取值 通過反復(fù)試驗(yàn)試湊的方式得出，以響應(yīng)速度加快并且無超調(diào)為標(biāo)準(zhǔn)； 5) 累加得橫滾舵機(jī)控制量u_roll、俯仰舵機(jī)控制量u_pitch和偏航舵機(jī)控制量u_ yaw ：
然后對(duì)橫滾舵機(jī)控制量u_roll、俯仰舵機(jī)控制量u_pitch和偏航舵機(jī)控制量u_yaw進(jìn) 行限幅； 6) 將橫滾舵機(jī)控制量、俯仰舵機(jī)控制量和偏航舵機(jī)控制量寫入FPGA內(nèi)部寄存器； 7) 將FPGA內(nèi)部重裝載寄存器置為OxFF，使寫入FPGA的舵機(jī)控制量生效。
9.根據(jù)權(quán)利要求3所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方法，其 特征在于，所述的DSP的經(jīng)緯度控制方法包括如下步驟： 1) 從FPGA讀取經(jīng)緯度給定值、經(jīng)緯度測(cè)量值和飛行模式； 2) 將表示經(jīng)緯度測(cè)量值的字符串解算成對(duì)應(yīng)的浮點(diǎn)數(shù)； 3) 將解算出的浮點(diǎn)數(shù)寫入FPGA，通知FPGA上傳經(jīng)緯度信息； 4) 判斷當(dāng)前飛行模式是否為位置模式，是則進(jìn)入下一步，否者直接結(jié)束； 5) 采用高斯克呂格投影方法，將經(jīng)緯度給定值和經(jīng)緯度測(cè)量值換算成兩點(diǎn)間南北向距 離偏差NS_error和東西向距離偏差EW_error ; 6) 采用坐標(biāo)變換，將南北向距離偏差NS_error和東西向距離偏差EW_error換算成機(jī) 身縱向距離偏差x_error和橫向距離偏差y_error ;坐標(biāo)變換公式為：
其中Ψ為無人直升機(jī)的偏航角； 7) 計(jì)算橫滾角給定增量APhi_G和俯仰角給定增量ATheta_G:
其中x_error(k)和7_61'1'〇1'(1〇是本次經(jīng)諱度控制周期計(jì)算出的機(jī)身縱向距離偏差和 橫向距離偏差，x_error(k-l)和7_61'1'〇1'(1^-1)是上次經(jīng)諱度控制周期計(jì)算出的機(jī)身縱向 距離偏差和橫向距離偏差。x_kp和x_ki是機(jī)身縱向距離控制的比例增益和積分增益，y_ kp和y_ki是機(jī)身橫向距離控制的比例增益和積分增益，它們的取值通過反復(fù)試驗(yàn)試湊的 方式得出，以響應(yīng)速度加快并且無超調(diào)為標(biāo)準(zhǔn)； 8)累加得橫滾角給定值Phi_G和俯仰角給定值Theta_G : Phi_G = APhi_G+Phi_G Theta_G = Δ Theta_G+Theta_G 然后將Phi_G和Theta_G限幅在-10°和10°之間。
10.根據(jù)權(quán)利要求3所述的基于DSP和FPGA的無人直升機(jī)飛行控制系統(tǒng)的控制方法， 其特征在于，所述的DSP的高度的控制方法包括如下步驟： 1) 從FPGA讀取大氣壓測(cè)量值、溫度測(cè)量值、高度給定值和飛行模式； 2) 根據(jù)校正參數(shù)和溫度對(duì)大氣壓測(cè)量值進(jìn)行溫度補(bǔ)償，得到校正大氣壓，然后利用大 氣壓和海拔高度的關(guān)系，將校正大氣壓解算成高度測(cè)量值； 3) 將解算出的高度測(cè)量值寫入FPGA，通知FPGA上傳高度信息； 4) 判斷當(dāng)前飛行模式是否為位置模式，是則進(jìn)入下一步，否則直接結(jié)束； 5) 用高度給定值減去高度測(cè)量值得到高度偏差z_error ; 6) 計(jì)算油門舵機(jī)控制量增量Au_pow : Δ u_pow = z_kp X (z_error(k)-z_error(k-1))+z_ki X z_error(k)； 其中z_error(k)是本次高度控制周期計(jì)算出的高度偏差，z_error(k-l)是上次高度 控制周期計(jì)算出的高度偏差。z_kp和z_ki是高度控制的比例增益和積分增益，它們的取值 通過反復(fù)試驗(yàn)試湊的方式得出，以響應(yīng)速度加快并且無超調(diào)為標(biāo)準(zhǔn)。 7) 累加得新油門舵機(jī)控制量u_pow = Au_pow+u_pow并對(duì)u_pow限幅； 8) 根據(jù)油門舵機(jī)控制量u_p〇w和總距舵機(jī)控制量u_col的對(duì)應(yīng)關(guān)系，計(jì)算出總距舵機(jī) 控制量u_col ; 9) 將油門舵機(jī)控制量和總距舵機(jī)控制量寫入FPGA內(nèi)部寄存器。
【文檔編號(hào)】G05B19/042GK104062933SQ201410260776
【公開日】2014年9月24日 申請(qǐng)日期:2014年6月12日 優(yōu)先權(quán)日:2014年6月12日
【發(fā)明者】吳愛國, 江濤, 馬園, 趙萌, 方星 申請(qǐng)人:天津大學(xué)