## 前言
我想要用導納控制來讓我的機械手臂來夾持跳動的心臟血管且順應著上下移動。但是在調整導納控制M、B、K參數前我應該要先確保我機器人系統的追蹤誤差越小越好(尤其是相位)，才能確保之後產生的誤差不是底層的控制器造成的，對之後也比較好鑑別出誤差來源。

所以我想要檢視機器人追蹤弦波的能力，然後重新調整PID控制器的參數。此外，人體的心臟頻率也不是單一頻率構成的，我剛好可以看一下在除了1Hz附近的地方機器人的響應如何。

## 事前準備 - 舊手臂上重新調整PID控制器參數
先選定頻率**1Hz 2mm**弦波作為調整的目標頻率，目標是盡可能縮小相位延遲。
主要調整Kp、Kd的數值
### 舊的響應(@1Hz and 2Hz)
> Tracking results for 2mm 1Hz sinusoidal wave:
> $$\begin{aligned}
|G| &= -2.03\ \text{dB} \\
\angle G &= -24.6^\circ \\
\tau &= 68.27\ \text{ms}
\end{aligned}$$

> Tracking results for 2mm 2Hz sinusoidal wave:
> $$\begin{aligned}
|G| &= -1.38\ \text{dB} \\
\angle G &= -40.7^\circ \\
\tau &= 56.56\ \text{ms}
\end{aligned}$$

### 新的響應(@1Hz and 2Hz)
> Tracking results for 2mm 2Hz sinusoidal wave:
> $$\begin{aligned}
|G| &= -0.41\ \text{dB} \\
\angle G &= -9.7^\circ \\
\tau &= 27.02\ \text{ms}
\end{aligned}$$
> Tracking results for 2mm 2Hz sinusoidal wave:
> $$\begin{aligned}
|G| &= -0.18\ \text{dB} \\
\angle G &= -13.6^\circ \\
\tau &= 18.88\ \text{ms}
\end{aligned}$$

調整好的PID參數如下
| $\text{Joint}$ | $K_p$ | $K_d$ | $K_i$ |
|:--------------------:|:--------:|:------:|:-------:|
| 2 | 0.00052 | 0.0008 | 0.00010 |
| 3 | 0.000205 | 0.0002 | 0.00010 |
| 4 | 0.00150 | 0.0004 | 0.00020 |
| 5 | 0.00060 | 0.0002 | 0.00040 |
| 6 | 0.00150 | 0.0002 | 0.00000 |
| $\text{Tool Driver}$ | 0.00075 | 0.0006 | 0.00012 |

## Sine Sweep實驗結果
:::spoiler 系統識別弦波響應詳細數據如下所示

| $f_0$ [Hz] | Gain [dB] | Phase [deg] | $\tau$ [s] |
|:----------:|:---------:|:-----------:|:----------:|
| 0.1 | -0.5613 | -8.78 | 0.2436 |
| 0.2 | -0.5599 | -8.64 | 0.1201 |
| 0.5 | -0.5187 | -8.67 | 0.0482 |
| 0.8 | -0.4939 | -9.25 | 0.0321 |
| 1.0 | -0.4098 | -9.72 | 0.0258 |
| 1.2 | -0.3946 | -10.09 | 0.0233 |
| 1.4 | -0.3248 | -10.95 | 0.0216 |
| 1.5 | -0.3503 | -11.14 | 0.0212 |
| 1.6 | -0.2728 | -11.63 | 0.0206 |
| 1.8 | -0.2229 | -12.41 | 0.0197 |
| 2.0 | -0.1592 | -13.43 | 0.0184 |
| 2.2 | -0.1975 | -14.34 | 0.0181 |
| 2.4 | -0.0703 | -15.69 | 0.0182 |
| 2.5 | 0.0000 | -17.14 | 0.0177 |
| 2.6 | -0.0997 | -17.84 | 0.0170 |
| 2.8 | 0.0142 | -21.61 | 0.0211 |
| 3.0 | -0.2251 | -25.64 | 0.0333 |
| 4.0 | -2.1171 | -45.62 | 0.0344 |
| 5.0 | -3.9062 | -72.28 | 0.0409 |
| 10.0 | -8.3568 | -136.81 | 0.0380 |

> 備註: Ts = 0.001s

:::

根據數據繪製出的Bode Plot如下:

## 系統Fitting結果
設定**4個Pole；3個zero**，fit結果如下

得到的Plant轉移函數為:$$G(z) = \frac{0.03222z^{-1} - 0.06097z^{-2} + 0.02885z^{-3}}{1 - 1.899z^{-1} - 0.09759z^{-2} + 1.899z^{-3} - 0.9023z^{-4}}$$

對應的Step Response圖形如下

## Some Experiments Setup
- 為了讓系統更接近線性化且貼近實際應用，在做system id的時候我把操作點都設在實際手術的位置附近
- 因為螺桿上下運動方向與重力方向相同，重力會產生非線性的影響，如果要更精確的得出線性模型，可以考慮直接在plant開路上加上一個偏移量來抵銷重力和摩擦力(其實就是在做feedback linearization)。