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

33

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.


34

7.3 Động cơ:
Điện áp hoạt động:

12 [v]

Tốc độ motor

8100 [RPM]

Tốc độ sau giam tốc

120 [RPM]

Tỉ số giảm tốc

64:1

Dòng tải tối đa

1400 [mA]

Dòng không tải

74 [mA]

Momen xoắn

1.72 [Nm]

Độ phân giải Encoder

12 [Xung/vòng]

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ến MPU6050 :
Điện áp hoạt động:

3..5 [v]

Giao tiếp

I2C

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


35

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]

RDS on

0.33[Ω]

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ơ


36

7.6 Bánh xe:

Đường kính bánh xe:

95 [mm]

Đường kính trục:

4 [mm]

Khối lượng tải:

100 [kg]

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

Hình 7.6: Bánh xe


37

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


38

#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);


39

}
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");


40

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;


41

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();
}
}


42

7.9 Lưu đồ giải thuật:
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
Hình 7.8: Lưu đồ giải thuật chương trình


43

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]).