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