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