Điều khiển cân bằng hệ pendubot

Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot Điều khiển cân bằng hệ pendubot

HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn TĨM TẮT Hệ thống thiếu cấu truyền động nhiều nhà nghiên cứu nước giới quan tâm, Pendubot ví dụ điển hình cho hệ thống thiếu cấu truyền động với hai bậc tự hoạt động mặt phẳng đứng Các nhà nghiên cứu đưa nhiều phương pháp điều khiển như: Thuật tốn cổ điển PID, tuyến tính hóa riêng phần, hồi tiếp trạng thái, giải thuật mờ… Trong đề tài tác giả áp dụng thuật tốn tồn phương tuyến tính LQR để điều khiển cân ngược hệ Pendubot Đề tài mô giải thuật điều khiển LQR để hệ thống cân sau thời gian nhỏ nhất, giải thuật thơng số thích hợp chọn lúc mơ mơ hình tốn học đối tượng nhúng vào DSP TMS320F28335 Mơ hình thực Pendubot sau thi công phần cứng nhúng giải thuật điều khiển giữ cân vị trí cân ngược ABSTRACT Underactuated mechanical system were paid attention by many researchers in Vietnam and all over the world, Pendubot is a typical example for the underactuated mechanical system with two degrees of freedom which operates in vertical plane The researchers came up with several methods of control such as PID algorithm, partial linearization, state feedback, fuzzy algorithm In this thesis, author applies Linear-Quadratic regulator method (LQR) to control balancing unstable inverted equilibrium Pedubot system This project simulates LQR control algorithm to balance system after the minimum time, algorithms and appropriate parameters have been selected during simulating mathematical model of the object which is embedded into DSP TMS320F28335 Actual model of Pendubot after constructing the hardware and embedding algorithm control will be kept in balance unstable inverted equilibrium position Trang iv HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn MỤC LỤC Quyết định giao đề tài Xác nhận cán hướng dẫn Lý lịch khoa học Trang i Lời cam đoan Trang ii Lời cảm ơn Trang iii Tóm tắt Trang iv Mục lục Trang v Chương 1: TỔNG QUAN Trang 01 1.1 Đặt vấn đề Trang 01 1.2 Tình hình nghiên cứu nước Trang 02 1.2.1 Tình hình nghiên cứu ngồi nước Trang 02 1.2.2 Tình hình nghiên cứu nước Trang 04 1.3 Giới thiệu đối tượng Pendubot Trang 07 1.3.1 Phần khí Trang 07 1.3.2 Phần điện Trang 08 1.4 Nhiệm vụ đề tài Trang 08 1.4.1 Mục đích nghiên cứu Trang 09 1.4.2 Phạm vi nghiên cứu Trang 09 Chương 2: CƠ SỞ LÝ THUYẾT Trang 10 2.1 Tổng quan Trang 10 2.2 Phương pháp điều khiển LQR Trang 10 2.2.1 Ổn định Lyapunov hệ tuyến tính hóa – tiêu chuẩn ổn định Lyapunov – phương pháp thứ Lyapunov Trang 10 2.2.2 Điều khiển tối ưu hệ tuyến tính với tiêu chất lượng dạng tồn phương – phương trình Riccati hệ liên tục Trang 12 Trang v HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn 2.2.3 Các bước thiết kế giải tốn tồn phương tuyến tính cho hệ thống liên tục Trang 14 2.2.4 Phương pháp điều khiển LQR áp dụng cho hệ Pendubot Trang 15 2.3 Cơ sở lý thuyết điều khiển PID Trang 17 2.4 Phương pháp nhận dạng thông số động Trang 27 2.4.1 Tổng quan Trang 27 2.4.2 Mơ hình hóa động Trang 27 Chương 3: NGHIÊN CỨU MƠ HÌNH Trang 28 3.1 Mơ hình động học hệ pendubot Trang 28 3.2 Mô hệ Pendubot Matlab/Simulink Trang 31 Chương 4: MÔ PHỎNG BỘ ĐIỀU KHIỂN HỆ PENDUBOT Trang 31 Xây dựng điều khiển giữ cân hệ Pendubot Trang 31 4.1 Mô điều khiển giữ cân LQR Trang 33 4.2 Mô điều khiển giữ cân LQR có nhiễu Trang 35 Chương 5: THIẾT KẾ THI CƠNG MƠ HÌNH THỰC TẾ VÀ ĐIỀU KHIỂN CÂN BẰNG HỆ PENDUBOT Trang 40 5.1 Giới thiệu Trang 40 5.2 Thiết kế khí hệ bendubot Trang 40 5.3 Mạch điều khiển hệ Pendubot Trang 42 5.3.1 Vi điều khiển DSP TMS320F28335 Trang 42 5.3.2 Module vi điều khiển TMS230F28335 Trang 44 5.3.3 Board điều khiển đọc giá trị encoder xuất xung PWM Trang 45 5.3.3.1 Sơ đồ nguyên lý chân DSP TMS320F28335 value module Trang 46 5.3.3.2 Sơ đồ nguyên lý khối đọc xung encoder Trang 47 Trang vi HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn 5.3.3.3 Sơ đồ nguyên lý khối phát xung PWM Trang 48 5.3.3.4 Sơ đồ nguyên lý khối giao tiếp với máy tính Trang 49 5.3.3.5 Sơ đồ nguyên lý mạch chuyển dổi xung vi sai qua xung NPN Trang 49 5.3.3.6 Khối cấp nguồn cho mạch điều khiển Trang 49 5.3.4 Mạch driver cầu H lái động Trang 51 5.4 Thiết kế chương trình điều khiển giữ cân hệ Pendubot Trang 51 5.4.1 Giới thiệu khối điều khiển thư viện hỗ trợ simulink Trang 51 5.4.2 Sơ đồ giải thuật chương trình điều khiển Trang 53 5.4.3 Khối thực hệ Pendubot thực tế Trang 55 5.4.3.1 Khối đọc xung Trang 55 5.4.3.2 Khối xử lý điều khiển lái động Trang 56 5.4.4 Khối truyền tín hiệu lên máy tính Trang 56 5.4.5 Chương trình điều khiển giữ cân hệ Pendubot dung phương pháp LQR Trang 57 5.4.5.1 Chương trình thực tế điều khiển Trang 57 5.5 Kết đo thực tế dùng LQR Trang 59 Chương 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CỦA ĐỀ TÀI Trang 60 6.1 Kết luận Trang 60 6.2 Đóng góp khoa học luận văn Trang 60 6.3 Hướng phát triển đề tài Trang 61 TÀI LIỆU THAM KHẢO Trang 62 Trang vii HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Danh mục hình Hình 1.1 Kết mơ ngõ vị trí hướng lên Trang 03 Hình 1.2 Kết mơ ngõ vị trí hướng lên có nhiễu Trang 03 Hình 1.3 Đáp ứng góc vận tốc góc hệ thống với phương pháp điều khiển lượng Trang 04 Hinh 1.4 Đáp ứng góc lệch q1, q2 lân cận vị trí top với điều khiển PID Trang 05 Hinh 1.5 Đáp ứng góc lệch q1, q2 lân cận vị trí Mid_L với điều khiển PID Trang 05 Hinh 1.6 Đáp ứng góc lệch q1, q2 lân cận vị trí top với điều khiển LQR Trang 05 Hinh 1.7 Đáp ứng góc lệch q1, q2 lân cận vị trí Mid_L với điều khiển LQR Trang 06 Hinh 1.8 Đáp ứng góc lệch q1, q2 lân cận vị trí top với điều khiển phản hồi trạng thái sử dụng khâu quan sát Luenberger Trang 06 Hinh 1.9 Đáp ứng góc lệch q1, q2 lân cận vị trí Mid_L với điều khiển phản hồi trạng thái sử dụng khâu quan sát Luenberger Trang 06 Hinh 1.10 Hệ thống Pendubot Trang 07 Hình 1.11 Các thành phần khí hệ Pendubot Trang 08 Hình 2.1 Khối động DC Trang 16 Hình 2.2 Khối động DC sau phân tích hàm truyền Trang 18 Hình 3.1 Mơ hình đơng lực Pendubot Trang 22 Hinh 3.2 Khối mô Pendubot Trang 25 Hình 3.3 Mơ mơ hình trạng thái hệ pendubot simulink Trang 25 Hình 4.1 Mơ hình mơ Simulink điều khiển Pendubot khơng có nhiễu tác động Trang 25 Trang viii HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Hình 4.2 Đáp ứng vị trí góc link1 hệ pendubot áp dụng điều khiển LQR Trang 27 Hình 4.3 Đáp ứng vị trí góc link2 hệ pendubot áp dụng điều khiển LQR Trang 27 Hình 4.4 Đáp ứng vận tốc góc link1 hệ pendubot áp dụng điều khiển LQR Trang 28 Hình 4.5 Đáp ứng vận tốc góc link2 hệ pendubot áp dụng điều khiển LQR Trang 28 Hình 4.6 Ngõ điều khiển Trang 29 Hình 4.7 Mơ hình mơ simulink điều khiển Pendubot có nhiễu tác động Tín hiệu nhiễu cho vào hệ pendubot Trang 29 Hình 4.8 Tín hiệu nhiễu đưa vào hệ pendubot mô Trang 30 Hình 4.9 Đáp ứng vị trí góc link1 hệ pendubot áp dụng điều khiển giữ cân có nhiễu Trang 30 Hình 4.10 Đáp ứng vị trí góc link2 hệ pendubot áp dụng điều khiển LQR giữ cân có nhiễu Trang 31 Hình 4.11 Đáp ứng vận tốc góc link1 hệ pendubot áp dụng điều khiển LQR giữ cân có nhiễu Trang 31 Hình 4.12 Đáp ứng vận tốc góc link2 hệ pendubot áp dụng điều khiển LQR giữ cân có nhiễu Trang 32 Hình 4.13 Tín hiệu ngõ điều khiển có nhiễu Trang 32 Hình 5.1 Encoder gắn link để đo vị trí link2 so với link1 Trang 34 Hình 5.2 Mơ hình pendubot thực tế đề tài Trang 35 Hình 5.3 Động DC có tích hợp encoder đo vị trí link1 Trang 35 Hình 5.4 Sơ đồ khối chức DSP TMS320F28335 Trang 37 Trang ix HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Hình 5.5 Board điều khiển TMS320F28335 Trang 38 Hình 5.6 Hình vẽ chi tiết chân, kích thước value module Trang 39 Hình 5.7 Hình sơ đồ chân TMS320F28335 value module Trang 40 Hình 5.8 Vị trí xếp chân value module Trang 41 Hình 5.9 IC đệm cho encoder Trang 41 Hình 5.10 Encoder ứng dụng xác định vị trí Trang 42 Hình 5.11 Sơ đồ nguyên lý mạch đệm phát xung điều khiển PWM Trang 42 Hình 5.12 Sơ đồ nguyên lý mạch giao thiếp SCI với máy tính Trang 42 Hình 5.13 Sơ đồ chuyển đổi xung vi sai qua xung NPN Trang 43 Hình 5.14 Sơ đồ mạch điều khiển trung tâm Trang 44 Hình 5.15 Bộ nguồn 24vdc thực tế cấp cho pendubot Trang 44 Hình 5.16 Sơ đồ nguyên lý mạch cầu H Trang 45 Hình 5.17 Mạch cầu H thực tế Trang 46 Hình 5.18 Quá trình lập trình viết code đổ xuống board vi điều khiển Trang 46 Hình 5.19 Thư viện Target Preferences Trang 47 Hình 5.20 Thư viện ngoại vi hỗ trợ cho dòng C28x3x Trang 47 Hình 5.21 Sơ đồ giải thuật điều khiển cân Pendubot Trang 48 Hình 5.22 Thiết lập chu kỳ điều khiển cho điều khiển Simulink Trang 49 Hình 5.23 Khối đọc encoder link1 Trang 49 Hình 5.24 Khối đọc encoder link2 Trang 50 Hình 5.25 Chương trình điều khiển hệ Pendubot thực tế Trang 51 Hình 5.26 Đáp ứng thực tế góc link1 Trang 53 Hình 5.27 Đáp ứng thực tế góc link Trang 53 Hình 5.28 Đáp ứng thực tế điện áp ngõ Trang 55 Trang x HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn CHƯƠNG TỔNG QUAN 1.1 Đặt vấn đề Trong năm trở lại chưa việc đề xuất phát triển robot thử nghiệm robot lại đa dạng Cuộc sống đương đại gợi ý cho nhà khoa học xu hướng chế tạo robot có tính ứng dụng vào thực tiễn cao Trong lĩnh vực phải kể đến dự án nghiên cứu cánh tay robot sử dụng lĩnh vực điều khiển khác lĩnh vực lắp đặt linh kiện IC vào bo mạch, hàn khung xe, máy bay Trong dây chuyền lắp ráp cần độ xác tỉ mỉ môi trường độc hại nguy hiểm lắp ráp sửa chữa cấu trúc lò phản ứng hạt nhân, mơi trường lịng đất hay khơng gian vũ trụ Trong lĩnh vực người máy người ta bắt đầu quan tâm đến việc điều khiển máy móc với số ngõ vào điều khiển nhỏ số cấu chấp hành Trong đó, chiều dài vector cấu hình hệ thống nhỏ chiều dài vector điều khiển Những hệ thống máy móc gọi hệ thống thiếu cấu truyền động Hệ thống thực nhiệm vụ phức tạp với số lượng cấu truyền động lại có ưu điểm như: nhẹ, rẻ, tiêu hao lượng độ tin cậy cao Với ưu điểm này, hệ thống thiếu cấu truyền động nhận quan tâm lớn năm trở lại Mặc khác phức tạp tính phi tuyến dao động hệ thống lớn nên vấn đề điều khiển cho loại truyền động khó khăn phức tạp Có nhiều mơ hình đặt trưng cho hệ thống thiếu cấu truyền động nhà khoa học nghiên cứu chế tạo như: Acrobot, mobile robot, Gyrobot, Pendubot, Con lắc ngược hai bật tự do, Pendubot ví dụ điển hình cho hệ thống thiếu cấu truyền động Pendubot robot có hai bật tự hoạt động mặt phẳng đứng Có cấu truyền động nằm khớp nối thứ nhất, khớp thứ nối với khớp thứ trục liên kết tự Khớp thứ gắn với cấu truyền động, động điều khiển momen Mục tiêu điều khiển hệ chuyển hệ robot hai bật tự từ vị trí cân lên vị trí cân giữ cân vị trí cân Trang HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Chúng ta thấy vấn đề điều khiển cân Pendubot nhà khoa học giới quan tâm nghiên cứu cải tiến, có nhiều giải thuật đề xuất để phát triển đề tài điều khiển Nhiều nhà báo, hội nghị khoa học đưa từ công trình nghiên cứu dựa vào phương pháp điều khiển cổ điển điều khiển hồi tiếp tuyến tính hóa riêng phần, hồi tiếp trạng thái, điều khiển dựa cân lượng, tiếp điều khiển đại phát triển nhà khoa học lại có hướng phát triển nhúng giải thuật mờ, mờ trượt thích nghi vào hệ thống Với ứng dụng rộng rãi luật điều khiển hệ thống thiếu cấu truyền động xu hướng phát triển robot ngày nay, học viên định nghiên cứu đề tài “Điều khiển cân hệ Pendubot” để hiểu rõ luật điều khiển Đề tài áp dụng luật điều khiển LQR để thực việc điều khiển hệ thống thơng số q trình cân nhằm nâng cao độ ổn định nội dung mà đề tài hướng đến 1.2 Tình hình nghiên cứu ngồi nước 1.2.1 Tình hình nghiên cứu ngồi nước Những đề xuất ban đầu cho hệ thống thiếu cấu truyền động có nhiều nhà nghiên cứu đề xuất để cân cho Pendubot Như [1] Spong and Block áp dụng điều chỉnh phương tuyến tính LQR để cân Pendubot [2] Fantoni, Lozano and Spong sử dụng lượng để cân mô Pendubot [3] Zhang anh Tam đề xuất dùng điều khiển lai để cân Pendubot, điều khiển lai bao gồm kiểm soát thời gian rời rạc thời gian liên tục kiểm sốt lúc Cịn [4] Xio Qing Ma điều khiển cân Pendubot thông qua điều khiển mờ Kết hợp lý thuyết điều chỉnh tuyến tính với cơng nghệ mờ Takagi-Sugeno để kiểm sốt theo dõi [5] Z.Wang, Y.Guo thực lái tay, xem lắc ngược điện tử, hệ thống nghiên cứu trường hợp Các cánh tay hướng ngược lắc giống Pendubot Phát triển điều khiển cân phát triển nhúng cho lái [6] Pavol Seman sử dụng điều khiển LQ để điều khiển mơ hình Pendubot Trang HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Sau ta xem số kết qủa số báo viết như: [6] Control of laboratory model of pendubot Trong báo tác giá sử dụng giải thuật LQR để cân hệ pendubot số vị trí Kết đạt báo Hình 1.1: Kết mơ ngõ vị trí hướng lên Hình 1.2: Kết mơ ngõ vị hướng lên có nhiễu Trong [2] Isabelle Fantoni, Rogelio Lozano, and Mark W.Spong (Energy Based Control of the Pendubot: Isabelle Fantoni, Rogelio Lozano, and Mark W.Spong) đề xuất phương pháp đưa Pendubot lên vị trí cân dựa hướng tiếp cận điều khiển lượng Trong luật điều khiển thiết kế dựa vào thuyết ổn định Lyapunov, tổng lượng hệ thống biến đổi thành để đưa Pendubot lên vị trí cân ngược Đặc tính hệ thống điều khiển vịng kín với luật biến đổi lượng Ưu điểm phương pháp đạt mục tiêu đưa hệ thống tới điểm cân ngược thẳng đứng hệ thống dao động trái hay phải xung quanh vị trí cân lân cận nhỏ xác định dao động lại vị trí cân cách dễ dàng Tuy nhiên, nhược điểm phương pháp luật điều khiển hướng tiếp cận lượng áp dụng, điều khiển vịng kín hệ thống khó xác định phức tạp Pendubot, luật điều khiển hạn chế miền giới hạn điều khiển Kết mô phỏng: Trang HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn 5.3.3.4 Sơ đồ nguyên lý khối giao tiếp với máy tính Sơ đồ nguyên lý khối giao tiếp với máy tính: sử dụng IC giao tiếp FT232R chuyển đổi tín hiệu giao tiếp SCI/UART từ vi điều khiển thành giao tiếp USB để truyền tín hiệu lên máy tính Hình 5.12: Sơ đồ ngun lý mạch giao thiếp SCI với máy tính 5.3.3.5 Sơ đồ nguyên lý mạch chuyển đổi xung vi sai qua xung PNP Hình 5.13: Sơ đồ chuyển đổi xung vi sai qua xung NPN 5.3.3.6 Khối cấp nguồn cho mạch điều khiển Có nhiệm vụ cung cấp nguồn Dc 24v cho mạch cầu H lái động Để cung cấp đủ công suất cho DSP, encoder, IC đệm mạch giao tiếp máy tính Trang 48 HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Hình 5.14: Sơ đồ mạch điều khiển trung tâm Hình 5.15: Bộ nguồn 24vdc thực tế cấp cho Pendubot Kết thi công board mạch điều khiển thực tế: Sau thi công lắp đặt mạch điều khiển hoạt động tốt, giá trị encoder đo xác khơng bị nhiễu Xung PWM xuất xác, mạch giao tiếp với máy tính chạy ổn định Trang 49 HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn 5.3.4 Mạch driver cầu H lái động Hình 5.16: Sơ đồ nguyên lý mạch cầu H Kết thi công thực tế mạch cầu H + Board cầu H-Brige dùng IC kích MC33833 hang free scale + Phần công suất dùng POWER MOSFETs IRF3205 có thơng số VDSS=55v, RDS 100KHz + Ngõ vào opto cách ly PWM+, PWM-, DIR+, DIR+ Sử dụng IC kích Fet chuyên dụng MC338833 freescale nên cycle đạt 100% ưu điểm so với IC kích fet khác + Có led báo nguồn, báo xung chiều + Board có hai nút nhấn DIR, PWM thích hợp cho việc test board tay Trang 50 HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn Hình 5.17: Mạch cầu H thực tế 5.4 Thiết kế chương trình điều khiển Pendubot 5.4.1 Giới thiệu khối điều khiển thư viện hỗ trợ simulink Để thiết kế chương trình ta sử dụng thư viện Matlab/simulink hỗ trợ lập trình đổ xuống chip TMS320F28335, taget support packet C2000lib Hỗ trợ tất MCU dòng vi điều khiển C2000 TI Trước hết ta phải lập trình khối matlab/ simulink, sau trình biên dịch hỗ trợ phát project mã C liên kết với Code composer studio v3.3 (CCS v3.3), sau CCS làm nhiệm vụ mơi trường build chạy debug chương trình Hình 5.18: Quá trình lập trình viết code đổ xuống board vi điều khiển Việc kết hợp thư viện Target For TI C2000 tích hợp sẵn Simulink Matlab với Code Composer Studio (CCS) hãng Taxes Intrument cho phép Trang 51 HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn người sử dụng dịch chương trình Simulink sang ngơn ngữ C chạy chip DSP C2000 TI C280x fixed point DSP, C281x fixed point DSP, C28x3x floating point DSP, … Hình 5.19: Thư viện target Preferences Các module ngoại vi xuất nhập I/O, đọc xung encoder QEP, điều chế độ rộng xung PWM, chuyển đổi ADC, khối truyền I2C, SCI, CAN DSP đọc hỗ trợ đầy đủ cấu hình dễ dàng trực quan Hình 5.20: Thư viện ngoại vi hỗ trợ cho dòng C28x3x Trang 52 HVTH: Phạm Văn Lợi GVHD: TS Trương Đình Nhơn 5.4.2 Sơ đồ giải thuật chương trình điều khiển Bắt đầu Khởi tạo hoạt động cho thiết bị ngoại vi Đọc xung phát từ encoder encoder No -π/5