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

陸　峰，章永進(jìn)，李　鵬，趙　明

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

?

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

陸峰1，章永進(jìn)2，李鵬3，趙明4

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

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

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

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

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

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

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

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

1　樣條插值算法的分析

1.1B樣條曲線的分析

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

(1)

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

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

Nk,n(u)的表達(dá)式為

(2)

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

1.2三次樣條曲線的分析

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

x:a=x0

y:y0y1…yn

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

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

(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é)合實(shí)際情況的樣條曲線的端點(diǎn)條件，即自由邊界、固定邊界、非節(jié)點(diǎn)邊界等，可以得出三次樣條的曲線方程(如圖1所示)。從其構(gòu)造過(guò)程可看出給定點(diǎn)是要求橫坐標(biāo)從小到大排列，不能有相同橫坐標(biāo)的點(diǎn)。當(dāng)插值點(diǎn)的橫坐標(biāo)相差很小而縱坐標(biāo)相差較大點(diǎn)時(shí)插值效果很差?？梢钥闯觯螛訔l曲線未能達(dá)到無(wú)人車(chē)決策時(shí)的要求，應(yīng)用效果不佳。

圖1　三次樣條擬合曲線

1.3Catmull-Rom樣條曲線的分析

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

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

對(duì)每?jī)蓚€(gè)點(diǎn)pi-1、pi進(jìn)行分段考慮(如圖2所示)，在每一段上，它受4個(gè)控制點(diǎn)的影響，分別是pi-2、pi-1、pi、pi+1，既然它是三次的，它就能通過(guò)以下多項(xiàng)式函數(shù)來(lái)表示[11]：

(3)

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

圖2　Catmull-Rom樣條曲線原理

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

(4)

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

(5)

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

(6)

由此可得

(7)

2　Catmull-Rom樣條插值算法實(shí)驗(yàn)結(jié)果

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

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

圖3　三次均勻B樣條插值

圖4　三次樣條插值

圖5　Catmull-Rom樣條插值

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

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

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

表1　3種插值算法比較表

在編程測(cè)試中，將這7個(gè)點(diǎn)每?jī)蓚€(gè)點(diǎn)插入200個(gè)點(diǎn)，即插值后生成1 400個(gè)點(diǎn)，所需要的時(shí)間為11 ms左右。而無(wú)人車(chē)在運(yùn)行時(shí)大約每100 ms處理幾百個(gè)道路點(diǎn)，因此Catmull-Rom樣條插值在效率上能夠支持無(wú)人車(chē)決策的需要。

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

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

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

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

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

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

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

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

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

3　結(jié)　語(yǔ)

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

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

[4]王幼民.機(jī)器人連續(xù)軌跡控制中的Bezier曲線軌跡優(yōu)化與控制[J].機(jī)械傳動(dòng),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ī)器人軌跡規(guī)劃應(yīng)用中的改進(jìn)研究[J].機(jī)械設(shè)計(jì)與制造,2008,9(9):170-172.

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

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

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

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

國(guó)家自然科學(xué)基金重大項(xiàng)目(91220301).

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

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

U412.3

A

1674-2192(2016)02- 0089- 05