Thiết kế bộ điều khiển phi tuyến cho xe hai bánh tự cân bằng

7.9 Lưu đồ giải thuật: Hình 7.8: Lưu đồ giải thuật chương trình Start Khởi tạo các biến; Cài đặt timer, interrupt; Cập nhật Timer dT; Đọc góc nghiêng θ, δ Kalman filter Cθ = SMC Cδ = Po

Chương 7 TRIỂN KHAI MÔ HÌNH

7.1 Sơ đồ tổng quát của hệ thống:

Hình 7.1: Sơ đồ tổng quát

7.2 Sơ đồ mạch điện:

Hình 7.2: Sơ đồ nguyên lý mạch điện

7.3 Động cơ:

Bảng 7.1: Các thông số kỹ thuật của động cơ

Hình 7.3: Động cơ sử dụng trong mô hình

7.4 Cảm biếnMPU6050 :

Giải đo Gyroscopes ±250, 500, 1000, 2000 [Deg/s] Giải đo Acceleration ± 2, 4, 8, 16 [g]

Bảng 7.2: Các thông số kỹ thuật của cảm biến vận tốc góc và gia tốc góc

Hình 7.4: Modul cảm biến vận tốc góc và gia tốc góc

7.5 Modul khiển động cơ LMD18200:

Điện áp hoạt động Vmax: 55 [v]

Dòng điện làm việc Imax 3 [A]

Bảng 7.3: Các thông số kỹ thuật của Modul điều khiển động cơ

Hình 7.5: Modul điều khiển động cơ

7.6 Bánh xe:

Bảng 7.4: Các thông số kỹ thuật của bánh xe

Hình 7.6: Bánh xe

7.7 Mô hình thực tế:

Hình 7.7: Mô hình thực tế

7.8 Phần mềm:

#include <master.h>

#include <math.h>

#include <mpu6050.h>

#include <mpu6050.c>

#include <slave_io.c>

#include <motor.c>

#include <kalman.c>

#define g 9.81

#define L 0.13

#define MW 0.19933

#define R 0.048

#define MB (0.83053 + 0.620)

#define D 0.21

#define k1 20

#define KS 350

float f1(float x1)

{

float temp1, temp2;

temp1 = ((-0.75 * g * sin(x1))/L);

temp2 = (((0.75 * cos(x1) * ((Mw * R) + (MB * L * cos(x1)))) / (((2 * Mw) + MB) * L)) - 1);

return(temp1/temp2);

}

float f2(float x1,x2)

{

float temp1, temp2;

temp1 = (( 0.75 * MB * sin(x1) * cos(x1) * x2 * x2)/ ((2* Mw)+ MB));

temp2 = (((0.75* cos(x1)* ((Mw* R)+ (MB* L* cos(x1)))) / (((2* Mw) +

MB)* L))- 1);

return(temp1/temp2);

}

float g1(float x1)

{

float temp1, temp2;

temp1 = (((0.75 * (1 + (sin(x1) * sin(x1)))) / (MB * L * L)) + ((0.75 *

cos(x1))/(((2 * Mw) + MB) * R * L)));

temp2 = (((0.75* cos(x1)* ((Mw* R)+ (MB* L* cos(x1)))) / (((2* Mw) +

MB)* L))- 1);

return(temp1/temp2);

}

float SMC(float x1,x2, ref)

{

float temp1, temp2, err,s,u ,r_dotdot;

err = ref - x1;

temp1 = (err - err_old)/d_t ; // e_dot

temp2 = (ref - r_old)/d_t; //r_dot

r_dotdot = (temp2 - r_dotdot_old)/d_t;

s = (k1*err)+temp1;

if (s > 0.5) s = 0.5;

else

if(s < -0.5) s = -0.5;

s = s * KS;

u = ((k1*temp1) + s + r_dotdot - f1(x1) - f2(x1,x2) )/g1(x1); r_dotdot_old = r_dotdot;

err_old = temp1;

r_old = ref;

return(u);

}

float pole_palacement(float ek)

{

float uk = (0.0158224*ek)-(0.01276*ek_1)+(0.12*uk_1); uk_1 = uk;

ek_1 = ek;

return(uk);

}

Void main(void)

{

printf("Startup \r\n");

set_pullup(TRUE,PIN_B4); //QEA

set_pullup(TRUE,PIN_B5); //QEB

setup_qei(QEI_MODE_X4|QEI_TIMER_INTERNAL,QEI_FILTER_DIV_16,Q EI_FORWARD); //| QEI_SWAP_AB

control_slave(QEI_4);

qei_set_count(0x0000);

control_slave(reset);

//init motor

printf("Init motor modul \r\n");

motor_init();

printf("Init MPU6050 modul ");

if(InitMpu6050() != 0x68)

{printf("Sensor not correct\r\n");

while(true) restart_wdt();

}

else

printf("Ok\r\n");

printf("Init Kalman \r\n");

init_kalman();

printf("Go \r\n");

delay_ms(500);

enable_interrupts(INT_RDA);

enable_interrupts(INTR_GLOBAL);

TICK_TYPE CurrentTick,PreviousTick;

unsigned int8 on_off = on;

float datadeg[10], deg_sum = 0;

unsigned int8 i;

for(i = 0;i<10;i++)

Datadeg[i]=0;

while(true)

{

CurrentTick = get_ticks();

int32 Dif = CurrentTick - PreviousTick;

PreviousTick = CurrentTick;

if (Dif < 0) Dif = Dif + 0xFFFFFFFF;

d_t = ((float)(Dif)*85.2) / 1000000.0; //1 stick = 85.2uS convert to second update_deg(); //read angle, kalman filter

deg_sum = (deg_sum - datadeg[i]) + Angle;

datadeg[i] = Angle;

if(++i == 10) i = 0;

Angle = deg_sum/10;

c_theta = SMC((Angle )*DEG_TO_RAD,rate, 0);

c_delta = pole_palacement(delta_ref - delta);

c_right = (c_theta + c_delta)/2;

c_left = (c_theta - c_delta)/2;

if(c_left > 0)

{left_forward; pwm_l = (unsigned int16)calcutlate_pwm(c_left);} else

{left_revert; pwm_l = (unsigned int16) calcutlate_pwm(-c_left);} if(c_right > 0)

{ right_forward; pwm_r = (unsigned int16) calcutlate_pwm(c_right);} else

{ right_revert; pwm_r = (unsigned int16) calcutlate_pwm(-c_right);} set_motor_pwm_duty(1,motor_left,pwm_l);

set_motor_pwm_duty(1,motor_right,pwm_r);

restart_wdt();

}

}

7.9 Lưu đồ giải thuật:

Hình 7.8: Lưu đồ giải thuật chương trình

Start

Khởi tạo các biến;

Cài đặt timer, interrupt;

Cập nhật Timer (dT);

Đọc góc nghiêng θ, δ

Kalman filter

Cθ = SMC()

Cδ = Pole()

Decoupling CLeft, CRight

Read ADC()

Calculate PWM

Motor control

7.10 Kết quả thực hiện mô hình:

Mô hình hoạt động dưới sự điều khiển bằng tín hiệu RF theo giao thức truyền nối tiếp RS232 Mô hình đã đáp ứng được các thao tác cần thiết như di chuyển về phía trước hoặc ngược lại, chuyển hướng rẽ trái, phải theo người điều khiển Tuy nhiên hiện, tượng dao động (chattering) vẫn còn xảy ra khi xe đứng yên ở vị trí cân bằng( θ = 0 [Rad])