B樣條理論在無人機(jī)航跡平滑中的應(yīng)用

段彩萍 劉超 王琪

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

關(guān)鍵詞：無人機(jī)：航跡平滑；B樣條；蟻群算法

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

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

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

1 算法理論

1.1初始航跡算法

無人機(jī)的初始航跡獲取，采用了標(biāo)準(zhǔn)的蟻群算法。蟻群算法（Ant Colony Optimization，ACO）即螞蟻算法，是一種用來尋找最優(yōu)路徑的概率型算法。

螞蟻在覓食的時(shí)候會(huì)在路上留下一種叫信息素的化學(xué)物質(zhì)，并且螞蟻們能夠感知信息素的強(qiáng)度，根據(jù)強(qiáng)度的大小指引自己的前進(jìn)方向，它們總是朝著強(qiáng)度高的地方前進(jìn)。因此，整個(gè)蟻群的覓食過程就表現(xiàn)出對(duì)信息素強(qiáng)度的一種正反饋現(xiàn)象。

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

螞蟻從起點(diǎn)（xo，Yo）出發(fā)，到達(dá)下個(gè)路口的時(shí)，螞蟻根據(jù)一定的概率會(huì)選擇下一個(gè)坐標(biāo)點(diǎn)，其選擇概率公式是：

τij是邊（i，j）上的信息素；α是信息素重要程度的參數(shù)；nij是從坐標(biāo)點(diǎn)i到坐標(biāo)點(diǎn)/的啟發(fā)式因子；聲是啟發(fā)因子的重要程度的參數(shù)。

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

1.2航跡平滑-B樣條

1946年，由Schoenberg最早提出B樣條理論：1972年，Deboor和Cox分別獨(dú)立地給出關(guān)于B樣條計(jì)算的標(biāo)準(zhǔn)算法。B樣條有多種等價(jià)定義，但Deboor-Cox的遞推定義得出的遞推性質(zhì)使得計(jì)算非常簡(jiǎn)便穩(wěn)定，并且已經(jīng)得到了普遍的認(rèn)同。Deboor-Cox遞推公式是B樣條理論最重要的進(jìn)展之一。以下是遞推公式：

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

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

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

B樣條曲線具有諸多性質(zhì)，正因?yàn)橛羞@些性質(zhì)，才使得B樣條曲線得以廣泛的應(yīng)用[6]。

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

（2）規(guī)范性，∑，Ni，k（u）=1。

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

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

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

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

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

2 算法模型

2.1獲取初始航跡

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

（1）初始化。設(shè)定飛行環(huán)境，對(duì)飛行區(qū)域離散化處理。

（2）求出蟻群算法的相關(guān)參數(shù)。主要是信息素和啟發(fā)式因子。啟發(fā)式因子考慮到雷達(dá)、導(dǎo)彈、高炮和大氣的威脅之外，還考慮了油耗的威脅，權(quán)重比為： W=[0.4 0.2 0.2 0.1 0.1]。 第i個(gè)節(jié)點(diǎn)的啟發(fā)因子是所有代價(jià)加權(quán)和的倒數(shù)。信息素增加強(qiáng)度系數(shù)Q=10，信息素?fù)]發(fā)程度p=0.6。每一代的每一只螞蟻的爬行路線的長(zhǎng)度存儲(chǔ)在細(xì)胞結(jié)構(gòu)中。

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

（4）狀態(tài)更新和路線記錄。

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

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

2.2平滑處理

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

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

定義域節(jié)點(diǎn)區(qū)間長(zhǎng)度按下式計(jì)算：于是可得節(jié)點(diǎn)值：

當(dāng)?shù)贸隹刂乒?jié)點(diǎn)d，和節(jié)點(diǎn)矢量時(shí)，即可得出3次B樣條曲線。

3 仿真結(jié)果

3.1初始航跡

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

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

3.2平滑后的航跡

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

4 結(jié)語

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

[參考文獻(xiàn)]

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

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

[3]張雷，王道波，段海濱基于粒子群優(yōu)化算法的無人戰(zhàn)斗機(jī)路徑規(guī)劃方法[J].系統(tǒng)工程與電子技術(shù)，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ī)航跡規(guī)劃及其動(dòng)態(tài)仿真[J].指揮控制與仿真，2012 （1）：29-32

[6]董本志，張小燕，于嗚，等三次NURBS曲線的Obrechkoff參數(shù)化插值算法[J].機(jī)械科學(xué)與技術(shù)，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樣條曲線優(yōu)化在機(jī)器人軌跡規(guī)劃中應(yīng)用[D].武漢：武漢科技大學(xué)，2007.

[9]李猛.基于智能優(yōu)化與RRT算法的無人機(jī)任務(wù)規(guī)劃方法研究[D].南京：南京航空航天大學(xué)，2012.

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