Chương 1: MODELING - HỆ CÁNH TAY ROBOT 2 BẬC TỰ DO 1.1 Mô hình hóa hệ cánh tay robot 2 bậc tự do Hình 1. Mô hình của hệ robot 2 bậc tự do Các thông số vật lí của hệ cánh tay robot hai bậc tự do: 𝐿1 = 𝐿2 = 1 : Chiều dài hai khớp (m) g = 9.8 :Gia tốc trọng trường (m/s2 ) 𝑚1 = 𝑚2 = 1 :Khối lượng của hai khớp (kg) Phân tích hệ thống cơ cánh tay robot 2 bậc tự do Giả sử trọng tâm nằm ở vị trí cuối khâu và moment quán tính ở các khâu là bằng không. Vị trí và vận tốc của m1 𝑥1 𝐿1 𝑐𝑜𝑠𝜃1 [ ]=[ ] 𝑦1 𝐿1 𝑠𝑖𝑛𝜃1 𝑥1̇ −𝐿1 𝑠𝑖𝑛𝜃1 [ ]=[ ] 𝜃̇ 𝑦1̇ 𝐿1 𝑐𝑜𝑠𝜃1 1 Vị trí và vận tốc của m2: 𝑥2 𝐿1 𝑐𝑜𝑠𝜃1 + 𝐿2𝑐𝑜𝑠(𝜃1 + 𝜃2 ) [ ]=[ ] 𝑦2 𝐿1 𝑠𝑖𝑛𝜃1 + 𝐿2𝑠𝑖𝑛(𝜃1 + 𝜃2 ) 𝑥2̇ −𝐿1 𝑠𝑖𝑛𝜃1 − 𝐿2𝑠𝑖𝑛(𝜃1 + 𝜃2 ) − 𝐿2𝑠𝑖𝑛(𝜃1 + 𝜃2 ) 𝜃1̇ [ ]=[ ][ ̇ ] 𝑦2̇ 𝐿1 𝑐𝑜𝑠𝜃1 + 𝐿2𝑐𝑜𝑠(𝜃1 + 𝜃2 ) 𝐿2𝑐𝑜𝑠(𝜃1 + 𝜃2 ) 𝜃2 Năng lượng động năng 1 1 𝐾1 = 𝑚1 𝑣1 2 = 𝑚1 (𝑥1̇ 2 + 𝑦1̇ 2 ) 2 2 4 1 1 𝐾2 = 𝑚2 𝑣2 2 = 𝑚2 (𝑥2̇ 2 + 𝑦2̇ 2 ) 2 2 Năng lượng thế năng 𝑃1 = 𝑚1 𝑔𝑦1 𝑃2 = 𝑚2 𝑔𝑦2 Hàm Lagrange 2 L(θ, θ̇) = ∑(𝐾𝑖 − 𝑃𝑖 ) i=1 Thế 𝐾1 ,𝐾2 ,𝑃1 ,𝑃1 vào hàm Lagrange ta được: 1 L(θ, θ̇) = [𝑚1 (𝑥1̇ 2 + 𝑦1̇ 2 ) + 𝑚2 (𝑥2̇ 2 + 𝑦2̇ 2 )] − g(𝑚1 𝑦1 + 𝑚2 𝑦2 ) 2 Theo Phương trình Euler – Lagrange tổng quát ta có: 𝑑 𝜕𝐿 𝜕𝐿 𝜏𝑖 = − , 𝑖 = 1,2 𝑑𝑡 𝜕𝜃𝑖̇ 𝜕𝜃𝑖 Với 𝜏 là Moment (Torque) cung cấp cho các khớp.2 Thực hiện mô phỏng bằng Matlab/Simulink hệ cánh tay robot 2 bậc tự do Xây dựng mô hình robot 2 dof bằng matlab simulink Hình 2. Mô hình robot 2 dof bằng matlab simulink Trong khối robot 2 dof Hình 3.
Sơ đồ trong khối robot 2 dof - Ta dùng khối MATLAB Function để mô tả hệ phương trình trạng thái của hệ cánh tay robot. Chương trình matlab như sau: %% HCMUTE %% Ngô Anh Tuấn & Lê Thanh Tùng 6 function ddtheta= Robot2dof(u,theta,dtheta,dt) %%Declaration m1=1; m2=1; l1=1; l2=1; g=9.81; %%Khai bao mot so viet tat t1=theta(1); t2=theta(2); dt1=dtheta(1); dt2=dtheta(2); c1=cos(t1); c2=cos(t2); s1=sin(t1); s2=sin(t2); c12=cos(t1+t2); %% M M=[m1*l1^2+m2*(l1^2+2*l1+l2*c2+l2^2),m2*(l1*l2*c2+l2^2);. m2*(l1*l2*c2+l2^2),m2*l2^2]; %% V V=[-m2*l1*l2*s2*(2*dt1*dt2+dt2^2);m2*l1*l2*dt1^2*s2]; %% G G=[(m1+m2)*g*l1*c1+m2*g*l2*c12;m2*g*12*c12]; %%Simalation ddtheta=(M^-1)*(-V-G)+(M^-1)*u-(M^-1)*dt; Kết quả mô phỏng 7 Hình 4. Đáp ứng của hệ cánh tay robot 2 bậc khi chưa có bộ điều khiển Nhận xét: - Với ngõ vào là góc quay tại 2 khớp 1 và 2 là các sóng sin.
Tuy nhiên ngõ ra ở mô hình robot các góc tương ứng không bám theo các ngõ vào. Lí do chưa có bộ điều khiển.3 Giới thiệu tổng quan về bộ điều khiển trượt Hình 5. Đồ thị biểu diễn hoạt động của bộ điều khiển trượt 8 Bộ điều khiển trượt (Sliding Mode Control - SMC) là một kỹ thuật điều khiển mạnh mẽ, được sử dụng rộng rãi trong việc quản lý và điều khiển các hệ thống không tuyến tính. Kỹ thuật này nổi bật với khả năng duy trì hiệu suất ổn định của hệ thống dù trong điều kiện có sự biến động của tham số hoặc sự hiện diện của nhiễu.
Bộ điều khiển trượt hoạt động dựa trên nguyên tắc thiết lập một mặt trượt (sliding surface) trong không gian trạng thái của hệ thống. Mục tiêu là điều khiển hệ thống sao cho nó đạt tới và duy trì trên mặt trượt này trong quá trình hoạt động. Khi hệ thống tiếp cận và di chuyển dọc theo mặt trượt, điều khiển trượt sẽ phát huy tác dụng, giảm thiểu sự ảnh hưởng của các yếu tố không mong muốn và đẩy nhanh quá trình hội tụ về trạng thái mong muốn. Một trong những ưu điểm chính của bộ điều khiển trượt là khả năng chịu nhiễu cao và khả năng thích nghi với sự thay đổi của các thông số hệ thống mà không yêu cầu điều chỉnh lại thiết kế điều khiển.
Điều này làm cho SMC trở thành lựa chọn ưu tiên trong các ứng dụng yêu cầu độ tin cậy cao và tính ổn định trước những bất ổn của môi trường và hệ thống, như trong ngành công nghiệp ô tô, hàng không, và các hệ thống điều khiển tự động. Bộ điều khiển trượt thường được sử dụng trong các hệ thống điều khiển động cơ, hệ thống điều khiển quá trình, và nhiều ứng dụng khác trong kỹ thuật điện và cơ khí. Sự kết hợp của tính đơn giản, hiệu quả và độ tin cậy cao làm cho phương pháp này vẫn luôn được cộng đồng kỹ sư và nhà nghiên cứu quan tâm và phát triển không ngừng.4 Thiết kế bộ điều khiển trượt (sliding mode) cho hệ cánh tay robot 2 bậc trên Chuẩn hóa phương trình vi phân xét trường hợp có nhiễu M(θ)θ̈ + V(θ, θ̇) + G(θ) + dα = τ Với dα là tín hiệu nhiễu hệ thống Từ phương trình trên ta suy ra: M(θ)θ̈ = τ − V(θ, θ̇) − G(θ) − dα => θ̈ = M(θ)−1 τ − M(θ)−1 (V(θ, θ̇) + G(θ)) − M(θ)−1 dα Ta đặt: u1 τ = u = [u ] 2 Phương trình vi phân hệ cánh tay robot hai bậc tự do có tín hiệu nhiễu trở thành: θ̈ = M −1 u − M −1 (V + G) − M −1 dα Đặt các biến trạng thái 𝑥1 𝑥̇ 1 = 𝜃̇ {𝑥̇ 1 = 𝑥2 = 𝜃̇ => { 𝑥̇ 2 = 𝜃̈ = M −1 u − M −1 (V + G) − M −1 dα 𝑥̇ 2 = 𝜃̈ 9 Ta biểu diễn quan hệ vào ra của đối tượng phi tuyến có tín hiệu nhiễu 𝑦̈ = 𝑎(𝑥) + 𝑏(𝑥)𝑢 + 𝑑𝑡 a(x) = −M −1 (V + G) ẋ 1 = x2 −> { b(x) = M −1 −> { ẋ 2 = a(x) + b(x)u + dt dt = −M −1 dα Các bước thiết kế bộ điều khiển trượt cho hệ Bước 1: Tính đạo hàm tín hiệu ra θ Ngõ ra θ = [ 1 ] là hai góc quay của hai trục bám theo tín hiệu đặt: θ2 𝜋 𝜃𝑑1 (𝑡) = 𝑠𝑖𝑛(0.2𝜋𝑡 + ) 6 6 Sai số giữa tín hiệu đặt và tín hiệu ngõ ra: 𝑒 = θ − 𝜃𝑑 = 𝑥1 − 𝜃𝑑 { 𝑒̇ = 𝜃̇ − 𝜃𝑑̇ = 𝑥2 − 𝜃𝑑̇ Bước 2: Biểu thức mặt trượt: Vì đây là hệ bậc 2 có n = 2 nên ta chọn mặt trượt s = λe + ė => ṡ = λė + ë Bước 3: Viết biểu thức bộ điều khiển trượt Bộ điều khiển trượt gồm hai thành phần: 𝑢𝑟 = 𝑢𝑒𝑞 + 𝑢𝑟 Trong đó 𝑢𝑒𝑞 được tính khi cho 𝑠̇ = −𝑘1 𝑠 và d(t)=0 => λė + ë = −𝑘1 𝑠 => λė + ẋ 2 − θ̈d = −𝑘1 𝑠 => λė + a(x) + b(x)ueq + dt − θ̈d = −𝑘1 𝑠 => λė + a(x) + b(x)ueq − θ̈d = −𝑘1 𝑠 => ueq = b(x)−1 (−a(x) + θ̈d − λė − 𝑘1 𝑠) Ta lại có 𝑢𝑟 = −𝑏(𝑥)−1 𝑘𝑠𝑖𝑔𝑛(𝑠) Ta có luật điều khiển của bộ điều khiển trượt: u = 𝑢𝑒𝑞 + 𝑢𝑟 10 => u(t) = 𝑏(𝑥)−1 [−a(x) + θ̈d − λė − 𝑘1 𝑠 − 𝑘𝑠𝑖𝑔𝑛(𝑠)] => u(t) = 𝑀[−M −1 (−V − G) + θ̈d − λė − 𝑘1 𝑠 − 𝑘𝑠𝑖𝑔𝑛(𝑠)] => u(t) = 𝑀[M −1 (V + G) + θ̈d − λė − 𝑘1 𝑠 − 𝑘𝑠𝑖𝑔𝑛(𝑠)] Trong đó K1 có tác dụng thay đổi tốc độ mà mặt trượt tiến về 0 và được chọn qua nhiều lần thử nghiệm, K được chọn dựa theo tín hiệu nhiễu mô phỏng. Bước 4: Thiết kế bộ lọc thông thấp tín hiệu vào đảm bảo tín hiệu khả vi bị chặn đến bậc 2: 1 𝐺𝐿𝐹1 = (0.5 Mô phỏng hệ thống với bộ điều khiển bằng Matlab Simulink và đánh giá kết quả • Xây dựng sơ đồ khối động học cánh tay robot 2 bậc tự do Hình 6.
Mô hình động học robot mô phỏng bằng Matlab Chương trình Matlab Function %% HCMUTE %% Ngô Anh Tuấn & Lê Thanh Tùng function ddtheta= Robot2dof(u,theta,dtheta,dt) %%Declaration m1=1; m2=1; l1=1; 11 l2=1; g=9.81; %%Khai bao mot so viet tat t1=theta(1); t2=theta(2); dt1=dtheta(1); dt2=dtheta(2); c1=cos(t1); c2=cos(t2); s1=sin(t1); s2=sin(t2); c12=cos(t1+t2); %% M M=[m1*l1^2+m2*(l1^2+2*l1+l2*c2+l2^2),m2*(l1*l2*c2+l2^2);.1 Xây dựng khối giá trị đặt 1.2 Xây dựng khối bộ điều khiển trượt - Chương trình Matlab Function %%HCMUTE %% NGÔ ANH TUẤN & LÊ THANH TÙNG function u=SMC(theta,dtheta,theta_r,dtheta_r,ddtheta_r) %%Declaration m1=1; 13 m2=1; l1=1; l2=1; g=9.81; %%Gan gia tri t1=theta(1,1); t2=theta(2,1); dt1=dtheta(1,1); dt2=dtheta(2,1); %%Khai bao mot so viet tat c1=cos(t1); c2=cos(t2); s1=sin(t1); s2=sin(t2); c12=cos(t1+t2); %% M M=[m1*l1^2+m2*(l1^2+2*l1+l2*c2+l2^2),m2*(l1*l2*c2+l2^2);.3 Xây dựng bộ điều khiển trượt cho cánh tay robot 2 bậc tự do Hình 7. Mô hình bộ điều khiển trượt cho hệ cánh tay robot 2 bậc tự do sử dụng Matlab 1.4 Kết quả Kết quả của bộ điều khiển trượt khi không có nhiễu (Bộ thông số lamda=50 , k=150, k1=55) 15 Hình 8. Đáp ứng ngõ ra khi hệ thống không có nhiễu Hình 9. Sai số ngõ ra của hệ thống khi không có nhiễu Hình 10.
Tín hiệu điều khiển của hệ thống khi không có nhiễu 16 • Kết quả của bộ điều khiển trượt khi có nhiễu(Bộ thông số lamda=30 , k=30, k1=40) Hình 11. Đáp ứng ngõ ra khi hệ thống có nhiễu Hình 12. Sai số ngõ ra khi hệ thống có nhiễu 17 Hình 13. Tín hiệu điều khiển khi hệ thống có nhiễu ❖ Nhận xét: + Khi không có nhiễu : Tín hiệu ngõ ra bám sát so với tín hiệu đặt với sai số xắp xỉ bằng 0 + Khi có nhiễu: Tín hiệu ngõ ra vẫn bám sát so với tín hiệu điều khiển , tín hiệu điều khiển có sự dao động hay còn gọi là chattering => Nhìn chung cả 2 trường hợp bộ điều khiển vẫn hoạt động tốt và đáp ứng yêu cầu đặt ra Tuy nhiên bộ điều khiển trượt cho hệ robot 2 bậc tự do nói riêng hay bộ điều khiển trượt nói chung vẫn còn một số nhược điểm như hiện tương rung (chattering), sự rung này làm tổn thất nhiệt trong các thiết bị điện, gây những dao động cho thiết bị cơ học và làm hỏng chúng.
18 Chương 2: MODELING - HỆ XE CON LẮC NGƯỢC 2.1 Mô hình toán học của đối tượng hệ xe con lắc ngược Các thông số kỹ thuật của hệ thống • u: lực tác động vào xe [N] • x: vị trí xe [m] • : góc lệch giữa con lắc và phương thẳng đứng [rad] • M = 1: khối lượng xe [kg] • g = 9.8: gia tốc trọng trường [ m / s 2 ] • m = 0.