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