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

背景技術(shù)：

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

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

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

而且，現(xiàn)有技術(shù)中的避碰算法很難解決多威脅障礙避碰的問題。諸如文獻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ù)最短接近點提出一種反應(yīng)式避障算法，通過單視覺傳感器獲取威脅障礙信息，但算法針對單威脅障礙避碰問題，不能解決多威脅障礙避碰復(fù)雜問題。而文獻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)境下的動態(tài)避碰情況。

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

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

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

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

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

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

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

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

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

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

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

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

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

步驟S5，求解避障方向。

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

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

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

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

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

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

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

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

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

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

避碰時，以無人飛行器的速度矢量v_u方向為縱軸y，速度矢量v_u順時針旋轉(zhuǎn)90°方向為橫軸x，建立動坐標(biāo)系P_uxy；

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

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

步驟S53，求解避碰方向。

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

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

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

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

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

附圖說明

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

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

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

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

圖5為單個速度障礙圓弧示意圖；

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

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

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

圖9為兩個一級威脅障礙的避碰圓弧示意圖；

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

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

圖12為一級和二級威脅障礙的避碰圓弧相隔較遠時的示意圖；

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

具體實施方式

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

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

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

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

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

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

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

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

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

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

在示意圖7中，UAV的兩個臨界速度矢量分別為和由圖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)$

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

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

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

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

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

那么，臨界相對速度矢量與障礙物的速度矢量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)可確定臨界相對速度矢量另一個臨界相對速度矢量可同理求得。那么，再由式(1)和(2)可求得UAV的兩個臨界速度矢量和

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

其中，

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

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

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

(一)單個一級威脅障礙物的避碰

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

(二)多威脅障礙的避碰

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

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

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

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

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

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

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

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

由于UAV避碰過程中可不考慮三級威脅障礙物的影響，則對一級和三級威脅障礙物的避碰可簡化為對單個威脅障礙物的避碰；當(dāng)已知障礙中沒有一級威脅障礙物時，UAV不需要對已知障礙進行避碰。

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

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

實施例1：UAV對單個一級威脅障礙物的避碰

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

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

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

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

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

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

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

表2單個一級障礙避碰情況下的UAV避障方向角范圍

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

實施例2：UAV對多個一級威脅障礙物的避碰

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

當(dāng)UAV在飛行到P_u(200,400)處同時探測到兩個威脅障礙物情況下。首先，通過傳感器裝置確定障礙物的信息為：位置坐標(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ù)速度障礙圓弧法可計算得到：兩個已知障礙都為一級威脅障礙物，UAV需要對兩個威脅障礙進行避碰。

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

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

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

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

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

表4多個一級障礙避碰情況下的UAV避障方向角范圍

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

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