畢業(yè)論文-旋翼式飛行機(jī)器人的飛行路徑規(guī)劃研究

HUNANUNIVERSITY畢業(yè)論文論文題目旋翼式飛行機(jī)器人的飛行路徑規(guī)劃研究學(xué)生姓名學(xué)生學(xué)號(hào)專業(yè)班級(jí)自動(dòng)1101班學(xué)院名稱電氣與信息工程學(xué)院指導(dǎo)老師學(xué)院院長(zhǎng)2015年5月26日湖南大學(xué)畢業(yè)設(shè)計(jì)(論文)湖南大學(xué)畢業(yè)設(shè)計(jì)(論文)第頁(yè)基于改進(jìn)蟻群算法的路徑規(guī)劃引言在20世紀(jì)90年代時(shí)，意大利學(xué)者M(jìn).Dorigo提出了一種新的群集智能算法，就是蟻群算法[32]。蟻群算法是模擬螞蟻這一類喜歡群居的昆蟲，雖然個(gè)體不能發(fā)揮很大的作用，但是蟻群群體卻能表現(xiàn)出個(gè)體所不能完成的行為。比如螞蟻在覓食的過(guò)程中總能找到畢竟最短的那條路線，原因是在螞蟻尋找食物時(shí)，會(huì)在經(jīng)過(guò)的路上釋放信息素，其它的螞蟻也能夠感知這種信息素，通過(guò)信息素的濃度從而達(dá)到正反饋的效應(yīng)，選擇最優(yōu)的路徑。蟻群算法應(yīng)用于車輛路徑、網(wǎng)絡(luò)路由、TSP問(wèn)題以及作業(yè)調(diào)度等問(wèn)題并取得了很好的效果。環(huán)境建模因要求旋翼式飛行機(jī)器人在室外進(jìn)行作業(yè)，故先建立合適其飛行的三維空間環(huán)境模型，采用蟻群算法進(jìn)行初步的路徑規(guī)劃?？紤]旋翼式飛行機(jī)器人在隨機(jī)產(chǎn)生地表和固定產(chǎn)生山峰相結(jié)合的三維地圖上運(yùn)動(dòng)，然后在此地圖上給出路徑規(guī)劃的目標(biāo)函數(shù)。三維空間環(huán)境的建模三維路徑規(guī)劃算法首先要有一個(gè)從三維地圖中抽象出的三維空間環(huán)境的模型[33]。本文采用的蟻群算法基于柵格法，故需要將三維空間環(huán)境抽象為柵格。圖3.1三維空間環(huán)境建模根據(jù)圖3.1，將三維地圖左下角的頂點(diǎn)A作為原點(diǎn)建立三維坐標(biāo)系，AD方向?yàn)閤軸方向也是經(jīng)度增加的方向，AB方向?yàn)閥軸方向也是緯度增加的方向，AE方向?yàn)閦軸方向也是海拔高度增加的方向。其中A點(diǎn)的海拔高度為0m。AD、AB、AE也是x軸、y軸、z軸方向的最大長(zhǎng)度。三維空間環(huán)境建立后，采用將等分成平面，再將等分平面再劃分的方式將三維空間環(huán)境抽象成一個(gè)個(gè)的網(wǎng)格點(diǎn)。沿著x軸方向?qū)⒌貓D中的AB邊等分為m個(gè)平面后，每個(gè)平面沿y軸方向m等分，沿z軸方向n等分，并求出這些等分線的交點(diǎn)，使得整個(gè)空間離散化一個(gè)三維點(diǎn)集合，如圖3.2所示。圖3.2等分平面的再劃分環(huán)境描述本文的蟻群算法在跨度為65km×65km的山地中搜索一條從起點(diǎn)到終點(diǎn)、能夠避開所有障礙物的最優(yōu)路徑。其中x軸和y軸方向的每個(gè)節(jié)點(diǎn)距離為1km，z軸方向每個(gè)節(jié)點(diǎn)距離為0.5km。該地圖可以設(shè)置山峰的個(gè)數(shù)、高度以及坡度，可以隨機(jī)產(chǎn)生地表的起伏度。原點(diǎn)A處的海拔為0m，路徑的起點(diǎn)和終點(diǎn)也可以自行設(shè)置。目標(biāo)函數(shù)尋找出旋翼式飛行機(jī)器人從起點(diǎn)到終點(diǎn)一系列點(diǎn)的集合，組成一系列直線后光滑成一條曲線，這個(gè)路徑不僅要求能夠安全避開障礙物，而且還要是最短的，目標(biāo)函數(shù)可以表示為式(3-1)：(3-1)式中，為最短路徑，為點(diǎn)的個(gè)數(shù)，為點(diǎn)的坐標(biāo)?；鞠伻核惴ㄏ伻核惴?，是用于在圖中找到一個(gè)概率優(yōu)化路徑的算法。被MarcoDorigo早1992年時(shí)第一次提出，這個(gè)算法的靈感來(lái)自于在尋找食物的時(shí)候，螞蟻會(huì)選擇最優(yōu)路徑?；鞠伻核惴ǖ脑韴D3.3中t=0時(shí)刻有100只經(jīng)過(guò)B、E點(diǎn)需要進(jìn)行路徑選擇，初始情況沒(méi)有信息素，所以螞蟻以相同的概率選擇路徑，即B點(diǎn)處各有50只螞蟻選擇BC、BD路徑，E點(diǎn)處各有50只螞蟻選擇EC、ED路徑，并釋放信息素。假設(shè)所有螞蟻都以相同的速度前進(jìn)(2cm/s)并且釋放等量的信息素，1s后選擇BC、EC的螞蟻分別到達(dá)E、B點(diǎn)，而BD、ED路徑上的螞蟻僅有BC、EC上的一半，因此BCE比BDE上的信息素更濃，最終螞蟻會(huì)選擇路徑ABCEF。圖3.3螞蟻在有障礙的情況下選擇路徑基本蟻群算法路徑搜索方法螞蟻會(huì)根據(jù)信息素的濃度選擇運(yùn)動(dòng)的位置，以螞蟻現(xiàn)在所在的位置為中心，螞蟻轉(zhuǎn)移公式如式(3-2)(3-2)式中，為按概率確定的下一到達(dá)位置。螞蟻從當(dāng)前節(jié)點(diǎn)轉(zhuǎn)換到下一可到達(dá)結(jié)點(diǎn)的概率[34]如(3-3)所示(3-3)式中，表示螞蟻下一步可以選擇的節(jié)點(diǎn)；表示螞蟻在第次搜索時(shí)和之間殘留的信息素強(qiáng)度；為信息素啟發(fā)因子；表示在次搜索時(shí)節(jié)點(diǎn)相對(duì)于節(jié)點(diǎn)的可見性,是一種期望啟發(fā)函數(shù)；是期望啟發(fā)因子，是螞蟻下一步可以選擇到達(dá)的結(jié)點(diǎn)。期望啟發(fā)函數(shù)定義為節(jié)點(diǎn)和之間的距離的倒數(shù)，如(3-4)所示(3-4)螞蟻經(jīng)過(guò)的路徑會(huì)留下信息素，信息素隨著時(shí)間的流逝而之間揮發(fā)。而信息素更新方法分為實(shí)時(shí)的和路徑信息素更新。實(shí)時(shí)的信息素更新是當(dāng)每只螞蟻選擇完節(jié)點(diǎn)后要對(duì)該節(jié)點(diǎn)的信息素進(jìn)行更新，如式(3-5)所示(3-5)式中，為信息素?fù)]發(fā)因子且為常數(shù)，和分別是節(jié)點(diǎn)和節(jié)點(diǎn)之間的路徑更新前和更新后的信息素強(qiáng)度，為該點(diǎn)的信息素初始值。當(dāng)所有螞蟻完成從起點(diǎn)到終點(diǎn)的路徑后，選擇長(zhǎng)度最短的路徑，對(duì)這條路徑上每個(gè)點(diǎn)的信息素進(jìn)行更新，時(shí)刻節(jié)點(diǎn)和按式(3-6)進(jìn)行信息素的更新。(3-6)式中，是和路徑之間增強(qiáng)的信息素，為第只螞蟻在本次循環(huán)中留在和路徑之間增強(qiáng)的信息素。MarcoDorigo提出了三種的計(jì)算模型[32]，(1)Ant-Cycle模型(3-7)(2)Ant-Density模型(3-8)Ant-Quantity模型 (3-9)式中，為信息素濃度的強(qiáng)度，表示第只螞蟻在本次迭代中走過(guò)的路徑長(zhǎng)度，表示第只螞蟻在本次迭代中從起點(diǎn)到終點(diǎn)走過(guò)的路徑長(zhǎng)度。根據(jù)三維環(huán)境改進(jìn)后的蟻群算法信息素更新的改進(jìn)對(duì)于信息素的使用，在基本的蟻群算法中，信息素的存儲(chǔ)載體一般是使用兩個(gè)相鄰節(jié)點(diǎn)中間的路徑段，但這種表示方法不能用在解決地圖比較大時(shí)的問(wèn)題。在三維情況的飛行路徑規(guī)劃問(wèn)題中，把整個(gè)空間離散成為一系列的點(diǎn)，這些點(diǎn)相對(duì)于二維情況下明顯增多。如果信息素的的載體為各個(gè)離散點(diǎn)間的連接路徑，那么沒(méi)有是辦法承受這樣算法復(fù)雜度。因此，要把信息素存儲(chǔ)在模型的各個(gè)離散的點(diǎn)中，每個(gè)離散點(diǎn)有一個(gè)信息素的值，該值表示對(duì)螞蟻的吸引度。此時(shí)信息素的更新仍然分為實(shí)時(shí)的信息素更新和路徑信息素更新，信息素的實(shí)時(shí)更新是指螞蟻經(jīng)過(guò)某個(gè)點(diǎn)的時(shí)候，這個(gè)點(diǎn)的信息素會(huì)有所減少，這樣可以增加螞蟻經(jīng)過(guò)其它點(diǎn)的概率，可以進(jìn)行全局搜索。信息素的更新公式如式(3-10)所示(3-10)式中，和是點(diǎn)更新前和更新后的信息素強(qiáng)度；是信息素強(qiáng)度的衰減系數(shù)。路徑信息素也就是全局信息素的更新，是指所有螞蟻完成一條路徑的搜索后，從所有的路徑集合中找出最短的路徑集合，增加這條最短路徑上所有點(diǎn)的信息素強(qiáng)度，更新如式(3-11)所示(3-11)式中，是信息素?fù)]發(fā)因子且為常數(shù)；為系數(shù)；為第只螞蟻?zhàn)哌^(guò)的路徑總長(zhǎng)度。啟發(fā)函數(shù)的改進(jìn)螞蟻在三維環(huán)境中從當(dāng)前點(diǎn)到下一個(gè)點(diǎn)時(shí)，啟發(fā)函數(shù)可以計(jì)算出選擇某個(gè)點(diǎn)的概率，啟發(fā)函數(shù)如式(3-12)所示(3-12)式中，是當(dāng)前點(diǎn)到下一個(gè)點(diǎn)間的路徑長(zhǎng)度，使得螞蟻選擇路徑較短的點(diǎn)；是安全值，當(dāng)路徑不可達(dá)時(shí)，該值為0，否則為1，使得螞蟻不會(huì)選擇不可達(dá)的障礙點(diǎn)；是下一個(gè)點(diǎn)到目標(biāo)點(diǎn)的路徑長(zhǎng)度，使得螞蟻選擇距離目標(biāo)路徑較短的點(diǎn)?？梢宰孕性O(shè)置這三個(gè)參數(shù)的重要程度。的計(jì)算如式(3-13)所示(3-13)式中，為當(dāng)前點(diǎn)，為下一個(gè)點(diǎn)。的計(jì)算如式(3-14)所示(3-14)式中，表示此點(diǎn)可看到的點(diǎn)的數(shù)量，表示這個(gè)點(diǎn)中不可達(dá)到的點(diǎn)的數(shù)量。的計(jì)算如式(3-15)所示(3-15)式中，為下一個(gè)點(diǎn)，為目標(biāo)點(diǎn)。其它部分的改進(jìn)適應(yīng)度函數(shù)值為長(zhǎng)度加高度，用來(lái)計(jì)算路徑是否有是否有所優(yōu)化，計(jì)算如式(3-16)所示(3-16)式中，用于記錄規(guī)劃的路徑，為螞蟻個(gè)數(shù)，中奇數(shù)和偶數(shù)分別記錄了路徑的軸坐標(biāo)和縱坐標(biāo)。 螞蟻的轉(zhuǎn)移概率式(3-17)所示(3-17)式中，為點(diǎn)當(dāng)前時(shí)刻點(diǎn)的信息素的值?；谙伻核惴ǖ穆窂揭?guī)劃實(shí)現(xiàn)及仿真蟻群路徑規(guī)劃算法的實(shí)現(xiàn)蟻群路徑規(guī)劃算法實(shí)現(xiàn)的流程圖如3.4所示。圖3.4蟻群路徑規(guī)劃算法流程圖如圖所示，首先對(duì)三維環(huán)境進(jìn)行離散化建模，再初始化蟻群算法的各個(gè)參數(shù)，根據(jù)啟發(fā)信息以及轉(zhuǎn)移概率選擇下個(gè)點(diǎn)的位置，更新局部信息素。如果到達(dá)目標(biāo)位置就計(jì)算路徑長(zhǎng)度并更新全局信息素，否則繼續(xù)尋找下個(gè)點(diǎn)的位置。當(dāng)滿足結(jié)束條件時(shí)，找到的即為最優(yōu)路徑。蟻群路徑規(guī)劃算法的仿真為了證明改進(jìn)后的蟻群算法具有更好的效果，分別使用基本蟻群算法和優(yōu)化蟻群算法在三維環(huán)境飛行空間下進(jìn)行了仿真。其中地圖如圖3.5所示，由6個(gè)山峰和隨機(jī)地表組成，坐標(biāo)為(10,10),(40,25),(35,50),(15,55),(20,20),(35,8)，起點(diǎn)為(1,40,10)，終點(diǎn)為(64,2,4)。圖3.5旋翼式飛行機(jī)器人飛行空間表3.1蟻群算法參數(shù)設(shè)置參數(shù)數(shù)值種群規(guī)模40迭代次數(shù)400信息素衰減率0.9信息素啟發(fā)因子0.5期望啟發(fā)因子2根據(jù)表3.1的參數(shù)進(jìn)行設(shè)置，分別使用基本蟻群算法以及改進(jìn)后的蟻群算法進(jìn)行路徑規(guī)劃仿真，路徑規(guī)劃結(jié)果和最佳個(gè)體適應(yīng)度變化趨勢(shì)如圖3.6和圖3.7所示，兩種算法結(jié)果的俯視圖如圖3.8所示。 圖3.6基本蟻群算法路徑規(guī)劃及適應(yīng)度變化趨勢(shì)圖3.7優(yōu)化蟻群算法路徑規(guī)劃及適應(yīng)度變化趨勢(shì)圖3.8基本蟻群算法和優(yōu)化蟻群算法路徑規(guī)劃的俯視圖表3.2兩種蟻群算法的仿真結(jié)果路徑長(zhǎng)度適應(yīng)度值基本蟻群算法236506.9335優(yōu)化蟻群算法177435.7887可以明顯看出改進(jìn)后的蟻群算法在同樣迭代次數(shù)下路徑比基本蟻群算法更加優(yōu)化且適應(yīng)度值更低。但由于在使用蟻群算法進(jìn)行規(guī)劃時(shí)，是采用離散化的方式進(jìn)行建模，故路徑不具有很好的平滑性和可跟蹤性，需要采用人工勢(shì)場(chǎng)法進(jìn)行平滑，具體方法將在第四章和第五章中提到。本章小結(jié)本章介紹了蟻群算法的發(fā)展背景和其基本理論，之后介紹了對(duì)三維環(huán)境空間抽象后的建模。針對(duì)三維環(huán)境，在基本蟻群算法上進(jìn)行了改進(jìn)，并進(jìn)行了仿真和對(duì)比，為后面兩種算法的結(jié)合奠定了基礎(chǔ)?；谌斯?shì)場(chǎng)法的路徑規(guī)劃引言人工勢(shì)場(chǎng)法是一種非常常用的方路徑規(guī)劃方法，該方法的優(yōu)點(diǎn)是計(jì)算量比較小且結(jié)構(gòu)簡(jiǎn)單，實(shí)時(shí)性也很好，可在實(shí)時(shí)避障和平滑軌跡等方面進(jìn)行應(yīng)用。但是在人工勢(shì)場(chǎng)法的缺點(diǎn)是溶于在局部最優(yōu)解上產(chǎn)生死鎖現(xiàn)象現(xiàn)象，即使得陷入局部最優(yōu)。由于該方法簡(jiǎn)單易用，已被大量使用在路徑規(guī)劃上，但由于人工勢(shì)場(chǎng)法的容易陷入局部最優(yōu)的缺點(diǎn)，所以往往適合另外的方法相結(jié)合使用。若僅僅使用蟻群算法得到的路徑不具有很好的連續(xù)性和可跟蹤性，故需要使用的人工勢(shì)場(chǎng)法進(jìn)行路徑平滑，本章中提出了一種在三維環(huán)境下使路徑平滑的人工勢(shì)場(chǎng)法?；救斯?shì)場(chǎng)法的路徑規(guī)劃人工勢(shì)場(chǎng)法的原理1986年Khatib提出了人工勢(shì)場(chǎng)法，主要就是構(gòu)造一個(gè)目標(biāo)產(chǎn)生的引力場(chǎng)和已知的障礙物產(chǎn)生的斥力場(chǎng)，引力場(chǎng)的矢量指向目標(biāo)位置的方向，斥力場(chǎng)的矢量朝著背離障礙物的方向，兩者共同作用的一個(gè)人工勢(shì)場(chǎng)[35]。通過(guò)勢(shì)場(chǎng)產(chǎn)生的力計(jì)算出當(dāng)前點(diǎn)下一步的路徑方向。人工勢(shì)場(chǎng)法的受力圖如4.1所示。圖4.1人工勢(shì)場(chǎng)法受力圖產(chǎn)生的人工勢(shì)場(chǎng)中，斥力場(chǎng)隨著和障礙物距離的增加從而減少，引力場(chǎng)隨著和目標(biāo)距離的增加和增加，產(chǎn)生的勢(shì)能的總和是兩個(gè)場(chǎng)的疊加?？刹捎锰荻葎?shì)場(chǎng)法進(jìn)行函數(shù)的表示，就是取沿著勢(shì)函數(shù)梯度下降方向。在這個(gè)人工勢(shì)場(chǎng)中，旋翼式飛行機(jī)器人受到的引力和斥力的合力這種虛擬力。目標(biāo)對(duì)旋翼式飛行機(jī)器人產(chǎn)生引力，障礙物則對(duì)旋翼式飛行機(jī)器人產(chǎn)生斥力，產(chǎn)生的合力為，使得旋翼式飛行機(jī)器人朝著目標(biāo)運(yùn)動(dòng)。具體示意圖如圖4.2所示。圖4.2人工勢(shì)場(chǎng)法中單點(diǎn)受力圖傳統(tǒng)的人工勢(shì)場(chǎng)法算法傳統(tǒng)的人工勢(shì)場(chǎng)法都是在二維的運(yùn)動(dòng)空間[36]下進(jìn)行研究的，但人工勢(shì)場(chǎng)法也可以用在三維的運(yùn)動(dòng)空間[37]中。若物體的位置用表示，該物體的受到的總勢(shì)場(chǎng)強(qiáng)度可由表示。目標(biāo)的位置用進(jìn)行表示，對(duì)該物體產(chǎn)生的引力勢(shì)場(chǎng)可用表示；障礙物的位置用進(jìn)行表示，對(duì)該物體產(chǎn)生的斥力勢(shì)場(chǎng)可用表示。在位置的總勢(shì)場(chǎng)強(qiáng)度如式(4-1)所示，其微分結(jié)果為產(chǎn)生的合力為式(4-2)(4-1)(4-2)式中，是產(chǎn)生的勢(shì)場(chǎng)在位置的梯度，梯度的方向是朝著位置處于勢(shì)場(chǎng)變化率最大的方向。對(duì)于三維空間中的任何點(diǎn)，如(4-3)所示(4-3)引力勢(shì)場(chǎng)的強(qiáng)度和斥力勢(shì)場(chǎng)的強(qiáng)度可分別用式(4-4)和(4-5)表示(4-4)(4-5)式中，和是相應(yīng)的正比例位置增益系數(shù)，是該物體與目標(biāo)點(diǎn)間的距離，是該物體與障礙物的最短距離，是障礙物的影響的距離，這個(gè)值可以自行設(shè)定，只有物體距離障礙物一定距離內(nèi)斥力場(chǎng)才有作用。引力勢(shì)場(chǎng)產(chǎn)生的引力和斥力勢(shì)場(chǎng)產(chǎn)生的斥力分別為相應(yīng)勢(shì)場(chǎng)的負(fù)梯度，如(4-6)和(4-7)所示(4-6)(4-7)受到的合力為式(4-8)所示(4-8)合力決定了該物體的運(yùn)動(dòng)方向。人工勢(shì)場(chǎng)法中物體的運(yùn)動(dòng)式中受到這個(gè)合力的作用，從而在避開障礙的情況下進(jìn)行到從起始點(diǎn)到目標(biāo)點(diǎn)的路徑規(guī)劃。傳統(tǒng)人工勢(shì)場(chǎng)法存在的問(wèn)題雖然人工勢(shì)場(chǎng)法在進(jìn)行路徑規(guī)劃時(shí)有著美觀、簡(jiǎn)便等優(yōu)點(diǎn)，但存在著一些問(wèn)題：在障礙物附近時(shí)可能呈不穩(wěn)定前進(jìn)；目標(biāo)在障礙物附近收到的斥力過(guò)大，導(dǎo)致目標(biāo)不可達(dá)；在某點(diǎn)時(shí)達(dá)到受力平衡狀態(tài)，則陷入局部最優(yōu)解。三維環(huán)境下使路徑平滑的人工勢(shì)場(chǎng)法三維環(huán)境下障礙物點(diǎn)的提取方法本文中利用的算法由于是將一個(gè)個(gè)的障礙點(diǎn)作為產(chǎn)生斥力場(chǎng)的源，故在面對(duì)連續(xù)的障礙物表面時(shí)需要提取一系列的障礙點(diǎn)，提取的方法如圖4.3所示。由于在生成三維空間地圖時(shí)的主要方法是由一個(gè)個(gè)點(diǎn)形成的連續(xù)面，故可將這些點(diǎn)作為障礙點(diǎn)。圖4.3取障礙點(diǎn)示意圖改進(jìn)的人工勢(shì)場(chǎng)法如果單獨(dú)利用蟻群算法得到的是一系列點(diǎn)和各點(diǎn)間相連的線組成的路徑，路徑的一階導(dǎo)數(shù)在結(jié)點(diǎn)處均不連續(xù)。對(duì)于旋翼式飛行機(jī)器人來(lái)說(shuō)，沿著這樣的路徑飛行可跟蹤性較差且仍可能會(huì)在某兩點(diǎn)之間直線運(yùn)動(dòng)時(shí)產(chǎn)生碰撞。若采用人工勢(shì)場(chǎng)法平滑路徑，算法簡(jiǎn)便且計(jì)算量較小，考慮到了障礙物對(duì)路徑的影響，被廣泛應(yīng)用?？紤]到傳統(tǒng)的人工勢(shì)場(chǎng)法出現(xiàn)的一些問(wèn)題，對(duì)斥力函數(shù)進(jìn)行了改進(jìn)。除此之外，由于平滑算法時(shí)增設(shè)許多目標(biāo)點(diǎn)，故能克服傳統(tǒng)的人工勢(shì)場(chǎng)法中出現(xiàn)的陷入局部最優(yōu)解的確定。若使用蟻群算法后得到的路徑為，其中為起點(diǎn)，為終點(diǎn)。本文采用如下方法進(jìn)行路徑平滑處理，例如當(dāng)處于結(jié)點(diǎn)時(shí)將下一結(jié)點(diǎn)，作為引力點(diǎn)，三維地圖模型的各個(gè)高度點(diǎn)作為斥力點(diǎn)，利用人工勢(shì)場(chǎng)法進(jìn)行平滑到的路徑，以此類推。引力勢(shì)場(chǎng)和引力函數(shù)[38]仍然采用原來(lái)的方式進(jìn)行表示，如式(4-9)和(4-10)所示(4-9)(4-10)式中，表示引力常量，表示旋翼式飛行機(jī)器人當(dāng)前位置到目標(biāo)或下一引力點(diǎn)的距離。針對(duì)目標(biāo)不可達(dá)的問(wèn)題對(duì)斥力函數(shù)進(jìn)行了改進(jìn)，在考慮斥力場(chǎng)的時(shí)候要計(jì)算旋翼式飛行機(jī)器人與目標(biāo)點(diǎn)之間的距離。新的斥力場(chǎng)函數(shù)如(4-11)所示(4-11)式中，表示斥力常量，表示旋翼式飛行機(jī)器人當(dāng)面位置到障礙物的距離，表示障礙物的影響距離。相應(yīng)的斥力采用以(4-12)-(4-14)形式表示(4-12)(4-13)(4-14)圖4.4三維圖中力的分解示意圖如圖4.4所示，力在三維圖中先分解為一個(gè)軸方向的分力和一個(gè)平面的分力，夾角為；再將分解為軸方向和軸方向的分力。當(dāng)旋翼式飛行機(jī)器人第步路徑點(diǎn)時(shí)，機(jī)器人的受力到前向目標(biāo)點(diǎn)引力和障礙物的斥力。由式(4-15)-(4-17)得到三維空間內(nèi)目標(biāo)點(diǎn)的引力在三個(gè)坐標(biāo)軸的分力，由式(4-18)-(4-20)得到三維空間內(nèi)目標(biāo)點(diǎn)的斥力在三個(gè)坐標(biāo)軸的分力。(4-15)(4-16)(4-17)(4-18)(4-19)(4-20)其中，為所在位置與引力點(diǎn)的連線與平面的夾角，為所在位置在平面的投影點(diǎn)與引力點(diǎn)的連線與軸的夾角；為所在位置與障礙物的連線與平面的夾角，為所在位置在平面的投影點(diǎn)與障礙物的連線與軸的夾角。將引力和斥力分別在三個(gè)坐標(biāo)軸上進(jìn)行合成后，再將三個(gè)分力進(jìn)行合成后得到所受的合力，確定下一步的路徑點(diǎn)。受到的合力為(4-21)(4-22)合力決定了旋翼式飛行機(jī)器人下一運(yùn)動(dòng)方向，可通過(guò)修改的值調(diào)節(jié)斥力大小，通過(guò)調(diào)節(jié)的值使得路徑平滑達(dá)到最優(yōu)效果。當(dāng)?shù)竭_(dá)最后一個(gè)引力點(diǎn)時(shí)，即引力為0時(shí)結(jié)束路徑平滑。這樣就可以得到一條光滑且安全無(wú)碰撞的路徑。改進(jìn)的人工勢(shì)場(chǎng)法的實(shí)現(xiàn)圖4.5為改進(jìn)后的人工勢(shì)場(chǎng)法的具體實(shí)現(xiàn)流程圖，除改進(jìn)斥力函數(shù)外，增設(shè)了多目標(biāo)點(diǎn)的方式以保證人工勢(shì)場(chǎng)法不會(huì)陷入局部最優(yōu)解。圖4.5改進(jìn)的人工勢(shì)場(chǎng)法流程圖如圖所示，先對(duì)環(huán)境進(jìn)行建模，再對(duì)人工勢(shì)場(chǎng)法的參數(shù)進(jìn)行初始化。計(jì)算當(dāng)前點(diǎn)的斥力和引力后，求得所受合力，根據(jù)合力確定下一點(diǎn)的方向，如果所受合力為為零則判斷是否到達(dá)最終目標(biāo)點(diǎn)還是中間目標(biāo)點(diǎn)，否則向著下一點(diǎn)。若沒(méi)有達(dá)到最終目標(biāo)點(diǎn)，則換成下一目標(biāo)點(diǎn)，否則畫出最優(yōu)路徑?；谌斯?shì)場(chǎng)法的路徑規(guī)劃仿真簡(jiǎn)單障礙物情況下的仿真表4.1人工勢(shì)場(chǎng)法參數(shù)設(shè)置參數(shù)設(shè)置數(shù)值起點(diǎn)位置(1,50,10)終點(diǎn)位置(65,5,10)引力增益系數(shù)10斥力增益系數(shù)5障礙影響距離3步長(zhǎng)0.7迭代次數(shù)500針對(duì)以上所提到的人工勢(shì)場(chǎng)算法在設(shè)置點(diǎn)狀障礙物和多目標(biāo)點(diǎn)的情況下進(jìn)行仿真，人工勢(shì)場(chǎng)法設(shè)置參數(shù)如表4.1所示。根據(jù)上表設(shè)置參數(shù)進(jìn)行仿真，其中障礙物的個(gè)數(shù)為7個(gè)，坐標(biāo)分別為(34,23,1),(23,53,10),(34,53,22),(43,23,18),(20,50,20),(40,10,5),(35,52,8)；多目標(biāo)個(gè)數(shù)為6個(gè)，坐標(biāo)分別為(2,5,4),(4,6,7),(34,6,17),(37,23,21),(32,46,21),(44,31,32)。圖4.6人工勢(shì)場(chǎng)法簡(jiǎn)單情況路徑規(guī)劃仿真結(jié)果如圖4.6所示，從仿真結(jié)果可以看出該人工勢(shì)場(chǎng)法在障礙物為點(diǎn)狀時(shí)可以很好的跟蹤各個(gè)目標(biāo)點(diǎn)并躲避障礙物。復(fù)雜障礙物情況下的仿真表4.2人工勢(shì)場(chǎng)法參數(shù)設(shè)置參數(shù)設(shè)置數(shù)值起點(diǎn)位置(1,39,8)終點(diǎn)位置(64,6,4)引力增益系數(shù)8斥力增益系數(shù)4障礙影響距離2步長(zhǎng)0.8迭代次數(shù)100復(fù)雜情況下人工勢(shì)場(chǎng)法的參數(shù)設(shè)置如表4.2所示，障礙物個(gè)數(shù)為65*65=4225個(gè)，多目標(biāo)個(gè)數(shù)為6個(gè)，分別為(15,14,6),(21,11,3),(31,9,2),(37,7,2.5),(47,2,3),(51,2,1)。仿真結(jié)果如圖4.7所示。圖4.7人工勢(shì)場(chǎng)法復(fù)雜情況路徑規(guī)劃本章小結(jié)本章首先對(duì)傳統(tǒng)的人工勢(shì)場(chǎng)法進(jìn)行了一個(gè)了解，介紹了如何構(gòu)建引力和斥力勢(shì)場(chǎng)。針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法的會(huì)目標(biāo)不可達(dá)和陷入局部最優(yōu)解的情況進(jìn)行了改進(jìn)，對(duì)斥力函數(shù)進(jìn)行了一些修改，使其更加合理。并針對(duì)下一章需要用到的人工勢(shì)場(chǎng)平滑算法進(jìn)行了介紹，提出了把蟻群算法的各路徑點(diǎn)當(dāng)作中間目標(biāo)點(diǎn)從而可以解決局部穩(wěn)定問(wèn)題。最后進(jìn)行了兩種情況的仿真驗(yàn)證了該人工勢(shì)場(chǎng)法可行且有效性?；趧?shì)場(chǎng)蟻群算法飛行路徑規(guī)劃引言人工勢(shì)場(chǎng)法是一種基于虛擬力的簡(jiǎn)便、有效的局部路徑規(guī)劃方法，具有較好的平滑性，不局限于柵格地圖，但是容易產(chǎn)生死鎖并陷入局部最優(yōu)解。蟻群算法是一種新型的啟發(fā)式路徑規(guī)劃算法，根據(jù)“信息素較濃的路線更近”原則，即可選擇出最佳路線。由于這個(gè)算法利用了正反饋機(jī)制，使得較短的路徑能夠有較大的機(jī)會(huì)得到選擇，并且由于采用了概率算法，所以它能夠不局限于局部最優(yōu)解。但是盲目性較大，且多是基于柵格法，故產(chǎn)生的路徑?jīng)]有很好的跟蹤性。因此本文針對(duì)這種情況，將人工勢(shì)場(chǎng)法較好的可跟蹤性和蟻群算法群集智能且不陷入死鎖的特點(diǎn)結(jié)合，先使用改進(jìn)的蟻群算法進(jìn)行三維路徑規(guī)劃，該蟻群算法針對(duì)三維情況，對(duì)信息素的更新和表示、啟發(fā)函數(shù)的設(shè)計(jì)的改進(jìn)，后使用人工勢(shì)場(chǎng)法進(jìn)行平滑，提高了路徑的可跟蹤性和安全性。在matlab中建立相應(yīng)的三維環(huán)境模型并作出路徑規(guī)劃仿真。勢(shì)場(chǎng)蟻群算法的實(shí)現(xiàn)針對(duì)旋翼式飛行機(jī)器人勢(shì)場(chǎng)蟻群算法的三維路徑規(guī)劃具體的算法流程如圖5.1所示。首先對(duì)三維環(huán)境進(jìn)行建模，初始化蟻群算法的各個(gè)參數(shù)，根據(jù)啟發(fā)信息函數(shù)及轉(zhuǎn)移概率選擇下一個(gè)點(diǎn)的位置，之后更新局部信息素。如果沒(méi)有達(dá)到目標(biāo)位置，繼續(xù)選擇下一個(gè)點(diǎn)的位置；如果達(dá)到了目標(biāo)位置，計(jì)算當(dāng)前的路徑長(zhǎng)度并更新全局信息素。如果滿足結(jié)束條件，則找到蟻群算法的最優(yōu)路徑。再對(duì)這條最優(yōu)路徑對(duì)人工勢(shì)場(chǎng)法進(jìn)行平滑處理，從而得到最后的優(yōu)化路徑。圖5.1勢(shì)場(chǎng)蟻群路徑規(guī)劃算法流程圖基于勢(shì)場(chǎng)蟻群飛行路徑規(guī)劃仿真為了驗(yàn)證本文所提出算法的有效性，在圖5.3所示的地圖環(huán)境下使用該算法，在matlab中進(jìn)行相應(yīng)的仿真。旋翼式飛行機(jī)器人的飛行路徑規(guī)劃仿真界面如圖5.2所示，生成的地圖即為圖5.3所示的三維環(huán)境空間，設(shè)置起點(diǎn)位置為(1,45,11)，終點(diǎn)位置為(64,12,12)。山峰個(gè)數(shù)為5個(gè)，分別在坐標(biāo)(10,10)，(40,25)，(35,50)，(20,35)，(15,50)處。圖5.2旋翼式飛行機(jī)器人的飛行路徑規(guī)劃仿真界面圖5.3建立的三維地圖模型表5.1蟻群算法參數(shù)設(shè)置參數(shù)數(shù)值種群規(guī)模40迭代次數(shù)400信息素衰減率0.9信息啟發(fā)因子0.5期望啟發(fā)因子2表5.2人工勢(shì)場(chǎng)法參數(shù)設(shè)置參數(shù)數(shù)值引力增益系數(shù)10斥力增益系數(shù)5障礙距離2步長(zhǎng)0.5迭代次數(shù)500(a)蟻群算法路徑規(guī)劃的仿真界面(b)蟻群算法路徑規(guī)劃的仿真結(jié)果圖5.4蟻群算法的路徑規(guī)劃(a)人工勢(shì)場(chǎng)法路徑規(guī)劃的仿真界面(b)人工勢(shì)場(chǎng)法路徑規(guī)劃的仿真結(jié)果圖5.5人工勢(shì)場(chǎng)法(a)勢(shì)場(chǎng)蟻群算法的路徑規(guī)劃仿真界面(b)勢(shì)場(chǎng)蟻群算法的路徑規(guī)劃仿真結(jié)果圖5.6勢(shì)場(chǎng)蟻群算法的路徑表5.1和表5.2分別為蟻群算法和人工勢(shì)場(chǎng)法設(shè)置的參數(shù)，圖5.4為只使用蟻群算法下的仿真界面和得出的最終路徑，圖5.5為只使用人工勢(shì)場(chǎng)法下的仿真界面和得出的最終路徑，圖5.6為在圖5.4的基礎(chǔ)上再使用人工勢(shì)場(chǎng)法進(jìn)行平滑后下的仿真界面和得出的最終路徑?？梢悦黠@看出圖5.4(b)中的路徑由幾段折線組成，光滑性和連續(xù)性較差；圖5.5(b)的路徑雖然具有較好的平滑性，但并不是最優(yōu)路徑；圖5.6(b)中的出的路徑是在使用人工勢(shì)場(chǎng)法光滑后得到的是若干點(diǎn)組成的一條光滑且連續(xù)的路徑。表5.3仿真結(jié)果算法路徑長(zhǎng)度蟻群算法117.3291人工勢(shì)場(chǎng)法104.0538勢(shì)場(chǎng)蟻群算法87.0797表5.3是仿真的結(jié)果，可以看出勢(shì)場(chǎng)蟻群算法的路徑長(zhǎng)度遠(yuǎn)小于單獨(dú)的蟻群算法和人工勢(shì)場(chǎng)法，即可搜索到最優(yōu)路徑。本章小結(jié)本章首先介紹了勢(shì)場(chǎng)蟻群融合算法流程圖，簡(jiǎn)要介紹了融合的方式。在給出的三維空間中，分別對(duì)單獨(dú)的蟻群算法、人工勢(shì)場(chǎng)法以及兩種算法融合后的路徑規(guī)劃情況進(jìn)行了仿真和分析。該算法首先利用蟻群算法進(jìn)行初步的路徑規(guī)劃，再利用人工勢(shì)場(chǎng)法對(duì)所得路徑進(jìn)行平滑。仿真結(jié)果表明，融合算法的效果明顯優(yōu)于兩種單獨(dú)的算法，利用人工勢(shì)場(chǎng)法進(jìn)行平滑，能夠充分考慮到障礙物對(duì)路徑的影響，并且使得旋翼式飛行機(jī)器人能夠更好的跟蹤該路徑?？偨Y(jié)與展望本文總結(jié)旋翼式飛行機(jī)器人具有飛行靈活、可控性強(qiáng)、抗干擾能力好等優(yōu)點(diǎn)，目前越來(lái)越得到研究者的關(guān)注，在軍事、民用和科研等方面應(yīng)用越來(lái)越廣泛。故研究其路徑規(guī)劃問(wèn)題是非常必要的。本文通過(guò)結(jié)合了路徑規(guī)劃算法當(dāng)今發(fā)展趨勢(shì)的情況下，查閱了大量國(guó)內(nèi)外文獻(xiàn)。在這些理論基礎(chǔ)上，針對(duì)隨機(jī)產(chǎn)生地表且存在山峰的三維地圖情況下，提出了一種針對(duì)旋翼式飛行機(jī)器人的、將人工勢(shì)場(chǎng)法和蟻群算法相結(jié)合的路徑規(guī)劃算法。通過(guò)兩種算法的結(jié)合彌補(bǔ)了各自的缺點(diǎn)并發(fā)揮了每種算法的優(yōu)點(diǎn)。本文具體做了以下幾個(gè)方面的工作：1、對(duì)無(wú)人駕駛飛行器的研究現(xiàn)狀進(jìn)行了歸納總結(jié)，分析了其未來(lái)的發(fā)展趨勢(shì)；對(duì)路徑規(guī)劃方法進(jìn)行了介紹，并將幾種常見的路徑規(guī)劃方法分為全局路徑規(guī)劃和局部路徑規(guī)劃，并分析了每種方法的優(yōu)缺點(diǎn)，提出了路徑規(guī)劃方法的研究趨勢(shì)。2、詳細(xì)的介紹了基本的蟻群算法的原理和路徑搜索方式以及該方法應(yīng)用的領(lǐng)域。在三維環(huán)境下提出了一種三維柵格環(huán)境的描述方式，并建立相應(yīng)的模型、確定目標(biāo)函數(shù)。針對(duì)飛行環(huán)境，對(duì)蟻群算法的信息素、啟發(fā)函數(shù)、適應(yīng)度函數(shù)和螞蟻的轉(zhuǎn)移概率進(jìn)行了改進(jìn)，并通過(guò)仿真比較了改進(jìn)后方法的有效性以及可行性。3、對(duì)人工勢(shì)場(chǎng)法的原理以及傳統(tǒng)的人工勢(shì)場(chǎng)法進(jìn)行介紹，并指出傳統(tǒng)的人工勢(shì)場(chǎng)法在應(yīng)用上存在的一些問(wèn)題。根據(jù)這些問(wèn)題并結(jié)合算法融合時(shí)人工勢(shì)場(chǎng)法所需起到的平滑作用，對(duì)人工勢(shì)場(chǎng)法的斥力勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn)，并增設(shè)多目標(biāo)點(diǎn)以解決人工勢(shì)場(chǎng)法陷入局部最優(yōu)解和目標(biāo)不可達(dá)問(wèn)題。對(duì)在三維空間中連續(xù)平面障礙物，提出了一種障礙點(diǎn)的提取方式。針對(duì)改進(jìn)的人工勢(shì)場(chǎng)法，分別在只有點(diǎn)狀障礙物和連續(xù)平面障礙物的情況下進(jìn)行仿真，該方法可以克服上述缺點(diǎn)。4、針對(duì)蟻群算法和人工勢(shì)場(chǎng)法在路徑規(guī)劃上有各自的優(yōu)缺點(diǎn)，將它們互補(bǔ)后提出了一種旋翼式飛行機(jī)器人的勢(shì)場(chǎng)蟻群算法。該算法該方法先建立三維地圖，在此基礎(chǔ)上用蟻群算法找到最優(yōu)路徑并用人工勢(shì)場(chǎng)法進(jìn)行路徑平滑優(yōu)化。5、在MATLAB2013a的軟件開發(fā)環(huán)境下，對(duì)本文提出的新算法進(jìn)行了仿真。仿真結(jié)果表明，利用人工勢(shì)場(chǎng)法進(jìn)行平滑，能夠充分考慮到障礙物對(duì)路徑的影響，并且使得旋翼式飛行機(jī)器人能夠更好的跟蹤該路徑。效果明顯優(yōu)于單獨(dú)的人工勢(shì)場(chǎng)法或單獨(dú)的蟻群算法，能夠獲得更優(yōu)的路徑。工作展望綜上所示，本文在基于勢(shì)場(chǎng)蟻群的旋翼式飛行機(jī)器人的路徑規(guī)劃上做了一定的研究工作，通過(guò)對(duì)該算法進(jìn)行了仿真，證明了該算法的可行，但本文作出的研究也有一定的缺陷和不足，未來(lái)要在以下幾個(gè)方面做進(jìn)一步深入的研究：本文是針對(duì)環(huán)境信息全部已知且在靜態(tài)環(huán)境中進(jìn)行的旋翼式飛行機(jī)器人路徑規(guī)劃，而在實(shí)際工作中，旋翼式飛行機(jī)器人不可能處于這樣理想化的狀態(tài)，可能環(huán)境信息是部分或完全未知的，這就需要我們研究能夠在動(dòng)態(tài)環(huán)境中進(jìn)行的旋翼式飛行機(jī)器人路徑規(guī)劃。本文在使用蟻群算法時(shí)是在柵格環(huán)境建模中進(jìn)行的，這是對(duì)旋翼式飛行機(jī)器人工作環(huán)境的一種理想化表示方法，如應(yīng)用在實(shí)際中將產(chǎn)生誤差，可能不能很好的避障。本文在研究路徑規(guī)劃方法時(shí)沒(méi)有考慮到旋翼式飛行機(jī)器人本身的大小，而是將其作為一個(gè)質(zhì)點(diǎn)進(jìn)行運(yùn)動(dòng)。在未來(lái)的研究中，由于旋翼式飛行機(jī)器人本身有一定的大小，如不考慮自身大小，可能會(huì)在距離障礙物過(guò)近時(shí)產(chǎn)生碰撞。本文雖然融合了蟻群算法和人工勢(shì)場(chǎng)法，且對(duì)每種算法進(jìn)行了一定的改進(jìn)。但是這些研究顯然不夠，未來(lái)可以考慮三種、四種甚至更多的算法融合在一起，能夠進(jìn)一步提高旋翼式飛行機(jī)器人路徑規(guī)劃的精讀和效率。本文在對(duì)提出的方法進(jìn)行仿真時(shí)，對(duì)相關(guān)參數(shù)的選擇只是根據(jù)經(jīng)驗(yàn)設(shè)定，并沒(méi)有太多的理論基礎(chǔ)提供支持，合理的選擇參數(shù)也是今后需要研究的。本文僅僅是對(duì)提出的勢(shì)場(chǎng)蟻群算法進(jìn)行了仿真，但是并沒(méi)有應(yīng)用到實(shí)際的旋翼式飛行機(jī)器人工作當(dāng)中。致謝時(shí)光荏苒，隨著論文的寫作接近尾聲，大學(xué)本科生活也即將畫上句號(hào)?；仡欉^(guò)去的四年，我感觸頗深，也對(duì)那些指導(dǎo)過(guò)我、幫助過(guò)我、激勵(lì)過(guò)我的人表示深深的感謝。首先我要感謝我的導(dǎo)師譚建豪教授，譚老師在我完成本科畢業(yè)設(shè)計(jì)的過(guò)程中給了我很大的幫助。在畢業(yè)設(shè)計(jì)的過(guò)程中遇到了很多的問(wèn)題，譚老師都給了我很多寶貴的意見，自己取得了這些成績(jī)都凝聚著老師的心血。譚老師不論是在畢業(yè)設(shè)計(jì)的研究思路、研究方法、論文寫作、論文修改還是論文定稿中，都有著很重要的作用。譚老師國(guó)際化的視野、淵博的學(xué)識(shí)、嚴(yán)謹(jǐn)縝密的治學(xué)風(fēng)格都影響著我今后的工作、學(xué)習(xí)和生活。感謝王媛媛學(xué)姐和張藝巍學(xué)姐，在完成畢業(yè)設(shè)計(jì)和論文的過(guò)程中提供了很多的幫助和支持，在遇到困難時(shí)給我提供了很多方法以及一些科研資料，幫助我解決科研難題。在有需要的時(shí)候她們都會(huì)主動(dòng)提供幫助，她們樂(lè)于助人的品質(zhì)讓我敬佩。感謝鐘航學(xué)長(zhǎng)，李力學(xué)長(zhǎng)和陳彥杰學(xué)長(zhǎng)在過(guò)程中對(duì)我的幫助和支持。還要感謝感謝我的家人和朋友們對(duì)我的鼓勵(lì)和支持，正是有個(gè)他們讓我能夠順利完成我的本科學(xué)業(yè)。最后感謝在百忙之中抽出時(shí)間評(píng)閱我論文的專家和學(xué)者們。王楚2015年5月參考文獻(xiàn)附錄A仿真界面圖A1旋翼式飛行機(jī)器人飛行路徑規(guī)劃仿真開始界面圖A2旋翼式飛行機(jī)器人飛行路徑規(guī)劃仿真界面附錄B地圖生成部分代碼1、生成山峰部分的函數(shù)function[data]=CeatHill(N,h,x0,y0,xi,yi,num)x=1:1:num;y=1:1:num;form=1:numforn=1:numSum=0;fork=1:Ns=h(k)*exp(-((x(m)-x0(k))/xi(k))^2-((y(n)-y0(k))/yi(k))^2);Sum=Sum+s;enddata(m,n)=Sum;endend2、生成地表部分的函數(shù)function[data]=SquareDiamond(N,initvalue,c)n=2^N;data=zeros(n+1);data(1,1)=initvalue;data(1,n+1)=initvalue;data(n+1,1)=initvalue;data(n+1,n+1)=initvalue;data=mytry(data,n,c,n);function[x]=rnd(absvalue)x=(rand(1)-0.5)*2*absvalue;function[data]=mytry(data,m,r,n)fori=1:m:nforj=1:m:ndata((i+i+m)/2,(j+j+m)/2)=(data(i,j)+data(i,j+m)+data(i+m,j)+data(i+m,j+m))/4+rnd(r);endendforj=1+m/2:m:ndata(1,j)=(data(1,j+m/2)+data(1+m/2,j)+data(1,j-m/2)+data(n+1-m/2,j))/4+rnd(r);endfori=1+m:m:nforj=1+m/2:m:ndata(i,j)=(data(i,j+m/2)+data(i+m/2,j)+data(i,j-m/2)+data(i-m/2,j))/4+rnd(r);endendforj=1+m/2:m:ndata(n+1,j)=(data(n+1,j+m/2)+data(1+m/2,j)+data(n+1,j-m/2)+data(n+1-m/2,j))/4+rnd(r);endfori=1+m/2:m:ndata(i,1)=(data(i,1+m/2)+data(i+m/2,1)+data(i,n+1-m/2)+data(i-m/2,1))/4+rnd(r);endfori=1+m/2:m:nforj=1+m:m:ndata(i,j)=(data(i,j+m/2)+data(i+m/2,j)+data(i,j-m/2)+data(i-m/2,j))/4+rnd(r);endendfori=1+m/2:m:ndata(i,n+1)=(data(i,1+m/2)+data(i+m/2,n+1)+data(i,n+1-m/2)+data(i-m/2,n+1))/4+rnd(r);endif(m>2)data=mytry(data,m/2,r/2,n);end附錄C蟻群算法部分代碼1、計(jì)算啟發(fā)值函數(shù)functionqfz=CacuQfz(Nexty,Nexth,Nowy,Nowh,endy,endh,abscissa,HeightData)ifHeightData(Nexty,abscissa)<NexthS=1;elseS=0;endD=100/(sqrt(1+(Nowh-Nexth)^2+(Nexty-Nowy)^2)+sqrt((65-abscissa)^2...+(endh-Nexth)^2+(endy-Nowy)^2));M=30/abs(Nexth+1);qfz=S*M*D;2、計(jì)算個(gè)體適應(yīng)度值函數(shù)functionfitness=CacuFit(path)[n,m]=size(path);fori=1:nfitness(i)=0;forj=2:m/2fitness(i)=fitness(i)+sqrt(1+(path(i,j*2-1)-path(i,(j-1)*2-1))^2...+(path(i,j*2)-path(i,(j-1)*2))^2)+abs(path(i,j*2));endend3、蟻群算法的路徑規(guī)劃function[path,pheromone]=searchpath(PopNumber,LevelGrid,PortGrid,pheromone,HeightData,starty,starth,endy,endh)ycMax=2;hcMax=2;decr=0.8;forii=1:PopNumberpath(ii,1:2)=[starty,starth];NowPoint=[starty,starth];forabscissa=2:PortGrid-1kk=1;fori=-ycMax:ycMaxforj=-hcMax:hcMaxNextPoint(kk,:)=[NowPoint(1)+i,NowPoint(2)+j];if(NextPoint(kk,1)<64)&&(NextPoint(kk,1)>0)&&(NextPoint(kk,2)<64) &&(NextPoint(kk,2)>0) qfz(kk)=CacuQfz(NextPoint(kk,1),NextPoint(kk,2),NowPoint(1), NowPoint(2),endy,endh,abscissa,HeightData);qz(kk)=qfz(kk)*pheromone(abscissa,NextPoint(kk,1),NextPoint(kk,2));kk=kk+1;elseqz(kk)=0;kk=kk+1;endendendsumq=qz./sum(qz);pick=rand;whilepick==0pick=rand;endfori=1:25pick=pick-sumq(i);ifpick<=0index=i;break;endendoldpoint=NextPoint(index,:);pheromone(abscissa+1,oldpoint(1),oldpoint(2))=0.5*pheromone(abscissa+1,oldpoint(1),oldpoint(2));path(ii,abscissa*2-1:abscissa*2)=[oldpoint(1),oldpoint(2)];NowPoint=oldpoint;endpath(ii,129:130)=[endy,endh];end附錄D人工勢(shì)場(chǎng)法部分代碼1、計(jì)算引力的函數(shù)function[Yatx,Yaty,Yatz]=compute_Attract(X,Xsum,k,anglea,angleb,b,Po,n)R1=((X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2+(X(3)-Xsum(1,3))^2);R2=((X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2);r1=sqrt(R1);r2=sqrt(R2);Yatx=k*r2*cos(angleb);%angle=Y(1)Yaty=k*r2*sin(angleb);Yatz=k*r1*sin(anglea);2、計(jì)算斥力的函數(shù)function[Yrerxx,Yreryy,Yrerzz,Yataxx,Yatayy,Yatazz]= compute_repulsion(X,Xsum,Xza,m,anglea_at,angleb_at,anglea_re,angleb_re,z,Po,a,c)Rat=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2+(X(3)-Xsum(1,3))^2;rat=sqrt(Rat);fori=1:mRrei(i)=(X(1)-Xsum(i,1))^2+(X(2)-Xsum(i,2))^2+(X(3)-Xsum(i,3))^2;rre(i)=sqrt(Rrei(i));R0=(Xsum(c,1)-Xza(i,1))^2+(Xsum(c,2)-Xza(i,2))^2+(Xsum(c,3)-Xza(i,3))^2;r0=sqrt(R0);ifrre(i)>Po%Yrerx(i)=0;Yrery(i)=0;Yrer