自動駕駛汽車的路徑規(guī)劃算法最早源于機器人的路徑規(guī)劃研究，但是就工況而言卻比機器人的路徑規(guī)劃復(fù)雜得多，自動駕駛車輛需要考慮車速、道路的附著情況、車輛最小轉(zhuǎn)彎半徑、外界天氣、行駛環(huán)境等因素。

本文將為大家介紹四種常用的路徑規(guī)劃算法，分別是搜索算法、隨機采樣、曲線插值和人工勢場法，以及六種常用的軌跡規(guī)劃函數(shù)。

一.路徑規(guī)劃方法

路徑規(guī)劃首先需要建立規(guī)劃模型，利用狀態(tài)空間法描述規(guī)劃模型是建立非線性優(yōu)化模型的關(guān)鍵。圖搜索算法可以很好地解決該問題，其基本思想是將車輛的初始位姿和目標(biāo)位姿映射到一個狀態(tài)空間，然后將狀態(tài)空間離散化，并將其構(gòu)成一個圖，隨后從圖中搜索滿足約束條件的最優(yōu)軌跡。目前主流的方法主要包括Voronoi圖、柵格地圖與代價地圖、Lattice狀態(tài)圖、駕駛通道圖等。為了兼顧實時性與障礙物約束空間處理能力，一般采用Lattice和通道圖方法生成安全軌跡。

隨機采樣方法是另一類被廣泛采用的方法，基本思想是在構(gòu)型空間中隨機采樣，并篩選出滿足性能需求的最優(yōu)采樣點，具備概率完備性，但其最大的缺點是舒適性較差，且計算效率隨著障礙物數(shù)量的增長而下降。最常用的方法包括概率路標(biāo)算法（Probabilistic Road Map，PRM）以及快速搜索隨機樹算法（Rapidly-exploring Random Trees，RRT）。

為了降低路徑規(guī)劃問題求解難度，確定性采樣方法得到了廣泛的應(yīng)用，具體包括多項式參數(shù)化模型和樣條曲線、螺旋線、回旋曲線、貝塞爾曲線等變種參數(shù)化曲線方法?；?strong>多項式參數(shù)化模型的規(guī)劃方法的設(shè)計思想是根據(jù)車輛的初始狀態(tài)和目標(biāo)狀態(tài)對變道軌跡進(jìn)行規(guī)劃，使車輛在指定的時間到達(dá)相鄰車道。試圖在用函數(shù)f(x,y,t)描述的函數(shù)族類中尋找一條軌跡，能充分描述車輛從起始位置過渡到目標(biāo)位置整個過程的動態(tài)特性。隨著多項式次數(shù)的變大，曲線的擬合效果越好，但次數(shù)的增多也會導(dǎo)致參數(shù)求解的運算量指數(shù)增長，通常選用五次多項式進(jìn)行變道軌跡的規(guī)劃。在x方向、y方向分別選用五次多項式構(gòu)造變道軌跡的曲線簇，如下式所示。

在道路結(jié)構(gòu)的約束下，由五次多項式規(guī)劃的曲線無論是在縱向上還是在側(cè)向上都能達(dá)到期望的位置，車輛能在規(guī)定的變道時間內(nèi)平順的完成變道，且軌跡曲線的曲率在起始點和終了點都能達(dá)到零的期望值。但是，基于多項式的軌跡規(guī)劃方法也存在變道時間和終了點必須預(yù)先已知的局限，對多項式中參數(shù)的確定需要有較充分的條件，對縱向車速變化的情況和實際車輛變道過程中終了點并不唯一的機動性和自適應(yīng)性較差。

基于貝塞爾曲線的路徑規(guī)劃方法通過控制點的選取來改變曲線的形狀，通常定義n階貝塞爾曲線由n+1個控制點組成，表達(dá)式為：

式中，Pi、t分別是控制點i的坐標(biāo)值與時間參數(shù)，Bi(t)是Bernstein多項式，具體為：

三次貝塞爾曲線的參數(shù)方程可表示為：

因其線條光滑且曲率值小的特點而被廣泛地應(yīng)用于軌跡曲線規(guī)劃中。 

此外，還有基于人工勢場法的軌跡規(guī)劃方法，其基本思想是假設(shè)行駛目標(biāo)點對車輛產(chǎn)生引力，而障礙物對車輛產(chǎn)生斥力，控制車輛沿勢場中“勢峰”間的“勢谷”前進(jìn)。其中，引力與車輛到行駛目標(biāo)點的距離成正比，斥力與車輛到障礙物的距離成反比。通過求解車輛所受引力和斥力的合力作為車輛的合外力來控制車輛的行駛速度和運動方向。該方法具有易于數(shù)學(xué)表達(dá)、反應(yīng)速度快、易于實現(xiàn)算法與環(huán)境形成閉環(huán)控制等優(yōu)點，但它在求解過程中極易出現(xiàn)局部最優(yōu)解而導(dǎo)致產(chǎn)生死鎖現(xiàn)象。

各種方法的對比分析如下表所示。

二.軌跡函數(shù)選擇

軌跡函數(shù)決定了車輛能否順暢、快速、舒適、安全地運行。常見的車輛換道軌跡有基于變道軌跡的等速偏移模型、圓弧換道模型，基于期望側(cè)向加速度的換道模型，余弦函數(shù)換道模型，余弦函數(shù)，雙曲正切函數(shù)加權(quán)換道模型和等速偏移正弦函數(shù)換道模型。軌跡函數(shù)可根據(jù)軌跡路徑曲率變化是否連續(xù)、起點和終點曲率是否為零、軌跡函數(shù)的靈活性進(jìn)行選擇。

等速偏移模型和圓弧換道模型是對實際變道軌跡進(jìn)行了簡化，等速偏移模型如圖1所示，其由三條直線段構(gòu)成，在軌跡起點C0、終點C3處曲率為零，但軌跡的曲率在C1、C2會發(fā)生突變，在實際行車中無法實現(xiàn)。

圖1 等速偏移模型

圓弧換道模型是在等速偏移模型的基礎(chǔ)上，采用圓弧作為起始段、終了段，兩端圓弧的曲率半徑均為R，如圖2所示。同樣，由于在圓弧端點C0、C1、C2、C3處軌跡曲率不連續(xù)，使得車輛在實際變道行駛中無法完全實現(xiàn)該軌跡。此外，由于圓弧換道模型是多階非線性曲線，計算麻煩，如果車輛要調(diào)整換道過程，比較困難。

圖2 圓弧換道模型

基于期望側(cè)向加速度的換道模型是基于換道車輛的側(cè)向加速度變化規(guī)律為線性變化、最大側(cè)向加速度不超過一定值而提出的，該方法認(rèn)為車輛在直線道路上進(jìn)行變道行駛時，側(cè)向加速度的形狀由兩個大小相等的正反梯形組成，梯形的高為側(cè)向加速度的最大值，梯形腰的斜率為側(cè)向加速度率，如圖3所示。通過對期望側(cè)向加速度進(jìn)行兩次積分得到理想變道軌跡?；趥?cè)向加速度的變道軌跡能夠很好的滿足變道過程中曲率連續(xù)變化的要求，且在軌跡起點、終點曲率為零，但該模型是分段函數(shù)，存在動態(tài)調(diào)整比較困難的局限性。

圖3 基于梯形加速度的換道模型

余弦函數(shù)換道模型由于其計算簡便、平滑性較好，是目前被廣泛采用的軌跡之一，其軌跡如圖4所示。假設(shè)兩車道的車道中心線距離為d，換道過程產(chǎn)生的縱向位移為L，余弦函數(shù)換道模型軌跡函數(shù)為：

對上式求導(dǎo)得：

則車輛換道模型的軌跡曲率K為：

雖然余弦函數(shù)換軌跡的曲率連續(xù)變化，但最大曲率出現(xiàn)在路徑起點x=0和終點處x=L處，最大曲率為

此時側(cè)向加速度最大，不滿足換道模型軌跡中起點和終點處曲率均應(yīng)為0的要求。

圖4 余弦函數(shù)換道模型

余弦函數(shù)和雙曲正切函數(shù)加權(quán)換道模型是余弦軌跡函數(shù)ycos(x)和雙曲正切換道函數(shù)ytanh(x)加權(quán)的軌跡，如圖5中虛線和實線分別表示余弦和雙曲正切函數(shù)換道軌跡函數(shù)。該模型通過引入縱向拉伸系數(shù)和加權(quán)系數(shù)，使得在軌跡起點和終點處的曲率值接近于0，但仍不為0。

圖5 余弦函數(shù)和雙曲正切函數(shù)加權(quán)換道模型

等速偏移正弦函數(shù)換道模型是將等速偏移軌跡函數(shù)與正弦函數(shù)疊加，具備了等速偏移軌跡側(cè)向加速度恒為0的優(yōu)點與正弦函數(shù)換道軌跡平滑性優(yōu)異的特點。該模型初始函數(shù)為：

為了滿足換道軌跡的一些要求，上式可改進(jìn)為：

式中，d為兩車道的車道中心線的距離，L為換道過程產(chǎn)生的沿車道方向的縱向位移。

等速偏移正弦函數(shù)軌跡如圖6所示，只要整個換道行程 L ≥ Lmin ，就能滿足車輛在公路的加速度和加速度變化率的限制條件，其中

u為車輛沿車道的縱向速度，amax為車輛允許的最大安全側(cè)向加速度，Jmax為允許的車輛最大側(cè)向沖擊度。通過對y進(jìn)行兩次求導(dǎo)，可計算出軌跡曲率，當(dāng)x=0或x=L時，曲率K=0，此時曲率半徑無窮大，車輛沿直線行駛。該模型既滿足了換道軌跡路徑曲率連續(xù)變化、不發(fā)生突變的要求，又實現(xiàn)了在換道路徑起點和終點處曲率均為0，使得車輛在換道起始時刻、結(jié)束時刻的運動方向與車道線保持平行。

圖6 等速偏移正弦函數(shù)加權(quán)換道模型