Báo cáo Đồ Án cảm biến cơ cấu chấp hành xe tự cân bằng

Khi hệ xe có xu hướng nghiêng về phía trước ψ > 0, nếu không có điều khiển thì theoquán tính, hai bánh xe sẽ tự động di chuyển ngược về phía sau phía ngược lại của thânrobot đang nghiêng

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH

KHOA CƠ KHÍ CHẾ TẠO MÁY

BÁO CÁO

ĐỒ ÁN CẢM BIẾN CƠ CẤU CHẤP HÀNH

XE TỰ CÂN BẰNG

GVHD: PGS.TS Hà Lê Như Ngọc Thành Nhóm Sinh Viên: 17

Thành viên nhóm:

Bùi Tấn Phong – 21146293 Nguyễn Khánh Duy – 21146200 Lâm Huy Hoàng – 21146229

TP.HCM, tháng 5 năm 2023

CHƯƠNG 1 CƠ SỞ LÝ THUYẾT

1.1 Nguyên lý cân bằng robot

Một Robot có khả năng tự giữ thăng bằng khi di chuyển trên hai bánh xe của nó được gọi

là robot hai bánh tự cân bằng Quá trình cân bằng là quá trình điều khiển hai động cơ bánh

xe sao cho đưa hệ xe về đúng vị trí cân bằng mà ta thiết lập, hay có thể nói cách khác nóduy trì trạng thái ổn định cho hệ thống Hai bánh xe là đối tượng điều khiển chính giúp hệ

xe chuyển động qua lại xung quanh ngưỡng cân bằng và ổn định

Hình 2.1: Nguyên lý hoạt động của hệ xe cân bằng

Khi xe đứng thăng bằng thẳng đứng thì góc nghiêng giữa thân xe và trục thẳng đứng (lựctrong trường) ψ=0

Khi hệ xe có xu hướng nghiêng về phía trước ψ > 0, nếu không có điều khiển thì theoquán tính, hai bánh xe sẽ tự động di chuyển ngược về phía sau (phía ngược lại của thânrobot đang nghiêng) điều đó sẽ làm xe bị đổ Trong tình huống này, chúng ta sẽ điều khiểnlàm sao cho 2 bánh xe chạy về phía trước (phía xe đang nghiêng) nhằm giảm góc lệch ψ =

0, xe sẽ thăng bằng trở lại

Tương tự như vậy, khi xe nghiêng về phía sau ψ < 0, nếu không có điều khiển thì xe sẽ dichuyển thẳng về phía trước (phía ngược lại của thân xe đang nghiêng) làm xe sẽ bị đổ Vìvậy chúng ta sẽ điều khiển sao cho 2 bánh xe chạy về phía sau (phía xe đang nghiêng) nhằmgiảm góc lệch ψ = 0, xe sẽ thăng bằng trở lại

Loại robot này đặt ra nhiều thử thách cho những người nghiên cứu trong điều kiện làmviệc không ổn định đòi hỏi người thực hiện phải lựa chọn giải thuật và đưa ra các thông sốphù hợp Lý thuyết để duy trì sự ổn định này dựa trên lý thuyết điều khiển con lắc ngược làmột đối tượng điều khiển phi tuyến tính kinh điển, mô hình xe cân bằng cũng dựa trên điều

đó mà phát triển lên, có nhiều điểm tương đồng giữa hai mô hình nhưng cả hai đều nhằmmục đích duy trì sự cân bằng và ổn định của hệ thống

Mục đích của nguyên lý con lắc ngược là giữ cho các bánh xe ở ngay dưới trọng tâm củakhung gầm robot Khi khung xe có xung hướng nghiêng về phía trước, hai bánh xe sẽ tựđộng chuyển động về phía trước sao cho góc nghiêng bằng không thì robot sẽ được duy trì

sự cân bằng Mặt khác, nếu khung nghiêng về sau hai bánh xe sẽ di chuyển ngược về phíasau

1.1.1 Mô hình toán học xe hai bánh tự cân bằng

Sinh viên đã tham khảo 2 bài báo [ CITATION Gwo17 \l 1033 ], [ CITATION Ngu \l 1033] và đưa ra mô hình toán học của xe hai bánh tự cân bằng, sơ đồ và hệ của xe hai bánh được

mô tả trong Hình 5 ,6 ,7 và định nghĩa các ký hiệu được thiết kế trong Bảng 1

Hình 2.2: Hình mô tả xe hai bánh tự cân bằng

(a) (b)

Hình 2.3: (a) Xe hai bánh khi nhìn nghiêng (b) Xe hai bánh khi nhìn từ trên xuống

Trong đề tài sẽ sử dụng các ký hiệu như sau:

Bảng 2.1: Ký hiệu và ý nghĩa của các đại lượng

Sử dụng phương pháp Euler-Lagrange để xây dựng mô hình động học Giả sử tại thời điểm t

= 0, robot di chuyển theo chiều dương trục x, ta có góc tịnh tiến trung bình của hai bánh xe

và góc xoay của robot được xác đính như sau:

sin 2 cos 2

m l

l m

W x x

Tọa độ bánh trái trong hệ qui chiếu

sin 2 cos 2

m r

r

m

W x x

Phương trình động năng của chuyển động quay:

Lấy đạo hàm L theo các biến ta được:

[(2 m+M ) R2+2 J w+2 n2J m]´θ+(MLR cosψ−2n2J m)ψ +−MLR ´ψ´ 2sin ψ=F θ 15015\*MERGEFORMAT (.)

Momen động lực do động cơ sinh ra:

F r=n K t i r+f m(ψ−´θ´ r)−f w ´θ r 20020\* MERGEFORMAT (.)

F ψ=−n K t i l−n K t i r−f m(ψ− ´θ´ l)−f m(ψ −´θ´ r) 21021\*MERGEFORMAT (.)

Sử dụng phương pháp PWM để điều khiển động cơ nên chuyển từ dòng điện sang điện áp động cơ

L m ´i l , r=v l ,r+K b(ψ−´θ´ l , r)−R m i l ,r22022\* MERGEFORMAT (.)Xem điện cảm phần ứng tương đối nhỏ (gần bằng 0) có thể bỏ qua, từ đó suy ra:

F ϕ= W

2 R a(v r−v l)− W2

2 R2(β +f w)´ϕ27027\* MERGEFORMAT (.)Thu được phương trình động lực học mô tả chuyển động robot như sau:

Hình 2.4: Sơ đồ khối hệ thống sử dụng bộ điều khiển PID

Công thức bộ điều khiển PID trên miền laplace

T : Thời gian khâu vi phân

1.2.2 Các phương pháp tìm thông số PID (K K K P, I, D)

 Dùng các giải thuật khác như GA, Fuzzy, noron để tìm các thông số PID

1.3 Cảm biến gia tốc góc nghiêng MPU-6050

Cảm biến đo góc nghiêng là một module gồm hai cảm biến đó là:

- Accelerometer: cảm biến đo gia tốc của module (bao gồm cả gia tốc trọng lực), có ba trụcxyz tương ứng với ba chiều không gian

- Gyroscope: cảm biến đo tốc độ quay của module quanh một trục, có ba trục xyz tươngứng với ba chiều không gian.

Khối điều khiển trung tâm sẽ nhận tín hiệu trả về từ hai cảm biến này và sẽ xử lý tính toán

ra được giá trị góc nghiêng của xe

MPU-6050 là một trong những cảm biến chuyển động đầu tiên trên thế giới có tới 6 trục(mở rộng tới 9 trục) cảm biến tích hợp trong 1 chip duy nhất MPU-6050 sử dụng công nghệđộc quyền MotionFusion của InvenSense có thể chạy trên các thiết bị di động, tay điềukhiển…

MPU6050 còn có 1 đơn vị tăng tốc phần cứng chuyên xử lý tín hiệu do cảm biến thu thập vàthực hiện các tính toán cần thiết Điều này giúp giảm bớt đáng kể phần xử lý tính toán của

vi điều khiển cải thiến tốc độ xử lý và cho ra phản hồi nhanh hơn Đây chính là điểm khácbiệt đáng kể của MPU6050 so với các cảm biến gia tốc và gyro khác

MPU-6050 có thể kết hợp với cảm biến từ trường (bên ngoài) để tạo thành bộ cảm biến 9góc đầy đủ thông qua giao tiếp I2C

Các cảm biến bên trong MPU-6050 sử dụng bộ chuyển đổi tương tự – số (Anolog to DigitalConverter – ADC) 16-bit cho ra kết quả chi tiết về góc quay, tọa độ… Với 16-bit sẽ có 2^16

= 65536 giá trị cho 1 cảm biến

Tùy thuộc vào yêu cầu của người thiết kế, cảm biến MPU6050 có thể hoạt động ở chế độtốc độ xử lý cao hoặc chế độ đo góc quay chính xác MPU6050 có khả năng đo ở phạm vi

 Con quay hồi chuyển: 250 500 1000 2000 dps

 Gia tốc: 2 4 8 16 g

Hình 2.5: Cảm biến góc nghiêng MPU6050

Bảng 2.2: Bảng kết nối chân của cảm biến MPU6050

CHƯƠNG 2 THIẾT KẾ PHẦN CỨNG VÀ

THI CÔNG HỆ THỐNG

2.1 Yêu cầu thiết kế

Để thiết kế phần cứng đảm bảo cho xe hoạt động cần lựa chọn các thiết bị phù hợp với cácyêu cầu sau:

 Đảm bảo tổng khổi lượng của xe để động cơ có thể kéo cân bằng được

 Bố trí các thiết bị phù hợp để trọng tâm của xe được đặt ở giữa

 Nguồn cung cấp đủ cho hệ thống hoạt động.

 Khung xe chắc chắn

 Cảm biến trả tín hiệu về tránh tối đa nhiễu và sai số

 Động cơ moment đủ để kéo tải

 Vi điều khiển và các cảm biến, đảm bảo xử lý tín hiệu nhanh

2.2 Thiết bị phần cứng

2.2.1 Danh sách thiết bị sử dụng

 Điện áp giao tiếp: 3-5 VDC

 Chuẩn giao tiếp: I2C

 Giá trị Gyroscope trong khoảng: ±250, ±500, ±1000,

 Điện áp đầu vào: 2-30VDC

 Công suất tối đa: 25W 1 cầu

 Dòng tối đa cho mỗi cầu H: 2A

 Mức điện áp logic: Low -0.3V

 Dung lượng: 2000mAh

 Điện sạc đầy: 4.2V

Bảng 3.3: Bảng thiết bị sử dụng

2.2.2 Sơ đồ kết nối phần cứng

Hình 3.6: Sơ đồ kết nối phần cứng

Chân A4 Input SDA cảm biến MPU Giao tiếp chuẩn

I2C

Bảng 3.4: Bảng chú thích chân và chức năng

2.3 Thiết kế phần cứng cho xe hai bánh

2.4 Lưu đồ giải thuật

Hình 3.9: Thuật toán chương trình chính

Hình 3.10: (a) Mặt sau của xe (b) Mặt trước của xe

Hình 3.11: Xe hai bánh khi nhìn nghiêng

Thông số của mô hình

CHƯƠNG 3 KẾT QUẢ THỰC TẾ

3.1 Thiết kế bộ điều khiển PID và kiếm chứng trên Arduino IDE

Thử sai nhiều lần với các thông số K P, K i, K d của bộ điều khiển PID kết hợp quan sát tín hiệu góc nghiêng của thân xe để tìm ra các thông số tối ưu của bộ điều khiển PID

TH1: với các thông số như bảng 4.1

Hình 4.12: Đáp ứng tín hiệu góc nghiêng với thông số PID TH1

Nhận xét: Với các thông số của bộ PID ở TH1 xe dao đông rất mạnh không thể cân bằng

TH2: với các thông số như bảng 4.2

Hình 4.13: Đáp ứng góc nghiêng với thông số bộ điều khiển PID TH2

Nhận xét: Với thông số của bộ PID TH2 thì sẽ dao động ít hơn nhưng vẫn không thể cân bằng

TH3: với các thông số như bảng 4.3

Hình 4.14: Đáp ứng góc nghiêng với thông số bộ điều khiển PID TH3

Nhận xét: Với thông số của bộ PID TH3 tín hiệu góc nghiêng thân xe đã cso thể bám vào tínhiệu đặt, nhưng vẫn còn dao động, nhưng xe vẫn không thể cân bằng

TH4: với các thông số như bảng 4.4

Hình 4.15: Đáp ứng góc nghiêng với thông số bộ điều khiển PID TH4

Nhận xét: Với thông số của bộ PID TH4 xe đã có thể cân bằng, nhưng vẫn còn dao độngnhỏ, thời gian xác lập khoảng 2s, sai số xác lập khoảng 0.8

CHƯƠNG 4 KẾT LUẬN

4.1 Nhận xét

Với bộ điều khiển PID cho hệ xe hai bánh tự cân bằng cho ra đáp ứng góc nghiêng chấpnhận được với sai số cũng như thời gian xác lập thấp, tuy nhiên vẫn không thể so sánh chấtlượng đối với các bộ điều khiển khác như LQR hay Fuzzy

4.2 Kết quả

 Tìm được phương trình toán học của hệ xe hai bánh tự cân bằng

 Thiết kế, tính toán lựa chọn các thiết bị phù hợp với mô hình

 Sử dụng bộ lọc Kalman để lọc nhiễu cho tính hiệu góc nghiêng

 Xây dựng được bộ điều khiển PID để cân bằng xe

// MPU control/status vars

success, !0 = error)

// orientation/motion vars

double Kp = 60 ; //60 Present 28 Perfect values as on now: 28, 2.2,

has gone high

pinMode ( ENA, OUTPUT ) ; /// out

pinMode ( IN1, OUTPUT ) ;

pinMode ( IN2, OUTPUT ) ;

pinMode ( IN3, OUTPUT ) ;

pinMode ( IN4, OUTPUT ) ;

pinMode ( ENB, OUTPUT ) ;

// pinMode(led, OUTPUT);

//digitalWrite(led,0);

Serial begin ( 9600 ) ;

// join I2C bus (I2Cdev library doesn't do this automatically)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire begin () ;

Fastwire:: setup ( 400 , true ) ;

// supply your own gyro offsets here, scaled for min sensitivity

mpu setXGyroOffset ( 220 )

mpu setYGyroOffset ( 76 ) ;

mpu setZGyroOffset ( 85 )

mpu setZAccelOffset ( 1788 ) // 1688 factory default for my test chip

// make sure it worked (returns 0 if so)

{

// turn on the DMP, now that it's ready

mpu setDMPEnabled ( true )

// enable Arduino interrupt detection

attachInterrupt ( 0 , dmpDataReady, RISING )

// set our DMP Ready flag so the main loop() function knows it's okay to use it

// get expected DMP packet size for later comparison

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial print ( ( "DMP Initialization failed (code " )) ;

Serial print ( devStatus )

// wait for MPU interrupt or extra packet(s) available

while ( !mpuInterrupt && fifoCount < packetSize )

// get current FIFO count

// check for overflow (this should never happen unless our code is too inefficient)

{

// reset so we can continue cleanly

mpu resetFIFO () ;

Serial println ( ( "FIFO overflow!" )) ;

// otherwise, check for DMP data ready interrupt (this should happen frequently)

while ( fifoCount < packetSize ) fifoCount = mpu getFIFOCount () ;

// read a packet from FIFO

mpu getFIFOBytes ( fifoBuffer, packetSize )

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= packetSize;

mpu dmpGetQuaternion ( &q, fifoBuffer ) ;

mpu dmpGetGravity ( &gravity, &q )

mpu dmpGetYawPitchRoll ( ypr, &q, &gravity ) ;

#if LOG_INPUT

Serial print ( "ypr\t" )

Serial print ( ypr [ ] * 180 /M_PI )