基于多路超聲波測(cè)距的無(wú)人機(jī)姿態(tài)檢測(cè)方法

多路超聲波測(cè)距系統(tǒng)獨(dú)立下降是無(wú)人機(jī)獨(dú)立飛行的一項(xiàng)重要技術(shù)，確保了無(wú)人機(jī)的安全恢復(fù)和重新使用。一般來(lái)說(shuō)，為了確保無(wú)人機(jī)的安全穩(wěn)定，必須保持正確的仰角。因此,降落階段的相對(duì)高度和相對(duì)姿態(tài)測(cè)量對(duì)無(wú)人機(jī)導(dǎo)航系統(tǒng)提出了較高的要求,無(wú)人機(jī)上常采用的姿態(tài)測(cè)量系統(tǒng)有視覺(jué)測(cè)姿系統(tǒng)、微慣性航姿系統(tǒng)、MIMU/GPS組合導(dǎo)航系統(tǒng)等。而MIMU/GPS組合導(dǎo)航系統(tǒng)結(jié)構(gòu)復(fù)雜,成本高,而其獲得的姿態(tài)角信息是相對(duì)于地理系的姿態(tài)信息,難以直接獲得相對(duì)于跑道的相對(duì)姿態(tài)角;視覺(jué)測(cè)姿系統(tǒng)需要處理的圖像數(shù)據(jù)信息量大,實(shí)時(shí)性不強(qiáng)。本文為了實(shí)時(shí)獲得飛機(jī)著陸段精確的相對(duì)高度和相對(duì)姿態(tài)角,以固定翼無(wú)人機(jī)為對(duì)象設(shè)計(jì)多路超聲波測(cè)高、測(cè)姿系統(tǒng),在對(duì)超聲波測(cè)距模塊標(biāo)定的基礎(chǔ)上,利用四路超聲波測(cè)距模塊同時(shí)測(cè)得機(jī)頭、機(jī)尾、左翼末端和右翼末端與地面的相對(duì)高度,并通過(guò)空間幾何關(guān)系推算無(wú)人機(jī)機(jī)翼中心與地面的相對(duì)高度和無(wú)人機(jī)姿態(tài),提高降落階段的無(wú)人機(jī)控制性能,實(shí)現(xiàn)無(wú)人機(jī)的安全著陸。本文提出的多路超聲波測(cè)距方法,在保證系統(tǒng)實(shí)時(shí)性的同時(shí)提高了系統(tǒng)的測(cè)距精度。1多路超聲測(cè)量的高度和測(cè)量原理1.1檢測(cè)范圍有限超聲波測(cè)距方法主要有相位檢測(cè)法、聲波幅值檢測(cè)法和渡越時(shí)間法。其中,相位檢測(cè)法精度高,但檢測(cè)范圍有限;聲波幅值檢測(cè)法易受反射介質(zhì)的影響。因此,當(dāng)前超聲波測(cè)距一般使用渡越時(shí)間法。渡越時(shí)間法是通過(guò)檢測(cè)發(fā)射超聲波與接收回波之間的時(shí)間差t,求出目標(biāo)障礙物距信號(hào)發(fā)射源的距離d,計(jì)算公式為:式中,v為超聲波在介質(zhì)中傳播的速度(m/s)。1.2無(wú)人機(jī)的機(jī)翼中心高度無(wú)人機(jī)在著陸時(shí)主要通過(guò)觀察無(wú)人機(jī)相對(duì)高度和相對(duì)姿態(tài)的變化調(diào)整無(wú)人機(jī)的飛行參數(shù)。多路超聲波測(cè)距模塊安裝在無(wú)人機(jī)的機(jī)頭、機(jī)尾、左翼末端和右翼末端,可測(cè)得無(wú)人機(jī)四個(gè)固定點(diǎn)的高度,通過(guò)幾何關(guān)系解算,可以得到無(wú)人機(jī)的機(jī)翼中心高度及橫滾角和俯仰角。無(wú)人機(jī)在著陸時(shí)俯仰與橫滾的角度一般不大,約在10°~15°以內(nèi),在這個(gè)角度變化范圍內(nèi),能夠確保每個(gè)超聲波測(cè)距模塊都可以接收到回波信號(hào)。如圖1所示,通過(guò)幾何關(guān)系可得俯仰角、橫滾角。式中,a1為超聲波測(cè)距模塊測(cè)得的機(jī)頭到地面的高度,a2為超聲波測(cè)距模塊測(cè)得的機(jī)尾到地面的高度,b1為超聲波測(cè)距模塊測(cè)得的右翼到地面的高度,b2為超聲波測(cè)距模塊測(cè)得的左翼到地面的高度,θ為無(wú)人機(jī)的俯仰角,γ為無(wú)人機(jī)的橫滾角,h為無(wú)人機(jī)的機(jī)翼中心高度。通過(guò)串口發(fā)送無(wú)人機(jī)的相對(duì)高度和姿態(tài)角信息給控制臺(tái)處理,便可實(shí)現(xiàn)無(wú)人機(jī)的自主安全著陸。2多路超聲波測(cè)距模塊的誤差補(bǔ)償單個(gè)超聲波測(cè)距模塊測(cè)距時(shí)會(huì)受到溫度、濕度、壓力等外界環(huán)境因素的影響,從而影響測(cè)距精度;而當(dāng)多個(gè)超聲波測(cè)距模塊安裝較近時(shí),在一定的距離以內(nèi)會(huì)出現(xiàn)相互串?dāng)_現(xiàn)象,從而影響到各個(gè)超聲波測(cè)距模塊的測(cè)距精度及穩(wěn)定性,因此,為保證多路超聲波測(cè)高、測(cè)姿系統(tǒng)的準(zhǔn)確性和穩(wěn)定性,我們研究了超聲波測(cè)距模塊誤差補(bǔ)償、誤差標(biāo)定以及串?dāng)_處理辦法,優(yōu)化了多路超聲波測(cè)高、測(cè)姿系統(tǒng)的測(cè)量準(zhǔn)確性及穩(wěn)定性。2.1超聲波測(cè)距模塊的補(bǔ)償超聲波是一種頻率在40kHz左右的聲波,因此,其在大氣中的傳播速度與大氣溫度、濕度、壓力有密切的關(guān)系。從超聲波測(cè)距原理可知,超聲波傳播速度的準(zhǔn)確性直接影響到超聲波測(cè)距模塊的距離測(cè)量精度,因此對(duì)超聲波測(cè)距模塊進(jìn)行溫度補(bǔ)償、濕度補(bǔ)償以及壓力補(bǔ)償,可以有效地提高超聲波測(cè)距模塊測(cè)距精度。通過(guò)公式(3)可補(bǔ)償氣壓、溫度、濕度變化對(duì)超聲波傳播速度的影響。式中,V0=331.45m/s為在0℃時(shí)標(biāo)準(zhǔn)氣壓P0=1.01325×105Pa和40%濕度時(shí)的超聲波傳播速度,Vc為利用大氣氣壓、溫度、濕度計(jì)算得到的當(dāng)前環(huán)境下精確的超聲波傳播速度,Pk為當(dāng)前的空氣壓力值(Pa),Tk為當(dāng)前的大氣溫度(℃),Hk為當(dāng)前的空氣濕度(%)。2.2靜態(tài)測(cè)距實(shí)驗(yàn)參數(shù)的標(biāo)定超聲波測(cè)距模塊的測(cè)距精度一般都為±5mm。然而在實(shí)際靜態(tài)測(cè)距中,測(cè)得的距離誤差遠(yuǎn)大于±5mm,這是由溫度、濕度、壓力、模塊工藝、障礙物材質(zhì)、障礙物表面不平等外界因素引起的,因此在對(duì)超聲波測(cè)距模塊溫度、濕度、壓力補(bǔ)償后,還需要對(duì)模塊進(jìn)行誤差標(biāo)定,令其誤差低于5mm。為了獲得超聲波測(cè)距模塊在其測(cè)距范圍內(nèi)相對(duì)精確的高度測(cè)量值,本次靜態(tài)測(cè)距實(shí)驗(yàn)分別選擇了10cm、50cm、100cm、150cm、200cm、250cm、300cm七個(gè)點(diǎn)進(jìn)行標(biāo)定實(shí)驗(yàn)。安裝四個(gè)模塊在同一平面上,分別在距離障礙物10cm、50cm、100cm、150cm、200cm、250cm、300cm采集一定時(shí)間的數(shù)據(jù),通過(guò)Matlab計(jì)算得到數(shù)據(jù)平均值,如下表所示。由上表可知,1號(hào)超聲波測(cè)距模塊最大誤差達(dá)到了1.2061cm,2號(hào)超聲波測(cè)距模塊最大誤差達(dá)到了2.6143cm,3號(hào)超聲波測(cè)距模塊最大誤差達(dá)到了6.6049cm,4號(hào)超聲波測(cè)距模塊最大誤差達(dá)到了3.3330cm。該測(cè)距精度無(wú)法滿足本文要求,因此本文采用二階線性擬合的方法標(biāo)定超聲波測(cè)距模塊?；谧钚《朔▽?duì)上述模塊輸出進(jìn)行誤差擬合,獲得擬合后補(bǔ)償公式如(4)所示:式中,xi是測(cè)量距離,yi是標(biāo)定后的值(i=1,2,3,4)。四個(gè)模塊標(biāo)定后的誤差如圖2所示,從圖中可以得知,在誤差標(biāo)定后,四個(gè)模塊的誤差都在±5mm范圍以內(nèi),有效提高了系統(tǒng)的測(cè)距精度。2.3超聲波測(cè)距模塊的設(shè)計(jì)及實(shí)現(xiàn)每個(gè)超聲波測(cè)距傳感器都存在一定的波束角α,當(dāng)兩個(gè)超聲波測(cè)距傳感器安裝過(guò)近或測(cè)量距離較遠(yuǎn),同時(shí)采集超聲波回波時(shí),超聲波測(cè)距模塊之間會(huì)發(fā)生相互串?dāng)_,導(dǎo)致測(cè)量到的距離信息跳變或失準(zhǔn)。為此,本文提出了一種方案,從軟件的角度有效地解決了多路超聲波測(cè)距串?dāng)_問(wèn)題。假設(shè)無(wú)人機(jī)的機(jī)長(zhǎng)為l1,翼長(zhǎng)為l2,波束角為α,機(jī)翼中心在機(jī)長(zhǎng)靠前的1/3處。按照本文的超聲波測(cè)距模塊布局方式,機(jī)頭頂端與兩翼末端的超聲波測(cè)距模塊距離最近,因此優(yōu)先考慮機(jī)頭頂端和左翼末端超聲波測(cè)距串?dāng)_問(wèn)題。如圖3所示,設(shè)無(wú)人機(jī)水平,運(yùn)用數(shù)學(xué)幾何關(guān)系,可以得到機(jī)頭頂端和左翼末端的超聲波測(cè)距模塊存在串?dāng)_的臨界高度hc的計(jì)算公式:因此以hc為界限,當(dāng)無(wú)人機(jī)機(jī)翼中心高度h大于hc時(shí),為避免各超聲波測(cè)距模塊間的串?dāng)_,采用分時(shí)觸發(fā)的方法,按模塊1→模塊2→模塊3→模塊4→模塊5循環(huán)觸發(fā)方式對(duì)不同超聲波測(cè)距模塊進(jìn)行觸發(fā)采樣,成功解決了因波束角而帶來(lái)的串?dāng)_問(wèn)題。同時(shí)隨著無(wú)人機(jī)高度的增加,單個(gè)超聲波測(cè)距傳感器所允許的最大采樣率降低,對(duì)姿態(tài)測(cè)量的實(shí)時(shí)性要求不高,為該方案提供了可行性。在高度h小于hc時(shí),由于各超聲波測(cè)距模塊間不存在串?dāng)_,且隨著高度的減小,單個(gè)超聲波測(cè)距傳感器所允許的最大采樣率提高,為進(jìn)一步提高姿態(tài)測(cè)量的實(shí)時(shí)性,采用多個(gè)超聲波測(cè)距模塊同時(shí)觸發(fā)采樣方式,保證了較高的采樣率,同時(shí)從軟件方面解決了串?dāng)_問(wèn)題,提高測(cè)距的精度及穩(wěn)定性。3多路超聲測(cè)量系統(tǒng)的設(shè)計(jì)3.1多路超聲波測(cè)距模塊為進(jìn)一步驗(yàn)證方法的可行性和工程實(shí)用性,本文以固定翼無(wú)人機(jī)為對(duì)象,設(shè)計(jì)了多路超聲波測(cè)高、測(cè)姿系統(tǒng)。基于多路超聲波測(cè)距模塊的相對(duì)高度和相對(duì)姿態(tài)測(cè)量系統(tǒng),將多個(gè)超聲波測(cè)距模塊分別固定在飛機(jī)的機(jī)頭頂端、機(jī)尾末端、左機(jī)翼末端、右機(jī)翼末端四個(gè)位置,系統(tǒng)安裝結(jié)構(gòu)示意圖如圖4所示,利用單片機(jī)控制,同時(shí)測(cè)得四個(gè)超聲波測(cè)距模塊與地面的相對(duì)高度信息,并通過(guò)幾何關(guān)系計(jì)算獲得機(jī)翼中心高度和飛機(jī)的姿態(tài)角(橫滾角和俯仰角)。3.2超聲波測(cè)距模塊的設(shè)計(jì)系統(tǒng)軟件設(shè)計(jì)采用模塊化設(shè)計(jì)思路,使程序方便調(diào)試,易于擴(kuò)展。由于超聲波測(cè)距模塊中波束角的存在,為解決超聲波測(cè)距模塊之間的串?dāng)_問(wèn)題,本文設(shè)計(jì)了分時(shí)采集和同時(shí)采集兩種不同的多路超聲波測(cè)距模式。以hc為界限,當(dāng)無(wú)人機(jī)機(jī)翼中心高度h大于hc時(shí),采用分時(shí)采集模式,當(dāng)無(wú)人機(jī)機(jī)翼中心高度h不大于hc時(shí),采用同時(shí)采集模式。4無(wú)人機(jī)測(cè)試系統(tǒng)誤差分析為進(jìn)一步獲得多路超聲波測(cè)高、測(cè)姿系統(tǒng)用于無(wú)人機(jī)相對(duì)高度和姿態(tài)測(cè)量的可行性和實(shí)用精度,本文設(shè)計(jì)了超聲波測(cè)高、測(cè)姿系統(tǒng)與激光測(cè)距系統(tǒng)和高精度航姿系統(tǒng)的對(duì)比實(shí)驗(yàn)。以無(wú)人滑翔機(jī)為驗(yàn)證平臺(tái)載體,著重對(duì)比分析了無(wú)人機(jī)進(jìn)行俯仰、橫滾、降落三種典型運(yùn)動(dòng)情況下的測(cè)量性能。如圖5所示,為無(wú)人機(jī)在高度100cm處20°范圍內(nèi)作快速俯仰運(yùn)動(dòng)時(shí)俯仰角的誤差曲線圖,俯仰角測(cè)量精度優(yōu)于3°。如圖6所示,為無(wú)人機(jī)在高度100cm處20°范圍內(nèi)做快速橫滾運(yùn)動(dòng)時(shí)橫滾角的誤差曲線圖,橫滾角測(cè)量精度優(yōu)于3°。如圖7所示,為無(wú)人機(jī)以10°的俯仰角降落時(shí),俯仰角測(cè)量精度優(yōu)于2°。綜上所述,當(dāng)無(wú)人機(jī)以20°作俯仰、橫滾運(yùn)動(dòng)時(shí),多路超聲波測(cè)高、測(cè)姿系統(tǒng)誤差精度優(yōu)于3°;當(dāng)無(wú)人機(jī)以10°左右的俯仰角降落時(shí),系統(tǒng)誤差精度優(yōu)于2°。角度越小,誤差精度越高。實(shí)驗(yàn)結(jié)果表明,該姿態(tài)測(cè)量方法符合小型無(wú)人機(jī)降落對(duì)姿態(tài)測(cè)量的應(yīng)用需求,且具有一定的實(shí)用性和優(yōu)越性。5測(cè)距和測(cè)姿誤差的控制本文針對(duì)無(wú)人機(jī)降落階段高精度測(cè)高、測(cè)姿的應(yīng)用需求,提出了利用多路超聲波測(cè)距模塊測(cè)量無(wú)人機(jī)相對(duì)高度及相對(duì)姿態(tài)的新方法,并通過(guò)對(duì)超聲波傳感器數(shù)據(jù)采集方法、測(cè)距、測(cè)姿誤差標(biāo)定和補(bǔ)償