本發(fā)明屬于無(wú)人飛行器空間避障飛行的控制領(lǐng)域，尤其是涉及一種基于速度障礙圓弧法的無(wú)人飛行器避障方法，該方法針對(duì)的是二維平面內(nèi)的無(wú)人飛行器避障問(wèn)題。

背景技術(shù)：

無(wú)人飛行器在任務(wù)飛行過(guò)程中可利用自身攜帶的傳感器裝置探測(cè)到周圍存在的動(dòng)靜態(tài)障礙物(后文將無(wú)人飛行器探測(cè)到的障礙物稱為已知障礙物)，這些動(dòng)靜態(tài)障礙物可能會(huì)影響無(wú)人飛行器的安全飛行。通常，為了簡(jiǎn)化對(duì)障礙物威脅的判定，將無(wú)人飛行器假設(shè)為一質(zhì)點(diǎn)，其當(dāng)前時(shí)刻位置坐標(biāo)為P_u(x_u,y_u)，同時(shí)根據(jù)無(wú)人飛行器和障礙物之間的相對(duì)尺寸大小將已知障礙物點(diǎn)O_i膨化為以P_oi為圓心、半徑為R_i(i＝1,2,…)的障礙圓，然后過(guò)點(diǎn)P_u作障礙圓的切線l_i1和l_i2，則l_i1和l₂形成二維速度障礙錐。

目前，主要通過(guò)無(wú)人飛行器和障礙物之間的相對(duì)速度矢量v_uoi和位置矢量之間的夾角α_i與障礙錐的半頂角α_0i大小關(guān)系進(jìn)行障礙威脅判斷。當(dāng)α_i≥α_0i時(shí)，已知障礙物O_i對(duì)無(wú)人飛行器沿當(dāng)前航跡飛行不具有威脅；當(dāng)α_i＜α_0i時(shí)，已知障礙物O_i對(duì)無(wú)人飛行器安全飛行產(chǎn)生威脅，無(wú)人飛行器需要對(duì)已知障礙物O_i進(jìn)行避碰。

但是，根據(jù)上述理論確定無(wú)人飛行器需要進(jìn)行避碰的障礙物時(shí)，卻忽略了暫時(shí)不具威脅障礙的“潛在危險(xiǎn)”。有些情況下，當(dāng)無(wú)人飛行器沿避碰重規(guī)劃的航跡飛行時(shí)，之前不具威脅障礙會(huì)對(duì)無(wú)人飛行器飛行產(chǎn)生威脅。為了確保無(wú)人飛行器避碰重規(guī)劃航跡的安全性，需要提供一種無(wú)人飛行器避碰方法，可綜合考慮已知障礙的“危險(xiǎn)”和“潛在危險(xiǎn)”。

而且，現(xiàn)有技術(shù)中的避碰算法很難解決多威脅障礙避碰的問(wèn)題。諸如文獻(xiàn)Mujumdar A,Padhi R.Reactive collision avoidance using nonlinear geometric and differential geometric guidance[J].Journal of guidance,Control,and Dynamics,2011,34(01):303-310.等根據(jù)最短接近點(diǎn)提出一種反應(yīng)式避障算法，通過(guò)單視覺(jué)傳感器獲取威脅障礙信息，但算法針對(duì)單威脅障礙避碰問(wèn)題，不能解決多威脅障礙避碰復(fù)雜問(wèn)題。而文獻(xiàn)Van den Berg J,Lin M,Manocha D.Reciprocal velocity obstacles for real-time multi-agent navigation[C]//IEEE InternationalConference on Robotics and Automation,2008:1928-1935.在多智能體飛行避碰中提出了一種互惠速度障礙法，用于解決多智能體相互之間的避碰，但該方法很難適應(yīng)復(fù)雜環(huán)境下的動(dòng)態(tài)避碰情況。

綜上所述，現(xiàn)有技術(shù)中缺乏合適的無(wú)人飛行器避障方法，來(lái)解決因忽略“潛在危險(xiǎn)”障礙而帶來(lái)的影響，以及解決多障礙避碰的復(fù)雜問(wèn)題。

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

本發(fā)明的目的在于提供一種無(wú)人飛行器避障方法，其能夠有效避免現(xiàn)有避障算法中因忽略“潛在危險(xiǎn)”障礙而帶來(lái)的影響，并能解決多障礙避碰的復(fù)雜問(wèn)題。

本發(fā)明提出的一種基于速度障礙圓弧法的無(wú)人飛行器避障方法，包括以下步驟：

步驟S1，構(gòu)建二維速度障礙錐；

根據(jù)無(wú)人飛行器在二維空間中的當(dāng)前時(shí)刻位置坐標(biāo)P_u(x_u，y_u)和探測(cè)到的已知障礙物O_i(i＝1,2,…)的位置坐標(biāo)P_oi(x_oi，y_oi)，將已知障礙物點(diǎn)O_i膨化為以P_oi為圓心、半徑為R_i(i＝1,2,…)的障礙圓，然后過(guò)點(diǎn)P_u作障礙圓的切線l_i1和l_i2，則l_i1和l_i2形成二維速度障礙錐，將該二維速度障礙錐內(nèi)部空間稱為相對(duì)碰撞區(qū)RCC；

步驟S2，建立速度障礙圓弧G；

將RCC沿已知障礙物O_i的速度矢量v_oi方向平移||v_oi||，得到絕對(duì)碰撞區(qū)ACC，切線l_i1和l_i2平移后記為l′_i1和l′_i2；

以無(wú)人飛行器位置點(diǎn)P_u為圓心，速度矢量大小||v_u||為半徑的速度圓⊙P_u，然后取速度圓⊙P_u與ACC相交且位于ACC內(nèi)部的圓弧為速度障礙圓弧G，即G＝⊙P_u∩ACC；速度圓⊙P_u與ACC邊界線l′_i1和l′_i2的交點(diǎn)為避障的兩個(gè)臨界狀態(tài)點(diǎn)P_i1和P_i2；

步驟S3，定義已知障礙物的威脅等級(jí)，并進(jìn)行避碰判斷；

當(dāng)無(wú)人飛行器的速度矢量v_u∈ACC時(shí)，將已知障礙物O_i定義為一級(jí)威脅障礙物；當(dāng)且時(shí)，將已知障礙物O_i定義為二級(jí)威脅障礙物；當(dāng)且時(shí)，將已知障礙物O_i定義為三級(jí)威脅障礙物；

對(duì)于一級(jí)威脅障礙物，需要進(jìn)行避碰；對(duì)于二級(jí)威脅障礙物，需要分析二級(jí)威脅障礙物在無(wú)人飛行器避碰過(guò)程中產(chǎn)生的影響；對(duì)于三級(jí)威脅障礙物，不需要對(duì)障礙物進(jìn)行避碰，則退出；

步驟S4，求解速度障礙圓弧參數(shù)；

步驟S5，求解避障方向。

優(yōu)選的，步驟S3中避碰判斷具體采用以下方法：

對(duì)于探測(cè)到的i(i＝1,2,…)個(gè)已知障礙物O_i，當(dāng)其中至少存在1個(gè)一級(jí)威脅障礙物時(shí)，需要進(jìn)行避碰；否則不需要對(duì)障礙物進(jìn)行避碰，則退出。

優(yōu)選的，步驟S3中的分析二級(jí)威脅障礙物在無(wú)人飛行器避碰過(guò)程中產(chǎn)生影響的具體方法為：

當(dāng)某個(gè)二級(jí)威脅障礙物產(chǎn)生的速度障礙圓弧與一級(jí)威脅障礙物的速度障礙圓弧相交時(shí)，則將該二級(jí)威脅障礙物看作一級(jí)威脅障礙物；

否則，在步驟S5后增加：步驟S6，在求解的避障方向中剔除該二級(jí)威脅障礙物速度障礙圓弧對(duì)應(yīng)的不可行速度矢量方向范圍，其中不可行速度矢量方向范圍定義為：若無(wú)人飛行器的速度矢量方向保持在某一范圍內(nèi)時(shí)，將與已知威脅障礙物發(fā)生碰撞，則稱該矢量方向范圍為不可行速度矢量范圍。

優(yōu)選的，步驟S4具體為：

步驟S41，將已知障礙物O_i的速度障礙圓弧表示為G_i＝(r_i,β_i，γ_i)，其中Q_oi＝(P_i1+P_i2)/2；β_i為矢量的方向角；γ_i對(duì)應(yīng)于速度障礙圓弧的1/2圓心角；

步驟S42，根據(jù)速度矢量關(guān)系，求解矢量后，進(jìn)而求解速度障礙圓弧參數(shù)。

優(yōu)選的，步驟S5具體為：

步驟S51，建立動(dòng)坐標(biāo)系；

避碰時(shí)，以無(wú)人飛行器的速度矢量v_u方向?yàn)榭v軸y，速度矢量v_u順時(shí)針旋轉(zhuǎn)90°方向?yàn)闄M軸x，建立動(dòng)坐標(biāo)系P_uxy；

步驟S52，設(shè)定避碰規(guī)則；

假設(shè)無(wú)人飛行器對(duì)已知障礙物避碰是通過(guò)法向加速度a_u進(jìn)行機(jī)動(dòng)，不改變無(wú)人飛行器速度矢量v_u的大小，即通過(guò)改變無(wú)人飛行器的速度矢量v_u方向進(jìn)行避碰；假設(shè)避碰過(guò)程中無(wú)人飛行器速度矢量v_u的方向角β_u取值范圍為[-π,π]；

步驟S53，求解避碰方向。

優(yōu)選的，步驟S53具體為：

對(duì)于單個(gè)已知障礙物，依據(jù)求解的速度障礙圓弧參數(shù)，確定避碰不可行速度矢量方向范圍為[β-γ,β+γ]，則相應(yīng)的避碰方向角范圍為[-π,β-γ]∪[β+γ,π]；

對(duì)于多個(gè)已知障礙物，依據(jù)求解的速度障礙圓弧參數(shù)，確定多威脅障礙下避碰不可行速度矢量方向范圍為[min{β_i-γ_i},max{β_i+γ_i}]，則相應(yīng)的避碰方向角范圍為[-π,min{β_i-γ_i}]∪[max{β_i+γ_i},π]。

優(yōu)選的，在求解避碰方向時(shí)，為了直觀地確定無(wú)人飛行器的避障方向，將所有已知障礙物速度障礙圓弧對(duì)應(yīng)的不可行速度矢量方向繪制在同一個(gè)一維坐標(biāo)軸上。

本發(fā)明提出的一種基于速度障礙圓弧法的無(wú)人飛行器避障方法，通過(guò)速度障礙圓弧參數(shù)量化了威脅障礙的影響，綜合考慮了“潛在危險(xiǎn)”障礙的影響，并且簡(jiǎn)化了復(fù)雜環(huán)境下無(wú)人飛行器對(duì)多障礙的避碰問(wèn)題，該方法大幅度提高了無(wú)人飛行器在復(fù)雜環(huán)境下的自主避障能力。

附圖說(shuō)明

圖1為基于速度障礙圓弧法的無(wú)人飛行器避障方法流程圖；

圖2為速度障礙錐示意圖；

圖3為二維平面速度障礙法原理圖；

圖4為速度障礙圓弧示意圖；

圖5為單個(gè)速度障礙圓弧示意圖；

圖6為UAV速度矢量旋轉(zhuǎn)臨界狀態(tài)圖；

圖7為速度矢量幾何關(guān)系圖；

圖8為單個(gè)一級(jí)威脅障礙避碰角度的坐標(biāo)軸表示示意圖；

圖9為兩個(gè)一級(jí)威脅障礙的避碰圓弧示意圖；

圖10為兩個(gè)一級(jí)威脅障礙避碰角度的坐標(biāo)軸表示示意圖；

圖11為一級(jí)和二級(jí)威脅障礙的避碰圓弧示意圖；

圖12為一級(jí)和二級(jí)威脅障礙的避碰圓弧相隔較遠(yuǎn)時(shí)的示意圖；

圖13為一級(jí)和二級(jí)威脅障礙的避碰圓弧相隔較近時(shí)的示意圖。

具體實(shí)施方式

下面結(jié)合附圖1至附圖13，介紹本發(fā)明的具體實(shí)施方式。

如圖1所示，本發(fā)明的基于速度障礙圓弧法的無(wú)人飛行器避障方法主要包括5個(gè)步驟：構(gòu)建二維速度障礙錐；建立速度障礙圓?。欢x已知障礙物的威脅等級(jí)，并進(jìn)行避碰判斷；求解速度障礙圓弧參數(shù)；求解避障方向。

無(wú)人飛行器(以下簡(jiǎn)稱UAV)在任務(wù)飛行過(guò)程中可利用自身攜帶的傳感器裝置探測(cè)到周圍存在的動(dòng)靜態(tài)障礙物(下稱UAV探測(cè)到的障礙物為已知障礙物)，這些動(dòng)靜態(tài)障礙物可能會(huì)影響UAV的安全飛行。因此，需要設(shè)計(jì)障礙威脅判斷準(zhǔn)則，確定障礙物是‘危險(xiǎn)’還是‘潛在危險(xiǎn)’。通常，為了簡(jiǎn)化對(duì)障礙物威脅判定，將UAV假設(shè)為一質(zhì)點(diǎn)，同時(shí)根據(jù)UAV和障礙物之間的相對(duì)尺寸大小對(duì)已知障礙物O_i(i＝1,2,…)進(jìn)行‘膨化’處理，障礙物‘膨化’后的圓半徑記作R_i(i＝1,2,…)。圖2為已知障礙物對(duì)UAV所產(chǎn)生的速度障礙錐示意圖，射線l_i1和l_i2過(guò)UAV質(zhì)點(diǎn)且為障礙圓的切線。

將圖2中速度障礙錐沿已知障礙物的速度矢量v_oi方向平移||v_oi||，得到如圖3所示的二維平面速度障礙法原理圖。在圖3中，障礙物對(duì)UAV形成的障礙錐稱為相對(duì)碰撞區(qū)(RelativeCollision Cone,RCC)，平移后形成的陰影障礙錐區(qū)域稱為絕對(duì)碰撞區(qū)(Absolute Collision Cone,ACC)。那么，根據(jù)示意圖3可對(duì)已知障礙威脅進(jìn)行判斷。

當(dāng)UAV的速度矢量v_u位于ACC內(nèi)時(shí)，已知障礙O_i對(duì)UAV的飛行具有威脅；當(dāng)UAV的速度矢量v_u在ACC之外時(shí)，已知障礙O_i對(duì)UAV的當(dāng)前航跡不會(huì)產(chǎn)生威脅。

假設(shè)UAV對(duì)已知威脅障礙避碰是通過(guò)法向加速度a_u進(jìn)行機(jī)動(dòng)，不改變UAV速度矢量v_u的大小，即通過(guò)改變UAV的速度矢量v_u方向進(jìn)行避碰。那么，以UAV的位置點(diǎn)P_u為圓心、速度矢量大小||v_u||為半徑的速度圓⊙P_u與已知威脅或具有‘潛在危險(xiǎn)’障礙物的ACC產(chǎn)生交集G＝⊙P_u∩ACC，交集G為速度圓⊙P_u在ACC內(nèi)的圓弧，稱為速度障礙圓弧。其中，速度圓⊙P_u與ACC邊界線l′_i1和l′_i2交點(diǎn)為避碰的兩個(gè)臨界狀態(tài)點(diǎn)P_i1和P_i2，示意圖如圖4所示。

為了準(zhǔn)確分析已知障礙的影響，定義已知障礙物的威脅等級(jí)。當(dāng)速度矢量v_u∈ACC時(shí)，定義已知障礙O_i為一級(jí)威脅障礙物；當(dāng)速度矢量且時(shí)，定義已知障礙O_i為二級(jí)威脅障礙物；當(dāng)速度矢量且時(shí)，定義已知障礙O_i為三級(jí)威脅障礙物。

UAV沿航跡飛行過(guò)程中，必須對(duì)一級(jí)威脅障礙物進(jìn)行避碰，且需要分析二級(jí)威脅障礙物在避碰中產(chǎn)生的影響，但可忽略三級(jí)威脅障礙物的影響。為了準(zhǔn)確分析一級(jí)和二級(jí)威脅障礙物的影響，需要對(duì)速度障礙圓弧G進(jìn)行定量表達(dá)。下面給出一級(jí)和二級(jí)威脅障礙物的速度障礙圓弧G定義。

記已知障礙O_i的速度障礙圓弧為G_i＝(r_i，β_i，γ_i)。其中，參數(shù)9i＝(P_i1+P_i2)/2；β_i為矢量的方向角；γ_i對(duì)應(yīng)于速度障礙圓弧的1/2圓心角。

將圖4中已知障礙O_i的速度障礙圓弧表示在UAV速度圓上的示意圖如圖5所示。對(duì)速度障礙圓弧三個(gè)參數(shù)r_i、β_i和γ_i的求解關(guān)鍵在求出矢量而矢量的求解需要求出ACC邊界線l′_i1和l′_i2上的兩個(gè)臨界狀態(tài)點(diǎn)P_i1和P_i2。下面通過(guò)示意圖6中的速度矢量幾何關(guān)系，確定臨界狀態(tài)點(diǎn)P_i1和P_i2所對(duì)應(yīng)的UAV兩個(gè)臨界速度矢量和圖6中的P_u′為平移后速度障礙錐的頂點(diǎn)，P_u′＝P_u+v_o，即UAV的位置坐標(biāo)P_u沿障礙速度矢量v_o方向平移速度矢量大小v_o得到的位置坐標(biāo)。

在示意圖7中，UAV的兩個(gè)臨界速度矢量分別為和由圖6中的速度矢量幾何關(guān)系可得，

${\hat{v}}_u=v_{oi}+{\hat{v}}_{uoi}---\left(1\right)$

${\hat{v}}_u^{\′}=v_{oi}+{\hat{v}}_{uoi}^{\′}---\left(2\right)$

臨界相對(duì)速度矢量和分別位于ACC邊界線l₁和l₂上，且可表示為

其中，P_u′＝P_u+v_oi，q_i1和q_i2分別為ACC邊界線l₁和l₂上的單位方向矢量。

q_i1和q_i2可通過(guò)障礙錐內(nèi)的角度幾何關(guān)系進(jìn)行求解。下面給出q_i1的求解方法，q_i2同理可求。假設(shè)邊界線l₁上存在一點(diǎn)Q_i1(x_i1,y_i1)，則點(diǎn)Q_i1可由式(5)和(6)確定。

其中，δ_i＝α_0i-α_i。

則單位方向矢量q_i1可表示為

那么，臨界相對(duì)速度矢量與障礙物的速度矢量v_oi的夾角可表示為

$cos(\∠({\hat{v}}_{uoi},v_{oi}\left)\right)=cos(\∠(q_{i1},v_{oi}\left)\right)=\frac{q_{i1}\·v_{oi}}{\left|\right|v_{oi}\left|\right|}---\left(8\right)$

根據(jù)速度矢量三角形關(guān)系可得，

$cos(\mathrm{\π}-\∠({\hat{v}}_{uoi},v_{oi}\left)\right)=\frac{\left|\right|{\hat{v}}_{uoi}|{|}^2+|\left|v_{oi}\right|{|}^2-\left|\right|v_u|{|}^2}{2\left|\right|{\hat{v}}_{uoi}\left|\right|\left|\right|v_{oi}\left|\right|}---\left(9\right)$

則由式(7)和(9)可確定臨界相對(duì)速度矢量另一個(gè)臨界相對(duì)速度矢量可同理求得。那么，再由式(1)和(2)可求得UAV的兩個(gè)臨界速度矢量和

為了便于求解已知障礙O_i的速度障礙圓弧參數(shù)，將圖6中的速度矢量幾何關(guān)系抽象到如圖7所示的矢量關(guān)系圖上。其中，Q_oi為線段P_i1P_i2的中點(diǎn)。則根據(jù)圖7的矢量關(guān)系圖可對(duì)矢量進(jìn)行求解。圖7中的矢量關(guān)系可表示為，

其中，

由式(10)和(11)可確定矢量

式(12)確定了已知障礙O_i的速度障礙圓弧矢量則相應(yīng)的速度障礙圓弧參數(shù)r_i、β_i和γ_i可以求解。

其中，X為慣性坐標(biāo)系的水平坐標(biāo)軸。

(一)單個(gè)一級(jí)威脅障礙物的避碰

根據(jù)求解的速度障礙圓弧參數(shù)β和γ，可以確定障礙圓弧所對(duì)應(yīng)的圓心角范圍為[β-γ,β+γ]。為直觀地確定UAV最優(yōu)避障方向，將UAV速度矢量v_u的方向角β_u及障礙圓弧所對(duì)應(yīng)的圓心角范圍[β-γ,β+γ]表示在一維坐標(biāo)軸上，示意圖如圖8所示。那么，從圖8可直觀地確定UAV的最優(yōu)避障方向。

(二)多威脅障礙的避碰

在未知?jiǎng)討B(tài)環(huán)境下，UAV可能同時(shí)探測(cè)到多個(gè)不同威脅等級(jí)的障礙物，下面以兩個(gè)障礙物的情況進(jìn)行分析，三個(gè)及三個(gè)以上的情況可根據(jù)速度障礙圓弧法類似分析。

(1)兩個(gè)一級(jí)威脅障礙物的避碰情況

兩個(gè)一級(jí)威脅障礙物的避碰圓弧示意圖如圖9所示。此時(shí)，UAV的速度矢量v_u位于兩個(gè)一級(jí)威脅障礙物的相交避碰圓弧內(nèi)。根據(jù)速度障礙圓弧法，可確定兩個(gè)一級(jí)威脅障礙物的障礙圓弧參數(shù)G₁＝(r₁,β₁,γ₁)和G₂＝(r₂,β₂,γ₂)。

根據(jù)兩個(gè)一級(jí)威脅障礙圓弧參數(shù)G₁和G₂，可確定障礙圓弧所對(duì)應(yīng)的圓心角范圍為[β₁-γ₁,β₁+γ₁]∪[β₂-γ₂,β₂+γ₂]，UAV速度矢量v_u的方向角β_u和障礙圓弧圓心角范圍一維坐標(biāo)表示如圖10所示。則從圖10中可直觀地確定UAV同時(shí)對(duì)兩個(gè)一級(jí)威脅障礙物避碰的最優(yōu)方向角。

(2)一級(jí)和二級(jí)威脅障礙物的避碰情況

圖11給出了一種一級(jí)和二級(jí)威脅障礙物的避碰圓弧示意圖。此時(shí)，UAV需要對(duì)一級(jí)威脅障礙物進(jìn)行避碰，同時(shí)考慮二級(jí)威脅障礙物的影響。根據(jù)速度障礙圓弧法求解的一級(jí)和二級(jí)威脅障礙物的圓弧參數(shù)，按照兩個(gè)一級(jí)威脅障礙物避碰的最優(yōu)方向角分析方法，可確定UAV的最優(yōu)避碰方向角。此時(shí)，UAV的最優(yōu)避碰方向角不僅考慮了“危險(xiǎn)”障礙的影響，也考慮了“潛在危險(xiǎn)”障礙的影響，提高了UAV避碰的可靠性和安全性。

圖12為一級(jí)和二級(jí)威脅障礙物的避碰圓弧相交情況，其它一級(jí)和二級(jí)威脅障礙物的避碰圓弧情況如圖12和圖13所示，UAV的避碰分析同圖11。

(3)其它威脅障礙的避碰情況

由于UAV避碰過(guò)程中可不考慮三級(jí)威脅障礙物的影響，則對(duì)一級(jí)和三級(jí)威脅障礙物的避碰可簡(jiǎn)化為對(duì)單個(gè)威脅障礙物的避碰；當(dāng)已知障礙中沒(méi)有一級(jí)威脅障礙物時(shí)，UAV不需要對(duì)已知障礙進(jìn)行避碰。

下面以Pythagorean Hodograph曲線路徑規(guī)劃為例對(duì)基于速度障礙圓弧法的避碰算法進(jìn)行驗(yàn)證。

假設(shè)UAV從起飛點(diǎn)P_s(0,0)到目標(biāo)點(diǎn)P_f(500,1000)處執(zhí)行任務(wù)，在未知?jiǎng)討B(tài)環(huán)境下UAV探測(cè)到動(dòng)態(tài)障礙物，并基于速度障礙圓弧法對(duì)UAV在線避障路徑重規(guī)劃進(jìn)行仿真驗(yàn)證。

實(shí)施例1：UAV對(duì)單個(gè)一級(jí)威脅障礙物的避碰

ⅰ)不考慮二級(jí)和三級(jí)威脅障礙物的影響

UAV在飛行到P_u(200,400)處時(shí)，探測(cè)到障礙物O。通過(guò)攜帶的傳感器裝置獲取的障礙物信息為：位置坐標(biāo)P_o(113.9,522.8)、速度矢量大小||v_o||＝40、速度方向角β_o＝15°、膨化半徑R＝45；另外，UAV自身的信息為：速度矢量大小||v_u||＝55、速度方向角β_u＝63.4°。那么，根據(jù)速度障礙圓弧法可計(jì)算得到：已知障礙為一級(jí)威脅障礙物，UAV需要對(duì)威脅障礙進(jìn)行避碰。

通過(guò)速度障礙圓弧參數(shù)計(jì)算方法，可得到已知障礙的圓弧參數(shù)G＝(50.6,84.1°,23.1°)。那么，由障礙圓弧參數(shù)可確定避障點(diǎn)處的避障方向范圍為[-180°,61°]∪[107.2°,180°]。

ⅱ)考慮二級(jí)和三級(jí)威脅障礙物的影響

當(dāng)UAV對(duì)單個(gè)一級(jí)威脅障礙避碰過(guò)程中，同時(shí)也探測(cè)到二級(jí)和三級(jí)威脅障礙物，根據(jù)威脅圓弧參數(shù)計(jì)算方法，可計(jì)算出一級(jí)和二級(jí)威脅障礙物的圓弧參數(shù)G，如表1所示。

表1單個(gè)一級(jí)威脅障礙避碰情況下的圓弧參數(shù)

根據(jù)表1中的威脅障礙圓弧參數(shù)G可確定單個(gè)一級(jí)威脅障礙物避碰情況下的UAV避障方向角范圍，如表2所示。

表2單個(gè)一級(jí)障礙避碰情況下的UAV避障方向角范圍

對(duì)比表2中不同情況下的避障方向角范圍，可直觀地得出二級(jí)威脅障礙物對(duì)UAV自主避障的影響，使可行的避障方向角范圍變小。

實(shí)施例2：UAV對(duì)多個(gè)一級(jí)威脅障礙物的避碰

ⅰ)不考慮二級(jí)和三級(jí)威脅障礙物的影響

當(dāng)UAV在飛行到P_u(200,400)處同時(shí)探測(cè)到兩個(gè)威脅障礙物情況下。首先，通過(guò)傳感器裝置確定障礙物的信息為：位置坐標(biāo)P_o1(114.0,522.9)、P_o2(322.9,486.0)；速度矢量大小||v_o1||＝40、||v_o2||＝35；速度方向角β_o1＝15°、β_o2＝125°；膨化半徑R₁＝45、R₂＝35。根據(jù)速度障礙圓弧法可計(jì)算得到：兩個(gè)已知障礙都為一級(jí)威脅障礙物，UAV需要對(duì)兩個(gè)威脅障礙進(jìn)行避碰。

依據(jù)速度障礙圓弧參數(shù)的計(jì)算方法，相應(yīng)的兩個(gè)已知一級(jí)威脅障礙物的圓弧參數(shù)為G₁＝(50.6,84.1°,23.1°)，G₂＝(53.4,72.2°,13.5°)。那么，由障礙圓弧參數(shù)可確定避障點(diǎn)處的避障方向范圍為[-180°,59.8°]∪[107.2°,180°]。

ⅱ)考慮二級(jí)和三級(jí)威脅障礙物的影響

依據(jù)速度障礙圓弧參數(shù)計(jì)算方法，得出的一級(jí)和二級(jí)威脅障礙圓弧參數(shù)G如表3所示。

表3多個(gè)一級(jí)威脅障礙避碰情況下的圓弧參數(shù)

由表3中威脅障礙的圓弧參數(shù)可確定考慮二級(jí)和三級(jí)威脅障礙物前后的UAV避障方向角范圍，如表4所示。

表4多個(gè)一級(jí)障礙避碰情況下的UAV避障方向角范圍

同樣，對(duì)比表4中不同情況下的UAV避障方向角范圍可得：考慮二級(jí)和三級(jí)威脅障礙物影響后，使得UAV避障方向角范圍變小。因此，UAV在二級(jí)威脅障礙影響下的避障方向角需要重新確定，上述仿真結(jié)果驗(yàn)證了避碰算法的有效性和實(shí)用性。

最后應(yīng)說(shuō)明的是，以上所述僅為本發(fā)明的部分優(yōu)選實(shí)施例，并不用于限制本發(fā)明，盡管參照前述實(shí)施例對(duì)發(fā)明進(jìn)行了詳細(xì)的說(shuō)明，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，其依然可以對(duì)前述各實(shí)施例所記載的技術(shù)方案進(jìn)行修改，或者對(duì)其中部分技術(shù)特征進(jìn)行等同替換。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。