Thông qua đó để nghiên cứu giải thuật LQR và ứng dụng để điều khiển hệ xe hai bánh tự cân bằng.Trong đề tài nhóm thực hiện so sánh các trường hợp sử dụng bộ điều khiển toàn phương tuyến
CƠ SỞ LÝ THUYẾT
Đặc tính động lực học
Xây dựng hệ phương trình trạng thái mô tả hệ thống xe hai bánh tự cân bằng
Hình 2.5 Mô hình xe hai bánh tự cân bằng trên mặt phẳng
Kí hiệu – [đơn vị] Ý nghĩa
[ ] m kg Khối lượng bánh xe
L m Khoảng cách từ trọng tâm robot đến trục bánh xe fw Hệ số ma sát giữa bánh xe và mặt phẳng di chuyển fm Hệ số ma sát giữa robot và động cơ
Jm kg m Moment quán tính của động cơ DC
Rm Ohm Điện trở của động cơ DC
Kb V rad Hệ số EMF của động cơ DC.
Kt Nm A Moment xoắn của động cơ DC n Tỉ số giảm tốc
[rad] Góc trung bình của bánh trái và phải
, [ ] l r rad Góc của bánh trái và phải
[rad] Góc nghiêng của phần thân robot
[rad] Góc xoay của robot
F F F Nm Moment phát động theo các phương khác nhau
F F Nm Moment phát động của động cơ bánh trái và phải , [ ] l r i i A Dòng điện động cơ bánh trái và phải , [ ] l r v v V Điện áp động cơ bánh trái và phải
Bảng 2.1 Kí hiệu và ý nghĩ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ó các phương trình sau:
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:
Trong đó toạ độ trung bình của xe trong hệ quy chiếu: m m m m m x x y y z R
Toạ độ bánh trái trong hệ quy chiếu:
525\* MERGEFORMAT (.) Toạ độ bánh phải trong hệ quy chiếu:
626\* MERGEFORMAT (.) Toạ độ tâm đối xứng giữa hai động cơ trong hệ quy chiếu: sin cos sin sin cos b m b m b m x x L y y L z z L 727\* MERGEFORMAT (.)
Phương trình động năng của chuyển động tịnh tiến:
Phương trình động năng của chuyển động quay:
10210\* MERGEFORMAT (.) là động năng quay của phần ứng động cơ trái và phải
Lấy đạo hàm L theo các biến , , ta được:
Moment động lực do động cơ DC sinh ra:
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ơ:
Xem điện cảm phần ứng tương đối nhỏ (gần bằng 0), có thể bỏ qua, suy ra:
Từ đó, các moment lực sinh ra:
Thu được phương trình động lực học mô tả chuyển động của xe như sau:
Một số phương pháp điều khiển có thể sử dụng cho hệ thống
2.2.1 Bộ điều khiển vi tích phân tỉ lệ
Bộ điều khiển PID là bộ điều khiển vòng kín, có sự kết hợp của ba bộ điều khiển: tỉ lệ, tích phân và vi phân có khả năng điều chỉnh sai số thấp nhất có thể, tăng tốc độ đáp ứng, giảm độ vọt lố, hạn chế sự dao động Bộ điều khiển dễ dàng thiết kế với việc ta chỉ cần chỉnh 3 thông số của bộ PID là K , K , K p i d
Hàm truyền của bộ điều khiển PID:
- Dễ dàng thiết kế vì ta chỉ điều chỉnh ba thông số K , K , Kp i d.
- Việc ta thiết kế bộ điều khiển PID ta phải tìm ba thông số của PID với hệ thống MIMO thì ta cần nhiều bộ PID để khiều khiển, với việc ta dùng nhiều bộ PID thì ta cần phải tìm nhiều thông số Trong thực tế thì 3 thông số K , K , K ta khó tìm chínhp i d xác được mà phải dựa vào kinh nghiệm.
- Bộ điều khiển PID khi ta thay đổi các thông số cơ bản của hệ thống thì 3 thông số ta tìm được nó không chính xác và độ ổn định của hệ thống bị thay đổi theo.
2.2.2 Bộ điều khiển vi tích phân tỉ lệ kết hợp với giải thuật di truyền
Với bộ điều khiển PID đã giới thiệu trước đó, nó có nhiều ưu điểm như việc tìm các thống số của bộ điều khiển ta phải dựa vào kinh nghiệm hoặc khi ta thay đổi khối lượng của vật thì làm cho tính ổn định của hệ thống không còn ổn định
Thuật toán giải thuật di truyền GA là thuật toán tối ưu ngẫu nhiên dựa trên cơ chế chọn lục tự nhiên và tiến hóa di truyền Được ứng dụng đầu tiền trong 2 lĩnh vực chính: tối ưu hóa và học tập của máy Nó được áp dụng vào bộ điều khiển PID nhầm cải thiện việc khi ta thay đổi các giá trị của hệ thống thì GA sẽ tìm và đưa ra giá trị tối ưu cho hệ thống.
Về mặt ưu điểm: Giúp cho hệ thống có thể hoạt động được khi ta thay đổi các giá trị của hệ thống như: khối lượng, kích thước,…
Về mặt nhược điểm: Thời gian tính toán các số liệu dựa nhiều về phần cứng, kết quả tìm được có thể làm cho RAM của vi điều khiển tràn và không tính được.
2.2.3 Bộ điều khiển mờ Điều khiển mờ là phương pháp điều khiển bắt chước quá trình xử lý thông tin không rõ ràng và ra quyết định điều khiển của con người.
Cơ sở toán học của bộ điều khiển mờ được Zadeh đưa ra từ năm 1965 Ngày nay, nó được sử dụng rộng rãi hơn trong lĩnh vực điều khiển tự động Các suy luận mờ được xây dựng nhờ vào lối suy luận logic của con người.
Một bộ điều khiển mờ cơ bản có cấu trúc như sau:
Hình 2.6 Cấu trúc cơ bản của bộ điều khiển mờ
- Tiền xử lý: chuẩn hoá hoặc tỷ lệ giá trị đo vào tầm giá trị chuẩn, lọc nhiễu.
- Mờ hóa: chuyển giá trị rõ phản hồi từ ngõ ra của đối tượng thành giá trị mờ để hệ quy tắc có thể suy luận được.
- Hệ quy tắc mờ: rút ra từ kinh nghiệm chuyên gia trong việc điều khiển đối tượng.
- Phương pháp suy diễn: từ hệ quy tắc được xây dựng sẽ có các phương pháp để tìm ngõ ra tương ứng qua khối giải mờ
- Giải mờ: chuyển giá trị mờ suy luận được ở ngõ ra của hệ quy tắc thành các giá trị rõ để điều khiển đối tượng.
- Hậu xử lý: tỷ lệ giá trị sau khi giải mờ thành giá trị vật lý để điều khiển hệ thống.
- Dễ thiết kế vì tập mờ và các mệnh đề mờ được xây dựng dựa trên lối suy luận của con người.
- Vì được ra đời lâu nên các luật mờ có thể dễ tìm được trên mạng và thay đổi chúng cho phù hợp với hệ thống thực tế.
- Có thể liên kết với bộ điều khiển PID để tăng tính ổn định của hệ thống. Nhược điểm:
- Nếu lối suy luận của ta sai thì kết quả cho ra có thể sai.
- Quá trình giải mờ tùy vào việc ta lựa chọn mô hình nào có thể khiến độ chính xác của kết quả cho được không đúng.
- Với hệ thống phức tạp nhiều đầu vào nhiều đầu ra thì ta cần nhiều tập cơ sở khác nhau và các luật mờ khác nhau Điều này có thể khiến cho hệ thống rắc rối hơn để thiết kế.
- Do suy luận để thiết kế nên không nắm rõ được bản chất, mô hình hệ thống.
- Đòi hỏi vi điều khiển có tốc độ xử lý nhanh, tốt và bộ nhớ cao.
2.2.4 Bộ điều khiển tối ưu
Một hệ điều khiển được thiết kế ở chế độ làm việc tốt nhất là hệ thống luôn ở trạng thái tối ưu theo một tiêu chuẩn chất lượng nào đó (đạt được giá trị cực trị).
Có nhiều bài toán điều khiển tối ưu, tuỳ theo:
- Loại đối tượng điều khiển.
- Miền thời gian liên tục hay rời rạc.
- Bài toán tối ưu có ràng buộc hay không.
Có hai phương pháp điều khiển tối ưu chính:
- Điều khiển tối ưu tĩnh: chỉ tiêu chất lượng không phụ thuộc thời gian.
- Điều khiển tối ưu động: chỉ tiêu chất lượng phụ thuộc thời gian, có các phương pháp sau:
+ Bài toán chỉnh toàn phương tuyến tính (Linear Quadratic Regulator – LQR). + Bài toán điều khiển tối ưu H2.
Khảo sát ưu và nhược điểm của bộ điều khiển tối ưu sử dụng phương pháp toàn phương tuyến tính (LQR) như sau:
- Hệ thống vẫn ổn định ngay cả khi ta thay đổi đặc tính trên hệ thống.
- Có thể thiết kế với các hệ thống phi tuyến phức tạp.
- Kết quả cho ra tính ổn định cao.
- Tính toán ma trận của bộ điều khiển LQR rất phức tạp.
- Việc lựa chọn ma trận trọng lượng thích hợp ở chỉ tiêu chất lượng rất quan trọng vì nó ảnh hưởng rất nhiều đến kết quả tính hoá.
- Nếu chịu ảnh hưởng của nhiễu hệ thống và nhiễu đo lường tác động vào hệ thống thì sẽ gây khó khăn khi cân bằng, tín hiệu điều khiển không ổn định nhiều gai gây ảnh hưởng tới động cơ.
Qua phân tích về ưu nhược điểm của từng bộ điều khiển trình bày ở trên thì nhóm quyết định xây dựng và mô phỏng hệ xe hai bánh tự cân bằng sử dụng bộ điều khiển toàn phương tuyến tính (LQR) để thực hiện khảo sát trong đề tài.
Điều khiển toàn phương tuyến tính
Hình 2.7 Sơ đồ khối của bộ lọc Kalman Xét hệ tuyến tính liên tục:
Trong đó: w t ( )là nhiễu hệ thống; v t ( )là nhiễu đo lường.
Giả sử nhiễu hệ thống và nhiễu đo lường có phân bố Gauss, không tương quan, có trung bình bằng 0 và phương sai là:
Bộ lọc Kalman liên tục: ˆ( ) [ ( )ˆ ( )] [ ( ) ˆ( )] ˆ( ) ˆ( ) x t Ax t Bu t L y t y t y t Cx t 36236\* MERGEFORMAT (.)
Trong đó L là độ lợi của bộ lọc Kalman:
Với là nghiệm của phương trình Ricatti:
Lời giải bộ lọc Kalman liên tục trên MATLAB: (với G là ma trận đơn vị)
2.3.2 Bài toán điều khiển toàn phương tuyến tính liên tục
Hình 2.8 Sơ đồ khối của bộ điều khiển toàn phương tuyến tính (LQR)
Trong đề tài này, chỉ xét bài toán LQR liên tục thời gian vô hạn. Đối tượng tuyến tính mô tả bởi phương trình trạng thái:
Chỉ tiêu chất lượng dạng toàn phương, trong đó thời điểm cuối tiến đến vô cùng:
41241\* MERGEFORMAT (.) Tín hiệu điều khiển tối ưu:
Và P là nghiệm bán xác định dương của phương trình đại số Ricatti:
Chú ý: trong trường hợp này K và P là không phụ thuộc vào thời gian
Giá trị cực tiểu của chỉ tiêu chất lượng: min T (0) (0)
Lời giải bộ điều khiển toàn phương tuyến tính liên tục:
2.3.3 Tuyến tính hoá hệ thống
Từ 229, 230, 231 ta có hệ phương trình mô tả hệ thống xe hai bánh tự cân bằng có dạng như sau:
Nếu chọn điểm cân bằng là:
Ta có thể tuyến tính hoá hệ thống 247 về dạng: x Ax Bu 52252\* MERGEFORMAT (.)
2.3.4 Khảo sát tính điều khiển được và quan sát được của hệ thống
Ma trận C quan sát được ba biến trạng thái là góc tịnh tiến trung bình của hai bánh xe , góc nghiêng của robot , góc xoay của robot được thể hiện như sau:
Ma trận điều khiển Mc [ ,B AB A B A B A B A B, 2 , 3 , 4 , 5 ]
Ma trận quan sát Mo [ ,C CA CA CA CA CA, 2 , 3 , 4 , 5 ] T
Kiểm tra, ta được rank M( c ) rank M( o ) 6.
Suy ra hệ thống điều khiển được và quan sát được.
2.3.5 Hàm chỉ tiêu chất lượng
Mục tiêu điều khiển là tìm vector điều khiển tối ưu K thoả mãn chỉ tiêu chất lượng J đạt cực tiểu:
67267\* MERGEFORMAT (.) Lúc này ta có các ma trận trọng số như sau:
Với các thông số Q Q Q Q Q Q R R 1 , , , , , , , 2 3 4 5 6 1 2 để tinh chỉnh cho bộ điều khiển.
Trong đó, thông số Q Q Q Q Q Q 1 , , , , , 2 3 4 5 6 lần lượt là các trọng số tối ưu cho các giá trị góc trung bình của bánh trái và bánh phải , vận tốc góc trung bình của bánh trái và bánh phải , góc nghiêng của robot , vận tốc góc nghiêng của robot , góc xoay của robot , vận tốc góc xoay của robot và thông số R R 1 , 2 là trọng số tối ưu cho tín hiệu điều khiển cho bánh trái v l và tín hiệu điều khiển cho bánh phải v r Nếu muốn biến trạng thái nào được ưu tiên trong việc điều khiển thì ta cho thông số trọng số tương ứng đó lớn hơn tương đối so với các trọng số còn lại trong hai ma trận Q và R Sau khi chọn được thông số điều khiển tương ứng thì ta tiến hành xây dựng thông số hồi tiếp K với tín hiệu điều khiển u Kx t ( ) Thông số K được tính dựa vào phương trình đại số 243.
Sơ đồ mô phỏng hệ thống trên MATLAB Simulink
Mô phỏng hệ thống với thông số mô hình như sau:
Kí hiệu - [đơn vị] Giá trị
Bảng 2.2 Thông số mô hình
Sau khi tuyến tính hoá hệ thống ta được hệ phương trình trạng thái dạng x Ax Bvới:
Xây dựng khối mô tả hệ thống với tham số mô hình như trên trong MATLAB Function:
Hình 2.9 Khối Two-wheeled Self Balancing Robot trong MATLAB Function
Trường hợp 1: Khi không điều khiển và hệ thống chịu tác động của nhiễu:
Hình 2.10 Sơ đồ mô phỏng hệ thống trong trường hợp 1
Trường hợp 2: Khi sử dụng bộ điều khiển LQR và hệ thống không chịu tác động của nhiễu:
Hình 2.11 Sơ đồ mô phỏng hệ thống trong trường hợp 2
Trường hợp 3: Khi sử dụng bộ điều khiển LQR và hệ thống chịu tác động của nhiễu:
Hình 2.12 Sơ đồ mô phỏng hệ thống trong trường hợp 3
Trường hợp 4: Khi sử dụng bộ điều khiển LQR kết hợp với bộ lọc Kalman để lọc nhiễu cho hệ thống:
Hình 2.13 Sơ đồ mô phỏng hệ thống trong trường hợp 4
THIẾT KẾ PHẦN CỨNG
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ác yê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.
- Nguồn cung cấp đủ cho hệ thống hoạt động.
- Cảm biến trả tín hiệu về tránh tối đa nhiễu và sai số.
- Khảo sát hoạt động trong thời gian ngắn.
- Động cơ moment đủ để kéo tải.
- Vi điều khiển đầy đủ chuẩn giao tiếp với máy tính và các cảm biến, đảm bảo xử lý tín hiệu nhanh.
Sơ đồ khối hệ thống
Sơ đồ khối mô tả hệ thống được chia thành các thành phần sau:
- Tầng đáy: Khối động cơ và cảm biến đọc xung của động cơ.
- Tầng thứ nhất: Khối điều khiển động cơ.
- Tầng thứ hai: Khối điều khiển trung tâm và Khối cảm biến đo góc nghiêng.
- Tầng thứ ba: Khối nguồn.
Hình 3.14 Sơ đồ khối hệ thống Chức năng của từng khối từng khối:
- Bộ khung xe: cố định từng khối thành phần của xe với nhau, lựa chọn kích thước chiều dài chiều rộng phù hợp để có thể chứa vừa các khối thành phần đã được nêu và dễ dàng cân bằng được.
- Khối cảm biến đo góc nghiêng có nhiệm vụ đọc dữ liệu trả về và gửi tín hiệu đó về Khối điều khiển trung tâm xử lý ra giá trị góc nghiêng của xe.
- Khối điều khiển trung tâm là một vi điều khiển, có nhiệm vụ chính là xử lý tất cả dữ liệu đọc về từ Khối đo góc nghiêng và Khối cảm biến đọc xung của động cơ, xử lý các thuật toán điều khiển đưa ra tín hiệu để điều khiển Khối điều khiển động cơ.
- Khối điều khiển động cơ: nhiệm vụ là nhận tín hiệu từ Khối điều khiểu trung tâm để cấp điện áp ra phù hợp để điều khiển Khối động cơ.
- Khối động cơ và cảm biến đọc xung của động cơ: nhận điện áp cấp từ Khối điều khiển động cơ điều khiển xe di chuyển qua lại, nhanh, chậm, dừng để cho xe có thể đạt được trạng thái cân bằng Ngoài ra còn có một bộ mã hoá quay encoder để trả về tín hiệu số xung đọc được, từ đó qua Khối điều khiển trung tâm xử lý có thể xác định chính xác được vị trí, tốc độ quay và chiều quay động cơ.
- Khối nguồn: cấp nguồn ổn định cho Khối điều khiển trung tâm, Khối điều khiển động cơ.
Lựa chọn thiết bị phần cứng
3.3.1 Khối điều khiển trung tâm
Khối điều khiển trung tâm là một vi điều khiển có nhiệm vụ xử lý dữ liệu đọc về từ cảm biến đo góc nghiêng và bộ mã hoá quay encoder từ đó xử lý các thuật toán về điều khiển xuất ra tín hiệu đến Khối điều khiển động cơ.
Cấu tạo chung của vi điều khiển bao gồm: Vi xử lý được kết nối với các port ngoại vi, các chân I/O, chân Analog đo từ giá trị 0-5V, port truyền thông bằng giao thức RS232, các chức năng ngắt ngoại/nội để kết nối đọc chân encoder, timer/counter, giao thức I2C để truyền/nhận dữ liệu nối tiếp đến vi điều khiển xử lý, chân cấp nguồn, ROM, RAM, EPROM,…
Khối điều khiển trung tâm phải có tốc độ xử lý nhanh với hiệu suất làm việc cao Một số Board vi điều khiển có thể xử dụng như Arduino Uno R3, STM32F4, Rasberry, họ vi điều khiển PIC,… Để thực hiện đề tài này chúng tôi sử dụng Board Arduino Mega 2560 Rev3 với các ưu điểm sau:
- Có số lượng chân I/O kết nối nhiều hơn so với Arduino Uno R3 có thể thoải mái trong việc đi dây hệ thống.
- Tốc độ xử lý phù hợp, có nhiều chân ngắt ngoại để đọc tín hiệu từ bộ mã hoá quay encoder trả về để điều khiển chính xác tốc độ, vị trí và chiều quay của động cơ.
- Có giao tiếp I2C để đọc tín hiệu trả về giá trị góc nghiêng từ cảm biến góc nghiêng IMU.
- Giá thành rẻ, thư viện hỗ trợ nhiều và cộng đồng người sử dụng lớn nên có thể dễ dàng lập trình và tìm kiếm thông tin nhanh chóng.
- Có nhiều chân hỗ trợ xuất xung PWM.
- Có nhiều tính năng nổi bật, chứa mọi thứ để hỗ trợ vi điều khiển.
- Có nguồn cấp tương thích với các loại board mạch ngoại vi hỗ trợ khác nhau như tín hiệu mức cao (5V) hoặc tín hiệu mức thấp (3.3V).
- Hỗ trợ nhiều chuẩn giao tiếp (đặc biệt là UART có giao tiếp với máy tính thông qua USB hoặc thông qua bluetooth và I2C để giao tiếp với cảm biến góc nghiêng IMU).
- Dễ dàng cấp nguồn từ USB từ máy tính hoặc từ các nguồn ngoài như pin, adapter AC-DC,…
- Vi điều khiển chính: ATmega2560.
- IC nạp và giao tiếp UART: ATmega16U2.
- Nguồn cấp: 5VDC từ cổng USB hoặc nguồn ngoài cắm từ giắc tròn DC (9VDC).
- Số chân đọc và ghi tín hiệu số (Digital I/O): 54 (trong đó 15 chân có khả năng xuất xung PWM).
- Số chân đọc tín hiệu tương tự (Analog Input) : 16.
- Cường độ dòng điện dòng điện trên mỗi chân I/O: 20mA.
- Cường độ dòng điện trên mỗi chân 3.3V: 50mA.
- Flash Memory: 256 KB trong đó 8 KB sử dụng cho bootloader.
- Số cổng UARTs giao tiếp nối tiếp phần cứng: 4.
- Một thạch anh dao động với tần số 16 MHz.
- Một cổng kết nối USB, một jack nguồn, một đầu ICSP, một nút Reset, một đèn LED tích hợp ở chân 13.
- Giao tiếp: USB, ICSP, SPI, I2C và UART.
Hình 3.15 Sơ đồ chân kết nối của Arduino Mega 2560 Rev3
3.3.2 Khối cảm biến đo góc nghiêng
Khối 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ục xyz 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. Đối với yêu cầu hệ thống, chỉ để xác định giá trị góc nghiêng của xe thì chúng tôi lựa chọn ra được loại cảm biến GY-521 6DOF IMU MPU6050 với các ưu điểm và nhược điểm sau:
+ Là loại cảm biến phổ biến nhất thị trường với giá thành hợp lý.
+ Có tích hợp sẵn phần cứng chuyên xử lý tín hiệu (Digital Motion Processor – DMP) do cảm biến thu thập và thực hiện các tính toán cần thiết giúp cải thiện tốc độ xử lý, phản hồi nhanh hơn và giảm bớt đáng kể phần xử lý tính toán của vi điều khiển. + Giao tiếp I2C phù hợp với Khối điều khiển trung tâm đã chọn.
+ Có nhiều nguồn thư viện để hỗ trợ trong việc xử lý tín hiệu và lập trình.
+ Tín hiệu trả về từ hai cảm biến có sai số trên mỗi trục làm lệch đi các giá trị đo so với thực tế.
+ Cả hai cảm biến đo gia tốc và tốc độ quay đều chịu ảnh hưởng của nhiễu.
=> Để có thể khắc phục nhược điểm này thì chúng tôi dùng bộ lọc Kalman xử lý các thuật toán để ước lượng trạng thái trả về từ hai cảm biến chính xác nhất Bộ lọc Kalman được chúng tôi sử dụng đã được viết sẵn dưới dạng thư viện cho vi điều khiển (https://github.com/TKJElectronics/KalmanFilter).
- Gồm 6 trục cảm biến (6DOF): cảm biến gia tốc (3 trục), cảm biến con quay hồi chuyển (3 trục).
- Bộ chuyển đổi tương tự sang số (ADC): 16 bit.
- Điện áp sử dụng: 3-5 VDC.
- Điện áp giao tiếp: 3-5 VDC.
- Giá trị Gyroscope trong khoảng: ±250, ±500, ±1000, ±2000 o /sec.
- Giá trị Accelerometer trong khoảng: ±2, ±4, ±8, ±16 g.
Hình 3.16 Cảm biến góc nghiêng GY-521 6DOF IMU MPU6050
Hình 3.17 Sơ đồ nối dây cảm biến với Arduino Mega 2560
Chân Kết nối và chức năng
VCC Chân cấp nguồn dương.
XDA Chân dữ liệu (kết nối với cảm biến khác) XCL Chân xung (kết nối với cảm biến khác)
ADO Bit0 của địa chỉ I2C
Bảng 3.3 Bảng kết nối chân của cảm biến MPU6050
3.3.3 Khối điều khiển động cơ
Khối điều khiển động cơ nhận tín hiệu PWM (điều chế độ rộng xung) từ Khối điều khiển trung tâm để dễ dàng điều khiển hai động cơ bánh trái và bánh phải quay thuận, quay nghịch, dừng hoặc theo tốc độ nhanh, chậm để xe dễ dàng đạt trạng thái cân bằng. Điều chế độ rộng xung (PWM) là một kỹ thuật điều chế được sử dụng để mã hóa một thông điệp thành một tín hiệu xung Mặc dù kỹ thuật điều chế này có thể được sử dụng để mã hóa thông tin để truyền tải, việc sử dụng chính của nó là cho phép điều khiển nguồn điện cung cấp cho các thiết bị điện, đặc biệt là để tải quán tính như động cơ
Giá trị trung bình của điện áp (và dòng điện) cung cấp cho tải được kiểm soát bằng cách thay đổi việc đóng cắt giữa nguồn và tải tắt với tốc độ rất nhanh Thời gian đóng càng lâu so với thời gian cắt, thì tổng công suất cung cấp cho tải càng cao. Ở đây, chúng tôi lựa chọn mạch điều khiển động cơ DC L298N (Dual Full Bridge Driver) với các ưu điểm sau:
- Dễ dàng sử dụng, dễ dàng lắp đặt, chi phi thấp, có hiệu năng cao.
- Một module có khả năng điều khiển hai động cơ.
- Tích hợp IC nguồn 7805 giúp cấp nguồn 5VDC cho các module khác (phù hợp để cấp nguồn cho Khối cảm biến tốc độ động cơ sử dụng encoder).
- Sử dụng trong một thời gian dài sẽ làm module nóng lên gây ra hao phí công suất
- Hiệu suất không đạt được tối đa.
=> Trong giới hạn điều khiển này thì chỉ cần điều khiển xe trong một thời gian ngắn để quan sát được xác lập của xe.
- IC chính: L298 - Dual Full Bridge Driver.
- Điện áp đầu vào: 5 - 12VDC.
- Công suất tối đa mỗi cầu H: 25W.
- Dòng tối đa cho mỗi cầu H: 2A.
- Dòng tín hiệu điều khiển: 0 – 36mA.
- Điện áp của tín hiệu điều khiển: 5 – 7 VDC.
- Mức điện áp logic: Low -0.3V - 1.5V, High: 2.3V – Vss.
- Công suất hao phí: 20W khi nhiệt độ IC ở 75 o C.
Hình 3.18 Mạch cầu H điều khiển động cơ DC L298N (Dual Full Bridge Driver)
Chân Kết nối và chức năng
12V Chân cấp nguồn dương (9 – 12VDC)
GND Chân cấp GND (nối chung với GND Arduino)
ENA Chân điều khiển mạch cầu H động cơ 1 (cấp xung PWM cho động cơ 1) IN1 Chân nhận tín hiệu điều khiển động cơ 1
IN2 Chân nhận tín hiệu điều khiển động cơ 1
ENB Chân điều khiển mạch cầu H động cơ 2 (cấp xung PWM cho động cơ 2) IN3 Chân nhận tín hiệu điều khiển động cơ 1
IN4 Chân nhận tín hiệu điều khiển động cơ 2
OUT1 Chân nối với chân dương M1 động cơ 1
OUT2 Chân nối với chân âm M2 động cơ 1
OUT3 Chân nối với chân dương M1 động cơ 2
OUT4 Chân nối với chân âm M2 động cơ 2
Bảng 3.4 Bảng kết nối chân của mạch cầu H L298N
3.3.4 Khối động cơ và cảm biến đọc xung của động cơ
Khối động cơ và cảm biến đọc xung của động cơ gồm:
- Hai động cơ bên phải và bên trái là cơ cấu chấp hành chính của xe để điều khiển xe cân bằng.
- Hai bộ mã hoá quay encoder gắn liền với mỗi động cơ giúp hồi tiếp xung vềKhối điều khiển trung tâm từ đó xử lý tín hiệu giúp xác định chính xác vị trí, tốc độ quay và chiều quay của từng động cơ.
Vì yêu cầu đặt ra là động cơ gắn sẵn bộ mã hoá quay encoder và động cơ kéo tải tầm 700g nên chúng tôi đã lựa chọn động cơ DC Servo GM25-370 DC Geared Motor loại 12VDC 250 vòng/phút với ưu điểm như sau:
- Động cơ tích hợp thêm Encoder hai kênh A,B.
- Cấu tạo bằng kim loại cho độ bền và ổn định cao.
- Hộp giảm tốc của động cơ có nhiều tỉ số truyền giúp dễ dàng lựa chọn giữa lực kéo và tốc độ.
- Động cơ được sử dụng nguyên liệu chất lượng cao với lõi dây đồng nguyên chất, lá thép 407, nam châm có độ từ tính mạnh,… cho sức mạnh và độ bền vượt trội với các loại giá rẻ khác trên thị trường.
+ Điện áp cấp cho động cơ hoạt động: 3 - 12VDC.
+ Đường kính động cơ: 25mm.
+ Tỉ số truyền 34:1 (động cơ quay 34 vòng trục chính hộp giảm tốc quay 1 vòng).
+ Dòng chịu đựng tối đa khi có tải: 750mA.
+ Tốc độ không tải: 250 vòng/phút.
+ Tốc độ chịu đựng tối đa khi có tải: 140 vòng/phút.
+ Lực kéo Moment định mức: 4.3 kg.cm.
+ Lực léo Moment tối đa: 5.2 kg.cm.
+ Chiều dài hộp số L: 21mm.
+ Điện áp cấp cho Encoder hoạt động: 3.3 – 5 VDC.
+ Cảm biến từ trường Hall, có 2 kênh AB lệch nhau.
+ Đĩa Encoder trả 11 xung/ 1 kênh/ 1 vòng.
+ Số xung Encoder mỗi kênh trên 1 vòng quay trục chính: 11 x 34 = 374 xung.
Hình 3.19 Động cơ DC Servo GM25-370 DC Geared Motor
Chân Kết nối và chức năng
M1 (đỏ) Dây cấp nguồn dương cho động cơ
GND (đen) Dây cấp nguồn âm cho động cơ
C1/A (vàng) Tín hiệu trả xung kênh A
C2/B (xanh lá) Tín hiệu trả xung kênh B
Dây cấp nguồn cho Encoder
M2 (trắng) Dây cấp nguồn âm cho động cơ
Bảng 3.5 Bảng kết nối chân của động cơ và encoder
Cung cấp nguồn ổn định cho các khối giúp hệ thống hoạt động tốt Có thể cấp nguồn cho hệ thống bằng nhiều cách như pin, nguồn xung tổ ong hoặc adapter chuyển đổi AC-DC,…
Lựa chọn khối cấp nguồn gồm:
- Nguồn gồm 3 pin Panasonic Lithium ion IRC18650.
- Mạch giảm áp chuyển đổi DC – DC LM2596 3A.
- Công tắc bập bênh 10x15 mm.
- Jack DC đực có dây. Ưu điểm của khối cấp nguồn đã được chọn:
- Khi sử dụng pin kết hợp với mạch giảm áp sẽ giúp ổn định được một mức điện áp cố định cấp cho hệ thống, phòng trường hợp sụt áp khi pin cạn.
- Sử dụng được linh động, gọn nhẹ hơn so với dùng nguồn tổ ong hay adapter.
- Lắp đặt, đi dây dễ dàng, không bị vướng dây nguồn khi xe chạy.
- Dễ dàng sử dụng, thay và sạc pin cho nguồn.
- Sử dụng công tắc giúp thuận tiện hơn trong việc đóng/tắt hệ thống.
- Sử dụng giắc DC đực thuận tiện trong việc cấp nguồn cho vi điều khiển.
- Một phần giúp tăng khối lượng của xe khiến xe dễ dàng cân bằng hơn.
Bảng đi dây hệ thống
L2 98 Động cơ 1 và Encoder 1 Động cơ
Bảng 3.6 Bảng đi dây phần nguồn
3.4.2 Phần tín hiệu điều khiển
8 Động cơ 1 và Encoder 1 Động cơ 2 và encoder 2
Bảng 3.7 Bảng đi dây phần tín hiệu điều khiển734Equation Section (Next)
THIẾT KẾ PHẦN MỀM
Yêu cầu thiết kế
Về phần mềm điều khiển hệ thống các yêu cầu thiết kế để xe đảm bảo cân bằng như sau:
- Đọc tín hiệu cảm biến MPU6050 thông qua truyền thông I2C.
- Đọc giá trị xung encoder trả về.
- Xuất xung PWM điều khiển mạch cầu H điều khiển động cơ theo tốc độ, chạy thuận, chạy nghịch, dừng động cơ.
- Giới hạn góc nghiêng để cân bằng xe từ -30 đến 30 o o
- Giao tiếp với máy tính thông qua cồng USB.
- Thiết kế giao diện để vẽ biểu đồ đáp ứng khi điều khiển xe cân bằng theo thời gian thực.
Xây dựng thuật toán điều khiển
Để sử dụng và lập trình Arduino Mega 2560 thì phần mềm sử dụng để lập trình là Arduino IDE 1.8.15, thuận tiện trong việc kết nối Arduino với máy tính bằng dây cáp USB.
Ngôn ngữ lập trình của Arduino là C/C++, lập trình đơn giản hơn nhiều so với các loại vi điều khiển khác nhờ cộng đồng người dùng lớn và sẵn các thư viện hỗ trợ lập trình đa dạng.
Chương trình điều khiển được đưa vào ở phần Phụ lục.
4.2.1 Thuật toán chương trình chính
Hình 4.23 Lưu đồ thuật toán chương trình chính
Phần khai báo và khởi tạo chương trình:
- Khai báo các thư viện sử dụng: Wire.h để giao tiếp I2C với cảm biến góc nghiêng MPU6050, và Kalman.h để sử dụng bộ lọc Kalman lọc nhiễu cho cảm biến góc nghiêng MPU6050.
- Định nghĩa các chân kết nối.
- Khai báo các biến và giá trị ban đầu.
- Khởi tạo Serial giao tiếp máy tính thông qua USB với baud rate là 115200.
- Khởi tạo sử dụng timer2 để xuất xung PWM ở chân 9 và chân 10.
- Khởi tạo các chân xuất tín hiệu ngõ ra là chân xuất PWM của mỗi bánh, chân điều chỉnh chiều quay của mỗi bánh.
- Khởi tạo các chân nhận tín hiệu ngõ vào là chân nhận tín hiệu từ encoder kênh
A và kênh B của mỗi bánh.
- Khởi tạo ngắt 0 sử dụng để đọc tín hiệu trả về từ kênh A bánh trái tác động cạnh lên, ngắt 1 sử dụng để đọc tín hiệu trả về từ kênh A bánh phải tác động cạnh lên.
- Khởi tạo hàm đọc giá trị cảm biến góc nghiêng từ giao tiếp I2C.
- Đọc và xử lý giá trị góc nghiêng trả về từ MPU6050.
- Thời gian lấy mẫu là 6ms.
- Cập nhập các giá trị theta, psi, phi và tính toán giá trị thetadot, psidot, phidot.
- Tính toán điện áp cung cấp ở bánh trái và bánh phải.
- Quy đổi sang PWM để xuất xung điều khiển cho mạch cầu.
- Xuất giá trị đọc được qua giao tiếp UART với máy tính đưa lên phần mềm giám sát.
4.2.2 Thuật toán xử lý xung trả về từ encoder và tính toán giá trị góc tới, góc xoay
Sử dụng chức năng ngắt ngoại của vi điều khiển để đọc giá trị xung encoder mà không làm ảnh hưởng đến chương trình chính.
Hình 4.24 Lưu đồ chương trình ngắt xử lý xung trả về từ encoder
Hình 4.25 Lưu đồ tính toán giá trị góc tới và góc xoay
- PIN 2 nối với encoder kênh A động cơ trái (sử dụng ngắt 0).
- PIN 22 nối với encoder kênh B động cơ trái (đọc tín hiệu trả về).
- PIN 3 nối với encoder kênh A động cơ phải (sử dụng ngắt 1).
- PIN 26 nối với encoder kênh B động cơ trái (đọc tín hiệu trả về).
Thuật toán đọc giá trị động cơ trái:
- Sử dụng ngắt 0 cho kênh A.
- Khi xung cạnh lên tác động thì chương trình ngắt thực hiện.
- Lúc này đọc giá trị trả về từ kênh B: nếu kênh B đang ở mức cao thì động cơ đang quay thuận, leftencoder + + và ngược lại kênh B đang ở mức thấp thì động cơ đang quay theo chiều nghịch, leftencoder - -.
Thuật toán đọc giá trị động cơ phải:
- Sử dụng ngắt 1 cho kênh A.
- Khi xung cạnh lên tác động thì chương trình ngắt thực hiện.
- Lúc này đọc giá trị trả về từ kênh B: nếu kênh B đang ở mức cao thì động cơ đang quay thuận, rightencoder + + và ngược lại kênh B đang ở mức thấp thì động cơ đang quay theo chiều nghịch, rightencoder - -.
Sử dụng phương pháp này sẽ tối ưu được tốc độ xử lý của vi điều khiển thay vì phương pháp sử dụng ngắt cho cả hai kênh A và B.
Thuật toán đọc giá trị góc tới và góc xoay :
Từ phần xây dựng mô hình toán đối tượng trong chương 2 ta có được công thức tính góc tới trung bình của hai bánh xe từ phương trình 22. Động cơ sử dụng hộp số với tỉ số truyền là 34:1, đĩa Encoder trả ra 11 xung/ vòng Nên số xung của Encoder trên 1 vòng quay trục chính động cơ là 34 x 11 = 374 xung/vòng.
Góc Encoder được xác định theo công thức:
Với leftencoder là xung encoder đọc được ở bánh trái. rightencoderlà xung encoder đọc được ở bánh phải.
4.2.3 Thuật toán đọc và xử lý giá trị từ cảm biến góc nghiêng
Hình 4.26 Lưu đồ thuật toán đọc và xử lý giá trị từ cảm biến góc nghiêng Phần khởi tạo:
- Thiết lập địa chỉ I2C là 0x68, 0x69 địa chỉ này giúp ta kết nối nhiều module, cảm biến khác nhau trên cùng 1 đường dữ liệu.
- Thiết lập các giá trị và mảng lưu giá trị thanh ghi từ module I2C.
- Thiết lập tần số I2C đến 400 kHz TWBR = ((F_CPU / 400000UL) - 16) / 2. Thuật toán đọc giá trị từ cảm biến:
- Kiểm tra khoảng thời gian Arduino hoạt động đến thời gian đặt thì lấy mẫu cảm biến 1 lần.
- Kiểm tra có giá trị trả về từ module I2C về không ?
- Cập nhập giá trị trả về vào mảng i2cData và trích xuất các giá trị gia tốc theo phương X biến AcX, gia tốc theo phương Y biến AcY, vận tốc con xoay hồi chuyển trục Y biến Gyro Ở đây chỉ cần quan tâm các giá trị cần thiết để tính góc pitch, đó chính là góc nghiêng của xe.
- Cập nhập thời gian chu kì đọc dt là thời gian từ lúc đọc tới lúc nhận được kết quả (quy ra s).
- Tính toán giá trị góc pitch và tỉ lệ vận tốc con xoay hồi chuyển Gyrorate
- Sử dụng hàm kalman.getAngle(pitch, Gyrorate,dt) trong thư viện Kalman.h để trả về giá trị góc nghiêng.
- Giới hạn vùng hoạt động của xe: Nếu góc nghiêng trả về trong khoảng từ -30 o đến 30 thì xe sẽ điều khiển cân bằng, ngoài khoảng đó thì ngắt động cơ dừng điều o khiển xe.
Sử dụng hàm Kalman lọc nhiễu ước lượng giá trị góc nghiêng trả về phẳng, ít gai hơn Từ đó ổn định được tín hiệu điều khiển cho hệ thống, giúp hệ thống hoạt động mượt mà hơn.
4.2.4 Thuật toán tính xung băm PWM mỗi bánh với giải thuật LQR và điều khiển động cơ
Hình 4.27 Lưu đồ thuật toán tính xung băm PWM mỗi bánh với giải thuật LQR Thuật toán tính xung băm PWM mỗi bánh:
Từ ma trận K tính toán được ở chương cơ sở lý thuyết, ma trận K sẽ có dạng:
Và tín hiệu điều khiển: l r u v v 76476\* MERGEFORMAT (.)
Theo giải thuật LQR thì tín hiệu điều khiển cấp cho động cơ là:
Suy ra tín hiệu điều khiển cho bánh trái và bánh phải lần lượt là:
Từ tín hiệu điều khiển, chuẩn hoá và giới hạn giá trị xung băm PWM: khi động cơ nghiêng 1 góc là 15 độ thì sẽ băm xung tối đa (tuỳ theo tần số băm xung cầu H sẽ có một mức giới hạn riêng).
Thuật toán điều khiển động cơ được thể hiện rõ trong lưu đồ:
Hình 4.28 Lưu đồ thuật toán điều khiển động cơ
4.2.5 Xuất và vẽ biểu đồ lên máy tính
Thiết lập truyền thông nối tiếp RS232 thóng qua cổng nạp USB với baud rate là
115200 với cú pháp Serial.begin(115200). Đọc giá trị theta psi phi của hệ thống thông qua cảm biến và encoder rồi lưu vào các biến nhớ.
Xuất giá trị ra serial bằng hàm Serial.println((String)(psi * ToDeg) + ',' + (String)(theta * ToDeg) + ',' + (String)(phi * ToDeg)); để gửi chuỗi ký tự lên máy tính cách nhau bởi dấu phẩy.
Thiết kế giao diện bằng ngôn ngữ lập trình C#, giá trị đọc cách nhau dấy phẩy để tách chuỗi và mã hóa về kiểu double khi ta gửi số liệu lên máy tính, sử dụng thư viện ZedGraph để tạo khung vẽ biểu đồ đường.
Hình 4.29 Lưu đồ đọc và biểu diễn đồ thị
Phần mềm giám sát
Phần mềm giám sát được thiết kế giao diện gồm:
- Khu vực kết nối thiết bị Terminal: chọn Baudrate và Port của vi điều khiển, nút Connect (kết nối), nút Disconnect (ngưng kết nối), Exit (thoát chương trình).
- Khu vựa Receivered (tín hiệu nhận được): trả về tín hiệu góc nghiêng (Psi) theo thời gian thực.
- Khu vực biểu đồ: vẽ biểu đồ quan sát góc nghiêng trả về với trục tung là góc nghiêng (độ), trục hoành là thời gian (s)
Hình 4.30 Giao diện giám sát hệ thống795Equation Section (Next)