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 Nhóm 20186312 20186310 20181660 20181381 Mục lục Mục lục Mục lục i Phần 1: Giới thiệu chung 1.1 Wheeled Mobile Robot 1.2 Một số loại Wheeled Mobile Robot .1 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 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 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; 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 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 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ự 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 ( X m , Ym ) Vector trạng thái xe hệ tọa độ tổng quát là: Phần 2: Mô hình hóa đối tượng Mobile Robot bánh  x (t )    q (t ) =  y (t )   t   ( ) (2.1) 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:  vR (t ) − vL (t )  (t ) = L  v(t ) = vL (t ) + vR (t )  (2.2) Phương trình động học ngồi xe hệ tọa độ tổng quát xác định sau:  x ( t ) = cos ( ( t ) ) v ( t )   y ( t ) = cos ( ( t ) ) v ( t )   ( t ) =  ( t ) (2.3) Để 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  cos 0  v    y = sin           1   (2.5) Như với đầu vào điều khiển vector vận tốc v =  v   , ta có ma trận S T với cột S trường vector thể hướng di chuyển xe: Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh cos 0 S =  sin  0  1 (2.6) cos   0 Trong phương chuyển động là: s1 = sin   ; s = 0     0  1  Phương trình 2.5 viết lại sau:  x  cos 0 v q =  y  =  sin  0   = S.v     1   (2.7) 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  =  − x2 sin ( +  ) + y2 cos ( +  ) = (2.8) Trong đó: ( x2 , y2 ) : tọa độ bánh trước Mối quan hệ ( x2 , y2 ) với ( x, y ) sau:  x2 = x + d cos    y2 = y + d sin  (2.9)  x2 = x −  d sin    y2 = y +  d cos  (2.10) Từ suy ra: Thay 2.10 vào 2.8 ta thu được: Phần 2: Mơ hình hóa đối tượng Mobile Robot bánh − x.sin  + y.cos  =  − x.sin ( +  ) + y.cos ( +  ) +  d cos  = (2.11) Từ 2.11, ta có ma trận ràng buộc A sau: cos   − sin  A=  − sin ( +  ) cos ( +  ) d cos  0  (2.12) Như ta có: A.q = (2.13) Từ 2.7 2.13 suy ra: A.S.v = Nói cách khác: A.S = (2.14) 2.2 Mơ hình động lực học Mơ hình xe bánh xét mơ hình có bánh sau bánh chủ động điều khiển động cơ, bánh trước bánh tự Hai cấu chấp hành sinh momen  R  L Mơ hình động lực học chuyển động xe mô tả công thức Lagrange sau: d  dt  qk m   P − + + g +  = f −  j a jk   k dk k  q  q j =1 k k  Trong đó: : hiệu động hệ thống P : công suất tổn hao ma sát k : số thành phần tọa độ tổng quát qk g k : trọng lực phân tích theo phương qk  d : thành phần nhiễu theo phương qk k (2.15) 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 động học vịng ngồi Dựa theo nội dung trình bày phần 3, ta thiết kế điều khiển vịng ngồi cho Mobile robot dựa theo hàm ứng viên Lyapunov Mục đích điều khiển : ( q − qref ) → t →  Quỹ đạo đặt robot ( xref ( t ) ; yref ( t ) )  y (t )   ref ( t ) = arctan  ref  x ( t )   ref   qref  xref    =  yref     ref  Mặt khác ta lại có: v = x + y ref ref  ref   yref d  = arctan   ref  dt    xref   xref yref − yref xref   = 2 xref + yref  Như vận tốc tham chiếu robot di chuyển quỹ đạo tham chiếu vref  Vref =   ref  Sai lệch vị trí robot hệ trục tọa độ gắn với xe xác định là: 24 Phần 4: Thiết kế điều khiển cho đối tượng Mobile Robot bánh  ex   cos    qe = ey  = R.( q − q ref ) =  − sin   e     sin    x − xref    cos    y − yref     −  ref   cos  ( x − xref ) + sin  ( y − yref )     = − sin  ( x − xref ) + cos  ( y − yref )     −  ref     Đạo hàm sai lệch: ex = − sin  ( x − xref ) + cos  ( x − xref ) +  cos  ( y − yref ) + sin  ( y − yref )   e y = − cos  ( x − xref ) − sin  ( x − xref ) −  sin  ( y − yref ) + cos  ( y − yref )  e =  − ref ( ) ( ) ex =  − sin  ( x − xref ) + cos  ( y − yref )   + ( x cos  + y sin  ) − ( xref cos  + yref sin  )    ey =  − cos  ( x − xref ) − sin  ( y − yref )  + ( − x sin  + y cos  ) + ( xref sin  − yref cos  )   e =  − ref  Mặt khác:  x = v.cos   y = v.sin    x = v cos  ref ref  ref  yref = vref sin ref  25 Phần 4: Thiết kế điều khiển cho đối tượng Mobile Robot bánh ex = .e y + v ( cos  + sin  ) − vref ( cos  ref cos  + sin  ref sin  )    e y = −.ex + v ( − cos  sin  + sin  cos  ) + vref ( cos  ref sin  − sin  ref cos  )  e =  − ref ex = e y + v − vref cos e   e y = ex + vref sin e  e =  − ref Suy ra:  ex   − cos e   qe =  ey  =  sin e  e     0 1 e y  v   v ref 0  + 0 ex      ref    0        Lựa chọn điều khiển:  vc   vref cos e + v fb     =   +  ref fb  c   v fb = k x ex Trong đó:   = k v e + k sin e  fb y ref y   k = k = 2  + gv x  ref ref  k y = gvref  g = 85 Chọn   = 0.9 4.2 Thiết kế điểu khiển động lực học vòng  v  vc   0  Mục tiêu điều khiển:    −    →   t →     c   0     v v Ta có:   = M −1  E  R  − V           L  Trong đó: 26 Phần 4: Thiết kế điều khiển cho đối tượng Mobile Robot bánh   m 0  cos   cos  sin      m   sin   =  m  T M = S M S =      J       0 J          cos  cos   1  cos  sin      1  T  sin  sin   =  L − L  E = S E =    1 r r     L −L  2      2     m 0   − sin    cos  sin     m    cos   = 0  T V = S M.S =    0       0 J   0   v   v =   = M −1.E.u   v  vc  Sai lệch vận tốc là: v e = v − v c =   −     c  Đạo hàm sai lệch: v e = v − v c = M −1.E.u − v c Chọn luật điều khiển: u = E−1.M.( C.v e + v c ) Trong ma trận C ma trận điều khiển 2x2 xác định dương Khi đó: ( ) v e = M −1.E E −1.M.( −C.v e + v c ) − v c = −C.v e  v e = e − C.t  lim v e = t →  200  Chọn C =    100  27 Phần 5: Mô hệ thống điều khiển Matlab Simulink Phần 5: Mô hệ thống điều khiển Matlab Simulink 5.1 Mơ mơ hình Knematic Sử dụng khối Matlab function Simulink function [x_dot, y_dot, phi_dot] = Kinematic(v, w, phi) % % % % q_dot = S * V = [ x_dot y_dot phi_dot ; ; ] % ma tran van toc V V = [ v ; w]; % S: ma tran cac phuong chuyen dong co ban S = [ cos(phi) ; sin(phi) ; ]; % Ma tran dau ra: q_dot = S * V; % Dau ra: x_dot = q_dot(1); y_dot = q_dot(2); phi_dot = q_dot(3); end 28 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 = = = = = 0.033 0.16 0.002 ; ; ; ; ; % 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 M = [ m 0 ; m ; 0 J ] ; % S: ma tran cac phuong chuyen dong co ban S = [ cos(phi) ; sin(phi) ; ]; % E: Ma tran chuyen tu khong gian truyen dong sang toa tong quat E = 1/r *[ cos(phi) cos(phi) ; sin(phi) sin(phi) ; L/2 -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 ; 29 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 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 R = [ cos(phi) sin(phi) 0; -sin(phi) cos(phi) 0; 0 1]; % % 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 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 L J = = = = ; 0.033; 0.16 ; 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 Phần 5: Mô hệ thống điều khiển Matlab Simulink Lấy trạng thái ban đầu robot  x ( ) ; y ( ) ; ( )  = 1,2;1,4;0 Quỹ đạo tham chiếu sử dụng: T T   2   xref = 1.1 + 0.7sin  30 t       y = 0,8 + 0,7sin  4 t     ref  30  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 Phần 5: Mô hệ thống điều khiển Matlab Simulink   8  x = 1.1 + 0.7sin  t  ref   30    y = 0,8 + 0,7sin  16 t     ref  30  Kết bám quỹ đạo robot: So sánh với kết 5.5.1: 34 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 2 0 thành: C =   0 1 Kết mô bám quỹ đạo robot: So sánh với quỹ đạo kịch thứ nhất: 35 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 ngồi đế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 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 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, (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 https://en.wikipedia.org/wiki/Mobile_robot 38 ... 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. .. 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ự 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. .. Thành phần mô hình động lực học: −1 ( v = M Eu − V ) Trong ma trận M, E, V xác định 2 .32 12 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