採用滑動模式設計，應用於NXT馬達定位控制

根據前述（線性非時變，可變結構滑動模式的概念），茲嘗試建立滑動模式控制應用於NXT馬達定位控制，結果定位效果顯著且迅速。

相關參數簡述如下：
（一）滑動曲面函數S(X)=0.7X1+X2=0
　　　其中X1=期望值-實際量測值，X2=X1變化率
（二）當S‧X1>0，提供一正向驅動控制量，本例為Ud=2*err;
　　　當S‧X1<0，提供一正向驅動控制量，本例為Ud=-2*err;
　　　滑動模式控制量Ueq=-0.5*err;
（三）總驅動控制量U=Ud+Ueq
（四）紀錄X1、X2及馬達回應的encoder轉動值，儲存至NXT主機中（本例指定為DataLog.txt）。

[執行結果]



左圖為定位控制的觀測值，期望值為180度。由圖中可看出定位速度很快。













左圖為x1,x2的平面軌跡圖。
迅速滑向原點。















上述參數可由使用者自行更改，以求較佳控制，筆者僅略以此控制法則說明滑動模式非常適用。

[原始程式]

task main()
{
   ResetRotationCount(OUT_A);
   long readvalue=0;
   long DesirePosition=180;
   long err=0,prev_err=0;;
   long cerr=0;
   float S=0;
   float Ud=0,Ueq=0;
   int U=0;
  
   byte fileHandle;
   short bytesWritten;
   DeleteFile("DataLog.txt");
   CreateFile("DataLog.txt",10000,fileHandle);
   string str_position;
   string str_err;
   string str_cerr;
   string str_log;
  
   while(!ButtonPressed(BTNCENTER,true))
   {
      str_position=NumToStr(readvalue);
      str_err=NumToStr(err);
      str_cerr=NumToStr(cerr);
      str_log= str_position+"  " +str_err+ "  " +str_cerr;
      WriteLnString(fileHandle,str_log,bytesWritten);
  
      readvalue=MotorRotationCount(OUT_A);
      err=DesirePosition-readvalue;
      cerr=(err-prev_err);
     
      S=0.7*(err)+(cerr);
      prev_err=err;
     
      if(S*err>0)
      {
          Ud=2*err;
      }
      if(S*err<0)        {            Ud=-2*err;        }        Ueq= -0.5*err;                U=Ud+Ueq;                if(U>100) U=100;
      if(U<-100) U=-100;         OnFwd(OUT_A,U);        TextOut(0,LCD_LINE1,"degree=");        NumOut(6*8,LCD_LINE1,readvalue);        Wait(50);        Off(OUT_A);        ClearScreen();     }     CloseFile(fileHandle); } 

線性非時變系統，可變結構滑動模式的概念

可變結構對於系統參數變動有極佳、強健的控制特性，研究所曾學習此一控制方法。由於目前正投入研究segway的平衡控制，所以重拾以前所學。

滑動模式(Sliding Mode, SM)是可變結構(Variable Structure System, VSS)的一支學問，其控制的主幹，在於建立滑動曲面函數，迫使系統狀態，往特定的滑動曲面逼近，並誘使其不離開滑動曲面，直至逼近期望值。

滑動曲面函數定義為：





S（Xi）=C1X1+C2X2+....+Cn-1Xn-1+Xn＝0
其中X即使用者定義的觀測變數，例如：
X1=期望值-實際測量值=誤差值，實際測量值可由感測器或儀表測得
X2=（當前時刻X1-前一時刻X1)/△T=誤差變化率
而C為常數係數，最高項次之係數為1

控制器根據系統觀測變數，提供輸入控制量，讓觀測的變數到達滑動區面函數，此一控制量稱之為到達模式控制量（reaching mode control，Ud）或(hitting) 。
當觀測變數到達或逼近滑動曲面後，為了使該觀測值不離開滑動曲面，更期望它沿著滑動曲面移動而滑向原點達到期望值，控制器必須提供足量的控制量（等效控制量，Ueq）來維持系統奔向期望值。如下圖即觀測變數沿著滑動曲面滑向原點示意圖。



（一）到達模式控制律(hitting law)
　　　條件：S‧X1>0
　　　Ud=α‧X1
　　　條件：S‧X1<0
　　　Ud=β‧X1
　　　其中α>0、β<0
（二）滑動模式控制律（sliding mode law）
　　　Ueq=γ‧X1
　（三）U=Ud+Ueq



由於現今的控制系統均採微電腦控制，所以離散時間的控制設計，觀測變數不可能準確落在滑動曲面上，當進入滑動曲面後，必然於曲面的兩側左右顫動(chatter)，但仍會逐步進入期望值。

假設有一線性非時變系統如下：











可知該狀態方程式如下：







若假設某一系統微分方程式為x"(t)+2x'(t)+3x(t)=u(t)，則狀態方程式表示如下：






其中x=x1、dx/dt=x2



若我假定滑動函數S:0.7(z1)+(z2)=0，此函數必通過z1,z2平面的原點。

滑動函數原常用x1,x2作為滑動函數之觀測變數，但易與系統狀態變數混淆，所以爰用z1,z2作為觀測變數。

[程式碼]

clear all
clc

%二階微分方程X'=AX+BU    Y=CX+DU
a11=0;a12=1;a21=-2;a22=-3;
A=[a11 a12;a21 a22];
B=[0;1];
C=[1 0];
D=0;
U=0;
T=0.005;           %取樣時間
T_End=20;          %結束時間
X=[0;0];           %狀態變數X1
dot_X=[0;0];
R=10;              %期望值(參考值)
cnt=1;
alpha=20;
beta=-20;
Theta=0;

%滑動函數參數S=C1*e+C2*ce
C1=0.7;
C2=1;

cond=-A(2,2)*C1-C1^2+A(2,1)/B(2,1);
[A,B,C,D]=c2dm(A,B,C,D,T);   %微分方程轉換離散方程模型

for i=0:T:T_End
dot_X=A*X+B*U;
N=C*X+D*U;

Theta=Theta+N*T;

err(cnt)=R-Theta;

if (cnt==1)
  cerr(cnt)=0;
else
  cerr(cnt)=(err(cnt)-err(cnt-1))/T;
end

S=C1*err(cnt)+C2*cerr(cnt);

if (S*err(cnt)>0)
  Ud=alpha*err(cnt);
end
if(S*err(cnt)<0)    
　　　Ud=beta*err(cnt);
end
Ueq=cond*err(cnt);
U=Ud+Ueq;     
X=dot_X;
cnt=cnt+1;
end

plot(err,cerr,'-');
axis([0 15 -15 0]);
hold on
X2=C1*[0 -15];
X1=[0 15];
plot(X1,X2,'r-');
hold off
grid
xlabel('X1');
ylabel('X2');  

[模擬執行結果]



從模擬的結果觀察得知，觀測變數x1,x2(即滑動函數定義的變數)其移動軌跡在初值迅速逼近滑動函數，並在函數的兩側切換控制量，使其維持在滑動面直至期望值。

segway兩輪平衡車控制的規劃議題

綜觀目前有做segway研究的專題，在機身維持平衡的控制器，都會計算車輪的移動角度以及移動角速度（RotationCount），想說為何要考慮車輪移動的角度及轉動角速度？一般說來，只要陀螺儀測到機身有前傾或後仰，得到傾斜角度，不就可以利用所得數據，給予控制？

和專家Bridan交換這個議題，Bridan給予的參考建議如下：

如果 Segway 只是原地平衡直立，我認為偵測車輪角度及加裝一顆光感應器應該足夠，因為從光感應器可以判別傾倒方向，然後控制車輪轉動角度，應該可以直立平衡，當原地直立平衡可以達成，再嘗試前進控制。

以上是我看了您的留言的想法，看來有空要試看看，況且不用再額外添購感應器。個人認為利用車輪角度控制最大的好處，應該是取值快速避免時間延遲誤差。

後來我也再次思考這個問題：測量輪子的轉角與轉角速度，並提供控制量，似乎有一種加速促成的作用。

1.假設一開始輪子的rotationcount()值為0，當segway前傾一個角度θ，控制器驅動車子往前移動，此刻車輪的rotationcount()有讀值且有轉角變化，所以控制器的控制量加成，驅使車子前進速度加快。

2.當車子移動使機身由前傾回到筆直的0度，對機身角度所提供的控制量會減少逼近至0，但因車子仍屬於移動狀態，所以rotationcount()仍有提供相對的控制量，所以極有機會使機身從筆直的狀態往後仰；

3.當機身後仰，控制器提供反向的控制量，促使機身往後退。

但據觀察，當機身由筆直狀態至後仰，因在此狀態交替的瞬間，車輪仍有可能處於前進而非後退，所以提供的控制量有可能有抵消的作用，使控制量減少，也許還是正值，可能仍會促使車子往前走，而這樣的結果反倒加速車子後仰。
所以實際的結果，會變成segway搖擺現象。不過很佩服目前採用上述作法的作品，竟然感受不到機身搖擺。真是厲害，看樣子try error不少時間，才能測驗出一個比較好的參數。

Runge-Kutta應用於NXT-兩輪平衡車角度的量測

陀螺儀對機身擾動非常敏感，在前述透過卡爾門濾波器的設計，可以有效減低它的敏
感性。

目前進行陀螺儀對二輪平衡車測定其傾斜機身角度，運用離散時間之轉角速度與時間
的乘積加總所得的角度，會有誤差的問題之外，陀螺儀在運作期間，也會因為飄移的問題
，使角度誤差的情況更加嚴重（換句話說：當二輪平衡車在前傾與後仰的維持平衡一
段時間後，當機身回到直立狀態的某一瞬間，其角度並非零值，可能為負或正），此
刻易使控制器產生誤判而使機身搖擺。

所以程式中，另行加入Runge-Kutta的二階微分技術，除了使測定的角度更加精準之
外，尚須扣除角度的偏移值（bias），將之應用在NXT，明顯改善本問題。

[NXC版本]

#define GyroScopePort IN_3
#define sample_size 100
#define GyroScale 4
#define t_scale 1e+6

//Define gyro offset parameters
float gyro_sum=0, gyro_avg=0;
int gyro_value=0;
int offset=0;
int i;

//Define Kalman filter parameter
float P=10;  //Process state covariance
float Kgain;        //The Kalman Filter Gain
float X_old=0;
float X_curr;
float Q=0.0001;     //The Probability of process noise w ,P(w)~N(0,Q)
float R=0.1;        //The Probability of measurement noise v, P(v)~N(0,R)

//Define BodyTiltTheta parameters
float theta=0,prev_theta=0;
float f1=0,f2=0;
float aa=0.001;
float theta_bias=0;
long prev_tick;


int KalmanFilter(int gyro_value)
{
X_curr=X_old;
P=P+Q;
Kgain=P/(P+R);
X_curr=X_curr+Kgain*(gyro_value-X_curr);
P=(1-Kgain)*P;
X_old=X_curr;
return (X_curr);
}


void CalcGyroOffset()
{
for(i=0;i<sample_size;i++)
{
   gyro_value=SensorHTGyro(GyroScopePort);
   gyro_sum+=gyro_value;
   Wait(4);
}
gyro_avg=gyro_sum/sample_size;
offset=gyro_avg+0.5;
}

void ShowLCD()
{

 TextOut(0,LCD_LINE1,"theta=");
 NumOut(6*6,LCD_LINE1,theta-theta_bias);
 Wait(20);
 ClearScreen();
}


task main()
{
SetSensorHTGyro(GyroScopePort);

CalcGyroOffset();
prev_tick=CurrentTick();
while(true)
{
  gyro_value=SensorHTGyro(GyroScopePort,offset);
  f2=KalmanFilter(gyro_value)/GyroScale;
  Wait(1);
  theta=prev_theta+(f1+2*f2)*(CurrentTick()-prev_tick)/t_scale;
  prev_theta=theta;
  gyro_value=SensorHTGyro(GyroScopePort,offset);
  f1=KalmanFilter(gyro_value)/GyroScale;
  prev_tick=CurrentTick();
  theta_bias=(1-aa)*theta_bias+aa*theta;
  ShowLCD();
 }
}

Kalman Filter 實現於NXT 感測器數據量測

接續前一個主題，我們期望Kalman Filter可以有效改善感測器對noise之敏感性。

理想的情況下，計算出陀螺儀初始的偏移量（bias）後，期望陀螺儀的感測元件讀值扣除偏移量之後，機身不動，理論上讀值應為某一個準位（例如直立時，讀值為0）

然而，很不幸地，陀螺儀感測元件在靜止狀態時，仍有數據產生飄移的現象。

在程式中加入了Kalman Filter之後，果真使讀值飄移的現象大幅改善。

以下為經過陀螺儀靜止狀態5秒鐘內的數據讀取所呈現的結果：

上圖，綠色線代表未濾波時的陀螺儀讀取數據；
紅色線代表濾波的結果。

Perfect~

Kalman Filter 減低noise 的模擬試驗

使用時機：
當系統測量的過程，遇到隨機擾動的noise時，便無法真正測得真實的數據，為了減少擾動誤差，所以加入了卡爾門濾波器（Kalman Filter ）。

以純量，說明卡爾門濾波器運作原理：

prior X(k|k-1)=AX(k-1)+BU(k)..................(1)
prior P(k|k-1)=AP(k-1|k-1)A'+Q=P(k-1|k-1)+Q...............(2)
其中：
1.W(k)為第k時刻的process state noise，與X獨立，且滿足Probability Normal Distribution
p(w)~N(0,Q)
2.我們假設A=1(認定第k時刻和第k-1時刻無變異)
3.假設P(k)是X(k)的covariance： P(k|k-1)以k-1時刻推測k時刻的prior值

在第k時刻，同時會有一個measurement value Z(k)，與prior H(k)X(k)的Residual:
Residual =Z(k)-H(k)X(k|k-1)=Z(k)-X(k|k-1).................(3)
假設H=1(測量值與process state都是相同的單位，無物理轉換關係)

posteriori X(k|k)=X(k|k-1)+Kgain*(Residual).................(4)
1.其中Kgain是Kalman filter 增益
2.Kgain=P(k|k-1)H'(HP(k|k-1)H'+R)-1=P(k|k-1)/(P(k|k-1)+R), P(k)為(2)的值
3.R是measurement noise與Z獨立，且滿足Probability Normal Distribution
p(v)~N(0,R)
4.X(k|k)是後測的process state，推測第k時刻的最佳值

P(k|k)=(1-Kgain)*P(k|k-1)...................(5)

-------------------------------
程式：

clear all
clc
A=1;
B=1;
H=1;
U=0;
N=300;
desire=25*ones(1,N);

X=zeros(1,N);
measurement_noise=randn(1,N);
process_noise=randn(1,N);
Y=2^0.5*(measurement_noise)+desire;     %NormalDistribution random value

% set initial state variable and its covariance value
X(1)=0;
P=1;
Q=1e-4;   %set probability of process noise covariance P(w)~N(0,Q)
R=1e-1;  %set probability of measurement noise covariance P(v)~N(0,R)

for k=2:N
 X(k)=X(k-1);
 P=P+Q;
 KGain=P/(P+R);
 X(k)=X(k)+KGain*(Y(k)-X(k));
 P=(1-KGain)*P;
end
plot(X);
hold on
plot(desire,'r-');
plot(Y,'g*');
xlabel('time');
ylabel('temperature');
hold off

------------------------------------
[結果]





















其中
1.紅色水平線-期望值（用以觀察濾波後的值與之對照）
2.綠色點-未濾波時的數據（受到擾動很嚴重）
3.藍色線-濾波後的數據（很貼近期望值）

用Kalman filter 的技術來減低noise干擾

一篇經典的文獻，探討感測器偵測值易受周遭環境影響而產生white noise，應用Kalman filter 的技術來減低noise干擾，以提升感測器精準的量測。

參考文獻http://www.swarthmore.edu/NatSci/echeeve1/Ref/Kalman/ScalarKalman.html