本發(fā)明屬于無(wú)人飛行器空間避障飛行的控制領(lǐng)域，尤其是涉及一種基于保角映射分析方法的無(wú)人飛行器空間避障方法。

背景技術(shù)：

當(dāng)前，無(wú)人飛行器無(wú)論是在民用領(lǐng)域還是軍事領(lǐng)域中都得到了廣泛的應(yīng)用。但隨著無(wú)人飛行器執(zhí)行任務(wù)范圍的擴(kuò)大和執(zhí)行環(huán)境的日益復(fù)雜，如何提高無(wú)人飛行器在復(fù)雜環(huán)境中執(zhí)行艱巨任務(wù)的生存能力成為無(wú)人飛行器技術(shù)研究中亟待解決的關(guān)鍵問(wèn)題之一。為無(wú)人飛行器配備高性能的自主避障系統(tǒng)可實(shí)現(xiàn)無(wú)人飛行器在復(fù)雜環(huán)境下對(duì)動(dòng)靜態(tài)障礙物的避碰，而自主避障系統(tǒng)的核心是自主避障算法。

為了提升無(wú)人飛行器的自主避障系統(tǒng)性能，可提高無(wú)人飛行器在復(fù)雜動(dòng)態(tài)環(huán)境中執(zhí)行任務(wù)的生存能力，這就要求自主避障系統(tǒng)配備的自主避障算法能實(shí)現(xiàn)對(duì)動(dòng)靜態(tài)威脅障礙物的避碰。現(xiàn)有避障算法對(duì)復(fù)雜環(huán)境中多威脅障礙物的避碰較為復(fù)雜，且多采用降維的思想進(jìn)行處理，很難得到無(wú)人飛行器對(duì)多威脅障礙物避碰的最小速度矢量偏轉(zhuǎn)方向。

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

針對(duì)上述提出的問(wèn)題，有必要提出一種新的空間避障方法，該方法能夠簡(jiǎn)單、直觀地處理多威脅障礙物條件下的避碰問(wèn)題，并能夠確定多動(dòng)態(tài)威脅障礙避碰的速度矢量方向。

本發(fā)明提出的避障方法具體包括以下步驟：

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

步驟S2，運(yùn)用構(gòu)建的三維速度障礙錐進(jìn)行避碰判斷；

步驟S3，建立相對(duì)碰撞區(qū)RCC和絕對(duì)碰撞區(qū)ACC；然后建立空間速度障礙球冠，并解出球冠的參數(shù)；

步驟S4，對(duì)空間速度障礙球冠進(jìn)行保角映射，求解避障方向，具體包括以下子步驟，

步驟S41，建立映射坐標(biāo)系，以為平面直角坐標(biāo)系的橫軸，oθ為平面直角坐標(biāo)系的縱軸，建立平面方向角直角坐標(biāo)系和θ分別表示為無(wú)人飛行器速度矢量v_u的偏航角和俯仰角，且

步驟S42，將空間速度障礙球冠的邊界保角映射為坐標(biāo)系內(nèi)以點(diǎn)為圓心的⊙C_o，⊙C_o的半徑為γ；其中，為矢量的方向角，而P_U為無(wú)人飛行器的位置，C為空間速度障礙球冠的中心點(diǎn)，即球冠大圓中心；γ為P_U與空間速度障礙球冠的大圓邊界形成圓錐的半頂角；

步驟S43，將當(dāng)前時(shí)刻無(wú)人飛行器速度矢量v_u的偏航角和俯仰角記為無(wú)人飛行器的速度矢量方向角則矢量與⊙C_o的交點(diǎn)為最小避障速度矢量偏轉(zhuǎn)方向；

步驟S5，求解避障點(diǎn)。

除了步驟S4及其子步驟外，其它步驟的具體方法可參見(jiàn)CN105717942A專利申請(qǐng)中的技術(shù)方案。

優(yōu)選的，當(dāng)障礙物為多個(gè)時(shí)，步驟S2要對(duì)多個(gè)障礙物進(jìn)行避障判斷，如果無(wú)人飛行器的速度矢量v_u與障礙物O_i的速度矢量v_oi之間的相對(duì)速度矢量v_uoi在障礙物O_i形成的障礙錐內(nèi)，則需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰，否則不需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰。

優(yōu)選的，當(dāng)需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰時(shí)，

步驟S42中需要對(duì)每個(gè)空間速度障礙球冠的邊界均進(jìn)行保角映射，將映射后所得各映射圓的并集記為∪⊙；

步驟S43中最小避障速度矢量偏轉(zhuǎn)方向?yàn)闊o(wú)人飛行器的速度矢量方向角到∪⊙邊界的最小距離點(diǎn)對(duì)應(yīng)方向。

本發(fā)明的提出的基于保角映射分析的無(wú)人飛行器空間避障方法，確定了無(wú)人飛行器對(duì)威脅障礙物避碰的最小速度矢量偏轉(zhuǎn)方向，簡(jiǎn)化了多威脅障礙物避碰分析。使用該方法處理多威脅障礙物條件下的避碰問(wèn)題，具有簡(jiǎn)單、直觀等優(yōu)點(diǎn)。

附圖說(shuō)明

圖1基于保角映射分析的無(wú)人飛行器空間避障流程圖；

圖2三維速度障礙錐示意圖；

圖3相對(duì)碰撞區(qū)RCC和絕對(duì)碰撞區(qū)ACC示意圖；

圖4空間速度障礙球冠示意圖；

圖5空間速度障礙球冠在速度球上的表示圖；

圖6映射坐標(biāo)系示意圖；

圖7無(wú)人飛行器對(duì)單威脅障礙物避碰的仿真結(jié)果；

圖8無(wú)人飛行器對(duì)多威脅障礙物避碰的仿真結(jié)果。

具體實(shí)施方式

下面結(jié)合附圖進(jìn)一步說(shuō)明本發(fā)明的具體實(shí)施方式。

如附圖1所示，本發(fā)明提出的避障方法主要包括：構(gòu)建三維速度障礙錐，運(yùn)用三維速度障礙錐進(jìn)行避碰判斷，建立空間速度障礙球冠并求解球冠參數(shù)，對(duì)空間速度障礙球進(jìn)行保角映射、進(jìn)而求解避障方向，以及求解避障點(diǎn)五個(gè)步驟。

具體包括以下步驟：

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

利用無(wú)人飛行器(Unmanned Aerial Vehicle，UAV)自身攜帶的障礙感知模塊，可感知周圍環(huán)境中存在的動(dòng)靜態(tài)障礙物，并可獲取障礙物的位姿信息Pose_obs(P_O,v_o)，即障礙物位置坐標(biāo)P_O(x_o,y_o,z_o)，障礙物速度矢量無(wú)人飛行器的飛行感知模塊可實(shí)時(shí)地獲取自身的位姿信息Pose_uav(P_U,v_u)，即無(wú)人飛行器速度矢量無(wú)人飛行器位置坐標(biāo)P_U(x_u,y_u,z_u)；設(shè)定障礙感知模塊的感知距離為d₀。

為簡(jiǎn)化無(wú)人飛行器對(duì)威脅障礙物的避碰，將無(wú)人飛行器簡(jiǎn)化為一點(diǎn)P_U，并將障礙物點(diǎn)膨化為以P_o為球心、半徑為R的障礙球，然后過(guò)P_U點(diǎn)作障礙球的切線，障礙球上所有的切點(diǎn)形成切線圓Tangent Circle，則所有以P_U為頂點(diǎn)障礙球的切線l形成的錐面為三維速度障礙錐，三維速度障礙錐也稱為空間碰撞錐，如圖2所示。

步驟S2，運(yùn)用構(gòu)建的三維速度障礙錐進(jìn)行避碰判斷；

根據(jù)無(wú)人飛行器速度v_u和障礙物速度v_o的大小和方向，求解無(wú)人飛行器相對(duì)于障礙物O的相對(duì)速度矢量v_uo的大小和方向；然后求解相對(duì)速度矢量v_uo與無(wú)人飛行器和障礙物的位置矢量之間的夾角大小α，如圖2所示；

然后求出三維速度障礙錐的半頂角α₀，其中R為障礙球PO的半徑，d₀為無(wú)人飛行器到障礙物O的距離。

當(dāng)障礙物為單個(gè)的時(shí)候，判斷障礙錐的半頂角α₀與α的關(guān)系，若α＜α₀，需要對(duì)障礙物進(jìn)行避碰，則繼續(xù)進(jìn)行步驟S3；若α＞α₀，不需要對(duì)障礙物進(jìn)行避碰，則退出。

當(dāng)障礙物為多個(gè)的時(shí)候，無(wú)人飛行器需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰的條件為：無(wú)人飛行器對(duì)每一個(gè)障礙物的避碰判斷條件同時(shí)成立。

即：無(wú)人飛行器需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰的判定條件可確定為

即無(wú)人飛行器的速度矢量v_u與障礙物O_i的速度矢量v_oi之間的相對(duì)速度矢量v_uoi都在障礙物O_i形成的障礙錐內(nèi)，此時(shí)需要對(duì)多個(gè)障礙物進(jìn)行避碰，否則不需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰。

步驟S3，建立相對(duì)碰撞區(qū)RCC和絕對(duì)碰撞區(qū)ACC；然后建立空間速度障礙球冠，并解出球冠的參數(shù)；

本步驟的方法可具體參見(jiàn)CN105717942A專利申請(qǐng)中的技術(shù)方案。

建立的相對(duì)碰撞區(qū)RCC和絕對(duì)碰撞區(qū)ACC如圖3所示；建立空間速度障礙球冠如圖4所示。將空間速度障礙球冠表示為其中r為速度球的球心P_U和球冠大圓中心C之間所形成矢量的長(zhǎng)度，和θ_r為矢量的方向角，即分別與ox和oz軸之間的夾角，并用γ表示無(wú)人飛行器位置坐標(biāo)點(diǎn)P_U與球冠G大圓邊界形成圓錐的半頂角，γ∈[0,π]。空間速度障礙球冠在速度球上的表示如圖5所示。

根據(jù)空間速度矢量幾何關(guān)系，解出球冠的參數(shù)以及γ。

步驟S4，對(duì)空間速度障礙球冠進(jìn)行保角映射，求解避障方向，具體包括以下子步驟，

步驟S41，建立映射坐標(biāo)系，以為平面直角坐標(biāo)系的橫軸，oθ為平面直角坐標(biāo)系的縱軸，建立平面方向角直角坐標(biāo)系和θ分別表示為無(wú)人飛行器速度矢量v_u的偏航角和俯仰角，且θ∈[-π/2,π/2]；建立的映射坐標(biāo)系如圖6所示。

步驟S42，將空間速度障礙球冠的邊界保角映射為坐標(biāo)系內(nèi)以點(diǎn)為圓心的⊙C_o，⊙C_o的半徑為γ；

當(dāng)需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰時(shí)，在步驟S42中需要對(duì)每個(gè)空間速度障礙球冠的邊界均進(jìn)行保角映射，將映射后所得各映射圓的并集記為∪⊙。

步驟S43，將當(dāng)前時(shí)刻無(wú)人飛行器速度矢量v_u的偏航角和俯仰角記為無(wú)人飛行器的速度矢量方向角

當(dāng)僅需要對(duì)單個(gè)障礙物進(jìn)行避碰時(shí)，則矢量與⊙C_o的交點(diǎn)為最小避障速度矢量偏轉(zhuǎn)方向；

當(dāng)需要同時(shí)對(duì)多個(gè)障礙物進(jìn)行避碰時(shí)，最小避障速度矢量偏轉(zhuǎn)方向?yàn)闊o(wú)人飛行器的速度矢量方向角到∪⊙邊界的最小距離點(diǎn)對(duì)應(yīng)方向。

步驟S5，求解避障點(diǎn)。

根據(jù)步驟S4中求解的最小避障速度矢量偏轉(zhuǎn)方向，采用現(xiàn)有技術(shù)中的方法求解避障點(diǎn)。

實(shí)施例1：?jiǎn)蝹€(gè)威脅障礙物的避碰

在動(dòng)態(tài)不確定環(huán)境下，無(wú)人飛行器執(zhí)行從起飛點(diǎn)P_s(0,0,0)至目標(biāo)點(diǎn)P_f(600,400,125)的飛行任務(wù)。無(wú)人飛行器在飛行過(guò)程中利用自身攜帶的傳感器裝置感知到周圍環(huán)境中的威脅障礙物，無(wú)人飛行器需要對(duì)威脅障礙物進(jìn)行避碰。其中，單個(gè)威脅障礙物的避碰為無(wú)人飛行器避障的最簡(jiǎn)單情況，表1給出了無(wú)人飛行器對(duì)單個(gè)威脅障礙物避碰的初始化條件。

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

根據(jù)表1中求解的威脅障礙物空間速度障礙球冠參數(shù)，可將威脅障礙物的空間速度障礙球冠進(jìn)行保角映射，相應(yīng)的最優(yōu)無(wú)人飛行器避碰速度矢量方向角為Q_avo(28.8,11.9)，避碰點(diǎn)為P_avo(216,180,41.7)。應(yīng)用于PH曲線路線規(guī)劃的仿真結(jié)果如圖7所示，仿真結(jié)果表明提出的三維空間無(wú)人飛行器自主避障算法能夠?qū)崿F(xiàn)對(duì)單威脅障礙物的避碰。

實(shí)施例2：多個(gè)威脅障礙物的避碰

當(dāng)無(wú)人飛行器同時(shí)感知到多個(gè)威脅障礙物時(shí)，表2給出了相應(yīng)的無(wú)人飛行器和威脅障礙物的初始化條件以及求解的威脅障礙物空間速度障礙球冠參數(shù)。以表2中的兩個(gè)威脅障礙物避碰為例，其它多威脅障礙物避碰可類似進(jìn)行避碰分析。

表2多威脅障礙物避碰的初始化條件

根據(jù)表2中求解的威脅障礙物空間速度障礙球冠參數(shù)，可對(duì)威脅障礙物的空間速度障礙球冠進(jìn)行保角映射，相應(yīng)的最優(yōu)避碰速度矢量方向角為Q_avo(30.7,15.4)，避碰點(diǎn)為P_avo(231,202,75.1)。應(yīng)用于PH曲線路線規(guī)劃的仿真結(jié)果如圖8所示，仿真結(jié)果表明提出的三維空間無(wú)人飛行器自主避障算法能夠?qū)崿F(xiàn)對(duì)多威脅障礙物的避碰。

仿真結(jié)果表明，本發(fā)明提出的基于保角映射分析的無(wú)人飛行器空間避障方法對(duì)提升無(wú)人飛行器在復(fù)雜動(dòng)態(tài)環(huán)境中執(zhí)行艱巨任務(wù)的安全性和生存能力具有重要意義。

此外，應(yīng)當(dāng)理解，以上所述僅為本發(fā)明的較佳實(shí)施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。