Đang tải... (xem toàn văn)
thiết lập các mô hình cơ khí cơ sở dựa trên những tính toán về động học và yêu cầu về cân bằng của xe,các phương trình vi phân chuyển động của xe sau đó sẽ chuyển sang tính toán cho phần điều khiển sau đó
Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. CHƯƠNG 3. THIẾT KẾ BỘ ĐIỀU KHIỂN CHO ROBOT HAI BÁNH TỰ CÂN BẰNG 3.1. Mô hình hóa robot hai bánh tự cân bằng. [1],[10],[11] Mô hình hệ thống của robot hai bánh tự cân bằng được phân tích ở Hình 3.1. Hình 3.1: Mô hình phân tích của robot hai bánh tự cân bằng Bảng 3.1: Bảng ký hiệu và giá trị các thông số của robot hai bánh tự cân bằng Ký hiệu Thông số Giá trị [đơn vị] W x , W y Vị trí của bánh xe theo hướng trục x, trục y [m] B x , B y Vị trí của trọng tâm thân robot theo hướng trục x và hướng trục y [m] Trích t ừ Lu ậ n V ă n Th ạ c S ĩ – Tác gi ả : Nguyễn Gia Minh Thảo . Khoa Đ i ệ n- Đ i ệ n T ử , Đ H Bách Khoa TpHCM. WL WR W M M M = = Khối lượng bánh xe, bánh xe trái và bánh xe phải có khối lượng bằng nhau 0.5 [kg] B M Khối lượng qui đổi tại trọng tâm của thân robot 7 [kg] B θ θ = Góc lật ở thân robot. [rad] δ Góc quay [rad] W R Bán kính bánh xe 0.075 [m] L Khoảng cách từ trọng tâm thân robot đến trục z của hai bánh xe 0.36 [m] D Khoảng cách giữa hai bánh xe 0.35 [m] , L R C C Moment tác dụng của động cơ kết nối với bánh xe trái, bánh xe phải [Nm] g Gia tốc trọng trường 9.8[ 2 ms − ] , , , L R L R H H V V Lực, phản lực tương tác giữa thân robot và hai bánh xe trái, phải [Nm] , TL TR V V Phản lực của mặt đất tương tác lên hai bánh xe trái, phải [Nm] , TL TR H H Lực ma sát giữa bánh xe trái, phải với mặt đường [Nm] • Xét ở bánh xe bên trái (bánh xe bên phải cũng tương tự) Sử dụng định luật 2 Newton lần lượt theo các trục x, trục y và trục quay của bánh xe. W W L TL L M x H H = − ɺɺ (3.1) W W WL TL L M y V V M g = − − ɺɺ (3.2) Trích t ừ Lu ậ n V ă n Th ạ c S ĩ – Tác gi ả : Nguyễn Gia Minh Thảo . Khoa Đ i ệ n- Đ i ệ n T ử , Đ H Bách Khoa TpHCM. WL WL L TL J C H R θ = − ɺɺ (3.3) • Vị trí của bánh xe và thân robot. WL WL x R θ = (3.4) WL WL x R θ → = ɺ ɺ (3.5) WL WL x R θ → = ɺɺ ɺɺ (3.6) W WR sin 2 L B B x x x L θ + = + (3.7) ( ) ( ) W WR WM os os 2 L B B B B B x x x L c L c x θ θ θ θ + → = + = + ɺ ɺ ɺ ɺ ɺ ɺ (3.8) ( ) ( ) ( ) 2 WM os sin B B B B B x L c L x θ θ θ θ → = − + ɺɺ ɺ ɺɺ ɺɺ (3.9) ( ) 1 os B B y L c θ = − − (3.10) ( ) sin B B B y L θ θ → = − ɺ ɺ (3.11) ( ) ( ) ( ) 2 sin os B B B B B y L L c θ θ θ θ → = − − ɺɺ ɺ ɺɺ (3.12) • Xét ở thân robot Sử dụng định luật 2 Newton lần lượt theo các trục x, trục y và trục quay tại điểm trọng tâm của thân robot. B B L R M x H H = + ɺɺ (3.13) ( ) sin L R B B L R B C L R B B C C M y V V M g F V V M g L θ + = + − + = + − + ɺɺ (3.14) ( ) ( ) ( ) sin os B B L R B L R B L R J V V L H H Lc C C θ θ θ = + − + − + ɺɺ (3.15) ( ) 2 L R D J H H δ δ = − ɺɺ (3.16) Trích t ừ Lu ậ n V ă n Th ạ c S ĩ – Tác gi ả : Nguyễn Gia Minh Thảo . Khoa Đ i ệ n- Đ i ệ n T ử , Đ H Bách Khoa TpHCM. Thế (3.15) , (3.14) vào (3.13), ta có: ( ) ( ) sin sin os L R B B B B B B B B B B L R C C J M y M g L M Lc x C C L θ θ θ θ + = + − − − + ɺɺ ɺɺ ɺɺ (3.17) Rút gọn (3.17) ta có : ( ) ( ) ( ) ( ) ( ) 2 sin os sin 1 sin B B B B B B B B B B L R J M L y c x M gL C C θ θ θ θ θ = − + − + + ɺɺ ɺɺ ɺɺ (3.18) T ừ (3.9) và (3.12), ta có: ( ) ( ) ( ) W WR W sin os os os 2 L B B B B B B B B x x y c x L c L c x θ θ θ θ θ θ + − = − − = − − ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ (3.19) Th ế (3.19) vào (3.18) ta có: ( ) ( ) ( ) ( ) 2 2 W os sin 1 sin B B B B B B B B B L R J M L c x M L M gL C C θ θ θ θ θ = − − + − + + ɺɺ ɺɺ ɺɺ (3.20) T ừ (3.1) suy ra: ( ) ( ) W W W WR L TL L TR R L R TL TR M x M x H H H H H H H H+ = − + − = − + + + ɺɺ ɺɺ (3.21) T ừ (3.3) suy ra : WL W L L TL C J H R θ − = ɺɺ ; WR WR R TR C J H R θ − = ɺɺ (3.21.b) Th ế (3.13), (3.21.b) vào (3.21), ta có: ( ) ( ) WL W WR WR W W WR L R L L B B C C J J M x x M x R θ θ + − + + = − + ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ (3.22) Ta có: 2 WL WR WM x x x + = ɺɺ ɺɺ ɺɺ ; WL W WR WR W W L J J J θ θ θ = = ɺɺ ɺɺ ɺɺ ; đặ t L R C C C = + Bi ể u th ứ c (3.22) s ẽ tr ở thành: ( ) W WL WR W WM 2 B B C J M x M x R θ θ − + = − + ɺɺ ɺɺ ɺɺ ɺɺ Mà: ( ) WL WR 1 2 B θ θ θ + = , suy ra : W W WM 2 2 B B B C J M x M x R θ − = − + ɺɺ ɺɺ ɺɺ (3.23) Th ế (3.9) và (3.23) ta có: ( ) ( ) ( ) 2 W W WM WM 2 2 os sin B B B B B B B B J C M x M L c M L M x R R θ θ θ θ θ = − + − − + ɺɺ ɺɺ ɺ ɺɺ ɺɺ (3.24) Trích t ừ Lu ậ n V ă n Th ạ c S ĩ – Tác gi ả : Nguyễn Gia Minh Thảo . Khoa Đ i ệ n- Đ i ệ n T ử , Đ H Bách Khoa TpHCM. T ừ ph ươ ng trình (3.20), (3.24) và thay : B θ b ở i θ , W M x b ở i x ; ta có h ệ ph ươ ng trình mô t ả h ệ th ố ng nh ư sau: ( ) ( ) ( ) ( ) ( ) ( ) 2 2 2 W W os sin 1 sin 2 2 os sin B B B B B B B J M L c x M L M gL C J C M x M L c M L M x R R θ θ θ θ θ θ θ θ θ θ = − − + − + = − + − − + ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ ɺ ɺɺ ɺɺ (3.25) • Xem moment quán tính c ủ a thân robot là m ộ t thanh có chi ề u dài L, kh ố i l ượ ng B M , quay quanh tr ụ c z – là tr ụ c n ố i gi ữ a hai bánh xe: 2 1 3 B B J M L = (3.26) • Xem moment quán tính c ủ a bánh xe robot là đĩ a tròn xoay có bán kính R, kh ố i l ượ ng W M , quay quanh tr ụ c z –tr ụ c n ố i gi ữ a hai bánh xe: 2 W w 1 2 J M R = (3.27) Th ế (3.26) và (3.27) vào h ệ ph ươ ng trình (3.25), ta có: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 2 2 2 W w 4 os sin 1 sin 3 2 sin os B B B B B B M L M L c x M gL C C M M x M L M R M L c R θ θ θ θ θ θ θ θ = − + − + + = − + + ɺɺ ɺɺ ɺ ɺɺ ɺɺ (3.28) Gi ả i h ệ ph ươ ng trình (3.28), ta có: ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) w W 2 2 2 W W w W 2 w w 0,75 os os 1 2 0,75 1 sin 0,75 sin os 0,75 sin 0,75 os 2 2 0,75 os os 2 0,75 0,75 os sin sin B B B B B B B B B B M R M L c c M M L M L c g c C M M L L M L M M RL M R M L c c M M x L M R g M R M L c M L L θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ + − = + + − + + + + + + − = + + − + ɺɺ ɺ ɺɺ ɺ ( ) ( ) ( ) ( ) 2 2 os 1 sin 1 B B M L c C M L R θ θ + + (3.29) Trích t ừ Lu ậ n V ă n Th ạ c S ĩ – Tác gi ả : Nguyễn Gia Minh Thảo . Khoa Đ i ệ n- Đ i ệ n T ử , Đ H Bách Khoa TpHCM. Đặ t bi ế n tr ạ ng thái nh ư sau: 1 2 3 4 , , , x x x x x x θ θ = = = = ɺ ɺ . H ệ ph ươ ng trình tr ạ ng thái mô t ả robot (3.29) đượ c vi ế t l ạ i nh ư sau: ( ) ( ) ( ) ( ) 1 2 2 1 1 2 1 2 1 1 3 4 4 3 1 4 , C x x x f x f x x g x x x x f x f x = = + + = = + ɺ ɺ ɺ ɺ ( ) ( ) 1 2 2 1 , C x g x + (3.30) V ớ i: C C C L R = + ( ) ( ) ( ) ( ) ( ) ( ) 1 1 1 w 1 1 W 0.75 sin 0.75 os os 1 2 B B g x L f x M R M L c x c x M M L − = + − + ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 2 1 1 2 W 2 1 2 w 1 1 W 0.75 sin os 2 , 0.75 os os 1 2 B B B B M L x c x x M M L f x x M R M L c x c x M M L + = + − + ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 2 1 1 2 W 1 1 w 1 1 W 0.75 1 sin 0.75 os 2 0.75 os os 1 2 B B B B x c x M L M M RL g x M R M L c x c x M M L + + + = + − + ( ) ( ) ( ) ( ) ( ) ( ) ( ) w 1 1 3 1 w 1 1 W 0,75 os sin 0,75 os os 2 B B B g M R M L c x x L f x M R M L c x c x M M L − + = + + − Trích t ừ Lu ậ n V ă n Th ạ c S ĩ – Tác gi ả : Nguyễn Gia Minh Thảo . Khoa Đ i ệ n- Đ i ệ n T ử , Đ H Bách Khoa TpHCM. ( ) ( )( ) ( ) ( ) ( ) ( ) 2 1 2 4 1 2 w 1 1 W sin , 0,75 os os 2 B B B M L x x f x x M R M L c x c x M M L = + + − ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 2 w 1 1 2 2 1 w 1 1 W 0,75 os 1 sin 1 0,75 os os 2 B B B B M R M L c x x M L R g x M R M L c x c x M M L + + + = + + − • T ừ h ệ ph ươ ng trình (3.30) ta th ự c hi ệ n mô ph ỏ ng mô hình robot hai bánh t ự cân b ằ ng trong Matlab/Simulink nh ư Hình 3.2 . Hình 3.2: S ơ đồ mô ph ỏ ng mô hình robot hai bánh trong MatLab/Simulink . n ố i gi ữ a hai bánh xe: 2 W w 1 2 J M R = (3 .27 ) Th ế (3 .26 ) và (3 .27 ) vào h ệ ph ươ ng trình (3 .25 ), ta có: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 2 2 2 W w 4 os sin 1 sin 3 2 sin os B B B B B B M. ɺɺ ɺɺ ɺɺ Mà: ( ) WL WR 1 2 B θ θ θ + = , suy ra : W W WM 2 2 B B B C J M x M x R θ − = − + ɺɺ ɺɺ ɺɺ (3 .23 ) Th ế (3.9) và (3 .23 ) ta có: ( ) ( ) ( ) 2 W W WM WM 2 2 os sin B B B B B B B B J C M. ) 2 2 2 W W os sin 1 sin 2 2 os sin B B B B B B B J M L c x M L M gL C J C M x M L c M L M x R R θ θ θ θ θ θ θ θ θ θ = − − + − + = − + − − + ɺɺ ɺɺ ɺɺ ɺɺ ɺɺ ɺ ɺɺ ɺɺ (3 .25 ) • Xem moment