# Layered View of Autonomous Vehicles

還記得自駕車的行為可以拆成以下的層級由左往右五個層級嗎：

| Sensing | Perception | Planning and Decision | Control | Actuation |
|:------------------:|:----------------:|:---------------------:|:-----------------:|:---------:|
| Radar | Segmentation | Route Planning | Steering Control | Engine |
| Lidar | Object Detection | Behavior Planning | Torque Control | Brake |
| Camera | Object Tracking | Motion Planning | Emission Control | Wheel |
| GPS | Localization | Prediction | Energy Management | Light |

沒錯，這週要講的就是 Planning(跟 Decision) 跟 Control。
>Actuation 不是這門課會 cover 的部分

## 大致架構

- Route Planning
- 輸入：Road Topology、User-Specified Destination
- 輸出：Sequence of Point
- 或者說路徑上的各個重要的點
- Behavior Planning
- 輸入：Route Planning 得到的 Sequence of Point、Perception output
- Perception output 是隨時接收的
- 輸出：Motion Specifiction
- Motion and Trajectory Planning
- 輸入：Behavior Planning 得到的 Motion Specifiction、Estimated Pose and Free Space
- 輸出：Reference Path
- Low-level Feedback Control
- 輸入：上面得到的 Reference Path、Estimated Vehicle State
- 輸出 Low-level Commands

# Route Planning

在 ADA (或 DSA) 中學過的圖論演算法，就有提到過搜尋 shortest path 的各種演算法，
例如適用無負權圖的 Dijkstra。

或者是使用 A star，可以詳見我的「[人工智慧導論筆記]()」

Edge 的權重可以是實際距離、耗油量等等。

# Behavior Planning

有了要走的路之後，Behavior Planning 就是要考慮實際開車時遇到的細節部分，例如遇到行人要怎麼辦等等。

常見的方案就是使用 Finite state machine based 的方法，或者更進階一點使用 Game theory，例如使用 minmax tree 等決定手段。

但是有些東西不是固定的，例如車子、腳踏車、行人的行為；所以這時候可以使用一些 model 做一些假設性的預測：

- Gaussian mixture models
- Switching linear dynamical and stochastic systems
- Gaussian Process Regression

或者是使用有機率性質的方法：

- Markov Decision Processes
- 例如 Partially Observable Markov Decision Processes / POMDPs

## POMDPs

- model：6-tuple $(Q,A,O,T,Z,R)$
- $Q$：狀態 states 的集合
- $A$：行為 actions 的集合
- $O$：觀察 observations 的集合
- $T$：狀態轉移函數
- $T(q,a,q')$ 在狀態 $q$，行為 $a$ 之下，會輸出轉移到 狀態 $q'$ 的機率
- $Z$：觀察現象函數
- $Z(q,a,o)$ 在狀態 $q$，行為 $a$ 之下，會輸出看到 觀察 $o$ 的機率
- $R$：獎勵函數
- $R(q,a)$ 在狀態 $q$，行為 $a$ 之下，會輸出得到的獎勵

## Markov Model

這邊大致複習一下類別：

| Markov Models | 能夠控制狀態轉移 | 不能控制狀態轉移 |
|:--------------------:|:-------------------:|:-----------------------------:|
| 可以完整的觀察到狀態 | Markov Chain | Markov Decision Process (MPD) |
| 不能完整的觀察到狀態 | Hidden Markov Chain | POMDP |

有興趣的可以深入了解。

# Motion and Trajectory Planning

決定一個動作要怎麼做，是不是合法的。

常見的方法有：

- Variational methods
- Graph-search methods
- Tree-based methods

## Formulation

- $X$ configuration 的集合
- $X_{\text{init}}$：初始 configuration 的集合
- $X_{\text{goal}}$：結束 configuration 的集合
- $X_{\text{free}}(t)$：在特定時間 $t\in [0,T]$ 允許的 configuration 的集合
- $T$ Planning horizon
- 一段時間的最後時間點
- $\Pi(X,T)$ 從時間 $[0,T]$ 對 $X$ 的所有連續函數
- $\pi \in \Pi$：連續函數 $\pi$ 就是一個 trajectory
- $D(\pi(t)),\pi'(t),\pi''(t),...)$ 可微的限制函數 constraints
- $J$：根據 $\pi$ 算出 cost

## 終極目標

$$
\begin{align}
&\pi^*=\min J(\pi)\\
s.t.&\ \pi(0)\in X_{\text{init}}\\
&\ \pi(T)\in X_{\text{goal}}\\
&\ \pi(t)\in X_{\text{free}}(t),\ \forall t \in [0,T]\\
&\ D(\pi(t)),\pi'(t),\pi''(t),...)
\end{align}
$$

## Variational methos

形式跟上面一樣：

$$
\begin{align}
&\pi^*=\min J(\pi)\\
s.t.&\ f(\pi(t),\pi'(t),\pi''(t),...)\\
&\ g(\pi(t),\pi'(t),\pi''(t),...)\\
\end{align}
$$

只不過把限制換成一些函數。

想要變成沒有限制的最佳化問題，常用的手法是將限制先對時間積分起來，然後乘上 cost 後加到原本要最小化的函數內。
>我們要最小化的目標，就是要找到一個最佳的時間 $t$，做出動作；所以理論上是要函數對 $t$ 微分

上面這個手段其實跟 lagrange 的方法很像。

## Graph-search methods

先把連續的 configuration 切成離散的；切的方法可以是：

- 人工去切
- 人類手工區分出哪些是合法的 trajectory
- Geometric
- 透過一種表示方法，來去分出不同類型的 trajectory
- 請參見講義的圖
- Sampling-based
- 簡單來說就是透過模擬，分辨出哪些操作是可行的，然後記錄下來，以後就可以根據這些紀錄做判斷

## Tree-based methos

這邊介紹 Rapidly Random Trees 這個方法。

簡單來說就是每次都會有一個隨機的點，然後要根據現有的點，走一定的距離，來最靠近那個隨機點。

# Low-level Feedback Control

一個 Execution 就是給一個初始狀態，然後我們根據輸入的信號(他是一個時間的函數)做出一些動作，同時擁有一些限制。

我們通常會最在意以下性質：

- Existence：是否存在一個解，或者說是否存在一個 Execution
- Uniqueness：這樣的解是唯一的嗎
- Stability：隨時間得到的解，是否最終會趨向一個穩定的狀態，而不是持續震盪，或是受到干擾後變動很大。

## Controller Design

希望設計一個控制器，來控制系統可以達到目標。

- Open-Loop (Feed-Forward) Control
- 控制器 Controller 只會根據(人類的)輸入做動作，設備 Plant 的輸出沒有影響
- 好處是便宜，壞處就是沒有人類操作的話得到的控制品質會很差
- Closed-Loop (Feedback) Control
- Plant 的輸出會回饋給控制器
- 好處就是提升了品質，壞處就是變貴了

## PID 控制器

這裡我們介紹一種 Closed-Loop (Feedback) Control：PID 控制器。

- Plant 的輸出會算出三種值回饋給 Controller
- 首先根據回饋，以及目標值的差距，求得誤差 $e(t)$，然後算出以下三者
- $P=K_pe(t)$：當前的誤差
- $I=K_i\int_{0}^{t} e(s)ds$：歷史的(過去的)誤差
- $D=K_d \frac{d}{dt}e(t)$：未來的誤差
- 算出的三個值會影響各種數值，也影響了下一次 controller 的輸出；例如
- Rise Time：上升所需的時間
- Overshoot：超過目標值得量
- Settling time：抵達目標值的時間
- Steady-state error：變成 stable 狀態時的值跟預期的值差多少
- Stability：穩定度