BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Nguyễn Tự Nam THIẾT KẾ QUỸ ĐẠO MẪU CHO HỆ CÁNH TAY MÁY XÚC TỰ ĐỘNG Chuyên ngành : Kỹ thuật Điều khiển Tự động hóa LUẬN VĂN THẠC SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC : TS VŨ THỊ THÚY NGA Hà Nội – Năm 2019 MỤC LỤC LỜI MỞ ĐẦU DANH MỤC CÁC KÍ HIỆU, CHỮ VIẾT TẮT DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ, BẢNG BIỂU 1.1 Tầm quan trọng toán thiết kế quỹ đạo mẫu 1.2 Một số phƣơng pháp thiết kế quỹ đạo cho cánh tay máy .7 1.2.1 Phƣơng pháp nội suy 1.2.2 Ứng dụng phƣơng pháp điều khiển thông minh 10 1.2.3 Ứng dụng thuật toán tối ƣu 11 CHƢƠNG 2: KỸ THUẬT B-SPLINE 14 2.1 Đƣờng cong B-splines 14 2.2 Các tính chất quan trọng B-Spline 15 CHƢƠNG 3: SỬ DỤNG THUẬT TOÁN B-SPLINE ĐỂ 24 THIẾT KẾ QUỸ ĐẠO TỐI ƢU CHO ROBOT 24 3.1 Giới thiệu chung đƣa toán tối ƣu 24 3.1.1 Giới thiệu chung .24 3.1.2 Tạo quỹ đạo tối ƣu thời gian 25 3.1.3 Tạo quỹ đạo tối ƣu lƣợng 27 3.1.4 Việc tạo quỹ đạo tối ƣu độ giật 28 3.1.5 Đƣa toán tối ƣu .29 3.2 Xác định quỹ đạo mẫu từ việc giải toán tối ƣu 32 3.2.1 Sử dụng điều kiện nội suy điều kiện ràng buộc 32 3.2.2 Biểu diễn ràng buộc động học 36 3.3 Các bƣớc thiết kế thuật toán để xây dựng quỹ đạo mẫu 38 3.4 Kết mơ thuật tốn 39 KẾT LUẬN VÀ KIẾN NGHỊ 46 PHỤ LỤC 52 LỜI MỞ ĐẦU Hiện cách mạng công nghiệp 4.0 diễn nhiều nước phát triển Nó mang lại nhiều hội thách thức cho người Cách mạng cơng nghiệp 4.0 cịn có tên gọi Industry 4.0 Industry 4.0 mơ tả mơi trường mà máy tính, tự động hóa người nói chung làm việc theo cách hoàn toàn Những robot, hay máy móc nói chung, kết nối vào hệ thống máy tính Các chuyên gia dự báo tương lai không xa, robot thay hầu hết cơng việc người Điều nói lên tỷ lệ lao động bậc thấp có nguy thất nghiệp cao, đòi hỏi lao động trình độ kĩ thuật cao Một cơng việc có nhu cầu lớn kĩ sư chuyên ngành o thiết kế điều khiển cho hệ thống robot vận hành theo mong muốn Để thiết kế điều khiển cho robot, nhiệm vụ quan trọng thiếu việc thiết kế quỹ đạo mẫu cho robot Chất lượng quỹ đạo mẫu robot ảnh hưởng lớn tới điều khiển đầu hệ thống Qũy đạo mẫu tốt với điều khiển tốt tạo đầu hệ thống tốt, giảm thiểu sai sót đưa robot vào đời sống sản xuất Một kĩ thuật để tạo quỹ đạo mẫu có chất lượng tốt tạo quỹ đạo tối ưu tối ưu thời gian, tối ưu độ giật… Và mục đích tạo quỹ đạo mẫu tối ưu, có chất lượng tốt, đáp ứng yêu cầu điều khiển robot tạo động lực cho em thực đề tài” “Thiết kế quỹ đạo mẫu đảm bảo tối ƣu thời gian độ giật cho cánh tay máy” Việc phát triển robot việc thiết yếu để phát triển công nghiệp, xã hội Sau công cải tiến mặt kích thước sức mạnh cho máy móc, việc tự động hóa vấn đề đặt dành cho nhiệm vụ sau Việc tự động hóa tăng an tồn cách loại bỏ người điều khiển khỏi máy móc, thay vào cơng cụ vi xử lý, cảm biến để làm 1|Page nhiệm vụ người điều khiển Để robot hoạt động cách tự động, ta cần phải điều khiển vị trí định hướng cho khâu khớp Để làm điều này, ta cần có mơ hình tốn động học, động lực học trajectory planning, chúng đầu vào điều khiển Một điều khiển tự động tốt điều tối quan trọng cho trajectory planning Ở luận văn em tập trung vào trajectory planning, định nghĩa là: tạo đầu vào mẫu cho hệ thống điều khiển robot cho chúng đảm bảo thực hoạt động mong muốn Trong thiết kế bản, trajectory planning rút path planning Trong trình nghiên cứu tìm hiểu, Em nhận giúp đỡ nhiệt tình TS Vũ Thị Thúy Nga Em xin chân thành cảm ơn Ngoài ra, Em xin cảm ơn thầy cô mơn Điều khiển tự động giúp đỡ Em hồn thành đồ án Vì thời gian có hạn, khơng thể tránh khỏi thiếu sót hạn chế, Em mong nhận góp ý thầy cô bạn cho nghiên cứu Em xin chân thành cảm ơn 2|Page DANH MỤC CÁC KÍ HIỆU, CHỮ VIẾT TẮT CÁC KÍ HIỆU p Bậc B-spline Bp (t ) B-spline với bậc p Ni , p (t ) Hàm sở bậc p Qi Các điểm điều khiển B-spline n 1 Số lượng điểm điều khiển ti Các điểm nút m 1 Số lượng điểm nút q (t ) Qũy đạo vị trí khớp v(t ) Qũy đạo vận tốc khớp a (t ) Qũy đạo gia tốc khớp j (t ) Qũy đạo độ giật khớp FOBJ Hàm mục tiêu thời gian độ giật CÁC CHỮ VIẾT TẮT PSO Particle Swarm Optimization GA Genetic Algorithm ABC Colony Artifical Bee TLBO Teaching Learning Based Optimization CPV Control Point Velocity CPA Control Point Acceleration CPJ Control Point Jerk 3|Page CÁC THUẬT NGỮ TIẾNG ANH Trajectory Qũy đạo Velocity Vận tốc Acceleration Gia tốc Jerk Độ giật Kinematic Động học Interpolation Nội suy 4|Page DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ, BẢNG BIỂU Hình 1: Đồ thị chuyển động hàm vị trí đa thức bậc ba .8 Hình 2: Thiết kế quỹ đạo với hàm vận tốc có dạng tam giác bậc thang Hình 1.3: Đồ thị chuyển động mơ tả cho chuyển động hình sin vị trí vận tốc Hình 4: Sơ đồ khối mạng nơ – ron động có phản hồi 11 Hình 1:Sơ đồ tam giác biểu diễn hàm sở theo De Boor………………… 14 Hình 2:Qũy đạo Bp (t ) trường hợp dãy nút bị kẹp…………………….17 Hình 3: Đồ thi hàm sở N0,2 (t ) , N1,2 (t ) , N 2,2 (t ) …………………………… 23 Hình 3.1: Mơ hình tay máy xúc………………………………………………………… 41 Hình 3.2:Qũy đạo vị trí khớp trường hợp kT , kJ ……………….42 Hình 3.3:Qũy đạo độ giật khớp trường hợp kT , kJ ………… 43 Hình 3.4: Qũy đạo vị trí khớp trường hợp kT , kJ 10 …………………….44 Hình 3.5: Qũy đạo độ giật khớp trường hợp kT , kJ 10 ………………….44 Bảng 1: Các kí hiệu đường cong B-spline (2.1) (2.2)………………15 Bảng 1: Ý nghĩa kí hiệu (3.1) 32 Bảng 2: Các điểm qua cho trước 41 Bảng 3: Các ràng buộc động học cho khớp 42 Bảng 4: Nghiệm thời gian toán tối ưu với kT , kJ 42 Bảng 5: Nghiệm thời gian toán tối ưu với kT , kJ 43 Bảng 6: So sánh giá trị độ giật trường hợp kT , kJ 45 5|Page CHƢƠNG 1: TỔNG QUAN VỀ BÀI TOÁN THIẾT KẾ QUỸ ĐẠO MẪU 1.1 Tầm quan trọng toán thiết kế quỹ đạo mẫu Trong toán kĩ thuật nay, toán thiết kế quỹ đạo mẫu toán cần đầu tư nghiên cứu chuyên sâu thêm tính quan trọng toán thực tiễn, đặc biệt toán điều khiển tay máy robot Trong việc điều khiển cánh tay máy, giả sử giải toán điều khiển để khớp bám theo giá trị đặt, nhiên thực tế cho thấy việc chưa đủ để cánh tay máy hoạt động Ngồi việc bám theo giá trị đặt quỹ đạo chuyển động khớp vấn đề cần tính tốn, ví dụ cánh tay máy gặp vật cản quỹ đạo khớp cuối phải không phép qua vật cản Bài toán thiết kế quỹ đạo mẫu định nghĩa sau: tìm luật chuyển động theo thời gian dọc theo đường cho trước yêu cầu định tính chất quỹ đạo phải đáp ứng Kết việc thiết kế quỹ đạo tạo tín hiệu đặt cho hệ thống tay máy, để hệ thống thực thi theo quỹ đạo định Hình dáng quỹ đạo, ràng buộc động học, động lực học đầu vào thuật toán thiết kế quỹ đạo mẫu Quỹ đạo khớp, hay nói quy luật thời gian vị trí, vận tốc, gia tốc đầu tốn Hình dáng quỹ đạo xác định không gian hoạt động hệ thống tay máy, công việc cần thực thi vật cản cần tránh dễ dàng mô tả thông qua khơng gian Bên cạnh đó, việc thiết kế quỹ đạo thiết kế không gian khớp Quỹ đạo khớp đạt thông qua việc nội suy hàm số mà đáp ứng ràng buộc động học động lực học Để hệ thống tay máy hoạt động ổn định, việc thiết kế quỹ đạo phải đảm bảo cho quỹ đạo tạo phải trơn, đặc biệt gia tốc khớp phải liên tục để độ giật (đạo hàm gia tốc) bị chặn Việc giới hạn độ giật quan trọng, độ giật lớn làm mịn kết cấu hệ thống tay máy, khả cao gây 6|Page dao động cộng hưởng không mong muốn Những rung lắc tạo quỹ đạo khơng trơn gây hại cho cấu chấp hành, tạo sai số lớn robot thực thi việc bám quỹ đạo Thêm vào đó, quỹ đạo có độ giật thấp thực thi cách nhanh chóng xác 1.2 Một số phƣơng pháp thiết kế quỹ đạo cho cánh tay máy 1.2.1 Phƣơng pháp nội suy Như trình bày mục 1, hình dáng quỹ đạo, ràng buộc động học, động lực học đầu vào thuật tốn thiết kế quỹ đạo mẫu Hình dáng quỹ đạo hiểu điểm cho trước mà canh tay máy cần qua Từ ý tưởng việc thiết kế quỹ đạo dựa phương pháp nội suy xét đến Quỹ đạo cần thiết kế thường nội suy theo hàm đa thức hàm sin Như biết vận tốc đạo hàm vị trí, gia tốc đạo hàm vận tốc, độ giật đạo hàm gia tốc Bởi vậy, nội suy theo hàm đa thức thường ưu tiên sử dụng hàm đa thức dễ tính đạo hàm Các cơng thức (1.1) đến (1.4) mơ tả đại lượng vị trí, vận tốc, gia tốc theo hàm đa thức s t co c1t c2t c3t (1.1) v t ds c1 2c2t 3c3t dt (1.2) a t dv 2c2 6c3t dt (1.3) j t da 6c3 dt (1.4) Trong hệ số c0 c3 định nghĩa tính chất chuyển động Hình 1.1 đồ thị chuyển động đơn giản mô tả hàm vị trí đa thức bậc ba, hàm vận tốc bậc hai hàm gia tốc bậc 7|Page c(57)=abs(CPA2(7))-WC(2); c(58)=abs(CPA2(8))-WC(2); c(59)=abs(CPA2(9))-WC(2); c(60)=abs(CPA2(10))-WC(2); c(61)=abs(CPA3(1))-WC(3); c(62)=abs(CPA3(2))-WC(3); c(63)=abs(CPA3(3))-WC(3); c(64)=abs(CPA3(4))-WC(3); c(65)=abs(CPA3(5))-WC(3); c(66)=abs(CPA3(6))-WC(3); c(67)=abs(CPA3(7))-WC(3); c(68)=abs(CPA3(8))-WC(3); c(69)=abs(CPA3(9))-WC(3); c(70)=abs(CPA3(10))-WC(3); c(71)=abs(CPJ1(1))-JC(1); c(72)=abs(CPJ1(2))-JC(1); c(73)=abs(CPJ1(3))-JC(1); c(74)=abs(CPJ1(4))-JC(1); c(75)=abs(CPJ1(5))-JC(1); c(76)=abs(CPJ1(6))-JC(1); c(77)=abs(CPJ1(7))-JC(1); c(78)=abs(CPJ1(8))-JC(1); c(79)=abs(CPJ1(9))-JC(1); c(80)=abs(CPJ2(1))-JC(2); c(81)=abs(CPJ2(2))-JC(2); c(82)=abs(CPJ2(3))-JC(2); c(83)=abs(CPJ2(4))-JC(2); c(84)=abs(CPJ2(5))-JC(2); c(85)=abs(CPJ2(6))-JC(2); c(86)=abs(CPJ2(7))-JC(2); c(87)=abs(CPJ2(8))-JC(2); c(88)=abs(CPJ2(9))-JC(2); c(89)=abs(CPJ3(1))-JC(3); c(90)=abs(CPJ3(2))-JC(3); 63 | P a g e c(91)=abs(CPJ3(3))-JC(3); c(92)=abs(CPJ3(4))-JC(3); c(93)=abs(CPJ3(5))-JC(3); c(94)=abs(CPJ3(6))-JC(3); c(95)=abs(CPJ3(7))-JC(3); c(96)=abs(CPJ3(8))-JC(3); c(97)=abs(CPJ3(9))-JC(3); ceq=[]; end Chƣơng trình xây dựng hàm tối ƣu FOBJ FOBJ.m function FOBJ= FOBJ(h) n=11;p=5;kT=1;kJ=0; % Nut: t1=0;t2=0;t3=0;t4=0;t5=0;t6=0;t7=h(1);t8=h(1)+h(2);t9=h(1)+h(2)+h(3);t10= h(1)+h(2)+h(3)+h(4);t11=h(1)+h(2)+h(3)+h(4)+h(5);t12=h(1)+h(2)+h(3)+h(4)+ h(5)+h(6);t13=h(1)+h(2)+h(3)+h(4)+h(5)+h(6)+h(7);t14=h(1)+h(2)+h(3)+h(4)+ h(5)+h(6)+h(7);t15=h(1)+h(2)+h(3)+h(4)+h(5)+h(6)+h(7); t16=h(1)+h(2)+h(3)+h(4)+h(5)+h(6)+h(7);t17=h(1)+h(2)+h(3)+h(4)+h(5)+h(6)+ h(7);t18=h(1)+h(2)+h(3)+h(4)+h(5)+h(6)+h(7); Uq= [ t1 t2 t3 t4 t5 t6 t7 t8 t9 t10 t11 t12 t13 t14 t15 t16 t17 t18]; Uv= [t2 t3 t4 t5 t6 t7 t8 t9 t10 t11 t12 t13 t14 t15 t16 t17]; Ua= [t3 t4 t5 t6 t7 t8 t9 t10 t11 t12 t13 t14 t15 t16]; t = [ t1 t2 t3 t4 t5 t6 t7 t8 t9 t10 t11 t12 t13 t14 t15 t16 t17 t18]; %Giai he phuong trinh: A =[ -p/(Uq(p+2)-Uq(2)), p/(Uq(p+2)-Uq(2)), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ((p)/(Uv(p + 1)*Uq(2) + Uq(p + 2)*Uv(2) - Uq(2)*Uv(2) - Uq(p + 2)*Uv(p + 1)) - (p^2)/(Uv(p + 1)*Uq(2) + Uq(p + 2)*Uv(2) - Uq(2)*Uv(2) Uq(p + 2)*Uv(p + 1))) (-(p)/(Uv(p + 1)*Uq(2) + Uq(p + 2)*Uv(2) - Uq(2)*Uv(2) - Uq(p + 2)*Uv(p + 1)) - (p)/(Uv(p + 1)*Uq(3) + Uq(p + 3)*Uv(2) - Uq(3)*Uv(2) - Uq(p + 3)*Uv(p + 1)) + + (p^2)/(Uv(p + 1)*Uq(2) + Uq(p + 2)*Uv(2) - Uq(2)*Uv(2) - Uq(p + 2)*Uv(p + 1)) + (p^2)/(Uv(p + 64 | P a g e 1)*Uq(3) + Uq(p + 3)*Uv(2) - Uq(3)*Uv(2) - Uq(p + 3)*Uv(p + 1))) ((p)/(Uv(p + 1)*Uq(3) + Uq(p + 3)*Uv(2) - Uq(3)*Uv(2) - Uq(p + 3)*Uv(p + 1)) - (p^2)/(Uv(p + 1)*Uq(3) + Uq(p + 3)*Uv(2) - Uq(3)*Uv(2) - Uq(p + 3)*Uv(p + 1))) 0 0 0 0 0; (p*(p - 1)*(p - 2))/((Uq(p + 2) - Uq(2))*(Uv(p + 1) - Uv(2))*(Ua(2) Ua(p))),-((((p/(Uq(p + 2) - Uq(2)) + p/(Uq(p + 3) - Uq(3)))*(p - 1))/(Uv(p + 1) - Uv(2)) + (p*(p - 1))/((Uq(p + 3) - Uq(3))*(Uv(p + 2) Uv(3))))*(p - 2))/(Ua(2) - Ua(p)), ((((p/(Uq(p + 3) - Uq(3)) + p/(Uq(p + 4) - Uq(4)))*(p - 1))/(Uv(p + 2) - Uv(3)) + (p*(p - 1))/((Uq(p + 3) Uq(3))*(Uv(p + 1) - Uv(2))))*(p - 2))/(Ua(2) - Ua(p)),-(p*(p - 1)*(p 2))/((Uq(p + 4) - Uq(4))*(Uv(p + 2) - Uv(3))*(Ua(2) - Ua(p))),0,0,0,0,0,0,0,0; 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 0,0,(t(8) - t(9))^4/((t(4) - t(9))*(t(5) - t(9))*(t(6) - t(9))*(t(7) - t(9))),((((t(8) - t(10))*(((t(6) t(9))*(t(7) - t(9))) + t(9))*(t(7) - t(10)))))/(t(6) t(9))^2)/((t(5) - t(10)))/(t(5) t(10)) - t(9))*(t(5) - ((t(7) - - t(8))*(t(8) - ((t(4) t(9))*(t(6) t(8))*(t(8) t(10)) t(9))*(t(6) + - - ((t(5) t(9))*(t(7) - - + - t(8))*(t(8) t(9))*(t(7) t(9)))/((t(6) - t(10)))/((t(7) - - - t(8))*(t(8) - t(9))))*(t(8) - - t(9))^3)/((t(4) - - t(9))),((((t(8) - t(10))*(((t(6) - t(8))*(t(8) - t(9)))/((t(6) - t(9))*(t(7) - t(9))) + ((t(7) - t(8))*(t(8) - t(10)))/((t(7) - t(9))*(t(7) - t(10)))))/(t(6) t(10)) + ((t(5) t(9))*(t(7) - - t(8))*(t(8) t(9))))*(t(5) - - t(9))^2)/((t(5) t(8)))/(t(5) - - t(9))*(t(6) - + - t(10)) ((((t(6) t(8))*(((t(6) - t(8))*(t(8) - t(9)))/((t(6) - t(9))*(t(7) - t(9))) + ((t(7) - t(8))*(t(8) - t(10)))/((t(7) - t(9))*(t(7) - t(10)))))/(t(6) t(10)) + ((t(7) t(10))*(t(7) - - t(8))^2*(t(8) t(11))))*(t(8) - - t(11)))/((t(7) t(11)))/(t(6) - - t(9))*(t(7) - t(11)),((((t(6) - t(8))*(((t(6) - t(8))*(t(8) - t(9)))/((t(6) - t(9))*(t(7) - t(9))) + ((t(7) - t(8))*(t(8) - t(10)))/((t(7) - t(9))*(t(7) - t(10)))))/(t(6) t(10)) + ((t(7) t(10))*(t(7) - t(8))^2*(t(8) t(11))))*(t(6) t(8))^3*(t(8) t(11))*(t(7) - - - t(12))),(t(7) t(11)))/((t(7) t(8)))/(t(6) t(12)))/((t(7) - - - - - t(9))*(t(7) t(11)) t(9))*(t(7) - t(8))^4/((t(7) + ((t(7) - t(10))*(t(7) - - t(9))*(t(7) - - t(10))*(t(7) - t(9))*(t(9) - t(9))*(t(9) - t(10))*(t(7) - t(11))*(t(7) - t(12))),0,0,0,0,0; 0,0,0,(t(9) t(10))*(t(8) - - t(10))^4/((t(5) t(10))),((((t(9) t(10)))/((t(7) - t(11)))/((t(8) - t(10))*(t(8) t(10))*(t(8) - - t(10))*(t(6) t(11))*(((t(7) t(10))) + ((t(8) t(11)))))/(t(7) - - t(11)) + ((t(6) - 65 | P a g e t(9))*(t(9) - t(10))^2)/((t(6) t(10))))*(t(9) - - t(11)))/(t(6) - t(10))^3)/((t(5) - t(10))*(t(6) t(10))),((((t(9) - t(11))*(((t(7) t(10))*(t(8) - t(10))) t(10))*(t(8) - t(11)))))/(t(7) t(10))^2)/((t(6) t(9)))/(t(6) - - + - ((t(8) - + - t(10))*(t(8) t(11)))/((t(8) - t(9))^2*(t(9) - t(12)))/((t(8) t(12))))*(t(9) - t(12)))/(t(7) t(9))*(t(9) t(10)))/((t(7) t(10))*(t(8) + t(9))*(t(9) - t(10))*(t(8) - - t(10)))/((t(7) - - t(9))*(t(9) - t(11)))/((t(8) - - t(9))*(t(9) - t(10))))*(t(6) - - t(9))*(t(9) - - t(9))*(t(9) - + - ((t(6) + ((t(8) t(11)))))/(t(7) - - t(9))*(((t(7) t(10))) - - t(10))*(t(8) - t(11)) t(10))*(t(8) - - - t(9))*(t(9) - - t(10))*(t(8) ((t(5) t(10))*(t(7) t(11)) - - - ((((t(7) t(10)))/((t(7) - t(11)) t(10))*(t(7) t(11)) t(10))*(t(7) t(12)),((((t(7) t(10))*(t(8) - + ((t(8) - - t(11))*(t(8) - - t(9))*(((t(7) - t(10))) + ((t(8) - t(9))*(t(9) - t(11)))/((t(8) - t(10))*(t(8) - t(11)))))/(t(7) - t(11)) + ((t(8) - t(9))^2*(t(9) - t(12)))/((t(8) - t(10))*(t(8) - t(11))*(t(8) t(12))))*(t(7) - t(13)))/((t(8) t(9)))/(t(7) - t(13))),(t(8) - t(12)) t(10))*(t(8) - - t(9))^4/((t(8) - + ((t(8) - t(9))^3*(t(9) - t(11))*(t(8) - t(12))*(t(8) - t(10))*(t(8) - t(11))*(t(8) - t(12))*(t(8) - t(13))),0,0,0,0,; 0,0,0,0,(t(10) t(11))*(t(9) - - t(11))^4/((t(6) t(11))),((((t(10) - - t(11))*(t(7) t(12))*(((t(8) t(11)))/((t(8) - t(11))*(t(9) - t(11))) t(12)))/((t(9) - t(11))*(t(9) - t(12)))))/(t(8) t(10))*(t(10) - t(11))^2)/((t(7) t(11))))*(t(10) - t(12)))/(t(7) t(11))^3)/((t(6) - - - - ((t(9) - + t(11))*(t(8) - - t(10))*(t(10) - - t(10))*(t(10) - t(12)) t(11))*(t(8) t(12)) t(11))*(t(7) + - ((t(7) - - t(11))*(t(9) - - t(10))*(t(10) - t(11))*(t(9) - ((t(6) t(11))*(t(8) + - t(11))),((((t(10) - t(12))*(((t(8) - t(10))*(t(10) - t(11)))/((t(8) t(11))*(t(9) - t(11))) t(11))*(t(9) - t(12)))))/(t(8) t(11))^2)/((t(7) - + ((t(9) - - t(10))*(t(10) t(12)) t(11))*(t(8) - + - ((t(7) t(11))*(t(9) - t(12)))/((t(9) - t(10))*(t(10) - t(11))))*(t(7) - t(10)))/(t(7) - t(12)) + ((((t(8) - t(10))*(((t(8) - t(10))*(t(10) t(11)))/((t(8) - t(11))*(t(9) - t(11))) t(12)))/((t(9) - t(11))*(t(9) - t(12)))))/(t(8) t(10))^2*(t(10) t(13))))*(t(10) t(10))*(t(10) - - t(13)))/((t(9) t(13)))/(t(8) t(11)))/((t(8) - - + ((t(9) - t(11))*(t(9) t(13)),((((t(8) t(11))*(t(9) - - t(10))*(t(10) t(12)) - + ((t(9) - t(12))*(t(9) - t(10))*(((t(8) - t(11))) + ((t(9) - t(10))*(t(10) - t(12)))/((t(9) - t(11))*(t(9) - t(12)))))/(t(8) - t(12)) + ((t(9) - t(10))^2*(t(10) - t(13)))/((t(9) - t(11))*(t(9) - t(12))*(t(9) - t(13))))*(t(8) - t(10)))/(t(8) - t(13)) + ((t(9) - t(10))^3*(t(10) - 66 | P a g e t(14)))/((t(9) - t(14))),(t(9) t(11))*(t(9) - - t(10))^4/((t(9) t(12))*(t(9) - - t(11))*(t(9) - t(13))*(t(9) - t(12))*(t(9) - t(13))*(t(9) - t(14))),0,0,0; 0,0,0,0,0,(t(11) - t(12))^4/((t(7) - t(12))*(t(8) - t(12))*(t(9) t(12))*(t(10) - t(12))),((((t(11) - t(13))*(((t(9) - t(11))*(t(11) - t(12)))/((t(9) - t(12))*(t(10) - t(12))) + ((t(10) - t(11))*(t(11) t(13)))/((t(10) - t(12))*(t(10) - t(13)))))/(t(9) - t(13)) + ((t(8) t(11))*(t(11) - t(12))))*(t(11) t(12))^2)/((t(8) - t(13)))/(t(8) t(12))^3)/((t(7) - - - t(12))*(t(9) t(13)) t(12))*(t(8) - + - ((t(7) - t(12))*(t(9) - t(12))*(t(10) - t(11))*(t(11) - t(12))*(t(10) - t(12))),((((t(11) - t(13))*(((t(9) - t(11))*(t(11) - t(12)))/((t(9) t(12))*(t(10) - t(12))) + ((t(10) - t(11))*(t(11) - t(13)))/((t(10) t(12))*(t(10) - t(13)))))/(t(9) t(12))^2)/((t(8) - - t(12))*(t(9) t(13)) - + ((t(8) t(12))*(t(10) - t(11))*(t(11) - t(12))))*(t(8) - t(11)))/(t(8) - t(13)) + ((((t(9) - t(11))*(((t(9) - t(11))*(t(11) t(12)))/((t(9) - t(12))*(t(10) - t(12))) + ((t(10) - t(11))*(t(11) t(13)))/((t(10) - t(12))*(t(10) - t(13)))))/(t(9) - t(13)) + ((t(10) t(11))^2*(t(11) - t(14)))/((t(10) - t(14))))*(t(11) - t(14)))/(t(9) t(14)),((((t(9) - t(12))*(t(10) - t(13))*(t(10) - t(11))*(((t(9) - t(11))*(t(11) - t(12)))/((t(9) - t(12))*(t(10) - t(12))) + ((t(10) t(11))*(t(11) - t(13)))/((t(10) - t(12))*(t(10) - t(13)))))/(t(9) - t(13)) + ((t(10) - t(11))^2*(t(11) - t(14)))/((t(10) - t(12))*(t(10) t(13))*(t(10) - t(11))^3*(t(11) t(14))*(t(10) - t(14))))*(t(9) - - t(15)))/((t(10) t(15))),(t(10) - t(11)))/(t(9) - - t(14)) + ((t(10) - t(12))*(t(10) - t(13))*(t(10) - t(11))^4/((t(10) - t(12))*(t(10) - t(13))*(t(10) - t(14))*(t(10) - t(15))),0,0; 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; 0 0 0 0 (-(2*p)/(Ua(n + p - 3)*Uq(n + p - 1)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p - 1) + Ua(n - 1)*Uq(n - 1)*Uv(n + p - 2) + Uq(n - 1)*Uv(n - 1)*Ua(n + p - 3) - Ua(n - 1)*Uq(n + p - 1)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p - 1) - Uq(n - 1)*Ua(n + p - 3)*Uv(n + p - 2) - Ua(n - 1)*Uq(n - 1)*Uv(n - 1)) + (3*p^2)/(Ua(n + p - 3)*Uq(n + p - 1)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p - 1) + Ua(n - 1)*Uq(n 1)*Uv(n + p - 2) + Uq(n - 1)*Uv(n - 1)*Ua(n + p - 3) - Ua(n - 1)*Uq(n + p - 1)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p - 1) - Uq(n 1)*Ua(n + p - 3)*Uv(n + p - 2) - Ua(n - 1)*Uq(n - 1)*Uv(n - 1)) (p^3)/(Ua(n + p - 3)*Uq(n + p - 1)*Uv(n + p - 2) + Ua(n - 1)*Uv(n 1)*Uq(n + p - 1) + Ua(n - 1)*Uq(n - 1)*Uv(n + p - 2) + Uq(n - 1)*Uv(n 1)*Ua(n + p - 3) - Ua(n - 1)*Uq(n + p - 1)*Uv(n + p - 2) - Uv(n - 1)*Ua(n 67 | P a g e + p - 3)*Uq(n + p - 1) - Uq(n - 1)*Ua(n + p - 3)*Uv(n + p - 2) - Ua(n 1)*Uq(n - 1)*Uv(n - 1))) ((p^3)/(Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p) - Ua(n + p - 3)*Uv(n + p - 2)*Uq(n) - Ua(n - 1)*Uv(n - 1)*Uq(n) - Ua(n - 1)*Uq(n + p)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p) + Ua(n - 1)*Uv(n + p - 2)*Uq(n) + Uv(n - 1)*Ua(n + p 3)*Uq(n)) - (3*p^2)/(Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 2) + Ua(n 1)*Uv(n - 1)*Uq(n + p) - Ua(n + p - 3)*Uv(n + p - 2)*Uq(n) - Ua(n 1)*Uv(n - 1)*Uq(n) - Ua(n - 1)*Uq(n + p)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p) + Ua(n - 1)*Uv(n + p - 2)*Uq(n) + Uv(n - 1)*Ua(n + p 3)*Uq(n)) + (2*p)/(Ua(n + p - 3)*Uq(n + p - 1)*Uv(n + p - 2) + Ua(n 1)*Uv(n - 1)*Uq(n + p - 1) + Ua(n - 1)*Uq(n - 1)*Uv(n + p - 2) + Uq(n 1)*Uv(n - 1)*Ua(n + p - 3) - Ua(n - 1)*Uq(n + p - 1)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p - 1) - Uq(n - 1)*Ua(n + p - 3)*Uv(n + p - 2) - Ua(n - 1)*Uq(n - 1)*Uv(n - 1)) - (2*p)/(Ua(n - 1)*Uq(n)*Uv(n) - Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 1) + Ua(n + p - 3)*Uq(n + p)*Uv(n) + Ua(n + p - 3)*Uv(n + p - 1)*Uq(n) - Ua(n + p - 3)*Uq(n)*Uv(n) + Ua(n - 1)*Uq(n + p)*Uv(n + p - 1) - Ua(n - 1)*Uq(n + p)*Uv(n) - Ua(n - 1)*Uv(n + p 1)*Uq(n)) - (3*p^2)/(Ua(n + p - 3)*Uq(n + p - 1)*Uv(n + p - 2) + Ua(n 1)*Uv(n - 1)*Uq(n + p - 1) + Ua(n - 1)*Uq(n - 1)*Uv(n + p - 2) + Uq(n 1)*Uv(n - 1)*Ua(n + p - 3) - Ua(n - 1)*Uq(n + p - 1)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p - 1) - Uq(n - 1)*Ua(n + p - 3)*Uv(n + p - 2) - Ua(n - 1)*Uq(n - 1)*Uv(n - 1)) + (p^3)/(Ua(n + p - 3)*Uq(n + p 1)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p - 1) + Ua(n - 1)*Uq(n 1)*Uv(n + p - 2) + Uq(n - 1)*Uv(n - 1)*Ua(n + p - 3) - Ua(n - 1)*Uq(n + p - 1)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p - 1) - Uq(n 1)*Ua(n + p - 3)*Uv(n + p - 2) - Ua(n - 1)*Uq(n - 1)*Uv(n - 1)) + (3*p^2)/(Ua(n - 1)*Uq(n)*Uv(n) - Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 1) + Ua(n + p - 3)*Uq(n + p)*Uv(n) + Ua(n + p - 3)*Uv(n + p - 1)*Uq(n) - Ua(n + p - 3)*Uq(n)*Uv(n) + Ua(n - 1)*Uq(n + p)*Uv(n + p - 1) - Ua(n - 1)*Uq(n + p)*Uv(n) - Ua(n - 1)*Uv(n + p - 1)*Uq(n)) - (p^3)/(Ua(n - 1)*Uq(n)*Uv(n) - Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 1) + Ua(n + p 3)*Uq(n + p)*Uv(n) + Ua(n + p - 3)*Uv(n + p - 1)*Uq(n) - Ua(n + p 3)*Uq(n)*Uv(n) + Ua(n - 1)*Uq(n + p)*Uv(n + p - 1) - Ua(n - 1)*Uq(n + p)*Uv(n) - Ua(n - 1)*Uv(n + p - 1)*Uq(n)) + (2*p)/(Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p) - Ua(n + p - 3)*Uv(n + p - 2)*Uq(n) - Ua(n - 1)*Uv(n - 1)*Uq(n) - Ua(n - 1)*Uq(n + p)*Uv(n + p 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p) + Ua(n - 1)*Uv(n + p - 2)*Uq(n) + Uv(n - 1)*Ua(n + p - 3)*Uq(n))) (-(3*p^2)/(Ua(n - 1)*Uq(n)*Uv(n) - Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 1) + Ua(n + p - 3)*Uq(n + p)*Uv(n) + Ua(n + p 68 | P a g e - 3)*Uv(n + p - 1)*Uq(n) - Ua(n + p - 3)*Uq(n)*Uv(n) + Ua(n - 1)*Uq(n + p)*Uv(n + p - 1) - Ua(n - 1)*Uq(n + p)*Uv(n) - Ua(n - 1)*Uv(n + p 1)*Uq(n)) + (p^3)/(Ua(n - 1)*Uq(n)*Uv(n) - Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 1) + Ua(n + p - 3)*Uq(n + p)*Uv(n) + Ua(n + p - 3)*Uv(n + p 1)*Uq(n) - Ua(n + p - 3)*Uq(n)*Uv(n) + Ua(n - 1)*Uq(n + p)*Uv(n + p - 1) - Ua(n - 1)*Uq(n + p)*Uv(n) - Ua(n - 1)*Uv(n + p - 1)*Uq(n)) - (2*p)/(Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p) - Ua(n + p - 3)*Uv(n + p - 2)*Uq(n) - Ua(n - 1)*Uv(n - 1)*Uq(n) - Ua(n - 1)*Uq(n + p)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p) + Ua(n 1)*Uv(n + p - 2)*Uq(n) + Uv(n - 1)*Ua(n + p - 3)*Uq(n)) - (2*p)/(Ua(n + p - 3)*Uq(n + p + 1)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + 1)*Uv(n + p - 1) Ua(n + p - 3)*Uq(n + p + 1)*Uv(n) - Ua(n - 1)*Uq(n + 1)*Uv(n) - Ua(n 1)*Uq(n + p + 1)*Uv(n + p - 1) - Uq(n + 1)*Ua(n + p - 3)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + p + 1)*Uv(n) + Uq(n + 1)*Ua(n + p - 3)*Uv(n)) + (3*p^2)/(Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p) - Ua(n + p - 3)*Uv(n + p - 2)*Uq(n) - Ua(n - 1)*Uv(n - 1)*Uq(n) Ua(n - 1)*Uq(n + p)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p - 3)*Uq(n + p) + Ua(n - 1)*Uv(n + p - 2)*Uq(n) + Uv(n - 1)*Ua(n + p - 3)*Uq(n)) + (3*p^2)/(Ua(n + p - 3)*Uq(n + p + 1)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + 1)*Uv(n + p - 1) - Ua(n + p - 3)*Uq(n + p + 1)*Uv(n) - Ua(n - 1)*Uq(n + 1)*Uv(n) - Ua(n - 1)*Uq(n + p + 1)*Uv(n + p - 1) - Uq(n + 1)*Ua(n + p 3)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + p + 1)*Uv(n) + Uq(n + 1)*Ua(n + p 3)*Uv(n)) - (p^3)/(Ua(n + p - 3)*Uq(n + p)*Uv(n + p - 2) + Ua(n - 1)*Uv(n - 1)*Uq(n + p) - Ua(n + p - 3)*Uv(n + p - 2)*Uq(n) - Ua(n - 1)*Uv(n 1)*Uq(n) - Ua(n - 1)*Uq(n + p)*Uv(n + p - 2) - Uv(n - 1)*Ua(n + p 3)*Uq(n + p) + Ua(n - 1)*Uv(n + p - 2)*Uq(n) + Uv(n - 1)*Ua(n + p 3)*Uq(n)) - (p^3)/(Ua(n + p - 3)*Uq(n + p + 1)*Uv(n + p - 1) + Ua(n 1)*Uq(n + 1)*Uv(n + p - 1) - Ua(n + p - 3)*Uq(n + p + 1)*Uv(n) - Ua(n 1)*Uq(n + 1)*Uv(n) - Ua(n - 1)*Uq(n + p + 1)*Uv(n + p - 1) - Uq(n + 1)*Ua(n + p - 3)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + p + 1)*Uv(n) + Uq(n + 1)*Ua(n + p - 3)*Uv(n)) + (2*p)/(Ua(n - 1)*Uq(n)*Uv(n) - Ua(n + p 3)*Uq(n + p)*Uv(n + p - 1) + Ua(n + p - 3)*Uq(n + p)*Uv(n) + Ua(n + p 3)*Uv(n + p - 1)*Uq(n) - Ua(n + p - 3)*Uq(n)*Uv(n) + Ua(n - 1)*Uq(n + p)*Uv(n + p - 1) - Ua(n - 1)*Uq(n + p)*Uv(n) - Ua(n - 1)*Uv(n + p 1)*Uq(n))) (-(3*p^2)/(Ua(n + p - 3)*Uq(n + p + 1)*Uv(n + p - 1) + Ua(n 1)*Uq(n + 1)*Uv(n + p - 1) - Ua(n + p - 3)*Uq(n + p + 1)*Uv(n) - Ua(n 1)*Uq(n + 1)*Uv(n) - Ua(n - 1)*Uq(n + p + 1)*Uv(n + p - 1) - Uq(n + 1)*Ua(n + p - 3)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + p + 1)*Uv(n) + Uq(n + 1)*Ua(n + p - 3)*Uv(n)) + (p^3)/(Ua(n + p - 3)*Uq(n + p + 1)*Uv(n + p - 69 | P a g e 1) + Ua(n - 1)*Uq(n + 1)*Uv(n + p - 1) - Ua(n + p - 3)*Uq(n + p + 1)*Uv(n) - Ua(n - 1)*Uq(n + 1)*Uv(n) - Ua(n - 1)*Uq(n + p + 1)*Uv(n + p 1) - Uq(n + 1)*Ua(n + p - 3)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + p + 1)*Uv(n) + Uq(n + 1)*Ua(n + p - 3)*Uv(n)) + (2*p)/(Ua(n + p - 3)*Uq(n + p + 1)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + 1)*Uv(n + p - 1) - Ua(n + p 3)*Uq(n + p + 1)*Uv(n) - Ua(n - 1)*Uq(n + 1)*Uv(n) - Ua(n - 1)*Uq(n + p + 1)*Uv(n + p - 1) - Uq(n + 1)*Ua(n + p - 3)*Uv(n + p - 1) + Ua(n - 1)*Uq(n + p + 1)*Uv(n) + Uq(n + 1)*Ua(n + p - 3)*Uv(n))); 0 0 0 0 (-p/(Uq(n + p)*Uv(n + p - 1) - Uq(n + p)*Uv(n) Uv(n + p - 1)*Uq(n) + Uq(n)*Uv(n)) + (p^2)/(Uq(n + p)*Uv(n + p - 1) Uq(n + p)*Uv(n) - Uv(n + p - 1)*Uq(n) + Uq(n)*Uv(n))) (p/(Uq(n + p)*Uv(n + p - 1) - Uq(n + p)*Uv(n) - Uv(n + p - 1)*Uq(n) + Uq(n)*Uv(n)) - p/(Uq(n + 1)*Uv(n + p - 1) - Uq(n + 1)*Uv(n) - Uq(n + p + 1)*Uv(n + p - 1) + Uq(n + p + 1)*Uv(n)) - (p^2)/(Uq(n + p)*Uv(n + p - 1) - Uq(n + p)*Uv(n) - Uv(n + p - 1)*Uq(n) + Uq(n)*Uv(n)) + (p^2)/(Uq(n + 1)*Uv(n + p - 1) - Uq(n + 1)*Uv(n) - Uq(n + p + 1)*Uv(n + p - 1) + Uq(n + p + 1)*Uv(n))) (p/(Uq(n + 1)*Uv(n + p - 1) - Uq(n + 1)*Uv(n) - Uq(n + p + 1)*Uv(n + p - 1) + Uq(n + p + 1)*Uv(n))-(p^2)/(Uq(n + 1)*Uv(n + p - 1) - Uq(n + 1)*Uv(n) - Uq(n + p + 1)*Uv(n + p - 1) + Uq(n + p + 1)*Uv(n))); 0 0 0 0 0 (-p/(Uq(n+p+1)-Uq(n+1))) (p/(Uq(n+p+1)- Uq(n+1)))]; B2=[0 0 0.6895 0.5045 0.4259 0.3951 0.5683 0.7857 0 0]'; B3=[0 0 -1.5033 -1.8025 -1.9384 -2.0209 -2.0401 -2.0260 0 0]'; B4=[0 0 0.8137 1.2980 1.5125 1.6258 1.4717 1.2403 0 0]'; % Tinh cac diem dieu khien: CPQ1=(A\B2)'; CPQ2=(A\B3)'; CPQ3=(A\B4)'; CPV1(1) = -(p*(CPQ1(1) - CPQ1(2)))/(Uq(p + 2) - Uq(2)); CPV1(2) = -(p*(CPQ1(2) - CPQ1(3)))/(Uq(p + 3) - Uq(3)); CPV1(3) = -(p*(CPQ1(3) - CPQ1(4)))/(Uq(p + 4) - Uq(4)); CPV1(4) = -(p*(CPQ1(4) - CPQ1(5)))/(Uq(p + 5) - Uq(5)); CPV1(5) = -(p*(CPQ1(5) - CPQ1(6)))/(Uq(p + 6) - Uq(6)); CPV1(6) = -(p*(CPQ1(6) - CPQ1(7)))/(Uq(p + 7) - Uq(7)); 70 | P a g e CPV1(7) = -(p*(CPQ1(7) - CPQ1(8)))/(Uq(p + 8) - Uq(8)); CPV1(8) = -(p*(CPQ1(8) - CPQ1(9)))/(Uq(p + 9) - Uq(9)); CPV1(9) = -(p*(CPQ1(9) - CPQ1(10)))/(Uq(p + 10) - Uq(10)); CPV1(10) = -(p*(CPQ1(10) - CPQ1(11)))/(Uq(p + 11) - Uq(11)); CPV1(11) = -(p*(CPQ1(11) - CPQ1(12)))/(Uq(p + 12) - Uq(12)); CPV2(1) = -(p*(CPQ2(1) - CPQ2(2)))/(Uq(p + 2) - Uq(2)); CPV2(2) = -(p*(CPQ2(2) - CPQ2(3)))/(Uq(p + 3) - Uq(3)); CPV2(3) = -(p*(CPQ2(3) - CPQ2(4)))/(Uq(p + 4) - Uq(4)); CPV2(4) = -(p*(CPQ2(4) - CPQ2(5)))/(Uq(p + 5) - Uq(5)); CPV2(5) = -(p*(CPQ2(5) - CPQ2(6)))/(Uq(p + 6) - Uq(6)); CPV2(6) = -(p*(CPQ2(6) - CPQ2(7)))/(Uq(p + 7) - Uq(7)); CPV2(7) = -(p*(CPQ2(7) - CPQ2(8)))/(Uq(p + 8) - Uq(8)); CPV2(8) = -(p*(CPQ2(8) - CPQ2(9)))/(Uq(p + 9) - Uq(9)); CPV2(9) = -(p*(CPQ2(9) - CPQ2(10)))/(Uq(p + 10) - Uq(10)); CPV2(10) = -(p*(CPQ2(10) - CPQ2(11)))/(Uq(p + 11) - Uq(11)); CPV2(11) = -(p*(CPQ2(11) - CPQ2(12)))/(Uq(p + 12) - Uq(12)); CPV3(1) = -(p*(CPQ3(1) - CPQ3(2)))/(Uq(p + 2) - Uq(2)); CPV3(2) = -(p*(CPQ3(2) - CPQ3(3)))/(Uq(p + 3) - Uq(3)); CPV3(3) = -(p*(CPQ3(3) - CPQ3(4)))/(Uq(p + 4) - Uq(4)); CPV3(4) = -(p*(CPQ3(4) - CPQ3(5)))/(Uq(p + 5) - Uq(5)); CPV3(5) = -(p*(CPQ3(5) - CPQ3(6)))/(Uq(p + 6) - Uq(6)); CPV3(6) = -(p*(CPQ3(6) - CPQ3(7)))/(Uq(p + 7) - Uq(7)); CPV3(7) = -(p*(CPQ3(7) - CPQ3(8)))/(Uq(p + 8) - Uq(8)); CPV3(8) = -(p*(CPQ3(8) - CPQ3(9)))/(Uq(p + 9) - Uq(9)); CPV3(9) = -(p*(CPQ3(9) - CPQ3(10)))/(Uq(p + 10) - Uq(10)); CPV3(10) = -(p*(CPQ3(10) - CPQ3(11)))/(Uq(p + 11) - Uq(11)); CPV3(11) = -(p*(CPQ3(11) - CPQ3(12)))/(Uq(p + 12) - Uq(12)); CPA1(1) = -((CPV1(1) - CPV1(2))*(p - 1))/(Uv(p + 1) - Uv(2)); CPA1(2) = -((CPV1(2) - CPV1(3))*(p - 1))/(Uv(p + 2) - Uv(3)); CPA1(3) = -((CPV1(3) - CPV1(4))*(p - 1))/(Uv(p + 3) - Uv(4)); CPA1(4) = -((CPV1(4) - CPV1(5))*(p - 1))/(Uv(p + 4) - Uv(5)); CPA1(5) = -((CPV1(5) - CPV1(6))*(p - 1))/(Uv(p + 5) - Uv(6)); CPA1(6) = -((CPV1(6) - CPV1(7))*(p - 1))/(Uv(p + 6) - Uv(7)); CPA1(7) = -((CPV1(7) - CPV1(8))*(p - 1))/(Uv(p + 7) - Uv(8)); CPA1(8) = -((CPV1(8) - CPV1(9))*(p - 1))/(Uv(p + 8) - Uv(9)); 71 | P a g e CPA1(9) = -((CPV1(9) - CPV1(10))*(p - 1))/(Uv(p + 9) - Uv(10)); CPA1(10) = -((CPV1(10) - CPV1(11))*(p - 1))/(Uv(p + 10) - Uv(11)); CPA2(1) = -((CPV2(1) - CPV2(2))*(p - 1))/(Uv(p + 1) - Uv(2)); CPA2(2) = -((CPV2(2) - CPV2(3))*(p - 1))/(Uv(p + 2) - Uv(3)); CPA2(3) = -((CPV2(3) - CPV2(4))*(p - 1))/(Uv(p + 3) - Uv(4)); CPA2(4) = -((CPV2(4) - CPV2(5))*(p - 1))/(Uv(p + 4) - Uv(5)); CPA2(5) = -((CPV2(5) - CPV2(6))*(p - 1))/(Uv(p + 5) - Uv(6)); CPA2(6) = -((CPV2(6) - CPV2(7))*(p - 1))/(Uv(p + 6) - Uv(7)); CPA2(7) = -((CPV2(7) - CPV2(8))*(p - 1))/(Uv(p + 7) - Uv(8)); CPA2(8) = -((CPV2(8) - CPV2(9))*(p - 1))/(Uv(p + 8) - Uv(9)); CPA2(9) = -((CPV2(9) - CPV2(10))*(p - 1))/(Uv(p + 9) - Uv(10)); CPA2(10) = -((CPV2(10) - CPV2(11))*(p - 1))/(Uv(p + 10) - Uv(11)); CPA3(1) = -((CPV3(1) - CPV3(2))*(p - 1))/(Uv(p + 1) - Uv(2)); CPA3(2) = -((CPV3(2) - CPV3(3))*(p - 1))/(Uv(p + 2) - Uv(3)); CPA3(3) = -((CPV3(3) - CPV3(4))*(p - 1))/(Uv(p + 3) - Uv(4)); CPA3(4) = -((CPV3(4) - CPV3(5))*(p - 1))/(Uv(p + 4) - Uv(5)); CPA3(5) = -((CPV3(5) - CPV3(6))*(p - 1))/(Uv(p + 5) - Uv(6)); CPA3(6) = -((CPV3(6) - CPV3(7))*(p - 1))/(Uv(p + 6) - Uv(7)); CPA3(7) = -((CPV3(7) - CPV3(8))*(p - 1))/(Uv(p + 7) - Uv(8)); CPA3(8) = -((CPV3(8) - CPV3(9))*(p - 1))/(Uv(p + 8) - Uv(9)); CPA3(9) = -((CPV3(9) - CPV3(10))*(p - 1))/(Uv(p + 9) - Uv(10)); CPA3(10) = -((CPV3(10) - CPV3(11))*(p - 1))/(Uv(p + 10) - Uv(11)); CPJ1(1) = ((CPA1(1) - CPA1(2))*(p - 2))/(Ua(2) - Ua(p)); CPJ1(2) = -((CPA1(2) - CPA1(3))*(p - 2))/(Ua(p + 1) - Ua(3)); CPJ1(3) = -((CPA1(3) - CPA1(4))*(p - 2))/(Ua(p + 2) - Ua(4)); CPJ1(4) = -((CPA1(4) - CPA1(5))*(p - 2))/(Ua(p + 3) - Ua(5)); CPJ1(5) = -((CPA1(5) - CPA1(6))*(p - 2))/(Ua(p + 4) - Ua(6)); CPJ1(6) = -((CPA1(6) - CPA1(7))*(p - 2))/(Ua(p + 5) - Ua(7)); CPJ1(7) = -((CPA1(7) - CPA1(8))*(p - 2))/(Ua(p + 6) - Ua(8)); CPJ1(8) = -((CPA1(8) - CPA1(9))*(p - 2))/(Ua(p + 7) - Ua(9)); CPJ1(9) = -((CPA1(9) - CPA1(10))*(p - 2))/(Ua(p + 8) - Ua(10)); CPJ2(1) = ((CPA2(1) - CPA2(2))*(p - 2))/(Ua(2) - Ua(p)); CPJ2(2) = -((CPA2(2) - CPA2(3))*(p - 2))/(Ua(p + 1) - Ua(3)); 72 | P a g e CPJ2(3) = -((CPA2(3) - CPA2(4))*(p - 2))/(Ua(p + 2) - Ua(4)); CPJ2(4) = -((CPA2(4) - CPA2(5))*(p - 2))/(Ua(p + 3) - Ua(5)); CPJ2(5) = -((CPA2(5) - CPA2(6))*(p - 2))/(Ua(p + 4) - Ua(6)); CPJ2(6) = -((CPA2(6) - CPA2(7))*(p - 2))/(Ua(p + 5) - Ua(7)); CPJ2(7) = -((CPA2(7) - CPA2(8))*(p - 2))/(Ua(p + 6) - Ua(8)); CPJ2(8) = -((CPA2(8) - CPA2(9))*(p - 2))/(Ua(p + 7) - Ua(9)); CPJ2(9) = -((CPA2(9) - CPA2(10))*(p - 2))/(Ua(p + 8) - Ua(10)); CPJ3(1) = ((CPA3(1) - CPA3(2))*(p - 2))/(Ua(2) - Ua(p)); CPJ3(2) = -((CPA3(2) - CPA3(3))*(p - 2))/(Ua(p + 1) - Ua(3)); CPJ3(3) = -((CPA3(3) - CPA3(4))*(p - 2))/(Ua(p + 2) - Ua(4)); CPJ3(4) = -((CPA3(4) - CPA3(5))*(p - 2))/(Ua(p + 3) - Ua(5)); CPJ3(5) = -((CPA3(5) - CPA3(6))*(p - 2))/(Ua(p + 4) - Ua(6)); CPJ3(6) = -((CPA3(6) - CPA3(7))*(p - 2))/(Ua(p + 5) - Ua(7)); CPJ3(7) = -((CPA3(7) - CPA3(8))*(p - 2))/(Ua(p + 6) - Ua(8)); CPJ3(8) = -((CPA3(8) - CPA3(9))*(p - 2))/(Ua(p + 7) - Ua(9)); CPJ3(9) = -((CPA3(9) - CPA3(10))*(p - 2))/(Ua(p + 8) - Ua(10)); t=[t4 t5 t6 t7 t8 t9 t10 t11 t12 t13 t14 t15]; %Ham muc tieu: %j=1 %[t6,t7) F1=@(x) (CPJ1(1).*((x - t(4)).^2/((t(2) - t(4)).*(t(3) - t(4))))+ CPJ1(2).*(- ((x - t(2)).*(x - t(4)))/((t(2) - t(4)).*(t(3) - t(4))) - ((x - t(3)).*(x - t(5)))/((t(3) - t(4)).*(t(3) - t(5))))+CPJ1(3).*((x - t(3)).^2/((t(3) - t(4)).*(t(3) - t(5))))).^2; %[t7,t8] F2=@(x) (CPJ1(2).*((x t(5))))+CPJ1(3).*(t(5))) - ((x - ((x - t(5)).^2/((t(3) t(3)).*(x - t(4)).*(x - - t(5)).*(t(4) t(5)))/((t(3) t(6)))/((t(4) - - - t(5)).*(t(4) - t(5)).*(t(4) - t(6))))+CPJ1(4).*((x - t(4)).^2/((t(4) - t(5)).*(t(4) - t(6))))).^2; %[t8,t9] F3=@(x) (CPJ1(3).*((x t(6))))+CPJ1(4).*(t(6))) - ((x - ((x - t(6)).^2/((t(4) t(4)).*(x - t(5)).*(x - - t(6)).*(t(5) t(6)))/((t(4) t(7)))/((t(5) - - - t(6)).*(t(5) - t(6)).*(t(5) - t(7))))+CPJ1(5).*((x - t(5)).^2/((t(5) - t(6)).*(t(5) - t(7))))).^2; %[t9,t10] 73 | P a g e F4=@(x) (CPJ1(4).*((x t(7))))+CPJ1(5).*(t(7))) - ((x - ((x - t(7)).^2/((t(5) t(5)).*(x - t(6)).*(x - - t(7)).*(t(6) t(7)))/((t(5) t(8)))/((t(6) - - - t(7)).*(t(6) - t(7)).*(t(6) - t(8))))+CPJ1(6).*((x - t(6)).^2/((t(6) - t(7)).*(t(6) - t(8))))).^2; %[t10,t11] F5=@(x) (CPJ1(5).*((x t(8))))+CPJ1(6).*(t(8))) - ((x - ((x - t(8)).^2/((t(6) t(6)).*(x - t(7)).*(x - - t(8)).*(t(7) t(8)))/((t(6) t(9)))/((t(7) - - - t(8)).*(t(7) - t(8)).*(t(7) - t(9))))+CPJ1(7).*((x - t(7)).^2/((t(7) - t(8)).*(t(7) - t(9))))).^2; %[t11,t12] F6=@(x) (CPJ1(6).*((x t(9))))+CPJ1(7).*(t(9))) - ((x - ((x - t(9)).^2/((t(7) t(7)).*(x - t(8)).*(x - - t(9)).*(t(8) t(9)))/((t(7) t(10)))/((t(8) - - - t(9)).*(t(8) - t(9)).*(t(8) - t(10))))+CPJ1(8).*((x - t(8)).^2/((t(8) - t(9)).*(t(8) - t(10))))).^2; %[t12,t13] F7=@(x) (CPJ1(7).*((x - t(10)).^2/((t(8) - t(10)).*(t(9) - t(10))))+CPJ1(8).*(- ((x - t(8)).*(x - t(10)))/((t(8) - t(10)).*(t(9) t(10))) - ((x - t(9)).*(x - t(11)))/((t(9) - t(10)).*(t(9) - t(11))))+CPJ1(9).*((x - t(9)).^2/((t(9) - t(10)).*(t(9) - t(11))))).^2; %j=2 %[] G1=@(x) (CPJ2(1).*((x - t(4)).^2/((t(2) - t(4)).*(t(3) - t(4))))+ CPJ2(2).*(- ((x - t(2)).*(x - t(4)))/((t(2) - t(4)).*(t(3) - t(4))) - ((x - t(3)).*(x - t(5)))/((t(3) - t(4)).*(t(3) - t(5))))+CPJ2(3).*((x - t(3)).^2/((t(3) - t(4)).*(t(3) - t(5))))).^2; %[t7,t8] G2=@(x) (CPJ2(2).*((x t(5))))+CPJ2(3).*(t(5))) - ((x - ((x - t(5)).^2/((t(3) t(3)).*(x - t(4)).*(x - - t(5)).*(t(4) t(5)))/((t(3) t(6)))/((t(4) - - - t(5)).*(t(4) - t(5)).*(t(4) - t(6))))+CPJ2(4).*((x - t(4)).^2/((t(4) - t(5)).*(t(4) - t(6))))).^2; %[t8,t9] G3=@(x) (CPJ2(3).*((x t(6))))+CPJ2(4).*(t(6))) - ((x - ((x - t(6)).^2/((t(4) t(4)).*(x - t(5)).*(x - - t(6)).*(t(5) t(6)))/((t(4) t(7)))/((t(5) - - - t(6)).*(t(5) - t(6)).*(t(5) - t(7))))+CPJ2(5).*((x - t(5)).^2/((t(5) - t(6)).*(t(5) - t(7))))).^2; %[t9,t10] 74 | P a g e G4=@(x) (CPJ2(4).*((x t(7))))+CPJ2(5).*(t(7))) - ((x - ((x - t(7)).^2/((t(5) t(5)).*(x - t(6)).*(x - - t(7)).*(t(6) t(7)))/((t(5) t(8)))/((t(6) - - - t(7)).*(t(6) - t(7)).*(t(6) - t(8))))+CPJ2(6).*((x - t(6)).^2/((t(6) - t(7)).*(t(6) - t(8))))).^2; %[t10,t11] G5=@(x) (CPJ2(5).*((x t(8))))+CPJ2(6).*(t(8))) - ((x - ((x - t(8)).^2/((t(6) t(6)).*(x - t(7)).*(x - - t(8)).*(t(7) t(8)))/((t(6) t(9)))/((t(7) - - - t(8)).*(t(7) - t(8)).*(t(7) - t(9))))+CPJ2(7).*((x - t(7)).^2/((t(7) - t(8)).*(t(7) - t(9))))).^2; %[t11,t12] G6=@(x) (CPJ2(6).*((x t(9))))+CPJ2(7).*(t(9))) - ((x - ((x - t(9)).^2/((t(7) t(7)).*(x - t(8)).*(x - - t(9)).*(t(8) t(9)))/((t(7) t(10)))/((t(8) - - - t(9)).*(t(8) - t(9)).*(t(8) - t(10))))+CPJ2(8).*((x - t(8)).^2/((t(8) - t(9)).*(t(8) - t(10))))).^2; %[t12,t13] G7=@(x) (CPJ2(7).*((x - t(10)).^2/((t(8) - t(10)).*(t(9) - t(10))))+CPJ2(8).*(- ((x - t(8)).*(x - t(10)))/((t(8) - t(10)).*(t(9) t(10))) - ((x - t(9)).*(x - t(11)))/((t(9) - t(10)).*(t(9) - t(11))))+CPJ2(9).*((x - t(9)).^2/((t(9) - t(10)).*(t(9) - t(11))))).^2; %j=3 %[t6,t7) L1=@(x) (CPJ3(1).*((x - t(4)).^2/((t(2) - t(4)).*(t(3) - t(4))))+ CPJ3(2).*(- ((x - t(2)).*(x - t(4)))/((t(2) - t(4)).*(t(3) - t(4))) - ((x - t(3)).*(x - t(5)))/((t(3) - t(4)).*(t(3) - t(5))))+CPJ3(3).*((x - t(3)).^2/((t(3) - t(4)).*(t(3) - t(5))))).^2; %[t7,t8] L2=@(x) (CPJ3(2).*((x t(5))))+CPJ3(3).*(t(5))) - ((x - ((x - t(5)).^2/((t(3) t(3)).*(x - t(4)).*(x - - t(5)).*(t(4) t(5)))/((t(3) t(6)))/((t(4) - - - t(5)).*(t(4) - t(5)).*(t(4) - t(6))))+CPJ3(4).*((x - t(4)).^2/((t(4) - t(5)).*(t(4) - t(6))))).^2; %[t8,t9] L3=@(x) (CPJ3(3).*((x t(6))))+CPJ3(4).*(t(6))) - ((x - ((x - t(6)).^2/((t(4) t(4)).*(x - t(5)).*(x - - t(6)).*(t(5) t(6)))/((t(4) t(7)))/((t(5) - - - t(6)).*(t(5) - t(6)).*(t(5) - t(7))))+CPJ3(5).*((x - t(5)).^2/((t(5) - t(6)).*(t(5) - t(7))))).^2; %[t9,t10] 75 | P a g e L4=@(x) (CPJ3(4).*((x t(7))))+CPJ3(5).*(t(7))) - ((x ((x - - t(7)).^2/((t(5) t(5)).*(x - t(6)).*(x - - t(7)).*(t(6) t(7)))/((t(5) t(8)))/((t(6) - - - t(7)).*(t(6) - t(7)).*(t(6) - t(8))))+CPJ3(6).*((x - t(6)).^2/((t(6) - t(7)).*(t(6) - t(8))))).^2; %[t10,t11] L5=@(x) (CPJ3(5).*((x t(8))))+CPJ3(6).*(t(8))) - ((x ((x - - t(8)).^2/((t(6) t(6)).*(x - t(7)).*(x - - t(8)).*(t(7) t(8)))/((t(6) t(9)))/((t(7) - - - t(8)).*(t(7) - t(8)).*(t(7) - t(9))))+CPJ3(7).*((x - t(7)).^2/((t(7) - t(8)).*(t(7) - t(9))))).^2; %[t11,t12] L6=@(x) (CPJ3(6).*((x t(9))))+CPJ3(7).*(t(9))) - ((x ((x - - t(9)).^2/((t(7) t(7)).*(x - t(8)).*(x - - t(9)).*(t(8) t(9)))/((t(7) t(10)))/((t(8) - - - t(9)).*(t(8) - t(9)).*(t(8) - t(10))))+CPJ3(8).*((x - t(8)).^2/((t(8) - t(9)).*(t(8) - t(10))))).^2; %[t12,t13] L7=@(x) (CPJ3(7).*((x - t(10)).^2/((t(8) - t(10)).*(t(9) - t(10))))+CPJ3(8).*(- ((x - t(8)).*(x - t(10)))/((t(8) - t(10)).*(t(9) t(10))) - ((x - t(9)).*(x - t(11)))/((t(9) - t(10)).*(t(9) - t(11))))+CPJ3(9).*((x - t(9)).^2/((t(9) - t(10)).*(t(9) - t(11))))).^2; FOBJ =kT.*(h(1)+h(2)+h(3)+h(4)+h(5)+h(6)+h(7))+kJ.*(integral(F1,t(3),t(4)) + integral(F2,t(4),t(5)) + integral(F3,t(5),t(6)) + integral(F4,t(6),t(7)) + integral(F5,t(7),t(8)) + integral(F6,t(8),(t(9))) + integral(F7,t(9),t(10)) + integral(G1,t(3),t(4)) + integral(G2,t(4),t(5)) + integral(G3,t(5),t(6)) integral(G5,t(7),t(8)) + + integral(G7,t(9),t(10)) integral(G4,t(6),t(7)) + integral(G6,t(8),(t(9))) + + integral(L1,t(3),t(4)) + integral(L2,t(4),t(5)) + integral(L3,t(5),t(6)) + integral(L4,t(6),t(7)) + integral(L5,t(7),t(8)) + integral(L6,t(8),(t(9))) + integral(L7,t(9),t(10))); Chương trình tìm nghiệm tối ưu hi nghiemtoiuu.m objective=@(h)FOBJ(h); %h0=[0.0713 0.2062 0.1229 0.0649 0.0992 0.0784 0.0542] 76 | P a g e h0= [0.0713 1.7907 0.4811 0.6106 0.8048 1.4161 0.0542]; A=[]; b=[]; Aeq=[]; beq=[]; lb=[]; ub=[]; nonlcon=@dieukien fmincon(objective,h0,A,b,Aeq,beq,lb,ub,nonlcon) 77 | P a g e ... pháp thiết kế quỹ đạo cho cánh tay máy 1.2.1 Phƣơng pháp nội suy Như trình bày mục 1, hình dáng quỹ đạo, ràng buộc động học, động lực học đầu vào thuật toán thiết kế quỹ đạo mẫu Hình dáng quỹ đạo. .. việc thiết kế quỹ đạo thiết kế không gian khớp Quỹ đạo khớp đạt thông qua việc nội suy hàm số mà đáp ứng ràng buộc động học động lực học Để hệ thống tay máy hoạt động ổn định, việc thiết kế quỹ đạo. .. đường cho trước yêu cầu định tính chất quỹ đạo phải đáp ứng Kết việc thiết kế quỹ đạo tạo tín hiệu đặt cho hệ thống tay máy, để hệ thống thực thi theo quỹ đạo định Hình dáng quỹ đạo, ràng buộc động