# 進度報告(2024/03/25)
## General Bicycle Model

### Degree of Freedom
$$
\delta_M=\delta_m+\delta_s
$$

* $\delta_M$ : Degree of maneuverability
* $\delta_m$ : mobility
* $\delta_s$ : steerability

GBM case: $\delta_m=2\ ,\ \delta_s=2\ \rightarrow\ \delta_M=4$ (impossible!!)
add constraint: $v_{fx}=v_{rx}=v_x \ \rightarrow\ \delta_M=3$

### Instantaneous center of rotation $ICR$
base on robot coodinate:
* front wheel steering: $\theta_f$
* front wheel velocity: $v_f$
* rear wheel steering: $\theta_r$
* rear wheel velocity: $v_r$

$ICR$ is the intersection $[x,y]$ of the two perpendicular line of the steering.
* slop of front wheel perpendicular line: $m_f$
* slop of rear wheel perpendicular line: $m_r$
$$
\begin{cases}
m_f=tan(\dfrac{\pi}{2}+\theta_f)=-cot(\theta_f)\\
m_r=tan(\dfrac{\pi}{2}+\theta_r)=-cot(\theta_r)
\end{cases}\ ,\
\begin{cases}
y=m_f(x-\dfrac{L}{2})=m_fx-\dfrac{m_fL}{2}\\
y=m_r(x+\dfrac{L}{2})=m_rx+\dfrac{m_rL}{2}
\end{cases}
$$
$$
\Rightarrow \begin{bmatrix}m_f & -1\\ m_r & -1\end{bmatrix}
\begin{bmatrix}x\\y\end{bmatrix} = \begin{bmatrix}\dfrac{m_fL}{2} \\ \dfrac{-m_rL}{2}\end{bmatrix}
\Rightarrow \begin{bmatrix}x\\y\end{bmatrix}
=\begin{bmatrix}\dfrac{-L(m_f+m_r)}{2(m_r-m_f)}\\\dfrac{-m_fm_rL}{m_r-m_f}\end{bmatrix}
$$

### rotation $\omega$
$$
\dfrac{R_r}{sin(\frac{\pi}{2}-\theta_f)}=\dfrac{R_f}{sin(\frac{\pi}{2}+\theta_r)}=\dfrac{L}{sin(\theta_f-\theta_r)}
\Rightarrow \begin{cases}R_r=\dfrac{Lcos\theta_f}{sin(\theta_f-\theta_r)}\\
R_f=\dfrac{Lcos\theta_r}{sin(\theta_f-\theta_r)}\end{cases}
$$
$$
\omega=\dfrac{v_f}{R_f}=\dfrac{v_r}{R_r}=\dfrac{v_fsin(\theta_f-\theta_r)}{Lcos(\theta_r)}=\dfrac{v_rsin(\theta_f-\theta_r)}{Lcos(\theta_f)}
$$
* if $ICR$ is in 1st or 4st quadrant $\begin{bmatrix}v_x\\v_y\end{bmatrix}=\omega\begin{bmatrix}y\\-x\end{bmatrix}$
* if $ICR$ is in 2st or 3st quadrant $\begin{bmatrix}v_x\\v_y\end{bmatrix}=\omega\begin{bmatrix}y\\x\end{bmatrix}$

### kinematic

#### Inverse kinematic (given goal -> find actuator): $\mathbf{w}=H\mathbf{v}$
$$
\begin{bmatrix}
v_{fx}\\v_{fy}\\v_{rx}\\v_{ry}
\end{bmatrix}=
\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}
$$
#### Forward kinematic (given actuator -> find goal): $\mathbf{v}=[H^TH]^{-1}H^T\mathbf{w}$
$$[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}v_x\\v_y\\\omega\end{bmatrix}=
\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{cases}
v_{fx}=v_f\cdot cos(\theta_f)\\
v_{fy}=v_f\cdot sin(\theta_f)\\
v_{rx}=v_r\cdot cos(\theta_r)\\
v_{ry}=v_r\cdot sin(\theta_r)
\end{cases}
\Rightarrow
\begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix}=
\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_f\cdot cos(\theta_f)\\v_f\cdot sin(\theta_f)\\
v_r\cdot cos(\theta_r)\\v_r\cdot sin(\theta_r)
\end{bmatrix}
$$

### robot in world coordinate
* velocity and rotation in robot coordinate: $\mathbf{^{R}v}=\begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix}$
* velocity and rotation in world coordinate: $\mathbf{\dot{x}}=\mathbf{^{I}v}=\begin{bmatrix}\dot{x}\\\dot{y}\\\dot{\psi}\end{bmatrix}=\begin{bmatrix}V_x\\V_y\\\omega\end{bmatrix}$
* robot to world rotation matrix: $^{I}_{R}T$
* robot coordinate orientation: $\psi$

$$
\begin{aligned}
\mathbf{\dot{x}}&=^{I}_{R}T\cdot \mathbf{^{R}v}\\
\begin{bmatrix}\dot{x}\\\dot{y}\\\ \dot{\psi}\end{bmatrix}&=
\begin{bmatrix}
cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0 \\ 0 & 0& 1
\end{bmatrix}
\begin{bmatrix}
v_x\\v_y\\\omega
\end{bmatrix}\\&=
\begin{bmatrix}
cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0 \\ 0 & 0& 1
\end{bmatrix}
\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_f\cdot cos(\theta_f)\\v_f\cdot sin(\theta_f)\\
v_r\cdot cos(\theta_r)\\v_r\cdot sin(\theta_r)
\end{bmatrix}
\end{aligned}
$$
### input & output
以上的式子加入$v_fcos\theta_f=v_rcos\theta_r$的限制，最終的代換會變成:
$$
\begin{cases}
\dot{x}=v_fcos\theta_fcos\psi-\dfrac{v_f}{2}(sin\theta_f+cos\theta_ftan\theta_r)sin\psi\\
\dot{y}=v_fcos\theta_fsin\psi+\dfrac{v_f}{2}(sin\theta_f+cos\theta_ftan\theta_r)cos\psi\\
\dot{\psi}=\dfrac{v_fsin(\theta_f-\theta_r)}{Lcos\theta_r}
\end{cases}
$$
因為有限制式，所以輸入其實只有$(v_f,\theta_f,\theta_r)$，其中$v_f$或$v_r$可以互相取代。

### simulation 環境建置
目前暫時是使用matlab，雖然還不知道怎麼控制GBM，但是還是可以先在matlab環境下畫出一台基本的GBM robot(有body fram也有steering wheel)，然後假裝讓它動起來。

### GBM硬體
上星期三跟張宇杭一起把車架都組完了，下一步會3D列印一些零件然後試著裝上去。

## 未來方向
* 如果要討論控制的話，由於GBM它是非線性系統，我現在還不知道可以怎麼解，尚未清楚Jacobian帶入值線性化是否適用，這部分我還需再查資料摸索
* 如果不討論控制的話，那我的下一步便是建立一個最基本的VO演算法，然後直接將每一時間點VO得出的速度命令丟入inverse kinematic中，然後觀察GBM的移動模式。但VO演算法並不會給出$\omega$的命令，所以其實可以預期GBM會一直用crab模式行走。