一種基于瞬時(shí)碰撞點(diǎn)的無(wú)人機(jī)非協(xié)作式實(shí)時(shí)避障方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明主要涉及到無(wú)人機(jī)領(lǐng)域,特指一種基于瞬時(shí)碰撞點(diǎn)的無(wú)人機(jī)非協(xié)作式實(shí)時(shí) 避障方法。
【背景技術(shù)】
[0002] 隨著無(wú)人機(jī)系統(tǒng)能力的不斷發(fā)展,眾多功能各異的無(wú)人機(jī)被廣泛應(yīng)用于各種民事 行動(dòng)中,這也導(dǎo)致無(wú)人機(jī)在使用空域數(shù)量的迅速增加。目前,無(wú)人機(jī)系統(tǒng)的自主能力普遍不 高,其操作使用主要是由地面站遙控或預(yù)編程的,沒(méi)有空間障礙的感知和規(guī)避(Sense and Avoid, SAA)能力,從而導(dǎo)致空中碰撞事故頻頻發(fā)生。
[0003] 目前,無(wú)人機(jī)系統(tǒng)實(shí)現(xiàn)感知與規(guī)避主要有兩種途徑:
[0004] 第一種途徑:通過(guò)地基感知與規(guī)避,在地面站基于無(wú)人機(jī)的狀態(tài)測(cè)控?cái)?shù)據(jù),實(shí)現(xiàn)同 一空域內(nèi)執(zhí)行不同任務(wù)的無(wú)人機(jī)之間時(shí)空沖突消解。
[0005] 第二種途徑:隨著傳感器技術(shù)和自動(dòng)化技術(shù)的進(jìn)步,實(shí)現(xiàn)機(jī)載的感知與規(guī)避。這也 分為兩大類,一類是安裝了 ADS-B(廣播式自動(dòng)相關(guān)監(jiān)視)和TCAS(交通預(yù)警和避撞系統(tǒng)) 的無(wú)人機(jī)之間的協(xié)作式避碰;另一類是未安裝ADS-B或者TCAS的無(wú)人機(jī)或者該裝置失靈條 件下無(wú)人機(jī)之間,以及無(wú)人機(jī)與空中其他障礙(高山、高建筑等)之間的非協(xié)作式避碰。非 協(xié)作式避碰作為無(wú)人機(jī)依托機(jī)載傳感器設(shè)備及時(shí)檢測(cè)障礙并實(shí)施有效規(guī)避的重要保障,對(duì) 于提高無(wú)人機(jī)飛行安全具有更為重要的意義,其技術(shù)已成為當(dāng)前研宄的熱點(diǎn)。
[0006] 非協(xié)作式避碰主要有三種方式:主動(dòng)系統(tǒng)、被動(dòng)系統(tǒng)和混合系統(tǒng)。主動(dòng)系統(tǒng)能發(fā)射 信號(hào)來(lái)檢測(cè)障礙物,包括機(jī)載微波雷達(dá)、毫米波雷達(dá)、激光、聲納、主動(dòng)電子掃描陣列(AESA) 雷達(dá)等主動(dòng)傳感器。被動(dòng)系統(tǒng)則用于檢測(cè)從障礙物散發(fā)的信號(hào),主要包括光電(EO)、紅外 (IR)等被動(dòng)傳感器?;旌舷到y(tǒng)采用主動(dòng)傳感器和被動(dòng)傳感器混合的探測(cè)模式。非協(xié)作式避 碰無(wú)需其它無(wú)人機(jī)擁有相同的系統(tǒng),可用于檢測(cè)包括飛機(jī)在內(nèi)的地面、空中的障礙物。
【發(fā)明內(nèi)容】
[0007] 本發(fā)明要解決的技術(shù)問(wèn)題就在于:針對(duì)現(xiàn)有技術(shù)存在的技術(shù)問(wèn)題,本發(fā)明提供一 種原理簡(jiǎn)單、易實(shí)現(xiàn)、能夠提高無(wú)人機(jī)安全性的基于瞬時(shí)碰撞點(diǎn)的無(wú)人機(jī)非協(xié)作式實(shí)時(shí)避 障方法。
[0008] 為解決上述技術(shù)問(wèn)題,本發(fā)明采用以下技術(shù)方案:
[0009] 一種基于瞬時(shí)碰撞點(diǎn)的無(wú)人機(jī)非協(xié)作式實(shí)時(shí)避障方法,其步驟為:
[0010] (1)障礙探測(cè):獲取障礙的相對(duì)運(yùn)動(dòng)狀態(tài);
[0011] ⑵障礙運(yùn)動(dòng)估計(jì):基于衛(wèi)星導(dǎo)航信息獲取本機(jī)狀態(tài)信息,計(jì)算出慣性空間障礙 的運(yùn)動(dòng)狀態(tài)
[0012] (3)碰撞沖突檢測(cè):判斷是否會(huì)與障礙發(fā)生碰撞;
[0013] (4)避障決策:基于步驟(3)的檢測(cè)結(jié)果,做出避障決策;
[0014] (5)基于考慮瞬時(shí)碰撞點(diǎn)的快速擴(kuò)展隨機(jī)樹(shù)算法的避障航線重規(guī)劃:基于瞬時(shí)碰 撞點(diǎn),引入航線評(píng)價(jià)啟發(fā)信息,實(shí)現(xiàn)避障航線重規(guī)劃。
[0015] 作為本發(fā)明的進(jìn)一步改進(jìn):所述步驟(5)的具體步驟為:
[0016] (5. 1):以當(dāng)前無(wú)人機(jī)的位置作為初始節(jié)點(diǎn)Nodeinit,初始化搜索樹(shù)結(jié)構(gòu),只包含一 個(gè)節(jié)點(diǎn);根據(jù)預(yù)測(cè)碰撞算法,得到預(yù)測(cè)碰撞時(shí)間T rallisim,以瞬時(shí)碰撞點(diǎn)為圓心,以無(wú)人機(jī)安 全距離Rsafe為半徑,形成預(yù)測(cè)碰撞區(qū)域Region Mllisim,將當(dāng)前本機(jī)位置、預(yù)測(cè)碰撞點(diǎn)和障礙 位置形成的三角形區(qū)域,稱為航線規(guī)避區(qū)Region avtjid;
[0017] (5. 2)基于基本RRT流程,按照以下步驟擴(kuò)展搜索樹(shù):
[0018] (5· 2· 1)產(chǎn)生隨機(jī)數(shù) P e [0, 1],如果 P〈Pjlj選擇 Node gMl作為目標(biāo)點(diǎn) Node target, 否則在未搜索區(qū)域范圍內(nèi)產(chǎn)生一個(gè)位于障礙區(qū)域外產(chǎn)生隨機(jī)點(diǎn)Noderand;g隨機(jī)點(diǎn)Node ^nd 未落入預(yù)測(cè)碰撞區(qū)和航線規(guī)避區(qū),則選取Noderand作為目標(biāo)點(diǎn)Node tmget,否則繼續(xù)生成 隨機(jī)點(diǎn)Noderand; 、%辦二分別落入了航線規(guī)避區(qū)Region avtji^預(yù)測(cè)碰撞區(qū)域 Regionrallisim,則將這兩個(gè)隨機(jī)點(diǎn)排除,選擇作為目標(biāo)點(diǎn)Nodetmget;
[0019] (5. 2. 2)在當(dāng)前生成樹(shù)T中,查詢與Nodetargrt最近的節(jié)點(diǎn),記為Node ,根據(jù)飛機(jī) 行進(jìn)步長(zhǎng),計(jì)算得到行進(jìn)節(jié)點(diǎn)Nodetemp,并判斷與Nodenem的運(yùn)動(dòng)時(shí)間t是否落入預(yù)測(cè)碰撞 時(shí)間T etjllisira^ ;如果t在T WlisimP^,則繼續(xù)判斷Node temp是否落入航線規(guī)避區(qū)Region av()id 和預(yù)測(cè)碰撞區(qū)域Regionrallisim,如果沒(méi)有,則轉(zhuǎn)入(5. 2. 3),否則舍棄隨機(jī)點(diǎn)Nodetemp并轉(zhuǎn)入 (5. 2. 1);
[0020] (5. 2. 3)將擴(kuò)展節(jié)點(diǎn)Nodetemp記為Node new,并加入搜索樹(shù)T中,作為Nodenem的子節(jié) 占 .
[0021] (5. 2. 4)如果I I N〇denew-N〇degMl I I彡ε,則搜索到目標(biāo)點(diǎn),跳到步驟(5. 3);否則 更新計(jì)算航線規(guī)避區(qū)Region-jPl預(yù)測(cè)碰撞區(qū)域Region eaLlisim以及碰撞時(shí)間T eaLlisim,并返 回步驟(5. 2);
[0022] (5. 2. 5)如果搜索時(shí)間超過(guò)搜索時(shí)間上限Tmax,則強(qiáng)制結(jié)束擴(kuò)展,跳到步驟(5. 3);
[0023] (5. 3)返回形成的擴(kuò)展搜索樹(shù),獲得Nodeinit從到Node 8。31的航線;如果是強(qiáng)制結(jié) 束,則返回從Nodeinit到距離Node _最近的葉節(jié)點(diǎn)的航線。
[0024] 作為本發(fā)明的進(jìn)一步改進(jìn):所述步驟(3)的具體步驟為:
[0025] (3. 1)分別求出障礙物速度以及無(wú)人機(jī)速度與基準(zhǔn)線的夾角分別為a,β,將障礙 物速度V,無(wú)人機(jī)的速度V b分解到垂直于兩者質(zhì)心的連線,得到V al和V bl:
[0026] Val=Vasina (1)
[0027] Vbl= V bsin β (2)
[0028] 將約束條件加強(qiáng),障礙物在無(wú)人機(jī)的前方需滿足如下條件:
[0029] Vacos a >〇 (3)
[0030] Vbcos β >0 (4)
[0031] (3.2)判斷:
[0032] 若Val= Vbl,則在當(dāng)前條件下飛行無(wú)人機(jī)與障礙物會(huì)發(fā)生碰撞,并得到瞬時(shí)碰撞點(diǎn) 的坐標(biāo)位置;若檢測(cè)到能夠發(fā)生碰撞,碰撞的時(shí)間約束通過(guò)計(jì)算得知,得出無(wú)人機(jī)和障礙物 相距距離為S,不進(jìn)行避障發(fā)生碰撞的時(shí)間I rallisim