Tách bài toán động học ngược thành 2 bài toán nhỏ : bài toán động học ngược về vị trí(position) và về hướng(orientantion)
Từ ma trận biến đổi của động học thuận, ta có thể phân tách thành 2 bài toán độc lập, mỗi bài có 3
biến số
Ma trận tình tiến xác định vị trí của hệ tọa độ gắn với P(px;py;pz) được mô tả trong hệ tọa độ. Ma trận thì chỉ hướng của hệ tọa độ gắn với P(px,py,pz) mô ta trong hệ tọa độ. Giai bài toán động học về hướng sau khi đã giải bài toán về vị trí.
1 bài toán động học ngược thì thường có nhiều nghiệm, vì vậy sẽ đặt chọn nghiệm trong khi hoặc sau khi giải bài toán động học ngược. Về thong thường thì để chọn lựa nghiệm cho tay máy robot có tư thế tối ưu nhất về lực, về momen hay để tránh vật cản.
T05 = T01 * T12 * T23 * T34 * T45 = Ma trận bậc 4 như bên dưới Ma trận chuyển đổi hệ 0 sang 5 ta đã tính được ở trên động học thuận
px = 10*cos(t1)*(17*cos(t2 + t3 + t4) + 21*cos(t2 + t3) + 30*cos(t2)) = 10**(17* + 21* + 30*)
py = 10*sin(t1)*(17*cos(t2 + t3 + t4) + 21*cos(t2 + t3) + 30*cos(t2)) = 10**(17* + 21* + 30*)
pz = 170*sin(t2 + t3 + t4) + 210*sin(t2 + t3) + 300*sin(t2) + 268 = 170* + 210* + 300* + 268
Trong đó: - = cos(t1) - = cos(t1+t2) - = cos(t2+t3+t4) - = sin(1) - = sin(t2+t3) - = sin(t2+t3+t4)
Nhân ma trận nghịch đảo của T01 cho hai vế T05 = T01 * T12 * T23 * T34 * T45 * = * (**** ) 4x4 1(ma trận đơn vị) 4x4 [* * * n1] [* * * m1] [* * * n2] [* * * m2] [* * * n3] [* * * m3] [0 0 0 1] [0 0 0 1] Ta sử dụng hàm T10 = inv(T01) để tìm ma trận nghịch đảo T10 = T10 =
[ cos(t1)/(cos(t1)^2 + sin(t1)^2), sin(t1)/(cos(t1)^2 + sin(t1)^2), 0, 0] [ 0, 0, 1, -268] [ sin(t1)/(cos(t1)^2 + sin(t1)^2), -cos(t1)/(cos(t1)^2 + sin(t1)^2), 0, 0] [ 0, 0, 0, 1] = T10*T05
= T10 *
=
[ (r11*cos(t1))/(cos(t1)^2 + sin(t1)^2) + (r21*sin(t1))/(cos(t1)^2 + sin(t1)^2), (r12*cos(t1))/(cos(t1)^2 + sin(t1)^2) + (r22*sin(t1))/(cos(t1)^2 + sin(t1)^2), (r13*cos(t1))/(cos(t1)^2 + sin(t1)^2) + (r23*sin(t1))/(cos(t1)^2 + sin(t1)^2), (px*cos(t1))/(cos(t1)^2 + sin(t1)^2) + (y*sin(t1))/(cos(t1)^2 + sin(t1)^2) ]
[ r31, r32, r33, z - 268] [ (r11*sin(t1))/(cos(t1)^2 + sin(t1)^2) - (r21*cos(t1))/(cos(t1)^2 + sin(t1)^2),
(r12*sin(t1))/(cos(t1)^2 + sin(t1)^2) - (r22*cos(t1))/(cos(t1)^2 + sin(t1)^2), (r13*sin(t1))/(cos(t1)^2 + sin(t1)^2) - (r23*cos(t1))/(cos(t1)^2 + sin(t1)^2), (px*sin(t1))/(cos(t1)^2 + sin(t1)^2) - (y*cos(t1))/(cos(t1)^2 + sin(t1)^2) ]
[ 0, 0, 0, 1] R11 R12 R13 px R21 R22 R23 py R31 R32 R33 pz 0 0 0 1
Sử dụng hàm n1 = simplify(T10T05(1,4)) = px*cos(t1) + py*sin(t1)
n2 = simplify(T10T05(2,4)) = pz – 268
n3 = simplify(T10T05(3,4)) = px*sin(t1) - py*cos(t1) t12t23t34t45 = ***
[ cos(t5)*(cos(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) - sin(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2))), -sin(t5)*(cos(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) - sin(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2))), cos(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + sin(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)),
300*cos(t2) + 210*cos(t2)*cos(t3) - 210*sin(t2)*sin(t3) + 170*cos(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + 170*sin(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) ]
[ cos(t5)*(cos(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + sin(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3))), -sin(t5)*(cos(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + sin(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3))), sin(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) - cos(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)),
300*sin(t2) + 210*cos(t2)*sin(t3) + 210*cos(t3)*sin(t2) - 170*cos(t4 + pi/2)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) + 170*sin(t4 + pi/2)*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) ]
[ sin(t5), cos(t5), 0, 0] [ 0, 0, 0, 1] Sử dụng hàm
m1 = simplify(t12t23t34t45 (1,4)) = 170*cos(t2 + t3 + t4) + 210*cos(t2 + t3) + 300*cos(t2) m2 = simplify(t12t23t34t45 (2,4)) = 170*sin(t2 + t3 + t4) + 210*sin(t2 + t3) + 300*sin(t2)
m3 = simplify(t12t23t34t45 (3,4)) = 0 Ta có (đã đơn giản hóa) :
n1= px* + py* m1=170* + 210*+ 300* n2= pz – 268 = m2=170* + 210* + 300* n3= px*- y* m3=0
n3=m3 px*- py* = 0 <=> = => tan(t1) = => t1 = atan2(py,px) TÍNH THETAL 3 n1=m1 px* + py*= 170* + 210*+ 300* n2=m2 pz – 268 = 170* + 210* + 300* px* + py* - 170* = 210*+ 300* pz – 268 - 170* = 210* + 300* Đặt A = px* + py* - 170* B = pz – 268 - 170* A = 210*+ 300* = B = 210* + 300* = = * + * +2*210*300** = * + * +2*210*300** + = *( + ) + *() + 2*210*300*(*+*) Ta dùng matlab để đơn giản hóa (*+*)
>>simplify(cos(t2+t3)*cos(t2)+sin(t2+t3)*sin(t2)) = cos(t3) =
+ = + + 2*210*300* =
A = px* + py* - 170* ,chọn t234=0
B = pz – 268 - 170* Thay A và B vào ta tính được = => tan(t3)= => t3 = atan2() TÍNH THETAL 2 Ta có A = 210*+ 300* A = 210*(* - *) + 300* B = 210* + 300* B = 210*(* + *) + 300* A = 210** - 210** + 300* A = *(210*+300) - 210** B = 210** + 210* * + 300* B = *(210*+300) + 210** = = Thế vào vế trên = = = = = - + = *(1+) = *() = = *
= A và B , thetal 3 đã tính được ở trên Vậy = =
=> tan(t2) = => t2 = atan2(,)
3.5.2 TÍNH TOÁN THEO PHƯƠNG PHÁP HÌNH HỌC Hình 3.11 Mặt cắt của robot dạng hình học Ta có các thông số sau : O1O2 = L2 = 300 (mm) O2O3 = L3 = 210 (mm) O3O5 = L4 + L5 = 170 (mm)
t1,t2,t3,t4,t5 lần lượt là các góc thetal để phù hợp cho việc lập trình trên matlab; θ234 = t234 = θ2+ θ3 + θ4 (đây là góc tự chọn có thể tự cho để cố định đầu robot)
β1 = b1; β1 = b2
Giải
Tanθ1 = => θ1 = t1 = atan2(Pz,Px) Để tìm θ2, ta tìm θ3 trước
Nhìn hình ta thấy góc =- θ3
Xét tam giác Δ(O1O2O3), áp dụng định lý cosin (= + – 2.(O1O2).(O2O3).COS(180- θ3)
Ta có 2 cung bù nhau cos(180 - θ3) = -cos θ3 nên : (= + + 2*L2*L3*cos(t3)
cos(t3) = (***)
O1K = O1M – KM = – (L4 + L5)*cos(t234) o GO4 = (L4 + L5)*sin(t234) O3K = Pz – L1 – GO4 = Pz – 268 – 170*sin(t234) ( = ( + ( = ( + (
Thay O1O3 vào (***) ta được cos(t3) sin(t3) =
tan(t3) = => θ3 = t3 = atan2(sin(t3),cos(t3)) TÍNH θ2 = β2 - β1
Xét ΔO2RO3 vuông góc tại R RO3 = L3*sin(t3)
RO2 = L3*cos(t3)
Xét ΔO1RO3 vuông góc tại R
RO1 = L2 + RO2 = L2 + L3*sin(t3) tan(b1) = => b1 = β1 = atan2(RO3,RO1)
tan(b2)= => b2 = β2 = atan2(O3K,O1K)
Vậy θ2 = β2 - β1hay t2 = b2-b1
Θ4 = θ234 – θ2 – θ3 (hay t4 = t234 – t2 – t3)
3.6 THIẾT KẾ BẢNG ĐIỀU KHIỂN ROBOT TRÊN MATLAB R2016A
Trước khi chúng ta làm bảng điều khiển trên matlab, ta càn đưa mô hình mô phỏng mà chúng ta đã vẽ bên Soliworks 2018 qua bên simulinks của matlab bằng cách tải file liên kết giữa hai app.
Hình 3.13 File liên kết giữa Soliworks và Matlab
B1: Sau khi đã tải file thành công, ta chạy ứng dụng matlab, chọn vào đường dẫn đến hộp thư tải file Simscape Mltiboby
B2 Mở Matlab và thực hiện lệnh trên của sổ command window và bấm enter :
install_addon(‘<zip_file_name>.zip>’)
B3 Thực hiện tiếp lệnh dưới, mệnh này mục đích là để liên kết 2 phần mềm lại
Smlink_linksw
Như vậy là ta đã hoàn thành việc cài đặt tiện ích liên kết Simulink lên Soliworks. Và sau khi đã add-in này thì bây giờ Soliworks đã có them nguồn lưu file với dạng xml phục vụ cho việc mô phỏng dựa trên kết cấu ứng dụng Matlab.
Để xuất file xml từ Soliworks sang matlab, ta vào mục Tools > Simscape Multibody > Export > SimMechanics First Generation > đặt tên và save.
Hình 3.14 Chuyển file xml từ Soliworks qua Matlab
Sau khi đã đã xuất ra được file Xml qua Matlab thì ta tiến hành import các link của robot lại với nhau bằng lệnh : mech_import
Sau khi import xong ta sẽ cho được mô hình tay máy robot.
Hình 3.15 Mô hình Robot đưa qua Matlab
Ở giữa mỗi 2 khâu là phần khớp, mỗi phần khớp thì sẽ có một chùm khối điều khiển trong mục file có đuôi .slx
Khối Constant: (khối hằng số) dùng tạo hằng số thực hoặc phức (không phụ thuộc vào thời gian). Khối Slider Gain nằm trong khối tổng quát Math Operatinons, dùng để thay đổi độ lợi của tín hiệu
bằng thanh trượt. Trong phần này ta dùng để thay đổi giá trị góc quay.
Khối Mux : dùng tồng hợp các tín hiệu đầu vào thành một tín hiệu tổng ở đầu ra.
Khối Joint Actuator : khối này là để điều khiên khớp bằng tín hiệu tổng mà khối mux đưa vào hay
còn gọi là lệnh cho khớp xoay.
Khối Joint Sensor : đưa tín hiệu feed back góc quay về bộ PID, để so sánh với góc đưa vào từ
Tiếp theo, ta tạo một file GUIDE để thiết kế bảng điều khiển cho tay máy robot
Hình 3.16 Bảng điều khiển
Sau khi đã hoàn thành xong bảng điều khiển ta tiến hành viết code cho bảng điều khiển.