# 進度報告(2024/06/27)
[TOC]

## 短期規劃與目前現況

digraph tree{
node1[label="build GBM"]
node2[label="test(give velocity command)"]
node3[label="GBM + VO algorithm",color=red]
node1 -> node2
node2 -> node1
node1 -> node3
}

現在在做的事，是測試之前寫的GBM模型(https://hackmd.io/4yCBYUmgSr-aq6e2MKuaFw)，修正各種bug。
:::warning
我考慮機器人的基本運動限制有:
* 前輪的最大速度 $v_f\leq v_{max}$
* 後輪的最大速度 $v_r\leq v_{max}$
* 在單位時間$\Delta t$內的前輪速變量(加速度) $\Delta v_f\leq \Delta v_{max}$
* 在單位時間$\Delta t$內的後輪速變量(加速度) $\Delta v_r\leq \Delta v_{max}$
* 在單位時間$\Delta t$內的前輪角度變量(角加速度) $\Delta \theta_f\leq \Delta \theta_{max}$
* 在單位時間$\Delta t$內的後輪角度變量(角加速度) $\Delta \theta_r\leq \Delta \theta_{max}$
:::

畢竟VO也是丟速度命令給機器人的演算法，我希望將VO與GBM整合前，要先確保我寫的模型是OK的。如此一來，一旦整合後發現有問題，就可以大致確定是VO演算法的不完善而非我模型的BUG。

測試的方式是給出幾個waypoints，讓模型去依序經過這幾點。目的是要讓至台模擬的機器人去執行各種速度向量$[v_x, v_y, \omega]^T$的命令，並查看動畫上或是GBM的輪速、轉向角數據有沒有甚麼不合理的狀況發生。怎麼發出速度命令給機器人?我的方式是直接指定GBM當前的位置到下一個waypoint的直線最大速度為$[v_x,v_y]$。而轉向的部分則是使用proportion control給出$\omega$，讓機器人的朝向能夠越來越趨近平行於當前速度。

以下是讓我的GBM模型執行個原始RRT所給出的路線。實際上我並沒有讓機器人照著RRT規畫路徑行走，而是讓RRT的每個節點都成為一個waypoints，再用上述的方法讓模型執行速度命令以依序通過各點。

{%youtube ozotPd4DlkY%}
RRT在膨脹地圖中生成的路徑
:::spoiler GBM的數據
:::

### 處理中的模型問題s
* 機器人的輪子不會倒退 :cry: (major)
* 速度取曲線不夠平滑，可能還需要更嚴格的運動限制 (minor)
* 程式太醜了 (minor)

基於以上的問題，目前正在著手修改GBM程式。

## 研究規劃
* 結合General Bicycle Modle 與 Velocity Obstacle algorithm (original one)
* 讓配有VO的GBM模型去應付各種衝反不確定性、隨機的動態環境
* 再想想看可以怎麼加入預測障礙物，以增強避障演算法的表現

## GBM的運動
在我的模型中，因為有給機器人的輪子加入運動限制，所以並非所有速度命令GBM都可以執行。所以每一次機器人收到速度命令，都會經過以下流程:
1. (從VO演算法)得到一速度命令
2. 由逆向運動學轉為控制輸入的命令
3. 檢查該控制輸入命令是否符合運動限制
4. 若不符合則會執行最佳化得出一組與原速度命令最接近的控制輸入

### 最基本的 inverse kinematic
從逆向運動學，可以推斷出給GBM一個速度命令$[v_x,v_y,\omega]^T$後，可以得到一組控制輸入的命令$[v_{f_x},v_{f_y},v_{r_x},v_{r_y}]^T$。
$$
\begin{bmatrix}
1&0&0\\0&1&L/2\\1&0&0\\0&1&-L/2
\end{bmatrix}\begin{bmatrix}
v_x\\v_y\\\omega
\end{bmatrix}=\begin{bmatrix}
v_{fx}\\v_{fy}\\v_{rx}\\v_{ry}
\end{bmatrix}
$$
從方程式和上圖可以得知，在控制輸入完全不含left null space 的情況下，GBM的速度向量才能有唯一解。

### 從pseudo inverse衍伸出來的 forward kinematic
然而，用廣以反矩陣求順向運動學時，會發現一個速度向量會對應到$R^1$無限個控制輸入的解。這是因為所有包含null space的控制輸入，都會被投影到一個$R^4$空間中的3維平面，以至於輸出的速度向量都是一樣的。
$$[H^TH]^{-1}H^T=\begin{bmatrix}1/2&0&1/2&0\\0&1/2&0&1/2\\0&1/L&0&-1/L\end{bmatrix}\
\Rightarrow\
\begin{bmatrix}
1/2&0&1/2&0\\0&1/2&0&1/2\\0&1/L&0&-1/L
\end{bmatrix}
\begin{bmatrix}
v_{fx}\\v_{fy}\\v_{rx}\\v_{ry}
\end{bmatrix}
=\begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix}
$$

雖然有無限多組控制輸入都可以執行相同的速度向量，但是在現實中，所有含有null space的控制輸入都會有輪胎力互相抵銷的情況。這不但會出現不可控的因子，導致機器人無法如預期行走。還會造成機器人震動，降低元件壽命。

所以，在我寫的模型中，一旦控制輸入的命令超出運動學限制，需要執行最佳化時，會加上一條equality constraint($v_{f_x}=v_{r_x}$)，用最佳化程式從一個四維空間中的三維平面找出一組最佳解。

## 硬體現況
前陣子有與京睿討論過，如果swerve drive可以自己做的話，就可以省下一筆經費。目前正在設計出版的prototype，打算用3D列印機復刻一個出來。加上有交換生的協助，希望很快就可以知道成效。