Thiết kế xe hai bánh tự cân bằng

1.4 Đối tượng nghiên cứu  Xây dựng phần cứng robot Thiết kế phần cứng, làm mạch điều khiển, mạch nguồn,  Tìm hiểu giải thuật lọc kalman, lọc bổ phụ để xử lý dữ liệu thu thập được  Tìm

ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH

TRƯỜNG ĐẠI HỌC BÁCH KHOA

KHOA ĐIỆN – ĐIỆN TỬ -o0o -

ĐỒ ÁN : THIẾT KẾ XE HAI BÁNH TỰ CÂN BẰNG

Nhóm:

Tp Hồ Chí Minh, Tháng 06/2019

CHƯƠNG 1 GIỚI THIỆU 1

1.1 Giới thiệu 1

1.2 Thế nào là xe hai bánh tự cân bằng (two wheels self balancing) 2

1.3 Mục tiêu đề tài 3

1.4 Đối tượng nghiên cứu 4

1.5 Phạm vi nghiên cứu 5

CHƯƠNG 2 XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN 1

2.1 Các giải thuật xử lí tín hiệu từ cảm biến 1

2.1.1 Lọc bổ phụ thông tần (complementary filter)

2.1.2 Lọc Kalman

2.1.2.1 Lý thuyết về bộ lọc Kalman

2.1.2.2 Quy trình ước lượng

2.1.2.3 Bản chất toán học của bộ lọc

2.1.2.4 Bản chất xác suất của bộ lọc

2.1.2.5 Giải thuật lọc Kalman rời rạc

2.1.2.6 Bộ lọc Kalman rời rạc đơn giản dùng cho phạm vi đồ án này

2.1.3 So sánh bộ lọc bổ phụ và bộ lọc Kalman rời rạc

2.2 Các giải thuật điều khiển hiện đại 2

2.2.1 Phương pháp điều khiển PID

2.2.2 Thiết kế bộ điều khiển PID Cascade

2.2.2.1 Quy trình thiết kế bộ điều khiển PID nối tiếp

2.2.2.2 Phương pháp điều chỉnh thủ công PID

2.3 Mô hinh lý thuyết động cơ DC 3

CHƯƠNG 3 THỰC HIỆN 1

3.1 Sơ đồ khối của hệ thống

3.2 Thiết kế cơ khí

3.2.1 Tóm tắt thiết kế

3.2.2 Thiết kế mô hình

3.3 Mạch điện tử 3

3.3.1 Nguồn điện 4

3.3.2 Mạch công suất điều khiển động cơ 4

3.3.3 Động cơ DC Servo Giảm tốc GA37

3.3.4 Vi điều khiển stm32f4

3.3.5 Gia tốc kế MPU-6050 GY-521

3.4 Các phương thức giao tiếp 5

3.4.1 Điều chế độ rộng xung PWM

3.4.2 Giao thức I2C

3.4.3 Giao tiếp UART

CHƯƠNG 4 KẾT LUẬN 1

4.1 Những kết quả đạt được

4.2 Hướng phát triển

1.2 Thế nào là xe hai bánh tự cân bằng (two wheel self balancing)

Đối với các xe ba hay bốn bánh, việc thăng bằng và giữ ổn định của chúng nhờ trọng tâm của chúng nằm trên bề mặt chân đế do các bánh xe tạo ra Đối với các xehai bánh có cấu trúc như xe đạp, việc thăng bằng khi di chuyển / đứng yên là điều hoàn toàn không thể, vì nguyên lý thăng bằng dựa trên tính chất con quay hồi chuyển ở hai bánh xe khi đang quay Còn đối với xe hai bánh tự cân bằng, là loại

xe chỉ có hai bánh với trục của hai bánh xe trùng nhau, để xe cân bằng, trọng tâm của xe (bao gồm cả người sử dụng) cần được giữ nằm ngay giữa các bánh xe Giống như ta giữ một cây gậy thẳng đứng cân bằng trong lòng bàn tay

1.3 Mục tiêu đề tài

Mục tiêu của đề tài là xây dựng mô hình robot hai bánh thực tế có khả năng tự cân bằng với cấu trúc đơn giản phục vụ cho việc nghiên cứu, giáo dục, giảng dạy tại phòng thí nghiệm, mô hình sản phẩm trưng bày hay những trò chơi với mục đích biểu diễn

1.4 Đối tượng nghiên cứu

 Xây dựng phần cứng robot

Thiết kế phần cứng, làm mạch điều khiển, mạch nguồn,

 Tìm hiểu giải thuật lọc kalman, lọc bổ phụ để xử lý dữ liệu thu thập được

 Tìm hiểu giải thuật để điều khiển cho một robot 2 bánh tự cân bằng

Sử dụng bộ điều khiẻn PID gồm 2 vòng điều khiển, vòng ngoài điều khiển góc, phần trong điều khiển tốc độ động cơ

Đó là những câu hỏi mà luận văn muốn hướng đến giải quyết các vấn đề thắc mắc trên

Trong phạm vi đề tài này, chúng em thiết kế để xe có thể tự đứng cân bằng được Mục tiêu là thực hiện giải thuật điều khiển PID trên đối tượng xe hai bánh cân bằng,

có kết quả thực tế được kiểm chứng

Chương 2

XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN

2.1 Các giải thuật xử lí tín hiệu từ cảm biến

Như đã biết, giá trị ngõ ra được quan tâm hang đầu của xe scooter hai bánh chính

là góc giữa tay lái thẳng đứng hoăc sàn xe với chiều trọng lực Nhiều loại cảm biến có thể dùng đo góc như encoder, resolver, inclinometer,… với trọng lực theo phương pháp không tiếp xúc với sàn là cảm biến đo vận tốc góc và cảm biến gia tốc ứng dụng

để đo góc tĩnh (cảm biến đo khoảng cách tới mặt đất bằng hồng ngoại chỉ dùng được với sàn đất phẳng, nằm ngang và láng) Để làm trơn nhiễu và kết hợp tín hiệu từ hai camr biến accelerometer và gyro, người ta thường dùng các bộ lọc trung bình, lọc bổ phụ thông tần complementary, lọc thích nghi – bộ lọc Kalman và các bộ lọc khác

2.1.1 Lọc bổ phụ thông tần (complementary filter)

Bộ lọc bổ phụ được sử dụng trong trường hợp khi có hai nguồn giá trị khác nhau trong việc ước lượng một giá trị, và đặc tính nhiễu của hai giá trị chẳng hạn một nguồn mang lại thông tin đúng trong vùng tần số thấp trong khi nguồn khác chỉ đúng trong vùng tần số cao Bộ lọc bổ phụ là kết hợp những ngõ ra của độ nghiêng và vận tốc gyro nhằm thu được khả năng ước lượng tốt nhất của sự định hướng, để đền bù cho sự trôi giá trị gyro (drift) và cho đáp ứng chậm của cảm biến đo nghiêng

Hình 2.1 Sơ đồ nguyên lý hoạt động của bộ lọc bổ phụ trên mô hình con lắc ngược

 Ước lượng góc được thu nhận là tổng của tín hiệu từ hai nhánh giá trị được biểudiễn ở hình 3.6 Góc nghiêng của cảm biến đo nghiêng được đưa vào bộ lọcGi(s) phân bổ giá trị góc nghiêng để ước lượng chỉ trong vùng tần số thấp Vận

tốc góc của gyro cung cấp vào bộ lọc Gg(s) Bộ lọc này được thiết kế để ngõ raphân bổ đến ước lượng chỉ trong vùng tần số cao Yêu cầu cơ bản của bộ lọc cóthể được tính như sau:

 Toàn bộ hệ thống có sự khuếch đại là hằng số và phase tổn thất nhỏ nhất ngay vùng tần số cắt của cảm biến nghiêng

 Để giữ độ nhạy offset của gyro là nhỏ nhất, cảm biến nghiêng nên được dùng trong vùng băng thông tần số rộng nhất có thể

 Số lượng tham số thiết kế nên nhỏ để dễ cho sự điều chỉnh và áp dụng bộ lọctrong thực tế của các vi điều khiển

2.1.2 Lọc Kalman

2.1.2.1 Lý thuyết về bộ lọc Kalman

Được đề xuất từ năm 1960 bởi giáo sư Kalman để thu thập và kết hợplinh động các thông tin từ cảm biến thành phần Một khi phương trình địnhhướng và mẫu thống kê nhiễu trên mỗi cảm biến được biết và xác định, bộlọc Kalman sẽ cho ước lượng giá trị tối ưu (chính xác do đã được loại sai số,nhiễu) như là đang sử dụng một tín hiệu ‘tinh khiết’ và có độ phân bổ khôngđổi Trong hệ thống này, tín hiệu cảm biến vào bộ lọc gồm hai tín hiệu: từcảm biến góc (inclinometer) và cảm biến vận tốc góc (gyro) Tín hiệu ngõ racủa bộ lọc là tín hiệu của inclinometer và gyro đã được loại nhiễu nhờ hainguồn tín hiệu hỗ trợ và xử lý lẫn nhau trong bộ lọc, thông qua quan hệ (vậntốc góc = đạo hàm/vi phân của giá trị góc)

 Một ví dụ mô phỏng về bộ lọc Kalman cho một máy bay chiến đấu bằngMatLAB :

Hình 2.2 Tín hiệu thu nhận chưa được lọc

Hình 2.3 Tín hiệu đã qua bộ lọc Kalman

Bộ lọc Kalman đơn giản là thuật toán xử lý dữ liệu hồi quy tối ưu Cónhiều cách xác định tối ưu, phụ thuộc tiêu chuẩn lựa chọn trình thông sốđánh giá Nó cho thấy rằng bộ lọc Kalman tối ưu đối với chi tiết cụ thể trongbất kỳ tiêu chuẩn có nghĩa nào Một khía cạnh của sự tối ưu này là bộ lọcKalman hợp nhất tất cả thông tin được cung cấp tới nó Nó xử lý tất cả giátrị sẵn có, ngoại trừ độ sai số, ước lượng giá trị hiện thời của những giá trịquan tâm, với cách sử dụng hiểu biết động học thiết bị giá trị và hệ thống,

mô tả số liệu thống kê của hệ thống nhiễu, gồm nhiễu ồn, nhiễu đo và sự

không chắc chắn trong mô hình động học, và những thông tin bất kỳ về điềukiện ban đầu của giá trị quan tâm.

2.1.2.2 Quy trình ước lượng

Kalman filter định vị vấn đề chung nhằm ước lượng giá trị x thuộc Rn

của tiến trình kiểm soát thời gian gián đoạn biểu diễn bằng phương trìnhtuyến stochastic khác nhau:

[2-3]

[2-4]

Trong thực tiễn, ma trận tương quan tiến trình nhiễu Q và tương quan

đo độ nhiễu R có lẽ thay đổi với mỗi bước thời gian hay giá trị, tuy nhiêngiả thuyết rằng ở đây chúng là hằng số

Ma trận Anxn trong phương trình [2-1] liên hệ với giá trị ở thời điểmtrước k-1 đến giá trị ở thời điểm hiện tại k, trong sự thiếu hụt không chỉ ởhàm lái dạng mà còn do nhiễu tiến trình Chú ý rằng trong thực tế A có lẽthay đổi tại mỗi thời điểm, nhưng ở đây, ta xem nó là hằng số Ma trận Bnx1

liên quan hoạt động kiểm soát tùy ý u thuộc Rl đối với x Ma trận Hmxn liênquan với giá trị đo zk Trong thực tế, H có thể thay đổi tại mỗi thời điểm haygiá trị, nhưng ta xem nó là hằng số

2.1.2.3 Bản chất toán học của bộ lọc

Theo định nghĩa xˆk  n (siêu âm) ước lượng trạng thái priori tạibước k đưa ra tiến trình priori tại bước k, và xˆk  n là ước lượng trạng

thái posteriori tại bước k với giá trị zk Từ đó có thể xác định sai số ướclượng priori và posteriori là:

[2-8]

Độ lệch (zk – H xˆ - k )được gọi là đại lượng cách tân hay giá trị thặng dư.Giá trị thặng dư phản ánh sự khác nhau giữa giá trị chuẩn tắc H xˆ - k vàgiá trị thực zk Giá trị thặng dư A của 0 nghĩa là hai thành phần này hoàntoàn nhất quán nhau

Ma trận K nxm trong công thức được chọn là độ lợi hay hệ số pha trộntối thiểu hóa tương quan sai số posteriori Một dạng kết quả K tối thiểuhóa được trình bày như sau:

[2-9]

Thấy rằng tương quan sai số giá trị R tiến tới 0, độ lợi K đánh giá giá trịthặng dư cao hơn Đặc biệt:

Ước lượng trạng thái posteriori [2-8] phản ánh trung bình (khoảng phân

bố thời gian đầu tiên) của sự phân bố trạng thái – thông thường nó đượcsắp xếp nếu các điều kiện của [2-3] và [2-4] xảy ra cùng lúc Tương quansai số ước lượng posteriori [2-7] phản ánh giá trị của sự phân bố trạng thái(khoảng phân bố thời gian thứ hai) Nói cách khác:

[2-13]

2.1.2.5 Giải thuật lọc Kalman rời rạc

Bộ lọc Kalman ước lượng tiến trình bằng việc sử dụng hình thức kiểmsoát phản hồi: bộ lọc ước lượng trạng thái tiến trình tại vài thời điểm và sau

đó thu sự phản hồi trong hình thức của giá trị (độ nhiễu) Chẳng hạn, phương

trình của bộ lọc Kalman chia thành hai nhóm: phương trình cập nhật thờigian và phương trình cập nhật giá trị Phương trình cập nhật thời gian chịutrách nhiệm dự đoán trước (thời gian) giá trị hiện tại và tương quan sai sốước lượng để đạt ước lượng priori cho thời điểm kế tiếp Phương trình cậpnhật giá trị chịu trách nhiệm đối với sự phản hồi – nghĩa là kết hợp giá trịmới vào ước lượng priori nhằm đạt sự cải tiến ước lượng posteriori

Phương trình cập nhật thời gian có thể xem là phương trình chuẩn tắc,trong khi phương trình cập nhật giá trị có thể xem là phương trình chính xác.Thật vậy, thuật toán ước lượng cuối cùng giống như thuật toán chính xác –chuẩn tắc nhằm giải quyết vấn đề số liệu được trình bày ở hình 4.4

Hình 2.4 Chu kỳ lọc Kalman rời rạc

 Cụ thể hơn

Hình 2.5 Chu kỳ lọc Kalman rời rạc

Phương trình cập nhật thời gian bộ lọc Kalman rời rạc:

[4-14]

Phương trình cập nhật giá trị cho bộ lọc Kalman rời rạc:

[4-15]

 Trong sơ đồ trên:

 x̂𝑘 là ký hiệu ước lượng dự đoán của trạng thái xk dựa trên quan sát ở thời điểm k-1

 x̂𝑘 là ký hiệu ước lượng dự đoán của trạng thái 𝑥𝑘 dựa trên quan sát ở thời điểm k

 Hiệp phương sai ước lượng dự đoán Pk là trung bình phương sai số trong ước lượng x̂𝑘

 Nhìn vào sơ đồ tóm tắt các phương trình của bộ lọc Kalman ta có thể thấy mụcđích chính của chúng ta là đi tìm hệ số K (độ lợi của bộ lọc Kalman, hệ số này

sẽ thay đổi sau mỗi vòng lặp tính toán) Tức là làm sao đưa ra được ước lượng trạng thái của hệ thống tại thời điểm k là sát nhất với trạng thái của hệ thống nhất

 Sau mỗi cặp cập nhật thời gian và giá trị, tiến trình được lập lại như trước ước lượng Posteriori đã từng dự đoán hay tiên đoán ước lượng Priori mới Trạng thái đệ quy tự nhiên là một trong nhiều điềm đặc thu hút của bộ lọc ra hoạt động của tất cả dữ liệu một cách trực tiếp cho mỗi chu kỳ ước lượng Bộ lọc Kalman thay thế điều kiện đệ quy ước lượng hiện tại cho những giá trị quá khứ

2.1.2.6 Bộ lọc Kalman rời rạc đơn giản dùng cho phạm vi đồ án này

Cảm biến góc được thiết kế bằng cách kết hợp một cảm biến gia tốc

mộ trục và một gyro vận tốc một trục tích hợp trên MPU6050 Bộ lọc

Kalman gồm hai trạng thái, với trạng thái là góc và trạng thái còn lại là giá trị

cơ sở Bộ lọc Kalman là một vấn đề thực sự phức tạp, mặc dù đã được tối ưunhiều lần đoạn code C

Trước tiên thiết lập các ma trận và các vector cần thiết cho bộ lọc

Kalman:

Góc và vận tốc được mô tả bởi không gian trạng thái tuyến tính

[2-16]

 Trong đó:

 x là giá trị góc (ở đây chúng ta quan tâm góc pitch của xe cân bằng)

 ẋ là vận tóc góc (theo phương nghiêng của góc pitch)

 Lưu ý: x và ẋ là góc xoay và vận tốc góc của một trục đang xét Hay nói mộtcách đơn giản là ta đang thiết kế bộ lọc Kalman với hai đầu vào và một đầu ra

Trạng thái của hệ thống tại thời điểm k được phát triển từ trạng thái của

hệ thống tại thời điểm (k-1):

Ma trận hiệp phương sai:

 Áp dụng phương trình toán học của bộ lọc Kalman:

 Bước 1: Trạng thái ước đoán:

[2-22]

 Bước 2: Dự đoán, ước lượng hiệp phương sai:

[2-23]

 Bước 3: Độ lệch đo lường

[2-24]

 Hiểu một cách đơn giản hơn:

Độ lệch đo lường = giá trị trạng thái đo được- giá trị dự đoán sẽ được đo trước đó

 Bước 4: Độ lệch phương sai

[2-25]

 Bước 5: Tính độ lợi Kalman

2.1.2.7 Thực hiện bộ lọc Kalman rời rạc cho mpu6050 và stm32f4

 Khai báo biến cấu trúc Kalman

Hình 2.6 Code các thông số cần thiết cho bộ lọc kalman

 Hàm lọc kalman, trả về góc được lọc

Hình 2.7 Code chu kỳ lọc Kalman rời rạc

2.1.3 So sánh bộ lọc bổ phụ và bộ lọc Kalman rời rạc

Hình 2.8 Lọc Kalman (tím) và lọc Complementary (đỏ) đo thực nghiệm

2.2 Các giải thuật điều khiển hiện đại.

2.2.1 Phương pháp điều khiển PID.

Hình 2.9 Giải thuật PID cổ điển

Sơ đồ điều khiển PID được đặt tên theo ba khâu hiệu chỉnh của nó, tổng của ba khâu này tạo thành bởi các biến điều khiển MV Ta có:

U (t )=P out+I out+D out

Khâu tỉ lệ được cho bởi:

P out=Kp∗e (t)

 Trong đó:

 Pout: thừ số tỉ lệ của đầu ra

 Kp: Độ lợi tỉ lệ, thông số điều chỉnh

 e: sai số e= SP-PV (set point và process variable)

 t: thời gian hay thời gian tức thời (hiện tại)

Hình 2.10 Đồ thị PV theo thời gian với Kp thử nghiệm(Ki, Kd là hằng số)

 Khâu tích phân:

Phân phối của khâu tích phân (đôi khi còn gọi là reset) tỉ lệ thuận với cả biên độsai số lẫn quãng thời gian xảy ra sự cố Tổng sai số tức thời theo thời gian (tích phân sai số) cho ta tích lũy bù đã được hiệu chỉnh trước đó Tích lũy sai số sau đí được nhân với độ lợi tích phân và cộng với tín hiệu đầu ra của bộ điều khiển Biên

độ phân phối của khâu tích phân trên tất cả tác động điều chỉnh được xác định bởi

độ lợi tích phân Ki

Thừa số tích phân cho bởi

 Iout: thừa số tích phân của đầu ra

 Ki: độ lợi tích phân, 1 thông số điều chỉnh

 e: sai số e= SP-PV

 t: thời gian hoặc thời gian tức thời

 𝜏: mộ biến tích phân trung gian

Hình 2.11 Đồ thị PV theo thời gian với Ki thử nghiệm(Kp, Kd là hằng số)

 Khâu vi phân:

Tốc độ thay đổi của sai số quá trình được tính toán bằng cách xác định độ dốc của sai số theo thời gian (tức là đạo hàm bậc một theo thờigian) và nhân tốc độ nàyvới độ lợi tỉ lệ Kd Biên độ của phân phối khâu vi phân (đôi khi còn gọi là tốc độ) trên tất cả các hành vi điều khiển được giới hạn bởi độ lợi vi phân, Kd.

Thừa số vi phân được cho bởi:

Giá trị càng lớn thì đáp ứng càng nhanh, do đó sai đố càng lớn, bùkhâu tỉ lệ càng lớn Một giá trị độ lợi tỉ lệ quá lớn sẽ dẫn đến quá trình mất ổnđịnh và dao động.

 Độ lợi tích phân Ki

Giá trị càng lớn, kéo theo sai số ổn định bị khử càng nhanh Đổi lại là

độ vọt lố càng lớn, bất kì sai số âm nào được tích phân trong suốt đáp ứngquá độ phải triệu tiêu tích phân bằng sai số dương trước khi tiến tới trạngthái ổn định

 Độ lợi vi phân Kd

Giá trị càng lớn càng giảm độ vọt lố, nhưng làm chậm đáp ứng quá độ

và có thể dẫn đến mất ổn định do khuếch đại nhiễu tín hiệu trong phép viphân sai số

2.2.2 Thiết kế bộ điều khiển PID Cascade

Bộ điều khiển PID gồm 2 bộ: một bộ kiểm soát vận tốc của xe với đầu vào là vận tốc đặt cho xe, bộ PID còn lại dùng để kiểm soát góc nghiêng của xe so với vị trí thẳng đứng Đầu ra của bộ điều khiển PID vận tốc là đầu vào của bộ PID góc

2.2.2.1 Quy trình thiết kế bộ điều khiển PID nối tiếp:

Đầu tiên, ta tiến hành thực hiện bộ điều khiển PID đơn giám sát góc nghiêng Tín hiệu đầu vào là góc đặt mà tại đó xe đứng cân bằng Tín hiệu từ cảm biến góc nghiêng MPU6050 qua bộ lọc Kalman sẽ được so sánh với góc đặt để đưa vào bộ PID Ngõ ra bộ điều khiển PID chính là giá trị xung PWM và được đưa vào mạch cầu

H để lái hai động cơ Nếu góc nghiêng càng lớn thì ngõ ra PID càng lớn như vậy momen quay của động cơ cũng mạnh để kịp thời đưa xe về vị trí cân bằng Một vấn đềđặt ra là hướng điều khiển động cơ Giả sử xe đang bị nghiêng về phía trước, khi đó

có thể coi giá trị ngõ ra PID>0, dựa vào đó ta sẽ xuất hiện hướng cho động cơ quay vềphía trước để cân bằng xe

Với chỉ 1 bộ điều khiển góc nghiêng thì cũng đủ để xe không bị ngã Tuy nhiên,một vấn đề đặt ra là xe dao động di chuyển tới lui Vì vậy, ta cần một bộ điều khiển PID để chỉnh vận tốc xe Bộ điều khiển này lấy tín hiệu đọc được từ encoder gắn trực tiếp lên hai bánh xe để xác định vận tốc bánh xe Giá trị vận tốc này sẽ so sánh với giátrị đặt để được một sai số và đưa vào bộ điều khiển PID vận tốc, ngõ ra của bộ điều khiển này sẽ là góc đặt của bộ PID góc nghiêng