無人車導航路徑關鍵點插值算法

陸　峰，章永進，李　鵬，趙　明

(1.軍事交通學院 研究生管理大隊，天津 300161； 2.軍事交通學院 軍用車輛系，天津 300161；3.軍事交通學院 聯合投送系，天津 300161； 4.96819部隊，北京 100088)

?

無人車導航路徑關鍵點插值算法

陸峰1，章永進2，李鵬3，趙明4

(1.軍事交通學院 研究生管理大隊，天津 300161； 2.軍事交通學院 軍用車輛系，天津 300161；3.軍事交通學院 聯合投送系，天津 300161； 4.96819部隊，北京 100088)

為解決無人車導航路徑關鍵點插值問題，利用Catmull-Rom樣條插值方法對關鍵點插值算法進行研究。在分析Catmull-Rom樣條插值算法原理的基礎上進行編程，并綜合考慮比較B樣條插值和三次樣條插值方法的優缺點，提出利用Catmull-Rom樣條插值方法對關鍵點進行插值。經驗證，該插值方法使插值曲線經過關鍵點，且插值算法不受坐標分布的影響，并對提供的點坐標的排列沒有嚴格要求，能夠滿足無人車路徑選擇的需要。

插值算法；導航路徑關鍵點；無人駕駛汽車

無人車要實現自主導航，可利用的地圖有兩種，一種是使用精確地圖，即利用精確專業的地圖規劃路線，得到無人車行駛的關鍵點。但這種方法需要制作精確專業的地圖，其制作和使用的成本較高。另一種方法就是利用現有的地圖，獲取地圖規劃的關鍵點，然后針對這些關鍵點進行插值，以提供給無人車使用。

百度地圖應用程序界面(application program interface，API)技術能夠為我們提供方便快捷的路徑導航檢索功能。百度地圖依據給定的起點終點的名稱、坐標等信息，可以較為迅速地由路網規劃出用戶所需的線路。通過調用百度地圖API技術，及時獲取規劃路線上關鍵點的信息，包括它的百度坐標等，這對無人車的駕駛導航有很大的幫助。但是，通過實踐發現，調用百度地圖API獲取的關鍵點較為稀疏，通常是幾十米或幾百米才有一個定位點，即其提供的道路點很少。而無人車對導航的要求約0.5 m一個點，因此，點的數量遠遠不能保證無人車的正常行駛。

通過對百度地圖API提供的關鍵點進行插值，使關鍵點變多、變密，滿足無人車導航的要求，是無人車研究中的一個重要問題。插值算法必須要滿足：第一，插值算法效率一定要高，否則會對車輛的導航和行駛造成很大的影響；第二，插值算法必須能滿足各種點的插值，不受坐標分布的影響；第三，考慮到不在車道上的關鍵點對無人車來說是無效的，插值算法還要求能經過所給定的關鍵點。

為了解決這個問題，人們已經提出了許多種樣條插值方法。如Bravo等[1]提出了β樣條插值方法；Berglund等[2]提出了將貝塞爾曲線引用至路徑規劃中；王幼民等[3-4]做了B樣條曲線軌跡優化的研究；Shizizu等[5]提出了利用回旋曲線進行軌跡優化的研究；文獻[6]在機器人軌跡生成算法中利用了三次樣條插值算法。這些方案在無人車導航路徑插值計算中能夠滿足一定程度的利用，但其應用范圍仍然有限。貝塞爾曲線雖然能使關鍵點形成圓滑的曲線，但有一定的弊端：一方面，特征多邊形頂點數決定了它的階次數，當頂點數較大時，不僅計算量增大，穩定性降低，且控制頂點對曲線的形狀控制減弱；另一方面，不具有局部性，即修改一個控制點對曲線產生全局性影響。針對以上弊端，1972年Gordon等用B樣條基函數代替Bernstein基函數，從而改進上述缺點。盡管它能使關鍵點通過插值形成圓滑的曲線，不受坐標分布的影響，且效率高，但其不能通過關鍵點，這會導致路徑規劃失效[7]。三次樣條插值能較好地在通過關鍵點的情況下擬合成圓滑的曲線，但其形成的原理對關鍵點的坐標提出較高的要求，很難得到廣泛的應用[8-9]。

本文在宏觀的路線規劃基礎上，綜合學習比較各種插值方法，引入了Catmull-Rom樣條插值方法，使宏觀規劃出的關鍵點得到細化，能夠供無人車導航決策使用[10]。

1　樣條插值算法的分析

1.1B樣條曲線的分析

第i段n次B樣條曲線的數學表達式為

(1)

式中：Pi,n(u)為n次B樣條插值曲線函數；Pi+k為插值點；Nk,n(u)為n次B樣條基函數，也稱B樣條分段混合函數。

在式(1)中，0≤u≤1；i=0,1,…，m，故可以看出B樣條曲線是分段定義的。如給定m+n+1個頂點Pi(i=0,1，…，m+n)，則可定義m+1段n次的參數曲線。

Nk,n(u)的表達式為

(2)

B樣條曲線的優點是修改某一控制點只引起與該控制點相鄰的曲線形狀發生變化，遠處的曲線形狀不受影響。其特點包括嚴格的凸包性、分段參數多項式、可微性或連續性、幾何不變性、局部可調性、變差縮減性等。這些優點使得B樣條曲線廣泛應用于交互式自由曲面設計。

1.2三次樣條曲線的分析

三次樣條曲線S(x)是一個分段定義的公式。給定n+1個數據點，共有n個區間。假設有以下節點：

x:a=x0

y:y0y1…yn

三次樣條曲線方程滿足以下條件：

(1)在每個分段區間[xi,xi+1](i=0,1，…,n-1,x遞增)，S(x)=Si(x)都是一個三次多項式；

(2)滿足Si(x)=yi(i=0,1，…,n)；

(3)S(x)、導數S′(x)、二階導數S″(x)在區間[a,b]都是連續的，即S(x)曲線是光滑的。

結合實際情況的樣條曲線的端點條件，即自由邊界、固定邊界、非節點邊界等，可以得出三次樣條的曲線方程(如圖1所示)。從其構造過程可看出給定點是要求橫坐標從小到大排列，不能有相同橫坐標的點。當插值點的橫坐標相差很小而縱坐標相差較大點時插值效果很差。可以看出，三次樣條曲線未能達到無人車決策時的要求，應用效果不佳。

圖1　三次樣條擬合曲線

1.3Catmull-Rom樣條曲線的分析

Catmull-Rom樣條插值是一種分段函數，它的特征就是每一個點pi處的切線值與它的前后相鄰的兩個點的斜率成一定的比例，即為t(pi+1-pi-1)。

Catmull-Rom樣條為一階連續，其中t就是張力系數，它影響了擬合曲線在控制點的彎曲程度，Catmull-Rom樣條通常將此設定為0.5。

對每兩個點pi-1、pi進行分段考慮(如圖2所示)，在每一段上，它受4個控制點的影響，分別是pi-2、pi-1、pi、pi+1，既然它是三次的，它就能通過以下多項式函數來表示[11]：

(3)

式中c0、c1、c2、c3為多項式系數。

圖2　Catmull-Rom樣條曲線原理

為了表示一些參數，本文利用如下初始條件：

(4)

將式(4)代入式(3)中得

(5)

由方程組(5)可解得各個系數值：

(6)

由此可得

(7)

2　Catmull-Rom樣條插值算法實驗結果

2.1經過給定關鍵點的比較

為了比較Catmull-Rom樣條插值函數結果和樣條插值函數計算結果的優劣，在Microsoft Visual Studio 2008開發環境中，首先給出7個關鍵點，設置在相鄰兩個點間插值點的數量為5，比較兩種方法的結果(如圖3—5所示)。

圖3　三次均勻B樣條插值

圖4　三次樣條插值

圖5　Catmull-Rom樣條插值

圖3—5中，黑色框中的點表示給出的這7個關鍵點，叉形點表示利用不同樣條插值算法根據這7個點所繪制出來的插值點。

由圖3可知，三次均勻B樣條形成的曲線較為平滑，而且效率較高，但是不能通過給定的關鍵點。由圖4可知，三次樣條曲線盡管能經過給定的控制點，但根據其原理和在編程時發現，其插值需要橫坐標從小到大分布，受關鍵點坐標影響。而由圖5可知，Catmull-Rom樣條擬合曲線可以較好地解決上述問題，不僅能通過關鍵點，插值時還不受關鍵點坐標的影響。

3種插值算法的比較見表1，結合上述的討論，考慮B樣條曲線和三次樣條曲線的特點，本文引入了Catmull-Rom樣條插值作為曲線插值的方法，不僅能滿足經過所有關鍵點的特點，還能較好地形成閉合曲線，滿足實驗與工作的需求，取得較好的結果。

表1　3種插值算法比較表

在編程測試中，將這7個點每兩個點插入200個點，即插值后生成1 400個點，所需要的時間為11 ms左右。而無人車在運行時大約每100 ms處理幾百個道路點，因此Catmull-Rom樣條插值在效率上能夠支持無人車決策的需要。

2.2Catmull-Rom樣條擬合閉合曲線

為了仿真Catmull-Rom樣條曲線擬合閉合回路的效果，本文給出了4個關鍵點，坐標分別為(150,140)、(200,190)、(250,140)、(200,90)。

圖6中，黑色方形點表示給定的4個點，叉形點表示根據這4個點，利用Catmull-Rom樣條曲線分別在兩個點間插入5個點所繪制出來的插值點。通過編程可以看出，此插值點不受點坐標影響，并能經過給定控制點。根據式(3)，各條曲線對應系數見表2。

圖6　Catmull-Rom樣條插值形成閉合回路實踐效果

多項式系數c0c1c2c3曲線1x250150250250y120220320220曲線2x-10001000y01000-100曲線3x-100200100-200y200100-200-100曲線3x100-100-100100y-100-100100100

2.3Catmull-Rom樣條擬合與百度地圖的結合

為了更好地體現曲線插值的效果與實際用途，利用JavaScript結合百度地圖API制作了百度地圖的網頁，并將其嵌入到C#開發出的程序中(如圖7、8左半部分所示)。通過給定起終點，利用百度地圖API獲取路徑規劃后的關鍵點坐標。將百度坐標通過回調函數近似轉化為WGS-84坐標。并將這種二維坐標點轉化為OpenGL下的三維坐標點，進行顯示。在每兩個點間插入10個點，插值后坐標顯示，并將插值之后的點加入繪制，進行對比，取得較好的效果(如圖7、8右半部分所示)。

圖7　關鍵點的OpenGL顯示

圖8　插值后關鍵點的OpenGL顯示

3　結　語

本文在獲得關鍵點的基礎上，將Catmull-Rom樣條插值算法應用到無人車路徑插值中，并將其與B樣條插值算法和三次樣條插值算法作對比，從理論和仿真實驗上證明了Catmull-Rom樣條插值算法的可行性，此算法不僅能彌補B樣條插值不能經過關鍵點的缺點，也能解決三次樣條插值受坐標影響的弊端，而且算法效率較高，能夠滿足無人車決策的需要。

[1]GARGI U, KASTURI R, STRAYER S H. Perfonnance characterization of video-shot-change detection methods[J].IEEE Circuits and System for Video Technology,2000,10(1):1-13.

[2]HAMPAPUR A, JAIN R, WEYMOUTH T.Digital video segmentation[G]//New York,Proceedings of the Second ACM International Conference on Multimedia, 1994:357-364.

[3]王幼民, 徐蔚鴻.機器人連續軌跡控制中的B樣條軌跡優化[J].機械設計,2000,10(10):33-34.

[4]王幼民.機器人連續軌跡控制中的Bezier曲線軌跡優化與控制[J].機械傳動,2003,3(3):43-47.

[5]SHIMIZU M,KOBUYASHI K,WATANABE K.Clothoidal curve-based path the generation for an autonomous mobile robot[G]//Proc of the 2006 International Joint Conference SICEICASE, 2006:478-481.

[6]張小江,高秀華.三次樣條插值在機器人軌跡規劃應用中的改進研究[J].機械設計與制造,2008,9(9):170-172.

[7]任重,楊燦軍,陳鷹.軌跡規劃中的B樣條插值算法[J].機電工程,2001,18(5):38-39.

[8]彭輝,曾碧.Hermite三次樣條插值的車型機器人路徑規劃研究[J].計算機工程與應用,2010,46(22):221-224.

[9]陳弘,劉海,喬勝華,等.基于三次樣條插值的車輛行駛數據分析[J].汽車技術,2013(8):54-57.

[10] 林夏菲,吳鳳鳴. Catmull-Rom插值算法在基于OpenGL的三維地形繪制中的應用實現[J].電腦知識與技術,2008,3(4):788-789,793.

[11]CATMULL E,ROM R.A Class of Local Interpolating Splines:Computer-Aided Geometric Design[M].San Francisco: Academic Press,1974:105.

(編輯：張峰)

Interpolation Algorithm in Key Points of Unmanned Vehicle Navigation Path

LU Feng1, ZHANG Yongjin2, LI Peng3, ZHAO Ming4

(1. Postgraduate Training Brigade, Military Transportation University, Tianjin 300161, China; 2. Military Vehicle Department, Military Transportation University, Tianjin 300161, China; 3. Joint Projection Department, Military Transportation University, Tianjin 300161, China; 4.Unit 96819, Beijing 100088, China)

To solve the problem of interpolation in key points of unmanned vehicle navigation path, the paper studies interpolation algorithm of key points with Catmull-Rom spline interpolation method. After analyzing the algorithm principle of Catmull-Rom spline interpolation, it programs and compares the advantages and disadvantages between B-spline interpolation and cubic spline interpolation method, and puts forward the method of interpolating key points with Catmull-Rom spline interpolation method. The test shows that this interpolation method makes interpolation curve going through key points and the interpolation algorithm will not be affected by coordinate distribution, and the arrangement of point coordinates is not strictly required.

interpolation algorithm; key points of navigation path; unmanned vehicle

2015- 07- 21；

2015- 09- 06．

國家自然科學基金重大項目(91220301).

陸峰(1993—)，男，碩士研究生.

10.16807/j.cnki.12-1372/e.2016.02.021

U412.3

A

1674-2192(2016)02- 0089- 05