# 進度報告(2024/11/18)

## 論文閱讀
**Review of Pedestrian Trajectory Prediction Methods: Comparing Deep Learning and Knowledge-Based Approaches**[^1]這篇review主要介紹了現今用於預測行人路徑的各種方法，主要分成兩大領域，KB和DL:
### Knowledge Based (KB):
* **社會力模型Social Force Model (SFM):**
模擬行人在受到周圍環境和其他行人的影響下的運動。琮祐學長使用的指數機率分布之碰撞風險可以算是這個領域的延伸。
* **ORCA:**
將速度障礙法(VO)的限制作線性簡化，可以用於模擬預測短期內行人之速度與位置。
### Deep learning (DL):
* 長短期記憶網絡LSTM
* 卷積神經網絡CNN
* 生成對抗網絡GAN

文章分析到: KB模型較適合短期的路人軌跡預測，或是應用於解釋高密度的擁擠環境；相對之下，DL模型在低密度的場景下依然能維持更高的的準確性，也能學習一些個體複雜的走路模式，但是這種模型依賴大量的訓練，且可能缺乏參數的解釋性。此外，這篇文章還提到**混合模型 (Hybrid Approach)** 試圖結合前兩種方法的優點並消除缺點。

我認為無論是這篇論文中提到的任一種方法，目的都是要將人類的走路行為做建模。並依照此模型做該行人的未來軌跡之預測。

## 行人環境的建置
正如同作車輛行為分析需要有道路模型，我應該也要做個行人模型。由於我的研究可能比較偏向讓機器人閃避環境裡的人類(或其他動態障礙)，因此，在模擬中讓機器人閃避的那些節點，不能僅是做單純的直線運動、圓周運動或任何預先規畫好的路徑。那些節點應概要被假設為人類，會自己朝目標前進，並有能力對其他節點作出反應，例如:
1. 行人A與行人B應有能力在相遇時進互相避讓
2. 行人面對機器人時應該也要作出閃避的反應

因為這部分並不是我研究的重點，所以目前架設一個簡易的行人環境即可。參考上面論文後，我決定先套用ORCA或是SFM在我的障礙物節點上。而用SFM做行人行為模擬一直有論文在討論，相較之下ORCA並不能完全適用於行人的走路行為，因此現階段的環境可能會先從一個簡易的SFM開始。

一般來說，基本的社會力模型Social Force Model(SFM)假設形人的移動模式受到三種力牽引:
1. $\mathbf{f}_{driven}$: 朝向目標的吸引力
2. $\mathbf{f}_{interaction}$: 與其他行人(或移動障礙物)的排斥力
3. $\mathbf{f}_{wall}$: 與牆壁的斥力

$$
m\frac{d\mathbf{v}}{dt}=\mathbf{f}_{driven}+\mathbf{f}_{interaction}+\mathbf{f}_{wall}
$$
假設行人質量為$m$，則行人的加速度向量，可以被寫為上述的公式。在我初步的模擬環境中，並沒有靜態障礙，因此，$\mathbf{f}_{wall}$可以被暫時拿掉，而剩下的兩種力的公式如下:
$$
\begin{aligned}
&\mathbf{f}_{driven} = m\frac{v_{id}\mathbf{e}-\mathbf{v}_c}{\tau}\\
&\mathbf{f}_{interaction}=\sum_{j=1}^{k}A\exp(\frac{D-d_j}{B})\mathbf{n}_j
\end{aligned}
$$
* $v_{id}$: 行人前進的理想速度
* $\mathbf{e}=\frac{\mathbf{p}_c-\mathbf{p}_g}{\|\mathbf{p}_c-\mathbf{p}_g\|}$: 行人當前位置$\mathbf{p}_c$朝向目標點$\mathbf{p}_g$的單位向量
* $\mathbf{v}_c$: 行人當前的速度向量
* $A$: 行人間的排斥力
* $B$: 排斥範圍
* $D$: 行人中心間的安全距離
* $d_j=\|\mathbf{p}_c-\mathbf{p}_j\|$: 行人與$j$行動障礙物的中心距離
* $\mathbf{n}_j=\frac{\mathbf{p}_c-\mathbf{p}_j}{\|\mathbf{p}_c-\mathbf{p}_j\|}$: 行人到$j$行動障礙物的單位向量

### 測試
目前測試的參數為:
* $v_{id}=1(m)$
* $A = 3\times10^2(N)$
* $B = 0.6(m)$
* $D = 0.11 + r_1 + r_2(m)$，r為節點半徑
* 情境: 兩顆節點(行人)
{%youtube eNH9C-r0a6g%}

## 預測節點位置
再重看了一遍10/21號進度報告提到的那篇論文**Real time trajectory prediction for collision risk estimation between vehicles[^2]**。他採用的預測法，是建立移動障礙物的線性模型，並量測動態障礙的當前位置及速度，利用線性的Kalman Filter獲得該障礙的狀態(位置、速度、加速度)估測值，再使用此估測狀態基於線性模型預測短時間的障礙物未來位置。我認為這種不需要太多障礙物參數，且短期內有效的預測法，應該適合我那種看不遠的MPC，所以想要套用該論文的方法在我的情境中。

首先是障礙物的線性模型:
$$
\mathbf{x}_i = \begin{bmatrix}x\\y\\v_x\\v_y\\a_x\\a_y\end{bmatrix}_i\ \mathbf{A} = \begin{bmatrix}1&0&\delta t&0&0&0\\0&1&0&\delta t&0&0\\0&0&1&0&\delta t&0\\0&0&0&1&0&\delta t\\0&0&0&0&1&0\\0&0&0&0&0&1\end{bmatrix}
$$
$$
\mathbf{x}_{i+1}=\mathbf{A}\mathbf{x}_i + \mathbf{w}_k
$$
* $\mathbf{A}$: prediction matrix
* $\mathbf{x}$: model state
* $\mathbf{w}_k$: process noise

接著是觀測模型:
$$
\mathbf{z}_i = \begin{bmatrix}x\\y\\v_x\\v_y\end{bmatrix}\
\mathbf{H} = \begin{bmatrix}1&0&0&0&0&0\\0&1&0&0&0&0\\0&0&1&0&0&0\\0&0&0&1&0&0\end{bmatrix}
$$
$$
\mathbf{z}_i = \mathbf{H}\mathbf{x}_i + \mathbf{v}_k
$$
* $\mathbf{H}$: observation matrix
* $\mathbf{x}$: measured state
* $\mathbf{w}_k$: measurement noise

Kalman Filter 的4步驟:
### 1. prediction step:
$$
\begin{aligned}
&\hat{\mathbf{x}}_k^-=\mathbf{A}\hat{\mathbf{x}}_{k-1}\\
&\mathbf{P}_k^-=\mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^T+\mathbf{Q}
\end{aligned}
$$
* $\hat{\mathbf{x}}_k^-$: 預測$k$時段的估測狀態
* $\hat{\mathbf{x}}_{k-1}$: 上一時刻的估測狀態
* $\mathbf{P}$: covariance matrix of estimated state
* $\mathbf{Q}$: covariance matrix of process noise($\mathbf{w}_k$)

### 2. caculating kalman gain:
$$
\mathbf{K}_k = \mathbf{P}_k^-\mathbf{H}^T(\mathbf{H}\mathbf{P}_k^-\mathbf{H}^T+\mathbf{R})^{-1}
$$
* $\mathbf{R}$: covariance matrix of measurement noise

### 3. estimation:
$$
\hat{\mathbf{x}}_k=(\mathbf{I}-\mathbf{K}_k\mathbf{H})\hat{\mathbf{x}}_k^- + \mathbf{K}_k\mathbf{z}_k
$$

### 4. update:
$$
\mathbf{P}_k = (\mathbf{I}-\mathbf{K}_k\mathbf{H})\mathbf{P}_k^-
$$

$\mathbf{Q}$和$\mathbf{R}$都是依情況調整的參數，而開啟Kalman Filter前，需要初始猜值$\hat{\mathbf{x}}_0$與$\mathbf{P}_0$。接下來的每一步都可以得到障礙物模型的估測狀態$\hat{\mathbf{x}}_k$與該估測狀態的變異數矩陣$\mathbf{P}_k$(開根後可得標準差$\sigma_k$)。我們可以將這個估測狀態$\hat{\mathbf{x}}_k$多次套入障礙物的線性模型，得到未來一段時間的障礙物位置$(\hat{\mathbf{x}}_{k+1}^-,\hat{\mathbf{x}}_{k+2}^-,\cdots)$。
$$
\begin{aligned}
&\hat{\mathbf{x}}_{k+1}^-=\mathbf{A}\hat{\mathbf{x}}_{k}\\
&\hat{\mathbf{x}}_{k+2}^-=\mathbf{A}\hat{\mathbf{x}}_{k+1}\\
&\ \ \vdots\\
&\hat{\mathbf{x}}_{k+n}^-=\mathbf{A}\hat{\mathbf{x}}_{k+n-1}
\end{aligned}
$$
假設該障礙物的未來位置皆為高斯分佈，那麼$k+1, k+2, \cdots$時段後的位置依然為高斯分佈，模型參數的$\mu$設為依照線性模型預估的位置，$\sigma$則設為$\sigma_k$。因此未來幾步的標準差，一樣可以用線性模型得到
$$
\begin{aligned}
&\vec{\sigma^2}_{k+1} = \mathbf{A}\vec{\sigma^2}_{k}\\
&\vec{\sigma^2}_{k+2} = \mathbf{A}\vec{\sigma^2}_{k+1}\\
&\ \ \vdots\\
&\vec{\sigma^2}_{k+n} = \mathbf{A}\vec{\sigma^2}_{k+n-1}
\end{aligned}
$$
如圖，假設藍線為未來$k$時段障礙物位置的機率分布，黃線為未來$k$時段障礙物速度的機率分布。依照前面的線性模型$x_{k+1} = x_k + v_k * \delta t$，代表障礙物$k+1$時段的位置依然是常態分佈，且是一個更胖的鐘型曲線(綠線)。
可以預想，將預測的障礙物依其未來的位置加上一個標準差的範圍，繪製於圖上，大概會呈現越來越大的趨勢，如圖:
### 測試
* 分別有藍、紅、綠三顆行人節點
* 較大的透明圓形為預測未來0.5秒內節點之位置
讓三位行人彼此交會，中間還有一個不會互相禮讓的機器人通過。可以看到藍色的節點雖然有嘗試閃掉機器人，但還是被撞到了。這至少可以證明，如果日後讓配有MPC的GBM無碰撞通過此情境，並不完全是SFM的功勞。
{%youtube asJ2sPhaGVM%}
可以看到，雖然使用線性模型加上Kalman Filter並沒辦法預測節點甚麼時候去改變行為，但是每當節點採取不同行動後，此方法都能馬上去修正節點的未來預測位置。
:::spoiler 行人節點"B1"的x-t，v-t，a-t圖(包含實際值與估計值+正負1個標準差)
:::

## 研究方向

目前的想法，是將我的研究分為三項領域:
1. **行人環境**: 採用SFM，創建各種工況給機器人，看要是一人一機、多人一機，或多人多機
2. **動態障礙預測**: 使用Kalman Filter做為短期預測的工具，提供障礙物未來的可能位置，讓MPC依此建立constraints
3. **MPC**: 發出motion command給GBM。原本使用VO法，不過目前正在嘗試柯琮祐學長一樣的風險場，說不定此方法更適合GBM。

## 實驗室設備
- [ ] 買GBM用電池
- [x] 修理兩台3D列印機
## Reference
[^1]: Korbmacher, Raphael, and Antoine Tordeux. "Review of pedestrian trajectory prediction methods: Comparing deep learning and knowledge-based approaches." IEEE Transactions on Intelligent Transportation Systems 23.12 (2022): 24126-24144.
[^2]: Ammoun, Samer, and Fawzi Nashashibi. "Real time trajectory prediction for collision risk estimation between vehicles." 2009 IEEE 5Th international conference on intelligent computer communication and processing. IEEE, 2009.