無人車路徑規(guī)劃子系統(tǒng)設(shè)計（A算法）

目錄TOC\o"1-3"\h\u243741緒論 緒論近年來，無人車的路徑規(guī)劃逐漸成為無人車研究的基礎(chǔ)和核心方向。它為幾個學(xué)科的技術(shù)研究提供了一個更廣闊的平臺。對于路徑規(guī)劃算法的研究，在理論仿真的基礎(chǔ)上，增加了車身本身的方向性，實現(xiàn)了智能小車的路徑規(guī)劃方案。路徑規(guī)劃不僅是無人車的智能化的代表，而且是無人車自主導(dǎo)航的基礎(chǔ)。以自行設(shè)計搭建的無人駕駛汽車為實驗平臺，研究了路徑規(guī)劃算法，逐步總結(jié)分析了Dijkstra算法等常用路徑規(guī)劃算法的優(yōu)缺點。最后，對A*算法進(jìn)行了選擇、掌握和實現(xiàn)。在理論和仿真相結(jié)合的基礎(chǔ)上，首先對環(huán)境進(jìn)行柵格化，然后通過車身子上的電子羅盤獲得更加準(zhǔn)確的實時方向，再通過A*算法設(shè)定車輛轉(zhuǎn)彎策略。最后，實現(xiàn)了無人車的路徑規(guī)劃策略。大多數(shù)人認(rèn)為無人駕駛汽車是21世紀(jì)才被發(fā)現(xiàn)的，但工程師們從20世紀(jì)初就開始研究它們，在可行性和實用性,并取得了很大的進(jìn)步,所以研究和開發(fā)開始比我們想象的更早。國外發(fā)展史通過無線電去控制三輪車20世紀(jì)初——無線電遙控根據(jù)“UnmannedSystemsofWorldWarsIandII”的作者埃弗里特（HREverett）的說法，第一輛無人車于1904年西班牙發(fā)明家萊昂納多·托雷斯·奎韋多（LeonardoTorres-Quevedo）制造的無線電遙控三輪車。并且在第一次世界大戰(zhàn)中，軍隊使用了無線電控制的各種小型車輛來運(yùn)送彈藥或引爆火藥。到了1925年某天的美國紐約巷子里，當(dāng)?shù)厥忻窨吹搅艘惠v無人駕駛的福特T型汽車在百老匯大街上徐徐行駛著，這個想法是用天線接收無線電信號來控制汽車的速度和方向。這是一輛由前美國陸軍工程師霍迪納開發(fā)的無線電控制汽車。無線電控制的汽車在當(dāng)時引起了很多關(guān)注，被媒體稱為“美國奇跡”，盡管它有幾個版本的改進(jìn)，但沒有實際意義的進(jìn)展。圖1.2.1早期無線電控制的無人車到了1932年，一輛由工程師“控制”的無線電遙控汽車失控沖進(jìn)人群，造成10多人受傷，這件事情使該工程師被逮捕，之后就使無人駕駛汽車退出了人們的視野。用線圈引導(dǎo)控制汽車1957年，美國77號公路上的一輛雪佛蘭汽車通過在道路上測試自動駕駛汽車的原理下使用線圈進(jìn)行導(dǎo)航控制，其原理是利用鋪設(shè)在道路上的電線進(jìn)行方向控制。當(dāng)時的自動駕駛實驗是由漢考克領(lǐng)導(dǎo)的，他是國家交通工程師汽車電子設(shè)備制造商RCA邀請的，電子設(shè)備制造商RCA受邀為車輛實現(xiàn)自動化。實現(xiàn)項目的自動化概念來源于諾曼?貝爾在1939年的世界博覽會上向未來城市的想法與通用汽車公司的諾曼·貝爾·格魯曼·迪斯合作提出了未來城市規(guī)劃的概念，城市中的所有車輛都將由無線電控制。同時為車輛提供電力和操作。圖1.2.2線圈引導(dǎo)控制汽車的概念藍(lán)圖為測試輪胎可靠性的“無人駕駛汽車”圖1.2.3無人駕駛汽車圖1.2.4控制臺1968年，德國汽車制造商大陸集團(tuán)（ContinentalGroup）為了測試輪胎的可靠性，提出了“無人駕駛汽車”的實驗。該實驗在德國康德.德羅姆（KantDerome）的測試軌道上進(jìn)行，由西門子、西屋電氣、慕尼黑大學(xué)和達(dá)姆施塔特大學(xué)的研究人員共同完成。當(dāng)汽車轉(zhuǎn)彎時，傳感器會向系統(tǒng)發(fā)送警報信號，引導(dǎo)汽車回到原來的位置，而控制站可以引導(dǎo)汽車剎車和加速。帶有傳感器的汽車到了1994年，德國工程師迪克曼斯為了做實驗，塔將將兩輛轎車停在法國巴黎的道路上，那個轎車有一個車載電腦系統(tǒng)，可以改變車行駛道路、油門和剎車。而早在1986年，迪克曼斯曾經(jīng)給一輛奔馳貨車配帶電腦、攝像頭和傳感器，并使其能接收道路標(biāo)記和信號等信息，并且能夠在實際情況的公路上駕駛。圖1.2.5原視頻截圖電腦和機(jī)器視覺1946年，在美國賓夕法尼亞大學(xué)發(fā)明了世界上第一臺電子計算機(jī)“電子數(shù)字積分計算機(jī)”。20世紀(jì)60年代，計算機(jī)科學(xué)技術(shù)開始迅速發(fā)展壯大，使計算機(jī)變得更小、更加可靠、功能更強(qiáng)大，它能完成很多復(fù)雜的任務(wù)：如圖像處理、運(yùn)算等，這為無人駕駛技術(shù)的發(fā)展創(chuàng)造了條件。1977年，日本筑波工程研究實驗室的S.Tsugawa和他的同事開發(fā)了第一種使用攝像頭測量導(dǎo)航信息的自動車輛——一種內(nèi)置兩個攝像頭和模擬計算機(jī)技術(shù)進(jìn)行信號處理的車輛。它能夠達(dá)到每小時30公里，但是在高架軌道的幫助下，這是我們所知道的第一個使用視覺設(shè)備的無人駕駛實驗，這開啟自動駕駛的新篇章。圖1.2.6自動控制車國內(nèi)無人車發(fā)展史關(guān)于無人駕駛汽車的發(fā)展，與國外相比而言，我國起步較晚。我國自20世紀(jì)90年代初期開始研究陸地自主車。當(dāng)時是以軍用平臺為背景，在智能移動機(jī)器人技術(shù)框架下發(fā)展。當(dāng)時，國家“863”計劃對智能移動機(jī)器人的主題方面研究進(jìn)行遙控駕駛防核化偵察車研究。“八五”期間，在總裝備部（原國防科工委）帶領(lǐng)下，由北京理工大學(xué)、國防科技大學(xué)、南京理工大學(xué)、清華大學(xué)、浙江大學(xué)、哈爾濱工業(yè)大學(xué)組成團(tuán)隊，開發(fā)出我國第一輛陸地自主樣車ATB-1(AutonomousTestBed-1)，也可以說是7B.8軍用智能機(jī)器人平臺，目前放置在北京理工大學(xué)。在“九五”時候研發(fā)出第二代陸地自主車ATB-2，然后又研制成第三代自主車，現(xiàn)在正在準(zhǔn)備第四代自主車的研制工作。軍用陸地自主車的研究為我國自主車的研究積累了關(guān)鍵技術(shù)，培養(yǎng)了人才隊伍。賀漢根、楊靜宇、何克忠、付夢印、陸際聯(lián)、劉濟(jì)林、朱淼良等教授為此做出了巨大貢獻(xiàn)。從2009年開始，國家自然科學(xué)基金委為了研究具有感受自然環(huán)境與智能與決策相結(jié)合的無人駕駛汽車驗證平臺還專門成立了“視聽覺信息的認(rèn)知計算”重大研究計劃組，把研究的這個平臺作為其科學(xué)目標(biāo)。直到2014年，已連續(xù)六年開展“中國智能車未來挑戰(zhàn)賽”，使我國無人駕駛汽車的研究發(fā)展更上一層樓，這與李德毅院士、鄭南寧院士以及專家組教授們的大力推動是分不開的。我國的一些自己創(chuàng)建的汽車品牌正在向無人駕駛、自動駕駛領(lǐng)域進(jìn)軍，并增強(qiáng)在這個領(lǐng)域的探索。由國家工信部允許的國內(nèi)第一個國家智能網(wǎng)聯(lián)汽車試點示范區(qū)域在2016年6月的上海嘉定正式投入建設(shè)。目前開放的封閉測試區(qū)為無人駕駛汽車提供綜合性測試場地和功能要求。目前國家正在為無人駕駛相關(guān)技術(shù)規(guī)范積極推動做出努力。雖然我國涉及無人駕駛汽車領(lǐng)域時間相對其他國家晚了一點，不過專業(yè)人士認(rèn)為，在更好的管理和消費(fèi)環(huán)境下使中國可能成為無人駕駛汽車的主要市場之一。據(jù)波士頓咨詢公司預(yù)計，到2035年全球無人駕駛汽車的可以賣掉將達(dá)1200萬輛，其中中國出售將超過四分之一。按這樣發(fā)展10年后，中國遍地都將是無人駕駛汽車并占據(jù)全球汽車領(lǐng)域80%的份額。近代發(fā)展方向（無人車路徑規(guī)劃（A*算法）方向）無人車作為研究的熱點問題之一，根據(jù)我國現(xiàn)在的無人駕駛汽車的發(fā)展?fàn)顩r，分析無人駕駛汽車現(xiàn)在及將來還將處于研發(fā)的初級階段，國內(nèi)無人駕駛汽車量產(chǎn)時間最早約出現(xiàn)在2022年，因此2018-2021年中國無人駕駛汽車行業(yè)主要還是先搞技術(shù)，以技術(shù)研發(fā)為主要目的。估計隨著國內(nèi)外技術(shù)成熟度的大幅度升高，國內(nèi)各車企會一直提高參與無人駕駛汽車研發(fā)的積極性。據(jù)相關(guān)權(quán)威公司估算，到2035年全球無人駕駛汽車銷量將達(dá)到2100萬輛左右，而我國市場的份額約占24%左右也就是約為504萬輛，其市場前景非?？捎^。而無人車研究的路徑規(guī)劃本來也是為了實現(xiàn)無人車，它從出發(fā)點到達(dá)目的地的行車軌跡和行車過程中的路徑是選擇的最優(yōu)或者近似最優(yōu)的無碰撞路徑。因為所處環(huán)境信息的不同，路徑規(guī)劃方案分為兩種：一種是基于已知的、靜態(tài)的全局無人車路徑規(guī)劃方案；另一種是基于未知的、動態(tài)的局部無人車路徑規(guī)劃方案。全局路徑規(guī)劃讓車輛的導(dǎo)航在全局上是全局最優(yōu)，而局部路徑規(guī)劃能夠使得導(dǎo)航更具備實時性，并且能夠?qū)θ值貓D上不明確的狀況或者無法描述的環(huán)境狀況進(jìn)行處理，所以通過兩者的結(jié)合，讓車輛導(dǎo)航具備更好的可用性和發(fā)展性。作為一名對無人小車有著濃厚興趣的初學(xué)者如果直接入手英偉達(dá)的Jetbot則開發(fā)門檻較高，于是我們選擇了對Jetbot做了較深度開發(fā)推出的JetbotAI智能機(jī)器人平臺，本文中也將通過該平臺搭配詳細(xì)的教程對Jetbot機(jī)器人開發(fā)環(huán)境搭建系統(tǒng)進(jìn)行學(xué)習(xí)。

需求分析與構(gòu)架設(shè)計通過前面的介紹，我們可以知道路徑規(guī)劃使得車輛導(dǎo)航具有最優(yōu)的特性，并且能夠?qū)崟r導(dǎo)航，并且能夠應(yīng)對地圖上不清楚的情況，或者不可描述的環(huán)境條件，因此通過兩者的結(jié)合，可以使車輛導(dǎo)航具有更好的可用性。本章將介紹全局路徑規(guī)劃，如何利用地圖信息，如何進(jìn)行起點和終點之間的地圖信息匹配，如何選擇最優(yōu)路徑以及如何進(jìn)行路徑規(guī)劃等問題進(jìn)行討論。介紹了相關(guān)的基本理論，如OSM地圖介紹，A*算法介紹。提出了OSM地圖的讀取方法和存儲方法及結(jié)構(gòu)、改進(jìn)的起點和終點算法、A*算法的具體實現(xiàn)、交叉口規(guī)劃優(yōu)化和道路信息補(bǔ)充方法。最后，介紹了系統(tǒng)并對實際結(jié)果進(jìn)行了分析。需求分析路徑規(guī)劃旨在實現(xiàn)無人車從起始狀態(tài)到目標(biāo)狀態(tài)的一條最優(yōu)或者近似最優(yōu)的無碰撞路徑。在無人車路徑規(guī)劃問題上，首先應(yīng)解決對環(huán)境的建模問題，其次應(yīng)解決的是尋找合適的算法實現(xiàn)路徑規(guī)劃策略。全局路徑規(guī)劃與局部路徑規(guī)劃常使用的路徑規(guī)劃算法有Dijkstra算法、A*算法、Fallback算法等。本文將重點從柵格法構(gòu)建的地圖中用A*算法模擬無人車行駛的路徑。全局和局部規(guī)劃系統(tǒng)的綜述為了給輸入任務(wù)點設(shè)計一條全局最優(yōu)的安全路徑，使生成的路徑能夠更好地處理復(fù)雜的交通道路，為局部路徑規(guī)劃提供詳細(xì)的路徑信息，全局路徑規(guī)劃系統(tǒng)的流程設(shè)計如下圖2.2.1：圖2.2.1全局和局部規(guī)劃系統(tǒng)示意圖全局路徑規(guī)劃無人車和移動機(jī)器人的導(dǎo)航主要差異是無人車的導(dǎo)航是在大量的離線地圖信息基礎(chǔ)上完成的，無人車比移動機(jī)器人更能在廣闊的視野中完成導(dǎo)航任務(wù)，現(xiàn)階段主要的基于離線地圖信息的全局路徑規(guī)劃算法有：(a)A*算法A*是一種啟發(fā)式搜索算法。通過設(shè)置合適的啟發(fā)式函數(shù)，從起始點開始估計每個搜索節(jié)點之間的消耗量。通過比較消耗的大小，選擇最優(yōu)擴(kuò)展點，直到找到目標(biāo)節(jié)點的位置。優(yōu)點是需要擴(kuò)展的節(jié)點少，穩(wěn)定性好。缺點是在實際應(yīng)用中忽略了自身對節(jié)點體積的限制，但有很好的改進(jìn)空間。(b)Dijkstra算法Dijkstra算法是一種典型的路徑最短算法，從起始點向外延伸到終點。遍歷所有節(jié)點可以得到最短路徑，成功率高，穩(wěn)定性好。然而，遍歷過多的節(jié)點會導(dǎo)致效率低下、大型復(fù)雜網(wǎng)絡(luò)的應(yīng)用能力低下以及無法處理負(fù)邊。(c)Fallback算法Dijkstra的一種改進(jìn)算法是Fallback算法，主要用于多條需要服務(wù)質(zhì)量(QoS)的路徑。用不同的服務(wù)質(zhì)量構(gòu)造目標(biāo)函數(shù)，用Dijkstra搜索路徑，根據(jù)服務(wù)質(zhì)量因子重復(fù)算法。優(yōu)缺點和Dijkstra一樣。國內(nèi)其他研究者也提出了許多基于Dijkstra算法、A*算法等經(jīng)典算法的路徑規(guī)劃算法的改進(jìn)算法。比如馮路、陸冬梅等人提出了交通網(wǎng)絡(luò)有限搜索區(qū)域的最短路徑算法，張連鵬、劉國林等人提出了GIS路徑優(yōu)化的方向優(yōu)先搜索法，陳燕燕、王東柱等人提出了分布式車輛導(dǎo)航系統(tǒng)路徑優(yōu)化的約束A*算法。這些算法對外傷的A*算法做了很多改進(jìn)。局部路徑規(guī)劃局部規(guī)劃主要是基于綜合融合無人車的傳感器感知信息形成的局部感知信息來規(guī)劃繞車行駛的局部路徑。局部路徑對環(huán)境的響應(yīng)速度很快，但不會在全局范圍內(nèi)收斂。目前常用的局部路徑規(guī)劃算法有：（a）人工勢場法人工勢場法是模擬引力斥力的作用下的物體運(yùn)動，目標(biāo)位置和運(yùn)動物體之間產(chǎn)生的是引力，運(yùn)動物體和障礙物之間產(chǎn)生的是斥力，通過建立引力和斥力場的函數(shù)進(jìn)行路徑的規(guī)劃，優(yōu)點是規(guī)劃出來的路徑安全并且平滑，簡單，但是存在局部最優(yōu)不是全局最優(yōu)的問題，引力場算法的設(shè)計是應(yīng)用成功的關(guān)鍵。（b）RRT（快速擴(kuò)展隨機(jī)樹）法RRT（快速擴(kuò)展隨機(jī)樹）法是一種多維空間中有效率的規(guī)劃方法。它以一個初始點作為根節(jié)點，通過隨機(jī)生成一個節(jié)點，取出隨機(jī)樹中的離隨機(jī)節(jié)點最近的節(jié)點根據(jù)步長生成樹的子節(jié)點，最后生成一個隨機(jī)擴(kuò)展的樹，當(dāng)樹中的葉子節(jié)點包含了終點或進(jìn)入了終點所在的區(qū)域，便可以在隨機(jī)樹中找到一條由從初始點到標(biāo)點的路徑。優(yōu)點是能夠適應(yīng)復(fù)雜環(huán)境，實現(xiàn)方法簡單，缺點是隨機(jī)產(chǎn)生點可能會出現(xiàn)局部最優(yōu)情況。其他的局部路徑規(guī)劃算法還有CMU（卡耐基梅隆大學(xué)）基于模型預(yù)測路徑生成器，利用模型預(yù)測軌跡生成初始和目標(biāo)車輛狀態(tài)的動態(tài)可行軌跡，適合高速地動態(tài)的環(huán)境，但是不適合非結(jié)構(gòu)化的道路和復(fù)雜的環(huán)境。還有最優(yōu)解算法，通過求解目標(biāo)函數(shù)的最優(yōu)解，二次的帶有非線性不等式作為約束的非線性最優(yōu)問題。適合于結(jié)構(gòu)化的道路，動態(tài)特性好。構(gòu)架設(shè)計A*算法是目前應(yīng)用最廣泛的路徑規(guī)劃算法，因為根據(jù)啟發(fā)函數(shù)的取值不同，A*能夠生成不僅僅，是路徑最短的路徑，還能生成時間消耗最少，轉(zhuǎn)角最少等路徑，擁有很強(qiáng)的靈活性，能夠滿足各種不同場景的需要。我們將利用matlab來對我們的A*算法進(jìn)行實驗仿真與驗證：圖2.3.1matlab桌面快捷圖標(biāo)基于柵格構(gòu)圖下的A*算法最短路徑規(guī)劃無人駕駛車輛的環(huán)境是通過網(wǎng)格建模方法,如下圖所示,和起始點和目標(biāo)點G的無人駕駛車輛設(shè)置。一個最優(yōu)或近似最優(yōu)無碰撞路徑是通過算法選擇的起點和終點之間的G，A*算法作為一種典型的啟發(fā)式算法,人工智能的搜索路徑問題有著廣泛的應(yīng)用,它將整個結(jié)構(gòu)放入網(wǎng)格環(huán)境中，通過設(shè)置起點S、目標(biāo)點G和障礙物B(灰度部分)，并通過相應(yīng)的評價函數(shù)對最優(yōu)路徑進(jìn)行分析，最終找出最優(yōu)路徑。圖2.3.2路徑規(guī)劃建模A*算法實現(xiàn)最佳路徑規(guī)劃的基本思想A*算法作為一種啟發(fā)式算法，通過構(gòu)造柵格信息環(huán)境，每個柵格代表一個節(jié)點，并設(shè)定移動?xùn)鸥翊笮?，其中水平移動和豎直移動一個柵格的對角線=√2。其設(shè)定原理是根據(jù)勾股定理即設(shè)定直角邊a，直角邊b，若a=b=2，則斜邊為2√2。如下圖2.3.3所示：圖2.3.3示意圖設(shè)定起點S、移動距離V、目標(biāo)點G和障礙物B（深色區(qū)域）。當(dāng)把無人車看作質(zhì)點，其從起點移動時，進(jìn)行路徑搜索。第一次搜索中VS－5=√2（VS－5表示從柵格S到柵格“5”的距離），VS－6=1，由于VS－6＜VS－5，所以將5號柵格作為下一個父節(jié)點。在第二次搜索時，由于VS－5=√2，V5－6=1，VS－6=14，即VS－6＜VS－5+V5－6，所以舍棄5號節(jié)點，選擇6號節(jié)點作為最優(yōu)節(jié)點，并繼續(xù)重復(fù)執(zhí)行其搜索過程。A*算法通過將死節(jié)點外的以及需要進(jìn)行對比的節(jié)點放到一個表中，然后對所有節(jié)點進(jìn)行比對，獲得一個“最優(yōu)節(jié)點”，實現(xiàn)全局范圍內(nèi)“最優(yōu)節(jié)點”的評判，進(jìn)而完成路徑規(guī)劃策略。文章具體內(nèi)容安排第一章為文章的緒論部分，主要介紹了本文研究智能小車的意義與背景，提到了路徑規(guī)劃的國內(nèi)外研究現(xiàn)狀并對提到了相應(yīng)的關(guān)鍵技術(shù)（全局路徑規(guī)劃與局部路徑規(guī)劃）。第二章為全局路徑規(guī)劃、局部路徑規(guī)劃進(jìn)行了初步分析且詳細(xì)說明了路徑規(guī)劃的常用方法和分類，以及各種算法之間的優(yōu)缺點。并對于這兩種規(guī)劃采取了較為中間的選取柵型構(gòu)圖下的A*算法最短路徑規(guī)劃的原理和具體實現(xiàn)方法。第三章即開始對于前面提出的選取柵型構(gòu)圖下的A*算法用matlab進(jìn)行實驗仿真與驗證。第四章從我們在matlab中完整的模擬仿真結(jié)果中的A*算法方式通過JetbotAI智能機(jī)器人平臺應(yīng)用在Jetbot小車中實踐。第五章對全文的總結(jié)，提出了本文的A*算法在matlab以及Jetbot小車取得的結(jié)果。路徑規(guī)劃A*算法的構(gòu)建及matlab實驗仿真算法構(gòu)建和分析A*算法是一種啟發(fā)式搜索算法，引入了最優(yōu)啟發(fā)式函數(shù)，可以在搜索過程中避免盲目性，其表達(dá)式如式①:f'(n)=g*(n)+h*(n)①其中路徑是由各個節(jié)點組成，可通過節(jié)點集合｛d1，d2，d3...dm...dn｝表示，dl表示起始節(jié)點，dn表示為當(dāng)前節(jié)點，dm表示目標(biāo)節(jié)點。f*（n）為代價函數(shù)，g*（n）是起始節(jié)點到當(dāng)前節(jié)點的最短距離，如果g*(n)=0，代價函數(shù)所表示的算法是最佳優(yōu)先搜索算法。h"*（n）是當(dāng)前節(jié)點到目標(biāo)節(jié)點的最短距離，如果h*(n)=0，代價函數(shù)所表示的算法是Dijkstra算法。目前是無法通過該節(jié)點預(yù)知該節(jié)點與目標(biāo)節(jié)點的最短距離，因此使用h（n）代替h*（n），因此替換過后的代價函數(shù)表達(dá)式如式②:f(n)=g(n)+h(n)②H（n）選取原則為h（n）的值不大于h*（n），通常使用歐式距離計算公式、曼哈頓距離計算公式和切比雪夫距離計算公式，其對應(yīng)表達(dá)式分別是公式③、④、⑤:③④⑤matlab程序的編寫A*算法在搜索中生產(chǎn)兩個表：“Open”表和“Close”表。Open表保存了所有已生成而未被檢查的節(jié)點，Close表中記錄已被檢查過的節(jié)點。算法中有一步是根據(jù)估計函數(shù)重排Open表中的節(jié)點，這樣，循環(huán)的每一步只考慮Open表中狀態(tài)最好（代價最?。┑墓?jié)點，如果被發(fā)現(xiàn)在Open表中存在同一個節(jié)點，就應(yīng)比較兩個節(jié)點代價的大小，如擴(kuò)展得到的新節(jié)點代價大于已有的節(jié)點代價，可以放棄擴(kuò)展得到的新節(jié)點，否則就以新節(jié)點代替原來的節(jié)點。如果在Close表中存在同一個節(jié)點號，則新生成的路徑的代價肯定大于原來路徑的代價，可以淘汰新生成的節(jié)點。主要實現(xiàn)功能：確定起始點、目標(biāo)點；定義一個地圖，并生成一些隨機(jī)障礙物；調(diào)用A算法函數(shù)，A函數(shù)返回值為路徑；繪制地圖、地圖障礙物、起點終點、路徑圖3.2.1A*算法流程圖圖中的O為Open表的縮寫，C為Close的縮寫為了讓大家更加直觀地容易理解該流程圖，下面我會舉個例子假如此時障礙物以及起點S，終點G已經(jīng)生成，現(xiàn)在我們將從S—G。如下圖3.2.2：圖3.2.2初始狀態(tài)：設(shè)置節(jié)點S的f(x)=S，節(jié)點S進(jìn)Open表，設(shè)置Close表為空。Open:S；Close：；估算節(jié)點S，考察與節(jié)點S相連的所有節(jié)點（有節(jié)點0，1）是否是目標(biāo)節(jié)點，如果不是則估算這些節(jié)點的f（x）值，將其放進(jìn)Open表，同時節(jié)點1被放入Close表；最后需要將open表中的節(jié)點按f（x）的值從小到大的順序排列。Open:1；Close：S；從Open表取出f（x）值最小的節(jié)點（節(jié)點1），考察與節(jié)點1相連的所有節(jié)點（節(jié)點2，3）是否是目標(biāo)節(jié)點，如果不是則估算這些節(jié)點的f（x）值，將其放進(jìn)Open表，同時節(jié)點1被放入Close表。最后需要將open表中的節(jié)點按f（x）的值從小到大的順序排列（由于節(jié)點2比節(jié)點4接近目標(biāo)節(jié)點，因此節(jié)點2的f（x）值小于節(jié)點4的f（x）值，節(jié)點2排在節(jié)點4的前面）。Open:2，3；Close：S，1；從Open表取出f（x）值最小的節(jié)點（節(jié)點2），考察與節(jié)點2相連的所有節(jié)點（節(jié)點4，5）是否是目標(biāo)節(jié)點，如果不是則估算這些節(jié)點的f（x）值，將其放進(jìn)Open表，同時節(jié)點2被放入Close表。將open表中的節(jié)點按f（x）的值從小到大的順序排列。Open:4，5；Close：S，1，2；從Open表取出f（x）值最小的節(jié)點（節(jié)點5），考察與節(jié)點5相連的所有節(jié)點（節(jié)點7）是否是目標(biāo)節(jié)點，如果不是則估算這些節(jié)點的f（x）值，將其放進(jìn)Open表，同時節(jié)點5被放入Close表。將open表中的節(jié)點按f（x）的值從小到大的順序排列。Open:8；Close：S，1，2，5；...依次類推直到：Open:G；Close：S，1，2，5，7，8，9，11，13；從Open表取出f（x）值最小的節(jié)點（節(jié)點4），考察與節(jié)點4相連的所有節(jié)點（節(jié)點5）是否是目標(biāo)節(jié)點，如果是則將節(jié)點4被放入Close表，然后將考察的節(jié)點（節(jié)點5）放入Close表，此時發(fā)現(xiàn)一條從開始地到目的地的路徑。Open:；Close：S，1，2，5，7，8，9，11，13，G；當(dāng)判斷到G已經(jīng)存在于Close表中后，將Close表中的點進(jìn)行排序并連接起來作為最佳的路徑規(guī)劃如下圖3.2.3：圖3.2.3偽代碼的編寫根據(jù)以上思路，A*算法的偽代碼如下：A*(){Open=[起始節(jié)點];Closed=[];while(Open表非空){從Open中取得一個節(jié)點X，并從OPEN表中刪除。if(X是目標(biāo)節(jié)點) { 求得路徑PATH； 返回路徑PATH； }for(每一個X的子節(jié)點Y){if(Y不在OPEN表和CLOSE表中) {求Y的估價值；并將Y插入OPEN表中；} //還沒有排序 elseif(Y在OPEN表中) {if(Y的估價值小于OPEN表的估價值){更新OPEN表中的估價值；}else//Y在CLOSE表中{ if(Y的估價值小于CLOSE表的估價值) {更新CLOSE表中的估價值；從CLOSE表中移出節(jié)點，并放入OPEN表中；}}}將X節(jié)點插入CLOSE表中；按照估價值將OPEN表中的節(jié)點排序；}//endfor}//endwhile}//endA*()仿真結(jié)果由于程序過長，接下來就不放完整程序貼圖了，完整的matlab程序放在最后的附錄。當(dāng)我們按章節(jié)3.2.2.中的偽代碼將完整的程序?qū)懗鰜磉\(yùn)行效果后如下圖：圖3.3.1程序運(yùn)行圖圖3.3.2matlab完整程序運(yùn)行結(jié)果1圖3.3.3matlab完整程序運(yùn)行結(jié)果2運(yùn)行結(jié)果中的黑色部分是我們隨機(jī)生成的障礙物，白色塊中的綠色點是我們隨機(jī)生成的起點S，紅色塊中的黃色小方塊是我們隨機(jī)生成的終點G，其他顏色的色塊是我們從起點到終點所走過的地方，越靠近終點紅色越深，其他接近紅色方塊的地方是我們從起點到終點最近的那個距離所能觸及的其他地方，起點與終點之間連接地線是我們仿真出來的最佳路徑。在本部分所涉及的全局變量是我們的起點S到終點G的大致規(guī)劃與方向，局部變量體現(xiàn)在每一個父節(jié)點選擇下一個子節(jié)點時的選擇從程序的運(yùn)行結(jié)果可驗證我們前面對A*算法的選擇以及通過matlab構(gòu)建仿真的方向以及成果的正確，在接下來的第四章我們將會把在matlab中所取得的A*經(jīng)驗放入Jetbot小車中進(jìn)行實體實驗仿真。路徑規(guī)劃A*算法在Jetbot小車的應(yīng)用與實驗A*算法第二、三章已經(jīng)介紹研究過，首先在環(huán)境信息已知的情況下利用路徑規(guī)劃A*算法獲得一條初始路徑，得到一條最優(yōu)的路徑。下面主要研究如何利用這A*算法在Jetbot小車上實現(xiàn)。路徑規(guī)劃算法設(shè)計首先，利用A*算法進(jìn)行全局的路徑規(guī)劃，此時可以獲得一條基于全局路徑規(guī)劃信息的初始路徑，之后采用激光雷達(dá)傳感器實時采集周圍的環(huán)境信息。局部路徑規(guī)劃主要在障礙物周圍進(jìn)行，也即是在路徑轉(zhuǎn)向點處進(jìn)行處理。在從起點到終點的路徑中如果出現(xiàn)障礙物，這時小車被阻擋，此時利用小車的轉(zhuǎn)向控制系統(tǒng)，也即是把障礙物模擬成一個轉(zhuǎn)向點，在這個點小車所要轉(zhuǎn)向角度控制便是局部路徑。接下來將局部路徑與全局路徑結(jié)合，最終得到一條最優(yōu)路徑。具體的實現(xiàn)步驟如下：圖4.1.1路徑規(guī)劃流程圖1.智能小車通過自身搭載的IMX219攝像頭采集室內(nèi)周圍環(huán)境，處理后利用柵格法建立環(huán)境地圖模型。2.在柵格地圖環(huán)境模型上確定起始點、目標(biāo)點以及各個障礙物的信息后利用改進(jìn)的A*算法進(jìn)行全局路徑規(guī)劃，獲得一條全局最優(yōu)的初始路徑。3.確定在這條初始路徑中處于障礙物附近的點，即初始路徑上的轉(zhuǎn)向點，記錄這些轉(zhuǎn)向點，將這些轉(zhuǎn)向點設(shè)為局部路徑規(guī)劃時的子目標(biāo)點。4.智能小車在進(jìn)行路徑規(guī)劃時，首先按照；初始路徑趨向目標(biāo)位置行進(jìn)，遇到障礙物，也即是需要轉(zhuǎn)向時，將上一步記錄的轉(zhuǎn)向點作為子目標(biāo)點。然后基于改進(jìn)人工勢場法進(jìn)；行局部路徑規(guī)劃，得到一條局部路徑。在此過程中如果到達(dá)子目標(biāo)點，則局部路徑規(guī)劃階段結(jié)束，否則繼續(xù)局部路徑規(guī)劃過程，直到到達(dá)子目標(biāo)點。5.在智能小車成功繞過障礙物以后便會回歸到初始路徑，將局部路徑與初始路徑連：接起來即是A*算法規(guī)劃出來的路徑。6.智能小車從起始點避開障礙物，依照A*算法路徑規(guī)劃出來的路線準(zhǔn)確、快速到達(dá)目標(biāo)點，則算法結(jié)束。A*路徑規(guī)劃算法流程如圖所表示。圖4.1.2A*路徑規(guī)劃算法流程圖JupyterLab軟件前面的算法研究主要集中在算法理論層面，而檢驗一個算法是否真實有效需要應(yīng)用到實際問題中去。因此，接下來主要進(jìn)行真實環(huán)境下Jetbot智能小車路徑規(guī)劃實驗來繼續(xù)本課題相關(guān)研究。要進(jìn)行真實環(huán)境下路徑規(guī)劃實驗首先要有一個好的軟件開發(fā)環(huán)境以及硬件平臺。對于初學(xué)階段的我們最適合使用JupyterLab交互式編程環(huán)境，所以我們的小車實際演示中將一直采用Jupyterlab進(jìn)行編程，我們可以在我們的電腦上通過web瀏覽器來訪問JupyterLab開發(fā)界面，但是我們的電腦必須要和Jetbot機(jī)器人處于同一網(wǎng)絡(luò)下，否則無法訪問，訪問方式為在瀏覽器上輸入JetbotIP.8888來進(jìn)行登錄，如果還沒有連接WIFI，采用的是無頭模式連接的話在瀏覽器上輸入:8888進(jìn)行登錄訪問，第一次登錄的時候可能需要我們輸入密碼，jetbot機(jī)器人的密碼為jetbot。輸入后點擊登錄進(jìn)行登錄。圖4.2.1JupyterLab開發(fā)界面程序編寫及小車的操作實驗障礙物的布置實驗場地為實訓(xùn)樓的419實驗室，在進(jìn)行實驗場景設(shè)置的時候，之所以選取419室內(nèi)主要是，我們平常就在419室有相對理想的以太網(wǎng)及局域網(wǎng)，對小車內(nèi)的編程、仿真環(huán)境較有利。并且相較于室外環(huán)境更為簡單一些，由于實驗室空間面積相對于室外比較小，且屬于密閉空間，為了進(jìn)行小車環(huán)境信息采集，建立環(huán)境地圖的時候，比較容易，也相對比較快，也易于建立封閉的地圖模型。我們布置了個70cm×100cm這樣在進(jìn)行路徑規(guī)劃實驗的時候可以減少建立地圖的時間。如圖4.3.1所示為真實實驗場景。圖4.3.1小車障礙物布置圖程序編寫流程設(shè)定攝像頭云臺的傾斜角度

在采集數(shù)據(jù)時，為了能夠采集到實質(zhì)的數(shù)據(jù)和保持和測試數(shù)據(jù)在同一個角度，保證我們訓(xùn)練的數(shù)據(jù)是有效的，我們需要設(shè)置一個統(tǒng)一的攝像頭傾斜角度。

from

jetbot

import

Robot

import

time

robot

=

Robot()

#

設(shè)置視覺云臺的傾斜角度

robot.makerobo_servo(0，90,1,70)#根據(jù)自己的實際角度設(shè)置

2.連接攝像頭

當(dāng)設(shè)置好傾斜角度之后，我們需要初始化攝像頭，并顯示所看到的畫面。

我們的神經(jīng)網(wǎng)絡(luò)采用224x224像素的圖像作為輸入。因此我們將攝像頭設(shè)置為該大小，以最小化文件大小，而最小化數(shù)據(jù)集。（我們已經(jīng)通過測試此像素適用于此任務(wù)）在某些情況下，收集數(shù)據(jù)時最好用較大的圖像尺寸，然后做處理的時候縮小到需要的大小。

import

traitlets

import

ipywidgets.widgets

as

widgets

from

IPython.display

import

display

from

jetbot

import

Camera,

bgr8_to_jpeg

camera

=

Camera.instance(width=224,

height=224)?

image

=

widgets.Image(format='jpeg',

width=224,

height=224)

#

這個寬度和高度不一定必須與相機(jī)匹配

camera_link

=

traitlets.dlink((camera,

'value'),

(image,

'value'),

transform=bgr8_to_jpeg)?

display(image)

Error

displaying

widget:

model

not

found

運(yùn)行完上面的代碼塊后，就可以實時地看到攝像頭拍攝到的畫面。

3.創(chuàng)建數(shù)據(jù)采集文件夾

接下來讓我們創(chuàng)建一些目錄存儲數(shù)據(jù)。我們將會建立一個叫dataset的文件夾，里面有兩個子文件夾，分別是free和blocked，用于分類放置每一個場景的圖片。

import

os

blocked_dir

=

'dataset/blocked'

free_dir

=

'dataset/free'?

#我們有這個“try/except”語句，因為如果目錄已經(jīng)存在，這些下一個函數(shù)可能會引發(fā)錯誤

try:

os.makedirs(free_dir)

os.makedirs(blocked_dir)

except

FileExistsError:

print('Directories

not

created

becasue

they

already

exist')

Directories

not

created

becasue

they

already

exist

運(yùn)行完上面的代碼塊后，你現(xiàn)在刷新左側(cè)的Jupyter文件瀏覽器，你應(yīng)該會見到這些目錄。

4.創(chuàng)建采集數(shù)據(jù)按鈕

接下來，我們將創(chuàng)建一些按鈕用來保存不同標(biāo)簽的快照。我們還將創(chuàng)建一些文本框，用于顯示到目前位置我們每個標(biāo)簽收集到的圖像數(shù)量。這很重要，因為我們要確保采集到的free圖像要和blocked圖像一樣多。還有助于我們了解整體收集了多少圖像。

button_layout

=

widgets.Layout(width='128px',

height='64px')

free_button

=

widgets.Button(description='add

free',

button_style='success',

layout=button_layout)

blocked_button

=

widgets.Button(description='add

blocked',

button_style='danger',

layout=button_layout)

free_count

=

widgets.IntText(layout=button_layout,

value=len(os.listdir(free_dir)))

blocked_count

=

widgets.IntText(layout=button_layout,

value=len(os.listdir(blocked_dir)))?

display(widgets.HBox([free_count,

free_button]))

display(widgets.HBox([blocked_count,

blocked_button]))

Error

displaying

widget:

model

not

found

Error

displaying

widget:

model

not

found

到此為止，那些按鈕是不會做任何事的。我們需要把保存圖像的函數(shù)附加到每一個按鈕的on_click事件中。我們會通過Image部件，把這些經(jīng)過壓縮處理的JPEG格式圖像保存到對應(yīng)的分類文件夾里。

為了確保我們不重復(fù)的任何文件名，我們將用python中的uuid包，它定義了uuid1方法從當(dāng)前時間和機(jī)器的MAC地址生成唯一標(biāo)識符。

from

uuid

import

uuid1?

def

save_snapshot(directory):

image_path

=

os.path.join(directory,

str(uuid1())

+

'.jpg')

with

open(image_path,

'wb')

as

f:

f.write(image.value)?

def

save_free():

global

free_dir,

free_count

save_snapshot(free_dir)

free_count.value

=

len(os.listdir(free_dir))

def

save_blocked():

global

blocked_dir,

blocked_count

save_snapshot(blocked_dir)

blocked_count.value

=

len(os.listdir(blocked_dir))

#在回調(diào)函數(shù)中使用了lambda函數(shù)，以忽略on_click事件傳遞參數(shù)

free_button.on_click(lambda

x:

save_free())

blocked_button.on_click(lambda

x:

save_blocked())

現(xiàn)在上面的按鈕應(yīng)該可以把圖片保存在free或者blocked文件夾里。你可以使用Jupyter

Lab的文件瀏覽器去查看這些文件。

現(xiàn)在開始動手收集一些數(shù)據(jù)1.把

JetBot

放在阻擋的情況下，并按下

add

blocked

按鈕

2.把

JetBot

放在通暢的情況下，并按下

add

free

按鈕

重復(fù)

1，2

（提示：我們可以把那些按鈕部件輸出在新的窗口，這樣方便操作。我們也將執(zhí)行下面的代碼把它們顯示在一起。）

以下是一些數(shù)據(jù)標(biāo)記的提示

嘗試不同方向、嘗試不同的照明、嘗試不同的對象/碰撞類型：例如墻壁，物體等、嘗試不同紋理的平面/物體：例如圖案，不同光滑度，玻璃等

最終，JetBot在現(xiàn)實世界中越多的場景數(shù)據(jù)其防撞行為就越好，所以得到各種各樣的數(shù)據(jù)很重要，而不僅僅是大量的數(shù)據(jù)?？赡苊總€分類都需要100個圖像（這不一定是一個科學(xué)的做法，僅僅是一個有用的提示）。收集這么多數(shù)據(jù)其實不用擔(dān)心，當(dāng)你開始收集的時候，就會變得很快完成。

display(image)

display(widgets.HBox([free_count,

free_button]))

display(widgets.HBox([blocked_count,

blocked_button]))

Error

displaying

widget:

model

not

found

Error

displaying

widget:

model

not

found

Error

displaying

widget:

model

not

found

注意：如果是在JetsonNano上直接訓(xùn)練數(shù)據(jù)，下面壓縮文件我們可以不用執(zhí)行下一步。當(dāng)我們收集足夠的數(shù)據(jù)的時候，我們需要把這些數(shù)據(jù)復(fù)制到我們的GPU平臺上進(jìn)行訓(xùn)練。首先，我們可以調(diào)用terminal*（命令行模式又或者叫終端）命令，進(jìn)行數(shù)據(jù)打包壓縮成一個‘.zip’文件。

！表示我們要將使用shell命令運(yùn)行－r表示包含所有包含子文件夾文件。-q表示zip命令不輸出任何信息

!zip-r-qdataset.zipdataset

我們應(yīng)該在Jupyter

Lab文件瀏覽器中看到名為dataset.zip的文件，右鍵點擊該文件進(jìn)行下載操作。

接下來，我們需要把這些數(shù)據(jù)上傳到我們的GPU平臺或者云計算機(jī)（我們將其稱為host主機(jī)）來訓(xùn)練我們的防撞神經(jīng)網(wǎng)絡(luò)。訓(xùn)練好模型之后，我們將返回到機(jī)器人JupyterLab環(huán)境，以使用該模型進(jìn)行實時演示！圖4.3.2圖4.3.3圖4.3.4圖4.3.5圖4.3.6圖4.3.7（圖4.3.2—圖4.3.7為程序仿真動作過程時的轉(zhuǎn)向抓拍照片）總結(jié)本文主要基于jetbot智能小車路徑規(guī)劃的A*算法。首先通過各種路徑規(guī)劃算法的優(yōu)劣對比選擇了全局路徑規(guī)劃和局部路徑規(guī)劃針對兩種算法各自存在的不足之處，分別進(jìn)行了一定的改進(jìn)A*算法，并進(jìn)行matlab仿真驗證。同時在jetbot智能小車硬件平臺上進(jìn)行了真實環(huán)境下的路徑規(guī)劃實際應(yīng)用，效果較好。第一章為文章的緒論部分，主要介紹了本文小車的研究背景和意義，討論了路徑規(guī)劃的國內(nèi)外研究現(xiàn)狀并對提到了相應(yīng)的關(guān)鍵技術(shù)（全局路徑規(guī)劃與局部路徑規(guī)劃）。第二章為全局路徑規(guī)劃、局部路徑規(guī)劃進(jìn)行了初步分析且詳細(xì)說明了路徑規(guī)劃的常用方法和分類，以及各種算法之間的優(yōu)缺點。并對于這兩種規(guī)劃采取了較為中間的選取柵型構(gòu)圖下的A*算法最短路徑規(guī)劃的原理和具體實現(xiàn)方法。第三章即開始對于前面提出的選取柵型構(gòu)圖下的A*算法用matlab進(jìn)行實驗仿真與驗證。第四章從我們在matlab中完整的模擬仿真結(jié)果中的A*算法方式通過JetbotAI智能機(jī)器人平臺應(yīng)用在Jetbot小車中實踐。第五章對全文的總結(jié)，提出了本文的A*算法在matlab以及Jetbot小車取得的結(jié)果，說明了我們的理論不僅在理論上有效，在實際Jetbot小車試驗上也有效。隨著三個多月畢業(yè)設(shè)計的結(jié)束，在大學(xué)的時光也接近了尾聲，我的畢業(yè)設(shè)計也終究完成了。在沒有做畢業(yè)設(shè)計前我覺得畢業(yè)設(shè)計僅僅是對這三年來所學(xué)知識的簡單總結(jié)，但是通過這次做畢業(yè)設(shè)計發(fā)現(xiàn)自我的看法有點太片面了。畢業(yè)設(shè)計不僅僅是對以往知識的檢驗，更是對自身潛能的提升。通過這次畢業(yè)設(shè)計使我明白了我原先的知識還比較欠缺，我需要學(xué)習(xí)的東西還有很多。鳴謝在此要感謝我的指導(dǎo)老師劉運(yùn)松老師對我的悉心指導(dǎo)和幫助，本文的理論及仿真試驗工作是在我的導(dǎo)師劉運(yùn)送老師的精心指導(dǎo)和照顧下完成的。從開篇緒論到總結(jié)，我學(xué)到了很多知識，經(jīng)歷了很多困難，但通過查閱大量相關(guān)資料，與同學(xué)、老師咨詢交流及自學(xué)也收獲很大。我所取得的每一個進(jìn)展和我編寫的每一個程序都致力于導(dǎo)師的辛勤工作和艱苦的努力。劉運(yùn)送老師嚴(yán)謹(jǐn)?shù)膶W(xué)術(shù)態(tài)度，對各個學(xué)科的深刻了解以及無私的奉獻(xiàn)精神給我?guī)砹松钌畹膯l(fā)。從我受人尊敬的導(dǎo)師那里，我不僅學(xué)到了扎實而廣泛的專業(yè)知識，而且學(xué)到了做人的真相。在以后的學(xué)習(xí)和工作中，我會牢記導(dǎo)師的指導(dǎo)和鼓勵，并盡最大努力取得更好的成績。在此，我要再次對導(dǎo)老師表示衷心的感謝和深切的敬意！并且在大學(xué)的三年學(xué)習(xí)中，每位老師都對我的學(xué)習(xí)，生活和學(xué)習(xí)給予了熱烈的關(guān)懷和幫助，這大大提高了我的水平，取得了長足的進(jìn)步。在此，我向所有關(guān)心和幫助我的老師，同學(xué)和朋友們表示衷心的感謝！最后，我要衷心感謝所有忙于審閱論文并參加答辯的老師、專家和教授。

參考文獻(xiàn)[1]馬靜；王佳斌；張雪.A*算法在無人車路徑規(guī)劃中的應(yīng)用

計算機(jī)技術(shù)與發(fā)展2016年第11期[2]“每天分享一點冷知識”.5個“無人駕駛汽車”的發(fā)展歷史，從無線電控制到車載傳感器網(wǎng)頁2020.[3]上海交通大學(xué)電子信息與電氣工程學(xué)院.畢業(yè)設(shè)計（論文－無人駕駛車輛路徑規(guī)劃方法研究與仿真

2019.[4]張廣林；胡小梅；柴劍飛；趙磊；俞濤.路徑規(guī)劃算法及其應(yīng)用綜述現(xiàn)代機(jī)械2011年第5期[5]趙德群；段建英；陳鵬宇；蘇晉海.基于A*算法的三維地圖最優(yōu)路徑規(guī)劃北京工業(yè)大學(xué)信息學(xué)部，北京2019.[6]ape_spring.A*searchalgorithm淺談CSDN博客2016.[7]宣仁虎.基于改進(jìn)A_算法和人工勢場法智能小車路徑規(guī)劃研究西安電子科技大學(xué)2020.

附錄matlab完整程序functionastardemon=20;%生成一個20*20的界面wallpercent=0.45;%45%的界面作為阻礙物（墻）[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent);%初始化界面setOpen=[startposind];setOpenCosts=[0];setOpenHeuristics=[Inf];setClosed=[];setClosedCosts=[];movementdirections={'R','L','D','U'};counterIterations=1;axishandle=createFigure(field,costchart,startposind,goalposind);while~max(ismember(setOpen,goalposind))&&~isempty(setOpen)%返回與A同大小的矩陣，其中元素1表示A中相應(yīng)位置的元素在B中也出現(xiàn)，0則是沒有出現(xiàn)[temp,ii]=min(setOpenCosts+setOpenHeuristics);%從OPEN表中選擇花費(fèi)最低的點temp,ii是其下標(biāo)(也就是標(biāo)號索引)[costs,heuristics,posinds]=findFValue(setOpen(ii),setOpenCosts(ii),...field,goalposind,'euclidean');%擴(kuò)展temp的四個方向點，獲得其坐標(biāo)posinds，各個方向點的實際代價costs，啟發(fā)代價heuristicssetClosed=[setClosed;setOpen(ii)];%將temp插入CLOSE表中setClosedCosts=[setClosedCosts;setOpenCosts(ii)];%將temp的花費(fèi)計入ClosedCosts%更新OPEN表分為三種情況if(ii>1&&ii<length(setOpen))%temp在OPEN表的中間，刪除tempsetOpen=[setOpen(1:ii-1);setOpen(ii+1:end)];setOpenCosts=[setOpenCosts(1:ii-1);setOpenCosts(ii+1:end)];setOpenHeuristics=[setOpenHeuristics(1:ii-1);setOpenHeuristics(ii+1:end)];elseif(ii==1)setOpen=setOpen(2:end);%temp是OPEN表的第一個元素，刪除tempsetOpenCosts=setOpenCosts(2:end);setOpenHeuristics=setOpenHeuristics(2:end);else%temp是OPEN表的最后一個元素，刪除tempsetOpen=setOpen(1:end-1);setOpenCosts=setOpenCosts(1:end-1);setOpenHeuristics=setOpenHeuristics(1:end-1);endforjj=1:length(posinds)%對于擴(kuò)展的四個方向的坐標(biāo)if~isinf(costs(jj))%如果此點的實際代價不為Inf，也就是沒有遇到墻if~max([setClosed;setOpen]==posinds(jj))%如果此點不在OPEN表和CLOSE表中fieldpointers(posinds(jj))=movementdirections(jj);%將此點的方向存在對應(yīng)的fieldpointers中costchart(posinds(jj))=costs(jj);%將實際代價值存入對應(yīng)的costchart中setOpen=[setOpen;posinds(jj)];%將此點加入OPEN表中setOpenCosts=[setOpenCosts;costs(jj)];%更新OPEN表實際代價setOpenHeuristics=[setOpenHeuristics;heuristics(jj)];%更新OPEN表啟發(fā)代價elseifmax(setOpen==posinds(jj))%如果此點在OPEN表中I=find(setOpen==posinds(jj));%找到此點在OPEN表中的位置ifsetOpenCosts(I)>costs(jj)%如果在OPEN表中的此點實際代價比現(xiàn)在所得的大costchart(setOpen(I))=costs(jj);%將當(dāng)前的代價存入costchart中，注意此點在costchart中的坐標(biāo)與其自身坐標(biāo)是一致的（setOpen(I)其實就是posinds(jj)），下同fieldpointerssetOpenCosts(I)=costs(jj);%更新OPEN表中的此點代價，注意此點在setOpenCosts中的坐標(biāo)與在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics(I)=heuristics(jj);%更新OPEN表中的此點啟發(fā)代價fieldpointers(setOpen(I))=movementdirections(jj);%更新此點的方向endelse%如果此點在CLOSE表中，說明已經(jīng)擴(kuò)展過此點I=find(setClosed==posinds(jj));ifsetClosedCosts(I)>costs(jj)%如果在CLOSE表中的此點實際代價比現(xiàn)在所得的大costchart(setClosed(I))=costs(jj);%將當(dāng)前的代價存入costchart中setClosedCosts(I)=costs(jj);%更新CLOSE表中的此點代價fieldpointers(setClosed(I))=movementdirections(jj);%更新此點的方向endendendendfunctionp=findWayBack(goalposind,fieldpointers)n=length(fieldpointers);%場的長度posind=goalposind;[py,px]=ind2sub([n,n],posind);%storeinitialpositionp=[pypx];while~strcmp(fieldpointers{posind},'S')%當(dāng)查詢到的點不是'S'起點時switchfieldpointers{posind}case'L' %如果獲得該點的來源點方向為左時px=px-1;case'R'%如果獲得該點的來源點方向為右時px=px+1;case'U'%如果獲得該點的來源點方向為上時py=py-1;case'D'%如果獲得該點的來源點方向為前時py=py+1;endp=[p;pypx];posind=sub2ind([nn],py,px);endfunction[cost,heuristic,posinds]=findFValue(posind,costsofar,field,...goalind,heuristicmethod)n=length(field);%場的長度[currentpos(1)currentpos(2)]=ind2sub([nn],posind);%獲得當(dāng)前點的行列坐標(biāo)，注意currentpos(1)是列坐標(biāo)，currentpos(2)是行坐標(biāo)[goalpos(1)goalpos(2)]=ind2sub([nn],goalind);%獲得目標(biāo)點的行列坐標(biāo)cost=Inf*ones(4,1);heuristic=Inf*ones(4,1);pos=ones(4,2);%向左查詢，那么就是從右邊來newx=currentpos(2)-1;newy=currentpos(1);ifnewx>0%如果沒有到邊界pos(1,:)=[newynewx];%獲得新的坐標(biāo)switchlower(heuristicmethod)case'euclidean'heuristic(1)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);%heuristic(1)為啟發(fā)函數(shù)計算的距離代價case'taxicab'heuristic(1)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(1)=costsofar+field(newy,newx);%costsofar為之前花費(fèi)的代價，field(newy,newx)為環(huán)境威脅代價，cost(1)為經(jīng)過此方向點的真實代價endnewx=currentpos(2)+1;newy=currentpos(1);ifnewx<=npos(2,:)=[newynewx];switchlower(heuristicmethod)case'euclidean'heuristic(2)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case'taxicab'heuristic(2)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(2)=costsofar+field(newy,newx);endnewx=currentpos(2);newy=currentpos(1)-1;ifnewy>0pos(3,:)=[newynewx];switchlower(heuristicmethod)case'euclidean'heuristic(3)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case'taxicab'heuristic(3)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(3)=costsofar+field(newy,newx);endnewx=currentpos(2);newy=currentpos(1)+1;ifnewy<=npos(4,:)=[newynewx];switchlower(heuristicmethod)case'euclidean'heuristic(4)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case'taxicab'heuristic(4)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(4)=costsofar+field(newy,newx);endposinds=sub2ind([nn],pos(:,1),pos(:,2));%posinds為此點擴(kuò)展的四個方向上的坐標(biāo)function[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent)%Thisfunctionwillcreateafieldwithmovementcostsandwalls,astart%andgoalpositionatrandom,amatrixinwhichthealgorithmwillstore%fvalues,andacellmatrixinwhichitwillstorepointers%createthefieldandplacewallswithinfinitecost初始化界面和墻field=ones(n,n)+10*rand(n,n);