Báo Cáo ĐKRB 1

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI
KHOA CƠ KHÍ
BỘ MÔN CƠ ĐIỆN TỬ
--------🙟🕮🙝--------
ĐIỀU KHIỂN ROBOT

Sinh viên thực hiện : Mã sinh viên :
1. Ngô Ngọc Linh 191300609
2. Bùi Xuân Bách 191340111
3. Hoàng Văn Long 191302529
4. Nguyễn Tiến Mạnh 191300673
Giảng viên hướng dẫn : PGS. Đinh Thị Thanh Huyền
Lớp : Cơ điện tử 2 K60 Khoa : Cơ khí
HÀ NỘI, 5/2023
THIẾT KẾ MÔ PHỎNG ĐIỀU KHIỂN ROBOT
ABB IRB 120
Contents
LỜI MỞ ĐẦU.................................................................................................................................2
Chương 1. Tổng quan về robot ABB IRB 120.............................................................................4
1.1.Tổng quan về robot ABB IRB 120.....................................................................................4
1.1.1.Tổng quan chung về robot ABB IRB 120...............................................................................4
1.1.2.Thông số kĩ thuật.....................................................................................................................5
Chương 2 : Tính toán động hoc.....................................................................................................9
2.1.Lập bảng DH và tính toán các ma trận..................................................................................9
Chương 3: Chọn động cơ.............................................................................................................78
3.1.Thông số động cơ...............................................................................................................78
3.1.1.Thông số động cơ Khâu 1: 130ST-M15025.................................................................79
Chương 4: Thiết kế bộ điều khiển...............................................................................................85
4.1.Thiết kế bộ điều khiển PD và PID....................................................................................85
4.2.Thiết kế bộ điều khiển PD...............................................................................................100
4.3 Thiết kế bộ điều khiển PID.............................................................................................104
4.4.Thiết kế bộ điều khiển PI................................................................................................108
4.5.Thiết kế bộ điều khiển PD bão hòa................................................................................112
4.6.Thiết kế bộ điều khiển PID bão hòa...............................................................................117
4.7.Thiết kế bộ điều khiển tiếp thuận...................................................................................122
4.8.Thiết kế bộ điều khiển Điểm Cực...................................................................................126
Bộ điều khiển gán điểm cực:............................................................................................134
KẾT LUẬN CHUNG.................................................................................................................141
NHẬN XÉT, ĐÁNH GIÁ CỦA GIẢNG VIÊN........................................................................142
TÀI LIỆU THAM KHẢO..........................................................................................................143
2
LỜI MỞ ĐẦU
Quãng thời gian bốn năm đại học là sinh viên của Đại học Giao thông vận tải
bản thân em đã tích lũy cho bản thân mình nhiều kiến thức. Tuy thời gian làm bài
tập ngắn ngủi nhưng là khoảng thời gian giúp bản thân em học hỏi thêm nhiều kiến
thức cũng như việc được tổng hợp lại các kiến thức, nâng cao kiến thức chuyên
môn.
Trong quá trình làm bài tập từ việc thiếu nhiều kinh nghiệm và gặp phải những
khó khăn nhất định nhưng được sự quan tâm, chỉ dạy, giúp đỡ của cô Đinh Thị
Thanh Huyền , em đã rút ra những thiếu sót của bản thân. Cuối cùng chúng em xin
gửi lời cảm ơn chân thành tới cô đã luôn hỗ trợ giúp đỡ em trong suốt khoảng thời
gian vừa qua.
Em xin chân thành cảm ơn !.
Hà Nội , ngày 5 tháng 2 năm 2023

Nhóm sinh viên thực hiện
Nhóm 5
3
Chương 1. Tổng quan về robot ABB IRB 120
1.1.Tổng quan về robot ABB IRB 120
1.1.1.Tổng quan chung về robot ABB IRB 120

Là loại robot nhỏ nhất của ABB, IRB 120 có đầy đủ tất cả các tính năng và rất thuần
thục trong các loại robot của ABB ứng dụng trong các nghành công nghiệp đóng gói nhỏ.
Trọng lượng được rút gọn chỉ có 25kg và thiết kế nhỏ gọn giúp cho robot này có thể lắp
ráp tại bất cứ đâu, ở trong một trạm làm việc hay trên một máy móc hay cạnh các robot
khác trong một dây chuyền sản xuất.
IRB 120 được ứng dụng trong phạm vi rộng trong các nghành công nghiệp bao gồm
điện tử, thực phẩm và đồ uống, máy móc, năng lượng mặt trời, nghành dược, y tế và lĩnh
vực nghiên cứu. Đây là thế hệ thứ 4 của công nghệ robot mới của ABB. Loại robot 6 trục
có khả năng tải lên tới 3kg (4 kg khi cổ tay cúi xuống) và tầm với 580mm, robot này có
thể thực hiện nhiều hoạt động khác nhau linh hoạt hơn các giải pháp tự động hoá cố định.
IRB 120 là hệ thống được xây dựng hoàn hảo để thiết kế cho các ứng dụng tiết kiệm chi
phí, ứng dụng đặc biệt trong không gian tối thiểu.
4
Hình 1.1: Mô hình cánh tay robot
1.1.2.Thông số kĩ thuật
 Tải trọng tối đa: 3kg
 Tầm với: 580mm
 Số trục: loại robot 6 trục nhanh nhất của ABB
 Sai số vị trí: +/- 0.01mm
 Trọng lượng: 25kg
 Nhiệt độ làm việc 50℃ tới 450℃
 Độ ồn: <70dB
 Tiêu chuẩn chống nước: IP30
5
 Có thể gá đặt nhiều góc độ khác nhau
 Lập trình: lập trình trên màn hình cảm ứng được kết nối với bộ điều khiển
Hình 1.2: Kích thước robot ABB IRB 120

Trong đó:
 RA = 121mm: bán kính quay tối thiểu của trục 1
 RB = 147mm: bán kính quay tối thiểu của trục 2
 RC = 70mm: bán kính quay tối thiểu của trục 3
Chuyển động của robot:
 Tốc độ tối đa: 6.2m/s
 Gia tốc tối đa: 28m/s2
6
Hình 1.3: Phạm vi chuyển động
7
Điểm Toạ độ điểm thức theo trục Góc xoay
Trục X Trục Z Trục 2 Trục 3
A 302 mm 630 mm 0° 0°
B 0 mm 870 mm 0° -77°
C 169 mm 300 mm 0° +70°
D 580 mm 270 mm +90° -77°
E 545 mm 91 mm +110° -77°
F -440 mm -50 mm -110° -110°
G -67 mm 445 mm -110° +70°
H -580 mm 270 mm -90° -77°
J -545 mm 91 mm -110° -77°
Bảng 1.1: Thông số phạm vi chuyển động
Trục di chuyển Chuyển động trục Giới hạn góc xoay Tốc đa trục tối đa
Trục 1 Xoay tròn +165° tới -165° 250° /s
Trục 2 Cánh tay +110° tới -110° 250° /s
Trục 3 Cánh tay +70° tới -90° 250° /s
Trục 4 Cổ tay +160° tới -160° 320° /s
Trục 5 Uốn +120° tới -120° 320° /s
Trục 6 Đổi hướng +400° tới -400° 420° /s
Bảng 1.2: Góc quay của robot
8
Chương 2 : Tính toán động hoc
2.1.Lập bảng DH và tính toán các ma trận
Hình 2.1: Hệ trục toạ độ trên cánh tay robot
Join α i [° ] ai [mm] θ i [° ] di [mm]

1 0 0 q1 L1
2 - π /2 0 q2 L2
3 0 L3 q3- π /2 0
4 - π /2 70 q4 L5
5 π /2 0 q5 L6
6 - π /2 0 q6 L7
Bảng 2.1: Bảng DH
9
Các giá trị kích thước: (mm)
L1 = 165; L2 = 125; L3 = 270; L4 = 70;
L5 = 134;L6 = 168;L7 = 72;
Các ma trận biến đổi thuần nhất
[ ] [ ]
c θ1 −s θ 1 0 0 c θ 2 −s θ2 0 0
0 s θ1 c θ1 0 0 1 0 0 1 L2
1T = ; 2T =
0 0 1 L1 −s θ2 −c θ 2 0 0
0 0 0 1 0 0 0 1
2
3 T = Tran(0,L3,0).Rot(Z,(θ3 −90 ° ¿ )
[ ][ ]
1 0 0 0 c ( θ 3−90 ) −s ( θ3 −90 ) 0 0
0 1 0 L3 s ( θ3−90 ) c ( θ 3−90 ) 0 0
=
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
[ ]
c (θ3 −90) −s ⁡(θ3−90) 0 0
s ⁡(θ3 −90) c ⁡(θ3 −90) 0 L3
= ;
0 0 1 0
0 0 0 1
3
4 T =¿Tran(0,L5,0).Rot(X,(−90 ° ¿)
[ ][ ][ ]
1 0 0 0 1 0 0 0 1 0 0 0
0 c (−90) −s (−90) 0 0 c(−90) −s(−90) L5
¿ 0 1 0 L5 = =
0 0 1 0 0 s(−90) c (−90) 0 0 s (−90) c(−90) 0
0 0 0 1 0 0 0 1 0 0 0 1
[ ]
1 0 0 0
0 0 1 L5
0 −1 0 0
;
0 0 0 1
10
[ ]
c θ 5 −s θ5 0 0
4 0 0 −1 −L 6
5T = ;
s θ5 c θ5 1 0
0 0 0 1
6
7 T = Tran(0,L7,0).Rot(X,(−90 ° ¿)
[ ][ ][ ]
1 0 0 0 1 0 0 0 1 0 0 0
0 1 0 L7 0 c (−90) −s(−90) 0 0 c (−90) −s (−90) L 7
= =
0 0 1 0 0 s(−90) c (−90) 0 0 s(−90) c (−90) 0
0 0 0 1 0 0 0 1 0 0 0 1
[ ]
1 0 0 0
0 0 1 L7
= ;
0 −1 0 0
0 0 0 1
Các thông số Mass properties của các khâu

* Khâu 1 :
Hình 2.2: Mass properties khâu 1

11
m1=3.0670; Ixx1=0.0250; Iyy1=0.0252; Izz1=0.0105; Ixy1=-0.000; Ixz1=0.0000;
Iyz1=0.000; dx1 =0; dy1=0; dz1=0
rRG1=[-0.0001;0.0000;0.0594;1]
[ ]
−I xx 1 + I yy 1+ I zz 1 2
+m1 d x1 I xy 1+ m1 d x 1 d y1 I xz 1 +m1 d x 1 d z 1 −m1 d x1
2
I xx 1−I yy 1 + I zz 1
J 1= I xy 2 +m1 d x1 d y 1 +m1 d 2 y1 I yz 1 +m1 d y1 d z 1 −m1 d y 1
2
I xx 1 + I yy 1−I zz 1 2
I xz 1 +m1 d x 1 d z 1 I yz 1 +m1 d y 1 d z 1 + m1 d z 1 −m1 d z 1
2
−m1 d x 1 −m1 d y 1 −m1 d z 1 m1
*Khâu 2
12
m2=3.9086; Ixx2=0.1004; Iyy2=0.02597; Izz2=0.0816; Ixy2=0.0004;
Ixz2=0.0000; Iyz2=-0.0012
dx2 =0; dy2=0; dz2=0
rRG2=[-0.0008;-0.1012;0.0019;1]
[ ]
−I xx 2 + I yy 2 + I zz 2
+m2 d 2x 2 I xy 2+ m2 d x2 d y 2 I xz 2+ m2 d x2 d z 2 −m2 d x 2
2
I xx2 −I yy 2 + I zz 2
J 2= I xy 2 +m2 d x 2 d y 2 + m2 d 2 y 2 I yz 2+ m2 d y 2 d z 2 −m2 d y 2
2
I xx 2+ I yy 2−I zz 2
I xz2 +m2 d x 2 d z 2 I yz2 +m2 d y2 d z 2 +m2 d 2 z 2 −m2 d z 2
2
−m2 d x 2 −m2 d y 2 −m2 d z 2 m2
*Khâu 3
13
m3=2.9437; Ixx3=0.0142; Iyy3=0.0182; Izz3=0.0281; Ixy3=0.0053; Ixz3=-0.0000;
Iyz3=-0.0000
dx3=0; dy3=0; dz3=0
rRG3=[0.0579;0.0228;0.0011;1]
[ ]
−I xx 3 + I yy 3 + I zz 3 2
+ m3 d x 3 I xy 3 +m3 d x 3 d y 3 I xz 3 +m3 d x 3 d z 3 −m3 d x 3
2
I xx 3−I yy 3 + I zz 3 2
J 3= I xy 3 +m3 d x 3 d y3 +m3 d y 3 I yz 3 +m3 d y3 d z 3 −m3 d y3
2
I xx 3+ I yy 3−I zz3 2
I xz 3+ m3 d x3 d z 3 I yz 3+ m3 d y 3 d z 3 m3 d z 3 −m3 d z 3
2
−m3 d x3 −m3 d y 3 −m3 d z 3 m3
14
*Khâu 4 :
m4=1.4476; Ixx4=0.0145; Iyy4=0.0129; Izz4=0.0033; Ixy4=-0.000; Ixz4=0.0003;

Iyz4=0.0004
dx4=0; dy4=0; dz4=0
rRG4=[0.0023;0.0026;0.0767;1]
15
[ ]
−I xx 4 + I yy 4 + I zz 4 2
+ m4 d x 4 I xy 4 +m 4 d x 4 d y4 I xz 4 +m4 d x 4 d z 4 −m4 d x 4
2
I xx 4 −I yy 4 + I zz 4 2
J4 = I xy 4 + m4 d x 4 d y 4 + m4 d y 4 I yz 4 +m4 d y 4 d z 4 −m4 d y 4
2
I xx 4 + I yy 4 −I zz 4
I xz 4 +m 4 d x 4 d z 4 I yz 4 +m 4 d y4 d z 4 m4 d2 z 4 −m4 d z 4
2
−m 4 d x 4 −m 4 d y 4 −m4 d z 4 m4
*Khâu 5 :

Iyz5=-0.0000
dx5=0; dy5=0; dz5=0
rRG5=[0.0001;-0.0011;0.0001;1]
16
[ ]
−I xx 5 + I yy 5 + I zz 5 2
+ m5 d x 5 I xy 5 +m5 d x 5 d y 5 I xz 5 +m5 d x 5 d z 5 −m5 d x 5
2
I xx 5−I yy 5 + I zz 5
J 5= I xy 5 +m5 d x 5 d y5 +m5 d 2 y 5 I yz 5 +m5 d y5 d z 5 −m5 d y5
2
I xx 5+ I yy 5−I zz5 2
I xz 5+ m5 d x5 d z 5 I yz 5+ m5 d y 5 d z 5 m5 d z 5 −m5 d z 5
2
−m5 d x5 −m5 d y 5 −m5 d z 5 m5
*Khâu 6:

Iyz6=-0.0000;
dx6=0; dy6=0; dz6=0
rRG6=[0.0002;0.0000;-0.0012;1]
17
[ ]
−I xx 6+ I yy 6 + I zz 6
+m6 d 2x 6 I xy 6 +m6 d x 6 d y 6 I xz 6 +m6 d x 6 d z 6 −m6 d x 6
2
I xx 6−I yy 6 + I zz 6 2
J 6= I xy 6 + m6 d x6 d y 6 +m6 d y 6 I yz 6 +m6 d y 6 d z 6 −m6 d y 6
2
I xx 6 + I yy 6−I zz 6 2
I xz 6 +m6 d x 6 d z 6 I yz 6 + m6 d y 6 d z 6 m5 d z 5 −m6 d z 6
2
−m6 d x 6 −m6 d y 6 −m6 d z 6 m6
*Áp dụng phương trình động lực học cho Robot và khai triển chi tiết ta được:
Khâu 1:
F 1=Tr
[ ∂T 1 ∂ T 1T
∂q 1
J1
∂ q1 ] [
q¨1+Tr
∂ T 2 ∂ T 2T
∂ q1
J2
∂ q1
q̈ 1+Tr
] [
∂ T 2 ∂T 2T
J q̈
∂ q2 2 ∂ q1 2 ]
[ ] [ ] [ ]
T T T
∂ T 3 ∂T 3 ∂ T 3 ∂T 3 ∂ T3 ∂ T3
+Tr J3 q̈1 +Tr J3 q̈ 2+Tr J q¨
∂ q1 ∂ q1 ∂ q2 ∂ q1 ∂ q3 3 ∂ q 1 3
[ ] [ ] [ ] [ ]
T T T T
∂ T 4 ∂T 4 ∂T4 ∂T4 ∂ T 4 ∂ T4 ∂T 4 ∂T 4
+Tr J4 q̈1 +Tr J4 q̈ 2+Tr J4 q̈3 +Tr J q¨
∂ q1 ∂ q1 ∂ q2 ∂ q1 ∂ q3 ∂ q1 ∂ q4 4 ∂ q1 4
+Tr
[
∂ T 5 ∂T 5T
J q̈ +Tr
∂ q1 5 ∂ q1 1
J
] [
∂ T 5 ∂T 5T
q̈ +Tr
∂ q2 5 ∂ q1 2
∂ T 5 ∂ T 5T
J
] [
q¨ +Tr
∂ q3 5 ∂ q 1 3
∂ T 5 ∂ T 5T
J
∂ q4 5 ∂ q1 4
q¨
] [ ]
[ ] [ ] [ ] [ ]
T T T ¨ T
∂ T 5 ∂T 5 ∂ T6 ∂ T 6 ∂ T 6 ∂T 6 ∂ T6 ∂ T6
+Tr J5 q̈5 +Tr J6 q1 +¿ Tr J6 q̈2 +Tr J6 q¨ ¿
∂ q5 ∂ q1 ∂ q1 ∂ q1 ∂q 2 ∂ q1 ∂ q3 ∂ q1 3
+Tr
[
∂ T 6 ∂ T 6T
J q¨ +Tr
∂ q4 6 ∂ q 1 4 ] [
∂ T 6 ∂ T 6T
J q¨ +Tr
∂ q5 6 ∂ q 1 5
∂ T 6 ∂T 6T
J q̈
∂q 5 6 ∂ q1 6 ] [ ]
[
∂ q1
∂2 T 1
+Tr 2 J 1
∂ T 1T 2
q̇ + Tr 2 J 2
∂q 1 1 ∂ q1 ] [
∂2 T 2 ∂ T 2 T 2
∂ q1 1
q̇ +Tr 2 J 2
∂ q2 ∂q 1 2] [
∂2 T 2 ∂ T 2 T 2
q̇
]
18
+2 Tr
[ ∂2 T 2
∂q 1 ∂q 2
J2
∂ T 2T
∂ q1 ] ∂2 T
q˙1 q̇2 +Tr 2 3 J 3
∂ q1 [
∂ T 3T 2
∂ q1
∂2 T
∂ q2 ] [
∂T T 2
q˙1 + Tr 2 3 J 3 3 q˙2
∂ q1 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T3
∂T 3 ∂ T3 ∂T3 ∂ T3 ∂T
+Tr 2 J 3 q̇32 +2 Tr J3 q˙1 q̇2 +2Tr J 3 3 q˙1 q˙3
∂ q3 ∂ q1 ∂ q1 ∂ q2 ∂ q1 ∂ q 1 ∂ q3 ∂ q1
[ ] [] [ ]
2 T 2 T 2 T
∂ T3 ∂T3 ∂T ∂T 4 ∂ T ∂T 4
+2 Tr J3 q̇2 q̇3 +Tr 2 4 J 4 q˙12+Tr 2 4 J 4 q̇22
∂q 2 ∂ q3 ∂ q1 ∂ q1 ∂ q1 ∂ q2 ∂ q1
+Tr
[ ∂2 T 4
∂ q3
2
J4
∂T 4T 2
∂ q1 ] ∂2 T
∂ q4 [
q˙3 +Tr 2 4 J 4
∂ q1] [
∂ T4T 2
q̇4 +2 Tr
]∂2 T 4
∂ q1 ∂ q2
J4
∂ T 4T
q̇ q̇
∂ q1 1 2
+2 Tr
[
∂2T 4
J
∂ T4T
q̇ q̇ +2 Tr
∂q 1 ∂q 3 4 ∂ q 1 1 3 ]
∂2 T 4
J
[
∂ T 4T
] [
q̇ q˙ +2 Tr
∂ q 1 ∂ q 4 4 ∂ q1 1 4 ]
∂2 T 4
J
∂T 4T
q˙ q̇
∂ q2∂ q3 4 ∂ q1 2 3
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T4 ∂ T4 ∂T4 ∂T ∂T 5 2
+2 Tr J4 q˙2 q˙4 +2 Tr J4 q˙3 q˙4 +Tr 2 5 J 5 q̇1
∂q 2 ∂ q4 ∂ q1 ∂q 3 ∂ q4 ∂ q1 ∂ q1 ∂ q1
+Tr
[ ∂2 T 5
∂ q2
2
J5
∂T 5T 2
∂ q1 ] ∂2 T
[
q̇2 +Tr 2 5 J 5
∂ q3 ] [
∂ T 5T 2
∂ q1 ]∂2 T
q̇3 +Tr 2 5 J 5
∂ q4
∂ T 5T 2
q˙
∂ q1 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5
∂T 5 2 ∂ T5 ∂T5 ∂ T5 ∂T
+Tr 2 J 5 q̇5 +2 Tr J5 q˙1 q̇2 +2Tr J 5 5 q˙1 q˙3
∂ q5 ∂ q1 ∂ q1 ∂ q2 ∂ q1 ∂ q 1 ∂ q3 ∂ q1
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T ∂ T5 ∂T5 ∂ T5 ∂T5
+2 Tr J 5 5 q˙1 q˙4 +2 Tr J5 q̇1 q̇5 +2 Tr J5 q̇ q̇
∂q 1 ∂q 4 ∂ q1 ∂ q1∂ q5 ∂q 1 ∂ q2∂ q3 ∂q 1 2 3
+2 Tr
[
∂2 T 5
J
∂ T 5T
q˙ q˙ +2 Tr
∂q 2 ∂ q4 5 ∂ q 1 2 4 ]
∂2 T 5
J
[
∂ T 5T
] [
q̇ q̇ +2 Tr
∂q 2 ∂q 5 5 ∂ q1 2 5
∂2 T 5
] J
∂ T 5T
q˙ q˙
∂q 3 ∂ q4 5 ∂ q 1 3 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T 5 ∂ T5 ∂T ∂ T ∂T 2
+2 Tr J5 q̇3 q̇5 +2 Tr J 5 5 q˙4 q˙5+Tr 2 6 J 6 6 q˙1
∂q 3 ∂ q5 ∂ q1 ∂ q4 ∂ q 5 ∂ q1 ∂ q1 ∂ q1
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 2 ∂ T ∂T 2 ∂ T ∂T 2
+Tr J6 q˙2 +Tr 2 6 J 6 6 q˙3 +Tr 2 6 J 6 6 q̇4
∂2 q2 ∂ q1 ∂ q3 ∂ q 1 ∂ q4 ∂ q 1
[
+Tr 2 J 6
∂ q5
∂2 T 6
∂T 6T 2
q˙ +Tr 2 J 6
∂ q1 5 ∂ q6 ]
∂2 T 6 ∂ T 6 T 2
∂ q1 6
q˙ + 2Tr
[ ] [
∂2 T 6
J
∂ T 6T
]
q˙ q˙
∂ q1 ∂ q2 6 ∂ q 1 1 2
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 3
J6
∂T 6T
∂ q1 ]
q̇1 q̇3 +2 Tr
[
∂2 T 6
∂ q1 ∂ q4] [
J6
∂ T 6T
∂ q1 ]
q̇1 q̇4 +2 Tr
∂2 T 6
∂ q1 ∂ q5
J6
∂T 6T
q˙ q˙
∂ q1 1 5
19
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 6
J6
∂T 6T
∂ q1
q̇1 q˙6+ 2Tr
]
∂2 T 6
∂ q2 ∂ q3
J6
∂ T 6T
∂ q1 [
q˙2 q˙3 +2Tr
∂2T 6
∂ q2∂ q4
J6
]
∂T 6T
q̇ q˙
∂ q1 2 4 [ ]
+2 Tr
[ ∂2 T 6
∂q 2 ∂ q5
J6
∂T 6T
∂ q1
q̇2 q̇ 5+2 Tr
]
∂2 T 6
∂ q2 ∂ q6
J6
∂ T 6T
∂ q1 [
q˙2 q˙6 +2Tr
∂2 T 6
∂ q3∂ q4
J6
]
∂T 6T
q˙ q˙
∂ q1 3 4 [ ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 ∂ T6 ∂ T6 ∂ T6 ∂ T6
+2 Tr J6 q̇3 q˙5+ 2Tr J6 q̇3 q̇6 +2 Tr J6 q˙ q˙
∂q 3 ∂ q5 ∂ q1 ∂ q3 ∂ q6 ∂ q1 ∂q 4 ∂ q5 ∂ q1 4 5
[ ] [ ]
2 T 2 T
∂ T6 ∂T6 ∂ T6 ∂T6 T ∂ T1 1
+2 Tr J6 q̇4 q˙6 +2Tr J6 q̇5 q̇6 −m 1 ⃗g r
⃗
∂q 4 ∂ q 6 ∂q 1 ∂ q5 ∂ q 6 ∂ q1 ∂ q1 G 1
∂T2 2 ∂T 3 3 ∂T 4 4 ∂T5 5 ∂ T6 6
−m2 ⃗g T ⃗r G −m3 ⃗g T r G −m4 ⃗g T
⃗ r G −m5 ⃗g T
⃗ r G −m6 ⃗gT
⃗ ⃗
r
∂ q1 2
∂ q1 ∂ q1 3
∂ q1 4
∂ q1 G 5 6
Khâu 2:
F 2=Tr
[ ∂T 2 ∂ T 2T
∂q 1
J2
∂ q2
q¨1+ Tr
∂ q2 ] [
∂ T 2 ∂ T 2T
J2
∂ q2
q¨2+Tr
∂ T 3 ∂ T 3T
∂ q1
J3
∂ q2] [
q¨1+ Tr
∂ T 3 ∂ T 3T
J
∂ q2 3 ∂ q2 2
q¨
] [ ]
[ ] [ ] [ ] [ ]
T T T T
∂ T 3 ∂T 3 ∂T4 ∂T4 ∂T 4 ∂ T 4 ∂T4 ∂T4
+Tr J3 q̈3 +Tr J4 q̈1 +Tr J4 q¨2+Tr J q̈
∂ q3 ∂ q2 ∂ q1 ∂ q2 ∂ q2 ∂ q2 ∂ q 3 4 ∂q 2 3
[ ] [ ] [ ] [ ]
¨
∂ T 4 ∂T 4T ∂ T 5 ∂ T 5T ∂ T 5 ∂ T 5T ∂ T 5 ∂ T 5T
+Tr J4 q̈4 +Tr J5 q 1+¿ Tr J5 q¨2+Tr J q¨ ¿
∂ q4 ∂ q2 ∂ q1 ∂ q2 ∂ q2 ∂ q2 ∂ q3 5 ∂ q 2 3
[ ] [ ] [ ] [ ]
¨
∂ T 5 ∂T 5T ∂T 5 ∂ T 5T ∂T 6 ∂T 6T ∂ T 6 ∂ T 6T
+Tr J q¨ +Tr J q̈ +Tr J q + ¿Tr J q̈ ¿
∂ q4 5 ∂ q2 4 ∂q 5 5 ∂ q2 5 ∂ q1 6 ∂ q2 1 ∂ q2 6 ∂ q2 2
+Tr
[
∂ T 6 ∂ T 6T
J q¨ +Tr
∂ q3 6 ∂ q 2 3
∂ T 6 ∂T 6T
J
]
q¨ +Tr
∂ q 4 6 ∂ q2 4
∂ T 6 ∂T 6T
J
[
q̈ +Tr
∂q 5 6 ∂ q2 5
∂ T 6 ∂ T 6T
J q¨
∂ q5 6 ∂ q 2 6 ] [ ] [ ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T2
∂T2 2 ∂ T ∂T 2 ∂ T2 ∂T2
+Tr 2 J 2 q̇1 +Tr 2 2 J 2 2 q˙2 +2 Tr J2 q̇ q̇
∂ q1 ∂ q2 ∂ q2 ∂ q2 ∂ q1 ∂ q2 ∂q 2 1 2
[
+Tr 2 J 3
∂ q1
∂2 T 3
∂T 3T 2 ∂2 T 3 ∂ T 3T 2
q̇ +Tr 2 J 3
∂ q2 1 ∂ q2 ]
∂ 2 T 3 ∂ T 3T 2
q̇ +Tr 2 J 3
∂q 2 2 ∂ q3
q˙
∂ q2 3 [ ] [ ]
+2 Tr
[ ∂2 T 3
∂q 1 ∂q 2
J3
∂ T 3T
∂ q2
q̇1 q̇2 +2 Tr
]
∂2T 3
∂ q1 ∂ q3
∂T T
[
J 3 3 q˙1 q̇3 +2Tr
∂ q2
∂2 T 3
∂ q2∂ q3
∂T T
]
J 3 3 q˙2 q̇3
∂ q2 [ ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T 4 ∂ T ∂T 4 ∂ T ∂T4
+Tr J4 q˙12+Tr 2 4 J 4 q̇22+ Tr 2 4 J 4 q̇32
∂ q1
2
∂ q2 ∂ q2 ∂ q 2 ∂ q3 ∂q 2
20
+Tr
[ ∂2 T 4
∂2 q4
J4
∂T 4T 2
∂ q2 ]
q˙4 +2 Tr
[
∂2 T 4
∂ q1 ∂ q2
J4
∂T 4T
∂ q2 ]
q˙1 q˙2 +2Tr
∂2 T 4
∂ q1 ∂ q3 [
J4
∂ T 4T
q˙ q˙
∂ q2 1 3 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T4 ∂ T4 ∂T 4 ∂ T4 ∂T4
+2 Tr J4 q˙1 q˙4 +2 Tr J4 q̇2 q̇3 +2 Tr J4 q̇ q˙
∂q 1 ∂q 4 ∂ q2 ∂ q2 ∂ q3 ∂ q2 ∂ q2∂ q4 ∂ q2 2 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T 4 ∂ T ∂T 5 2 ∂T ∂T5 2
+2 Tr J4 q˙3 q˙4 +Tr 2 5 J 5 q̇1 +Tr 2 5 J 5 q̇2
∂q 3 ∂ q4 ∂ q2 ∂ q1 ∂ q 2 ∂ q2 ∂ q 2
[
+Tr 2 J 5
∂ q3
∂2 T 5
∂T 5T 2
q̇ +Tr 2 J 5
∂ q2 3 ∂ q4 ]
∂2 T 5 ∂ T 5T 2
q̇ +Tr 2 J 5
∂ q2 4 ∂ q5 [
∂2 T 5 ∂T 5T 2
q̇
∂ q2 5 ] [ ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T5 ∂ T5 ∂ T5 ∂ T5 ∂T 5
+2 Tr J5 q̇1 q̇2 +2 Tr J5 q˙1 q̇3 +2Tr J5 q̇ q˙
∂q 1 ∂q 2 ∂ q2 ∂ q1 ∂ q3 ∂ q2 ∂ q1 ∂ q4 ∂ q2 1 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T5 ∂ T5 ∂T5 ∂ T5 ∂T
+2 Tr J5 q̇1 q̇5 +2 Tr J5 q̇2 q̇3 +2 Tr J 5 5 q̇ 2 q˙4
∂q 1 ∂q 5 ∂ q2 ∂ q 2 ∂q 3 ∂ q2 ∂ q2 ∂ q4 ∂ q2
+2 Tr
[ ∂2 T 5
∂q 2 ∂ q5
J5
∂ T 5T
∂ q2 ]
q̇2 q̇5 +2 Tr
[
∂2 T 5
∂q 3 ∂ q4
J5
]
∂ T 5T
∂ q2
q˙3 q˙4 +2 Tr
∂2 T 5
∂ q3 ∂ q5
J5
[
∂T 5T
q̇ q̇
∂ q2 3 5 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T ∂ T ∂T 2 ∂ T ∂T 2
+2 Tr J 5 5 q˙4 q̇5 +Tr 2 6 J 6 6 q˙1 +Tr 2 6 J 6 6 q˙2
∂q 4 ∂ q5 ∂ q2 ∂ q1 ∂ q2 ∂ q2 ∂ q2
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6
∂T 6 2 ∂ T ∂T 2 ∂T ∂T 6 2
+Tr 2 J 6 q˙3 +Tr 2 6 J 6 6 q˙4 +Tr 2 6 J 6 q̇ 5
∂ q3 ∂ q2 ∂ q4 ∂ q2 ∂ q5 ∂ q2
[
+Tr 2 J 6
∂ q6
∂2 T 6
∂T 6T 2
q˙ + 2Tr
∂ q2 6
∂2 T 6
J
]
∂T 6T
[
q̇ q˙ + 2Tr
∂ q1 ∂ q2 6 ∂ q2 1 2 ] [
∂2 T 6
J
∂T 6T
q˙ q˙
∂ q1 ∂ q3 6 ∂ q2 1 3 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂ T6 ∂ T6 ∂T 6 ∂ T6 ∂T 6
+2 Tr J6 q˙1 q˙4 +2 Tr J6 q̇1 q˙5+2 Tr J6 q˙ q˙
∂q 1 ∂q 4 ∂ q2 ∂ q1 ∂ q5 ∂ q2 ∂ q1 ∂ q5 ∂ q2 1 6
+2 Tr
[ ∂2 T 5
∂q 2 ∂ q3
J5
∂ T 5T
∂ q2 ]
q̇2 q̇3 +2 Tr
[
∂2 T 5
∂q 2 ∂q 4
J5
]
∂ T 5T
∂ q2
q˙2 q˙4 +2 Tr
∂2 T 5
∂q 2 ∂q 5
J5
[
∂ T 5T
q̇ q̇
∂ q2 2 5 ]
2 Tr
[ ∂2T 6
∂ q1∂ q5
J6
∂ T 6T
∂ q2 ]
q̇2 q̇6 +2 Tr
∂2 T 6
[
∂ q3 ∂ q4
J6
]
∂ T 6T
∂q 2
q̇3 q̇ 4+ 2Tr
∂2 T 6
∂ q3 ∂ q5
J6
[
∂ T 6T
q˙ q̇
∂ q2 3 5 ]
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 5
J6
∂T 6T
∂ q2 ]
q̇3 q˙6+ 2Tr
[
∂2 T 6
∂ q 4 ∂q 5
J6
]
∂T 6T
∂ q2
q˙4 q̇5 +2 Tr
∂2 T 6
∂ q 4 ∂ q6 [
∂T T
J 6 6 q˙4 q̇6
∂ q2 ]
21
+2 Tr
[ ∂2 T 6
∂q 5 ∂ q6
J6
∂T 6T
q˙ q˙
∂ q2 5 6 ]
∂T2 2 ∂T 3 3 ∂T 4 4 ∂T5 5 ∂ T6 6
−m2 ⃗g T ⃗r G −m3 ⃗g T r G −m4 ⃗g T
⃗ r G −m5 ⃗g T
⃗ r G −m6 ⃗gT
⃗ ⃗
r
∂ q2 2
∂ q2 ∂ q2
3 4
∂ q2 ∂ q1 G
5 6
Khâu 3:
[ ] [ ] [ ] [ ]
T T T T
∂ T3 ∂ T3 ∂ T3 ∂ T3 ∂ T 3 ∂ T3 ∂T4 ∂T4
F 3=T r J3 q̈ 1+Tr J3 q¨2+Tr J3 q¨3+ Tr J q̈
∂ q1 ∂ q3 ∂ q2 ∂ q3 ∂ q3 ∂ q3 ∂ q1 4 ∂ q3 1
+Tr
[ ∂ T 4 ∂T 4T
∂ q2
J4
∂ q3
q̈2 +Tr
∂ q3
J4
] [
∂T 4 ∂ T 4T
∂ q3 ] [
q̈ 3+Tr
∂ q4 ] [
∂ T 4 ∂ T 4T
J4
∂ q3 ]
q̈4 + Tr
∂ T 5 ∂ T 5T
J
∂ q1 5 ∂ q3 1
q¨
+Tr
[ ∂ T 5 ∂T 5T
∂ q2
J5
∂ q3
q̈ 2+Tr
∂ q3
J5
] [
∂ T 5 ∂ T 5T
∂ q3 ] [
q¨3+Tr
∂ q4 ] [
∂ T 5 ∂ T 5T
J5
∂ q3 ]
q¨4 +Tr
∂ T 5 ∂T 5T
J q̈
∂ q5 5 ∂ q3 5
[ ] [ ] [ ] [ ]
T T T T
∂ T6 ∂ T6 ∂T6 ∂T6 ∂T 6 ∂T 6 ∂T6 ∂T6
+Tr J6 q¨1+ Tr J6 q̈2 +Tr J6 q¨3+ Tr J q̈
∂ q1 ∂ q3 ∂ q2 ∂ q3 ∂ q3 ∂ q3 ∂ q 4 6 ∂q 3 4
[ ] [ ] [ ]
T T 2 T
∂ T6 ∂ T6 ∂ T 6 ∂T 6 ∂ T ∂T 3 2
+Tr J6 q¨5 +Tr J6 q̈6 +Tr 2 3 J 3 q̇1
∂ q5 ∂ q3 ∂ q6 ∂ q3 ∂ q1 ∂ q3
+Tr
[ ∂2 T 3
∂ q2
2
J3
∂T 3T 2
∂ q3 ] [∂2 T
∂ q3 ] [
q̇2 +Tr 2 3 J 3
∂ T 3T 2
∂q 3 ]
q̇3 +2Tr
∂2 T 3
∂ q1 ∂ q2
∂T T
J 3 3 q˙1 q˙2
∂ q3
+2 Tr
[
∂2 T 3
J
∂ T 3T
q̇ q̇ +2 Tr
∂q 1 ∂q 3 3 ∂ q3 1 3 ] [
∂2 T 3
J
∂ T 3T
] [ ]
∂2 T 4 ∂ T 4 T 2
q̇ q̇ +Tr 2 J 4
∂ q 2 ∂q 3 3 ∂q 3 2 3 ∂ q1 ∂ q3 1
q˙
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T 4 ∂ T ∂T 4 ∂T ∂T4
+Tr J4 q˙22+Tr 2 4 J 4 q̇32 +Tr 2 4 J 4 q˙42
∂ q2
2
∂ q3 ∂ q3 ∂ q 3 ∂ q4 ∂ q3
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T 4 ∂ T4 ∂T 4 ∂ T4 ∂T4
+2 Tr J4 q̇1 q̇2 +2 Tr J4 q˙1 q˙3 +2Tr J4 q̇ q˙
∂q 1 ∂q 2 ∂ q3 ∂ q1∂ q3 ∂ q3 ∂ q1∂ q4 ∂ q3 1 4
+2 Tr
[
∂2T 4
J
∂ T 4T
q̇ q̇ +2 Tr
∂q 2 ∂ q3 4 ∂ q 3 2 3 ] [
∂2 T 4
J
∂ T 4T
] [
q˙ q˙ +2 Tr
∂q 2 ∂q 4 4 ∂ q3 2 4 ]∂2 T 4
J
∂ T 4T
q˙ q˙
∂ q 3 ∂q 4 4 ∂ q3 3 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5
∂T 5 2 ∂T ∂T5 2 ∂ T ∂T 2
+Tr 2 J 5 q̇1 +Tr 2 5 J 5 q̇2 +Tr 2 5 J 5 5 q˙3
∂ q1 ∂ q3 ∂ q2 ∂q 3 ∂ q3 ∂ q 3
22
+Tr
[ ∂2 T 5
∂2 q 4
J5
∂T 5T 2
∂ q3 ] [ ∂2 T ∂T T 2
q˙4 + Tr 2 5 J 5 5 q˙5 +2 Tr
∂ q5 ∂ q3 ]
∂2 T 5
∂ q1 ∂ q2
J5
[
∂T 5T
q̇ q̇
∂ q3 1 2 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T5 ∂ T5 ∂T 5 ∂ T5 ∂T5
+2 Tr J5 q̇1 q̇3 +2 Tr J5 q̇ 1 q˙4 +2 Tr J5 q˙ q̇
∂q 1 ∂q 3 ∂ q3 ∂ q1 ∂ q4 ∂ q3 ∂ q1∂ q5 ∂ q3 1 5
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T5 ∂ T5 ∂ T5 ∂ T5 ∂T5
+2 Tr J5 q̇2 q̇3 +2 Tr J5 q˙2 q˙4 +2 Tr J5 q̇ q̇
∂q 2 ∂ q3 ∂ q3 ∂q 2 ∂q 4 ∂ q3 ∂q 2 ∂q 5 ∂ q3 2 5
+2 Tr
[ ∂2 T 5
∂q 3 ∂ q4
J5
∂ T 5T
∂ q3 ]
q˙3 q˙4 +2 Tr
∂2 T 5
∂ q3 ∂ q5
J5
[
∂T 5T
∂ q3
q̇3 q˙5+2 Tr
]
∂2 T 5
[
∂ q4 ∂ q 5
J5
∂ T 5T
q̇ q˙
∂ q3 4 5 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 ∂T ∂ T6 ∂ T ∂T
+Tr J6 q˙12 +Tr 2 6 J 6 q˙22+Tr 2 6 J 6 6 q˙32
∂ q1
2
∂ q3 ∂ q2 ∂ q3 ∂ q3 ∂ q3
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6
∂T 6 ∂T ∂T 6 ∂T ∂T 6
+Tr 2 J 6 q˙42 +Tr 2 6 J 6 q̇52 +Tr 2 6 J 6 q˙62
∂ q4 ∂ q3 ∂ q5 ∂ q3 ∂ q6 ∂ q3
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 2
J6
∂ T 6T
∂ q3 ]
q̇1 q̇2 +2 Tr
∂2 T 6
∂q 1 ∂q 3
J6
[
∂T 6T
∂ q3
q̇1 q̇3 +2 Tr
]
∂2 T 6
[
∂ q1 ∂ q4
J6
∂ T 6T
q̇ q̇
∂ q3 1 4 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 ∂ T6 ∂T 6 ∂ T6 ∂ T6
+2 Tr J6 q̇1 q̇5 +2 Tr J6 q˙1 q˙6+ 2Tr J6 q˙ q̇
∂q 1 ∂q 5 ∂ q3 ∂ q1 ∂ q6 ∂ q3 ∂ q2 ∂ q3 ∂ q3 2 3
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T ∂ T6 ∂T 6 ∂ T6 ∂T
+2 Tr J 6 6 q˙2 q̇4 +2 Tr J6 q̇ 2 q˙5+ 2Tr J 6 6 q˙2 q̇6
∂q 2 ∂ q4 ∂ q3 ∂ q2 ∂ q5 ∂ q3 ∂ q2 ∂ q6 ∂ q3
+2 Tr
[
∂2 T 6
J
∂ T 6T
q̇ q̇ + 2Tr
∂q 3 ∂ q4 6 ∂ q 3 3 4 ]
∂2 T 6
J
∂ T 6T
[
q˙ q̇ +2 Tr
∂ q3 ∂ q5 6 ∂ q 3 3 5
∂2T 6
J
∂ T 6T
] [
q̇ q˙
∂ q 3 ∂ q 6 6 ∂ q3 3 6 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂ T6 ∂ T6 ∂T 6 ∂ T6 ∂T 6
+2 Tr J6 q̇4 q˙5+ 2Tr J6 q˙4 q̇6 +2 Tr J6 q̇ q˙
∂q 4 ∂ q5 ∂ q3 ∂ q 4 ∂q 6 ∂ q3 ∂ q 5 ∂q 6 ∂ q3 5 6
4
T ∂T3 3 T ∂T4 T ∂ T5 5 T ∂T 6 6
−m3 ⃗g r G −m 4 ⃗g
⃗ r G −m 5 ⃗g
⃗ ⃗r G −m 6 ⃗g r
⃗
∂ q3 3
∂ q3 ∂ q3
4 5
∂ q1 G 6
Khâu 4:
[ ] [ ] [ ] [ ]
T T T T
∂T4 ∂T4 ∂T 4 ∂ T 4 ∂T4 ∂T4 ∂ T 4 ∂T 4
F 4=Tr J4 q̈1 +Tr J4 q¨2+Tr J4 q̈3 +Tr J q¨
∂ q1 ∂ q4 ∂ q2 ∂ q4 ∂ q3 ∂ q4 ∂ q4 4 ∂ q4 4
[ ] [ ] [ ] [ ]
T T ¨ T T
∂ T 5 ∂T 5 ∂ T 5 ∂T 5 ∂ T5 ∂ T5 ∂ T5 ∂ T5
+Tr J q +¿ Tr J q̈ +Tr J q̈ +Tr J q¨ ¿
∂ q1 5 ∂ q4 1 ∂ q2 5 ∂ q4 2 ∂ q3 5 ∂ q4 3 ∂ q4 5 ∂ q4 4
23
[ ] [ ] [ ] [ ]
¨
∂ T 5 ∂T 5T ∂ T 6 ∂ T 6T ∂ T 6 ∂T 6T ∂ T 6 ∂ T 6T
+Tr J q̈ +Tr J q +¿ Tr J q̈ +Tr J q¨ ¿
∂ q5 5 ∂ q4 5 ∂ q1 6 ∂ q4 1 ∂q 2 6 ∂ q 4 2 ∂ q3 6 ∂ q 4 3
+Tr
[
∂ T 6 ∂ T 6T
J q¨ +Tr
∂ q4 6 ∂ q 4 4
J
] [
∂ T 6 ∂ T 6T
q¨ +Tr
∂ q5 6 ∂ q 4 5
∂ T 6 ∂T 6T
J q̈
∂ q6 6 ∂ q4 6 ] [ ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T 4 ∂ T ∂T 4 ∂ T ∂T4
+Tr J4 q˙12+Tr 2 4 J 4 q̇22+ Tr 2 4 J 4 q̇32
∂ q1
2
∂ q4 ∂ q2 ∂ q 4 ∂ q3 ∂ q 4
[
+Tr 2 J 4
∂ q4
∂2 T 4
∂T 4T 2
q˙ +2 Tr
∂ q4 4
∂2 T 4
] [
J
] [
∂T 4T
q˙ q˙ +2Tr
∂ q 1 ∂ q 2 4 ∂ q4 1 2 ]
∂2 T 4
J
∂ T 4T
q˙ q˙
∂ q1 ∂ q3 4 ∂ q4 1 3
+2 Tr
[ ∂2 T 4
∂q 1 ∂q 4
J4
∂ T 4T
∂q 4 ] [ ] [
q˙1 q˙4 +2 Tr
∂2 T 4
∂ q2 ∂ q3
J4
]∂T 4 T
∂ q4
q̇2 q̇3 +2 Tr
∂2 T 4
∂ q2∂ q4
J4
∂ T 4T
q̇ q˙
∂ q4 2 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T4 ∂T 4 ∂ T ∂T 5 2 ∂T ∂T5 2
+2 Tr J4 q˙3 q˙4 +Tr 2 5 J 5 q̇1 +Tr 2 5 J 5 q̇2
∂q 3 ∂ q4 ∂ q4 ∂ q1 ∂ q 4 ∂ q2 ∂ q 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5
∂T 5 2 ∂T ∂T5 2 ∂ T ∂T 5 2
+Tr 2 J 5 q̇3 +Tr 2 5 J 5 q̇4 +Tr 2 5 J 5 q̇5
∂ q3 ∂q 4 ∂ q4 ∂ q4 ∂ q5 ∂ q4
+2 Tr
[ ∂2 T 5
∂q 1 ∂q 2
J5
∂ T 5T
∂ q4 ] [ ] [
q̇1 q̇2 +2 Tr
∂2T 5
∂ q1 ∂ q3 ]
J5
∂ T 5T
∂ q4
q˙1 q̇3 +2Tr
∂2 T 5
∂ q1 ∂ q4
J5
∂T 5T
q̇ q˙
∂q 4 1 4
+2 Tr
[
∂2 T 5
J
∂ T 5T
q̇ q̇ +2 Tr
∂q 1 ∂q 5 5 ∂ q 4 1 5 ] [
∂2 T 5
] [
J
∂ T 5T
]
q̇ q̇ +2 Tr
∂ q 2 ∂q 3 5 ∂ q 4 2 3
∂2 T 5
J
∂ T 5T
q̇ q˙
∂ q 2 ∂ q 4 5 ∂ q4 2 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T5 ∂ T5 ∂ T5 ∂ T5 ∂T 5
+2 Tr J5 q̇2 q̇5 +2 Tr J5 q˙3 q˙4 +2 Tr J5 q̇ q̇
∂q 2 ∂ q5 ∂ q4 ∂q 3 ∂ q4 ∂ q4 ∂ q3 ∂ q5 ∂q 4 3 5
2 Tr
[∂2 T 5
J
∂ T 5T
] [
q˙ q̇ +Tr 2 J 6
∂ q 4 ∂ q5 5 ∂ q4 4 5 ∂ q1 ] [
∂2 T 6 ∂T 6T 2
]∂2 T 6 ∂ T 6 T 2
q˙ +Tr 2 J 6
∂ q4 1 ∂ q2 ∂ q4 2
q˙
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 2 ∂ T ∂T 2 ∂T ∂T 6 2
+Tr J6 q˙3 +Tr 2 6 J 6 6 q˙4 +Tr 2 6 J 6 q̇ 5
∂2 q3 ∂ q4 ∂ q4 ∂ q 4 ∂ q5 ∂ q 4
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6
∂T 6 ∂ T6 ∂T 6 ∂ T6 ∂T 6
+Tr 2 J 6 q˙62+ 2Tr J6 q̇ 1 q˙2+ 2Tr J6 q˙ q˙
∂ q6 ∂ q4 ∂ q1 ∂ q2 ∂ q4 ∂ q1 ∂ q3 ∂ q4 1 3
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂ T6 ∂ T6 ∂T 6 ∂ T6 ∂ T6
+2 Tr J6 q˙1 q˙4 +2 Tr J6 q̇1 q˙5+2 Tr J6 q˙ q˙
∂q 1 ∂q 4 ∂ q4 ∂ q1 ∂ q5 ∂ q4 ∂ q1 ∂ q6 ∂ q4 1 6
24
+2 Tr
[ ∂2 T 6
∂q 2 ∂ q3
J6
∂T 6T
∂q 4 ]
q̇2 q̇ 3+2 Tr
[
∂2 T 6
∂ q2 ∂ q4
J6
∂ T 6T
∂ q4 ]
q̇2 q̇4 + 2Tr
[
∂2 T 6
∂ q2 ∂ q5
J6
∂ T 6T
]
q˙ q˙
∂ q4 2 5
+2 Tr
[ ∂2 T 6
∂q 2 ∂ q6
J6
∂T 6T
∂ q4 ]
q̇ 2 q˙6+ 2Tr
[
∂2 T 6
∂ q3 ∂ q 4
J6
∂T 6T
∂q 4 ]
q̇3 q˙4 +2 Tr
[
∂2T 6
∂ q3∂ q5
J6
∂ T 6T
]
q̇ q̇
∂ q4 3 5
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 ∂ T6 ∂T 6 ∂ T6 ∂T6
+2 Tr J6 q˙3 q˙6 +2Tr J6 q˙4 q̇5 +2 Tr J6 q̇ q˙
∂q 3 ∂ q6 ∂ q4 ∂ q 4 ∂ q5 ∂ q4 ∂q 4 ∂ q6 ∂ q4 4 6
[ ]
2 T 4
∂ T6 ∂T 6 T ∂T4 T ∂T 5 5 T ∂T6 6
+2 Tr J6 q˙5 q˙6−m4 ⃗g r G −m5 ⃗g
⃗ r G −m6 ⃗g
⃗ r
⃗
∂q 5 ∂ q6 ∂ q4 ∂ q4 ∂ q4 4
∂ q1 G
5 6
Khâu 5:
[ ] [ ] [ ] [ ]
T T ¨ T T
∂T 5 ∂ T 5 ∂T 5 ∂ T 5 ∂T 5 ∂T 5 ∂ T 5 ∂T 5
F 5=Tr J5 q1 +¿ Tr J5 q̈2 +Tr J5 q̈3 +Tr J5 q¨ ¿
∂ q1 ∂ q5 ∂ q2 ∂ q5 ∂ q3 ∂ q5 ∂ q4 ∂ q5 4
+Tr
[
∂ T 5 ∂T 5T
J q̈ +Tr
∂ q5 5 ∂ q5 5
J
] [
∂ T 6 ∂ T 6T ∂2 T 5 ∂ T 5 T 2
q̈ +Tr 2 J 5
∂ q6 6 ∂ q 5 6 ∂ q1 ∂ q5 1 ] [ ] [
∂ 2 T 5 ∂ T 5T 2
q̇ +Tr 2 J 5
∂ q2
q˙
∂ q5 2 ]
+Tr
[ ∂2 T 5
∂ q3
2
J5
∂ q5 ] [
∂T 5T 2 ∂2 T
q̇3 +Tr 2 5 J 5
∂ q4
∂ T 5T 2
∂ q5 ] [
∂2 T
]
q̇4 +Tr 2 5 J 5
∂ q5
∂T 5T 2
q̇
∂ q5 5
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T5 ∂T5 ∂ T5 ∂ T5 ∂ T5 ∂T 5
+2 Tr J5 q̇1 q̇2 +2 Tr J5 q˙1 q̇3 +2Tr J5 q̇ q˙
∂q 1 ∂q 2 ∂q 5 ∂ q1 ∂ q3 ∂ q5 ∂ q1 ∂ q4 ∂ q5 1 4
+2 Tr
[
∂2 T 5
J
∂ T 5T
q̇ q̇ +2 Tr
∂q 1 ∂q 5 5 ∂ q5 1 5 ] ∂2 T 5
J
[
∂ T 5T
] [
q̇ q̇ +2 Tr
∂ q 2 ∂q 3 5 ∂q 5 2 3
∂2 T 5
J
∂ T 5T
q̇ q˙
∂ q 2 ∂ q 4 5 ∂ q5 2 4 ]
+2 Tr
[
∂2 T 5
J
∂ T 5T
q̇ q̇ +2 Tr
∂q 2 ∂ q5 5 ∂ q5 2 5 ] ∂2 T 5
J
[
∂ T 5T
q˙ q˙ +2 Tr
∂q 3 ∂ q4 5 ∂ q 5 3 4 ] [∂2 T 5
J
∂T 5T
q̇ q̇
∂ q3 ∂ q5 5 ∂ q5 3 5 ]
[ ] [ ] [ ]
¨
∂2 T 5 ∂ T 5T ∂ T 6 ∂ T 6T ∂ T 6 ∂ T 6T
+2 Tr J q˙ q̇ +Tr J q +¿ Tr J q̈ ¿
∂q 4 ∂ q5 5 ∂ q 5 4 5 ∂ q1 6 ∂ q 5 1 ∂ q 2 6 ∂ q5 2
+Tr
[
∂ T 6 ∂ T 6T
J q¨ +Tr
∂ q3 6 ∂ q 5 3
J
] [
∂ T 6 ∂T 6T
q¨ +Tr
∂ q 4 6 ∂ q5 4
∂ T 6 ∂T 6T
J
] [
q̈ +Tr
∂q 5 6 ∂ q5 5 ] [
∂ T 6 ∂ T 6T
J q¨
∂ q6 6 ∂ q 5 6 ]
[
+Tr 2 J 6
∂ q1
∂2 T 6
∂T 6T 2
q˙ +Tr 2 J 6
∂ q5 1 ∂ q2 ] [
∂2 T 6 ∂ T 6T 2 ∂2 T 6 ∂ T 6T 2
q˙ +Tr 2 J 6
∂ q5 2 ∂ q3
q˙
∂ q5 3 ] [ ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 2 ∂T ∂T 6 2 ∂T ∂T 6 2
+Tr J6 q˙4 +Tr 2 6 J 6 q̇5 +Tr 2 6 J 6 q˙6
∂2 q 4 ∂ q5 ∂ q5 ∂ q 5 ∂ q6 ∂ q 5
25
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 2
J6
∂ T 6T
∂ q5 ]
q̇1 q̇2 +2 Tr
[
∂2 T 6
∂q 1 ∂q 3
J6
∂T 6T
∂ q5 ]
q̇1 q̇3 +2 Tr
[
∂2 T 6
∂ q1 ∂ q4
J6
∂ T 6T
]
q̇ q̇
∂ q5 1 4
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 5
J6
∂T 6T
∂ q5 ]
q̇1 q̇5 +2 Tr
[
∂2 T 6
∂ q1 ∂ q6
J6
∂T 6T
∂ q5 ]
q˙1 q˙6+ 2Tr
[
∂2 T 6
∂ q2 ∂ q3
J6
∂ T 6T
q˙ q̇
∂ q5 2 3 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂ T6 ∂ T6 ∂T 6 ∂ T6 ∂ T6
+2 Tr J6 q˙2 q̇4 +2 Tr J6 q̇ 2 q˙5+ 2Tr J6 q˙ q̇
∂q 2 ∂ q4 ∂ q5 ∂ q2 ∂ q5 ∂ q5 ∂ q2 ∂ q6 ∂ q5 2 6
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂ T6 ∂ T6 ∂ T6 ∂ T6 ∂T6
+2 Tr J6 q̇3 q̇4 + 2Tr J6 q˙3 q̇5 +2 Tr J6 q̇ q˙
∂q 3 ∂ q4 ∂ q5 ∂ q3 ∂ q5 ∂ q5 ∂ q3∂ q6 ∂ q5 3 6
+2 Tr
[
∂2 T 6
J
∂ T 6T
q̇ q˙ + 2Tr
∂q 4 ∂ q5 6 ∂ q 5 4 5 ]
∂2 T 6
J
∂T 6T
[q˙ q̇ +2 Tr
∂ q 4 ∂q 6 6 ∂ q5 4 6
∂2 T 6
]J
∂T 6T
[q̇ q˙
∂ q 5 ∂q 6 6 ∂ q5 5 6 ]
T ∂T5 5 T ∂ T6 6
−m5 ⃗g r G −m6 ⃗g
⃗ ⃗r
∂ q5 5
∂ q1 G 6
Khâu 6:
[ ] [ ] [ ] [ ]
T T ¨ T T
∂T 6 ∂T 6 ∂ T6 ∂ T6 ∂T 6 ∂T 6 ∂ T6 ∂ T6
F 6=Tr J6 q 1+¿ Tr J6 q̈2 +Tr J6 q̈3 +Tr J6 q¨ ¿
∂ q1 ∂ q6 ∂ q2 ∂ q6 ∂q 3 ∂ q6 ∂ q4 ∂ q6 4
[ ] [ ] [ ] [ ]
T T 2 T 2 T
∂ T6 ∂ T6 ∂ T 6 ∂T 6 ∂ T ∂T ∂ T ∂T
+Tr J6 q¨5 +Tr J6 q̈6 +Tr 2 6 J 6 6 q˙12+Tr 2 6 J 6 6 q˙22
∂ q5 ∂ q6 ∂ q6 ∂ q6 ∂ q1 ∂ q6 ∂ q2 ∂ q6
+Tr
[ ∂2 T 6
∂ q3
2
J6
∂T 6T 2
∂ q6 ] [ ∂2 T ∂T T
] [
∂2 T
q˙3 +Tr 2 6 J 6 6 q˙42 +Tr 2 6 J 6
∂ q4 ∂ q6 ∂ q6
∂T 6T 2
q̇
∂ q6 5 ]
[
+Tr 2 J 6
∂ q6
∂2 T 6
∂T 6T 2
q˙ + 2Tr
∂ q6 6
∂2 T 6
J
] [
∂T 6T
q̇ q˙ + 2Tr
∂ q1 ∂ q2 6 ∂ q6 1 2
∂2 T 6
] [
J
∂T 6T
q˙ q˙
∂ q1 ∂ q3 6 ∂ q6 1 3 ]
+2 Tr
[ ∂2 T 6
∂q 1 ∂q 4
∂T T
]
J 6 6 q˙1 q˙4 +2 Tr
∂ q6
∂2 T 6
∂ q1 ∂ q5 [
J6
∂T 6T
∂ q6 ]
q̇1 q˙5+2 Tr
∂2 T 6
[
∂ q1 ∂ q6
J6
∂ T 6T
q˙ q˙
∂ q6 1 6 ]
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 ∂ T6 ∂T6 ∂ T6 ∂ T6
+2 Tr J6 q̇2 q̇ 3+2 Tr J6 q̇2 q̇4 + 2Tr J6 q˙ q˙
∂q 2 ∂ q3 ∂ q6 ∂ q2 ∂ q4 ∂ q6 ∂ q2 ∂ q5 ∂ q6 2 5
[ ] [ ] [ ]
2 T 2 T 2 T
∂ T6 ∂T 6 ∂ T6 ∂T 6 ∂ T6 ∂T6
+2 Tr J6 q̇ 2 q˙6+ 2Tr J6 q̇3 q˙4 +2 Tr J6 q̇ q̇
∂q 2 ∂ q6 ∂ q6 ∂ q3 ∂ q 4 ∂ q6 ∂ q3∂ q5 ∂ q6 3 5
+2 Tr
[
∂2 T 6
J
∂T 6T
q˙ q˙ +2Tr
∂q 3 ∂ q6 6 ∂ q6 3 6 ]
∂2 T 6
J
∂T 6T
[
q˙ q̇ +2 Tr
∂ q 4 ∂ q5 6 ∂ q6 4 5 ]∂2 T 6
J
∂ T 6T
[
q̇ q˙
∂q 4 ∂ q6 6 ∂ q 6 4 6 ]
26
+2 Tr
[ ∂2 T 6
∂q 5 ∂ q6
J6
∂T 6T
∂ q5 ]
q˙5 q˙6−m 6 ⃗g
T ∂T 6 6
∂ q1 G
r
⃗
6
CODE MATLAB
clc;
sympref('FloatingPointOutput', false)
syms q1 q2 q3 q4 q5 q6 qd1 qd2 qd3 qd4 qd5 qd6 qdd1 qdd2 qdd3 qdd4 qdd5 qdd6 ;
L1=0.165;
L2=0.125;
L3=0.27;
L4=0.7;
L5=0.134;
L6=0.168;
L7=0.072;
g=10;
dx1=0; dy1=0; dz1=0;

dx2=0; dy2=0 ; dz2=0;
dx3=0; dy3=0 ; dz3=0;
dx4=0 ; dy4=0 ; dz4=0;
dx5=0 ; dy5=0 ; dz5=0;
dx6=0 ; dy6= 0; dz6=0;
% Vecto gia toc trong truong

Vg=[0;0;-g;1];
% Vecto toa do khoi tam trong he quy chieu cua khau

rRG1=[-0.0001;0.0000;0.0594;1];
rRG2=[-0.0008;-0.1012;0.0019;1];
rRG3=[0.0579;0.0228;0.0011;1];
27
rRG4=[0.0023;0.0026;0.0767;1];
rRG5=[0.0001;-0.0011;0.0001;1];
rRG6=[0.0002;0.0000;-0.0012;1];
% Ma tran gia quan tinh

%Khau 1
m1=3.067; Ixx1=0.0250;Iyy1=0.0252;Izz1=0.0105;Ixy1=-0;Ixz1=0.0000;Iyz1=0;
J1=[(-Ixx1+Iyy1+Izz1)/2+m1*dx1*dx1 Ixy1+m1*dx1*dy1 Ixz1+m1*dx1*dz1 -m1*dx1;

Ixy1+m1*dx1*dy1 (Ixx1-Iyy1+Izz1)/2+m1*dy1*dy1 Iyz1+m1*dy1*dz1 -m1*dy1;
Ixz1+m1*dx1*dz1 Iyz1+m1*dy1*dz1 (Ixx1+Iyy1-Izz1)/2+m1*dz1*dz1 -m1*dz1;
-m1*dx1 -m1*dy1 -m1*dz1 m1];
%Khau 2
m2=3.9086; Ixx2=0.1004;Iyy2=0.026;Izz2=0.0816;Ixy2=0.0004;Ixz2=0;Iyz2=-0.0012;

-m2*dx2 -m2*dy2 -m2*dz2 m2];
%Khau 3
m3=2.9437; Ixx3=0.0142;Iyy3=0.0182;Izz3=0.0281;Ixy3=0.0053;Ixz3=-0;Iyz3=-0;

-m3*dx3 -m3*dy3 -m3*dz3 m3];
%Khau 4
m4=1.4476; Ixx4=0.0145;Iyy4=0.0129;Izz4=0.0033;Ixy4=-0;Ixz4=0.0003;Iyz4=0.0004;

-m4*dx4 -m4*dy4 -m4*dz4 m4];
%Khau 5
m5=0.5466; Ixx5=0.0008;Iyy5=0.0004;Izz5=0.001;Ixy5=0;Ixz5=-0;Iyz5=-0;
28
-m5*dx5 -m5*dy5 -m5*dz5 m5];
%Khau 6
m6=0.0137; Ixx6=0.000;Iyy6=0;Izz6=0.000;Ixy6=0;Ixz6=-0.000;Iyz6=-0.000;

-m6*dx6 -m6*dy6 -m6*dz6 m6];
% Ma tran bien doi thuan nhat

T01=[cos(q1) -sin(q1) 0 0;sin(q1) cos(q1) 0 0; 0 0 1 L1;0 0 0 1];
T12=[cos(q2) -sin(q2) 0 0;0 0 1 L2; -sin(q2) -cos(q2) 0 0;0 0 0 1];
T23=[cos(q3) -sin(q3) 0 L3;sin(q3) cos(q3) 0 0; 0 0 1 0;0 0 0 1];
T34=[cos(q4) -sin(q4) 0 L4;0 0 1 L5; -sin(q4) -cos(q4) 0 0;0 0 0 1];
T45=[cos(q5) -sin(q5) 0 0;0 0 -1 -L6; sin(q5) cos(q5) 1 0;0 0 0 1];
T56=[cos(q6) -sin(q6) 0 0;0 0 1 L7; -sin(q6) -cos(q6) 0 0;0 0 0 1];
T02=T01*T12;
T03=T02*T23;
T04=T03*T34;
T05=T04*T45;
T06=T05*T56;
% He phuong trinh vi phan chuyen dong

%khau 1
F1=trace(diff(T01,q1)*J1*transpose(diff(T01,q1)))*qdd1+...
trace(diff(T02,q1)*J2*transpose(diff(T02,q1)))*qdd1+...
29
trace(diff(diff(T01,q1),q1)*J1*transpose(diff(T01,q1)))*qd1*qd1+...
2*trace(diff(diff(T02,q1),q2)*J2*transpose(diff(T02,q1)))*qd1*qd2+...
30
2*trace(diff(diff(T06,q5),q6)*J6*transpose(diff(T06,q1)))*qd5*qd6-...
m1*transpose(Vg)*diff(T01,q1)*rRG1-...
m6*transpose(Vg)*diff(T06,q1)*rRG6;
% Khau 2
31
32
33
%Khau 3
34
35
%Khau 4
36
%Khau 5
37
38
%Khau 6
F1=simplify(F1);
F2=simplify(F2);
F3=simplify(F3);
F4=simplify(F4);
F5=simplify(F5);
F6=simplify(F6);
Kết quả tính được

39
F1 = (293732403*qdd1)/312500000 + (7164291*qdd1*cos(2*q2))/50000000 +
(5305749*qdd1*cos(2*q4))/1250000000 - (73597*qdd1*cos(2*q5))/1250000000 +
(334233*qd2^2*cos(q2))/2000000 - (qdd1*sin(2*q2))/2500 + (3*qd2^2*sin(q2))/2500 +
(3794931*qdd1*cos(q3))/10000000 - (3*qdd2*cos(q2))/2500 +
(117663*qdd1*cos(q4))/5000000 + (82611*qdd1*cos(q5))/625000000 -
(36322911*qdd1*sin(q3))/500000000 + (334233*qdd2*sin(q2))/2000000 +
(823641*qdd1*sin(q4))/12500000 - (36322911*qd1*qd3*cos(q3))/500000000 +
(823641*qd1*qd4*cos(q4))/12500000 - (3794931*qd1*qd3*sin(q3))/10000000 -
(117663*qd1*qd4*sin(q4))/5000000 - (82611*qd1*qd5*sin(q5))/625000000 +
(474503173*qdd1*cos(2*q2)*cos(2*q3))/1000000000 +
(73597*qdd1*cos(2*q4)*cos(2*q5))/1250000000 -
(9682051*qdd1*cos(2*q2)*sin(2*q3))/50000000 -
(9682051*qdd1*cos(2*q3)*sin(2*q2))/50000000 -
(474503173*qdd1*sin(2*q2)*sin(2*q3))/1000000000 - (qd1*qd2*cos(2*q2))/1250 +
(1345293*qdd2*cos(q2)*cos(q3))/40000000 +
(1345293*qdd3*cos(q2)*cos(q3))/40000000 -
(6171693*qdd4*cos(q2)*cos(q3))/312500000 - (7164291*qd1*qd2*sin(2*q2))/25000000
- (5305749*qd1*qd4*sin(2*q4))/625000000 + (73597*qd1*qd5*sin(2*q5))/625000000 +
(140553*qdd2*cos(q2)*sin(q3))/800000 + (140553*qdd2*cos(q3)*sin(q2))/800000 +
(3176901*qdd1*cos(q3)*sin(q4))/125000000 +
(3176901*qdd2*cos(q4)*sin(q2))/125000000 + (140553*qdd3*cos(q2)*sin(q3))/800000
+ (140553*qdd3*cos(q3)*sin(q2))/800000 - (33291*qdd1*cos(q5)*sin(q3))/125000000 -
(8631*qdd1*cos(q4)*sin(q5))/12500000 - (3176901*qdd4*cos(q2)*sin(q4))/125000000 -
(1345293*qdd2*sin(q2)*sin(q3))/40000000 - (1345293*qdd3*sin(q2)*sin(q3))/40000000
+ (6171693*qdd4*sin(q2)*sin(q3))/312500000 + (1233*qdd1*sin(q4)*sin(q5))/5000000
+ (3794931*qdd1*cos(2*q2)*cos(q3))/10000000 +
(140553*qd2^2*cos(q2)*cos(q3))/800000 +
(3176901*qd2^2*cos(q2)*cos(q4))/125000000 +
(140553*qd3^2*cos(q2)*cos(q3))/800000 -
(3176901*qd4^2*cos(q2)*cos(q4))/125000000 -
40
(36322911*qdd1*cos(2*q2)*sin(q3))/500000000 -
(36322911*qdd1*sin(2*q2)*cos(q3))/500000000 -
(1345293*qd2^2*cos(q2)*sin(q3))/40000000 -
(1345293*qd2^2*cos(q3)*sin(q2))/40000000 -
(1345293*qd3^2*cos(q2)*sin(q3))/40000000 -
(1345293*qd3^2*cos(q3)*sin(q2))/40000000 -
(3794931*qdd1*sin(2*q2)*sin(q3))/10000000 +
(25893*qdd1*sin(2*q4)*sin(q5))/312500000 - (140553*qd2^2*sin(q2)*sin(q3))/800000 -
(140553*qd3^2*sin(q2)*sin(q3))/800000 - (140553*qd2*qd3*sin(q2)*sin(q3))/400000 -
(3176901*qd1*qd3*sin(q3)*sin(q4))/125000000 +
(8631*qd1*qd4*sin(q4)*sin(q5))/12500000 +
(33291*qd1*qd5*sin(q3)*sin(q5))/125000000 -
(82611*qdd1*cos(2*q2)*cos(2*q3)*cos(q5))/625000000 +
(823641*qdd1*cos(2*q2)*cos(2*q3)*sin(q4))/12500000 -
(3*qdd1*cos(2*q2)*sin(2*q3)*cos(q4))/10000 -
(8631*qdd1*cos(2*q3)*sin(2*q2)*cos(q5))/12500000 +
(5305749*qd2^2*sin(2*q4)*cos(q2)*cos(q3))/625000000 +
(5305749*qd3^2*sin(2*q4)*cos(q2)*cos(q3))/625000000 -
(7695921*qdd1*cos(2*q2)*sin(2*q3)*sin(q4))/625000000 -
(7695921*qdd1*cos(2*q3)*sin(2*q2)*sin(q4))/625000000 +
(82611*qdd1*sin(2*q2)*sin(2*q3)*cos(q5))/625000000 -
(823641*qdd1*sin(2*q2)*sin(2*q3)*sin(q4))/12500000 -
(5305749*qd2^2*sin(2*q4)*sin(q2)*sin(q3))/625000000 -
(5305749*qd3^2*sin(2*q4)*sin(q2)*sin(q3))/625000000 -
(36322911*qd1*qd2*cos(2*q2)*cos(q3))/250000000 -
(36322911*qd1*qd3*cos(2*q2)*cos(q3))/500000000 +
(7695921*qdd2*cos(q2)*cos(q3)*cos(q4))/625000000 +
41
(1233*qdd3*cos(q2)*cos(q3)*cos(q5))/10000000 -
(3794931*qd1*qd2*cos(2*q2)*sin(q3))/5000000 -
(3794931*qd1*qd2*sin(2*q2)*cos(q3))/5000000 -
(3794931*qd1*qd3*cos(2*q2)*sin(q3))/10000000 -
(3794931*qd1*qd3*sin(2*q2)*cos(q3))/10000000 +
(25893*qd1*qd4*cos(2*q4)*sin(q5))/156250000 +
(25893*qd1*qd5*sin(2*q4)*cos(q5))/312500000 -
(3*qdd2*cos(q2)*cos(q3)*sin(q4))/10000 +
(823641*qdd2*cos(q3)*cos(q4)*sin(q2))/12500000 -
(3*qdd4*cos(q2)*cos(q4)*sin(q3))/10000 + (3*qdd4*cos(q3)*cos(q4)*sin(q2))/10000 +
(36322911*qd1*qd2*sin(2*q2)*sin(q3))/250000000 +
(36322911*qd1*qd3*sin(2*q2)*sin(q3))/500000000 -
(5305749*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q4))/1250000000 -
(220791*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q5))/1250000000 +
(117663*qdd2*cos(q2)*sin(q3)*sin(q4))/10000000 +
(117663*qdd2*cos(q3)*sin(q2)*sin(q4))/10000000 -
42
(3*qdd2*sin(q2)*sin(q3)*sin(q4))/10000 + (3*qdd3*sin(q2)*sin(q3)*sin(q4))/10000 +
(33291*qdd2*sin(q2)*sin(q4)*sin(q5))/125000000 +
(5305749*qdd1*cos(2*q4)*sin(2*q2)*sin(2*q3))/1250000000 +
(220791*qdd1*cos(2*q5)*sin(2*q2)*sin(2*q3))/1250000000 -
(9682051*qd1*qd2*cos(2*q2)*cos(2*q3))/25000000 -
(9682051*qd1*qd3*cos(2*q2)*cos(2*q3))/25000000 +
(73597*qdd4*cos(2*q5)*cos(q2)*cos(q3))/312500000 +
(823641*qd2^2*cos(q2)*cos(q3)*cos(q4))/12500000 +
(823641*qd3^2*cos(q2)*cos(q3)*cos(q4))/12500000 -
(823641*qd4^2*cos(q2)*cos(q3)*cos(q4))/12500000 -
(474503173*qd1*qd2*cos(2*q2)*sin(2*q3))/500000000 -
(474503173*qd1*qd2*cos(2*q3)*sin(2*q2))/500000000 -
(474503173*qd1*qd3*cos(2*q2)*sin(2*q3))/500000000 -
(474503173*qd1*qd3*cos(2*q3)*sin(2*q2))/500000000 -
(73597*qd1*qd4*cos(2*q5)*sin(2*q4))/625000000 -
(73597*qd1*qd5*cos(2*q4)*sin(2*q5))/625000000 +
(3176901*qdd1*cos(2*q2)*cos(q3)*sin(q4))/125000000 -
(33291*qdd1*cos(2*q2)*cos(q5)*sin(q3))/125000000 -
43
(33291*qdd1*sin(2*q2)*cos(q3)*cos(q5))/125000000 +
(117663*qd2^2*cos(q2)*cos(q3)*sin(q4))/10000000 -
(7695921*qd2^2*cos(q2)*cos(q4)*sin(q3))/625000000 -
(7695921*qd2^2*cos(q3)*cos(q4)*sin(q2))/625000000 -
(1233*qd2^2*cos(q2)*cos(q5)*sin(q3))/10000000 -
(1233*qd2^2*cos(q3)*cos(q5)*sin(q2))/10000000 +
(117663*qd3^2*cos(q2)*cos(q3)*sin(q4))/10000000 -
(7695921*qd3^2*cos(q2)*cos(q4)*sin(q3))/625000000 -
(7695921*qd3^2*cos(q3)*cos(q4)*sin(q2))/625000000 -
(1233*qd3^2*cos(q2)*cos(q5)*sin(q3))/10000000 -
(1233*qd3^2*cos(q3)*cos(q5)*sin(q2))/10000000 +
(117663*qd4^2*cos(q2)*cos(q3)*sin(q4))/10000000 +
(7695921*qd4^2*cos(q2)*cos(q4)*sin(q3))/625000000 +
(7695921*qd4^2*cos(q3)*cos(q4)*sin(q2))/625000000 -
(25893*qd5^2*cos(q2)*cos(q3)*sin(q5))/156250000 -
(1233*qd5^2*cos(q2)*cos(q5)*sin(q3))/10000000 -
(1233*qd5^2*cos(q3)*cos(q5)*sin(q2))/10000000 +
(9682051*qd1*qd2*sin(2*q2)*sin(2*q3))/25000000 +
(9682051*qd1*qd3*sin(2*q2)*sin(2*q3))/25000000 +
(5305749*qdd2*sin(2*q4)*cos(q2)*sin(q3))/625000000 +
(5305749*qdd3*sin(2*q4)*cos(q3)*sin(q2))/625000000 -
(73597*qdd4*cos(2*q5)*sin(q2)*sin(q3))/312500000 +
(3*qd2^2*cos(q2)*sin(q3)*sin(q4))/10000 + (3*qd2^2*cos(q3)*sin(q2)*sin(q4))/10000 -
(823641*qd2^2*cos(q4)*sin(q2)*sin(q3))/12500000 +
(3*qd3^2*cos(q2)*sin(q3)*sin(q4))/10000 + (3*qd3^2*cos(q3)*sin(q2)*sin(q4))/10000 -
(823641*qd3^2*cos(q4)*sin(q2)*sin(q3))/12500000 +
(33291*qd2^2*cos(q2)*sin(q4)*sin(q5))/125000000 -
(3*qd4^2*cos(q2)*sin(q3)*sin(q4))/10000 - (3*qd4^2*cos(q3)*sin(q2)*sin(q4))/10000 +
44
(823641*qd4^2*cos(q4)*sin(q2)*sin(q3))/12500000 -
(33291*qd4^2*cos(q2)*sin(q4)*sin(q5))/125000000 -
(33291*qd5^2*cos(q2)*sin(q4)*sin(q5))/125000000 -
(3176901*qdd1*sin(2*q2)*sin(q3)*sin(q4))/125000000 -
(117663*qd2^2*sin(q2)*sin(q3)*sin(q4))/10000000 -
(117663*qd3^2*sin(q2)*sin(q3)*sin(q4))/10000000 -
(117663*qd4^2*sin(q2)*sin(q3)*sin(q4))/10000000 +
(25893*qd5^2*sin(q2)*sin(q3)*sin(q5))/156250000 +
(140553*qd2*qd3*cos(q2)*cos(q3))/400000 -
(33291*qd1*qd3*cos(q3)*cos(q5))/125000000 +
(3176901*qd1*qd4*cos(q3)*cos(q4))/125000000 -
(8631*qd1*qd5*cos(q4)*cos(q5))/12500000 -
(1345293*qd2*qd3*cos(q2)*sin(q3))/20000000 -
(1345293*qd2*qd3*cos(q3)*sin(q2))/20000000 +
(6171693*qd2*qd4*cos(q2)*sin(q3))/312500000 +
(6171693*qd2*qd4*cos(q3)*sin(q2))/312500000 +
(6171693*qd3*qd4*cos(q2)*sin(q3))/312500000 +
(6171693*qd3*qd4*cos(q3)*sin(q2))/312500000 +
(1233*qd1*qd4*cos(q4)*sin(q5))/5000000 + (1233*qd1*qd5*cos(q5)*sin(q4))/5000000
+ (82611*qdd1*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/625000000 -
(25893*qdd1*cos(2*q2)*sin(2*q3)*cos(q5)*sin(q4))/156250000 +
(82611*qdd1*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5))/625000000 -
(5305749*qd1*qd4*sin(2*q2)*sin(2*q3)*sin(2*q4))/625000000 -
(220791*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(2*q5))/625000000 +
(8631*qdd1*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/12500000 +
45
(25893*qd2^2*cos(2*q4)*sin(q2)*sin(q3)*sin(q5))/156250000 -
(73597*qd2^2*sin(2*q5)*cos(q2)*sin(q3)*sin(q4))/312500000 -
(73597*qd2^2*sin(2*q5)*cos(q3)*sin(q2)*sin(q4))/312500000 +
(25893*qd3^2*cos(2*q4)*sin(q2)*sin(q3)*sin(q5))/156250000 -
(33291*qd1*qd2*cos(2*q2)*cos(q3)*cos(q5))/62500000 -
(33291*qd1*qd3*cos(2*q2)*cos(q3)*cos(q5))/125000000 +
(3176901*qd1*qd4*cos(2*q2)*cos(q3)*cos(q4))/125000000 +
(25893*qdd2*cos(q2)*cos(q3)*cos(q4)*cos(q5))/156250000 +
(5305749*qd2*qd3*sin(2*q4)*cos(q2)*cos(q3))/312500000 +
(5305749*qd2*qd4*cos(2*q4)*cos(q2)*sin(q3))/312500000 +
(5305749*qd2*qd4*cos(2*q4)*cos(q3)*sin(q2))/312500000 -
(qd4*qd6*sin(2*q6)*cos(q2)*cos(q3))/5000 +
(8631*qdd4*cos(q2)*cos(q3)*cos(q4)*sin(q5))/12500000 +
(8631*qdd5*cos(q2)*cos(q3)*cos(q5)*sin(q4))/12500000 -
(3176901*qd1*qd2*cos(2*q2)*sin(q3)*sin(q4))/62500000 -
(3176901*qd1*qd2*sin(2*q2)*cos(q3)*sin(q4))/62500000 +
(33291*qd1*qd2*sin(2*q2)*cos(q5)*sin(q3))/62500000 -
46
(3176901*qd1*qd3*cos(2*q2)*sin(q3)*sin(q4))/125000000 -
(33291*qd1*qd3*sin(2*q2)*cos(q5)*sin(q3))/125000000 -
(33291*qd1*qd5*cos(2*q2)*sin(q3)*sin(q5))/125000000 +
(82611*qdd2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/625000000 -
(25893*qdd2*cos(q4)*cos(q5)*sin(q2)*sin(q3))/156250000 +
(5305749*qd2*qd3*sin(2*q4)*sin(q2)*sin(q3))/312500000 +
(73597*qd4*qd5*sin(2*q5)*sin(q2)*sin(q3))/156250000 -
(qd4*qd6*sin(2*q6)*sin(q2)*sin(q3))/5000 +
(73597*qd2^2*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3))/625000000 +
(8631*qdd2*cos(q2)*sin(q3)*sin(q4)*sin(q5))/12500000 +
47
(8631*qdd3*cos(q3)*sin(q2)*sin(q4)*sin(q5))/12500000 -
(25893*qdd1*cos(2*q2)*cos(2*q3)*sin(2*q4)*sin(q5))/312500000 +
(73597*qdd1*cos(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/312500000 +
(73597*qdd1*cos(2*q3)*sin(2*q2)*sin(2*q5)*cos(q4))/312500000 -
(82611*qdd2*sin(q2)*sin(q3)*sin(q4)*sin(q5))/625000000 -
(82611*qdd3*sin(q2)*sin(q3)*sin(q4)*sin(q5))/625000000 +
(1233*qdd4*sin(q2)*sin(q3)*sin(q4)*sin(q5))/10000000 -
(73597*qd2^2*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/625000000 -
(73597*qd3^2*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/625000000 -
(3*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q4))/5000 -
(8631*qd1*qd3*cos(2*q2)*cos(2*q3)*cos(q5))/6250000 +
(823641*qd1*qd4*cos(2*q2)*cos(2*q3)*cos(q4))/12500000 +
(25893*qdd1*sin(2*q2)*sin(2*q3)*sin(2*q4)*sin(q5))/312500000 -
(7695921*qd1*qd2*cos(2*q2)*cos(2*q3)*sin(q4))/312500000 +
(82611*qd1*qd2*cos(2*q2)*sin(2*q3)*cos(q5))/312500000 +
(82611*qd1*qd2*cos(2*q3)*sin(2*q2)*cos(q5))/312500000 -
(7695921*qd1*qd3*cos(2*q2)*cos(2*q3)*sin(q4))/312500000 +
(82611*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(q5))/625000000 -
(33291*qdd1*cos(2*q2)*cos(q3)*cos(q4)*sin(q5))/125000000 -
(1233*qd2^2*cos(q2)*cos(q3)*cos(q4)*sin(q5))/10000000 -
48
(1233*qd4^2*cos(q2)*cos(q3)*cos(q4)*sin(q5))/10000000 +
(823641*qd1*qd2*cos(2*q2)*sin(2*q3)*sin(q4))/6250000 -
(823641*qd1*qd2*cos(2*q3)*sin(2*q2)*sin(q4))/6250000 +
(3*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q4))/5000 +
(8631*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q5))/6250000 -
(73597*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q4)*cos(2*q5))/1250000000 -
(25893*qdd2*cos(2*q4)*cos(q2)*sin(q3)*sin(q5))/156250000 -
(25893*qdd2*cos(2*q4)*cos(q3)*sin(q2)*sin(q5))/156250000 +
(73597*qdd2*sin(2*q5)*cos(q2)*cos(q3)*sin(q4))/312500000 -
49
(73597*qdd4*sin(2*q5)*cos(q3)*cos(q4)*sin(q2))/312500000 +
(8631*qd2^2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/12500000 +
(8631*qd3^2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/12500000 -
(8631*qd4^2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/12500000 -
(7695921*qd1*qd2*sin(2*q2)*sin(2*q3)*sin(q4))/312500000 +
(7695921*qd1*qd3*sin(2*q2)*sin(2*q3)*sin(q4))/312500000 -
(82611*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(q5))/625000000 +
(33291*qdd1*sin(2*q2)*cos(q4)*sin(q3)*sin(q5))/125000000 -
(82611*qd2^2*cos(q2)*sin(q3)*sin(q4)*sin(q5))/625000000 -
(82611*qd2^2*cos(q3)*sin(q2)*sin(q4)*sin(q5))/625000000 +
(823641*qd2*qd3*cos(q2)*cos(q3)*cos(q4))/6250000 -
(33291*qd1*qd5*cos(q3)*cos(q4)*cos(q5))/125000000 +
(73597*qdd1*cos(2*q4)*cos(2*q5)*sin(2*q2)*sin(2*q3))/1250000000 -
(73597*qdd2*sin(2*q5)*sin(q2)*sin(q3)*sin(q4))/312500000 -
(8631*qd2^2*sin(q2)*sin(q3)*sin(q4)*sin(q5))/12500000 -
50
(8631*qd3^2*sin(q2)*sin(q3)*sin(q4)*sin(q5))/12500000 +
(117663*qd2*qd3*cos(q2)*cos(q3)*sin(q4))/5000000 -
(1233*qd2*qd3*cos(q3)*cos(q5)*sin(q2))/5000000 +
(3*qd2*qd3*cos(q2)*sin(q3)*sin(q4))/5000 +
(3*qd2*qd3*cos(q3)*sin(q2)*sin(q4))/5000 -
(5305749*qd1*qd2*cos(2*q2)*cos(2*q4)*sin(2*q3))/625000000 +
51
(220791*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(2*q5))/625000000 -
(117663*qd2*qd3*sin(q2)*sin(q3)*sin(q4))/5000000 +
(1233*qd3*qd5*sin(q2)*sin(q3)*sin(q5))/5000000 -
(8631*qdd1*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/12500000 -
(25893*qd2^2*cos(2*q4)*cos(q2)*cos(q3)*sin(q5))/156250000 -
(25893*qd3^2*cos(2*q4)*cos(q2)*cos(q3)*sin(q5))/156250000 -
(25893*qd2*qd3*cos(2*q4)*cos(q2)*cos(q3)*sin(q5))/78125000 -
(25893*qd2*qd5*cos(2*q4)*cos(q3)*cos(q5)*sin(q2))/156250000 +
(33291*qd1*qd2*cos(2*q2)*cos(q4)*sin(q3)*sin(q5))/62500000 +
(33291*qd1*qd2*sin(2*q2)*cos(q3)*cos(q4)*sin(q5))/62500000 +
52
(73597*qd1*qd2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3))/625000000 +
(25893*qd2*qd3*cos(2*q4)*sin(q2)*sin(q3)*sin(q5))/78125000 -
(73597*qd2*qd3*sin(2*q5)*cos(q2)*sin(q3)*sin(q4))/156250000 -
(73597*qd2*qd3*sin(2*q5)*cos(q3)*sin(q2)*sin(q4))/156250000 +
(73597*qd2*qd5*cos(2*q5)*sin(q2)*sin(q3)*sin(q4))/156250000 +
(73597*qd3*qd5*cos(2*q5)*sin(q2)*sin(q3)*sin(q4))/156250000 -
(qd4*qd6*sin(2*q6)*cos(q2)*sin(q3)*sin(q4))/2500 -
(qd4*qd6*sin(2*q6)*cos(q3)*sin(q2)*sin(q4))/2500 -
(33291*qd1*qd4*sin(2*q2)*sin(q3)*sin(q4)*sin(q5))/125000000 -
(73597*qd1*qd4*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4))/625000000 -
(73597*qd1*qd5*cos(2*q4)*sin(2*q2)*sin(2*q3)*sin(2*q5))/625000000 -
(8631*qd1*qd5*cos(2*q2)*cos(2*q3)*cos(q4)*cos(q5))/12500000 +
(82611*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/312500000 -
(25893*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q5)*sin(q4))/78125000 +
(25893*qd1*qd4*cos(2*q2)*sin(2*q3)*cos(q4)*cos(q5))/156250000 -
(25893*qd1*qd4*cos(2*q3)*sin(2*q2)*cos(q4)*cos(q5))/156250000 +
53
(qd4*qd6*cos(2*q5)*sin(2*q6)*cos(q2)*cos(q3))/5000 +
(8631*qd1*qd2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/6250000 +
(8631*qd1*qd4*cos(2*q2)*cos(2*q3)*sin(q4)*sin(q5))/12500000 +
(8631*qd1*qd5*sin(2*q2)*sin(2*q3)*cos(q4)*cos(q5))/12500000 -
(82611*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/312500000 +
(25893*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q5)*sin(q4))/78125000 -
(82611*qd1*qd4*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/625000000 -
(73597*qd2*qd3*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/312500000 +
(25893*qd1*qd5*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/156250000 +
(qd4*qd6*cos(2*q5)*sin(2*q6)*sin(q2)*sin(q3))/5000 -
(8631*qd1*qd4*sin(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/12500000 +
(8631*qd4*qd5*cos(q2)*cos(q3)*cos(q4)*cos(q5))/6250000 -
(1233*qd2*qd3*cos(q2)*cos(q3)*cos(q4)*sin(q5))/5000000 -
54
(82611*qd4*qd5*cos(q3)*cos(q4)*cos(q5)*sin(q2))/312500000 +
(qd4*qd6*cos(q2)*cos(q4)*cos(q5)*sin(q3))/1000 +
(qd4*qd6*cos(q3)*cos(q4)*cos(q5)*sin(q2))/1000 +
(8631*qd2*qd3*cos(q2)*cos(q3)*sin(q4)*sin(q5))/6250000 -
(8631*qd4*qd5*cos(q4)*cos(q5)*sin(q2)*sin(q3))/6250000 +
(73597*qd1*qd2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/156250000 +
(73597*qd1*qd3*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/156250000 -
(25893*qd1*qd4*cos(2*q2)*cos(2*q3)*cos(2*q4)*sin(q5))/156250000 -
(73597*qd1*qd5*cos(2*q3)*cos(2*q5)*sin(2*q2)*cos(q4))/156250000 -
(82611*qd2*qd3*cos(q2)*sin(q3)*sin(q4)*sin(q5))/312500000 -
(82611*qd2*qd3*cos(q3)*sin(q2)*sin(q4)*sin(q5))/312500000 +
(qd4*qd6*cos(q2)*sin(q3)*sin(q4)*sin(q5))/1000 -
55
(qd4*qd6*cos(q3)*sin(q2)*sin(q4)*sin(q5))/1000 -
(8631*qd2*qd3*sin(q2)*sin(q3)*sin(q4)*sin(q5))/6250000 +
(25893*qd1*qd2*cos(2*q2)*sin(2*q3)*sin(2*q4)*sin(q5))/156250000 +
(25893*qd1*qd2*cos(2*q3)*sin(2*q2)*sin(2*q4)*sin(q5))/156250000 -
(73597*qd1*qd2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/156250000 +
(73597*qd1*qd3*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/156250000 -
(25893*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(2*q4)*cos(q5))/312500000 -
(33291*qd1*qd5*cos(2*q2)*cos(q3)*cos(q4)*cos(q5))/125000000 +
(qd4*qd6*sin(2*q5)*sin(2*q6)*cos(q2)*cos(q4)*sin(q3))/5000 +
(qd4*qd6*sin(2*q5)*sin(2*q6)*cos(q3)*cos(q4)*sin(q2))/5000 +
(qd4*qd6*cos(2*q6)*cos(q2)*cos(q4)*cos(q5)*sin(q3))/2500 +
(qd4*qd6*cos(2*q6)*cos(q3)*cos(q4)*cos(q5)*sin(q2))/2500 +
(qd4*qd6*cos(2*q6)*cos(q2)*sin(q3)*sin(q4)*sin(q5))/2500 +
(qd4*qd6*cos(2*q6)*cos(q3)*sin(q2)*sin(q4)*sin(q5))/2500
F2= (3782891779*qdd2)/2500000000 + (2676462679*qdd3)/2500000000 -

(4168141*cos(q2))/312500 - (4944379*sin(q2))/1250000 -
(5305749*qdd2*cos(2*q4))/625000000 + (73597*qdd2*cos(2*q5))/625000000 -
(5305749*qdd3*cos(2*q4))/625000000 + (73597*qdd3*cos(2*q5))/625000000 -
(36322911*qd3^2*cos(q3))/500000000 + (3*qd4^2*cos(q4))/10000 -
(8631*qd5^2*cos(q5))/12500000 - (157597023*cos(q2)*cos(q3))/10000000 -
(3794931*qd3^2*sin(q3))/10000000 + (7695921*qd4^2*sin(q4))/625000000 +
(22363027*cos(q2)*sin(q3))/5000000 + (22363027*cos(q3)*sin(q2))/5000000 +
(157597023*sin(q2)*sin(q3))/10000000 + (qd1^2*cos(2*q2))/2500 +
(7164291*qd1^2*sin(2*q2))/50000000 - (3*qdd1*cos(q2))/2500 +
56
(3794931*qdd2*cos(q3))/5000000 + (3794931*qdd3*cos(q3))/10000000 +
(82611*qdd2*cos(q5))/312500000 + (82611*qdd3*cos(q5))/312500000 -
(7695921*qdd4*cos(q4))/625000000 + (167347*qdd5*cos(q4))/156250000 +
(334233*qdd1*sin(q2))/2000000 - (36322911*qdd2*sin(q3))/250000000 +
(823641*qdd2*sin(q4))/6250000 - (36322911*qdd3*sin(q3))/500000000 +
(823641*qdd3*sin(q4))/6250000 + (3*qdd4*sin(q4))/10000 -
(8631*qdd5*sin(q5))/12500000 + (904213*sin(q2)*sin(q3)*sin(q4))/1000000 -
(36322911*qd2*qd3*cos(q3))/250000000 + (823641*qd2*qd4*cos(q4))/6250000 +
(823641*qd3*qd4*cos(q4))/6250000 - (3794931*qd2*qd3*sin(q3))/5000000 -
(82611*qd2*qd5*sin(q5))/312500000 - (82611*qd3*qd5*sin(q5))/312500000 -
(167347*qd4*qd5*sin(q4))/156250000 - (73597*qdd2*cos(2*q4)*cos(2*q5))/625000000
- (73597*qdd3*cos(2*q4)*cos(2*q5))/625000000 +
(36322911*qd1^2*cos(2*q2)*cos(q3))/500000000 +
(3794931*qd1^2*cos(2*q2)*sin(q3))/10000000 +
(3794931*qd1^2*sin(2*q2)*cos(q3))/10000000 -
(73597*qd4^2*sin(2*q5)*cos(q4))/312500000 -
(36322911*qd1^2*sin(2*q2)*sin(q3))/500000000 +
(1345293*qdd1*cos(q2)*cos(q3))/40000000 -
(25893*qdd4*cos(q4)*cos(q5))/156250000 + (82611*qdd5*cos(q4)*cos(q5))/625000000
+ (5305749*qd2*qd4*sin(2*q4))/312500000 +
(5305749*qd3*qd4*sin(2*q4))/312500000 - (73597*qd2*qd5*sin(2*q5))/312500000 -
(73597*qd3*qd5*sin(2*q5))/312500000 + (140553*qdd1*cos(q2)*sin(q3))/800000 +
+ (3176901*qdd2*cos(q3)*sin(q4))/62500000 -
- (8631*qdd2*cos(q4)*sin(q5))/6250000 - (33291*qdd3*cos(q5)*sin(q3))/125000000 +
(3176901*qdd4*cos(q4)*sin(q3))/125000000 - (8631*qdd3*cos(q4)*sin(q5))/6250000 -
(33291*qdd5*cos(q3)*sin(q5))/125000000 +
(9682051*qd1^2*cos(2*q2)*cos(2*q3))/50000000 -
(1345293*qdd1*sin(q2)*sin(q3))/40000000 - (82611*qdd4*sin(q4)*sin(q5))/625000000 -
57
(25893*qdd5*sin(q4)*sin(q5))/156250000 +
(474503173*qd1^2*cos(2*q2)*sin(2*q3))/1000000000 +
(474503173*qd1^2*cos(2*q3)*sin(2*q2))/1000000000 -
(9682051*qd1^2*sin(2*q2)*sin(2*q3))/50000000 -
(33291*qd3^2*cos(q3)*cos(q5))/125000000 -
(33291*qd5^2*cos(q3)*cos(q5))/125000000 -
(83237*cos(q2)*cos(q3)*cos(q4))/2500000 - (82611*qd4^2*cos(q4)*sin(q5))/625000000
+ (25893*qd4^2*cos(q5)*sin(q4))/156250000 -
(82611*qd5^2*cos(q4)*sin(q5))/625000000 -
(25893*qd5^2*cos(q5)*sin(q4))/156250000 -
(904213*cos(q2)*cos(q3)*sin(q4))/1000000 + (3687*cos(q2)*cos(q5)*sin(q3))/1000000
+ (3687*cos(q3)*cos(q5)*sin(q2))/1000000 -
(25893*qdd2*sin(2*q4)*sin(q5))/156250000 -
(25893*qdd3*sin(2*q4)*sin(q5))/156250000 -
(73597*qdd4*sin(2*q5)*sin(q4))/312500000 -
(3176901*qd3^2*sin(q3)*sin(q4))/125000000 -
(3176901*qd4^2*sin(q3)*sin(q4))/125000000 +
(83237*cos(q4)*sin(q2)*sin(q3))/2500000 + (2733*cos(q2)*sin(q3)*sin(q5))/5000000 +
(2733*cos(q3)*sin(q2)*sin(q5))/5000000 - (137*cos(q2)*sin(q3)*sin(q6))/5000000 -
(137*cos(q3)*sin(q2)*sin(q6))/5000000 - (3176901*qd1*qd4*sin(q2)*sin(q4))/62500000
- (3176901*qd2*qd3*sin(q3)*sin(q4))/62500000 +
(8631*qd2*qd4*sin(q4)*sin(q5))/6250000 +
(33291*qd2*qd5*sin(q3)*sin(q5))/62500000 +
(8631*qd3*qd4*sin(q4)*sin(q5))/6250000 +
(33291*qd3*qd5*sin(q3)*sin(q5))/62500000 +
(33291*qd1^2*cos(2*q2)*cos(q3)*cos(q5))/125000000 +
(3176901*qd1^2*cos(2*q2)*sin(q3)*sin(q4))/125000000 +
(3176901*qd1^2*sin(2*q2)*cos(q3)*sin(q4))/125000000 -
(33291*qd1^2*sin(2*q2)*cos(q5)*sin(q3))/125000000 +
58
(25893*qd2*qd4*cos(2*q4)*sin(q5))/78125000 -
(25893*qd2*qd5*sin(2*q4)*cos(q5))/156250000 -
(25893*qd3*qd4*cos(2*q4)*sin(q5))/78125000 -
(25893*qd3*qd5*sin(2*q4)*cos(q5))/156250000 -
(73597*qd4*qd5*cos(2*q5)*sin(q4))/156250000 + (qd4*qd6*sin(2*q6)*cos(q4))/2500 -
(3*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4))/10000 +
(8631*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5))/12500000 +
(7695921*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4))/625000000 -
(82611*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5))/625000000 -
(82611*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5))/625000000 +
(823641*qd1^2*cos(2*q2)*sin(2*q3)*sin(q4))/12500000 +
(823641*qd1^2*cos(2*q3)*sin(2*q2)*sin(q4))/12500000 -
(3*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4))/10000 -
(8631*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5))/12500000 -
(7695921*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4))/625000000 -
(2733*cos(q2)*cos(q3)*cos(q4)*cos(q5))/5000000 +
59
(73597*qd2*qd4*cos(2*q5)*sin(2*q4))/312500000 +
(73597*qd2*qd5*cos(2*q4)*sin(2*q5))/312500000 +
(73597*qd3*qd4*cos(2*q5)*sin(2*q4))/312500000 +
(73597*qd3*qd5*cos(2*q4)*sin(2*q5))/312500000 +
(3687*cos(q2)*cos(q3)*cos(q4)*sin(q5))/1000000 +
(33291*qd3^2*cos(q4)*sin(q3)*sin(q5))/125000000 +
(33291*qd4^2*cos(q4)*sin(q3)*sin(q5))/125000000 +
(33291*qd5^2*cos(q4)*sin(q3)*sin(q5))/125000000 +
(2733*cos(q4)*cos(q5)*sin(q2)*sin(q3))/5000000 +
(137*cos(q3)*cos(q6)*sin(q2)*sin(q5))/5000000 -
(5305749*qd1^2*cos(2*q2)*cos(2*q4)*sin(2*q3))/1250000000 -
(5305749*qd1^2*cos(2*q3)*cos(2*q4)*sin(2*q2))/1250000000 -
(220791*qd1^2*cos(2*q2)*cos(2*q5)*sin(2*q3))/1250000000 -
(220791*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2))/1250000000 -
(3687*cos(q4)*sin(q2)*sin(q3)*sin(q5))/1000000 -
(33291*qd2*qd3*cos(q3)*cos(q5))/62500000 +
(3176901*qd2*qd4*cos(q3)*cos(q4))/62500000 +
(3176901*qd3*qd4*cos(q3)*cos(q4))/62500000 -
(8631*qd2*qd5*cos(q4)*cos(q5))/6250000 - (8631*qd3*qd5*cos(q4)*cos(q5))/6250000
- (137*sin(q2)*sin(q3)*sin(q4)*sin(q6))/5000000 -
(6171693*qd1*qd4*cos(q2)*sin(q3))/312500000 -
(6171693*qd1*qd4*cos(q3)*sin(q2))/312500000 -
(82611*qd4*qd5*cos(q5)*sin(q4))/312500000 + (qd4*qd6*cos(q4)*sin(q5))/1000 +
(qd4*qd6*cos(q5)*sin(q4))/1000 +
60
(33291*qd1^2*cos(2*q2)*cos(q4)*sin(q3)*sin(q5))/125000000 -
(33291*qd1^2*sin(2*q2)*cos(q3)*cos(q4)*sin(q5))/125000000 -
(73597*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3))/1250000000 -
(73597*qd1^2*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2))/1250000000 +
(qd4*qd6*cos(2*q6)*cos(q4)*sin(q5))/2500 +
(82611*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/625000000 +
(8631*qd1^2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/12500000 -
(82611*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/625000000 -
(137*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6))/5000000 -
(137*cos(q4)*cos(q5)*cos(q6)*sin(q2)*sin(q3))/5000000 +
(qd4*qd6*sin(2*q5)*sin(2*q6)*sin(q4))/5000 -
61
(73597*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/312500000 -
(25893*qd1^2*cos(2*q2)*sin(2*q3)*sin(2*q4)*sin(q5))/312500000 -
(25893*qd1^2*cos(2*q3)*sin(2*q2)*sin(2*q4)*sin(q5))/312500000 +
(73597*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/312500000 -
(73597*qd1*qd4*sin(2*q5)*cos(q2)*cos(q3)*cos(q4))/156250000 -
62
(82611*qd1*qd5*cos(q5)*sin(q2)*sin(q3)*sin(q4))/312500000
F3= (2676462679*qdd2)/2500000000 + (2676462679*qdd3)/2500000000 -

(5305749*qdd2*cos(2*q4))/625000000 + (73597*qdd2*cos(2*q5))/625000000 -
(5305749*qdd3*cos(2*q4))/625000000 + (73597*qdd3*cos(2*q5))/625000000 +
(36322911*qd1^2*cos(q3))/1000000000 + (36322911*qd2^2*cos(q3))/500000000 +
(3*qd4^2*cos(q4))/10000 - (8631*qd5^2*cos(q5))/12500000 -
(157597023*cos(q2)*cos(q3))/10000000 + (3794931*qd1^2*sin(q3))/20000000 +
(3794931*qd2^2*sin(q3))/10000000 + (7695921*qd4^2*sin(q4))/625000000 +
(22363027*cos(q2)*sin(q3))/5000000 + (22363027*cos(q3)*sin(q2))/5000000 +
(157597023*sin(q2)*sin(q3))/10000000 + (3794931*qdd2*cos(q3))/10000000 +
(82611*qdd2*cos(q5))/312500000 + (82611*qdd3*cos(q5))/312500000 -
(7695921*qdd4*cos(q4))/625000000 + (167347*qdd5*cos(q4))/156250000 -
(36322911*qdd2*sin(q3))/500000000 + (823641*qdd2*sin(q4))/6250000 +
(823641*qdd3*sin(q4))/6250000 + (3*qdd4*sin(q4))/10000 -
(8631*qdd5*sin(q5))/12500000 + (904213*sin(q2)*sin(q3)*sin(q4))/1000000 +
63
(823641*qd2*qd4*cos(q4))/6250000 + (823641*qd3*qd4*cos(q4))/6250000 -
(82611*qd2*qd5*sin(q5))/312500000 - (82611*qd3*qd5*sin(q5))/312500000 -
(167347*qd4*qd5*sin(q4))/156250000 - (73597*qdd2*cos(2*q4)*cos(2*q5))/625000000
- (73597*qdd3*cos(2*q4)*cos(2*q5))/625000000 +
(36322911*qd1^2*cos(2*q2)*cos(q3))/1000000000 +
(3794931*qd1^2*cos(2*q2)*sin(q3))/20000000 +
(3794931*qd1^2*sin(2*q2)*cos(q3))/20000000 -
(73597*qd4^2*sin(2*q5)*cos(q4))/312500000 -
(36322911*qd1^2*sin(2*q2)*sin(q3))/1000000000 +
(1345293*qdd1*cos(q2)*cos(q3))/40000000 -
+ (5305749*qd2*qd4*sin(2*q4))/312500000 +
(5305749*qd3*qd4*sin(2*q4))/312500000 - (73597*qd2*qd5*sin(2*q5))/312500000 -
(73597*qd3*qd5*sin(2*q5))/312500000 + (140553*qdd1*cos(q2)*sin(q3))/800000 +
- (33291*qdd2*cos(q5)*sin(q3))/125000000 - (8631*qdd2*cos(q4)*sin(q5))/6250000 -
(8631*qdd3*cos(q4)*sin(q5))/6250000 +
(9682051*qd1^2*cos(2*q2)*cos(2*q3))/50000000 -
(1345293*qdd1*sin(q2)*sin(q3))/40000000 - (82611*qdd4*sin(q4)*sin(q5))/625000000 -
(25893*qdd5*sin(q4)*sin(q5))/156250000 +
(474503173*qd1^2*cos(2*q2)*sin(2*q3))/1000000000 +
(474503173*qd1^2*cos(2*q3)*sin(2*q2))/1000000000 -
(9682051*qd1^2*sin(2*q2)*sin(2*q3))/50000000 +
(33291*qd1^2*cos(q3)*cos(q5))/250000000 +
(33291*qd2^2*cos(q3)*cos(q5))/125000000 -
(83237*cos(q2)*cos(q3)*cos(q4))/2500000 - (82611*qd4^2*cos(q4)*sin(q5))/625000000
+ (25893*qd4^2*cos(q5)*sin(q4))/156250000 -
(82611*qd5^2*cos(q4)*sin(q5))/625000000 -
(25893*qd5^2*cos(q5)*sin(q4))/156250000 -
(904213*cos(q2)*cos(q3)*sin(q4))/1000000 + (3687*cos(q2)*cos(q5)*sin(q3))/1000000
64
+ (3687*cos(q3)*cos(q5)*sin(q2))/1000000 -
(25893*qdd2*sin(2*q4)*sin(q5))/156250000 -
(25893*qdd3*sin(2*q4)*sin(q5))/156250000 -
(73597*qdd4*sin(2*q5)*sin(q4))/312500000 +
(3176901*qd1^2*sin(q3)*sin(q4))/250000000 +
(3176901*qd2^2*sin(q3)*sin(q4))/125000000 +
(2733*cos(q3)*sin(q2)*sin(q5))/5000000 - (137*cos(q2)*sin(q3)*sin(q6))/5000000 -
(137*cos(q3)*sin(q2)*sin(q6))/5000000 + (8631*qd2*qd4*sin(q4)*sin(q5))/6250000 +
(8631*qd3*qd4*sin(q4)*sin(q5))/6250000 +
(33291*qd1^2*cos(2*q2)*cos(q3)*cos(q5))/250000000 +
(3176901*qd1^2*cos(2*q2)*sin(q3)*sin(q4))/250000000 +
(3176901*qd1^2*sin(2*q2)*cos(q3)*sin(q4))/250000000 -
(33291*qd1^2*sin(2*q2)*cos(q5)*sin(q3))/250000000 +
(25893*qd2*qd4*cos(2*q4)*sin(q5))/78125000 -
(25893*qd2*qd5*sin(2*q4)*cos(q5))/156250000 -
(25893*qd3*qd4*cos(2*q4)*sin(q5))/78125000 -
(25893*qd3*qd5*sin(2*q4)*cos(q5))/156250000 -
(73597*qd4*qd5*cos(2*q5)*sin(q4))/156250000 + (qd4*qd6*sin(2*q6)*cos(q4))/2500 -
(3*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4))/10000 +
(8631*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5))/12500000 +
65
(7695921*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4))/625000000 -
(82611*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5))/625000000 -
(82611*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5))/625000000 +
(823641*qd1^2*cos(2*q2)*sin(2*q3)*sin(q4))/12500000 +
(823641*qd1^2*cos(2*q3)*sin(2*q2)*sin(q4))/12500000 -
(3*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4))/10000 -
(8631*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5))/12500000 -
(7695921*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4))/625000000 -
(2733*cos(q2)*cos(q3)*cos(q4)*cos(q5))/5000000 +
(73597*qd2*qd4*cos(2*q5)*sin(2*q4))/312500000 +
(73597*qd2*qd5*cos(2*q4)*sin(2*q5))/312500000 +
(73597*qd3*qd4*cos(2*q5)*sin(2*q4))/312500000 +
(73597*qd3*qd5*cos(2*q4)*sin(2*q5))/312500000 +
(5305749*qdd1*sin(2*q4)*cos(q3)*sin(q2))/625000000 -
(33291*qd1^2*cos(q4)*sin(q3)*sin(q5))/250000000 -
(33291*qd2^2*cos(q4)*sin(q3)*sin(q5))/125000000 +
(5305749*qd1^2*cos(2*q2)*cos(2*q4)*sin(2*q3))/1250000000 -
(5305749*qd1^2*cos(2*q3)*cos(2*q4)*sin(2*q2))/1250000000 -
(220791*qd1^2*cos(2*q2)*cos(2*q5)*sin(2*q3))/1250000000 -
(220791*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2))/1250000000 -
(8631*qd2*qd5*cos(q4)*cos(q5))/6250000 - (8631*qd3*qd5*cos(q4)*cos(q5))/6250000
66
- (137*sin(q2)*sin(q3)*sin(q4)*sin(q6))/5000000 -
(6171693*qd1*qd4*cos(q2)*sin(q3))/312500000 -
(6171693*qd1*qd4*cos(q3)*sin(q2))/312500000 -
(82611*qd4*qd5*cos(q5)*sin(q4))/312500000 + (qd4*qd6*cos(q4)*sin(q5))/1000 +
(qd4*qd6*cos(q5)*sin(q4))/1000 +
(33291*qd1^2*sin(2*q2)*cos(q3)*cos(q4)*sin(q5))/250000000 -
(73597*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3))/1250000000 -
67
(137*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6))/5000000 -
(qd4*qd6*sin(2*q5)*sin(2*q6)*sin(q4))/5000 -
(73597*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/312500000 -
(73597*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/312500000 -
68
F4= (6171693*qdd4)/312500000 - (73597*qdd4*cos(2*q5))/312500000 -

(823641*qd1^2*cos(q4))/25000000 - (823641*qd2^2*cos(q4))/12500000 -
(823641*qd3^2*cos(q4))/12500000 + (117663*qd1^2*sin(q4))/10000000 +
(25893*qd5^2*sin(q5))/156250000 + (5305749*qd1^2*sin(2*q4))/1250000000 -
(5305749*qd2^2*sin(2*q4))/625000000 - (5305749*qd3^2*sin(2*q4))/625000000 -
(7695921*qdd2*cos(q4))/625000000 - (7695921*qdd3*cos(q4))/625000000 -
(25893*qdd5*cos(q5))/156250000 + (3*qdd2*sin(q4))/10000 + (3*qdd3*sin(q4))/10000
- (823641*qd2*qd3*cos(q4))/6250000 + (167347*qd2*qd5*sin(q4))/156250000 +
(167347*qd3*qd5*sin(q4))/156250000 - (25893*qd1^2*cos(2*q4)*sin(q5))/312500000 +
(25893*qd2^2*cos(2*q4)*sin(q5))/156250000 +
(25893*qd3^2*cos(2*q4)*sin(q5))/156250000 -
(6171693*qdd1*cos(q2)*cos(q3))/312500000 -
(25893*qdd2*cos(q4)*cos(q5))/156250000 - (25893*qdd3*cos(q4)*cos(q5))/156250000
- (5305749*qd2*qd3*sin(2*q4))/312500000 + (73597*qd4*qd5*sin(2*q5))/156250000 -
(qd4*qd6*sin(2*q6))/5000 - (3176901*qdd1*cos(q2)*sin(q4))/125000000 +
69
(3176901*qdd2*cos(q4)*sin(q3))/125000000 +
(6171693*qdd1*sin(q2)*sin(q3))/312500000 - (82611*qdd2*sin(q4)*sin(q5))/625000000
- (82611*qdd3*sin(q4)*sin(q5))/625000000 +
(73597*qd1^2*cos(2*q5)*sin(2*q4))/1250000000 -
(73597*qd2^2*cos(2*q5)*sin(2*q4))/625000000 -
(73597*qd3^2*cos(2*q5)*sin(2*q4))/625000000 -
(3176901*qd1^2*cos(q3)*cos(q4))/250000000 -
(3176901*qd2^2*cos(q3)*cos(q4))/125000000 -
(1233*qd1^2*cos(q4)*sin(q5))/10000000 - (904213*cos(q2)*cos(q4)*sin(q3))/1000000 -
(904213*cos(q3)*cos(q4)*sin(q2))/1000000 -
(73597*qdd2*sin(2*q5)*sin(q4))/312500000 -
(73597*qdd3*sin(2*q5)*sin(q4))/312500000 - (8631*qd1^2*sin(q4)*sin(q5))/25000000 -
(8631*qd2^2*sin(q4)*sin(q5))/12500000 - (8631*qd3^2*sin(q4)*sin(q5))/12500000 +
(3176901*qd1*qd2*sin(q2)*sin(q4))/62500000 -
(8631*qd2*qd3*sin(q4)*sin(q5))/6250000 -
(3176901*qd1^2*cos(2*q2)*cos(q3)*cos(q4))/250000000 +
(3176901*qd1^2*sin(2*q2)*cos(q4)*sin(q3))/250000000 -
(25893*qd2*qd3*cos(2*q4)*sin(q5))/78125000 -
(73597*qd2*qd5*cos(2*q5)*sin(q4))/156250000 -
(73597*qd3*qd5*cos(2*q5)*sin(q4))/156250000 -
(3*qdd1*cos(q2)*cos(q4)*sin(q3))/10000 + (3*qdd1*cos(q3)*cos(q4)*sin(q2))/10000 +
(823641*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4))/25000000 +
(7695921*qd1^2*cos(2*q2)*sin(2*q3)*cos(q4))/1250000000 +
70
(7695921*qd1^2*cos(2*q3)*sin(2*q2)*cos(q4))/1250000000 +
(33291*qdd2*sin(q3)*sin(q4)*sin(q5))/125000000 -
(3*qd1^2*cos(2*q2)*sin(2*q3)*sin(q4))/20000 -
(3*qd1^2*cos(2*q3)*sin(2*q2)*sin(q4))/20000 +
(823641*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4))/25000000 +
(73597*qdd1*cos(2*q5)*cos(q2)*cos(q3))/312500000 -
(73597*qd2*qd3*cos(2*q5)*sin(2*q4))/312500000 +
(qd4*qd6*cos(2*q5)*sin(2*q6))/5000 -
(73597*qdd1*cos(2*q5)*sin(q2)*sin(q3))/312500000 -
(33291*qd1^2*cos(q3)*sin(q4)*sin(q5))/250000000 -
(33291*qd2^2*cos(q3)*sin(q4)*sin(q5))/125000000 +
(5305749*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q4))/1250000000 -
(3687*cos(q3)*sin(q2)*sin(q4)*sin(q5))/1000000 +
(6171693*qd1*qd2*cos(q2)*sin(q3))/312500000 +
(6171693*qd1*qd2*cos(q3)*sin(q2))/312500000 +
(6171693*qd1*qd3*cos(q2)*sin(q3))/312500000 +
(6171693*qd1*qd3*cos(q3)*sin(q2))/312500000 +
(25893*qd2*qd5*cos(q4)*sin(q5))/78125000 +
(25893*qd3*qd5*cos(q4)*sin(q5))/78125000 +
(5305749*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q4))/1250000000 -
(33291*qd1^2*sin(2*q2)*sin(q3)*sin(q4)*sin(q5))/250000000 -
71
(73597*qd1^2*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4))/1250000000 +
(8631*qdd1*cos(q2)*cos(q3)*cos(q4)*sin(q5))/12500000 -
(73597*qd1*qd5*sin(2*q5)*sin(q2)*sin(q3))/156250000 +
(25893*qd1^2*cos(2*q3)*sin(2*q2)*cos(q4)*cos(q5))/312500000 -
(8631*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4)*sin(q5))/25000000 +
(82611*qd1^2*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/1250000000 +
(82611*qd1^2*cos(2*q3)*sin(2*q2)*sin(q4)*sin(q5))/1250000000 +
(8631*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/25000000 -
(25893*qd1^2*cos(2*q2)*cos(2*q3)*cos(2*q4)*sin(q5))/312500000 +
72
73
F5= (167347*qdd5)/156250000 + (82611*qd2^2*sin(q5))/625000000 +

(82611*qd3^2*sin(q5))/625000000 - (73597*qd1^2*sin(2*q5))/312500000 +
(73597*qd2^2*sin(2*q5))/312500000 + (73597*qd3^2*sin(2*q5))/312500000 -
(73597*qd4^2*sin(2*q5))/312500000 + (167347*qdd2*cos(q4))/156250000 +
(167347*qdd3*cos(q4))/156250000 - (25893*qdd4*cos(q5))/156250000 -
(8631*qdd2*sin(q5))/12500000 - (8631*qdd3*sin(q5))/12500000 -
(3687*sin(q2)*sin(q3)*sin(q5))/1000000 + (82611*qd2*qd3*sin(q5))/312500000 -
(15059*qd2*qd4*sin(q4))/9765625 - (15059*qd3*qd4*sin(q4))/9765625 +
(7*qd4*qd6*sin(q5))/5000 + (82611*qd1^2*cos(q2)^2*sin(q5))/625000000 +
(82611*qd1^2*cos(q3)^2*sin(q5))/625000000 +
+ (73597*qd2*qd3*sin(2*q5))/156250000 - (33291*qdd2*cos(q3)*sin(q5))/125000000 -
(25893*qdd2*sin(q4)*sin(q5))/156250000 - (25893*qdd3*sin(q4)*sin(q5))/156250000 +
74
(8631*qd1^2*cos(q4)*cos(q5))/12500000 + (8631*qd2^2*cos(q4)*cos(q5))/12500000 +
(8631*qd3^2*cos(q4)*cos(q5))/12500000 - (2733*cos(q2)*cos(q3)*cos(q5))/5000000 -
(1233*qd1^2*cos(q5)*sin(q4))/10000000 + (3687*cos(q2)*cos(q3)*sin(q5))/1000000 -
(33291*qd2^2*sin(q3)*sin(q5))/125000000 + (2733*cos(q5)*sin(q2)*sin(q3))/5000000 -
(8631*qd1^2*cos(q2)^2*cos(q4)*cos(q5))/12500000 -
(8631*qd1^2*cos(q3)^2*cos(q4)*cos(q5))/12500000 +
(73597*qd1^2*cos(q2)^2*cos(q5)*sin(q5))/156250000 +
(73597*qd1^2*cos(q3)^2*cos(q5)*sin(q5))/156250000 -
(73597*qd2^2*cos(q4)^2*cos(q5)*sin(q5))/156250000 -
(73597*qd3^2*cos(q4)^2*cos(q5)*sin(q5))/156250000 -
(33291*qd1^2*cos(q2)^2*sin(q3)*sin(q5))/125000000 +
(73597*qd2*qd4*cos(q5)^2*sin(q4))/78125000 +
(73597*qd3*qd4*cos(q5)^2*sin(q4))/78125000 - (qd4*qd6*cos(q6)^2*sin(q5))/1250 +
(82611*qd1^2*cos(q2)^2*cos(q3)^2*sin(q5))/312500000 +
(33291*qd2^2*cos(q3)*cos(q4)*cos(q5))/125000000 -
(137*cos(q2)*cos(q3)*cos(q5)*cos(q6))/5000000 -
(73597*qd1^2*cos(q2)*cos(q4)*sin(q2))/156250000 -
(73597*qd1^2*cos(q3)*cos(q4)*sin(q3))/156250000 +
(25893*qd2^2*cos(q4)*cos(q5)*sin(q4))/156250000 +
(25893*qd3^2*cos(q4)*cos(q5)*sin(q4))/156250000 +
75
(8631*qd1^2*cos(q2)*sin(q2)*sin(q5))/12500000 +
(8631*qd1^2*cos(q3)*sin(q3)*sin(q5))/12500000 +
(8631*qd2*qd3*cos(q4)*cos(q5))/6250000 -
(25893*qd2*qd4*cos(q4)*sin(q5))/78125000 -
(25893*qd3*qd4*cos(q4)*sin(q5))/78125000 -
(8631*qd1^2*cos(q2)*cos(q3)^2*sin(q2)*sin(q5))/6250000 -
(8631*qd1^2*cos(q2)^2*cos(q3)*sin(q3)*sin(q5))/6250000 +
(1233*qdd1*cos(q2)*cos(q3)*cos(q4)*cos(q5))/10000000 -
(73597*qd2*qd3*cos(q4)^2*cos(q5)*sin(q5))/78125000 +
(8631*qdd1*cos(q2)*cos(q3)*cos(q5)*sin(q4))/12500000 +
(8631*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)*cos(q5))/6250000 -
(73597*qd1^2*cos(q2)^2*cos(q3)^2*cos(q5)*sin(q5))/78125000 +
(73597*qd1^2*cos(q2)^2*cos(q4)^2*cos(q5)*sin(q5))/156250000 +
(73597*qd1^2*cos(q3)^2*cos(q4)^2*cos(q5)*sin(q5))/156250000 -
76
(33291*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5))/125000000 +
(3*qd1*qd2*sin(q2)*sin(q3)*sin(q4))/5000 + (3*qd1*qd3*sin(q2)*sin(q3)*sin(q4))/5000
+ (73597*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*sin(q2))/78125000 +
(73597*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*sin(q3))/78125000 +
(73597*qd1^2*cos(q2)*cos(q4)*cos(q5)^2*sin(q2))/78125000 +
(73597*qd1^2*cos(q3)*cos(q4)*cos(q5)^2*sin(q3))/78125000 -
(25893*qd1^2*cos(q2)^2*cos(q4)*cos(q5)*sin(q4))/156250000 -
(25893*qd1^2*cos(q3)^2*cos(q4)*cos(q5)*sin(q4))/156250000 -
(73597*qd1*qd2*cos(q2)*cos(q3)*cos(q5)^2*sin(q4))/78125000 +
(25893*qd1*qd2*cos(q2)*cos(q4)^2*cos(q5)*sin(q3))/78125000 +
(25893*qd1*qd2*cos(q3)*cos(q4)^2*cos(q5)*sin(q2))/78125000 -
(73597*qd1*qd2*cos(q5)^2*sin(q2)*sin(q3)*sin(q4))/78125000 +
(73597*qd1*qd3*cos(q5)^2*sin(q2)*sin(q3)*sin(q4))/78125000 -
(73597*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)^2*sin(q2))/39062500 -
(73597*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)^2*sin(q3))/39062500 +
77
(25893*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)*cos(q5)*sin(q4))/78125000 -
(33291*qd1^2*cos(q2)*cos(q4)*cos(q5)*sin(q2)*sin(q3))/125000000 -
(73597*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*cos(q5)*sin(q5))/78125000 +
(82611*qd1^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)*sin(q5))/312500000 -
(82611*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)*sin(q2))/312500000 -
(82611*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)*sin(q3))/312500000 -
(25893*qd1^2*cos(q2)*cos(q3)^2*sin(q2)*sin(q4)*sin(q5))/78125000 -
(25893*qd1^2*cos(q2)^2*cos(q3)*sin(q3)*sin(q4)*sin(q5))/78125000 -
(8631*qd1^2*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q2)*sin(q3))/6250000 +
(73597*qd1^2*cos(q2)*cos(q3)*cos(q5)*sin(q2)*sin(q3)*sin(q5))/78125000 -
(25893*qd1^2*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4))/78125000 +
(73597*qd1*qd2*cos(q2)*cos(q4)*cos(q5)*sin(q3)*sin(q4)*sin(q5))/78125000 +
(73597*qd1^2*cos(q2)*cos(q3)*cos(q4)^2*cos(q5)*sin(q2)*sin(q3)*sin(q5))/78125000
78
F6= (137*cos(q6)*(cos(q2 + q3) + sin(q2 + q3)*sin(q4)))/5000000 +
(137*sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)))/5000000
Chương 3: Chọn động cơ
3.1.Thông số động cơ
Hình 3.1: Thông số động cơ 130ST-M15025
79
3.1.1.Thông số động cơ Khâu 1: 130ST-M15025
Moment quán tính phần ứng: Jm1= 2.77.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 = 25
Hệ số cản của cơ cấu dẫn động: Bm1= 0.00295 Nms/rad
Hệ số sức phản điện động: Kb1 = 0.67Vs/rad
Hệ số moment: Km1= 1.11 Nm/A
Điện trở phần ứng động cơ: R1= 0,48 ( )
Gia tốc trọng trường: g= 9.8m/s2

Moment quán tính phần ứng: Jm= 2.77.10-3 kgm2
Tỉ số truyền từ động cơ đến các khớp:r 2= 20
Hệ số cản của cơ cấu dẫn động: Bm= 0.00295 Nms/rad
Hệ số sức phản điện động: Kb= 0.67Vs/rad
Hệ số moment: Km= 1.11 Nm/A
Điện trở phần ứng động cơ: R= 0,48 ( )

80
Hệ số sức phản điện động: Kb = 0.67Vs/rad

Tỉ số truyền từ động cơ đến các khớp:r 4=10


81
 Với các 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:
{
J eff 1 θ̈ m 1+ Beff 1 θ̇m 1=u 1−d 1
J eff 2 θ̈ m2 + Beff 2 θ̇m 2=u 2−d 2
J eff 3 θ̈m3 + Beff 3 θ̇m 3=u 3−d 3
J eff 4 θ̈m 4 +B eff 4 θ̇m 4 =u4 −d 4
J eff 5 θ̈m5 + Beff 5 θ̇m 5=u 5−d 5
J eff 6 θ̈m6 + Beff 6 θ̇m 6 =u6−d6
Khâu 1 :
Kb1 Km1 K τ
J m 1 θ̈m 1 +( B ¿ ¿ m1+ ) θ̇ m 1= m 1 V − l 1 ¿
R1 R 1 1 r1
τ l 1 =F 1
[ ]
0,94 kb 1 km1 km1
J m 1+ θ̈ m 1 +(B ¿ ¿ m1+ ) θ̇ m 1 = V −d ¿
r 21 r1 r1 1 1
0,94 −3 0,94
J eff 1=J m 1+ 2
=2,77.10 + 2 =0,0043
r1 25
Kb1. Km1 0,67 . 1,11
Beff 1=Bm 1 + =0.00295+ =1,5523
R1 0,48
K m1 1,11
u1¿ .V 1 = =2,3125 V 1
R1 0,48
1
d 1= *[F1
r1
82
Khâu 2 :
K b 2 K m2 K τ
J m 2 θ̈m 2 +(B ¿ ¿ m 2+ ) θ̇ m 2= m 2 V − l 2 ¿
R2 R2 2 r 2
τ l 2 =F 2
[ ]
1,5131 K b2 K m2 K m2
J m 2+ θ̈ m 2 +(B ¿ ¿ m2+ ) θ̇ m 2 = V −d ¿
2
r2 R2 R2 2 2
1,5131 −3 1,5131
J eff 2 =J m 2+ 2
=2,77. 10 + =0,0065
r2 202
Kb2. Km2 0,67 . 1,11
Beff 2=Bm 2 + =0.00295+ =1,5523
R2 0,48
K m2 1,11
u2¿ V2 = =2,3125V2
R2 0,48
1
d 2=
r 2 *[F2
Khâu 3:
K b 3 K m3 Km3 τl3
J m 3 θ̈m 3 +( B ¿ ¿ m 3+ ) θ̇m 3 = V − ¿
R3 R3 3 r 3
τ l 3 =F3
[ ]
1,0706 Kb3 K m3 K m3
J m 3+ θ̈m 3 +(B ¿ ¿ m3+ ) θ̇ m 3= V −d ¿
2
r3 R3 R3 3 3
1,0706 −3 1,0706
J eff 3 =J m 3 + 2
=2,77.10 + =0,0075
r3 152
K b 3 . K m3 0,67 . 1,11
Beff 3=Bm 3 + =0.00295+ =1,5523
R3 0,48
K m3 1,11
u3 = V 3= =2,3125V3
R3 0,48
1
d 3=
r 3 *[F3
Khâu 4:
K b4 K m4 K m4 τl4
J m 4 θ̈ m 4 +(B ¿ ¿ m 4+ ) θ̇m 4 = V − ¿
R4 R 4 4 r4
83
τ l 4=F 4
[ ]
0,0197 K b 4 K m4 K m4
J m4+ θ̈m 4 +(B ¿ ¿ m 4+ ) θ̇ m 4= V −d ¿
2
r4 R4 R4 4 4
0,0197 0,0197
J eff 4=J m 4+ 2
=2,77.10−3+ 2
=0,003
r4 10
Kb4 . Km4 0,67 . 1,11
Beff 4 =Bm 4 + =0.00295+ =1,5523
R4 0,48
K m4 1,11
K4¿ V4 = V =2,3125 V 4
R4 0,48 4
1
d4=
r 4 *[F4
Khâu 5:
K b5 Km5 K τ
J m 5 θ̈m 5 +( B ¿ ¿ m 5+ ) θ̇m 5 = m 5 V − l 5 ¿
R5 R5 5 r 5
τ l 5 =F5
[ ]
0,001 K b5 K m5 K m5
J m 5+ θ̈m5 +(B ¿ ¿ m 5+ ) θ̇m 5 = V −d ¿
2
r5 R5 R5 5 5
0,001 −3 0,001
J eff 5 =J m 5 + 2
=2,77. 10 + =0,0028
r5 10 2
K b 5 . K m5 0,67 . 1,11
Beff 5=Bm 5 + =0.00295+ =1,5523
R5 0,48
K m5 1,11
K5¿ V5 = V =2,3125V 5
R5 0,48 5
1
d 5=
r 5 *[F5
Khâu 6:
Kb6 K m6 Km6 τl6
J m 6 θ̈m 6 +(B ¿ ¿ m6+ ) θ̇m 6= V − ¿
r6 R6 6 r 6
τ l 6 =F6
84
[ J m6+
4,5.10−6
2
r6 ]
θ̈ m 6+( B ¿ ¿ m6 +
K b6 Km6
R6
) θ̇ m6 =
K m6
V −d ¿
R6 6 6
0 0
J eff 6 =J m 6 + 2
=2,77.10−3+ 2 =0,0027
r6 5
Kb6 . K m6 0,67 .1,11
Beff 6=B m 6+ =0.00295+ =1,5523
R6 0,48
K m6 1,11
K6¿ V 6= V =2,3125V 6
R6 0,48 6
1
d 6 = *[F6
r6
Ta thu được:
{
0,0043. θ̈m 1 +1,5523 . θ̇ m 1=2,3125.V 1 −d 1
0,0065. θ̈m 2 +1,5523 . θ̇ m 2=2,3125.V 2 −d 2
0,0075. θ̈ m 3 +1,5523. θ̇ m3 =2,3125.V 3 −d 3
0,003. θ̈ m 4 +1,5523 . θ̇m 4 =2,3125.V 4−d 4
0,0028. θ̈ m 5 +1,5523. θ̇ m5 =2,3125.V 5 −d 5
0,0027. θ̈ m 6+ 1,5523. θ̇m 6 =2,3125.V 6 −d 6
85
Chương 4: Thiết kế bộ điều khiển
4.1.Thiết kế bộ điều khiển PD và PID
(PT1)
0,0043. θ̈m 1 +1,5498 . θ̇ m 1=2,3125.V 1 −d 1
Điều khiện ổn định : e ss ≤ 0,01

t r ≤1( s)
PO ≤ 10 %
t s ≤3( s)
Bộ điều khiển PD có dạng :
U ( s)=K p ( θm (s)−θ m (s )) −K D . S . θm (s)
d
Phương trình hệ kín trở thành :

0,0043. s 2 θ m (s )+1,5523 . s .θ m (s )=2,3125. K p 1 ( θ dm ( s )−θm ( s ) )-2,3125. K d 1 . s . θm (s)- D(s)
¿> [ 0,0043. s2 +( 2,3125. K d 1+1,5523) . s+ 2,3125. K p 1 ] θm (s)=2,3125. K p 1 θ dm (s )−D (s)

2
Đặt 0,0043. s + ( 2,3125. K d 1+ 1,5523 ) . s+2,3125. K p 1=Ω(s)
Đa thức đặc trưng của hệ:
d d
K 1 . K p 1 θm 1 ( s) D(s) 2,3125. K p θm ( s) D(s )
θm 1 (s)= − = −
Ω(s) Ω(s) Ω(s) Ω(s)
86
d 0,0043. s2 + ( 2,3125. K d 1 +1,5523 ) . s d D(s )
E1 ( s)=θ ( s ) – θm ( s )=
m . θm (s)+¿
Ω(s) Ω (s)
Sai số xác lập:

Ta coi D(s)0,001 để thuận tiện cho việc thiết kế
D(s) = D/s.
e ss =lim s . E 1( s)=lim ⁡ s .
s→0 s→0 [ 0,0043. s2 + ( 2,3125. K d 1 +1,5523 ) . s d
Ω(s)
. θm (s)+
D(s )
Ω(s) ]
¿ lim
s→0 [ ] [ ]
D(s)
Ω(s)
s =lim
s→0
D
=
0,001
Ω(s ) 2,3125. K p 1
0,001
e ss = ≤ 0,01=¿ K p 1 ≥ 0,043
2,3125. K p 1
Đa thức đặc trưng :

2
Ω( s)=0,0043. s + ( 2,3125. K d 1 +1,5523 ) . s +2,3125. K p 1
2,3125. K d 1+ 1,5523 2,3125. K p 1
=0,0043.( s2 + s+ )
0,0043 0,0043
= 0,0043( s2 +2. ξ . W n . s +W 2n ¿
{
2,3125. K p 1 2
=W n
0,0043
2,3125. K d 1 +1,5523
=2. ξ . W n
↔
{
K p=0,0018.W 2n
K d =0,0037. ξ . W n−0,6713
0,0043
Độ quá điều chỉnh:
Có po =exp (√ )−ξ . π
1−ξ
2
≤0,1
−ξ . π 0,1
→ ≥e
√ 1−ξ 2
→ 0,11≤ ξ ≤ 1
4 4
Thời gian ổn định: t s= ξ .W ≤3 → ξ . W n ≥ 3
n
Thời gian tăng:
87
4
t r= [1,589 ξ 3−0,156 ξ2 +0,924 ξ ] ≤1
Wn
4
Chọn ξ=1 thì ξ . W n ≥
3
W n ≥ 1,333
Chọn W n =220
Khi đó:
{
2,3125. K p 1 2
=W n
0,0043
2,3125. K d 1 +1,5523
=2. ξ . W n
= {
K p1 =87,12
K d 1=0,1412
0,0043
Bộ điều khiển PID:

Với:
K p 1=87,12
K d 1=0,1412
( B eff + K . K d 1 ) . K p 1
Ki<
j eff
2
Đa thức đặc trưng: Ω( s)=0,0043. s + ( 2,3125. K d +1,5523 ) . s+2,3125. K p
¿ j eff s2 + ( 2,3125. K d + Beff ) . s+2,3125. K p
{
j eff =0,0043
¿> Beff =1,5523
K =2,3125
Do đó: K i <38136,1
Chọn K i=5
(PT2)
0,0065. θ̈m 2 +1,5523 . θ̇ m 2=2,3125.V 2 −d 2
Điều kiện ổn định :

Sai số xác lập: e ss ≤ 0,01
Thời gian ổn định: t s ≤3 s
88
Độ quá điều chỉnh : po ≤10 %
Thời gian tăng: t r ≤1 s

U ( s)=K p ( θdm (s)−θ m (s )) −K D . S . θm (s)
Pt hệ kín trở thành :

0,0065. s 2 θ m (s )+1,5523 . s .θ m (s )=2,3125. K p 2 ( θ dm ( s )−θm ( s ) )-2,3125. K d 2 . s . θm (s)- D (s)
¿> [ 0,0065. s2 +( 2,3125. K d 2+1,5523) . s+ 2,3125. K p 2 ] θm (s)=2,3125. K p 1 θ dm (s )−D (s)

2
Đặt 0,0065. s +(2,3125. K d 2 +1,5523). s+2,3125. K p 2=Ω(s)
d d
K 2 . K p 2 θm 1 ( s) D(s) 2,3125. K p θ m (s) D(s)
θm 2 (s)= − = −
Ω(s) Ω(s) Ω(s) Ω(s)
d 0,0065. s2 +(2,3125. K d 2+ 1,5523). s d D(s )

E2 ( s)=θ ( s ) – θm ( s )=
m . θm ( s)+¿
Ω(s ) Ω (s)

[ ]
2
0,0065 s + ( 2,3125 K d 2 +1,5523 ) s θ 2 D
e ss =lim s . E 2( s) =¿ lim ⁡ s . . +
s→0 s→0 Ω(s ) s s . Ω ( s)
¿ lim
s→0 [ 2
0,001
0,0065. s + ( 2,3125. K d 2 +1,5523 ) . s+ 2,3125. K p 2
=
]
0,001
2,3125. K p2
0,001
e ss = ≤ 0,01=¿ K p 2 ≥ 0,0432
2,3125. K p 2

Ω( s)=0,0065. s2 + ( 2,3125. K d 2 +1,5523 ) . s +2,3125. K p2
2,3125. K d 2+ 1,5523 2,3125. K p 2
=0,0065.( s2 + s+ )
0,0065 0,0065
= 0,0065( s2 +2. ξ . W n . s +W 2n ¿
89
{
2,3125. K p 2
=W 2n
0,0065
2,3125. K d 2 +1,5523
=2. ξ . W n
↔
{
K p =0,0028.W 2n
K d =0,0056. ξ . W n−0,6713
0,0065
1−ξ
2
≤0,1
−ξ . π 0,1
→ ≥e
√ 1−ξ 2
→ 0,11≤ ξ ≤ 1
4 4
n
Thời gian tăng:

4
t r= [1,589 ξ 3−0,156 ξ2 +0,924 ξ ] ≤1
Wn
4
3
W n ≥ 1,333
Chọn W n =165
Khi đó:
{
2,3125. K p 2
=W 2n
0,0065
2,3125. K d 2 +1,5498
=2. ξ . W n
= {
K p 2=76,23
K d 2=0,2527
0,0065

Với:
K p 2=76,23
K d 2=0,2527
( B eff + K . K d 2 ) . K p2
Ki<
j eff
2
90
2
¿ j eff s + ( 2,3125. K d + Beff ) . s+2,3125. K p
{
j eff =0,0065
¿> Beff =1,5523
K =2,3125
Do đó: K i <25058,2
Chọn K i=5
(PT3)
0,0075. θ̈m 3 +1,5523 . θ̇ m 3=2,3125.V 3−d 3


U ( t )=K p ( θdm−θm ) −K D . θm

0,0075. s θ m (s )+1,5523 . s .θ m (s )=2,3125. K p 3 ( θ m ( s )−θm ( s ) )-2,3125. K d 3 . s . θm (s)- D (s)
2 d
¿> [ 0,0075. s +( 2,3125. K d 3+ 1,5523) . s+ 2,3125. K p 3 ] θm (s)=2,3125. K p 3 θm (s)−D (s)

2 d
2
Đặt 0,0075. s +(2,3125. K d 3 +1,5523). s+2,3125. K p 3=Ω(s)
K 3 . K p 3 θmd 1 ( s) D(s ) 2,3125. K p θ dm (s ) D(s)
θm 3 (s)= − = −
Ω(s) Ω(s) Ω(s) Ω(s )
d 0,0075. s2 +(2,3125. K d 3+ 1,5523) . s d D(s )

E3 ( s)=θ ( s ) – θm ( s )=
m . θm ( s)+¿
Ω(s ) Ω(s)

91
e ss =lim s . E 3( s )=¿ lim ⁡ s .
s→0 s→0 [
0,0075 s 2+ ( 2,3125 K d 3 +1,5523 ) s θ 3
Ω ( s)
. +
D
s s . Ω( s ) ]
¿ lim
s→0 [ 0,001
] =
0,001
0,0075. s + ( 2,3125. K d 3 +1,5523 ) . s+ 2,3125. K p 3 2,3125. K p 3
2
0,001
e ss = ≤0,01=¿ K p 3 ≥ 0,043
2,3125. K p 3

Ω( s)=0,0075. s2 + ( 2,3125. K d 3 +1,5523 ) . s +2,3125. K p 3
2,3125. K d 3+ 1,5523 2,3125. K p 3
=0,0075.( s2 + s+ )
0,0075 0,0075
= 0,0075( s2 +2. ξ . W n . s +W 2n ¿
{
2,3125. K p3 2
=W n
{
2
0,0075 ↔ K p =0,0035.W n
2,3125. K d 3 +1,5523 K d =0,0065. ξ . W n −0,6713
=2. ξ .W n
0,0075
1−ξ
2
≤0,1
−ξ . π 0,02
→ ≥e
√ 1−ξ 2
→ 0,02≤ ξ ≤ 1
4 4
n
Thời gian tăng:

4
t r= [1,589 ξ 3−0,156 ξ2 +0,924 ξ ] ≤1
Wn
4
3
W n ≥ 1,333
Chọn W n =154
Khi đó:
92
{
2,3125. K p 3
=W 2n
0,0065
2,3125. K d 3 +1,5498
=2. ξ .W n
= {
K p 3=83,006
K d 3 =0,3297
0,0065

Với:
K p 3=83,006
K d 3=0,3297
( B eff + K . K d ) . K p 3
Ki<
j eff
2
2
¿ j eff s + ( 2,3125. K d + Beff ) . s+2,3125. K p
{
j eff =0,0075
¿> Beff =1,5523
K =2,3125
Do đó: K i <25618,2
Chọn K i=5
(PT4)
0,003. θ̈m 4 +1,5523 . θ̇m 4 =2,3125.V 4−d 4


93
0,003. s 2 θ m (s )+1,5523 . s .θ m (s )=2,3125. K p ( θdm ( s )−θm ( s ) )-2,3125. K d . s . θm (s)- D(s)
¿> [ 0,003. s +(2,3125. K d +1,5523). s +2,3125. K p ] θ m(s) =2,3125. K p θm ( s)−D (s)

2 d
2
Đặt 0,003. s +(2,3125. K d +1,5523) . s+2,3125. K p=Ω(s)
K 4 . K p θ dm 1(s) D(s ) 2,3125. K p θ dm ( s) D(s)
θm 4(s )= − = −
Ω(s) Ω(s) Ω(s ) Ω(s)
d 0,003. s2 +(2,3125. K d +1,5523). s d D(s )

E 4 (s ) =θm ( s ) – θm ( s )= . θm (s)+¿
Ω(s) Ω(s)

e ss =lim s . E 4 (s )=¿ lim ⁡ s .

s→0 s →0 [
0,003 s2 + ( 2,3125 K d 1 +1,5523 ) s θ4
Ω( s )
. +
D
s s . Ω( s ) ]
¿ lim
s→0 [ 0,001
=
]
0,001
0,003. s + ( 2,3125. K d +1,5523 ) . s +2,3125. K p 2,3125. K p
2
0,001
e ss = ≤0,01=¿ K p ≥0,043
2,3125. K p

Ω( s)=0,003. s2 + ( 2,3125. K d +1,5523 ) . s+2,3125. K p
2,3125. K d +1,5523 2,3125. K p
=0,003.(s2 + s+ )
0,003 0,003
= 0,003( s2 +2. ξ . W n . s +W 2n ¿
{
2,3125. K p
=W 2n
{
2
0,003 ↔ K p =0,0012.W n
2,3125. K d +1,5523 K d =0,0026. ξ . W n −0,6713
=2. ξ .W n
0,003
Có po =exp (√ )
−ξ . π
1−ξ 2
≤0,1
94
−ξ . π 0,02
→ ≥e
√ 1−ξ 2
→ 0,02≤ ξ ≤ 1
4 4
n
Thời gian tăng:

4
t r= [1,589 ξ 3−0,156 ξ2 +0,924 ξ ] ≤1
Wn
4
3
W n ≥ 1,333
Chọn W n =323
Khi đó:
{
2,3125. K p 2
=W n
0,003
2,3125. K d +1,5498
=2. ξ .W n
= {
K p=125,1948
K d =0,1685
0,003

Với:
K p =125,1948
K d =0,1685
( B eff + K . K d ) . K p
Ki<
j eff
2
¿ j eff s2 + ( 2,3125. K d + Beff ) . s+2,3125. K p
{
j eff =0,003
¿> Beff =1,5523
K =2,3125
Do đó: K i <81040.94
Chọn K i=5
95
(PT5)
0,0028. θ̈m 5 +1,5523 . θ̇ m 5=2,3125.V 5−d 5


U ( s)=K p ( θm (s)−θ m (s )) −K D . S . θm (s)

d

0,0028. s 2 θ m (s )+1,5523 . s .θ m (s )=2,3125. K p ( θdm ( s )−θm ( s ) )-2,3125. K d . s . θm (s)- D (s)
¿> [ 0,0028. s +(2,3125. K d +1,5523). s +2,3125. K p ] θ m(s) =2,3125. K p θm (s)−D (s)

2 d
2
Đặt 0,0028. s +(2,3125. K d +1,5523) . s+2,3125. K p=Ω(s)
K 5 . K p θ dm 1( s) D(s ) 2,3125. K p θ dm (s) D(s)
θm 5 (s)= − = −
d
2
0,0028. s +(2,3125. K d +1,5523) . s d D(s )
E5 ( s)=θ m ( s ) – θm ( s )= . θm (s)+¿
Ω(s) Ω (s)

[ ]
2
0,0028 s + ( 2,3125 K d 1 +1,5523 ) s θ 5 D
e ss =lim s . E 5( s )=¿ lim ⁡ s . . +
s→0 s→0 Ω ( s) s s . Ω( s )
¿ lim
s→0 [ 2
0,001
0,0028. s + ( 2,3125. K d +1,5523 ) . s +2,3125. K p
=
]
0,001
2,3125. Kp
0,001
e ss = ≤0,01=¿ K p ≥0,043
2,3125. K p
96
2
Ω( s)=0,0028. s + ( 2,3125. K d +1,5523 ) . s+2,3125. K p
2,3125. K d +1,5523 2,3125. K p
=0,0028.( s2 + s+ )
0,0028 0,0028
= 0,0028( s2 +2. ξ . W n . s +W 2n ¿
{
2,3125. K p 2
=W n
{
2
0,0028 ↔ K p =0,0012.W n
2,3125. K d +1,5523 K d =0,0024. ξ . W n −0,6713
=2. ξ .W n
0,0028
1−ξ
2
≤0,1
−ξ . π 0,02
→ ≥e
√ 1−ξ 2
→ 0,02≤ ξ ≤ 1
4 4
n
Thời gian tăng:

4
t r= [1,589 ξ 3−0,156 ξ2 +0,924 ξ ] ≤1
Wn
4
3
W n ≥ 1,333
Chọn W n =366
Khi đó:
{
2,3125. K p 2
=W n
0,0028
2,3125. K d +1,5498
=2. ξ .W n
= {
K p=160.7472
K d =0.2071
0,0028
97
Với:
K p =160.7472
K d =0.2071
( B eff + K . K d ) . K p
Ki<
j eff
2
2
¿ j eff s + ( 2,3125. K d + Beff ) . s+2,3125. K p
{
j eff =0,0028
¿> Beff =1,5523
K =2,3125
Do đó: K i <11647,9
Chọn K i=5
(PT6)
0,0027. θ̈m 6 +1,5523 . θ̇ m 6=2,3125. V 6−d6



0,0027. s 2 θ m (s )+1,5523 . s .θ m (s )=2,3125. K p ( θdm ( s )−θm ( s ) )-2,3125. K d . s . θm (s)- D (s)
¿> [ 0,0027. s2 +(2,3125. K d +1,5523). s +2,3125. K p ] θ m(s) =2,3125. K p θdm (s)−D(s)
98
2
Đặt 0,0027. s +(2,3125. K d +1,5523) . s+2,3125. K p=Ω(s)
d d
K 5 . K p θ m 1( s) D(s) 2,3125. K p θm (s) D(s)
θm 6 (s) = − = −
d 0,0027. s2 +(2,3125. K d +1,5523). s d D(s )

E6 ( s)=θm ( s ) – θm ( s )= . θm ( s)+¿
Ω (s) Ω (s)

[ ]
2
0,0027 s + ( 2,3125 K d 1 +1,5523 ) s θ6 D
e ss =lim s . E 6 (s )=¿ lim ⁡ s . . +
s→0 s→ 0 Ω ( s) s s . Ω( s )
¿ lim
s→0 [ 0,001
=
]
0,001
0,0027. s + ( 2,3125. K d +1,5523 ) . s +2,3125. K p 2,3125. K p
2
0,001
e ss = ≤0,01=¿ K p ≥0,043
2,3125. K p

Ω( s)=0,0027. s2 + ( 2,3125. K d +1,5523 ) . s+2,3125. K p
2,3125. K d +1,5523 2,3125. K p
=0,0027.( s2 + s+ )
0,0027 0,0027
= 0,0027( s2 +2. ξ . W n . s +W 2n ¿
{
2,3125. K p 2
=W n
{
2
0,0027 ↔ K p =0,0011.W n
2,3125. K d +1,5523 K d =0,0023. ξ . W n −0,6713
=2. ξ .W n
0,0027
1−ξ 2
≤0,1
−ξ . π 0,02
→ ≥e
√ 1−ξ 2
→ 0,02≤ ξ ≤ 1
99
4 4
n
Thời gian tăng:

4
t r= [1,589 ξ 3−0,156 ξ2 +0,924 ξ ] ≤1
Wn
4
3
W n ≥ 1,333
Chọn W n =373
Khi đó:
{
2,3125. K p
=W 2n
0,0027
2,3125. K d +1,5498
=2. ξ .W n
= {
K p=290.62
K d =0.5109
0,00287

Với:
K p =290.62
K d =0.5109
( B eff + K . K d ) . K p
Ki<
j eff
2
¿ j eff s2 + ( 2,3125. K d + Beff ) . s+2,3125. K p
{
j eff =0,0027
¿> Beff =1,5523
K =2,3125
Do đó: K i <1247,9
Chọn K i=5
100
4.2.Thiết kế bộ điều khiển PD
Hình 4.1.Simulink bộ điều khiển PD
- Sau khi thực hiện mô phỏng trên phần mềm Matlab ta thu được các kết quả như sau
Khâu 1:
101
Hình 4.2: Kết quả mô phỏng matlab
Khâu 2

Khâu 3:
102
Khâu 4
Khâu 5
103
Khâu 6
104
4.3 Thiết kế bộ điều khiển PID
Hình 4.8:Mô hình Simulink bộ điều khiển PID

105
Khâu 1
Khâu 2
106
Khâu 3
Khâu 4
107
Khâu 5
Khâu 6
108
4.4.Thiết kế bộ điều khiển PI
Hình 4.15: Mô hình Simulink bộ điều khiển PI
109
Khâu 1
Khâu 2
110
Khâu 3
Khâu 4
111
Khâu 5
Khâu 6
112
4.5.Thiết kế bộ điều khiển PD bão hòa
Hình 4.22: Mô hình Simulink bộ điều khiển PID bão hoà
113
Khâu 1
Khâu 2
114
Khâu 3
115
Khâu 4
Khâu 5
116
Khâu 6
117
4.6.Thiết kế bộ điều khiển tiếp thuận
Hình 4.36: Mô hình Simulink bộ điều khiển tiếp thuận
118
Khâu 1 :
Khâu 2
119
Khâu 3
Khâu 4
120
Khâu 5
121
Khâu 6
4.8.Thiết kế bộ điều khiển Điểm Cực

Thiết kế bộ điều khiển gán điểm cực sử dụng phương trình trạng thái:
Ta có được các phương trình như sau:
Khâu 1:
J m 1 . θ̈ m 1−B m 1 . θ̇m 1=K 1 u1−d 1 .
J11 J 12 J 13 J 14 J15 J 16
( J m1 + ) θ̈m1 + θ̈m2 + θ̈m3 + θ̈m4 + θ̈ m5 + θ̈ = B m1 θ̇ m1 - K 1 u1 +d 1
r
2
1
r1 r2 r1 r3 r1 r4 r1 r5 r 1 r 6 m6
Khâu 2:
J m2 . θ̈ m2 + Bm2 . θ̇m2 = K 2 u 2 - d2 .
J 22 J21 J23 J 24 J 25 J26
( J m2 + ) θ̈m2 + θ̈m1 θ̈m3 + θ̈m4 + θ̈m5 + θ̈ = B θ̇ - K u + d
r
2
2
r2 r1 r2 r3 r2r4 r2 r5 r 2 r 6 m6 m2 m2 2 2 2
122
Khâu 3:
J m3 . θ̈m3 + Bm3 . θ̇m3 = K 3 u3 -d 3 .

J 33 J 31 J32 J 34 J 35 J36
(J m3 + ) θ̈m3 + θ̈m1 + θ̈m2 + θ̈ m4 + θ̈m5 + θ̈ = B θ̇ - K u +d
r
2
3
r3 r1 r3 r2 r3 r 4 r3 r5 r 3 r 6 m6 m3 m3 3 3 3
Khâu 4:
J m4 . θ̈m4 +. θ̇m4 = K 4 u 4 - d 4 .
J 44 J 41 J 42 J 43 J 45 J 46
(J m4 + 2
) θ̈m4 +
r4 r1
θ̈m1 +
r4 r2
θ̈m2 +
r 4 r3
θ̈ m3 +
r4 r5
θ̈m5 + θ̈ = B θ̇ - K u + d
r 4 r 6 m6 m4 m4 4 4 4
r4
Khâu 5:
J m5 . θ̈m5 + Bm5 . θ̇m5 = K 5 u5 - d 5 .

J 55 J 51 J52 J 53 J 54 J56
(J m5 + 2
) θ̈m5 +
r5 r1
θ̈ m1 +
r5 r2
θ̈m2 +
r5 r3
θ̈m3 +
r5 r4
θ̈m4 + θ̈ = B θ̇ - K u +d
r 5 r 6 m6 m5 m5 5 5 5
r 5
Khâu 6:
J m6 . θ̈m6 +. θ̇m6 = K6 u 6 - d6 .
J66 J 61 J 62 J 63 J64 J 65
(J m6 + 2
) θ̈m6 +
r6 r1
θ̈m1 +
r6 r2
θ̈m2 +
r6 r3
θ̈m3 +
r6 r4
θ̈m4 + θ̈ = B θ̇ - K u + d
r 6 r 5 m5 m6 m6 6 6 6
r 6
Ta có ma trận:
123
[ ]
J 11 J 12 J 13 J 14 J 15 J 16
J m 1+
r 2
1
r1 r2 r1 r3 r1 r4 r1 r5 r1 r6
[]
J 21 J 22 J 23 J 24 J 25 J 26
J m 2+
r2 r1 r
2
2
r2 r3 r2 r4 r2 r5 r2 r6 θ̈ m 1
J 31 J 32 J 33 J 34 J 35 J 36 θ̈ m
J m3+ 2 2
r3 r1 r3 r3 r3 r3 r4 r3 r5 r3 r 6 θ̈ m
J 41 J 42 J 43 J 44 J 45 J 46
3
=
θ̈m
Jm 4 + 2 4
r4 r1 r4 r2 r 4 r3 r4 r4 r5 r 4 r6 θ̈ m 5
J 51 J 52 J 53 J 54 J 55 J 56 θ̈ m
J m 5+ 2 6
r5 r1 r5 r2 r5 r3 r5 r4 r5 r5 r 6
J 61 J 62 J 63 J 64 J 65 J 66
J m6+
r6 r1 r6 r2 r 6 r3 r6 r4 r6 r5 r 26
[ ][ ] [ ] [ ][ ]
B m1 0 0 0 0 0 θ̇m K1 0 0 0 0 1 0 u1 d1
0 Bm2 0 0 0 0 θ̇m 0 K2 0 0 0 2
0 u2 d2
0 0 Bm3 0 0 0 0 0 K3 0 0 0 u3
. θ̇m + 3 . d3
0 0 0 Bm 4 0 0 θ̇ m 0 0 0 K4 0 0 u4 d4
4
0 0 0 0 Bm5 0 θ̇m 0 0 0 0 K5 0 u5 d5
5
0 0 0 0 0 B m 6 θ̇ 0 0 0 0 0 K 6 u6 d6
m 6
Trong đó ta xác định được các thông số sau:

J m =2,77.10−3 ; J m = 2,77.10−3 ; J m = 2,77.10−3 ; J m = 2,77.10−3 ; J m = 2,77.10−3 ; J m = 2,77.10−3 ;
1 2 3 4 5 6
J 11 =0,9399; J 12 =0; J13 =0; J 14 =0; J 15 =0; J 16 =0;

J 21 =0; J 22 =1,5132; J23 =1,0706 ; J 24 =0; J 25 =0; J 26 =0;
J 31 =0; J 32 =1,0706; J33 =1,0706; J 34 =0; J 35 =0; J 36 =0;
J 41 =0; J 42 =0; J 43 =0; J 44 =0,0197; J 45 =0; J 46 =0;
J 51 =0; J 52 =0; J53 =0; J54 =0; J55 =0.0011; J 56 =0;
J 61 =0; J 62 =0; J 63 =0; J64 =0; J65 =0; J66 =0;
Bm =0,00295; Bm =0,00295; Bm =0,00295; Bm =0,00295; Bm =0,00295; Bm =0,00295;
1 2 3 4 5 6
K 1 =2,3125; K2 =2,3125; K3 =2,3125; K4 =2,3125; K5 =2,3125; K 6 =2,3125;

r 1 =25; r 2 =20; r 3 =15; r 4 =10; r 5 =10; r 6 =5;
Thay số vào phương trình trên ta được:
124
[ ][ ]
4.2738 .10−3 0 0 0 0 0 θ̈1
0 6.553.10 −3
3.5686 .10 −3
0 0 0 θ̈2
−3 −3
0 3.5686.10 7.5282 .10 0 0 0 θ̈3
0 0 0 2.967 .10
−3
0 0
=
θ̈4
−3
0 0 0 0 2.781 .10 0 θ̈5
−3
0 0 0 0 0 2.888.10 θ̈6
[ ][ ]
0.00295 0 0 0 0 0 θ̇ 1
0 0.00295 0 0 0 0 θ̇ 2
0 0 0.00295 0 0 0 θ̇3
0 0 0 0.00295 0 0 +
θ̇4
0 0 0 0 0.00295 0 θ̇5
0 0 0 0 0 0.00295 θ̇
6
[ ][ ] [ ]
2.3125 0 0 0 0 0 u1 d1
0 2.3125 0 0 0 0 u2 d2
0 0 2.3125 0 0 0 u3 d3
0 0 0 2.3125 0 0 u4
- d4
0 0 0 0 2.3125 0 u5 d5
0 0 0 0 0 2.3125 u d6
6
Ta có:
[]
θ̈m1
θ̈m2
θ̈m =J −1 ( B θ̇+ Ku−D ) (1)

3
θ̈m4
θ̈m5
θ̈m6
Đặt x 1 = θm 1 ; x 2 = θm 2 ; x 3 = θm 3 ; x 4 = θm 4; x 5 = θm 5 ; x 6 = θm 6 ;
x 7 = θ̇m 1 ; x 8 = θ̇m 2 ; x 9 = θ̇m 3 ; x 10 = θ̇m 4; x 11 = θ̇m 5 ; x 12 = θ̇m 6 ;
Từ phương trình (1), ta có:
125
[ ][ ][ ]
ẋ 1 x1
0 0 0 0 0 0 0 0 0 0 0 0
ẋ 2 x2
0 0 0 0 0 0 0 0 0 0 0 0
ẋ 3 0 0 0 0 0 0 0 0 0 0 0 0 x3
ẋ 4 0 0 0 0 0 0 0 0 0 0 0 0 x4
ẋ 5 0 0 0 0 0 0 0 0 0 0 0 0 x5
ẋ 6 0 0 0 0 0 0 0 0 0 0 0 0 x6
ẋ 7 0 0 0 0 0 0 0.6902 0 0 0 0 0 x7
ẋ 8 0 0 0 0 0 0 0 0.6068 −0.2876 0 0 0 x8
ẋ 9 0 0 0 0 0 0 0 −0.2876 0.5282 0 0 0 x9
ẋ10 0 0 0 0 0 0 0 0 0 0.9942 0 0 x10
0 0 0 0 0 0 0 0 0 0 1.0607 0
ẋ11 x
0 0 0 0 0 0 0 0 0 0 0 1.0214 11
ẋ12 x 12
[ ][ ]
541.0875 0 0 0 0 0 u1
0 475.6886 −225.4911 0 0 0 u2
0 −225.4911 414.0681 0 0 0 u3
0 0 0 779.4068 0 0 u4
-
0 0 0 0 831.5354 0 u5
0 0 0 0 0 800.7274 u
[ ]
6
0
0
0
0
0
0
233.9838 d 1
205.7032d 2 −97.5096 d 3
−97.5096 d 2 +179.0564 d 3
337.0407 d 4
359.5828 d 5
346.2603 d 6
126
Kết quả mô phỏng matlab:
Khâu 1
Khâu 2
127
Khâu 3
Khâu 4
Khâu 5
128
Khâu 6
Nhận xét :
+ Bộ điều khiển cho tín hiệu điều khiển ở các khâu ổn định bám sát theo đường phi
tuyến
129
CODE MATLAB
Bộ điều khiển gán điểm cực:
*Chương trình chính
clc;
clear;
syms d1 d2 d3 d4 d5 d6;
miu=[0;0;0;0;0;0;5;5;10;10;1;1];
x0=[0;0;0;0;0;0;0.05;0.05;0.05;0.05;0.05;0.05];
%Jmi
jm1=2.27*10^(-3);jm2=2.27*10^(-3);jm3=2.27*10^(-3);jm4=2.27*10^(-3); jm5=2.27*10^(-
3); jm6=2.27*10^(-3);
%ri
r1=25;r2=20;r3=15;r4=10; r5=10; r6=5;
%Jij
j11=0.9399;j12=0;j13=0;j14=0;j15=0;j16=0;
j21=0;j22=1.5132;j23=1.0706;j24=0;j25=0;j26=0;
j31=0;j32=1.0706;j33=1.0706;j34=0;j35=0;j36=0;
j41=0;j42=0;j43=0;j44=0.0197;j45=0;j46=0;
j51=0;j52=0;j53=0;j54=0;j55=0.0011;j56=0;
j61=0;j62=0;j63=0;j64=0;j65=0;j66=0;
%Ki
k1=2.3125;k2=2.3125;k3=2.3125;k4=2.3125; k5=2.3125; k6=2.3125;
%Bmi
Bm1=0.00295;Bm2=0.00295;Bm3=0.00295;Bm4=0.00295; Bm5=0.00295; Bm6=0.00295;
J=[jm1+j11/(r1*r1) j12/(r1*r2) j13/(r1*r3) j14/(r1*r4) j15/(r1*r5) j16/(r1*r6);

j21/(r2*r1) jm2+j22/(r2*r2) j23/(r2*r3) j24/(r2*r4) j25/(r2*r5) j26/(r2*r6);
j31/(r3*r1) j32/(r3*r2) jm3+j33/(r3*r3) j34/(r3*r4) j35/(r3*r5) j36/(r3*r6);
j41/(r4*r1) j42/(r4*r2) j43/(r4*r3) jm4+j44/(r4*r4) j45/(r4*r5) j46/(r4*r6);
j51/(r5*r1) j52/(r5*r2) j53/(r5*r3) j54/(r5*r4) jm5+ j55/(r5*r5) j56/(r5*r6);
j61/(r6*r1) j62/(r6*r2) j63/(r6*r3) j64/(r6*r4) j65/(r6*r5) jm6+j66/(r6*r6);];
Bm=[0.00295 0 0 0 0 0 ;
0 0.00295 0 0 0 0 ;
0 0 0.00295 0 0 0;
0 0 0 0.00295 0 0;
0 0 0 0 0.00295 0;
0 0 0 0 0 0.00295;];
130
Km=[2.3125 0 0 0 0 0 ;
0 2.3125 0 0 0 0 ;
0 0 2.3125 0 0 0;
0 0 0 2.3125 0 0;
0 0 0 0 2.3125 0;
0 0 0 0 0 2.3125;];
A=[0 0 0 0 0 0 1 0 0 0 0 0 ;
0 0 0 0 0 0 0 1 0 0 0 0 ;
0 0 0 0 0 0 0 0 1 0 0 0 ;
0 0 0 0 0 0 0 0 0 1 0 0 ;
0 0 0 0 0 0 0 0 0 0 1 0 ;
0 0 0 0 0 0 0 0 0 0 0 1 ;
0 0 0 0 0 0 0.6902 0 0 0 0 0 ;
0 0 0 0 0 0 0 0.6068 -0.2876 0 0 0 ;
0 0 0 0 0 0 0 -0.2876 0.5282 0 0 0 ;
0 0 0 0 0 0 0 0 0 0.9942 0 0 ;
0 0 0 0 0 0 0 0 0 0 1.0607 0 ;
0 0 0 0 0 0 0 0 0 0 0 1.0214 ;];
B=[ 0 0 0 0 0 0 ;
0 0 0 0 0 0 ;
0 0 0 0 0 0 ;
0 0 0 0 0 0 ;
0 0 0 0 0 0 ;
0 0 0 0 0 0 ;
541.0875 0 0 0 0 0;
0 475.6886 -225.4911 0 0 0;
0 -225.4911 414.0681 0 0 0;
0 0 0 779.4068 0 0;
0 0 0 0 831.5354 0;
0 0 0 0 0 800.7274;];
C=eye(6);
D=[ 0;
0;
0;
0;
0;
0;
233.9838*d1;
131
205.7032*d2-97.5096*d3;
-97.5096*d2+179.0564*d3;
337.040*d4;
359.5828*d5;
346.2603*d6;];
P = [B A*B A^2*B A^3*B A^4*B A^5*B A^6*B];
rank(P);
K=place(A,B,miu);
times=0:0.5:10;
% xac dinh cac bien trang thai cua he kin tuyen tinh
xt=zeros(length(times),12); %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)';
end
% xac dinh cac bien trang thai cua he kin phi tuyen
[T,xt_non]=ode45(@(t,x_nonlinear)sp1(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);
grid on;
ylabel('Van toc goc x2','FontSize',14,'FontName','time');
title('Do thi van toc goc phi theo thoi gian');
132
figure(2);
subplot(2,2,1);
grid on;
subplot(2,2,2);
grid on;
figure(3);
subplot(2,2,1);
grid on;
subplot(2,2,2);
grid on;
133
figure(4);
subplot(2,2,1);
grid on;
subplot(2,2,2);
grid on;
figure(5);
subplot(2,2,1);
grid on;
subplot(2,2,2);
134
grid on;
figure(6);
subplot(2,2,1);
grid on;
subplot(2,2,2);
grid on;
*Chương trình con

function xd_nonlinear=sp1(t,x_nonlinear,K)
% Các tham số của hệ
K1=K(1,:);
K2=K(2,:);
K3=K(3,:);
K4=K(4,:);
K5=K(5,:);
135
K6=K(6,:);
d1=sin(x_nonlinear(1));
%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;
u6=-K6*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
xd1=x7;
xd2=x8;
xd3=x9;
xd4=x10;
xd5=x11;
xd6=x12;
xd7=0.6902*x7+541.0875*u1;
xd8=0.6068*x8-0.2876*x9+475.6886*u2-225.4911*u3;
xd9= -0.2876*x8+0.5282*x9-225.4911*u2+414.0681*u3;
xd10=0.9942*x10+779.4068*u4;
xd11=1.0607*x11+831.5354*u5;
xd12=1.0214*x12+800.7274*u6;
xd_nonlinear=[xd1;xd2;xd3;xd4;xd5;xd6;xd7;xd8;xd9;xd10;xd11;x12];
end
136
KẾT LUẬN CHUNG
Trong thời gian làm thiết kế mô phỏng , em đã tìm hiểu được các thông
số kỹ thuật cũng như nguyên lý hoạt động của robot ABB IRB 120 , đã có cơ hội
được làm quen tìm hiểu nhiều hãng , loại robot . Bên cạnh đó, em cũng đã được
làm quen và trải nghiệm mô phỏng robot ABB IRB 120 trên trên phần mềm
Matlab- Simulink.
Tuy thời gian làm bài tập lớn không được dài nhưng đã trang bị cho em
nhiều kiến thức bổ ích, khả năng tự học và định hướng tư duy.
Một lần nữa nhóm em xin chân thành cảm ơn cô Đinh Thị Thanh Huyền
đã tạo điều kiện và hướng dẫn em trong quá trình làm bài tập lớn vừa qua.
137
NHẬN XÉT, ĐÁNH GIÁ CỦA GIẢNG VIÊN
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
138
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
………………………………………………………………………………………
…………………………………………………………………………………
Điểm :……………….( Bằng chữ :………………………)
TÀI LIỆU THAM KHẢO
139

Báo Cáo ĐKRB 1

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Báo Cáo ĐKRB 1

Uploaded by

Copyright:

Available Formats

BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI

ĐIỀU KHIỂN ROBOT

2. Bùi Xuân Bách 191340111

3. Hoàng Văn Long 191302529

4. Nguyễn Tiến Mạnh 191300673

Giảng viên hướng dẫn : PGS. Đinh Thị Thanh Huyền

Lớp : Cơ điện tử 2 K60 Khoa : Cơ khí

Em xin chân thành cảm ơn !.

Hà Nội , ngày 5 tháng 2 năm 2023

1.1.Tổng quan về robot ABB IRB 120

1.1.1.Tổng quan chung về robot ABB IRB 120

Hình 1.2: Kích thước robot ABB IRB 120

Trục X Trục Z Trục 2 Trục 3

C 169 mm 300 mm 0° +70°

D 580 mm 270 mm +90° -77°

E 545 mm 91 mm +110° -77°

F -440 mm -50 mm -110° -110°

G -67 mm 445 mm -110° +70°

H -580 mm 270 mm -90° -77°

J -545 mm 91 mm -110° -77°

Bảng 1.1: Thông số phạm vi chuyển động

Bảng 1.2: Góc quay của robot

Hình 2.1: Hệ trục toạ độ trên cánh tay robot

Join α i [° ] ai [mm] θ i [° ] di [mm]

L1 = 165; L2 = 125; L3 = 270; L4 = 70;

L5 = 134;L6 = 168;L7 = 72;

Các ma trận biến đổi thuần nhất

Các thông số Mass properties của các khâu

Hình 2.2: Mass properties khâu 1

dx2 =0; dy2=0; dz2=0

dx3=0; dy3=0; dz3=0

Hình 2.5: Mass properties khâu 4

m4=1.4476; Ixx4=0.0145; Iyy4=0.0129; Izz4=0.0033; Ixy4=-0.000; Ixz4=0.0003;

dx4=0; dy4=0; dz4=0

Hình 2.6: Mass properties khâu 5

m5=0.5466; Ixx5=0.0008; Iyy5=0.0004; Izz5=0.0009; Ixy5=0.0000; Ixz5=-0.0000;

dx5=0; dy5=0; dz5=0

Hình 2.7: Mass properties khâu 6

dx6=0; dy6=0; dz6=0

dx1=0; dy1=0; dz1=0;

% Vecto gia toc trong truong

% Vecto toa do khoi tam trong he quy chieu cua khau

% Ma tran gia quan tinh

J1=[(-Ixx1+Iyy1+Izz1)/2+m1*dx1*dx1 Ixy1+m1*dx1*dy1 Ixz1+m1*dx1*dz1 -m1*dx1;

J2=[(-Ixx2+Iyy2+Izz2)/2+m2*dx2*dx2 Ixy2+m2*dx2*dy2 Ixz2+m2*dx2*dz2 -m2*dx2;

J3=[(-Ixx3+Iyy3+Izz3)/2+m3*dx3*dx3 Ixy3+m3*dx3*dy3 Ixz3+m3*dx3*dz3 -m3*dx3;

J4=[(-Ixx4+Iyy4+Izz4)/2+m4*dx4*dx4 Ixy4+m4*dx4*dy4 Ixz4+m4*dx4*dz4 -m4*dx4;

J6=[(-Ixx6+Iyy6+Izz6)/2+m6*dx6*dx6 Ixy6+m6*dx6*dy6 Ixz6+m6*dx6*dz6 -m6*dx6;

% Ma tran bien doi thuan nhat

% He phuong trinh vi phan chuyen dong

Kết quả tính được

F2= (3782891779*qdd2)/2500000000 + (2676462679*qdd3)/2500000000 -

F3= (2676462679*qdd2)/2500000000 + (2676462679*qdd3)/2500000000 -

F4= (6171693*qdd4)/312500000 - (73597*qdd4*cos(2*q5))/312500000 -

F5= (167347*qdd5)/156250000 + (82611*qd2^2*sin(q5))/625000000 +

Chương 3: Chọn động cơ

Hình 3.1: Thông số động cơ 130ST-M15025

3.1.2.Thông số động cơ Khâu 2: 130ST-M15025

3.1.3.Thông số động cơ Khâu 3: 130ST-M15025

3.1.4.Thông số động cơ Khâu 4: 130ST-M15025

3.1.5.Thông số động cơ Khâu 5: 130ST-M15025

3.1.6.Thông số động cơ Khâu 6: 130ST-M15025

4.1.Thiết kế bộ điều khiển PD và PID

J1=[(-Ixx1+Iyy1+Izz1)/2+m1dx1dx1 Ixy1+m1dx1dy1 Ixz1+m1dx1dz1 -m1*dx1;

J2=[(-Ixx2+Iyy2+Izz2)/2+m2dx2dx2 Ixy2+m2dx2dy2 Ixz2+m2dx2dz2 -m2*dx2;

J3=[(-Ixx3+Iyy3+Izz3)/2+m3dx3dx3 Ixy3+m3dx3dy3 Ixz3+m3dx3dz3 -m3*dx3;

J4=[(-Ixx4+Iyy4+Izz4)/2+m4dx4dx4 Ixy4+m4dx4dy4 Ixz4+m4dx4dz4 -m4*dx4;

J6=[(-Ixx6+Iyy6+Izz6)/2+m6dx6dx6 Ixy6+m6dx6dy6 Ixz6+m6dx6dz6 -m6*dx6;

F2= (3782891779qdd2)/2500000000 + (2676462679qdd3)/2500000000 -

F3= (2676462679qdd2)/2500000000 + (2676462679qdd3)/2500000000 -

F4= (6171693qdd4)/312500000 - (73597qdd4cos(2q5))/312500000 -

F5= (167347qdd5)/156250000 + (82611qd2^2*sin(q5))/625000000 +