990712-Segway再現

一直對Segway兩輪平衡車設計不出來耿耿於懷。話說Segway兩輪平衡車我打從著手設計程式至今，大概有5個多月了吧。讀了很多相關的文章，從陀螺儀的理論、陀螺儀的定位方法、研究所的相關論文；也使用了Robolab、Labview以及現在使用的Bricxcc等工具，現在稍稍有一點點頭緒。

不過，目前還有缺點：

1.車子往後傾斜，車輪不會動？

2.車子在行駛時，在數秒後車輪會有搖擺的現象（影片中可以看到），之後碰到障礙物後，就直接倒下。

以上都是待改進之處。

程式碼：

#define GyroPort IN_3

#define dt 0.001

#define GyroScale 4

#define t_scale 500

int flag=0;

long GyroBiasVal=0;

long curr_time=0;

long GyroCounter=0;

float bias=0.0000;

float theta=0,pre_theta=0,theta_bias=0;

float f1=0,f2=0;

long t_old=0;

float power=0;

float x=0,pre_x=0,dot_x=0;

float aa=0.001;

float K1=0,K2=90,K3=8,K4=10,V=0.195,K_pwr=0;

void initGyro(){

　curr_time=CurrentTick();

　PlayTone(440,50);

　while(CurrentTick()<(3000+curr_time)){ //3秒內，計算gyro bias

　　GyroBiasVal=GyroBiasVal+SensorRaw(GyroPort);

　　TextOut(0,LCD_LINE5,"Computing Bias..");

　　Wait(20);

　　GyroCounter++;

　}

　PlayTone(880,50);

　bias=GyroBiasVal/GyroCounter;

}//End of initGyro Function

void get_body_theta(){

　f2=(SensorRaw(GyroPort)-bias)/GyroScale;

　Wait(1);

　theta=pre_theta+(f1+2*f2)*(CurrentTick()-t_old)/t_scale;

　pre_theta=theta;

　f1=(SensorRaw(GyroPort)-bias)/GyroScale;

　t_old=CurrentTick();

　x=MotorTachoCount(OUT_B);

　dot_x=x-pre_x;

　pre_x=x;

　theta_bias=(1-aa)*theta_bias+aa*theta;

}//End of get_body_theta ,use Runge Kutta method order 2

task main(){

　SetSensorHTGyro(GyroPort);

　initGyro();

　ClearScreen();

　t_old=CurrentTick();

　ResetTachoCount(OUT_BC);

　//↓Gain computing based on Battery Voltage

　K_pwr=0.7+(0.7-1.1)/(8816-8196)*(BatteryLevel()-8816);　

　while(true){

　　get_body_theta();

　　power=K_pwr*(K1*x+K2*(dot_x-V)+K3*(theta-theta_bias)+K4*(f1));

　　if(power>100) power=100;

　　if(power<-100) power=-100;

　　SetOutput(OUT_BC, Power, power, OutputMode,OUT_MODE_MOTORON,

　　　　　　　RunState,OUT_RUNSTATE_RUNNING,

　　　　　　　UpdateFlags,UF_UPDATE_MODE+UF_UPDATE_SPEED);

}

}//End of Task Main

==================

執行結果：

參考文獻：

1.wiki-nxt_segway-Jean-Philippe Panaget's website

2.以陀螺儀 (HiTechnic Gyro)推算物體的旋轉角度的實作測試

3.Balance Robot

4.Self-Balancing Segway NXTway-Gyro bot

5.兩輪平衡車-樂高機器人

6.A PID Controller For Lego Mindstorms Robots.

7.NXTway-GS Building

8.NXTway Model-Based

9.DSP主控之兩輪機器人平衡與兩輪同步控制，蔡僑倫，國立中央大學-93.06

10.以樂高NXT 實作智慧型兩輪平衡車，羅嘉寧等，銘傳大學-2009資訊科技與實務國際研討會

11.Hitechnic Sensor Website

12.數值分析Runge-Kutta Method order 2,4

13.Kalman Filter

Runge Kutta Method order 4-Example 1

Example 1: solve dy/dx=f(x,y)=3exp(-x)-0.4y，y(0)=5，find y(3)=？

Ans:

使用Runge Kutta Order 4求近似解，已知Runge Kutta Method order 4的一般式：

dy/dx=f(x,y)

→y(k+1)=y(k)＋(1/6)×h×(k1+2k2+2k3+k4)

其中:

k1=f(x(k),y(k))

k2=f(x(k)+(1/2)h,y(k)+(1/2)k1×h)

k3=f(x(k)+(1/2)h,y(k)+(1/2)k2×h)

k4=f(x(k)+h,y(k)+k3×h)

h: 遞增步距

程式範例：

%dy/dx=f(x,y)=3exp(-x)-0.4y, y(0)=5

%Using Runge-Kutta order 4: y(k+1)=y(k)+1/6(k1+2k2+2k3+k4)h

clear all;

clc

h=0.1; %Step

L=0;

R=3;

X=0;Y=5;

counter=1;

for v=L+h:h:R

　k1=Func(X,Y);

　k2=Func(X+0.5*h,Y+0.5*k1*h);

　k3=Func(X+0.5*h,Y+0.5*k2*h);

　k4=Func(X+h,Y+k3*h);

　Y=Y+h*(k1+2*k2+2*k3+k4)/6;

　Y_sol=10*exp(-0.4*(X+h))-5*exp(-(X+h));

　err(counter)=Y_sol-Y;

　Yk(counter)=Y;

　Yk_sol(counter)=Y_sol;

　counter=counter+1;

　X=X+h;

　plot(Yk,'r-')

　hold on

　plot(Yk_sol,'b*')

end

hold off

副程式func.m

function k=Func(x,y)

k=3*exp(-x)-0.4*y;

end

----------------------------------

執行結果：

h=0.1（分析步距）

原微分方程式的解y(x)=10e^-0.4x-5e^-x

上圖中，紅色線為期望值，藍色點是近似值。

利用RK4分析微分方程式的動態行為，其近似值逼近期望值。

總誤差=Σ(每一點的誤差)=1.3587×10^-6

梯形積分法

f(x)在封閉區間[a,b]求積分，若f(x)為複雜的函數， 對於求解積分，可用

數值分析的梯形積分法來求解。

梯形積分法，得到的結果，對照原函數定積分的值，可發現誤差值很小。所以被廣泛運用。

以下用matlab 語法，撰寫梯形積分的algorithm：

%Function F(x)=∫sin(x)dx，積分範圍0~π

a=0;

b=pi;

n=1000;

A=0;

for i=a:(b-a)/n:b

　　if i==a

　　　f1=sin(i);

　　else

　　　f2=sin(i);

　　　A=A+0.5*(f1+f2)*(b-a)/n;

　　　f1=f2;

　　end

end

disp('梯形積分法面積=');

A

---------------------

經執行後，A＝2.0000

與積分範圍[0,pi]，∫sin(x)dx=2的結果一致。

Runge Kutta Method order 2

微分方程式y'=y； y(0)=1

Ans:

使用Runge Kutta Method order 2:

Runge-Kutta Method通式：

y'＝f(x,y)

x_i=x₀+ih,i=0,1,2…

y_k+1＝y_k＋(1/2)．(k₁+k₂)

其中

k₁=h．f(x_k,y_k)

k₂=h．f(x_k＋h,y_k＋k1)

假設h=0.5(漸進步距)

所以：

y'＝f(x,y)＝y

y(0)＝1，換句話說y₀＝1

x₁=x₀+i×h=0+1×0.5=0.5

k₁＝h×f(x₀,y₀)＝0.5×f(0,1)=0.5×1=0.5

k₂＝h×f(x₀＋h,y₀＋k₁)＝0.5×f(0＋0.5,1＋0.5)=0.5×f(0.5,1.5)=0.5×1.5=0.75

其中f(x,y)=y，帶入x=0,y=1進f(x,y)中，得f(0,1)=1

--> y₁＝y₀＋(1/2)×(k₁+k₂)=1+(1/2)×(0.5+0.75)=1.625

x₂=x₀+i×h=0+2×0.5=1.0

k₁＝h×f(x₁,y₁)＝0.5×f(0.5,1.625)=0.5×1.625=0.8125

k₂＝h×f(x₁＋h,y₁＋k₁)

＝0.5×f(0.5＋0.5,1.625＋0.8125)

＝0.5×f(1,2.4375)=0.5×2.4375=1.2188

-->y₂＝y₁＋(1/2)×(k₁+k₂)=1.625+(1/2)×(0.8125+1.2188)=2.6406

對正式解而言，前述y’=y，y(0)=1的通解是y=e^x：

1. y(0)=1

2. y(0.5)=1.6487，經由Runge-Kutta方法計算的y1=1.625，

誤差率＝(1.6487-1.625)/1.625=1.46%

3.y(1)=2.71828，經由Runge-Kutta方法計算的y2=2.6406，

誤差率＝(2.71828-2.6406)/2.6406=2.94%

NXT lego 機器人學術網站

YCChen 協作平台

這個網站很不錯，提供很多優秀的作品，甚至也有學術的相關研究。

值得參考！

網址：http://sites.google.com/site/ycchenster/home/xue-shu-yan-jiu/lego

NXT-自動路邊停車(Auto Parking Car)

作品說明：

一、硬體：

1.nxt主機

2.nxt-Motor×3

3.UltraSonic Sensor×1

4.馬達A控制車子後輪前進後退、馬達B控制車子超音波感測器的偵測方向、

馬達C控制車子前輪轉向（steering）

程式碼：

作品欣賞：

NXT_Clock

題目：

1.用 NXT 就可以設計一個電子鐘，平時正常顯示時間 HH:MM:SS，不做任何組裝。
2.按橘色鍵兩秒可以進入編輯模式，時間計時暫停，被編輯的數字閃爍，其它的數字仍然恆亮不閃爍。
3.再按橘色鍵可選擇小時、分鐘或秒數，第一次切換編輯小時，第二次能編輯分鐘，第三次可調整秒數，第四次回到小時編輯。
4.按橘色鍵兩秒則脫離編輯模式，恢復正常時間顯示。
5.左右方向鍵可將數值上下調整，調整時進位或退位不會影響到其它數字。

程式碼：

作品展示：