LQRで経路追従

## 動作動画
https://www.youtube.com/watch?v=XXuZxzfc1EI

## 経路追従
### LQR
#### 状態方程式の導出

目標経路への追従が目的であるため、偏差系の状態方程式を構築する。今回の状態方程式に関するロボットモデルと目標経路をFig 1に示す。また変数をTable 1に示す。

**Table 1. LQRの変数**
|  パラメータ名  |  機能  |
| ---- | ---- |
|  $e_d$  | ロボット位置と目標経路の距離差  |
| $\dot{e_d}$ | $e_d$の時間微分した値（速度）  |
| $\theta$ | ロボット角度 |
|  $\theta_r$ | 目標経路の角度  |
| $e_{\theta}$ | ロボット角度と目標経路の角度差  |
|  $\dot{e_{\theta}}$ | $\theta$の時間微分した値（角速度） |
| $v_t$ | ロボットの並進速度 |
| $\omega_t$ | ロボットの角速度 |
| $d_t$ | 制御周期  |

<img width="584" alt="model.png (13.2 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/584e61b7-e696-46e4-866c-c68728a8419f.png">

**Fig 1. モデル図**

<img width="584" alt="model_sincos.png (24.2 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/573746b7-72a2-4577-901d-d1a74a3e1709.png">

**Fig 2. $e_d$の導出**


状態変数は
```math
    X_t=\left[
    \begin{matrix} e_d \\ \dot{e_d} \\ e_{\theta} \\ \dot{e_{\theta}} \end{matrix}
    \right]
```

であり、制御量$u_t$はロボットへの角速度$\omega_t$とする。

```math
    u_t = \omega_t
```

状態変数$e_{\theta}$は目標角度$ \theta_r$と$\theta$の差であり、

```math
    e_{\theta} = \theta_r - \theta
```

$e_{\theta}$を時間微分した$\dot{e_{\theta}}$は角速度であるため、制御量$u_t$と$\dot{e_{\theta}}$は等しい。

```math
    \dot{e_{\theta}} = u_t = \omega_t
```

状態変数$e_d$（Fig 2）はロボット位置と目標経路の距離差であるため、$e_d$を時間微分した$\dot{e_d}$は

```math
    \dot{e_d} = v_t\cos{e_{\theta}}\sin{e_{\theta}}
```

$e_{\theta}$が小さいという前提を利用して線形化すると

```math
    \cos{e_{\theta}} = 1
```

```math
    \sin{e_{\theta}} = e_{\theta}
```

```math
    \dot{e_d} = v_t e_{\theta}
```

ここまでをまとめると、状態方程式は以下のとおりになる。

```math
\begin{align}
    X_{t+1} \nonumber\\
    &= AX_t + Bu_t \nonumber\\
    &= \left[
        \begin{matrix} 1 & dt & 0 & 0\\ 0 & 0 & v_t & 0\\ 0 & 0 & 1 & dt\\ 0 & 0 & 0 & 0 \end{matrix}
        \right]
        \left[
        \begin{matrix} e_d \\ \dot{e_d} \\ e_{\theta} \\ \dot{e_{\theta}} \end{matrix}
        \right]
        +
        \left[
        \begin{matrix} 0 \\ 0 \\ 0 \\ 1 \end{matrix}
        \right]\omega_t
\end{align}
```

#### フィードバックゲインの導出

フィードバックゲイン$K$は式(1)の$Z$が変化しなくなるまで更新し続けた$Z$を、式(2) に利用することで求まる。

$$
Z = A^TZA-A^TZB(R+B^TZB)^{-1}B^TZA+Q \tag{1}
$$

$Q$は状態に関係する重み行列。$Q$の各行は$e_d$, $\dot{e_d}$,$ e_{\theta} $, $\dot{e_{\theta}}$に対応しているから、特に素早く収束させたい状態のパラメータを大きくする。$R$は制御量に関係する重み行列。今回は制御量が$\omega_t$のみであるため、$R$は行列ではなくスカラとなる。 

```math
    Q=\left[
    \begin{matrix} q_1 & 0 & 0 & 0\\ 0 & q_2 & 0 & 0\\ 0 & 0 & q_3 & 0\\ 0 & 0 & 0 & q_4 \end{matrix}
    \right]
```

```math
    K=(B^TZB+R)^{-1}(B^TZA)\tag{2}
```

```math
    u_t = \omega_t = -K X_t \tag{3}
```

式(3)で求めた制御量$\omega_t$はロボットに入力できないため、モーター入力に変換する必要がある。Fig 3は今回使用するロボットであるRoombaの角速度とモータ入力の関係図。近似直線から求めた、傾き$0.0179$と切片$-0.2826$からモータ入力に変換する。

```math
    MotorInput = \frac{1}{0.0179} \omega_t - 0.2826
```

<img width="584" alt="motor-w.png (31.2 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/b0696a72-6d79-4827-ba2c-3866a587d68a.png">

**Fig 3. ロボット角速度[rad/s] - モータ入力**

## 実験

実験では、目標経路のスプライン曲線とオドメトリから測定した経路の誤差を測定する。目標経路は2種類のスプライン曲線を生成して、LQRによって追従させる。ロボットはRoombaを使用した。
動作動画：https://www.youtube.com/watch?v=XXuZxzfc1EI

### 結果

#### 経路1

1種類目の目標経路と追従結果をFig 4とFig 5に示す。追従誤差は最大で約0.025[m]となった。

<img width="460.8" alt="spline-path-1.png (24.6 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/ea0d84bc-a24e-429a-9c6b-32cf69c9f2d5.png">

**Fig 4. 目標経路1**

<img width="460.8" alt="path2-4.png (54.1 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/4e311c5e-1858-4541-8db9-c13f19d101c5.png">

**Fig 5. 目標経路1の追従誤差**

#### 経路2

2種類目の目標経路と追従結果をFig 6とFig 7に示す。追従誤差は最大で約0.025[m]となった。

<img width="460.8" alt="spline-path-2.png (26.1 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/545adb27-07ca-4baf-9a44-233083754a3c.png">

**Fig 6. 目標経路2**

<img width="460.8" alt="path1-4.png (49.1 kB)" src="https://img.esa.io/uploads/production/attachments/18204/2023/01/25/111414/d7434107-2131-470f-b23a-c1f1bb6371ad.png">

**Fig 7. 目標経路2の追従誤差**

## 参考
- https://myenigma.hatenablog.com/entry/2017/08/08/153651
- https://myenigma.hatenablog.com/entry/2016/11/05/033219
- https://qiita.com/taka_horibe/items/47f86e02e2db83b0c570#%E8%BB%8A%E4%B8%A1%E3%81%AE%E8%BB%8C%E9%81%93%E8%BF%BD%E5%BE%93%E5%95%8F%E9%A1%8C%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%81%AB%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B
- Tao Yang, Ziwen Bai, Zhiqiang Li, Nenglian Feng and Liqing Chen, "Intelligent Vehicle Lateral Control Method Based on
Feedforward + Predictive LQR Algorithm"