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

App Inventor for NXT:手機與NXT藍芽連線

試驗:
建立Android手機與NXT主機的藍芽連線，並於手機端顯示NXT的主機名稱、電池電量及版本

一、硬體部分:
1.啟動手機的藍芽(BT ON)
2.啟動NXT的藍芽(BT Power ON)

二、App Inventor 軟體畫面:
















三、Block Editor

[lejos]使用TachoPilot類別及travel、rotate等方法，使車子運動軌跡呈正方形