> For the complete documentation index, see [llms.txt](https://sejkai.gitbook.io/academic/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://sejkai.gitbook.io/academic/ncku-robotic-navigation-and-exploration/kinetic_model_vehicle_control.md).

# Kinetic Model & Vehicle Control

## Control Theory

* Control system
  * 能夠影響整個系統未來 state 的機制
* Control theory
  * 因應 output 來改變 input

### Open Loop Control

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fj4MolzFJmFXlDU%2Fopen_loop_control.png?generation=1584509932662531\&alt=media)

例如汽車油門要根據汽車的速度變化 (e.g. 上下坡) 而改變

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fj77b6_pLUgks2-%2Fopen_loop_control_example.png?generation=1584509931221751\&alt=media)

### Close Loop Control

只有 input-output 很難去做修正，所以需要透過前一次 output 來修正

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fj9dGDUuM2H-21t%2Fclose_loop_control.png?generation=1584509930330841\&alt=media)

* Sensor
  * 量測上一次的 output 結果，用來和 reference 比對
* Reference
  * 用來比對和 output 的結果，計算出 error
* Controller
  * 根據 error 改變 input

例如根據汽車當前的速度，來調整要加速或減速

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjDXuOIb0sKi4TN%2Fclose_loop_control_example.png?generation=1584509931417673\&alt=media)

### Linear Time Invariant System

我們可以將 time domain 轉置成 frequency domain 重整一下 loop control

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjFnvmTnzPKxrfd%2Flinear_time_loop_control.png?generation=1584509930817181\&alt=media)

所有的運算和參數都以頻率來表達

* G = Input 運算結果
* H = Sensor 運算結果
* D = Controller 運算結果
* r = reference
* e = error
* y = output

我們可以得到 $$e = r - yH$$ (誤差等於 reference 減去 sensor 所算的結果)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjJYLTnJNA5y4Zj%2Flinear_time_loop_control_combine.png?generation=1584509931855357\&alt=media)

頻率中 D 到 G 的運算可以視為相乘，我們可以得到 y 等於 e 經過 D 和 G 兩個運算

$$
y = e \cdot D \cdot G
$$

接著經過一連串的運算

$$
\begin{aligned}
e &= \frac{y}{DG} \\
r - yH &= \frac{y}{DG}\\
(DG)(r-yH) &= y\\
DGr -DGyH &= y\\
DGr &= y + DGyh = y(1+DGH)\\
y &= \frac{DGr}{1+DGH}
\end{aligned}
$$

於是我們可以將 output (y) 看成是 reference (r) 經過 $$\frac{DG}{1+DGH}$$ 運算而來

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjLsep7KMfxYRiM%2Flinear_time_loop_control_open.png?generation=1584509930322480\&alt=media)

變成一個 open loop control

## Control Methods

### PID Control

* PID 分別為三個詞的縮寫
  * Proportional gain
  * Integral gain
  * Differential gain

#### Proportional gain

Controller 設定一個值來與前一個 error 相乘，得到下一次修改的 input，這個值就叫做 proportional gain

在下圖，老皮根據終點距離來得到 error 並和 0.1 (proportional gain) 相乘

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjOLBNMCswUrI6U%2Fproportional_gain.png?generation=1584509933205022\&alt=media)

* 當距離 100 時，走 10 m/s (100\*0.1)
* 當距離 90 時，走 9 m/s (90\*0.1)
* 當距離 0 時，就停止了 (0\*0.1)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjQRv22L9bYnl8u%2Fproportional_gain_result.png?generation=1584509931718709\&alt=media)

**Problem**

老皮若想往天空飛，那只用 proportional gain 勢必是無法完美達成的，因為最終會停止掉下來，又往上升上去

假設老皮的螺旋槳轉速 200 rpm 可以對抗重力維持在空中

$$
\begin{aligned}
\text{Error} \times \text{Gain} &= \text{Propeller speed}\\
100 \times 2 &= 200 \text{rpm}\\
40 \times 5 &= 200 \text{rpm}\\
20 \times 10 &= 200 \text{rpm}\\
2 \times 100 &= 200 \text{rpm}\\
\end{aligned}
$$

不管設計多少的 gain (2, 5, 10, 100, ...) 都無法讓老皮到達並停留在天空上的終點

這個情況稱為 **steady state error** (y 會隨時間接近 r，但永遠存在 error)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjT7b5aAF1bNQF_%2Fsteady_state_error.png?generation=1584509932802010\&alt=media)

#### Integral gain

我們可以加入一個 integrator 來解決 steady state error 造成的問題

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjVeM9IVpXkWswY%2Fintegral_gain.png?generation=1584509933315126\&alt=media)

新增的 integrator 會累積 error 的資訊來補充 proportional gain 不足的量

例如 error 變成 0 時，integrator 就提供 200 rpm 來讓老皮維持在高空

**Problem**

Integrator 若沒有良好設計，會超過 200 rpm 讓老皮繼續往上飛

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjXCN-aRC1KrTyp%2Fintegral_gain_problem.png?generation=1584509930557134\&alt=media)

而超過 reference 又產生了 negative error，讓 proportional gain 變負，老皮往下降

#### Differential gain

若能預測 error 變化量，就能預防 integral gain 忽高忽低的問題

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fja1xOn3cK5kaf2%2Fdifferential_gain.png?generation=1584509932455968\&alt=media)

我們在 controller 加上第三個 derivative 元件

因為 error 是往下變小的，變化量就是 error 的斜率 (紅線)

因為斜率是負的，所以 derivative 也產生一個負值，來和 integrator 抗衡

#### Summary

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjcggznhKbtH7mC%2Fpid_control.png?generation=1584509932036123\&alt=media)

PID control 可以寫成 discrete form

$$
K\_pe(t) + K\_i\sum\_0^te\_t + K\_d(e(t)-e(t-1))
$$

其中的 $$K\_p, K\_i, K\_d$$ 分別就代表了 proportional, integral, differential gain 的參數

通常就是調整這三個參數，來完成一個好的 controller

## Basic Kinematic Model

若我們想讓車子移動到路徑上到達終點，可以用前後和左右兩種 control 方式

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fjef_6a_i8JMNj8%2Fcar_pid_control.png?generation=1584509932511417\&alt=media)

若只使用 PID control 來完成，需要調整太多的參數

所以我們必須要引入一些車輛的特性，來減輕 control system 的負擔

* 在低速時
  * 可以用簡單的幾何模型來描述 car state
* 在高速時
  * 因會產生側向滑動，所以需要套入更難的動力學模型

我們用 x, y 代表車子的二維座標，$$\theta$$ 代表車子的轉向，合起來為車子的狀態

$$
\text{state: } \xi\_1 = \begin{bmatrix}x\y\\\theta\end{bmatrix}
$$

因為座標有分車子當前的座標，還有世界座標，我們有一個 rotation matrix 可以轉換兩個座標系統

$$
R(\theta) = \begin{bmatrix}
\cos\theta & \sin\theta & 0 \\
-\sin\theta & \cos\theta & 0 \\
0& 0& 1
\end{bmatrix}
$$

基本的模型 (basic kinematic model) 指的就是狀態 (state) 的變化 (derivative)

> 我們會在符號上加上一點代表微分後的變化

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2g8e-1de4_lTMmcvXC%2F-M2g8fji8d4X8YxXojo0%2Fbasic_kinematic_model.png?generation=1584509931695473\&alt=media)

State 變化可以從車子的當前狀態，乘上 rotation matrix 的反矩陣得到

* $$\dot{x\_R}$$ 是 $$x\_R$$ 的變化，也就是前進速度 ($$v$$)
* 而側向是沒有速度的，所以 $$\dot{y\_R} = 0$$
* $$\dot{\theta}$$ 是 $$\theta$$ 的變化，也就是角速度 ($$\omega$$)

$$
\begin{aligned}
\text{Kinematic Model:} \\
\begin{bmatrix}\dot{x} \ \dot{y} \ \dot{\theta}\end{bmatrix} &=
R(\theta)^{-1}\begin{bmatrix}\dot{x\_R}\\\dot{y\_R}\\\dot{\theta}\end{bmatrix} \\
&= \begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0& 0& 1\end{bmatrix}
\begin{bmatrix}v\0\\\omega\end{bmatrix} \\
&= \begin{bmatrix}v\cos(\theta) \ v\sin(\theta) \\\omega\end{bmatrix}
\end{aligned}
$$

### Differential Drive Vehicle

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2kpmiIe-wuW6AytNXT%2F-M2kq0YM7GiKZs9M44Z9%2Fdifferential_drive_vehicle.png?generation=1584588660850201\&alt=media)

現在來考慮兩輪的自走車活動模型

* P: 原點
* l: 原點分別到兩輪的距離
* r: 輪子的半徑
* 座標依然是 $$x\_R, y\_R$$

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2kpmiIe-wuW6AytNXT%2F-M2kq0YQuHPlr5-lAFXo%2Fdifferential_drive_vehicle2.png?generation=1584588660607313\&alt=media)

兩輪的轉速分別是 $$\phi\_1$$ 和 $$\phi\_2$$

* 右輪 (左輪) 速度 = 半徑 \* 角速度
  * $$\text{right: } r \times \dot{\phi\_1}$$
  * $$\text{left: } r \times \dot{\phi\_2}$$
* 原點的速度就是右輪 (左輪) 速度的一半
  * $$\dot{x\_{R1}} = \frac{r\dot{\phi\_1}}{2}$$
  * $$\dot{x\_{R2}} = \frac{r\dot{\phi\_2}}{2}$$
* 原點的角速度 = 速度 / 到輪子的距離 
  * $$\omega\_1 = \frac{r\dot{\phi\_1}}{2l}$$
  * $$\omega\_2 = \frac{-r\dot{\phi\_2}}{2l}$$
  * 要注意左輪旋轉半徑是 $$-l$$

而原點的運動就是左右兩輪相加

$$
\begin{aligned}
\text{Kinematic Model:} \\
\begin{bmatrix}\dot{x} \ \dot{y} \ \dot{\theta}\end{bmatrix} &=
R(\theta)^{-1}\begin{bmatrix}\dot{x\_R}\\\dot{y\_R}\\\dot{\theta}\end{bmatrix} \\
&= \begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0& 0& 1\end{bmatrix}
\begin{bmatrix}
\frac{r\dot{\phi\_1}}{2}+\frac{r\dot{\phi\_2}}{2}\\
0\\
\frac{r\dot{\phi\_1}}{2l}-\frac{r\dot{\phi\_2}}{2l}
\end{bmatrix} \\
\end{aligned}
$$

左右輪的馬達轉速通常不會設成參數，而是用 $$v, \omega$$ 來推導

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2kpmiIe-wuW6AytNXT%2F-M2kq0YUw-G-cy-0NHpB%2Frpm_inference.png?generation=1584588664308266\&alt=media)

* 直線前進，相同轉速，相同方向 ($$\frac{v}{r}$$) 
* 原地旋轉，相同轉速，相反方向 (正負 $$\omega$$)

### Pure Pursuit Control

Pure pursuit control 將根據速度與角速度來畫圓弧移動到前方某個點的位置

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2kpmiIe-wuW6AytNXT%2F-M2kq0YXV_ceUeRwDpGG%2Fpure_pursuit_control.png?generation=1584588661151355\&alt=media)

* $$L\_d$$ 是車子與目標點的距離
* $$R$$ 是畫圓產生的半徑
* $$\alpha$$ 是車子直線方向和 $$L\_d$$ 的夾角
  * $$\alpha = \arctan\left(\frac{y-y\_g}{x-x\_g}\right) - \theta$$
  * 因為圓心角 = 兩倍的弦切角
  * 所以圓心角就是 $$2 alpha$$
* 因為是等腰三角形
  * 所以其他兩個角是 $$\frac{\pi}{2} - \alpha$$

根據正弦定理可以得到

$$
\begin{aligned}
\frac{L\_d}{\sin(2\alpha)} &= \frac{R}{\sin(\frac{\pi}{2}-\alpha)} \\\\
R &= \frac{L\_d\sin(\frac{\pi}{2}-\alpha)}{\sin(2\alpha)}
&= \frac{L\_d\cos(\alpha)}{2\sin(\alpha)\cos(\alpha)}
&= \frac{L\_d}{2\sin(\alpha)} \\\\
\omega &= \frac{v}{R} = \frac{2v\sin(\alpha)}{L\_d}
\end{aligned}
$$

也就是說，若速度 v 透過 PID 為已知的值，那就可以推出對應的角速度 ($$\omega$$)

> Ld 通常用速度來決定，速度越快就越遠
>
> * e.g., $$L\_d = kv + L\_{fc}$$
> * 其中的 $$k, L\_{fc}$$ 是可調參數

## Kinematic Bicycle Model

上面講的模型可以自由移動旋轉，但真正的車子是有一定的幾何限制 (**nonholonomic constraints**)

而生活中最常見的移動機構設計是 bicycle model (汽車可以把前後的兩個輪子各別簡化為一個)

* 前輪控制方向 (方向盤)
* 後輪控制速度 (引擎)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2kpmiIe-wuW6AytNXT%2F-M2kq0YdYWphT-GcHEVi%2Fkinematic_bicycle_model.png?generation=1584588661150068\&alt=media)

* 後輪為車輛原點 (x, y)
* 車輛轉向 (車軸方向) $$\theta$$
* 方向盤轉角 $$\delta$$
* 車軸長度 $$L$$

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2nBwy2iec4TSBQlhC-%2F-M2nBzZEhtwJngdIeROZ%2Fkinematic_bicycle_model2.png?generation=1584628238706014\&alt=media)

將前輪放大可以得到一些細節

* 車子以 v 的速度向前
* 有兩個世界座標的軸分量 (weight) 為 $$\dot{x\_f}$$ 和 $$\dot{y\_f}$$

計算兩個 weight 對車子垂直方向的 weight

* $$\dot{x\_f}\sin(\theta+\delta)$$
* $$\dot{y\_f}\cos(\theta+\delta)$$

考慮在低速下，兩個 weight 相加會抵消，就可以得到前後輪的 equation (**Nonholonomic constraint equations**)

$$
\begin{aligned}
(1) && \dot{x\_f}\sin(\theta+\delta) - \dot{y\_f}\cos(\theta+\delta) = 0 && \text{(front wheel)}\\
(2) && \dot{x}\sin(\theta) - \dot{y}\cos(\theta) = 0 && \text{(rear wheel)}
\end{aligned}
$$

我們的目標是算出車輛**原點的運動**，可以從後輪座標推得前輪座標 (**Front wheel position**)

$$
\begin{aligned}
x\_f = x + L\cos(\theta) \\
y\_f = y + L\sin(\theta)
\end{aligned}
$$

將前輪座標帶回 (1) 就可以得到**基於車輛原點的限制方程式**

$$
\begin{aligned}
(3) && \dot{x}\sin(\theta+\delta) - \dot{y}\cos(\theta+\delta) - \dot{\theta}L\cos(\delta) = 0
\end{aligned}
$$

由 (2) 和 (3) 可以得到一組解，代表原點的變化

$$
\begin{aligned}
(4) && \dot{x} = v\cos(\theta)\\
(5) && \dot{y} = v\sin(\theta)
\end{aligned}
$$

將 (4) 和 (5) 再帶回 (3) 就可以得到角速度 ($$\dot{\theta}$$)

$$
\dot{\theta} = \frac{v\tan(\delta)}{L}
$$

**於是我們就可以得到完整的 kinematic bicycle model (基於方向盤轉角** $$\delta$$**)**

$$
\begin{bmatrix}
\dot{x}\\\dot{y}\\\dot{\theta}
\end{bmatrix} =
\begin{bmatrix}
\cos(\theta) \ \sin(\theta) \ \frac{\tan(\delta)}{L}
\end{bmatrix} v
$$

以及一些相關的 properties

$$
\begin{aligned}
&\bullet R\dot{\theta} = v \\
&\bullet \frac{v\tan(\delta)}{L} = \frac{v}{R} \\
&\bullet \tan(\delta) = \frac{L}{R}
\end{aligned}
$$

### Pure Pursuit Control for Bicycle Model

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2nBwy2iec4TSBQlhC-%2F-M2nBzZMJh75uBwHCP-n%2Fbicycle_model_pure_pursuit_control.png?generation=1584628238179937\&alt=media)

我們可以將 bicycle model 應用於 pure pursuit control

* $$\alpha$$ 和 $$R$$ 和原本的 pure pursuit control 一樣
* 我們可以用上面的 bicycle model properties 來求得方向盤轉角 ($$\delta$$)

$$
\begin{aligned}
&\tan(\delta) = \frac{L}{R} \\
&\delta = \arctan\left(\frac{L}{R}\right) = \arctan\left(\frac{2L\sin(\alpha)}{L\_d}\right)
\end{aligned}
$$

## Stanley Control

Pure pursuit control 雖然好用但不夠穩定，而 Stanley control 提供了漸進穩定的效果

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2nBwy2iec4TSBQlhC-%2F-M2nBzZPFiKHny9oDhj-%2Fstanley_control.png?generation=1584628237791318\&alt=media)

在 stanley control 會根據當前最近目標點，找到**切線、法線**做為新的座標系

* $$v$$: 前輪方向
* $$\delta$$: 方向盤方向
* $$\theta\_e$$: 路徑上的法線方向
* $$\delta - \theta\_e$$: 速度方向與路徑方向夾角

而法線狀態 (微分) 就是以下式子，可以當作追蹤的誤差

$$
\dot{e} = v\sin(\delta - \theta\_e)
$$

加入誤差對時間變化的假設，希望誤差隨時間變化漸進到 0

$$
\begin{aligned}
\dot{e} &= -ke, \text{ where } k > 0  \\
-ke &= v\sin(\delta - \theta\_e) \\
\delta &= \arcsin\left(-\frac{ke}{v}\right) + \theta\_e
\end{aligned}
$$

最終可以得到方向盤控制量 $$\delta$$ (其中 k 是調整漸進程度的參數)

因為當 $$\lvert -ke/vf \rvert > 1$$ 時為 undefined，所以可以改成近似的 local exponential stability (LES)

$$
\delta = \arctan\left(-\frac{ke}{v}\right) + \theta\_e
$$

改成 arctan 可避免 undefined 但在角度很大時，可能會造成誤差變大

## LQR Control

因為太難的運動模型無法直接分析 error function，所以 LQR control 運用 cost function 概念

* 運動模型是 linear form
* Cost function 是 quadratic form

$$
\text{cost function } c = \underbrace{x^TQx}*{\text{state error}} + \underbrace{u^TRu}*{\text{minimum control}}
$$

其中 **Q, R 矩陣**分別代表 state, control 在不同維度的重要性

而最終就是要將以下的 total objective function 最小化

$$
\text{minimize } J = \int\_0^T \left\[x(t)^TQx(t) + u(t)^TRu(t)\right]dt + x^T(T) Sx(T)
$$

若以下狀態從現在到終點 (terminal state) $$\left\[ u\_t^\ast ,u\_{t+1}^\ast , u\_{t+2}^\ast , \cdots , u\_T^\ast \right]$$ 是最佳解

那麼 $$\left\[ u\_{t+1}^\ast , u\_{t+2}^\ast , \cdots , u\_T^\ast \right]$$ 也會是最佳解

所以我們可以應用 **dynamic programming** 從最佳解的最終狀態，遞迴解回現在狀態

### Value function

通常我們並不知道 terminal state，或者是 terminal state 需要無限時間

這時候我們就會用 value function $$V(x)$$

$$
V(x\_t) = \min\_u \left( x\_t^TQx\_t + u\_tRu\_t + V(x\_{t+1}) \right)
$$

* $$V(x)$$ 代表最佳情況下，未來所有代價的總和
* 當前 V = 當下最佳控制的代價 + 下一刻 V

我們可以假設 $$V(X)$$ 是 quadratic form (寫成以下，其中 P 是對稱矩陣)

$$
V(x\_t) = x\_t^T P\_t x\_t
$$

再將 linear motion model ($$Ax\_t+Bu\_t$$) 帶入當中得到

$$
\begin{aligned}
V\left(x\_{t}\right)&
\=\min *{\mathbf{u}}\left{x*{t}^{T} Q x\_{t}+u\_{t} R u\_{t}+x\_{t+1}^{T} P\_{t+1} x\_{t+1}\right}
\\
&=\min *{\mathbf{u}}\left{x*{t}^{T} Q x\_{t}+u\_{t} R u\_{t}+\left(A x\_{t}+B u\_{t}\right)^{T} P\_{t+1}\left(A x\_{t}+B u\_{t}\right)\right}
\\
&=\min *{\mathbf{u}}\left{x*{t}^{T}\left(Q+A^{T} P\_{t+1} A\right) x\_{t}+2 x^{T} A^{T} P B u+u\_{t}^{T}\left(R+B^{T} P\_{t+1} B\right) u\_{t}\right}
\end{aligned}
$$

因為我們假設的 value function 是 quadratic 形式，所以可以用微分來求最佳控制 ($$u^\ast$$)

$$
\begin{aligned}
\&V\left(x\_{t}\right)=x\_{t}^{T} P\_{t} x\_{t}=\min *{u}\left{x*{t}^{T}\left(Q+A^{T} P\_{t+1} A\right) x\_{t}+2 x^{T} A^{T} P B u+u\_{t}^{T}\left(R+B^{T} P\_{t+1} B\right) u\_{t}\right}
\\
&\frac{\partial}{\partial u}\left\[x\_{t}^{T}\left(Q+A^{T} P\_{t+1} A\right) x\_{t}+2 x^{T} A^{T} P B u\_{t}^{*}+u\_{t}^{* T}\left(R+B^{T} P\_{t+1} B\right) u\_{t}^{*}\right]=0
\\
&2\left(x^{T} A^{T} P\_{t+1} B\right)^{T}+2\left(R+B^{T} P\_{t+1} B\right) u\_{t}^{*}=0
\\
\&u\_{t}^{\*}=-\left(R+B^{T} P\_{t+1} B\right)^{-1} B^{T} P\_{t+1} A x\_{t}
\end{aligned}
$$

得到的 $$u^\ast$$ 就可以帶回 $$V(x) = x\_t^TP\_tx\_t$$

$$x\_{t}^{T} P\_{t} x\_{t}=x\_{t}^{T}\left(Q+A^{T} P\_{t+1} A-A^{T} P\_{t+1} B\left(R+B^{T} P\_{t+1} B\right)^{-1} B^{T} P\_{t+1} A\right) x\_{t}$$

把兩側的 $$x$$ 都拿掉就可以得到 $$P$$，這個 P 矩陣是 **discrete algebraic Riccati equations (DARE)**

$$
P\_{t}=Q+A^{T} P\_{t+1} A-A^{T} P\_{t+1} B\left(R+B^{T} P\_{t+1} B\right)^{-1} B^{T} P\_{t+1} A
$$

P 代表了前後時刻的轉換方程

> * 順帶一提，在連續的情況下是 continuous algebraic Riccati equations (CARE)
>
>   $$\dot{P}=-P A-A^{T} P+P B R^{-1} P-Q$$

因為 P 不會隨時間變化，所以式子中等式左右的 P 可以改成相同的形式

$$
P=Q+A^{T} P A-A^{T} P\_{t+1} B\left(R+B^{T} P B\right)^{-1} B^{T} P A
$$

在實作時還可以更簡化，使用 iterative 的方式來求 P 直到收斂

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2nwsII78Plhi1VXVS8%2F-M2nwtYx7s_PyZ-YtB0q%2Fiteratively_find_riccati_equation.png?generation=1584640794670810\&alt=media)

### LQR Control for Kinematic Model

$$
\begin{aligned}
\text{Define: } \\
&\text{State }x = \left\[ e, \dot{e}, \theta, \dot{\theta}\right]\\
&\text{Matrix } Q, R
\end{aligned}
$$

* $$e$$: 橫向的最近距離 (誤差)
  * $$\dot{e}$$: 橫向距離 (誤差) 的改變量
* $$\theta$$: 方向誤差
  * $$\dot{\theta}$$: 方向誤差的改變量

兩個改變量存在是為了限制 state 誤差改變量不要太大

接著需要 linear kinematic motion model:

$$\frac{d}{d t}\left\[\begin{array}{l} e \ \dot{e} \ \theta \ \dot{\theta} \end{array}\right]=\left\[\begin{array}{llll} 1 & d t & 0 & 0 \ 0 & 0 & v & 0 \ 0 & 0 & 1 & d t \ 0 & 0 & 0 & 0 \end{array}\right]\left\[\begin{array}{l} e \ \dot{e} \ \theta \ \dot{\theta} \end{array}\right]+\left\[\begin{array}{c} 0 \ 0 \ 0 \ \frac{v \tan (\delta)}{L} \end{array}\right]$$

* 橫向距離 = 上個時間點距離 (1) + 橫向速度 (dt)
* 角度 = 上個時間點角度 (1) + 角速度 (dt)
* 最後加的方向盤控制量 ($$\delta$$) 並不是線性的
  * 所以可以用 $$\delta$$ 來近似取代 $$\tan(\delta)$$

$$\approx\left\[\begin{array}{cccc} 1 & d t & 0 & 0 \ 0 & 0 & v & 0 \ 0 & 0 & 1 & d t \ 0 & 0 & 0 & 0 \end{array}\right]\left\[\begin{array}{c} e \ \dot{e} \ \theta \ \dot{\theta} \end{array}\right]+\left\[\begin{array}{l} 0 \ 0 \ 0 \ \frac{v}{L} \end{array}\right] \delta=A x+B u$$

於是就可以得到 LQR 可解的線性模型 ($$Ax + Bu$$)

然後用 DARE 求出 P matrix

$$P=Q+A^{T} P A-A^{T} P B\left(R+B^{T} P B\right)^{-1} B^{T} P A$$

再求出最佳的控制量

$$u\_{t}^{\*}=-\left(R+B^{T} P\_{t+1} B\right)^{-1} B^{T} P\_{t+1} A x\_{t}$$

## Summary

* Basic Kinematic Model

$$
\begin{aligned}
\begin{bmatrix}\dot{x} \ \dot{y} \ \dot{\theta}\end{bmatrix} &=
R(\theta)^{-1}\begin{bmatrix}\dot{x\_R}\\\dot{y\_R}\\\dot{\theta}\end{bmatrix} \\
&= \begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0& 0& 1\end{bmatrix}
\begin{bmatrix}v\0\\\omega\end{bmatrix} \\
&= \begin{bmatrix}v\cos(\theta) \ v\sin(\theta) \\\omega\end{bmatrix}
\end{aligned}
$$

* Differential Drive Vehicle

$$
\begin{aligned}
\text{Kinematic Model:} \\
\begin{bmatrix}\dot{x} \ \dot{y} \ \dot{\theta}\end{bmatrix} &=
R(\theta)^{-1}\begin{bmatrix}\dot{x\_R}\\\dot{y\_R}\\\dot{\theta}\end{bmatrix} \\
&= \begin{bmatrix}
\cos\theta & -\sin\theta & 0 \\
\sin\theta & \cos\theta & 0 \\
0& 0& 1\end{bmatrix}
\begin{bmatrix}
\frac{r\dot{\phi\_1}}{2}+\frac{r\dot{\phi\_2}}{2}\\
0\\
\frac{r\dot{\phi\_1}}{2l}-\frac{r\dot{\phi\_2}}{2l}
\end{bmatrix} \\
\end{aligned}
$$

* Kinematic Bicycle Model

$$
\begin{aligned}
&\begin{bmatrix}
\dot{x}\\\dot{y}\\\dot{\theta}
\end{bmatrix} =
\begin{bmatrix}
\cos(\theta) \ \sin(\theta) \ \frac{\tan(\delta)}{L}
\end{bmatrix} v
\\\\
&\bullet R\dot{\theta} = v \\
&\bullet \frac{v\tan(\delta)}{L} = \frac{v}{R} \\
&\bullet \tan(\delta) = \frac{L}{R}
\end{aligned}
$$

* Control Algorithms

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M2nwsII78Plhi1VXVS8%2F-M2nwtZ4BPFWI8Dc6h3t%2Fcontrol_algorithms.png?generation=1584640794850815\&alt=media)