某四輪移動(dòng)小車路徑規(guī)劃系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)分析案例目錄TOC\o"1-3"\h\u15039某四輪移動(dòng)小車路徑規(guī)劃系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)分析案例 19079第1章四輪移動(dòng)小車系統(tǒng)設(shè)計(jì) 1230621.1硬件設(shè)計(jì) 130221.2軟件設(shè)計(jì) 3158981.3位置控制程序設(shè)計(jì) 414412第2章四輪移動(dòng)小車路徑規(guī)劃研究及試驗(yàn) 682252.1全局路徑規(guī)劃研究 621262.1.1Dijkstra算法與A*算法 6184192.1.2基于遺傳算法的小車全局路徑規(guī)劃 9232812.2局部路徑規(guī)劃研究 12155862.3路徑規(guī)劃算法試驗(yàn) 15第1章四輪移動(dòng)小車系統(tǒng)設(shè)計(jì)1.1硬件設(shè)計(jì)首先，建立四輪移動(dòng)汽車的實(shí)驗(yàn)平臺(tái)。完成移動(dòng)結(jié)構(gòu)、驅(qū)動(dòng)系統(tǒng)、感覺系統(tǒng)和承載平臺(tái)的組裝。四輪車的驅(qū)動(dòng)機(jī)構(gòu)和運(yùn)動(dòng)是輪式汽車的基本運(yùn)動(dòng)結(jié)構(gòu)。圖1.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)采用ArduinoTeensy1.1底層驅(qū)動(dòng)板如圖1.1（b）搭配直流電源如圖1.1（c）所示。傳感系統(tǒng)由外部傳感器和內(nèi)部傳感器組成。內(nèi)部傳感器用于檢測汽車自身的狀態(tài)，外部傳感器用于感知外部世界，檢測工作對象和工作環(huán)境的狀態(tài)，如視覺、聽覺、觸覺等。本文選用圖1.1（d）所示的RplidarA1激光雷達(dá)傳感器?？刂葡到y(tǒng)的任務(wù)是根據(jù)汽車的操作指令和傳感器的信號(hào)反饋，控制汽車的執(zhí)行機(jī)構(gòu)完成規(guī)定的動(dòng)作和功能。為實(shí)現(xiàn)四輪小車靈活、易擴(kuò)展、模塊化的優(yōu)點(diǎn)，控制系統(tǒng)采用主控板如圖1.1(e)所示，和開源卡片式電腦RaspberryPi3b如圖1.1（f）所示。(a)移動(dòng)小車(b)底層驅(qū)動(dòng)板(c)直流電源(d)激光傳感器(e)主控板(f)卡片式電腦圖1.1各部件及四輪小車實(shí)物四輪移動(dòng)小車硬件系統(tǒng)主要由小車控制系統(tǒng)和服務(wù)器兩部分組成，其結(jié)構(gòu)框圖如圖1.2所示。控制系統(tǒng)由RplidarA1激光傳感器、電源模塊、主控板慣性測量、位置控制模塊，每部分將各自傳感器接收到的信息通過WIFI傳給服務(wù)器從而遠(yuǎn)程控制小車運(yùn)動(dòng)和RVIZ的可視化接受。位置控制模塊主要由GY-85九軸陀螺儀傳感器、底層驅(qū)動(dòng)板ArduinoTeensy1.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)程操控。圖1.2四輪小車系統(tǒng)設(shè)計(jì)框架1.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)如圖1.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)和更新地圖。圖1.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ā)布和訂閱控制信息。1.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ù)器，如圖1.4所示。圖1.4位置控制程序流程圖

第2章四輪移動(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í)，很容易實(shí)現(xiàn)小車的路徑規(guī)劃。本章重點(diǎn)研究了A*算法和遺傳算法兩種小車全局路徑規(guī)劃算法，DWA動(dòng)態(tài)窗口規(guī)劃算法用以完成局部路徑規(guī)劃。2.1全局路徑規(guī)劃研究2.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)組成一條可行路徑，該算法為貪心算法的策略。如圖2.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í)間、搜索效率等問題。圖2.1Dijkstra算法路徑規(guī)劃A*算法結(jié)合了最佳優(yōu)先級(jí)搜索策略，在Dijkstra算法的基礎(chǔ)上添加了啟發(fā)式搜索算法，如圖2.2中的算法路徑搜索。使用以下函數(shù)f(n)來計(jì)算每個(gè)節(jié)點(diǎn)的優(yōu)先級(jí)： (2.1)圖2.2A*算法路徑搜索f(n)是節(jié)點(diǎn)的綜合優(yōu)先級(jí)。f(n)由g(n)和h(n)組成，優(yōu)先級(jí)越高，f(n)越??；其中g(shù)(n)是當(dāng)前節(jié)點(diǎn)從起點(diǎn)的開銷，h(n)是當(dāng)前節(jié)點(diǎn)從終點(diǎn)的開銷；每次從優(yōu)先級(jí)隊(duì)列中選擇f(n)值最小的節(jié)點(diǎn)作為下一個(gè)要遍歷的節(jié)點(diǎn)。正如柵格地圖模型，下列距離模型可用于h(n)：圖2.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)，所以選用曼哈頓距離。2.1.2基于遺傳算法的小車全局路徑規(guī)劃遺傳算法通過全局搜索空間區(qū)域，自適應(yīng)地調(diào)整搜索方向直接對結(jié)構(gòu)對象進(jìn)行操作。路徑優(yōu)化過程如圖2.4所示：圖2.4路徑規(guī)劃遺傳算法流程圖提出了一種基于柵格地圖構(gòu)建的移動(dòng)車輛路徑規(guī)劃方法。單個(gè)網(wǎng)格的區(qū)域需要綜合考慮環(huán)境地圖的障礙物信息，每個(gè)網(wǎng)格都按照建立的坐標(biāo)系進(jìn)行編號(hào)，其中白網(wǎng)格為可行區(qū)域，黑網(wǎng)格為障礙物。對于填充初始化步驟，主要目的是生成多條可達(dá)路徑。首先在每一行按順序取出一個(gè)無障礙網(wǎng)格，然后用公式2.2判斷網(wǎng)格是否連續(xù)：（2.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+xi新網(wǎng)格的坐標(biāo)由公式(2.3)獲得，初始化路徑流程如圖2.5所示。圖2.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-1xi遺傳算法首先需要經(jīng)過選擇操作，而輪盤賭中的選擇方法比基于訂單的選擇方法更為簡單，易于實(shí)現(xiàn)。輪盤賭的過程就是選擇高概率而放棄低概率。因此，基于輪盤賭選擇法，隨機(jī)選取適應(yīng)度值較大的第一部分的概率較高，適應(yīng)度函數(shù)的第一部分為：（2.5）對于移動(dòng)小車路徑的平滑度考慮，此時(shí)考慮鄰近三點(diǎn)間的距離：（2.6）小車的行走方式：直角、鈍角、銳角和直角。路徑規(guī)劃的最佳方法是直線，其次是鈍角和直角，最不利的方法是銳角。此時(shí)，汽車轉(zhuǎn)彎過多，很容易導(dǎo)致汽車姿態(tài)的巨大誤差。而且，在考慮相鄰三點(diǎn)時(shí)，直線距離最大，平滑度最高，因此，在路徑規(guī)劃中給懲罰函數(shù)剩下的路一個(gè)較低的分?jǐn)?shù)。此時(shí)，適應(yīng)度函數(shù)的第二部分是：（2.7）完整的適應(yīng)度函數(shù)如下：（2.8）此時(shí)，我們需要給適應(yīng)度函數(shù)的第一部分和適應(yīng)度函數(shù)的第二部分賦值，因?yàn)橛行┞窂绞亲疃痰模荒芸紤]平滑度，所以我們需要給a和b賦值，其和為1。在輪盤賭算法的基礎(chǔ)上，每個(gè)個(gè)體出現(xiàn)的概率為：（2.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)是右上角，如圖2.6（a）所示。此時(shí)，只考慮汽車的最短距離。從圖2.6（b）可以看出，算法經(jīng)過約10次迭代后收斂，網(wǎng)格圖的單位長度為1，規(guī)劃路徑中有許多直角和銳角，車輛行走困難。對于圖2.6（c），考慮到車輛的路徑長度和平滑度，并賦予平滑度更大的權(quán)重，規(guī)劃路徑更平滑，兩種情況下的路徑長度相等。圖2.6基于遺傳算法的路徑規(guī)劃仿真通過考慮路徑的平滑度和最短距離，增加了迭代次數(shù)，使路徑更為平滑，適合汽車行走。在a=1的情況下，汽車只考慮最短路徑。此時(shí)，雖然得到了一條較好的路徑，但沒有考慮汽車的運(yùn)動(dòng)學(xué)模型。直角或銳角的巨大轉(zhuǎn)角容易導(dǎo)致車輛的定位偏差，因此需要綜合考慮車輛路徑的平順性。2.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ù)。車輛限速：（2.10）四輪小車的運(yùn)動(dòng)是由電機(jī)驅(qū)動(dòng)的，電機(jī)的轉(zhuǎn)矩是有限的，所以小車的加速度也是有限的，所以單位時(shí)間內(nèi)小車速度的變化也是有限的：（2.11）在式(2.10)中，計(jì)算小車的最大加速度和減速度。如果環(huán)境中有障礙物，則需要在障礙物前方一定距離處停車?？紤]到車輛的加速度，速度范圍為：（2.12）式中，表示在采樣空間中，當(dāng)前采樣值下距障礙物最近的距離。這種情況需要模擬汽車的運(yùn)動(dòng)軌跡。當(dāng)觀察到障礙物時(shí)，只有當(dāng)前的采樣速度才能允許車輛停在障礙物前面，此速度將被接受。小車速度的空間范圍如式(2.12)所示：（2.13）確定最優(yōu)軌道是必要的：(2.14)在公式(2.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)。為了使評估過程順利進(jìn)行，有必要在添加前對這三個(gè)部分進(jìn)行規(guī)范化。(2.15)規(guī)范化方程（2.13）中的三個(gè)評估函數(shù)，其中n是軌跡總數(shù)，i是當(dāng)前軌跡。通過將三個(gè)索引標(biāo)準(zhǔn)化，然后將它們相加，可以平滑評價(jià)函數(shù)，并避免某個(gè)索引過大。為了反映汽車的局部規(guī)劃能力，采用了MATLAB仿真分析方法。首先，將起點(diǎn)坐標(biāo)定義為坐標(biāo)軸的原點(diǎn)，終點(diǎn)為坐標(biāo)系中的（10，10）。圖2.8顯示了汽車的本地路徑規(guī)劃和導(dǎo)航。地圖上的障礙物信息用圖中的紅色圓圈表示，汽車在地圖上不斷規(guī)劃路徑，通過在評價(jià)函數(shù)在地圖上選擇正確的路徑運(yùn)動(dòng)，達(dá)到避障和合理規(guī)劃的目的。 （a） (b) (c)