B樣條理論在無人機航跡平滑中的應用

段彩萍 劉超 王琪

摘要：航跡規劃是指根據飛行要求，設計出一條從起點到終點的最優航跡，航跡平滑是航跡規劃的最后階段，好的平滑效果可以保證所規劃的航跡具有實際可飛性，因此，對航跡平滑算法的研究對整個無人機的航跡規劃都具有很大的意義。文章針對無人機航跡規劃問題所需滿足的約束條件，建立相應的機動性能模型和威脅約束模型，平滑階段主要包括雷達、導彈、高炮等威脅模型。首先將規劃空間柵格化，其次通過標準蟻群算法獲得無人機的初始航跡，狀態轉移策略僅根據信息素及啟發因子按照蟻群算法概率公式進行選擇，最后根據航跡平滑中參數分布不均勻的特點，選擇非均勻三次B樣條處理初始航跡。整個算法在Matlab平臺上仿真實現，經過計算仿真，實驗證明非均勻三次B樣條曲線呈現出較好的平滑效果。

關鍵詞：無人機：航跡平滑；B樣條；蟻群算法

航跡規劃一般有3個階段，第一階段是根據無人機機型和載荷的不同分配不同的任務；第二階段是在不考慮各種約束條件的情況下，進行航跡規劃，得到總代價最小的航跡；第三階段需要將一些因素考慮進去，比如最小飛行高度、最大轉彎半徑等，使第二階段的航跡具有實際可飛性，第三階段中最重要的一步即航跡平滑。航跡平滑是使航跡滿足可飛要求，并且與威脅保持著安全距離。國內外文獻對航跡規劃的研究有很多，著重對航跡平滑的研究文獻相對而言不是特別豐富。

對無人機航跡平滑的研究比較多的是kalman濾波，最小二乘曲線擬合法，圓弧串聯法，平滑算子法等。經典的kalman濾波法需要假定目標運動狀態模型，對目標機動的適應性較差。最小二乘法能減少測量以及模型的先驗信息，同時降低初始值的敏感度，如果強機動目標階數比較低，此時用最小二乘法得到的擬合精度和效果都不是特別理想。文獻[1]闡述了一種基于模糊推理機制的平滑算法來調整航點的位置：文獻[2]闡述了一種基于時間開關的平滑算法，該方法僅對各航跡的轉折點做了平滑處理，適用于局部平滑。文獻[3]使用了RTS平滑算法，其平滑步驟有點滯后，擬合的實時性比較難以保證。

通過文獻分析，B樣條曲線法是目前比較實用的航跡平滑算法。B樣條理論應用范圍比較廣泛，例如用B樣條函數處理力學問題；在圖像領域，B樣條結合小波方法可以完成完整性的認證和壓縮實驗數據；在遙測領域，應用不等距B樣條提高遙測數據精度。在空間軌跡優化上，尤其在機器人軌跡規劃[4]中，B樣條的優越性得到充分的發揮，其既能進行局部優化又可以進行全局調整。本文應用非均勻B樣條曲線優化初始航跡，可以使得無人機飛行航跡平滑連續且滿足飛行的約束條件。

1 算法理論

1.1初始航跡算法

無人機的初始航跡獲取，采用了標準的蟻群算法。蟻群算法（Ant Colony Optimization，ACO）即螞蟻算法，是一種用來尋找最優路徑的概率型算法。

螞蟻在覓食的時候會在路上留下一種叫信息素的化學物質，并且螞蟻們能夠感知信息素的強度，根據強度的大小指引自己的前進方向，它們總是朝著強度高的地方前進。因此，整個蟻群的覓食過程就表現出對信息素強度的一種正反饋現象。

由此可知，螞蟻之間利用信息素通信，根據信息素的濃度選擇路徑，螞蟻具有記憶行為，一只螞蟻搜索過的路徑在下次搜索時候，將不再被選擇，并且以此建立禁忌表，一只螞蟻很難到達食物源，但是整個蟻群的搜索就會容易到達目的地。

螞蟻從起點（xo，Yo）出發，到達下個路口的時，螞蟻根據一定的概率會選擇下一個坐標點，其選擇概率公式是：

τij是邊（i，j）上的信息素；α是信息素重要程度的參數；nij是從坐標點i到坐標點/的啟發式因子；聲是啟發因子的重要程度的參數。

蟻群算法本身具有很多特點，以下4點是最主要的部分：（1）并行算法，每只螞蟻的搜索過程是獨立的，螞蟻之間僅通過信息素傳遞信息，所以蟻群算法可以看作一個分布式的多智能體系系統。（2）自組織性，即螞蟻覓食是一個從無序到有序的過程。（3）較強的魯棒性，蟻群算法的求解結果不依賴于初始路線的選擇，在搜索過程中不需要進行人工干預，而且參數少，設置簡單。（4）正反饋算法，正反饋特性是蟻群算法最重要的一個特征。蟻群算法的特點讓初始航跡的獲取變得簡單[5]。

1.2航跡平滑-B樣條

1946年，由Schoenberg最早提出B樣條理論：1972年，Deboor和Cox分別獨立地給出關于B樣條計算的標準算法。B樣條有多種等價定義，但Deboor-Cox的遞推定義得出的遞推性質使得計算非常簡便穩定，并且已經得到了普遍的認同。Deboor-Cox遞推公式是B樣條理論最重要的進展之一。以下是遞推公式：

Ni，k（u）的第一個下標i表示序號，第二下標k表示B樣條次數。從遞推公式可得，欲確定第i個k次B樣條Ni，k（u），需要用到ui，ui+1，…，ui+k+1共k+2個節點。方程中n+l個控制頂點di（i=0，l，…，n）需要用到n+l個k次B樣條基函數Ni，k（u）（i=0，1，…，n）?？疾霣樣條曲線定義在區間u∈[uiui+1]上的曲線，忽略基函數取零項，B樣條曲線可表示為：

由式（3）可知，B樣條曲線的定義仍然采用控制頂點來定義，且與其他頂點無關，這樣不僅克服了貝塞爾曲線不具有局部性質的特點，又保留了它的優良控制的特性。

B樣條根據不同的分類標準可以分為多種類型。根據節點的分布情況可劃分成4種類型，分別是：均勻B樣條曲線、準均勻B樣條曲線、一般非均勻B樣條曲線、分段貝齊爾曲線。均勻、準均勻和非均勻3種類型早就存在，STEP標準把分段貝齊爾曲線作為一種特殊的類型[1]。還可按照首末端點是否分離和相重劃分為開曲線和閉曲線。

B樣條曲線具有諸多性質，正因為有這些性質，才使得B樣條曲線得以廣泛的應用[6]。

（1）遞推性，遞推性可由遞推公式表明。

（2）規范性，∑，Ni，k（u）=1。

（3）局部支撐性質：其他從公式還可以看出它包含了非負性。

（4）比貝塞爾曲線具有更強的凸包性；B樣條曲線的凸包是多個曲線段凸包的并集，它的凸包區域小于或者等于同一組控制頂點定義的貝塞爾曲線的凸包區域，B樣條曲線恒位于其凸包之內。凸包性導致順序k+1個頂點重合時，由這些頂點定義的k次B樣條曲線退化到這一個重合點：順序k+1個頂點共線時，由這些頂點定義的k次B樣條曲線退化為一直線段[1]。

（5）幾何不變性與仿射不變性。由于實際得到的航跡節點通常是非等間距的，因此采用非均勻的B樣條曲線進行計算。非均勻B樣條基的節點矢量定義是任意分布的節點矢量U=[uo，u1…，un+k+1]，只要在數學上成立（其中節點序列非遞減，兩端節點重復度≤k+1，內節點重復度≤k）都可取。

考慮到計算的復雜度，一般選擇二次或三次B樣條曲線，只要曲線曲率大于等于無人機最小轉彎半徑即可。曲線曲率的計算公式如下：

曲線曲率半徑，？，只要通過B樣條算法平滑后的航跡的曲率滿足r≥rmin（無人機的最小轉彎半徑），就認為滿足無人機的飛行約束條件。

2 算法模型

2.1獲取初始航跡

首先要確定無人機的飛行空間，其次是需要將飛行空間柵格化，即離散處理。因此此研究的對象是在二維空間進行的航跡規劃，可用』軸表示水平方向，y軸表示垂直方向，在』軸和y軸方向都進行25等分，交點（xi，yi）即為無人機的航跡坐標點，本次飛行考慮的威脅因素有雷達威脅，高炮威脅，大氣威脅以及導彈威脅。本次仿真假設無人機起點坐標為（x0，y0），飛行的目的地坐標是（Xn，yn）。應用蟻群算法規劃處一條從起飛點到終點的路線，對初始航跡要求是避開所有的危險因素，同時要盡可能地縮短起點到終點的距離。算法步驟如下[7-8]。

（1）初始化。設定飛行環境，對飛行區域離散化處理。

（2）求出蟻群算法的相關參數。主要是信息素和啟發式因子。啟發式因子考慮到雷達、導彈、高炮和大氣的威脅之外，還考慮了油耗的威脅，權重比為： W=[0.4 0.2 0.2 0.1 0.1]。 第i個節點的啟發因子是所有代價加權和的倒數。信息素增加強度系數Q=10，信息素揮發程度p=0.6。每一代的每一只螞蟻的爬行路線的長度存儲在細胞結構中。

（3）迭代循環。迭代開始的初始狀態假定無人機已經在起點處，那么禁忌表應當剔除起點坐標。螞蟻能看到的范圍可以簡化成一個方格，速度半徑是其中的一個重要參數，假設速度半徑為3，那么整個觀察的范圍即為3X3個方格，螞蟻的移動距離也在這個范圍之內。但是起點位置能選擇的下個坐標點只有3個。采用轉輪賭法選擇下一步到底選擇哪個坐標點。循環的停止條件是螞蟻沒有遇到食物或者走進了死循環。

（4）狀態更新和路線記錄。

（5）信息素更新。信息素更新主要是信息素濃度的增強和揮發。迭代次數N每累加一次，所有路徑上的信息素就揮發一次，揮發系數設為p，公式如下： τij（N+1）=ρτij（N）+（1-ρ）△τij，ρ∈（0，1） （5）

此外，算法采用擇優法，即總是保留上一代最佳的航跡作為這一代蟻群算法航跡中的一條航跡，這樣有利于收斂性。

2.2平滑處理

B樣條曲線定義的三要素分別是：控制頂點、B樣條次數K，以及確定節點矢量，即己知這三點，就確定了一條k次B樣條曲線。而非均勻B樣條曲線的定義與其他B樣條曲線有所不同，除了要己知控制節點di（i=0，1，…，n），還需確定它的節點矢量U= [u0，u1，…，Un+k+l]中具體的節點值。算法的實現框如圖1所示。

對于插值曲線，相比均勻參數化，弦長參數化方法更加合理些。弦長參數化使得順序的兩個數據的參數值和它的距離成正比例關系。同時還需要參數規范化，規范到[0，1]區間，其目的是方便計算規范后的結果對插值沒有影響。求控制節點采用逆推法。 確定節點矢量有兩種方法，分別是里森菲爾德方法和哈德利一賈德法。由于里森菲爾德法需要考慮樣條次數的奇偶性，而哈德利一賈德法采用統一的計算公式，構成k條邊的k+1個頂點相應的B樣條曲線段，且不考慮其他頂點對該曲線段沒有影響，所以后者更加合理[9-10]。

定義域節點區間長度按下式計算：于是可得節點值：

當得出控制節點d，和節點矢量時，即可得出3次B樣條曲線。

3 仿真結果

3.1初始航跡

本文在4G內存的PC機上，使用Matlab7.o進行蟻群算法獲得無人機初始航跡的仿真。無人機的起點坐標是（10，20），終點航跡坐標是（55，34），航跡坐標均代表距離單位，在仿真中都無量綱。Matlab軟件運行蟻群算法程序得出的初始航跡如圖2所示。

圖2中雷達的坐標點為（26，55）、（52，45）、（35，26）、（51，31），導彈的坐標點為（17，22）、（24，35），（30，62），（40，38），高炮和大氣的威脅分別有兩個和3個。從運行的結果可以看出，初始航跡有幾處并不光滑，實際飛行中，尖角處的曲線曲率小于最小轉彎半徑，因此無人機不能進行尖角處的轉彎，該初始航跡不符合無人機實際飛行需求，需要對以上結果進行平滑處理。

3.2平滑后的航跡

經過3次非均勻B樣條處理前后的航跡對比如圖3所示。 從仿真結果可看出，平滑后的航跡比較光滑，沒有尖角，符合無人機飛行的條件，滿足曲率半徑大于等于無人機最小轉彎半徑的要求。

4 結語

本文針對的是無人機二維航跡平滑問題，實驗結果證明，3次非均勻B樣條曲線插補的航跡平滑方法使得航跡過渡自然，航路光滑沒有尖角，航向沒有突變性，航跡的轉彎半徑基本大于無人機所允許的最小的轉彎半徑。同時還保證了曲線的保凸性，平滑后的航跡經過所有的航點，且逼近原始的航跡曲線。本文所研究的方法獲得了較好的航跡平滑效果，保證了所規劃出的無人機航跡的真實可飛性。 本文的航跡規劃模型是二維平面的航跡規劃，威脅不多且分布的范圍不是很廣泛，且是將威脅因素都抽象為一個點，因此不需要用到高次B樣條曲線。但實際的飛行環境是很復雜的，因此后續的工作中將考慮擴大威脅范圍，飛行環境更加復雜化，并將算法應用到三維空間。

[參考文獻]

[1]席慶彪，蘇鵬，劉慧霞基于A*算法的無人機航路規劃算法[J].火力與指揮控制，2013 （11）：5-9

[2]李林峰，馬蕾三次均勻B樣條在工業機器人軌跡規劃中的應用研究[J].科學技術與工程，2013 （13）：3621-3625，3646

[3]張雷，王道波，段海濱基于粒子群優化算法的無人戰斗機路徑規劃方法[J].系統工程與電子技術，2008 （3）：506-510

[4]TAYLOR R H.Planning and execution of straight line manipulator trajectories[J].Ibm Journal of Research & Development， 1979 （4）：424-436.

[5]王緒芝，姚敏，趙敏，等基于蟻群算法的無人機航跡規劃及其動態仿真[J].指揮控制與仿真，2012 （1）：29-32

[6]董本志，張小燕，于嗚，等三次NURBS曲線的Obrechkoff參數化插值算法[J].機械科學與技術，2016 （11）：1721-1726.

[7]THORSTEN G， DOMINIK H.Path planning and execution in fast-changing envlronmentsVVith known and unknown obstacles[C].SanDiego： Conference on Intelligent Robots and Systems， 2007

[8]陳丹.基于遺傳算法-B樣條曲線優化在機器人軌跡規劃中應用[D].武漢：武漢科技大學，2007.

[9]李猛.基于智能優化與RRT算法的無人機任務規劃方法研究[D].南京：南京航空航天大學，2012.

[10]陳金廣，高新波.基于分段RTS平滑的凸組合航跡融合算法[J].計算機科學，2010 （4）：175-178.