Tổng quan về điều khiển và xõy dựng luật điều khiển vị trớ cho robot cú số bậc tự do dư
3.4. ỏp dụng phương phỏp Jacobien chuyển vị vào mụ phỏng điều khiển robot
Bảng số liệu của robot :
Bảng 3.1. Thụng số vật lý Robot
Chiều dài thanh thứ 1: 0.07 m Khối lượng thanh thứ 1 : 0.3 kg Chiều dài thanh thứ 2: 0.05 m Khối lượng thanh thứ 2 : 0.3 kg Chiều dài thanh thứ 3: 0.05 m Khối lượng thanh thứ 3 : 0.3 kg Chiều dài thanh thứ 4: 0.05 m Khối lượng thanh thứ 4 : 0.3 kg Chiều dài thanh thứ 5: 0.05 m Khối lượng thanh thứ 5 : 0.3 kg
3.4.1. Xõy dựng luật điều khiển mụ phỏng robot
Phương trỡnh động học của robot trong mặt phẳng ngang cú dạng
Trong đú :
- Ma trận H
- Ma trận V
Ma trận H và ma trận V thể hiện mối liờn hệ động lực học giữa cỏc khớp trong robot . Chi tiết 2 ma trận này đó được trỡnh bày tại chương 2 của quyển đồ ỏn.
Luật điều khiển theo phương phỏp ma trận Jacobien chuyển vị cú dạng tổng quỏt là :
(3.12) Theo yờu cầu của đề tài thỡ ta phải điều khiển đồng thời 2 điểm tỏc động cuối E1 và E2 tới vị trớ đặt E1d và E2d .Luật điều khiển cho từng điểm tỏc động cuối sẽ là :
(3.13)
(3.14) Để điều khiển đồng thời 2 điểm , luật điều khiển chung cho toàn hệ thống sẽ là :
= + (3.15)
3.4.2. ỏp dụng tiến hành mụ phỏng trờn simulink
ỏp dụng luật điều khiển được xõy dựng trong phần 3.4.1 tiến hành mụ phỏng trờn simulink
Giải thớch tỏc dụng cỏc khối chức năng trong mụ hỡnh :
- Khối Dynamic : tớnh toỏn động học robot , đưa kết quả ra là ma trận đạo hàm
bậc 2 của cỏc khớp
- Khối Dynamic 1 : tớnh toỏn động học thuận robot với đầu vào là số đo cỏc
khớp của robot tuõn theo bài toỏn động học thuận đưa ra được vị trớ của cỏc điểm tỏc động cuối E1 và E2
- Khối u[q.qdot] : thuật điều khiển của robot , nhận dữ liện phản hồi về từ robot
, tớnh toỏn và đưa ra 4 biến điều khiển vị trớ của 2 điểm tỏc động cuối về hệ thống
- Khối h[q.qdot] : nhiệm vụ của khối này là tớnh toỏn ma trận V trong phương
trỡnh động lực học .
Trong mụ hỡnh simulink trờn , toàn bộ phần tớnh toỏn được lập trỡnh bằng cỏc m- file gồm cú cỏc file :
- Con_in3.m : lập trỡnh thuật điều khiển
- Con_para3.m : Thụng số của bộ điều khiển và thụng số đặt - M_term3.m : Chương trính tớnh toỏn cỏc hệ số của ma trận H - Dyna3.m : Chương trỡnh tớnh toỏn động lực học
- Parasyst3.m : Khai bỏo thụng số vật lý của robot
- Jaco1.m : Chương trỡnh tớnh toỏn ma trận Jacobien của tay mỏy thứ nhất - Jaco2m : Chương trỡnh tớnh toỏn ma trận Jacobien của tay mỏy thứ hai - H3.m : Chương trỡnh tớnh toỏn ma trận V
-Vị trớ ban đầu Điểm tỏc động cuối E1 Điểm tỏc động cuối E2 -Vị trớ đặt Điểm tỏc động cuối E1 Điểm tỏc động cuối E2 -Ma trận Kp -Ma trận Kv Kết quả mụ phỏng :
Nhận xột quỏ trỡnh mụ phỏng
Khi tiến hành mụ phỏng , ta đó thay đối nhiều lần hệ số KP và KV để nhận được kết quả mụ phỏng tốt nhất cú thể để sử dụng điều khiển cho robot . Ta nhận thấy :
- Thời gian xỏc lập của đồ thị khỏc nhau với từng bộ số KP và KV
- Khi KP
>> KV thỡ hệ thống bị dao động mạnh , độ quỏ điều chỉnh vượt rất cao .
- Khi KP
<< KV thỡ hệ thống sẽ chạy rất chậm thời gian xỏc lập lớn .
- Lựa chọn bộ số KP
= 180 và KV
Chương 4