Kết quả:
Hình 3. 45 Đồ thị thể hiện góc nghiêng của xe so với góc nghiêng mong muốn
Ta thấy biên độ dao động vẫn còn khá lớn (từ -5 đến 10 độ), điều đó có nghĩa là xe cân bằng vẫn chưa được ổn định. Ngoài ra, mô hình chỉ dừng lại ở mức tự đứng được chứ chưa thể đáp ứng được vấn đề di chuyển và chuyển hướng, góc nghiêng xe cũng rất nhỏ.
Hình 3. 46 Đồ thị thể hiện PID ứng với góc nghiêng của xe
Đánh giá:
Nhìn chung, xe cân bằng vẫn chưa được ổn định, biên độ dao động của xe vẫn còn khá lớn. Tuy nhiên, xe có thể chịu được ngoại lực tác động lớn mà không bị đổ.
Sự cân bằng ổn định của xe có thể phụ thuộc vào các yếu tố sau:
- Tín hiệu từ cảm biến góc nghiêng chưa chính xác tuyệt đối, dễ bị nhiễu, thậm chí là bị mất tín hiệu.
- Phương pháp điều khiển: khi thiết kế mô hình theo kiểu này ta gặp khó khăn lớn về mặt phương pháp điều khiển, phương pháp PID thông dụng thì không có tính ổn định cao, thời gian đưa xe trở lại vị trí cân bằng là quá lâu.
3.4.3.6. Khảo sát thời gian cân bằng của xe ở nhiều tốc độ
Mục đích của việc khảo sát này nhằm tìm ra tốc độ phù hợp nhất cho việc điều chỉnh tốc độ bánh đà ,cũng như giữ thăng bằng cho xe nên vì thế nhóm em sẽ khảo sát thời gian thực dài nhất mà xe có thể tự giữ thăng bằng ở từng tốc độ lần lượt là 130 vòng/s, 90 vòng/s, 60 vòng/s và 30 vòng/s. Ở lần khảo sát này trục bánh đà sẽ xoay tự do mà không cần đến sự điều khiển.
Đồ thị bên dưới kết hợp cả việc đo tốc độ của bánh đà (đơn vị: vòng/s) cũng nhưng là đo góc nghiêng của bánh đà (đơn vị: độ).
Góc nghiêng cân bằng của bánh đà dao động trong khoảng từ -4o đến 4o. Có 3 đường được hiển thị trên đồ thị, trong đó:
Đường màu xanh dương: hiển thị tốc độ của bánh đà
Đường màu đỏ: hiển thị tốc độ của bánh đà đã qua bộ lọc Kalman Đường màu xanh lá: hiển thị góc nghiêng của bánh đà
Hình 3. 47 Đồ thị tốc độ bánh đà ở 130 vòng/s
Ở tốc độ bánh đà là 130 vòng/s, góc nghiêng bánh đà dao động trong khoảng cân bằng 4o đến 4o thì thời gian xe cân bằng đo được là 21s.
Hình 3. 48 Đồ thị tốc độ bánh đà ở 90 vòng/s
Ở tốc độ bánh đà là 130 vòng/s, góc nghiêng bánh đà dao động trong khoảng cân bằng 4o đến 4o thì thời gian xe cân bằng đo được là 10s.
Hình 3. 49 Đồ thị tốc độ bánh đà ở 60 vòng/s
Ở tốc độ bánh đà là 130 vòng/s, góc nghiêng bánh đà dao động trong khoảng cân bằng 4o đến 4o thì thời gian xe cân bằng đo được là 6s.
Hình 3. 50 Đồ thị tốc độ bánh đà ở 30 vòng/s
Ở tốc độ 30 vòng/s xe không thể cân bằng được
Nhận xét:
Khi giữ cho tốc độ bánh đà ổn định tại một giá trị tốc độ thì ở giá trị tốc độ càng cao thì thời gian cân bằng của xe càng lâu. Ở tốc độ bánh đà thấp khoảng 30 vòng/s thì xe không thể giữ thăng bằng. Vì vậy chọn tốc độ bánh đà là 130 vòng/s
Chương 4. KẾT LUẬN VÀ ĐỀ NGHỊ 4.1. Những hạn chế
Xe chỉ đang đáp ứng trên phương diện thăng bằng. Chưa thể vừa di chuyển hay lái vừa cân bằng
Xe cân bằng còn chưa được ổn định.
Phạm vi góc cân bằng của xe nhỏ. Với góc nghiêng ban đầu của xe lớn thì bộ điều khiển còn chưa đáp ứng được.
Xe khá dài khó khăn cho việc cua xe
Các chi tiết thiết kế còn chưa đạt chuẩn để đáp ứng những ý tưởng ban đầu mong muốn.
4.2. Hướng phát triển mô hình
Hướng cho mô hình xe có thể tự cân bằng khi di chuyển và lúc xe vào khúc cua. Hướng phát triển đề lắp thêm một bánh đà giúp xe ổn định đƣợc tốt hơn.
Hướng cho xe cân bằng có thể chạy tự động và né vật cản hay điều khiển từ xa bằng bộ điển khiển…
4.3. Nhận định trong tương lai
Hiện nay tiềm năng phát triển xe cân bằng là rất lớn và đó cũng là xu hướng phát triển các dòng xe hai bánh hiện nay. Trong tương lai dòng xe Bicycle robot sẽ thay thế dòng các dòng xe hiện nay vì thế nghiên cứu về xe cân bằng là đề tài cấp thiết và có giá trị thực tiễn cao. Đề tài sẽ giúp tìm hiểu các phương pháp cân bằng xe và bản chất của con quay hồi chuyển.
Đề tài trình bày các phương pháp điều khiển cân bằng xe hiện nay bằng con quay hồi chuyển (CMG) Con quay hồi chuyển là một thiết bị truyền động trao đổi mô men để cân bằng xe đạp. Con quay hồi chuyển là một thiết bị khuếch đại mô men xoắn hiệu quả và thời gian phản ứng ngắn. Xe đạp, bộ điều khiển và con quay hồi chuyển tạo ra một bộ điều khiển vòng kín
TÀI LIỆU THAM KHẢO
[1]GyroscopicStabilizationof UnstableVehicles: Configurations, Dynamics, andControl [2]StephenC. Spry*andAnouckR. Girard March31, 2008
[3]DESIGN AND DEVELOPMENT OF A SELF-BALANCING BICYCLE USING CONTROL MOMENT GYRO Pom Yuan Lam (B.Eng. (Hons.), NTU)
[4]Ứng dụng thuật toán giảm bậc cho bài toán điều khiển cân bằng xe hai bánh ,ThsVũNgọc Kiên [5]http://cadcamcae.edu.vn/nguyen-ly-con-quay-hoi-chuyen-catia-bkhn [6]cdn.intechopen.com/pdfs/5617.pdf [7]http://gala.gre.ac.uk/11744/1/11744_LeStructure_specified_H_loop_(conf._proceedin gs)_2010.pdf [8]www.umich.edu/~arclab/macccs/media/gyrovehicle.pdf [9]http://www.scholarbank.nus.edu.sg/bitstream/handle/10635/36573/PomYL_v1.pdf?se quence=1 [10]www2.hcmuaf.edu.vn/data/phucnt/file/bo-dieu-khien_pid.pdf(PID) [11]https://vi.wikipedia.org/wiki/Bộ_điều_khiển_PID [12] https://khoere.com/173/dieu-khien-toc-do-dong-co-dung-pid-voi-arduino [13]http://codientu.org/threads/24511/ [14]http://hocdientu.vn/threads/dieu-khien-va-do-toc-do-voi-dong-co-encoder-334- xung.507/ [15] http://arduino.vn/result/5473-robot-hai-banh-tu-can-bang-2-wheel-self-balancing- robot
PHỤ LỤC
Sơ đồ mạch điều khiển
Code thu tín hiệu MPU 6050 /* Phần khai báo
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector; int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal; long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer; boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
/* Chương trình thu tín hiệu MPU6050 thông qua I2C void read_mpu_6050_data() {
Wire.write(0x3B);
Wire.endTransmission(); Wire.requestFrom(0x68, 14); while (Wire.available() < 14);
acc_x = Wire.read() << 8 | Wire.read(); acc_y = Wire.read() << 8 | Wire.read(); acc_z = Wire.read() << 8 | Wire.read(); temperature = Wire.read() << 8 | Wire.read(); gyro_x = Wire.read() << 8 | Wire.read(); gyro_y = Wire.read() << 8 | Wire.read(); gyro_z = Wire.read() << 8 | Wire.read(); }
void setup_mpu_6050_registers() { //Activate the MPU-6050
Wire.beginTransmission(0x68); Wire.write(0x6B); Wire.write(0x00); Wire.endTransmission(); Wire.beginTransmission(0x68); Wire.write(0x1C); Wire.write(0x10); Wire.endTransmission();
//Configure the gyro (500dps full scale) Wire.beginTransmission(0x68);
Wire.write(0x1B); Wire.write(0x08);
Wire.endTransmission(); }
/* Chương trình tính toán và lọc cảm biến MPU6050 void calculate_MPU() {
gyro_x -= gyro_x_cal; gyro_y -= gyro_y_cal; gyro_z -= gyro_z_cal; //0.0000611 = 1 / (250Hz / 65.5) angle_pitch += gyro_x * 0.0000611; angle_roll += gyro_y * 0.0000611;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //Accelerometer angle calculations
acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
if (abs(acc_y) < acc_total_vector) {
angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296; }
if (abs(acc_x) < acc_total_vector) {
angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296; }
angle_pitch_acc -= 0.5; angle_roll_acc -= 0.5; if (set_gyro_angles) {
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; } else { angle_pitch = angle_pitch_acc; angle_roll = angle_roll_acc; set_gyro_angles = true; }
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; }
/* Chương trình set giá trị mong muốn void set() {
for (int i = 0; i < 200; i++) { calculate_MPU();
target_angle += angle_pitch_output; }
target_angle /= 200; }
Code PID tham chiếu thƣ viện PID_v1
PID::PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, int ControllerDirection) { PID::SetOutputLimits(0, 255); SampleTime = 100; PID::SetControllerDirection(ControllerDirection); PID::SetTunings(Kp, Ki, Kd); lastTime = millis()-SampleTime; inAuto = false; myOutput = Output; myInput = Input; mySetpoint = Setpoint; } void PID::Compute() { if(!inAuto) return;
unsigned long now = millis(); int timeChange = (now - lastTime); if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/ double input = *myInput;
double error = *mySetpoint - input; ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax; else if(ITerm < outMin) ITerm= outMin; double dInput = (input - lastInput); /*Compute PID Output*/
double output = kp * error + ITerm- kd * dInput; if(output > outMax) output = outMax;
else if(output < outMin) output = outMin; *myOutput = output;
/*Remember some variables for next time*/ lastInput = input;
lastTime = now; }
}
Code đọc tín hiệu Encoder void encoder() { if (digitalRead(encoder0PinA) == HIGH) { if (digitalRead(encoder0PinB) == LOW) { encoderPos = encoderPos - 1; } else { encoderPos = encoderPos + 1; } } else { if (digitalRead(encoder0PinB) == LOW) {
encoderPos = encoderPos + 1; } else { encoderPos = encoderPos - 1; } } }
Code chƣơng trình chính #include <PinChangeInt.h> #include <PID_v1.h> #define encoder0PinA 2 #define encoder0PinB 4 #include <Wire.h> #define M1 5 #define M2 9 double kp = 20 , ki = 0.02 , kd = 0.42315; double input = 0, output = 0, setpoint = 0; long temp;
float encoderPos = 0;
//--- #include <Wire.h>
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector; int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal; long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer; boolean set_gyro_angles;
float angle_pitch_output, angle_roll_output; float target_angle;
float error, lasterror; float PMW;
//---
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); void setup() {
Wire.begin();
setup_mpu_6050_registers(); digitalWrite(13, HIGH);
for (int cal_int = 0; cal_int < 2000 ; cal_int ++) { read_mpu_6050_data(); gyro_x_cal += gyro_x; gyro_y_cal += gyro_y; gyro_z_cal += gyro_z; delay(3); } gyro_x_cal /= 2000; gyro_y_cal /= 2000; gyro_z_cal /= 2000; set(); digitalWrite(13, LOW); loop_timer = micros(); pinMode(encoder0PinA, INPUT); digitalWrite(encoder0PinA, HIGH); pinMode(encoder0PinB, INPUT); digitalWrite(encoder0PinB, HIGH); attachInterrupt(0, encoder, CHANGE); TCCR1B = TCCR1B & 0b11111000 | 1; myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-255, 255); Serial.begin (115200); } void loop() { calculate_MPU(); // encoder(); // Serial.print(micros()); // Serial.print(" "); setpoint = target_angle; Serial.print(setpoint); input = angle_pitch_output; myPID.Compute(); pwmOut(output); Serial.print(" "); Serial.println(input); // Serial.print(" "); // Serial.print(encoderPos); // Serial.print(" "); // Serial.println(output);
while (micros() - loop_timer < 4000); loop_timer = micros();
}
void pwmOut(int out) { if (out < 0) { analogWrite(M1, 3 * out); analogWrite(M2, 0); } else { analogWrite(M1, 0);
analogWrite(M2, abs(3 * out)); }
}
Code điện thân xe int buttonL = 2; int L = 8; int buttonR = 3; int R = 9; int buttonPHA = 4; int PHA = 10; int buttonCOI = 5; int COI = 11; void setup() { Serial.begin(9600); pinMode(buttonL, INPUT_PULLUP); pinMode(L, OUTPUT); pinMode(buttonR, INPUT_PULLUP); pinMode(R, OUTPUT); pinMode(buttonPHA, INPUT_PULLUP); pinMode(PHA, OUTPUT); pinMode(buttonCOI, INPUT_PULLUP); pinMode(COI, OUTPUT); } //int buttonStatus = 0; void loop() {
int buttonStatusL = digitalRead(buttonL); int buttonStatusR = digitalRead(buttonR); int buttonStatusPHA = digitalRead(buttonPHA); int buttonStatusCOI = digitalRead(buttonCOI); Serial.println(buttonStatusL);
Serial.println(buttonStatusR); Serial.println(buttonStatusPHA); Serial.println(buttonStatusCOI);
delay(200);
if (buttonStatusL == 0) {
digitalWrite(L, LOW); delay(500); digitalWrite(L, HIGH); delay(500); }
else{
digitalWrite(L, HIGH); }
if (buttonStatusR == 0) {
digitalWrite(R, LOW); delay(500); digitalWrite(R, HIGH); delay(500); } else{ digitalWrite(R, HIGH); } if (buttonStatusPHA == 0) { digitalWrite(PHA, LOW); } else{ digitalWrite(PHA, HIGH); } if (buttonStatusCOI == 0) { // turn LED on:
digitalWrite(COI, LOW); } else{ digitalWrite(COI, HIGH); } }
Code hiển thị tốc độ và góc nghiêng bánh đà #include <SimpleKalmanFilter.h> SimpleKalmanFilter bo_loc(1, 1, 0.001); float count = 0; float v=0; int a=0; int goc; float u_kalman; void pulse() {if(digitalRead(2) == LOW) count++; else count--; } void setup() { Serial.begin(9600); pinMode(2 ,INPUT_PULLUP); pinMode(4, INPUT_PULLUP); attachInterrupt(0 ,pulse, FALLING); pinMode(3, INPUT); } void loop() { int ap = analogRead(3); goc=map(ap,0,1023,0,360); a=goc-180; v=count*10/4; count=0; u_kalman = bo_loc.updateEstimate(v); u_kalman = bo_loc.updateEstimate(u_kalman); u_kalman = bo_loc.updateEstimate(u_kalman); u_kalman = bo_loc.updateEstimate(u_kalman);
delay(100); Serial.print(v); Serial.print(","); Serial.print(u_kalman); Serial.print(","); Serial.println(a); }