兩萬(wàn)字簡(jiǎn)述自動(dòng)駕駛路徑規劃的常用算法
加入技術(shù)交流群
掃碼加入
和技術(shù)大咖面對面交流
海量資料庫查詢(xún)
1. 1 自動(dòng)駕駛系統分類(lèi)自動(dòng)駕駛系統沒(méi)有嚴謹的分類(lèi)，但行業(yè)內普遍喜歡將自動(dòng)駕駛系統區別為模塊化的和端到端的，圖1所示為兩者系統的原理框圖對比。
圖1 模塊化和端到端自動(dòng)駕駛系統原理簡(jiǎn)圖
1.1.1 模塊化自動(dòng)駕駛系統這是最經(jīng)典也是業(yè)界采用最多的一種自動(dòng)駕駛系統，也是最簡(jiǎn)明清爽的一種結構，其作用是實(shí)時(shí)地求解出連續的控制輸出使得自動(dòng)駕駛車(chē)輛可以安全地由初始位置行駛到目標位置?；谀K化的思想，將自動(dòng)駕駛系統劃分為三層：環(huán)境感知層、決策規劃層和控制執行層。每一層還可以劃分為不同的模塊，每個(gè)模塊還可以劃分為不同的子模塊……。環(huán)境感知層就像是人的眼睛和耳朵，負責對外部環(huán)境進(jìn)行感知并將感知結果送入決策規劃層。決策規劃層就像是人的大腦，在接收到感知信息后進(jìn)行分析、決策，并生成加減速、變道、直行等控制命令?？刂茍绦袑泳拖袢说碾p手和雙腳，在接收到控制命令后控制執行器完成加速、轉向等操作。模塊化自動(dòng)駕駛系統中每一層都是關(guān)鍵和核心。但從實(shí)現自動(dòng)駕駛功能的角度，環(huán)境感知層是基礎，決策規劃層是核心，控制執行層是保障。作為核心的決策規劃層帶著(zhù)自動(dòng)駕駛往“更安全、更舒適、更高效”的道路上狂奔，畢竟小小的失誤小則影響乘坐舒適性、通行效率，大則影響人身財產(chǎn)安全。在模塊化自動(dòng)駕駛系統中，不同團隊負責不同的模塊，可以實(shí)現更好的分工合作，從而提高開(kāi)發(fā)效率。同時(shí)團隊內部可以對負責的模塊進(jìn)行充分的評估，了解各模塊的性能瓶頸所在，從而讓我們能對最后的0.1%的不足有更清晰的認知，技術(shù)的迭代、更新。缺點(diǎn)就是整個(gè)系統非常復雜、龐大、需要人工設計成百上千個(gè)模塊。二是對車(chē)載硬件計算能力要求高，如果越來(lái)越多的子模塊采用深度學(xué)習網(wǎng)絡(luò )，這將帶來(lái)災難性的計算需求爆炸?；谀K化的自動(dòng)駕駛系統，我們可能花10%的時(shí)間就實(shí)現了99.9%的問(wèn)題，但我們還需要花90%的時(shí)間去解決最后0.1%的不足。這個(gè)系統的難度之大，已經(jīng)遠超一家公司的能力范圍，需要一個(gè)協(xié)作的生態(tài)。1.1.2 端到端自動(dòng)駕駛系統術(shù)語(yǔ)端到端（End to End）來(lái)源于深度學(xué)習，指的是算法直接由輸入求解出所需的輸出，即算法直接將系統的輸入端連接到輸出端。2016年NVIDIA將端到端的深度學(xué)習技術(shù)應用在自動(dòng)駕駛汽車(chē)之后，端到端自動(dòng)駕駛迅速捕獲圈內一眾大佬的芳心，各種demo更是層出不窮。所謂端到端自動(dòng)駕駛是指車(chē)輛將傳感器采集到的信息（原始圖像數據、原始點(diǎn)云數據等），直接送入到一個(gè)統一的深度學(xué)習神經(jīng)網(wǎng)絡(luò )，神經(jīng)網(wǎng)絡(luò )經(jīng)過(guò)處理之后直接輸出自動(dòng)駕駛汽車(chē)的駕駛命令（方向盤(pán)轉角、方向盤(pán)轉速、油門(mén)踏板開(kāi)度、制動(dòng)踏板開(kāi)度等）。2016年NVIDIA發(fā)表了論文《End to End Learning for Self-Driving Cars》，拉開(kāi)了端到端自動(dòng)駕駛內卷的序幕。論文首先展示了訓練數據的采集系統，如圖2所示。論文中只涉及了車(chē)道保持功能，因此訓練數據也只對攝像機的視頻數據和人類(lèi)駕駛員操作方向盤(pán)的角度數據進(jìn)行了采集。
圖4 訓練過(guò)的網(wǎng)絡(luò )用于從單中心前向攝像機中生成轉向命令
在端到端自動(dòng)駕駛中，沒(méi)有人工設計的繁復規則，只需要極少的來(lái)自人類(lèi)的訓練數據，深度學(xué)習神經(jīng)網(wǎng)絡(luò )就會(huì )學(xué)會(huì )駕駛。且不用關(guān)心有沒(méi)有高精地圖覆蓋、此時(shí)是行駛在高速主干路還是城區道路、道路上車(chē)道線(xiàn)有沒(méi)有缺失等。相比模塊化自動(dòng)駕駛系統，端到端自動(dòng)駕駛系統設計難度低，硬件成本小，還能借助數據的多樣性獲得不同場(chǎng)景下的泛用性。各方面條件得天獨厚，從理論層面看堪稱(chēng)自動(dòng)駕駛的終極夢(mèng)想。然而端到端深度學(xué)習神經(jīng)網(wǎng)絡(luò )是一個(gè)完完全全的黑盒子，不具解釋分析性，可靠性、靈活性差，工程師們沒(méi)有辦法對它進(jìn)行系統化的解釋分析，而是只能依靠推測和實(shí)驗進(jìn)行調整。最終帶來(lái)的結果是安全難以得到保障，而自動(dòng)駕駛最最關(guān)注的恰是安全。比如端到端自動(dòng)駕駛系統下汽車(chē)做出一個(gè)汽車(chē)減速左轉的行動(dòng)，工程師們無(wú)法確定這是因為汽車(chē)看到行人，還是因為看到較遠處的紅燈。但是，在模塊化的自動(dòng)駕駛系統下，由于多個(gè)識別系統嵌套，相對好理解到底汽車(chē)所做的每一個(gè)舉動(dòng)背后的邏輯。這也意味著(zhù)，如果端到端系統出現問(wèn)題時(shí)，工程師們并不能對其對癥下****，做出合理的應對。更多情況下甚至只能簡(jiǎn)單向模型灌注更多的數據，希冀它能在進(jìn)一步的訓練中“自行”解決問(wèn)題。這也會(huì )大大降低端到端自動(dòng)駕駛系統原本開(kāi)發(fā)簡(jiǎn)單的優(yōu)勢。1.2 決策規劃分層架構決策規劃的任務(wù)，就是在對感知到的周邊物體的預測軌跡的基礎上，結合結合自動(dòng)駕駛車(chē)輛的和當前位置，對車(chē)輛做出最合理的決策和控制。正如人的大腦又分為左腦和右腦、并負責不同的任務(wù)一樣，模塊化自動(dòng)駕駛系統中決策規劃層也可以繼續細分為執行不同任務(wù)的子層。而這一分層設計最早其實(shí)是源自2007年舉辦的DAPRA城市挑戰賽，比賽中多數參賽隊伍都將自動(dòng)駕駛系統的決策規劃方式包括三層：全局路徑規劃層（Route Planning）、行為決策層（Behavioral Layer）和運動(dòng)規劃層（Motion Planning），如圖5所示。
圖6 全局路徑規劃示例在全局路徑規劃的時(shí)候，也可以基于地圖精度和豐富度，提前考慮道路曲率半徑、坡度等信息，來(lái)避免搜索出部分參數超出ODD要求的全局路徑。但是高度隨機的交通參與者、高度動(dòng)態(tài)的交通流以及高度復雜的道路結構，全局路徑規劃是無(wú)法考慮周到的，因此還需要基于具體的行為決策進(jìn)行后面的運動(dòng)規劃，也就是局部路徑規劃。行為決策層在收到全局路徑后，結合感知環(huán)境信息、交通規則信息、車(chē)輛狀態(tài)信息、駕駛場(chǎng)景信息等，推導判斷下一分鐘或下一秒時(shí)刻的情況，作出車(chē)道保持、車(chē)輛跟隨、車(chē)道變換和制動(dòng)避撞等的適合當前交通環(huán)境的駕駛行為。如圖7所示，自車(chē)在檢測到前方存在低速行駛車(chē)輛，且右側車(chē)道滿(mǎn)足變道條件后，作出向右變道的駕駛行為決策。
圖7 行為決策示例運動(dòng)規劃層也被稱(chēng)為局部路徑規劃層，與全局路徑規劃聚焦在分鐘到小時(shí)級別的規劃不同，運動(dòng)規劃聚焦在毫秒級到秒級的規劃。規劃的時(shí)候，根據輸入的行為決策信息、結合車(chē)輛實(shí)時(shí)位姿信息、局部環(huán)境信息、全局路徑參考信息等，在“安全、舒適、效率”的精神引領(lǐng)下，規劃生成一條滿(mǎn)足特定約束條件的平滑軌跡軌跡（包括行駛軌跡、速度、方向等），并輸入給控制執行層。如圖8所示，在車(chē)輛收到行為決策層的左變道指令后，主車(chē)基于各種信息規劃出幾條可行的路徑，如何規劃出最優(yōu)的路徑也是下文要介紹的內容。
圖8 運動(dòng)規劃示意圖全局路徑規劃與運動(dòng)規劃作為兩個(gè)層級的不同規劃，現將其特點(diǎn)匯總為表1。表1 全局路徑規劃與運動(dòng)規劃特點(diǎn)對比
正菜之前，我們先來(lái)了解一下圖（包括有向圖和無(wú)向圖）的概念。圖是圖論中的基本概念，用于表示物體與物體之間存在某種關(guān)系的結構。在圖中，物體被稱(chēng)為節點(diǎn)或頂點(diǎn)，并用一組點(diǎn)或小圓圈表示。節點(diǎn)間的關(guān)系稱(chēng)作邊，可以用直線(xiàn)或曲線(xiàn)來(lái)表示節點(diǎn)間的邊。
如果給圖的每條邊規定一個(gè)方向，那么得到的圖稱(chēng)為有向圖，其邊也稱(chēng)為有向邊，如圖9所示。在有向圖中，與一個(gè)節點(diǎn)相關(guān)聯(lián)的邊有出邊和入邊之分，而與一個(gè)有向邊關(guān)聯(lián)的兩個(gè)點(diǎn)也有始點(diǎn)和終點(diǎn)之分。相反，邊沒(méi)有方向的圖稱(chēng)為無(wú)向圖。
數學(xué)上，常用二元組G =（V，E）來(lái)表示其數據結構，其中集合V稱(chēng)為點(diǎn)集，E稱(chēng)為邊集。對于圖6所示的有向圖，V可以表示為{A，B，C，D，E，F，G}，E可以表示為{<A，B>，<B，C>，<B，F>，<B，E>，<C，E>，<E，B>，<F，G>}。<A，B>表示從頂點(diǎn)A發(fā)向頂點(diǎn)B的邊，A為始點(diǎn)，B為終點(diǎn)。
在圖的邊中給出相關(guān)的數，稱(chēng)為權。權可以代表一個(gè)頂點(diǎn)到另一個(gè)頂點(diǎn)的距離、耗費等，帶權圖一般稱(chēng)為網(wǎng)。
在全局路徑規劃時(shí)，通常將圖10所示道路和道路之間的連接情況，通行規則，道路的路寬等各種信息處理成有向圖，其中每一個(gè)有向邊都是帶權重的，也被稱(chēng)為路網(wǎng)（Route Network Graph）。
圖10 道路連接情況那么，全局路徑的規劃問(wèn)題就變成了在路網(wǎng)中，搜索到一條最優(yōu)的路徑，以便可以盡快見(jiàn)到那個(gè)心心念念的她，這也是全局路徑規劃算法最樸素的愿望。而為了實(shí)現這個(gè)愿望，誕生了Dijkstra和A*兩種最為廣泛使用的全局路徑搜索算法。
2.1 Dijkstra算法
戴克斯特拉算法（Dijkstra’s algorithm）是由荷蘭計算機科學(xué)家Edsger W. Dijkstra在1956年提出，解決的是有向圖中起點(diǎn)到其他頂點(diǎn)的最短路徑問(wèn)題。
假設有A、B、C、D、E、F五個(gè)城市，用有向圖表示如圖11，邊上的權重代表兩座城市之間的距離，現在我們要做的就是求出起點(diǎn)A城市到其它城市的最短距離。
圖11 五個(gè)城市構建的有向圖用Dijkstra算法求解步驟如下：
（1）創(chuàng )建一個(gè)二維數組E來(lái)描述頂點(diǎn)之間的距離關(guān)系，如圖12所示。E[B][C]表示頂點(diǎn)B到頂點(diǎn)C的距離。自身之間的距離設為0，無(wú)法到達的頂點(diǎn)之間設為無(wú)窮大。
圖12 頂點(diǎn)之間的距離關(guān)系（2）創(chuàng )建一個(gè)一維數組Dis來(lái)存儲起點(diǎn)A到其余頂點(diǎn)的最短距離。一開(kāi)始我們并不知道起點(diǎn)A到其它頂點(diǎn)的最短距離，一維數組Dis中所有值均賦值為無(wú)窮大。接著(zhù)我們遍歷起點(diǎn)A的相鄰頂點(diǎn)，并將與相鄰頂點(diǎn)B和C的距離3（E[A][B]）和10（E[A][C]）更新到Dis[B]和Dis[C]中，如圖13所示。這樣我們就可以得出起點(diǎn)A到其余頂點(diǎn)最短距離的一個(gè)估計值。
圖13 Dis經(jīng)過(guò)一次遍歷后得到的值（3）接著(zhù)我們尋找一個(gè)離起點(diǎn)A距離最短的頂點(diǎn)，由數組Dis可知為頂點(diǎn)B。頂點(diǎn)B有兩條出邊，分別連接頂點(diǎn)C和D。因起點(diǎn)A經(jīng)過(guò)頂點(diǎn)B到達頂點(diǎn)C的距離8（E[A][B] + E[B][C] = 3 + 5）小于起點(diǎn)A直接到達頂點(diǎn)C的距離10，因此Dis[C]的值由10更新為8。同理起點(diǎn)A經(jīng)過(guò)B到達D的距離5（E[A][B] + E[B][D] = 3 + 2）小于初始值無(wú)窮大，因此Dis[D]更新為5，如圖14所示。
圖14 Dis經(jīng)過(guò)第二次遍歷后得到的值（4）接著(zhù)在剩下的頂點(diǎn)C、D、E、F中，選出里面離起點(diǎn)A最近的頂點(diǎn)D，繼續按照上面的方式對頂點(diǎn)D的所有出邊進(jìn)行計算，得到Dis[E]和Dis[F]的更新值，如圖15所示。
圖15 Dis經(jīng)過(guò)第三次遍歷后得到的值（5）繼續在剩下的頂點(diǎn)C、E、F中，選出里面離起點(diǎn)A最近的頂點(diǎn)C，繼續按照上面的方式對頂點(diǎn)C的所有出邊進(jìn)行計算，得到Dis[E]的更新值，如圖16所示。
圖16 Dis經(jīng)過(guò)第四次遍歷后得到的值（6）繼續在剩下的頂點(diǎn)E、F中，選出里面離起點(diǎn)A最近的頂點(diǎn)E，繼續按照上面的方式對頂點(diǎn)E的所有出邊進(jìn)行計算，得到Dis[F]的更新值，如圖17所示。
圖17 Dis經(jīng)過(guò)第五次遍歷后得到的值（7）最后對頂點(diǎn)F所有點(diǎn)出邊進(jìn)行計算，此例中頂點(diǎn)F沒(méi)有出邊，因此不用處理。至此，數組Dis中距離起點(diǎn)A的值都已經(jīng)從“估計值”變?yōu)榱恕按_定值”。
基于上述形象的過(guò)程，Dijkstra算法實(shí)現過(guò)程可以歸納為如下步驟：
（1）將有向圖中所有的頂點(diǎn)分成兩個(gè)集合P和Q，P用來(lái)存放已知距離起點(diǎn)最短距離的頂點(diǎn)，Q用來(lái)存放剩余未知頂點(diǎn)?？梢韵胂?，一開(kāi)始，P中只有起點(diǎn)A。同時(shí)我們創(chuàng )建一個(gè)數組Flag[N]來(lái)記錄頂點(diǎn)是在P中還是Q中。對于某個(gè)頂點(diǎn)N，如果Flag[N]為1則表示這個(gè)頂點(diǎn)在集合P中，為1則表示在集合Q中。
（2）起點(diǎn)A到自己的最短距離設置為0，起點(diǎn)能直接到達的頂點(diǎn)N，Dis[N]設為E[A][N]，起點(diǎn)不能直接到達的頂點(diǎn)的最短路徑為設為∞。
（3）在集合Q中選擇一個(gè)離起點(diǎn)最近的頂點(diǎn)U（即Dis[U]最?。┘尤氲郊螾。并計算所有以頂點(diǎn)U為起點(diǎn)的邊，到其它頂點(diǎn)的距離。例如存在一條從頂點(diǎn)U到頂點(diǎn)V的邊，那么可以通過(guò)將邊U->V添加到尾部來(lái)拓展一條從A到V的路徑，這條路徑的長(cháng)度是Dis[U]+e[U][V]。如果這個(gè)值比目前已知的Dis[V]的值要小，我們可以用新值來(lái)替代當前Dis[V]中的值。
（4）重復第三步，如果最終集合Q結束，算法結束。最終Dis數組中的值就是起點(diǎn)到所有頂點(diǎn)的最短路徑。
2.2 A*算法
1968年，斯坦福國際研究院的Peter E. Hart, Nils Nilsson以及Bertram Raphael共同發(fā)明了A*算法。A*算法通過(guò)借助一個(gè)啟發(fā)函數來(lái)引導搜索的過(guò)程，可以明顯地提高路徑搜索效率。
下文仍以一個(gè)實(shí)例來(lái)簡(jiǎn)單介紹A*算法的實(shí)現過(guò)程。如圖18所示，假設小馬要從A點(diǎn)前往B點(diǎn)大榕樹(shù)底下去約會(huì )，但是A點(diǎn)和B點(diǎn)之間隔著(zhù)一個(gè)池塘。為了能盡快提到達約會(huì )地點(diǎn)，給姑娘留下了一個(gè)守時(shí)踏實(shí)的好印象，我們需要給小馬搜索出一條時(shí)間最短的可行路徑。
圖18 約會(huì )場(chǎng)景示意圖A*算法的第一步就是簡(jiǎn)化搜索區域，將搜索區域劃分為若干柵格。并有選擇地標識出障礙物不可通行與空白可通行區域。一般地，柵格劃分越細密，搜索點(diǎn)數越多，搜索過(guò)程越慢，計算量也越大；柵格劃分越稀疏，搜索點(diǎn)數越少，相應的搜索精確性就越低。
如圖19所示，我們在這里將要搜索的區域劃分成了正方形（當然也可以劃分為矩形、六邊形等）的格子，圖中藍色格子代表A點(diǎn)（小馬當前的位置），紫色格子代表B點(diǎn)（大榕樹(shù)的位置），灰色格子代表池塘。同時(shí)我們可以用一個(gè)二維數組S來(lái)表示搜素區域，數組中的每一項代表一個(gè)格子，狀態(tài)代表可通行和不可通行。
圖19 經(jīng)過(guò)簡(jiǎn)化后的搜索區域接著(zhù)我們引入兩個(gè)集合OpenList和CloseList，以及一個(gè)估價(jià)函數F = G + H。OpenList用來(lái)存儲可到達的格子，CloseList用來(lái)存儲已到達的格子。G代表從起點(diǎn)到當前格子的距離，H表示在不考慮障礙物的情況下，從當前格子到目標格子的距離。F是起點(diǎn)經(jīng)由當前格子到達目標格子的總代價(jià)，值越小，綜合優(yōu)先級越高。
G和H也是A*算法的精髓所在，通過(guò)考慮當前格子與起始點(diǎn)的距離，以及當前格子與目標格子的距離來(lái)實(shí)現啟發(fā)式搜索。對于H的計算，又有兩種方式，一種是歐式距離，一種是曼哈頓距離。
歐式距離用公式表示如下，物理上表示從當前格子出發(fā)，支持以8個(gè)方向向四周格子移動(dòng)（橫縱向移動(dòng)+對角移動(dòng)）。
曼哈頓距離用公式表示如下，物理上表示從當前格子出發(fā)，支持以4個(gè)方向向四周格子移動(dòng)（橫縱向移動(dòng)）。這是A*算法最常用的計算H值方法，本文H值的計算也采用這種方法。
現在我們開(kāi)始搜索，查找最短路徑。首先將起點(diǎn)A放入到OpenList中，并計算出此時(shí)OpenList中F值最小的格子作為當前方格移入到CloseList中。由于當前OpenList中只有起點(diǎn)A這個(gè)格子，所以將起點(diǎn)A移入CloseList，代表這個(gè)格子已經(jīng)檢查過(guò)了。
接著(zhù)我們找出當前格子A上下左右所有可通行的格子，看它們是否在OpenList當中。如果不在，加入到OpenList中計算出相應的G、H、F值，并把當前格子A作為它們的父節點(diǎn)。本例子，我們假設橫縱向移動(dòng)代價(jià)為10，對角線(xiàn)移動(dòng)代價(jià)為14。
我們在每個(gè)格子上標出計算出來(lái)的F、G、H值，如圖20所示，左上角是F，左下角是G，右下角是H。通過(guò)計算可知S[3][2]格子的F值最小，我們把它從OpenList中取出，放到CloseList中。
圖20 第一輪計算后的結果接著(zhù)將S[3][2]作為當前格子，檢查所有與它相鄰的格子，忽略已經(jīng)在CloseList或是不可通行的格子。如果相鄰的格子不在OpenList中，則加入到OpenList，并將當前方格子S[3][2]作為父節點(diǎn)。
已經(jīng)在OpenList中的格子，則檢查這條路徑是否最優(yōu)，如果非最優(yōu)，不做任何操作。如果G值更小，則意味著(zhù)經(jīng)由當前格子到達OpenList中這個(gè)格子距離更短，此時(shí)我們將OpenList中這個(gè)格子的父節點(diǎn)更新為當前節點(diǎn)。
對于當前格子S[3][2]來(lái)說(shuō)，它的相鄰5個(gè)格子中有4個(gè)已經(jīng)在OpenList，一個(gè)未在。對于已經(jīng)在OpenList中的4個(gè)格子，我們以它上面的格子S[2][2]舉例，從起點(diǎn)A經(jīng)由格子S[3][2]到達格子S[2][2]的G值為20（10+10）大于從起點(diǎn)A直接沿對角線(xiàn)到達格子S[2][2]的G值14。顯然A經(jīng)由格子S[3][2]到達格子S[2][2]不是最優(yōu)的路徑。當把4個(gè)已經(jīng)在OpenList 中的相鄰格子都檢查后，沒(méi)有發(fā)現經(jīng)由當前方格的更好路徑，因此我們不做任何改變。
對于未在OpenList的格子S[2][3]（假設小馬可以斜穿墻腳），加入OpenList中，并計算它的F、G、H值，并將當前格子S[3][2]設置為其父節點(diǎn)。經(jīng)歷這一波騷操作后，OpenList中有5個(gè)格子，我們需要從中選擇F值最小的那個(gè)格子S[2][3]，放入CloseList中，并設置為當前格子，如圖21所示。
圖21 第二輪計算后的結果重復上面的故事，直到終點(diǎn)也加入到OpenList中。此時(shí)我們以當前格子倒推，找到其父節點(diǎn)，父節點(diǎn)的父節點(diǎn)……，如此便可搜索出一條最優(yōu)的路徑，如圖22中紅色圓圈標識。
圖22 最后計算得到的結果基于上述形象的過(guò)程，A*算法實(shí)現過(guò)程可以歸納為如下步驟：
（1）將搜索區域按一定規則劃分，把起點(diǎn)加入OpenList。
（2）在OpenList中查找F值最小的格子，將其移入CloseList，并設置為當前格子。
（3）查找當前格子相鄰的可通行的格子，如果它已經(jīng)在OpenList中，用G值衡量這條路徑是否更好。如果更好，將該格子的父節點(diǎn)設置為當前格子，重新計算F、G值，如果非更好，不做任何處理；如果不在OpenList中，將它加入OpenList中，并以當前格子為父節點(diǎn)計算F、G、H值。
（4）重復步驟（2）和步驟（3），直到終點(diǎn)加入到OpenList中。
2.3 兩種算法比較
Dijkstra算法的基本思想是“貪心”，主要特點(diǎn)是以起點(diǎn)為中心向周?chē)鷮訉訑U展，直至擴展到終點(diǎn)為止。通過(guò)Dijkstra算法得出的最短路徑是最優(yōu)的，但是由于遍歷沒(méi)有明確的方向，計算的復雜度比較高，路徑搜索的效率比較低。且無(wú)法處理有向圖中權值為負的路徑最優(yōu)問(wèn)題。
A*算法將Dijkstra算法與廣度優(yōu)先搜索（Breadth-First-Search，BFS）算法相結合，并引入啟發(fā)函數（估價(jià)函數），大大減少了搜索節點(diǎn)的數量，提高了搜索效率。但是A*先入為主的將最早遍歷路徑當成最短路徑，不適用于動(dòng)態(tài)環(huán)境且不太適合高維空間，且在終點(diǎn)不可達時(shí)會(huì )造成大量性能消耗。
圖24是兩種算法路徑搜索效率示意圖，左圖為Dijkstra算法示意圖，右圖為A*算法示意圖，帶顏色的格子表示算法搜索過(guò)的格子。由圖23可以看出，A*算法更有效率，手術(shù)的格子更少。
03行為決策常用算法
作為L(cháng)4級自動(dòng)駕駛的優(yōu)秀代表Robotaxi，部分人可能已經(jīng)在自己的城市欣賞過(guò)他們不羈的造型，好奇心強烈的可能都已經(jīng)體驗過(guò)他們的無(wú)人“推背”服務(wù)。作為一個(gè)占有天時(shí)地利優(yōu)勢的從業(yè)人員，我時(shí)常在周末選一個(gè)人和的時(shí)間，叫個(gè)免費Robotaxi去超市買(mǎi)個(gè)菜。
在基于規則的方法中，通過(guò)對自動(dòng)駕駛車(chē)輛的駕駛行為進(jìn)行劃分，并基于感知環(huán)境、交通規則等信息建立駕駛行為規則庫。自動(dòng)駕駛車(chē)輛在行駛過(guò)程中，實(shí)時(shí)獲取交通環(huán)境、交通規則等信息，并與駕駛行為規則庫中的經(jīng)驗知識進(jìn)行匹配，進(jìn)而推理決策出下一時(shí)刻的合理自動(dòng)駕駛行為。正如全局路徑規劃的前提是地圖一樣，自動(dòng)駕駛行為分析也成為基于規則的行為決策的前提。不同應用場(chǎng)景下的自動(dòng)駕駛行為不完全相同，以高速主干路上的L4自動(dòng)駕駛卡車(chē)為例，其自動(dòng)駕駛行為可簡(jiǎn)單分解為單車(chē)道巡航、自主變道、自主避障三個(gè)典型行為。單車(chē)道巡航是卡車(chē)L4自動(dòng)駕駛系統激活后的默認狀態(tài)，車(chē)道保持的同時(shí)進(jìn)行自適應巡航。此駕駛行為還可以細分定速巡航、跟車(chē)巡航等子行為，而跟車(chē)巡航子行為還可以細分為加速、加速等子子行為，真是子子孫孫無(wú)窮盡也。自主變道是在變道場(chǎng)景（避障變道場(chǎng)景、主干路變窄變道場(chǎng)景等）發(fā)生及變道空間（與前車(chē)和后車(chē)的距離、時(shí)間）滿(mǎn)足后進(jìn)行左/右變道。自主避障是在前方出現緊急危險情況且不具備自主變道條件時(shí)，采取的緊急制動(dòng)行為，避免與前方障礙物或車(chē)輛發(fā)生碰撞。其均可以繼續細分，此處不再展開(kāi)。上面列舉的駕駛行為之間不是獨立的，而是相互關(guān)聯(lián)的，在一定條件滿(mǎn)足后可以進(jìn)行實(shí)時(shí)切換，從而支撐起L4自動(dòng)駕駛卡車(chē)在高速主干路上的自由自在?，F將例子中的三種駕駛行為之間的切換條件簡(jiǎn)單匯總如表2，真實(shí)情況比這嚴謹、復雜的多，此處僅為后文解釋基于規則的算法所用。
表2 狀態(tài)間的跳轉事件
圖24 狀態(tài)跳轉示意圖基于有限狀態(tài)機理論構建的智能車(chē)輛自動(dòng)駕駛行為決策系統，可將復雜的自動(dòng)駕駛過(guò)程分解為有限個(gè)自動(dòng)駕駛駕駛行為，邏輯推理清晰、應用簡(jiǎn)單、實(shí)用性好等特點(diǎn)，使其成為當前自動(dòng)駕駛領(lǐng)域目前最廣泛使用的行為決策方法。但該方法沒(méi)有考慮環(huán)境的動(dòng)態(tài)性、不確定性以及車(chē)輛運動(dòng)學(xué)以及動(dòng)力學(xué)特性對駕駛行為決策的影響，因此多適用于簡(jiǎn)單場(chǎng)景下，很難勝任具有豐富結構化特征的城區道路環(huán)境下的行為決策任務(wù)。3.2 基于學(xué)習的方法上文介紹的基于規則的行為決策方法依靠專(zhuān)家經(jīng)驗搭建的駕駛行為規則庫，但是由于人類(lèi)經(jīng)驗的有限性，智能性不足成為基于規則的行為決策方法的最大制約，復雜交通工況的事故率約為人類(lèi)駕駛員的百倍以上。鑒于此，科研工作者開(kāi)始探索基于學(xué)習的方法，并在此基礎上了誕生了數據驅動(dòng)型學(xué)習方法和強化學(xué)習方法。數據驅動(dòng)型學(xué)習是一種依靠自然駕駛數據直接擬合神經(jīng)網(wǎng)絡(luò )模型的方法，首先用提前采集到的老司機開(kāi)車(chē)時(shí)的自然駕駛數據訓練神經(jīng)網(wǎng)絡(luò )模型，訓練的目標是讓自動(dòng)駕駛行為決策水平接近老司機。而后將訓練好的算法模型部署到車(chē)上，此時(shí)車(chē)輛的行為決策就像老司機一樣，穿行在大街小巷。讀者可參見(jiàn)端到端自動(dòng)駕駛章節中介紹的NVIDIA demo案例。強化學(xué)習方法通過(guò)讓智能體（行為決策主體）在交互環(huán)境中以試錯方式運行，并基于每一步行動(dòng)后環(huán)境給予的反饋（獎勵或懲罰），來(lái)不斷調整智能體行為，從而實(shí)現特定目的或使得整體行動(dòng)收益最大。通過(guò)這種試錯式學(xué)習，智能體能夠在動(dòng)態(tài)環(huán)境中自己作出一系列行為決策，既不需要人為干預，也不需要借助顯式編程來(lái)執行任務(wù)。強化學(xué)習可能不是每個(gè)人都聽(tīng)過(guò)，但DeepMind開(kāi)發(fā)的圍棋智能AlphaGo（阿爾法狗），2016年3月戰勝世界圍棋冠軍李世石，2017年5月后又戰勝?lài)迨澜缗琶谝豢聺嵉氖?，大家應該都有所耳聞。更過(guò)分的是，半年后DeepMind在發(fā)布的新一代圍棋智能AlphaZero（阿爾法狗蛋），通過(guò)21天的閉關(guān)修煉，就戰勝了家族出現的各種狗子們，成功當選狗蛋之王。而賦予AlphaGo及AlphaZero戰勝人類(lèi)棋手的魔法正是強化學(xué)習，機器學(xué)習的一種。機器學(xué)習目前有三大派別：監督學(xué)習、無(wú)監督學(xué)習和強化學(xué)習。監督學(xué)習算法基于歸納推理，通過(guò)使用有標記的數據進(jìn)行訓練，以執行分類(lèi)或回歸；無(wú)監督學(xué)習一般應用于未標記數據的密度估計或聚類(lèi)；強化學(xué)習自成一派，通過(guò)讓智能體在交互環(huán)境中以試錯方式運行，并基于每一步行動(dòng)后環(huán)境給予的反饋（獎勵或懲罰），來(lái)不斷調整智能體行為，從而實(shí)現特定目的或使得整體行動(dòng)收益最大。通過(guò)這種試錯式學(xué)習，智能體能夠在動(dòng)態(tài)環(huán)境中自己作出一系列決策，既不需要人為干預，也不需要借助顯式編程來(lái)執行任務(wù)。這像極了馬戲團訓練各種動(dòng)物的過(guò)程，馴獸師一個(gè)抬手動(dòng)作（環(huán)境），動(dòng)物（智能體）若完成相應動(dòng)作，則會(huì )獲得美味的食物（正反饋），若沒(méi)有完成相應動(dòng)作，食物可能換成了皮鞭（負反饋）。時(shí)間一久，動(dòng)物就學(xué)會(huì )基于馴獸師不同的手勢完成不同動(dòng)作，來(lái)使自己獲得最多數量的美食。大道至簡(jiǎn)，強化學(xué)習亦如此。一個(gè)戰勝人類(lèi)圍棋冠軍的“智能”也僅由五部分組成：智能體（Agent）、環(huán)境（Environment）、狀態(tài)（State）、行動(dòng)（Action）和獎勵（Reward）。強化學(xué)習系統架構如圖25所示，結合自動(dòng)駕駛代客泊車(chē)中的泊入功能，我們介紹一下各組成的定義及作用。
圖25 強化學(xué)習系統架構代客泊車(chē)泊入功能的追求非常清晰，就是在不發(fā)生碰撞的前提下，實(shí)現空閑停車(chē)位的快速泊入功能。這個(gè)過(guò)程中，承載強化學(xué)習算法的控制器（域控制器/中央計算單元）就是智能體，也是強化學(xué)習訓練的主體。智能體之外的整個(gè)泊車(chē)場(chǎng)景都是環(huán)境，包括停車(chē)場(chǎng)中的立柱、車(chē)輛、行人、光照等。訓練開(kāi)始后，智能體實(shí)時(shí)從車(chē)載傳感器（激光雷達、相機、IMU、超聲波雷達等）讀取環(huán)境狀態(tài)，并基于當前的環(huán)境狀態(tài)，采取相應的轉向、制動(dòng)和加速行動(dòng)。如果基于當前環(huán)境狀態(tài)采用的行動(dòng)，是有利于車(chē)輛快速泊入，則智能體會(huì )得到一個(gè)獎勵，反之則會(huì )得到一個(gè)懲罰。在獎勵和懲罰的不斷刺激下，智能體學(xué)會(huì )了適應環(huán)境，學(xué)會(huì )了下次看到空閑車(chē)位時(shí)可以一把倒入，學(xué)會(huì )了面對不同車(chē)位類(lèi)型時(shí)采取不同的風(fēng)騷走位。從上述例子，我們也可以總結出訓練出一個(gè)優(yōu)秀的“智能”，大概有如下幾個(gè)步驟：（1）創(chuàng )建環(huán)境。定義智能體可以學(xué)習的環(huán)境，包括智能體和環(huán)境之間的接口。環(huán)境可以是仿真模型，也可以是真實(shí)的物理系統。仿真環(huán)境通常是不錯的起點(diǎn)，一是安全，二是可以試驗。（2）定義獎勵。指定智能體用于根據任務(wù)目標衡量其性能的獎勵信號，以及如何根據環(huán)境計算該信號?？赡苄枰?jīng)過(guò)數次迭代才能實(shí)現正確的獎勵塑造。（3）創(chuàng )建智能體。智能體由策略和訓練算法組成，因此您需要：（a）選擇一種表示策略的方式（例如，使用神經(jīng)網(wǎng)絡(luò )或查找表）。思考如何構造參數和邏輯，由此構成智能體的決策部分。
（b）選擇合適的訓練算法。大多數現代強化學(xué)習算法依賴(lài)于神經(jīng)網(wǎng)絡(luò )，因為后者非常適合處理大型狀態(tài)/動(dòng)作空間和復雜問(wèn)題。
（4）訓練和驗證智能體。設置訓練選項（如停止條件）并訓練智能體以調整策略。要驗證經(jīng)過(guò)訓練的策略，最簡(jiǎn)單的方法是仿真。（5）部署策略。使用生成的 C/C++ 或 CUDA 代碼等部署經(jīng)過(guò)訓練的策略表示。此時(shí)無(wú)需擔心智能體和訓練算法；策略是獨立的決策系統。強化學(xué)習方法除了具有提高行為決策智能水平的能力，還具備合并決策和控制兩個(gè)任務(wù)到一個(gè)整體、進(jìn)行統一求解的能力。將決策與控制進(jìn)行合并，這樣既發(fā)揮了強化學(xué)習的求解優(yōu)勢，又能進(jìn)一步提高自動(dòng)駕駛系統的智能性。實(shí)際上，人類(lèi)駕駛員也是具有很強的整體性的，我們很難區分人類(lèi)的行為中哪一部分是自主決策，哪一部分是運動(dòng)控制?，F階段強化學(xué)習方法的應用還處在摸索階段，應用在自動(dòng)駕駛的潛力還沒(méi)有被完全發(fā)掘出來(lái)，這讓我想起了母校的一句校歌：“能不奮勉乎吾曹？”04運動(dòng)規劃常用算法有了全局路徑參考信息，有了局部環(huán)境信息了，有了行為決策模塊輸入的決策信息，下一步自然而然的就要進(jìn)行運動(dòng)規劃，從而生成一條局部的更加具體的行駛軌跡，并且這條軌跡要滿(mǎn)足安全性和舒適性要求。
考慮到車(chē)輛是一個(gè)具有巨大慣性的鐵疙瘩且沒(méi)有瞬間移動(dòng)的功能，如果僅考慮瞬時(shí)狀態(tài)的行駛軌跡，不規劃出未來(lái)一段時(shí)間有前瞻性的行駛軌跡，那么很容易造成一段時(shí)間后無(wú)解。因此，運動(dòng)規劃生成的軌跡是一種由二維空間和一維時(shí)間組成的三維空間中的曲線(xiàn)，是一種偏實(shí)時(shí)的路徑規劃。運動(dòng)規劃的第一步往往采用隨機采樣算法，即走一步看一步，不斷更新行駛軌跡。代表算法是基于采樣的方法：PRM、RRT、Lattice。這類(lèi)算法通過(guò)隨機采樣的方式在地圖上生成子節點(diǎn)，并與父節點(diǎn)相連，若連線(xiàn)與障礙物無(wú)碰撞風(fēng)險，則擴展該子節點(diǎn)。重復上述步驟，不斷擴展樣本點(diǎn)，直到生成一條連接起點(diǎn)到終點(diǎn)的路徑。4.1 PRM算法概率路標法 (Probabilistic Road Maps, PRM），是一種經(jīng)典的采樣方法，由Lydia E.等人在1996年提出。PRM主要包含三個(gè)階段，一是采樣階段，二是碰撞檢測階段，三是搜索階段。圖26為已知起點(diǎn)A和終點(diǎn)B的地圖空間，黑色空間代表障礙物，白色空間代表可通行區域。在采樣階段中，PRM首先在地圖空間進(jìn)行均勻的隨即采樣，也就是對地圖進(jìn)行稀疏采樣，目的是將大地圖簡(jiǎn)化為較少的采樣點(diǎn)。
圖26 PRM工作原理示意圖（來(lái)源：https://mp.weixin.qq.com/s/WGOUf7g0C4Od4X9rnCfqxA）在碰撞檢測階段，剔除落在障礙物上的采樣點(diǎn)，并將剩下的點(diǎn)與其一定距離范圍內的點(diǎn)相連，同時(shí)刪除穿越障礙物的連線(xiàn)，從而構成一張無(wú)向圖。在搜索階段，利用全局路徑規劃算法章節介紹的搜索算法（Dijkstra、A*等）在無(wú)向圖中進(jìn)行搜索，從而找出一條起點(diǎn)A到終點(diǎn)B之間的可行路徑。算法步驟可以總結為：（1）構造無(wú)向圖G =（V，E），其中V代表隨機采樣的點(diǎn)集，E代表兩采樣點(diǎn)之間所有可能的無(wú)碰撞路徑，G初始狀態(tài)為空。（2）隨機撒點(diǎn)，并選取一個(gè)無(wú)碰撞的點(diǎn)c(i)加入到V中。（3）定義距離r，如果c(i)與V中某些點(diǎn)的距離小于r，則將V中這些點(diǎn)定義為c(i)的鄰域點(diǎn)。（4）將c(i)與其鄰域點(diǎn)相連，生成連線(xiàn)t，并檢測連線(xiàn)t是否與障礙物發(fā)生碰撞，如果無(wú)碰撞，則將t加入E中。（5）重復步驟2-4，直到所有采樣點(diǎn)（滿(mǎn)足采樣數量要求）均已完成上述步驟。（6）采用圖搜索算法對無(wú)向圖G進(jìn)行搜索，如果能找到起始點(diǎn)A到終點(diǎn)B的路線(xiàn)，說(shuō)明存在可行的行駛軌跡。PRM算法相比基于搜索的算法，簡(jiǎn)化了環(huán)境、提高了效率。但是在有狹窄通道場(chǎng)景中，很難采樣出可行路徑，效率會(huì )大幅降低。4.2 RRT快速探索隨機樹(shù)（Rapidly Exploring Random Trees，RRT），是Steven M. LaValle和James J. Kuffner Jr.在1998年提出的一種基于隨機生長(cháng)樹(shù)思想實(shí)現對非凸高維空間快速搜索的算法。與PRM相同的是兩者都是基于隨機采樣的算法，不同的是PRM最終生成的是一個(gè)無(wú)向圖，而RRT生成的是一個(gè)隨機樹(shù)。RRT的最顯著(zhù)特征就是具備空間探索的能力，即從一點(diǎn)向外探索拓展的特征。RRT分單樹(shù)和雙樹(shù)兩種類(lèi)型，單樹(shù)RRT將規起點(diǎn)作為隨機樹(shù)的根節點(diǎn)，通過(guò)隨機采樣、碰撞檢測的方式為隨機樹(shù)增加葉子節點(diǎn)，最終生成一顆隨機樹(shù)。而雙樹(shù)RRT則擁有兩顆隨機樹(shù)，分別以起點(diǎn)和終點(diǎn)為根節點(diǎn)，以同樣的方式進(jìn)行向外的探索，直到兩顆隨機樹(shù)相遇，從而達到提高規劃效率的目的。下面以圖27所示的地圖空間為例介紹單樹(shù)RRT算法的實(shí)現過(guò)程。在此地圖空間中，我們只知道起點(diǎn)A和終點(diǎn)B以及障礙物的位置（黑色的框）。
圖27 RRT算法舉例的地圖空間
對于單樹(shù)RRT算法，我們將起點(diǎn)A設置為隨機樹(shù)的根，并生成一個(gè)隨機采樣點(diǎn)，如圖27所示，隨機采樣點(diǎn)有下面這幾種情況。（1）隨機采樣點(diǎn)1落在自由區域中，但是根節點(diǎn)A和隨機采樣點(diǎn)1之間的連線(xiàn)存在障礙物，無(wú)法通過(guò)碰撞檢測，采樣點(diǎn)1會(huì )被舍棄，重新再生成隨機采樣點(diǎn)。（2）隨機采樣點(diǎn)2落在障礙物的位置，采樣點(diǎn)2也會(huì )被舍棄，重新再生成隨機采樣點(diǎn)。（3）隨機采樣點(diǎn)3落在自由區域，且與根節點(diǎn)A之間的連線(xiàn)不存在障礙物，但是超過(guò)根節點(diǎn)的步長(cháng)限制。但此時(shí)這個(gè)節點(diǎn)不會(huì )被簡(jiǎn)單的舍棄點(diǎn)，而是會(huì )沿著(zhù)根節點(diǎn)和隨機采樣點(diǎn)3的連線(xiàn)，找出符合步長(cháng)限制的中間點(diǎn)，將這個(gè)中間點(diǎn)作為新的采樣點(diǎn)，也就是圖28中的4。
圖28 不同隨機采樣點(diǎn)舉例
接著(zhù)我們繼續生成新的隨機采樣點(diǎn)，如果新的隨機采樣點(diǎn)位于自由區域，那么我們就可以遍歷隨機樹(shù)中已有的全部節點(diǎn)，找出距離新的隨機采樣點(diǎn)最近的節點(diǎn)，同時(shí)求出兩者之間的距離，如果滿(mǎn)足步長(cháng)限制的話(huà)，我們將接著(zhù)對這兩個(gè)節點(diǎn)進(jìn)行碰撞檢測，如果不滿(mǎn)足步長(cháng)限制的話(huà)，我們需要沿著(zhù)新的隨機采樣點(diǎn)和最近的節點(diǎn)的連線(xiàn)方向，找出一個(gè)符合步長(cháng)限制的中間點(diǎn)，用來(lái)替代新的隨機采樣點(diǎn)。最后如果新的隨機采樣點(diǎn)和最近的節點(diǎn)通過(guò)了碰撞檢測，就意味著(zhù)二者之間存在邊，我們便可以將新的隨機采樣點(diǎn)添加進(jìn)隨機樹(shù)中，并將最近的點(diǎn)設置為新的隨機采樣點(diǎn)的父節點(diǎn)。重復上述過(guò)程，直到新的隨機采樣點(diǎn)在終點(diǎn)的步長(cháng)限制范圍內，且滿(mǎn)足碰撞檢測。則將新的隨機采樣點(diǎn)設為終點(diǎn)B的父節點(diǎn)，并將終點(diǎn)加入隨機樹(shù)，從而完成迭代，生成如圖29所示的完整隨機樹(shù)。
圖29 隨機樹(shù)結算結果示例
相比PRM，RRT無(wú)需搜索步驟、效率更高。通過(guò)增量式擴展的方式，找到路徑后就立即結束，搜索終點(diǎn)的目的性更強。但是RRT作為一種純粹的隨機搜索算法，對環(huán)境類(lèi)型不敏感，當地圖空間中存在狹窄通道時(shí)，因被采樣的概率低，導致算法的收斂速度慢，效率會(huì )大幅下降，有時(shí)候甚至難以在有狹窄通道的環(huán)境找到路徑。圖30展示了 RRT應對存在狹窄通道地圖空間時(shí)的兩種表現，一種是RRT很快就找到了出路，一種是一直被困在障礙物里面。
圖30 RRT面對狹窄通道時(shí)的表現（來(lái)源：https://mp.weixin.qq.com/s/O8aK2RwOUXtcPSKQbDViiQ）圍繞如何更好的“進(jìn)行隨機采樣”、“定義最近的點(diǎn)”以及“進(jìn)行樹(shù)的擴展”等方面，誕生了多種改進(jìn)型的算法，包括雙樹(shù)RRT-Connect（雙樹(shù)）、lazy-RRT, RRT-Extend等。PRM和RRT都是一個(gè)概率完備但非最優(yōu)的路徑規劃算法，也就是只要起點(diǎn)和終點(diǎn)之間存在有效的路徑，那么只要規劃的時(shí)間足夠長(cháng)，采樣點(diǎn)足夠多，必然可以找到有效的路徑。但是這個(gè)解無(wú)法保證是最優(yōu)的。采用PRM和RRT等隨機采樣算法生成的行駛軌跡，大多是一條條線(xiàn)段，線(xiàn)段之間的曲率也不不連續，這樣的行駛軌跡是不能保證舒適性的，所以還需要進(jìn)一步進(jìn)行曲線(xiàn)平滑、角度平滑處理。代表算法是基于曲線(xiàn)插值的方法：RS曲線(xiàn)、Dubins曲線(xiàn)、多項式曲線(xiàn)、貝塞爾曲線(xiàn)和樣條曲線(xiàn)等。所有基于曲線(xiàn)插值方法要解決的問(wèn)題就是：在圖31上的若干點(diǎn)中，求出一條光滑曲線(xiàn)盡可能逼近所有點(diǎn)。下文以多項式曲線(xiàn)和貝塞爾曲線(xiàn)為例，介紹曲線(xiàn)插值算法的示例。
圖31 曲線(xiàn)插值方法要解決的問(wèn)題描述
4.3 多項式曲線(xiàn)
找到一條曲線(xiàn)擬合所有的點(diǎn)，最容易想到的方法就是多項式曲線(xiàn)。常用的有三階多項式曲線(xiàn)、五階多項式曲線(xiàn)和七階多項式曲線(xiàn)。理論上只要多項式的階數足夠高，就可以擬合各種曲線(xiàn)。但從滿(mǎn)足需求和工程實(shí)現的角度，階數越低越好。車(chē)輛在運動(dòng)規劃中，舒適度是一個(gè)非常重要的指標，在物理中衡量舒適性的物理量為躍度(Jerk)，它是加速度的導數。Jerk的絕對值越小意味著(zhù)加速度的變化越平緩，加速度的變化越平緩意味著(zhù)越舒適。而五次多項式曲線(xiàn)則被證明是在運動(dòng)規劃中可以使Jerk比較小的多項式曲線(xiàn)。以圖30所示換道場(chǎng)景為例，已知Frenet坐標系下?lián)Q道起點(diǎn)和終點(diǎn)的六個(gè)參數s0、v0、a0、st、vt、at，采用橫縱向解耦分別進(jìn)行運動(dòng)規劃的方法，可得橫向位置x(t)和縱向位置y(t)關(guān)于時(shí)間t的五次多項式表達式。
圖32 二階貝塞爾曲線(xiàn)示例接著(zhù)我們以每條直線(xiàn)的起點(diǎn)開(kāi)始，向各自的終點(diǎn)按比例t取點(diǎn)，如圖中的A和B。隨后我們將A和B相連得到一條直線(xiàn)，也按相同的比例t取點(diǎn)，便可得到C點(diǎn)，這也是二階貝塞爾曲線(xiàn)在比例為t時(shí)會(huì )經(jīng)過(guò)的點(diǎn)。比列t滿(mǎn)足如下的公式。
*博客內容為網(wǎng)友個(gè)人發(fā)布，僅代表博主個(gè)人觀(guān)點(diǎn)，如有侵權請聯(lián)系工作人員刪除。
