Kinetic Model & Vehicle Control

Control Theory

• Control system

  • 能夠影響整個系統未來 state 的機制

• Control theory

  • 因應 output 來改變 input

Open Loop Control

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

Close Loop Control

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

• Sensor

  • 量測上一次的 output 結果，用來和 reference 比對

• Reference

  • 用來比對和 output 的結果，計算出 error

• Controller

  • 根據 error 改變 input

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

Linear Time Invariant System

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

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

• G = Input 運算結果

• H = Sensor 運算結果

• D = Controller 運算結果

• r = reference

• e = error

• y = output

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

接著經過一連串的運算

變成一個 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) 相乘

• 當距離 100 時，走 10 m/s (100*0.1)

• 當距離 90 時，走 9 m/s (90*0.1)

• 當距離 0 時，就停止了 (0*0.1)

Problem

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

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

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

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

Integral gain

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

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

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

Problem

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

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

Differential gain

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

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

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

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

Summary

PID control 可以寫成 discrete form

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

Basic Kinematic Model

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

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

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

• 在低速時

  • 可以用簡單的幾何模型來描述 car state

• 在高速時

  • 因會產生側向滑動，所以需要套入更難的動力學模型

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

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

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

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

Differential Drive Vehicle

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

• P: 原點

• l: 原點分別到兩輪的距離

• r: 輪子的半徑

• 右輪 (左輪) 速度 = 半徑 * 角速度
• 原點的速度就是右輪 (左輪) 速度的一半
• 原點的角速度 = 速度 / 到輪子的距離

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

Pure Pursuit Control

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

• • 因為圓心角 = 兩倍的弦切角
• 因為是等腰三角形

根據正弦定理可以得到

Ld 通常用速度來決定，速度越快就越遠

Kinematic Bicycle Model

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

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

• 前輪控制方向 (方向盤)

• 後輪控制速度 (引擎)

• 後輪為車輛原點 (x, y)

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

• 車子以 v 的速度向前

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

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

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

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

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

以及一些相關的 properties

Pure Pursuit Control for Bicycle Model

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

Stanley Control

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

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

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

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

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

LQR Control

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

• 運動模型是 linear form

• Cost function 是 quadratic form

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

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

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

Value function

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

• 當前 V = 當下最佳控制的代價 + 下一刻 V

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

• 順帶一提，在連續的情況下是 continuous algebraic Riccati equations (CARE)

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

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

LQR Control for Kinematic Model

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

接著需要 linear kinematic motion model:

• 橫向距離 = 上個時間點距離 (1) + 橫向速度 (dt)

• 角度 = 上個時間點角度 (1) + 角速度 (dt)

然後用 DARE 求出 P matrix

再求出最佳的控制量

Summary

• Basic Kinematic Model

• Differential Drive Vehicle

• Kinematic Bicycle Model

• Control Algorithms