【正文】 mesh(a,b,c),hold onplot3(tx,tyy,tzz,39。 end%plot3(tx,tyy,tzz,39。,39。)。p = polyfit(tx,ty,5)。 } t=time(0)* t。 }// while((pre_path[i].lon!=0)||(pre_path[i].lat!=0)||(pre_path[i].alt!=0)) while(pre_path[i].lon!=0) { PreThrDistance = puter_distance(pre_path[i].lon,pre_path[i].lat,pre_path[i].alt, thr_path[i].lon,thr_path[i].lat,thr_path[i].alt)。 pre_path[num1+i+j].alt = save[j].alt。 pre_path[num1+i].alt = rrtz[i]。 rrtz[m] = ztemp[knew]。 knew = j。 } //判斷該tempt點是否遇到威脅 for(int k=0。 ytemp[j]=rrty[kmin]+min_step*r*sin(det_anglex)。 det_anglex = anglex[j+1]anglex[j]。 } } //計算角度 if((xrand[j]rrtx[kmin])!=0) { float fz = yrand[j]rrty[kmin]。 float min_step,min_dis=10000。 yrand[j]=rrty[m]+r*con。 j10 。 r = sqrt((rj)/)。 } if(rrty[m] = endy) for(j=0 。 j++) { r=random()。 int kmin=0,knew。 float endz = pre_path[num+5].alt。 for(int a=0。 } if((fp2=fopen(,rb))==NULL) { printf(cannot open\n)。 dis=(x1x2)*(x1x2)+(y1y2)*(y1y2)+(z1z2)*(z1z2)。// printf(%f %f %f,pre_path[k].lon,pre_path[k].lat,pre_path[k].alt)。 } if((fp2=fopen(,rb))==NULL) { printf(cannot open\n)。 dis=(x1x2)*(x1x2)+(y1y2)*(y1y2)+(z1z2)*(z1z2)。 float lat。在畢業(yè)設(shè)計的過程中，學(xué)姐的幫助和提點使我在算法設(shè)計過程少走很多彎路。于是，我們提出了基于RRT的實時避障航跡規(guī)劃算法?？梢钥吹街匾?guī)劃航跡在避障后回到了預(yù)定航跡上。，我們得到了重規(guī)劃后的航路點。如果是強制結(jié)束，則返回從到距離最近的葉節(jié)點的路徑。對于超出無人機可達范圍內(nèi)的目標(biāo)點，則引入隨機變量,通過航向和步長計算。以探測到威脅時刻的那個航路點作為初始點，在RRT樹中將它作為根節(jié)點，采用一種特殊的增量方式構(gòu)造搜索空間，通過逐漸增加葉節(jié)點的方式生成隨機擴展樹。（3） RandomState函數(shù)。第三章 基于改進RRT的無人機實時避障路徑規(guī)劃3．1 RRT基本原理及其應(yīng)用快速擴展隨機樹（rapidlyexploring random tree，RRT）是一種基于采樣的單查詢隨機搜索算法，. LaValle 首次提出。對于未確定戰(zhàn)場環(huán)境，需要實時規(guī)劃或局部規(guī)劃，但目前存在的問題是計算時間過長而達不到及時規(guī)避威脅的要求或模型考慮過于簡單，導(dǎo)致實際飛行的威脅大于規(guī)劃的威脅，因此規(guī)劃出的航路實際不可飛。幾何方法。北京航空航天大學(xué)洪曄為解決算法時空開銷大、無人機航向改變頻繁的缺點，提出一種基于狀態(tài)聚類方法的HMDP模型，并將其拓展到三維規(guī)劃中。隨機勢力場依賴于一個好的啟發(fā)式函數(shù)，當(dāng)遇到威脅、運動學(xué)上的差異約束或是動力約束時，它就會變得困難起來。（6）無人機的最小步長。無人機在整個飛行過程中的飛行路程，受到飛機燃油和飛行時間配給的限制。威脅代價——威脅代價主要與其威脅范圍s相關(guān)，威脅代價可表示為。第三章 基于改進RRT的無人機實時避障路徑規(guī)劃：本章眾多研究RRT算法的原理特點，基于RRT的避障路徑規(guī)劃。由于不可能事先考慮到所有可能出現(xiàn)的情況，簡單的反射型避障無法滿足復(fù)雜多變的戰(zhàn)場環(huán)境，而基于推理的避障將使無人機在遭遇障礙時，能夠?qū)崟r的做出合理的決策，從而具有更高的智能性和更好的戰(zhàn)場適應(yīng)性。其二，數(shù)據(jù)鏈容易受干擾。對于非協(xié)作式機動障礙，無人機很難預(yù)測其飛行路線，不便做出合理的規(guī)避策略，甚至在復(fù)雜的環(huán)境中，還可能會引起多次沖突，對有人機而言，有人機傳感器配合人眼能很好的探測發(fā)現(xiàn)威脅并準(zhǔn)確感知。張躍東等人提出了基于單目視覺的無人機障礙探測算法。研究結(jié)果顯示，避障成功率達到96%。因此，可能導(dǎo)致需要強大的計算處理能力。當(dāng)出現(xiàn)未預(yù)料到的障礙時，很難改變已經(jīng)下指令機動的動作。（3） 感知功能。隨著無人機系統(tǒng)技術(shù)的不斷發(fā)展，眾多功能各異的無人機被廣泛應(yīng)用于各種軍事和非軍事行動中。仿真測試結(jié)果表明，算法簡單、快速、有效地規(guī)劃出了三維可行航跡。本文闡述了無人機三維航路規(guī)劃面臨的關(guān)鍵性問題及發(fā)展趨勢，針對非協(xié)助式的三維障礙規(guī)避問題，提出了一種基于快速隨機擴展樹（RRT）方法的三維路徑規(guī)劃算法。無人機飛行途中遇到的障礙可分為協(xié)作式障礙和非協(xié)作式障礙。系統(tǒng)監(jiān)視環(huán)境，并收集入侵者當(dāng)前合適的狀態(tài)信息，例如，飛機位置、速度、朝向等。考慮無人機平臺的性能，完成規(guī)避機動行為。防撞系統(tǒng)并不是試圖給出一條逃逸航線，相反，而是請求一個爬升或俯沖機動。Brandon采用圖像特征跟蹤方法研究小型無人機的避障問題。李耀軍等人基于多源信息融合對無人機感知與規(guī)避做了深入研究。國內(nèi)研究無人機感知與規(guī)避問題并沒有完全滿足無人機進入民用空域飛行的需求，關(guān)鍵在于無人機自主決策的智能性仍受到質(zhì)疑。然而，數(shù)據(jù)鏈傳輸存在兩個問題：其一，傳輸需要時間。針對無人機實現(xiàn)穩(wěn)定的編隊構(gòu)型、對無人機高度一致、速度一致、時間一致等問題，國內(nèi)外提出了很多一致性協(xié)議，并展開了大量理論研究工作。另外，盡管無人機能承受的機動過載較有人機大得多，但軌跡還必須考慮無人機的物理性能，以確保無人機能執(zhí)行所規(guī)劃機動動作。規(guī)劃出的飛行路徑應(yīng)該使無人機能避開威脅，同時盡量縮短航程，并滿足機動性能以及無人機的最大加速度、俯仰角、偏航角等約束要求。則航路代價為通過上述分析，得航路的目標(biāo)函數(shù)可表示為:無人機實際飛行過程中還需要考慮動力學(xué)約束，主要包括以下幾個方面：（1）無人機的最小飛行高度。無人機在飛行過程中在二維平面中的左右偏轉(zhuǎn)角度受其最大偏航角約束，具體可寫為：（5）無人機的最遠視距F。在這種情況下，無人機需要在飛行的過程中實時處理新出現(xiàn)的威脅，依靠實時探測到的環(huán)境及威脅信息，經(jīng)過重規(guī)劃來獲得當(dāng)前已知信息下的最優(yōu)飛行航跡。近年來，許多學(xué)者在三維航路規(guī)劃方面作了大量的工作，主要研究如下：海軍航空工程學(xué)院范洪達將三維航路分解為水平方向和垂直方向的兩個航路并分別應(yīng)用改進的遺傳算法和適應(yīng)角法進行規(guī)劃仿真。華中科技大學(xué)嚴(yán)江江針對三維航路規(guī)劃的實時性問題，不同于通常的最優(yōu)優(yōu)先算法，該方法使用可行優(yōu)先的準(zhǔn)則，有效地剪除了搜索空間，提高了搜索效率，從而使三維航路規(guī)劃能夠應(yīng)用于實時航路規(guī)劃中。隨著信息化戰(zhàn)爭進程的發(fā)展及科技的日益創(chuàng)新，防空體系日益完善，雷達及導(dǎo)彈性能更加優(yōu)越，事先很難完成探測和定位，因此，三維低空突防航路規(guī)劃及實時動態(tài)規(guī)劃具有很強的實戰(zhàn)意義，但無人機航路規(guī)劃尚存在以下問題：（1） 航路系統(tǒng)約束過于簡化。本文采用一種路徑規(guī)劃的隨機數(shù)據(jù)結(jié)構(gòu)以解決航跡的最優(yōu)性以及規(guī)劃的實時性問題。注意到新點一般是朝目標(biāo)點靠近的，如果算法在向新點行進的過程中遇到威脅，則返回空值NULL，表示擴展失敗。所以需要改進隨機點的選擇方式，引入航跡評價啟發(fā)信息，剪裁冗余節(jié)點，對航路進行平滑等，提高規(guī)劃的性能。為了將RRT算法用于無人機實時避障航跡規(guī)劃上，本文提出以下改進策略：（1）改進隨機點的選擇。其中，估計距離為到目標(biāo)點的歐式距離，如式(4)所示。從實驗結(jié)果看出：避碰的安全距離越大，算法擴展的節(jié)點數(shù)也越多，相應(yīng)的計算時間也越多；擴展步長很小的情況下，為了到達目標(biāo)點，所需要產(chǎn)生的節(jié)點數(shù)也很多，當(dāng)然規(guī)劃時間也越長?？梢钥吹綀D中重規(guī)劃得到的航跡與威脅球沒有接觸，也就是說該航跡是可行的。其中主要有隨機勢力場法和概率地圖法。在整個畢業(yè)設(shè)計的過程中，牛老師一直關(guān)心我畢業(yè)設(shè)計的完成情況，對于我如何安排時間以及需要哪些相關(guān)知識都予以了指導(dǎo)，為我提供了必要的幫助。彭輝，王林，沈林成[8]范洪達，葉文，馬向玲．面向低空突防的三維航路規(guī)劃仿真研究[J]．計算機仿真，2004，21(2)：3537．[9]李銳，劉占辰，荊獻勇．基于啟發(fā)式算法的無人機三維航跡規(guī)劃仿真研究[J]．電光與控制，2009，16(8)：2731．[10]洪曄，房建成．基于HMDP的無人機三維路徑規(guī)劃[J]．北京航空航天大學(xué)學(xué)報，2009，35(1)：100．103．[11]夏潔，高金源，余舟毅．基于禁忌搜索的啟發(fā)式任務(wù)路徑規(guī)劃算法[J]．控制與決策，2002，17(11)：773776．[12]陳謀，肖健，姜長生．基于改進蟻群算法的無人機三維航路規(guī)劃[J]．吉林大學(xué)學(xué)報(工學(xué))，2008，38(4)：99l9915．[13]鮑帆，姜長生．基于LPA+算法的無人機三維航跡快速規(guī)劃研究[J]．電光與控制，2008，15(12)：10．13．[14]陳冬，周德云，馮琦．基于粒子群優(yōu)化算法的無人機航跡規(guī)劃[J]．彈箭與制導(dǎo)學(xué)報，2007，27(4)：340 //設(shè)置隨機數(shù)種子，未調(diào)用srand函數(shù)時種子的值默認(rèn)為一，隨機數(shù)只產(chǎn)生一次，以后的都一樣 a=rand()%100。 qq = thr_path。pplon,amp。 } fclose(fp1)。 qq = thr_path。 float anglex[11]={0},anglez[11]={0}。 rrtz[0] = pre_path[num1].alt。 save[j].alt = pre_path[num+5+j].alt。 xrand[j]=rrtx[m]+r*con。 j++) { r=random()。 } if(rrtz[m] endz) for(j=0 。 r = sqrt((rj)/)。 j10 。 yrand[j]=rrty[m]2*r*con。 t++) { float min = puter_distance(xrand[j],yrand[j],zrand[j], rrtx[t],rrty[t],rrtz[t])。 anglez[j+1] = atan(xyDistance / (zrand[j]rrtz[kmin]))。 } else if(det_anglex ph) { xt