自主駕駛系統路徑規劃之技術研究 - artc.org.tw ·...

中華民國第十八屆車輛工程學術研討會，屏東科技大學車輛工程系，台灣屏東，2013 年 12 月 13 日。 The 18th National Conference on Vehicle Engineering, Dec. 13, 2013, NPUST., Pingtung, Taiwan. F4 – 089 ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

自主駕駛系統路徑規劃之技術研究

王通溫 1、曾柏凱、洪銘鴻

財團法人車輛研究測試中心

1E-mail:[email protected]

科專計畫編號: 102-EC-17-A-01-05-0843

摘要

本研究以開發自主無人駕駛車輛之路徑軌跡規劃

為主，設計一無人自主駕駛車輛行駛於停車場域之路徑

規劃。利用即時動態全球定位系統擷取車輛位置，建立

動態行駛與停車情境下影像與雷射雷達擷取當前路徑

障礙物後的數位地圖，有效的將影像與倒車雷達輔助停

車障礙物偵測與估測，在純追蹤演算法追蹤平行與倒車

停車之車身控制，路徑最佳化透過 Matlab 的模擬，並

利用 LabVIEW 設計模糊邏輯控制與計算出車身動態軌

跡，達成無人自主駕駛行駛於停車場域內的目標，對於

台灣狹小停車空間可協助駕駛者完成自動停車功能。

關鍵詞：車輛軌跡、無人駕駛車、純追蹤演算法。

1. 前言自主無人駕駛技術隨著汽車工業的快速發展與推動下

日趨成熟，例如：自動停車[1~3]、自動跟車系統[4~5]。

然而，現代都市中停車空間不足的問題經常是駕駛者的

困擾，一般駕駛經驗時常遇到繞行目的地週邊許久後找

到的停車空間不足、受駕駛視線的限制、駕駛者估算車

身與障礙物間距與停車後不方便下車等等問題，產生更

多停車碰撞或放棄尋覓許久車位的問題；此外，在自主

駕駛必須考慮的層級在於路徑的規劃，以車輛導航透過

全球定位系統提供車輛位置，透過導航設備所包含的資

料庫來規劃較長距離的路徑，自主駕駛層級而言，需要

的是短距離且即時處理的路徑規劃，例如：車道變換、

障礙物閃避與自動停車的駕駛輔助系統，而大部分系統

涵蓋了環境感知、路徑規劃與駕駛控制三個層級，若可

以結合現代的感知及控制技術，發展一套車子可自動輔

助駕駛者判斷與決策的系統，相信許多駕駛停車的不便

利性議題，即可以在發展自主無人駕駛輔助系統後，獲

得相當的改善。

2. 研究方法本研究主要開發一自主駕駛系統雛形車，藉由先進

駕駛輔助系統的技術整合，將已開發的車道偏移警示、

前方防撞警示、慣性導航與即時動態全球衛星定位系統

等技術整合，以投入停車輔助技術的開發。主要的內容

包含停車場情境設計、車輛動態模型、停車空間估計、

車輛位置估測與路徑規劃等，以上技術包含停車空間偵

測與定位：結合超音波與影像感測器，進行空間量測定

位；停車路徑規劃與路徑追蹤控制：系統依據該車的起

始位置與迴轉半徑，進行路徑規劃與追蹤動作；車輛慣

性定位與電動轉向控制：主要為車輛相對位置推算與方

向盤轉向控制；人機操作顯示界面：提供系統與駕駛者

方便的互動。

2.1 停車場情境設計自主駕駛停車場情境如圖 1.示意圖，本研究針對無

人駕駛車輛行駛於停車場域之路線建立數值化地圖，於

自主駕駛車輛規劃到達的路段提供如航點信息，停止標

誌的位置，車道寬度，檢查點的位置和停車點的位置。

圖 1(b)顯示設定的航點有三種型態，退出航點(Exit

waypoint, 標示為紅色圓點)表示為車輛駛離本路段可

進入下一或二路段航點;進入航點(Entry Waypoint, 標

示為藍色圓點)分佈於路段進入點與鄰接路段中(檢查航

點 Checkpoint Waypoint, 標示為綠色圓點)等。為準確維

持車輛行駛路線，研究設計任務路徑結合即時動態全球

衛星定位系統將目前車輛所在位置與航向資訊透過系

統讀取其任務路徑與檢查航點追蹤，以維持目前航向正

確性，任務路徑透過系統將車輛控制命令傳送至車輛控

制元件，其命令格式如圖 1(b)圖顯示停車系統路徑設計

相關航點，右方附表顯示，每一道路路段區塊設定，其

格式涵蓋車輛行駛的軌跡資訊與停車場道路速限資

訊，設定為每一路段皆必須執行一次設定。

圖 1 (a)自主駕駛停車場情境示意圖

mailto:[email protected]


圖 1 (b)自主駕駛軌跡航點示意圖

考量一般停車場速度限制，本研究之系統停車速度為每

小時 5 公里，一般障礙物閃避時速為 5~10 公里，因此，

系統轉向控制可忽略輪胎的側向力，在低速時可滿足阿

克曼幾何轉向(Ackerman condition)理論值。

2.2 停車場域路徑規劃本研究主要以即時動態全球衛星定位系統，如圖 2

所示架構，使用者可上網預約停車場目前可停位置狀

態，透過使用者介面選擇所需要之停車位置，停車系統

回應停車位置資訊，建立出車輛行駛路徑，以提供無人

駕駛車輛追蹤路徑。圖 3 顯示自主駕駛車輛控制流程，

自主駕駛車輛配置車輛位置估測與停車位置估測演算

法，以個人行動電腦為基礎計算所需要之行車路徑，此

外，系統中需要環境偵測資訊，透過 LM291 雷射雷達

與車載攝影機蒐集行駛路徑障礙物與可行駛的空間，系

統會建立一張數位地圖顯示車輛與環境中觀測的人車

物相對應的距離與角度關係，車輛透過車身網路將方向

與輪速提供慣性導航電路計算目前車身姿態，系統建立

當前速度下最佳行駛路徑，駕駛者可於停車場入口下

車，駕駛者參考最佳路徑與停車格建議後透過智慧裝置

命令車輛以自主駕駛車輛行駛至停車格，車載系統將執

行系統規劃之路徑，透過純追蹤演算法與模糊邏輯控制

自主駕駛車輛至停放位置。圖 4 說明使用者與車載系統

透過網路獲得車輛目前行駛位置顯示，告知駕駛者當前

車輛狀態與位置，完成停車動作。

圖 2 自主駕駛架構示意圖

圖 3 自主駕駛車輛控制流程圖

圖 4 自主駕駛車輛行駛路徑圖，左圖顯示市售 GPS

量測停車軌跡路徑；右圖顯示 Google Map呈

現停車軌跡圖。

2.3 車輛動態模型本研究以車輛運動模型建立車輛與停車環境的相

對關係，結合車體的行進特性以規劃最佳行駛路徑。圖

5說明車輛動態模型主要是以阿克曼轉向幾何關係進行

建立移動路徑，因其理論假設車輛速度是非常小的條件

時成立，此時車輛的側向力是可忽略[6]。因而車輛的運

動受到 Non-Holonomic 限制，要控制車輛倒車至停車定

點，就必須跟隨車輛的運動特性(Car-Like Motion)去設

計控制系統，而為了能準確的預測倒車時的行進路徑，

車輛實際的運動軌跡必須加以數學模式化。其中(xf , yf)

為前軸中心點的座標，(xr ,yr)為後軸中心點座標，v 為

前軸中心點的速度，l 為軸距，w 為後輪距，ψ (等於

圖 5 之ψ)為前軸中心點轉向角，θ為車輛中心軸與水

平方向的夾角。基於後輪無滑移現象，亦即後輪軌跡的

徑向速度為零，其表示如下：

0sincos

rr xy (1) 由圖 2 可得到前、後軸中心點的座標關係為：

coslxx fr (2)


sinlyy fr (3) 將(2)~(3)代入(1)式可以獲得

0sincos

lxy ff (4) 為獲得方向角度與車速間的關係，可由圖 2 獲得前軸中

心點的 x、y方向速度

)cos(

vx f (5)

)sin(

vy f (6)

將(5)~(6)代入(4)式可以獲得車輛迴轉角速度

l

v

s i n

(7)

將(2)~(3)微分後可得其速度關係

sinlxx fr

(8)

coslyyfr

(9)

由(5)~(7)代入(8)~(9)可求得後軸中心點 X、Y 方向速度

為

sincosvx r

(10)

cossinvy r

(11)

其次將(7)積分後代入(10)~(11)，再對時間積分即可獲得

後軸中心點的軌跡

tl

vldtvtxr

sinsincotcoscos)( (12)

dtvtyr cossin)(

cotsin

coscot ltl

vl (13)

然而，(12)與(13)式主要由前輪轉向所獲得的轉向角，

需要藉由方向盤轉角的量測，來預測倒車時後輪的行進

軌跡，經由圖 3 輪速幾何轉向可以獲得左、右後輪所行

走的距離為SR與SL，Rwheel 為輪子半徑，NR與NL為輪速的轉數。

RwheelR NRS (14)

LwheelL NRS (15)

圖 5 車輛運動學模型示意圖

由(14)與(15)式可以獲得速度與時間和左右後輪移動距

離的關係

T

SSv LRr

1

2 (16)

rear

LR

l

SS (17)

藉由所得到的後輪速與轉向角建立汽車的座標系統如

下：

Tvxx rrr cos (18)

Tvyy rrr sin (19)

圖 6.輪速幾何轉向示意圖

透過(12)與(13)方程式計算後可以獲得車輛後軸心軌跡

可藉圖 6 之幾何關係延伸描繪左右後輪的軌跡點，因積

分時的時間需要較小的間隔，本研究以每 0.1 秒及輪胎

轉向角 30 度代入方程式計算，可獲得圖 7 之左右後輪

軌跡描繪點。

圖 7 模擬倒車軌跡路徑圖

2.4 行駛空間估測法與路徑決策機制自主駕駛停車導引系統的停車空間偵測，需要準確

判斷道路兩旁車輛之位置，以計算停車空間是否足夠，

並預估停車格的二維座標。本研究在路徑規劃演算執行

前，透過車載影像、雷射雷達蒐集即時的道路障礙位

置，依據雷射雷達產生的距離與角度與行駛車輛速度上

的關係，經過影像辨識確認障礙物的類別後，建立一張

模糊控制的電子地圖，如圖8所示。地圖中以車輛為中

心並區分8個方向監控四周的靜態或動態物體，車輛行

進時由動態方程式與方向轉角的關係建立當前速度的


曲率與可行駛路徑估測，其中行駛方向與方向轉角的關

係透過阿克曼轉向角獲得，計算核心估測出行駛時左右

轉向合理的轉向曲率，產生車輛當前速度下難易轉向角

度，如圖中藍色線接近LF(難轉向)與接近F(易轉向)的估

測曲線。

圖 8. 模糊控制演算法情境示意圖

圖 9(a)模糊邏輯控制車輛前進演算的歸屬函數

車輛行駛控制策略以模糊邏輯控制為主，圖 8 顯示

依據 2.2 小節所建立之人車物位置電子地圖，利用人機

介面呈現影像範圍內可辨識的相關位置於地圖中，依據

辨識後障礙物角度與距離關係，設計模糊控制，此圖為

一張 100100 公尺的數值圖，受限於 LM291 可辨識距

離約為 80 公尺設計，並考量影像辨識距離約有 25 公

尺，設計出模糊控制系統，在可行駛的範圍內操作車輛

行駛至目標位置，圖 9(a)顯示模糊邏輯控制前進控制的

歸屬函數，圖 9(b) 模糊邏輯控制倒車控制的歸屬函

數，輸入的變數有二，考慮行駛的位置與方向為主；輸

圖 9(b)模糊邏輯控制車輛倒車演算的歸屬函數

圖 10(a)模糊邏輯控制車輛前進關係函數

圖 10(b)模糊邏輯控制車輛倒車關係函數

出變數為方向盤控制角度，車輛之停車速度為 5 公


里不控制油門的操作設計停車狀態。圖 10(a)模糊邏輯

控制車輛前進關係函數圖，圖 10(b)模糊邏輯控制車輛

倒車關係函數圖，目前系統以模擬測試是否可執行，配

合其他系統演算技術整合後，將裝車測試可行性。

2.5 路徑控制演算法車輛路徑規劃採用Pure Pursuit algorithm [7]進行路

徑追蹤。Pure Pursuit algorithm為一成功被應用於目標追

蹤的演算法。Pure Pursuit algorithm主要目的在於使得自

主車輛可以正確地行駛於規劃的路徑上，其主要作用為

分段控制自主車輛的轉向曲率，讓自動駕駛車輛行駛於

規劃之路徑上。由於是室外停車場，且利用RTK-GPS

精準定位出停車場的行車路徑，因此我們利用Pure

Pursuit algorithm追蹤停車場的行車路徑將可讓自動駕

駛車輛控制於規劃的停車場行車路徑。

Pure Pursuit 之原理如圖 11 所示，自動駕駛車輛以自身

後軸中心點為原點(0,0)，視追蹤路徑上的某一點(x,y)為

當前追蹤目標點，兩點直線距離為 l, 輛旋轉半徑為 r,

因此我們可以得到下列兩個式子: 222 lyx (20)

rdx (21) 由(21)得到,

xrd (22) 由圖 XX 再得到關係式,

222 ryd (23)

圖 11. Pure Pursuit 追蹤方法 [7]

將(22)帶入(23)，得到

222 ryxr (24) 簡單整理後得到,

x

lr

2

2

(25)

因此，利用(25)將可得到以自身後軸中心點為原點(0,0)

到追蹤路徑上的某一點(x,y)的曲率,

2

2

l

x (26)

當知道迴轉曲率即可知道車輛方向盤轉角。因此，自動

駕駛車輛便可行駛於規劃之路徑上。如圖 12, 為一 Pure

Pursuit 路徑追蹤示意圖。圖中自動駕駛車輛以 A 點為

起始點，開始追蹤紅色已規劃路徑，藍色路徑為自動駕

駛車輛行經路徑。最後自動駕駛車輛於 B 點成功行駛於

已規劃路徑上。

A B

圖 12 Pure Pursuit 路徑追蹤示意圖。

2.6 車輛系統架構與功能實做車輛有效率的停入空間可分解三個程序，首先是依

據車輛目前的位置，計算出程式初始值以推導出目標空

間座標，接續第二個動作是要計算出何處是反轉方向盤

之位置，以便後續操作順利進入停車空間，此時的車輛

行進方向是往後進行的；最後的動作即操控車輛向前移

動至停車空間之中央位置。

圖13自主駕駛車輛感測系統

研究所使用的系統環境感測空間說明如圖13所

示，系統包含雷射雷達、機器視覺、盲點偵測技術與後

方倒車與後方倒車影像等功能運用於自主駕駛車輛環

境感知系統。圖14(a) RTK-GPS與一般市售GPS同時擷

取訊號圖匹配在Google Map呈現即時位置，自主駕駛車

實作上採用兩顆即時全球定位系統(RTK-GPS)分別置

於車輛重心與後輪軸中心的車頂上，以即時獲得車輛航

向數據。圖14(b)說明一般市售GPS在轉彎處與橫向行駛

時容易出現漂移，其軌跡點並非在實際道路上。14(c)

顯示出兩顆即時全球定位系統航跡圖，藉由此圖可說明

定位的準確度，同時擷取出軌跡與航向等資訊提供自主


車輛控制。

圖14(a) RTK-GPS與一般市售GPS同時擷取訊號圖。

圖14(b) RTK-GPS與一般市售GPS訊號圖。

圖14(c)同時接收兩顆RTK-GPS訊號圖。

另一方面，論文的路徑追蹤是先利用模擬的方式計

算出系統最佳行駛路徑，有些重要的參數，包含有車輛

的寬度與車輛最小迴轉半徑，與停車格空間資訊等，圖

15為倒車停車模式模擬，路徑透過Matlab的模擬倒車軌

跡，紅色與藍色為障礙物，黃色標示為行駛車輛，以倒

車方式行駛進入狹小空間，因此模擬時並未參考現有停

車空間設計，未來應用於實際停車空間設計時，可朝向

較高密度的停車空間來設計停車場停放位置。

圖 15 倒車入庫模式停車軌跡示意圖

3. 結論利用即時動態全球定位系統擷取車輛位置，建立動

態行駛與停車情境下影像與雷射雷達擷取當前路徑障

礙物後的數位地圖，有效的將影像與倒車雷達輔助停車

障礙物偵測與估測，在純追蹤演算法追蹤平行與倒車停

車之車身控制，路徑最佳化透過Matlab的模擬，並利用

LabVIEW設計模糊邏輯控制與計算出車身動態軌跡，達

成無人自主駕駛行駛於停車場域內的目標，對於台灣狹

小停車空間可協助駕駛者完成自動停車功能。

4. 致謝本研究特別感謝經濟部技術處創新前瞻技術研究計畫

編號：102-EC-17-A-01-05-0843 給予的支持。此外，特

別感謝本中心許展維博士給予論文上的協助。

5. 參考文獻 [1] J. Timpner, L. Wolf, “A Back-end System for an

Autonomous Parking and Charging System for

Electric Vehicles”, Electric Vehicle Conference

(IEVC), 2012 IEEE International, pp. 1-8, 2012.

[2] P. Masoumi Moghri, M.R. Karami, R. Ghaderi, “A real-time intelligent parallel parking system for a car

like mobile robot”, Artificial Intelligence and Signal

Processing (AISP), 2012 16th CSI International Symposium on. pp. 532-537, 2012

[3] D.B. Wang, H.W. Liang, T. Mei, H. Zhu “Research on self-parking path planning algorithms”, Vehicular

Electronics and Safety (ICVES), 2011 IEEE

International Conference on. pp. 258-262, 2011。 [4] K.D. Kusano, H.C. Gabler, “Safety Benefits of

Forward Collision Warning, Brake Assist,

and Autonomous Braking Systems in Rear-End

Collisions”, Intelligent Transportation Systems, IEEE

Transactions on. pp. 1546-1555, 2012

http://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Timpner,%20J..QT.&searchWithin=p_Author_Ids:38233250700&newsearch=truehttp://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=6179369http://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=6179369http://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Masoumi%20Moghri,%20P..QT.&searchWithin=p_Author_Ids:38488383700&newsearch=truehttp://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Masoumi%20Moghri,%20P..QT.&searchWithin=p_Author_Ids:38488383700&newsearch=truehttp://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Masoumi%20Moghri,%20P..QT.&searchWithin=p_Author_Ids:38488383700&newsearch=truehttp://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=6305329http://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=6305329http://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=6305329http://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Daobin%20Wang.QT.&searchWithin=p_Author_Ids:37963653400&newsearch=truehttp://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Huawei%20Liang.QT.&searchWithin=p_Author_Ids:37397743300&newsearch=truehttp://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Tao%20Mei.QT.&searchWithin=p_Author_Ids:37273533300&newsearch=truehttp://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Hui%20Zhu.QT.&searchWithin=p_Author_Ids:37958730800&newsearch=truehttp://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=5975664http://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=5975664http://ieeexplore.ieee.org/xpl/mostRecentIssue.jsp?punumber=5975664http://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Kusano,%20K.D..QT.&searchWithin=p_Author_Ids:38518378500&newsearch=truehttp://ieeexplore.ieee.org/search/searchresult.jsp?searchWithin=p_Authors:.QT.Gabler,%20H.C..QT.&searchWithin=p_Author_Ids:38018433600&newsearch=truehttp://ieeexplore.ieee.org/xpl/RecentIssue.jsp?punumber=6979http://ieeexplore.ieee.org/xpl/RecentIssue.jsp?punumber=6979


[5] Y. Kageyama, Look, no hand! New Toyota parks itself. http://www.cnn.com, 14 January (2004)

[6] R. N. Jazar, Vehicle Dynamics : Theory and Application, Springer, 2008.

[7] R. C. Coulter, “Implementation of the pure-pursuit path tracking algorithm,” Tech. Rep.

CMU-RI-TR-92-01, Robotics Institute, Carnegie

Mellon University, Pittsburgh, Pa, USA, 1992
http://www.cnn.com/

自主駕駛系統路徑規劃之技術研究 - artc.org.tw ·...

Documents