本發(fā)明屬于無人機技術領域，尤其涉及一種無人機高度控制方法及系統(tǒng)。

背景技術：

當前對無人機技術的研究主要集中在如何提高無人機的飛行能力，使其能應對不同的飛行環(huán)境，并且在復雜的環(huán)境中能夠進行自適應飛行，安全穩(wěn)定的完成目標任務。近些年，隨著各種新技術的不斷應用，無人機系統(tǒng)的復雜性及功能的自動化程度日益增加。但是，由于飛行環(huán)境的高度動態(tài)化、不確定性以及飛行任務的復雜性，無人機在規(guī)劃與決策方面面臨著新的技術挑戰(zhàn)，現(xiàn)有的基于程序化的自動控制策略已經不能滿足未來先進多功能無人機對復雜飛行環(huán)境下的多任務的需求，自主飛行控制能力的提高將是未來無人機飛行控制系統(tǒng)發(fā)展的主要目標。無人機的縱向控制方法是無人機自主飛行研究領域的一個重要課題。無人機的飛行環(huán)境具有復雜性和不確定性，在自主飛行過程中會遇到來自上側和下側的障礙或地形的起伏變化，因此無人機在垂直方向上必須有一定的自主控制能力，以確保飛行過程的安全和穩(wěn)定。

無人機在水平方向的控制方法研究起步較早，技術也相對成熟。在縱向控制方面，現(xiàn)階段無論在學校還是商業(yè)應用都缺乏相應研究?，F(xiàn)今無人機在縱向的控制研究主要集中在利用氣壓計實現(xiàn)的高度保持功能，以及融合了超聲波傳感器數(shù)據(jù)的簡單地形跟隨。這種簡單的縱向控制策略，既不能滿足快速地形跟隨中對障礙物的預判，也無法對來自上側的障礙物進行有效規(guī)避。

現(xiàn)有無人機高度控制的方法主要有超聲波法、激光/紅外法以及視覺法等，這些方法都存在缺陷和不足。

超聲波法是利用超聲波測距傳感器來探測物體的位置，超聲波測距使用時間差測距法。超聲波測距的成本低，實現(xiàn)方法簡單，技術成熟，是無人機測距的常用方法。但是超聲波傳感器的采樣頻率低，受吸音材質影響較大。另一方面，單超聲波傳感器避障存在由于超聲波的方向性不好造成對障礙物的定位不精確，存在探測盲區(qū)等缺點，而多個超聲波傳感器同時使用會產生串擾。

激光/紅外法是利用電磁波來探測物體的位置。激光傳感器使用時間法或相位法來測量距離。激光傳感器的光束高度集中，方向性好，因此可得到障礙物的準確位置，彌補了超聲波測距在方向性上的不足。激光測距的精確度很高，但是激光傳感器成本高，設備體積大，功耗高，而且一些激光傳感器發(fā)射的激光，對人的眼睛有傷害。現(xiàn)有的紅外傳感器的測距原理與激光傳感器類似，采用與激光傳感器不同頻率的紅外光進行測距，不同的是現(xiàn)有的紅外傳感器采用測量反射光強度的變化來測量距離。由于被測物體表面材質的反射率不同，所以紅外測距的精度較差，距離也有限。

視覺法是利用單目或者雙目攝像頭來探測物體的位置。無人機通過攝像頭采集周圍環(huán)境的圖像信息，并通過圖像算法得到自身與被測物體之間的距離，然后實施控制策略。視覺測距法具有探測范圍廣，同時可以獲得物體的形狀、速度等信息的優(yōu)勢。但是圖像數(shù)據(jù)在后期處理中，由于邊緣銳化、特征提取等圖像處理方法計算量大，實時性差，對中央處理器要求高。而且視覺測距法不能檢測到玻璃等透明障礙物的存在，另外受視場光線強弱、煙霧的影響很大，成本也較高。

技術實現(xiàn)要素：

本發(fā)明實施例所要解決的技術問題在于提供一種無人機高度控制方法及系統(tǒng)，旨在解決現(xiàn)有技術中的無人機的縱向控制不精確的問題。

本發(fā)明實施例第一方面提供了一種無人機高度控制方法，所述方法包括：

將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對所述高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度；

將所述目標速度與測量速度做差，得到速度誤差，并將所述速度誤差輸入所述比例控制器，并結合預估加速度，得到目標加速度；

將所述目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度；

其中，所述測量高度為下向傳感器當前時刻測得的距離數(shù)據(jù)，所述下向傳感器包括多個與無人機平面呈預置角度且方向向下的傳感器，所述下向傳感器測得的距離數(shù)據(jù)，具體為各所述與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各所述與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。

本發(fā)明實施例第二方面提供了一種無人機高度控制系統(tǒng)，所述系統(tǒng)包括：

目標速度獲取模塊，用于將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對所述高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度；

目標加速度獲取模塊，用于將所述目標速度與測量速度做差，得到速度誤差，并將所述速度誤差輸入所述比例控制器，并結合預估加速度，得到目標加速度；

油門輸出量獲取模塊，用于將所述目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度；

其中，所述測量高度為下向傳感器當前時刻測得的距離數(shù)據(jù)，所述下向傳感器包括多個與無人機平面呈預置角度且方向向下的傳感器，所述下向傳感器測得的距離數(shù)據(jù)，具體為各所述與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各所述與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。

從上述本發(fā)明實施例可知，本發(fā)明通過將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度，將目標速度與測量速度做差，得到速度誤差，并將速度誤差輸入比例控制器，并結合預估加速度，得到目標加速度，將目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度，其中，測量高度為下向傳感器當前時刻測得的距離數(shù)據(jù)，下向傳感器包括多個與無人機平面呈預置角度且方向向下的傳感器，下向傳感器測得的距離數(shù)據(jù)，具體為各與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。相較于現(xiàn)有技術，本發(fā)明中的無人機可以實現(xiàn)對地形的快速跟隨，高度保持，并對來自無人機上方的障礙物進行有效地規(guī)避，本發(fā)明的成本低、功耗小、適用場景多、飛行效果好，產品粘性得到提高。

附圖說明

為了更清楚地說明本發(fā)明實施例或現(xiàn)有技術中的技術方案，下面將對實施例或現(xiàn)有技術描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實施例，對于本領域技術人員來講，在不付出創(chuàng)造性勞動性的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

附圖1是本發(fā)明第一實施例提供的無人機高度控制方法的實現(xiàn)流程示意圖；

附圖2是本發(fā)明第二實施例提供的無人機高度控制系統(tǒng)的結構示意圖；

附圖3是本發(fā)明中tof距離傳感器測量原理框圖；

附圖4是本發(fā)明中pid控制器的數(shù)學模型。

具體實施方式

為使得本發(fā)明實施例的發(fā)明目的、特征、優(yōu)點能夠更加的明顯和易懂，下面將結合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發(fā)明一部分實施例，而非全部實施例?；诒景l(fā)明中的實施例，本領域技術人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本發(fā)明保護的范圍。

請參閱附圖1，附圖1為本發(fā)明第一實施例提供的無人機高度控制方法的實現(xiàn)流程示意圖。

本實施例中的上向傳感器和下向傳感器均可使用窄波束單點測量傳感器，即飛行時間(timeofflight，tof)距離傳感器，其具有測量距離遠(10m)、精度高(10cm)、體積小、成本低，互相之間無串擾，可以多個同時使用的優(yōu)點。

如附圖1所示，該方法主要包括以下步驟：

s101、將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度；

修正后的高度誤差的計算公式為：

erroralt＝(tof_alttarget-tof_alt)-(baro_alttarget-baro_alt)①；

其中，①式中的erroralt為修正后的高度誤差，tof_alttarget為目標高度，tof_alt為測量高度，baro_alttarget為前一時刻氣壓計測得的高度數(shù)據(jù)，baro_alt為當前時刻氣壓計測得的高度數(shù)據(jù)。baro_alttarget-baro_alt為前后兩個時刻的氣壓計測得的高度值的差值，因此本步驟中融合了氣壓計的值。

下向傳感器可包括多個與無人機平面呈預置角度且方向向下的傳感器，在本實施例中，下向傳感器包括兩個與無人機平面垂直且方向向下的傳感器，其測量的數(shù)據(jù)分別為tof1和tof2，其權重分別為k1和k2；下向傳感器還包括兩個向前下方與無人機平面呈30°夾角的傳感器，其測量的數(shù)據(jù)分別為tof3和tof4，其權重分別為k3和k4。下向傳感器測得的距離數(shù)據(jù)，具體為：各與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。

測量高度tof_alt的計算公式為：

tof_alt＝k1tof1+k2tof2+k3tof3+k4tof4②；

其中，②式中的tof_alt為測量高度，k1和k2均為0.3，k3和k4均為0.2。

需要說明的是，下向傳感器的個數(shù)、預置角度均可根據(jù)需要自行設定，各下向傳感器的權重也可自行設置。

目標速度計算公式為：

veltarget＝velestimate+kp0·erroralt③；

其中，③式中的veltarget為目標速度，velestimate為預估速度，是當前時刻無人機在垂直方向上的速度的估算值，估算值的計算不依賴傳感器，只與當前油門值和死區(qū)上下邊界油門值以及最低最高油門值、垂直方向允許的最大速度有關，其中最大速度是用戶設定的參數(shù)，kp0為比例系數(shù)，erroralt為修正后的高度誤差。

①式中的目標高度tof_alttarget和測量高度tof_alt的獲取方法包括：

在保證地形跟隨效果的基礎上，為了保證地形跟隨中無人機對上側障礙物有一定預判和規(guī)避能力，本實施例可在無人機上方相應安裝上向tof距離傳感器，①式中的目標高度tof_alttarget的計算方法有以下兩種：

當上向傳感器當前時刻測得的距離數(shù)據(jù)小于預置安全距離時，將下向傳感器當前時刻測得的距離數(shù)據(jù)與目標距離的差作為目標高度，目標距離為預置安全距離與上向傳感器當前時刻測得的距離數(shù)據(jù)的差，上向傳感器為與無人機平面垂直且方向向上的傳感器，此時目標高度tof_alttarget的計算公式為：

tof_alttarget＝tofbottom-(distancesafe-toftop)④；

其中，④式中的tof_alttarget為目標高度，tofbottom為下向傳感器當前時刻測得的距離數(shù)據(jù)，distancesafe為預置安全距離，toftop為上向傳感器當前時刻測得的距離數(shù)據(jù)。

當上向傳感器當前時刻測得的距離數(shù)據(jù)大于或等于預置安全距離時，將無人機油門桿處于死區(qū)時，下向傳感器測得的距離數(shù)據(jù)作為目標高度tof_alttarget。

將下向傳感器當前時刻測得的距離數(shù)據(jù)作為測量高度tof_alt。

進一步地，由于環(huán)境因素的影響，無人機平面可能不處于標準的水平面，因此下向傳感器當前時刻測得的距離數(shù)據(jù)可能不準確，可以對該距離數(shù)據(jù)進行修正，修正公式如下：

tof_altcorrect＝tof_alt·cosαpitchcosβroll⑤；

其中，⑤式中的tof_altcorrect為測量高度的修正值，tof_alt為測量高度，αpitch為當前時刻無人機的俯仰角，βroll為當前時刻無人機的橫滾角，當前時刻的俯仰角和橫滾角均可由陀螺儀測得。可使用tof_altcorrect替換①式中的tof_alt進行計算，以提高獲取的高度誤差的精確度。由于αpitch和βroll均為陀螺儀積分計算得到的姿態(tài)角角度值，因此本方法融合了陀螺儀的值。

s102、將目標速度與測量速度做差，得到速度誤差，并將速度誤差輸入比例控制器，并結合預估加速度，得到目標加速度；

其中，預估速度為當前時刻的測量加速度在垂直方向的分量的積分，無人機當前時刻的測量加速度可由加速度計直接測得。

速度誤差計算公式為：

errorvel＝veltarget-accel_vel⑥；

其中，⑥式中的errorvel為速度誤差，veltarget為目標速度，accel_vel為測量速度。由于accel_vel是加速度計測得的加速度值通過積分計算得到的速度值，因此本步驟融合了加速度計的值。

目標加速度獲取公式為：

acceltarget＝accelestimate+kp1·errorvel⑦；

其中，⑦式中的acceltarget為目標加速度，accelestimate為預估加速度，是前饋控制器輸出的加速度值，此加速度值是前后兩幀目標速度的差分值，kp1為比例系數(shù)，errorvel為速度誤差。

s103、將目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度。

油門輸出量的獲取公式為：

其中，⑧式中的throut為油門輸出量，acceltarget為目標加速度，kp2、ki2、kd2均為比例系數(shù)，c為預置的控制器初始值。

tof技術是一種利用特定頻率的近紅外光，通過調制光線并計算調制光往返傳感器與被測物體間的時間差或相位差來計算傳感器到被測物體間距離的測距技術。

tof距離傳感器是利用tof技術生產的距離測量傳感器。tof距離傳感器發(fā)出經調制的近紅外光，遇物體后反射，傳感器通過計算光線發(fā)射和反射時間差或相位差，再根據(jù)調制光的波長計算出被測物體的距離，以產生深度信息。根據(jù)tof測距的原理，待測距離d的計算公式為：

若調制光的角頻率為ω，在待測距離d上往返一次產生的相位延遲為φ，則對應時間t可表示為:

t＝φ/ω；

將此關系代入待測距離d的計算公式中，待測距離d可表示為：

式中：

φ——調制光往返一次產生的相位延遲；

ω——調制光的角頻率，ω＝2πf；

n——測量光線所包含半波長的個數(shù)；

δφ——調制光往返一次產生的相位延遲不足π的部分；

δn——測量光線所包含的半波長數(shù)的小數(shù)部分。

在給定調制和標準大氣條件下，c/4πf是一個常數(shù)，此時距離的測量變成了對測量光線所包含半波長個數(shù)的測量，即測量n或φ。隨著機械加工技術和無線電測相技術的不斷發(fā)展，現(xiàn)在對φ的測量已經能達到很高的精度。圖3為tof距離傳感器測量原理框圖。

本實施例中使用級聯(lián)式比例積分微分(pid)控制器，包括比例控制器(p控制器)、積分控制器(i控制器)和微分控制器(d控制器)。pid是比例、積分、微分的縮寫，將偏差的比例、偏差的積分和偏差的微分通過線性組合構成控制量，用這一控制量對被控對象進行控制，其數(shù)學模型如圖4所示。

pid控制器的控制規(guī)律為：

式中：

k——采樣時刻，k＝0、1、2…；

u0——pid控制器的初始值；

uk——第k次采樣時刻的輸出值；

ek——第k次采樣時刻輸入的偏差值；

ek-1——第k-1次采樣時刻輸入的偏差值；

kp——比例系數(shù),改變kp可提高系統(tǒng)的響應速度，減小靜態(tài)誤差，但太大會增大超調量和穩(wěn)定時間；

ki——積分系數(shù),與kp的作用基本相似，但要使靜態(tài)誤差為0，必須使用積分；

kd——微分系數(shù),與kp、ki的作用相反，主要是為了減小超調量和穩(wěn)定時間。

本發(fā)明實施例提供的無人機高度控制方法，通過將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度，將目標速度與測量速度做差，得到速度誤差，并將速度誤差輸入比例控制器，并結合預估加速度，得到目標加速度，將目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度，其中，測量高度為下向傳感器當前時刻測得的距離數(shù)據(jù)，下向傳感器包括多個與無人機平面呈預置角度且方向向下的傳感器，下向傳感器測得的距離數(shù)據(jù)，具體為各與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。相較于現(xiàn)有技術，本發(fā)明中的無人機可以實現(xiàn)對地形的快速跟隨，高度保持，并對來自無人機上方的障礙物進行有效地規(guī)避，本發(fā)明的成本低、功耗小、適用場景多、飛行效果好，產品粘性得到提高。

請參閱附圖2，附圖2是本發(fā)明第二實施例提供的無人機高度控制系統(tǒng)的結構示意圖，為了便于說明，僅示出了與本發(fā)明實施例相關的部分。附圖2示例的無人機高度控制系統(tǒng)，主要包括：目標高度獲取模塊201、測量高度獲取模塊202、目標速度獲取模塊203、目標加速度獲取模塊204以及油門輸出量獲取模塊205。

目標高度獲取模塊201，用于當上向傳感器當前時刻測得的距離數(shù)據(jù)大于或等于預置安全距離時，將無人機油門桿處于死區(qū)時，下向傳感器測得的距離數(shù)據(jù)作為目標高度，上向傳感器為與無人機平面垂直且方向向上的傳感器，當上向傳感器當前時刻測得的距離數(shù)據(jù)小于預置安全距離時，將下向傳感器當前時刻測得的距離數(shù)據(jù)與目標距離的差作為目標高度，目標距離為預置安全距離與上向傳感器當前時刻測得的距離數(shù)據(jù)的差。

測量高度獲取模塊202，用于將下向傳感器當前時刻測得的距離數(shù)據(jù)作為測量高度。

其中，測量高度為下向傳感器當前時刻測得的距離數(shù)據(jù)，下向傳感器包括多個與無人機平面呈預置角度且方向向下的傳感器，下向傳感器測得的距離數(shù)據(jù)，具體為各與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。

目標速度獲取模塊203，用于將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度。

目標加速度獲取模塊204，用于將目標速度與測量速度做差，得到速度誤差，并將速度誤差輸入所述比例控制器，并結合預估加速度，得到目標加速度。

油門輸出量獲取模塊205，用于將目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度。

進一步地，測量高度獲取模塊202，具體用于獲取當前時刻下向傳感器測得的距離數(shù)據(jù)，以及，通過陀螺儀測得的角度數(shù)據(jù)對當前時刻下向傳感器測得的距離數(shù)據(jù)進行數(shù)據(jù)修正，將數(shù)據(jù)修正后的距離數(shù)據(jù)作為測量高度。

進一步地，上向傳感器與下向傳感器均為窄波束單點測量傳感器。

上述各功能模塊實現(xiàn)各自功能的具體過程，可參考前述第一實施例提供的無人機高度控制方法的相關內容，此處不再贅述。

本發(fā)明實施例提供的無人機高度控制系統(tǒng)，通過將目標高度與測量高度做差，得到高度誤差，通過氣壓計測得的高度數(shù)據(jù)對高度誤差進行數(shù)據(jù)修正，將經數(shù)據(jù)修正后的高度誤差輸入比例控制器，并結合預估速度，得到目標速度，將目標速度與測量速度做差，得到速度誤差，并將速度誤差輸入比例控制器，并結合預估加速度，得到目標加速度，將目標加速度輸入比例積分微分控制器，并結合預置的控制器初始值，得到油門輸出量，以控制無人機的高度，其中，測量高度為下向傳感器當前時刻測得的距離數(shù)據(jù)，下向傳感器包括多個與無人機平面呈預置角度且方向向下的傳感器，下向傳感器測得的距離數(shù)據(jù)，具體為各與無人機平面呈預置角度且方向向下的傳感器測得的距離數(shù)據(jù)與各與無人機平面呈預置角度且方向向下的傳感器的權重的乘積之和。相較于現(xiàn)有技術，本發(fā)明中的無人機可以實現(xiàn)對地形的快速跟隨，高度保持，并對來自無人機上方的障礙物進行有效地規(guī)避，本發(fā)明的成本低、功耗小、適用場景多、飛行效果好，產品粘性得到提高。

需要說明的是，對于前述的各方法實施例，為了簡便描述，故將其都表述為一系列的動作組合，但是本領域技術人員應該知悉，本發(fā)明并不受所描述的動作順序的限制，因為依據(jù)本發(fā)明，某些步驟可以采用其它順序或者同時進行。其次，本領域技術人員也應該知悉，說明書中所描述的實施例均屬于優(yōu)選實施例，所涉及的動作和模塊并不一定都是本發(fā)明所必須的。

在上述實施例中，對各個實施例的描述都各有側重，某個實施例中沒有詳述的部分，可以參見其它實施例的相關描述。

以上為對本發(fā)明所提供的無人機高度控制方法及系統(tǒng)的描述，對于本領域的技術人員，依據(jù)本發(fā)明實施例的思想，在具體實施方式及應用范圍上均會有改變之處，綜上，本說明書內容不應理解為對本發(fā)明的限制。