本發(fā)明涉及無(wú)人機(jī)自動(dòng)飛行控制系統(tǒng)，尤其是一種無(wú)人機(jī)的半自主仿地飛行系統(tǒng)及其控制方法。
背景技術(shù)：
：無(wú)人機(jī)的高度控制是無(wú)人機(jī)領(lǐng)域的一個(gè)難點(diǎn)，尤其是近地表仿地飛行。一般的無(wú)人機(jī)高度控制通過(guò)氣壓計(jì)和加速度計(jì)融合計(jì)算無(wú)人機(jī)當(dāng)前高度。這種方法的問(wèn)題有：1、氣壓計(jì)通過(guò)獲取當(dāng)前位置氣壓值計(jì)算當(dāng)前位置的海拔高度，但由于無(wú)人機(jī)的機(jī)動(dòng)造成無(wú)人機(jī)周圍空氣氣壓變化，獲取的高度可能不準(zhǔn)確2、在近地面飛行時(shí)，無(wú)人機(jī)與地表形成的風(fēng)場(chǎng)影響對(duì)氣壓計(jì)數(shù)據(jù)存在較大干擾3、無(wú)法感知地表高度變化，跟隨地形飛行。目前無(wú)人機(jī)仿地飛行的主流方案是采用超聲波傳感器，通過(guò)超聲波反饋的無(wú)人機(jī)相對(duì)地面的數(shù)據(jù)直接修正無(wú)人機(jī)高度控制的方法。它在一定程度上可以解決無(wú)人機(jī)近地表仿地飛行的問(wèn)題，但是超聲波傳感器更新率慢，易受干擾，噪聲較大，由于沒(méi)有融合氣壓計(jì)和加速度計(jì)對(duì)超聲波數(shù)據(jù)進(jìn)行濾波，在較為復(fù)雜地表下（如玉米地，麥地，灌木叢等）高度控制不穩(wěn)定，無(wú)法在較大載荷的工業(yè)無(wú)人機(jī)中應(yīng)用。技術(shù)實(shí)現(xiàn)要素：本發(fā)明為了克服現(xiàn)有技術(shù)方案的不足，提供了一種適應(yīng)復(fù)雜地表環(huán)境的高精度高可靠性半自主仿地飛行的技術(shù)方案。為了實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案為：一種無(wú)人機(jī)的半自主仿地飛行系統(tǒng)，包括GPS定位模塊、慣性測(cè)量模塊、氣壓計(jì)模塊、雷達(dá)高度計(jì)、電機(jī)驅(qū)動(dòng)器、槳葉動(dòng)力結(jié)構(gòu)以及飛行控制器，其特征在于：飛行控制器獲取GPS定位模塊、慣性測(cè)量模塊以及雷達(dá)高度計(jì)的數(shù)據(jù)進(jìn)行融合算法的計(jì)算，獲取無(wú)人機(jī)位置、姿態(tài)、高度數(shù)據(jù)，從而控制電機(jī)驅(qū)動(dòng)器，使無(wú)人機(jī)在半自主模式下沿航線進(jìn)行仿地飛行。一種無(wú)人機(jī)的半自主仿地飛行控制方法，其特征在于包括如下步驟：步驟1，無(wú)人機(jī)操作人員選擇雷達(dá)高度計(jì)；步驟2，飛行控制器根據(jù)雷達(dá)設(shè)備ID在線識(shí)別接入的雷達(dá)高度計(jì)，根據(jù)檢測(cè)到雷達(dá)高度計(jì)類型，自動(dòng)設(shè)置輸入低通濾波器的默認(rèn)截止頻率和數(shù)據(jù)更新頻率；步驟3，飛行控制器讀取慣性測(cè)量模塊和氣壓計(jì)模塊的數(shù)據(jù)，通過(guò)融合算法得到無(wú)人機(jī)的相對(duì)高度；步驟4，飛行控制器通過(guò)雷達(dá)高度計(jì)修正后的高度數(shù)據(jù)對(duì)無(wú)人機(jī)進(jìn)行高度控制，實(shí)現(xiàn)無(wú)人機(jī)的跟隨地表起伏的變化飛行；步驟5，無(wú)人機(jī)可通過(guò)地面站規(guī)劃航線實(shí)現(xiàn)無(wú)人機(jī)半自主或自主飛行。本發(fā)明與現(xiàn)有技術(shù)的有益效果體現(xiàn)在：1、本發(fā)明可通過(guò)地表類型和飛行任務(wù)選擇合適的高度傳感器，無(wú)人機(jī)控制會(huì)自動(dòng)識(shí)別高度傳感器類型匹配對(duì)應(yīng)的濾波算法，實(shí)現(xiàn)最佳的高度控制性能2、本發(fā)明通過(guò)觀測(cè)高度數(shù)據(jù)噪聲，自動(dòng)識(shí)別地表類型，并融合加速度計(jì)數(shù)據(jù)對(duì)高度數(shù)據(jù)濾波，實(shí)現(xiàn)獲取穩(wěn)定準(zhǔn)確的相對(duì)地表高度數(shù)據(jù)3、本發(fā)明可適應(yīng)各種復(fù)雜地表，在復(fù)雜地表下依然后較好的近地表仿地飛行效果4、在自動(dòng)仿地飛行過(guò)程中，操作人員可干預(yù)調(diào)整飛行高度，結(jié)束干預(yù)后無(wú)人機(jī)可按調(diào)整后高度繼續(xù)自主飛行，可方便實(shí)際操作和較好的處理緊急情況。附圖說(shuō)明圖1為本發(fā)明的硬件結(jié)構(gòu)圖；圖2為本發(fā)明的卡爾曼濾波算法原理圖；圖3為本發(fā)明的高度修正控制器算法原理圖；具體實(shí)施方式下面結(jié)合附圖與實(shí)施例對(duì)本發(fā)明作進(jìn)一步的說(shuō)明。一種無(wú)人機(jī)的半自主仿地飛行系統(tǒng)，如圖1所示，包括GPS定位模塊、慣性測(cè)量模塊、氣壓計(jì)模塊、雷達(dá)高度計(jì)、電機(jī)驅(qū)動(dòng)器、槳葉動(dòng)力結(jié)構(gòu)以及飛行控制器，GPS定位模塊通過(guò)UART接口與飛行控制器連接，慣性測(cè)量模塊通過(guò)SPI接口與飛行控制器連接，雷達(dá)高度計(jì)通過(guò)UART接口與飛行控制器連接，電機(jī)驅(qū)動(dòng)器通過(guò)PWM接口與飛行控制器連接，電機(jī)驅(qū)動(dòng)器的動(dòng)力輸出軸與槳葉動(dòng)力結(jié)構(gòu)連接，飛行控制器獲取GPS定位模塊、慣性測(cè)量模塊以及雷達(dá)高度計(jì)的數(shù)據(jù)進(jìn)行融合算法的計(jì)算，獲取無(wú)人機(jī)位置、姿態(tài)、高度數(shù)據(jù)，從而控制電機(jī)驅(qū)動(dòng)器，使無(wú)人機(jī)在半自主模式下沿航線進(jìn)行仿地飛行。其中，半自主模式是指：地面站完成航線規(guī)劃后，將飛行航線通過(guò)數(shù)據(jù)鏈路傳輸?shù)綗o(wú)人機(jī)，無(wú)人機(jī)可按規(guī)劃的航線自動(dòng)飛行，同時(shí)無(wú)人機(jī)的飛行高度可由無(wú)人機(jī)操作人員通過(guò)遙控器控制調(diào)整，在操作人員不再操作時(shí)無(wú)人機(jī)可按照當(dāng)前相對(duì)地表高度繼續(xù)沿航線飛行。GPS定位模塊用于獲取無(wú)人機(jī)的位置坐標(biāo)，計(jì)算無(wú)人機(jī)當(dāng)前速度，使無(wú)人機(jī)可以在半自主模式下的沿航線飛行；慣性測(cè)量模塊用于獲取無(wú)人機(jī)的加速度和角速度，并計(jì)算無(wú)人機(jī)的當(dāng)前歐拉角，從而控制無(wú)人機(jī)穩(wěn)定飛行；氣壓計(jì)模塊用于獲取無(wú)人機(jī)附近的氣壓數(shù)據(jù)；雷達(dá)高度計(jì)包括微波雷達(dá)高度傳感器、紅外雷達(dá)高度傳感器兩種種，用于測(cè)量無(wú)人機(jī)相對(duì)地表的高度。飛行控制器是無(wú)人機(jī)的控制核心部件，獲取所有外部傳感器的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，并根據(jù)控制算法計(jì)算執(zhí)行機(jī)構(gòu)的控制量控制無(wú)人機(jī)穩(wěn)定工作。本發(fā)明的高精度高可靠半自主仿地飛行算法即運(yùn)行在無(wú)人機(jī)飛行控制器中。一種無(wú)人機(jī)的半自主仿地飛行控制方法，步驟如下：步驟1，無(wú)人機(jī)操作人員選擇雷達(dá)高度計(jì)；具體為，雷達(dá)高度計(jì)包括微波雷達(dá)高度傳感器、紅外雷達(dá)高度傳感器兩種，無(wú)人機(jī)操作人員根據(jù)表1選擇合適的高度傳感器。表1不同地表對(duì)應(yīng)的適用的高度傳感器列表通過(guò)選擇合適的雷達(dá)高度計(jì)，從而獲取良好的高度測(cè)量性能。步驟2，飛行控制器根據(jù)雷達(dá)設(shè)備ID在線識(shí)別接入的雷達(dá)高度計(jì)，根據(jù)檢測(cè)到雷達(dá)高度計(jì)類型，自動(dòng)設(shè)置輸入低通濾波器的默認(rèn)截止頻率和數(shù)據(jù)更新頻率；步驟3，飛行控制器讀取慣性測(cè)量模塊和氣壓計(jì)模塊的數(shù)據(jù)，通過(guò)融合算法得到無(wú)人機(jī)的相對(duì)高度（相對(duì)起飛位置高度）；具體為：步驟3.1，計(jì)算當(dāng)前氣壓相對(duì)高度；根據(jù)氣壓計(jì)模塊的氣壓數(shù)據(jù)通過(guò)氣壓模型計(jì)算當(dāng)前位置的氣壓相對(duì)高度；其中，氣壓模型的近似計(jì)算公式為：，其中：hc為測(cè)量點(diǎn)相對(duì)參考基準(zhǔn)高度；ps、Ts為參考基準(zhǔn)的表面壓力和溫度；hs為參考點(diǎn)的大地高程；hb為測(cè)量點(diǎn)的大地高程；pb為測(cè)量點(diǎn)表面壓力；R=287.1J·Kg-1·K-1為氣體常數(shù)；kT=6.5×10-3·m-1為大氣溫度梯度；g0=9.80665m·s-2為平均重力加速度；取參考基準(zhǔn)為起飛位置，ps、Ts為起飛位置的表面壓力和溫度，則hc為無(wú)人機(jī)的氣壓相對(duì)高度（相對(duì)起飛位置）。步驟3.2，通過(guò)融合算法計(jì)算無(wú)人機(jī)的當(dāng)前相對(duì)高度（相對(duì)于起飛位置的高度）；融合算法流程如下：步驟3.2.1,構(gòu)造量測(cè)序列vk；量測(cè)序列vk由加速度計(jì)、氣壓計(jì)的測(cè)量數(shù)據(jù)構(gòu)成：，其中，az為無(wú)人機(jī)垂直方向加速度測(cè)量值；步驟3.2.2，構(gòu)造量測(cè)矩陣Ck；量測(cè)矩陣Ck為：，步驟3.2.3，構(gòu)造高度融合狀態(tài)方程，，量測(cè)方程為：，，其中：xk為狀態(tài)向量，h為相對(duì)高度，v為垂直方向速度，a為垂直方向加速度，b高度測(cè)量偏差,Ak是系統(tǒng)的狀態(tài)方程，是系統(tǒng)噪聲矩陣，、是系統(tǒng)噪聲，Ck量測(cè)矩陣，vk為量測(cè)序列；步驟3.2.4，構(gòu)造卡爾曼濾波算法估計(jì)高度的狀態(tài)方程，如圖2所示，具體為：狀態(tài)預(yù)測(cè)方程：，狀態(tài)估計(jì)方程：，Gk為濾波器增益，計(jì)算方程為：Pk,k-1為誤差協(xié)方差矩陣，估計(jì)方程為：，步驟3.3，高度修正，如圖3所示，具體為：計(jì)算雷達(dá)噪聲值，飛行控制器讀取雷達(dá)高度計(jì)的數(shù)據(jù)，計(jì)算雷達(dá)原始數(shù)據(jù)Data（t）與雷達(dá)低通濾波器輸出數(shù)據(jù)的差值的平方為雷達(dá)數(shù)據(jù)的噪聲值Noise（t）式中：Noise（t）：雷達(dá)噪聲；Data（t）:雷達(dá)原始數(shù)據(jù)；F():低通濾波器；dt為數(shù)據(jù)更新周期；通過(guò)判斷雷達(dá)數(shù)據(jù)的噪聲水平判斷當(dāng)前地表類型，根據(jù)雷達(dá)噪聲值調(diào)整高度修正器的PID參數(shù)，使高度修正平滑穩(wěn)定。如表2所示，常見(jiàn)的地表有：平地、低桿農(nóng)作物地表、高桿農(nóng)作物地表、樹林等。地表類型雷達(dá)反饋噪聲值（單位：cm）平地小于10低桿農(nóng)作物（水稻、小麥）小于30高桿農(nóng)作物（玉米）小于100樹林（果樹林）大于100表2地表與雷達(dá)噪聲對(duì)應(yīng)關(guān)系表其中高度修正的過(guò)程具體為：1、由航線指令或者操縱者遙控指令給出仿地控制的高度指令；2、雷達(dá)輸出相對(duì)地表高度信息，經(jīng)過(guò)地表檢測(cè)算法得到高度修正PID控制器PID參數(shù)；3、雷達(dá)數(shù)據(jù)與仿地高度指令數(shù)據(jù)經(jīng)過(guò)高度修正PID控制器得到無(wú)人機(jī)的相對(duì)高度的控制指令；4、由氣壓計(jì)與加速度計(jì)經(jīng)過(guò)融合算法得到的相對(duì)高度作為反饋，經(jīng)過(guò)高度控制PID得到高度控制量通過(guò)電機(jī)驅(qū)動(dòng)器控制無(wú)人機(jī)高度。步驟4，飛行控制器通過(guò)雷達(dá)高度計(jì)修正后的高度數(shù)據(jù)對(duì)無(wú)人機(jī)進(jìn)行高度控制，實(shí)現(xiàn)無(wú)人機(jī)的跟隨地表起伏的變化飛行；步驟5，無(wú)人機(jī)可通過(guò)地面站規(guī)劃航線實(shí)現(xiàn)無(wú)人機(jī)半自主或自主飛行，具體為：步驟5.1，通過(guò)測(cè)繪獲得飛行區(qū)域坐標(biāo)后地面站根據(jù)坐標(biāo)數(shù)據(jù)規(guī)劃無(wú)人機(jī)航線，將航線通過(guò)數(shù)據(jù)鏈路上傳到無(wú)人機(jī)；步驟5.2，無(wú)人機(jī)起飛后由操作人員通過(guò)遙控切換到半自主模式，無(wú)人機(jī)將按照航線自動(dòng)飛行；步驟5.3，在自主飛行中無(wú)人機(jī)操作人員通過(guò)遙控器改變無(wú)人機(jī)相對(duì)地表的飛行高度，無(wú)人機(jī)操作人員不再改變飛行高度時(shí)無(wú)人機(jī)將沿著修改后的高度自主飛行。以上所述實(shí)施方式僅表達(dá)了本發(fā)明的一種實(shí)施方式，但并不能因此而理解為對(duì)本發(fā)明范圍的限制。應(yīng)當(dāng)指出，對(duì)于本領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變形和改進(jìn)，這些都屬于本發(fā)明的保護(hù)范圍。當(dāng)前第1頁(yè)1 2 3