一種基于樣條插值的局部路徑規(guī)劃模型與實現(xiàn)

胡崢楠 佘鋒

摘 要：局部路徑規(guī)劃是自動駕駛的基礎(chǔ)研究領(lǐng)域，為了實現(xiàn)局部路徑規(guī)劃中備選軌跡的有效生成，提出了一種基于樣條插值的局部路徑規(guī)劃的建模方法。針對局部路徑規(guī)劃的參考線生成，設(shè)計了基于樣條插值的參考線生成模型。并基于生成的參考線，進(jìn)一步推導(dǎo)了備選軌跡的生成方法，從而為自動駕駛的控制決策規(guī)劃功能的軌跡選擇、避障分析等功能提供有效的數(shù)學(xué)分析基礎(chǔ)。仿真結(jié)果驗證，文中提出的參考線數(shù)學(xué)模型對不同的道路曲線形狀具有較強(qiáng)的適應(yīng)性和可實現(xiàn)性。同時，通過對路徑規(guī)劃的離散采樣點使用了參數(shù)三次樣條插值函數(shù)來擬合行駛參考線，保證參考線上點的曲率的連續(xù)性，并符合車輛的運動學(xué)特性。

關(guān)鍵詞：無人駕駛;路徑規(guī)劃;樣條插值;軌跡生成;參考線

中圖分類號：TP391

文獻(xiàn)標(biāo)志碼：A

文章編號：1007-757X（2020）11-0106-05

Abstract：Local path planning is the foundation field of research of automatic driving. In order to achieve the effective generation of alternative trajectory in local path planning， this paper proposes a modeling method of local path planning based on spline interpolation. For the local path planning， a reference line generation model based on spline interpolation is designed. Based on the generated reference line， the generation method of alternative trajectory is further derived， it provides an effective mathematical analysis basis for the trajectory selection， obstacle avoidance analysis and other functions of planning function of automatic driving. The simulation results show that the reference line mathematical model proposed in this paper has strong adaptability and feasibility for different road curve shapes. This paper also uses the parameter cubic spline interpolation function to fit the driving reference line through the discrete sampling points of the path planning， and this can ensure the curvature continuity of the reference line points， and meet the kinematic characteristics of the vehicle.

Key words：autopilot;path planning;spline interpolation;trajectory generation;line

0?引言

無人駕駛汽車技術(shù)包含感知融合、控制決策規(guī)劃、高精度定位等多種技術(shù)。在無人駕駛智能車相關(guān)的多項關(guān)鍵技術(shù)中，規(guī)劃技術(shù)是保障無人駕駛車輛安全行駛的關(guān)鍵技術(shù)環(huán)節(jié)。一般而言，路徑規(guī)劃的功能是為了實現(xiàn)在有障礙物的環(huán)境中，搜索出從給定起點到終點的優(yōu)化行駛路徑，使無人駕駛車輛在行駛過程中安全、可靠地繞過檢測到的障礙物。在路徑規(guī)劃的設(shè)計中，主要解決無人駕駛車輛行駛過程中的兩大問題—導(dǎo)航和避障。

根據(jù)規(guī)劃范圍，路徑規(guī)劃可分為全局路徑規(guī)劃與局部路徑規(guī)劃。全局路徑規(guī)劃從全局范圍上為車輛規(guī)劃出全局的行駛路徑;局部路徑規(guī)劃指無人駕駛車輛通過自身攜帶的傳感器獲取周圍的環(huán)境信息，并基于高精地圖，使無人駕駛車輛獲取一條安全無碰撞的最佳路線。相對于全局路徑規(guī)劃，局部路徑規(guī)劃主要基于對當(dāng)前局部環(huán)境信息的感知，實時性地規(guī)劃行駛路徑。

局部路徑規(guī)劃算法的是在完成車道級的全局路徑規(guī)劃和行為決策的基礎(chǔ)上，規(guī)劃未來較短一段時間內(nèi)車輛的行駛路徑。算法最終將輸出按時間等分的期望行駛軌跡離散點信息（包括位置、速度、航向角等）。

在局部路徑規(guī)劃中，參考線設(shè)計是規(guī)劃路徑形成與選擇的基礎(chǔ)。參考線的特征是：假設(shè)在沒有障礙物的情況下，車輛沿車道行駛時的理想路徑。一般而言，參考線理想路徑只和車道的幾何形狀與交通規(guī)則有關(guān)。參考線可以在制作環(huán)境信息時測量確定，然后保存在高精度地圖中，基于這種方式獲取參考線是最為方便和準(zhǔn)確的。但目前高精度地圖信息和格式還沒統(tǒng)一，有些高精度地圖中僅保存了可以確定車道的若干個離散點的信息。因此，生成車輛行駛的參考線是局部路徑規(guī)劃的前提條件。在參考線的設(shè)計中，還需要考慮是否滿足曲率特性和運動學(xué)特性。目前，如何設(shè)計參考線生成模型，仍然還是正在探索的問題。本文基于離散采樣，對離散點使用了參數(shù)三次樣條插值函數(shù)進(jìn)行數(shù)學(xué)建模，并擬合行駛參考線，從而保證參考線上點的曲率連續(xù)性，并符合車輛的運動學(xué)特性。

1?樣條插值軌跡生成算法

1.1?局部路徑規(guī)劃設(shè)計準(zhǔn)則

聯(lián)合式（11）-式（17），可得到關(guān)于4個未知參數(shù)的線性方程組，從而可以求解得到c0，c1，c2，c3。每個采樣點對應(yīng)一組解，基于每一組解，可以生成一條備選路徑。

備選路徑生成步驟如下。

2?仿真結(jié)果

為了驗證樣條插值參考線生成模型的效果，對平面坐標(biāo)系下的不同形狀進(jìn)行了仿真驗證，如圖4所示。

圖4（a）所示的圓為單位圓，圓心為（0，0），在圓上取16點等分個圓弧等分點時的插值樣例;

圖4（b）為平面坐標(biāo)系下，邊長為2的正方形，取圖形周長上的8個等分點時的插值樣例;圖4（c）為平面坐標(biāo)系下，邊長為2的正方形，取圖形周長上的16個等分點時的插值樣例。從圖4的結(jié)果可以看出，參數(shù)樣條插值參考線生成模型對不同的圖形具有較強(qiáng)的適用性，其對原始圖形的擬合效果取決于采樣點的選取，采樣點越多擬合效果越好。在實際中，采樣點數(shù)量的選擇可以根據(jù)擬合精度需求來確定，如圖5所示。

圖5是在一條長約240米，寬6米的S型車道上，以沿參考系向前100米為終點位置，生成備選路徑的樣例圖。其中圖5（b）是圖5（a）的放大圖。圖5中，展示了車輛處于三種不同初始狀態(tài)的備選路徑組，終點位置橫向偏移的采樣寬度均為0.5米。備選路徑組A的初始位置設(shè)置為橫向偏移為0、夾角為0度;備選路徑組B的初始位置為橫向偏移為1米、夾角為0度;備選路徑組C的初始位置，橫向偏移為0和夾角為-0.1（弧度制）。從圖5可以看出，根據(jù)本文所提到的路徑生成算法，可以生成完備的備用路徑，為后續(xù)的曲率檢驗、避障規(guī)劃和路徑選擇提供了良好的分析模型。

3?總結(jié)

為了有效地實施無人駕駛中的路徑選擇、避障評估與規(guī)劃，本文基于樣條插值函數(shù)的理論，設(shè)計參考線生成模型，并基于參考線的設(shè)計，進(jìn)一步設(shè)計并分析了備選路徑設(shè)計的方法。經(jīng)仿真驗證，本文提出的基于插值函數(shù)理論的參考線模型與備選路徑生成的方法，對于不同的道路形狀有較好的適應(yīng)性，并具有較好的可實現(xiàn)性。同時，本文所提出的方法也建立了明確的數(shù)學(xué)模型，為局部路徑規(guī)劃的曲率分析、避障分析提供了良好的技術(shù)條件。

參考文獻(xiàn)

[1]?Werling M， Kammel S， Ziegler J， et al. Optimal trajectories for time-critical street scenarios using discretized terminal manifolds[J].The International Journal of Robotics Research， 2012，31（3）：259-346.

[2]?Lim W， Lee S， Sunwoo M， et al. Hierarchical trajectory planning of an autonomous car based on the integration of a sampling and an optimization method[J]. IEEE Transactions on Intelligent Transportation Systems， 2018，19（2）：613-626.

[3]?Li X， Sun Z， Cao D， et al. Real-Time Trajectory Planning for Autonomous Urban Driving：Framework， Algorithms， and Verifications[J]. IEEE/ASME Transactions on Mechatronics， 2016， 21（2）：740-753.

[4]?A.G. Cunningham， E. Galceran， R.M. Eustice， E. Olson. “Mpdm：driving，” in 2015 IEEE International Conference on Robotics and Automation（ICRA）. IEEE， 2015， 1670-1677.

[5]?Open DRIVE Format SpecRev1.5M：http：//www.opendrive.org/，2019.

[6]?申澤邦，雍賓賓，周慶國，等. 無人駕駛原理與實踐[M].北京：機(jī)械工業(yè)出版社，2019.

[7]?許小勇， 鐘太勇. 三次樣條插值函數(shù)的構(gòu)造與Matlab實現(xiàn)[J]. 兵工自動化， 2006（11）：76-78.

[8]?蘇步青. 關(guān)于三次參數(shù)樣條曲線的一些注記[J]. 應(yīng)用數(shù)學(xué)學(xué)報， 1976（1）：49-58.

[9]?Werling M， Ziegler J， Kammel S， et al. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame[C]// Robotics and Automation （ICRA）， 2010 IEEE International Conference on. IEEE， 2010：987-993.

（收稿日期：2019.11.01）