本發(fā)明涉及山地果園植保無(wú)人機(jī)避障技術(shù)領(lǐng)域,尤其涉及一種基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障裝置及方法。
背景技術(shù):
在山地丘陵環(huán)境下,果樹(shù)隨地形地貌的變化而起伏不平,因此果園植保無(wú)人機(jī)在進(jìn)行植保作業(yè)時(shí)飛行狀態(tài)需隨果園果樹(shù)的變化而隨時(shí)進(jìn)行調(diào)整,同時(shí),果園里往往存在著果樹(shù)枝干、電線桿,高壓線等潛在空中隱患,因此,植保無(wú)人機(jī)在山地果園作業(yè)風(fēng)險(xiǎn)比平地作業(yè)大大增加。
目前,在無(wú)人機(jī)避障方面,目前采用的方法主要包括通過(guò)激光雷達(dá)、圖像攝像頭、光流傳感器或者超聲波傳感器來(lái)識(shí)別障礙物,并反饋給飛控,進(jìn)而飛控用于控制無(wú)人機(jī)規(guī)避障礙物。但這些避障方法和技術(shù)均沒(méi)有分級(jí)考慮引入遠(yuǎn)、中、近三級(jí)障礙物探測(cè)及其避障處理方法,從而降低復(fù)雜作業(yè)環(huán)境下無(wú)人機(jī)飛行的可靠性。
因此,設(shè)計(jì)出一種基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障裝置及方法具有較高的理論和實(shí)際意義。
技術(shù)實(shí)現(xiàn)要素:
為了克服現(xiàn)有技術(shù)存在的缺點(diǎn)與不足,本發(fā)明提供一種基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障裝置及方法,滿足山地果園環(huán)境下的地形、地貌、植被在復(fù)雜多變情況下果園無(wú)人機(jī)作業(yè)可靠的需要。
為解決上述技術(shù)問(wèn)題,本發(fā)明提供如下技術(shù)方案:一種基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障裝置,包括均與無(wú)人機(jī)飛控連接的微波雷達(dá)、若干超聲波傳感器以及若干近程紅外傳感器陣列,所述微波雷達(dá)用于探測(cè)無(wú)人機(jī)的遠(yuǎn)距離障礙物信息,所述超聲波傳感器用于探測(cè)無(wú)人機(jī)周邊中距離障礙物信息,所述近程紅外傳感器用于探測(cè)無(wú)人機(jī)近距離障礙物信息。
進(jìn)一步地,所述超聲波傳感器和近程紅外傳感器陣列數(shù)量均為4個(gè),所述微波雷達(dá)設(shè)置于無(wú)人機(jī)飛控頂部,所述超聲波傳感器和近程紅外傳感器陣列均設(shè)置于無(wú)人機(jī)機(jī)翼上;所述近程紅外傳感器陣列包括5個(gè)近程紅外傳感器,所述近紅外傳感器相互之間夾角為90度,用于獲取近距離障礙物信息。
本發(fā)明另一目的是提供一種基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障方法,包括下述步驟:
S1、在無(wú)人機(jī)飛行過(guò)程中,微波雷達(dá)對(duì)無(wú)人機(jī)360度范圍內(nèi)的物體進(jìn)行掃描,探測(cè)物體與無(wú)人機(jī)之間的距離,并建立遠(yuǎn)距離避障指數(shù)函數(shù);
S2、無(wú)人機(jī)持續(xù)更新遠(yuǎn)距離避障指數(shù)函數(shù)的遠(yuǎn)距離避障指數(shù),若前行路徑方向的遠(yuǎn)距離避障指數(shù)大于最小遠(yuǎn)距離避障指數(shù)的設(shè)定值,則無(wú)人機(jī)在前行路徑方向的正負(fù)10度范圍內(nèi),選擇最小避障指數(shù)值的方向進(jìn)行飛行;無(wú)人機(jī)在最小避障指數(shù)值的方向飛行時(shí),若最小避障指數(shù)值小于設(shè)定值,則無(wú)人機(jī)持續(xù)更新遠(yuǎn)距離避障指數(shù)函數(shù)的遠(yuǎn)距離避障指數(shù);若最小避障指數(shù)值大于等于設(shè)定值,則啟動(dòng)超聲波傳感器,建立中距離避障指數(shù)函數(shù);
S3、無(wú)人機(jī)持續(xù)更新中距離避障指數(shù)函數(shù)的中距離避障指數(shù),若中距離避障指數(shù)小于給定值,則關(guān)閉超聲波傳感器,返回步驟S2;若中距離避障指數(shù)大于另一給定值,則啟動(dòng)近程紅外傳感器陣列;
S4、近程紅外傳感器陣列探測(cè)周邊的障礙物距離,建立近距離避障指數(shù)函數(shù);
S5、無(wú)人機(jī)持續(xù)更新近距離避障指數(shù)函數(shù)的近距離避障指數(shù),并根據(jù)近距離避障指數(shù)調(diào)整飛行路徑;
S6、若最小近距離避障指數(shù)大于調(diào)整值,則無(wú)人機(jī)進(jìn)行180度調(diào)整飛行方向,并選擇最小近距離避障指數(shù)方向飛行;
S7、無(wú)人機(jī)進(jìn)行180度調(diào)整飛行方向后,若此時(shí)的近距離避障指數(shù)小于調(diào)整值,則關(guān)閉近程紅外傳感器陣列并返回步驟S2;若此時(shí)的近距離避障指數(shù)大于等于調(diào)整值,則返回步驟S6。
進(jìn)一步地,所述步驟S1的遠(yuǎn)距離避障指數(shù)函數(shù)為:
其中,α(θ)表示遠(yuǎn)距離避障指數(shù),θ表示方向角,以正北方向?yàn)榱愣冉?,按順時(shí)針?lè)较蜻M(jìn)行角度的增加;d(θ)表示距離障礙物的距離。
進(jìn)一步地,所述步驟S2,若當(dāng)前行徑方向的遠(yuǎn)距離避障指數(shù)大于0.3,0.3所述設(shè)定值,則無(wú)人機(jī)在前行路徑方向的正負(fù)10度范圍內(nèi),選擇最小避障指數(shù)值αmin(θ)的方向進(jìn)行飛行,最小避障指數(shù)值αmin(θ)為:
αmin(θ)=min(α(θ)),
若最小避障指數(shù)值αmin(θ)小于0.30,則無(wú)人機(jī)持續(xù)更新遠(yuǎn)距離避障指數(shù)函數(shù)的遠(yuǎn)距離避障指數(shù),最小避障指數(shù)值αmin(θ)大于等于0.30,則啟動(dòng)超聲波傳感器,建立中距離避障指數(shù)β(φ)函數(shù),所述中距離避障指數(shù)β(φ)函數(shù)為:
其中,φi表示第i個(gè)超聲波傳感器的方向角,以正北方向?yàn)榱愣冉?;i等于1、2、3、4,分別表示安裝在無(wú)人機(jī)上的四個(gè)超聲波傳感器;m(φi)表示超聲波傳感器探測(cè)到的距離障礙物的距離。
進(jìn)一步地,所述步驟S3,若前行路徑方向的中距離避障指數(shù)小于0.03,0.03為所述給定值,則關(guān)閉超聲波傳感器,返回步驟S2;若前行路徑方向的大于0.07,0.07為所述另一給定值,則無(wú)人機(jī)將飛行速度減半,同時(shí)啟動(dòng)近程紅外傳感器陣列。
進(jìn)一步地,所述步驟S4中距離避障指數(shù)γ(xij,yij)函數(shù)為:
其中,xij表示球面坐標(biāo)的方位角,yij表示球面坐標(biāo)的仰角;其中i等于1、2、3、4,分別表示無(wú)人機(jī)上的四個(gè)近程紅外傳感器陣列;j等于1、2、3、4、5,分別表示紅外近程傳感器陣列中相互垂直的5個(gè)近程紅外傳感器;c(xij,yij)表示近程紅外傳感器陣列探測(cè)到的距離障礙物的距離。
進(jìn)一步地,所述步驟S5中根據(jù)距離避障指數(shù)調(diào)整飛行路徑,具體為:
S51、若前行路徑方向的近距離避障指數(shù)小于0.012,則關(guān)閉近程紅外傳感器陣列,返回步驟S3;否則,進(jìn)入下一步驟;
S52、若前行路徑方向的近距離避障指數(shù)γ(xij,yij)小于0.03,則繼續(xù)飛行;若大于等于0.03,無(wú)人機(jī)將飛行速度減半,在前行路徑方向的正負(fù)90度范圍內(nèi),選擇最小近距離避障指數(shù)值的方向進(jìn)行飛行,并進(jìn)入步驟S6,其中最小近距離避障指數(shù)值表示為:
γmin(xij,yij)=min(γmin(xij,yij))
其中,
進(jìn)一步地,所述步驟S6,具體為:
若最小近距離避障指數(shù)值大于0.03,0.03為所述調(diào)整值,則無(wú)人機(jī)首先180度調(diào)整其飛行方向,即:
接著,無(wú)人機(jī)選擇最小近距離避障指數(shù)方向飛行;
所述步驟S6的持續(xù)時(shí)間為1秒,在1秒的持續(xù)時(shí)間內(nèi):
若近距離避障指數(shù)小于0.03,則繼續(xù)飛行;
若近距離避障指數(shù)大于0.03,則無(wú)人機(jī)保持懸停,并等待人工干預(yù)操作。
進(jìn)一步地,所述步驟S7,具體為:
若近距離避障指數(shù)小于0.03,0.03為所述調(diào)整值,則關(guān)閉近程紅外傳感器陣列,并返回步驟S2,同時(shí)方向角更新為:
若近距離避障指數(shù)大于等于0.03,則返回步驟S6。
采用上述技術(shù)方案后,本發(fā)明至少具有如下有益效果:
(1)、本發(fā)明方法采用遠(yuǎn)、中、近三級(jí)障礙物探測(cè)及其避障處理方法,提高了無(wú)人機(jī)飛行作業(yè)的可靠性;
(2)、本發(fā)明方法的近距離探測(cè)采用近程紅外傳感器陣列,具有成本較低、安裝簡(jiǎn)單、耗電小、體積小和精度高等優(yōu)點(diǎn),滿足果園無(wú)人機(jī)近距離障礙物高精度實(shí)時(shí)探測(cè)的需要。
附圖說(shuō)明
圖1是本發(fā)明基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障裝置的結(jié)構(gòu)示意圖。
具體實(shí)施方式
需要說(shuō)明的是,在不沖突的情況下,本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互結(jié)合,下面結(jié)合附圖和具體實(shí)施例對(duì)本申請(qǐng)作進(jìn)一步詳細(xì)說(shuō)明。
本實(shí)施例所述基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障裝置,如圖1所示,包括旋翼1、飛控2、微波雷達(dá)3、超聲波傳感器4、近程紅外傳感器陣列5。如圖1所示,旋翼1、微波雷達(dá)3、超聲波傳感器4、近程紅外傳感器陣列5均與飛控2相連。
如圖1所示,所述微波雷達(dá)3,部署在植保無(wú)人機(jī)正中的無(wú)人機(jī)飛控2頂部,用于探測(cè)無(wú)人機(jī)360度方向上的遠(yuǎn)距離障礙物信息,遠(yuǎn)距離范圍一般指的是1-100米。
如圖1所示,所述超聲波傳感器4,部署在植保無(wú)人機(jī)部署在植保無(wú)人機(jī)的四個(gè)方向,相對(duì)于機(jī)身方向,向外安裝,用于探測(cè)無(wú)人機(jī)周邊中距離障礙物信息,中距離的范圍一般為1-100分米。所述超聲波傳感器4共有四個(gè),分別部署在植保無(wú)人機(jī)機(jī)翼上的四個(gè)方向。
所述紅外近程傳感器陣列5,該陣列包括5個(gè)近紅外傳感器,這五個(gè)近紅外傳感器相互之間夾角為90度,可獲取上、下、左、右和沿機(jī)翼外側(cè)的五個(gè)方向的近距離障礙物信息,近距離的范圍一般為1-100厘米,從而可探測(cè)除機(jī)身方向外,其他五個(gè)方向的近距離障礙物距離。所述紅外近程傳感器陣列5共有四組,分別部署在植保無(wú)人機(jī)機(jī)翼上的四個(gè)方向。
本實(shí)施例中基于上述裝置的基于三級(jí)避障機(jī)制的果園植保無(wú)人機(jī)避障方法,步驟是:果園無(wú)人機(jī)通過(guò)微波雷達(dá)獲取遠(yuǎn)距離障礙物信息,并計(jì)算遠(yuǎn)距離避障指數(shù),選取遠(yuǎn)距離避障指數(shù)較小的方向飛行,如障礙物持續(xù)接近無(wú)人機(jī),并使得遠(yuǎn)距離障礙物避障指數(shù)大于0.3,則啟動(dòng)超聲波傳感器,探測(cè)中距離障礙物信息,并計(jì)算中距離避障指數(shù),一旦中距離障礙物避障指數(shù)大于0.07,則啟動(dòng)近程紅外傳感器陣列,并計(jì)算近距離避障指數(shù),果園無(wú)人機(jī)根據(jù)近距離避障指數(shù)持續(xù)調(diào)整其飛行方向。
具體包括以下步驟:
(1)微波雷達(dá)對(duì)無(wú)人機(jī)360度范圍內(nèi)的物體進(jìn)行掃描,探測(cè)其無(wú)無(wú)人機(jī)之間的距離,建立遠(yuǎn)距離避障指數(shù)α(θ)函數(shù):
其中θ表示方向角,以正北方向?yàn)榱愣冉牵错槙r(shí)針?lè)较蚪嵌仍黾?;d(θ)表示距離障礙物的距離,以米為單位。
(2)無(wú)人機(jī)在飛行過(guò)程中,持續(xù)更新遠(yuǎn)距離避障指數(shù)α(θ),當(dāng)前行路徑方向的避障指數(shù)大于0.3,則其在前行路徑方向的正負(fù)10度范圍內(nèi),選擇最小避障指數(shù)值αmin(θ)的方向進(jìn)行飛行:
(3)如果αmin(θ)小于0.30,則回到步驟(2),如果αmin(θ)大于0.3,則進(jìn)入步驟(4)。
(4)啟動(dòng)超聲波傳感器,建立中距離避障指數(shù)β(φ)函數(shù):
其中φi表示第i個(gè)超聲波傳感器的方向角,以正北方向?yàn)榱愣冉牵琲等于1、2、3、4,分別表示安裝在無(wú)人機(jī)上的四個(gè)超聲波傳感器,m(φi)表示超聲波傳感器探測(cè)到的距離障礙物的距離,以分米為單位。
(5)無(wú)人機(jī)在飛行過(guò)程中,持續(xù)更新避障指數(shù)β(φi),當(dāng)前行路徑方向的中距離避障指數(shù)小于0.03,則關(guān)閉超聲波傳感器,返回步驟(2),否則進(jìn)入步驟(6)。
(6)當(dāng)前行路徑方向的大于0.07,則其飛行速度減半,同時(shí)啟動(dòng)近程紅外傳感器陣列。
(7)近程紅外傳感器陣列探測(cè)周邊的障礙物距離,建立近距離避障指數(shù)γ(xij,yij)函數(shù):
其中xij表示球面坐標(biāo)的方位角,yij表示球面坐標(biāo)的仰角,其中i等于1、2、3、4,分別表示無(wú)人機(jī)上的四個(gè)近程紅外傳感器陣列,j等于1、2、3、4、5,分別代表紅外近程傳感器陣列中相互垂直的五個(gè)近程紅外傳感器,c(xij,yij)表示近程紅外傳感器陣列探測(cè)到的距離障礙物的距離,以厘米為單位。
(8)無(wú)人機(jī)在飛行過(guò)程中,持續(xù)更新近距離避障指數(shù)γ(xij,yij),并根據(jù)該近距離避障指數(shù)調(diào)整飛行路徑。
所述步驟(8)中,飛行路徑調(diào)整過(guò)程具體如下:
(8-1)當(dāng)前行路徑方向的近距離避障指數(shù)小于0.012,則關(guān)閉近程紅外傳感器陣列,返回步驟(5),否則進(jìn)入步驟(8)。
(8-2)當(dāng)前行路徑方向的近距離避障指數(shù)γ(xij,yij)小于0.03,則繼續(xù)飛行,如果大于0.03其飛行速度減半,在前行路徑方向的正負(fù)90度范圍內(nèi),選擇最小近距離避障指數(shù)值的方向進(jìn)行飛行,并進(jìn)入步驟(9):
(9)如最小近距離避障指數(shù)值大于0.03,則無(wú)人機(jī)首先180度調(diào)整其飛行方向,即:
接著,無(wú)人機(jī)選擇最小近距離避障指數(shù)方向飛行:
所述步驟(9)持續(xù)時(shí)間為1秒,在1秒持續(xù)時(shí)間內(nèi):
(9-1)如近距離避障指數(shù)小于0.03,則繼續(xù)飛行;
(9-2)如近距離避障指數(shù)大于0.03,則無(wú)人機(jī)保持懸停,并等待人工干預(yù);
(10)無(wú)人機(jī)180度調(diào)整其飛行方向,
(11)如近距離避障指數(shù)小于0.03,則關(guān)閉近程紅外傳感器陣列,并返回步驟(2),同時(shí)方向角更新為:
如近距離避障指數(shù)大于0.03,則關(guān)閉近程紅外傳感器陣列,并繼續(xù)返回步驟(9)。
盡管已經(jīng)示出和描述了本發(fā)明的實(shí)施例,對(duì)于本領(lǐng)域的普通技術(shù)人員而言,可以理解的是,在不脫離本發(fā)明的原理和精神的情況下可以對(duì)這些實(shí)施例進(jìn)行多種等效的變化、修改、替換和變型,本發(fā)明的范圍由所附權(quán)利要求及其等同范圍限定。