來複習一下課本的小知識...
下面是一個典型的二階線性系統，彈簧(k) ─ 阻尼(c) ─ 質量塊(m)系統及其對應的運動方程式(equation of motion):
u為系統的施力，在控制方塊與硬體中，也可對應為控制命令或是馬達轉動所需的電壓。
由運動方程式可推導系統的特徵方程式，其方程式的根就是特徵值(eigenvalues)，也是系統的極點(poles)，這些抽象的值實際上決定了控制系統表現的行為，極點的實部表示衰減行為，虛部顯示振盪行為，這些皆可由系統參數(m、c、k)推導而得其量值。
考慮施加一控制力u，質量塊會由a點移動到b點，(b - a) 為位移 ( x )， a 到 b 之間的運動過程被稱為暫態響應(transient response)，其運動的方式被極點決定，例如多快到達b點(settling time)，運動是否會過頭(overshoot)，振動的量值等等。若對這些行為不滿意，便可以透過控制的手段來改善，這時候狀態回授的方法便派上用場。
將感測器量到的狀態(位移量、速度等)乘以一個係數回授加到新的控制命令中，使得:
可以發現透過狀態回授的方法，可以改變原方程式的係數，也就像是改變系統的參數，比如說原機構中的彈簧，其彈性係數由 k 變成 k + K1，而阻尼也有類似的效果。當參數不同，便會產生新的特徵方程式與特徵值，那麼極點的位置也就隨之變化，藉此可以改善系統的行為，像是反應速度變快或是變穩定等等。
了解這個原理之後，再回到倒單擺。倒單擺的數學模型當然沒這麼簡單，基本上是非線性的，然而利用泰勒展開的概念可以推導其 Jacobian 矩陣，那麼就可以將原運動方程在平衡點進行線性化，得到類似上面的結果。
不過在程式撰寫方面，模型有沒有線性化並不是那麼重要，實際上程式有很大一部分都在處理和數學與控制無關的事......
下面是一個運用狀態回授進行倒單擺控制的Arduino程式碼範例，由於內容包含了自訂的資源庫(libary)，相關數值的設定也必須搭配實際的硬體，所以要套用到其他的模組，程式必須得在作調整。
將感測器量到的狀態(位移量、速度等)乘以一個係數回授加到新的控制命令中，使得:
代入原來方程式變成
整裡可得
了解這個原理之後，再回到倒單擺。倒單擺的數學模型當然沒這麼簡單，基本上是非線性的，然而利用泰勒展開的概念可以推導其 Jacobian 矩陣，那麼就可以將原運動方程在平衡點進行線性化，得到類似上面的結果。
不過在程式撰寫方面，模型有沒有線性化並不是那麼重要，實際上程式有很大一部分都在處理和數學與控制無關的事......
下面是一個運用狀態回授進行倒單擺控制的Arduino程式碼範例，由於內容包含了自訂的資源庫(libary)，相關數值的設定也必須搭配實際的硬體，所以要套用到其他的模組，程式必須得在作調整。
#include <Wire.h>
#include <SPI.h>
#include "InvPenSensor.h"
#include "InvPenActuator.h"
#define MIN_COMMAND 80
#define GAIN_ANGLE 2150
#define GAIN_OMEGA 20
#define REF 0.175
InvPenSensor sensor;
InvPenActuator actuator;
int command, forwardCount = 0, backwardCount = 0;
float omega, lastOmega = 0, pseudoOmega,
angle, lastAngle = 0, pseudoAngle,
dt, reference = 0.175;
unsigned long time = 0, steady = 0;
void setup() {
Serial.begin(9600);
sensor.init();
actuator.init();
while(lastAngle == 0) {
time = micros();
lastAngle = sensor.getAngle();
}
steady = millis();
steadyProcess();
}
void steadyProcess() {
while( (millis() - steady) < 2000) {
pseudoAngle = sensor.getAngle();
dt = 0.000001 * (micros() - time);
time = micros();
sensor.getGY(&pseudoOmega);
omega = pseudoOmega * PI / 2587.5;
if( pseudoAngle != 0) {
angle = 0.5 * (lastAngle + omega * dt) + 0.5 * pseudoAngle;
} else {
angle = lastAngle + omega * dt;
}
lastAngle = angle;
}
sensor.getGY(&pseudoOmega);
lastOmega = pseudoOmega * PI / 2587.5;
}
int counter = 0;
void loop() {
while(1) {
command = MIN_COMMAND;
pseudoAngle = sensor.getAngle();
dt = 0.000001 * (micros() - time);
time = micros();
sensor.getGY(&pseudoOmega);
omega = 2 * pseudoOmega * PI / 2587.5 - lastOmega;
if( pseudoAngle != 0) {
angle = 0.98 * (lastAngle + omega * dt) + 0.02 * pseudoAngle;
} else {
angle = lastAngle + 1.5 * omega * dt;
}
lastAngle = angle;
lastOmega = omega;
/////////////////////////////////////////////////////////////////////////
if( (angle - reference) > 0.0) {
command += GAIN_ANGLE * (angle - reference);
if(omega > 0.00) {
command += GAIN_OMEGA * omega;
}
command = constrain(command, MIN_COMMAND, 255);
actuator.driveBackward(command);
forwardCount = 0;
backwardCount ++;
// Serial.println("BACK");
} else if( (angle - reference) < -0.0) {
command -= GAIN_ANGLE * (angle - reference);
if(omega < -0.00) {
command -= GAIN_OMEGA * omega;
}
command = constrain(command, MIN_COMMAND, 255);
actuator.driveForward(command);
backwardCount = 0;
forwardCount ++;
// Serial.println("FRONT");
}
reference = REF + 0.00002 * (forwardCount - backwardCount);
} // while(1)
}
這裡是一個實際的demo影片
在理論分析的階段，我們依著數學推導的結果來設計控制器，但是撰寫程式碼，完全又是不同的思維，例如在規畫控制方塊的時候，並不會有判斷邏輯敘述( if(...){...} )的概念，但是這在程式碼中卻是稀鬆平常，又例如在計算控制命令時，很少關注 u 的正負符號與上下限問題，因為方程式總是能自動滿足，然而實際上，驅動電壓最高可能就5v，也不會有負電壓，因此這些數值上的對應細節都只能靠程式來調整，只要有一個環節不正確，那麼連帶控制理論就不成立，哪怕那是多高等的控制技巧都將無力施展。也因此，程式中常需要利用許多"非線性"的寫法來達成原本可能只是"線性"的控制策略。例如:
1. 由於馬達驅動有最低與最高電壓限制，對應的控制命令就必須要有最小與最大值。
2. 每次計算控制命令時，皆由最小值向上累加，以確保命令在可驅動範圍內。
3.在感測的角度變號時，控制命令的計算也要變號，以確保其值始終為正(才有辦法做analogWrite)，但仍要可以區分其方向，所以會利用 if 判斷式，將不同計算式限制在不同的角度區間內。
4. 與角度同方向的角速度變化才做回授。
5. 須對參考值(reference)做回授校正。
最後一點其實很重要也是設計控制器時經常忽略的地方，在理論分析回授增益時，都會將參考值設為零，表示要使倒單擺控制到平衡點。當此值不為零，則表示要使倒單擺向某一側維持一個傾斜角，這時倒單擺需要不斷加速才能維持平衡。因此，如果要讓倒單擺乖乖留在原地，便要確保參考值維持在零點，也就是要設定reference在平衡點的角度上，然而受限於硬體安裝偏差或是配重的影響，這個角度很難事先量測，因此在程式中會先給一個近似的估測值(REF值)，再透過回授的方式來持續校正到平衡點，校正的方式則類似積分控制器。
至於那些增益到底要設多少，理論分析當然可以計算出個值，但不如動手多做嘗試來得有感覺。
