單純作為閱讀筆記，文章內(nèi)容可能有些混亂。
1. Introduction
作者先介紹了一些關(guān)于自動駕駛汽車在未來城市交通系統(tǒng)中的潛在作用，包括增加安全性、提高生產(chǎn)力、提高可達性、提高道路效率以及對環(huán)境的積極影響；接著介紹自動駕駛汽車的起源與發(fā)展，當然少不了介紹DARPA；作者還介紹了關(guān)于MoD（Mobility on Demands）System相比傳統(tǒng)以私家車為主的交通方式的優(yōu)勢（方便、便宜），這也是自動駕駛汽車共享化的一個方向。
……
像Google、Tesla、Uber等等公司都在自動駕駛領(lǐng)域有著大量的投入與研發(fā)。
……
城市場景下的自動駕駛一直是熱點，DARPA Urban Challenge和V-Charge Project等等都介紹了大量這方面的工作。城市場景主要要解決動態(tài)、復(fù)雜的交通環(huán)境、交通參與者等等對自動駕駛汽車的影響。
下面是AI對V-Charge Project的解釋：
V-Charge項目是一個歐洲研究項目，旨在開發(fā)先進技術(shù)，實現(xiàn)自動泊車和車輛充電。該項目專注于開發(fā)一種完全自動化系統(tǒng)，使電動汽車（EV）可以在公共停車場內(nèi)自動泊車和充電，無需人為干預(yù)。
V-Charge系統(tǒng)包括幾個組件，包括：
自動泊車（AVP）系統(tǒng) - 這個系統(tǒng)使EV可以使用先進的傳感器和算法自動停放在指定的停車位上。
自動充電（AC）系統(tǒng) - 這個系統(tǒng)使EV可以自動連接和充電，無需人為干預(yù)。
車隊管理（FM）系統(tǒng) - 這個系統(tǒng)允許操作員實時管理和監(jiān)控EV的充電和泊車。
V-Charge項目由歐盟資助，涉及多家領(lǐng)先的技術(shù)公司和研究機構(gòu)，包括博世、寶馬和牛津大學。該項目旨在通過促進EV的使用，使充電和泊車更加方便和可訪問，從而減少城市交通的環(huán)境影響。
……
下面是作者給出的架構(gòu)圖：
經(jīng)典的以perception, planning以及control為主模塊的架構(gòu)圖。還考慮到V2V通信對自動駕駛系統(tǒng)的作用。
由于研究方向的緣故，我將重點關(guān)注PNC的部分,其他部分暫且省略。
2. Perception
暫略
3. Planning
3.1. Autonomous Vehicle Planning Systems
典型的架構(gòu)是將Planning分為三層：Mission Planning、Brhavioral Planning、Motion Planning。
- Mission Planning：高層次的規(guī)劃，例如決定是否、何時何地、如何出行、最佳路線等。
- Brhavioral Planning：基于其他agent或交通規(guī)則作決策，例如超車、讓行、并道等等。
- Motion Planning：運動規(guī)劃，最典型的目標是到達目標區(qū)域，同時避免障礙物碰撞。
3.2. Mission Planning
提供路由網(wǎng)絡(luò)定義文件(Route Network Definition File，RNDF)作為先驗信息。RNDF包含可通行路段的拓撲結(jié)構(gòu)，還包括停車標志(Stop Sign)位置、車道寬度和停車位位置等信息。RNDF早期是手動標注的，現(xiàn)在也有研究自動標注、在線標注的。
路由網(wǎng)絡(luò)圖 Road Network Graph (RNG) 存儲相關(guān)信息。
經(jīng)典的算法：Dijkstra，Astar，……
這里作者還提供了一篇route planning的綜述
Bast, Hannah, et al. “Route Planning in Transportation Networks.” arXiv: Data Structures and Algorithms,arXiv: Data Structures and Algorithms, Apr. 2015。
3.3. Behavioral Planning
Behavioral Planning需要使得車輛既能滿足交規(guī)、沿著路徑行駛的需求，也能與周邊交通參與者進行交互。這可以通過局部目標設(shè)置、虛擬障礙物放置、可行駛區(qū)域邊界調(diào)整和區(qū)域啟發(fā)式成本調(diào)整的組合來實現(xiàn)。類似于Apollo等等rule-based的方案。
FSM在行為決策中經(jīng)常被使用，但是由于是人工設(shè)計的，無法考慮到所有的復(fù)雜場景，當遇到未知場景時，就容易出問題。
作者還介紹了其他的一些工作，包括使用Linear-Temporal Logic (LTL) 完成超車場景的實驗以及一些對于尋找更大規(guī)則庫以適應(yīng)更復(fù)雜場景的嘗試等等。
3.4. Motion Planning
- Motion Planning典型的指標就是計算復(fù)雜度以及算法的完備性。
- Motion Planning的問題可以被視作一個“搬鋼琴問題”，房屋和鋼琴的結(jié)構(gòu)圖作為規(guī)劃器的輸入，得到一條安全無碰撞的路徑。同時，該問題是一個PSPACE-hard的問題。
- PSPACE復(fù)雜度類是指一個問題可以通過使用確定性圖靈機在輸入長度為 n n n時，使用的內(nèi)存（空間）按照 O ( n k ) O(n^k) O(nk)的漸進趨勢增長來解決，其中 k ≥ 0 k≥0 k≥0。確定性圖靈機是一種假設(shè)設(shè)備，它可以在一次只能更改一個符號/值的磁帶上操作，并且對于任何給定情況只能指定一種操作。一個問題A若被認為是PSPACE-hard，那么其中的每個問題/語言 B B B都可以通過多項式時間歸約為 A A A，即 B ≤ p A B≤pA B≤pA，這意味著任何 B B B都可以在多項式時間內(nèi)轉(zhuǎn)換為 A A A的實例。
- Motion Planning 的核心就是將連續(xù)空間模型轉(zhuǎn)換為離散空間模型。兩類轉(zhuǎn)變方式：
- combinatorial planning:對原始問題做到完全離散化描述
- sampling-based planning：對configuration space利用碰撞檢查對離散采樣點進行搜索。
3.4.1. Combinatorial Planning
combinatorial planning的方式對于低維度的問題有著不錯的解決效率，但對于多障礙物、高維的復(fù)雜問題，其計算復(fù)雜度將會大大增加。
在工程應(yīng)用中，直接從障礙物幾何中分解空間比較困難。一種簡單的方法是對configuration space或機器人actions統(tǒng)一的離散化，以便可以通過有限搜索來找到解決方案路徑。通過在離散空間中工作，大大減少了窮舉搜索的復(fù)雜性。但這種方式的完備性或者最優(yōu)性取決于離散化的精度。
DARPA Grand Challenges和DUC中提到的幾種方法：
- 基于對運動學可達軌跡集進行離散化的方法
- 基于道路幾何形狀生成軌跡搜索樹
- 基于優(yōu)化的速度平滑方法進一步改進上述方法。
Wenda Xu, et al. “A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles.” 2012 IEEE International Conference on Robotics and Automation, 2012, https://doi.org/10.1109/icra.2012.6225063。 - Astar
- Cell decomposition,這一類的方法接觸的比較少
作者介紹了一些行為決策的方法。比如結(jié)合Cell Decomposition以及Linear Temporal Logic (LTL)的方法生成遵守交規(guī)的路徑；基于LTL在部分情況下打破規(guī)則的方法；Cell Decomposition結(jié)合POMDP或MOMDP的方法來應(yīng)對不確定的環(huán)境。Bandyopadhyay, Tirthankar, et al. “Intention-Aware Motion Planning.” Springer Tracts in Advanced Robotics,Algorithmic Foundations of Robotics X, 2013, pp. 475–91, https://doi.org/10.1007/978-3-642-36279-8_29.
POMDP會假設(shè)機器人運動以及觀測中的不確定性，并在解決最優(yōu)策略中考慮這些不確定性。(Mixed Observability Markov Decision Processes, MOMDP) 則是對POMDP的一種拓展，在一些狀態(tài)下是部分可觀測的，另一些狀態(tài)下是完全可觀測的。Ong, Sylvie C. W., et al. “Planning under Uncertainty for Robotic Tasks with Mixed Observability.” The International Journal of Robotics Research, July 2010, pp. 1053–68, https://doi.org/10.1177/0278364910369861.
POMDP難以精確求解，通常采用估計都方式去代替；同時還面臨者計算復(fù)雜度隨著狀態(tài)量與計算視野的增長而快速增長的問題。也有些研究聚焦于連續(xù)空間中的POMDP。作者還介紹了高效的基于點的值迭代求解器SARSOP。
回頭繼續(xù)細看這些論文…
3.4.2. Sampling-Based Planning
Sampling-based的方法通常具有概率完備性?；诓蓸拥乃惴ǖ淖凅w主要不同于生成搜索樹的方法。經(jīng)典的方法有PRM和RRT。
規(guī)劃不僅需要考慮完備性和效率，也要考慮規(guī)劃出的路徑質(zhì)量(這段主要指的是最優(yōu)性，對于無人車的軌跡，還需要考慮平滑程度、時效性等等因素)。這里作者介紹了bias-RRT，以及PRM*、RRT*、Fast Marching Trees(FMT*)、Stable Sparse Trees (SST*)等漸進最優(yōu)的算法。
3.5. Planning in Dynamic Environments
在城市場景中實現(xiàn)自動駕駛，需要考慮大量的不確定性，這些不確定性可能來自于傳感器的精度誤差、定位誤差、環(huán)境變化、控制執(zhí)行時的誤差以及影響最大的周圍障礙物的移動變化。
3.5.1. Decision Making Structures for Obstacle Avoidance
DARPA中有一種做法：對沿著路徑可能發(fā)生碰撞的區(qū)域或交叉口合并區(qū)域進行檢測，利用周圍車輛軌跡來計算TTC，若即將發(fā)生碰撞，則會停止。這種方式相對保守。下面提到的defensive driving也是類似的對碰撞軌跡進行檢測。這種方法的優(yōu)點是計算簡單，因為忽略了時間維度；但是導(dǎo)致其只能應(yīng)用于比較簡單的場景。
基于POMDP的方法可以清晰闡述自動駕駛汽車運動過程中的各類不確定性因素，包括自車控制過程中的不確定性以及周圍環(huán)境、障礙物的不確定性，但POMDP難以建模，且需要對狀態(tài)空間進行離散化。
3.5.2. Planning in Space-Time
為了更好地考慮障礙物的運動軌跡，需要引入時間維度。對于障礙物感知而言，獲取移動障礙物的瞬時速度和位置是相對容易的，但是獲取障礙物未來的行駛軌跡是比較困難的。作者在這里介紹了利用恒定速度估計障礙物的軌跡，并用快速迭代的重規(guī)劃去減小誤差；也介紹了一種擴大障礙物boundingbox相對保守的策略。
這張圖是障礙物在 R 2 \mathbf R^2 R2space-time空間中的軌跡預(yù)測可視化圖。左1和左2不考慮障礙物未來軌跡的不確定性；剩余兩種方式分別考慮了速度限制和加速度限制。這樣的思想同樣可以用到自車速度、加速度的邊界估計以及軌跡邊界估計中。
3.5.3. Control Space Obstacle Representations
另一種方式是直接在控制空間里進行規(guī)劃。在這里作者提到了多機器人規(guī)劃的一種算法：Reciprocal Velocity Obstacles。
3.6. Planning Subject to Differential Constraints
微分約束?。。?/p>
Motion Planning的問題最終會變成一個high-level的控制問題。以往的方法中，常常由于計算復(fù)雜度、建模的復(fù)雜度忽略了控制的重要性，這會使得Motion Planning規(guī)劃出的軌跡在控制部分會產(chǎn)生較大誤差，使得軌跡效用低下，甚至會產(chǎn)生危險。執(zhí)行軌跡和規(guī)劃軌跡的誤差，使得在規(guī)劃部分碰撞檢測的效用更低，可能會產(chǎn)生危險。
長度更長的軌跡可能比長度較短的軌跡執(zhí)行時間要短。
對于一個系統(tǒng)，最重要的微分約束是時間的遞進，因為時間 t t t必定會以恒定的速率 t ˙ = 1 \dot t = 1 t˙=1增長
機器人微分約束主要應(yīng)用于以下幾個方向：
- 利用微分約束生成速度剖面，在這個剖面上的幾何路徑采用解耦的方式進行。（其實就是路徑-速度解耦的思想），作者這里給出兩篇論文。其中一篇應(yīng)該是速度-路徑規(guī)劃解耦思想的最早來源。Kant, Kamal, and Steven W. Zucker. “Toward Efficient Trajectory Planning: The Path-Velocity Decomposition.” The International Journal of Robotics Research, Aug. 1986, pp. 72–89, https://doi.org/10.1177/027836498600500304.
- 應(yīng)用于采樣算法中，當構(gòu)建樹時，直接利用direct integrated的方式實時求解每一段的連接。這部分作者主要推薦了一些kinodynamic 隨機采樣的算法。
- 考慮轉(zhuǎn)向半徑限制。比如Dubins曲線以及Reeds-Shepp曲線。還有一些啟發(fā)式的算法。
微分約束解耦的方式通常求解效率低下同時可能存在找不到解的情況；綜合微分約束(direct integrated differential constraint)可以彌補這些缺點，但是計算更為復(fù)雜。
通過將待采樣的狀態(tài)限制為一系列從起始條件開始并在obstacle-free的空間里能經(jīng)由符合運動學(kinodynamic)約束的軌跡到達的狀態(tài)集合，從而使得采樣更加高效。
將Reachability Guidance (RG)和最近鄰 (Nearest Neighbor，NN) 搜索運用到狀態(tài)采樣之中，可以為顯著提升效率。這是利用傳統(tǒng)的歐式空間距離進行搜索所不能做到的。
這一段內(nèi)容介紹了幾種將可達性指導(dǎo)策略（RG）應(yīng)用于運動規(guī)劃的分析方法。其中，通過球盒定理（Ball Box Theorem）將可達子空間表示為線性動力學逼近系統(tǒng)的超矩形，從而將RRT*算法的漸近最優(yōu)性擴展到可達性指導(dǎo)變體。類似的方法在PRM和FMT的約束處理變體中也被應(yīng)用，以證明其漸近最優(yōu)性。此外，還介紹了Goal-Rooted Feedback Motion Tree (GR-FMT) 和用于解決雙點邊界值問題的分析方法，但它們都有一定的限制條件。
還有利用機器學習的方法，比如SVM去尋找可達狀態(tài)。
3.7. Incremental Planning and Replanning
由于在實際工況中，自車通常不能觀測到整體環(huán)境，只能觀測到環(huán)境的部分，因此增長式的planning(incremental planning)就十分有必要了。同時由于動態(tài)環(huán)境的變化，原先規(guī)劃出來的軌跡可能不再適用，因此就需要重規(guī)劃Replanning。
Incremental planning需要獲得一系列子目標或者利用啟發(fā)式的算法對軌跡庫進行打分。作者介紹了利用FSM生成子目標的算法，當遇到規(guī)劃路徑被阻塞時生成子目標，子目標是在預(yù)先定義的路徑上并且間隔預(yù)先定義的距離。
作者在這一部分再次介紹了MOD（Mobility on Demands），提到了MOD中預(yù)先定義的path的重要性：可以避免局部規(guī)劃陷入極小值。
考慮到每個規(guī)劃周期需要有限的計算時間，并且環(huán)境可能在此期間發(fā)生變化，規(guī)劃的安全機制也應(yīng)該仔細設(shè)計。
Pendleton, Scott, et al. “Autonomous Golf Cars for Public Trial of Mobility-on-Demand Service.” 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015, https://doi.org/10.1109/iros.2015.7353517.
上面這篇文獻同樣是速度-路徑解耦的方式進行規(guī)劃，根據(jù)從所需路徑的加權(quán)縱向和橫向偏移測量的最近障礙物的接近程度規(guī)定減速的程度（該方法被稱為Dynamic Virtual Bumper）。不過在這篇文獻中，動態(tài)障礙物只是被當做放大之后的靜態(tài)障礙物。
此外，還有一些綜合考慮速度、空間路徑的一些方法，比如Inevitable Collision State Avoidance (ICS-AVOID) 、Non-Linear Velocity Obstacles (NLVO)、Time-Varying Dynamic Window。
4. Control
Control代表了系統(tǒng)的執(zhí)行能力，也稱為運動控制，是將意圖轉(zhuǎn)換為動作的過程。其目的通過向硬件級別提供必要的輸入來執(zhí)行計劃的意圖。Controllers、Measurements、Models等基本概念就不多做解釋了。
4.1. Classical Control
控制理論基礎(chǔ)，不詳細闡述了。
反饋控制時比較常用的經(jīng)典控制方法。典型的反饋控制就是PID。
u
(
t
)
=
k
d
e
˙
+
k
p
e
+
k
i
∫
e
(
t
)
d
t
u(t)=k_d\dot{e}+k_pe+k_i\int e(t)dt
u(t)=kd?e˙+kp?e+ki?∫e(t)dt
單純的反饋控制會有以下的缺點：
- 對誤差的響應(yīng)延遲，只有誤差發(fā)生時才會響應(yīng)。
- 對干擾、模型誤差、測量噪聲采用同樣的機制進行響應(yīng)，這種耦合的方式不如獨立響應(yīng)這些誤差的方式更合乎邏輯。
增加前饋控制項可以彌補這些缺陷。
前饋和反饋的比較。
現(xiàn)代控制理論的狀態(tài)空間控制方法，這方面的控制方法非常多。
典型的線性狀態(tài)空間模型：
x
(
t
)
˙
=
A
x
(
t
)
+
B
u
(
t
)
y
(
t
)
=
C
x
(
t
)
+
D
u
(
t
)
\dot{x(t)}=Ax(t)+Bu(t)\\ y(t)=Cx(t)+Du(t)
x(t)˙?=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t)
非線性狀態(tài)空間模型可以用以下公式表示：
x t + 1 = f ( x t , u t ) + ? t y t = h ( x t ) + δ t x_{t+1}=f(x_t,u_t)+\epsilon_t\\ y_t=h(x_t)+\delta_t xt+1?=f(xt?,ut?)+?t?yt?=h(xt?)+δt?
其中， x t x_t xt?表示狀態(tài)向量， u t u_t ut?表示控制向量， y t y_t yt?表示觀測向量。 f f f是狀態(tài)轉(zhuǎn)移函數(shù)， h h h是觀測函數(shù)， ? t \epsilon_t ?t?和 δ t \delta_t δt?為噪聲，通常假設(shè)為高斯白噪聲。這是一般的非線性狀態(tài)空間模型的表達方式，也存在特殊的非線性狀態(tài)空間模型形式。
2自由度控制器也可以應(yīng)用于非線性系統(tǒng)。前饋用于生成參考軌跡，而反饋用于補償干擾和誤差。
4.2. Model Predictive Control
MPC三大要素：預(yù)測模型、反饋控制、滾動優(yōu)化。
出于規(guī)劃和預(yù)測的目的，自動控制系統(tǒng)需要模型的介入。這就不得不提經(jīng)典的MPC了。MPC將模型預(yù)測和優(yōu)化控制相結(jié)合，通過將控制問題轉(zhuǎn)化為優(yōu)化問題，將未來一段時間內(nèi)控制動作的優(yōu)化和最優(yōu)化控制策略的實時調(diào)整相結(jié)合，從而實現(xiàn)優(yōu)化控制。
MPC的核心思想是基于預(yù)測模型，對未來系統(tǒng)狀態(tài)進行預(yù)測，并利用這些預(yù)測結(jié)果確定控制輸入，以使控制性能最優(yōu)化。通過優(yōu)化來選擇最優(yōu)的控制策略，將未來的多步控制動作序列預(yù)先計算出來，然后僅執(zhí)行第一個控制動作。在下一個控制周期開始時，該過程將再次重復(fù)。這樣，MPC可以處理非線性和非穩(wěn)態(tài)問題，同時適用于各種不同類型的控制目標和約束。
MPC的主要優(yōu)點包括：
- 能夠處理非線性、時變和分布式參數(shù)系統(tǒng)；
- 能夠處理各種不同類型的約束條件，如輸入、輸出和狀態(tài)約束；
- 具有強魯棒性，能夠應(yīng)對模型不確定性和擾動；
- 能夠?qū)Χ喾N控制目標進行優(yōu)化，如跟蹤、穩(wěn)定性和能耗等。
MPC的框架
不過MPC比較消耗計算資源，若車載芯片算力大大提升，MPC的應(yīng)用價值將會更高。
MPC 已應(yīng)用于多種汽車控制應(yīng)用，包括牽引控制、制動和轉(zhuǎn)向、車道保持等。模型預(yù)測控制技術(shù)也被應(yīng)用于各種軌跡跟蹤控制問題。
MPC問題的一般描述形式：
t t t為時間序列。 v ( h ∣ t ) v(h|t) v(h∣t)代表 v v v在時間 t t t處預(yù)測時長為 h h h步。等式(7)、(8)是離散時間系統(tǒng)模型的描述，采樣間隔為 T s T_s Ts?， x ∈ R n x\in \mathbf R^n x∈Rn是系統(tǒng)的狀態(tài)變量， u ∈ R m u\in \mathbf R^m u∈Rm為控制輸入， y ∈ R p y\in \mathbf R^p y∈Rp是系統(tǒng)輸出。 U ( t ) = ( u ( 0 ∣ t ) ， ? ? ? ， u ( N ? 1 ∣ t ) ) U(t) = (u(0|t)，···，u(N?1|t)) U(t)=(u(0∣t)，???，u(N?1∣t))是控制輸入的序列，需要求解其最優(yōu)值， N N N是預(yù)測時域的長度。cost function由階段性的代價 L L L和終端代價 F F F共同構(gòu)成。 N c N_c Nc?和 N c u N_{cu} Ncu?分別是狀態(tài)約束和輸出約束的時域長度。 N u N_u Nu?為控制時域的長度。
MPC在每一輪時間下求解一段最優(yōu)序列 U ? ( t ) U^*(t) U?(t)并且輸出序列的第一個值。
4.3. Trajectory Generation and Tracking
依據(jù)已知的路徑進行軌跡生成主要有兩種方式：一種通過優(yōu)化方法在生成軌跡的同時去跟蹤軌跡；另一種則是將兩個部分分開進行。
4.3.1. Combined Trajectory Generation and Tracking
Combined Trajectory Generation and Tracking這種方式通常針對于最優(yōu)時間軌跡方面的應(yīng)用，對于復(fù)雜的環(huán)境，其實時性可能會出現(xiàn)問題。
作者這里給了一篇文獻：Kunz, Tobias, and Mike Stilman. Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. 2013, https://doi.org/10.7551/mitpress/9816.001.0001.
4.3.2. Separate Trajectory Generation and Tracking
Trajectory Generation
軌跡生成問題可以歸結(jié)為兩點邊界值問題(boundary value problem)。邊界條件通常是包含起始狀態(tài) x ( t 0 ) = x 0 x(t_0) = x_0 x(t0?)=x0? 和最終目標狀態(tài) x ( t f ) = x f x(t_f ) = x_f x(tf?)=xf? 的約束，系統(tǒng)動力學 x ˙ = f ( x , u ) \dot x = f (x, u) x˙=f(x,u) 作為附加約束。如果給定滿足邊界條件的狀態(tài)軌跡 x ( t ) x(t) x(t)沒有控制輸入 u ( t ) u(t) u(t)，則軌跡被定義為不可行的。
軌跡生成主要有兩類方法：
- 基于傳感器。主要應(yīng)用于機器人領(lǐng)域，依賴環(huán)境感知生成響應(yīng)軌跡，不考慮相關(guān)動力學因素。
- 基于動力學。這方面有許多優(yōu)化算法，比如應(yīng)用遺傳算法、梯度下降等等。這類方法也有在輔助駕駛上的應(yīng)用。
Trajectory Tracking
軌跡相較于路徑，還有對運動速度的描述（其實在更多其他的資料里，軌跡相比于路徑多了時間 t t t這一個維度）。
主要有兩種方法：
- 基于幾何的方法。
- 基于模型的方法?；谀Ｐ偷姆椒ㄍǔ７譃榛谶\動學和基于動力學的方法；前者適用于低速、曲率突變小的場景；后者適用于高速、曲率較大的場景。基于模型的方法通常需要路徑是連續(xù)的，但可能對干擾和較大偏差魯棒性較差。
Geometric Path Tracking
Pure pursuit
基于幾何的跟蹤算法使用簡單的幾何關(guān)系去推導(dǎo)轉(zhuǎn)向控制律。這些方法使用前視距離去估計誤差，算法的復(fù)雜程度從簡單的圓弧計算到復(fù)雜的幾何定理都有。純追蹤算法就是一個典型的基于幾何的算法。
純追蹤算法的輸入是一系列的路徑點，而不是連續(xù)的曲線，這也就不需要考慮離散化所帶來的影響。但是純追蹤算法難以適用于高速場景或者曲率突變的場景，同時前視距離沒選好的話，會存在穩(wěn)態(tài)誤差(本質(zhì)就是個P控制器)。
為了避免限制路徑的生成方式，路徑應(yīng)該分段線性的方式進行表示。通過這一原理，一個光滑的有連續(xù)的一階導(dǎo)和二階導(dǎo)的軌跡曲線，應(yīng)該有一系列密集點進行表示，從而應(yīng)用于更多算法。(這一部分還需進一步的理解)
下面的文獻是純追蹤算法的一個變種，基準點不是選在后軸，而是在距離后軸的一段距離 x b x_b xb?的 e e e點上。
Kuwata, Yoshiaki, et al. “Motion Planning in Complex Environments Using Closed-Loop Prediction.” AIAA Guidance, Navigation and Control Conference and Exhibit, 2008, https://doi.org/10.2514/6.2008-7166.
還有研究證明通過正確組合前視距離以及系統(tǒng)延遲可以使得純追蹤算法穩(wěn)定。
純追蹤算法有一個可調(diào)的參數(shù)前視距離 L f w L_{fw} Lfw?， L f w L_{fw} Lfw?越小，跟蹤時會離路徑更近，但會產(chǎn)生震蕩； L f w L_{fw} Lfw?越大，會有跟蹤誤差，但跟蹤效果平滑。
推導(dǎo)過程比較簡單，就直接將原文截進來用了。
純追蹤算法的缺陷：沒有考慮到目標點所在路徑的曲率以及目標點的航向角。純追蹤算法僅僅計算了圓弧的幾何特性，忽略了車輛的橫向動力學特性，特別是當速度提升時，橫向動力學的影響將會更大。
Stanley
Stanley算法也十分經(jīng)典。
論文：Hoffmann, Gabriel M., et al. “Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing.” 2007 American Control Conference, 2007, https://doi.org/10.1109/acc.2007.4282788.
Stanley算法通過非線性控制律來計算轉(zhuǎn)向命令。該控制律主要考慮了從車輛前軸計算的跟蹤誤差 e f a e_{fa} efa?和航向角誤差 θ e \theta_e θe?
θ
e
=
θ
?
θ
p
\theta_e=\theta-\theta_p
θe?=θ?θp?，
θ
\theta
θ是車輛的航向角，
θ
p
\theta_p
θp?是道路在
(
x
p
,
y
p
)
(x_p,y_p)
(xp?,yp?)點處的切線方向所形成的角。
如此可以得到轉(zhuǎn)向角計算公式：
δ
(
t
)
=
θ
e
(
t
)
+
t
a
n
?
1
(
k
e
f
a
(
t
)
v
x
(
t
)
)
\delta(t)=\theta_e(t)+tan^{-1}\left(\frac{ke_{fa}(t)}{v_x(t)}\right)
δ(t)=θe?(t)+tan?1(vx?(t)kefa?(t)?)
k
k
k為增益系數(shù)，
v
x
(
t
)
v_x(t)
vx?(t)為車輛的縱向速度。
優(yōu)點：能夠收斂到0。相比純追蹤算法，跟蹤效果更好且不會出現(xiàn)"cut cornor"的問題。高速狀態(tài)下，適應(yīng)性好。
缺點： 對干擾魯棒性差，更容易發(fā)生震蕩。需要提供擁有連續(xù)曲率的路徑而不是離散點。
Trajectory Tracking with a Model
De Luca, Oriolo, and Samson等人提出了一種基于運動學自行車模型的控制算法《Feedback control of a nonholonomic car-like robot》。
如圖所示，他們將路徑描述成關(guān)于長度
s
s
s的函數(shù)，
θ
p
\theta_p
θp?為
x
x
x軸和
(
x
p
,
y
p
)
(x_p,y_p)
(xp?,yp?)切線方向的夾角。航向角誤差則為：
θ
e
=
θ
?
θ
p
(
s
)
\theta_e=\theta -\theta_p(s)
θe?=θ?θp?(s)，道路曲率被定義為：
κ
(
s
)
=
d
θ
p
(
s
)
d
s
\kappa(s)=\frac{d\theta_p(s)}{ds}
κ(s)=dsdθp?(s)?，兩邊同乘以
s
˙
\dot s
s˙，得
θ
˙
p
(
s
)
=
κ
(
s
)
s
˙
\dot \theta_p(s)=\kappa(s)\dot s
θ˙p?(s)=κ(s)s˙。
s
˙
和
e
˙
r
a
\dot s和\dot e_{ra}
s˙和e˙ra?如下表示：
s
˙
=
v
c
o
s
(
θ
e
)
+
θ
˙
p
e
r
a
e
r
a
=
v
s
i
n
(
θ
e
)
\begin{aligned}\dot{s}&=vcos(\theta_e)+\dot{\theta}_pe_{ra}\\e_{ra}&=vsin(\theta_e)\end{aligned}
s˙era??=vcos(θe?)+θ˙p?era?=vsin(θe?)?
最后得到運動學模型：
[
s
˙
e
˙
r
a
θ
˙
e
δ
˙
]
=
[
c
o
s
(
θ
e
)
1
?
e
˙
r
a
κ
(
s
)
s
i
n
(
θ
e
)
(
t
a
n
δ
L
)
?
κ
(
s
)
c
o
s
(
θ
e
)
1
?
e
˙
r
a
κ
(
s
)
0
]
v
[
0
0
0
1
]
δ
˙
\left[\begin{array}{c}\dot{s}\\\dot{e}_{ra}\\\dot{\theta}_{e}\\\dot \delta\end{array}\right]=\left[\begin{array}{c}\frac{cos(\theta_{e})}{1-\dot{e}_{ra}\kappa(s)}\\sin(\theta_{e})\\(\frac{tan\delta}L)-\frac{\kappa(s)cos(\theta_{e})}{1-\dot{e}_{ra}\kappa(s)}\\0\end{array}\right]v\left[\begin{array}{c}0\\0\\0\\1\end{array}\right]\dot{\delta}
?s˙e˙ra?θ˙e?δ˙?
?=
?1?e˙ra?κ(s)cos(θe?)?sin(θe?)(Ltanδ?)?1?e˙ra?κ(s)κ(s)cos(θe?)?0?
?v
?0001?
?δ˙文章來源：http://www.zghlxwxcb.cn/news/detail-709654.html
MPC在路徑跟蹤控制中應(yīng)用不少，但是耗時較大。作者同樣給了幾篇關(guān)于基于動力學/運動學模型的MPC的相關(guān)文獻。文章來源地址http://www.zghlxwxcb.cn/news/detail-709654.html
到了這里，關(guān)于【論文筆記】Perception, Planning, Control, and Coordination for Autonomous Vehicles的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
