-PAGEIII-基于遺傳算法的四輪移動(dòng)小車全局路徑規(guī)劃實(shí)證研究摘要發(fā)展科技就是提高社會(huì)生產(chǎn)力的大環(huán)境下，各個(gè)國家都在大力發(fā)展智能化技術(shù)，我國在智能小車技術(shù)研究方面扮演著越來越重要的角色。而在過去，人們對于車輛的研究僅限于自主式移動(dòng)，從而導(dǎo)致了小車在現(xiàn)實(shí)生活中無法完成更多功能的任務(wù)，使用范圍受限。為此，對路徑規(guī)劃算法進(jìn)行研究，使小車不僅能避開障礙物，還能沿著提前預(yù)定好的路線行走。首先，利用光電編碼器對小車運(yùn)動(dòng)學(xué)進(jìn)行分析與研究，分別研究光束模型和似然場模型兩種激光雷達(dá)模型，說明小車坐標(biāo)系和世界坐標(biāo)系的轉(zhuǎn)換關(guān)系，闡述柵格地圖、拓?fù)涞貓D和特征地圖的利弊。然后，小車系統(tǒng)由硬件和軟件組成。硬件設(shè)計(jì)包括零部件的選取和四輪移動(dòng)小車試驗(yàn)平臺的搭建。軟件設(shè)計(jì)主要通過ROS節(jié)點(diǎn)傳送的信息實(shí)現(xiàn)位置控制，并且應(yīng)用ROS集成體系架構(gòu)，服務(wù)器端節(jié)點(diǎn)RVIZ，分布式ROS構(gòu)架和主控制器端節(jié)點(diǎn)完成軟件系統(tǒng)的設(shè)計(jì)。利用PID控制算法實(shí)現(xiàn)小車系統(tǒng)功能的可控性。最后，從全局路徑規(guī)劃和局部路徑規(guī)劃兩個(gè)方面對小車路徑規(guī)劃進(jìn)行研究。對整體路徑規(guī)劃仿真算法Dijkstra算法和A*算法分別進(jìn)行了綜合比較，分析了基于遺傳算法的小車全局路徑規(guī)劃。結(jié)合動(dòng)態(tài)窗口法對小車進(jìn)行局部路徑規(guī)劃研究。在ROS操作系統(tǒng)的控制下完成路徑規(guī)劃試驗(yàn)。關(guān)鍵詞:四輪移動(dòng)小車；激光雷達(dá)；系統(tǒng)設(shè)計(jì)；路徑規(guī)劃；目錄摘要 IAbstract II目錄 III第一章緒論 11.1研究背景及意義 11.2研究現(xiàn)狀及分析 11.3論文的主要內(nèi)容及結(jié)構(gòu) 3第二章四輪移動(dòng)小車運(yùn)動(dòng)學(xué)分析及環(huán)境建模 42.1四輪移動(dòng)小車運(yùn)動(dòng)學(xué)模型 42.2激光雷達(dá)模型 52.2.1激光雷達(dá)測距原理 52.2.2激光雷達(dá)測距數(shù)學(xué)模型 62.3四輪移動(dòng)小車的坐標(biāo)系統(tǒng) 92.4地圖模型 102.4.1柵格地圖 102.4.2拓?fù)涞貓D 112.4.3特征地圖 11第三章四輪移動(dòng)小車系統(tǒng)設(shè)計(jì) 133.1硬件設(shè)計(jì) 133.2軟件設(shè)計(jì) 153.3位置控制程序設(shè)計(jì) 16第四章四輪移動(dòng)小車路徑規(guī)劃研究及試驗(yàn) 174.1全局路徑規(guī)劃研究 174.1.1Dijkstra算法與A*算法 174.1.2基于遺傳算法的小車全局路徑規(guī)劃 204.2局部路徑規(guī)劃研究 234.3路徑規(guī)劃算法試驗(yàn) 26結(jié)論 28第一章緒論1.1研究背景及意義在科學(xué)技術(shù)高速發(fā)展的背景下，智能機(jī)器人在生活中應(yīng)用越來越廣泛，比如兒童游戲區(qū)和各種游樂場所的多功能自動(dòng)化玩具、企業(yè)生產(chǎn)加工流水線上完成指定任務(wù)的機(jī)器人、危險(xiǎn)環(huán)境下的工程搶修機(jī)器人等。由此可見，機(jī)器人事業(yè)的興盛在當(dāng)今社會(huì)中的影響愈來愈重要。智能交通小車是一種新型交通系統(tǒng)，運(yùn)用導(dǎo)航、計(jì)算機(jī)、傳感、自動(dòng)導(dǎo)航技術(shù)實(shí)現(xiàn)自動(dòng)控制行駛、感知周圍環(huán)境信息、路徑規(guī)劃等多種功能。移動(dòng)小車導(dǎo)航需要小車有準(zhǔn)確快速感知周圍環(huán)境信息和實(shí)現(xiàn)自主穩(wěn)定移動(dòng)的處理功能，其核心技術(shù)特點(diǎn)在于即時(shí)終點(diǎn)定位與移動(dòng)地圖系統(tǒng)構(gòu)建（SimultaneousLocalizationAndMapping，SLAM）和及時(shí)自主移動(dòng)導(dǎo)航（AutonomousNavigation）[1]，自主移動(dòng)導(dǎo)航的關(guān)鍵技術(shù)是可行路徑路線規(guī)劃。路徑規(guī)劃需要傳感器對環(huán)境信息的感知，小車傳感器可分為內(nèi)部傳感器和外部傳感器，外部傳感器主要有距離傳感器、感覺傳感器,內(nèi)部傳感器主要有速度和加速度傳感器、位置傳感器等[2]。目前，智能移動(dòng)小車的廣泛應(yīng)用仍受到制造成本的嚴(yán)重制約，小車不能使用谷歌無人車[3]中所使用的高精度成像雷達(dá)。然而，單線程的激光雷達(dá)掃描成像頻率相對較低，但測量精度仍然很高。路徑規(guī)劃技術(shù)的應(yīng)用領(lǐng)域較為廣泛，雖然攝像機(jī)激光傳感器能夠帶來的室內(nèi)環(huán)境相關(guān)信息比較豐富，但室內(nèi)地圖不是太適合幫助小車在多個(gè)室內(nèi)環(huán)境下同時(shí)進(jìn)行路徑規(guī)劃。激光雷達(dá)的測距結(jié)果精度較高，這樣會(huì)更好的幫助小車對室內(nèi)環(huán)境相關(guān)信息的正確分析。本課題的研究意義主要在于實(shí)現(xiàn)一個(gè)基于移動(dòng)小車系統(tǒng)設(shè)計(jì)和路徑規(guī)劃的綜合研究系統(tǒng)。搭建小車硬件系統(tǒng)，利用ROS控制系統(tǒng)實(shí)現(xiàn)小車運(yùn)動(dòng)的可控化。從全局路徑規(guī)劃和局部路徑規(guī)劃的兩個(gè)方向完成小車路徑規(guī)劃研究。通過系統(tǒng)仿真和模擬試驗(yàn)，驗(yàn)證了該研究系統(tǒng)是否能夠在已知地圖的情況下進(jìn)行路徑規(guī)劃。1.2研究現(xiàn)狀及分析小車行駛路徑性能規(guī)劃解決問題主要是根據(jù)一定的小車性能指標(biāo)，從初始小車狀態(tài)發(fā)展到目標(biāo)小車狀態(tài)，尋找最優(yōu)或性能接近最優(yōu)的一種無碰撞小車路徑的解決方法[4]。小車路徑規(guī)劃能使汽車避開外界障礙物，最大限度地減小對汽車傳感器感應(yīng)范圍的影響。根據(jù)對掃描到的環(huán)境信息掌握程度，可以劃分為一個(gè)全局路徑規(guī)劃和一個(gè)局部路徑規(guī)劃[5]。對于全局路徑規(guī)劃算法，最常用的方法有Dijkstra算法[6]、A*算法[7-8]、D*算法[9]等，其中A*算法在目前因?yàn)槠浜唵?、成熟而適用范圍最廣。然而，傳統(tǒng)的路徑規(guī)劃搜索方法沒有直接考慮一個(gè)機(jī)器人網(wǎng)絡(luò)本體的路徑規(guī)模，而是直接采用一種啟發(fā)式方法搜索最優(yōu)化的路徑，在大規(guī)模范圍的網(wǎng)絡(luò)環(huán)境下，由于各個(gè)節(jié)點(diǎn)路徑數(shù)量的不斷增加，會(huì)直接導(dǎo)致實(shí)時(shí)性能比較差。Dijkstra這個(gè)算法可以完全保證在大規(guī)模范圍的地圖環(huán)境中能找到一條最優(yōu)化的路徑，但由于該節(jié)點(diǎn)算法同時(shí)需要遍歷地圖網(wǎng)絡(luò)中的所有節(jié)點(diǎn)網(wǎng)格，因此不能完全適用于復(fù)雜的大范圍規(guī)模地圖環(huán)境。為了大大提高A*存儲(chǔ)算法的強(qiáng)大實(shí)時(shí)計(jì)算性能，Peng等人[10]為了有效避免遍歷多個(gè)OPEN表和CLOSED表的大量時(shí)間成本開銷，通過一次次的操作可以直接訪問指定的一個(gè)A*存儲(chǔ)陣列，提高了操作效率。王紅衛(wèi)等人[11]分析指出，為了能夠使之前A*地圖算法重新規(guī)劃的拐點(diǎn)路徑更加平滑，人們在之前地圖信息中重新建立的拐點(diǎn)路徑中可以判斷三個(gè)拐點(diǎn)相鄰地圖節(jié)點(diǎn)之間的主要障礙物位置信息，并通過不斷減少相鄰節(jié)點(diǎn)間的信息數(shù)量來平滑算法規(guī)劃的拐點(diǎn)路徑，但由于拐點(diǎn)的信息存在仍然可能會(huì)嚴(yán)重限制機(jī)器人完成各種姿態(tài)上的轉(zhuǎn)換。Guruji等人[12]確定了一個(gè)在碰撞前的啟發(fā)式運(yùn)算函數(shù)，加快了處理時(shí)間。為了有效實(shí)現(xiàn)復(fù)雜場地環(huán)境下的無人實(shí)時(shí)移動(dòng)路徑路線規(guī)劃，王維等人[13]基于A*的算法，考慮到復(fù)雜場地環(huán)境和多個(gè)障礙物，對父節(jié)點(diǎn)的實(shí)時(shí)代價(jià)計(jì)算函數(shù)的值進(jìn)行一個(gè)指數(shù)級的加權(quán)，使移動(dòng)機(jī)器人快速準(zhǔn)確的接近目標(biāo)節(jié)點(diǎn)。傳統(tǒng)的分析算法往往存在著實(shí)時(shí)性差等諸多缺點(diǎn)。基于遺傳算法的搜索路徑優(yōu)化規(guī)劃算法采用傳統(tǒng)概率導(dǎo)數(shù)優(yōu)化計(jì)算方法，直接對結(jié)構(gòu)中的對象節(jié)點(diǎn)進(jìn)行導(dǎo)數(shù)操作，無需過分考慮導(dǎo)數(shù)值的運(yùn)算和取值函數(shù)的邏輯連續(xù)性，通過進(jìn)行全局集合搜索和對空間節(jié)點(diǎn)區(qū)域自身的適應(yīng)可以調(diào)整最優(yōu)搜索路徑方向，Holland等人[14]首先將遺傳算法廣泛應(yīng)用于現(xiàn)代機(jī)器人最優(yōu)搜索路徑的規(guī)劃選擇，將現(xiàn)在機(jī)器人最優(yōu)路徑規(guī)劃中的每個(gè)空間節(jié)點(diǎn)可以看作一個(gè)群體關(guān)系中的一個(gè)原始個(gè)體，并在遺傳算法中通過反向交叉和集合變異兩種操作可以完成迭代，最終通過一個(gè)集合全局搜索空間區(qū)域得到最優(yōu)搜索路徑。由于外界環(huán)境大多數(shù)都是相對動(dòng)態(tài)的，小車通常需要快速避開一些已知地圖中不可能存在的局部障礙物，因此我們需要定期進(jìn)行一個(gè)局部避障路徑的規(guī)劃工作來快速完成整個(gè)小車的避障。局部空間路徑移動(dòng)規(guī)劃主要算法包括人工動(dòng)態(tài)窗口群算法（DWA）、人工移動(dòng)勢場法、蟻群耦合算法、粒子群耦合算法等[15]，人工勢場的算法通過一個(gè)虛擬吸引力場對移動(dòng)小車產(chǎn)生吸引力，目標(biāo)點(diǎn)為重力，障礙物為斥力。通過計(jì)算小車所受力的合力來驅(qū)動(dòng)小車運(yùn)動(dòng)，便于使用小車進(jìn)行底層自動(dòng)控制管理系統(tǒng)的實(shí)時(shí)自動(dòng)控制。但它同時(shí)具有速度局部偏離極值的特點(diǎn)，在狹窄的目標(biāo)通道中容易快速擺動(dòng)，在發(fā)現(xiàn)目標(biāo)鎖定點(diǎn)附近等處有許多障礙物時(shí)無法快速找到正確路徑[16]。動(dòng)態(tài)窗口設(shè)計(jì)算法于1997年6月首次被提出。將基于局部避障優(yōu)化問題這一描述定義為基于速度空間的局部優(yōu)化避障問題。該替代算法在選擇滿足所有速度限制約束的矢量條件下，通過迭加替代可以選擇最優(yōu)化的速度約束矢量。該矢量算法已經(jīng)充分考慮了局部速度路徑約束的問題，在局部環(huán)境速度約束的條件下可以選擇最優(yōu)化的速度路徑矢量，非常好的適合于進(jìn)行局部高速路徑矢量規(guī)劃[17]。1.3論文的主要內(nèi)容及結(jié)構(gòu)主要研究基于激光雷達(dá)的四輪移動(dòng)小車系統(tǒng)設(shè)計(jì)與路徑規(guī)劃技術(shù)。首先完成四輪移動(dòng)小車系統(tǒng)設(shè)計(jì)，然后研究了在已知地圖條件下四輪移動(dòng)小車的路徑規(guī)劃問題，最后通過仿真和實(shí)際測試，實(shí)現(xiàn)了四輪移動(dòng)小車的路徑規(guī)劃。第一章分析了研究背景與意義，如今小車在各行各業(yè)中的應(yīng)用，并從小車的系統(tǒng)設(shè)計(jì)、路徑規(guī)劃算法兩個(gè)方面的研究現(xiàn)狀進(jìn)行說明，最后說明文章主要內(nèi)容與結(jié)構(gòu)。第二章研究了小車運(yùn)動(dòng)學(xué)和環(huán)境建模。首先對運(yùn)動(dòng)學(xué)中的傳感器的組成、原理和相關(guān)計(jì)算公式進(jìn)行說明。然后研究了激光雷達(dá)的測距原理，比較分析了波束模型和似然場模型存在的問題和解決方法。最后分析了小車坐標(biāo)系的轉(zhuǎn)換和三種地圖模型的優(yōu)缺點(diǎn)，重點(diǎn)分析了本文采用的柵格地圖。第三章詳細(xì)研究了四輪小車的系統(tǒng)整體設(shè)計(jì)。從系統(tǒng)硬件的設(shè)計(jì)、軟件的設(shè)計(jì)、位置控制設(shè)計(jì)三方面具體實(shí)現(xiàn)小車系統(tǒng)的設(shè)計(jì)。第四章主要研究了四輪移動(dòng)小車路徑規(guī)劃的設(shè)計(jì)和試驗(yàn)。其中，路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃。第五章主要是課題總結(jié)和未來展望，總結(jié)了這篇文章的重點(diǎn)研究課題內(nèi)容和未來發(fā)展規(guī)劃中一些目前有待深入研究而不能解決的技術(shù)問題。

第二章四輪移動(dòng)小車運(yùn)動(dòng)學(xué)分析及環(huán)境建模如今汽車的很多高端功能都依賴于傳感器的感知能力，就比如汽車的自動(dòng)導(dǎo)航、車自身溫度的檢測、時(shí)速表等信息的顯示都離不開傳感器的作用。傳感器在小車行駛過程中就像是人的大腦一樣，賦予了汽車視覺、觸覺、力的感知等多種功能，實(shí)現(xiàn)智能化處理。每個(gè)功能的實(shí)現(xiàn)都源自于傳感器，受傳感器采集到的數(shù)據(jù)所控制。對小車的運(yùn)動(dòng)學(xué)進(jìn)行分析有助于我們把握小車的運(yùn)動(dòng)狀態(tài)，激光雷達(dá)技術(shù)可用于探測距離、方位等有效信息，研究小車坐標(biāo)系來描述小車運(yùn)動(dòng)情況，地圖的構(gòu)建用以實(shí)現(xiàn)小車路徑規(guī)劃。2.1四輪移動(dòng)小車運(yùn)動(dòng)學(xué)模型光電編碼器一般會(huì)安裝在驅(qū)動(dòng)輪子的電機(jī)上。主要作用是記錄輪子轉(zhuǎn)軸轉(zhuǎn)動(dòng)一圈所輸出的位移量，也就是光電編碼器的分辨率。再通過記錄輪子轉(zhuǎn)軸在某一時(shí)間段內(nèi)轉(zhuǎn)動(dòng)的轉(zhuǎn)數(shù)，應(yīng)用相應(yīng)的計(jì)算公式就可算出這一時(shí)間段內(nèi)小車的行駛里程，也就是里程計(jì)的工作原理。然而，由于路面不平滑導(dǎo)致車輪抖動(dòng)和噪聲對光電編碼器的影響，對搜集到的里程信息造成很大的誤差，隨著小車的行駛，誤差不斷累加。還有安裝車輪時(shí)形成的誤差，會(huì)導(dǎo)致計(jì)算精確度不夠高。光電編碼器通過測量驅(qū)動(dòng)電機(jī)上碼盤的轉(zhuǎn)速μ、輪子的直徑d（單位：mm）和光電編碼器的精度ρ，推算出機(jī)器人位姿的變化量δ。采樣頻率可以設(shè)定，一般在QUOTE30Hz30Hz(2.1)里程計(jì)是通過編碼器輸出的左右驅(qū)動(dòng)輪脈沖增量QUOTENLNL和QUOTENRNR，和編碼器的分辨率δ來分別計(jì)算左右驅(qū)動(dòng)輪在采樣周期的位移增量QUOTE?SL?SL和?SRQUOTE?SR：(2.2)當(dāng)小車在轉(zhuǎn)彎時(shí)，由于直線模型無法直接描述其運(yùn)動(dòng)軌跡，故采用圓弧模型計(jì)算小車在轉(zhuǎn)彎時(shí)的運(yùn)動(dòng)情況。圖2.1輪式里程計(jì)圓弧模型小車的位姿變化可用t?1時(shí)刻的姿態(tài)QUOTEXt-1(xt-1,yt-1,θt-1)Xt-1(x(2.3)小車?yán)锍逃?jì)的輸入模型為QUOTEut=(?Dt,?θ(2.4)里程計(jì)的誤差來源有很多，系統(tǒng)誤差有測量儀器精確度不夠高、安裝車輪時(shí)位置控制不夠準(zhǔn)確形成的誤差等，無法消除。非系統(tǒng)誤差有路面不平滑導(dǎo)致車輪抖動(dòng)和噪聲對光電編碼器的影響，對搜集到的里程信息造成很大的誤差，隨著小車的行駛，誤差不斷累加，小車停止后誤差不再增加。2.2激光雷達(dá)模型2.2.1激光雷達(dá)測距原理本次課題所使用的雷達(dá)是型號名稱為A1M1的新型激光雷達(dá)RPLIDAR，由美國思嵐激光科技在2013年自主研發(fā)，目前國內(nèi)市場價(jià)格相對較低。360°掃描，掃描距離為8米，可用于機(jī)器人路徑規(guī)劃研究等領(lǐng)域。圖2.2激光雷達(dá)RPLIDARA1RPLIDARA1激光雷達(dá)系統(tǒng)的穩(wěn)定性較好，雷達(dá)轉(zhuǎn)速不會(huì)隨著光源電壓的變化而改變，因此選擇該型號激光雷達(dá)不會(huì)對路徑規(guī)劃造成很大誤差。掃描方向都是順時(shí)針方向。一般情況下，激光雷達(dá)都是安裝在小車中心位置，以激光雷達(dá)為坐標(biāo)原點(diǎn)建立直角坐標(biāo)系，其中QUOTEρi和diρi和di是當(dāng)前狀態(tài)下激光雷達(dá)到障礙物的遠(yuǎn)近，QUOTEθiQUOTEzi=ρi，θ式中，N是掃描點(diǎn)數(shù)，可自行設(shè)定。2.2.2激光雷達(dá)測距數(shù)學(xué)模型通過對比分析光束模型和似然場模型來研究激光雷達(dá)在進(jìn)行路徑規(guī)劃時(shí)采用的數(shù)學(xué)模型，著重分析了本設(shè)計(jì)用到的似然場模型。相較于光束模型，似然場模型適用于各種環(huán)境，而且計(jì)算量小。（1）光束模型預(yù)期的光束模型QUOTEpztxt,m因此當(dāng)移動(dòng)車輛處于動(dòng)態(tài)環(huán)境中時(shí)，用QUOTEphit表示phit表示第一個(gè)高斯測距模型，由于地圖是靜態(tài)的，所以不包括在地圖中的物體，如行人，應(yīng)該從靜態(tài)地圖中刪除，并用指數(shù)分布QUOTEpshortpshort表示，處理這類問題的簡單方法是將其作為傳感器噪聲直接去除，此時(shí)自動(dòng)激光雷達(dá)將自動(dòng)返回到最大精度測量區(qū)域范圍QUOTEzmaxzmax，即能以QUOTEzmaxzmax為中心的點(diǎn)群分布QUOTEpmaxpmax表示，即最大精度測量區(qū)域范圍已經(jīng)明確建立。當(dāng)一個(gè)激光測距系統(tǒng)產(chǎn)生各自無法解釋的外力測量時(shí)，例如激光發(fā)射的多束激光被多個(gè)壁反彈或激光受到多個(gè)傳感器的干擾時(shí)，為了有效簡化這種測量情況，用QUOTEprandprand在激光雷達(dá)天線測距測量范圍內(nèi)的完全均勻分布曲線來精確表示。圖2.3激光雷達(dá)測距模型分布考慮到幾率和密度為1，加權(quán)平均求和時(shí)通過四個(gè)分布對應(yīng)的參數(shù)QUOTEzhitzhit、QUOTEzshortzshort、QUOTEzmaxzmax、QUOTEzrandzrand進(jìn)行的，假如四個(gè)參數(shù)和為1，則有：(2.6)從計(jì)算的角度看，由于每次掃描返回值都有上百個(gè)，而且掃描頻率也很高，導(dǎo)致計(jì)算傳感器全部返回值是很難實(shí)現(xiàn)的。又因?yàn)槊看螔呙韬蠖家M(jìn)行射線投射操作，從而不能總是實(shí)時(shí)地將掃描整合到當(dāng)前的置信度中。為了解決這一問題，僅綜合激光雷達(dá)掃描過的一部分等間隔測量結(jié)果。并且不會(huì)輕易受相鄰測量的噪聲影響。基于波束的模型存在算法的局限性和光滑性不夠。當(dāng)小車身處一個(gè)復(fù)雜的障礙物運(yùn)動(dòng)環(huán)境中時(shí)，其在不同障礙位置的能量分布不連續(xù)。當(dāng)會(huì)議室里放置有許多小型桌椅和沙腿時(shí)，因?yàn)樾≤嚨囊粋€(gè)細(xì)微角度變化就會(huì)導(dǎo)致小車局部坐標(biāo)值在空間中x-y的巨大局部偏移，也就是說，在這樣一個(gè)不光滑的模型中，存在著更多的小車局部極大值偏移值。由于利用光線的反射原理，激光雷達(dá)的每一個(gè)光線掃描工作周期有數(shù)百個(gè)光線數(shù)值，這些數(shù)值都可能需要進(jìn)行一個(gè)光線投影的運(yùn)算，計(jì)算量很大。（2）似然場模型似然場模型計(jì)算是一種在特定計(jì)算環(huán)境下的數(shù)學(xué)假設(shè)設(shè)計(jì)算法，相似于高斯平滑的計(jì)算環(huán)境，它不要求計(jì)算是在激光雷達(dá)感應(yīng)器上數(shù)學(xué)模型的基本條件概率，其主要數(shù)學(xué)思想是通過全局三角變換將一個(gè)激光雷達(dá)的掃描坐標(biāo)點(diǎn)直接投影到位于地圖中心的一個(gè)全局坐標(biāo)系中。與其它波束模型方法相比，似然場模型通過一個(gè)三角形的映射將預(yù)先觀測到的結(jié)果通過投影映射到一個(gè)全局坐標(biāo)系，只有在電子探測器遇到障礙物時(shí)才會(huì)具有意義，該模型因而放棄了最大觀測距離下對觀測結(jié)果值的自動(dòng)讀取。為達(dá)到模型的平滑度，測量到的結(jié)果由一個(gè)模型平均值為0的高斯函數(shù)進(jìn)行估計(jì)，測量的坐標(biāo)由歐氏模型距離間的dist來表示，即使所測量到的結(jié)果在一個(gè)相應(yīng)的小車坐標(biāo)系下與歐氏模型距離間的速度地圖上模擬重合：(2.7)給出了似然場模型的幾率分配：(2.8)測量過程中的波束噪聲由高斯概率值的估計(jì)值表示；對于一個(gè)測定未成功時(shí)，即在第二種測量情況下討論波束模型時(shí)，它由一個(gè)函數(shù)點(diǎn)陣組合表示；對于不能正確解釋的隨機(jī)函數(shù)測量，它是由一個(gè)統(tǒng)一的分布表示。表2.1似然場模型概率計(jì)算偽代碼似然場概率估計(jì)算法：1:q=12:forallkto3:if4:5:6:7:8:endif9:endfor10:returnq從表2.1可以看出，似然場模型中，第三行是在檢測激光雷達(dá)讀數(shù)是否為最大值，如果答案是，則可以丟棄。第4-7行主要測量將障礙物高斯模糊后的數(shù)據(jù)。第6行用歐氏距離表示測量距離。第7行整合均勻分布和點(diǎn)群分布。將障礙物高斯核處理后，用全局坐標(biāo)X-Y來表示測距的似然估計(jì)。采用的似然場模型優(yōu)點(diǎn)是對障礙物進(jìn)行高斯處理后，使觀測結(jié)果更加平滑，適用于結(jié)構(gòu)化和非結(jié)構(gòu)化環(huán)境。并且通過對地圖進(jìn)行光柵化來加快運(yùn)行速度，本代碼最耗時(shí)的步驟是第6步，通過查表可以得到光柵化的地圖。2.3四輪移動(dòng)小車的坐標(biāo)系統(tǒng)在小車研究領(lǐng)域中，有多個(gè)坐標(biāo)系可用來描述小車的位置和姿態(tài)信息，并且在一定條件下各坐標(biāo)系的數(shù)據(jù)可以相互轉(zhuǎn)換。系統(tǒng)中所有點(diǎn)的坐標(biāo)在任何時(shí)候都可以用世界坐標(biāo)系來描述，世界坐標(biāo)系是絕對的。通過將小車坐標(biāo)系QUOTEORXRYRORXRYR中的環(huán)境信息轉(zhuǎn)換為世界坐標(biāo)系圖2.4小車坐標(biāo)系圖2.4顯示了小車坐標(biāo)系與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系?？赏ㄟ^公式（2.9）將m點(diǎn)的小車坐標(biāo)QUOTEX0=[x0，y0]TX0=（2.9）2.4地圖模型2.4.1柵格地圖柵格地圖（Gridmap）分為矩形柵格地圖和扇形柵格地圖。主要研究矩形柵格地圖，它是由一系列大小一致的矩形柵格組成，柵格大小可人為設(shè)定。如下圖2.4表示一個(gè)簡單的矩形柵格地圖，由直線分割得到。白色表示可行區(qū)域，黑色表示有障礙物，在矩形柵格地圖上可形象直觀的進(jìn)行路徑規(guī)劃研究，有利于數(shù)據(jù)的存儲(chǔ)和計(jì)算。圖2.5柵格地圖2.4.2拓?fù)涞貓D拓?fù)涞貓D（Toplogicalmap）就是用簡單的點(diǎn)線連接表示兩點(diǎn)之間的互通性。在室內(nèi)，節(jié)點(diǎn)代表門、桌子和拐角等位置，之間的連線表示可以到達(dá)。拓?fù)涞貓D分為星型、網(wǎng)狀型和簇狀型拓?fù)涞貓D，如圖2.6所示。由于空間復(fù)雜度比較低，可進(jìn)行有效的路徑規(guī)劃，但不容易識別指定位置，也有可能造成規(guī)劃出的路徑不是最佳的。圖2.6拓?fù)涞貓D2.4.3特征地圖特征地圖（Featuremap）是以簡單幾何形狀中的點(diǎn)、線段和圓弧來表示環(huán)境信息的一種地圖，如圖2.7所示。首先對環(huán)境中各自的特征信息進(jìn)行識別與提取，收集環(huán)境中的目標(biāo)特征，然后獲取物體的地理坐標(biāo)，計(jì)算出目標(biāo)特征的地理坐標(biāo)，生成特征地圖。然而，在重新定位時(shí)如果數(shù)據(jù)匹配錯(cuò)誤，將會(huì)導(dǎo)致路徑規(guī)劃錯(cuò)誤。圖2.7特征地圖

第三章四輪移動(dòng)小車系統(tǒng)設(shè)計(jì)3.1硬件設(shè)計(jì)首先，建立四輪移動(dòng)汽車的實(shí)驗(yàn)平臺。完成移動(dòng)結(jié)構(gòu)、驅(qū)動(dòng)系統(tǒng)、感覺系統(tǒng)和承載平臺的組裝。四輪車的驅(qū)動(dòng)機(jī)構(gòu)和運(yùn)動(dòng)是輪式汽車的基本運(yùn)動(dòng)結(jié)構(gòu)。圖3.1（a）顯示了由兩層組成的車身。采用輪式移動(dòng)機(jī)構(gòu)。前輪是控制輪，后輪是驅(qū)動(dòng)輪，可差速轉(zhuǎn)動(dòng)。驅(qū)動(dòng)系統(tǒng)是為機(jī)械結(jié)構(gòu)提供動(dòng)力的裝置。驅(qū)動(dòng)系統(tǒng)采用ArduinoTeensy3.1底層驅(qū)動(dòng)板如圖3.1（b）搭配直流電源如圖3.1（c）所示。傳感系統(tǒng)由外部傳感器和內(nèi)部傳感器組成。內(nèi)部傳感器用于檢測汽車自身的狀態(tài)，外部傳感器用于感知外部世界，檢測工作對象和工作環(huán)境的狀態(tài)，如視覺、聽覺、觸覺等。本文選用圖3.1（d）所示的RplidarA1激光雷達(dá)傳感器?？刂葡到y(tǒng)的任務(wù)是根據(jù)汽車的操作指令和傳感器的信號反饋，控制汽車的執(zhí)行機(jī)構(gòu)完成規(guī)定的動(dòng)作和功能。為實(shí)現(xiàn)四輪小車靈活、易擴(kuò)展、模塊化的優(yōu)點(diǎn)，控制系統(tǒng)采用主控板如圖3.1(e)所示，和開源卡片式電腦RaspberryPi3b如圖3.1（f）所示。(a)移動(dòng)小車(b)底層驅(qū)動(dòng)板(c)直流電源(d)激光傳感器(e)主控板(f)卡片式電腦圖3.1各部件及四輪小車實(shí)物四輪移動(dòng)小車硬件系統(tǒng)主要由小車控制系統(tǒng)和服務(wù)器兩部分組成，其結(jié)構(gòu)框圖如圖3.2所示?？刂葡到y(tǒng)由RplidarA1激光傳感器、電源模塊、主控板慣性測量、位置控制模塊，每部分將各自傳感器接收到的信息通過WIFI傳給服務(wù)器從而遠(yuǎn)程控制小車運(yùn)動(dòng)和RVIZ的可視化接受。位置控制模塊主要由GY-85九軸陀螺儀傳感器、底層驅(qū)動(dòng)板ArduinoTeensy3.1、差速PID調(diào)速驅(qū)動(dòng)器、360線AB編碼器、12V366PRM直流有刷電機(jī)等形成閉環(huán)系統(tǒng)，選擇PID(ProportionalIntegralDerivative)算法實(shí)行控制，采用360線AB編碼器進(jìn)行小車的車輪線速度計(jì)算，小車的角速度由GY-85陀螺儀控制，完成小車的線速度、角速度等控制信息準(zhǔn)確回發(fā)到服務(wù)器端，對小車精準(zhǔn)定位。電源選擇12V2800mAh可充電大電流鋰電池為小車充電（市面上的電池盒和連接器有無數(shù)種，其中一種見圖2），3節(jié)18650型鋰電池組成電芯。利用RplidarA1激光雷達(dá)，主控制器把獲取的環(huán)境二維激光數(shù)據(jù)連接ROS之間的節(jié)點(diǎn)并將數(shù)據(jù)通過無線傳輸給服務(wù)器，實(shí)現(xiàn)二維環(huán)境下的地圖構(gòu)建。主控制器選用的BCM2837處理器為1.2GHz64位，搭載四核ARMCortexA53的開源卡片式電腦RaspberryPi3B，內(nèi)置10/100自適應(yīng)網(wǎng)卡，配備多種接口和1G內(nèi)存，可通過802.11nWIFI無線USB網(wǎng)卡與服務(wù)器進(jìn)行無線網(wǎng)絡(luò)通信。戴爾筆記本電腦安裝開源操作系統(tǒng)ROS和Ubuntu16.04操作系統(tǒng)作為服務(wù)器，主控制器傳送來的里程信息、速度、位姿的接收主要由服務(wù)器端的PC機(jī)承擔(dān)，用于RVIZ可視化的整個(gè)區(qū)域地圖的構(gòu)建和遠(yuǎn)程操控。圖3.2四輪小車系統(tǒng)設(shè)計(jì)框架3.2軟件設(shè)計(jì)ROS（RobotOperatingSystem）作為目前面向機(jī)器人的主流開源操作系統(tǒng)，其提供相應(yīng)的工具和庫便于完成整個(gè)機(jī)器人操作系統(tǒng)框架搭建。ROS支持Ubuntu16.04操作系統(tǒng)和樹莓派的Raspbian操作系統(tǒng)，在本節(jié)中簡要概述移動(dòng)機(jī)器人的ROS集成體系結(jié)構(gòu)及通過ROS節(jié)點(diǎn)發(fā)布的消息進(jìn)行位置控制。通過地圖建設(shè)的圖形化處理，解決了樹莓派地區(qū)數(shù)據(jù)處理量過大的問題。采用ROS的分布式處理框架來完成不同節(jié)點(diǎn)之間的通信，保證樹莓派和虛擬機(jī)在同一網(wǎng)段中。ROS是一種基于圖狀架構(gòu)，不同節(jié)點(diǎn)的進(jìn)程能接受，發(fā)布，整合小車信息。在服務(wù)器端，我們可以接收移動(dòng)汽車的角速度和線速度修正、IMU修正、鍵盤調(diào)試和連續(xù)傳輸信息，并合并里程信息、激光雷達(dá)等節(jié)點(diǎn)。小車結(jié)構(gòu)如圖3.3所示。RVIZ被配置為顯示構(gòu)建的地圖，用來估計(jì)和校正汽車的姿態(tài)和最新的激光掃描數(shù)據(jù)。服務(wù)器節(jié)點(diǎn)RVIZ主要負(fù)責(zé)消息的可視化：(1)鍵盤調(diào)試。使用鍵盤上的指令，控制汽車行駛并接收反饋的里程信息。(2)線速度和角速度的校正。為了獲得更高的精度，對線速度和速度校正碼進(jìn)行了修正。(3)SLAM。獲得最新的激光掃描和估計(jì)測距姿態(tài)，從而校正汽車姿態(tài)和更新地圖。圖3.3小車分布式ROS架構(gòu)移動(dòng)小車主控制器的終端節(jié)點(diǎn)主要負(fù)責(zé)傳感器數(shù)據(jù)的采集和小車運(yùn)動(dòng)的控制：(1)激光雷達(dá)。從RPLIDARA1激光傳感器接收激光數(shù)據(jù)信息；(2)里程信息。通過讀取編碼器數(shù)據(jù)并使用該算法，可以估計(jì)出汽車的位置和姿態(tài)；(3)基本控制器。Arduino控制器被配置為遠(yuǎn)程節(jié)點(diǎn)，訂閱主題動(dòng)作，接收汽車的角速度和線速度，控制汽車精確移動(dòng)；(4)Arduino。與樹莓派上位機(jī)建立通信，Arduino節(jié)點(diǎn)發(fā)布和訂閱控制信息。3.3位置控制程序設(shè)計(jì)通過小車與服務(wù)器操作系統(tǒng)的連接來完成小車位置的實(shí)時(shí)控制。在初始化小車角速度和線速度的前提下，服務(wù)器接收和分析小車位置。接收到當(dāng)前位置的參數(shù)后，計(jì)算誤差，并用PID算法調(diào)節(jié)電機(jī)轉(zhuǎn)速。選擇GY－85陀螺儀控制角速度，根據(jù)電機(jī)上的編碼器計(jì)算線速度。服務(wù)器發(fā)送指令來控制輸出量，小車通過RplidarA1激光傳感器做出相應(yīng)的移動(dòng)和避障，并將當(dāng)前姿態(tài)信息傳送到服務(wù)器，如圖3.4所示。圖3.4位置控制程序流程圖

第四章四輪移動(dòng)小車路徑規(guī)劃研究及試驗(yàn)小車路徑路線規(guī)劃主要是根據(jù)最短行駛距離，尋找規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)之間的無碰撞行駛路徑?；谏弦徽乱苿?dòng)小車系統(tǒng)的整體設(shè)計(jì)，結(jié)合二維激光雷達(dá)和柵格地圖的相關(guān)知識，很容易實(shí)現(xiàn)小車的路徑規(guī)劃。本章重點(diǎn)研究了A*算法和遺傳算法兩種小車全局路徑規(guī)劃算法，DWA動(dòng)態(tài)窗口規(guī)劃算法用以完成局部路徑規(guī)劃。4.1全局路徑規(guī)劃研究4.1.1Dijkstra算法與A*算法Dijkstra算法是用于計(jì)算一個(gè)節(jié)點(diǎn)到另一個(gè)節(jié)點(diǎn)的最短路徑的典型算法。對于單源最短路徑，首先指定圖中的一個(gè)頂點(diǎn)為起點(diǎn)，以起點(diǎn)為中心向外擴(kuò)展，記錄下從起點(diǎn)到各個(gè)點(diǎn)的距離。然后在剩余頂點(diǎn)中找出與源點(diǎn)距離最近的一個(gè)可行頂點(diǎn)作為第二個(gè)點(diǎn)向外擴(kuò)展，依此類推，直到所有的點(diǎn)都被選完。每個(gè)被選定的點(diǎn)組成一條可行路徑，該算法為貪心算法的策略。如圖4.1所示，這是模擬Dijkstra算法的路徑規(guī)劃。綠色網(wǎng)格是起點(diǎn)，黃色網(wǎng)格是終點(diǎn)，紅色網(wǎng)格是基于源點(diǎn)的可行范圍，黑色網(wǎng)格是障礙物，藍(lán)色表示區(qū)域邊界。這種遍歷搜索方式的優(yōu)點(diǎn)是能以最短路徑找到最優(yōu)解，但沒有考慮到搜索時(shí)間、搜索效率等問題。圖4.1Dijkstra算法路徑規(guī)劃A*算法結(jié)合了最佳優(yōu)先級搜索策略，在Dijkstra算法的基礎(chǔ)上添加了啟發(fā)式搜索算法，如圖4.2中的算法路徑搜索。使用以下函數(shù)f(n)來計(jì)算每個(gè)節(jié)點(diǎn)的優(yōu)先級： (4.1)圖4.2A*算法路徑搜索f(n)是節(jié)點(diǎn)的綜合優(yōu)先級。f(n)由g(n)和h(n)組成，優(yōu)先級越高，f(n)越??；其中g(shù)(n)是當(dāng)前節(jié)點(diǎn)從起點(diǎn)的開銷，h(n)是當(dāng)前節(jié)點(diǎn)從終點(diǎn)的開銷；每次從優(yōu)先級隊(duì)列中選擇f(n)值最小的節(jié)點(diǎn)作為下一個(gè)要遍歷的節(jié)點(diǎn)。正如柵格地圖模型，下列距離模型可用于h(n)：圖4.3A*算法流程圖曼哈頓距離：，如果圖形中只允許四個(gè)運(yùn)動(dòng)方向，則可以使用此方法表示當(dāng)前位置的距離。對角距離：，如果每個(gè)節(jié)點(diǎn)都是正方形，且允許向相鄰節(jié)點(diǎn)傾斜運(yùn)動(dòng)，那么函數(shù)可以對角測量。歐幾里得距離：QUOTEhn=xn-x0采用的汽車四輪運(yùn)動(dòng)底盤，基本上可以實(shí)現(xiàn)任意方向的運(yùn)動(dòng)，所以選用曼哈頓距離。4.1.2基于遺傳算法的小車全局路徑規(guī)劃遺傳算法通過全局搜索空間區(qū)域，自適應(yīng)地調(diào)整搜索方向直接對結(jié)構(gòu)對象進(jìn)行操作。路徑優(yōu)化過程如圖4.4所示：圖4.4路徑規(guī)劃遺傳算法流程圖提出了一種基于柵格地圖構(gòu)建的移動(dòng)車輛路徑規(guī)劃方法。單個(gè)網(wǎng)格的區(qū)域需要綜合考慮環(huán)境地圖的障礙物信息，每個(gè)網(wǎng)格都按照建立的坐標(biāo)系進(jìn)行編號，其中白網(wǎng)格為可行區(qū)域，黑網(wǎng)格為障礙物。對于填充初始化步驟，主要目的是生成多條可達(dá)路徑。首先在每一行按順序取出一個(gè)無障礙網(wǎng)格，然后用公式4.2判斷網(wǎng)格是否連續(xù)：（4.2）abs函數(shù)是絕對值函數(shù)。當(dāng)且僅當(dāng)D=1時(shí)，這兩個(gè)網(wǎng)格可以判斷為連續(xù)的，而其他網(wǎng)格則是不連續(xù)的，網(wǎng)格不連續(xù)時(shí)取中間網(wǎng)格：QUOTExnew=int(xi+1+新網(wǎng)格的坐標(biāo)由公式(4.3)獲得，初始化路徑流程如圖4.5所示。圖4.5初始化路徑流程圖通過初始化路徑過程，可以得到無數(shù)條連續(xù)的路徑，最優(yōu)路徑由適應(yīng)度函數(shù)確定。為了實(shí)際考慮，我們考慮了兩個(gè)部分。首先，我們需要實(shí)現(xiàn)最短路徑。其次，對于研究的四輪移動(dòng)小車而言，道路的轉(zhuǎn)彎與其他運(yùn)動(dòng)方式相比會(huì)導(dǎo)致一個(gè)巨大的定位誤差，因此對路徑規(guī)劃的平滑度要求很高。路徑長度計(jì)算如下：QUOTEd總=i=1end-1遺傳算法首先需要經(jīng)過選擇操作，而輪盤賭中的選擇方法比基于訂單的選擇方法更為簡單，易于實(shí)現(xiàn)。輪盤賭的過程就是選擇高概率而放棄低概率。因此，基于輪盤賭選擇法，隨機(jī)選取適應(yīng)度值較大的第一部分的概率較高，適應(yīng)度函數(shù)的第一部分為：（4.5）對于移動(dòng)小車路徑的平滑度考慮，此時(shí)考慮鄰近三點(diǎn)間的距離：（4.6）小車的行走方式：直角、鈍角、銳角和直角。路徑規(guī)劃的最佳方法是直線，其次是鈍角和直角，最不利的方法是銳角。此時(shí)，汽車轉(zhuǎn)彎過多，很容易導(dǎo)致汽車姿態(tài)的巨大誤差。而且，在考慮相鄰三點(diǎn)時(shí)，直線距離最大，平滑度最高，因此，在路徑規(guī)劃中給懲罰函數(shù)剩下的路一個(gè)較低的分?jǐn)?shù)。此時(shí)，適應(yīng)度函數(shù)的第二部分是：（4.7）完整的適應(yīng)度函數(shù)如下：（4.8）此時(shí)，我們需要給適應(yīng)度函數(shù)的第一部分和適應(yīng)度函數(shù)的第二部分賦值，因?yàn)橛行┞窂绞亲疃痰?，但不能考慮平滑度，所以我們需要給a和b賦值，其和為1。在輪盤賭算法的基礎(chǔ)上，每個(gè)個(gè)體出現(xiàn)的概率為：（4.9）在選擇過程完成之后，即在個(gè)體的數(shù)目通過連續(xù)復(fù)制達(dá)到預(yù)定數(shù)目之后，執(zhí)行交叉操作。交叉運(yùn)算符的值設(shè)置為0.8以生成0到1的值。當(dāng)小于0.8時(shí)，執(zhí)行交叉操作，并且確定兩條路徑中的同一點(diǎn)作為插入操作的交點(diǎn)。完成交叉操作后，判斷隨機(jī)值的大小和交叉算子的0.06，如果小于0.06，則完成變異操作。為了避免突變點(diǎn)的值出現(xiàn)在操作區(qū)域之外，必須定義一個(gè)新的標(biāo)記點(diǎn)來替換先前的突變點(diǎn)，并過濾掉不相關(guān)的點(diǎn)。MATLAB用于模擬分析。首先，建立了包含400個(gè)障礙物單元的柵格地圖。起點(diǎn)是左下角的網(wǎng)格，終點(diǎn)是右上角，如圖4.6（a）所示。此時(shí)，只考慮汽車的最短距離。從圖4.6（b）可以看出，算法經(jīng)過約10次迭代后收斂，網(wǎng)格圖的單位長度為1，規(guī)劃路徑中有許多直角和銳角，車輛行走困難。對于圖4.6（c），考慮到車輛的路徑長度和平滑度，并賦予平滑度更大的權(quán)重，規(guī)劃路徑更平滑，兩種情況下的路徑長度相等。圖4.6基于遺傳算法的路徑規(guī)劃仿真通過考慮路徑的平滑度和最短距離，增加了迭代次數(shù)，使路徑更為平滑，適合汽車行走。在a=1的情況下，汽車只考慮最短路徑。此時(shí)，雖然得到了一條較好的路徑，但沒有考慮汽車的運(yùn)動(dòng)學(xué)模型。直角或銳角的巨大轉(zhuǎn)角容易導(dǎo)致車輛的定位偏差，因此需要綜合考慮車輛路徑的平順性。4.2局部路徑規(guī)劃研究在地圖已知的條件下，車輛可以得到全局路徑規(guī)劃，但不能避免遇到臨時(shí)障礙物。此時(shí)需要進(jìn)行局部路徑規(guī)劃操作并做出避障反應(yīng)。在完成局部路徑規(guī)劃以避開障礙物后，它可以按照系統(tǒng)規(guī)劃的一個(gè)全局路徑進(jìn)行移動(dòng)。目前對于局部窗口路徑規(guī)劃主要方法有動(dòng)態(tài)窗口勢場法和靜態(tài)人工窗口勢場法。在靜態(tài)人工勢場法中，目的地是引力源。通過虛擬化，目標(biāo)點(diǎn)將吸引汽車在環(huán)境中的位置，環(huán)境中的障礙物將擊退汽車。因此，可以同時(shí)計(jì)算汽車在環(huán)境中的位置和姿態(tài)。人工勢場法通過設(shè)置相應(yīng)的參數(shù)，可以有效地避開障礙物，具有良好的實(shí)時(shí)性。然而，不完全的局部信息可能導(dǎo)致錯(cuò)誤的路徑規(guī)劃。因此，結(jié)合汽車運(yùn)動(dòng)學(xué)模型，采用動(dòng)態(tài)窗口法進(jìn)行局部路徑規(guī)劃。動(dòng)態(tài)窗口算法對不同速度空間條件中的多組不同數(shù)據(jù)速度進(jìn)行自動(dòng)采樣，并在這些不同速度下自動(dòng)模擬汽車在特定行駛時(shí)間和速度空間條件中的行駛軌跡。動(dòng)態(tài)窗口的主要意義是根據(jù)汽車的運(yùn)動(dòng)學(xué)模型，通過控制增減運(yùn)動(dòng)速度，在可行的范圍內(nèi)可以獲得有限的速度空間的采樣。四輪移動(dòng)汽車只能向前移動(dòng)和旋轉(zhuǎn)，軌道是直線或圓弧，所以在短時(shí)間內(nèi)，車速可以看作是一個(gè)常數(shù)。車輛限速：（4.10）四輪小車的運(yùn)動(dòng)是由電機(jī)驅(qū)動(dòng)的，電機(jī)的轉(zhuǎn)矩是有限的，所以小車的加速度也是有限的，所以單位時(shí)間內(nèi)小車速度的變化也是有限的：（4.11）在式(4.10)中，計(jì)算小車的最大加速度和減速度。如果環(huán)境中有障礙物，則需要在障礙物前方一定距離處停車。考慮到車輛的加速度，速度范圍為：（4.12）式中，表示在采樣空間中，當(dāng)前采樣值下距障礙物最近的距離。這種情況需要模擬汽車的運(yùn)動(dòng)軌跡。當(dāng)觀察到障礙物時(shí)，只有當(dāng)前的采樣速度才能允許車輛停在障礙物前面，此速度將被接受。小車速度的空間范圍如式(4.12)所示：（4.13）確定最優(yōu)軌道是必要的：(4.14)在公式(4.14)中，指的是方位角評價(jià)函數(shù)，用來評價(jià)四輪移動(dòng)小車以當(dāng)前速度到達(dá)模擬軌道末端時(shí)方向與目標(biāo)之間的角度誤差。是指到評價(jià)函數(shù)的距離，并計(jì)算出與障礙物之間的距離。如果此時(shí)道路上沒有障礙物，這是一個(gè)常數(shù)。指的是評價(jià)函數(shù)的速度，在局部導(dǎo)航的過程中，小車不僅能避開障礙物，還能以更高的速度到達(dá)目標(biāo)點(diǎn)。為了使評估