BTL Robot5

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 191313722
2. Bùi Xuân Bách 191333121
3. Hoàng Văn Long 191302741
4. Nguyễn Tiến Mạnh 191300967
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, 2/2023
THIẾT KẾ MÔ PHỎNG ĐIỀU KHIỂN ROBOT

ABB IRB 120
Họ Và Tên Mã sinh viên Lớp Khoa Chuyên Ngành
Đặng Xuân Long 191313722 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử

Lưu Vũ Hầu 191333121 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử
Nguyễn Công Thuận 191302741 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử
Tạ Văn Thắng 191300967 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử
Nhóm sinh viên thực hiện : Nhóm 7
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
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.
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
 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
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
Tốc độ tối đa: 6.2m/s
Gia tốc tối đa: 28m/s2
Đ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°
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
CHƯƠNG 2 : TÍNH TOÁN ĐỘNG HỌC

2.1.Lập bảng DH và tính toán các ma trận
Hình 2.1: Gán hệ trục tọa độ cho cánh tay robot.

Hình 2.2: Lược đồ cánh tay robot
Bảng DH:
Khâu a i-1 ∝i-1 di θi
1 0 0 l1 θ1
2 l0 90˚ 0 90˚+θ 2
3 l3 0 0 θ3
4 l4 90˚ l5 θ4
5 0 -90˚ l6 θ5
6 0 90˚ l7 θ6
Từ bảng DH ta có ma trận thuần nhất của các khâu:
[ ]
c θ1 −s θ1 00
0 s θ1 c θ1 00
T 1=T =1¿ ¿
0 0 1 l1
0 0 01
0 0 1
T 2=T =2¿ T∗1¿ 2T ¿ ¿
1
T =2¿ Trans ( 0 ,0 , l 2 )∗Trans ( l 0 , 0 ,0 )∗Rot ( X , 90 ˚ )∗Rot (Z , 90 ˚ +θ2 )¿
[ ][ ][ ][ ]
1 0 00 1 0 0 l 0 1 0 0 0 −s θ2 −c θ2 00
0 1 00 0 1 0 0 0 0 −1 0 c θ2 −s θ 2 00
¿ ∗ ∗ ∗
0 0 1 l2 0 0 10 0 1 0 0 0 0 10
0 0 01 0 0 01 0 0 0 1 0 0 01
[ ]
−s θ2 −c θ 2 0 l 0
0 0 −1 0
¿
c θ 2 −s θ2 0 l 2
0 0 0 1
0 0 2
T 3=T =3¿ T∗2¿ 3T ¿ ¿
[ ]
c θ3 −s θ 3 0 l3
2 s θ 3 c θ3 00
T = 3¿ ¿
0 0 10
0 0 01
0 0 3
T 4=T = 4¿ T∗3¿ 4T ¿ ¿
3
T =4 ¿ Rot ( X , 90 ˚ )∗Trans ( l 4 , 0 , 0 )∗Trans(0 , 0 , l5 )∗Rot ( Z ,θ 4 ) ¿
[ ][ ] [ ] [ ]
1 0 0 0 1 0 0 l4 1 0 0 0 c θ4 −s θ4 00
0 0 −1 0 0 1 00 0 1 0 0 s θ4 c θ4 00
¿ ∗ ∗
0 1 0 0 0 0 10 0 0 1 l5 0 0 10
0 0 0 1 0 0 01 0 0 01 0 0 01
[ ]
c θ4 −s θ4 0 l 4
0 0 −1 −l 5
¿
s θ4 c θ4 0 0
0 0 0 1
0 0 4
T 5=T =5¿ T∗4 ¿ 5T ¿ ¿
4
T =5 ¿ Trans ( 0 , 0 , l 6 )∗Rot ( X ,−90 ˚ )∗Rot (Z , θ5 )¿
[ ] [ ][ ][ ]
1 0 00 1 0 0 0 c θ5 −s θ5 00 c θ5 −s θ5 00
0 1 00 0 0 1 0 s θ5 c θ 5 00 0 0 10
¿ ∗ =
0 0 1 l6 0 −1 0 0 0 0 1 0 −s θ 5 −c θ5 0 l6
0 0 01 0 0 01 0 0 01 0 0 01
0 0 5
T 6=T =6¿ T∗5¿ 6T ¿¿
[ ]
c θ6 −s θ 6 0 0
5 0 0 −1 −l 7
T = 6¿ ¿
s θ 6 c θ6 0 0
0 0 0 1
 Vecto tọa độ trọng tâm (bỏ qua khâu đế)

Trọng tâm khâu 1: x1= 0,0385; y1= 0,0009; z1= 0,0763 (m)
1
⃗r =⌊ 0,0385 0,0009 0,0763 1 ⌋
. G
Trọng tâm khâu 2: x2 = 0,1113; y2 = -0,0001; z2= 0,0068 (m).

2
⃗r =⌊ 0,1113 −0,0001 0,0068 1 ⌋
. G
Trọng tâm khâu 3: x3= 0,1113; y3= -0,0001; z3= 0,0068 (m).
3
r⃗ =⌊ 0,1113 −0,0001 0,0068 1 ⌋
. G
Trọng tâm khâu 4: x4= 0; y4= 0,0008; z4= 0,0865 (m).

4
⃗r =⌊ 0 0,0008 0,0865 ⌋
. G
Trọng tâm khâu 5: x5= 0; y5= -0,0167; z5= 0 (m).

5
r⃗ =⌊ 0 −0,0167 0 1 ⌋
. G
Trọng tâm khâu 6: x6= 0,0002; y6= 0; z6= -0,0067 (m).

6
. Gr⃗ =⌊ 0,0002 0−0,0067 1 ⌋
 Vecto gia tốc trọng trường

⃗g=⌊ 0 0−g 1 ⌋ T
 Momen quán tính tại trọng tâm (kg.m2)
Khâu 1: Khâu 2: Khâu 3:

Ixx1= 0,0793 Ixx2= 0,0324 Ixx3= 0,0324
Iyx1= 0,0015 Iyx2= 0,0018 Iyx3= 0,0018
Izx1= 0,0263 Izx2= 0,0042 Izx3= 0,0042
Ixy1= 0,0015 Ixy2= 0,0018 Ixy3= 0,0018
Iyy1= 0,0784 Iyy2= 0,157 Iyy3= 0,157
Izy1= 0,0011 Izy2= 0,0001 Izy3= 0,0001
Ixz1= 0,0263 Izx2= 0,0042 Ixz3= 0,0042
Iyz1= 0,0011 Izy2= 0,0001 Iyz3= 0,0001
Izz1= 0,067 Izz2= 0,1374 Izz3= 0,1374
Khâu 4: Khâu 5: Khâu 6:

Ixx4= 0,0121 Ixx5= 0,0005 Ixx6= 0
Iyx4= 0 Iyx5= 0 Iyx6= 0
Izx4= 0 Izx5= 0 Izx6= 0
Ixy4= 0 Ixy5= 0 Ixy6= 0
Iyy4= 0,0113 Iyy5= 0,0002 Iyy6= 0
Izy4= 0,0001 Izy5= 0 Izy6= 0
Ixz4= 0 Ixz5= 0 Ixz6= 0
Iyz4= 0,0001 Iyz5= 0 Iyz6= 0
Izz4= 0,0017 Izz5= 0,0005 Izz6= 0
● Ma trận giả quán tính được tính theo công thức:
[ ]
-I xx + I yy + Izz 2
+m d x I xy +m d x d y I xz +m d x d z m x -m d x
2
I xx -Iyy + I zz
J i = I xy +m d x d y +m d 2y Iyz +m d y d z m y -m d y
2
I xx + I yy - I zz
I xz +m d x d z I yz +m d y d z +md 2z m z -m d z
2
m x -m d x m y -m d y m z -m d z m
[ ]
0.0331 0.0015 0.0263 0
J1 = 0.0015 0.0340 0.0011 0
0.0263 0.0011 0.0454 0
0 0 0 5.4798
[ ]
0.1310 0.0018 0.0042 0
J 2 = 0.0018 0.0064 0.0001 0
0.0042 0.0001 0.0260 0
0 0 0 5.0639
[ ]
0.1310 0.0018 0.0042 0
J 3 = 0.0018 0.0064 0.0001 0
0.0042 0.0001 0.0260 0
0 0 0 5.0639
[ ]
0.0004 0 0 0
J4 = 0 0.0013 0.0001 0
0 0.0001 0.0108 0
0 0 0 1.0215
[ ]
0.0001 0 0 0
J5 = 0 0.0004 0 0
0 0 0.0001 0
0 0 0 0.3480
[ ]
0 0 0 0
J6 = 0 0 0 0
0 0 0 0
0 0 0 0.0145
2.2.Lập phương trình động lực học.

Ta có lực tổng quát của khâu i:
[ ] [ ]
n j n j j T n 2 T
∂ Tj ∂ Tj ∂ Tj ∂ Tj T ∂T j
F i = ∑ ∑ Tr Jj q̈ k + ∑ ∑ ∑ Tr Jj q̇ k q̇m - ∑ m i ⃗g . j . r⃗G
j=i k=1 ∂ qk ∂ qi j=i k=1 m=1 ∂ q k ∂ q m ∂q i j=i ∂ qi j
Áp dụng phương trình ĐLH cho Robot 6 bậc tự do ( n=6) và khai triển chi tiết ta được:
[ ] [ ] [ ] [ ] [ ] [
T T T T T
∂ T 1 ∂T 1 ∂T 2 ∂ T 2 ∂ T 2 ∂ T2 ∂ T3 ∂ T3 ∂ T 3 ∂T 3 ∂
F 1=tr J1 q̈ 1+tr J2 q̈1 +tr J2 q¨2 +tr J3 q¨1+ tr J3 q¨2+tr
∂ q1 ∂ q 1 ∂ q1 ∂ q 1 ∂ q 2 ∂ q1 ∂ q1 ∂ q1 ∂ q2 ∂ q 1 ∂
[ ] [ ] [ ] [ ]
2 T 2 T 2 T 2 T
∂ T6 ∂T6 ∂ T6 ∂T 6 ∂ T6 ∂T6 ∂ T6 ∂T
+2 tr J6 q̇3 q̇5 +2 tr J6 q̇ 3 q˙6 +tr J6 q̇4 q˙4 +2 tr J 6 6 q˙4 q
∂ q3∂ q5 ∂ q1 ∂ q 3 ∂q 6 ∂ q 1 ∂ q 4 ∂ q4 ∂ q1 ∂ q 4 ∂ q5 ∂ q1
∂ T1 1 ⃗T ∂ T 2 2r −m ⃗T ∂T3 3 T ∂T4 4 T ∂T5 5

−m1 ⃗ ⃗ ⃗
T
g . .r G ¿−m 2 g . . G 3g . . r G −m 4 g . . r G −m 5 g . r
∂ q1 1
∂ q1 ∂ q1 2
∂ q1 ∂ q1 . G 3 4 5
∂T6 6
−m6 ⃗
T
g . r
∂ q1 . G 6
[ ] [ ] [ ] [ ] [ ] [
T T T T T
∂ T2 ∂ T2 ∂T 2 ∂T 2 ∂T3 ∂T3 ∂T3 ∂T3 ∂ T 3 ∂ T3 ∂
F 2=tr J2 q¨1+ tr J2 q̈2 +tr J3 q̈1 +tr J3 q̈2 +tr J3 q¨3 +tr
∂ q 1 ∂ q2 ∂ q2 ∂ q 2 ∂q 1 ∂ q 2 ∂ q 2 ∂ q2 ∂ q 3 ∂ q2 ∂
[ ] [ ]
2 T 2 T
∂ T2 ∂T2 ∂ T2 ∂T
+tr J2 q̇1 q̇1 +2 tr J 2 2 q˙1 q˙2
∂q 1 ∂q 1 ∂ q 2 ∂ q1 ∂ q 2 ∂ q2
[ ] [ ] [ ] [ ]
2 T 2 T 2 T 2 T
∂ T2 ∂T2 ∂ T3 ∂T3 ∂ T3 ∂T ∂ T3 ∂T
+tr J2 q̇2 q̇2 +tr J3 q̇1 q̇1 +2 tr J 3 3 q˙1 q̇2 +2 tr J 3 3 q˙1 q˙3 +
∂q 2 ∂ q2 ∂ q 2 ∂q 1 ∂ q1 ∂ q 2 ∂ q 1 ∂ q 2 ∂ q2 ∂ q1 ∂ q3 ∂ q2
∂ T2 2 T ∂T 3 3 T ∂T 4 4 T ∂T5 5 T ∂T6 6
−m2 ⃗ ⃗ ⃗ ⃗ ⃗
T
g . .r G −m3 g . . r G −m 4 g . .r G −m5 g . . r G −m 6 g . r
∂ q2 ∂ q2
2
∂ q2 3
∂q 2 4
∂ q2 . G 5 6
[ ] [ ] [ ] [ ] [ ] [
T T T T T
∂ T 3 ∂ T3 ∂ T3 ∂ T3 ∂ T 3 ∂T 3 ∂T 4 ∂T 4 ∂T 4 ∂ T 4
F 3=tr J3 q¨1 +tr J3 q¨2+ tr J3 q¨3+ tr J4 q̈ 1+tr J q̈ +tr
∂ q 1 ∂ q3 ∂ q2 ∂ q3 ∂ q3 ∂ q 3 ∂ q1 ∂ q3 ∂ q2 4 ∂ q 3 2
[ ] [ ] [ ]
T T T
∂T 5 ∂ T 5 ∂ T 6 ∂T 6 ∂ T 6 ∂T 6
+tr J5 q̈5 +tr J6 q̈1 +tr J q̈
∂ q 5 ∂ q3 ∂ q1 ∂ q3 ∂q 2 6 ∂ q 3 2
[ ] [ ] [ ]
T T T
∂T 6 ∂T 6 ∂T 6 ∂T 6 ∂T6 ∂T6
+tr J6 q̈3 +tr J6 q¨4 +tr J q̈
∂q 3 ∂ q 3 ∂ q4 ∂ q3 ∂ q5 6 ∂ q3 5
[ ] [ ] [ ] [ ] [
T 2 T 2 T 2 T
∂T 6 ∂T 6 ∂ T3 ∂T 3 ∂ T3 ∂T3 ∂ T3 ∂T ∂
+tr J6 q̈6 + tr J3 q̇ 1 q̇ 1+2 tr J3 q̇1 q̇2 +2 tr J 3 3 q̇1 q̇3 +tr
∂ q6 ∂ q3 ∂ q1 ∂ q1 ∂ q 3 ∂ q 1 ∂ q 2 ∂ q3 ∂ q 1 ∂ q 3 ∂ q3 ∂q
∂T3 3 T ∂T4 4 T ∂ T5 5 T ∂ T6 6
−m3 ⃗ ⃗ ⃗ ⃗
T
g . . r G −m 4 g . . r G −m 5 g . . r G −m 6 g . r
∂ q3 3
∂q 3 4
∂ q3 5
∂ q3 . G 6
[ ] [ ] [ ] [ ] [ ]
T T T T T
∂T 4 ∂T 4 ∂T4 ∂T4 ∂T4 ∂T4 ∂T4 ∂T4 ∂T 5 ∂T 5
F 4=tr J4 q̈1 +tr J4 q̈2 +tr J4 q̈3 +tr J4 q¨4 +tr J q̈
∂ q1 ∂ q4 ∂ q2 ∂ q4 ∂q 3 ∂ q4 ∂ q4 ∂q 4 ∂ q1 5 ∂ q 4 1
[ ] [ ] [ ] [ ] [ ] [
T T T T T
∂T 5 ∂ T 5 ∂T5 ∂T5 ∂T5 ∂T5 ∂T 5 ∂T 5 ∂T 6 ∂T 6 ∂T
+tr J5 q̈2 +tr J5 q̈3 +tr J5 q̈4 +tr J5 q̈5 +tr J6 q̈ 1+tr
∂q 2 ∂ q4 ∂ q 3 ∂ q4 ∂ q 4 ∂ q4 ∂ q5 ∂ q 4 ∂ q1 ∂ q 4 ∂ q2
T ∂T 4 4 T ∂T 5 5 T ∂T6 6
−m4 ⃗
g . ⃗
.r G −m5 g .
⃗
. r G −m 6 g . r
∂ q4 4
∂ q4 5
∂ q4 . G 6
[ ] [ ] [ ] [ ] [ ] [
T T T T T
∂ T 5 ∂ T5 ∂ T5 ∂ T5 ∂ T 5 ∂T 5 ∂T 5 ∂T 5 ∂ T 5 ∂ T5 ∂
F 5=tr J5 q¨1 +tr J5 q¨2+ tr J5 q¨3+ tr J5 q¨4 +tr J5 q¨5 +tr
∂ q 1 ∂ q5 ∂ q2 ∂ q5 ∂ q3 ∂ q 5 ∂q 4 ∂ q 5 ∂ q 5 ∂ q5
[ ] [ ]
2 T 2 T
∂ T6 ∂T 6 ∂ T6 ∂T6
+tr J6 q˙3 q˙3 +2tr J6 q̇ q˙
∂q 3 ∂ q3 ∂ q 5 ∂ q3 ∂ q 4 ∂ q 5 3 4
[ ] [ ] [ ] [ ]
2 T 2 T 2 T 2 T
∂ T6 ∂T6 ∂ T6 ∂T 6 ∂ T6 ∂T6 ∂ T6 ∂T
+2 tr J6 q̇3 q̇5 +2 tr J6 q̇ 3 q˙6 +tr J6 q̇4 q˙4 +2 tr J 6 6 q˙4 q
∂ q3∂ q5 ∂ q5 ∂ q 3 ∂q 6 ∂ q 5 ∂ q 4 ∂ q4 ∂ q5 ∂ q 4 ∂ q5 ∂ q5
[ ] [ ] [ ] [ ] [ ] [
T T T T T
∂T6 ∂T6 ∂T6 ∂T6 ∂ T 6 ∂T 6 ∂T 6 ∂T 6 ∂T6 ∂T6
F 6=tr J6 q̈1 +tr J6 q̈2 +tr J6 q̈3 +tr J6 q¨4 +tr J q̈ +tr
∂ q 1 ∂ q6 ∂ q2 ∂ q6 ∂ q3 ∂ q6 ∂ q4 ∂ q6 ∂ q 5 6 ∂ q6 5
Code Matlab:
clc ;
syms q1 q2 q3 q4 q5 q6 qd1 qd2 qd3 qd4 qd5 qd6 qdd1 qdd2 qdd3 qdd4 qdd5 qdd6 ;
Vg=[0; 0; -9.8; 1];
rRG1= [0.0385; 0.0009; 0.0763; 1];
rRG2 = [0.1113; -0.0001; 0.0068; 1 ];
rRG3 = [ 0.1113; -0.0001; -0.0068 ; 1 ];
rRG4 = [ 0 ; 0.0008; 0.0865; 1 ];
rRG5 = [ 0 ; -0.0167; 0; 1 ];
rRG6 = [ 0.0002 ; 0; -0.0067; 1 ];
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;
m1=5.4798; m2=5.0639; m3=5.0639; m4=1.0215;m5=0.3480;m6=0.0145;
%khau1
Ixx1 = 0.0793; Ixy1 =0.0015; Ixz1 = 0.0263;
Iyx1 =0.0015; Iyy1=0.0784; Iyz1 = 0.0011;
Izx1 = 0.0263; Izy1 = 0.0011; Izz1 = 0.0670;
%khau2
Ixx2 = 0.0324; Ixy2 = 0.0018; Ixz2 = 0.0042;
Iyx2 = 0.0018; Iyy2= 0.1570; Iyz2 = 0.0001;
Izx2 = 0.0042; Izy2 = 0.0001; Izz2 = 0.1374;
%khau3
Ixx3 = 0.0324; Ixy3 = 0.0018; Ixz3 = 0.0042;
Iyx3 = 0.0018; Iyy3 = 0.1570; Iyz3 = 0.0001;
Izx3 = 0.0042; Izy3 = 0.0001; Izz3 = 0.1374;
%khau4
Ixx4 = 0.0121; Ixy4 = 0; Ixz4 = 0;
Iyx4 = 0; Iyy4 = 0.0113; Iyz4 = 0.0001;
Izx4 = 0; Izy4 = 0.0001; Izz4 = 0.0017;
%khau5
Ixx5 = 0.0005; Ixy5 = 0; Ixz5 = 0;
Iyx5 = 0; Iyy5 = 0.0002; Iyz5 = 0;
Izx5 = 0; Izy5 = 0; Izz5 = 0.0005;
%khau6
Ixx6 = 0; Ixy6 = 0; Ixz6 = 0;
Iyx6 = 0; Iyy6 = 0; Iyz6 = 0;
Izx6 = 0; Izy6 = 0; Izz6 = 0;
%Tinh J
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]
-m2*dx2 -m2*dy2 -m2*dz2 m2]
-m3*dx3 -m3*dy3 -m3*dz3 m3]
-m4*dx4 -m4*dy4 -m4*dz4 m4]
-m5*dx5 -m5*dy5 -m5*dz5 m5]
-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 0.165; 0 0 0 1];
T12=[-sin(q2) -cos(q2) 0 0.1050; 0 0 -1 0; cos(q2) -sin(q2) 0 0.130; 0 0 0 1];
T23=[ cos(q3) -sin(q3) 0 0.280; sin(q3) cos(q3) 0 0; 0 0 1 0; 0 0 0 1];
T34=[ cos(q4) -sin(q4) 0 0.1408; 0 0 -1 -0.1478; sin(q4) cos(q4) 0 0;0 0 0 1];
T45=[ cos(q5) -sin(q5) 0 0; 0 0 1 0; -sin(q5) -cos(q5) 0 0.16; 0 0 0 1];
T56=[ cos(q6) -sin(q6) 0 0; 0 0 -1 -0.0845; 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
F1=trace(diff(T01,q1)*J1*transpose(diff(T01,q1)))*qdd1+...
trace(diff(T02,q1)*J2*transpose(diff(T02,q1)))*qdd1+...
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+...
trace(diff(diff(T06,q6),q6)*J6*transpose(diff(T06,q1)))*qd6*qd6-...
m1*transpose(Vg)*diff(T01,q1)*rRG1-...
m6*transpose(Vg)*diff(T06,q1)*rRG6;
2*trace(diff(diff(T05,q1),5)*J5*transpose(diff(T05,q4)))*qd1*qd5+...
F1=simplify(F1)
F2=simplify(F2)
F3=simplify(F3)
F4=simplify(F4)
F5=simplify(F5)
F6=simplify(F6)
Kết quả code Matlab:

F1 =
(1096440261881*qdd1)/1600000000000 - (3938221*qdd1*cos(2*q2))/12500000 +
(9571731*qdd1*cos(2*q4))/64000000000 - (3228269*qdd1*cos(2*q5))/64000000000 + (qd2^2*cos(q2))/10000
+ (9*qdd1*sin(2*q2))/5000 + (21*qd2^2*sin(q2))/5000 + (106568*qdd1*cos(q3))/1953125 -
(21*qdd2*cos(q2))/5000 + (7542639*qdd1*cos(q5))/20000000000 - (9478413*qdd1*sin(q2))/25000000 +
(1148679*qdd1*sin(q3))/15625000 + (qdd2*sin(q2))/10000 - (9478413*qd1*qd2*cos(q2))/25000000 +
(1148679*qd1*qd3*cos(q3))/15625000 - (106568*qd1*qd3*sin(q3))/1953125 -
(7542639*qd1*qd5*sin(q5))/20000000000 - (13644537567*qdd1*cos(2*q2)*cos(2*q3))/320000000000 +
(3228269*qdd1*cos(2*q4)*cos(2*q5))/64000000000 - (13737411*qdd1*cos(2*q2)*sin(2*q3))/390625000 -
(13737411*qdd1*cos(2*q3)*sin(2*q2))/390625000 +
(13644537567*qdd1*sin(2*q2)*sin(2*q3))/320000000000 + (9*qd1*qd2*cos(2*q2))/2500 +
(3446037*qdd1*cos(q2)*cos(q3))/62500000 - (21*qdd2*cos(q2)*cos(q3))/5000 -
(21*qdd3*cos(q2)*cos(q3))/5000 + (3938221*qd1*qd2*sin(2*q2))/6250000 -
(9571731*qd1*qd4*sin(2*q4))/32000000000 + (3228269*qd1*qd5*sin(2*q5))/32000000000 -
(79926*qdd1*cos(q2)*sin(q3))/1953125 - (79926*qdd1*cos(q3)*sin(q2))/1953125 +
(qdd2*cos(q2)*sin(q3))/10000 + (qdd2*cos(q3)*sin(q2))/10000 + (qdd3*cos(q2)*sin(q3))/10000 +
(qdd3*cos(q3)*sin(q2))/10000 + (34307*qdd1*cos(q5)*sin(q3))/100000000 +
(33628269*qdd4*cos(q2)*sin(q3))/16000000000 + (33628269*qdd4*cos(q3)*sin(q2))/16000000000 +
(53911*qdd1*cos(q4)*sin(q5))/312500000 - (102921*qdd4*cos(q4)*sin(q5))/800000000 -
(102921*qdd5*cos(q5)*sin(q4))/800000000 - (3446037*qdd1*sin(q2)*sin(q3))/62500000 +
(21*qdd2*sin(q2)*sin(q3))/5000 + (21*qdd3*sin(q2)*sin(q3))/5000 -
(106568*qdd1*cos(2*q2)*cos(q3))/1953125 + (qd3^2*cos(q2)*cos(q3))/10000 -
(1148679*qdd1*cos(2*q2)*sin(q3))/15625000 - (1148679*qdd1*sin(2*q2)*cos(q3))/15625000 +
(21*qd3^2*cos(q2)*sin(q3))/5000 + (21*qd3^2*cos(q3)*sin(q2))/5000 +
(106568*qdd1*sin(2*q2)*sin(q3))/1953125 - (qd3^2*sin(q2)*sin(q3))/10000 +
(102921*qd4^2*sin(q4)*sin(q5))/800000000 + (102921*qd5^2*sin(q4)*sin(q5))/800000000 +
(1275691*qd1*qd2*sin(q2)*sin(q3))/31250000 + (79926*qd1*qd3*sin(q2)*sin(q3))/1953125 -
(qd2*qd3*sin(q2)*sin(q3))/5000 - (33628269*qd2*qd4*sin(q2)*sin(q3))/16000000000 -
(33628269*qd3*qd4*sin(q2)*sin(q3))/16000000000 - (53911*qd1*qd4*sin(q4)*sin(q5))/312500000 -
(34307*qd1*qd5*sin(q3)*sin(q5))/100000000 + (7542639*qdd1*cos(2*q2)*cos(2*q3)*cos(q5))/20000000000 -
(53911*qdd1*cos(2*q2)*sin(2*q3)*cos(q5))/312500000 -
(53911*qdd1*cos(2*q3)*sin(2*q2)*cos(q5))/312500000 + (qdd1*cos(2*q2)*sin(2*q3)*sin(q4))/10000 +
(qdd1*cos(2*q3)*sin(2*q2)*sin(q4))/10000 - (7542639*qdd1*sin(2*q2)*sin(2*q3)*cos(q5))/20000000000 -
(9571731*qd2^2*sin(2*q4)*cos(q2)*sin(q3))/32000000000 -
(9571731*qd2^2*sin(2*q4)*cos(q3)*sin(q2))/32000000000 -
(9571731*qd3^2*sin(2*q4)*cos(q2)*sin(q3))/32000000000 -
(9571731*qd3^2*sin(2*q4)*cos(q3)*sin(q2))/32000000000 - (1148679*qd1*qd2*cos(2*q2)*cos(q3))/7812500
- (1148679*qd1*qd3*cos(2*q2)*cos(q3))/15625000 + (102921*qdd1*cos(q2)*cos(q3)*cos(q5))/400000000 +
(213136*qd1*qd2*cos(2*q2)*sin(q3))/1953125 + (213136*qd1*qd2*sin(2*q2)*cos(q3))/1953125 +
(106568*qd1*qd3*cos(2*q2)*sin(q3))/1953125 + (106568*qd1*qd3*sin(2*q2)*cos(q3))/1953125 -
(qdd2*cos(q2)*cos(q4)*sin(q3))/10000 - (qdd2*cos(q3)*cos(q4)*sin(q2))/10000 -
(qdd3*cos(q2)*cos(q4)*sin(q3))/10000 - (qdd3*cos(q3)*cos(q4)*sin(q2))/10000 +
(34307*qdd1*cos(q3)*cos(q4)*sin(q5))/100000000 + (qdd4*cos(q2)*cos(q3)*sin(q4))/10000 -
(4828269*qdd5*cos(q2)*cos(q3)*sin(q4))/8000000000 + (1148679*qd1*qd2*sin(2*q2)*sin(q3))/7812500 +
(1148679*qd1*qd3*sin(2*q2)*sin(q3))/15625000 +
(9571731*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q4))/64000000000 +
(9684807*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q5))/64000000000 -
(102921*qdd1*cos(q5)*sin(q2)*sin(q3))/400000000 - (34307*qdd2*cos(q2)*sin(q4)*sin(q5))/100000000 +
(34307*qdd4*cos(q4)*sin(q2)*sin(q5))/100000000 + (34307*qdd5*cos(q5)*sin(q2)*sin(q4))/100000000 -
(qdd4*sin(q2)*sin(q3)*sin(q4))/10000 + (4828269*qdd5*sin(q2)*sin(q3)*sin(q4))/8000000000 -
(9571731*qdd1*cos(2*q4)*sin(2*q2)*sin(2*q3))/64000000000 -
(9684807*qdd1*cos(2*q5)*sin(2*q2)*sin(2*q3))/64000000000 -
(13737411*qd1*qd2*cos(2*q2)*cos(2*q3))/195312500 -
(13737411*qd1*qd3*cos(2*q2)*cos(2*q3))/195312500 - (qd2^2*cos(q2)*cos(q3)*cos(q4))/10000 -
(qd3^2*cos(q2)*cos(q3)*cos(q4))/10000 + (qd4^2*cos(q2)*cos(q3)*cos(q4))/10000 +
(13644537567*qd1*qd2*cos(2*q2)*sin(2*q3))/160000000000 +
(13644537567*qd1*qd2*cos(2*q3)*sin(2*q2))/160000000000 +
(13644537567*qd1*qd3*cos(2*q2)*sin(2*q3))/160000000000 +
(13644537567*qd1*qd3*cos(2*q3)*sin(2*q2))/160000000000 -
(3228269*qd1*qd4*cos(2*q5)*sin(2*q4))/32000000000 -
(3228269*qd1*qd5*cos(2*q4)*sin(2*q5))/32000000000 -
(34307*qdd1*cos(2*q2)*cos(q5)*sin(q3))/100000000 - (34307*qdd1*sin(2*q2)*cos(q3)*cos(q5))/100000000 +
(9571731*qdd2*sin(2*q4)*cos(q2)*cos(q3))/32000000000 +
(9571731*qdd3*sin(2*q4)*cos(q2)*cos(q3))/32000000000 -
(3228269*qdd4*cos(2*q5)*cos(q2)*sin(q3))/16000000000 -
(3228269*qdd4*cos(2*q5)*cos(q3)*sin(q2))/16000000000 +
(13737411*qd1*qd2*sin(2*q2)*sin(2*q3))/195312500 + (13737411*qd1*qd3*sin(2*q2)*sin(2*q3))/195312500
+ (qd2^2*cos(q4)*sin(q2)*sin(q3))/10000 + (qd3^2*cos(q4)*sin(q2)*sin(q3))/10000 -
(qd4^2*cos(q4)*sin(q2)*sin(q3))/10000 - (9571731*qdd2*sin(2*q4)*sin(q2)*sin(q3))/32000000000 -
(9571731*qdd3*sin(2*q4)*sin(q2)*sin(q3))/32000000000 +
(34307*qd2^2*sin(q2)*sin(q4)*sin(q5))/100000000 - (34307*qd4^2*sin(q2)*sin(q4)*sin(q5))/100000000 -
(34307*qd5^2*sin(q2)*sin(q4)*sin(q5))/100000000 - (1275691*qd1*qd2*cos(q2)*cos(q3))/31250000 -
(79926*qd1*qd3*cos(q2)*cos(q3))/1953125 + (qd2*qd3*cos(q2)*cos(q3))/5000 +
(33628269*qd2*qd4*cos(q2)*cos(q3))/16000000000 + (34307*qd1*qd3*cos(q3)*cos(q5))/100000000 +
(33628269*qd3*qd4*cos(q2)*cos(q3))/16000000000 + (53911*qd1*qd5*cos(q4)*cos(q5))/312500000 -
(102921*qd4*qd5*cos(q4)*cos(q5))/400000000 - (3183537*qd1*qd2*cos(q2)*sin(q3))/62500000 -
(3183537*qd1*qd2*cos(q3)*sin(q2))/62500000 - (3446037*qd1*qd3*cos(q2)*sin(q3))/62500000 -
(3446037*qd1*qd3*cos(q3)*sin(q2))/62500000 + (21*qd2*qd3*cos(q2)*sin(q3))/2500 +
(21*qd2*qd3*cos(q3)*sin(q2))/2500 - (7542639*qdd1*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/20000000000 -
(7542639*qdd1*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5))/20000000000 -
(3228269*qd2^2*sin(2*q5)*cos(q2)*cos(q3)*sin(q4))/16000000000 -
(3228269*qd3^2*sin(2*q5)*cos(q2)*cos(q3)*sin(q4))/16000000000 +
(9571731*qd1*qd4*sin(2*q2)*sin(2*q3)*sin(2*q4))/32000000000 +
(53911*qdd1*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/312500000 -
(3228269*qdd2*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/32000000000 -
(3228269*qdd3*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/32000000000 -
(34307*qd1*qd2*cos(2*q2)*cos(q3)*cos(q5))/50000000 -
(34307*qd1*qd3*cos(2*q2)*cos(q3)*cos(q5))/100000000 +
(3228269*qd2^2*sin(2*q5)*sin(q2)*sin(q3)*sin(q4))/16000000000 +
(3228269*qd3^2*sin(2*q5)*sin(q2)*sin(q3)*sin(q4))/16000000000 -
(7542639*qdd4*cos(q2)*cos(q3)*cos(q4)*sin(q5))/20000000000 -
(7542639*qdd5*cos(q2)*cos(q3)*cos(q5)*sin(q4))/20000000000 +
(34307*qd1*qd2*sin(2*q2)*cos(q5)*sin(q3))/50000000 +
(34307*qd1*qd3*sin(2*q2)*cos(q5)*sin(q3))/100000000 -
(9571731*qd2*qd4*cos(2*q4)*sin(q2)*sin(q3))/16000000000 +
(3228269*qd2*qd4*cos(2*q5)*sin(q2)*sin(q3))/16000000000 -
(102921*qdd1*cos(q2)*cos(q4)*sin(q3)*sin(q5))/400000000 -
(53911*qdd3*cos(q2)*cos(q3)*sin(q4)*sin(q5))/312500000 +
(7542639*qdd2*cos(q2)*sin(q3)*sin(q4)*sin(q5))/20000000000 -
(7542639*qdd3*cos(q3)*sin(q2)*sin(q4)*sin(q5))/20000000000 +
(7542639*qdd4*cos(q4)*sin(q2)*sin(q3)*sin(q5))/20000000000 +
(3228269*qdd1*cos(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/16000000000 -
(3228269*qdd1*cos(2*q3)*sin(2*q2)*sin(2*q5)*cos(q4))/16000000000 -
(3228269*qd2^2*cos(2*q5)*sin(2*q4)*cos(q2)*sin(q3))/32000000000 -
(3228269*qd3^2*cos(2*q5)*sin(2*q4)*cos(q3)*sin(q2))/32000000000 +
(53911*qdd2*sin(q2)*sin(q3)*sin(q4)*sin(q5))/312500000 +
(53911*qdd3*sin(q2)*sin(q3)*sin(q4)*sin(q5))/312500000 -
(53911*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q5))/156250000 -
(53911*qd1*qd3*cos(2*q2)*cos(2*q3)*cos(q5))/156250000 + (qd1*qd2*cos(2*q2)*cos(2*q3)*sin(q4))/5000 -
(7542639*qd1*qd2*cos(2*q2)*sin(2*q3)*cos(q5))/10000000000 -
(7542639*qd1*qd2*cos(2*q3)*sin(2*q2)*cos(q5))/10000000000 +
(qd1*qd3*cos(2*q2)*cos(2*q3)*sin(q4))/5000 -
(7542639*qd1*qd3*cos(2*q2)*sin(2*q3)*cos(q5))/10000000000 -
(7542639*qd1*qd3*cos(2*q3)*sin(2*q2)*cos(q5))/10000000000 +
(qd1*qd4*cos(2*q2)*sin(2*q3)*cos(q4))/10000 + (qd1*qd4*cos(2*q3)*sin(2*q2)*cos(q4))/10000 -
(7542639*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(q5))/20000000000 -
(34307*qdd1*cos(2*q2)*cos(q3)*cos(q4)*sin(q5))/100000000 -
(3228269*qdd4*sin(2*q5)*cos(q2)*cos(q3)*cos(q4))/16000000000 +
(53911*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q5))/156250000 +
(53911*qd1*qd3*sin(2*q2)*sin(2*q3)*cos(q5))/156250000 +
(53911*qd1*qd5*cos(2*q2)*sin(2*q3)*sin(q5))/312500000 +
(53911*qd1*qd5*cos(2*q3)*sin(2*q2)*sin(q5))/312500000 +
(3228269*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q4)*cos(2*q5))/64000000000 -
(7542639*qd2^2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/20000000000 -
(7542639*qd3^2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/20000000000 +
(7542639*qd5^2*cos(q2)*cos(q3)*sin(q4)*sin(q5))/20000000000 -
(qd1*qd2*sin(2*q2)*sin(2*q3)*sin(q4))/5000 - (qd1*qd3*sin(2*q2)*sin(2*q3)*sin(q4))/5000 +
(7542639*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(q5))/20000000000 +
(34307*qdd1*sin(2*q2)*cos(q4)*sin(q3)*sin(q5))/100000000 -
(3228269*qdd3*sin(2*q5)*cos(q3)*sin(q2)*sin(q4))/16000000000 +
(3228269*qdd4*sin(2*q5)*cos(q4)*sin(q2)*sin(q3))/16000000000 +
(53911*qd2^2*cos(q2)*sin(q3)*sin(q4)*sin(q5))/312500000 +
(53911*qd3^2*cos(q3)*sin(q2)*sin(q4)*sin(q5))/312500000 -
(53911*qd5^2*cos(q3)*sin(q2)*sin(q4)*sin(q5))/312500000 - (qd2*qd3*cos(q2)*cos(q3)*cos(q4))/5000 +
(34307*qd1*qd5*cos(q3)*cos(q4)*cos(q5))/100000000 -
(4828269*qd4*qd5*cos(q2)*cos(q3)*cos(q4))/8000000000 -
(3228269*qdd1*cos(2*q4)*cos(2*q5)*sin(2*q2)*sin(2*q3))/64000000000 +
(7542639*qd2^2*sin(q2)*sin(q3)*sin(q4)*sin(q5))/20000000000 +
(7542639*qd3^2*sin(q2)*sin(q3)*sin(q4)*sin(q5))/20000000000 -
(102921*qd1*qd2*cos(q2)*cos(q5)*sin(q3))/400000000 -
(102921*qd1*qd5*cos(q2)*cos(q3)*sin(q5))/400000000 +
(34307*qd4*qd5*cos(q4)*cos(q5)*sin(q2))/50000000 + (qd2*qd3*cos(q4)*sin(q2)*sin(q3))/5000 -
(34307*qd1*qd3*cos(q4)*sin(q3)*sin(q5))/100000000 +
(4828269*qd2*qd5*cos(q3)*sin(q2)*sin(q4))/8000000000 -
(9571731*qd1*qd2*cos(2*q2)*cos(2*q4)*sin(2*q3))/32000000000 -
(9684807*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(2*q5))/32000000000 +
(102921*qd1*qd5*sin(q2)*sin(q3)*sin(q5))/400000000 -
(53911*qdd1*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/312500000 +
(3228269*qdd2*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3))/32000000000 +
(3228269*qdd3*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3))/32000000000 +
(34307*qd1*qd2*cos(2*q2)*cos(q4)*sin(q3)*sin(q5))/50000000 +
(34307*qd1*qd2*sin(2*q2)*cos(q3)*cos(q4)*sin(q5))/50000000 +
(34307*qd1*qd4*cos(2*q2)*cos(q3)*sin(q4)*sin(q5))/100000000 -
(34307*qd1*qd5*sin(2*q2)*cos(q4)*cos(q5)*sin(q3))/100000000 -
(3228269*qd1*qd2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3))/32000000000 -
(34307*qd1*qd4*sin(2*q2)*sin(q3)*sin(q4)*sin(q5))/100000000 +
(3228269*qd2*qd3*sin(2*q5)*sin(q2)*sin(q3)*sin(q4))/8000000000 +
(3228269*qd1*qd4*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4))/32000000000 +
(3228269*qd1*qd5*cos(2*q4)*sin(2*q2)*sin(2*q3)*sin(2*q5))/32000000000 -
(53911*qd1*qd5*cos(2*q2)*cos(2*q3)*cos(q4)*cos(q5))/312500000 +
(3228269*qd3*qd4*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3))/16000000000 -
(7542639*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/10000000000 -
(7542639*qd1*qd3*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/10000000000 -
(7542639*qd1*qd5*cos(2*q2)*sin(2*q3)*cos(q4)*cos(q5))/20000000000 -
(7542639*qd1*qd5*cos(2*q3)*sin(2*q2)*cos(q4)*cos(q5))/20000000000 +
(53911*qd1*qd2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/156250000 +
(53911*qd1*qd4*cos(2*q2)*cos(2*q3)*sin(q4)*sin(q5))/312500000 -
(3228269*qd2*qd3*cos(2*q5)*sin(2*q4)*cos(q2)*sin(q3))/16000000000 -
(53911*qd1*qd5*sin(2*q2)*sin(2*q3)*cos(q4)*cos(q5))/312500000 -
(3228269*qd2*qd5*sin(2*q4)*sin(2*q5)*cos(q2)*cos(q3))/16000000000 -
(3228269*qd3*qd5*sin(2*q4)*sin(2*q5)*cos(q2)*cos(q3))/16000000000 +
(7542639*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/10000000000 +
(7542639*qd1*qd3*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/10000000000 +
(7542639*qd1*qd4*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/20000000000 +
(7542639*qd1*qd4*cos(2*q3)*sin(2*q2)*sin(q4)*sin(q5))/20000000000 -
(53911*qd1*qd4*sin(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/312500000 +
(3228269*qd2*qd5*sin(2*q4)*sin(2*q5)*sin(q2)*sin(q3))/16000000000 +
(3228269*qd3*qd5*sin(2*q4)*sin(2*q5)*sin(q2)*sin(q3))/16000000000 -
(7542639*qd4*qd5*cos(q2)*cos(q3)*cos(q4)*cos(q5))/10000000000 -
(102921*qd1*qd2*cos(q2)*cos(q3)*cos(q4)*sin(q5))/400000000 -
(102921*qd1*qd5*cos(q3)*cos(q4)*cos(q5)*sin(q2))/400000000 +
(7542639*qd2*qd3*cos(q2)*cos(q3)*sin(q4)*sin(q5))/10000000000 +
(7542639*qd4*qd5*cos(q4)*cos(q5)*sin(q2)*sin(q3))/10000000000 -
(3228269*qd1*qd2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/8000000000 -
(3228269*qd1*qd5*cos(2*q3)*cos(2*q5)*sin(2*q2)*cos(q4))/8000000000 +
(102921*qd1*qd2*cos(q4)*sin(q2)*sin(q3)*sin(q5))/400000000 +
(7542639*qd2*qd3*sin(q2)*sin(q3)*sin(q4)*sin(q5))/10000000000 +
(3228269*qd1*qd2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/8000000000 +
(3228269*qd1*qd3*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/8000000000 +
(3228269*qd1*qd4*cos(2*q2)*sin(2*q3)*sin(2*q5)*sin(q4))/16000000000 +
(3228269*qd1*qd4*cos(2*q3)*sin(2*q2)*sin(2*q5)*sin(q4))/16000000000 -
(34307*qd1*qd5*cos(2*q2)*cos(q3)*cos(q4)*cos(q5))/100000000 -
(3228269*qd4*qd5*cos(2*q5)*cos(q2)*cos(q3)*cos(q4))/8000000000
F2 =
(701290696431*qdd2)/800000000000 + (186958408431*qdd3)/800000000000 +
(3492134779684473*cos(q2))/703687441776640000 -
(16337114375790721807*sin(q2))/703687441776640000 - (9571731*qdd2*cos(2*q4))/32000000000 +
(3228269*qdd2*cos(2*q5))/32000000000 - (9571731*qdd3*cos(2*q4))/32000000000 +
(3228269*qdd3*cos(2*q5))/32000000000 + (9478413*qd1^2*cos(q2))/50000000 +
(1148679*qd3^2*cos(q3))/15625000 + (53911*qd5^2*cos(q5))/312500000 +
(484689936111424922991*cos(q2)*cos(q3))/140737488355328000000 - (106568*qd3^2*sin(q3))/1953125 +
(qd4^2*sin(q4))/10000 - (130764422600993796697*cos(q2)*sin(q3))/17592186044416000000 -
(130764422600993796697*cos(q3)*sin(q2))/17592186044416000000 -
(484689936111424922991*sin(q2)*sin(q3))/140737488355328000000 - (9*qd1^2*cos(2*q2))/5000 -
(3938221*qd1^2*sin(2*q2))/12500000 - (21*qdd1*cos(q2))/5000 + (213136*qdd2*cos(q3))/1953125 +
(79772*qdd3*cos(q3))/1953125 + (7542639*qdd2*cos(q5))/10000000000 +
(7542639*qdd3*cos(q5))/10000000000 - (qdd4*cos(q4))/10000 + (4828269*qdd5*cos(q4))/8000000000 +
(qdd1*sin(q2))/10000 + (1148679*qdd2*sin(q3))/7812500 + (1360107*qdd3*sin(q3))/31250000 +
(53911*qdd5*sin(q5))/312500000 + (1148679*qd2*qd3*cos(q3))/7812500 -
(213136*qd2*qd3*sin(q3))/1953125 - (7542639*qd2*qd5*sin(q5))/10000000000 -
(3228269*qdd2*cos(2*q4)*cos(2*q5))/32000000000 - (3228269*qdd3*cos(2*q4)*cos(2*q5))/32000000000 +
(1148679*qd1^2*cos(2*q2)*cos(q3))/15625000 - (106568*qd1^2*cos(2*q2)*sin(q3))/1953125 -
(106568*qd1^2*sin(2*q2)*cos(q3))/1953125 - (3228269*qd4^2*sin(2*q5)*cos(q4))/16000000000 -
(1148679*qd1^2*sin(2*q2)*sin(q3))/15625000 - (21*qdd1*cos(q2)*cos(q3))/5000 +
(7542639*qdd5*cos(q4)*cos(q5))/20000000000 + (9571731*qd2*qd4*sin(2*q4))/16000000000 +
(9571731*qd3*qd4*sin(2*q4))/16000000000 - (3228269*qd2*qd5*sin(2*q5))/16000000000 -
(3228269*qd3*qd5*sin(2*q5))/16000000000 + (qdd1*cos(q2)*sin(q3))/10000 + (qdd1*cos(q3)*sin(q2))/10000
+ (34307*qdd2*cos(q5)*sin(q3))/50000000 + (53911*qdd2*cos(q4)*sin(q5))/156250000 +
(34307*qdd3*cos(q5)*sin(q3))/100000000 + (53911*qdd3*cos(q4)*sin(q5))/156250000 +
(34307*qdd5*cos(q3)*sin(q5))/100000000 + (13737411*qd1^2*cos(2*q2)*cos(2*q3))/390625000 +
(21*qdd1*sin(q2)*sin(q3))/5000 - (7542639*qdd4*sin(q4)*sin(q5))/20000000000 -
(13644537567*qd1^2*cos(2*q2)*sin(2*q3))/320000000000 -
(13644537567*qd1^2*cos(2*q3)*sin(2*q2))/320000000000 -
(13737411*qd1^2*sin(2*q2)*sin(2*q3))/390625000 + (39963*qd1^2*cos(q2)*cos(q3))/1953125 +
(34307*qd3^2*cos(q3)*cos(q5))/100000000 + (34307*qd5^2*cos(q3)*cos(q5))/100000000 +
(3400453*cos(q2)*cos(q3)*cos(q5))/50000000 + (3446037*qd1^2*cos(q2)*sin(q3))/125000000 +
(3446037*qd1^2*cos(q3)*sin(q2))/125000000 - (7542639*qd4^2*cos(q4)*sin(q5))/20000000000 -
(7542639*qd5^2*cos(q4)*sin(q5))/20000000000 - (3228269*qdd4*sin(2*q5)*sin(q4))/16000000000 -
(39963*qd1^2*sin(q2)*sin(q3))/1953125 +
(5635523098714729*cos(q2)*sin(q3)*sin(q4))/703687441776640000 +
(5635523098714729*cos(q3)*sin(q2)*sin(q4))/703687441776640000 -
(3400453*cos(q5)*sin(q2)*sin(q3))/50000000 + (33628269*qd1*qd4*sin(q2)*sin(q3))/16000000000 -
(53911*qd2*qd4*sin(q4)*sin(q5))/156250000 - (34307*qd2*qd5*sin(q3)*sin(q5))/50000000 -
(53911*qd3*qd4*sin(q4)*sin(q5))/156250000 - (34307*qd3*qd5*sin(q3)*sin(q5))/50000000 +
(34307*qd1^2*cos(2*q2)*cos(q3)*cos(q5))/100000000 - (34307*qd1^2*sin(2*q2)*cos(q5)*sin(q3))/100000000
- (3228269*qd4*qd5*cos(2*q5)*sin(q4))/8000000000 - (qdd1*cos(q2)*cos(q4)*sin(q3))/10000 -
(qdd1*cos(q3)*cos(q4)*sin(q2))/10000 + (34307*qdd2*cos(q3)*cos(q4)*sin(q5))/50000000 +
(34307*qdd3*cos(q3)*cos(q4)*sin(q5))/100000000 + (34307*qdd5*cos(q4)*cos(q5)*sin(q3))/100000000 +
(53911*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5))/312500000 - (34307*qdd1*cos(q2)*sin(q4)*sin(q5))/100000000
- (qd1^2*cos(2*q2)*cos(2*q3)*sin(q4))/10000 + (7542639*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5))/20000000000
+ (7542639*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5))/20000000000 -
(34307*qdd4*sin(q3)*sin(q4)*sin(q5))/100000000 - (53911*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5))/312500000 +
(qd1^2*sin(2*q2)*sin(2*q3)*sin(q4))/10000 + (3228269*qd2*qd4*cos(2*q5)*sin(2*q4))/16000000000 +
(3228269*qd2*qd5*cos(2*q4)*sin(2*q5))/16000000000 +
(3228269*qd3*qd4*cos(2*q5)*sin(2*q4))/16000000000 +
(3228269*qd3*qd5*cos(2*q4)*sin(2*q5))/16000000000 +
(102921*qd1^2*cos(q2)*cos(q5)*sin(q3))/800000000 + (102921*qd1^2*cos(q3)*cos(q5)*sin(q2))/800000000 -
(1421*cos(q2)*cos(q3)*cos(q6)*sin(q5))/50000000 - (34307*qd3^2*cos(q4)*sin(q3)*sin(q5))/100000000 -
(34307*qd4^2*cos(q4)*sin(q3)*sin(q5))/100000000 - (34307*qd5^2*cos(q4)*sin(q3)*sin(q5))/100000000 -
(3400453*cos(q2)*cos(q4)*sin(q3)*sin(q5))/50000000 - (3400453*cos(q3)*cos(q4)*sin(q2)*sin(q5))/50000000
+ (9571731*qd1^2*cos(2*q2)*cos(2*q4)*sin(2*q3))/64000000000 +
(9571731*qd1^2*cos(2*q3)*cos(2*q4)*sin(2*q2))/64000000000 +
(9684807*qd1^2*cos(2*q2)*cos(2*q5)*sin(2*q3))/64000000000 +
(9684807*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2))/64000000000 -
(9571731*qdd1*sin(2*q4)*sin(q2)*sin(q3))/32000000000 + (1421*cos(q2)*sin(q3)*sin(q4)*sin(q6))/50000000
+ (1421*cos(q3)*sin(q2)*sin(q4)*sin(q6))/50000000 + (1421*cos(q6)*sin(q2)*sin(q3)*sin(q5))/50000000 -
(53911*qd2*qd5*cos(q4)*cos(q5))/156250000 + (53911*qd3*qd5*cos(q4)*cos(q5))/156250000 -
(7542639*qd4*qd5*cos(q5)*sin(q4))/10000000000 -
(34307*qd1^2*cos(2*q2)*cos(q4)*sin(q3)*sin(q5))/100000000 -
(3228269*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3))/64000000000 +
(3228269*qd1^2*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2))/64000000000 -
(3228269*qdd1*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/32000000000 +
(7542639*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/20000000000 -
(7542639*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/20000000000 +
(102921*qd1^2*cos(q2)*cos(q3)*cos(q4)*sin(q5))/800000000 -
(1421*cos(q2)*cos(q4)*cos(q5)*cos(q6)*sin(q3))/50000000 -
(1421*cos(q3)*cos(q4)*cos(q5)*cos(q6)*sin(q2))/50000000 +
(3228269*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/16000000000 -
(34307*qd2*qd5*cos(q3)*cos(q4)*cos(q5))/50000000 + (34307*qd3*qd5*cos(q3)*cos(q4)*cos(q5))/50000000
- (34307*qd1*qd4*cos(q2)*cos(q4)*sin(q5))/50000000 - (34307*qd1*qd5*cos(q2)*cos(q5)*sin(q4))/50000000
- (3228269*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/16000000000 +
(qd1*qd4*cos(q2)*sin(q3)*sin(q4))/5000 + (qd1*qd4*cos(q3)*sin(q2)*sin(q4))/5000 -
(34307*qd2*qd3*cos(q4)*sin(q3)*sin(q5))/50000000 - (34307*qd2*qd4*cos(q3)*sin(q4)*sin(q5))/50000000 -
(34307*qd3*qd4*cos(q3)*sin(q4)*sin(q5))/50000000 - (34307*qd4*qd5*cos(q5)*sin(q3)*sin(q4))/50000000 +
(3228269*qdd1*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3))/32000000000 -
(53911*qd1*qd5*cos(q5)*sin(q2)*sin(q3)*sin(q4))/156250000
F3 =
(186958408431*qdd2)/800000000000 + (186958408431*qdd3)/800000000000 -
(9571731*qdd2*cos(2*q4))/32000000000 + (3228269*qdd2*cos(2*q5))/32000000000 -
(9571731*qdd3*cos(2*q4))/32000000000 + (3228269*qdd3*cos(2*q5))/32000000000 -
(1148679*qd1^2*cos(q3))/31250000 - (1148679*qd2^2*cos(q3))/15625000 +
(53911*qd5^2*cos(q5))/312500000 + (484689936111424922991*cos(q2)*cos(q3))/140737488355328000000 +
(53284*qd1^2*sin(q3))/1953125 + (106568*qd2^2*sin(q3))/1953125 + (qd4^2*sin(q4))/10000 -
(130764422600993796697*cos(q2)*sin(q3))/17592186044416000000 -
(130764422600993796697*cos(q3)*sin(q2))/17592186044416000000 -
(484689936111424922991*sin(q2)*sin(q3))/140737488355328000000 + (106568*qdd2*cos(q3))/1953125 +
(7542639*qdd2*cos(q5))/10000000000 + (7542639*qdd3*cos(q5))/10000000000 - (qdd4*cos(q4))/10000 +
(4828269*qdd5*cos(q4))/8000000000 + (1148679*qdd2*sin(q3))/15625000 +
(53911*qdd5*sin(q5))/312500000 - (7542639*qd2*qd5*sin(q5))/10000000000 -
(3228269*qdd2*cos(2*q4)*cos(2*q5))/32000000000 - (3228269*qdd3*cos(2*q4)*cos(2*q5))/32000000000 +
(1148679*qd1^2*cos(2*q2)*cos(q3))/31250000 - (53284*qd1^2*cos(2*q2)*sin(q3))/1953125 -
(53284*qd1^2*sin(2*q2)*cos(q3))/1953125 - (3228269*qd4^2*sin(2*q5)*cos(q4))/16000000000 -
(1148679*qd1^2*sin(2*q2)*sin(q3))/31250000 - (21*qdd1*cos(q2)*cos(q3))/5000 +
(7542639*qdd5*cos(q4)*cos(q5))/20000000000 + (9571731*qd2*qd4*sin(2*q4))/16000000000 +
(9571731*qd3*qd4*sin(2*q4))/16000000000 - (3228269*qd2*qd5*sin(2*q5))/16000000000 -
(3228269*qd3*qd5*sin(2*q5))/16000000000 + (qdd1*cos(q2)*sin(q3))/10000 + (qdd1*cos(q3)*sin(q2))/10000
+ (34307*qdd2*cos(q5)*sin(q3))/100000000 + (53911*qdd2*cos(q4)*sin(q5))/156250000 +
(53911*qdd3*cos(q4)*sin(q5))/156250000 + (13737411*qd1^2*cos(2*q2)*cos(2*q3))/390625000 +
(21*qdd1*sin(q2)*sin(q3))/5000 - (7542639*qdd4*sin(q4)*sin(q5))/20000000000 -
(13644537567*qd1^2*cos(2*q2)*sin(2*q3))/320000000000 -
(13644537567*qd1^2*cos(2*q3)*sin(2*q2))/320000000000 -
(13737411*qd1^2*sin(2*q2)*sin(2*q3))/390625000 + (39963*qd1^2*cos(q2)*cos(q3))/1953125 -
(34307*qd1^2*cos(q3)*cos(q5))/200000000 - (34307*qd2^2*cos(q3)*cos(q5))/100000000 +
(3400453*cos(q2)*cos(q3)*cos(q5))/50000000 + (3446037*qd1^2*cos(q2)*sin(q3))/125000000 +
(3446037*qd1^2*cos(q3)*sin(q2))/125000000 - (7542639*qd4^2*cos(q4)*sin(q5))/20000000000 -
(7542639*qd5^2*cos(q4)*sin(q5))/20000000000 - (3228269*qdd4*sin(2*q5)*sin(q4))/16000000000 -
(39963*qd1^2*sin(q2)*sin(q3))/1953125 +
(5635523098714729*cos(q2)*sin(q3)*sin(q4))/703687441776640000 +
(5635523098714729*cos(q3)*sin(q2)*sin(q4))/703687441776640000 -
(3400453*cos(q5)*sin(q2)*sin(q3))/50000000 + (33628269*qd1*qd4*sin(q2)*sin(q3))/16000000000 -
(34307*qd1^2*cos(2*q2)*cos(q3)*cos(q5))/200000000 - (34307*qd1^2*sin(2*q2)*cos(q5)*sin(q3))/200000000
- (3228269*qd4*qd5*cos(2*q5)*sin(q4))/8000000000 - (qdd1*cos(q2)*cos(q4)*sin(q3))/10000 -
(qdd1*cos(q3)*cos(q4)*sin(q2))/10000 + (34307*qdd2*cos(q3)*cos(q4)*sin(q5))/100000000 +
(53911*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5))/312500000 - (qd1^2*cos(2*q2)*cos(2*q3)*sin(q4))/10000 +
(7542639*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5))/20000000000 +
(7542639*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5))/20000000000 -
(53911*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5))/312500000 + (qd1^2*sin(2*q2)*sin(2*q3)*sin(q4))/10000 +
(3228269*qd2*qd4*cos(2*q5)*sin(2*q4))/16000000000 +
(3228269*qd2*qd5*cos(2*q4)*sin(2*q5))/16000000000 +
(3228269*qd3*qd4*cos(2*q5)*sin(2*q4))/16000000000 +
(3228269*qd3*qd5*cos(2*q4)*sin(2*q5))/16000000000 +
(102921*qd1^2*cos(q2)*cos(q5)*sin(q3))/800000000 + (102921*qd1^2*cos(q3)*cos(q5)*sin(q2))/800000000 -
(1421*cos(q2)*cos(q3)*cos(q6)*sin(q5))/50000000 + (34307*qd1^2*cos(q4)*sin(q3)*sin(q5))/200000000 +
(34307*qd2^2*cos(q4)*sin(q3)*sin(q5))/100000000 - (3400453*cos(q2)*cos(q4)*sin(q3)*sin(q5))/50000000 -
(3400453*cos(q3)*cos(q4)*sin(q2)*sin(q5))/50000000 +
(9571731*qd1^2*cos(2*q2)*cos(2*q4)*sin(2*q3))/64000000000 +
(9571731*qd1^2*cos(2*q3)*cos(2*q4)*sin(2*q2))/64000000000 +
(9684807*qd1^2*cos(2*q2)*cos(2*q5)*sin(2*q3))/64000000000 +
(9684807*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2))/64000000000 -
(9571731*qdd1*sin(2*q4)*sin(q2)*sin(q3))/32000000000 + (1421*cos(q2)*sin(q3)*sin(q4)*sin(q6))/50000000
+ (1421*cos(q3)*sin(q2)*sin(q4)*sin(q6))/50000000 + (1421*cos(q6)*sin(q2)*sin(q3)*sin(q5))/50000000 -
(53911*qd3*qd5*cos(q4)*cos(q5))/156250000 - (7542639*qd4*qd5*cos(q5)*sin(q4))/10000000000 -
(34307*qd1^2*cos(2*q2)*cos(q4)*sin(q3)*sin(q5))/200000000 -
(3228269*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3))/64000000000 +
(3228269*qdd1*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3))/32000000000 +
(7542639*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5))/20000000000 -
(7542639*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5))/20000000000 +
(3228269*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4))/16000000000 -
(3228269*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4))/16000000000 +
(qd1*qd4*cos(q2)*sin(q3)*sin(q4))/5000 + (qd1*qd4*cos(q3)*sin(q2)*sin(q4))/5000 -
(3228269*qdd1*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3))/32000000000 -
(53911*qd1*qd5*cos(q5)*sin(q2)*sin(q3)*sin(q4))/156250000
F4 =
(33628269*qdd4)/16000000000 - (3228269*qdd4*cos(2*q5))/16000000000 +
(9571731*qd1^2*sin(2*q4))/64000000000 - (9571731*qd2^2*sin(2*q4))/32000000000 -
(9571731*qd3^2*sin(2*q4))/32000000000 - (qdd2*cos(q4))/10000 - (qdd3*cos(q4))/10000 +
(4828269*qd2*qd5*sin(q4))/8000000000 + (4828269*qd3*qd5*sin(q4))/8000000000 -
(9571731*qd2*qd3*sin(2*q4))/16000000000 - (3*qd1*qd5*sin(2*q4))/40000 +
(3228269*qd4*qd5*sin(2*q5))/8000000000 + (33628269*qdd1*cos(q2)*sin(q3))/16000000000 +
(33628269*qdd1*cos(q3)*sin(q2))/16000000000 - (102921*qdd1*cos(q4)*sin(q5))/800000000 -
(7542639*qdd2*sin(q4)*sin(q5))/20000000000 - (7542639*qdd3*sin(q4)*sin(q5))/20000000000 +
(3228269*qd1^2*cos(2*q5)*sin(2*q4))/64000000000 - (3228269*qd2^2*cos(2*q5)*sin(2*q4))/32000000000 -
(3228269*qd3^2*cos(2*q5)*sin(2*q4))/32000000000 -
(5635523098714729*cos(q2)*cos(q3)*cos(q4))/703687441776640000 -
(3228269*qdd2*sin(2*q5)*sin(q4))/16000000000 - (3228269*qdd3*sin(2*q5)*sin(q4))/16000000000 +
(53911*qd1^2*sin(q4)*sin(q5))/625000000 + (53911*qd2^2*sin(q4)*sin(q5))/312500000 +
(53911*qd3^2*sin(q4)*sin(q5))/312500000 +
(5635523098714729*cos(q4)*sin(q2)*sin(q3))/703687441776640000 -
(53911*qd2*qd3*sin(q4)*sin(q5))/156250000 - (3228269*qd2*qd5*cos(2*q5)*sin(q4))/8000000000 -
(3228269*qd3*qd5*cos(2*q5)*sin(q4))/8000000000 + (qdd1*cos(q2)*cos(q3)*sin(q4))/10000 +
(34307*qdd1*cos(q4)*sin(q2)*sin(q5))/100000000 - (qd1^2*cos(2*q2)*sin(2*q3)*cos(q4))/20000 -
(qd1^2*cos(2*q3)*sin(2*q2)*cos(q4))/20000 - (qdd1*sin(q2)*sin(q3)*sin(q4))/10000 -
(34307*qdd2*sin(q3)*sin(q4)*sin(q5))/100000000 - (3228269*qd2*qd3*cos(2*q5)*sin(2*q4))/16000000000 +
(3*qd1*qd5*cos(2*q5)*sin(2*q4))/40000 - (3228269*qdd1*cos(2*q5)*cos(q2)*sin(q3))/16000000000 -
(3228269*qdd1*cos(2*q5)*cos(q3)*sin(q2))/16000000000 -
(34307*qd2^2*cos(q3)*sin(q4)*sin(q5))/100000000 - (3400453*cos(q2)*cos(q3)*sin(q4)*sin(q5))/50000000 +
(9571731*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q4))/64000000000 +
(1421*cos(q4)*sin(q2)*sin(q3)*sin(q6))/50000000 + (33628269*qd1*qd2*cos(q2)*cos(q3))/16000000000 +
(33628269*qd1*qd3*cos(q2)*cos(q3))/16000000000 + (3400453*sin(q2)*sin(q3)*sin(q4)*sin(q5))/50000000 -
(9571731*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q4))/64000000000 -
(34307*qd1^2*cos(2*q2)*cos(q3)*sin(q4)*sin(q5))/200000000 +
(3228269*qd1^2*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4))/64000000000 -
(53911*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4)*sin(q5))/625000000 -
(7542639*qd1^2*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/40000000000 -
(7542639*qd1^2*cos(2*q3)*sin(2*q2)*sin(q4)*sin(q5))/40000000000 +
(53911*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4)*sin(q5))/625000000 -
(3228269*qdd1*sin(2*q5)*cos(q2)*cos(q3)*cos(q4))/16000000000 -
(1421*cos(q5)*cos(q6)*sin(q2)*sin(q3)*sin(q4))/50000000 +
(828269*qd1*qd5*cos(q2)*cos(q3)*cos(q4))/8000000000 +
(3228269*qd1^2*cos(2*q2)*sin(2*q3)*sin(2*q5)*sin(q4))/32000000000 -
(3228269*qd1^2*cos(2*q3)*sin(2*q2)*sin(2*q5)*sin(q4))/32000000000 -
(qd1*qd2*cos(q2)*sin(q3)*sin(q4))/5000 - (qd1*qd2*cos(q3)*sin(q2)*sin(q4))/5000 -
(qd1*qd3*cos(q2)*sin(q3)*sin(q4))/5000 - (qd1*qd3*cos(q3)*sin(q2)*sin(q4))/5000 -
(828269*qd1*qd5*cos(q4)*sin(q2)*sin(q3))/8000000000 - (3*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(2*q4))/40000
+ (3228269*qd1*qd2*sin(2*q5)*cos(q2)*cos(q4)*sin(q3))/8000000000 +
(3*qd1*qd5*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4))/40000 -
(3228269*qd1*qd2*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3))/16000000000 +
(3228269*qd1*qd3*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3))/16000000000 +
(53911*qd1*qd2*cos(q4)*sin(q2)*sin(q3)*sin(q5))/156250000 -
(828269*qd1*qd5*cos(2*q5)*cos(q2)*cos(q3)*cos(q4))/8000000000
F5 =
(4828269*qdd5)/8000000000 + (7542639*qd1^2*sin(q5))/20000000000 +
(7542639*qd2^2*sin(q5))/20000000000 + (7542639*qd3^2*sin(q5))/20000000000 +
(3228269*qd2^2*sin(2*q5))/16000000000 + (3228269*qd3^2*sin(2*q5))/16000000000 -
(3228269*qd4^2*sin(2*q5))/16000000000 + (4828269*qdd2*cos(q4))/8000000000 +
(4828269*qdd3*cos(q4))/8000000000 + (53911*qdd2*sin(q5))/312500000 + (53911*qdd3*sin(q5))/312500000
+ (7542639*qd2*qd3*sin(q5))/10000000000 - (4028269*qd2*qd4*sin(q4))/4000000000 -
(4028269*qd3*qd4*sin(q4))/4000000000 - (7542639*qd1^2*cos(q2)^2*sin(q5))/20000000000 -
(7542639*qd1^2*cos(q3)^2*sin(q5))/20000000000 + (7542639*qdd2*cos(q4)*cos(q5))/20000000000 +
(7542639*qdd3*cos(q4)*cos(q5))/20000000000 + (3228269*qd2*qd3*sin(2*q5))/8000000000 -
(102921*qdd1*cos(q5)*sin(q4))/800000000 + (34307*qdd2*cos(q3)*sin(q5))/100000000 -
(53911*qd2^2*cos(q4)*cos(q5))/312500000 - (53911*qd3^2*cos(q4)*cos(q5))/312500000 +
(34307*qd1^2*sin(q3)*sin(q5))/100000000 + (34307*qd2^2*sin(q3)*sin(q5))/100000000 -
(3400453*cos(q2)*sin(q3)*sin(q5))/50000000 - (3400453*cos(q3)*sin(q2)*sin(q5))/50000000 -
(53911*qd1^2*cos(q2)^2*cos(q4)*cos(q5))/312500000 -
(53911*qd1^2*cos(q3)^2*cos(q4)*cos(q5))/312500000 -
(3228269*qd1^2*cos(q2)^2*cos(q5)*sin(q5))/8000000000 -
(3228269*qd1^2*cos(q3)^2*cos(q5)*sin(q5))/8000000000 +
(3228269*qd1^2*cos(q4)^2*cos(q5)*sin(q5))/8000000000 -
(3228269*qd2^2*cos(q4)^2*cos(q5)*sin(q5))/8000000000 -
(3228269*qd3^2*cos(q4)^2*cos(q5)*sin(q5))/8000000000 -
(34307*qd1^2*cos(q2)^2*sin(q3)*sin(q5))/100000000 + (3228269*qd2*qd4*cos(q5)^2*sin(q4))/4000000000 +
(3228269*qd3*qd4*cos(q5)^2*sin(q4))/4000000000 - (4828269*qdd1*cos(q2)*cos(q3)*sin(q4))/8000000000 +
(34307*qdd2*cos(q4)*cos(q5)*sin(q3))/100000000 + (34307*qdd1*cos(q5)*sin(q2)*sin(q4))/100000000 +
(7542639*qd1^2*cos(q2)^2*cos(q3)^2*sin(q5))/10000000000 +
(4828269*qdd1*sin(q2)*sin(q3)*sin(q4))/8000000000 - (34307*qd1^2*cos(q3)*cos(q4)*cos(q5))/100000000 -
(34307*qd2^2*cos(q3)*cos(q4)*cos(q5))/100000000 + (3400453*cos(q2)*cos(q3)*cos(q4)*cos(q5))/50000000
+ (3228269*qd1^2*cos(q2)*cos(q4)*sin(q2))/8000000000 +
(102921*qd1^2*cos(q2)*cos(q3)*sin(q5))/800000000 +
(3228269*qd1^2*cos(q3)*cos(q4)*sin(q3))/8000000000 - (1421*cos(q2)*cos(q5)*cos(q6)*sin(q3))/50000000 -
(53911*qd1^2*cos(q3)*sin(q3)*sin(q5))/312500000 - (3400453*cos(q4)*cos(q5)*sin(q2)*sin(q3))/50000000 -
(102921*qd1^2*sin(q2)*sin(q3)*sin(q5))/800000000 - (53911*qd2*qd3*cos(q4)*cos(q5))/156250000 -
(53911*qd1^2*cos(q2)*cos(q3)^2*sin(q2)*sin(q5))/156250000 -
(53911*qd1^2*cos(q2)^2*cos(q3)*sin(q3)*sin(q5))/156250000 -
(3228269*qd2*qd3*cos(q4)^2*cos(q5)*sin(q5))/4000000000 -
(53911*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)*cos(q5))/156250000 +
(3228269*qd1^2*cos(q2)^2*cos(q3)^2*cos(q5)*sin(q5))/4000000000 -
(3228269*qd1^2*cos(q2)^2*cos(q4)^2*cos(q5)*sin(q5))/8000000000 -
(3228269*qd1^2*cos(q3)^2*cos(q4)^2*cos(q5)*sin(q5))/8000000000 +
(7542639*qd1^2*cos(q2)*cos(q4)*cos(q5)*sin(q2))/20000000000 +
(102921*qd1^2*cos(q2)*cos(q4)*cos(q5)*sin(q3))/800000000 +
(1421*cos(q4)*cos(q6)*sin(q2)*sin(q3)*sin(q5))/50000000 -
(4028269*qd1*qd4*cos(q2)*cos(q3)*cos(q4))/4000000000 +
(34307*qd1*qd2*cos(q2)*cos(q5)*sin(q4))/50000000 + (qd1*qd2*cos(q2)*sin(q3)*sin(q4))/5000 +
(qd1*qd2*cos(q3)*sin(q2)*sin(q4))/5000 + (qd1*qd3*cos(q2)*sin(q3)*sin(q4))/5000 +
(qd1*qd3*cos(q3)*sin(q2)*sin(q4))/5000 + (4028269*qd1*qd4*cos(q4)*sin(q2)*sin(q3))/4000000000 +
(34307*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5))/100000000 -
(3228269*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*sin(q2))/4000000000 -
(3228269*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*sin(q3))/4000000000 -
(3228269*qd1^2*cos(q2)*cos(q4)*cos(q5)^2*sin(q2))/4000000000 -
(3228269*qd1^2*cos(q3)*cos(q4)*cos(q5)^2*sin(q3))/4000000000 +
(3228269*qd1*qd2*cos(q2)*cos(q5)^2*sin(q3)*sin(q4))/4000000000 +
(3228269*qd1*qd3*cos(q3)*cos(q5)^2*sin(q2)*sin(q4))/4000000000 -
(3228269*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)^2*sin(q2))/2000000000 +
(3228269*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)^2*sin(q3))/2000000000 -
(34307*qd1^2*cos(q2)*cos(q4)*cos(q5)*sin(q2)*sin(q3))/100000000 +
(3228269*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*cos(q5)*sin(q5))/4000000000 -
(7542639*qd1^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)*sin(q5))/10000000000 +
(7542639*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)*sin(q2))/10000000000 +
(7542639*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)*sin(q3))/10000000000 +
(3228269*qd1*qd4*cos(q2)*cos(q3)*cos(q4)*cos(q5)^2)/4000000000 -
(53911*qd1^2*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q2)*sin(q3))/156250000 -
(3228269*qd1^2*cos(q2)*cos(q3)*cos(q5)*sin(q2)*sin(q3)*sin(q5))/4000000000 +
(3228269*qd1*qd2*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q4)*sin(q5))/4000000000 +
(3228269*qd1*qd3*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q4)*sin(q5))/4000000000 -
(3228269*qd1*qd2*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4)*sin(q5))/4000000000 -
(3228269*qd1*qd3*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4)*sin(q5))/4000000000 -
(3228269*qd1^2*cos(q2)*cos(q3)*cos(q4)^2*cos(q5)*sin(q2)*sin(q3)*sin(q5))/4000000000
F6 =
(1421*sin(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)))/50000000 - (1421*cos(q2 +
q3)*cos(q6)*sin(q4))/50000000
CHƯƠNG 3: CHỌN ĐỘNG CƠ
2.1: Hình ảnh động cơ

2.2: Bảng thông số động cơ
Động cơ khâu 1: 130ST-M07725
Khối lượng khâu: m 1 = 5.4798(kg)
Chiều dài khâu: l 1 = 0.2815(m)
Điện trở phần ứng động cơ: R 1= 1.01(Ω)
Moment quán tính phần ứng: J m = 0.00153(kg.m 2 )
1
Điện áp phần ứng: V1= 220(V)

Tỷ số truyền từ động cơ tới các khớp: r 1= 25
Hệ số cản của cơ cấu dẫn động:Bm = 0.6621(N.M.s/rad)
1
Hệ số sức phản điện động:K b = 0.6493(V.s/rad)

1
Hệ số moment: K m = 1.03(N.M/A)
1
Gia tốc trọng trường: g = 9.8 (m/s 2)

2

2
2
2

3

3

3
3

Khối lượng khâu: m 4= 1.0215(kg)
Chiều dài khâu: l 4= 0.0927(m)
4
Điện áp phần ứng: V4 = 220(V)

4

4
4

Moment quán tính phần ứng: J m = 0.00153(kg.m 2 ) 5

Hệ số cản của cơ cấu dẫn động:Bm = 0.6621(N.M.s/rad) 5
Hệ số sức phản điện động:K b = 0.6493(V.s/rad)5
5

Moment quán tính phần ứng: J m = 0.00153(kg.m 2 ) 6

Hệ số cản của cơ cấu dẫn động:Bm = 0.6621(N.M.s/rad) 6
Hệ số sức phản điện động:K b = 0.6493(V.s/rad)6
6
CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN

3.1 Phương trình động lực học robot cho từng khâu
+ Với thông số đã cho của bài toán ta thiết lập được hệ phương trình động lực
{
J eff θ̈m + Beff θ̇m = K1 u1 - d 1
1 1 1 1
J eff θ̈m + Beff θ̇m = K2 u 2 - d 2

2 2 2 2
J eff θ̈ m + Beff θ̇m = K 3 u 3 - d3

học robot như sau: 3 3 3 3
J eff θ̈m
4 4
+ Beff θ̇m4 4
= K 4 u4 - d4
J eff θ̈ m
5 5
+ Beff θ̇m5 5
= K 5 u 5 - d5
J eff θ̈ m
6 6
+ Beff θ̇m6 6
= K 6 u6 - d 6
Khâu 1:
[ ]
1 Kb Km Km τl
Jm + 2
. J 11 θ̈m + ( Bm + ) θ̇m = V1-
1 1 1 1
1
r1 1
R1 1
R1 r1 1
Với:
J eff = J m +
1
[ 1
1
2
r1 ][ 1
. J 11 = 0,00153 + 2 . 0,685275 = 0,00262
25 ]
Kb K m 0,6493.1 , 03
Beff = Bm + 1 1
= 0,6621 + =1,3242
1 1
R1 1 , 01
Km 1, 03
K1 = 1
= =1,0198
R1 1 ,01
1
τ l = F 1 → d1 = . F 1 = 1 . F1
1
r1 25
Khâu 2:
[ ]
1 Kb Km Km τl
Jm + 2
. J 22 θ̈m + ( Bm + 2
) θ̇m =
2 2
V2 - 2
2
r2 2 2
R2 2
R2 r2
Với:
J eff = J m +
2
[ 2
1
2
r2 ] [ 1
. J 22 = 0,00153 + 2 . 0,876613 =0,00372
20 ]
Kb K m 0,6493.1 , 03
Beff = Bm + 2
= 0,6621 +
2
= 1,3242
2 2
R2 1.01
K m 1 , 03
K2 = = 2
=1,0198
R2 1, 01
1 1
τ l = F2 → d 2 = . F2= .F 2
2
r2 20
Khâu 3:
[ ]
1 Kb Km Km τl
Jm + 2
. J 33 θ̈m + ( Bm + 3
) θ̇m =
3 3
V3 - 3
3
r3 3 3
R3 3
R3 r3
Với:
J eff = J m +
3
[ 3
1
2
r3 ][ 1
]
. J 33 = 0,00153 + 2 . 0,233698 = 0,00257
15
Kb Km 0,6493.1, 03
Beff = Bm + 3 3
= 0,6621 + =1,3242
3 3
R3 1, 01
Km 1, 03
K3 = 3
= =1,0198
R3 1 ,01
1
τ l = F3 → d 3 = . F3 = 1 . F3
3
r3 15
Khâu 4:
[ ]
1 Kb K m Km τl
Jm + . J 44 θ̈ m + ( B m + 4
) θ̇
4
m = 4
V 4 - 4
4
r 24 4 4
R4 4
R4 r4
Với:
J eff = J m +
4
[ 4
1
2
r4 ][ 1
. J 44 = 0,00153 + 2 .0,0021 01 = 0,00155
10 ]
Kb Km 0,6493.1 , 03
Beff = Bm + 4
= 0,6621 +
4
=1,3242
4 4
R4 1 , 01
Km 1, 03
K4 = = 4
=1,0198
R4 1 ,01
1
τ l = F4 → d 4 = .F 4 = 1 . F4
4
r4 10
Khâu 5:
[ ]
1 Kb Km Km τl
Jm + 2
. J 55 θ̈m + ( Bm + ) θ̇m =
5 5
V5 -5 5
5
r5 5 5
R5 R5 5
r5
Với:
J eff = J m +
5
[ 5
1
2
r5 ][ 1
. J 55 = 0,00153 + 2 . 0,000603 = 0,00154
10 ]
Kb Km 0,6493.1 , 03
Beff = Bm + 5 5
= 0,6621 + = 1,3242
5 5
R5 1 , 01
Km 1, 03
K5 = 5
= =1,0198
R5 1 ,01
1
τ l = F5 → d 5 = . F5 = 1 . F5
5
r5 10
Khâu 6:
[ ]
1 K b Km Km τl
Jm + 2
. J 66 θ̈m + ( Bm + ) θ̇m =
6 6
V6-6 6
6
r6 6 6
R6 R66
r6
Với:
J eff = J m +
6
[ 6
1
2
r6 ] [ 1
. J66 = 0,00153 + 2 . 0 =0,00153
10 ]
K b Km 0,6493.1 , 03
Beff = Bm + 6 6
= 0,6621 + =1,3242
6 6
R6 10
Km 1, 03
K6 = 6
= =1,0198
R6 1 ,01
1
τ l = F6 → d 6 = . F6 = 1 . F6
6
r6 10
3.2 Thiết kế bộ điều khiển PD và PID
Chọn đầu vào mong muốn q1 = q2 =q3 =q4 = q5 = q6 =0.5 rad
Phương trình khâu 1:
J eff θ̈m + Beff θ̇m = K1 . u1 - d1 (1)
1 1 1 1
Điều khiện ổn định : e ss ≤ 0,01

t r ≤ 1(s)
P . O ≤ 10%
t s ≤ 5 (s)
Bộ điều khiển PD:
Tín hiệu đầu vào U(s) được xác định bởi:

U (s) = Kp ( θdm (s) - θ m (s) ) - KD .S. θm (s)(2)
K D là hệ số vi phân
K p là hệ số tỉ lệ
Sử dụng phép biến đổi Laplace cho hai vế của phương trình (1) và kết hợp với phương
trình (2) →Phương trình hệ kín trở thành :
2,19. 10-3 .s2 .θm (s ) + 1,3242.s. θm (s ) = K1 K p ( θdm ( s ) - θm ( s )) - K 1 K d .s. θm(s) - D(s)
→ [ 2,19.10 -3 s2 + ( K1 K d +1,3242 ).s + K1 K p ] . θm(s) = K1 Kp θdm (s) - D(s)

-3 2
Đặt Ω(s) = 2,19.10 s + ( K1 Kd +1,3242 ) .s+ K1 K p
Với: Ω(s) là đa thức đặc trưng của hệ kín
K1 K p d D(s)
→ θm(s) = . θm (s) -
Ω(s) Ω(s)
Sai số xác lập:
-3 2
d 2,19.10 s + ( K1 K d + 1,3242 ) .s d D
E(s) = θ (s) - θm(s) =
m . θm (s) + (s)
Ω(s) Ω(s)
Để thuận tiện cho việc thiết kế ta coi D(s) là rất nhỏ D(s) = D(s) =0,001
e ss = lim s. E(s ) = lim

s→0 [
s 2 ( 2,19.10 -3 s + ( K 1 K d + 1,3242 ) ) 0.5 0.001
s→0 Ω( s )
.
s
+
Ω( s ) ]
= lim
s→0 [ 0,001
-3 2
2,19. 10 s + ( K1 Kd + 1,3242 ) .s+ K 1 K p
=
0,001
K1 K p ]
0.001
Điều kiện: e ss = K K ≤ 0,01 → K1 K p ≥ 0,1
1 p
K 1 = 1,0198 → K p ≥ 0,098 (3)

-3 2
Đa thức đặc trưng : Ω(s ) = 2,19. 10 s + ( K1 Kd +1,3242 ) .s + K1 Kp
-3 2 1,0198 .K d + 1,3242 1,0198 .K p
=2,19. 10 ( s + -3
s+ -3
)
2,19. 10 2,19.10
= 2,19. 10-3 ( s 2 + 2.ξ . W n .s + W 2n )
{
1,0198 .K p 2
= Wn
{
-3 2
2,19. 10 ↔ K p = 0,00215. W n
1,0198.K d + 1,3242 Kd ≈ 0, 0043 .ξ . W n - 0,00165
-3
= 2.ξ . W n
2,19.10
Độ quá điều chỉnh:

-ξπ
Có P.O = exp( ) ≤ 0,1
√1- ξ 2
-ξπ 0,1
≤e
√1- ξ 2
0,1 0,1 2
ξ -e 2 (-e ) 2
≥ → ξ ≥ .(1-ξ )
√1- ξ2 π π
2
0,1 2 2
(- e ) / π
1≥ξ ≥ 0,1 2
|e |
1+ 2
π
→0.11≤ ξ ≤1
4 4
Thời gian ổn định: ts = ≤ 5 → ξ. W n ≥
ξ. W n 5
Thời gian tăng:

4
tr = [ 1,589 ξ 3 - 0,156 ξ 2 + 0,924ξ ] ≤ 1
Wn
4
Chọn ξ= 1 thì ξ. W n ≥
5
W n ≥ 0.8
Chọn W n = 50
Khi đó:
2 2
K p = 0,00215 . W n = 0, 00215 .50 = 5,375 → TMĐK ( 3 )
K d ≈ 0,0043 .ξ . W n - 0,00165 = 0,0043.1.50 - 0,00165 = 0,21335
Bộ điều khiển PID:

Với:
K p = 5,375
K d = 0,21335
( Beff + K 1 . Kd ) . K p
Ki<
j eff
-3 2
Đa thức đặc trưng: Ω(s ) = 2,19.10 s + ( K 1 K d + 1,3242 ) .s+ K1 K p
2
=j eff s + ( 1,0198. Kd + Beff ) .s + 1,0198 . Kp
{
j eff = 2,19. 10-3
→ Beff = 1,3242
K1 = 1,0198
Do đó: K i < 3784,035

Chọn K i = 5

J eff θ̈m + Beff θ̇m = K2 . u 2 - d 2 (4)
2 2 2 2

t r ≤ 1(s)
PO ≤ 10%
t s ≤ 5 (s)

U (s) = Kp ( θdm (s) - θ m (s) ) - KD .S. θm (s) (5)
2,372. 10-3 .s2 .θm (s ) + 1,3242.s. θm (s ) = K 2 K p ( θdm ( s ) - θm ( s ) ) - K2 Kd .s. θm(s) - D(s)
→ [ 2,372.10 -3 s2 + ( K 2 K d + 1,3242 ).s + K2 Kp ] θm(s) = K 2 K p θdm (s) - D(s)

-3 2
Đặt Ω(s) = 2,372.10 s + ( K2 Kd + 1,3242 ) .s + K2 K p
K2 Kp d D(s)
→ θm(s) = . θm (s) -
Ω(s) Ω(s)
-3 2
d 2,372. 10 s + ( K2 Kd +1,3242 ) .s d D
E(s) = θ (s) - θm(s) =
m . θm (s)+ (s)
Ω(s) Ω(s)

s→0 s→0 [ s2 (2,372. 10-3 s+ ( K 2 K d + 1,3242 ) )
Ω( s )
.
0.5 0.001
s
+
Ω( s ) ]
= lim
s→0 [ 0,001
-3 2
=
0,001
2,372. 10 s + ( K2 Kd + 1,3242 ) .s+ K2 Kp K2 K p ]
0.001
1 p
K 2 = 1,0198 → K p ≥ 0,098 (6)

-3 2
Đa thức đặc trưng : Ω(s ) = 2,372.10 s + ( K2 Kd +1,3242 ) .s+ K 2 K p
-3 2 1,0198.K d +1,3242 1,0198.K p
=2,372. 10 ( s + -3
s+ -3
)
2,372.10 2,372. 10
= 2,372. 10-3 ( s 2 +2.ξ . W n .s+ W 2n )
{
1,0198.K p 2
= Wn
{
-3 2
2,372.10 ↔ K p = 0,002326. W n
1,0198.K d +1,3242 K d ≈ 0,00465.ξ . W n - 0,0018
-3
=2.ξ . W n
2,372.10

-ξπ
Có P.O = exp( ) ≤ 0,1
√1- ξ 2
-ξπ 0,1
≤ e
√1- ξ 2
0,1 0,1 2
ξ -e (-e )
2 2
≥ → ξ ≥ 2 .(1-ξ )
√1- ξ π
2
π
0,1 2 2
(- e ) / π
1≥ξ ≥ 0,1 2
|e |
1+ 2
π
→0.11 ≤ ξ ≤1
4 4
ξ. W n 5
Thời gian tăng:

4
tr = [ 1,589 ξ 3 -0,156 ξ2 +0,924ξ ] ≤ 1
Wn
4
5
W n ≥ 0.8
Chọn W n = 45
Khi đó:
2 2
K p = 0,002326. W n = 0,002326 . 45 = 4,71015→TMĐK ( 6 )
K d ≈ 0,00465. ξ . W n - 0,0018 = 0,00465 .1.45- 0,0018 = 0,20745

Với :
K p =4,71015
K d =0,20745
( Beff + K 2 . Kd ) . K p
Ki<
j eff
-3 2
Đa thức đặc trưng: Ω(s ) = 2,372.10 s + ( K2 Kd +1,3242 ) .s+ K 2 K p
2
=j eff s + ( 1,0198. Kd + Beff ) .s+1,0198. Kp
{
jeff =2,372. 10-3
=> Beff = 1,3242
K2 = 1,0198
Do đó: K i < 3049,6

Chọn K i = 5

J eff θ̈m + Beff θ̇m = K 3 . u 3 - d 3 (7)
3 3 3 3
t r ≤ 1(s)
PO ≤ 10%
t s ≤ 5 (s)

2,41. 10-3 .s2 .θm (s ) + 1,3242.s. θm ( s) = K 3 K p ( θdm ( s ) - θm ( s ) ) - K3 Kd .s. θm(s) - D(s)
→ [ 2,41.10 -3 s2 +( K3 Kd + 1,3242 ).s+K3 K p ] θ m(s) = K3 Kp θdm (s) - D(s)

−3 2
Đặt Ω(s)=2, 41.10 s + ( K 3 K d +1,3242 ) . s+ K 3 K p
K3 Kp d D(s)
→ θm(s) = . θm (s) -
Ω(s) Ω(s)
-3 2
d 2,41. 10 s + ( K3 Kd +1,3242 ) .s d D
E(s) = θ (s) - θm(s) =
m . θm (s)+ (s)
Ω(s) Ω(s)

s→0 s→0 [ s2 (2,41. 10-3 s+ ( K 3 Kd +1,3242 ) )
Ω( s )
.
0.5 0.001
s
+
Ω( s ) ]
= lim
s→0 [ 0,001
-3 2
=
0,001
2,41. 10 s + ( K3 K d + 1,3242 ) .s+ K3 K p K 3 K p ]
0.001
Điều kiện: e ss = K K ≤ 0,01=> K 3 K p ≥ 0,1
3 p
K 3 = 1,0198 → K p ≥ 0,098 (9)

-3 2
Đa thức đặc trưng : Ω(s ) = 2,41. 10 s + ( K3 Kd + 1,3242 ) .s+ K3 Kp
-3 2 1,0198.K d +1,32426 1,0198.K p
=2,41. 10 ( s + -3
s+ -3
)
2,41.10 2,41.10
= 2,41. 10-3 ( s 2 +2.ξ . W n .s+ W 2n )
{
1,0198.K p 2
= Wn
{
-3 2
2,41.10 ↔ K p = 0,00236. W n
1,0198.K d +1,3242 K d ≈ 0,004726.ξ . W n - 0,00182
-3
=2.ξ . W n
2,41.10

-ξπ
Có P.O = exp( ) ≤ 0,1
√1- ξ 2
-ξπ 0,1
≤ e
√1- ξ 2
0,1 0,1 2
ξ -e 2 (-e ) 2
≥ → ξ ≥ .(1-ξ )
√1- ξ π
2
π
2
0,1 2 2
(- e ) / π
1≥ξ ≥ 0,1 2
|e |
1+ 2
π
→0.11 ≤ ξ ≤1
4 4
ξ. W n 5
Thời gian tăng:

4
tr = [ 1,589 ξ 3 -0,156 ξ2 +0,924ξ ] ≤ 1
Wn
4
5
W n ≥ 0.8
Chọn W n = 40
Khi đó:
2 2
K p = 0,00236. W n = 0,00236 . 40 = 3,776→TMĐK ( 9 )
K d ≈ 0,004726. ξ . W n - 0,00182 = 0,004726 .1.40- 0,00182 = 0,18722

Với :
K p = 3,776
K d = 0,18722
( Beff + K 3 . Kd ) . K p
Ki<
j eff
-3 2
2
=j eff s + ( 1,0198. Kd + Beff ) .s+1,0198. Kp
{
jeff =2,41. 10-3
=> Beff = 1,3242
K3 = 1,0198
Do đó: K i < 2373,9

Chọn K i = 5

4 4 4 4

t r ≤ 1(s)
PO ≤ 10%
t s ≤ 5 (s)

Sử dụng phép biến đổi Laplace cho hai vế của phương trình (10) và kết hợp với
phương trình (11) →Phương trình hệ kín trở thành :
1,551. 10-3 .s2 . θm (s ) + 1,3242.s. θm (s ) = K 4 Kp ( θdm ( s ) - θm ( s ) ) - K 4 Kd .s. θm(s) - D(s)
→ [ 1,551.10 -3 s 2 +( K4 K d +1,3242 ).s+ K4 K p ] θm(s) = K4 K p θdm (s) - D(s)

−3 2
Đặt Ω(s)=1,551.10 s + ( K 4 K d +1,3242 ) . s+ K 4 K p
K4 K p d D(s)
→ θm(s) = .θm (s) -
Ω(s) Ω(s)
-3 2
d 1,551. 10 s + ( K 4 Kd +1,3242 ) .s d D
E(s) = θ (s) - θm(s) =
m . θm (s)+ (s)
Ω(s) Ω(s)

s→0 s→0 [ s2 (1,551. 10-3 s+ ( K 4 Kd + 1,3242 ) )
Ω( s )
.
0.5 0.001
s
+
Ω( s ) ]
= lim
s→0 [ 0,001
-3 2
=
0,001
1,551. 10 s + ( K 4 Kd +1,3242 ) .s+ K 1 K p K4 K p ]
0.001
Điều kiện: e ss = K K ≤ 0,01→ K4 K p ≥ 0,1
4 p
K 4 = 1,0198 → Kp ≥ 0,098(12)
-3 2
Đa thức đặc trưng : Ω(s ) = 1,551.10 s + ( K4 Kd + 1,3242 ) .s+ K4 K p
-3 2 1,0198.K d +1,3242 1,0198.K p
=1,551. 10 ( s + -3
s+ -3
)
1,551.10 1,551. 10
= 1,551. 10-3 ( s 2 +2.ξ . W n .s+ W 2n )
{
1,0198.K p 2
)= W n
{
-3 2
1,551.10 ↔ K p = 0,00152. W n
1,0198.K d +1,3242 K d ≈ 0,00304.ξ . W n - 0,001 17
-3
=2.ξ . W n
1,551.10

-ξπ
Có P.O = exp( ) ≤ 0,1
√1- ξ 2
-ξπ 0,1
≤ e
√1- ξ 2
0,1 0,1 2
ξ -e 2 (-e ) 2
≥ → ξ ≥ .(1-ξ )
√1- ξ π
2
π
2
0,1 2 2
(- e ) / π
1≥ξ ≥ 0,1 2
|e |
1+ 2
π
→0.11 ≤ ξ ≤1
4 4
ξ. W n 5
Thời gian tăng:

4
tr = [ 1,589 ξ 3 -0,156 ξ2 +0,924ξ ] ≤ 1
Wn
4
5
W n ≥ 0.8
Chọn W n = 35
Khi đó:
2 2
K p = 0,0 0152. W n = 0,0 0152 .35 = 1,862→TMĐK ( 12 )
K d ≈ 0,00304. ξ . W n - 0,00117 = 0,00304 .1.35- 0,00117 = 0,10523

Với :
K p = 1,862
K d = 0,10523
( Beff + K 4 . Kd ) . Kp
Ki<
j eff
-3 2
Đa thức đặc trưng: Ω(s ) = 1,551.10 s + ( K4 Kd + 1,3242 ) .s+ K4 K p
2
=j eff s + ( 1,0198. Kd + Beff ) .s+ 1,0198 . K p
{
jeff =1,551. 10-3
=> Beff = 1,3242
K4 = 1,0198
Do đó: K i < 1718,55

Chọn K i = 5

5 5 5 5

t r ≤ 1(s)
PO ≤ 10%
t s ≤ 5 (s)

phương trình (14) →Phương trình hệ kín trở thành:
1,536. 10-3 .s2 .θ m ( s) +1,3242 .s. θm ( s) = K5 Kp ( θdm ( s ) - θm ( s ) ) - K5 Kd .s. θm(s) - D(s)
→ [ 1,536.10 -3 s2 +( K 5 K d + 1,3242 ).s+K 5 Kp ] θm(s) = K5 K p θdm (s) - D(s)

-3 2
Đặt Ω(s) = 1,536.10 s + ( K5 Kd + 1,3242 ) .s+ K5 Kp

K5 Kp d D(s)
→ θm(s) = . θm (s) -
Ω(s) Ω(s)
-3 2
d 1,536.10 s + ( K5 K d + 1,3242 ) .s D(s)
E(s) = θ (s) - θm(s) =
m . θdm (s)+
Ω(s) Ω(s)

s→0 s→0 [ s2 (1,536. 10-3 s+ ( K5 Kd + 1,3242 ) )
Ω( s )
.
0.5 0.001
s
+
Ω( s ) ]
= lim
s→0 [ 0,001
-3 2
=
0,001
1,536. 10 s + ( K 5 Kd +1,3242 ) .s+ K5 K p K5 Kp ]
0.001
5 p
K 5 = 1,0198 → K p ≥ 0,098 (15)

-3 2
Đa thức đặc trưng : Ω(s ) = 1,551.10 s + ( K5 Kd +1,3242 ) .s+ K 5 K p
-3 2 1,0198.K d + 1,3242 1,0198.K p
=1,536. 10 (s + -3
s+ -3
)
1,536. 10 1,536. 10
= 1,536. 10-3 ( s2 +2.ξ . W n .s+ W 2n )
{
1,0198 .K p 2
)= W n
{
-3 2
1,536. 10 ↔ K p = 0,0 015 . W n
1,0198.K d +1,3242 K d ≈ 0,003.ξ . W n - 0,00 116
-3
=2.ξ . W n
1,536.10

-ξπ
Có P.O = exp( ) ≤ 0,1
√1- ξ 2
-ξπ 0,1
≤ e
√1- ξ 2
0,1 0,1 2
ξ -e (-e )
2 2
≥ → ξ ≥ 2 .(1-ξ )
√1- ξ π
2
π
0,1 2 2
(- e ) / π
1≥ξ ≥ 0,1 2
|e |
1+ 2
π
→0.11 ≤ ξ ≤1
4 4
ξ. W n 5
Thời gian tăng:

4
tr = [ 1,589 ξ 3 -0,156 ξ2 +0,924ξ ] ≤ 1
Wn
4
5
W n ≥ 0.8
Chọn W n = 38
Khi đó:
2 2
K p = 0,0 015. W n = 0,0015 . 38 = 2,1766→TMĐK ( 15 )
K d ≈ 0,003. ξ . W n - 0,00116 = 0,003 .1.38- 0,00116 = 0,11284

Với :
K p = 2,1766
K d = 0,11284
( Beff + K 5 . Kd ) . K p
Ki<
j eff
-3 2
2
=j eff s + ( 1,0198. Kd + Beff ) .s+1,0198. Kp
{
jeff =1,536. 10-3
=> Beff = 1,3242
K5 = 1,0198
Do đó: K i < 2039,5

Chọn K i = 5
J eff θ̈m + Beff θ̇ m = K6 . u6 - d6 (16)
6 6 6 6

t r ≤ 1(s)
PO ≤ 10%
t s ≤ 5 (s)

phương trình (17) →Phương trình hệ kín trở thành :
1,53. 10-3 .s2 . θm (s ) + 1,3242.s. θm (s ) = K6 K p ( θdm ( s ) - θm ( s ) ) - K6 Kd .s. θm(s) - D(s)
→ [ 1,53.10 -3 s2 +( K 6 Kd +1,3242 ).s+ K6 Kp ] θm(s) = K 6 K p θdm (s) - D(s)

-3 2
Đặt Ω(s) = 1,53.10 s + ( K6 Kd + 1,3242 ) .s+ K6 K p

K6 K p d D(s)
→ θm(s) = . θm (s) -
Ω(s) Ω(s)
-3 2
d 1,53.10 s + ( K6 K d + 1,3242 ) .s D(s)
E(s) = θ (s) - θm(s) =
m . θdm (s)+
Ω(s) Ω(s)

s→0 s→0 [ s2 (1,53. 10-3 s+ ( K6 Kd + 1,3242 ) )
Ω( s )
.
0.5 0.001
s
+
Ω( s ) ]
= lim
s→0 [ 0,001
-3 2
=
0,001
1,53. 10 s + ( K 6 Kd +1,3242 ) .s+ K 6 Kp K6 K p ]
0.001
Điều kiện: e ss = K K ≤ 0,01 → K 3 K p ≥ 0,1
6 p
K 6 = 1,0198 → K p ≥ 0,098 (18)

-3 2
Đa thức đặc trưng : Ω(s ) = 1,53.10 s + ( K6 Kd +1,3242 ) .s+ K 6 Kp
-3 2 1,0198.K d + 1,3242 1,0198.K p
=1,53. 10 (s + -3
s+ -3
)
1,53. 10 1,53. 10
= 1,53. 10-3 ( s 2 +2.ξ . W n .s+ W2n )
{
1,0198 .K p 2
)= W n
{
-3 2
1,53. 10 ↔ K p = 0,0 015 . W n
1,0198.K d +1,3242 K d ≈ 0,003.ξ . W n - 0,001155
-3
=2.ξ . W n
1,53.10

-ξπ
Có P.O = exp( ) ≤ 0,1
√1- ξ 2
-ξπ 0,1
≤ e
√1- ξ 2
0,1 0,1 2
ξ -e 2 (-e ) 2
≥ → ξ ≥ .(1-ξ )
√1- ξ π
2
π
2
0,1 2 2
(- e ) / π
1≥ξ ≥ 0,1 2
|e |
1+ 2
π
→0.11 ≤ ξ ≤1
4 4
ξ. W n 5
Thời gian tăng:

4
tr = [ 1,589 ξ 3 -0,156 ξ2 +0,924ξ ] ≤ 1
Wn
4
5
W n ≥ 0.8
Chọn W n = 42
Khi đó:
2 2
K p = 0,0015. W n = 0,0015 . 42 = 2,646 →TMĐK ( 18 )
K d ≈ 0,003. ξ . W n - 0,001155 = 0,003 .1.42- 0,001155 = 0,124845

Với :
K p =2,646
K d =0,124845
( Beff + K 6 . K d ) . K p
Ki<
j eff
-3 2
Đa thức đặc trưng: Ω(s ) = 1,53.10 s + ( K6 Kd +1,3242 ) .s+ K 6 Kp
2
=j eff s + ( 1,0198. Kd + Beff ) .s+1,0198. Kp
{
jeff =1,53. 10-3
=> Beff = 1,3242
K6 =1,0198
Do đó: K i < 2510,27

Chọn K i = 5
1. Xây dựng sơ đồ simulink

Các hàm Function trong simulink:
Function1:
function y=fcn(u)
r1=25;
r2=20;
r3=15;
r4=10;
r5=10;
r6=10;
qd1=u(2)/r1; qdd1=u(3)/r1;
q2=u(4)/r2; qd2=u(5)/r2; qdd2=u(6)/r2;
q3=u(7)/r3; qd3=u(8)/r3; qdd3=u(9)/r3;
q4=u(10)/r4; qd4=u(11)/r4; qdd4=u(12)/r4;
q5=u(13)/r5; qd5=u(14)/r5; qdd5=u(15)/r5;
y=1/r1*0,4153*qdd1 - 0,0623*qdd1*cos(2*q2) + 0,00015*qdd1*cos(2*q4) - 0,00005*qdd1*cos(2*q5) +
0,0001*qd2^2*cos(q2)+ 0,0018*qdd1*sin(2*q2) + 0,0042*qd2^2*sin(q2) - 0,0042*qdd2*cos(q2) + 0,000015*qdd1*cos(q5)
+ 0,0001*qdd2*sin(q2) - 0,000015*qd1*qd5*sin(q5) - 0,0597*qdd1*cos(2*q2)*cos(2*q3) +
0,00005*qdd1*cos(2*q4)*cos(2*q5) + 0,0224*qdd1*cos(2*q2)*sin(2*q3) + 0,0224*qdd1*cos(2*q3)*sin(2*q2) +
0,0597*qdd1*sin(2*q2)*sin(2*q3) + 0,0036*qd1*qd2*cos(2*q2) - 0,0307*qdd1*cos(q2)*cos(q3) -
0,0042*qdd2*cos(q2)*cos(q3) - 0,0042*qdd3*cos(q2)*cos(q3) + 0,1246*qd1*qd2*sin(2*q2) - 0,0003*qd1*qd4*sin(2*q4) +
0,0001*qd1*qd5*sin(2*q5) - 0,0409*qdd1*cos(q2)*sin(q3) - 0,0409*qdd1*cos(q3)*sin(q2) + 0,0001*qdd2*cos(q2)*sin(q3)
+ 0,0001*qdd2*cos(q3)*sin(q2) + 0,0001*qdd3*cos(q2)*sin(q3) + 0,0001*qdd3*cos(q3)*sin(q2)+
0,0021*qdd4*cos(q2)*sin(q3) + 0,0021*qdd4*cos(q3)*sin(q2) + 0,0001*qdd1*cos(q4)*sin(q5) -
0,0001*qdd4*cos(q4)*sin(q5) - 0,0001*qdd5*cos(q5)*sin(q4) + 0,0307*qdd1*sin(q2)*sin(q3) +
0,0042*qdd2*sin(q2)*sin(q3) + 0,0042*qdd3*sin(q2)*sin(q3) + 0,0001*qd3^2*cos(q2)*cos(q3) +
0,0042*qd3^2*cos(q2)*sin(q3)+ 0,0042*qd3^2*cos(q3)*sin(q2) - 0,0001*qd3^2*sin(q2)*sin(q3) +
0,0001*qd4^2*sin(q4)*sin(q5) + 0,0408*qd1*qd2*sin(q2)*sin(q3) + 0,0409*qd1*qd3*sin(q2)*sin(q3) -
0,0002*qd2*qd3*sin(q2)*sin(q3) - 0,0021*qd2*qd4*sin(q2)*sin(q3) - 0,0021*qd3*qd4*sin(q2)*sin(q3) -
0,00017*qd1*qd4*sin(q4)*sin(q5) + 0,000015*qdd1*cos(2*q2)*cos(2*q3)*cos(q5) -
0,00017*qdd1*cos(2*q2)*sin(2*q3)*cos(q5) - 0,00017*qdd1*cos(2*q3)*sin(2*q2)*cos(q5) +
0,0001*qdd1*cos(2*q2)*sin(2*q3)*sin(q4) + 0,0001*qdd1*cos(2*q3)*sin(2*q2)*sin(q4) -
0,000015*qdd1*sin(2*q2)*sin(2*q3)*cos(q5) - 0,0003*qd2^2*sin(2*q4)*cos(q2)*sin(q3) -
0,0003*qd2^2*sin(2*q4)*cos(q3)*sin(q2) - 0,0003*qd3^2*sin(2*q4)*cos(q2)*sin(q3) -
0,0003*qd3^2*sin(2*q4)*cos(q3)*sin(q2) + 0,0002*qdd1*cos(q2)*cos(q3)*cos(q5) - 0,0001*qdd2*cos(q2)*cos(q4)*sin(q3)
- 0,0001*qdd2*cos(q3)*cos(q4)*sin(q2) - 0,0001*qdd3*cos(q2)*cos(q4)*sin(q3) - 0,0001*qdd3*cos(q3)*cos(q4)*sin(q2) +
0,0001*qdd4*cos(q2)*cos(q3)*sin(q4) - 0,0006*qdd5*cos(q2)*cos(q3)*sin(q4) +
0,00015*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q4) + 0,00015*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q5) -
0,0002*qdd1*cos(q5)*sin(q2)*sin(q3) - 0,0001*qdd4*sin(q2)*sin(q3)*sin(q4) + 0,0006*qdd5*sin(q2)*sin(q3)*sin(q4)-
0,00015*qdd1*cos(2*q4)*sin(2*q2)*sin(2*q3) - 0,00015*qdd1*cos(2*q5)*sin(2*q2)*sin(2*q3) +
0,0448*qd1*qd2*cos(2*q2)*cos(2*q3) + 0,0448*qd1*qd3*cos(2*q2)*cos(2*q3) - 0,0001*qd2^2*cos(q2)*cos(q3)*cos(q4) -
0,0001*qd3^2*cos(q2)*cos(q3)*cos(q4) + 0,0001*qd4^2*cos(q2)*cos(q3)*cos(q4) + 0,1195*qd1*qd2*cos(2*q2)*sin(2*q3)
+ 0,1195*qd1*qd2*cos(2*q3)*sin(2*q2)+ 0,1195*qd1*qd3*cos(2*q2)*sin(2*q3) + 0,1195*qd1*qd3*cos(2*q3)*sin(2*q2)-
0,0001*qd1*qd4*cos(2*q5)*sin(2*q4) - 0,0001*qd1*qd5*cos(2*q4)*sin(2*q5) + 0,0003*qdd2*sin(2*q4)*cos(q2)*cos(q3)
+ 0,0003*qdd3*sin(2*q4)*cos(q2)*cos(q3) - 0,0002*qdd4*cos(2*q5)*cos(q2)*sin(q3) -
0,0002*qdd4*cos(2*q5)*cos(q3)*sin(q2) - 0,0448*qd1*qd2*sin(2*q2)*sin(2*q3) - 0,0448*qd1*qd3*sin(2*q2)*sin(2*q3) +
0,0001*qd2^2*cos(q4)*sin(q2)*sin(q3) + 0,0001*qd3^2*cos(q4)*sin(q2)*sin(q3) - 0,0001*qd4^2*cos(q4)*sin(q2)*sin(q3) -
0,0003*qdd2*sin(2*q4)*sin(q2)*sin(q3) - 0,0003*qdd3*sin(2*q4)*sin(q2)*sin(q3) - 0,0408*qd1*qd2*cos(q2)*cos(q3) -
0,0409*qd1*qd3*cos(q2)*cos(q3) + 0,0002*qd2*qd3*cos(q2)*cos(q3) + 0,0021*qd2*qd4*cos(q2)*cos(q3) +
0,0021*qd3*qd4*cos(q2)*cos(q3) + 0,00017*qd1*qd5*cos(q4)*cos(q5) - 0,0002*qd4*qd5*cos(q4)*cos(q5) +
0,0349*qd1*qd2*cos(q2)*sin(q3) + 0,0349*qd1*qd2*cos(q3)*sin(q2) + 0,0307*qd1*qd3*cos(q2)*sin(q3) +
0,0307*qd1*qd3*cos(q3)*sin(q2) + 0,0084*qd2*qd3*cos(q2)*sin(q3) + 0,0084*qd2*qd3*cos(q3)*sin(q2) -
0,000015*qdd1*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5) - 0,000015*qdd1*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5) -
0,0002*qd2^2*sin(2*q5)*cos(q2)*cos(q3)*sin(q4)) - 0,0002*qd3^2*sin(2*q5)*cos(q2)*cos(q3)*sin(q4) +
0,0002*qd4^2*sin(2*q5)*cos(q2)*cos(q3)*sin(q4) + 0,0003*qd1*qd4*sin(2*q2)*sin(2*q3)*sin(2*q4) +
0,0003*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(2*q5) + 0,00017*qdd1*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5) -
0,0001*qdd2*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3) - 0,0001*qdd3*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3) +
0,0006*qd2*qd4*cos(2*q4)*cos(q2)*cos(q3) - 0,0002*qd2*qd4*cos(2*q5)*cos(q2)*cos(q3) +
0,0006*qd3*qd4*cos(2*q4)*cos(q2)*cos(q3) - 0,0002*qd3*qd4*cos(2*q5)*cos(q2)*cos(q3) 0 +
0,0002*qd2^2*sin(2*q5)*sin(q2)*sin(q3)*sin(q4) + 0,0002*qd3^2*sin(2*q5)*sin(q2)*sin(q3)*sin(q4) -
0,0002*qd4^2*sin(2*q5)*sin(q2)*sin(q3)*sin(q4) - 0,000015*qdd4*cos(q2)*cos(q3)*cos(q4)*sin(q5) -
0,000015*qdd5*cos(q2)*cos(q3)*cos(q5)*sin(q4) - 0,0006*qd2*qd3*sin(2*q4)*cos(q2)*sin(q3) -
0,0006*qd2*qd3*sin(2*q4)*cos(q3)*sin(q2) - 0,0006*qd2*qd4*cos(2*q4)*sin(q2)*sin(q3) +
0,0002*qd2*qd4*cos(2*q5)*sin(q2)*sin(q3) - 0,0006*qd3*qd4*cos(2*q4)*sin(q2)*sin(q3) +
0,0002*qd3*qd4*cos(2*q5)*sin(q2)*sin(q3) + 0,0004*qd4*qd5*sin(2*q5)*cos(q2)*sin(q3) +
0,0004*qd4*qd5*sin(2*q5)*cos(q3)*sin(q2) - 0,0002*qdd1*cos(q2)*cos(q4)*sin(q3)*sin(q5) -
0,0002*qdd1*cos(q3)*cos(q4)*sin(q2)*sin(q5) - 0,00017*qdd2*cos(q2)*cos(q3)*sin(q4)*sin(q5) -
0,00017*qdd3*cos(q2)*cos(q3)*sin(q4)*sin(q5) + 0,00017*qdd4*cos(q2)*cos(q4)*sin(q3)*sin(q5) +
0,00017*qdd5*cos(q3)*cos(q5)*sin(q2)*sin(q4) - 0,000015*qdd2*cos(q2)*sin(q3)*sin(q4)*sin(q5) -
0,000015*qdd2*cos(q3)*sin(q2)*sin(q4)*sin(q5) 0 - 0,000015*qdd3*cos(q2)*sin(q3)*sin(q4)*sin(q5) -
0,000015*qdd3*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 0,000015*qdd4*cos(q4)*sin(q2)*sin(q3)*sin(q5) +
0,000015*qdd5*cos(q5)*sin(q2)*sin(q3)*sin(q4) - 0,0002*qdd1*cos(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4) -
0,0002*qdd1*cos(2*q3)*sin(2*q2)*sin(2*q5)*cos(q4) - 0,0001*qd2^2*cos(2*q5)*sin(2*q4)*cos(q2)*sin(q3) -
0,0001*qd2^2*cos(2*q5)*sin(2*q4)*cos(q3)*sin(q2) - 0,0001*qd3^2*cos(2*q5)*sin(2*q4)*cos(q2)*sin(q3) -
0,0001*qd3^2*cos(2*q5)*sin(2*q4)*cos(q3)*sin(q2) + 0,00017*qdd2*sin(q2)*sin(q3)*sin(q4)*sin(q5) +
0,00017*qdd3*sin(q2)*sin(q3)*sin(q4)*sin(q5) - 0,00034*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q5) -
0,00034*qd1*qd3*cos(2*q2)*cos(2*q3)*cos(q5)+ 0,0002*qd1*qd2*cos(2*q2)*cos(2*q3)*sin(q4) -
0,00003*qd1*qd2*cos(2*q2)*sin(2*q3)*cos(q5) - 0,00003*qd1*qd2*cos(2*q3)*sin(2*q2)*cos(q5) +
0,0002*qd1*qd3*cos(2*q2)*cos(2*q3)*sin(q4) - 0,00003*qd1*qd3*cos(2*q2)*sin(2*q3)*cos(q5)-
0,00003*qd1*qd3*cos(2*q3)*sin(2*q2)*cos(q5) + 0,0001*qd1*qd4*cos(2*q2)*sin(2*q3)*cos(q4) +
0,0001*qd1*qd4*cos(2*q3)*sin(2*q2)*cos(q4) - 0,000015*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(q5) -
0,0002*qdd4*sin(2*q5)*cos(q2)*cos(q3)*cos(q4) + 0,0003*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q5) +
0,0003*qd1*qd3*sin(2*q2)*sin(2*q3)*cos(q5) + 0,00017*qd1*qd5*cos(2*q2)*sin(2*q3)*sin(q5) +
0,00017*qd1*qd5*cos(2*q3)*sin(2*q2)*sin(q5) + 0,00005*qdd1*cos(2*q2)*cos(2*q3)*cos(2*q4)*cos(2*q5) -
0,000015*qd2^2*cos(q2)*cos(q3)*sin(q4)*sin(q5) - 0,000015*qd3^2*cos(q2)*cos(q3)*sin(q4)*sin(q5) +
0,000015*qd4^2*cos(q2)*cos(q3)*sin(q4)*sin(q5)- 0,0002*qd1*qd2*sin(2*q2)*sin(2*q3)*sin(q4) -
0,0002*qd1*qd3*sin(2*q2)*sin(2*q3)*sin(q4) + 0,000015*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(q5) -
0,0002*qdd2*sin(2*q5)*cos(q2)*sin(q3)*sin(q4) - 0,0002*qdd2*sin(2*q5)*cos(q3)*sin(q2)*sin(q4) -
0,0002*qdd3*sin(2*q5)*cos(q2)*sin(q3)*sin(q4) - 0,0002*qdd3*sin(2*q5)*cos(q3)*sin(q2)*sin(q4) +
0,0002*qdd4*sin(2*q5)*cos(q4)*sin(q2)*sin(q3) + 0,00017*qd2^2*cos(q2)*sin(q3)*sin(q4)*sin(q5) +
0,00017*qd2^2*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 0,00017*qd3^2*cos(q2)*sin(q3)*sin(q4)*sin(q5) +
0,00017*qd3^2*cos(q3)*sin(q2)*sin(q4)*sin(q5) - 0,00017*qd4^2*cos(q2)*sin(q3)*sin(q4)*sin(q5) -
0,00017*qd4^2*cos(q3)*sin(q2)*sin(q4)*sin(q5) - 0,0002*qd2*qd3*cos(q2)*cos(q3)*cos(q4) -
0,0006*qd4*qd5*cos(q2)*cos(q3)*cos(q4) - 0,00005*qdd1*cos(2*q4)*cos(2*q5)*sin(2*q2)*sin(2*q3) +
0,000015*qd2^2*sin(q2)*sin(q3)*sin(q4)*sin(q5) + 0,000015*qd3^2*sin(q2)*sin(q3)*sin(q4)*sin(q5) -
0,000015*qd4^2*sin(q2)*sin(q3)*sin(q4)*sin(q5) - 0,00025*qd1*qd2*cos(q2)*cos(q5)*sin(q3) -
0,00025*qd1*qd2*cos(q3)*cos(q5)*sin(q2) - 0,00025*qd1*qd3*cos(q2)*cos(q5)*sin(q3) -
0,00025*qd1*qd3*cos(q3)*cos(q5)*sin(q2) - 0,00025*qd1*qd5*cos(q2)*cos(q3)*sin(q5)+
0,0002*qd2*qd3*cos(q4)*sin(q2)*sin(q3) + 0,0006*qd2*qd5*cos(q2)*sin(q3)*sin(q4) +
0,0006*qd3*qd5*cos(q3)*sin(q2)*sin(q4) + 0,0006*qd4*qd5*cos(q4)*sin(q2)*sin(q3) -
0,0003*qd1*qd2*cos(2*q2)*cos(2*q4)*sin(2*q3) - 0,0003*qd1*qd2*cos(2*q3)*cos(2*q4)*sin(2*q2) -
0,0003*qd1*qd4*cos(2*q2)*cos(2*q3)*sin(2*q4) - 0,0003*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(2*q5) +
0,00025*qd1*qd5*sin(q2)*sin(q3)*sin(q5) - 0,00017*qdd1*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5) +
0,0001*qdd2*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3) + 0,0001*qdd3*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3) -
0,0004*qd2*qd3*sin(2*q5)*cos(q2)*cos(q3)*sin(q4) - 0,0004*qd2*qd5*cos(2*q5)*cos(q2)*sin(q3)*sin(q4) -
0,0004*qd2*qd5*cos(2*q5)*cos(q3)*sin(q2)*sin(q4) - 0,0004*qd3*qd5*cos(2*q5)*cos(q2)*sin(q3)*sin(q4) -
0,0004*qd3*qd5*cos(2*q5)*cos(q3)*sin(q2)*sin(q4) + 0,0004*qd4*qd5*cos(2*q5)*cos(q4)*sin(q2)*sin(q3) -
0,0001*qd1*qd2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3) -0,0001*qd1*qd2*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2) -
0,0001*qd1*qd3*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3) - 0,0001*qd1*qd3*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2)
- 0,0001*qd1*qd4*cos(2*q2)*cos(2*q3)*cos(2*q5)*sin(2*q4) -
0,0001*qd1*qd5*cos(2*q2)*cos(2*q3)*cos(2*q4)*sin(2*q5) + 0,0004*qd2*qd3*sin(2*q5)*sin(q2)*sin(q3)*sin(q4) +
0,0001*qd1*qd4*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4) + 0,0001*qd1*qd5*cos(2*q4)*sin(2*q2)*sin(2*q3)*sin(2*q5) -
0,00017*qd1*qd5*cos(2*q2)*cos(2*q3)*cos(q4)*cos(q5) + 0,0002*qd2*qd4*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3) +
0,0002*qd3*qd4*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3) - 0,00003*qd1*qd2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5) -
0,00003*qd1*qd3*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5) 0 - 0,000015*qd1*qd5*cos(2*q2)*sin(2*q3)*cos(q4)*cos(q5) -
0,000015*qd1*qd5*cos(2*q3)*sin(2*q2)*cos(q4)*cos(q5)+ 0,00034*qd1*qd2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5) +
0,00034*qd1*qd2*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5) + 0,00034*qd1*qd3*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5) +
0,00034*qd1*qd3*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5) + 0,00017*qd1*qd4*cos(2*q2)*cos(2*q3)*sin(q4)*sin(q5) -
0,0002*qd2*qd3*cos(2*q5)*sin(2*q4)*cos(q2)*sin(q3) - 0,0002*qd2*qd3*cos(2*q5)*sin(2*q4)*cos(q3)*sin(q2)+
0,00017*qd1*qd5*sin(2*q2)*sin(2*q3)*cos(q4)*cos(q5) - 0,0002*qd2*qd4*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3) -
0,0002*qd2*qd5*sin(2*q4)*sin(2*q5)*cos(q2)*cos(q3) - 0,0002*qd3*qd4*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3) -
0,0002*qd3*qd5*sin(2*q4)*sin(2*q5)*cos(q2)*cos(q3) + 0,00003*qd1*qd2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5) +
0,00003*qd1*qd3*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5) + 0,000015*qd1*qd4*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5) +
0,000015*qd1*qd4*cos(2*q3)*sin(2*q2)*sin(q4)*sin(q5) - 0,00017*qd1*qd4*sin(2*q2)*sin(2*q3)*sin(q4)*sin(q5) +
0,0002*qd2*qd5*sin(2*q4)*sin(2*q5)*sin(q2)*sin(q3) + 0,0002*qd3*qd5*sin(2*q4)*sin(2*q5)*sin(q2)*sin(q3) -
0,00003*qd4*qd5*cos(q2)*cos(q3)*cos(q4)*cos(q5) - 0,00025*qd1*qd2*cos(q2)*cos(q3)*cos(q4)*sin(q5) -
0,00025*qd1*qd3*cos(q2)*cos(q3)*cos(q4)*sin(q5) - 0,00025*qd1*qd5*cos(q2)*cos(q4)*cos(q5)*sin(q3) -
0,00025*qd1*qd5*cos(q3)*cos(q4)*cos(q5)*sin(q2) + 0,0003*qd4*qd5*cos(q2)*cos(q4)*cos(q5)*sin(q3) +
0,0003*qd4*qd5*cos(q3)*cos(q4)*cos(q5)*sin(q2) - 0,00003*qd2*qd3*cos(q2)*cos(q3)*sin(q4)*sin(q5) +
0,00003*qd4*qd5*cos(q4)*cos(q5)*sin(q2)*sin(q3) - 0,004*qd1*qd2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4) -
0,004*qd1*qd3*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4) - 0,004*qd1*qd5*cos(2*q2)*cos(2*q5)*sin(2*q3)*cos(q4) -
0,004*qd1*qd5*cos(2*q3)*cos(2*q5)*sin(2*q2)*cos(q4) + 0,00025*qd1*qd2*cos(q4)*sin(q2)*sin(q3)*sin(q5) +
0,00025*qd1*qd3*cos(q4)*sin(q2)*sin(q3)*sin(q5) + 0,00025*qd1*qd4*cos(q2)*sin(q3)*sin(q4)*sin(q5) +
0,00025*qd1*qd4*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 0,0003*qd2*qd3*cos(q2)*sin(q3)*sin(q4)*sin(q5) +
0,0003*qd2*qd3*cos(q3)*sin(q2)*sin(q4)*sin(q5) + 0,00003*qd2*qd3*sin(q2)*sin(q3)*sin(q4)*sin(q5) +
0,004*qd1*qd2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4) + 0,004*qd1*qd3*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4) +
0,0002*qd1*qd4*cos(2*q2)*sin(2*q3)*sin(2*q5)*sin(q4) + 0,0002*qd1*qd4*cos(2*q3)*sin(2*q2)*sin(2*q5)*sin(q4) -
0,004*qd4*qd5*cos(2*q5)*cos(q2)*cos(q3)*cos(q4)
Function2 :
function y=fcn(u)
r1=25;
r2=20;
r3=15;
r4=10;
r5=10;
r6=10;
qd1=u(2)/r1; qdd1=u(3)/r1;
q2=u(4)/r2; qd2=u(5)/r2; qdd2=u(6)/r2;
q3=u(7)/r3; qd3=u(8)/r3; qdd3=u(9)/r3;
q4=u(10)/r4; qd4=u(11)/r4; qdd4=u(12)/r4;
q5=u(13)/r5; qd5=u(14)/r5; qdd5=u(15)/r5;
y=1/r2 * 0,3368*qdd2 + 0,1994*qdd3 + 0,0049*cos(q2)- 5,5233*sin(q2) - 0,0003*qdd2*cos(2*q4) +
0,0001*qdd2*cos(2*q5) - 0,0003*qdd3*cos(2*q4) + 0,0001*qdd3*cos(2*q5) + 0,00017*qd5^2*cos(q5) -
0.5653*cos(q2)*cos(q3) + 0,0001*qd4^2*sin(q4) - 7.433*cos(q2)*sin(q3) - 7.433*cos(q3)*sin(q2) +
0.5653*sin(q2)*sin(q3) - 0,0018*qd1^2*cos(2*q2) - 0,0623*qd1^2*sin(2*q2) - 0,0042*qdd1*cos(q2) +
0,00003*qdd2*cos(q5) + 0,00003*qdd3*cos(q5) - 0,0001*qdd4*cos(q4) + 0,0006*qdd5*cos(q4) +
0,0001*qdd1*sin(q2) + 0,00017*qdd5*sin(q5) - 0,00003*qd2*qd5*sin(q5) - 0,00003*qd3*qd5*sin(q5) -
0,0006*qd4*qd5*sin(q4) - 0,0001*qdd2*cos(2*q4)*cos(2*q5) - 0,0001*qdd3*cos(2*q4)*cos(2*q5) -
0,0002*qd4^2*sin(2*q5)*cos(q4) - 0,0042*qdd1*cos(q2)*cos(q3) + 0,000015*qdd5*cos(q4)*cos(q5) +
0,0006*qd2*qd4*sin(2*q4) + 0,0006*qd3*qd4*sin(2*q4) - 0,0002*qd2*qd5*sin(2*q5) -
0,0002*qd3*qd5*sin(2*q5) + 0,0001*qdd1*cos(q2)*sin(q3) + 0,0001*qdd1*cos(q3)*sin(q2) +
0,00034*qdd2*cos(q4)*sin(q5) + 0,00034*qdd3*cos(q4)*sin(q5) - 0,0224*qd1^2*cos(2*q2)*cos(2*q3) +
0,0042*qdd1*sin(q2)*sin(q3) - 0,000015*qdd4*sin(q4)*sin(q5) - 0,0597*qd1^2*cos(2*q2)*sin(2*q3) -
0,0597*qd1^2*cos(2*q3)*sin(2*q2) + 0,0224*qd1^2*sin(2*q2)*sin(2*q3) + 0,0204*qd1^2*cos(q2)*cos(q3) +
0,068*cos(q2)*cos(q3)*cos(q5) - 0,0153*qd1^2*cos(q2)*sin(q3) –
0,0153*qd1^2*cos(q3)*sin(q2) - 0,000015*qd4^2*cos(q4)*sin(q5) - 0,000015*qd5^2*cos(q4)*sin(q5) -
0,0002*qdd4*sin(2*q5)*sin(q4) - 0,0204*qd1^2*sin(q2)*sin(q3) + 0.008*cos(q2)*sin(q3)*sin(q4) +
0.008*cos(q3)*sin(q2)*sin(q4) - 0,068*cos(q5)*sin(q2)*sin(q3) + 0,0021*qd1*qd4*sin(q2)*sin(q3) –
0,00034*qd2*qd4*sin(q4)*sin(q5) - 0,00034*qd3*qd4*sin(q4)*sin(q5) - 0,0004*qd4*qd5*cos(2*q5)*sin(q4) -
0,0001*qdd1*cos(q2)*cos(q4)*sin(q3) - 0,0001*qdd1*cos(q3)*cos(q4)*sin(q2) +
0,00017*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5) - 0,0001*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4) +
0,000015*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5) + 0,000015*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5)-
0,00017*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5) + 0,0001*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4) +
0,0002*qd2*qd4*cos(2*q5)*sin(2*q4) + 0,0002*qd2*qd5*cos(2*q4)*sin(2*q5) +
0,0003*qdd1*sin(2*q4)*cos(q2)*cos(q3) + 0,0001*qd1^2*cos(q2)*cos(q5)*sin(q3) +
0,0001*qd1^2*cos(q3)*cos(q5)*sin(q2) - 0,00002*cos(q2)*cos(q3)*cos(q6)*sin(q5) -
0,068*cos(q2)*cos(q4)*sin(q3)*sin(q5) - 0,068*cos(q3)*cos(q4)*sin(q2)*sin(q5) +
0,00015*qd1^2*cos(2*q2)*cos(2*q4)*sin(2*q3) + 0,00015*qd1^2*cos(2*q3)*cos(2*q4)*sin(2*q2) +
0,00015*qd1^2*cos(2*q2)*cos(2*q5)*sin(2*q3)+ 0,00015*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2) -
0,0003*qdd1*sin(2*q4)*sin(q2)*sin(q3) + 0,00002*cos(q2)*sin(q3)*sin(q4)*sin(q6) +
0,00002*cos(q3)*sin(q2)*sin(q4)*sin(q6) + 0,00002*cos(q6)*sin(q2)*sin(q3)*sin(q5) -
0,0021*qd1*qd4*cos(q2)*cos(q3) + 0,00034*qd2*qd5*cos(q4)*cos(q5) + 0,00034*qd3*qd5*cos(q4)*cos(q5) -
0,00003*qd4*qd5*cos(q5)*sin(q4) + 0,00005*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3) +
0,00005*qd1^2*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2) -
0,0001*qdd1*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3) +
0,0006*qd1*qd4*cos(2*q4)*cos(q2)*cos(q3) + 0,0002*qd1*qd4*cos(2*q5)*cos(q2)*cos(q3) -
0,0006*qd1*qd4*cos(2*q4)*sin(q2)*sin(q3) - 0,0002*qd1*qd4*cos(2*q5)*sin(q2)*sin(q3) -
0,00017*qdd1*cos(q2)*cos(q3)*sin(q4)*sin(q5) + 0,000015*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5) -
0,000015*qdd1*cos(q2)*sin(q3)*sin(q4)*sin(q5) - 0,000015*qdd1*cos(q3)*sin(q2)*sin(q4)*sin(q5) -
0,00017*qd1^2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5) - 0,00017*qd1^2*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5)
+ 0,00017*qdd1*sin(q2)*sin(q3)*sin(q4)*sin(q5) - 0,000015*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5) +
0,00012*qd1^2*cos(q2)*cos(q3)*cos(q4)*sin(q5) - 0,00002*cos(q2)*cos(q4)*cos(q5)*cos(q6)*sin(q3) -
0,00002*cos(q3)*cos(q4)*cos(q5)*cos(q6)*sin(q2) + 0,0002*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4) -
0,0002*qdd1*sin(2*q5)*cos(q2)*sin(q3)*sin(q4) - 0,0002*qdd1*sin(2*q5)*cos(q3)*sin(q2)*sin(q4) -
0,00012*qd1^2*cos(q4)*sin(q2)*sin(q3)*sin(q5) - 0,0002*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4) +
0,0002qd1*qd4*cos(q2)*sin(q3)*sin(q4)+ 0,0002qd1*qd4*cos(q3)*sin(q2)*sin(q4) -
0,0006*qd1*qd5*cos(q2)*sin(q3)*sin(q4) - 0,0006*qd1*qd5*cos(q3)*sin(q2)*sin(q4) +
0,0001*qdd1*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3) - 0,0004*qd1*qd4*sin(2*q5)*cos(q2)*cos(q4)*sin(q3) -
0,0004*qd1*qd5*cos(2*q5)*cos(q3)*sin(q2)*sin(q4) +
0,0002*qd1*qd4*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3)-
0,0002*qd1*qd4*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3) -
0,0002*qd1*qd5*sin(2*q4)*sin(2*q5)*cos(q2)*cos(q3) +
0,0002*qd1*qd5*sin(2*q4)*sin(2*q5)*sin(q2)*sin(q3) - 0,00034*qd1*qd4*cos(q2)*cos(q3)*cos(q4)*sin(q5) -
0,00034*qd1*qd5*cos(q2)*cos(q3)*cos(q5)*sin(q4) - 0,00003*qd1*qd4*cos(q2)*cos(q4)*sin(q3)*sin(q5) -
0,00003*qd1*qd4*cos(q3)*cos(q4)*sin(q2)*sin(q5) - 0,00003*qd1*qd5*cos(q2)*cos(q5)*sin(q3)*sin(q4) -
0,00003*qd1*qd5*cos(q3)*cos(q5)*sin(q2)*sin(q4) + 0,00034*qd1*qd4*cos(q4)*sin(q2)*sin(q3)*sin(q5) +
0,00034*qd1*qd5*cos(q5)*sin(q2)*sin(q3)*sin(q4)
Function3 :
function y=fcn(u)
r1=25;
r2=20;
r3=15;
r4=10;
r5=10;
r6=10;
qd1=u(2)/r1; qdd1=u(3)/r1;
q2=u(4)/r2; qd2=u(5)/r2; qdd2=u(6)/r2;
q3=u(7)/r3; qd3=u(8)/r3; qdd3=u(9)/r3;
q4=u(10)/r4; qd4=u(11)/r4; qdd4=u(12)/r4;
q5=u(13)/r5; qd5=u(14)/r5; qdd5=u(15)/r5;
y=1/r3*0,1994*qdd2 + 0,1994*qdd3- 0,0003*qdd2*cos(2*q4) + 0,0001*qdd2*cos(2*q5) -
0,0003*qdd3*cos(2*q4) + 0,0001*qdd3*cos(2*q5) + 0,00017*qd5^2*cos(q5) - 0.5653*cos(q2)*cos(q3) +
0,0001*qd4^2*sin(q4) - 7.433*cos(q2)*sin(q3) - 7.433*cos(q3)*sin(q2) + 0.5653*sin(q2)*sin(q3) +
0,00003*qdd2*cos(q5) + 0,00003*qdd3*cos(q5) - 0,0001*qdd4*cos(q4) + 0,0006*qdd5*cos(q4) +
0,00017*qdd5*sin(q5) - 0,00003*qd2*qd5*sin(q5) - 0,00003*qd3*qd5*sin(q5) - 0,0001*qd4*qd5*sin(q4) -
0,0001*qdd2*cos(2*q4)*cos(2*q5) - 0,0001*qdd3*cos(2*q4)*cos(2*q5) - 0,00005*qd4^2*sin(2*q5)*cos(q4) -
0,0042*qdd1*cos(q2)*cos(q3) + 0,000015*qdd5*cos(q4)*cos(q5) + 0,0006*qd2*qd4*sin(2*q4) +
0,0006*qd3*qd4*sin(2*q4)- 0,0002*qd2*qd5*sin(2*q5) - 0,0002*qd3*qd5*sin(2*q5) +
0,0001*qdd1*cos(q2)*sin(q3) + 0,0001*qdd1*cos(q3)*sin(q2) + 0,00034*qdd2*cos(q4)*sin(q5) +
0,00034*qdd3*cos(q4)*sin(q5) - 0,0224*qd1^2*cos(2*q2)*cos(2*q3) + 0,0042*qdd1*sin(q2)*sin(q3) -
0,000015*qdd4*sin(q4)*sin(q5) –
0,0597*qd1^2*cos(2*q2)*sin(2*q3) - 0,0597*qd1^2*cos(2*q3)*sin(2*q2) +
0,0224*qd1^2*sin(2*q2)*sin(2*q3) + 0,0204*qd1^2*cos(q2)*cos(q3) + 0,068*cos(q2)*cos(q3)*cos(q5) -
0,0153*qd1^2*cos(q2)*sin(q3) - 0,0153*qd1^2*cos(q3)*sin(q2) - 0,000015*qd4^2*cos(q4)*sin(q5) -
0,000015*qd5^2*cos(q4)*sin(q5) –
0,0002*qdd4*sin(2*q5)*sin(q4) - 0,0204*qd1^2*sin(q2)*sin(q3) + 0.008*cos(q2)*sin(q3)*sin(q4) +
0.008*cos(q3)*sin(q2)*sin(q4) - 0,068*cos(q5)*sin(q2)*sin(q3) + 0,0021*qd1*qd4*sin(q2)*sin(q3) -
0,00034*qd2*qd4*sin(q4)*sin(q5) - 0,00034*qd3*qd4*sin(q4)*sin(q5) –
0,0001*qd4*qd5*cos(2*q5)*sin(q4) - 0,0001*qdd1*cos(q2)*cos(q4)*sin(q3) -
0,0001*qdd1*cos(q3)*cos(q4)*sin(q2)+ 0,00017*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5) -
0,0001*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4) + 0,000015*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5) +
0,000015*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5) - 0,00017*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5) +
0,0001*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4) +
0,0003*qdd1*sin(2*q4)*cos(q2)*cos(q3) + 0,00012*qd1^2*cos(q2)*cos(q5)*sin(q3) +
0,00012*qd1^2*cos(q3)*cos(q5)*sin(q2) - 0,00002*cos(q2)*cos(q3)*cos(q6)*sin(q5) -
0,068*cos(q2)*cos(q4)*sin(q3)*sin(q5) - 0,068*cos(q3)*cos(q4)*sin(q2)*sin(q5) +
0,00015*qd1^2*cos(2*q2)*cos(2*q5)*sin(2*q3) + 0,00015*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2) –
0,0003*qdd1*sin(2*q4)*sin(q2)*sin(q3) + 0,00002*cos(q2)*sin(q3)*sin(q4)*sin(q6) +
0,00002*cos(q3)*sin(q2)*sin(q4)*sin(q6) + 0,00002*cos(q6)*sin(q2)*sin(q3)*sin(q5) -
0,0021*qd1*qd4*cos(q2)*cos(q3) + 0,00034*qd2*qd5*cos(q4)*cos(q5) + 0,00034*qd3*qd5*cos(q4)*cos(q5) -
0,00003*qd4*qd5*cos(q5)*sin(q4) + 0,00005*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3) +
0,00005*qd1^2*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2) -
0,0001*qdd1*cos(2*q5)*sin(2*q4)*sin(q2)*sin(q3) + 0,0006*qd1*qd4*cos(2*q4)*cos(q2)*cos(q3) +
0,0002*qd1*qd4*cos(2*q5)*cos(q2)*cos(q3) - 0,0006*qd1*qd4*cos(2*q4)*sin(q2)*sin(q3) -
0,0002*qd1*qd4*cos(2*q5)*sin(q2)*sin(q3) - 0,00017*qdd1*cos(q2)*cos(q3)*sin(q4)*sin(q5) +
0,000015*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5) - 0,000015*qdd1*cos(q2)*sin(q3)*sin(q4)*sin(q5) -
0,000015*qdd1*cos(q3)*sin(q2)*sin(q4)*sin(q5) - 0,00017*qd1^2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5) -
0,00017*qd1^2*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5) + 0,00017*qdd1*sin(q2)*sin(q3)*sin(q4)*sin(q5) -
0,000015*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5) + 0,00012*qd1^2*cos(q2)*cos(q3)*cos(q4)*sin(q5) -
0,00002*cos(q2)*cos(q4)*cos(q5)*cos(q6)*sin(q3) - 0,00002*cos(q3)*cos(q4)*cos(q5)*cos(q6)*sin(q2) +
0,0002*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4) - 0,0002*qdd1*sin(2*q5)*cos(q2)*sin(q3)*sin(q4) -
0,0002*qdd1*sin(2*q5)*cos(q3)*sin(q2)*sin(q4) - 0,00012*qd1^2*cos(q4)*sin(q2)*sin(q3)*sin(q5) -
0,0002*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4) + 0,0002*qd1*qd4*cos(q2)*sin(q3)*sin(q4) +
0,0002*qd1*qd4*cos(q3)*sin(q2)*sin(q4) –
0,0006*qd1*qd5*cos(q2)*sin(q3)*sin(q4) - 0,0006*qd1*qd5*cos(q3)*sin(q2)*sin(q4) +
0,0001*qdd1*cos(2*q5)*sin(2*q4)*cos(q2)*cos(q3) - 0,0004*qd1*qd4*sin(2*q5)*cos(q2)*cos(q4)*sin(q3) -
0,0004*qd1*qd5*cos(2*q5)*cos(q3)*sin(q2)*sin(q4) + 0,0002*qd1*qd4*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3)
- 0,0002*qd1*qd4*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3) -
0,0002*qd1*qd5*sin(2*q4)*sin(2*q5)*cos(q2)*cos(q3) +
0,0002*qd1*qd5*sin(2*q4)*sin(2*q5)*sin(q2)*sin(q3)-
0,00034*qd1*qd4*cos(q2)*cos(q3)*cos(q4)*sin(q5) - 0,00034*qd1*qd5*cos(q2)*cos(q3)*cos(q5)*sin(q4) -
0,00003*qd1*qd5*cos(q2)*cos(q5)*sin(q3)*sin(q4) - 0,00003*qd1*qd5*cos(q3)*cos(q5)*sin(q2)*sin(q4) +
0,00034*qd1*qd4*cos(q4)*sin(q2)*sin(q3)*sin(q5) + 0,00034*qd1*qd5*cos(q5)*sin(q2)*sin(q3)*sin(q4)
Function4 :
function y=fcn(u)
r1=25;
r2=20;
r3=15;
r4=10;
r5=10;
r6=10;
qd1=u(2)/r1; qdd1=u(3)/r1;
q2=u(4)/r2; qd2=u(5)/r2; qdd2=u(6)/r2;
q3=u(7)/r3; qd3=u(8)/r3; qdd3=u(9)/r3;
q4=u(10)/r4; qd4=u(11)/r4; qdd4=u(12)/r4;
q5=u(13)/r5; qd5=u(14)/r5;
y = 1/r4* 0,0021*qdd4 - 0,0002*qdd4*cos(2*q5) + 0,00015*qd1^2*sin(2*q4)– 0,0003*qd2^2*sin(2*q4) +
0,0001*qd3^2*sin(2*q4) – 0,0001*qdd2*cos(q4) - 0,0001*qdd3*cos(q4) +0,0006*qd2*qd5*sin(q4) +
0,0011*qd3*qd5*sin(q4)– 0,0006*qd2*qd3*sin(2*q4) - 0,000075*qd1*qd5*sin(2*q4) +
0,0004*qd4*qd5*sin(2*q5) +0,0021*qdd1*cos(q2)*sin(q3) + 0,0021*qdd1*cos(q3)*sin(q2) –
0,00012*qdd1*cos(q4)*sin(q5) + 0,000025*qd1^2*cos(2*q2)*cos(2*q3) – 0,000015*qdd2*sin(q4)*sin(q5) –
0,000015*qdd3*sin(q4)*sin(q5) –0,00013*qd1^2*cos(2*q2)*sin(2*q3) – 0,00013*qd1^2*cos(2*q3)*sin(2*q2)
+ 0,00005*qd1^2*cos(2*q5)*sin(2*q4) – 0,0001*qd2^2*cos(2*q5)*sin(2*q4) –
0,0001*qd3^2*cos(2*q5)*sin(2*q4) –
0,000025*qd1^2*sin(2*q2)*sin(2*q3) + 0,0002*qd1^2*cos(q2)*cos(q3)-
0.008*cos(q2)*cos(q3)*cos(q4) + 0,000018*qd1^2*cos(q2)*sin(q3) + 0,000018*qd1^2*cos(q3)*sin(q2) -
0,0002*qdd2*sin(2*q5)*sin(q4) - 0,0002*qdd3*sin(2*q5)*sin(q4) - 0,0002*qd1^2*sin(q2)*sin(q3) +
0,00008*qd1^2*sin(q4)*sin(q5) + 0,00017*qd2^2*sin(q4)*sin(q5) + 0,00017*qd3^2*sin(q4)*sin(q5) +
0.008*cos(q4)*sin(q2)*sin(q3) - 0,0021*qd1*qd2*sin(q2)*sin(q3) - 0,0021*qd1*qd3*sin(q2)*sin(q3) +
0,00034*qd2*qd3*sin(q4)*sin(q5) - 0,0004*qd2*qd5*cos(2*q5)*sin(q4) - 0,0007*qd3*qd5*cos(2*q5)*sin(q4)
+ 0,0001*qdd1*cos(q2)*cos(q3)*sin(q4) + 0,00017*qd1^2*cos(2*q2)*cos(2*q3)*cos(q5) -
0,00005*qd1^2*cos(2*q2)*sin(2*q3)*cos(q4) - 0,00005*qd1^2*cos(2*q3)*sin(2*q2)*cos(q4) +
0,000015*qd1^2*cos(2*q2)*sin(2*q3)*cos(q5) + 0,000015*qd1^2*cos(2*q3)*sin(2*q2)*cos(q5) -
0,0001*qdd1*sin(q2)*sin(q3)*sin(q4) - 0,00017*qd1^2*sin(2*q2)*sin(2*q3)*cos(q5) -
0,0002*qd2*qd3*cos(2*q5)*sin(2*q4) + 0,000075*qd1*qd5*cos(2*q5)*sin(2*q4) -
0,0002*qdd1*cos(2*q5)*cos(q2)*sin(q3) - 0,0002*qdd1*cos(2*q5)*cos(q3)*sin(q2) +
0,00012*qd1^2*cos(q2)*cos(q5)*sin(q3) + 0,00012*qd1^2*cos(q3)*cos(q5)*sin(q2) -
0,00002*cos(q2)*cos(q3)*cos(q4)*sin(q6) - 0,068*cos(q2)*cos(q3)*sin(q4)*sin(q5) +
0,00015*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q4) - 0,000013*qd1^2*cos(2*q2)*cos(2*q4)*sin(2*q3) -
0,00003*qd1^2*cos(2*q3)*cos(2*q5)*sin(2*q2) + 0,00002*cos(q4)*sin(q2)*sin(q3)*sin(q6) +
0,0021*qd1*qd2*cos(q2)*cos(q3) + 0,0021*qd1*qd3*cos(q2)*cos(q3) + 0,068*sin(q2)*sin(q3)*sin(q4)*sin(q5)
- 0,00015*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q4) + 0,000075*qd1*qd5*sin(2*q2)*sin(2*q3)*sin(2*q4) +
0,00005*qd1^2*cos(2*q2)*cos(2*q3)*cos(2*q5)*sin(2*q4) +
0,000013*qd1^2*cos(2*q2)*cos(2*q4)*cos(2*q5)*sin(2*q3) +
0,000013*qd1^2*cos(2*q3)*cos(2*q4)*cos(2*q5)*sin(2*q2) - 0,0006*qd1*qd2*cos(2*q4)*cos(q2)*cos(q3) -
0,0002*qd1*qd2*cos(2*q5)*cos(q2)*cos(q3) - 0,0006*qd1*qd3*cos(2*q4)*cos(q2)*cos(q3) -
0,0002*qd1*qd3*cos(2*q5)*cos(q2)*cos(q3) - 0,00005*qd1^2*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4) -
0,000015*qdd1*cos(q2)*cos(q3)*cos(q4)*sin(q5) + 0,0006*qd1*qd2*cos(2*q4)*sin(q2)*sin(q3) +
0,0002*qd1*qd2*cos(2*q5)*sin(q2)*sin(q3) + 0,0006*qd1*qd3*cos(2*q4)*sin(q2)*sin(q3) +
0,0002*qd1*qd3*cos(2*q5)*sin(q2)*sin(q3) + 0,0001*qd1*qd5*sin(2*q5)*cos(q2)*sin(q3) +
0,0001*qd1*qd5*sin(2*q5)*cos(q3)*sin(q2) + 0,00017*qdd1*cos(q2)*cos(q4)*sin(q3)*sin(q5) +
0,00017*qdd1*cos(q3)*cos(q4)*sin(q2)*sin(q5) + 0,000015*qd1^2*cos(2*q2)*cos(2*q3)*cos(q4)*sin(q5) +
0,000015*qdd1*cos(q4)*sin(q2)*sin(q3)*sin(q5) - 0,00008*qd1^2*cos(2*q2)*cos(2*q3)*sin(q4)*sin(q5) -
0,00017*qd1^2*cos(2*q2)*sin(2*q3)*cos(q4)*sin(q5) - 0,00017*qd1^2*cos(2*q3)*sin(2*q2)*cos(q4)*sin(q5) -
0,0000075*qd1^2*cos(2*q2)*sin(2*q3)*sin(q4)*sin(q5) -
0,0000075*qd1^2*cos(2*q3)*sin(2*q2)*sin(q4)*sin(q5) -
0,000015*qd1^2*sin(2*q2)*sin(2*q3)*cos(q4)*sin(q5) + 0,00008*qd1^2*sin(2*q2)*sin(2*q3)*sin(q4)*sin(q5)
- 0,0002*qdd1*sin(2*q5)*cos(q2)*cos(q3)*cos(q4) + 0,00012*qd1^2*cos(q2)*cos(q3)*cos(q4)*sin(q5) -
0,00002*cos(q2)*cos(q3)*cos(q5)*cos(q6)*sin(q4) + 0,00005*qd1^2*cos(2*q2)*cos(2*q3)*sin(2*q5)*cos(q4)
+ 0,0002*qdd1*sin(2*q5)*cos(q4)*sin(q2)*sin(q3) - 0,00012*qd1^2*cos(q2)*sin(q3)*sin(q4)*sin(q5) -
0,00012*qd1^2*cos(q3)*sin(q2)*sin(q4)*sin(q5) - 0,00012*qd1^2*cos(q4)*sin(q2)*sin(q3)*sin(q5) +
0,00002*cos(q5)*cos(q6)*sin(q2)*sin(q3)*sin(q4) + 0,0001*qd1*qd5*cos(q2)*cos(q3)*cos(q4) -
0,0001*qd1^2*cos(2*q2)*sin(2*q3)*sin(2*q5)*sin(q4) - 0,0001*qd1^2*cos(2*q3)*sin(2*q2)*sin(2*q5)*sin(q4)
- 0,00005*qd1^2*sin(2*q2)*sin(2*q3)*sin(2*q5)*cos(q4) - 0,0002*qd1*qd2*cos(q2)*sin(q3)*sin(q4) -
0,0002*qd1*qd2*cos(q3)*sin(q2)*sin(q4) - 0,0002*qd1*qd3*cos(q2)*sin(q3)*sin(q4) -
0,0002*qd1*qd3*cos(q3)*sin(q2)*sin(q4) - 0,0001*qd1*qd5*cos(q4)*sin(q2)*sin(q3) -
0,000075*qd1*qd5*cos(2*q2)*cos(2*q3)*sin(2*q4) + 0,0004*qd1*qd2*sin(2*q5)*cos(q2)*cos(q4)*sin(q3) +
0,0004*qd1*qd2*sin(2*q5)*cos(q3)*cos(q4)*sin(q2) + 0,0004*qd1*qd3*sin(2*q5)*cos(q2)*cos(q4)*sin(q3) +
0,0004*qd1*qd3*sin(2*q5)*cos(q3)*cos(q4)*sin(q2) + 0,0001*qd1*qd5*cos(2*q5)*cos(q4)*sin(q2)*sin(q3) +
0,000075*qd1*qd5*cos(2*q2)*cos(2*q3)*cos(2*q5)*sin(2*q4) -
0,000075*qd1*qd5*cos(2*q5)*sin(2*q2)*sin(2*q3)*sin(2*q4) -
0,0002*qd1*qd2*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3) -
0,0002*qd1*qd3*cos(2*q4)*cos(2*q5)*cos(q2)*cos(q3) +
0,0002*qd1*qd2*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3) +
0,0002*qd1*qd3*cos(2*q4)*cos(2*q5)*sin(q2)*sin(q3) + 0,00034*qd1*qd2*cos(q2)*cos(q3)*cos(q4)*sin(q5)
+ 0,00034*qd1*qd3*cos(q2)*cos(q3)*cos(q4)*sin(q5) + 0,00003*qd1*qd2*cos(q2)*cos(q4)*sin(q3)*sin(q5) +
0,00003*qd1*qd2*cos(q3)*cos(q4)*sin(q2)*sin(q5) + 0,00003*qd1*qd3*cos(q2)*cos(q4)*sin(q3)*sin(q5) +
0,00003*qd1*qd3*cos(q3)*cos(q4)*sin(q2)*sin(q5) - 0,00034*qd1*qd2*cos(q4)*sin(q2)*sin(q3)*sin(q5) -
0,00034*qd1*qd3*cos(q4)*sin(q2)*sin(q3)*sin(q5) - 0,00015*qd1*qd5*cos(2*q2)*sin(2*q3)*sin(2*q5)*sin(q4)
- 0,00015*qd1*qd5*cos(2*q3)*sin(2*q2)*sin(2*q5)*sin(q4) -
0,0001*qd1*qd5*cos(2*q5)*cos(q2)*cos(q3)*cos(q4)
Function5 :
function y=fcn(u)
r1=25;
r2=20;
r3=15;
r4=10;
r5=10;
r6=10;
qd1=u(2)/r1; qdd1=u(3)/r1;
q2=u(4)/r2; qd2=u(5)/r2; qdd2=u(6)/r2;
q3=u(7)/r3; qd3=u(8)/r3; qdd3=u(9)/r3;
q4=u(10)/r4; qd4=u(11)/r4;
q5=u(13)/r5; qdd5=u(15)/r5;
y = 1/r5*0,0006*qdd5 + 0,000015*qd1^2*sin(q5) + 0,000015*qd2^2*sin(q5) + 0,000015*qd3^2*sin(q5) +
0,0002*qd2^2*sin(2*q5) + 0,0002*qd3^2*sin(2*q5) - 0,0002*qd4^2*sin(2*q5) + 0,0006*qdd2*cos(q4) +
0,0006*qdd3*cos(q4) + 0,00017*qdd2*sin(q5) + 0,00017*qdd3*sin(q5) + 0,00003*qd2*qd3*sin(q5) -
0,001*qd2*qd4*sin(q4) - 0,001*qd3*qd4*sin(q4) - 0,000015*qd1^2*cos(q2)^2*sin(q5) -
0,000015*qd1^2*cos(q3)^2*sin(q5) + 0,000015*qdd2*cos(q4)*cos(q5) + 0,000015*qdd3*cos(q4)*cos(q5) +
0,0004*qd2*qd3*sin(2*q5) - 0,00012*qdd1*cos(q5)*sin(q4) - 0,00017*qd2^2*cos(q4)*cos(q5) -
0,00017*qd3^2*cos(q4)*cos(q5) - 0,068*cos(q2)*sin(q3)*sin(q5) - 0,068*cos(q3)*sin(q2)*sin(q5) -
0,00017*qd1^2*cos(q2)^2*cos(q4)*cos(q5) - 0,00017*qd1^2*cos(q3)^2*cos(q4)*cos(q5) -
0,0004*qd1^2*cos(q2)^2*cos(q5)*sin(q5) - 0,0004*qd1^2*cos(q3)^2*cos(q5)*sin(q5) +
0,0004*qd1^2*cos(q4)^2*cos(q5)*sin(q5) - 0,0004*qd2^2*cos(q4)^2*cos(q5)*sin(q5) -
0,0004*qd3^2*cos(q4)^2*cos(q5)*sin(q5) + 0,0008*qd2*qd4*cos(q5)^2*sin(q4) +
0,0008*qd3*qd4*cos(q5)^2*sin(q4) - 0,0006*qdd1*cos(q2)*cos(q3)*sin(q4) +
0,00003*qd1^2*cos(q2)^2*cos(q3)^2*sin(q5) + 0,0006*qdd1*sin(q2)*sin(q3)*sin(q4) +
0,068*cos(q2)*cos(q3)*cos(q4)*cos(q5) + 0,0004*qd1^2*cos(q2)*cos(q4)*sin(q2) +
0,00012*qd1^2*cos(q2)*cos(q3)*sin(q5) + 0,0004*qd1^2*cos(q3)*cos(q4)*sin(q3) -
0,00002*cos(q2)*cos(q5)*cos(q6)*sin(q3) - 0,00002*cos(q3)*cos(q5)*cos(q6)*sin(q2) +
0,00017*qd1^2*cos(q2)*sin(q2)*sin(q5) + 0,00017*qd1^2*cos(q3)*sin(q3)*sin(q5) -
0,068*cos(q4)*cos(q5)*sin(q2)*sin(q3) - 0,00012*qd1^2*sin(q2)*sin(q3)*sin(q5) -
0,00034*qd2*qd3*cos(q4)*cos(q5) - 0,00034*qd1^2*cos(q2)*cos(q3)^2*sin(q2)*sin(q5) -
0,00034*qd1^2*cos(q2)^2*cos(q3)*sin(q3)*sin(q5) - 0,0008*qd2*qd3*cos(q4)^2*cos(q5)*sin(q5) -
0,000015*qdd1*cos(q2)*cos(q3)*cos(q5)*sin(q4) + 0,00034*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)*cos(q5) +
0,0008*qd1^2*cos(q2)^2*cos(q3)^2*cos(q5)*sin(q5) - 0,0004*qd1^2*cos(q2)^2*cos(q4)^2*cos(q5)*sin(q5) -
0,0004*qd1^2*cos(q3)^2*cos(q4)^2*cos(q5)*sin(q5) + 0,000015*qdd1*cos(q5)*sin(q2)*sin(q3)*sin(q4) -
0,000015*qd1^2*cos(q2)*cos(q4)*cos(q5)*sin(q2) + 0,00012*qd1^2*cos(q2)*cos(q4)*cos(q5)*sin(q3) +
0,00012*qd1^2*cos(q3)*cos(q4)*cos(q5)*sin(q2) - 0,000015*qd1^2*cos(q3)*cos(q4)*cos(q5)*sin(q3) -
0,00002*cos(q2)*cos(q3)*cos(q4)*cos(q6)*sin(q5) + 0,00002*cos(q4)*cos(q6)*sin(q2)*sin(q3)*sin(q5) -
0,001*qd1*qd4*cos(q2)*cos(q3)*cos(q4) + 0,0002*qd1*qd2*cos(q2)*sin(q3)*sin(q4) +
0,0002*qd1*qd3*cos(q3)*sin(q2)*sin(q4) + 0,001*qd1*qd4*cos(q4)*sin(q2)*sin(q3) -
0,0008*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*sin(q2) - 0,0008*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*sin(q3) -
0,0008*qd1^2*cos(q2)*cos(q4)*cos(q5)^2*sin(q2) - 0,0008*qd1^2*cos(q3)*cos(q4)*cos(q5)^2*sin(q3) +
0,0008*qd1*qd2*cos(q2)*cos(q5)^2*sin(q3)*sin(q4) + 0,0008*qd1*qd2*cos(q3)*cos(q5)^2*sin(q2)*sin(q4) +
0,0008*qd1*qd3*cos(q2)*cos(q5)^2*sin(q3)*sin(q4) + 0,0008*qd1*qd3*cos(q3)*cos(q5)^2*sin(q2)*sin(q4) -
0,0008*qd1*qd4*cos(q4)*cos(q5)^2*sin(q2)*sin(q3) +
0,0016*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)^2*sin(q2) +
0,0016*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)^2*sin(q3) +
0,0008*qd1^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*cos(q5)*sin(q5) -
0,00003*qd1^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)*sin(q5) +
0,00034*qd1*qd2*cos(q2)*cos(q3)*cos(q5)*sin(q4) + 0,00034*qd1*qd3*cos(q2)*cos(q3)*cos(q5)*sin(q4) +
0,00003*qd1*qd2*cos(q2)*cos(q5)*sin(q3)*sin(q4) + 0,00003*qd1*qd2*cos(q3)*cos(q5)*sin(q2)*sin(q4) +
0,00003*qd1*qd3*cos(q2)*cos(q5)*sin(q3)*sin(q4) + 0,00003*qd1*qd3*cos(q3)*cos(q5)*sin(q2)*sin(q4) -
0,00034*qd1*qd2*cos(q5)*sin(q2)*sin(q3)*sin(q4) - 0,00034*qd1*qd3*cos(q5)*sin(q2)*sin(q3)*sin(q4) +
0,00003*qd1^2*cos(q2)*cos(q3)^2*cos(q4)*cos(q5)*sin(q2) +
0,00003*qd1^2*cos(q2)^2*cos(q3)*cos(q4)*cos(q5)*sin(q3) +
0,0008*qd1*qd4*cos(q2)*cos(q3)*cos(q4)*cos(q5)^2 -
0,00034*qd1^2*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q2)*sin(q3) -
0,0008*qd1^2*cos(q2)*cos(q3)*cos(q5)*sin(q2)*sin(q3)*sin(q5) +
0,0008*qd1*qd2*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q4)*sin(q5) +
0,0008*qd1*qd3*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q4)*sin(q5) -
0,0008*qd1*qd2*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4)*sin(q5) -
0,0008*qd1*qd3*cos(q4)*cos(q5)*sin(q2)*sin(q3)*sin(q4)*sin(q5) -
0,0008*qd1^2*cos(q2)*cos(q3)*cos(q4)^2*cos(q5)*sin(q2)*sin(q3)*sin(q5)
Function6 :
function y=fcn(u)
r1=25;
r2=20;
r3=15;
r4=10;
r5=10;
r6=10;
q2=u(4)/r2;
q3=u(7)/r3;
q4=u(10)/r4;
q5=u(13)/r5;
q6=u(16)/r6;
y = 1/r6*0,000001*cos(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)) +
0,000025*sin(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)) - 0,000025*cos(q2 +
q3)*cos(q6)*sin(q4) + 0,000001*cos(q2 + q3)*sin(q4)*sin(q6)
2. Hệ số chương trình
Hệ số chương trình
%cac he so dong co khau 1:
Jm1=0.00153;
Jeff1= 2,19. 10−3 ;
Beff1= 1,3242;
K1=1,0198;
Jm2=0.00153;
Jeff2= 2, 372. 10−3 ;
Beff2= 1,3242;
K2=1,0198;
Jm3=0.00153;
Jeff3= 2,41. 10−3 ;
Beff3=1,3242;
K3=1,0198;
Jm4=0.00153;
Jeff4= 1,551 .10−3 ;
Beff4=1,3242;
K4=1,0198;
Jm5=0.00153;
Jeff5= 1,536 . 10−3 ;
Beff5=1,3242;
K5=1,0198;
Jm6=0.00153;
Jeff6= 1,53 .10−3 ;
Beff6=1,3242 ;
K6=1,0198;
3. Thiết kế bộ điều khiển PD
4. Thiết kế bộ điều khiển PID
5. Thiết kế bộ điều khiển PD bão hòa
6. Thiết kế bộ điều khiển tiếp thuận
7.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:
Đối với khâu 1:
J m1 . θ̈m1 + Bm1 . θ̇m1 = K 1 u 1 - d 1 .
J 11 J 12 J13 J14 J 15 J 16
( J m1 + ) θ̈m1 + θ̈m2 + θ̈m3 + θ̈m4 + θ̈m5 + θ̈ = B θ̇ - K u +d
r
2
1
r1 r2 r1 r3 r1 r 4 r1 r5 r 1 r 6 m6 m1 m1 1 1 1

J m2 . θ̈m2 + Bm2 . θ̇m2 = K 2 u 2 - d 2 .
J 22 J 21 J 23 J24 J 25 J 26
( J m2 + ) θ̈m2 + θ̈ m1 θ̈ m3 + θ̈m4 + θ̈m5 + θ̈ = B θ̇ - K u +d
r
2
2
r2 r1 r2 r3 r2 r4 r2 r5 r 2 r 6 m6 m2 m2 2 2 2
J m3 . θ̈m3 + Bm3 . θ̇m3 = K3 u3 - d3 .

J 33 J 31 J32 J 34 J 35 J 36
(J m3 + ) θ̈m3 + θ̈m1 + θ̈ m2 + θ̈m4 + θ̈m5 + θ̈ = B θ̇ - K u + d
r
2
3
r3 r1 r3 r2 r3 r4 r3 r5 r 3 r 6 m6 m3 m3 3 3 3

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 +
r 4 r2
θ̈ m2 +
r4 r3
θ̈m3 +
r4 r5
θ̈m5 + θ̈ = B θ̇ - K u + d
r 4 r 6 m6 m4 m4 4 4 4
r 4

J m5 . θ̈m5 + Bm5 . θ̇m5 = K5 u5 - d5 .
J55 J 51 J 52 J53 J 54 J 56
(J m5 + ) θ̈m5 + θ̈m1 + θ̈ m2 + θ̈m3 + θ̈m4 + θ̈ = B θ̇ - K u + d
r
2
5
r5 r1 r5 r2 r5 r3 r5 r 4 r 5 r 6 m6 m5 m5 5 5 5

J m6 . θ̈m6 + . θ̇ m6 = K 6 u 6 - d6 .
J66 J 61 J 62 J 63 J 64 J6 5
(J m6 + 2
) θ̈m6 +
r 6 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 xác định được ma trận:
J 11 J 12 J 13
J 14 J 15 J 16
Jm + 2
1
r 1
r1 r2 r1 r3
r1 r 4 r1 r 5 r1 r6
[]
J 21 J J 23
J 24 J 25 J 26
J m + 22
r2 r1 2
r 22 r 2 r 4 r 2 r 5 r 2 r 6 θ̈m
r2 r3 1
J 31 J 32 J 33 J 34 J 35 J 36 θ̈m
Jm + 2 2
r3 r1 r3 r2 r 3 r 3 r 4 r 3 r 5 r 3 r 6 θ̈m
3
J 44 J 45 J 46
. = 3
J 41 J 42 J 43 θ̈m
Jm + 2 4
r4 r1 r 4 r2 r 4 r3 r44
r4 r5 r4 r6 θ̈m 5
J 51 J 52 J 53 J 54 J 55 J 56 θ̈m
Jm + 2 6
r5 r1 r5 r 2 r5 r3 r5 r4 r5 r5 r65
J 61 J 62 J 63 J 64 J 65 J 66
Jm + 2
r6 r1 r6 r 2 r6 r3 r6 r4 r6 r5 r6 6
[ ][ ] [ ][ ] [ ]
Bm 1
0 0 0 0 θ̇ m
0 K1 0 0 0 0 1 0 u1 d1
0 0 0 0 0 θ̇m
Bm 2
0 K2 0 0 0 2
0 u2 d2
0 Bm 0 00 0 θ̇ 0 0 K3 0 0 0 u3
. m + 3 3 . − d3
0 0 0 Bm 0 0 θ̇m 0 0 0 K4 0
4 4
0 u4 d4
0 0 0 0 Bm 0 θ̇m 0 0 0 0 K5 5
0 u5 d5
0 0 0 0
5
0 0 0 0 0 Bm θ̇m 0 K 6 u6 d6
6 6
Trong đó ta xác định được các thông số sau:

J m =0.00153; J m =0.00153; J m =0.00153; J m =0.00153; J m =0.00153; J m =0.00153;
1 2 3 4 5 6
J 11 =0.4153; J 12 =0; J13 =0; J14 =0; J15 =0; J16 =0;
J 21 =0; J 22 =0.3368; J 23 =0.1994; J 24 =0; J 25 =0; J 26 =0;
J 31 =0; J 32 =0.1994; J 33 =0.1994; J 34 =0; J 35 =0; J 36 =0;
J 41 =0; J 42 =0; J 43 =0; J 44 =0.0021; J 45 =0; J 46 =0;
J 51 =0; J 52 =0; J 53 =0; J 54 =0; J 55 =0.0006; J 56 =0;
J 61 =0; J 62 =0; J 63 =0; J 64 =0; J 65 =0; J 66 =0;
Bm =0,6621; Bm =0,6621; Bm =0,6621; Bm =0,6621; Bm =0,6621; Bm =0,6621;
1 2 3 4 5 6
K 1 =1,0198; K2 =1,0198; K3 =1,0198; K4 =1,0198; K5 =1,0198; K6 =1,0198;

r 1 =25; r 2 =20; r 3 =15; r 4 =10; r 5 =10; r 6 =10;
Thay số vào phương trình trên ta được:
[ ][ ]
θ̈m
0,00219 0 0 0 0 0 1
0 0,002372 0,000664 0 0 0 θ̈m 2
0 0,000664 0,00241 0 0 0 . θ̈m

0 0 0 0,001551 0 0 = 3
θ̈m
0 0 0 0 0,001536 0
4
θ̈m
0 0 0 0 0 0,00153 5
θ̈m 6
[ ][ ] [ ][ ] [ ]
θ̇ m
0,6621 0 0 0 0 0 1,0198 0 0 0 0 0 u1
1 d1
0 0,6621 0 0 0 0 θ̇ m 0 1,0198 0 0 0 0 u2
2 d2
0 0 0,6621 0 0 0 . θ̇ m 0 0 1,0198 0 0 0 . u3 − d 3
0 0 0 0,6621 0 0 + 0 0 0 1,0198 0
3
0
θ̇ m u4 d4
0 0 0 0 0,6621 0
4
θ̇ m 0 0 0 0 1,0198 0 u5 d5
0 0 0 0 0 0,6621 0 0 0 0 0 5
1,0198 u d6
θ̇ m 6
6
Ta có:
[]
θ̈m 1
θ̈m 2
θ̈m −1
3 =J ( B θ̇+ Ku−D ) (1)
θ̈m 4
θ̈m 5
θ̈m 6
Đặt : x 1=θm x 2=θm x 3=θm x 4 =θm

1 2 3 4
x 5=θm x 6=θ m
5 6
x 7=θ̇ m x 8=θ̇ m
1 2
x 9=θ̇ m x 10=θ̇m x 11 =θ̇m x12=θ̇ m

3 4 5 6
[ ][ ][ ]
Từ phương trình (1) ta thành lập được ma trận:
ẋ 1 x1
0 0 00 0 01 0 00 0 0
ẋ 2 x2
0 0 00 0 00 1 00 0 0
ẋ 3 x3
0 0 00 0 00 0 10 0 0
ẋ 4 0 0 00 0 00 0 01 0 0 x4
ẋ 5 0 0 00 0 00 0 00 1 0 x5
ẋ 6 0 0 00 0 00 0 00 0 1 . x6
= 0 0 0 0 0 0 302.3288 0 0 0 0 0 x7
ẋ 7
ẋ 8 0 0 0 0 0 0 0 302.4592 −83.3332 0 0 0 x
8
x˙9 0 0 0 0 0 0 0 −83.3332 297.6902 0 0 0 x
9
0 0 0 0 0 0 0 0 0 426.8859 0 0
x˙10 x 10
0 0 00 0 00 0 0 0 431.0547 0
x˙11 x 11
0 0 00 0 00 0 0 0 0 432.7451
x˙12 x 12
[ ][ ]
0 0 00 0 0 0
0 0 00 0 0 0
[]
0 0 00 0 0 0
0 0 00 0 0 u1 0
0 0 00 0 0 u2 0
0
+ 0 0 00 0 0 . u3 − 456.6210 . d 1
465.6621 0 0 0 0 0 u4
0 465.8630 −128.3540 0 0 0 u5 456.8180. d 2−125.8619 . d 3
0 −128.3540 458.5175 0 0 0 u −125.8619 . d 2 +449.6151 d 3
6
0 0 0 657.5113 0 0 644.7453. d 4
0 0 0 0 663.9323 0 651.0417 . d 5
0 0 0 0 0 666.5360 653.5948 . d 6
Xây dựng code trên Matlab ta thu được kết quả như sau:

BTL Robot5

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

BTL Robot5

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

THIẾT KẾ MÔ PHỎNG ĐIỀU KHIỂN ROBOT

Họ Và Tên Mã sinh viên Lớp Khoa Chuyên Ngành

Đặng Xuân Long 191313722 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử

Nguyễn Công Thuận 191302741 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử

Tạ Văn Thắng 191300967 Cơ điện tử 2 K60 Cơ khí Kỹ thuật cơ điện tử

Nhóm sinh viên thực hiện : Nhóm 7

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

CHƯƠNG 2 : TÍNH TOÁN ĐỘNG HỌC

Hình 2.1: Gán hệ trục tọa độ cho cánh tay robot.

Từ bảng DH ta có ma trận thuần nhất của các khâu:

 Vecto tọa độ trọng tâm (bỏ qua khâu đế)

Trọng tâm khâu 2: x2 = 0,1113; y2 = -0,0001; z2= 0,0068 (m).

Trọng tâm khâu 4: x4= 0; y4= 0,0008; z4= 0,0865 (m).

Trọng tâm khâu 5: x5= 0; y5= -0,0167; z5= 0 (m).

Trọng tâm khâu 6: x6= 0,0002; y6= 0; z6= -0,0067 (m).

 Vecto gia tốc trọng trường

 Momen quán tính tại trọng tâm (kg.m2)

Khâu 1: Khâu 2: Khâu 3:

Khâu 4: Khâu 5: Khâu 6:

● Ma trận giả quán tính được tính theo công thức:

2.2.Lập phương trình động lực học.

∂ T1 1 ⃗T ∂ T 2 2r −m ⃗T ∂T3 3 T ∂T4 4 T ∂T5 5

Kết quả code Matlab:

CHƯƠNG 3: CHỌN ĐỘNG CƠ

2.1: Hình ảnh động cơ

Điện áp phần ứng: V1= 220(V)

Hệ số sức phản điện động:K b = 0.6493(V.s/rad)

Gia tốc trọng trường: g = 9.8 (m/s 2)

Động cơ khâu 2: 130ST-M07725

Điện áp phần ứng: V2= 220(V)

Gia tốc trọng trường: g = 9.8 (m/s 2)

Động cơ khâu 3: 130ST-M07725

Điện áp phần ứng: V3= 220(V)

Hệ số sức phản điện động:K b = 0.6493(V.s/rad)

Gia tốc trọng trường: g = 9.8 (m/s 2)

Động cơ khâu 4: 130ST-M07725

Điện áp phần ứng: V4 = 220(V)

Hệ số sức phản điện động:K b = 0.6493(V.s/rad)

Gia tốc trọng trường: g = 9.8 (m/s 2)

Động cơ khâu 5: 130ST-M07725

Điện áp phần ứng: V5= 220(V)

Hệ số sức phản điện động:K b = 0.6493(V.s/rad)5

Gia tốc trọng trường: g = 9.8 (m/s 2)

Động cơ khâu 6: 130ST-M07725

Điện áp phần ứng: V6= 220(V)

Hệ số sức phản điện động:K b = 0.6493(V.s/rad)6

Gia tốc trọng trường: g = 9.8 (m/s 2)

CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN

J eff θ̈m + Beff θ̇m = K2 u 2 - d 2

J eff θ̈ m + Beff θ̇m = K 3 u 3 - d3

Điều khiện ổn định : e ss ≤ 0,01

Bộ điều khiển PD:

Tín hiệu đầu vào U(s) được xác định bởi:

→ [ 2,19.10 -3 s2 + ( K1 K d +1,3242 ).s + K1 K p ] . θm(s) = K1 Kp θdm (s) - D(s)

e ss = lim s. E(s ) = lim

K 1 = 1,0198 → K p ≥ 0,098 (3)

Độ quá điều chỉnh:

Thời gian tăng:

Bộ điều khiển PID: