TRƯỜNG ĐẠI HỌC BÁCH KHOA TP.

HỒ CHÍ MINH
KHOA ĐIỆN – ĐIỆN TỬ
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

ĐỒ ÁN 1
ÁP DỤNG CÁC KẾT QUẢ MATLAB ĐIỀU KHIỂN ROBOT
THỰC

Giảng viên hướng dẫn: ThS. Nguyễn Đức Hoàng

Sinh viên thực hiện:
MSSV Họ và tên
2010452 Nguyễn Tuấn Ngọc

TP. HỒ CHÍ MINH, 2023

MỤC LỤC
 CHƯƠNG I: CÁC KHÁI NIỆM CƠ BẢN
1, Số bậc tự do
Trong mô hình toán học số bậc tự do được thể hiện sẽ là tương ứng với số chuyển động
độc lập của tay máy. Bậc tự do là số chuyển động độc lập hay số tọa độ cần thiết để định
vị và định hướng của vật thể ở tay tay máy đủ cơ cấu chấp hành trong không gian làm
việc. Để biểu diễn hoàn chỉnh một đối tượng trong không gian cần 6 tham số: 3 tọa độ để
xác định vị trí đối tượng trong không gian và 3 tọa độ biểu diễn hướng của vật thể. Động
học tay máy công nghiệp nối tiếp số bậc tự do sẽ tương ứng với số khớp được truyền
động.
Mỗi robot bao gồm nhiều khâu (links) liên kết với nhau qua các khớp (joint), để tạo thành
một chuỗi động học xuất phát từ khâu cơ bản (base) đứng yên. Hệ tọa độ gắn với khâu cơ
bản gọi là hệ tọa độ cơ bản. Hệ tọa độ gắn với các khâu tiếp theo gọi là hệ tọa độ suy
rộng. Cấu hình của robot sẽ được xác định tại từng thời điểm bởi các phép biến đổi xoay
hoặc tịnh tiến của các hệ tọa độ suy rộng.
2, Ma trận Homogeneous:
Ma trận xoay được dùng để diễn tả góc xoay giữa các hệ tọa độ khác nhau. Để
quay quanh trục r bất kỳ góc ϑ , ta thực hiện lần lượt như sau:

 Quay quanh trục Z góc ∅ .

 Quay quanh trục Y góc θ lúc

này trục p trùng trục Z nên quay
quanh r chính là quay quanh Z
góc ϑ .

 Quay trả lại quanh trục Y góc θ .

 Quay trả lại quanh trục Z góc ∅

Hình . Phép quay trong không gian
Lúc đó ta có ma trận xoay như sau:

A RB =Rot ¿ ¿

[ ]
r 2x (1−cos ϑ )+cos ϑ r x r y (1−cos ϑ )−r z sin ϑ r x r z (1−cos ϑ )+r y sin ϑ
Rot (¿ r , ϑ )= r x r y (1−cos ϑ )+r z sin ϑ 2
r (1−cos ϑ )+cos ϑ
y r y r z (1−cos ϑ )−r x sin ϑ ¿
r x r z (1−cos ϑ )−r y sin ϑ r y r z (1−cos ϑ )+r x sin ϑ r 2z (1−cos ϑ )+cos ϑ
 Khi tính toán động học ngược thì góc quay có thể xác định như sau:

ϑ =cos
−1
( r +r 2+r −1 ) Trong đó :cos
11 22 33 −1
( x)=arccos(¿ x)¿

Vecto quay r được xác định như sau:

[] [ ]
rx r 32−r 23
1
Ar = r y = r −r
2sin ϑ 13 31
rz r 21−r 12

3, Ma trận Homogeneous

Thông thường để biểu diễn một phép chuyển hệ tọa độ, ta phải thực hiện phép
xoay ma trận R rồi đến dịch chuyển tâm của hệ tọa độ. Ma trận homogeneous cho phép ta
thực hiện điều này một cách dễ dàng bằng các phép nhân ma trận. Cấu trúc ma trận
homogeneous trong hệ tọa độ Cartesian như sau:

H=
[ R0 d1] ; R ∈ SO (3 ) ,d ∈ R
Với R là ma trận xoay mô tả góc xoay của hệ trục tọa độ theo Rx, Ry, Rz và d là
ma trận vị trí mô tả độ lớn hệ trục tọa độ di chuyển theo các phương x,y,z.

Để diễn tả các phép xoay từ điểm đầu đến điểm cuối robot ta chỉ cần nhân các ma
trận Homogeneous lại với nhau theo nguyên tắc sau đây:
0 0 1
H 2=H 1 H 2
 Hình 4.1 Chuyển đổi giữa các hệ tọa độ
4, Phương pháp Denavit–Hartenberg (DH)
Giả sử trong chuỗi động học của tay máy có n khâu, khâu thứ i nối khớp thứ i với khớp
thứ i+1.

Hình 4.2 Mô hình hó a khâ u và khớ p theo DH

 Trục zi nằm dọc theo trục của khớp (i+1).

 Trục xi nằm dọc theo đường vuông góc chung hướng từ z i-1 đến zi. Trong
trường hợp 2 trục giao nhau thì ⃗x i=⃗z i−1 x ⃗z i , trục ⃗y i=⃗z i x ⃗xi .

 Đặt gốc tọa độ Oi, tại giao điểm giữa z i và pháp tuyến chung nhỏ nhất của
trục zi và zi-1. Giao điểm chung với trục zi-1 là gốc Oi’ của hệ Oi’ – xi’,yi’,zi’

Xác định bộ thông số Denavit-Hartenberg:

 Gọ i ai là khoả ng cá ch giữ trụ c zi-1 và trụ c zi dọ c theo trụ c xi.

 Gọ i αi là gó c xoay zi-1 quanh trụ c xi đến song song vớ i zi.

 Gọ i di là khoả ng cá ch dọ c theo trụ c zi-1 giữ a xi-1 và xi.

 Gọ i θi là gó c xoay xi-1 quanh trụ c zi đến song song vớ i xi.

Xác định biến khớp:

  Nếu khớp (i) là khớp quay thì θi là biến khớp.

 Nếu khớp (i) là khớp tịnh tiến thì di là biến khớp.

Hình 4.3 Các thông số mô tả của phương pháp DH

Trên cơ sở các hệ tọa độ đã ấn định cho tất cả các khâu của robot thì mối
quan hệ giữa các hệ tọa độ nối tiếp nhau (i-1) và (i) được xác định bởi các phép
quay và tịnh tiến sau đây:

 Quay quanh zi-1 một góc θi.

 Tịnh tiến dọc theo zi-1 một đoạn di.

 Tịnh tiến dọc theo xi-1, xi một đoạn ai.

 Quay quanh xi một góc xoắn αi.

Ma trận chuyển đổi i-1Ti được tính theo công thức sau:

[ ]
cos θi −sin θ icos α i sin θ isin α i a i cos θ i
i-1 sin θi cos θ icos α i −cos θ i sin α i ai sin θ i
Ti =
0 sin α i cos α i d i
0 0 0 1

Khi robot có n khâu thì ma trận chuyển đổi được xác định theo công thức:
0
Tn =0T11T2 ... n-1Tn
 Từ đó có thể tính được vị trí và hướng của điểm cuối trên góc tọa độ.

CHƯƠNG II: TÍNH TOÁN CỦA PHƯƠNG TRÌNH CỦA ROBOT.

1, Động học thuận của robot OpenMANIPULATOR-X
Động học thuận là một phương trình mô tả vị trí và hướng của robot trong không gian
tương ứng với từng giá trị của các joint (khớp) của robot. Dựa vào giá trị các biến khớp ta
có thể tính ra vị trí chính xác trong không gian của các khớp và phần tử chấp hành đầu
cuối.

Dựa trên mô hình động học tổng quát ta có bảng Denavit-Hartenberg:

Link a α d θ
1 0 π d1 θ1
2
2 a2 0 0 θ2
3 a3 0 0 θ3

4 a4 0 0 θ4
 [ ]
cos θi −sin θi cos α i sinθ i sin α i ai cos θi
sinθ i cos θ i cos α i −cos θi sin α i ai sinθ i
T=
0 sin α i cos α i di
0 0 0 1

Ma trận chuyển từ hệ tọa độ i về hệ i-1

[ ]
cos ( θ1 ) 0 sin ( θ1 ) a1 cos ( θ1 )
sin ( θ1 ) 0 −cos ( θ1 ) a1 sin ( θ1 )
0
T1 =
0 1 0 d1
0 0 0 1

[ ]
cos ( θ2 ) −sin ( θ2 ) 0 a2 cos ( θ2 )
sin ( θ2 ) cos ( θ2 ) 0 a2 sin ( θ2 )
1
T2 =
0 0 1 0
0 0 0 1

[ ]
cos ( θ3 ) −sin ( θ3 ) 0 a3 cos ( θ3 )
sin ( θ3 ) cos ( θ3 ) 0 a3 sin ( θ3 )
2
T3 =
0 0 1 0
0 0 0 1

[ ]
cos ( θ 4 ) −sin ( θ4 ) 0 a4 cos ( θ 4 )
sin ( θ 4 ) cos ( θ4 ) 0 a4 sin ( θ 4 )
3
T4 =
0 0 1 0
0 0 0 1

Thực hiện phép nhân ma trận trên ta có hàm mô tả động học thuận:

[ ]
nx ox ax px
n o a py
0
T4 = 0T1. 1T2. 2T3. 3T4 = y y y
nz o z a z pz
0 0 0 1

nx = -c4(c1s23-c123)-s4(c12s3+c13s2)
ny = -c4(s123-s1c23)-s4(s13c2+s12c3)
nz = c4(c2s3+s2c3)+s4(c23-s23) , c4(c23-s23)-s4(c2s3+s2c3)
ox = s4(c1s23-c123)-c4(c12s3+c13s2)
oy = s4(s123-s1c23)-c4(s13c2+s12c3)
oz = c4(c23-s23)-s4(c2s3+s2c3)
ax = s1
ay = -c1
az = 0
px = a2c12-a4c4(c1s23-c123)-a4s4(c12s3+c13s2)+a3c123-a3c1s23
py = a2s1c2-a4s4(s13c2+s12c3)-a4c4(s123-s1c23)+a3s1c23-a3s123
pz = d1+a2s2+a4c4(c2s3+s2c3)+a4s4(c23-s23)+a3c2s3+a3s2c3
+ Chú thích:
C12 = Cos(θ1 +θ2 ¿

-Chọn vị trí của khớp thứ nhất làm base

+Ví trí của khớp thứ 2 so với base trong Oxyz: (T01(1,4), T01(2,4), T01(3,4))
+Ví trí của khớp thứ 3 so với base trong Oxyz: (T02(1,4), T02(2,4), T02(3,4))
+Vị trí của khớp thứ 4 so với base trong Oxyz: (T03(1,4), T03(3,4), T03(3,4))
+Vị trí của End effector so với base trong Oxyz: (T04(1,4), T04(2,4), T04(3,4))
Động học nghịch của robot OpenMANIPULATOR-X
Một điểm trong không gian được thể hiện bằng ma trận:

𝑇= [ R0 d1]; 𝑅 ∈ 𝑆𝑂(3),𝑑 ∈ ℝ
Với:

d là vị trí của điểm P trong không gian XYZ

R là ma trận chỉ hướng của điểm P

Ta có: 0T5 = 0T1. 1T2. 2T3. 3T4. 4T5

Tính theta1:

c1 = px/√ p 2x + p2y

s1 = py/√ p 2x + p2y

θ1= arctan2(Px, Py)

Dựa vào phương trình tính theta2, 3 và 4 ???

Hoạch định quỹ đạo robot:

TỔNG QUÁT : Chuyển động của 1 robot có 2 dạng :
Dạng 1: Chuyển động chỉ cần quan tâm đến điểm đầu, hướng và vị trí của điểm
cuối.Dạng này cách thức di chuyển của robot là không quan trọng, di chuyển từ điểm đến
điểm. Thường dùng trong các thao tác gắp, đặt , vận chuyển phôi.
Dạng 2: huyển động robot bắt buộc phải qua các điểm trung gian, robot di chuyển trong
các công nghệ. Robot di chuyển liên tục. Bài toán động học chú ý: từ quỹ đạo chuyển
động của khâu cuối , quy luật chuyển động của các khớp được quan tâm.
Hoạch định quỹ đạo: xác định quy luật chuyển động các biến khớp để điều khiển
chuyển động từng khớp và tổng hợp chung thành chuyển động của cả robot theo một quỹ
đạo xác định.
Để thiết kế quỹ đạo robot, người ta thường dùng phương pháp xấp xỉ các đa thức bậc n,
các quỹ đạo thương gặp là:
+ Qũy đạo CS (Cubic Segment): Tương đương đa thức bậc 3;
+ Qũy đạo LS (Linear Segment): Tương đương đa thức bậc 1;
+ Qũy đạo LSPB (Linear Segment with Parabolic Blend): Phối hợp đa thức bậc 2 với đa
thức bậc 1.
+Qũy đạo BBPB (Bang Bang Parabolic Blend): là trường hợp đặc biệt của quỹ đạo LSPB
khi đoạn tuyến tính thu về bằng 0 và xuất hiện điểm uốn.

Qũy đạo BBPB

 Qũy đạo đa thức bậc 3:
qi(t) = ai + bi( t -tk ) + ci( t -tk )2 + di( t -tk )3
qi(tk) = qk và qi’(tk) = qk’
qi(tk+1) = qk+1 và qi’(tk+1) = qk+1’
Thiết kế quỹ đạo CS của khớp thứ i đi qua hai điểm nút có giá trị qo và qf . Với các
ràng buộc qo’ = 0 và qf’ = 0.
Phương trình quỹ đạo:

Phương trình vận tốc:

Phương trình gia tốc:

Phương trình gia tốc:

  Qũy đạo là đường tuyến tính:
qo (t) = αo + vt
Với v = const;
 Qũy đạo là đường Parabol (LSPB):
Trong đồ án lần này, chúng tôi sử dụng quỹ đạo trên để quy hoạch quỹ đạo cho
robot: Với cách quy hoạch quỹ đạo vận tốc hình thang, vận tốc có giai đoạn tăng
tốc với gia tốc khác không và giai đoạn giảm tốc ở cuối quỹ đạo với gia tốc khác
không và thường bằng lúc tăng tốc.
Đồ thị vận tốc theo thời gian có dạng hình thang như hình phía trên. Do diện tích
hình thang là quãng đường đi nên ta có biểu thức:

𝑡𝑓 chính là thời gian tăng tốc cũng như giảm tốc. Với các thông số đặt trước là
quãng được lúc bắt đầu 𝑞0 và quãng được kết thúc kết thúc 𝑞1, tốc độ lớn nhất cho
phép 𝑣𝑚𝑎𝑥, từ đó ta tính được độ lớn gia tốc cần thiết ở giai đoạn tăng tốc cũng như
giảm tốc.
Sau khi đã có được các thông số 𝑞𝑐̇ và 𝑡𝑐 ta tính được các biểu thức:

Từ đó: ta tiến hành hoạch định đường đi theo thời gian

Đặt x, y, z là tọa độ của end effector của robot tại thời điểm t, tọa độ điểm bắt đầu
và kết thúc lần lượt là (𝑥𝑠,𝑦𝑠,𝑧𝑠) và (𝑥𝑒,𝑦𝑒,𝑧𝑒), khoảng cách ban đầu 𝑝0, khoảng
cách sau khi thực hiện chuyển động là 𝑝1, quãng đường cần đi được hiện tại là q

Động học vận tốc:

Xét tay máy có p khớp và r liên kết: Các biến khớp của hệ:
q = [ q 1 … qn ]T
Tọa độ trọng tâm tuyệt đối:
rci = [ x ci y ci y ci ]T
Vận tốc dài được xác định:
d rci d rci
vci = = . q̇ = JTi . q̇

[ ]
dt dt
d xci (q) d xci(q)
…
dq 1 dqn
d rci d yci d yci(q)
Với JTi = = …
dt dq 1 dqn
d zci d zci (q )
…
dq 1 dqn
Từ vận tốc góc của các khớp xoay, ta có thể tính vận tốc các khớp thông qua ma
trận jacobi:
[ θ̇ 1 θ˙2 θ˙3 θ˙4 ] = J-1 [ v/x v/y v/z v/wy]