無人車作為一個(gè)復(fù)雜軟硬件結(jié)合系統(tǒng)，其安全可靠運(yùn)行需要車載硬件、傳感器集成、感知預(yù)測(cè)，以及控制規(guī)劃等多個(gè)模塊的協(xié)同配合工作。作者認(rèn)為最關(guān)鍵的部分是感知預(yù)測(cè)和決策控制規(guī)劃的緊密配合。狹義上的決策規(guī)劃控制部分，包含了無人車行為決策（Behavior Decision）、動(dòng)作規(guī)劃（Motion Planning）， 以及反饋控制（Feedback Control）這三個(gè)模塊。而從更寬泛的概念來說，無人車的決策規(guī)劃控制模塊，緊密依賴于上游的路由尋徑（Routing）以及交通預(yù)測(cè)（Prediction）的計(jì)算結(jié)果，所以本文也對(duì)路由尋徑和交通預(yù)測(cè)模塊進(jìn)行介紹。

　　系統(tǒng)框架和模塊劃分

　　圖1展示了一種無人車軟件系統(tǒng)的典型功能模塊劃分。其中感知（Perception）模塊負(fù)責(zé)從傳感器數(shù)據(jù)中探測(cè)計(jì)算出周邊環(huán)境的物體及其屬性。這些物體信息經(jīng)過預(yù)測(cè)模塊的計(jì)算，生成預(yù)測(cè)軌跡傳遞給決策規(guī)劃控制系統(tǒng)中的行為決策模塊。決策規(guī)劃控制系統(tǒng)的另一個(gè)上游模塊是路由尋徑模塊，其作用在簡單意義上可以理解為無人車軟件系統(tǒng)內(nèi)部的導(dǎo)航，即在宏觀層面上指導(dǎo)無人車軟件系統(tǒng)的控制規(guī)劃模塊按照什么樣的道路行駛從而實(shí)現(xiàn)從起始點(diǎn)到目的地點(diǎn)。值得注意的是這里的路由尋徑雖然在一定程度上類似傳統(tǒng)的導(dǎo)航，但其細(xì)節(jié)上緊密依賴于專門為無人車導(dǎo)航繪制的高精度地圖，所以和傳統(tǒng)的導(dǎo)航還是有本質(zhì)的不同。一般來說，路由尋徑會(huì)作為單獨(dú)的模塊來進(jìn)行實(shí)現(xiàn)，而交通預(yù)測(cè)部分，則既可以做為感知模塊的業(yè)務(wù)延伸，也可以看成是決策規(guī)劃控制模塊的外圍模塊而單獨(dú)進(jìn)行實(shí)現(xiàn)。

　　圖1 無人車軟件系統(tǒng)模塊

　　決策規(guī)劃控制（Decision，Planning & Control）系統(tǒng)的任務(wù)，就是在對(duì)感知到的周邊物體的預(yù)測(cè)軌跡的基礎(chǔ)上，結(jié)合無人車的路由意圖和當(dāng)前位置，對(duì)車輛做出最合理的決策和控制。整個(gè)決策規(guī)劃控制軟件系統(tǒng)，可以按照解決問題的不同層面，如圖1所示自上而下劃分為行為決策（Behavioral Decision）、動(dòng)作規(guī)劃（MotionPlanning），以及反饋控制（Feedback Control）這三個(gè)模塊。

　　其中行為決策模塊（Decision），可以直觀理解成無人車的“副駕駛”。行為決策接受路由尋徑的結(jié)果，同時(shí)也接收感知預(yù)測(cè)和地圖信息。綜合這些輸入信息，行為決策模塊在宏觀上決定了無人車如何行使。宏觀層面的決策包括在道路上的正常跟車，在遇到交通燈和行人時(shí)的等待避讓，以及在路口和其他車輛的交互通過等。例如，在路由尋徑要求無人車保持當(dāng)前車道（Lane）行駛，感知發(fā)現(xiàn)前方有一輛正常行駛的車輛，行為決策的決定便很可能是跟車行為。 動(dòng)作規(guī)劃模塊，在圖1的劃分中，解決的是具體的無人車動(dòng)作（Motion）的規(guī)劃問題。其功能可以理解為，在一個(gè)較小的時(shí)空區(qū)域內(nèi)，具體解決無人車從A點(diǎn)到B點(diǎn)如何行駛的問題。動(dòng)作規(guī)劃模塊在這里解決的問題，相對(duì)行為決策，又更加具體了一步。動(dòng)作規(guī)劃需要具體把一個(gè)短暫時(shí)間t內(nèi)從A到B的中間路徑點(diǎn)做出規(guī)劃，包括選擇途經(jīng)哪些具體的路徑點(diǎn)，以及到達(dá)每個(gè)路徑點(diǎn)時(shí)，無人車的速度，朝向，加速度等。不僅如此，動(dòng)作規(guī)劃還需要保證兩點(diǎn)：一是在后續(xù)時(shí)間內(nèi)，生成從A到B的時(shí)空路徑需要保持一定的一致性；二是，這些生成的A到B之間的路徑點(diǎn)，包括到達(dá)每個(gè)點(diǎn)的速度朝向加速度等，都在下游的反饋控制的實(shí)際可操作的物理范圍之內(nèi)。決策規(guī)劃控制系統(tǒng)最下層的模塊是反饋控制模塊。這是一個(gè)直接和無人車底層控制接口CAN-BUS對(duì)接的模塊。其核心任務(wù)是消化上層動(dòng)作規(guī)劃模塊的輸出軌跡點(diǎn)，通過一系列結(jié)合車身屬性和外界物理因素的動(dòng)力學(xué)計(jì)算，轉(zhuǎn)換成對(duì)車輛Drive-By-Wire控制的油門，剎車，以及方向盤信號(hào)，從而盡可能地控制車去實(shí)際執(zhí)行這些軌跡點(diǎn)。反饋控制模塊主要涉及對(duì)車輛自身控制，以及和外界物理環(huán)境交互的建模。

　　上述模塊的劃分方法，非常有效地將無人車決策控制規(guī)劃這樣一個(gè)復(fù)雜問題，按照計(jì)算邏輯從抽象到具體的做出了非常合理的切分。這樣的劃分使得每個(gè)模塊可以各司其職專注解決本層次的問題，從而提升了整個(gè)復(fù)雜軟件系統(tǒng)的開發(fā)效率。

　　預(yù)測(cè)模塊（Prediction）

　　作為決策規(guī)劃控制模塊的直接數(shù)據(jù)上游之一，預(yù)測(cè)模塊的作用是對(duì)感知所探測(cè)到的物體進(jìn)行行為預(yù)測(cè)，并且將預(yù)測(cè)的結(jié)果具體化為時(shí)間空間維度的軌跡傳遞給下游模塊。一般而言，感知模塊所輸出的物體信息包括位置，速度，朝向以及物體分類（如車輛，行人，自行車）等物理屬性。這些感知所計(jì)算輸出的物體屬性偏向于客觀的物理屬性。利用這些輸出的屬性，結(jié)合客觀的物理規(guī)律，可以對(duì)物體做出一個(gè)在非常短時(shí)間內(nèi)的“瞬時(shí)預(yù)測(cè)”。預(yù)測(cè)模塊所需要解決的問題，不僅僅局限于結(jié)合物理規(guī)律對(duì)物體做出預(yù)測(cè)，往往更重要的是結(jié)合物體和周邊環(huán)境，以及積累的歷史數(shù)據(jù)知識(shí)，對(duì)感知到的物體做出更為宏觀的行為預(yù)測(cè)。例如在圖2中，行為預(yù)測(cè)需要在宏觀層面預(yù)測(cè)圖中的車輛是否會(huì)保持直行還是右轉(zhuǎn)通過路口。

　　圖2 無人車周邊物體行為預(yù)測(cè)

　　行為預(yù)測(cè)的軌跡，既包括了障礙物在將來一段時(shí)間內(nèi)運(yùn)動(dòng)的方向，還體現(xiàn)了它們?cè)谶\(yùn)動(dòng)中的速度變化。譬如行人過馬路的時(shí)候會(huì)預(yù)測(cè)他們使用較為恒定的步行速度，車輛轉(zhuǎn)彎的時(shí)候會(huì)先減速后加速，而加減速的快慢也取決于彎道的弧度和長短。實(shí)際的無人車系統(tǒng)中，往往將宏觀層面的行為預(yù)測(cè)和軌跡生成抽象成兩個(gè)問題來解決。

　　宏觀層面的行為預(yù)測(cè)問題，往往可以抽象成經(jīng)典的機(jī)器學(xué)習(xí)問題，并且利用基于大數(shù)據(jù)的深度學(xué)習(xí)技術(shù)來解決。例如， 在假設(shè)車輛按照高精地圖劃分的道路（Lane）行駛的前提下，我們可以認(rèn)為在任何一個(gè)時(shí)刻，車輛可行駛的每一個(gè)Lane序列都是一個(gè)需要進(jìn)binary classification的樣本。在這個(gè)假設(shè)下，我們不需要對(duì)直行、并道、路口拐彎等場(chǎng)景進(jìn)行區(qū)分處理，因?yàn)闊o論是直行、并道，和路口拐彎，都可以統(tǒng)一看成是車輛在不同Lane序列上的行駛。車輛的宏觀行為預(yù)測(cè)問題，變簡化為對(duì)于Lane序列的Binary Classification問題。

　　圖3 無人車行為預(yù)測(cè)中的Lane序列Binary Classification抽象

　　如圖3所示，在t時(shí)刻，無人車主車位于Lane 1，此時(shí)按照Lane序列的可能展開途徑，我們考慮三條軌跡：

　　· Trajectory 1： Lane 1、Lane 2、Lane 3對(duì)應(yīng)路口右轉(zhuǎn)；

　　· Trajectory 2：Lane 1、Lane 6、Lane 8對(duì)應(yīng)路口直行；

　　· Trajectory 3：Lane 1、Lane 4、Lane 5、Lane7對(duì)應(yīng)換道后直行通過路口。

　　假設(shè)在t+w時(shí)刻，無人車經(jīng)過Lane 6行駛到Lane8的位置，那么軌跡Trajectory 2便成為該Binary Classification的正樣本，其余兩條軌跡便成為負(fù)樣本。在這種基于Lane序列的問題抽象下，所有的正負(fù)樣本可以從歷史數(shù)據(jù)的回放中獲得，并成為模型訓(xùn)練的樣本數(shù)據(jù)。另一方面，模型的特征抽取可以結(jié)合一定時(shí)間內(nèi)的如下信息來設(shè)計(jì)：

　　· 車輛本身的物理信息：速度、朝向等；

　　· 車輛相對(duì)于道路的信息：在Lane上的橫向/縱向位移和速度，相對(duì)Lane邊界的距離等；

　　· 車輛周邊的其他物體的信息：車輛周圍例如左右相鄰Lane是否有障礙物等。

　　在上述的樣本標(biāo)簽定義和特征抽取下，無人車Prediction部分的宏觀行為預(yù)測(cè)可以很好的抽象成典型的機(jī)器學(xué)習(xí)問題來解決。在預(yù)測(cè)得出的宏觀行為基礎(chǔ)上，相關(guān)的軌跡生成和速度預(yù)測(cè)可以通過特定的規(guī)則或者物理模型來實(shí)現(xiàn)。

　　路由尋徑（Routing）

　　無人車路徑規(guī)劃的Routing尋徑問題，雖然也是要解決從A點(diǎn)到B點(diǎn)的路由問題，但由于其輸出結(jié)果并不是為實(shí)際的駕駛員所使用，而是給下游的行為決策（Decision）和動(dòng)作規(guī)劃（Planning）等模塊作為輸入，其路徑規(guī)劃的層次要更加深入到無人車所使用的高精地圖的車道（Lane）級(jí)別。如圖4所示，其中的箭頭線段代表高精地圖級(jí)別的道路劃分和方向。lane1，lane2，………，lane8構(gòu)成了一條Routing輸出的路由片段序列?？梢钥吹?，無人車地圖級(jí)別的Lane劃分并非和實(shí)際的自然道路劃分對(duì)應(yīng)。比如lane2，lane5，lane7都代表了由地圖定義繪制的“虛擬”轉(zhuǎn)向Lane。類似的，一條較長的自然道路，也可能被劃分為若干個(gè)lane（例如lane3，lane4 ）。

　　做為整體無人車決策控制規(guī)劃（Decision，Planning & Control）系統(tǒng)的最上游模塊，路由尋徑模塊的輸出嚴(yán)格依賴于無人車高精地圖（HD-Map）的繪制。在高精地圖定義繪制的路網(wǎng)（Road Graph）的道路（Lane）劃分的基礎(chǔ)上，以及在一定的最優(yōu)策略定義下，路由尋徑模塊需要解決的問題是計(jì)算出一個(gè)從起點(diǎn)到終點(diǎn)的最佳道路（Lane）行駛序列：{（lane，start_posotion，end_position）i}，其中，（lane，start_posotion，end_position）i我們稱作一個(gè)Routing Segment（路由片段），所在的道路由lane來標(biāo)識(shí)，start_posotion，end_position分別代表在這條道路上的起始縱向距離和結(jié)束縱向距離。

　　圖4 無人車路由尋徑模塊（Routing）的高精地圖道路（Lane）級(jí)別尋徑路由

　　圖5 無人車尋徑（Routing）基于Lane Point的有向帶權(quán)圖上的最短路徑問題抽象

　　我們可以把無人車在高精地圖的Lane級(jí)別尋徑問題，抽象成一個(gè)在帶權(quán)有向圖上的最短路徑搜索問題（如圖5所示）。路由尋徑（Routing）模塊首先會(huì)基于Lane級(jí)別的高精度地圖，在一定范圍內(nèi)所有可能經(jīng)過的Lane上進(jìn)行分散“撒點(diǎn)”，我們稱這些點(diǎn)為“Lane Point”。這些點(diǎn)代表了對(duì)無人車可能經(jīng)過的Lane上的位置的抽樣。這些點(diǎn)與點(diǎn)之間，由有向帶權(quán)的邊進(jìn)行連接。Lane Point之間連接的權(quán)，代表了無人車從一個(gè)點(diǎn)行駛到另一個(gè)點(diǎn)的潛在代價(jià)（Cost）。在這樣的有向帶權(quán)圖的問題抽象下，路由尋徑問題可以利用常見的A*算法或者Dijkstra算法來進(jìn)行實(shí)現(xiàn)。

　　行為決策（Behavioral Decision）

　　行為決策（Behavior Decision）層在整個(gè)無人車決策規(guī)劃控制軟件系統(tǒng)中扮演著“副駕駛”的角色。這個(gè)層面匯集了所有重要的車輛周邊信息，不僅包括了無人車本身的當(dāng)前位置、速度、朝向以及所處車道，還收集了無人車一定距離以內(nèi)所有重要的感知相關(guān)的障礙物信息以及預(yù)測(cè)軌跡。行為決策層需要解決的問題，就是在知曉這些信息的基礎(chǔ)上，決定無人車的行駛策略。這些信息具體包括：

　　所有的路由尋徑結(jié)果：比如無人車為了達(dá)到目的地，需要進(jìn)入的車道是什么（target lane）。

　　· 無人車的當(dāng)前自身狀態(tài)：車的位置速度朝向，以及當(dāng)前主車所在的車道。

　　· 無人車的歷史信息：在上一個(gè)行為決策（Behavioral Decision）周期，無人車所做出的決策是什么？是跟車，停車，轉(zhuǎn)彎或者是換道？

　　· 無人車周邊的障礙物信息：無人車周邊一定距離范圍內(nèi)的所有障礙物信息。例如周邊的車輛所在的車道，鄰近的路口有哪些車輛，它們的速度位置如何？以及在一個(gè)較短的時(shí)間內(nèi)它們的意圖和預(yù)測(cè)的軌跡。周邊是否有自行車或者行人，以及他們的位置速度軌跡等；

　　· 無人車周邊的交通標(biāo)識(shí)信息：一定范圍內(nèi)的Lane的變化情況。比如路由尋徑的結(jié)果是在Lane1的縱向位移10m處換道進(jìn)入對(duì)應(yīng)的相鄰Lane2的縱向位移20m處，那么Lane 1的合法的縱向位移換道空間是多大？比如從一個(gè)直行Lane行駛結(jié)束，需要進(jìn)入下一個(gè)左轉(zhuǎn)Lane，兩條Lane的交界處是否有紅綠燈或者人行道？

　　· 當(dāng)?shù)氐慕煌ㄒ?guī)則：例如道路限速，是否可以紅燈右拐等等。

　　無人車的行為決策模塊， 就是要在上述所有信息的基礎(chǔ)上，做出如何行駛的決策。可以看出，無人車的行為決策模塊是一個(gè)信息匯聚的地方。由于需要考慮如此多種不同類型的信息以及受到非常本地化的交規(guī)限制，行為決策問題往往很難用一個(gè)單純的數(shù)學(xué)模型來進(jìn)解決。往往更適合行為決策模塊的解決方法，是利用一些軟件工程的先進(jìn)觀念來設(shè)計(jì)一些規(guī)則引擎系統(tǒng)。例如在DARPA無人車競(jìng)賽中，Stanford的無人車系統(tǒng)“Junior”利用一系列cost設(shè)計(jì)和有限狀態(tài)機(jī)（Finite State Machine）來設(shè)計(jì)無人車的軌跡和操控指令。在近來的無人車規(guī)劃控制相關(guān)工作中，基于馬爾可夫決策過程（Markov Decision Process）的模型也開始被越來越多得應(yīng)用到無人車行為層面的決策算法實(shí)現(xiàn)當(dāng)中。簡而言之，行為決策層面需要結(jié)合路由尋徑的意圖，周邊物體和交通規(guī)則，輸出宏觀的行為層面決策指令供下游的動(dòng)作規(guī)劃模塊去更具體地執(zhí)行。其具體的指令集合設(shè)計(jì)則需要和下游的動(dòng)作規(guī)劃模塊達(dá)成一致。

　　動(dòng)作規(guī)劃（Motion Planning）

　　在行為決策層下游的模塊是動(dòng)作規(guī)劃（Motion Planning）。其任務(wù)是具體將行為決策的宏觀指令解釋成一條帶有時(shí)間信息的軌跡曲線，來給最底層的反饋控制來進(jìn)行實(shí)際對(duì)車的操作。更具體而言，動(dòng)作規(guī)劃模塊試圖解決在一定的約束條件下優(yōu)化某個(gè)范圍內(nèi)的時(shí)空路徑問題。這里的“時(shí)空路徑”指車輛在一定時(shí)間段行駛的軌跡。該軌跡不僅包括位置信息，還包括了整條軌跡的時(shí)間信息和車輛姿態(tài)：即到達(dá)每個(gè)位置的時(shí)間，速度，以及相關(guān)的運(yùn)動(dòng)變量如加速度，曲率，曲率的高階導(dǎo)數(shù)等。動(dòng)作規(guī)劃可以拆分成為兩個(gè)問題：軌跡規(guī)劃（Trajectory Planning）和速度規(guī)劃（Speed Planning）來解決。其中軌跡規(guī)劃只解決在二維平面上，根據(jù)行為決策和綜合地圖信息定義的某種Cost函數(shù)下，優(yōu)化軌跡的問題；而速度規(guī)劃問題則是在選定了一個(gè)或者若干個(gè)軌跡（Trajectory）之后，解決用什么樣的速度來行駛的問題。xˉ=（x，y，θ，k，v），其中（x，y）表示車輛在二維平面的位置，θ表示車輛的朝向，k表示曲率（也即朝向θ的變化率），v表示車輛的速度（即軌跡任意點(diǎn)的切線速度）。車輛的這些姿態(tài)變量的標(biāo)量大小滿足如下關(guān)系：

　　其中曲率的k大小往往由系統(tǒng)的輸入限制條件決定。在此基礎(chǔ)上，考慮一條由車輛運(yùn)動(dòng)產(chǎn)生的連續(xù)軌跡（Path）。我們稱沿著軌跡的方向的位移為S方向。軌跡相對(duì)于車輛姿態(tài)的系統(tǒng)關(guān)系由下列偏微分方程式給出：

　　我們的軌跡規(guī)劃（Trajectory Planning）算法非常依賴于地圖對(duì)于道路的定義。這里我們定義道路由其道路中心線（Center Line）所定義，且定義道路的采樣函數(shù)為：

　　，其中s代表道路的中心線切向方向的位移（也稱為縱向位移s）。于此對(duì)應(yīng)的是道路的中心線垂直方向位移l，也稱之為橫向位移。如果考慮一個(gè)車輛的姿態(tài)點(diǎn)p點(diǎn)在道路上（s，l）坐標(biāo)，那么其實(shí)際的姿態(tài)和（s，l）的關(guān)系滿足：

　　其中曲率Kr定義為在道路轉(zhuǎn)彎的內(nèi)側(cè)曲率加大（隨縱向位移l加大），外側(cè)曲率則減小。我們使用右手坐標(biāo)系，所以如圖6所示在靠近原點(diǎn)處朝x軸的正方向，縱向位移l朝著y軸正方向加大。假設(shè)對(duì)于某條道路Lane（k），其縱向?qū)挾萳k保持不變。那么該條道路變可以表示成為一個(gè)隨著中心線橫向位移s的點(diǎn)集{p（s，lk）：s∈R+}。我們稱這樣的一個(gè)坐標(biāo)系統(tǒng)為坐標(biāo)系統(tǒng)。

　　圖6 XY平面下的SL坐標(biāo)系統(tǒng)及其網(wǎng)格劃分

　　在上述的車輛模型和道路模型下，我們討論軌跡規(guī)劃所產(chǎn)生的軌跡曲線。首先我們定義車輛的軌跡（Trajectory）為一個(gè)從［0，1］區(qū)間到車輛姿態(tài)向量集合C={x? }的連續(xù)映射：ρ：［0，1］→C 。其中，車輛的初始姿態(tài)向量為x? =（x，y，θ，k）。每條軌跡終點(diǎn)處如圖7所示，軌跡1的終點(diǎn)姿態(tài)為ρ1（1）=qend1軌跡2的終點(diǎn)姿態(tài)向量為ρ2（1）=qend2，初始姿態(tài)為ρ1（0）=ρ2（0）=qinit。軌跡優(yōu)化的目標(biāo)便是在所有可能的軌跡曲線中，篩選出滿足邊界條件的軌跡曲線，再尋找一條/若干條平滑且Cost函數(shù)最低的曲線。其中軌跡的候選曲線我們用類似在路由尋徑（Routing）模塊中介紹的“撒點(diǎn)”的采樣方式來生成。參考圖7，在某條Lane的SL坐標(biāo)系下，我們按照均勻切分的S和L方向的方格內(nèi)，在固定S和L間隔下，考慮每個(gè)（si，lj）區(qū)域的中心點(diǎn)（如圖7所示，又稱為軌跡點(diǎn)Trajectory Point）。一條候選的軌跡（Trajectory）便可以看做是沿著Lane的中心線縱向位移s方向連接不同Trajectory Point的平滑曲線。在圖7所示的道路SL分割和采樣下，可能的Trajectory Point有16個(gè)（4個(gè)s位置，4個(gè)l位置），從車輛的初始位置出發(fā)，我們只考慮在s方向單調(diào)增大的可能，不考慮城市綜合道路行駛中的倒車情況，那么總的候選曲線的總條數(shù)為44=256條。軌跡優(yōu)化便是要在這256條候選的曲線中找出Cost最優(yōu)的軌跡。

　　圖7 SL坐標(biāo)系下道路的分割采樣以及可能的軌跡

　　我們采用多項(xiàng)式螺旋線來連接軌跡點(diǎn)Trajectory Point，從而生成候選的曲線。多項(xiàng)式螺旋線，如圖8所示，代表了一類曲率可以用弧長（對(duì)應(yīng)我們軌跡中的s方向）的多項(xiàng)式函數(shù)來表示的曲線簇。我們使用三階（Cubic）或者五階（Quintic）的多項(xiàng)式螺旋線，其曲率K和軌跡弧長S的關(guān)系K（S）為：或者

　　圖8 多項(xiàng)式螺旋線以及車輛姿態(tài)的螺旋線示意圖

　　基于這種使用三階（五階）螺旋線連接的軌跡（Trajectory），其參數(shù)可以快速有效的通過梯度下降（Gradient Descent）的方法來搜索。以三階多項(xiàng)式為例，我們考慮從車輛初始姿態(tài)qinit=（xI，yI，θI，KI）到目標(biāo)姿態(tài)qgoal=（xG，yG，θG，KG），且具有連續(xù)曲率的三階螺旋線：在初始狀態(tài) 時(shí)，考慮曲率的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)均需要滿足初始狀態(tài)的限制，我們可以得到：，這樣使得實(shí)際未知參數(shù)減少到2個(gè)（K3，SG），利用梯度向量我們可以快速尋找到非常接近初始狀態(tài)限制的三階螺旋線的參數(shù)。在上述的所有候選曲線中，我們可以根據(jù)業(yè)務(wù)的不同需要來設(shè)置Cost函數(shù)，然后選擇出在任何時(shí)間點(diǎn)Cost最小且滿足邊界條件限制的曲線。由于候選曲線隨著我們采樣間隔隨指數(shù)增長，往往將Trajectory Point建立成某種有向帶權(quán)圖，然后利用圖論中的搜索方法結(jié)合Cost設(shè)置來選取最優(yōu)曲線。具體的搜索方法可以參考中的動(dòng)態(tài)編程方法。

　　在軌跡規(guī)劃選定了一條或者若干條曲線后，速度規(guī)劃部分將決定車輛以什么樣的速度來通過這條曲線。軌跡規(guī)劃在選取曲線時(shí)的Cost設(shè)置偏重于靜態(tài)障礙物，而速度規(guī)劃在選取曲線時(shí)的Cost設(shè)置應(yīng)該注意以下幾點(diǎn)：

　　· 動(dòng)態(tài)的障礙物信息：如Prediction模塊預(yù)測(cè)的軌跡信息；

　　· 上游Decision輸出的宏觀層面指令：如對(duì)某個(gè)障礙物需要避讓（Yield）；

　　· 狀態(tài)量的連續(xù)性限制：如速度，加速度等均不能跳變。

　　在此基礎(chǔ)之上，速度規(guī)劃可以將曲線的縱向位移S和時(shí)間變量T建立一個(gè)二維平面，將上述的限制投影在該平面上求解這個(gè)問題。這種考慮曲線縱向位移s和時(shí)間t的速度規(guī)劃求解方式稱之為S-T求解。類似得，如果考慮橫向位移，也可以在三維的S-L-T空間內(nèi)進(jìn)行求解。

　　反饋控制（Feedback Control）

　　無人車反饋控制模塊中常用的車輛控制模型為自行車模型。在該模型中，車輛姿態(tài)（Pose）是處于一個(gè)二維的平面坐標(biāo)系內(nèi)，并且可以由車輛所處的位置（position）以及車身和坐標(biāo)平面的夾角（heading）來完全描述。同時(shí)我們假設(shè)車輛前后輪由一個(gè)剛性（rigid）不變的軸連接，其中車輛的前輪可以在一定的角度范圍內(nèi)自由轉(zhuǎn)動(dòng)，而車輛的后輪保持和車身的平行關(guān)系不能轉(zhuǎn)動(dòng)。前輪的轉(zhuǎn)動(dòng)對(duì)應(yīng)實(shí)際車輛控制中方向盤的轉(zhuǎn)動(dòng)。

　　車輛的自行車模型所代表的車輛姿態(tài)如圖9所示。這里我們使用一個(gè)基于x-y的二維平面，其中e^x和e^y分別代表其x和y方向的單元向量。向量pr和向量pf分別代表車輛后輪和前輪與地面的接觸點(diǎn)。車輛的朝向角θ代表車輛和x軸的夾角（即向量pr和單元向量e^x的夾角）。方向盤轉(zhuǎn)角δ定義為前輪朝向和車輛朝向角的夾角。其中前后輪與地面接觸點(diǎn)的向量pf和pr之間滿足：

　　其中y˙f和y˙r分別代表車輛前后輪在和地面接觸點(diǎn)處的瞬時(shí)速度向量?？紤]車輛的后輪速度在x-y軸的投影標(biāo)量xr：=pr·e^x和xy：=pr·e^y以及后輪的切向速度，那么上述的向量pf和pr之間的關(guān)系限制在后輪相關(guān)分量上的表現(xiàn)形式為：其中 l代表車輛前后軸中心間距。類似地，用車輛前輪相關(guān)分量的表現(xiàn)形式為：

　　這里前后輪的切向速度標(biāo)量大小滿足：

　　圖9 車輛控制的自行車模型

　　在上述的車輛模型下，反饋控制（Feedback Control）需要解決的問題便是找到滿足車輛動(dòng)態(tài)姿態(tài)限制的方向盤轉(zhuǎn)角δ∈［δmin，δmax］的以及前向速度vr∈［δmin，δmax］。而對(duì)這些狀態(tài)量的控制可以是一個(gè)典型的PID反饋控制系統(tǒng)（如圖10所示）。其中e（t）代表當(dāng)前的跟蹤誤差，而這個(gè)跟蹤的變量誤差可以是軌跡的縱向/橫向誤差，角度/曲率誤差或者是若干車輛姿態(tài)狀態(tài)變量的綜合誤差。其中P控制器代表對(duì)當(dāng)前誤差的反饋，其增益由KP控制；I和D控制器分別代表積分項(xiàng)和微分項(xiàng)，其增益分別有KI和KD來控制。

　　圖10 基于PID的反饋控制系統(tǒng)

　　具體到無人車的反饋控制（Feedback Control）模塊，我們需要解決的問題是控制車輛盡可能遵循上游動(dòng)作規(guī)劃（Motion Planning）所輸出的時(shí)空軌跡?？梢允褂脙蓚€(gè)基于PID反饋控制的控制器來分別控制方向盤轉(zhuǎn)角δ以及前進(jìn)速度vs。