基于SVM的受約束D*算法在無人車尋路中的應用?

劉曉濤 蔡云飛 王田橙

基于SVM的受約束D*算法在無人車尋路中的應用?

劉曉濤 蔡云飛 王田橙

（南京理工大學計算機科學與工程學院 南京 210094）

針對在未知環境無人車受約束控制條件下的動態路徑平滑規劃問題，提出了一種基于支持向量機（SVM）的D*改進算法。該算法通過柵格法對環境建模，建立車輛受約束動態方程，在動態規劃路徑選取之后使用SVM算法對無人車轉向位置處進行局部路徑平滑優化。實現無人車在真實環境中流暢穩定的運動。實驗結果表明：該算法能夠在未知環境中規劃出平滑的動態路徑，具有較好的可靠性和穩定性。

無人車；受約束D*算法；SVM；線性核；動態路徑平滑規劃

AbstractFor the problem of the dynamic path planning and path smoothing of automated vehicle in unknown environment with constraints，an improved algorithm of D*based on support vector machine（SVM）is presented.In this algorithm the environ?mental modeling is based on the grid method and then establish the constrained dynamic equation of the vehicle.After the dynamic planning path is selected，the SVM algorithm is used to smooth optimization the local path of automated vehicle at the steering posi?tion.By this algorithm automated vehicle can smooth and stable movement in real environment.Experimental results show that this algorithm can dynamically plan a smooth path in unknown environment，which has sufficient reliability and stability.

Key Wordsautomated vehicle，constrained D*algorithm，SVM，linear kernel，dynamic path planning and path smoothing

Class NumberTP391

1 引言

無人車涉及認知科學、人工智能、機器人技術與車輛工程等交叉學科，是各種新興技術的綜合試驗床與理想載體，也是當今前沿科技的重要發展方向。無人駕駛技術是傳感器、計算機、人工智能、通信、導航定位、模式識別、智能控制等多門學科的綜合體，無人駕駛汽車的關鍵技術包括環境感知、導航定位、路徑規劃、決策控制等。環境感知模塊相當于無人駕駛汽車的眼和耳，無人駕駛汽車通過環境感知模塊識別自己所處的環境，感知自身位置以及分析處理傳感器傳回的環境信息。導航定位模塊用于確認無人駕駛車所處的地理位置，是路徑規劃的前提和保障。路徑規劃模塊是實現無人駕駛的基礎。路徑規劃問題指機器人按照某些性能指標搜索一條從起始狀態到目標狀態的最優或者近似最優的無碰撞路徑。決策控制模塊用于無人駕駛車下一步的行為決策，用于控制無人車的運動［1］。

路徑規劃是支撐無人車系統自主駕駛的基礎，是無人車研究中的一個基本和關鍵的問題。無人駕駛車的自主駕駛需要路徑規劃模塊給出合理的路線。在自主駕駛系統中，通過傳感器的感知，發現路線中的障礙后，需要路徑規劃模塊合理的處理，規避障礙物。在自主駕駛系統中，無人車完全自主行駛，路徑規劃模塊需要給無人車提供確定的行駛方向等信息。總之，路徑規劃技術是無人車發展中一個至關重要的部分，成熟的路徑規劃技術，可以提高無人車行駛的效率、安全性和穩定性。

本文主要的研究思路是基于支持向量機與受車輛模型約束的D*改進算法。受約束車輛模型對車輛控制軌跡的影響主要包括速度影響和方向影響。受約束車輛模型的研究對整個規劃過程具有非常重要的意義。

2 相關研究工作

對于路徑規劃問題，國內外學者已經探索出很多有效的方法，路徑規劃可以分為全局路徑規劃和局部路徑規劃。全局路徑規劃主要有可視圖法和柵格法等，局部路徑規劃主要有遺傳算法和人工勢場等［2～3］。

可視圖法［4］是20世紀70年代末期提出，其最大的特點是將障礙物描述為多邊形或者多面體。該方法把對象視為一個點，將對象、目標點和障礙物多邊形各個頂點進行組合連接，必須保證所有的組合連線不能穿過障礙物。此方法簡單可行，但是在多維空間環境下，算法變的特別復雜。柵格法［5，7］是20世紀80年代提出，該方法的思想是將環境分解成一系列簡單的網格。柵格法多采用基于位置碼的四叉樹建模方法，搜索策略有靜態的A*算法和動態的D*算法以及它們的一些改進算法。柵格法具有容易創建、維護和理解的優點，能夠存儲和維護的數據量大。人工勢場法［6］是20世紀80年代提出的一種虛擬力法，其基本思想是將物體的運動模擬為一種虛擬的人工力場中的運動，目標點對物體具有吸引力，障礙物點對物體具有排斥力。該方法具有一個明顯的缺點就是可能產生死鎖現象，吸引力和排斥力的平衡使得物體停留在環境中的某一點。遺傳算法是近年來提出的新的算法，將局部路徑規劃推向了智能化和仿生化的方向發展。該算法選取隨機的遺傳因子，多代遺傳后往往會有很好的結果，并且在發生死鎖時允許系統回退。但是由于遺傳因子的隨機性，該算法有很小的概率出現遺傳方向的偏離，產生特別差的規劃效果。

綜合路徑規劃現有的技術，很多研究者提出了多個算法融合的新算法，實踐結果表明，融合算法的效率往往比單一的某個算法要高，同時在實際復雜的環境應用中也具有更高的魯棒性。

3 柵格地圖的構建

3.1 柵格地圖的優點

無人車的路徑規劃模塊僅需要識別出環境中的障礙物，采用激光雷達傳感器獲取環境中的距離信息，對環境的圖形化數據要求不高，只對地圖做障礙物標記處理，選用柵格地圖完全滿足上述需求。柵格地圖數據結構簡單，易于算法的實現，對二維空間數據的處理非常容易，易于與激光雷達數據的同步，對環境信息描述簡單，減少了信息量，易于模擬環境信息。柵格地圖還具有輸出方法快速簡潔，非常有利于算法數據的融合。因此選用柵格地圖來對局規劃的環境進行建模。

3.2 柵格的構建方法

本文使用了一系列節點來存儲柵格地圖中的節點。柵格地圖的每一個網格點都是一個節點，節點中存儲了有關的信息，柵格節點主要用于標記環境中的狀態。對于算法的每個輸入也是以節點的形式傳遞信息。無人車所在的環境是由基本元素組成的矩形區域，每一個元素就是一個柵格。柵格的分辨率為a，即每個柵格的邊長為a。整個柵格的大小和分辨率均可以根據需要對區域進行擴大或者縮小。定義X和Y作為無人車在柵格中的坐標，為柵格中的所有節點按順序進行編號。柵格地圖中的節點可以簡單視為占據、空白和未知三種類型。根據改進的D*算法的需要，為了找到最終的路徑，需要保存柵格中元素的父節點信息，定義存儲當前元素的父節點信息。

算法需要對柵格地圖中的一系列節點進行處理，本文定義算法節點來對柵格信息進行處理，需要保證算法節點與柵格節點的同步性。算法節點的選取采用如圖1所示的方向。

圖1 鄰接節點

上下左右四個方向權值設置為1，四個對角線方向權值設置為1.4。這八個方向類似于八叉樹的八個子節點，除了柵格邊緣的節點，每個節點都存在與其相鄰的八個子節點。

4 受約束車輛模型

一個兩輪驅動的無人車的運動方程如下［8～9］：

其中，x?，y?分別表示在X軸和Y軸上前進的距離，r表示輪子的半徑，η表示車輛模型底盤寬度的一半，?表示前進方向與X軸的夾角，VR，VL分別表示右輪的速度和左輪的速度。?的取值范圍為-＜?＜0和0＜?＜。

經過一系列簡單的數學變換，式（1）可以變形為式（2）：

上述兩個式子分別表示右輪速度與無人車在X軸方向上前進距離的關系和左輪速度與無人車在Y軸方向上前進距離的關系。這樣變換使得實驗用車在單個坐標軸上的位移只與其中一個輪子的速度有關，這樣設計的好處就是方便計算實驗用車的轉彎半徑、輪子轉動圈數等數據。

在實際的實驗環境下，還需要考慮輪子打滑以及里程計本身的誤差造成的位移誤差，我們用ex，ey表示誤差項，因此，公式形式如下：實驗用車和車體坐標系示意如圖2和圖3所示。

圖2 實驗用車

圖3 車體坐標系

本實驗采用圖2所示無人車，車底盤半徑約20cm，共有四個輪子，其中左右輪為主動輪，前后輪為從動輪，故圖3中描繪的車輪只畫出了左右主動輪，而省略掉了前后的從動輪。實驗用車的傳感器是Hokuyo單線雷達，雷達安裝在車的前部距離地面15cm處。

5 D*算法

本文采用柵格法進行環境建模，使用D*算法［10～11］的改進算法進行路徑的搜索。D*算法與A*算法［12］類似，區別在于A*算法是靜態的路徑規劃，而D*算法是動態的路徑規劃。在整個規劃過程中，D*算法允許路徑上障礙物位置隨意的變化。這一點在真實環境下是非常關鍵的。算法需要維護一個OPEN表，OPEN表用于傳遞節點之間權值發生變化后的信息以及計算路徑長度。每一個節點都有一個tag作為標識位，有NEW、OPEN、CLOSED三個狀態。如果一個節點從來沒有出現在OPEN表中，則tag=NEW；如果一個節點當前處于OPEN表中，則tag=OPEN；如果一個節點從OPEN表中移除，則tag=CLOSED。

本章主要介紹D*算法的定義、描述以及受約束車輛模型對D*算法的影響。

5.1 D*算法的簡介

D*算法的目的是移動機器人從起始點S按照規劃的路徑，規避障礙，到達目標點G，衡量路徑優劣的標準是機器人移動過程中的消耗，即成本問題，用 f(S，G)表示。這一問題可以簡單的描述為選取一系列有效節點{X1，…，Xn}，使得機器人按照一定的順序在節點之間移動，直到到達目標點。節點之間連接的弧具有正的權值，這些有效點之間弧的權值之和就是機器人的成本。每個節點Xi（其中1≤i≤n）都有一個后繼節點，這個節點就是后繼節點的父節點，路徑就是通過當前節點與父節點連接而形成的。節點X與其后繼節點之間的消耗定義為C(X，Y)，C(X，Y)是判斷路徑成本是否增加的一個重要參數。X和Y在空間上是鄰接的，即節點Y是以節點X為中心的八個方向上節點中的一個。

定義 g(S，Xi)表示起始點S到當前節點 Xi之間的成本。定義h(Xi，G)表示當前節點 Xi到目標點G之間的成本。因此，總的成本表示為

D*算法主要包含兩部分內容：第一部分用于計算到目標點最優路徑的成本；第二部分用于改變兩個節點之間弧的權值和獲得OPEN表中成本發生變化的節點。算法初始化時，將所有的節點狀態設置為NEW，即tag=NEW。將所有節點的h(Xi，G)設置為0，并將目標點G放入OPEN表中，即tag=OPEN。

算法的偽代碼流程如下：

開始：

1 Insert（Goal Point）；

2 While（return of p（x）＞-1）

3 Return of p（x）=p（x）；

4 end

p（x）：

1 Neighbor（x）；//X節點的鄰接節點

2 Cost（x，neighbor of x）；//鄰接節點花費

3 Insert（neighbor of x）；

動態障礙：

1 If（x==障礙）

2 Modify（）；//修改花費

3 While（return of p（x）＞-1）

4 return of p（x）=p（x）；

5 end

6 end

5.2 受約束車輛模型對D*算法的影響

在實際應用中，D*算法給出的路徑并不能作為無人車運動的軌跡。無人車的運動軌跡受車輛模型的影響比較大，還需要考慮車輛模型對D*算法的影響。

本實驗用車可以實現原地180°轉身，所以在受約束的D*算法中遇到死鎖情況，需要調整實驗用車的速度參數，使其原地掉頭回退一步，實驗用車的這一特性解決了車輛模型對死鎖問題的約束。針對死鎖問題的算法解決方案是出現死鎖位置后回溯到之前不存在死鎖問題的節點上。在回退之后，重新計算當前位置到目的地的最短路徑，同時在遇到路徑消耗相同的兩條路時，采用了路徑優先級，優先選擇最快尋找的路徑，這樣的設計很好地避免了死鎖問題。

本實驗用車的雷達傳感器掃描角度范圍為前方0°～180°，受雷達傳感器的約束，除開死鎖問題，其他環境下應避免向后方的轉向。因此，受車輛模型和雷達傳感器的雙重約束下，采用盡量小的轉彎角度來確保車輛的安全和效率。未改進的D*算法轉彎角度為α=45°，約束后的轉彎角度調整為β1=18.5°和β2=26.5°兩次小角度轉彎。如下圖所示：

圖4 轉彎角度

6 受約束模型下的SVM改進

上述D*算法可以得出全局最優路徑，但是實際情況中，在局部路徑規劃中可能需要對全局的路徑進行一定的平滑處理，這樣做的目的是使得機器人前進的路徑更為有效。考慮到機器人的運動特性，需要根據機器人的轉彎半徑和模型等問題，使路徑較為平滑。本文采用基于SVM的平滑技術，使得機器人的運動更加的流暢，降低消耗。

6.1 支持向量機（SVM）

支持向量機（SVM）是機器學習中的一種分類算法，是一種將樣本劃分為兩類的分類模型，其基本模型定義為特征空間上的間隔最大的線性分類器，即支持向量機的學習策略便是間隔最大化，最終可轉化為一個凸二次規劃問題的求解［13］。

假 設 訓 練 樣 本 集 合 D={(x1，y1)，(x2，y2)，…，，用于劃分兩類樣本的超平面用如下線性方程描述：

其中 w=(w1，w2，…，wd)為法向量，決定超平面的方向；b為位移項，決定了超平面與原點之間的距離。

6.2 基于核函數的SVM

SVM是一個兩類樣本的分類器，與路徑規劃中障礙物點和非障礙物點剛好吻合。SVM中的yi∈{ -1，+1 }，對于障礙物點，取 yi=-1，表示不可通行；對于非障礙物點，取 yi=+1，表示可以通行。以下為不同的核函數確定的障礙物與非障礙物之間的分割平面效果圖對比：

以上三個實驗采用相同的數據，實驗結果均為障礙物與非障礙物之間確定出的分割平面。在本次試驗中，樣本可以視為是在二維的平面上的運動。在局部優化的過程中，我們認為總是存在一個分割平面可以將障礙物與非障礙物進行分類。SVM在無人車的運動環境下可以描述為，尋找局部路徑中障礙物與非障礙物之間的分類平面。為了局部優化D*算法的路徑，我們引入了線性核函數：

圖5 線性核SVM

圖6 Sigmoid核SVM

圖7 高斯核SVM

基于線性核的SVM的原理是在保證實驗用車在障礙物之間可通行的最大寬度內，通過SVM的線性劃分，增大實驗用車的轉彎角度，且同時可以縮短無人車的行進距離［14～18］。

實驗用車車體直徑約40cm，算法描述的兩個障礙物之間的距離小于40cm，則認為兩個障礙物之間不可通行，用上下或者左右鄰接的兩個障礙物柵格表示。兩個障礙物在柵格地圖中處于對角線位置時，若對角線之間的距離大于40cm，則認為可以通行。

假設無人車轉彎因子為a，a受轉彎角度的大小影響；無人車前進距離為x；函數A表示轉彎消耗；函數D表示前進消耗；常量E表示無人車系統的自身消耗；代價函數F(a，x)，表示線性SVM平滑部分的總消耗。

在這種情況下算法應用了線性SVM［19～20］，在確保實驗用車運動到障礙物之間對角線之上后，對后邊的路徑進行線性劃分，這樣就可以在下次遇到障礙物之前調整車的位姿，因此縮短了路徑長度。同時在算法進行線性SVM優化前，無人車是在檢測到下一障礙物之后進行位姿調整，優化后提前調整位姿，無人車具有更大的轉彎角度，節省了無人車的運行時間，提高了效率。

7 實驗及分析

7.1 實驗環境

本次實驗的環境如圖8（a），環境中的障礙物隨機擺放，實驗采集的雷達圖像如圖8（b），圖中可以明顯地看出整個實驗環境的邊界，中間部分的障礙物。邊界左下角部分不夠明顯是因為，雷達的掃描線受玻璃門的影響而沒有返回其邊界的數據。

圖8 實驗環境

7.2 實驗結果

以下為D*算法的一次路徑規劃實驗：實驗環境為VS2010，柵格規模為30×30，柵格節點總數為31×31，黃色矩形圖標代表起點，綠色六角圖標代表終點，黑色矩形圖標代表初始障礙物，紅叉代表動態添加的障礙物。圖9為一次完整的路徑規劃過程，圖9（a）為隨機標記的障礙物，算法給出的最短路徑；圖9（b）為模擬無人車的移動；圖9（c）為路徑點上出現新的障礙，無人車后退一步；圖9（d）為算法給出了最新的最短路徑；圖9（e）為無人車遇到堵塞，搜索周圍給出的最短路徑；圖9（f）為重新規劃并最終到達終點。整個過程可以看出算法不存在死鎖的情況，具有很好的魯棒性。

圖9 一次完整的規劃

由于實驗所在環境的影響，測試時調整了地圖創建的柵格的尺寸。圖10為接收雷達數據后繪制的環境地圖以及最終路徑對比圖，由于雷達采集數據存在一定的誤差，柵格地圖經過了一些手工的標定。以下為未優化的D*算法與線性核SVM優化后的D*算法的結果比較圖（以下對比只選取最終的路徑比較）。

圖10 對比結果圖

表1為多次實驗結果后的得出的路徑長度（單位：CM）。從表中可以看出，基于SVM的受約束D*改進算法比傳統的D*算法具有更短的路徑，同時經過SVM線性平滑后，無人車的轉彎時間會更短。無人車應用本算法有更高的效率。

表1 算法路徑長度對比

8 結語

對于移動機器人的動態路徑平滑規劃問題，本文使用了受約束D*的算法的動態路徑規劃和基于SVM的路徑平滑。受約束D*的算法的路徑規劃在實際的環境下具有較好的表現，考慮到機器人的模型及尺寸以及運動，能夠生成可行的運動軌跡。基于SVM的路徑平滑能夠在處理障礙物與非障礙物的交界處表現較好，可以使得路徑更為的平滑，使移動機器人能夠安全流暢的通過障礙物。實驗時我們設置了多個障礙以檢測算法的正確性，從模擬結果與實際場景的結果，驗證了算法具有有效性和實用性。

［1］趙陽.無人駕駛汽車關鍵技術［J］.應用技術，2011，26：272.

ZHAO Yang.The Key Technologies of Automated Vehicle［J］.China Science and Technology Review，2011，26：272.

［2］朱大奇，顏明重.移動機器人路徑規劃技術綜述［J］.控制與決策，2010，25（7）：961-967.

ZHU Daqi，YAN Mingzhong.Survey on technology of mo?bile robot path planning［J］.Control and Decision，2010，25（7）：961-967.

［3］曲道奎，杜振軍，徐殿國，等.移動機器人路徑規劃方法研究［J］.機器人，2008，30（2）：97-101，106.

QU Daokui，DU Zhenjun，WU Dianguo，et al.Research on Path Planning for a Mobile Robot［J］.ROBOT，2008，30（2）：97-101，106.

［4］T.Lozano-Perez，M.Wesley.An Algorithm for Planning Collision-free Paths Among Polyhedral Obstacles［J］.Communica-tions of the ACM，1979，22（5）：436-450.

［5］M.Metea，J.Tsai.Route planning for intelligence autono?mous land vehicles using hierarchical terrain representa?tion［C］//Ro-botics and Automation.1987 IEEE Interna?tional Conference on，1987：1947-1952.

［6］O Khatib.Real-time Obstacle Avoidance for Manipulators and Mobile Robots［J］.Int J Robotics Research，1986，5（1）：90-98.

［7］張彪，曹其新，王雯珊.使用三維柵格地圖的移動機器人路徑規劃［J］.西安交通大學學報，2013，47（10）：57-61.

ZHANG Biao，CAO Qixin，WANG Wenshan.An Algo?rithm for Mobile Robot Path Planning Based on 3D Grid Map［J］.Journal of Xi`an Jiaotong University，2013，47（10）：57-61.

［8］I.Zohar，A.Ailon，R.Rabinovici.Mobile robot char?ac-terized by dynamic and kinematic equations and actua?tor dynamics：Trajectory tracking and related application.Robotics and Auton.Syst.，2011，59（6）：343-353，

［9］Kuo-Ho Su，Feng-Li Lian，Chan-Yun Yang.Navigation design with SVM path planning and fuzzy-basedpath tracking for wheeled agent［C］//2012 International confer?ence on Fuzzy Theory and Its Applications，2012：273-278.

［10］Anthony Stentz.Optimal and Efficient Path Planning for Partially-Known Environments［M］.The Robotics Insti?tute；Cmegie Mellon University；Pittsburgh，PA 15213.

［11］Anthony Stentz.The Focussed D*Algorithm for Re?al-Time Replanning［C］//Robotics Institute Carnegie Mellon University Pittsburgh，Pennsylvania 15213 U.S.A

［12］P.E.Hart，N.J.Nilsson，and B.Raphael.A formal ba?sis for the heuristic determination of minimum cost paths in graphs［C］//IEEE Trans.Syst.Sci.and Cybernetics，SSC-4（2）：100-107，1968.

［13］N.M.Amato，Y.Wu.A Randomized Roadmap Method for Path and Manipulation Planning［J］.In Proceedings of 1996 IEEE Int.Conf.on Robotics and Automa-tion，pp.113-120，1996.

［14］S.Avidan.Support Vector Tracking［C］//IEEE Trans.On Pattern Analysis and Machine Intelligence，Vol

［15］C.J.Burges.A Tutorial on Support Vector Machines for Pattern Recognition［J］.Data Mining and Knowledge Dis?covery，1998，2（2）：121-167.

［16］ Nikhil ajaj，Niko J.Murrell，Julie G.Whitney，Jan P.Alle?bach；George T.C.Chiu.Expert-prescribed weighting for support vector machineclassification［C］//2016 IEEE International Conference on Advanced Intelligent Mecha?tronics（AIM）.913-918，2016.

［17］Hiroshi Kawase， Yojiro Mori， Hiroshi Hasegawa，Ken-ichi Sato.Dynamic Router Performance Control Uti?liz-ing SupportVector Machines for Energy Consumption Reduction［J］.IEEE Transactions on Network and Ser?vice Manage-ment.Vol.pp，1-1，2016.

［18］ Gaze latent support vector machine for image classifica?tion.Xin Wang，Nicolas Thome，Matthieu Cord 2016［C］//IEEE Interna-tional Conference on Image Process?ing（ICIP）.236-240，2016.

［19］M.Krishna Satya Varma，N.K.K.Rao，K.K.Raju，G.P.S.Varma.Pixel-Based Classification Us-ing Support Vector MachineClassifier［C］//2016 IEEE 6th Interna?tional Conference on Advanced Computing（IACC）.51-55，2016.

［20］Pin-Kao Huang，Jyh-Horng Chou.Uniform design meth?od to finding the optimal parameters for support vector machine［C］//2016 International Conference on System Science and Engineering（ICSSE）.1-3，2016.

An App lication of Constrained D*A lgorithm Based on Support Vector M achine in Automated Vehicle Routing

LIU Xiaotao CAI Yunfei WANG Tiancheng
（College of Computer Science and Engineering，Nanjing University of Science&Technology，Nanjing 210094）

TP391

10.3969/j.issn.1672-9722.2017.09.013

2017年4月1日，

2017年5月17日

國家軍口核高基（編號：2015ZX01041101）；國家自然基金青年項目（編號：61305134）；教育部博士點基金項目（編號：20133219120035）資助。

劉曉濤，男，碩士研究生，研究方向：智能機器人路徑規劃。蔡云飛，男，講師，研究方向：模式識別與智能系統。王田橙，男，碩士研究生，研究方向：智能機器人SLAM算法。