Luận án tiến sĩ kỹ thuật nghiên cứu hệ thống chuyển động và điều khiển tay máy tự động thu hoạch dứa

i CỘNG HÒA Xà HỘI CHỦ NGHĨA VIỆT NAM Độc lập –Tự – Hạnh phúc LỜI CAM ĐOAN Tôi cam đoan cơng trình nghiên cứu khoa học riêng tôi, hướng dẫn khoa học PGS.TS Nguyễn Phạm Thục Anh TS Hoàng Sơn Các số liệu, kết nghiên cứu trình bày luận án trung thực chưa cơng bố cơng trình nghiên cứu khoa học khác Hà Nội, ngày tháng 10 năm 2022 Tác giả luận án Bùi Lê Cường Quốc ii LỜI CẢM ƠN Lời xin trân trọng cảm ơn Ban lãnh đạo, tập thể cán phòng Đào tạo sau Đại học; quý thầy cô thuộc Khoa Cơ điện Cơng trình Trường Đại học Lâm nghiệp; cảm ơn Công ty cổ phần thiết bị chuyên dùng Việt Nam - Hà Nội Ban giám hiệu, lãnh đạo phòng ban, Khoa Kỹ thuật Trường Cao đẳng nghề Bình Thuận tạo điều kiện cho phép tham gia học tập giúp đỡ suốt q trình nghiên cứu hồn thành luận án tiến sĩ Tơi xin bày tỏ lịng cảm ơn sâu sắc tới PGS.TS Nguyễn Phạm Thục Anh TS Hoàng Sơn định hướng nghiên cứu, tận tình bảo với tận tâm, trách nhiệm cao giúp đỡ tơi suốt q trình nghiên cứu hồn thiện luận án Trân trọng cảm ơn nhà khoa học thuộc câu lạc Cơ khí động lực, nhà khoa học chuyên ngành khí động lực, khí Trường Đại học Lâm nghiệp; Học viện Nông nghiệp Việt Nam; Học viện Kỹ thuật quân sự; Trường Đại học Bách khoa Hà Nội; Trường Đại học Giao thông vận tải; Trường Đại học Công nghiệp Hà Nội,… giúp đỡ tơi hồn thành luận án Trân trọng cảm ơn bạn bè đồng nghiệp, đặc biệt thành viên gia đình tạo điều kiện giúp đỡ, ủng hộ động viên suốt thời gian học tập nghiên cứu Tác giả luận án Bùi Lê Cường Quốc iii MỤC LỤC LỜI CAM ĐOAN I LỜI CẢM ƠN II MỤC LỤC III DANH MỤC CÁC TỪ KÝ HIỆU VÀ TỪ VIẾT TẮT VII DANH MỤC CÁC BẢNG BIỂU xii DANH MỤC CÁC HÌNH………………………………………………… xiii MỞ ĐẦU Chương TỔNG QUAN VỀ VẤN ĐỀ NGHIÊN CỨU 1.1 Tổng quan sản xuất dứa Việt Nam 1.1.1 Đặc điểm điều kiện tự nhiên khu vực trồng dứa vùng Tây Nam Bộ .4 1.1.2 Đặc điểm mẫu vườn dứa đến thời điểm thu hoạch 1.1.3 Công nghệ thiết bị thu hoạch dứa Vùng Tây Nam Bộ 1.1.4 Những tồn thu hoạch dứa vùng Tây Nam Bộ Việt Nam 10 1.2 Công nghệ thiết bị thu hoạch dứa số nước giới 11 1.3 Tổng quan cơng trình nghiên cứu thiết bị tự động hóa thu hoạch nơng sản giới 13 1.3.1 Các cơng trình nghiên cứu robot lĩnh vực nông nghiệp 13 1.3.2 Tổng quan cơng trình nghiên cứu thiết bị thu hoạch dứa 15 1.4 Tổng quan cơng trình nghiên cứu thiết bị thu hoạch dứa Việt Nam 18 1.5 Mục tiêu nghiên cứu 19 1.6 Nội dung nghiên cứu luận án 20 1.6.1 Nghiên cứu lý thuyết 20 1.6.2 Nghiên cứu thực nghiệm 20 1.7 Đối tượng nghiên cứu luận án 20 iv 1.7.1 Thiết bị nghiên cứu 20 1.7.2 Đối tượng tay máy thu hoạch dứa 21 1.8 Phương pháp nghiên cứu luận án 23 1.8.1 Phương pháp nguyên cứu lý thuyết 23 1.8.2 Phương pháp nghiên cứu thực nghiệm 23 Kết luận chương 23 Chương ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CHUYỂN ĐỘNG TAY MÁY THU HOẠCH DỨA 25 2.1 Cấu tạo nguyên lý hoạt động tay máy thu hoạch dứa 25 2.1.1 Cấu tạo tay máy thu hoạch dứa 26 2.1.2 Nguyên lý hoạt động 30 2.2 Xây dựng mơ hình động học tay máy thu hoạch dứa 31 2.2.1 Mối quan hệ hệ tọa độ 31 2.2.2 Phương trình động học tay máy 33 2.3 Xây dựng mơ hình động lực học chuyển động tay máy hái dứa 35 2.3.1 Các giả thiết qui đổi mơ hình động lực học 35 2.3.2 Phương trình động lực học lý tưởng 36 2.3.3 Phương trình động lực học quy đầu trục động khớp trường hợp lý tưởng 39 2.4 Xây dựng mơ hình động lực học tay máy hái dứa xét tới tác động nhiễu từ môi trường làm việc 42 2.4.1 Phương trình động lực học tay máy hái dứa xét tới tác động nhiễu từ môi trường làm việc 44 2.4.2 Phương trình động lực học quy đổi đầu trục động khớp xét tới tác động nhiễu từ môi trường làm việc 49 2.5 Phân tích đặc điểm mơ hình động lực học tay máy hái dứa 51 2.5.1 Khảo sát mối liên hệ mơ hình động lực học lý tưởng mơ hình động lực học tay máy xét tới tác động môi trường làm việc 51 v 2.5.2 Phân tích thành phần sai lệch mơ hình lý thuyết mơ hình thực tế tay máy thu hoạch dứa 52 2.6 Các thuật toán điều khiển đề xuất mơi trường làm việc lý tưởng 53 2.6.1 Thuật tốn phi tuyến tựa mơ hình 54 2.6.2 Phương pháp momen tính tốn (PD-bù trọng trường) 58 2.6.3 Phương pháp PID 59 Kết luận chương 61 Chương ĐIỀU KHIỂN CHUYỂN ĐỘNG TAY MÁY THU HOẠCH DỨA TỰ ĐỘNG 63 3.1 Nhận diện dứa cần thu hoạch 64 3.1.1 Thuật toán YOLO 64 3.1.2 Nghiên cứu nhận diện dứa 66 3.2.2 Huấn luyện nhận diện phân biệt dứa xanh chín (huấn luyện lần 2) 70 3.2.3 Huấn luyện nhận diện phân biệt dứa xanh chín dựa vào đặc điểm thân trái đài trái (huấn luyện lần 3) 71 3.2.4 Kết huấn luyện 71 3.4 Điều khiển chuyển động cánh tay máy hái dứa 80 3.4.1 Phương trình động lực học rút gọn tương ứng với hệ thống điều khiển chuyển động 80 3.4.2 Điều khiển chuyển động cánh tay máy dựa thuật toán PD kinh điển (Proportional Derivative - PD) 84 3.4.3 Điều khiển chuyển động cánh tay với thuật toán tỷ lệ đạo hàm kết hợp với ước tính nhiễu mơi trường (PD+DE; Proportional Derivative +Disturbance Estimation) 87 3.4.4 Mô phân tích kết điều khiển 90 Kết luận chương 3: 98 Chương NGHIÊN CỨU THỰC NGHIỆM 100 vi 4.1 Mục tiêu nhiệm vụ cùa nghiên cứu thực nghiệm 100 4.1.1 Mục tiêu nghiên cứu thực nghiệm 100 4.1.2 Nhiệm vụ nghiên cứu thực nghiệm 101 4.1.3 Đối tượng nghiên cứu thực nghiệm 101 4.2 Cơ sở xác định tính xác mơ hình lý thuyết qua số liệu thực nghiệm 102 4.2.1 Cơ sở lý thuyết .102 4.2.2 Kiểm tra tính đồng phương sai .103 4.2.3 Kiểm tra tính tương thích số liệu thực nghiệm công thức lý thuyết 103 4.3 Xây dựng hệ thống thực nghiệm 104 4.3.1 Cấu trúc phần cứng hệ thống 104 4.3.2 Phần mềm mã nguồn .106 4.3.3 Mơ hình thực nghiệm tồn hệ thống 106 4.3.4 Dụng cụ, thiết bị đo hỗ trợ trình thực nghiệm 108 4.4 Tổ chức thực nghiệm .111 4.4.1 Tổ chức thực nghiệm kiểm chứng kết đọc tọa độ camera 111 4.4.2 Tổ chức thực nghiệm kiểm chứng chuyển động tay máy hái dứa 112 4.5 Kết thực nghiệm .114 4.5.1 Kết thực nghiệm xác định tọa độ dứa camera 114 4.5.2 Kết thực nghiệm chuyển động tay máy thu hoạch dứa 117 Kết luận chương 128 KẾT LUẬN VÀ KIẾN NGHỊ 129 TUYỂN TẬP CÁC BÀI BÁO Đà CÔNG BỐ VỀ KẾT QUẢ CỦA LUẬN ÁN 130 TÀI LIỆU THAM KHẢO 131 PHỤ LỤC vii DANH MỤC CÁC TỪ KÝ HIỆU VÀ TỪ VIẾT TẮT TT Ký hiệu Đơn vị Ý nghĩa (1) (2) (3) (4) LHM - Liên hợp máy CNH - Công nghiệp hóa HĐH - Hiện đại hóa Oc X cYc Zc - Hệ tọa độ gắn với camera O0t1 X 0t1Y0t Z 0t - Hệ tọa độ gốc gán cho tay máy số O0t2 X 0t2 Y0 t Z t - Hệ tọa độ gốc gán cho tay máy số rx - Lượng dịch chuyển khớp tay máy ry - Lượng dịch chuyển khớp tay máy rz - Lượng dịch chuyển khớp tay máy 10 xc , yc , zc - Tọa độ dứa so với camera 11 L N Hàm Lagrange 12 K kgm/s Tổng động 13 P J 14 M dx Nm 15 Kx Nm 16 M dy Nm 17 ky Nm 18 g m/s2 2 Tổng Mômen xoắn trục động thứ Hệ số quy đổi mômen động khớp thứ Mômen xoắn trục động thứ hai Hệ số quy đổi mômen động khớp thứ hai Gia tốc trọng trường viii Đơn vị Ý nghĩa TT Ký hiệu 19 w d (t ) 20 m1 kg Khối lượng khớp 21 m2 kg Khối lượng khớp 22 m3 kg Khối lượng khớp 23 F N Lực tác động lên khớp thực tế 24 F0 N 25 Fxo N Lực tác động lên trục X 26 FYo N Lực tác động lên trục Y 27 FZo N Lực tác động lên trục Z 28 H0 - 29 H - 30 G - 31 G0 - 32 P1 J Thế tác động lên khớp 33 P2 J Thế tác động lên khớp 34 P3 J Thế tác động lên khớp 35 K1 kgm/s Động khớp 36 K2 kgm/s Động khớp 37 K3 kgm/s Động khớp 38 rx m/s Vận tốc dịch chuyển trục X 39 ry m/s Vận tốc dịch chuyển trục Y Sai lệch mơ hình lý tưởng mơ hình thực tế Lực tác động lên khớp giả thuyết lý tưởng Ma trận đặc trưng cho quán tính khớp tay máy trường hợp lý tưởng Ma trận đặc trưng cho quán tính khớp tay máy trường hợp thực tế Ma trận biểu thị thành phần trọng lực Ma trận biểu thị thành phần trọng lực lý tưởng ix TT Ký hiệu Đơn vị Ý nghĩa 40 rz m/s Vận tốc dịch chuyển trục Z 41 rx m/s2 Gia tốc dịch chuyển trục X 42 ry m/s2 Gia tốc dịch chuyển trục Y 43 rz m/s2 Gia tốc dịch chuyển trục Z 44  gtx % 45 k gtx - 46  rx % 47 Rx m 48  gty % 49 k gty - Tỷ số truyền hộp giảm tốc Y 50 dy % Hiệu suất truyền động puly-đai 51 Ry m Bán kính puly 52 Dd(t) - 53 Gd - 54 V - Hàm lượng Lyapunov 55  - Sai số vị trí cánh tay 56 b - 57 Md N/m Hiệu suất truyền động hộp giảm tốc trục X Tỷ số truyền hộp giảm tốc X Hiệu suất truyền động răng-bánh Bán kính bánh Hiệu suất truyền động hộp giảm tốc trục Y Ma trận đặc trưng cho rung lắc ma sát (nhiễu môi trường) Ma trận biểu thị thành phần trọng lực quy đầu trục động Đại diện cho giới hạn nhiễu môi trường Mômen cần phát từ hai trục động khớp tay máy x TT Ký hiệu Đơn vị Ý nghĩa 58 Hd0 - 59 qd - 60 Mdx0 N/m 61 Mdy0 N/m 62 b - 63 q0 - 64 qd - 65 Mdx N/m 66 Mdy N/m 67 fW - Thành phần bù nhiễu môi trường 68 PD - Proportional Derivative 69 PDDE - 70  Độ Tọa độ gốc so với trục X0 71  Độ Tọa độ gốc so với trục Y0 72  Độ Tọa độ gốc so với trục Z0 73  Độ Phạm vi nhìn dọc máy ảnh (nhìn dài) 74  Độ Ma trận đặc trưng cho quán tính khớp khớp Tọa độ dứa camera cung cấp Mômen động cần sinh trục X trường hợp lý tưởng Mômen động cần sinh trục Y trường hợp lý tưởng Sai lệch ước tính giới hạn nhiễu mơi trường Vị trí ban đầu tay máy Vị trí tọa độ dứa mà tay máy cần di chuyển Mômen động cần sinh trục X môi trường làm việc Mômen động cần sinh trục Y môi trường làm việc Proportional Derivative Disturbance Estimation Phạm vi nhìn ngang máy ảnh (nhìn rộng) PHỤ LỤC PHỤ LỤC Hàm S-function khối Join_input1 function [sys,x0,str,ts] = input(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 3, sys=mdlOutputs(t,x,u); case {2,4,9} sys=[]; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates=0; sizes.NumDiscStates=0; sizes.NumOutputs =3; sizes.NumInputs = 0; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys = simsizes(sizes); x0 = []; str = []; ts = []; function sys=mdlOutputs(t,x,u) %a=u(1); qf=1; %qf=(150*pi)/180; tf=4; %Thoi gian di chuyen la 4s q0=0; qdd=6*(qf-q0)/tf^2; tb=tf/2-sqrt(qdd^2*tf^2-(4*qdd*(qf-q0)))/(2*qdd); qb=q0+0.5*qdd*tb^2; if t>=0 && t=tb && t=tf-tb && ttf q1_d=qf; dq1_d=0; ddq1_d=0; end sys(1)=q1_d; sys(2)=dq1_d; sys(3)=ddq1_d; Hàm S-function khối Join_input2 function [sys,x0,str,ts] = input(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 3, sys=mdlOutputs(t,x,u); case {2,4,9} sys=[]; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates=0; sizes.NumDiscStates=0; sizes.NumOutputs =3; sizes.NumInputs = 0; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys = simsizes(sizes); x0 = []; str = []; ts = []; function sys=mdlOutputs(t,x,u) %a=u(1); qf=1.5; %(70*pi)/180; tf=4; %thoi gian di chuyen 4s q0=0; qdd=6*(qf-q0)/tf^2; tb=tf/2-sqrt(qdd^2*tf^2-(4*qdd*(qf-q0)))/(2*qdd); qb=q0+0.5*qdd*tb^2; if t>=0 && t=tb && t=tf-tb && ttf q2_d=qf; dq2_d=0; ddq2_d=0; end sys(1)=q2_d; sys(2)=dq2_d; sys(3)=ddq2_d; Hàm S-function khối Join_input2 function [sys,x0,str,ts] = spacemodel(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); case 3, sys=mdlOutputs(t,x,u); case {2,4,9} sys=[]; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; %sizes.NumContStates = 2; sizes.NumOutputs = 2; %4 sizes.NumInputs= -1; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1;% Thay doi so voi ban goc la sys = simsizes(sizes); x0 = []; % bien trang thai bao dau la ( thay doi so voi ban goc, vi goc ko dung bien trang thai) str = []; ts = [0 0]; % % -THEM DE TINH UOC LUONG CUA GIOI HAN CHAN d^ % Hàm S-function khối PDcontroller function sys=mdlOutputs(t,x,u) q1_d=u(1); %quy dao dat khop //(t,x,u) dq1_d=u(2);%van toc dat khop %ddq1_d=u(3);%gia toc dat khop q2_d= u(4); % quy dao dat khop dq2_d=u(5);%van toc dat khop %ddq2_d=u(6);% gia toc dat khop q1=u(7);%u(7); % quy dao th?t c?a kh?p dq1=u(8); %u(8); % V?n t?c th?t c?a kh?p q2=u(9);%u(9); %quy ??o th?t c?a kh?p dq2=u(10); %u(10); % V?n t?c th?t c?a kh?p %q=[q1;q2]; % Ma tr?n qu? ??o th?t kh?p %dq=[dq1;dq2];% Ma tran van toc that khop % Dinh nghia ma tra sai leche1=q1_d-q1;% sai lech quy dao khop (chinh la epsilon 1) e2=q2_d-q2;% chinh la epsilon (do matlab khong co ky hieu epsilon, dung bien e thay the) e=[e1;e2]; de1=dq1_d-dq1; %sai lech van toc khop (dao ham bac nhat cua epsilon1) de2=dq2_d-dq2; % sai lech van toc khop (dao ham bac nhat cua epsilon2) de=[de1;de2]; %ma tran sai lech quy dao (ma tra epsilon) %ma tran sai lech van toc % -% - LUAT DIEU KHIEN PD -Kp=[30 0; 30]; Kd=[30 0;0 30]; tol= Kp*e+Kd*de; sys(1)=tol(1); sys(2)=tol(2); Hàm S-funsion khối PDDEcontroller function [sys,x0,str,ts] = spacemodel(t,x,u,flag) switch flag, case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); case 3, sys=mdlOutputs(t,x,u); case {2,4,9} sys=[]; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 2; sizes.NumOutputs = 4; sizes.NumInputs= -1; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 0;% Thay doi so voi ban goc la sys = simsizes(sizes); x0 = [0;1]; % bien trang thai bao dau la ( thay doi so voi ban goc, vi goc ko dung bien trang thai) str = []; ts = []; % % -THEM DE TINH UOC LUONG CUA GIOI HAN CHAN d^ -% % -THEM DE TINH UOC LUONG CUA GIOI HAN CHAN d^ -function sys=mdlDerivatives(t,x,u) q1_d=u(1); dq1_d=u(2);%-pi*sin(pi*t); ddq1_d=u(3);%-pi^2*cos(pi*t); q2_d=u(4); dq2_d=u(5);%pi*cos(pi*t); ddq2_d=u(6);%-pi^2*sin(pi*t); q1=u(7); dq1=u(8); q2=u(9); dq2=u(10); q=[q1;q2]; dq=[dq1;dq2]; dq_d=[dq1_d,dq2_d]'; ddq_d=[ddq1_d,ddq2_d]'; e=[q1-q1_d,q2-q2_d]'; de=[dq1-dq1_d,dq2-dq2_d]'; teta=max([1,norm(q),(norm(dq))^2]);%max([1,norm(q)]); % tinh teta (2.80) %gama=5*eye(2); %eye(2)=[1 0;0 1] ma tran don vi bac %s=gama*e+de;% TInh mat truot»¬Ä£Ãỉ gama1=0.1;gama2=0.1; sys(1)=gama1*teta*norm(dq); sys(2)=-gama2*x(2); function sys=mdlOutputs(t,x,u) q1_d=u(1); %quy dao dat khop //(t,x,u) dq1_d=u(2);%van toc dat khop %ddq1_d=u(3);%gia toc dat khop q2_d= u(4); % quy dao dat khop dq2_d=u(5);%van toc dat khop %ddq2_d=u(6);% gia toc dat khop q1=u(7);%u(7); % quy dao th?t c?a kh?p dq1=u(8); %u(8); % V?n t?c th?t c?a kh?p q2=u(9);%u(9); %quy ??o th?t c?a kh?p dq2=u(10); %u(10); % V?n t?c th?t c?a kh?p q=[q1;q2]; % Ma tr?n qu? ??o th?t kh?p dq=[dq1;dq2];% Ma tran van toc that khop % Dinh nghia ma tra sai leche1=q1_d-q1;% sai lech quy dao khop (chinh la epsilon 1) e2=q2_d-q2;% chinh la epsilon (do matlab khong co ky hieu epsilon, dung bien e thay the) e=[e1;e2]; de1=dq1_d-dq1; %sai lech van toc khop (dao ham bac nhat cua epsilon1) de2=dq2_d-dq2; % sai lech van toc khop (dao ham bac nhat cua epsilon2) de=[de1;de2]; %ma tran sai lech quy dao (ma tra epsilon) %ma tran sai lech van toc % TINH THANH PHAN BI NHIEU fw teta=max([1,norm(q),(norm(dq))^2]); %max([1,norm(q),(norm(dq))^2); fw=-(x(1)*teta)^2*dq/(x(1)*teta*norm(dq)+x(2)^2); % - LUAT DIEU KHIEN%PD -Kp=[30 0; 30]; Kd=[30 0;0 30]; tol= Kp*e+Kd*de+fw; sys(1)=tol(1); %Momen dau truc dong co truc X sys(2)=tol(2); %Momen dau truc dong co truc Y %sys(3)=fw(1); %Tin hieu momen bu nhieu truc X %sys(4)=fw(2); %Tin hieu momen bu nhieu truc Y sys(3)=x(1); sys(4)=x(2); Hàm S-function khối Plant (động lực học cánh tay máy) %S-function for continuous state equation function [sys,x0,str,ts]=s_function(t,x,u,flag) switch flag, %Initialization case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); %Outputs case 3, sys=mdlOutputs(t,x,u); %Unhandled flags case {2, 4, } sys = []; %Unexpected flags otherwise error(['Unhandled flag = ',num2str(flag)]); end %mdlInitializeSizes function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs= 4;%6 sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys=simsizes(sizes); x0=[0.0;0;0.0;0];%da sua str=[]; ts=[]; function sys=mdlDerivatives(t,x,u) Md(1)=u(1); %tol dai dien cho momen truc dong co Md Md(2)=u(2); g=9.8; m1=38; m2=4.5; m3=9.5; kx=392.2; ky=400.65; a=-pi/20;%Dai dien cho goc anfa b=-pi/20;%Dai dien cho goc beta c=-pi/20;%Dai dien cho goc gama Hd=[(m1*(cos(a))^2+m2*(cos(b))^2+m3*(cos(c))^2)/kx 0; (m2*(cos(b))^2+m3*(cos(c))^2)/ky]; Gd=[-m1*g*sin(a)/kx; -m2*g*sin(b)/ky]; if 2