+ Bước 2: Xác định các Links: Các Links liên kết được đặt tên th số liên kết mà chúng được gắn và... Có 2 phư ng pháp để giải bài t án động học... nghịch là phư ng pháp hình học và phư n
Trang 1BỘ GIÁ DỤC & ĐÀ TẠ TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ TH ẬT TP HỒ CHÍ MINH
Phạm Gi Kh ơng-21151120
Tp Hồ Chí Minh tháng 12 năm 2024
Trang 2MỤC LỤCMỤC LỤC HÌNH ẢNH 5 MỤC LỤC BẢNG 6 CHƯ NG 1: ĐỘNG HỌC TH ẬN 7
Trang 3CHƯ NG 10: MÔ PHỎNG KIỂM CHỨNG 65
10.2.2 Đánh giá kết q ả củ động học th ận trên M tl b 6710.3 Mô phỏng và kiểm chứng kết q ả động học nghịch 69
10.3.2 Đánh giá kết q ả củ động học nghịch trên M tl b 7110.4 Kiểm chứng kết hợp Động học th ận, Động học nghịch và mô phỏng Simsc p 7510.4.1 Thiết kế fil mô phỏng kiểm chứng động học th ận, nghịch và Simsc p 7510.4.2 Mô phỏng kiểm chứng tư thế r b t với các bộ nghiệm 7510.5 Kiểm chứng q y h ạch q ỹ đạ bằng phần mềm Simsc p 78
CHƯ NG 11: KẾT L ẬN T SK 2 95
Trang 4CHƯ NG 13: THIẾT KẾ MÔ HÌNH R B T TRÊN S LIDW RK 99
Trang 517.2.2 Thiết kế các chư ng trình điề khiển trên MATLAB Sim link 132
Trang 6MỤC LỤC HÌNH ẢNH
Hình 1 Hệ cánh t y R b t 11
Hình 2 Đặt trục hệ cánh t y R b t 13
Hình 3 Q y h ạch ch yển động 22
Hình 4 Mô phỏng kiểm tr động học th ận trên M tl b 29
Hình 5 Mô hình R b t tr ng trường hợp 1 30
Hình 6 Kết q ả tọ độ th được tr ng trường hợp 1 30
Hình 7 Mô hình R b t tr ng trường hợp 2 31
Hình 8 Kết q ả th được tọ tr ng trường hợp 2 31
Hình 9 Mô phỏng kết q ả tính t án động học nghịch 32
Hình 10 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 1 34
Hình 11 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 1 34
Hình 12 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 2 34
Hình 13 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 2 35
Hình 14 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 3 35
Hình 15 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 3 35
Hình 16 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 4 36
Hình 17 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 4 36
Hình 18 Mô phỏng q y h ạch q ỹ đạ th đường thẳng 37
Hình 19 Kết q ả mô phỏng vị trí th trục x s 5 ch kỳ 38
Hình 20 Kết q ả mô phỏng vận tốc th trục x s 5 ch kỳ 39
Hình 21 Kết q ả mô phỏng vị trí th trục y s 5 ch kỳ 39
Hình 22 Kết q ả mô phỏng vận tốc th trục y s 5 ch kỳ 40
Hình 23 Mô phỏng q y h ạch q ỹ đạ th hình t m giác 41
Hình 24 Kết q ả mô phỏng t ạ độ th trục x 43
Hình 25 Kết q ả mô phỏng vận tốc th trục x 44
Hình 26 Kết q ả mô phỏng t ạ độ th trục y 44
Hình 27 Kết q ả mô phỏng vận tốc th trục y 45
Hình 28 Mô tả R b t trên S lidw rk 47
Hình 29 Hệ tọ độ trên S lidw rk 49
Hình 40 Thiết lập đ n vị đ trên phần mềm S lidW rks 57
Hình 31 Bản vẽ 2D khâ đế 58
Hình 32 Bản vẽ 2D link 1 58
Hình 33 Bản vẽ 2D link 2 59
Hình 34 Bản vẽ 2D củ End Eff ct r 59
Hình 35 Click chọn Simsc p tại cử sổ Add-Ins 60
Hình 36 Fil sim link mô phỏng hệ cánh t y r b t 3 bậc 61
Hình 37 Chỉnh sử giá trị gi tốc trọng trường 61
Hình 38 Chỉnh sử các khối R v l t 62
Trang 7Hình 40 Q y h ạch ch yển động 63
Hình 41 Tư thế r b t tr ng trường hợp 1 69
Hình 42 Tư thế r b t tr ng trường hợp 2 69
Hình 43 Tư thế r b t tr ng trường hợp 3 70
Hình 44 Mô phỏng kiểm tr động học th ận trên M tl b 70
Hình 45 Mô hình r b t tr ng trường hợp 1 71
Hình 46 Kết q ả tọ độ th được tr ng trường hợp 1 72
Hình 47 Mô hình r b t tr ng trường hợp 2 72
Hình 48 Kết q ả tọ độ th được tr ng trường hợp 2 73
Hình 49 Mô phỏng kiểm tr động học nghịch trên M tl b 73
Hình 50 Kết q ả mô phỏng củ bộ nghiệm số 1 với trường hợp 1 76
Hình 51 Kết q ả mô phỏng củ bộ nghiệm số 1 với trường hợp 2 76
Hình 52 Kết q ả mô phỏng củ bộ nghiệm số 2 với trường hợp 1 76
Hình 53 Kết q ả mô phỏng củ bộ nghiệm số 2 với trường hợp 2 77
Hình 54 Kết q ả mô phỏng củ bộ nghiệm số 3 với trường hợp 1 77
Hình 55 Kết q ả mô phỏng củ bộ nghiệm số 3 với trường hợp 2 77
Hình 56 Kết q ả mô phỏng củ bộ nghiệm số 4 với trường hợp 1 78
Hình 57 Kết q ả mô phỏng củ bộ nghiệm số 4 với trường hợp 2 78
Hình 58 Fil mô phỏng kiểm chứng trên M tl b Sim link 79
Hình 59 Kết q ả mô phỏng các bộ nghiệm tr ng trường hợp 1 80
Hình 60 Kết q ả mô phỏng các bộ nghiệm tr ng trường hợp 2 80
Hình 61 Kết q ả mô phỏng các bộ nghiệm tr ng trường hợp 3 81
Hình 62 Không gi n làm việc củ cánh t y r b t 83
Hình 63 Q ỹ đạ đường thẳng đi q 2 điểm tr ng không gi n làm việc củ r b t 84
Hình 64 Mô phỏng q y h ạch q ỹ đạ đường thẳng trên MATLAB Sim link 85
Hình 65 R b t thực hiện q y h ạch q ỹ đạ đường thẳng 87
Hình 66 Kết q ả mô phỏng vị trí th trục x s 5 ch kỳ 88
Hình 67 Kết q ả mô phỏng vận tốc th trục x s 5 ch kỳ 88
Hình 68 Kết q ả mô phỏng vị trí th trục y s 5 ch kỳ 89
Hình 69 Kết q ả mô phỏng vận tốc th trục y s 5 ch kỳ 89
Hình 70 Kết q ả mô phỏng vị trí th trục z s 5 ch kỳ 90
Hình 71 Kết q ả mô phỏng vận tốc th trục z s 5 ch kỳ 90
Hình 72 Q ỹ đạ t m giác đi q 3 điểm tr ng không gi n làm việc củ r b t 91
Hình 73 Mô phỏng q y h ạch q ỹ đạ t m giác 91
Hình 74 R b t thực hiện q y h ạch q ỹ đạ t m giác 95
Hình 75 Vị trí th trục x s 5 ch kỳ q y h ạch q ỹ đạ t m giác 95
Hình 76 vận tốc th trục x s 5 ch kỳ q y h ạch q ỹ đạ t m giác 96
Hình 77 Vị trí th trục y s 5 ch kỳ q y h ạch q ỹ đạ t m giác 96
Hình 78 Vận tốc th trục y s 5 ch kỳ q y h ạch q ỹ đạ t m giác 97
Hình 79 Vị trí th trục z s 5 ch kỳ q y h ạch q ỹ đạ t m giác 97
Trang 8Hình 80 Vận tốc th trục z s 5 ch kỳ q y h ạch q ỹ đạ t m giác 98
Hình 85 Mô hình r b t 3 bậc tự d thiết kế trên S lidW rks 103
Hình 86 Thiết lập đ n vị đ trên phần mềm S lidW rks 104
Hình 87 Bản vẽ 2D khâ đế 105
Hình 88 Bản vẽ 2D link 1 106
Hình 89 Bản vẽ 2D link 2 107
Hình 810 Bản vẽ 2D nd ff ct r 108
Hình 811 Click chọn Simsc p tại cử sổ Add-Ins 109
Hình 88 Fil sim link mô phỏng hệ cánh t y r b t 3 bậc 110
Hình 89 Chỉnh sử giá trị gi tốc trọng trường 110
Hình 90 Chỉnh sử các khối R v l t 111
Hình 91 Fil mô phỏng Simsc p để kiểm chứng các góc q y 111
Hình 92 Đặt các hệ trục t ạ độ trên phần mềm S lidW rks 113
Hình 93 Q y h ạch ch yển động 122
Hình 94 Lư đồ giải th ật q y h ạch q ỹ đạ th đường thẳng 125
Hình 95 Lư đồ giải th ật q y h ạch q ỹ đạ t m giác 127
Hình 96 Ví dụ về T rq 130
Hình 97 S đồ khối phư ng pháp cánh t y đòn 131
Hình 98 S đồ kết khối củ hệ thống 134
Hình 99 H i gói hỗ trợ Ard in củ MATLAB 135
Hình 100 Cài đặt thành công h i gói hỗ trợ Ard in 136
Hình 101 Khối C ntin s S rv Writ điề khiển động c RC S rv 137
Hình 102 Khối St nd rd S rv Writ điề khiển động c RC S rv 137
Hình 103 Cài đặt các thông số tr ng cử sổ M d l S ttings 138
Hình 104 Cấ hình kết nối Ard in với MATLAB Sim link 138
Hình 105 Chư ng trình điề khiển góc q y các khớp 139
Hình 106 Không gi n làm việc củ r b t 139
Hình 107 Chư ng trình kiểm tr các bộ nghiệm động học nghịch 140
Hình 108 Chư ng trình q y h ạch q ỹ đạ đường thẳng 140
Hình 109 Chư ng trình q y h ạch q ỹ đạ t m giác 141
Hình 110 Tổng q n mô hình cánh t y r b t b bậc tự d 142
Hình 111 Chư ng trình kiểm chứng góc q y trường hợp 1 142
Hình 112 Tư thế r b t tr ng trường hợp 1 143
Hình 113 Đáp ứng góc q y q1 trường hợp 1 143
Hình 114 Đáp ứng góc q y q2 trường hợp 1 144
Hình 115 Đáp ứng góc q y q3 trường hợp 1 144
Hình 116 Tư thế r b t tr ng trường hợp 2 145
Hình 117 Đáp ứng góc q y q1 trường hợp 2 145
Hình 118 Đáp ứng góc q y q2 trường hợp 2 146
Trang 9Hình 120 Tư thế r b t tr ng trường hợp 3 147
Hình 121 Đáp ứng góc q y q1 trường hợp 3 147
Hình 122 Đáp ứng góc q y q2 trường hợp 3 148
Hình 123 Đáp ứng góc q y q3 trường hợp 3 148
Hình 124 Vị trí điểm P nằm tr ng không gi n làm việc củ r b t 149
Hình 125 Chư ng trình kiểm chứng động học nghịch với t ạ độ điểm P 149
Hình 126 Tư thế r b t với bộ nghiệm thứ nhất 150
Hình 127 Đáp ứng góc q y q1 với bộ nghiệm 1 150
Hình 128 Đáp ứng góc q y q2 với bộ nghiệm 1 151
Hình 129 Đáp ứng góc q y q3 với bộ nghiệm 1 151
Hình 130 Tư thế r b t với bộ nghiệm thứ h i 152
Hình 131 Đáp ứng góc q y q1 với bộ nghiệm 2 153
Hình 132 Đáp ứng góc q y q2 với bộ nghiệm 2 153
Hình 133 Đáp ứng góc q y q3 với bộ nghiệm 2 154
Hình 134 Kết q ả mô phỏng kiểm chứng bộ nghiệm thứ b 154
Hình 135 Kết q ả mô phỏng kiểm chứng bộ nghiệm thứ tư 155
Hình 136 Vị trí h i điểm A và B tr ng không gi n làm việc củ r b t 155
Hình 137 Chư ng trình q y h ạch q ỹ đạ ch h i điểm A và B 156
Hình 138 Đáp ứng ngõ r x khi q y h ạch q ỹ đạ th đường thẳng 156
Hình 139 Đáp ứng ngõ r y khi q y h ạch q ỹ đạ th đường thẳng 157
Hình 140 Đáp ứng ngõ r z khi q y h ạch q ỹ đạ th đường thẳng 157
Hình 141 Đáp ứng góc q y q1 khi q y h ạch q ỹ đạ th đường thẳng 158
Hình 142 Đáp ứng góc q y q2 khi q y h ạch q ỹ đạ th đường thẳng 158
Hình 143 Đáp ứng góc q y q3 khi q y h ạch q ỹ đạ th đường thẳng 159
Hình 144 Vị trí b điểm A, B, C tr ng không gi n làm việc củ r b t 159
Hình 145 Chư ng trình q y h ạch q ỹ đạ ch b điểm A, B và C 160
Hình 146 Đáp ứng ngõ r x khi q y h ạch q ỹ đạ th t m giác 160
Hình 147 Đáp ứng ngõ r y khi q y h ạch q ỹ đạ th t m giác 161
Hình 148 Đáp ứng ngõ r z khi q y h ạch q ỹ đạ th t m giác 161
Hình 149 Đáp ứng góc q y q1 khi q y h ạch q ỹ đạ th t m giác 162
Hình 150 Đáp ứng góc q y q2 khi q y h ạch q ỹ đạ th t m giác 162
Hình 151 Đáp ứng góc q y q3 khi q y h ạch q ỹ đạ th t m giác 163
Trang 10MỤC LỤC BẢNG
Bảng 1 Thông số và ký hiệ củ R b t 13
Bảng 2 Bảng DH củ hệ cánh t y R b t 14
Bảng 3 Thông số và ký hiệ củ R b t 50
Bảng 4 Bảng DH củ hệ cánh t y R b t 50
Bảng 5 Thông số và ký hiệ củ r b t 114
Bảng 6 Bảng DH củ hệ cánh t y 114
Bảng 7 Khối lượng các khâ tư ng ứng 131
Bảng 8 D nh sách thiết bị 133
Trang 11T SK 1: TÍNH T ÁN, KIỂM CHỨNG ĐỘNG HỌC, Q H ẠCH Q Ỹ ĐẠ
R B T 3 D F VỀ MẶT LÝ TH ẾT VÀ TRÊN M TL B SIM LINK
CHƯ NG 1: ĐỘNG HỌC TH ẬN 1.1 Mô tả về R b t
Hình 1 Hệ cánh t y Robot
Tr ng kì thực tập Tr ng kỳ thực tập, chúng t điề khiển một cánh t y r b t với 3 bậc tự d , sử dụng các khớp x y để điề khiển h ạt động củ cánh t y Hệ thống này b gồm b khớp x y và b đ ạn liên kết (các khâ ), giúp xác định vị trí và hướng củ cánh t y tr ng không gi n b chiề Cánh t y r b t được cấ tạ thành
b đ ạn liên kết: , , và Tr ng đó, l₁ là độ dài đ ạn kết nối từ để đến khớp
đầ tiên, là độ dài củ đ ạn nối giữ khớp đầ tiên và khớp thứ h i, còn là
đ ạn nối từ khớp thứ h i đến điểm c ối củ cánh t y.
Trang 12Với sự linh h ạt củ các khớp và đ ạn liên kết này, cánh t y r b t có khả năng thực hiện những ch yển động phức tạp, giúp ch điểm đầ c ối đạt tới vị trí và định hướng m ng m ốn tr ng không gi n Để điề khiển cánh t y r b t chính xác, cần phải nắm rõ các ng yên lý động học củ hệ thống, b gồm động học th ận (tính t án vị trí đầ c ối dự trên các góc khớp) và động học nghịch (tính t án các góc khớp để đạt đến vị trí đầ c ối).
1.2 Trình bà đặt trục th m difi d DH
Q y tắc đặt trục r b t th phư ng pháp D n vit – H rt nb rg C nv nti n:
+ Bước 1: Gán trục c sở: Trục {0} sẽ được gán ch liên kết {0} Chọn trục 𝑧0 dọc th trục 𝑧1 khi biến khớp đầ tiên bằng 0 Sử dụng q y ước này, chúng t có
𝑎0 = 0 và 𝛼0 = 0 Đảm bả rằng 𝑑0 = 0 nế là khớp q y và 𝜃𝑖 = 0 nế là khớp lăng trụ.
+ Bước 2: Xác định các Links: Các Links liên kết được đặt tên th số liên kết mà chúng được gắn và Xác định khớp: Trục z củ kh ng {i}, được gọi là 𝑧𝑖, trùng với trục khớp i Liên kết i có h i trục khớp, 𝑧𝑖 và 𝑧𝑖+1 Trục 𝑧𝑖 được gán ch khớp i
+ Bước 6: Trục 𝑦𝑖được đặt dự trên q y tắc bàn t y phải.
+ Bước 7: Gán trục c ối (trục EE): Nế khớp n là khớp q y, hướng củ 𝑥𝑛được chọn dọc th hướng củ 𝑥𝑛−1khi 𝜃𝑛 = 0 và gốc củ kh ng {n} được chọn s ch
dn = 0 Nế khớp n là khớp lăng trụ, hướng củ xn được chọn s ch 𝜃𝑛 = 0 và gốc củ kh ng {n} được chọn tại gi điểm củ 𝑥𝑛−1với 𝑧𝑛s ch 𝑑𝑛 = 0.
+ Bước 8: Xác định các thông số tr ng bảng DH:
𝒂𝒊−𝟏là kh ảng cách đ từ trục 𝑧𝑖−1đến 𝑧𝑖 dọc th trục 𝑥𝑖−1.
Trang 13𝒅𝒊 là kh ảng cách từ 𝑥𝑖−1 đến 𝑥𝑖 dọc th trục 𝑧𝑖.
𝜽𝒊 là góc giữ 𝑥𝑖−1 đến 𝑥𝑖 dọc th trục 𝑧𝑖.
Hình 2 Đặt trục hệ cánh t y Robot
1.3 Lập bảng DH
Bảng 1 Thông số và ký hiệ củ Robot
Trang 14- Công thức m trận chu ển đổi đồng nhất củ r b t:
T có m trận ch yển đổi giữ hệ trục tọ độ {i − 1} s ng hệ trục tọ độ {i} là :
- M trận chu ển đổi đồng nhất củ R b t:
T có m trận ch yển đổi giữ hệ trục tọ độ {0} s ng hệ trục tọ độ {1} là :
T có m trận ch yển đổi giữ hệ trục tọ độ {1} s ng hệ trục tọ độ {2} là :
Trang 15T có m trận ch yển đổi giữ hệ trục tọ độ {2} s ng hệ trục tọ độ {3} là :
T có m trận ch yển đổi giữ hệ trục tọ độ {3} s ng hệ trục tọ độ {E𝐸} là :
T có:
1.5 Tính t án vị trí và h ớng củ điểm đầu cuối
Trang 16 M trận hướng củ điểm đầ c ối:
M trận vị trí củ điểm đầ c ối:
CHƯ NG 2: ĐỘNG HỌC NGHỊCH 2.1 Cách tiếp cận tính t án
Mục đích củ bài t án động học nghịch là tìm các biến khớp củ r b t khi biết
vị trí khâ tác động c ối củ t y máy Có 2 phư ng pháp để giải bài t án động học
Trang 17nghịch là phư ng pháp hình học và phư ng pháp đại số Ở đây nhóm đã áp dụng phư ng pháp đại số để giải Bằng việc sử dụng các phép biến đổi đại số, các công thức lượng giác, phép đồng nhất hệ số t có thể tính t án được các biến khớp
𝑞1, 𝑞2, 𝑞3dưới dạng tổng q át.
2.2 Các lời giải bài t án động học nghịch
T viết lại m trận 𝑇𝐸𝐸𝐵 dưới dạng như s :
Trang 21 Bộ nghiệm thứ b :
Bộ nghiệm thứ tư:
Trang 22CHƯ NG 3: Q H ẠCH Q Ỹ ĐẠ CH R B T 3.1 Giới thiệu về qu h ạch quỹ đạ
- Q y h ạch q ỹ đạ là một phần nhỏ củ một vấn đề tổng thể như là dẫn đường (n vig ti n) h y q y h ạch ch yển động (M ti n pl nning) Hệ thống cấp bậc điển hình ch q y h ạch ch yển động như s :
T sk pl nning – Thiết kế một vài mục tiê (Ví dụ như nhặt đồ vật trược mặt
bạn lên, gắp vật, thả vật, hàn, …)
P th pl nning – Đư r dường dẫn khả thi từ điểm đầ đến điểm c ối Một
đường dẫn l ôn b gồm tập hợp củ các điểm th m chiế
Tr j ct r pl nning – Đư r lịch trình th thời gi n làm s để di ch yển
th đường dẫn mà có tính r ng b ộc ví dụ như vị trí, vận tốc, gi tốc.
Tr j ct r f ll wing – Khi kế h ạch đã được lên thì cần một hệ thống điề
khiển để ch kế h ạch có thể di ch yển đầy đủ và chính xác nhất.
Hình 3 Q y hoạch ch yển động
3.2 Qu h ạch quỹ đạ ở không gi n khớp và không gi n làm việc
- Q y h ạch q ỹ đạ ở không gi n làm việc là các điểm th m chiế và nội s y nằm trên trục tọ độ D sc rt s (vị trí và hướng) củ một vị trí cụ thể trên cánh t y r b t (thường là khâ c ối – nd ff ct r).
Trang 23- Q y h ạch q ỹ đạ ở không gi n khớp là các điểm th m chiế và nội s y trực tiếp trên các vị trí khớp Cả 2 q ỹ đạ ở các không gi n khác nh
- Đối với mỗi không gi n khác nh sẽ có những ư nhược điểm riêng, ư nhược điểm củ mỗi không gi n.
Không gi n làm việc:
- Ư điểm: + Ch yển động có thể dự đ án.
+ Có khả năng tránh chướng ngại vật và v chạm tốt h n.
- Nhược điểm: + Thực hiện chậm h n.
+ Ch yển động củ c cấ tr yền động không tr n tr và khó xác định h n.
Trang 25S khi tìm được các hệ số củ đ thức bậc b t có thể q y h ạch q ỹ đạ ch r b t
th hàm bậc b
3.4 Qu h ạch quỹ đạ th đ ờng thẳng
T cần q y h ạch q ỹ đạ ch r b t di ch yển từ điểm A đến điểm B với thời gi n
là tf rồi từ điểm B về điểm A với thời gi n là tf th hàm đ thức bậc b T àn bộ ch
kỳ được lặp lại 5 lần trước khi dừng lại tại điểm A.
Lư đồ giải th ật: gọi 𝑡 là thời gi n thực tế, TN là tổng thời gi n chạy củ 5 ch
kỳ T có thể tính t án được thời gi n hiện tại tr ng ch kỳ th ộc kh ảng q y h ạch
q ỹ đạ nà bằng phép chi lấy dư củ 𝑡 ch thời gi n 1 ch kỳ là 2tf Từ đây t tiến hành q y h ạch q ỹ đạ với mỗi kh ảng thời gi n tư ng ứng và sẽ dừng lại tại điểm
A khi thời gi n thực tế bằng với tổng thời gi n 5 ch kỳ.
Dự và lư đồ giải th ật và phư ng pháp q y h ạch q ỹ đạ bằng hàm đ thức bậc 3 nhóm tiến hành mô phỏng q y h ạch q ỹ đạ trên M tl b Sim link.
Trang 273.5 Qu h ạch quỹ đạ th hình t m giác
T cần q y h ạch q ỹ đạ ch r b t di ch yển từ điểm A đến điểm B với thời gi n
là tf, từ điểm B đến điểm C với thời gi n là tfvà từ điểm C về điểm A cũng với thời
gi n là tf th hàm đ thức bậc b T àn bộ ch kỳ được lặp lại 5 lần trước khi dừng lại tại điểm A Lư đồ giải th ật được trình bày tư ng tự như q ỹ đạ đường thẳng Gọi 𝑡 là thời gi n thực tế, TN là tổng thời gi n chạy củ 5 ch kỳ T có thể tính
t án được thời gi n hiện tại tr ng ch kỳ th ộc kh ảng q y h ạch q ỹ đạ nà bằng phép chi lấy dư củ t ch thời gi n 1 ch kỳ là 3tf Từ đây t tiến hành q y h ạch
q ỹ đạ với mỗi kh ảng thời gi n tư ng ứng và sẽ dừng lại tại điểm A khi thời gi n thực tế bằng với tổng thời gi n 5 ch kỳ.
Dự và lư đồ giải th ật và phư ng pháp q y h ạch q ỹ đạ bằng hàm đ thức bậc 3 nhóm tiến hành mô phỏng q y h ạch q ỹ đạ trên M tl b Sim link.
Trang 29CHƯ NG 4: MÔ PHỎNG VÀ ĐÁNH GIÁ 4.1 Động học thuận
Hình 4 Mô phỏng kiểm tr động học th ận trên M tl b
Chư ng trình tr ng khối kiểm tr động học th ận DHT tr ng khối M tl b
Trang 30 Xét trường hợp 1: q1 = q2 = q3 = 0
Hình 5 Mô hình Robot trong trường hợp 1
T thấy được tọ độ củ EE lần lượt là x=35; y=0; z=20.
Hình 6 Kết q ả tọ độ th được trong trường hợp 1
Trang 31Hình 7 Mô hình Robot trong trường hợp 2
T thấy được tọ độ củ EE là x=10; y=25; z=20.
Hình 8 Kết q ả th được tọ trong trường hợp 2
Th được x=10; y=25; z=20.
Trang 32 Nhận xét: Kết q ả mô phỏng ch thấy tọ độ củ điểm EE trên hình vẽ trùng
khớp với kết q ả tính t án b n đầ , từ đó có thể kết l ận rằng kết q ả tính t án
là chính xác.
4.2 Động học nghịch
Hình 9 Mô phỏng kết q ả tính toán động học nghịch
Nhóm tiến hành kiểm tr kết q ả tính t án động học nghịch trên M tl b
Sim link Công thức tính động học nghịch được thiết lập tr ng khối DHT, với tọ
độ điểm c ối x, y, z (tính từ công thức động học th ận đã được xác thực) làm
thông số đầ và Kết q ả đầ r là các giá trị góc q y q1, q2, q3 tư ng ứng ch mỗi khớp.
Trang 33 Kết quả mô phỏng
Nhóm tiến hành mô phỏng với 2 trường hợp:
Trường hợp 1: q1 = 0°,q = 0°,q = 0°
Trang 34 Trường hợp 2: q1 = 90°,q2 = 0°,q3 = 0°
Xét bộ nghiệm số 1:
Hình 10 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 1
Hình 11 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 1
Nhận xét: Với bộ nghiệm số 1 ch kết q ả góc q y 𝑞1,𝑞2,𝑞3 chính xác ch cả 2 trường hợp Tiếp tục kiểm chứng bộ nghiệm này với nhiề trường hợp khác.
Xét bộ nghiệm số 2:
Trang 35Hình 13 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 2
Nhận xét: bộ nghiệm số 2 tính t án r kết q ả chư chính xác ch 2 trường hợp
Xét bộ nghiệm số 3:
Hình 14 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 3
Hình 15 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 3
Nhận xét: Bộ nghiệm số 3 tính t án r kết q ả chư chính xác ch 2 trường hợp
Xét bộ nghiệm số 4:
Trang 36Hình 16 Kết q ả mô phỏng trường hợp 1 với bộ nghiệm 4
Hình 17 Kết q ả mô phỏng trường hợp 2 với bộ nghiệm 4
Nhận xét: Bộ nghiệm số 4 tính t án r kết q ả chư chính xác ch 2 trường hợp Kết luận: Thông q kết q ả mô phỏng, việc tính t án động học nghịch bằng
phư ng pháp đại số mà nhóm đã trình bày ở chư ng 2, t th được 1 bộ nghiệm đư
Trang 37Hình 18 Mô phỏng q y hoạch q ỹ đạo th o đường thẳng
Chư ng trình c d tr ng khối Tr j ct ryL p:
%% Pr gr m by C nchimn n
f ncti n [x, y] = D(t, P0, v0, Pf, vf, tf)
l p_c nt = 5; % Xác định số lần lặp
T_t t l = 2 * tf * l p_c nt; % Tổng thời gi n để thực hiện đủ 5 lần đi và về
% Tính t án thời gi n tư ng đối tr ng mỗi ch kỳ (từ A đến B h ặc B về A) cycl _tim = m d(t, 2 * tf); % Mỗi ch kỳ đi và về mất 2*tf giây
% Kiểm tr nế t đã vượt q á thời gi n T_t t l (tức là đã h àn thành 5 lần lặp)
Trang 38f ncti n [x, y] = Tr j ct ryPl nning(t, P0, v0, Pf, vf, tf)
Trang 39Nhận xét: Kết q ả mô phỏng ch thấy hệ h ạt động ổn định đúng với tọ đồ điểm
đầ c ối mà t m ng m ốn, s 2s đạt đến vị trí điểm B và s 4s q y về vị trí điểm A, với ch kì 5 lần thì dừng tại vị trí điểm A.
Hình 20 Kết q ả mô phỏng vận tốc th o trục x s 5 ch kỳ
Nhận xét: Kết q ả mô phỏng ch thấy hệ h ạt động ổn định đúng với vận tốc mà
t m ng m ốn, s 2s vận tốc giảm và s 4s vận tốc tăng, với ch kì 5 lần thì vận tốc bị triệt tiê về không d dừng tại vị trí điểm A mà t m ng m ốn.
Hình 21 Kết q ả mô phỏng vị trí th o trục y s 5 ch kỳ
Trang 40Nhận xét: Kết q ả mô phỏng ch thấy hệ h ạt động ổn định đúng với tọ độ điểm
đầ c ối mà t m ng m ốn, s 2s đạt đến vị trí điểm B và s 4s q y về vị trí điểm A, với ch kì 5 lần thì dừng tại vị trí điểm A.
Hình 22 Kết q ả mô phỏng vận tốc th o trục y s 5 ch kỳ
Nhận xét: Kết q ả mô phỏng ch thấy hệ h ạt động ổn định đúng với vận tốc mà t
m ng m ốn, vận tốc tăng tr ng 2s và vận tốc giảm tr ng 2s tiếp th , với ch kì 5 lần thì vận tốc bị triệt tiê về không d dừng tại vị trí điểm A mà t m ng m ốn.
Kết luận: Thông q kết q ả mô phỏng t thấy q ỹ đạ ch yển động đi th đúng
với t ạ độ 2 điểm mà t cần q y h ạch q ỹ đạ , đồng thời dừng lại s 5 ch kỳ vận tốc th 2 trục bằng 0 tại 2 đầ A và B th ả mãn các điề kiện đặt r b n đầ
4.3.2 Qu h ạch quỹ đạ th hình t m giác bằng hàm đ thức bậc b
Chọn A(10,0), B 5,20 , C(- 25, - 5) là 3 điểm cần q y q y h ạch q ỹ đạ th ộc không gi n làm việc củ r b t Thời gi n đi q A là tA = 0s, đi q B tại tB = 2s, đi
q C tại tC = 4s s đó trở về A với t = 6s Vận tốc khi di ch yển q A, B và C lần lượt là vA = 0,0 T, vB = 0,0 Tvà vC = 0,0 T.
