Bài tập lớn điều khiển robot UTC Đại Học Giao Thông Vận Tải tai liệu mang tính chất tham khảo mọi người có thể tải về xem và tham khảo cảm ơn mọi người nhiều ạ. mọi ý kiến có thể gửi về email vuvipvai182gmail.com de duocj giai dap em, minh cảm ơn mọi người rất nhiều ạ
ĐỘNG LỰC HỌC ROBOT
Robot RV-M1 MITSUBISHI
Hình 1.1 Mô hình 3D Robot RV-M1 MITSUBISHI
Cấu Tạo Robot RV-M1 MITSUBISHI
Hình 1.2.6 bàn tay nắm bắt
Động học Robot
1.3.1 Robot gắn hệ trục tọa độ 3D và lập bảng DH
Hình 1.3.1 Gắn tọa độ 3D trên Robot
Hình 1.3.2 Gắn hệ trục tọa độ và góc quay
1.3.2 Ma trận biến đổi thuần nhất giữa hai khâu liên tiếp.
Do cách chọn trục sai so với quy tắc của bảng ĐH nên tính lại ta có:
[ cos(t1)*cos(t5) + cos(t2)*cos(t3)*sin(t1)*sin(t4)*sin(t5) + cos(t2)*cos(t4)*sin(t1)*sin(t3)*sin(t5) + cos(t3)*cos(t4)*sin(t1)*sin(t2)*sin(t5)
- sin(t1)*sin(t2)*sin(t3)*sin(t4)*sin(t5), cos(t2)*cos(t3)*cos(t5)*sin(t1)*sin(t4) - cos(t1)*sin(t5) + cos(t2)*cos(t4)*cos(t5)*sin(t1)*sin(t3) + cos(t3)*cos(t4)*cos(t5)*sin(t1)*sin(t2)
- cos(t5)*sin(t1)*sin(t2)*sin(t3)*sin(t4), sin(t2 - t1 + t3 + t4)/2 - sin(t1 + t2 + t3 + t4)/2, l3*sin(t1)*sin(t2)*sin(t3) - l2*cos(t2)*sin(t1) - l3*cos(t2)*cos(t3)*sin(t1) - l4*cos(t1) - l5*cos(t2)*cos(t3)*cos(t4)*sin(t1) + l5*cos(t2)*sin(t1)*sin(t3)*sin(t4) + l5*cos(t3)*sin(t1)*sin(t2)*sin(t4) + l5*cos(t4)*sin(t1)*sin(t2)*sin(t3)
[ cos(t5)*sin(t1) - cos(t1)*cos(t2)*cos(t3)*sin(t4)*sin(t5) - cos(t1)*cos(t2)*cos(t4)*sin(t3)*sin(t5) - cos(t1)*cos(t3)*cos(t4)*sin(t2)*sin(t5) + cos(t1)*sin(t2)*sin(t3)*sin(t4)*sin(t5), cos(t1)*cos(t5)*sin(t2)*sin(t3)*sin(t4) - cos(t1)*cos(t2)*cos(t3)*cos(t5)*sin(t4)
- cos(t1)*cos(t2)*cos(t4)*cos(t5)*sin(t3) - cos(t1)*cos(t3)*cos(t4)*cos(t5)*sin(t2) - sin(t1)*sin(t5), cos(t1 + t2 + t3 + t4)/2 + cos(t2 - t1 + t3 + t4)/2, l2*cos(t1)*cos(t2) - l4*sin(t1) + l3*cos(t1)*cos(t2)*cos(t3) - l3*cos(t1)*sin(t2)*sin(t3) + l5*cos(t1)*cos(t2)*cos(t3)*cos(t4) - l5*cos(t1)*cos(t2)*sin(t3)*sin(t4) - l5*cos(t1)*cos(t3)*sin(t2)*sin(t4) - l5*cos(t1)*cos(t4)*sin(t2)*sin(t3) ]
1.3.3 Phương trình lực động lực học Robot
Thông số các khâu trong Robot : khâu 0 :
Hình 1.3.3 Thông số trong đế khâu Khối lượng 13,776995kg
Momen quán tính khâu đế (khâu 0):
Ixx = 0,113818 Ixy = 0 Ixz = -0,022731Iyx = 0 Iyy =0,149769 Iyz = 0,000001Izx = -0,022731 Izy = 0,000001 Izz = 0,110865
Hình 1.3.4 Thông số trong khâu 1 Khổi lượng 12,184665 kg
Ixx = 0,145561 Ixy = 0,000015 Ixz = 0,006287Iyx = 0,000015 Iyy = 0,165763 Iyz = 0,00005Izx = 0,006287 Izy = 0,00005 Izz = 0,089568
Hình 1.3.5 Thông số trong khâu 2 Khổi lượng 12,144184 kg
Ixx = 0,031061 Ixy = 0 Ixz = -0,000046Iyx = 0 Iyy = 0,360491 Iyz = 0,00009Izx = -0,000046 Izy = 0,000090 Izz = 0,350351
Hình 1.3.6 Thông số trong khâu 3 Khổi lượng 3,047721 kg
Hình 1.3.7 Thông số trong khâu 4
Hình 1.3.8 Thông số trong khâu 5
Vector biểu diễn khối tâm của từng khâu trong hệ quy chiếu của chính nó: rRG0 = [ -0,028935 ; 0 ; 0,060721 ; 1 ] rRG1= [-0,025155 ; -0,00005 ; -0,082116 ; 1 ] rRG2 = [0,108136 ; -0,003646 ; -0,003177 ; 1 ] rRG3 = [ 0,097551 ; -0,000122 ; 0 ; 1 ] rRG4 = [ 0 ; -0,010914 ; 0; 1 ] rRG5 = [ 0 ; -0,000029 ; 0,024545 ; 1 ]
Vector gia tốc trọng trường:
Ma trận giả quán tính các khâu :
Phương trình động lực học Robot
Tr [ ∂ ∂ 2 2 T q 1 1 J 1 ∂ T ∂ q 1 1 T ] q ´ 1 2 + Tr [ ∂ ∂ 2 2 T q 1 2 J 2 ∂ T ∂ q 2 T 1 ] q ´ 1 2 +Tr [ ∂ ∂ 2 2 T q 1 3 J 3 ∂T ∂q 3 T 1 ] q ´ 1 2 + Tr [ ∂ ∂ 2 2 T q 1 4 J 4 ∂T ∂ q 4 T 1 ] q ´ 1 2 +Tr [ ∂ ∂ 2 2 T q 1 5 J 5 ∂T ∂ q 5 T 1 ] q ´ 1 2
(50728546202701*qd2^2*cos(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qd2^2*cos(q2 + q3 + q4 + 2*q5))/18446744073709551616 + (50728546202701*qd3^2*cos(q2 + q3 + q4 - 2*q5))/18446744073709551616
(1421*qdd1*cos(2*q2 + q3 + q4))/20000 + (1421*qdd1*cos(q3 + q4))/20000 + (143*qdd1*cos(q3))/3125 + (9*qdd2*cos(q2))/100000 + (1421*qdd1*cos(q4))/
(50728546202701*qdd2*sin(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qdd2*sin(q2 + q3 + q4 + 2*q5))/18446744073709551616 + (50728546202701*qdd3*sin(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qdd3*sin(q2 + q3 + q4 + 2*q5))/18446744073709551616 + (50728546202701*qdd4*sin(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qdd4*sin(q2 + q3 + q4 + 2*q5))/18446744073709551616 + (143*qdd1*cos(2*q2 + q3))/3125 + (50728546202701*qdd1*cos(2*q2 + 2*q3 + 2*q4 - 2*q5))/36893488147419103232 + (50728546202701*qdd1*cos(2*q2 + 2*q3 + 2*q4 + 2*q5))/36893488147419103232 -
(143*qd1*qd3*sin(q3))/3125 - (1421*qd1*qd4*sin(q4))/31250 -
(1421*qd1*qd2*sin(2*q2 + 2*q3 + q4))/15625 - (1421*qd1*qd3*sin(2*q2 + 2*q3 + q4))/15625 - (1421*qd1*qd4*sin(2*q2 + 2*q3 + q4))/31250 +
- 2*q5))/9223372036854775808 + (50728546202701*qd2*qd4*cos(q2 + q3 + q4 + 2*q5))/9223372036854775808 - (50728546202701*qd2*qd5*cos(q2 + q3 + q4 - 2*q5))/9223372036854775808 - (50728546202701*qd2*qd5*cos(q2 + q3 + q4 + 2*q5))/9223372036854775808 + (50728546202701*qd3*qd4*cos(q2 + q3 + q4 - 2*q5))/9223372036854775808 -
2*q5))/9223372036854775808 - (50728546202701*qd3*qd5*cos(q2 + q3 + q4 + 2*q5))/9223372036854775808 - (50728546202701*qd4*qd5*cos(q2 + q3 + q4 - 2*q5))/9223372036854775808 - (50728546202701*qd4*qd5*cos(q2 + q3 + q4 + 2*q5))/9223372036854775808 - (286*qd1*qd2*sin(2*q2 + q3))/3125 - (143*qd1*qd3*sin(2*q2 + q3))/3125 - (50728546202701*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5))/18446744073709551616 -
2*q5))/18446744073709551616 - (3034489400125221*qd2*qd5*cos(q2 + q3 + q4))/4611686018427387904 - (3034489400125221*qd3*qd5*cos(q2 + q3 + q4))/4611686018427387904 - (3034489400125221*qd4*qd5*cos(q2 + q3 + q4))/4611686018427387904 -
2*q4))/144115188075855872000000 + (2*qd1*qd2*cos(2*q2 + 2*q3))/15625 + (2*qd1*qd3*cos(2*q2 + 2*q3))/15625 -
2*q3))/11258999068426240000000 - (1421*qd1*qd2*sin(2*q2 + q3 + q4))/10000 - (1421*qd1*qd3*sin(2*q2 + q3 + q4))/20000 -
(1421*qd1*qd4*sin(2*q2 + q3 + q4))/20000 - (1421*qd1*qd3*sin(q3 + q4))/20000 - (1421*qd1*qd4*sin(q3 + q4))/20000
(1421*qd1^2*sin(2*q2 + 2*q3 + q4))/31250 + (143*qd1^2*sin(2*q2 + q3))/3125 + (50728546202701*qd1^2*sin(2*q2 + 2*q3 + 2*q4 -
2*q4))/288230376151711744000000 + (1421*qdd2*cos(q3 + q4))/10000 + (1421*qdd3*cos(q3 + q4))/20000 + (1421*qdd4*cos(q3 + q4))/20000 -
(qd1^2*cos(2*q2 + 2*q3))/15625 + (779994182062992063261*qd1^2*sin(2*q2 + 2*q3))/22517998136852480000000 + (9*qdd1*cos(q2))/100000 +
(1421*qd1^2*sin(2*q2 + q3 + q4))/20000 + (50728546202701*qdd1*sin(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qdd1*sin(q2 + q3 + q4 + 2*q5))/18446744073709551616 - (1421*qd3^2*sin(q3 + q4))/20000 - (1421*qd4^2*sin(q3 + q4))/20000 - (286*qd2*qd3*sin(q3))/3125 +
(1421*qd2*qd4*sin(q4))/15625 - (1421*qd3*qd4*sin(q4))/15625 -
(1421*qd2*qd3*sin(q3 + q4))/10000 + (1421*qd2*qd4*sin(q3 + q4))/10000 - (1421*qd3*qd4*sin(q3 + q4))/10000
(1421*qd4^2*sin(q4))/31250 + (1421*qd1^2*sin(2*q2 + 2*q3 + q4))/31250 + (143*qd1^2*sin(2*q2 + q3))/6250 + (50728546202701*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5))/36893488147419103232 + (50728546202701*qd1^2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5))/36893488147419103232 +
2*q4))/288230376151711744000000 + (1421*qdd2*cos(q3 + q4))/20000 - (qd1^2*cos(2*q2 + 2*q3))/15625 + (779994182062992063261*qd1^2*sin(2*q2 + 2*q3))/22517998136852480000000 + (143*qdd2*cos(q3))/3125 +
(1421*qdd4*cos(q4))/31250 + (1421*qd1^2*sin(2*q2 + q3 + q4))/40000 + (50728546202701*qdd1*sin(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qdd1*sin(q2 + q3 + q4 + 2*q5))/18446744073709551616 + (1421*qd1^2*sin(q3 + q4))/40000 + (1421*qd2^2*sin(q3 + q4))/20000 +
(1421*qd2*qd4*sin(q4))/15625 - (1421*qd3*qd4*sin(q4))/15625 -
(1421*qd3^2*sin(q4))/31250 + (1421*qd1^2*sin(2*q2 + 2*q3 + q4))/62500 + (50728546202701*qd1^2*sin(2*q2 + 2*q3 + 2*q4 -
2*q4))/288230376151711744000000 + (1421*qdd2*cos(q3 + q4))/20000 + (1421*qdd2*cos(q4))/31250 + (1421*qdd3*cos(q4))/31250 +
(1421*qd1^2*sin(2*q2 + q3 + q4))/40000 + (50728546202701*qdd1*sin(q2 + q3 + q4 - 2*q5))/18446744073709551616 - (50728546202701*qdd1*sin(q2 + q3 + q4 + 2*q5))/18446744073709551616 + (1421*qd1^2*sin(q3 + q4))/40000 + (1421*qd2^2*sin(q3 + q4))/20000 + (1421*qd2*qd3*sin(q4))/15625 -
(50728546202701*qd1*qd2*cos(q2 + q3 + q4 - 2*q5))/9223372036854775808 + (50728546202701*qd1*qd2*cos(q2 + q3 + q4 +
- 2*q5))/9223372036854775808 + (50728546202701*qd1*qd3*cos(q2 + q3 + q4 + 2*q5))/9223372036854775808 + (50728546202701*qd1*qd4*cos(q2 + q3 + q4 - 2*q5))/9223372036854775808 + (50728546202701*qd1*qd4*cos(q2 + q3 + q4 + 2*q5))/9223372036854775808 -
(3034489400125221*qd1*qd2*cos(q2 + q3 + q4))/4611686018427387904 - (3034489400125221*qd1*qd3*cos(q2 + q3 + q4))/4611686018427387904 - (3034489400125221*qd1*qd4*cos(q2 + q3 + q4))/4611686018427387904 - (50728546202701*qd2*qd3*sin(2*q5))/4611686018427387904 +
Kết quả sau khi rut gọn các phương trình
(0.00000275*qd2^2*cos(q2 + q3 + q4 - 2*q5)) - (0.00000275*qd2^2*cos(q2 + q3 + q4 + 2*q5)) + (0.00000275*qd3^2*cos(q2 + q3 + q4 - 2*q5)) -
(0.00000275*qd3^2*cos(q2 + q3 + q4 + 2*q5)) + (0.00000275*qd4^2*cos(q2 + q3 + q4 - 2*q5)) - (0.00000275*qd4^2*cos(q2 + q3 + q4 + 2*q5)) +
(0.000046*qdd2*sin(q2)) + (0.00000275*qdd2*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd2*sin(q2 + q3 + q4 + 2*q5)) + (0.00000275*qdd3*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd3*sin(q2 + q3 + q4 + 2*q5)) +
(0.00000275*qdd4*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd4*sin(q2 + q3 + q4 + 2*q5)) + (143*qdd1*cos(2*q2 + q3))/3125 +
(0.000658*qdd5*sin(q2 + q3 + q4)) - (143*qd1*qd3*sin(q3))/3125 -
(0.045472*qd1*qd4*sin(q4)) - (0.090944*qd1*qd2*sin(2*q2 + 2*q3 + q4)) - (0.090944*qd1*qd3*sin(2*q2 + 2*q3 + q4)) - (1421*qd1*qd4*sin(2*q2 + 2*q3 + q4))/31250 + (0.0000055*qd2*qd3*cos(q2 + q3 + q4 - 2*q5)) -
(0.0000055*qd2*qd3*cos(q2 + q3 + q4 + 2*q5)) - (0.0000055*qd2*qd4*cos(q2 + q3 + q4 - 2*q5)) + (0.0000055*qd2*qd4*cos(q2 + q3 + q4 + 2*q5)) -
(0.0000055*qd2*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd2*qd5*cos(q2 + q3 + q4 + 2*q5)) + (0.0000055*qd3*qd4*cos(q2 + q3 + q4 - 2*q5)) -
(0.0000055*qd3*qd4*cos(q2 + q3 + q4 + 2*q5)) - (0.0000055*qd3*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd3*qd5*cos(q2 + q3 + q4 + 2*q5)) -
(0.0000055*qd4*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd4*qd5*cos(q2 + q3 + q4 + 2*q5)) - (286*qd1*qd2*sin(2*q2 + q3))/3125 -
(143*qd1*qd3*sin(2*q2 + q3))/3125 - (0.00000275*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) - (0.00000275*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5)) - (0.00000275*qd1*qd3*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) -
(0.000658*qd2*qd5*cos(q2 + q3 + q4)) - (0.000658*qd3*qd5*cos(q2 + q3 + q4)) - (0.000658*qd4*qd5*cos(q2 + q3 + q4)) - (0.591555*qd1*qd2*sin(2*q2))
- (0.0000055*qd1*qd5*sin(2*q5)) - (0.1653865*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4)) - (0.1653865*qd1*qd3*sin(2*q2 + 2*q3 + 2*q4)) -
(0.1653865*qd1*qd4*sin(2*q2 + 2*q3 + 2*q4)) + (2*qd1*qd2*cos(2*q2 + 2*q3))/15625 + (2*qd1*qd3*cos(2*q2 + 2*q3))/15625 -
(0.0692774*qd1*qd2*sin(2*q2 + 2*q3)) - (0.0692774*qd1*qd3*sin(2*q2 + 2*q3)) - (0.1421*qd1*qd2*sin(2*q2 + q3 + q4)) - (0.07105*qd1*qd3*sin(2*q2 + q3 + q4)) - (0.07105*qd1*qd4*sin(2*q2 + q3 + q4)) -
(0.07105*qd1*qd3*sin(q3 + q4)) - (0.07105*qd1*qd4*sin(q3 + q4))
(2.573676*cos(q2)) + (0.475888*sin(q2)) - (0.0000055*qdd2*cos(2*q5)) - (0.0000055*qdd3*cos(2*q5)) - (0.0000055*qdd4*cos(2*q5)) -
(0.045472*qd1^2*sin(2*q2 + 2*q3 + q4)) + (143*qd1^2*sin(2*q2 + q3))/3125 + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) +
(0.2957775*qd1^2*sin(2*q2)) + (0.08269325*qd1^2*sin(2*q2 + 2*q3 + 2*q4)) + (0.1421*qdd2*cos(q3 + q4)) + (0.07105*qdd3*cos(q3 + q4)) +
(0.07105*qd1^2*sin(2*q2 + q3 + q4)) + (0.00000275*qdd1*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd1*sin(q2 + q3 + q4 + 2*q5)) - (1421*qd3^2*sin(q3 + q4))/20000 - (0.07105*qd4^2*sin(q3 + q4)) - (286*qd2*qd3*sin(q3))/3125 + (1421*qd2*qd4*sin(q4))/15625 - (1421*qd3*qd4*sin(q4))/15625 -
(0.0000055*qd1*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 + 2*q5)) + (0.000658*qd1*qd5*cos(q2 + q3 + q4)) +
(0.000011*qd2*qd5*sin(2*q5)) + (0.000011*qd3*qd5*sin(2*q5)) +
(0.000011*qd4*qd5*sin(2*q5)) - (0.1421*qd2*qd3*sin(q3 + q4)) +
(0.1421*qd2*qd4*sin(q3 + q4)) - (0.1421*qd3*qd4*sin(q3 + q4))
(1421*qd1^2*sin(2*q2 + 2*q3 + q4))/31250 + (143*qd1^2*sin(2*q2 + q3))/6250 + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) +
- (qd1^2*cos(2*q2 + 2*q3))/15625 + (0.0346387*qd1^2*sin(2*q2 + 2*q3)) + (143*qdd2*cos(q3))/3125 + (1421*qdd2*cos(q4))/15625 +
(0.035525*qd1^2*sin(2*q2 + q3 + q4)) + (0.00000275*qdd1*sin(q2 + q3 + q4 -2*q5)) - (0.00000275*qdd1*sin(q2 + q3 + q4 + 2*q5)) + (1421*qd1^2*sin(q3 + q4))/40000 + (1421*qd2^2*sin(q3 + q4))/20000 +
(1421*qd2*qd4*sin(q4))/15625 - (1421*qd3*qd4*sin(q4))/15625 -
(0.0000055*qd1*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 + 2*q5)) + (0.000658*qd1*qd5*cos(q2 + q3 + q4)) +
(0.000011*qd2*qd5*sin(2*q5)) + (0.000011*qd3*qd5*sin(2*q5)) +
(0.08269325*qd1^2*sin(2*q2 + 2*q3 + 2*q4)) + (0.07105*qdd2*cos(q3 + q4)) + (0.045472*qdd2*cos(q4)) + (0.045472*qdd3*cos(q4 +
(0.035525*qd1^2*sin(2*q2 + q3 + q4)) + (0.00000275*qdd1*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd1*sin(q2 + q3 + q4 + 2*q5)) +
(1421*qd2*qd3*sin(q4))/15625 - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 + 2*q5)) +
(0.000658*qd1*qd5*cos(q2 + q3 + q4)) + (0.000011*qd2*qd5*sin(2*q5)) + (0.000011*qd3*qd5*sin(2*q5)) + (0.000011*qd4*qd5*sin(2*q5))
(0.000658*qdd1*sin(q2 + q3 + q4)) + (0.0000055*qd1*qd2*cos(q2 + q3 + q4 - 2*q5)) + (0.0000055*qd1*qd2*cos(q2 + q3 + q4 + 2*q5)) +
(0.0000055*qd1*qd3*cos(q2 + q3 + q4 - 2*q5)) + (0.0000055*qd1*qd3*cos(q2 + q3 + q4 + 2*q5)) + (0.0000055*qd1*qd4*cos(q2 + q3 + q4 - 2*q5)) +
(0.0000055*qd1*qd4*cos(q2 + q3 + q4 + 2*q5)) - (0.000658*qd1*qd2*cos(q2 + q3 + q4)) - (0.000658*qd1*qd3*cos(q2 + q3 + q4)) -
(0.000658*qd1*qd4*cos(q2 + q3 + q4)) - (0.000011*qd2*qd3*sin(2*q5)) + (0.000011*qd2*qd4*sin(2*q5)) - (0.000011*qd3*qd4*sin(2*q5))
CHỌN ĐỘNG CƠ
Bảng thông số động cơ 110ST-IM02030
Hình 2.1 Bảng thông số động cơ
Thông số động cơ từng khâu
2.2.1 Động cơ khâu 1 : 110ST-IM0203
Moment quán tính phần ứng: Jm1= 0,31.10 -3 kgm2 Điện áp phần ứng : V = 220 (V)
Tỉ số truyền từ động cơ đến các khớp: r 1 = 20
Hệ số cản của cơ cấu dẫn động: Bm1= 0.032 Nms/rad
Hệ số sức phản điện động: Kb1 = 0,65 Vs/rad
Hệ số moment: Km1= 0.8 Nm/A Điện trở phần ứng động cơ: R1= 3,6 ( )
2.2.2 Động cơ khâu 2 : 110ST-IM02030
Moment quán tính phần ứng: Jm2= 0,31.10 -3 kgm2 Điện áp phần ứng : V = 220 (V)
Tỉ số truyền từ động cơ đến các khớp: r 2 = 15
Hệ số cản của cơ cấu dẫn động: Bm2= 0.032 Nms/rad
Hệ số sức phản điện động: Kb2 = 0,65 Vs/rad
Hệ số moment: Km2= 0.8 Nm/A Điện trở phần ứng động cơ: R2= 3,6 ( )
2.2.3 Động cơ khâu 3 : 110ST-IM02030
Moment quán tính phần ứng: Jm3= 0,31.10 -3 kgm2 Điện áp phần ứng : V = 220 (V)
Tỉ số truyền từ động cơ đến các khớp: r 3 = 15
Hệ số cản của cơ cấu dẫn động: Bm3= 0.032 Nms/rad
Hệ số sức phản điện động: Kb3 = 0,65 Vs/rad
Hệ số moment: Km3= 0.8 Nm/A Điện trở phần ứng động cơ: R3= 3,6 ( )
2.2.4 Động cơ khâu 4 : 110ST-IM02030
Moment quán tính phần ứng: Jm4= 0,31.10 -3 kgm2 Điện áp phần ứng : V = 220 (V)
Tỉ số truyền từ động cơ đến các khớp: r 4 = 15
Hệ số cản của cơ cấu dẫn động: Bm4= 0.032 Nms/rad
Hệ số sức phản điện động: Kb4 = 0,65 Vs/rad
Hệ số moment: Km4= 0.8 Nm/A Điện trở phần ứng động cơ: R4= 3,6 ( )
2.2.5 Động cơ khâu 5 : 110ST-IM02030
Moment quán tính phần ứng: Jm5= 0,31.10 -3 kgm2 Điện áp phần ứng : V = 220 (V)
Tỉ số truyền từ động cơ đến các khớp: r 5 = 10
Hệ số cản của cơ cấu dẫn động: Bm5= 0.032 Nms/rad
Hệ số sức phản điện động: Kb5= 0,65 Vs/rad
Hệ số moment: Km5= 0.8 Nm/A Điện trở phần ứng động cơ: R5= 3,6 ( )
Phương trình chuyển động của Robot
Với thông số đã cho của bài toán ta thiết lập được hệ phương trình động lực học robot như sau :
(9*qd2^2*sin(q2))/100000 + (0.00000275*qd2^2*cos(q2 + q3 + q4 - 2*q5)) - (0.00000275*qd2^2*cos(q2 + q3 + q4 + 2*q5)) + (0.00000275*qd3^2*cos(q2 + q3 + q4 - 2*q5)) - (0.00000275*qd3^2*cos(q2 + q3 + q4 + 2*q5)) +
(0.00000275*qd4^2*cos(q2 + q3 + q4 - 2*q5)) - (0.00000275*qd4^2*cos(q2 + q3 + q4 + 2*q5)) + (0.0346*qdd1*cos(2*q2 + 2*q3)) + (qdd1*sin(2*q2 +
2*q3))/15625 + (0.07105*qdd1*cos(2*q2 + q3 + q4)) + (0.07105*qdd1*cos(q3 + q4)) + (143*qdd1*cos(q3))/3125 + (9*qdd2*cos(q2))/100000 +
(0.000046*qdd2*sin(q2)) + (0.00000275*qdd2*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd2*sin(q2 + q3 + q4 + 2*q5)) + (0.00000275*qdd3*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd3*sin(q2 + q3 + q4 + 2*q5)) +
(0.00000275*qdd4*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd4*sin(q2 + q3 + q4 + 2*q5)) + (143*qdd1*cos(2*q2 + q3))/3125 +
(0.000658*qdd5*sin(q2 + q3 + q4)) - (143*qd1*qd3*sin(q3))/3125 -
(0.045472*qd1*qd4*sin(q4)) - (0.090944*qd1*qd2*sin(2*q2 + 2*q3 + q4)) - (0.090944*qd1*qd3*sin(2*q2 + 2*q3 + q4)) - (1421*qd1*qd4*sin(2*q2 + 2*q3 + q4))/31250 + (0.0000055*qd2*qd3*cos(q2 + q3 + q4 - 2*q5)) -
(0.0000055*qd2*qd3*cos(q2 + q3 + q4 + 2*q5)) - (0.0000055*qd2*qd4*cos(q2 + q3 + q4 - 2*q5)) + (0.0000055*qd2*qd4*cos(q2 + q3 + q4 + 2*q5)) -
(0.0000055*qd2*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd2*qd5*cos(q2 + q3 + q4 + 2*q5)) + (0.0000055*qd3*qd4*cos(q2 + q3 + q4 - 2*q5)) -
(0.0000055*qd3*qd4*cos(q2 + q3 + q4 + 2*q5)) - (0.0000055*qd3*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd3*qd5*cos(q2 + q3 + q4 + 2*q5)) -
(0.0000055*qd4*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd4*qd5*cos(q2 + q3 + q4 + 2*q5)) - (286*qd1*qd2*sin(2*q2 + q3))/3125 -
(143*qd1*qd3*sin(2*q2 + q3))/3125 - (0.00000275*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) - (0.00000275*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5)) - (0.00000275*qd1*qd3*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) -
(0.000658*qd2*qd5*cos(q2 + q3 + q4)) - (0.000658*qd3*qd5*cos(q2 + q3 + q4)) - (0.000658*qd4*qd5*cos(q2 + q3 + q4)) - (0.591555*qd1*qd2*sin(2*q2))
- (0.0000055*qd1*qd5*sin(2*q5)) - (0.1653865*qd1*qd2*sin(2*q2 + 2*q3 + 2*q4)) - (0.1653865*qd1*qd3*sin(2*q2 + 2*q3 + 2*q4)) -
(0.1653865*qd1*qd4*sin(2*q2 + 2*q3 + 2*q4)) + (2*qd1*qd2*cos(2*q2 + 2*q3))/15625 + (2*qd1*qd3*cos(2*q2 + 2*q3))/15625 -
(0.0692774*qd1*qd2*sin(2*q2 + 2*q3)) - (0.0692774*qd1*qd3*sin(2*q2 + 2*q3)) - (0.1421*qd1*qd2*sin(2*q2 + q3 + q4)) - (0.07105*qd1*qd3*sin(2*q2
(0.07105*qd1*qd3*sin(q3 + q4)) - (0.07105*qd1*qd4*sin(q3 + q4))]
15 2 0,8522 =4,0975 10 -3 d 2 = 15 1 ¿ (0.2398209*qdd3) + (0.1664395*qdd4) - (2.9757112*cos(q2 + q3 + q4)) - (31.085992*cos(q2 + q3)) + (2.573676*cos(q2)) + (0.475888*sin(q2)) - (0.0000055*qdd2*cos(2*q5)) - (0.0000055*qdd3*cos(2*q5)) -
(143*qd1^2*sin(2*q2 + q3))/3125 + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5)) + (0.2957775*qd1^2*sin(2*q2)) + (0.08269325*qd1^2*sin(2*q2 + 2*q3 + 2*q4)) + (0.1421*qdd2*cos(q3 + q4)) + (0.07105*qdd3*cos(q3 + q4)) +
2*q5)) - (0.00000275*qdd1*sin(q2 + q3 + q4 + 2*q5)) - (1421*qd3^2*sin(q3 + q4))/20000 - (0.07105*qd4^2*sin(q3 + q4)) - (286*qd2*qd3*sin(q3))/3125 + (1421*qd2*qd4*sin(q4))/15625 - (1421*qd3*qd4*sin(q4))/15625 -
(0.0000055*qd1*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 + 2*q5)) + (0.000658*qd1*qd5*cos(q2 + q3 + q4)) +
(0.000011*qd2*qd5*sin(2*q5)) + (0.000011*qd3*qd5*sin(2*q5)) +
(0.000011*qd4*qd5*sin(2*q5)) - (0.1421*qd2*qd3*sin(q3 + q4)) +
(0.1421*qd2*qd4*sin(q3 + q4)) - (0.1421*qd3*qd4*sin(q3 + q4))]
15 2 0,2398 =1,3757 10 -3 d 3 = 15 1 ¿ (0.2398209*qdd2) + (0.1664395*qdd4) - (2.9757112*cos(q2 + q3 + q4)) + (27.498408*cos(q2 + q3)) - (0.0000055*qdd2*cos(2*q5)) -
(1421*qd4^2*sin(q4))/31250 + (1421*qd1^2*sin(2*q2 + 2*q3 + q4))/31250 + (143*qd1^2*sin(2*q2 + q3))/6250 + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5)) + (0.08269325*qd1^2*sin(2*q2 + 2*q3 + 2*q4)) + (0.07105*qdd2*cos(q3 + q4))
(0.035525*qd1^2*sin(2*q2 + q3 + q4)) + (0.00000275*qdd1*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd1*sin(q2 + q3 + q4 + 2*q5)) + (1421*qd1^2*sin(q3 + q4))/40000 + (1421*qd2^2*sin(q3 + q4))/20000 +
(1421*qd2*qd4*sin(q4))/15625 - (1421*qd3*qd4*sin(q4))/15625 -
(0.0000055*qd1*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 + 2*q5)) + (0.000658*qd1*qd5*cos(q2 + q3 + q4)) +
(0.000011*qd2*qd5*sin(2*q5)) + (0.000011*qd3*qd5*sin(2*q5)) +
15 2 0,1664 =1,0495 10 -3 d 4 = 15 1 ¿ (0.1664395*qdd2) + (0.1664395*qdd3) - (2.8347088*cos(q2 + q3 + q4)) - (0.0000055*qdd2*cos(2*q5)) - (0.0000055*qdd3*cos(2*q5)) -
(0.022736*qd1^2*sin(2*q2 + 2*q3 + q4)) + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) + (0.000001375*qd1^2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5)) + (0.08269325*qd1^2*sin(2*q2 + 2*q3 + 2*q4)) + (0.07105*qdd2*cos(q3 + q4)) + (0.045472*qdd2*cos(q4)) + (0.045472*qdd3*cos(q4 +
(0.035525*qd1^2*sin(2*q2 + q3 + q4)) + (0.00000275*qdd1*sin(q2 + q3 + q4 - 2*q5)) - (0.00000275*qdd1*sin(q2 + q3 + q4 + 2*q5)) +
(1421*qd2*qd3*sin(q4))/15625 - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 - 2*q5)) - (0.0000055*qd1*qd5*cos(q2 + q3 + q4 + 2*q5)) +
(0.000658*qd1*qd5*cos(q2 + q3 + q4)) + (0.000011*qd2*qd5*sin(2*q5)) + (0.000011*qd3*qd5*sin(2*q5)) + (0.000011*qd4*qd5*sin(2*q5))]
10 2 0,000658 =3,1658 10 -4 d 5 = 10 1 ¿ (2.90521*cos(q2 + q3 + q4)) - (0.000001357*qd1^2*sin(2*q2 + 2*q3 + 2*q4 - 2*q5)) + (0.000001357*qd1^2*sin(2*q2 + 2*q3 + 2*q4 + 2*q5)) + (0.00000275*qd1^2*sin(2*q5)) - (0.0000055*qd2^2*sin(2*q5)) -
(0.000658*qdd1*sin(q2 + q3 + q4)) + (0.0000055*qd1*qd2*cos(q2 + q3 + q4 - 2*q5)) + (0.0000055*qd1*qd2*cos(q2 + q3 + q4 + 2*q5)) +
(0.0000055*qd1*qd3*cos(q2 + q3 + q4 - 2*q5)) + (0.0000055*qd1*qd3*cos(q2 + q3 + q4 + 2*q5)) + (0.0000055*qd1*qd4*cos(q2 + q3 + q4 - 2*q5)) +
(0.0000055*qd1*qd4*cos(q2 + q3 + q4 + 2*q5)) - (0.000658*qd1*qd2*cos(q2
(0.000658*qd1*qd4*cos(q2 + q3 + q4)) - (0.000011*qd2*qd3*sin(2*q5)) + (0.000011*qd2*qd4*sin(2*q5)) - (0.000011*qd3*qd4*sin(2*q5))]
THIẾT KẾ BỘ ĐIỀU KHIỂN
Bộ điều khiển PI, PD,PID và PD bão hòa
3.1.1 Xác định K p , K d và K i trong bộ điều khiển
Bộ điều khiển PD có dạng:
Phương trình hệ kín trở thành:
↔ [1,6622 10 -3 S 2 + ( K 1 K d + 0,046).s + K 1 K p ] θ m (s) = K 1 K d θ m d (s) - D(s) Đặt Ω(s) =1,6622 10 -3 S 2 + ( K 1 K d + 0,046).s + K 1 K p Đa thức đặc trưng của hệ:
−¿ Sai số xác lập: Để thuận tiện cho việc thiết kế ta coi D(s) là rất nhỏ D(s)= 0,001 e ss = lim s →0 s E ( s ) = lim s → 0 ¿ ¿ ]
5 (3) Thời gian tang : t r = w 1 n [1,589 ξ 3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿ ≤1(4) Thử chọn ξ=1 thì w n ≥ 3,3711 ¿>¿ chọn w n = 80
J eff Đa thức đặc trưng:
Bộ điều khiển PD có dạng:
Phương trinhg hệ kín trở thành:
↔ [4,0975 10 -3 S 2 + ( K 2 K d + 0,046).s + K 2 K p ] θ m (s) = K 2 K d θ m d (s) - D(s) Đặt Ω(s) = 4,0975 10 -3 S 2 + ( K 2 K d + 0,046).s + K 2 K p Đa thức đặc trưng của hệ:
−¿ Sai số xác lập: Để thuận tiện cho việc thiết kế ta coi D(s) là rất nhỏ D(s)= 0,001 e ss = lim s → 0 s E ( s ) = ¿ ]
5 (3) Thời gian tang: t r = w 1 n [1,589 ξ 3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿ ≤1(4) Thử chọn ξ=1 thì w n ≥ 3,3711 ¿>¿ chọn w n = 50
J eff Đa thức đặc trưng:
Bộ điều khiển PD có dạng:
Phương trinhg hệ kín trở thành:
↔ [1,3757 10 -3 S 2 + ( K 3 K d + 0,046).s + K 3 K p ] θ m (s) = K 3 K d θ m d (s) - D(s) Đặt Ω(s) = 1,3757 10 -3 S 2 + ( K 3 K d + 0,046).s + K 3 K p Đa thức đặc trưng của hệ:
−¿ Sai số xác lập: Để thuận tiện cho việc thiết kế ta coi D(s) là rất nhỏ D(s)= 0,001 e ss = lim s → 0 s E ( s ) = ¿ ]
J eff Đa thức đặc trưng:
Bộ điều khiển PD có dạng:
Phương trinh hệ kín trở thành:
↔ [1,0495 10 -3 S 2 + ( K 4 K d + 0,046).s + K 4 K p ] θ m (s) = K 4 K d θ m d (s) - D(s) Đặt Ω(s) = 1,0495 10 -3 S 2 + ( K 4 K d + 0,046).s + K 4 K p Đa thức đặc trưng của hệ:
−¿ Sai số xác lập: Để thuận tiện cho việc thiết kế ta coi D(s) là rất nhỏ D(s)= 0,001 e ss = lim s → 0 s E ( s ) = ¿ ]
5 (3) Thời gian tang: t r = w 1 n [1,589 ξ 3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿ ≤1(4) Thử chọn ξ=1 thì w n ≥ 3.371 ¿>¿ chọn w n = 100
J eff Đa thức đặc trưng:
Bộ điều khiển PD có dạng:
Phương trinh hệ kín trở thành:
↔ [3,1658 10 -4 S 2 + ( K 5 K d + 0,046).s + K 5 K p ] θ m (s) = K 5 K d θ m d (s) - D(s) Đặt Ω(s) = 3,1658 10 -4 S 2 + ( K 4 K d + 0,046).s + K 5 K p Đa thức đặc trưng của hệ:
−¿ Sai số xác lập: Để thuận tiện cho việc thiết kế ta coi D(s) là rất nhỏ D(s)= 0,001 e ss = lim s → 0 s E ( s ) = ¿ ]
5 (3) Thời gian tang: t r = w 1 n [1,589 ξ 3 −0,156 ξ 2 +0,924 ξ+1,0141 ¿ ≤1(4) Thử chọn ξ=1 thì w n ≥ 3.371 ¿>¿ chọn w n = 130
J eff Đa thức đặc trưng:
Sơ đồ mô phỏng Simulink
Hình 3.1.1 Sơ đồ Simulink bộ điều khiển PD
Các khối subsystems trong sơ đồ simulink bộ điều khiển PD
Hình 3.1.2 Kết quả mô phỏng góc thetal 1Nhận xét: hệ thống ổn định sau 0,5(s) và đã bám theo đường mong muốn ban đầu là 9 (rad) đảm bảo hệ thống khâu 1 hoạt động ổn định
Hình 3.1.3 Kết quả mô phỏng góc thetal 2Nhận xét: hệ thống ổn định sau 0,4(s) và đã bám theo đường mong muốn ban đầu là 3 (rad) Có sai số E= 3.5%
Hình 3.1.4 Kết quả mô phỏng góc thetal 3Nhận xét: hệ thống ổn định sau 0,4(s) và đã bám theo đường mong muốn ban đầu là 8 (rad) Có sai số E= 1,875%
Hình 3.1.5 Kết quả mô phỏng góc thetal 4Nhận xét: hệ thống ổn định sau 0,3(s) và đã bám theo đường mong muốn ban đầu là 6(rad) Có sai số E= 0,33%
Hình 3.1.6 Kết quả mô phỏng góc thetal 5Nhận xét: hệ thống ổn định sau 0,35(s) và đã bám theo đường mong muốn ban đầu là 5(rad) Có sai số E= 0,6%
Sơ đồ mô phỏng Simulink
Hình 3.1.7 Sơ đồ Simulink bộ điều khiển PI
Hình 3.1.8 Kết quả mô phỏng góc thetal 1Nhận xét: hệ thống ổn định sau 0,3(s) và đã bám theo đường mong muốn ban đầu là 9(rad) Có độ vọt lỗ P= 3,3%
Hình 3.1.9 Kết quả mô phỏng góc thetal 2Nhận xét: hệ thống ổn định sau 4,5(s) và đã bám theo đường mong muốn ban đầu là 9(rad) Có độ vọt lố P= 19% do chọn hệ số chưa được tốt
Hình 3.1.10 Kết quả mô phỏng góc thetal 3Nhận xét: hệ thống ổn định sau 2,7(s) và đã bám theo đường mong muốn ban đầu là 8(rad) Có độ vọt lố P= 3,75%
Hình 3.1.11 Kết quả mô phỏng góc thetal 4Nhận xét: hệ thống ổn định sau 0,8(s) và đã bám theo đường mong muốn ban đầu là 6(rad) Có độ vọt lố P= 0,8% sai số E =0,5%
Hình 3.1.12 Kết quả mô phỏng góc thetal 5Nhận xét: hệ thống ổn định sau 0,6(s) và đã bám theo đường mong muốn ban đầu là 5(rad) Có sai số E =0,48%
Sơ đồ mô phỏng Simulink
Hình 3.1.13 Sơ đồ Simulink bộ điều khiển PID
Hình 3.1.14 Kết quả mô phỏng góc thetal 1Nhận xét: hệ thống ổn định sau 0,71(s) và đã bám theo đường mong muốn ban đầu là 9(rad) Có sai số E =0,16%
Hình 3.1.15 Kết quả mô phỏng góc thetal 2Nhận xét: hệ thống ổn định sau 0,42(s) và đã bám theo đường mong muốn ban đầu là 3(rad) Có sai số E =3,6%
Hình 3.1.16 Kết quả mô phỏng góc thetal 3Nhận xét: hệ thống ổn định sau 0,4(s) và đã bám theo đường mong muốn ban đầu là 8(rad) Có sai số E =1,875%
Hình 3.1.17 Kết quả mô phỏng góc thetal 4Nhận xét: hệ thống ổn định sau 0,3(s) và đã bám theo đường mong muốn ban đầu là 6(rad) Có sai số E =0,1%
Hình 3.1.18 Kết quả mô phỏng góc thetal 5Nhận xét: hệ thống ổn định sau 0,38(s) và đã bám theo đường mong muốn ban đầu là 5(rad) Có sai số E =0,1%
Bộ điều khiển PD bão hòa
Sơ đồ mô phỏng Simulink
Hình 3.1.19 Sơ đồ Simulink bộ điều khiển PD bão hòa
Hình 3.1.20 Kết quả mô phỏng góc thetal 1Nhận xét: hệ thống ổn định sau 0,4(s) và đã bám theo đường mong muốn ban đầu là 2(rad) Có sai số E =7,5%
Hình 3.1.21 Kết quả mô phỏng góc thetal 2Nhận xét: hệ thống ổn định sau 0,4(s) và đã bám theo đường mong muốn ban đầu là 2,5(rad) Có sai số E =7,2%
Hình 3.1.22 Kết quả mô phỏng góc thetal 3Nhận xét: hệ thống ổn định sau 0,3(s) và đã bám theo đường mong muốn ban đầu là 1.25(rad) Có sai số E =4%
Hình 3.1.23 Kết quả mô phỏng góc thetal 4Nhận xét: hệ thống ổn định sau 0,25(s) và đã bám theo đường mong muốn ban đầu là 2(rad) Có sai số E =1%
Hì nh 3.1.24 Kết quả mô phỏng góc thetal 5Nhận xét: hệ thống ổn định sau 0,3(s) và đã bám theo đường mong muốn ban đầu là 2(rad) Có sai số E =1,1%
Thiết kế bộ điều khiển gán điểm cực phản hồi trạng thái
Ta có được các phương trình như sau:
Ta xác định được ma trận:
Trong đó ta xác địnhđươc các giá trị thông số như sau : jm1=0.00031;jm2=0.00031;jm3=0.00031;jm4=0.00031; jm5=0.00031; j11=0.5409;j12=0;j13=0;j14=0; j15=0; j21=0;j22=0.8522;j23=0;j24=0; j25=0; j31=0;j32=0;j33=0.2398;j34=0; j35=0; j41=0;j42=0;j43=0;j44=0.01664; j45=0; j15=0;j25=0;j35=0;j45=0; j55=0.000658;
Bm1=0.032;Bm2=0.0032;Bm3=0.0032;Bm4=0.0032; Bm5=0.0032; k1=0.8;k2=0.8;k3=0.8;k4=0.8; k5=0.8; r1 ;r2;r3;r4;r5
Thay số vào phương trình trên ta được:
Ta thiết lập được phương trình:
Từ phương trình (1) ta thành lập được ma trận:
Xây dựng code trên Matlab ta thu được kết quả như sau: a.Chương trình chính clc; clear; syms d1 d2 d3 d4 d5; miu=[0;0;0;0;0;0.6;0.75;0.9;0.3;0.5]; x0=[0;0;0;0;0;0.01;0.01;0.01;0.01;0.01];
%Jmi jm1=0.00031;jm2=0.00031;jm3=0.00031;jm4=0.00031;jm5=0.00031;
Bm1=0.032;Bm2=0.032;Bm3=0.032;Bm4=0.032;Bm5=0.032;
J=[jm1+j11/(r1*r1) j12/(r1*r2) j13/(r1*r3) j14/(r1*r4) j15/(r1*r5); j21/(r2*r1) jm2+j22/(r2*r2) j23/(r2*r3) j24/(r2*r4) j25/(r2*r5); j31/(r3*r1) j32/(r3*r2) jm3+j33/(r3*r3) j34/(r3*r4) j35/(r3*r5); j41/(r4*r1) j42/(r4*r2) j43/(r4*r3) jm4+j44/(r4*r4) j45/(r4*r5); j51/(r5*r1) j52/(r5*r2) j53/(r5*r3) j54/(r5*r4) jm5+j55/(r5*r5)];
% xac dinh cac bien trang thai cua he kin tuyen tinh xt=zeros(length(times),10); %bien xt chua gia tri cua bien trang thai theo thoi gian
% Tính giá tri cua bien trang thai tai tung thoi diem for i=1:length(times) xt(i,:)=(expm((A-B*K)*times(i))*x0)';
% xac d?nh cac bien trang thai cua he kin phi tuyen
[T,xt_non]=ode45(@(t,x_nonlinear)mimo_nonlinear(t,x_nonlinear,K),times,x0); %do thi ket qua mo phong so sanh gua he phi tuyen va tuyen tinh figure(1); subplot(2,2,1); plot(times,xt(:,1),'-',times,xt_non(:,1),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,1),times,0) subplot(2,2,2); plot(times,xt(:,6),'-',times,xt_non(:,6),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(2); subplot(2,2,1); plot(times,xt(:,2),'-',times,xt_non(:,2),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,2),times,0) subplot(2,2,2); plot(times,xt(:,7),'-',times,xt_non(:,7),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(3); subplot(2,2,1); plot(times,xt(:,3),'-',times,xt_non(:,3),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,3),times,0) subplot(2,2,2); plot(times,xt(:,8),'-',times,xt_non(:,8),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(4); subplot(2,2,1); plot(times,xt(:,4),'-',times,xt_non(:,4),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,4),times,0) subplot(2,2,2); plot(times,xt(:,9),'-',times,xt_non(:,9),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); figure(5); subplot(2,2,1); plot(times,xt(:,5),'-',times,xt_non(:,5),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Goc phi x1','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi goc phi theo thoi gian'); stepinfo(xt(:,5),times,0) subplot(2,2,2); plot(times,xt(:,10),'-',times,xt_non(:,10),' ','LineWidth',2); grid on; xlabel('Thoi gian (s)','FontSize',14,'FontName','time'); ylabel('Van toc goc x2','FontSize',14,'FontName','time'); set(gca,'fontsize',14); legend('linear','nonlinear'); title('Do thi van toc goc phi theo thoi gian'); end b.Chương trình con function xd_nonlinear=mimo_nonlinear(t,x_nonlinear,K)
%B? ?i?u khi?n gán ?i?m c?c u1=-K1*x_nonlinear; u2=-K2*x_nonlinear; u3=-K3*x_nonlinear; u4=-K4*x_nonlinear; u5=-K5*x_nonlinear;
%Các bi?n tr?ng thái x1=x_nonlinear(1); x2=x_nonlinear(2); x3=x_nonlinear(3); x4=x_nonlinear(4); x5=x_nonlinear(5); x6=x_nonlinear(6); x7=x_nonlinear(7); x8=x_nonlinear(8); x9=x_nonlinear(9); x10=x_nonlinear(10);
%Ph??ng tr?nh ??ng l?c h?c con l?c ??n xd1=x6; xd2=x7; xd3=x8; xd4=x9; xd5=x10; xd6=0.1925*x6+48.88*u1; xd7=0.078*x7+48.88*u2; xd8=0.2326*x8+48.88*u3; xd9=0.3049*x9+48.88*u4; xd10=0.98*x10+48.88*u5; xd_nonlinear=[xd1;xd2;xd3;xd4;xd5;xd6;xd7;xd8;xd9;xd10]; end
PHỤ LỤC : Chương trình mô phỏng Matlab
1 Chương trình Matlab tính ma trận biến đổi thuần nhất
%Tinh ma tran bien doi thuan nhat clc syms L1 L2 L3 L4 L5 t1 t2 t3 t4 t5 t6 x y z pi % cac an dung trong ma tran n = input('so luong he truc toa do')
T = eye(4); for k = 1:n fprintf('He truc toa do',k \n);
C(k) = input('chuyen vi cua he truc, 0/1(0: tinh tien, 1: quay)') if C(k) == 0 dx=input('dich chuyen theo truc x: ') dy=input('dich chuyen theo truc y: ') dz=input('dich chuyen theo truc z: ')
Q = input('Quay quanh truc x/y/z: ') t = input('Gia tri quanh truc: ') if Q == x
A = [1 0 0 0; 0 cos(t) -sin(t) 0; 0 sin(t) cos(t) 0; 0 0 0 1]; elseif Q == y
A = [cos(t) 0 sin(t) 0; 0 1 0 0; -sin(t) 0 cos(t) 0; 0 0 0 1]; elseif Q == z
A = [cos(t) -sin(t) 0 0; sin(t) cos(t) 0 0; 0 0 1 0; 0 0 0 1]; end end
2 Chương trình MatLab thiết lập hệ phương trình vi phân chuyển động của Robot syms q1 q2 q3 q4 q5 qd1 qd2 qd3 qd4 qd5 qdd1 qdd2 qdd3 qdd4 qdd5 g Vg=[0; 0; -9.8; 1]; rRG0=[ -0.029 ; 0.00 ; 0.061 ; 1 ]; rRG1=[-0.025 ; 0.00 ; -0.82 ; 1 ]; rRG2=[0.108 ; -0.004 ; -0.003 ; 1 ]; rRG3=[ 0.98 ; 0.00 ; 0.00 ; 1 ]; rRG4=[ 0 ; -0.011 ; 0; 1 ]; rRG5=[ 0 ; 0 ; 0.025 ; 1 ]; m1.18; m2.14; m3=3.05; m4=0.654; m5=0.7027;
J1=[(-Ixx1+Iyy1+Izz1)/2, Ixy1, Ixz1,0; Ixy1,(Ixx1-Iyy1+Izz1)/2, Iyz1,0; Ixz1, Iyz1,(Ixx1+Iyy1-Izz1)/2,0;0,0,0,m1];
J2=[(-Ixx2+Iyy2+Izz2)/2, Ixy2, Ixz2,0; Ixy2,(Ixx2-Iyy2+Izz2)/2, Iyz2,0; Ixz2, Iyz2,(Ixx2+Iyy2-Izz2)/2,0;0,0,0,m2];
J3=[(-Ixx3+Iyy3+Izz3)/2, Ixy3, Ixz3,0; Ixy3,(Ixx3-Iyy3+Izz3)/2, Iyz3,0; Ixz3, Iyz3,(Ixx3+Iyy3-Izz3)/2,0;0,0,0,m3];
J4=[(-Ixx4+Iyy4+Izz4)/2, Ixy4, Ixz4,0; Ixy4,(Ixx4-Iyy4+Izz4)/2, Iyz4,0; Ixz4, Iyz4,(Ixx4+Iyy4-Izz4)/2,0;0,0,0,m4];
J5=[(-Ixx5+Iyy5+Izz5)/2, Ixy5, Ixz5,0; Ixy5,(Ixx5-Iyy5+Izz5)/2, Iyz5,0; Ixz5, Iyz5,(Ixx5+Iyy5-Izz5)/2,0;0,0,0,m5];
%ma tran bien doi thuan nhat
%he phuong trinh vi phan chuyen dong
F1=trace(diff(T01,q1)*J1*transpose(diff(T01,q1)))*qdd1+ trace(diff(T02,q1)*J2*transpose(diff(T02,q1)))*qdd1+ trace(diff(T02,q2)*J2*transpose(diff(T02,q1)))*qdd2+ trace(diff(T03,q1)*J3*transpose(diff(T03,q1)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q1)))*qdd2+ trace(diff(T03,q3)*J3*transpose(diff(T03,q1)))*qdd3+ trace(diff(T04,q1)*J4*transpose(diff(T04,q1)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q1)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q1)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q1)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q1)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q1)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q1)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q1)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q1)))*qdd5+ trace(diff(diff(T01,q1),q1)*J1*transpose(diff(T01,q1)))*qd1*qd1+ trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q1)))*qd1*qd1+ trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q1)))*qd2*qd2+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q1)))*qd1*qd1+ trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q1)))*qd2*qd2+ trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q1)))*qd3*qd3+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q1)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q1)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q1)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q1)))*qd4*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q1)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q1)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q1)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q1)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q1)))*qd5*qd5+ 2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q1)))*qd1*qd2+
2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q1)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q1)))*qd1*qd3+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q1)))*qd2*qd3+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q1)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q1)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q1)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q1)))*qd2*qd3- 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q1)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q1)))*qd3*qd4+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q1)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q1)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q1)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q1)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q1)))*qd2*qd3- 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q1)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q1)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q1)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q1)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q1)))*qd4*qd5+ m1*transpose(Vg)*diff(T01,q1)*rRG1- m2*transpose(Vg)*diff(T02,q1)*rRG2- m3*transpose(Vg)*diff(T03,q1)*rRG3- m4*transpose(Vg)*diff(T04,q1)*rRG4- m5*transpose(Vg)*diff(T05,q1)*rRG5;
F2=trace(diff(T02,q1)*J2*transpose(diff(T02,q2)))*qdd1+ trace(diff(T02,q2)*J2*transpose(diff(T02,q2)))*qdd2+ trace(diff(T03,q1)*J3*transpose(diff(T03,q2)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q2)))*qdd2+ trace(diff(T03,q3)*J3*transpose(diff(T03,q2)))*qdd3+ trace(diff(T04,q1)*J4*transpose(diff(T04,q2)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q2)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q2)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q2)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q2)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q2)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q2)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q2)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q2)))*qdd5+ trace(diff(diff(T02,q1),q1)*J2*transpose(diff(T02,q2)))*qd1*qd1+ trace(diff(diff(T02,q2),q2)*J2*transpose(diff(T02,q2)))*qd2*qd2+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q2)))*qd1*qd1+ trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q2)))*qd2*qd2+ trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q2)))*qd3*qd3+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q2)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q2)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q2)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q2)))*qd4*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q2)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q2)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q2)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q2)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q2)))*qd5*qd5+ 2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q2)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q2)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q2)))*qd1*qd3+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q2)))*qd2*qd3+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q2)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q2)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q2)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q2)))*qd2*qd3+
2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q2)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q2)))*qd3*qd4- 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q2)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q2)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q2)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q2)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q2)))*qd2*qd3- 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q2)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q2)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q2)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q2)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q2)))*qd4*qd5+ m2*transpose(Vg)*diff(T02,q2)*rRG2- m3*transpose(Vg)*diff(T03,q2)*rRG3- m4*transpose(Vg)*diff(T04,q2)*rRG4- m5*transpose(Vg)*diff(T05,q2)*rRG5;
F3=trace(diff(T03,q1)*J3*transpose(diff(T03,q3)))*qdd1+ trace(diff(T03,q2)*J3*transpose(diff(T03,q3)))*qdd2+ trace(diff(T03,q3)*J3*transpose(diff(T03,q3)))*qdd3+ trace(diff(T04,q1)*J4*transpose(diff(T04,q3)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q3)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q3)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q3)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q3)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q3)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q3)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q3)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q3)))*qdd5+ trace(diff(diff(T03,q1),q1)*J3*transpose(diff(T03,q3)))*qd1*qd1+ trace(diff(diff(T03,q2),q2)*J3*transpose(diff(T03,q3)))*qd2*qd2+ trace(diff(diff(T03,q3),q3)*J3*transpose(diff(T03,q3)))*qd3*qd3+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q3)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q3)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q3)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q3)))*qd4*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q3)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q3)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q3)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q3)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q3)))*qd5*qd5+ 2*trace(diff(diff(T03,q1),q2)*J3*transpose(diff(T03,q3)))*qd1*qd2+ 2*trace(diff(diff(T03,q1),q3)*J3*transpose(diff(T03,q3)))*qd1*qd3+ 2*trace(diff(diff(T03,q2),q3)*J3*transpose(diff(T03,q3)))*qd2*qd3+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q3)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q3)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q3)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q3)))*qd2*qd3+ 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q3)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q3)))*qd3*qd4- 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q3)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q3)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q3)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q3)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q3)))*qd2*qd3- 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q3)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q3)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q3)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q3)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q3)))*qd4*qd5+ m3*transpose(Vg)*diff(T03,q3)*rRG3- m4*transpose(Vg)*diff(T04,q3)*rRG4- m5*transpose(Vg)*diff(T05,q3)*rRG5;
F4=trace(diff(T04,q1)*J4*transpose(diff(T04,q4)))*qdd1+ trace(diff(T04,q2)*J4*transpose(diff(T04,q4)))*qdd2+ trace(diff(T04,q3)*J4*transpose(diff(T04,q4)))*qdd3+ trace(diff(T04,q4)*J4*transpose(diff(T04,q4)))*qdd4+ trace(diff(T05,q1)*J5*transpose(diff(T05,q4)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q4)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q4)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q4)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q4)))*qdd5+ trace(diff(diff(T04,q1),q1)*J4*transpose(diff(T04,q4)))*qd1*qd1+ trace(diff(diff(T04,q2),q2)*J4*transpose(diff(T04,q4)))*qd2*qd2+ trace(diff(diff(T04,q3),q3)*J4*transpose(diff(T04,q4)))*qd3*qd3+ trace(diff(diff(T04,q4),q4)*J4*transpose(diff(T04,q4)))*qd4*qd4+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q4)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q4)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q4)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q4)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q4)))*qd5*qd5+ 2*trace(diff(diff(T04,q1),q2)*J4*transpose(diff(T04,q4)))*qd1*qd2+ 2*trace(diff(diff(T04,q1),q3)*J4*transpose(diff(T04,q4)))*qd1*qd3+ 2*trace(diff(diff(T04,q1),q4)*J4*transpose(diff(T04,q4)))*qd1*qd4+ 2*trace(diff(diff(T04,q2),q3)*J4*transpose(diff(T04,q4)))*qd2*qd3+ 2*trace(diff(diff(T04,q2),q4)*J4*transpose(diff(T04,q4)))*qd2*qd4+ 2*trace(diff(diff(T04,q3),q4)*J4*transpose(diff(T04,q4)))*qd3*qd4- 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q4)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q4)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q4)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q4)))*qd1*qd5+
2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q4)))*qd2*qd3- 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q4)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q4)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q4)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q4)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q4)))*qd4*qd5+ m4*transpose(Vg)*diff(T04,q4)*rRG4- m5*transpose(Vg)*diff(T05,q4)*rRG5;
F5=trace(diff(T05,q1)*J5*transpose(diff(T05,q5)))*qdd1+ trace(diff(T05,q2)*J5*transpose(diff(T05,q5)))*qdd2+ trace(diff(T05,q3)*J5*transpose(diff(T05,q5)))*qdd3+ trace(diff(T05,q4)*J5*transpose(diff(T05,q5)))*qdd4+ trace(diff(T05,q5)*J5*transpose(diff(T05,q5)))*qdd5+ trace(diff(diff(T05,q1),q1)*J5*transpose(diff(T05,q5)))*qd1*qd1+ trace(diff(diff(T05,q2),q2)*J5*transpose(diff(T05,q5)))*qd2*qd2+ trace(diff(diff(T05,q3),q3)*J5*transpose(diff(T05,q5)))*qd3*qd3+ trace(diff(diff(T05,q4),q4)*J5*transpose(diff(T05,q5)))*qd4*qd4+ trace(diff(diff(T05,q5),q5)*J5*transpose(diff(T05,q5)))*qd5*qd5+ 2*trace(diff(diff(T05,q1),q2)*J5*transpose(diff(T05,q5)))*qd1*qd2+ 2*trace(diff(diff(T05,q1),q3)*J5*transpose(diff(T05,q5)))*qd1*qd3+ 2*trace(diff(diff(T05,q1),q4)*J5*transpose(diff(T05,q5)))*qd1*qd4+ 2*trace(diff(diff(T05,q1),q5)*J5*transpose(diff(T05,q5)))*qd1*qd5+ 2*trace(diff(diff(T05,q2),q3)*J5*transpose(diff(T05,q5)))*qd2*qd3- 2*trace(diff(diff(T05,q2),q4)*J5*transpose(diff(T05,q5)))*qd2*qd4+ 2*trace(diff(diff(T05,q2),q5)*J5*transpose(diff(T05,q5)))*qd2*qd5+ 2*trace(diff(diff(T05,q3),q4)*J5*transpose(diff(T05,q5)))*qd3*qd4+ 2*trace(diff(diff(T05,q3),q5)*J5*transpose(diff(T05,q5)))*qd3*qd5+ 2*trace(diff(diff(T05,q4),q5)*J5*transpose(diff(T05,q5)))*qd4*qd5+ m5*transpose(Vg)*diff(T05,q4)*rRG5;