一種基于理想流體數(shù)值計(jì)算的無人機(jī)三維航路規(guī)劃方法
【專利摘要】本發(fā)明公開了一種基于流體力學(xué)的無人機(jī)三維航路生成方法，屬于航路規(guī)劃領(lǐng)域，本發(fā)明首先計(jì)算得到沿X軸來流方向，位于原點(diǎn)的圓球障礙繞流流線，利用旋轉(zhuǎn)平移矩陣得到平面中任意來流方向與任意障礙位置的繞流流線，根據(jù)流線是否滿足部分重合條件采用兩種方式將所有的流線合并成一條流線，之后根據(jù)無人機(jī)約束對三維流線進(jìn)行處理得到無人機(jī)三維航路。本發(fā)明借鑒了自然界流水能夠避開巖石的現(xiàn)象，將流體計(jì)算與航路規(guī)劃相結(jié)合，同時(shí)考慮無人機(jī)飛行約束，在地形較為復(fù)雜時(shí)，能夠規(guī)劃出光滑且易于飛行的三維飛行避障航路。本發(fā)明地形建模簡單，計(jì)算量小，滿足無人機(jī)約束，實(shí)現(xiàn)方便。
【專利說明】一種基于理想流體數(shù)值計(jì)算的無人機(jī)三維航路規(guī)劃方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于航路規(guī)劃領(lǐng)域，具體地說是指一種基于理想流體數(shù)值計(jì)算的無人機(jī)三 維航路規(guī)劃方法。

【背景技術(shù)】
[0002] 由于無人機(jī)優(yōu)越的靈活性、超高的性價(jià)比，使其無論在民用和軍事領(lǐng)域均有著極 其廣泛的應(yīng)用。無人機(jī)自主飛行技術(shù)中有一個(gè)重要的分支，就是無人機(jī)航路規(guī)劃。無人機(jī) 航路規(guī)劃是指在特定的任務(wù)背景下，尋找使無人機(jī)由起始點(diǎn)按照一條較優(yōu)的飛行路徑，最 終到達(dá)目標(biāo)點(diǎn)的飛行航路。這種飛行航路應(yīng)該滿足無人機(jī)自身的物理約束條件，同時(shí)應(yīng)該 能夠躲避障礙和敵方威脅。在防空技術(shù)日益完善的現(xiàn)代戰(zhàn)爭中，無人機(jī)航路規(guī)劃是提高無 人機(jī)作戰(zhàn)能效，實(shí)施遠(yuǎn)程精確打擊的有效手段。
[0003] 目前的航路規(guī)劃方法有很多，大致可以分為五類：基于圖形的方法、隨機(jī)型搜索方 法、決策型搜索方法、人工勢場法和人工智能法。這五類方法又被細(xì)分為多種代表性方法， 許多學(xué)者對此進(jìn)行了深入和廣泛的探討。但是這五類方法均最早是從機(jī)器人航路規(guī)劃提出 的，因此對無人機(jī)航路規(guī)劃的研究明顯晚于而且少于機(jī)器人。對無人機(jī)進(jìn)行航路規(guī)劃必須 考慮以下幾點(diǎn)：首先，無人機(jī)的物理特點(diǎn)和機(jī)動性與機(jī)器人有很大不同，因此在進(jìn)行航路規(guī) 劃時(shí)充分考慮到無人機(jī)約束，保證航路的可行性。其次，實(shí)際的無人機(jī)飛行環(huán)境遠(yuǎn)比機(jī)器人 復(fù)雜，不僅涉及范圍廣、地形復(fù)雜，而且可能存在著敵方火炮或防空陣威脅，并且戰(zhàn)場中的 態(tài)勢可能隨時(shí)發(fā)生變化。最后，充分利用無人機(jī)的飛行能力，探索適合無人機(jī)飛行的三維航 路規(guī)劃方法，將會為無人機(jī)航路規(guī)劃拓展出更實(shí)用、更有價(jià)值的應(yīng)用前景。
[0004] 雖然國內(nèi)外學(xué)者對無人機(jī)航路規(guī)劃進(jìn)行了大量研究，在一些方面取得了成功。但 是這些研究成果在進(jìn)行實(shí)際的工程應(yīng)用時(shí)，不得不面臨的問題是：1.航路不夠光滑，存在 折線，無人機(jī)實(shí)際飛行困難；2.無人機(jī)的三維航路規(guī)劃方法較少，由機(jī)器人航路規(guī)劃方法 演變來的許多算法并不真正適合無人機(jī)使用。
[0005] 為此，發(fā)明以無人機(jī)航路規(guī)劃問題為背景，充分考慮無人機(jī)約束，根據(jù)理想流體的 繞流性質(zhì)，將航路規(guī)劃與流體數(shù)值計(jì)算的相結(jié)合，為無人機(jī)三維航路規(guī)劃提出一種全新的 解決思路和方法，最終為無人機(jī)的發(fā)展研究提供更有力的技術(shù)支撐，同時(shí)為無人機(jī)拓展更 加廣闊的應(yīng)用前景。


【發(fā)明內(nèi)容】

[0006] 針對現(xiàn)有技術(shù)中存在的問題，本發(fā)明目的是要提出一種基于理想流體數(shù)值計(jì)算的 無人機(jī)三維航路規(guī)劃方法，通過本方法能夠在地形較為復(fù)雜的情況下，快速的生成一條滿 足無人機(jī)約束的光滑三維避障飛行航路。
[0007] 為了實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案為一種基于理想流體數(shù)值計(jì)算的無 人機(jī)三維航路規(guī)劃方法，其首先根據(jù)無人機(jī)約束對地形中的障礙進(jìn)行預(yù)處理，之后計(jì)算出 理想情況下沿X軸來流方向，位于原點(diǎn)的圓球障礙繞避障航路，最后利用疊加原理計(jì)算多 障礙同時(shí)存在時(shí)的復(fù)雜地形無人機(jī)航路；具體步驟如下：
[0008] 步驟一：確定無人機(jī)的轉(zhuǎn)彎半徑，并與地形中的所有障礙半徑進(jìn)行比較，根據(jù)轉(zhuǎn)彎 半徑和障礙半徑的大小關(guān)系，以及障礙間的相對位置，進(jìn)行地形的預(yù)處理，具體方法如下：
[0009] 1、障礙物重疊：計(jì)算與兩個(gè)障礙同時(shí)內(nèi)切的虛擬障礙，將虛擬障礙視為單個(gè)障礙 進(jìn)行避障，令O i和0i+1分別代表第i個(gè)和第i+Ι個(gè)障礙的球心，Ri和Ri+1分別表示O i和0i+1 的半徑，在線段OiOw上找到一點(diǎn)Onew，以該點(diǎn)為圓心，做出與圓O i和〇i+1同時(shí)相內(nèi)切的圓 Onew，在三維情況下這種方法仍然是可行的，其中Onew必須滿足下式條件
[0010] I OnewOi I +Ri = 10new0i+11 +Ri+1 ;
[0011] 2、單個(gè)障礙：
[0012] 2. 1目標(biāo)點(diǎn)和起始點(diǎn)的連線沒有穿過障礙的中心，則將障礙的半徑進(jìn)行虛擬擴(kuò)張， 使障礙物半徑大于等于無人機(jī)的轉(zhuǎn)彎半徑，根據(jù)不同的安全距離，虛擬障礙物半徑的計(jì)算 公式為：虛擬障礙物半徑=無人機(jī)轉(zhuǎn)彎半徑+安全距離；
[0013] 2. 2目標(biāo)點(diǎn)和起始點(diǎn)的連線穿過了障礙的中心，則預(yù)處理分為"真實(shí)障礙半徑比最 小轉(zhuǎn)彎半徑小"和"真實(shí)障礙半徑比最小轉(zhuǎn)彎半徑大"兩種情況；
[0014] 2. 2. 1當(dāng)"真實(shí)障礙半徑比最小轉(zhuǎn)彎半徑小"時(shí)，令圓O1為真實(shí)障礙，將目標(biāo)點(diǎn)和 起始點(diǎn)的連線作為X軸，則X軸穿過了 O1，設(shè)無人機(jī)最小轉(zhuǎn)彎半徑為R〇,過真實(shí)障礙O1做垂 直于X軸的直線交O 1與A點(diǎn)，在AO1的延長線上找到一點(diǎn)O2，使得AO2 = Rtl ;那么O2和Rtl所 確定的虛線圓O2，其為虛擬障礙，并且O 1比O2的半徑小，根據(jù)O2的設(shè)置方法，可知=O1是O 2 的內(nèi)切圓，切點(diǎn)為A ;在航路規(guī)劃時(shí)，將O2作為障礙進(jìn)行躲避，可以一方面滿足無人機(jī)飛行 約束，一方面避免了停滯點(diǎn)問題；
[0015] 2. 2. 2當(dāng)"真實(shí)障礙的半徑比最小轉(zhuǎn)彎半徑大"時(shí)，此時(shí)真實(shí)障礙的半徑比無人機(jī) 最小轉(zhuǎn)彎半徑大，只要在AO1的延長線上找到一點(diǎn)O 2，以O(shè)2為圓心作圓，使得O1O2 = Γ(Ι，Γ(Ι是 設(shè)定的一個(gè)小量，則O2的半徑大于O 1，而O1的半徑又大于最小轉(zhuǎn)彎半徑，因此該虛擬障礙的 設(shè)計(jì)是滿足無人機(jī)約束的，并且X軸不會穿過O 2，解決了停滯點(diǎn)問題；
[0016] 將地形中的所有障礙，根據(jù)本步驟中的1和2進(jìn)行預(yù)處理，預(yù)處理后的地形中進(jìn)行 航路規(guī)劃得到的航路，滿足無人機(jī)約束；
[0017] 步驟二：單個(gè)障礙時(shí)，根據(jù)定常忽略粘性的不可壓縮理想流體，沿空間直角坐標(biāo)系 X軸來流方向作用于位于原點(diǎn)的圓球障礙繞流問題的解析解，在該直角坐標(biāo)系下求得流速 沿Χ、Υ和Z軸的分量；
[0018] 對于空間中任意一點(diǎn)Ρ，極坐標(biāo)系建立在點(diǎn)P與X軸所構(gòu)成的平面上；空間直角坐 標(biāo)系的坐標(biāo)原點(diǎn)與平面二維極坐標(biāo)系的極點(diǎn)重合，為點(diǎn)0,空間直角坐標(biāo)系的X軸與平面二 維極坐標(biāo)系的極軸Γ軸重合；在該極坐標(biāo)系下流體的速度勢P為
[0019]

【權(quán)利要求】
1. 一種基于理想流體數(shù)值計(jì)算的無人機(jī)三維航路規(guī)劃方法，其特征在于：首先根據(jù)無 人機(jī)約束對地形中的障礙進(jìn)行預(yù)處理，之后計(jì)算出理想情況下沿X軸來流方向，位于原點(diǎn) 的圓球障礙繞避障航路，最后利用疊加原理計(jì)算多障礙同時(shí)存在時(shí)的復(fù)雜地形無人機(jī)航 路；具體步驟如下： 步驟一：確定無人機(jī)的轉(zhuǎn)彎半徑，并與地形中的所有障礙半徑進(jìn)行比較，根據(jù)轉(zhuǎn)彎半徑 和障礙半徑的大小關(guān)系，以及障礙間的相對位置，進(jìn)行地形的預(yù)處理； 步驟二：根據(jù)定常忽略粘性的不可壓縮理想流體，沿空間直角坐標(biāo)系X軸來流方向作 用于位于原點(diǎn)的圓球障礙繞流問題的解析解，在該直角坐標(biāo)系下求得流速沿X、Y和Z軸的 分量； 對于空間中任意一點(diǎn)P，極坐標(biāo)系建立在點(diǎn)P與X軸所構(gòu)成的平面上；空間直角坐標(biāo)系 的坐標(biāo)原點(diǎn)與平面二維極坐標(biāo)系的極點(diǎn)重合，為點(diǎn)0,空間直角坐標(biāo)系的X軸與平面二維極 坐標(biāo)系的極軸Γ軸重合；在該極坐標(biāo)系下流體的速度勢供為
OP與極軸之間的夾角為Θ角，點(diǎn)p與點(diǎn)〇的距離為IOPI = r，V ~為流體 的流速，a為圓球障礙的半徑，在空間直角坐標(biāo)系下，P的坐標(biāo)為（X，y，z)，則有
，用u、V和W代表三維情況下流速沿空間直角坐標(biāo)系X、Y和Z 軸的分量，則
步驟三：計(jì)算每個(gè)障礙的加權(quán)系數(shù)； 用m代表地形中障礙的數(shù)量，假設(shè)第i個(gè)障礙的半徑為ai，其球心位置為Ρ, Α. Λ ；)，定 義距離函數(shù)h如下：
-β,.其中 i = 1. · · m (x，y，z)表示步驟二中P點(diǎn)的坐標(biāo)，并定義插值函數(shù)α如下
其中 i = I. . . m ; 步驟四：結(jié)合步驟二和步驟三的方程組，對每個(gè)障礙進(jìn)行方程組求解并最后求和，得到 無人機(jī)三維飛行航路。
2. 根據(jù)權(quán)利要求1所述的基于理想流體數(shù)值計(jì)算的無人機(jī)三維航路規(guī)劃方法，其特征 在于：所述步驟1的具體方法如下： 1) 、障礙物重疊：計(jì)算與兩個(gè)障礙同時(shí)內(nèi)切的虛擬障礙，將虛擬障礙視為單個(gè)障礙進(jìn)行 避障，令Oi和〇i+1分別代表第i個(gè)和第i+Ι個(gè)障礙的球心，R i和Ri+1分別表示Oi和〇i+1的 半徑，在線段O iOw上找到一點(diǎn)Onew，以該點(diǎn)為圓心，做出與圓Oi和〇 i+1同時(shí)相內(nèi)切的圓〇MW， 其中Onew必須滿足下式條件 〇new〇i I +Ri = I 〇new〇i+l I +Ri+1 ； 2) 、單個(gè)障礙： 2. 1目標(biāo)點(diǎn)和起始點(diǎn)的連線沒有穿過障礙的中心，則將障礙的半徑進(jìn)行虛擬擴(kuò)張，使障 礙物半徑大于等于無人機(jī)的轉(zhuǎn)彎半徑，根據(jù)不同的安全距離，虛擬障礙物半徑的計(jì)算公式 為：虛擬障礙物半徑=無人機(jī)轉(zhuǎn)彎半徑+安全距離； 2. 2目標(biāo)點(diǎn)和起始點(diǎn)的連線穿過了障礙的中心，則預(yù)處理分為"真實(shí)障礙半徑比最小轉(zhuǎn) 彎半徑小"和"真實(shí)障礙半徑比最小轉(zhuǎn)彎半徑大"兩種情況； 2. 2. 1當(dāng)"真實(shí)障礙半徑比最小轉(zhuǎn)彎半徑小"時(shí)，令圓O1為真實(shí)障礙，將目標(biāo)點(diǎn)和起始 點(diǎn)的連線作為X軸，則X軸穿過了 O1，設(shè)無人機(jī)最小轉(zhuǎn)彎半徑為Rtl,過真實(shí)障礙O1做垂直于 X軸的直線交O1與A點(diǎn)，在AO1的延長線上找到一點(diǎn)O2，使得AO 2 = Rtl ;那么O2和Rtl所確定 的虛線圓O2，其為虛擬障礙，并且O 1比O2的半徑小，根據(jù)O2的設(shè)置方法，可知=O1是O 2的內(nèi) 切圓，切點(diǎn)為A ;在航路規(guī)劃時(shí)，將O2作為障礙進(jìn)行躲避； 2. 2. 2當(dāng)"真實(shí)障礙的半徑比最小轉(zhuǎn)彎半徑大"時(shí)，此時(shí)真實(shí)障礙的半徑比無人機(jī)最小 轉(zhuǎn)彎半徑大，只要在AO1的延長線上找到一點(diǎn)O2，以O(shè)2為圓心作圓，使得O 1O2 = Γ(Ι，Γ(Ι是設(shè)定 的一個(gè)小量，則O2的半徑大于O 1，而O1的半徑又大于最小轉(zhuǎn)彎半徑，并且X軸不會穿過〇2。
【文檔編號】G01C21/00GK104390640SQ201410639743
【公開日】2015年3月4日 申請日期:2014年11月13日 優(yōu)先權(quán)日:2014年11月13日
【發(fā)明者】梁宵, 孟光磊, 田豐, 陳國棟 申請人:沈陽航空航天大學(xué)