本發(fā)明屬于無(wú)人飛行器空間避障飛行的控制領(lǐng)域，尤其是涉及一種動(dòng)態(tài)不確定環(huán)境下無(wú)人飛行器平面自主避障方法。

背景技術(shù)：

無(wú)人飛行器在任務(wù)飛行過(guò)程中可利用自身攜帶的傳感器裝置探測(cè)到周圍存在的動(dòng)靜態(tài)障礙物，這些動(dòng)靜態(tài)障礙物可能會(huì)影響無(wú)人飛行器的安全飛行。對(duì)于靜態(tài)障礙物，無(wú)人飛行器可以通過(guò)現(xiàn)有的方法較為輕松地實(shí)現(xiàn)避障；但對(duì)于動(dòng)態(tài)障礙物，尤其是對(duì)于具有一定機(jī)動(dòng)性能的動(dòng)態(tài)障礙物，現(xiàn)有技術(shù)中還缺少簡(jiǎn)單實(shí)用的無(wú)人飛行器平面自主避障方法。

目前，無(wú)人飛行器在避障過(guò)程中，主要依靠無(wú)人飛行器自身攜帶的傳感器對(duì)無(wú)人飛行器的當(dāng)前時(shí)刻位置P_U(x_u,y_u)、速度v_u(v_u cosθ_u,v_u sinθ_u)，其中v_u為速度矢量v_u的大小，θ_u為速度矢量v_u的方向角進(jìn)行探測(cè)；并依靠無(wú)人飛行器自身攜帶的傳感器探測(cè)動(dòng)態(tài)障礙物O的位置P_O(x_o,y_o)、速度v_o(v_o cosθ_o,v_o sinθ_o)，其中v_o為速度矢量v_o的大小，θ_o為速度矢量v_o的方向角。傳感器的探測(cè)誤差會(huì)影響后續(xù)的無(wú)人飛行器避障，但是現(xiàn)有的無(wú)人飛行器自主避障方法很少考慮傳感器的探測(cè)誤差帶來(lái)的影響。

綜上所述，現(xiàn)有的無(wú)人飛行器自主避障方法中，很少考慮動(dòng)態(tài)障礙物的運(yùn)動(dòng)不確定性(即動(dòng)態(tài)障礙物的機(jī)動(dòng)性)和傳感器的探測(cè)誤差帶來(lái)的避障影響，上述問(wèn)題的忽略勢(shì)必給后續(xù)的無(wú)人飛行器自主避障帶來(lái)安全隱患，并使得避障的可靠性大幅度降低。

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

本發(fā)明的目的在于提供一種無(wú)人飛行器避障方法，其能夠有效避免現(xiàn)有避障算法中因忽略“動(dòng)態(tài)障礙物的機(jī)動(dòng)性能”和“傳感器的探測(cè)誤差”而帶來(lái)的影響，能夠?qū)崿F(xiàn)動(dòng)態(tài)不確定環(huán)境下的無(wú)人飛行器平面自主避障

本發(fā)明提出的一種動(dòng)態(tài)不確定環(huán)境下無(wú)人飛行器平面自主避障方法，包括以下步驟：

步驟S1，探測(cè)無(wú)人飛行器和動(dòng)態(tài)障礙物的位置和速度；

通過(guò)無(wú)人飛行器攜帶的傳感器探測(cè)無(wú)人飛行器的當(dāng)前時(shí)刻位置P_U(x_u,y_u)、速度v_u(v_u cosθ_u,v_u sinθ_u)，其中v_u為速度矢量v_u的大小，θ_u為速度矢量v_u的方向角；并探測(cè)動(dòng)態(tài)障礙物O的位置P_O(x_o,y_o)、速度v_o(v_o cosθ_o,v_o sinθ_o)，其中v_o為速度矢量v_o的大小，θ_o為速度矢量v_o的方向角；

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

將動(dòng)態(tài)障礙物O膨化為以P_O為圓心、半徑為R_o的障礙圓S_c，然后過(guò)點(diǎn)P_U作障礙圓S_c的兩條切線，則兩條切線形成障礙錐CC；然后將障礙錐CC沿v_o的速度矢量方向平移v_o，得到二維速度障礙錐VO；

步驟S3，選擇動(dòng)態(tài)不確定性表示方法，表示速度障礙區(qū)；

將動(dòng)態(tài)障礙物O的機(jī)動(dòng)性和傳感器的探測(cè)誤差通過(guò)障礙物的速度矢量v_o方向角偏差進(jìn)行表示，并將最大不確定性表示為±Δθ_o，其中逆時(shí)針為正，順時(shí)針為負(fù)；將障礙物速度矢量v_o對(duì)應(yīng)的兩個(gè)最大不確定性的速度矢量表示為v_o′和v_o"，并將v_o、v_o′和v_o"形成的速度圓弧表示為S_arcs；則速度圓弧S_arcs上任意一點(diǎn)對(duì)應(yīng)的障礙物速度矢量都將產(chǎn)生一個(gè)VO，將其最大不確定性對(duì)應(yīng)的速度障礙錐分別表示為VO′和VO"，VO′的頂點(diǎn)為P_vo′，VO"的頂點(diǎn)為P_vo"；則考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO為：∪VO＝VO′∪…∪VO∪…∪VO"；

步驟S4，根據(jù)步驟S3得到速度障礙區(qū)∪VO，求解避障方向。

由于考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO是一個(gè)不規(guī)則的圖形，為了減小在避障過(guò)程中數(shù)據(jù)計(jì)算量，需將上述的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO進(jìn)行規(guī)則化的處理。優(yōu)選的采用如下方法，步驟S4具體為：

步驟S41，將考慮動(dòng)態(tài)不確定性的速度障礙區(qū)規(guī)則化；

以VO的頂點(diǎn)P_vo為圓心，線段P_voP_vo′為半徑，作⊙P_vo；然后分別作VO兩條邊界線l_vo1，l_vo2的平行線l_vo1^*，l_vo2^*并與⊙P_vo相切，并取l_vo1^*，l_vo2^*與VO軸線P_voQ_o的交點(diǎn)P_vo^*；將以P_vo^*為頂點(diǎn)，以l_vo1^*和l_vo2^*為兩條邊界線的二維速度障礙錐VO^*作為規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)；

步驟S42，根據(jù)步驟S41得到的VO^*，進(jìn)而求解避障方向。

為了便于求解規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)VO^*，采用速度障礙圓弧法求解避障方向，優(yōu)選的采用如下方法，步驟S42具體為：

步驟S421，建立速度障礙圓弧G^*；

以P_U為圓心，速度矢量大小v_u為半徑做速度圓⊙P_U，然后取速度圓⊙P_U與VO^*相交且位于VO^*內(nèi)部的圓弧為速度障礙圓弧G^*，即G^*＝⊙P_U∩VO^*；⊙P_U與VO^*的兩條邊界線l_vo1^*和l_vo2^*的交點(diǎn)為和

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

步驟S423，求解避障方向。

在求解速度障礙圓弧參數(shù)的過(guò)程中，優(yōu)選的采用如下方法，步驟S422具體為：

步驟S4221，將速度障礙圓弧表示為其中為矢量的方向角；β^*對(duì)應(yīng)于速度障礙圓弧的1/2圓心角；

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

現(xiàn)有的避障方法主要是對(duì)“危險(xiǎn)”障礙物進(jìn)行避碰，但卻忽略了暫時(shí)不具威脅障礙的“潛在危險(xiǎn)”。有些情況下，當(dāng)無(wú)人飛行器沿避碰重規(guī)劃的航跡飛行時(shí)，之前不具威脅的障礙物會(huì)對(duì)無(wú)人飛行器飛行產(chǎn)生威脅。為了確保無(wú)人飛行器避碰重規(guī)劃航跡的安全性，需要綜合考慮障礙物的“危險(xiǎn)”和“潛在危險(xiǎn)”，優(yōu)選的采用以下方法，步驟S423具體為：

步驟S4231，定義動(dòng)態(tài)障礙物O的威脅等級(jí)，并進(jìn)行避碰判斷；

當(dāng)無(wú)人飛行器的速度矢量v_u∈VO^*時(shí)，將動(dòng)態(tài)障礙物O定義為一級(jí)威脅障礙物；當(dāng)且時(shí)，將動(dòng)態(tài)障礙物O定義為二級(jí)威脅障礙物；當(dāng)且時(shí)，將動(dòng)態(tài)障礙物O定義為三級(jí)威脅障礙物；

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

步驟S4232，求解避障方向。

當(dāng)動(dòng)態(tài)障礙物O存在多個(gè)時(shí)，且動(dòng)態(tài)障礙物O的威脅等級(jí)存在多個(gè)時(shí)，優(yōu)選的采用如下避碰判斷方法，步驟S4231中避碰判斷的方法具體為：

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

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

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

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

優(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è)動(dòng)態(tài)障礙物，依據(jù)求解的速度障礙圓弧參數(shù)，確定避碰不可行速度矢量方向范圍為則相應(yīng)的避碰方向角范圍為

對(duì)于多個(gè)動(dòng)態(tài)障礙物，依據(jù)求解的速度障礙圓弧參數(shù)，確定多威脅障礙下避碰不可行速度矢量方向范圍。當(dāng)動(dòng)態(tài)障礙物的數(shù)量為i個(gè)時(shí)，則多威脅障礙下避碰不可行速度矢量方向范圍為則相應(yīng)的避碰方向角范圍為

附圖說(shuō)明

圖1為動(dòng)態(tài)不確定環(huán)境下無(wú)人飛行器平面自主避障方法流程圖；

圖2為碰撞錐CC示意圖；

圖3為二維速度障礙錐VO示意圖；

圖4為二維速度障礙錐VO動(dòng)態(tài)不確定性表示示意圖；

圖5為規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)示意圖；

圖6為速度障礙圓弧法示意圖；

圖7為規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)VO^*的速度障礙圓弧法示意圖；

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

圖9為多威脅障礙物速度障礙圓弧示意圖。

具體實(shí)施方式

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

如圖1所示，本發(fā)明的一種動(dòng)態(tài)不確定環(huán)境下無(wú)人飛行器平面自主避障方法主要包括4個(gè)步驟：探測(cè)無(wú)人飛行器和動(dòng)態(tài)障礙物的位置和速度；構(gòu)建二維速度障礙錐；選擇動(dòng)態(tài)不確定性表示方法，表示速度障礙區(qū)；求解避障方向。

在未知?jiǎng)討B(tài)環(huán)境下，無(wú)人飛行器執(zhí)行任務(wù)的過(guò)程中，利用自身攜帶的傳感器裝置，可以探測(cè)到周圍環(huán)境中存在的動(dòng)態(tài)障礙物O，并可以獲取自身和障礙物O的位置和速度信息。無(wú)人飛行器的位置坐標(biāo)為P_U(x_u,y_u)，速度矢量為v_u(v_u cosθ_u,v_u sinθ_u)，其中v_u為速度矢量v_u的大小，θ_u為速度矢量v_u的方向角；障礙物的位置坐標(biāo)為P_O(x_o,y_o)，速度矢量為v_o(v_ocosθ_o,v_o sinθ_o)，其中v_o為速度矢量v_o的大小，θ_o為速度矢量v_o的方向角。為便于建立無(wú)人飛行器的避障模型，將無(wú)人飛行器簡(jiǎn)化為一點(diǎn)，并根據(jù)無(wú)人飛行器和障礙物之間的相對(duì)大小關(guān)系將障礙物“膨脹”為半徑為R_o的威脅圓S_c。同時(shí)，設(shè)定無(wú)人飛行器傳感器的探測(cè)范圍為d_uo。無(wú)人飛行器和動(dòng)態(tài)障礙物O之間形成的碰撞錐CC如圖2所示。

碰撞錐CC可表示為其中，l_uo為以P_U為頂點(diǎn)并與相對(duì)速度矢量v_uo共線的射線，M為射線l_uo與威脅圓S_c的交點(diǎn)。

二維速度障礙錐VO的定義基于碰撞錐CC，VO的示意圖如圖3所示。根據(jù)圖3可對(duì)VO進(jìn)行定義，即將CC內(nèi)所有的點(diǎn)平移速度矢量v_o得到的點(diǎn)集合。

本發(fā)明將障礙物的機(jī)動(dòng)性和傳感器的探測(cè)誤差通過(guò)障礙物的速度矢量v_o方向角偏差進(jìn)行表示，并將最大不確定性表示為±Δθ_o，其中逆時(shí)針為正，順時(shí)針為負(fù)，示意圖如圖4所示。障礙物速度矢量v_o對(duì)應(yīng)于兩個(gè)最大不確定性的速度矢量為v_o′和v_o"，并滿足||v_o′||＝||v_o"|＝v_o，且形成的速度圓弧表示為S_arcs。那么，速度圓弧S_arcs上任意一點(diǎn)對(duì)應(yīng)的障礙物速度矢量都將產(chǎn)生一個(gè)VO，最大不確定性對(duì)應(yīng)的速度障礙錐分別為VO′和VO"。因此，考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO可表示為：∪VO＝VO′∪…∪VO∪…∪VO"。

由VO的定義可對(duì)無(wú)人飛行器探測(cè)到的未知障礙物威脅性進(jìn)行判斷，當(dāng)P_U+v_u∈VO時(shí)，未知障礙物具有威脅，無(wú)人飛行器需要對(duì)障礙物進(jìn)行避碰；當(dāng)時(shí)，未知障礙物不具有威脅，此時(shí)無(wú)人飛行器不會(huì)與障礙物相碰。為簡(jiǎn)化未知障礙物威脅性判斷，可根據(jù)示意圖2中的角度大小關(guān)系得出未知障礙物威脅性判斷的結(jié)論。

當(dāng)α＜γ時(shí)，未知障礙物具有威脅，無(wú)人飛行器需要對(duì)障礙物進(jìn)行避碰；當(dāng)α≥γ時(shí)，未知障礙物不具有威脅，無(wú)人飛行器不需要對(duì)障礙物進(jìn)行避碰。其中，α為相對(duì)速度矢量v_uo與CC軸線P_UP_O之間的夾角，γ為CC頂角的一半。

α和γ的大小滿足下面關(guān)系式，

同理，可逐一判斷無(wú)人飛行器是否需要對(duì)考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO中的VO′和VO″等其它速度障礙錐進(jìn)行避碰，進(jìn)而求解避障方向。由于速度障礙區(qū)∪VO是一個(gè)不規(guī)則圖形，直接采用速度障礙區(qū)∪VO進(jìn)行避碰判斷，并求解避障方向的計(jì)算量大。

為此，本發(fā)明在求解避障方向前，先對(duì)∪VO進(jìn)行幾何處理，將考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO進(jìn)行規(guī)則化。

動(dòng)態(tài)不確定性可表示為障礙物的速度圓弧S_arcs，相應(yīng)所有速度障礙頂點(diǎn)的集合也為圓弧，且曲率半徑等于速度圓弧S_arcs半徑v_o?？紤]到∪VO的幾何形狀屬于不規(guī)則圖形，為了簡(jiǎn)化計(jì)算量，對(duì)∪VO進(jìn)行幾何處理，將考慮動(dòng)態(tài)不確定性的速度障礙區(qū)∪VO進(jìn)行規(guī)則化，具體如圖5所示。

以VO的頂點(diǎn)P_vo為圓心，線段P_voP_vo′為半徑，作⊙P_vo。則頂點(diǎn)位于⊙P_vo內(nèi)的速度障礙包含了頂點(diǎn)在不確定性圓弧上的速度障礙，而不確定性圓弧可通過(guò)障礙物的速度圓弧S_arcs進(jìn)行定義，

不確定性圓弧由障礙物的速度圓弧S_arcs轉(zhuǎn)化得到，不確定性圓弧上的每一點(diǎn)對(duì)應(yīng)于速度圓弧S_arcs上相應(yīng)的VO頂點(diǎn)，且和S_arcs之間具有相同的圓心角和曲率半徑。

分別作VO兩條邊界線l_vo1，l_vo2的平行線l_vo1^*，l_vo2^*并與⊙P_vo相切，同時(shí)l_vo1^*，l_vo2^*與VO的軸線P_voQ_o交于點(diǎn)P_vo^*。則對(duì)∪VO進(jìn)行幾何擴(kuò)展后形成的規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)VO^*具有與VO類似的幾何形狀，VO^*的頂點(diǎn)為P_vo^*，兩條邊界線為l_vo1^*、l_vo2^*，且VO^*與VO之間有如下結(jié)論，

VO^*與VO共軸線、頂角相等，且產(chǎn)生的VO^*是考慮動(dòng)態(tài)不確定性的最小速度障礙區(qū)。

⊙P_vo的半徑P_voP_vo′，VO^*的半頂角γ^*，頂點(diǎn)P_vo^*和兩條邊界線l_vo1^*，l_vo2^*的斜率可由下列關(guān)系式確定。

γ^*＝γ (4)

則VO^*的所有參數(shù)能夠由式(3)～(7)進(jìn)行求解，那么規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)可以被建立并表示。

以無(wú)人飛行器的位置坐標(biāo)點(diǎn)P_U為圓心，無(wú)人飛行器的速度矢量大小v_u為半徑，作⊙P_U，⊙P_U與VO相交，示意圖如圖6所示。這種通過(guò)位于VO內(nèi)的圓弧大小G來(lái)量化威脅障礙物影響的方法稱為速度障礙圓弧法。

在示意圖6中，⊙P_U與VO的臨界邊界線l_vo1相交于點(diǎn)A₁和A₂，對(duì)應(yīng)于點(diǎn)A₁和A₂為無(wú)人飛行器避障的兩個(gè)臨界狀態(tài)點(diǎn)，即無(wú)人飛行器的期望速度矢量滿足，

為準(zhǔn)確表達(dá)位于VO內(nèi)的圓弧大小G，采用三個(gè)參數(shù)r，β對(duì)其進(jìn)行描述，即其中，P_mid＝(A₁+A₂)/2，為矢量與ox軸之間的夾角，β為速度圓弧對(duì)應(yīng)的圓心角一半。下面對(duì)速度圓弧三個(gè)參數(shù)進(jìn)行求解，

根據(jù)式(9)～(11)的幾何關(guān)系，可確定臨界點(diǎn)A₁的坐標(biāo)。同理，A₂的坐標(biāo)也可以確定。那么，線段A₁A₂的中點(diǎn)P_mid坐標(biāo)也可以確定。因此，位于VO內(nèi)的圓弧大小G的三個(gè)參數(shù)可確定為，

當(dāng)考慮威脅障礙的機(jī)動(dòng)性和傳感器探測(cè)誤差時(shí)，需要使用規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)VO^*。根據(jù)速度障礙圓弧法，可對(duì)規(guī)則化后的考慮動(dòng)態(tài)不確定性的速度障礙區(qū)VO^*進(jìn)行避碰，示意圖如圖7所示。

從圖7中可以得出，當(dāng)考慮動(dòng)態(tài)不確定性時(shí)，產(chǎn)生的速度障礙圓弧變大。參照前述實(shí)施例中VO參數(shù)的求解方法，只要將VO參數(shù)替換成VO^*的參數(shù)，即可實(shí)現(xiàn)對(duì)VO^*速度障礙圓弧參數(shù)的求解。

下面通過(guò)VO^*的速度障礙圓弧法對(duì)單個(gè)威脅障礙物避碰和多威脅障礙物避碰方向角的求解進(jìn)行分析。

(1)單個(gè)威脅障礙物避碰

①對(duì)于單個(gè)一級(jí)威脅障礙物而言，需要進(jìn)行避碰，單個(gè)速度障礙圓弧示意圖如圖8所示，該速度障礙圓弧大小則該速度障礙圓弧對(duì)應(yīng)的圓心角范圍為即確定的避碰不可行速度矢量方向范圍為則相應(yīng)的避碰方向角范圍為對(duì)于一級(jí)威脅障礙物而言，無(wú)人飛行器的速度矢量v_u方向角θ_u位于p內(nèi)。因此，將無(wú)人飛行器的速度矢量v_u方向角θ_u偏轉(zhuǎn)出p，即可實(shí)現(xiàn)無(wú)人飛行器對(duì)威脅障礙物的避碰。

②對(duì)于單個(gè)二級(jí)或三級(jí)威脅障礙物而言，暫不需要進(jìn)行避碰。

(2)多威脅障礙物避碰

當(dāng)無(wú)人飛行器同時(shí)需要對(duì)多威脅障礙物避碰時(shí)，可通過(guò)兩個(gè)威脅障礙物的避碰進(jìn)行分析，三個(gè)及三個(gè)以上威脅障礙物的避碰可類似進(jìn)行分析。示意圖9給出了兩個(gè)威脅障礙圓弧示意圖。

①兩個(gè)威脅障礙中均為一級(jí)威脅障礙物。威脅障礙物O₁對(duì)應(yīng)的速度障礙圓弧為威脅障礙物O₂對(duì)應(yīng)的速度障礙圓弧為且無(wú)人飛行器的速度矢量v_u方向角滿足

θ_u∈p₁∩p₂ (19)

而兩個(gè)威脅障礙圓弧對(duì)應(yīng)的圓心角范圍p可表示為

p＝p₁∪p₂ (21)

那么，當(dāng)無(wú)人飛行器的速度矢量v_u方向角偏θ_u轉(zhuǎn)出p時(shí)，即可實(shí)現(xiàn)對(duì)兩個(gè)威脅障礙物的避碰。

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

③兩個(gè)威脅障礙中一個(gè)一級(jí)威脅障礙物，一個(gè)三級(jí)威脅障礙物。則不需考慮三級(jí)威脅障礙物的影響，對(duì)一級(jí)和三級(jí)威脅障礙物的避碰可簡(jiǎn)化為對(duì)單個(gè)威脅障礙物的避碰。

④其它威脅障礙的避碰

當(dāng)已知障礙中沒(méi)有一級(jí)威脅障礙物時(shí)，無(wú)人飛行器則不需要對(duì)已知障礙進(jìn)行避碰。

實(shí)施例1：

在動(dòng)態(tài)不確定環(huán)境下，設(shè)定無(wú)人飛行器從起飛點(diǎn)P_s(0,0)到目標(biāo)點(diǎn)P_t(500,1000)處執(zhí)行任務(wù)。在無(wú)人飛行器飛行過(guò)程中，通過(guò)自身攜帶的傳感器裝置，探測(cè)到周圍環(huán)境中的威脅障礙物，下面基于動(dòng)態(tài)不確定環(huán)境下的無(wú)人飛行器自主避障算法對(duì)無(wú)人飛行器進(jìn)行避碰仿真，仿真考慮動(dòng)態(tài)環(huán)境中的不確定性。無(wú)人飛行器和障礙物的初始化條件如表1所示。其中，無(wú)人飛行器探測(cè)到的障礙物中只有一個(gè)具有一級(jí)威脅，其它都為三級(jí)威脅障礙物。

表1單個(gè)威脅障礙避碰初始化條件

根據(jù)障礙圓弧大小的計(jì)算方法，可以得到表2中在不同動(dòng)態(tài)不確定性下的威脅障礙圓弧參數(shù)。其中，Δθ₀為0°時(shí)表示不考慮復(fù)雜環(huán)境下的動(dòng)態(tài)不確定性。對(duì)比表2中不同動(dòng)態(tài)不確定下的威脅障礙圓弧參數(shù)，可以得出：障礙圓弧范圍的大小與動(dòng)態(tài)不確定性成正相關(guān)。

表2不同動(dòng)態(tài)不確定性下的威脅障礙圓弧參數(shù)

那么，無(wú)人飛行器對(duì)威脅障礙物的避碰方向角可確定為障礙圓弧范圍以外的值。根據(jù)初始化條件和文獻(xiàn)“張毅,楊秀霞,周硙硙.基于速度障礙法的多無(wú)人飛行器可飛行航跡優(yōu)化生成[J].系統(tǒng)工程與電子技術(shù),2015,37(2):323‐330.”中的計(jì)算方法，可以得出在動(dòng)態(tài)不確定性Δθ₀＝5°時(shí)的避障時(shí)間t＝2.63、避障點(diǎn)為(207,548)。相應(yīng)的仿真結(jié)果表明：無(wú)人飛行器在考慮動(dòng)態(tài)不確定性下，能夠?qū)崿F(xiàn)對(duì)單個(gè)一級(jí)威脅障礙物的避碰。

實(shí)施例2：

當(dāng)無(wú)人飛行器同時(shí)探測(cè)到多威脅障礙物時(shí)，相應(yīng)的初始化條件如表3所示。從表3中可以得出：障礙物O₁，O₂具有一級(jí)威脅；而O₃，O₄為三級(jí)威脅障礙物。

表3多威脅障礙避碰初始化條件

表4給出了兩個(gè)威脅障礙在不同動(dòng)態(tài)不確定性下的圓弧參數(shù)，而整個(gè)障礙圓弧范圍為兩個(gè)威脅障礙圓弧范圍的并集，且隨動(dòng)態(tài)不確定性的增加而變大。

表4不同動(dòng)態(tài)不確定性下的威脅障礙圓弧參數(shù)

同樣，無(wú)人飛行器對(duì)多威脅障礙物的避碰方向角可確定為整個(gè)障礙圓弧范圍以外的值，再根據(jù)相應(yīng)的初始化條件可確定避障時(shí)間t＝2.38、避障點(diǎn)為(291,516)。相應(yīng)的仿真結(jié)果表明：無(wú)人飛行器能夠?qū)崿F(xiàn)在動(dòng)態(tài)不確定性下的多威脅障礙避碰。

最后應(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)。