本發(fā)明涉及通用航空管制領域技術，特別涉及一種基于準均勻樣條曲線的無人機路徑修正方法及系統(tǒng)。

背景技術：

隨著各種無人機的普及和小型化，有關無人機管制技術的研究也變得越來越多，研究方向也越來越細致。無人機樓宇穿行除了障礙物不規(guī)則、陡變的特征外，還有一個顯著的特征就是密集排列。當需要規(guī)劃的路徑非常長的時候，路徑也有很大的概率會變得很復雜，如果路徑是由控制點所控制的曲線，那遠遠不是3、5個控制點所能解決的；而密集的樓宇還有一個問題，就是如果某一區(qū)域的路徑規(guī)劃沒問題的話，我們并不希望它改變，因為改變會引發(fā)不確定性。也就是說，當一個區(qū)域的路徑與障礙沖突需要修正的時候，其它區(qū)域的路徑能保持不變。而bezier曲線、ph曲線都是全局曲線，改變一處，整條曲線都會跟著改變，這種特性對于復雜的長路徑規(guī)劃和密集排列的樓宇障礙非常不利。

對于無人機的路徑規(guī)劃問題，除了考慮避障，還要考慮的是無人機的跟隨問題。如果規(guī)劃的路徑，無人機無法按照路徑飛行，那這樣的路徑規(guī)劃出來其效果也會打折扣。通常，無人機的跟隨性和無人機本身的物理特性有關，不同的無人機跟隨性也會不同。但它們基本上的飛行路徑都是曲率連續(xù)(c2連續(xù))的，所以，對無人機規(guī)劃出的路徑也必須是曲率連續(xù)的。

因此，亟待研發(fā)一種既能夠滿足飛行路徑全段c2曲率連續(xù)性，整條b樣條曲線中出現直線段并不會影響到無人機的跟隨性，且不需要修正的曲線段盡可能多地保持原樣的無人機路徑修正方法及系統(tǒng)。

技術實現要素：

有鑒于此，有必要提供一種既能夠滿足飛行路徑全段c2曲率連續(xù)性，整條b樣條曲線中出現直線段并不會影響到無人機的跟隨性，且不需要修正的曲線段盡可能多地保持原樣的無人機路徑修正方法及系統(tǒng)。

一種基于準均勻樣條曲線的無人機路徑修正方法，所述基于準均勻樣條曲線的無人機路徑修正方法包括如下步驟：

s1、建立動態(tài)障礙物模型，規(guī)劃一條連接起點和終點的直線段路徑；

s2、以直線段之間的交點為管制點，直線段路徑為控制多邊形，繪制準均勻三次b樣條曲線；

s3、找出與障礙物相交的曲線段以及相交曲線段所對應直線段，在對應直線段上添加控制點，調整曲線使其通過該控制點；

s4、更新管制點組和曲線，并根據無人機的曲率約束和安全約束微調管制點。

一種基于準均勻樣條曲線的無人機路徑修正系統(tǒng)，所述基于準均勻樣條曲線的無人機路徑修正系統(tǒng)包括如下功能模塊：

初級線路規(guī)劃模塊，用于建立動態(tài)障礙物模型，規(guī)劃一條連接起點和終點的直線段路徑；

b樣條曲線繪制模塊，用于以直線段之間的交點為管制點，直線段路徑為控制多邊形，繪制準均勻三次b樣條曲線；

曲線調整模塊，用于找出與障礙物相交的曲線段以及相交曲線段的四個管制點，根據順序連接四個管制點的形狀，對曲線進行相應調整；

線路更新模塊，用于更新管制點組和曲線，并根據無人機的曲率約束和安全約束微調管制點。

本發(fā)明提供一種基于準均勻樣條曲線的無人機路徑修正方法及系統(tǒng)，其基于控制點越多，曲線的局部特性越突出的原理，并結合b樣條曲線的特性，本發(fā)明采用增加控制點的辦法來修正b樣條曲線，使曲線逐漸靠近由控制點構成的多邊形，從而得到一條能夠精確避開障礙物的，既滿足了飛行路徑全段c2曲率連續(xù)性，又使不需要修正的曲線段盡可能多地保持原樣的無人機飛行路徑。

附圖說明

圖1是本發(fā)明所述的基于準均勻樣條曲線的無人機路徑修正方法的步驟流程圖；

圖2是本發(fā)明所述的基于準均勻樣條曲線的無人機路徑修正方法流程圖；

圖3是圖1中步驟s1的子流程圖；

圖4是圖1中步驟s3的子流程圖；

圖5為模型中用vorinoi圖法和a*搜索算法產生直線段路徑的示意圖；

圖6為模型中初始的b樣條曲線的示意圖；

圖7為在線段中間插入單個控制點的路徑調整示意圖；

圖8為在線段中間插入兩個控制點的路徑調整示意圖；

圖9為移動中間控制點的路徑調整示意圖；

圖10為計算插入控制點的曲率和安全距離的示意圖；

圖11為兩條相信曲線段與同一個障礙相交的示意圖；

圖12為在相領兩條控制線段上添加兩個控制點的示意圖。

具體實施方式

為了使本發(fā)明的目的、技術方案及優(yōu)點更加清楚明白，以下結合附圖及實施例，對本發(fā)明進行進一步詳細說明，應當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

如圖1所示，本發(fā)明實施例提供一種基于準均勻樣條曲線的無人機路徑修正方法，如圖2所示，所述基于準均勻樣條曲線的無人機路徑修正方法包括如下步驟：

s1、建立動態(tài)障礙物模型，規(guī)劃一條連接起點和終點的直線段路徑。

具體如圖3所示，所述步驟s1包括以下分步驟：

s11、繪制初始路徑周圍的障礙，每一個障礙用矩形表示；

s12、用voronoi圖法產生連接圖，如圖5所示；

s13、結合無人機具體執(zhí)行任務的需求，通過a*搜索算法得到一條連接起點和終點的直線段路徑，如圖5(b)所示；

s2、以直線段之間的交點為管制點，直線段路徑為控制多邊形，繪制準均勻三次b樣條曲線，如圖6所示。

具體的，在得到準均勻三次b樣條曲線后，需要對管制點進行微調，使其滿足無人機的曲率約束和安全約束。

所述三次b樣條曲線是由任意數量的曲線段組成的完全的分段三次多項式。系數變化引起某一區(qū)間上的三次片段變化，并從一個區(qū)間到另一個區(qū)間逐個進行。b樣條曲線不通過其控制點。

對于任意一個b樣條片段，其公式為：

其中qi為第i個b樣條片段，pi是按控制點順序排列的一個四個點的集合。

在這樣的表示形式下，一條b樣條曲線是m-2個曲線段的系列。為方便起見，將其標記為q3、q4、q5、……、qm，這些曲線段由m+1個控制點p0、p1、……、pm來定義的，其同m≥3。每一個曲線段都是由四個控制點來定義的，而每一個控制點會影響連續(xù)的四條且只有這四條曲線段。這就是b樣條曲線的局部控制特性。

準均勻b樣條曲線是非均勻b樣條曲線的一種特殊形式，它通過在兩端插入結點，而在其它地方采用均勻的結點。這樣就使起點和終點的連續(xù)結點值區(qū)間減小到零，從而使b樣條曲線與兩個端點相交。經常會使用的結點序列為[0,0,0,0,1,2,…,n-2,n-1,n,n,n,n]。

b樣條曲線表現出位置連續(xù)性(切線連續(xù)性(c1)和曲率連續(xù)性(c2))、局部性、凸包性、保凸性、對稱性、遞推性、幾何及仿射不變性等性能。其中，連續(xù)性是最重要的特征，且曲線上的控制點可以任意移動而不影響連續(xù)性；而局部性也是很重要的特征，以三次曲線為例，每一個曲線段都是由四個控制點來定義的，而每一個控制點會影響連續(xù)的四條且只有這四條曲線段。

由于所述準均勻三次b樣條曲線僅與起點和終點相交，與其它控制點不相交，所以盡管所有的控制點和控制多邊形都能避開障礙物，但b樣條曲線本身卻未必能避開。因此，規(guī)劃出來的初始路徑難免會有與障礙物相交的情況出現，因此，需要進行步驟s3，進行相應的修正措施。

s3、找出與障礙物相交的曲線段以及相交曲線段所對應直線段，在對應直線段上添加控制點，調整曲線使其通過該控制點。

具體的，分段的樣條曲線具有如下特性：首先，它是曲線連續(xù)的；第二，理論上樣條曲線的控制點可以無限增加而不影響它的計算復雜度，因此樣條曲線的形式可以很復雜，適合長距離的路徑規(guī)劃；第三，樣條曲線是由若干條曲線段構成的，因此它是局部性的，改變一個控制點，只會改變與該控制點有關的相鄰幾條曲線段，而其它的曲線段可以保持不變，這種性質很適合密集樓宇的路徑規(guī)劃。

b樣條曲線具有如下特性：當三個控制點在同一條直線上時，那么b樣條曲線就經過中點的那個控制點；而當四個控制點在同一條直線上時，那么b樣條曲線在中間兩個控制點之前是一條直線段，對于無人機來說，其采納路徑優(yōu)先度為直線>c2曲線>c1曲線>c0曲線，只要滿足全段c2曲率連續(xù)性，整條b樣條曲線中出現直線段并不會影響到無人機的跟隨性。

結合分段的樣條曲線以及b樣條曲線的特性，所述步驟s3的具體修正措施如下。

如圖4所示，所述步驟s3包括以下分步驟：

s31、找出與障礙物相交的曲線段，以及相交曲線段所對應直線段的四個管制點，

s32、根據順序連接四個管制點的形狀，確定在對應直線段上添加控制點以及添加控制點的數量；

s33、根據控制點對曲線進行調整，使曲線通過添加的控制點。

其中，步驟s32包括下列情況；

情況一：如果四個管制點的連接形狀為四邊形，則在對應直線段上添加一個控制點，使曲線通過該控制點；如果四個管制點的連接形狀為四邊形，且在對應直線段上添加一個控制點之后仍無法避開障礙，則取消該控制點，并在對應直線段上添加兩個控制點，將修正的曲線的中間段變成直線段，使曲線通過該直線段。

如圖7(a)所示，假設四個管制點分別為p0、p1、p2、p3，如果p0、p1、p2、p3之間的連線構成四邊形，根據b樣條曲線凸包特性，在p1和p2之間的線段上添加一個控制點p12，曲線段將變成如圖7(b)所示，則曲線避開障礙物，滿足無人機路徑規(guī)劃要求。

如果進行上述調整后還不能避開障礙物，如圖8(a)所示，則表示兩個障礙物之間通道很窄，對于這種情況，以直線通過這個通道則更安全。因此，取消p12，在p1和p2之間添加兩個控制點p12和p21，將修正的曲線的中間段變成直線，如圖8(b)所示，避開障礙物，滿足無人機路徑規(guī)劃要求。

情況二：如果四個管制點的連接形狀為閃電形，則將位于四個管制點中間的兩個管制點中的其中一個管制點向相鄰兩個管制點的方向進行調整，使閃電形調整為四邊形，之后再根據四邊形的規(guī)則進行調整。

如圖9(a)所示，如果p0、p1、p2、p3之間的連線構成閃電形，即線段p0p3和p1p2相交，則首先要將閃電形調整為四邊形，即將p1向線段p0p2之間移動，或p2向p1p3之間移動，本實施例中是將p2向p1p3之間移動；之后在p1和p2之間的線段上添加一個控制點p12，在p2和p3之間的線段上添加一個控制點p23，使曲線段將變成如圖9(b)所示，則曲線避開障礙物，滿足無人機路徑規(guī)劃要求。

上述路徑規(guī)劃中，如圖10所示，最佳控制點的求取步驟如下：

1)在對應直線段上選擇多個點，分別以每一個點為添加控制點來修正路徑；

2)判斷每一條修正路徑是否還與障礙物相交，剔除相交的路徑，保留不相交路徑所對應的控制點；

3)根據控制點的曲率和安全距離的權值，計算出每一個控制點修正后曲線的最大曲率和最短安全距離，代入到下列公式中：

式中，k為修正曲線在p12處的曲率；l為p12到障礙物最短安全距離；k越小越能保障無人機的通行能力，而l越大越能保障無人機的飛行安全，w1和w2分別為k和的權值，不同的使用者可以設定不同的權值，當無人機對最大曲率的要求嚴格時，可提高w1的比重；當無人機大小比較大時，則可提高w2的比重。

選擇的最小值，其對應的控制點即是最佳添加控制點。

步驟s32還包括情況三；

情況三：如果四個管制點的連接形狀為四邊形，且同一個障礙物與連續(xù)兩個曲線段相交，則取兩條對應直線段的四個管制點的合集得到五個管制點，在兩條對應直線段上分別添加一個控制點，使曲線同時通過這兩個控制點。

在某種極端情況下，可能會出現同一個障礙物與連續(xù)兩個曲線段相交的情況，如圖11所示，那么令這兩個曲線段的管制點分別為p0、p1、p2、p3和p1、p2、p3、p4，可以認為兩個曲線段的管制點為p0、p1、p2、p3、p4五個點。這種極端情況很大的可能性是該障礙物與它周圍的障礙物之間的距離較近，因此控制多邊形離該障礙物較近所致(按voronoi法，控制多邊形不會與障礙相交)，如圖9所示。

出現這樣的情況時，用移動管制點的方法會十分受限，因此還是采用添加控制點的方法，在線段p1p2和p2p3上分別添加兩個控制點p12和p23，則修正后的路徑如圖12所示。

情況三中，所述兩條對應直線段上添加的兩個控制點組成一個控制點組，其中最佳的控制點組的求取步驟如下：

1)在兩條對應直線段上分別選擇m和n個點構成m*n個點組，分別以每一個點組為添加控制點組來修正路徑；

2)判斷每一條修正路徑是否還與障礙物相交，剔除相交的路徑，保留不相交路徑所對應的控制點組；

3)根據控制點的曲率和安全距離的權值，計算出每一控制點組修正后曲線的最大曲率和最短安全距離，代入到下列公式中：

選擇的最小值，其對應的控制點即是最佳添加控制點組。

s4、更新管制點組和曲線，并根據無人機的曲率約束和安全約束微調管制點。

根據上述基于準均勻樣條曲線的無人機路徑修正方法，本發(fā)明還提供一種基于準均勻樣條曲線的無人機路徑修正系統(tǒng)，所述基于準均勻樣條曲線的無人機路徑修正系統(tǒng)包括如下功能模塊：

初級線路規(guī)劃模塊，用于建立動態(tài)障礙物模型，規(guī)劃一條連接起點和終點的直線段路徑；

b樣條曲線繪制模塊，用于以直線段之間的交點為管制點，直線段路徑為控制多邊形，繪制準均勻三次b樣條曲線；

曲線調整模塊，用于找出與障礙物相交的曲線段以及相交曲線段的四個管制點，根據順序連接四個管制點的形狀，對曲線進行相應調整；

線路更新模塊，用于更新管制點組和曲線，并根據無人機的曲率約束和安全約束微調管制點。

其中，所述初級線路規(guī)劃模塊包括如下功能單元：

模型建立單元，用于繪制初始路徑周圍的障礙，每一個障礙用矩形表示；

圖形生成單元，用于用voronoi圖法產生連接圖；

路徑計算單元，用于結合無人機具體執(zhí)行任務的需求，通過a*搜索算法得到一條連接起點和終點的直線段路徑。

其中，所述曲線調整模塊包括如下功能單元：

管制點提取單元、用于找出與障礙物相交的曲線段，以及相交曲線段所對應直線段的四個管制點，

控制點確定單元、用于根據順序連接四個管制點的形狀，確定在對應直線段上添加控制點以及添加控制點的數量；

路徑調整單元、用于根據控制點對曲線進行調整，使曲線通過添加的控制點。

本發(fā)明提供了一種基于準均勻樣條曲線的無人機路徑修正方法及系統(tǒng)，其基于控制點越多，曲線的局部特性越突出的原理，并結合b樣條曲線的特性，本發(fā)明采用增加控制點的辦法來修正b樣條曲線，使曲線逐漸靠近由控制點構成的多邊形，從而得到一條能夠精確避開障礙物的，既滿足了飛行路徑全段c2曲率連續(xù)性，又使不需要修正的曲線段盡可能多地保持原樣的無人機飛行路徑。

本發(fā)明應用在低空無人機飛躍山地和城市樓宇的情況下，能夠具有較高的避障效率，能夠提升能充分提高無人機飛行的效能，能夠規(guī)劃出復雜的路徑而不顯著增加運算量。因此，可廣泛應用于無人機路徑規(guī)劃的實際任務應用。

以上裝置實施例與方法實施例是一一對應的，裝置實施例簡略之處，參見方法實施例即可。

本說明書中各個實施例采用遞進的方式描述，每個實施例重點說明的都是與其他實施例的不同之處，各個實施例之間相同相似部分互相參見即可。

專業(yè)人員還可以進一步意識到，結合本文中所公開的實施例描述的各示例的單元及算法步驟，能夠以電子硬件、計算機軟件或者二者的結合來實現，為了清楚地說明硬件和軟件的可互換性，在上述說明中已經按照功能性一般性地描述了各示例的組成及步驟。這些功能究竟以硬件還是軟件方式來執(zhí)行，取決于技術方案的特定應用和設計約束條件。專業(yè)技術人員可以對每個特定的應用來使用不同方法來實現所描述的功能，但是這種實現不應超過本發(fā)明的范圍。

結合本文中所公開的實施例描述的方法或算法的步驟可以直接用硬件、處理器執(zhí)行的軟件模塊，或者二者的結合來實施。軟件模塊可以置于隨機儲存器、內存、只讀存儲器、電可編程rom、電可擦除可編程rom、寄存器、硬盤、可移動磁盤、cd-rom、或技術領域內所公知的任意其他形式的存儲介質中。

上面結合附圖對本發(fā)明的實施例進行了描述，但是本發(fā)明并不局限于上述的具體實施方式，上述的具體實施方式僅僅是示意性的，而不是限制性的，本領域的普通技術人員在本發(fā)明的啟示下，在不脫離本發(fā)明宗旨和權利要求所保護的范圍情況下，還可做出很多形式，這些均屬于本發(fā)明的保護之內。