Điều khiển chống rung bằng phương pháp tạo dạng lai

Điều khiển chống rung phƣơng pháp tạo dạng lai Mục lục Mục lục Danh mục hình vẽ LỜI MỞ ĐẦU Chƣơng NHU CẦU THỰC TIỄN CỦA VIỆC CHỐNG RUNG 1.1 Hiện tƣợng rung động thực tế sản xuất .6 1.1.1 Hiện tƣợng rung động cầu trục di chuyển .6 1.1.2 Hiện tƣợng rung động chất lỏng 1.1.3 Hiện tƣợng rung động cánh tay robot 1.2 Lợi ích việc chống rung 1.3 Phƣơng pháp chống rung Chƣơng PHƢƠNG PHÁP TẠO DẠNG LAI – HYBRID SHAPE 11 2.1 Đặt vấn đề 11 2.2 Cấu trúc điều khiển 12 2.2.1 Bộ điều khiển PID 14 2.2.2 Bộ lọc thông thấp .15 2.2.3 Bộ lọc chắn dải Notch Filter 17 2.3 Tối ƣu hóa điều khiển 21 Chƣơng THUẬT TOÁN ĐƠN HÌNH .24 3.1 Bài toán tìm giá trị nhỏ .24 3.2 Thuật toán 25 3.3 Ứng dụng vào toán tìm số tối ƣu cho điều khiển Hybrid Shape .28 Chƣơng THIẾT KẾ BỘ ĐIỂU KHIỂN HYBRID SHAPE .30 4.1 Giới thiệu mô hình 30 4.2 Mô hình toán học hệ lắc 31 Điều khiển chống rung phƣơng pháp tạo dạng lai 4.3 Cấu trúc điều khiển 35 4.4 Nhận dạng tần số dao động riêng lắc 35 4.5 Thiết kế điều khiển theo phƣơng pháp PID 37 4.5.1 Thiết kế điều khiển PI 37 4.5.2 Kết mô theo phƣơng pháp PID 38 4.6 Thiết kế điều khiển theo phƣơng pháp Hybrid Shape 41 4.6.1 Thiết kế điểu khiển Hybrid Shape 41 4.6.2 Kết mô với điều khiển Hybrid Shape 43 4.7 So sánh kết hai phƣơng pháp 47 4.7.1 So sánh dao động .47 4.7.2 Nhận xét 49 Chƣơng KẾT LUẬN VÀ KIẾN NGHỊ 50 5.1 Kết luận .50 5.2 Kiến nghị 50 TÀI LIỆU THAM KHẢO 51 PHỤ LỤC .52 Điều khiển chống rung phƣơng pháp tạo dạng lai Danh mục hình vẽ Hình 1.1 Đồ thị dao động tắt dần Hình 1.2 Hệ cầu trục .7 Hình 1.3 Dao động mức chất lỏng Hình 1.4 Rung động công nghiệp luyện thép Hình 1.5 Dao động vận hành cánh tay robot Hình 2.1 Mô hình cầu trục .11 Hình 2.2 Mô hình lắc đơn 12 Hình 2.3 Cấu trúc hệ thống điều khiển .12 Hình 2.4 Cấu trúc điều khiển Hybrid Shape 13 Hình 2.5 Sơ đồ điều khiển PID 14 Hình 2.6 Đồ thị lọc thông thấp .16 Hình 2.7 Đồ thị Bode lọc thông thấp .16 Hình 2.8 Đồ Bode lọc chắn dải với ξ = 0.01 𝜔 = 5.75 [rad/s] 18 Hình 2.9 Đồ thị Bode lọc chắn dải với ξ = 0.0001 𝜔 = 5.75 [rad/s] 18 Hình 2.10 So sánh kết thảy đổi hệ số tắt dần 19 Hình 2.11 Đồ thị bode lọc chắn dải với ξ = 0.0001 ω = 9.28 [rad/s] 19 Hình 2.12 Đồ thị bode lọc chắn dải với ξ = 0.0001 ω = 12.57 [rad/s] 20 Hình 2.13 So sánh kết thay đổi tần số góc .20 Hình 3.1 Ý tƣởng thuật toán đơn hình 24 Hình 3.2 Sơ đồ thuật toán đơn hình .25 Hình 3.3 Lƣu đồ thuật toán phƣơng pháp đơn hình 28 Hình 4.1 Mô hình lắc 30 Hình 4.2 Mô hình lắc 31 Hình 4.3 Cấu trúc điều khiển hệ thống 35 Điều khiển chống rung phƣơng pháp tạo dạng lai Hình 4.4 Dao động lắc 36 Hình 4.5 Tần số dao động lắc đơn 36 Hình 4.6 Sơ đồ cấu trúc với điều khiển PID 38 Hình 4.7 Sơ đồ mô Matlab 39 Hình 4.8 Đồ thị điện áp điều khiển điều khiển PID .39 Hình 4.9 Đồ thị quỹ đạo vị trí xe 40 Hình 4.10 Đồ thị dao động lắc .41 Hình 4.11 Sơ đồ mô với điều khiển Hybrid Shape Matlab 44 Hình 4.12 Đồ thị điện áp điều khiển 45 Hình 4.13 Đồ thị quỹ đạo vị trí xe .46 Hình 4.14 Đồ thị dao động lắc .47 Hình 4.15 So sánh dao động lắc theo hai phƣơng pháp 48 Hình 4.16 So sánh quỹ đạo lắc theo hai phƣơng pháp với quỹ đạo đặt 48 Điều khiển chống rung phƣơng pháp tạo dạng lai LỜI MỞ ĐẦU Rung động tƣợng phổ biến xuất thiết bị hoạt động, làm giảm tuổi thọ, độ bền hiệu suất làm việc thiết bị gây tai nạn lao động đáng tiếc Từ nhu cầu thực tiễn đòi hỏi phải nghiên cứu phƣơng pháp chống rung cho thiết bị Đã có nhiều phƣơng pháp chống rung đƣợc đề xuất, nhƣ phƣơng pháp tạo dạng tín hiệu đầu vào, điều khiển PID Nhƣng phƣơng pháp khó triệt tiêu đƣợc nhiễu gây ra, đặc biệt dao động cầu trục, lắc Những phƣơng pháp đƣợc thiết kế miền thời gian hay miền tần số nhƣng có nhƣợc điểm định Luận văn “Điều khiển chống rung phương pháp tạo dạng lai” nghiên cứu phƣơng pháp đơn giản hiệu đƣợc sử dụng rộng rãi để chống rung cho thiết bị Phƣơng pháp tạo dạng lai đƣợc thiết kế miền thời gian miền tần số Các phƣơng pháp đƣợc sử dụng để chống dung cho hệ cầu trục Luận văn đƣợc chia làm năm chƣơng Chương 1: Nhu cầu thực tiễn việc chống rung Chương 2: Phương pháp tạo dạng lai Hybrid Shape Chương 3: Thuật toán đơn hình Chương 4: Thiết kế điều khiển Hybrid Shape Chương 5: Kết luận kiến nghị Để hoàn thành luận văn em xin chân thành cảm ơn TS Dƣơng Minh Đức nhiệt tình hƣớng dẫn em suốt trình thực luận văn Hà Nội, ngày 25 tháng 04 năm 2015 Học viên thực Quyền Trƣờng Giang Chƣơng Nhu cầu thực tiễn việc chống rung Chƣơng NHU CẦU THỰC TIỄN CỦA VIỆC CHỐNG RUNG 1.1 Hiện tƣợng rung động thực tế sản xuất Rung động dao động học phát sinh từ động dụng cụ sản xuất Những dao động tắt dần Sự rung động ảnh hƣởng đến tuổi thọ chi tiết máy, gây vấn đề an toàn sản xuất đặc biệt ảnh hƣởng đến độ xác chất lƣợng sản phẩm Do lực cản môi trƣờng nên dao động tắt dần có dạng phƣơng trình: ( ) (𝜔 ) (1.1) Trong đó, A0, ω0, φ0 lần lƣợt biên độ, tần số, góc pha ban đầu dao động điều hòa; β hệ số tắt dần dao động Hình 1.1 Đồ thị dao động tắt dần Một nguyên nhân gây rung động chuyển động có gia tốc thiết bị Nhƣ di chuyển cầu trục, cánh tay rô bốt, bình đựng chất lỏng từ vị trí sang vị trí khác 1.1.1 Hiện tƣợng rung động cầu trục di chuyển Chƣơng Nhu cầu thực tiễn việc chống rung Cầu trục thiết bị đƣợc sử dụng rộng rãi lĩnh vực nhiều nhà máy, bến cảng, khu vực cần vận chuyển thiết bị có khối lƣợng lớn Trên thực tế cầu trục thƣờng đƣợc điều khiển tay dựa vào kinh nghiệm ngƣời vận hành Tuy nhiên với hệ thống cầu trục quan trọng, yêu cầu độ xác cao, đảm bảo an toàn cho ngƣời thiết bị chống rung cho tải trọng cầu trục di chuyển cần thiết Hình 1.2 Hệ cầu trục 1.1.2 Hiện tƣợng rung động chất lỏng Hình 1.3 Dao động mức chất lỏng Rung động không xuất với chuyển động hệ thống học, mà xuất hiển bề mặt chất lỏng xuất gia tốc Hiện Chƣơng Nhu cầu thực tiễn việc chống rung tƣợng dao động với biên độ lớn gây tràn chất lỏng khỏi bình chứa, gây ảnh hƣởng tới thiết bị khác gây nguy hiểm tới ngƣời nhiều trƣờng hợp Trong thực tế việc chuyển bình chất lỏng xuất nhiều nhƣ công nghiệp luyện kim, nồi kim loại nấu chảy lỏng có nhiệt độ cao di chuyển đến khuôn đúc xuất rung động Với nhiệt độ cao nhƣ rung động gây an toàn cho ngƣời thiết bị xung quanh Hình 1.4 Rung động công nghiệp luyện thép 1.1.3 Hiện tƣợng rung động cánh tay robot Robot công nghiệp tay máy tự động thiếu sản xuất công nghiệp đại, đƣợc lập trình sẵn để hoạt động theo chƣơng trình mong muốn Các cánh tay robot thƣờng đƣợc ghép nhiều chi tiết khác nhau, chúng không cứng tuyệt đối nên xuất rung động di chuyển Bên cạnh với yêu cầu đòi hỏi độ xác cao cánh tay robot nên rung động cần phải loại bỏ tới mức tối đa Ví dụ nhƣ robot hàn, robot gắp vật cần phải di chuyển cách nhanh chóng xác đến vị trí cần hàn, cần thao tác… Chƣơng Nhu cầu thực tiễn việc chống rung Hình 1.5 Dao động vận hành cánh tay robot 1.2 Lợi ích việc chống rung Qua số tƣợng kể rung động thực tế ta thấy đƣợc vai trò quan trọng việc chống rung nhƣ lợi ích việc chống rung mang lại: - Đảm bảo an toàn lao động cho ngƣời vận hành máy, giảm tổn hao kinh tế rung động - Đảm bảo chất lƣợng sản phẩm - Tuổi thọ máy móc đƣợc kéo dài loại bỏ đƣợc rung động có hại - Do hạn chế đƣợc dao động nên tiết kiệm đƣợc lƣợng yêu cầu cấu chấp hành giảm - Giảm thời gian việc chờ đợi hết rung động Qua ta khẳng định đƣợc lần tầm quan trọng việc giảm thiểu rung động cần đƣợc giải triệt để 1.3 Phƣơng pháp chống rung Chƣơng Nhu cầu thực tiễn việc chống rung Qua số ví dụ kể ta thấy việc triệt tiêu rung động có hại sản xuất điều cần thiết Có nhiều phƣơng pháp để chống rung nhƣ sử dụng điều khiển PID, điều khiển Hybrid… Các dao động ví dụ tồn dƣới dạng dao động tắt dần, hình thành sau có tác động gia tốc (lúc bắt đầu dừng chuyển động) Các hệ thống cần thiết điều khiển chống rung để loại bỏ dao động Trong luận văn xin đƣợc trình bày phƣơng pháp Hybrid Shape, phƣơng pháp đơn giản, hiệu để loại bỏ dao động dƣ thừa Lý thuyết mô kết phƣơng pháp đƣợc trình bày chƣơng sau luận văn 10 Chƣơng Thiết kế điều khiển Hybrid Shape Hình 4.11 Sơ đồ mô với điều khiển Hybrid Shape Matlab 44 Chƣơng Thiết kế điều khiển Hybrid Shape - Đặt thông số mô hình:  Quỹ đạo đặt có dạng 2-1-2 từ vị trí ban đầu đến vị trí 0.6 [m] thời gian [s], gia tốc [m/s2]  Bộ điều khiển Hybrid Shape có tham số là:  Hàm truyền đối tƣợng điều khiển có dạng tích phân quán tính bậc 1: ( ) 1 ( (4.54) 1)  Sử dụng công cụ mô Simulink Matlab với thời gian trích mẫu 0.001 [ms] thu đƣợc kết nhƣ sau: - Đồ thị điện áp điều khiển: 1.8 1.6 1.4 Dien ap [V] 1.2 0.8 0.6 0.4 0.2 -0.2 Thoi gian [s] Hình 4.12 Đồ thị điện áp điều khiển 45 10 Chƣơng Thiết kế điều khiển Hybrid Shape Từ đồ thị điện áp điều khiển ta thấy, biên độ điện áp hoàn toàn thỏa mãn (| |) điều kiện loại nghiệm - Đồ thị quỹ đạo đối tƣợng: 0.7 0.6 Vi tri [m] 0.5 0.4 0.3 0.2 0.1 Vi tri thuc Vi tri dat 0 Thoi gian [s] 10 Hình 4.13 Đồ thị quỹ đạo vị trí xe Đồ thị so sánh với quỹ đạo đặt cho thấy vị trí thực xe có dạng giống với quỹ đạo đặt, nhƣng bị trễ khoảng thời gian 46 Chƣơng Thiết kế điều khiển Hybrid Shape Đồ thị dao động lắc: 0.04 0.03 Goc lech [rad] 0.02 0.01 -0.01 -0.02 -0.03 -0.04 10 Thoi gian [s] Hình 4.14 Đồ thị dao động lắc Đồ thị dao động lắc sử dụng điều khiển Hybrid Shape, cho thấy: dao động lắc có biên độ dao động nhỏ sau thời gian xác lập Ta quan sát đƣợc tần số dao động lắc nhƣ hình dƣới đây: 4.7 So sánh kết hai phƣơng pháp 4.7.1 So sánh dao động 47 Chƣơng Thiết kế điều khiển Hybrid Shape 0.08 Bo dieu khien PID Bo dieu khien Hybrid Shape 0.06 Goc lech [rad] 0.04 0.02 -0.02 -0.04 -0.06 -0.08 Thoi gian [s] 10 Hình 4.15 So sánh dao động lắc theo hai phƣơng pháp Dao động lắc với tín hiệu điều khiển điều khiển Hybrid Shape nhanh chóng tắt dần ổn định sau vị trí đƣợc điều khiển đến vị trí mong muốn 0.7 0.6 Vi tri xe [m] 0.5 0.4 0.3 0.2 0.1 0 Vi tri thuc - Bo dieu khien Hybrid Shape Vi tri thuc - Bo dieu khien PID Vi tri dat Thoi gian [s] 10 Hình 4.16 So sánh quỹ đạo lắc theo hai phƣơng pháp với quỹ đạo đặt 48 Chƣơng Thiết kế điều khiển Hybrid Shape Quỹ đạo xe đƣợc điều khiển điều khiển Hybrid Shape trễ so với quỹ đạo xe đƣợc điều khiển điều khiển PID 4.7.2 Nhận xét - Từ kết mô trên, ta rút số nhận xét nhƣ sau: - Phƣơng pháp Hybrid Shape cho tín hiệu vị trí có dạng giống với quỹ đạo đặt nhƣng bị trễ khoảng thời gian 1.18 [s] Nguyên nhân trễ đƣợc hiểu hàm truyền điều khiển hàm truyền đối tƣợng điều khiển có thành phần xấp xỉ khâu trễ Đó thành phần quán tính đƣợc xấp xỉ khâu trễ - Dựa vào đồ thị dao động miền thời gian miền tần số, ta dễ dàng quan sát thấy phƣơng pháp Hybrid Shape làm giảm đáng kể dao động lắc, xe dừng lại dao động lắc nhanh chóng bị tắt - So với phƣơng pháp sử dụng điều khiển PID, phƣơng pháp Hybrid Shape giúp làm giảm biên độ dao động cách nhanh chóng ổn định Từ đáp ứng đƣợc yêu cầu hệ thống đặt - Nhƣ vậy, phƣơng pháp Hybrid Shape để chống rung có hiệu rõ rệt thực tế 49 Chƣơng Kết luận kiến nghị Chƣơng KẾT LUẬN VÀ KIẾN NGHỊ 5.1 Kết luận Luận văn nghiên cứu phƣơng pháp chống rung kỹ thuật, đối tƣợng cụ thể cầu trục Một số kết đạt đƣợc nhƣ sau: - Xây dựng đƣợc mô hình cầu trục tƣợng rung động xuất cầu trục làm việc, gây tác động có hại không mong muốn, việc chống rung cho hệ cầu trục cần thiết - Phân tích, so sánh đƣợc số phƣơng pháp chống rung thƣờng đƣợc sử dụng nhƣ PID, tạo dạng lai – Hybrid Shape Ƣu điểm phƣơng pháp tạo dạng lai bái toán chống rung có xuất dao động riêng - Luận văn nghiên cứu phƣơng pháp tạo dạng lai Hybrid Shape đƣợc thiết kế miền thời gian tần số, cho kết ổn định, chống nhiễu tốt đặc biệt với nhiễu dạng dao động - Thuật toán Simplex đƣợc sử dụng hiệu để tìm số tối ƣu cho điều khiển - Phƣơng pháp cho kết mô phù hợp với lý thuyết đề 5.2 Kiến nghị Luận văn dừng lại việc nghiên cứu phƣơng pháp chống rung hệ cần trục lý thuyết mô hình mô Matlab/Simulink Vì đề tài tiếp tục phát triển cách xây dựng hệ cần trục phòng nghiệm để kiểm chứng thuật toán chống rung vừa đƣợc xây dựng 50 Điều khiển chống rung phƣơng pháp tạo dạng lai TÀI LIỆU THAM KHẢO [1] J A Nelder and R Mead, A simplex method for function minimization, Computer Journal, 1965, pp 308-313 [2] K Yano, T Toda and K.Terashima, “Sloshing Suppression Control of Automatic Pouring Robot by Hybrid Shape Approach”, Proceedings of the 40th IEEE Conference on Decision and Control, Orlando, Florida USA, December 4-7, 2001, pp 1328-1333 [3] Melanie Mitchell, An introduction to Genetic Algorithms, 1999 Available: https://svn-d1.mpi-inf.mpg.de/AG1/MultiCoreLab/papers/ebook-fuzzymitchell-99.pdf [4] Nguyễn Doãn Phƣớc, Lý thuyết điều khiển tuyến tính, NXB Khoa học kỹ thuật, 2009 [5] Rahul malhotra, Narinder Singh and Yaduvir Singh, “Genetic Algorithms: Concepts, Design for Optimization of Process Controllers”, Computer and Information Science, Vol 4, No 2, March 2011, pp 39-54 [6] Genetic Algorithm and Direct Search Toolbox User’s Guide, pp 2-24 Available: http://www.mathworks.com/help/releases/R13sp2/pdf_doc/gads/gads_tb.pd f 51 Điều khiển chống rung phƣơng pháp tạo dạng lai PHỤ LỤC P1 Các bảng số liệu dùng mô Bảng P.1 Thông số mô hình lắc đơn Tham số K Giá ý hiệu trị Khối lƣợng nặng m 0.1 [Kg] Chiều dài dây treo L 0.3 [m] Gia tốc trọng trƣờng g 9.8 [m/s ] Hệ số cản không khí r 0.001 P2 Phƣơng pháp Runge Kutta  Giải phương trình vi phân - Xét phƣơng trình vi phân cấp có dạng nhƣ sau: ( ) ( ) với Giả sử tìm đƣợc giá trị gần ( ) muốn tìm yi+1 y(xi+1) Ta viết công thức Taylor: ( ) ( với ( ( ( ) ) ( ) ( ) 1) ) ( )( ( ) ) ( ) ( ) ( ( ) ( ) ( ( )) ta có: ( ) ( Để tính - Đặt: ( )( ( ) ) ( ) ) ( *) ( ) ( 1) ta dùng phƣơng pháp Runger Kutta nhƣ sau: () () () ( () **) Trong đó: 52 Điều khiển chống rung phƣơng pháp tạo dạng lai { () ( () ( () () ( () ) Cần xác định hệ số a, b, ; α, β, γ, ; - Dùng công thức Runger Kutta bậc 2: () ( ) { () ( () ) () ) vế phải (**) () ) ( () ***) Ta có: ( ) Do vế phải (*) là: ( ) ( ( ( )) ( )) ( ( ) ) ( [ ( - Mặt khác, theo công thức Taylor ta có: ( ) () () ( ) ( ) Do đó, vế phải (***) là: ( ) ( ) ( ) [ Cân số hạng chứa h h ta đƣợc: 1 { ( )) )] ( ) ( ) ( )] Suy ra: α = a; ; với a đƣợc chọn - Nếu a = 1/2 r1 = r2 = Ta có công thức Euler - Nếu a = r1 = 1/2 r2 = 1/2 Ta có công thức Euler cải tiến - Tƣơng tự ta nhận đƣợc công thức Runger Kutta bậc 4: ( ) ( ) ( ) ( ) ( { )  Ứng dụng phƣơng pháp Runger kutta - Xét hàm truyền đạt hệ thống điều khiển có dạng nhƣ sau: 53 Điều khiển chống rung phƣơng pháp tạo dạng lai ( ) - Đặt ( ) ( ) [ ] ( ) [ ( ) ] - Mô hình trạng thái hệ thống là: { ̇ - Trong đó: 1 [ ] - Đặt: ( ( ) ) ) ( ( ( ( ( ) ) ) ) - Trong đó: h bƣớc tính tích phân 54 Điều khiển chống rung phƣơng pháp tạo dạng lai P3 Code chƣơng trình tối ƣu điều khiển Hybrid Shape Chƣơng trình %***************************************************************** ********* % Chuong trinh chinh %***************************************************************** ********* % Diem ban dau P1 = [10 0.32]; P2 = [20 0.5]; P3 = [9 0.1]; P = [P1 P2 P3]; % He so Simplex a = 1.0; b = 0.5; c = 2.0; % The Simplex for j=1:1:20 [y1] = rungerkutta(P(1), P(2)); [y2] = rungerkutta(P(3), P(4)); [y3] = rungerkutta(P(5), P(6)); [yh, [yl, Ph = Pl = n] = lonnhat(y1, y2, y3); k] = nhonhat(y1, y2, y3); [P(2*n-1) P(2*n)]; [P(2*k-1) P(2*k)]; Pg = (P1+P2+P3-Ph)/2; % Trong tam [yg] = rungerkutta(Pg(1), Pg(2)); Ps = (1+a)*Pg - a*Ph; % Phep doi xung [ys] = rungerkutta(Ps(1), Ps(2)); % -%Kiem tra dieu kien simplex if ((sqrt((y1 - yg)^2+(y2 - yg)^2 + (y3 - yg)^2)/3) >= 10^-3) % -if (ysyh) for i=1:2:6 P(i) = (P(i) + P(2*k-1))/2; P(i+1) = (P(i) + P(2*k))/2; end else Ph = Pss; P(2*n-1)= Pss(1); P(2*n) = Pss(2); end else Ph = Ps; P(2*n-1)= Ps(1); P(2*n) = Ps(2); end end else break; end end toiuu = Pl; %***************************************************************** ********* Hàm kiểm tra %***************************************************************** ********* % Ham kiem tra %***************************************************************** ********* function [ktra] = kiemtra(ys, y1, y2, y3, n) y = [y1, y2, y3]; ktra = 1; for i=1:1:3 if (i==n) continue; else if (ys>y(i)) ktra = ktra*1; else ktra = ktra*0; end end end %***************************************************************** ******* Hàm tìm giá trị lớn %***************************************************************** ********* %Ham tim gia tri lon nhat %***************************************************************** ********* function[ymax, n] = lonnhat(y1, y2, y3) y = [y1 y2 y3]; i = 1; 56 Điều khiển chống rung phƣơng pháp tạo dạng lai ymax = y(i); n = 1; for i=2:1:3 if (y(i) > ymax) ymax = y(i); n = i; end end %***************************************************************** ********* Hàm tìm giá trị nhỏ %***************************************************************** ********* % Ham tim gia tri nho nhât %***************************************************************** ********* function[ymin, k] = nhonhat(y1, y2, y3) y = [y1 y2 y3]; i = 1; ymin = y(i); k = 1; for i=2:1:3 if (y(i) < ymin) ymin = y(i); k = i; end end %***************************************************************** ********* Hàm tạo quỹ đạo đặt 2-1-2 %***************************************************************** ********* % Ham tinh quy dao 2-1-2 %***************************************************************** ********* function[q] = quidaodat(qo, qc, ddq, t1, t2, tc, t) if tt1)&&(tt2)&&(t