一種基于ph曲線的無人機(jī)路徑規(guī)劃方法
【專利摘要】本發(fā)明公開了一種基于PH曲線的無人機(jī)路徑規(guī)劃方法，包括：(1)初始化無人機(jī)運(yùn)動條件和任務(wù)地圖；(2)根據(jù)初始條件設(shè)定多個(gè)路徑控制點(diǎn)，并根據(jù)控制點(diǎn)形成PH曲線，并將其表示為貝塞爾n階多項(xiàng)式；(3)將貝塞爾n階多項(xiàng)式限定為五階多項(xiàng)式，并據(jù)此對其進(jìn)行求導(dǎo)運(yùn)算和兩點(diǎn)間插值運(yùn)算，確定出無人機(jī)路徑曲線形狀，并計(jì)算出相應(yīng)控制點(diǎn)位置參數(shù)；(4)調(diào)整回歸系數(shù)以改變切矢量的長度，以滿足PH曲線約束條件和無人機(jī)約束條件，從而實(shí)現(xiàn)對控制點(diǎn)的修正，獲得可規(guī)避障礙物的無人機(jī)路徑。本發(fā)明的方法可以解決現(xiàn)有技術(shù)中存在的無法規(guī)避路徑中障礙物、路徑規(guī)劃精確度低、路徑不滿足無人機(jī)運(yùn)動學(xué)約束的問題。
【專利說明】
一種基于PH曲線的無人機(jī)路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于無人機(jī)路徑規(guī)劃技術(shù)領(lǐng)域，尤其是涉及一種無人機(jī)避障飛行路徑規(guī)劃 方法。
【背景技術(shù)】
[0002] 無人機(jī)(Unmanned Aerial Vehicle)，通常是指利用無線電遙控設(shè)備和自備的程 序控制裝置操縱的不載人飛機(jī)。與載人飛機(jī)相比，它具有體積小、造價(jià)低、使用方便、對操作 環(huán)境要求低、生存能力較強(qiáng)等特性，這些特有的優(yōu)越性能使其在應(yīng)用方面具有很大的發(fā)展 潛力。軍事上可應(yīng)用于情報(bào)獲取、地面戰(zhàn)場偵察和監(jiān)視、近距離空中支持和禁飛巡邏、電子 戰(zhàn)、通信中繼等方面，在民用方面一樣大有可為，如重大自然災(zāi)害之后的搜索與救援、巡邏 監(jiān)視和目標(biāo)跟蹤、輯毒和反走私、高壓線/大橋/水壩和地震后路段的檢查、航拍和成圖等方 面。
[0003] 無人機(jī)路徑規(guī)劃的目的是在滿足特定的約束條件下，尋找初始位置和目的地之間 的一條最優(yōu)或接近最優(yōu)的飛行路徑。根據(jù)無人機(jī)對環(huán)境信息掌握的程度不同，路徑規(guī)劃可 以分為基于模型的全局路徑規(guī)劃和基于傳感器的局部路徑規(guī)劃。其中，全局規(guī)劃方法主要 包括構(gòu)型空間法、拓?fù)浞?、柵格解耦法、自由空間法、神經(jīng)網(wǎng)絡(luò)法等。
[0004] 專利文獻(xiàn)CN 104808688A公開了一種無人機(jī)曲率連續(xù)可調(diào)路徑規(guī)劃方法，包括以 下步驟:第一步，路徑曲率連續(xù)化，利用參數(shù)化的CatmulΙ-Rom曲線連接各路標(biāo)點(diǎn)，通過優(yōu)化 插值方法保證連結(jié)點(diǎn)處曲率連續(xù);第二步，計(jì)算路徑曲線各點(diǎn)曲率值，查找并標(biāo)注曲率值超 過規(guī)定閾值的曲線起點(diǎn)和終點(diǎn)；第三步，采用最小曲率圓轉(zhuǎn)移法，利用曲率值單調(diào)變化的 Bezier曲線連接起點(diǎn)和終點(diǎn)。與現(xiàn)有技術(shù)相比，提出了一種經(jīng)過所有路標(biāo)點(diǎn)的平滑路徑規(guī) 劃方法，采用參數(shù)化的Catmul Ι-Rom曲線，能夠確保路徑經(jīng)過所有路標(biāo)點(diǎn)，同時(shí)通過優(yōu)化插 值算法，保證路徑各點(diǎn)曲率連續(xù)，而且利用曲率單調(diào)平滑連接算法，控制路徑曲率極值范 圍，使路徑在滿足無人機(jī)運(yùn)動學(xué)條件前提下，具有精確性和可行性。
[0005] 但是上述路徑規(guī)劃方法對路徑中障礙物的規(guī)避問題涉及不多，而且其規(guī)劃精度仍 有提尚空間。

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

[0006] 本發(fā)明的目的是提供一種無人機(jī)避障飛行路徑規(guī)劃方法，其利用PH曲線實(shí)現(xiàn)對路 徑的優(yōu)化，解決現(xiàn)有技術(shù)中存在的無法規(guī)避路徑中障礙物、路徑規(guī)劃精確度低、路徑不滿足 無人機(jī)運(yùn)動學(xué)約束的問題。
[0007] 為實(shí)現(xiàn)上述目的，按照本發(fā)明的一個(gè)方面，提供一種基于PH曲線的無人機(jī)路徑規(guī) 劃方法，包括：
[0008] (1)初始化無人機(jī)運(yùn)動條件和任務(wù)地圖，包括設(shè)定無人機(jī)路徑的起始點(diǎn)、終點(diǎn)以及 沿途中需要經(jīng)過的目標(biāo)點(diǎn)，標(biāo)注各障礙物的位置坐標(biāo)和威脅半徑，設(shè)定無人機(jī)的最大飛行 速度、最大飛行加速度以及最大飛行曲率；
[0009] (2)根據(jù)初始條件設(shè)定多個(gè)路徑控制點(diǎn)，并根據(jù)所述控制點(diǎn)形成PH曲線，并將其表 示為貝塞爾η階多項(xiàng)式；
[0010] ⑶根據(jù)最低階含有拐點(diǎn)的ΡΗ曲線為五階ΡΗ曲線，將所述貝塞爾η階多項(xiàng)式限定為 五階多項(xiàng)式，并據(jù)此對其進(jìn)行求導(dǎo)運(yùn)算和兩點(diǎn)間插值運(yùn)算，確定出無人機(jī)路徑曲線形狀，并 計(jì)算出相應(yīng)控制點(diǎn)位置參數(shù)；
[0011] (4)調(diào)整回歸系數(shù)以改變切矢量的長度，以滿足ΡΗ曲線約束條件和無人機(jī)約束條 件，實(shí)現(xiàn)對控制點(diǎn)的修正，從而獲得可規(guī)避障礙物的無人機(jī)路徑。
[0012] 作為本發(fā)明的進(jìn)一步優(yōu)選，所述貝塞爾η階多項(xiàng)式為：
[0014]式中，r(q) |q=o和r(q) |q=1分別表示路徑的起點(diǎn)和終點(diǎn)，q是被正規(guī)化后的路徑參 數(shù)
[0015] 作為本發(fā)明的進(jìn)一步優(yōu)選，所述步驟(3)中，對應(yīng)所述五階PH曲線的控制點(diǎn)為六 個(gè)，包括路徑起點(diǎn)PQ、終點(diǎn)P5以及位于起點(diǎn)與終點(diǎn)之間從該起點(diǎn)至終點(diǎn)依次排布的四個(gè)控制 點(diǎn)P1，P2，P3和P4，該步驟(3 )具體過程如下：
[0016] (3.1)將無人機(jī)路徑起點(diǎn)和終點(diǎn)位置切線方向的正切向量Ι?^ΓΙ和IFKI表示成切 矢量形式，切矢量分別用回歸系數(shù)cdP C1修正：
[0019] 其中，C0和C1為回歸系數(shù)，COe [ 1，00 ]，Cle [ 1，00 ]，錢；.和齡分別為初始位置和終止 位置的切向角，山和也分別為路徑起點(diǎn)和終點(diǎn)位置切線方向的切矢量。
[0020] (3.2)將q = 0和q=l時(shí)得到的初始位置和目標(biāo)位置坐標(biāo)帶入所述五階多項(xiàng)式并進(jìn) 行一階求導(dǎo)，得到控制點(diǎn)P#PP4的位置參數(shù)：
[0023]作為本發(fā)明的進(jìn)一步優(yōu)選，所述步驟(4)中，對控制點(diǎn)的修正包括對控制點(diǎn)P2和P3 的修正，其中，所述控制點(diǎn)p#Pp3的數(shù)學(xué)表達(dá)式為：

[0031]作為本發(fā)明的進(jìn)一步優(yōu)選，控制點(diǎn)P1，P4，P5的位置參數(shù)的數(shù)學(xué)表達(dá)式如下；
[0035] 總體而言，通過本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，具有以下有益效 果:本發(fā)明中通過PH曲線具體為五階PH曲線進(jìn)行無人機(jī)路徑規(guī)劃，并通過控制點(diǎn)的求導(dǎo)和 插值運(yùn)算，確定路徑曲線形狀，同時(shí)結(jié)合調(diào)整回歸系數(shù)以改變切矢量的長度，以滿足PH曲線 約束條件和無人機(jī)約束條件，從而實(shí)現(xiàn)對控制點(diǎn)的修正，獲得可規(guī)避障礙物的無人機(jī)路徑， 具有精度高，能夠規(guī)避障礙物的優(yōu)勢。
【附圖說明】
[0036] 圖1:為本發(fā)明實(shí)施例的方法流程圖；
[0037]圖2:為本發(fā)明實(shí)施例的貝塞爾曲線圖；
[0038] 圖3:為本發(fā)明實(shí)施例的和聲算法流程圖；
[0039] 圖4:為本發(fā)明實(shí)施例的碰撞條件判據(jù)圖；
[0040] 圖5:為本發(fā)明實(shí)施例具體實(shí)施例示意圖；
【具體實(shí)施方式】
[0041] 為了便于本領(lǐng)域普通技術(shù)人員理解和實(shí)施本發(fā)明，下面結(jié)合附圖及實(shí)施例對本發(fā) 明作進(jìn)一步的詳細(xì)描述，應(yīng)當(dāng)理解，此處所描述的實(shí)施示例僅限于說明和解釋本發(fā)明，并不 用于限定本發(fā)明。
[0042] 如圖1，本發(fā)明實(shí)施例所提供的一種無人機(jī)基于PH曲線和和聲搜索算法進(jìn)行規(guī)避 障礙物的路徑規(guī)劃方法，包括以下步驟：
[0043] (1)初始化無人機(jī)運(yùn)動條件和任務(wù)地圖，標(biāo)注各障礙物的位置坐標(biāo)和威脅半徑，設(shè) 定無人機(jī)的最大飛行速度、最大飛行加速度、最大飛行曲率；
[0044] 該步驟中，具體包括初始化地圖，設(shè)定無人機(jī)在該次任務(wù)中的起始點(diǎn)、終點(diǎn)以及沿 途中需要經(jīng)過的目標(biāo)點(diǎn)，根據(jù)無人機(jī)運(yùn)動學(xué)約束，設(shè)定無人機(jī)飛行的高度，最大飛行速度， 最大飛行加速度，最大飛行曲率等。
[0045] 設(shè)定飛行過程中無人機(jī)在指定高度勻速飛行。
[0046] (2)根據(jù)初始條件給定多個(gè)控制點(diǎn)po，P1，…，pn，根據(jù)相應(yīng)控制點(diǎn)形成對應(yīng)的PH曲 線，并表示成B6zier形式下的η階多項(xiàng)式。
[0047] 本步驟中，根據(jù)給定的飛行任務(wù)，飛行地圖和運(yùn)動學(xué)約束條件設(shè)定控制點(diǎn)，并根據(jù) 控制點(diǎn)寫出Bezier樣條曲線，如圖2,其具體實(shí)現(xiàn)包括以下步驟：
[0048] (2 · 1)根據(jù)無人機(jī)飛行條件設(shè)定控制點(diǎn)Pk = (Xk，yk)，k = 0，，1，2，…，η，Xk，yk分別表 示控制點(diǎn)二維坐標(biāo)?？刂泣c(diǎn)用PH曲線表示，并表達(dá)為Β?ζ ier形式下的n階多項(xiàng)式：
[0050]式中：r(q) |q=o和r(q) |q=1分別表示路徑的起點(diǎn)和終點(diǎn)，q是被正規(guī)化后的路徑參 數(shù)。
[0052] (2.2)利用求導(dǎo)公式求得η階Bgzier曲線的第r階導(dǎo)數(shù)，可以寫成如下形式：
[0054] 其中：
[0055] Δ rPj =pj+1-Pj，Δ 〇pj =Pj。
[0056] (2.3)在路徑規(guī)劃中，最低階含有拐點(diǎn)的PH曲線是五次PH曲線，即η = 5。取6個(gè)控制 點(diǎn)：PQ(xo，y())，pi(xi，yi)，P2(X2，y2)，p3(X3，y3)，p4(X4，y4)，p5(X5，y5)，當(dāng)n = 5時(shí)等式（1)變?yōu)?如下形式：
[0058] (2.4)將五階Β?ζ i er曲線表示成由控制點(diǎn)組成時(shí)的形式，控制點(diǎn)之間的數(shù)學(xué)表達(dá) 式如下；
[0060] 其中，pk= (xk，yk)，k = 0,，1，2，···，n，xk，yk分別表示控制點(diǎn)二維坐標(biāo)，uq，ui，U2，vo， VI，V2為互素多項(xiàng)式，用這些多項(xiàng)式來構(gòu)造 PH曲線。
[0061] (2.5)根據(jù)等式(3)求出五階PH路徑的一階導(dǎo)數(shù)，用于之后的插值運(yùn)算。路徑r(q) 的一階導(dǎo)數(shù)為：
[0063] (3)對5階PH多項(xiàng)式作求導(dǎo)運(yùn)算和兩點(diǎn)間插值運(yùn)算，確定連接曲線形狀，計(jì)算得出 控制點(diǎn)PQ、PI、P5、P4位置參數(shù)。
[0064] 見圖3,依托只能算法的優(yōu)勢，利用和聲搜索算法進(jìn)行反復(fù)迭代搜索，直到找到最 優(yōu)的位置參數(shù)。和聲搜索算法是業(yè)內(nèi)成熟的算法，在此不再贅述。
[0065] (3.1)將無人機(jī)路徑起點(diǎn)和終點(diǎn)位置切線方向的正切向量|兩瓦|和1??]表示成切 矢量形式，切矢量分別用回歸系數(shù)CQ和C1修正，c〇e [1，00]，ClE [1，00];
[0067] (3.2)將q = 0和q=l時(shí)得到的初始位置和目標(biāo)位置坐標(biāo)帶入到等式(4)和(6)求路 徑的一階導(dǎo)數(shù)，得到控制點(diǎn)P1和P4，計(jì)算公式如下所示：
[0069] (4)調(diào)整曲線參數(shù)增加切矢量丨^?和Ι?1的長度，修正控制點(diǎn)pdPp3進(jìn)行規(guī)避 障礙物，并同時(shí)滿足PH曲線約束條件和無人機(jī)飛行約束條件。
[0070] 當(dāng)無人機(jī)路徑上的點(diǎn)到障礙物中心的距離小于威脅半徑時(shí)，這判斷無人機(jī)將于障 礙物碰撞，反之則證明無人機(jī)路徑安全，如圖4所示。
[0071] (4.1)結(jié)合等式(5)和等式⑶計(jì)算得到(UQ，VQ)和(U2，V2)，計(jì)算結(jié)果如下式所示；
[0074] (4.2)將(110，￥0)和(112，￥2)帶入到公式(5)可以求得(111，￥1)的表達(dá)式 ；
[0077] (4.3)將(110，￥0)、（111，￥1)和(112，￥2)帶入到公式(5)求得控制點(diǎn)口2和口3;
[0078] (4.4)將得到的6個(gè)控制點(diǎn)代入PH曲線的表達(dá)式，即下面公式（11)，調(diào)整回歸系數(shù) CdPC1的大小，改變起始點(diǎn)和終止點(diǎn)的切矢量長度，使路徑規(guī)避障礙物，并得到最終規(guī)劃好 的無人機(jī)可飛行路徑，完成路徑規(guī)劃。
[0080] 如圖5,在一個(gè)具體實(shí)施例中，無人機(jī)任務(wù)的起始點(diǎn)坐標(biāo)和終點(diǎn)目標(biāo)分別為(0,0)， (100，100 )，測試地圖大小120* 120m。障礙物1的位置中心坐標(biāo)為(35，40 )，威脅半徑大小為 l〇m，障礙物2的位置中心坐標(biāo)為(35,40)，威脅半徑大小為20m。圖中點(diǎn)畫線和虛線為路徑規(guī) 劃調(diào)整過程中的路徑，都不能規(guī)避障礙物，只有實(shí)線滿足無人機(jī)可飛行條件，并能規(guī)避障礙 物。
[0081] 應(yīng)當(dāng)理解，上述針對較佳實(shí)施例的描述較為詳細(xì)，并不能因此而認(rèn)為是對本發(fā)明 專利保護(hù)范圍的限制，本領(lǐng)域的普通技術(shù)人員在本發(fā)明的啟示下，在不脫離本發(fā)明權(quán)利要 求所保護(hù)的范圍情況下，還可以做出替換或者變形，均落入本發(fā)明的保護(hù)范圍之內(nèi)，本發(fā)明 的請求保護(hù)范圍應(yīng)所附權(quán)利要求為準(zhǔn)。
【主權(quán)項(xiàng)】
1. 一種基于PH曲線的無人機(jī)路徑規(guī)劃方法，包括： (1) 初始化無人機(jī)運(yùn)動條件和任務(wù)地圖，包括設(shè)定無人機(jī)路徑的起始點(diǎn)、終點(diǎn)以及沿途 中需要經(jīng)過的目標(biāo)點(diǎn)，標(biāo)注各障礙物的位置坐標(biāo)和威脅半徑，設(shè)定無人機(jī)的最大飛行速度、 最大飛行加速度以及最大飛行曲率； (2) 根據(jù)初始條件設(shè)定多個(gè)路徑控制點(diǎn)，并根據(jù)所述控制點(diǎn)形成PH曲線，并將其表示為 貝塞爾η階多項(xiàng)式； (3) 根據(jù)最低階含有拐點(diǎn)的ΡΗ曲線為五階ΡΗ曲線，將所述貝塞爾η階多項(xiàng)式限定為五階 多項(xiàng)式，并據(jù)此對其進(jìn)行求導(dǎo)運(yùn)算和兩點(diǎn)間插值運(yùn)算，確定出無人機(jī)路徑曲線形狀，并計(jì)算 出相應(yīng)控制點(diǎn)位置參數(shù)； (4) 調(diào)整回歸系數(shù)以改變切矢量的長度，以滿足ΡΗ曲線約束條件和無人機(jī)約束條件，實(shí) 現(xiàn)對控制點(diǎn)的修正，從而獲得可規(guī)避障礙物的無人機(jī)路徑。2. 根據(jù)權(quán)利要求1所述的一種基于ΡΗ曲線的無人機(jī)路徑規(guī)劃方法，其中，所述貝塞爾η 階多項(xiàng)式為：式中，r(q)|q=o和分別表示路徑的起點(diǎn)和終點(diǎn)，q是被正規(guī)化后的路徑參數(shù)，3. 根據(jù)權(quán)利要求1或2所述的一種基于PH曲線的無人機(jī)路徑規(guī)劃方法，其中，所述步驟 (3)中，對應(yīng)所述五階PH曲線的控制點(diǎn)為六個(gè)，包括路徑起點(diǎn)po、終點(diǎn)p 5以及位于起點(diǎn)與終點(diǎn) 之間從該起點(diǎn)至終點(diǎn)依次排布的四個(gè)中間控制點(diǎn)pi，P2，P3和P4，該步驟⑶具體過程如下： (3.1) 將無人機(jī)路徑起點(diǎn)和終點(diǎn)位置切線方向的正切向量丨和|p4Psl表示成切矢量 形式，切矢量分別用回歸系數(shù)cdPC1修正：其中，CQ和C1為回歸系數(shù)，c〇e [1，〇〇]，ci￡ [1，〇〇]，05和叫分別為初始位置和終止位置 的切向角，山和也分別為路徑起點(diǎn)和終點(diǎn)位置切線方向的切矢量； (3.2) 將q = 0和q=l時(shí)得到的初始位置和目標(biāo)位置坐標(biāo)帶入所述五階多項(xiàng)式并進(jìn)行一 階求導(dǎo)，得到中間控制點(diǎn)P#PP4的位置參數(shù)：4. 根據(jù)權(quán)利要求3所述的一種基于PH曲線的無人機(jī)路徑規(guī)劃方法，其中，中間控制點(diǎn)p2 和P3的數(shù)學(xué)表達(dá)式為：其中，a，b，c為中間參數(shù)，且5.根據(jù)權(quán)利要求1或2所述的一種基于PH曲線的無人機(jī)路徑規(guī)劃方法，其中，其中，中間 控制點(diǎn)pi，P4，P5的位置參數(shù)的數(shù)學(xué)表達(dá)式如下；
【文檔編號】G05D1/10GK105867421SQ201610352890
【公開日】2016年8月17日
【申請日】2016年5月25日
【發(fā)明人】高亮, 易進(jìn), 鄔嘉揚(yáng), 徐奧, 李新宇
【申請人】華中科技大學(xué)