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

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

?

無人車導(dǎo)航路徑關(guān)鍵點插值算法

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

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

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

插值算法；導(dǎo)航路徑關(guān)鍵點；無人駕駛汽車

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

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

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

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

本文在宏觀的路線規(guī)劃基礎(chǔ)上，綜合學(xué)習(xí)比較各種插值方法，引入了Catmull-Rom樣條插值方法，使宏觀規(guī)劃出的關(guān)鍵點得到細化，能夠供無人車導(dǎo)航?jīng)Q策使用[10]。

1　樣條插值算法的分析

1.1B樣條曲線的分析

第i段n次B樣條曲線的數(shù)學(xué)表達式為

(1)

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

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

Nk,n(u)的表達式為

(2)

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

1.2三次樣條曲線的分析

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

x:a=x0

y:y0y1…yn

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

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

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

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

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

圖1　三次樣條擬合曲線

1.3Catmull-Rom樣條曲線的分析

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

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

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

(3)

式中c0、c1、c2、c3為多項式系數(shù)。

圖2　Catmull-Rom樣條曲線原理

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

(4)

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

(5)

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

(6)

由此可得

(7)

2　Catmull-Rom樣條插值算法實驗結(jié)果

2.1經(jīng)過給定關(guān)鍵點的比較

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

圖3　三次均勻B樣條插值

圖4　三次樣條插值

圖5　Catmull-Rom樣條插值

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

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

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

表1　3種插值算法比較表

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

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

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

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

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

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

2.3Catmull-Rom樣條擬合與百度地圖的結(jié)合

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

圖7　關(guān)鍵點的OpenGL顯示

圖8　插值后關(guān)鍵點的OpenGL顯示

3　結(jié)　語

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

[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]王幼民, 徐蔚鴻.機器人連續(xù)軌跡控制中的B樣條軌跡優(yōu)化[J].機械設(shè)計,2000,10(10):33-34.

[4]王幼民.機器人連續(xù)軌跡控制中的Bezier曲線軌跡優(yōu)化與控制[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]張小江,高秀華.三次樣條插值在機器人軌跡規(guī)劃應(yīng)用中的改進研究[J].機械設(shè)計與制造,2008,9(9):170-172.

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

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

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

[10] 林夏菲,吳鳳鳴. Catmull-Rom插值算法在基于OpenGL的三維地形繪制中的應(yīng)用實現(xiàn)[J].電腦知識與技術(shù),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．

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

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

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

U412.3

A

1674-2192(2016)02- 0089- 05