BÀI tập lớn học PHẦN mô HÌNH hóa mô PHỎNG hệ THỐNG điều KHIỂN đề bài mô hình hóa và mô phỏng hệ thống điều khiển đối tượng mobile robot loại 3 bánh

ĐẠI HỌC BÁCH KHOA HÀ NỘI TRƯỜNG ĐIỆN – ĐIỆN TỬ KHOA TỰ ĐỘNG HÓA BÁO CÁO BÀI TẬP LỚN HỌC PHẦN MƠ HÌNH HĨA & MƠ PHỎNG HỆ THỐNG ĐIỀU KHIỂN ĐỀ BÀI: Mơ hình hóa mơ hệ thống điều khiển đối tượng Mobile Robot loại bánh GVHD: Ts Vũ Thị Thúy Nga Nhóm sinh viên thực hiện: Chu Hải Long Nguyễn Khánh Châu Trịnh Minh Trương Hải Đăng Hà Nội, 2022 TIEU LUAN MOI download : skknchat123@gmail.com Mục lục Mục lục Mục lục i Phần 1: Giới thiệu chung .1 1.1 Wheeled Mobile Robot 1.2 Một số loại Wheeled Mobile Robot 1.2.1 Differential Drive 1.2.2 Bicycle Drive 1.3 Lựa chọn mơ hình WMR bánh Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh 2.1 Mơ hình động học 2.2 Mơ hình động lực học Phần 3: Các phương án điều khiển đối tượng Mobile Robot 13 3.1 Tổng quan điều khiển 13 3.2 Các hướng tiếp cận .14 3.2.1 Điều khiển hướng điều khiển tịnh tiến 14 3.2.2 Các hướng tiếp cận 15 3.3 Điều khiển bám quỹ đạo 18 3.3.1 Bám quỹ đạo sử dụng hướng tiếp cận .18 3.3.2 Phân tích thành phần feedforward thành phần feedback 18 3.3.3 Tuyến tính hóa phản hồi 19 3.3.4 Phát triển mơ hình sai lệch theo dõi quỹ đạo động học 20 3.3.5 Bộ điều khiển tuyến tính 21 Phần 4: Thiết kế điều khiển cho đối tượng Mobile robot bánh 24 4.1 Thiết kế điều khiển động học vịng ngồi 24 4.2 Thiết kế điểu khiển động lực học vòng 26 Phần 5: Mô hệ thống điều khiển Matlab Simulink 28 5.1 Mơ mơ hình Knematic 28 5.2 Mơ mơ hình Dynamic 29 i TIEU LUAN MOI download : skknchat123@gmail.com Mục lục 5.3 Bộ điều khiển vịng ngồi Kynematic Controller 30 5.4 Bộ điều khiển vòng Dynamic Controller 31 5.5 Kết mô 32 5.5.1 Kết mô với kịch thứ 32 5.5.2 Kết mô kịch thứ hai 33 5.5.3 Kết mô với kịch thứ ba .35 5.5.4 Kết mô với kịch thứ tư 36 Tài liệu tham khảo .38 ii TIEU LUAN MOI download : skknchat123@gmail.com Phần 1: Giới thiệu chung Phần 1: Giới thiệu chung 1.1 Wheeled Mobile Robot Wheeled Mobile Robot (WMR) loại robot có có khả di chuyển bánh xe môi trường xung quanh WMR “tự động”, nghĩa có khả tự điều chỉnh hướng di chuyển môi trường mà không cần đến thiết bị dẫn hướng vật lý hay điện Thành phần WMR bao gồm điều khiển, cảm biến, cấu chấp hanh (động cơ, bánh xe) hệ thống điện 1.2 Một số loại Wheeled Mobile Robot 1.2.1 Differential Drive Differrential Dive loại WMR có bánh với bánh bố trí đồng trục bánh chủ động điều khiển được, tức chúng gắn với cấu chấp hanh (động cơ) riêng biệt để điều khiển riêng bánh Ở loại xe thơng thường khó để đứng trạng thái cân tự nhiên nên sử dụng thực tế 1.2.2 Bicycle Drive Khác với Differrential Dive, Bicycle Drive có bánh, bánh bố trí đường thẳng, thơng thường có bánh chủ động bánh điều khiển góc lái, cấu tạo giống xe đạp Loại robot gặp tính ứng dụng thực tiễn khơng cao 1.2.3 Tricycle Drive Tricycle Drive kết hợp loại WMR nêu trên, có bánh xe bánh sau bố trí đồng trục, bánh trước làm bánh lái; TIEU LUAN MOI download : skknchat123@gmail.com Phần 1: Giới thiệu chung bánh xe gắn với cấu chấp hanh để điều khiển bánh cịn lại để tự sử dụng cấu chấp hành để điều khiển bánh lái phía trước với tốc độ góc lái mong muốn 1.2.4 Car Drive WMR loại có cấu tạo tương tự ô tô với bánh trước bánh thay đổi góc lái 1.2.5 Omni Robot Là loại robot kế đặc biệt với trục bánh đồng quy điểm, đặc điểm giúp cho robot di chuyển theo phương tới điểm Ngoài loại WMR vừa nêu trên, nhiều loại WMR khác nữa, số ví dụ điển hình WMR TIEU LUAN MOI download : skknchat123@gmail.com Phần 1: Giới thiệu chung 1.3 Lựa chọn mơ hình WMR bánh Như giới thiệu trên, WMR loại bánh có loại Tricycle Drive (hay Nonholonomic) Omni Robot, Tricyle Drive chia tiếp thành loại nhỏ tùy thuộc vào việc bánh gắn cấu chấp hành Hai bánh sau truyền động Robot nonholonomic Bánh trước truyền động Omnirobot Ba bánh độc lập Đối với đề tài này, nhôm lựa chọn mơ hình robot Tricycle Drive với bánh sau bánh chủ động gắn với cấu chấp hanh để điều khiển bánh trước bánh tự TIEU LUAN MOI download : skknchat123@gmail.com Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh 2.1 Mơ hình động học Mơ hình xe mơ tả hình H2.1: Trong đó: ICR: tâm quay tức thời xe R(t): bán kính tức thời quỹ đạo chuyển động xe : Tốc độ góc xe quanh tâm ICR v : vận tốc dài theo phương dọc xe r : bán kính bánh xe : góc bánh lái so với trục Om X m Mơ hình xe đặt hệ trục tọa độ tổng quát (X g ,Yg ), hệ trục tọa độ chuyển động gắn với xe tổng quát là: TIEU LUAN MOI download : skknchat123@gmail.com Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh x (t ) q (t )= (2.1) (t ) Do chuyển động xe nhờ truyền động bánh sau từ điều chỉnh hướng xe di chuyển, nên vận tốc dài xe theo phương dọc xe xác định bởi: (t) = (2.2) v (t) = Phương trình động học ngồi xe hệ tọa độ tổng quát xác định sau: x (t y (t ) = cos ( (t )).v (t ) ) = cos ( (t )).v (t ) ( t ) = (t ) Để ngắn gọn cách trình bày, ta tạm bỏ qua phụ thuộc đại lượng vào thời gian, hệ phương trình viết lại sau: x = cos v y = sin v (2.4) Hay ta viết dạng ma trận sau: x (2.5) Như với đầu vào điều khiển vector vận tốc v = v T , ta có ma trận S với cột S trường vector thể hướng di chuyển xe: TIEU LUAN MOI download : skknchat123@gmail.com Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh (2.6) S= Trong phương chuyển động là: Phương trình 2.5 viết lại sau: (2.7) q= Tuy vậy, nói xe chuyển động dọc theo bánh xe, chuyển động trượt sang bánh Do chuyển động xe chịu ràng buộc sau: − x sin + y.cos = (2.8) Trong đó: (2 Mối quan hệ x ( (2.9) Từ suy ra: (2.10) Thay 2.10 vào 2.8 ta thu được: Phần 5: Mô hệ thống điều khiển Matlab Simulink 5.2 Mơ mơ hình Dynamic Sử dụng khối Matlab function Simulink function [v_dot, w_dot] = Dynamic(tau_r, tau_l, phi, v, w) % % % % % % Cac thong so cua xe m: khoi luong xe (kg) r: ban kinh banh xe (m) L: khoang cach giua banh xe (m) J: moment quan tinh cua xe (kg.m^2) d_G: lech cua khoi tam so voi diem chinh giua truc banh xe (m) m r L J d_G = % vector momen dau vao u= [ tau_r ; tau_l]; % ma tran van toc V V = [ v ; w]; % M: ma tran khoi luong va quan tinh he thong % S: ma tran cac phuong chuyen dong co ban S = [ cos(phi) ; sin(phi) ; 1 ]; % E: Ma tran chuyen tu khong gian truyen dong sang toa tong quat E = 1/r *[ cos(phi) sin(phi) L/2 cos(phi) ; sin(phi) ; -L/2 ]; % S_dot: dao ham ma tran S S_dot = [ -w*sin(phi) ; w*cos(phi) ; 0 ]; % Xac dinh cac ma tran trung gian doi bien M_=S'*M*S TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink V_ = S' * M * S_dot * V; E_=S'*E; % Ma tran dau ra: V_dot = inv(M_) * (E_*u - V_); % Dau ra: v_dot = V_dot(1); w_dot = V_dot(2); end 5.3 Bộ điều khiển vịng ngồi Kynematic Controller Đầu vào điều khiển tư tham chiếu tư phản hồi vê từ đối tượng; đầu điều khiển giá trị tốc độ dài tốc độ góc điều khiển cho điều khiển vòng 30 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink function [v,w] = fcn(x, y, phi, x_ref, y_ref, phi_ref, v_ref, w_ref) q_REF = [x_ref;y_ref;phi_ref]; % x_ref = q_REF(1); % y_ref = q_REF(2); % phi_ref = q_REF(3); V_ref = [v_ref;w_ref]; % v_ref = V_REF(1); % w_ref = V_REF(2); % q feedback: q_FB = [x;y;phi]; % x = q_FB(1); y = q_FB(2); phi = q_FB(3); % Ma tran chuyen % % Sai lech vi tri e = R * (q_REF-q_FB); eX = e(1); eY = e(2); ePHI = e(3); % he so bo dieu khien zeta = 0.9; g=85; Kx = * zeta * sqrt(w_ref^2 + g * v_ref^2); Kphi = Kx; Ky = g * v_ref; % bo dieu khien: v = v_ref * cos(ePHI)+ Kx * eX; w = w_ref + Ky * v_ref * eY + Kphi * sin(ePHI); end 5.4 Bộ điều khiển vòng Dynamic Controller 31 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink Đầu vào điều khiển tín hiệu vận tốc điều khiển từ điều khiển vịng ngồi tín hiệu tốc độ phản hồi từ đối tượng; đầu giá trị moment đặt vào mơ hình Dynamic cảu đối tượng function [tau_R, tau_L] = dynamic_ctr(v_REF, w_REF, v_REF_dot, w_REF_dot, v_FB, w_FB) V_REF = [v_REF;w_REF]; V_REF_dot = [v_REF_dot; w_REF_dot]; V_FB = [v_FB; w_FB]; % Cac thong so cua xe % m: khoi luong xe (kg) % r: ban kinh banh xe (m) % L: khoang cach giua banh xe (m) % J: moment quan tinh cua xe (kg.m^2) % d_G: lech cua khoi tam so voi diem chinh giua truc banh xe (m) m = ; r = 0.033; L =0.16; J = 0.002 ; M_ = [ m 0; J]; E_ = 1/r * [ L/2 ; -L/2 ]; % V_dot = inv(M_) * E_ * u err = - V_REF + V_FB; K = [ 200 0; 100]; u = (inv(E_)*M_) * (-K * err + V_REF_dot); tau_R = u(1); tau_L = u(2); end 5.5 Kết mô 5.5.1 Kết mô với kịch thứ Với kịch mô thứ này, ta lựa điều khiển Phần nêu 32 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink Lấy trạng thái ban đầu robot x (0 ); y (0 ); đạo tham chiếu sử dụng: (0 ) = 1,2;1, Quỹ 4;0 T x ref y ref Kết bám quỹ đạo robot: Song, để kiểm tra khả tác động tới mơ hình điều khiển ta thử thêm số kịch điều khiển khác phần 5.5.2 Kết mô kịch thứ hai Vị trí ban đầu quỹ đạo tham chiếu ta sử dụng tương tự phần 5.5.1, song với kịch ta thử kiểm tra tính đáp ứng hệ thống quỹ đạo tham chiếu thay đổi với tốc độ nhanh so với với tần số nhanh lần: 33 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink x ref y ref Kết bám quỹ đạo robot: So sánh với kết 5.5.1: 34 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink 5.5.3 Kết mô với kịch thứ ba Ở phần ta kiểm tra độ tác động điều khiển vịng tới mơ hình thay đổi ma trận xác định dương điều khiển động lực học thành: C= Kết mô bám quỹ đạo robot: So sánh với quỹ đạo kịch thứ nhất: 35 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink Nhận xét: Như khả bám quỹ đạo robot thay đổi ma trận xác định dương điều khiển vịng khơng thay đổi nhiều trường hợp Điều đáp ứng điêu khiển vòng nhanh so với vịng ngồi dù có thay đổi ảnh hưởng tới hệ thống nhỏ 5.5.4 Kết mô với kịch thứ tư Đối với kịch này, ta xét đến khả tác động điều khiển vòng đến hệ thống cách thay đổi kệ số tắt dần = 0.75 hệ số g = 200 Khi kết bám quỹ đạo thu sau: So với kịch thứ : 36 TIEU LUAN MOI download : skknchat123@gmail.com Phần 5: Mô hệ thống điều khiển Matlab Simulink Nhận xét: Như ta thấy rằng, so với thay đổi điều khiển vòng trong, việc thay đổi điều khiển vịng ngồi tác động nhiều tới hệ thống 37 TIEU LUAN MOI download : skknchat123@gmail.com Tài liệu tham khảo Tài liệu tham khảo [1] WHEELED MOBILE ROBOTICS, Gregor Klancar, Andrej Zdešar, Sašo Blažic, Igor Škrjanc [2] Mobile robot, 2016, https://en.wikipedia.org/wiki/Mobile_robot (accessed 18.07.16) [3] “Design and implementation of an adaptive sliding-mode dynamic controller for wheeled mobile robots”, Chih-Yang Chen, Tzuu-Hseng S Li *, Ying-Chieh Yeh, Cha-Cheng Chang 38 TIEU LUAN MOI download : skknchat123@gmail.com ... mơ hình WMR bánh Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh 2.1 Mơ hình động học 2.2 Mô hình động lực học Phần 3: Các phương án điều khiển đối tượng Mobile Robot. .. MOI download : skknchat1 23@ gmail.com Phần 3: Các phương án điều khiển đối tượng Mobile Robot Phần 3: Các phương án điều khiển đối tượng Mobile Robot 3. 1 Tổng quan điều khiển Như trình bày trên,... nhận 23 TIEU LUAN MOI download : skknchat1 23@ gmail.com Phần 4: Thiết kế điều khiển cho đối tượng Mobile Robot bánh Phần 4: Thiết kế điều khiển cho đối tượng Mobile robot bánh 4.1 Thiết kế điều khiển