Đề tà robot hai bánh tự cân bằng

1 Chương GIỚI THIỆU 1.1 Giới thiệu Cùng với phát triển khoa học kỹ thuật, ngày robot có khả thay người làm việc môi trường độc hại, sản xuất bắt chước người hình thức, hành vi suy nghĩ Hiện lĩnh vực robot phát triển nhanh nhờ vào phát triển liên tục công nghệ, robot chế tạo để phục vụ cho nhiều mục đích khác Với ý tưởng sử dụng robot thay người “Robot hai bánh tự cân bằng” đề tài nhiều tác giả quan tâm mơ hình robot có khả di chuyển nhanh, linh hoạt lại không chiếm nhiều không gian [1], [3] 1.2 Mục tiêu đề tài Mục tiêu đề tài xây dựng mơ hình robot hai bánh thực tế có khả tự cân có cấu trúc đơn giản phục vụ cho việc nghiên cứu, giảng dạy phịng thí nghiệm, mơ hình sản phẩm trưng bày hay trị chơi với mục đích giải trí 1.3 Phương pháp nghiên cứu Đề tài tiếp cận dựa phương pháp sau:  Phương pháp khảo sát tài liệu, tìm hiểu tài liệu liên quan đến đến đề tài như: cấu trúc robot hai bánh tự cân bằng, cảm biến IMU, mạch điều khiển động  Phương pháp khảo sát thuật toán lọc nhiễu cho cảm biến IMU như: lọc Kalman, lọc Complementary thuật toán điều khiển PID  Phương pháp thực nghiệm tiến hành xây dựng thuật tốn mơ hình robot hai bánh thực tế 1.4 Giới hạn đề tài Đề tài tập trung vào việc xây dựng mơ hình phần cứng robot như: kết cấu khí, mạch điều khiển động cơ, thuật toán vi điều khiển lọc Kalman n giải thuật cân PID Robot cân vị trí chưa có khả vừa di chuyển vừa cân 1.5 Tình hình nghiên cứu nước giới 1.5.1 Trong nước Mơ hình robot hai bánh tự cân luận văn thạc sĩ tác giả Nguyễn Gia Minh Thảo, trường Đại học Bách Khoa TP Hồ Chí Minh Hình 1.1 Mơ hình robot hai bánh cân 1.5.2 Nước Một số kỹ thuật viên sinh viên ngành Cơ điện tử, Tự động hóa nghiên cứu cho đời nhiều dạng robot hai bánh cân Sau số thơng tin mơ hình robot hai bánh tự cân 1.5.2.1 nBot nBot ông Anderson sáng chế nBot lấy ý tưởng cân bánh xe phải chạy theo hướng mà phần robot ngã Nếu bánh xe di chuyển để trọng tâm robot ln rơi vào robot giữ cân n Hình 1.2 Robot hai bánh cân nBot 1.5.2.2 EquipoiseBot EquipoiseBot sử dụng hai cảm biến cảm biến gia tốc cảm biến quay hồi chuyển Góc nghiêng robot tính toán từ hai giá trị khác Một từ cảm biến gia tốc cách sử dụng thuật toán Tangens-Funktion từ cảm biến quay hồi chuyển sử dụng phương pháp tích phân Hai giá trị kết hợp lọc giá trị sử dụng cho đầu vào PID để điều khiển động cơ, giữ cho robot cân Hình 1.3 Robot hai bánh cân EquipoiseBot n 1.5.2.3 WobblyBot WobblyBot thực chất mô hình sử dụng lắc ngược, gắn phần thân hai bánh xe WobblyBot thiết kế với phần robot nặng nhiều so với phần giúp robot có khả giữ cân tốt Hình 1.4 Robot hai bánh cân WobblyBot 1.5.2.4 tiltOne tiltOne robot hai bánh cân với chiều cao 90 cm có khả chở tải trọng lên tới 50 kg Nguyên lý hoạt động giống robot hai bánh có kích thước nhỏ hơn, sử dụng hai cảm biến cảm biến gia tốc cảm biến quay hồi chuyển thuật tốn PID để điều khiển robot cân Hình 1.5 Robot hai bánh cân tiltOne n 1.5.2.5 Robot kiểu rolling hãng TOYOTA Đây robot có cơng dụng phục vụ người hãng TOYOTA thiết kế Mẫu robot có khả di chuyển nhanh chiếm không gian Đồng thời đôi tay robot làm nhiều cơng việc khác nhau, chủ yếu sử dụng với mục đích giải trí Hình 1.6 Robot hai bánh hãng TOYOTA 1.6 Nội dung đề tài Nội dung phần lại đề tài gồm chương sau:  Chương 2: THIẾT KẾ MÔ HÌNH HỆ THỐNG Nội dung chương trình bày sơ đồ khối hệ thống, xây dựng mơ hình robot hai bánh thực tế, thiết kế hệ thống điện thành phần tham gia điều khiển robot như: bo Arduino Due, mạch điều khiển động cơ, cảm biến IMU GY-86, động Planet Pin LI-PO Phần cuối chương giới thiệu phương thức giao tiếp sử dụng mô hình như: điều chế độ rộng xung PWM, giao thức I2C, giao tiếp UART thu thập liệu để vẽ dạng sóng ngõ phần mềm Labview  Chương 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CÂN BẰNG Phần đầu chương giới thiệu nguyên lý điều khiển cân robot, giới thiệu tổng quan lọc Kalman, sở, chất, giải thuật lọc Kalman rời rạc Giải thuật điều khiển PID sử dụng để đáp ứng ngõ nhanh, xác n việc cân robot cuối giải thuật điều khiển robot cân bám theo vị trí ban đầu  Chương 4: KẾT QUẢ THỰC NGHIỆM Chương trình bày kết thực nghiệm độ xác sử dụng lọc Complementary, lọc Kalman so với liệu thô từ cảm biến, xây dựng giải thuật điều khiển PID mô hình thực tế  Chương 5: KẾT LUẬN Nội dung chương trình bày tóm tắt kết mà đề tài đạt hướng phát triển để khắc phục giới hạn nhằm hoàn thiện đề tài tốt n Chương THIẾT KẾ MƠ HÌNH HỆ THỐNG 2.1 Sơ đồ khối hệ thống Cảm biến IMU GY-86 I 2C Arduino Due AT91SAM3X8E PWM Mạch điều khiển động cầu H Nguồn cung cấp Hình 2.1 Sơ đồ khối hệ thống 2.2 Tính tốn thiết kế cho mơ hình hệ thống Giả sử góc nghiêng mà robot giữ cân giới hạn khoảng ±200 so với góc vị trí cân lý tưởng 900 theo trục y C 50 cm 50 cm 20 70 A α B AB = ? a) Robot trạng thái cân b) Trạng thái robot góc nghiêng 200 so với góc vị trí cân Hình 2.2 Qng đường robot cần di chuyển để thực cân nghiêng góc 200 so với góc vị trí cân n Như mơ tả Hình 2.2 để robot giữ cân ngã robot phải di chuyển đoạn đường khoảng thời gian cho phép để hứng trọng tâm robot, giá trị tính tốn sau: (2.1) t = ? C 50 cm 50 cm 20 70 A α B 17.1 cm b) Trạng thái robot góc nghiêng 20 so với góc vị trí cân a) Robot trạng thái cân Hình 2.3 Mơ tả thời gian robot ngã từ trạng thái cân đến góc nghiêng 200 Thời gian ngã robot từ vị trí cân đến góc nghiêng giới hạn xác định phương pháp thực nghiệm mô hình thực tế Giá trị góc nghiêng thu thập theo thời gian truyền máy tính Bảng 2.1 Bảng 2.1 Bảng thu thập thời gian robot ngã lực hút Trái Đất Góc nghiêng theo trục y (độ) Thời gian Lần Lần Lần Lần 88.420 88.010 88.270 88.450 50 88.320 88.050 88.230 88.410 100 88.010 87.770 87.890 88.120 150 88.800 88.680 88.670 88.970 200 88.510 88.410 88.370 88.670 (ms) n 250 87.710 87.300 88.070 88.410 300 87.690 87.450 87.170 88.100 350 87.380 87.180 86.850 87.830 400 86.150 86.010 85.980 86.010 450 85.740 85.400 86.170 86.350 500 85.270 85.130 85.570 85.910 550 84.080 84.490 85.210 84.630 600 83.300 83.920 84.560 84.760 650 83.590 82.470 83.320 83.780 700 82.130 82.140 82.530 82.660 750 80.120 80.710 80.400 81.610 800 78.120 79.010 77.440 79.480 850 74.640 74.640 76.450 900 70.860 76.190 70.520 70.790 73.060 950 66.320 64.820 66.540 70.190 Dữ liệu thu thập Bảng 2.1 cho thấy thả robot ngã tự từ vị trí cân 880 (đây góc nghiêng vị trí cân thu từ cảm biến mơ hình thực tế, góc sai lệch so với góc nghiêng lý tưởng giới hạn khí mơ hình) đến góc nghiêng giới hạn 88 ± 200 khoảng thời gian trung bình 950 ms, giá trị thời gian giới hạn cần thiết để robot di chuyển để giữ cân Giả sử chọn bánh xe có bán kính 5cm moment xoắn động 63 Ncm lực đẩy tính theo công thức sau: (2.2) Suy vận tốc cần thiết động cơ: (2.3) Công suất động tính theo cơng thức: (2.4) n 10 Dựa vào giá trịnh tính tốn, thơng số động chọn Bảng 2.2 để đáp ứng yêu cầu Bảng 2.2 Chọn động với thông số Động DC planet Công suất 60W Moment xoắn 63 N.cm Tốc độ chưa qua giảm tốc 9000 vòng/phút Tốc độ qua giảm tốc 468 vịng/phút 2.3 Mơ hình robot hai bánh thực tế Đề tài sử dụng bo mạch Arduino Due AT91SAM3X8E đóng vai trò “bộ não” robot, điều khiển cho robot giữ cân Khung robot chế tạo từ inox với hai động DC đặt đồng trục, cho phép robot di chuyển theo hai hướng trước sau Hai động điểu khiển hai mạch cầu H sử dụng IC IR2184 để điều khiển kích FET, cảm biến IMU GY-86 sử dụng để xác định góc nghiêng robot Hai động DC 24V, 60W tốc độ 468 vòng/phút, encoder 500 xung/vòng Hai bánh xe bọc cao su có nhiều rãnh để tăng độ bám cao giúp robot cân tốt Nút nhấn điều khiển Pin Li-PO 550 mm Bo Arduino Due AT91SAM3X8E 450 mm Cảm biến IMU GY-86 Động DC Mạch điều khiển động cầu H Hình 2.4 Mơ hình robot hai bánh tự cân n 29 Bước tính tốn để tăng Kalman ( ) (3.32) Lưu ý trạng thái khởi động, thiết lập ma trận hiệp phương sai lỗi sau: [ ] (3.33) Để tăng độ lợi Kalman ma trận x 1: [ ] (3.34) Bây cập nhật hậu ước tính trạng thái tại: ̂ ̂ ̃ (3.35) Cuối cập nhật ma trận hiệp phương sai lỗi hậu: Nơi gọi ma trận đơn vị xác định là: (3.36) I gọi ma trận đơn vị xác định : I= [ ] (3.37)  Thực biến đổi phương trình lọc:  Cập nhật thời gian (dự đoán)  Từ phương trình (3.25) suy ra: ̇ [ ̇] =[ ̇ [ ] ̇ ̇ =[ +[ ̇ ̇ ̇ ] ̇k ̇ ] góc ước đốn trước  Từ phương trình (3.26) biến đổi sau: n ] ̇k ] ̇ =[ Vậy góc ước đốn +[ ][ ̇] + 30 [ ] =[ ][ [ ] ] [ [ ] [ [ ] ̇ [ ] ] [ [ ̇ [  Biến đổi phương trình (3.28) được: ̃ ̂ =[ ][ ] [ ]  Từ phương trình (3.30): ( ) =[ ][ ] [ ]  Biến đổi phương trình (3.32): ( ) [ ] [ [ ] n ] [ ] ̇ ] ̇  Cập nhật giá trị đo (hiệu chỉnh) ̇ ] ] ] 31 [ ]  Khai triển phương trình (3.35) sau: ̂ [ ̇] ̂ ̃ [ ̇] [ ] ̃ [ ̇] [ ̃ ] ̃ Cuối khai triển phương trình ma trận hiệp phương sai lỗi hậu (3.36): [ ] [ ] [ ] [ [ ] [ ] [ ] ] [ [ [ ] ] ] 3.4 Giải thuật điều khiển PID PID viết tắt cụm từ (Proportional Integral Derivative) giải thuật dùng nhiều ứng dụng điều khiển tự động với yêu cầu xác, nhanh ổn định Bộ điều khiển PID bao gồm thông số riêng biệt gọi điều khiển ba khâu: tỷ lệ, tích phân đạo hàm, viết tắt là: P, I D  Hàm truyền điều khiển PID sau: (3.38) Trong đó: : Độ lợi khâu tỷ lệ : Độ lợi khâu tích phân : Độ lợi khâu vi phân n 32  Cấu trúc điều khiển PID Hình 3.4 Hệ thống điều khiển vịng kín Trong đó: Controller: Bộ điều khiển cung cấp tín hiệu điều khiển cho đối tượng điều khiển Plant: Đối tượng điều khiển Biến e thành phần sai lệch giá trị mong muốn (R) giá trị ngõ thực tế (Y) Tín hiệu sai lệch (e) gửi tới điều khiển PID, điều khiển tính tốn thành phần tích phân lẫn vi phân (e) Tín hiệu (u) điều khiển bằng: ∫ (3.39) Lúc đối tượng điều khiển có tín hiệu vào (u) tín hiệu (Y) (Y) hồi tiếp cảm biến để tiếp tục tính sai lệch (e) Sau điều khiển tiếp tục lặp lại  Đặc tính điều khiển PID  Thành phần tỉ lệ (KP) có tác dụng làm tăng tốc độ đáp ứng hệ làm giảm, không triệt tiêu sai số xác lập hệ (steady-state error)  Thành phần tích phân (KI) có tác dụng triệt tiêu sai số xác lập làm giảm tốc độ đáp ứng hệ  Thành phần vi phân (KD) làm tăng độ ổn định hệ thống, giảm độ vọt lố cải thiện tốc độ đáp ứng hệ  Ảnh hưởng thành phần KP, KI, KD hệ kín tóm tắt bảng sau: n 33 Bảng 3.1 Ảnh hưởng thành phần KP, KI, KD hệ kín Thơng Thời gian đáp Độ vọt Thời gian xác Sai số xác số ứng lố lập lập KP Giảm Tăng Ít thay đổi Giảm KI Giảm Tăng Tăng Triệt tiêu KD Ít thay đổi Giảm Giảm Ít thay đổi 3.5 Xây dựng giải thuật PID điều khiển robot Để tìm hệ thức PID rời rạc, xét đồ thị sau: Hình 3.5 Đồ thị biểu diễn hệ số PID theo thời gian Đường biểu diễn điểm mong muốn Đường biểu diễn điểm thực tế Đường đồ thị rời rạc hóa vận tốc động ∆t thời gian lấy mẫu  Thành phần tích phân ∑ ∫ (3.40)  Thành phần vi phân [ n ] (3.41) 34 Do lấy gần ∆t = є >0 thì: [ ] (3.42) Tóm lại: [ ( ) ] ∑ (3.43) Đặt: (3.44) ∑ (3.45) Trong công thức (3.44) thời gian lấy mẫu ∆t nhỏ, nên bỏ qua ∆t Khi tìm hệ số Kp, Ki, Kd bao gồm ∆t Khi cơng thức lại viết lại sau: (3.46) Điều khiện biên: u(0) = duty >  Tìm hệ số PID phương pháp thực nghiệm:  Cho hệ số Kp, Ki, Kd không  Tăng Kp từ nhỏ đến lớn ngõ đáp ứng với rung động dao động ổn định  Giảm Kp xuống giá trị phản ứng  Tăng Ki từ nhỏ đến lớn, quan sát đáp ứng hệ thống thông số thay đổi Tăng dao động mức cho phép  Cuối tăng Kd hệ thống đạt tối ưu  Tìm hệ số PID phương pháp Ziegler-Nichols Độ lợi KI KD lúc đầu gán độ lợi P tăng P tiến tới độ lợi tới hạn Ku đầu vòng điều khiển bắt đầu dao động Ku thời gian dao động Pu dùng để gán độ lợi sau: n 35 Bảng 3.2 Phương pháp Ziegler–Nichols Dạng điều khiển KP KI P 0.50Ku PI 0.45Ku 1.2 KP/Pu PID 0.60Ku KP/Pu KD KP.Pu/8 3.6 Giải thuật điều khiển robot cân bám theo vị trí ban đầu (a) Robot bị trơi qua bên trái (b) Robot vị trí ban đầu (c) Robot bị trơi qua bên phải Hình 3.6 Robot cân bám theo vị trí ban đầu Dựa vào giá trị góc nghiêng thu từ cảm biến qua lọc Kalman giải thuật PID giữ cho robot cân lực hấp dẫn trái đất hay lực tác động từ bên làm cho robot bị ngã hướng Để giữ cân robot di chuyển khỏi vị trí ban đầu Để điều khiển robot cân dao động quanh vị trí ban đầu giá trị sai lệch vị trí so với vị trí ban đầu tính tốn n 36 Bắt đầu Điều khiển robot di chuyển vị trí ban đầu Lấy liệu góc nghiêng robot từ cảm biến Sai Giá trị sai lệch vị trí = ? Đúng Tính tốn sai lệch vị trí so với vị trí ban đầu Tính tốn sai số góc nghiêng robot cân góc nghiêng robot Điều khiển chiều quay tốc độ động để robot cân Hình 3.7 Sơ đồ thuật giải robot bám theo vị trí ban đầu n 37 Chương KẾT QUẢ THỰC NGHIỆM 4.1 Độ xác cảm biến Để điều khiển robot giữ cân thơng tin góc nghiêng robot đóng vai trị quan Giá trị góc nghiêng cung cấp cảm biến IMU tích hợp gồm có cảm biến gia tốc cảm biến quay hồi chuyển Hai cảm biến dạng cảm biến vi điện tử nên nhạy với rung động Giá trị góc thu trực tiếp từ cảm biến có dao động lớn phải xử lý lọc nhiễu trước đưa vào giải thuật cân Hình 4.1 trình bày kết so sánh giá trị góc nghiêng theo trục y thu trực tiếp từ cảm biến IMU (đường màu xanh lá), giá trị góc nghiêng theo trục y lọc Complementary (đường màu xanh dương) góc nghiêng theo trục y lọc Kalman (đường màu đỏ) Hình 4.1 Biểu đồ so sánh góc nghiêng theo trục y lọc Complementary, lọc Kalman góc nghiêng theo trục y thu trực tiếp từ cảm biến IMU Đây liệu thu thập sau đưa qua cổng UART vẽ phần mềm Labview Kết Hình 4.1 cho thấy giá trị góc nghiêng theo trục y n 38 robot từ hai lọc Complementary Kalman có độ ổn định cao so với giá trị góc nghiêng theo trục y thu trực tiếp từ cảm biến Hình 4.2 Biểu đồ so sánh thời gian đáp ứng lọc Complementary lọc Kalman Trên Hình 4.2 lọc Kalman (đường màu đỏ) có thời gian đáp ứng chậm so với lọc Complementary (đường màu xanh lam) Tuy nhiên để biết lọc Complementary hay lọc Kalman có độ ổn định cao độ ổn định ngõ hai lọc biễu diễn Hình 4.3 Hình 4.3 Biểu đồ so sánh độ ổn định ngõ lọc Kalman với lọc Complementary n 39 So với lọc Kalman (đường màu đỏ) lọc Complementary (đường màu xanh lam) dễ thực tốn tài nguyên xử lý vi điều khiển khả đáp ứng mặt thời gian nhanh hơn, kết so sánh mô tả Hình 4.2 Xét vị trí góc theo trục y robot đứng cân 86.6°, so với lọc Complementary lọc Kalman có thời gian đáp ứng chậm độ ổn định lọc Kalman lại có độ dao động nhỏ khả ổn định cao hơn, bị trơi so với lọc Complementary đươc vẽ phần mềm Labview Hình 4.3 4.2 Giải thuật cân PID mơ hình robot Hình 4.4 Góc nghiêng theo trục y robot thực giải thuật cân Hình 4.4 trình bày kết thực giải thuật PID cân mơ hình robot thực tế Giá trị góc xét (đường màu đỏ) robot đứng cân 88° góc theo trục y, giá trị góc nghiêng trục y (đường màu xanh lam) thu thập robot thực cân có độ dao động nhỏ chênh lệch khoảng 3° Giải thuật PID thực với hệ số , Các thơng số có dựa vào phương pháp Ziegler-Nichols phương pháp thực nghiệm n 40 4.3 Kết thực giải thuật mô hình robot Kết thực nghiệm mơ hình robot cho thấy robot có khả giữ cân vị trí đặt ban đầu Trong q trình robot giữ cân có lực tác động từ bên ngồi theo phương ngang làm robot ngã với góc nghiêng cho phép 880 ± 200 khoảng thời gian lớn 0.95 giây robot tiếp tục giữ cân di chuyển quay lại vị trí đặt ban đầu Video kết thực nghiệm robot thực cân khơng có lực tác động từ bên ngồi có lực tác động từ bên ngồi đăng tải địa chỉ: http://www.youtube.com/watch?v=v_81TG475as http://www.youtube.com/watch?v=_DAv26ln9JI&feature=share n 41 Chương KẾT LUẬN 5.1 Kết luận Kết đạt đề tài:  Xây dựng thành cơng mơ hình robot thực tế hướng đến mục tiêu phục vụ cho việc giảng dạy phịng thí nghiệm chun ngành Điều khiển Tự động hóa  Thu thập liệu góc nghiêng trực tiếp từ robot truyền máy tính thơng qua cổng UART sau vẽ đồ thị phần mềm Labview để đánh giá, so sánh kết thực nghiệm giữ giá trị góc nghiêng thơ giá trị sau qua lọc Complementary, lọc Kalman  Xây dựng thành công giải thuật cân bám theo vị trí ban đầu dựa thuật tốn PID Hình 5.1 Robot thực việc cân 5.2 Hướng phát triển  Tìm hiểu thêm thuật toán điều khiển khác như: fuzzy, nơ ron, điều khiển trượt… để tăng thêm hiệu cho thuật toán cân n 42  Cần xây dựng thêm giải thuật di chuyển điều khiển thông qua RF, Bluetooth dựa hệ điều hành Android để phát triển thành sản phẩm giải trí hồn thiện  Đề tài tiền đề cho việc xây dựng mơ hình xe hai bánh tự cân di chuyển địa hình phẳng n 43 TÀI LIỆU THAM KHẢO [1] Alwafi Husein, Attitude and altitude control of two wheel trirotor hybrid robot, Masters thesis, 2013 [2] Ayỗa GệầMEN, Design of two wheeled electric vehicle, Master’s thesis, 2011 [3] Christian sundin, Filip thorstensson, Autonomous balancing robot, Master’s thesis, 2012 [4] Maria R Bageant, Balancing a two- wheeled Segway robot, Bachelor’s thesis, 2006 [5] Welch, G and G Bishop, An Introduction to the Kalman Filter, 2006 [6] Đỗ Bình Nguyên, Điều khiển lắc ngược di động dùng phương pháp điều khiển phi tuyến, Luận văn thạc sĩ, 2012 n