數學推導與模擬是在設計控制器前，了解一個系統必經的過程之一，雖然倒單擺作為一經典教材，其運動方程式幾乎已成陳腔濫調一般，各種變化形式，或者更複雜的倒單擺模型亦是層出不窮，幾乎在網路上都找得到。不過本篇文章還是會先回到最原始的倒單擺數學推導，並搭配Processing軟體模擬其動態，作為專題記錄。
得到倒單擺的運動方程式之後，下一步就是透過電腦模擬分析來作為設計控制器前的準備。學校裡大概都是使用MATLAB來執行這樣的工作，先將方程式寫入m-file，再由主程式呼叫ode45進行求解，然而ode45這個經典的數值分析方法並非MATLAB不可，任何可執行計算的程式語言都可以套用。所以若是已經厭倦每次模擬都只能看到醜醜的響應曲線圖，不妨試用Processing，因為其以繪圖見長，直接把模型的動態顯示出來不是更直觀嗎?
那麼ode45很難寫嗎? 其實不然，有興趣可以找找MATLAB或是其他相似軟體的原始碼，在其函數中真正執行RK45(Runge-Kutta-Fehlberg Method)的部分大概也只佔其函數定義三分之一的篇幅。
在Processing中，我把倒單擺的參數設定、模型顯示、數學描述與RK45都包在一個class中，由於計算中會用到向量與矩陣，所以還必須定義一些相關的函數，或是直接import JAVA的矩陣類別也可以，範例如下:
class invPendulum {
// System Parameters
final float gravity = 9.8,
massPendulum = 1.5,
massCart = 0.3,
lengthArm = 0.6,
momentInertia = massPendulum * lengthArm * lengthArm,
A = lengthArm + momentInertia / (massPendulum * lengthArm),
B = ( massPendulum + massCart ) / (massPendulum * lengthArm);
int systemOrder = 4;
// Initial Condition
float iniTheta = 0.1,
iniOmega,
iniDisplacement,
iniVelocity;
// State Variables
float displacement,
velocity,
theta,
thetaLimit = 0.78,
omega,
z[] = new float[systemOrder],
zTemp[] = new float[systemOrder],
z4[] = new float[systemOrder],
z5[] = new float[systemOrder];
// Calculation Variables
float torque,
force,
alpha,
acceleration;
// Adaptive RK45 Method Parameters
final float tolerance = 0.001,
maxIteration = 5,
a2 = 0.25,
a3[] = {3/32f, 9/32f},
a4[] = {1932/2197f, -7200/2197f, 7296/2197f},
a5[] = {439/216f, -8.0, 3680/513f, -845/4104f},
a6[] = {-8/27f, 2.0, -3544/2565f, 1859/4104f, -11/40f},
b1[] = {16/135f, 0.0, 6656/12825f, 28561/56430f, -9/50f, 2/55f}, // Order 5
b2[] = {25/216f, 0.0, 1408/2565f, 2197/4101f, -0.2, 0.0}, // Order 4
c[] = {0.25, 3/8f, 12/13f, 1.0, 0.5};
float h = 0.01,
k1[] = new float[systemOrder],
k2[] = new float[systemOrder],
k3[] = new float[systemOrder],
k4[] = new float[systemOrder],
k5[] = new float[systemOrder],
k6[] = new float[systemOrder];
// Control Signal
float u = 0;
// Visualized Model
PShape redPin;
final float refX = 729.546, refY = 1839.63,
scaleFactor = 0.1;
PVector refPoint;
float groundHeight;
invPendulum(int W, int H) {
redPin = loadShape("redPin.svg");
redPin.scale(scaleFactor);
refPoint = new PVector( scaleFactor * refX , scaleFactor * refY);
iniDisplacement = W/2;
groundHeight = 0.5 * H;
println( A * B );
displacement = iniDisplacement;
velocity = iniVelocity;
theta = iniTheta;
omega = iniOmega;
z[0] = displacement;
z[1] = velocity;
z[2] = theta;
z[3] = omega;
}
float[] equationMotion(float x[]) {
float k[] = new float[systemOrder];
force = A * ( u + pow(x[3], 2) * sin(x[2]) ) - 0.5 * gravity * sin(2 * x[2]);
acceleration = force / ( A * B - pow(cos(x[2]), 2) );
torque = gravity * B * sin(x[2]) - 0.5 * pow(x[3], 2) * sin(2 * x[2]) - u * cos(x[2]);
alpha = torque / ( A * B - pow(cos(x[2]), 2) );
k[0] = x[1];
k[1] = acceleration;
k[2] = x[3];
k[3] = alpha;
return k;
} // float[] equationMotion(float x[])
float[] equationConstrain(float x[]) {
float k[] = new float[systemOrder];
force = A * u;
acceleration = force / ( A * B - pow(cos(x[2]), 2) );
torque = - u * cos(x[2]);
alpha = torque / ( A * B - pow(cos(x[2]), 2) );
k[0] = x[1];
k[1] = acceleration;
k[2] = x[3];
k[3] = alpha;
return k;
} // float[] equationConstrain(float x[])
void RK45() {
float eps[] = new float[systemOrder], maxEps = 1.0;
float s;
int iterationCount = 0;
if( abs(z[2]) < thetaLimit ) {
while( (iterationCount < maxIteration) && (maxEps > tolerance) ) {
k1 = vectorScaling( equationMotion(z), h );
zTemp = vectorAddition( z,
vectorScaling(k1, 2) );
k2 = vectorScaling( equationMotion(zTemp), h );
zTemp = vectorAddition(
vectorAddition( z,
vectorScaling(k1, a3[0]) ),
vectorScaling(k2, a3[1]) );
k3 = vectorScaling( equationMotion(zTemp), h );
zTemp = vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, a4[0]) ),
vectorScaling(k2, a4[1]) ),
vectorScaling(k3, a4[2]) );
k4 = vectorScaling( equationMotion(zTemp), h );
zTemp = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, a5[0]) ),
vectorScaling(k2, a5[1]) ),
vectorScaling(k3, a5[2]) ),
vectorScaling(k4, a5[3]) );
k5 = vectorScaling( equationMotion(zTemp), h );
zTemp = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, a6[0]) ),
vectorScaling(k2, a6[1]) ),
vectorScaling(k3, a6[2]) ),
vectorScaling(k4, a6[3]) ),
vectorScaling(k5, a6[4]) );
k6 = vectorScaling( equationMotion(zTemp), h );
// println("k1");
// println(k1);
// println("k2");
// println(k2);
// println("k3");
// println(k3);
// println("k4");
// println(k4);
// println("k5");
// println(k5);
// println("k6");
// println(k6);
z4 = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, b2[0]) ),
vectorScaling(k3, b2[2]) ),
vectorScaling(k4, b2[3]) ),
vectorScaling(k5, b2[4]) );
z5 = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, b1[0]) ),
vectorScaling(k3, b1[2]) ),
vectorScaling(k4, b1[3]) ),
vectorScaling(k5, b1[4]) ),
vectorScaling(k6, b1[5]) );
// println("z4");
// println(z4);
// println("z5");
// println(z5);
eps = vectorAddition( z5, vectorScaling( z4, -1) );
eps = vectorAbs(eps);
maxEps = max(eps);
// println(eps);
// println(maxEps);
// println(h);
s = 0.84 * pow( tolerance * h / (vectorNorm(eps)) , 0.25 );
// println(s);
if( maxEps > tolerance ) {
h *= s;
}
} // while( (iterationCount < maxIeration) && (maxEps > tolerance) )
z = z5;
}
else {
z[2] = constrain(z[2], -thetaLimit, thetaLimit);
z[3] = 0;
while( (iterationCount < maxIteration) && (maxEps > tolerance) ) {
k1 = vectorScaling( equationConstrain(z), h );
zTemp = vectorAddition( z,
vectorScaling(k1, 2) );
k2 = vectorScaling( equationConstrain(zTemp), h );
zTemp = vectorAddition(
vectorAddition( z,
vectorScaling(k1, a3[0]) ),
vectorScaling(k2, a3[1]) );
k3 = vectorScaling( equationConstrain(zTemp), h );
zTemp = vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, a4[0]) ),
vectorScaling(k2, a4[1]) ),
vectorScaling(k3, a4[2]) );
k4 = vectorScaling( equationConstrain(zTemp), h );
zTemp = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, a5[0]) ),
vectorScaling(k2, a5[1]) ),
vectorScaling(k3, a5[2]) ),
vectorScaling(k4, a5[3]) );
k5 = vectorScaling( equationConstrain(zTemp), h );
zTemp = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, a6[0]) ),
vectorScaling(k2, a6[1]) ),
vectorScaling(k3, a6[2]) ),
vectorScaling(k4, a6[3]) ),
vectorScaling(k5, a6[4]) );
k6 = vectorScaling( equationConstrain(zTemp), h );
// println("k1");
// println(k1);
// println("k2");
// println(k2);
// println("k3");
// println(k3);
// println("k4");
// println(k4);
// println("k5");
// println(k5);
// println("k6");
// println(k6);
z4 = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, b2[0]) ),
vectorScaling(k3, b2[2]) ),
vectorScaling(k4, b2[3]) ),
vectorScaling(k5, b2[4]) );
z5 = vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition(
vectorAddition( z,
vectorScaling(k1, b1[0]) ),
vectorScaling(k3, b1[2]) ),
vectorScaling(k4, b1[3]) ),
vectorScaling(k5, b1[4]) ),
vectorScaling(k6, b1[5]) );
// println("z4");
// println(z4);
// println("z5");
// println(z5);
eps = vectorAddition( z5, vectorScaling( z4, -1) );
eps = vectorAbs(eps);
maxEps = max(eps);
// println(eps);
// println(maxEps);
// println(h);
s = 0.84 * pow( tolerance * h / (vectorNorm(eps)) , 0.25 );
// println(s);
if( maxEps > tolerance ) {
h *= s;
}
} // while( (iterationCount < maxIeration) && (maxEps > tolerance) )
z = z5;
}
if( abs(z[2]) >= thetaLimit ) {
z[2] = constrain(z[2], -thetaLimit, thetaLimit);
z[3] = 0;
}
displacement = z[0];
velocity = z[1];
theta = z[2];
omega = z[3];
if( millis() > 2000 & millis() < 2009 & u == 0 ) {
u = 350;
println("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
} else {
u = 0;
}
println(omega);
} // void RK45()
void updateModel() {
redPin.rotate(theta);
pushStyle();
fill(200, 250, 150, 180);
smooth();
ellipse(iniDisplacement + 100 * (displacement - iniDisplacement), groundHeight, 20, 20);
stroke(200, 100);
line(0, groundHeight, width, groundHeight);
popStyle();
shape(redPin,
iniDisplacement + 100 * (displacement - iniDisplacement) - refPoint.x*cos(theta) + refPoint.y*sin(theta), // Display with 'cm' in horizontal direction
groundHeight - refPoint.x*sin(theta) - refPoint.y*cos(theta));
redPin.resetMatrix();
redPin.scale(scaleFactor);
}
} // class invPendulum
一些向量計算的函數:
static float[] vectorScaling(float x[], float a) {
int s = x.length;
float[] _x = new float[s];
for(int j = 0; j < s; j++) {
_x[j] = a * x[j];
}
return _x;
} // float[] vectorScaling(float x[], float a)
static float[] vectorAddition(float x[], float y[]) {
int sx = x.length;
int sy = y.length;
float[] _x = new float[sx];
if(sx == sy) {
for(int j = 0; j < sx; j++) {
_x[j] = x[j] + y[j];
}
return _x;
} else {
println("Vector Domension Error! Can't Apply Addition Calculation!");
float[] error = {-1};
return error;
}
} // float[] vectorAddition(float x[], float y[])
static float[] vectorAbs(float x[]) {
int s = x.length;
float[] _x = new float[s];
for(int j = 0; j < s; j++) {
_x[j] = abs(x[j]);
}
return _x;
} // float[] vectorAbs(float x[])
static float vectorNorm(float x[]) {
int s = x.length;
float y = 0;
for(int j = 0; j < s; j++) {
y += sq(x[j]);
}
return sqrt(y);
} // float[] vectorNorm(float x[])
整體程式碼並不長，要特別注意的地方可能是在運動方程式與求解的部分需對邊界條件(Boundary)另外作處理，例如傾斜角度有最大值，倒單擺在邊界上的運動方程式是不同的，必須分開計算。寫好class之後，只要在主程式中呼叫就可以看到自訂的倒單擺模型並即時顯示其模擬的動態。
利用Processing 跑倒單擺模擬
另外網路上可以找到非常多關於RK45 mothed 的理論講解，基本上套入公式即可，一些參考資料如下:
有模擬器的協助，便比較能夠掌握控制器的設計及預期的結果。
