TÓM tắt LUẬN án GIẢI bài TOÁN NGƯỢC ĐỘNG học ĐỘNG lực học và điều KHIỂN TRƯỢT ROBOOT dư dẫn ĐỘNG dựa TRÊN THUẬT TOÁN HIỆU CHỈNH GIA LƯỢNG véc tơ tọa độ SUY RỘNG

BỘ GIÁO DỤC VÀ ĐÀO TẠO VIỆN KHOA HỌC VÀ CÔNG NGHỆ
VIỆT NAM


VIỆN CƠ HỌC





TRẦN HOÀNG NAM



GIẢI BÀI TOÁN NGƯỢC ĐỘNG HỌC,
ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN TRƯỢT RÔBỐT DƯ DẪN ĐỘNG
DỰA TRÊN THUẬT TOÁN HIỆU CHỈNH GIA LƯỢNG
VÉC TƠ TỌA ĐỘ SUY RỘNG




Chuyên ngành : Cơ học kỹ thuật
Mã số : 62.52.02.01



TÓM TẮT LUẬN ÁN TIẾN SĨ KỸ THUẬT

HÀ NỘI – 2010
Công trình được hoàn thành tại:
VIỆN CƠ HỌC - VIỆN KHOA HỌC VÀ CÔNG NGHỆ VIỆT NAM




Người hướng dẫn khoa học :
1. GS. TSKH. Nguyễn Văn Khang, Trường ĐH Bách khoa Hà Nội
2. PGS. TS. Nguyễn Phong Điền, Trường ĐH Bách khoa Hà Nội



Phản biện 1:
Phản biện 2:
Phản biện 3:




Luận án sẽ được bảo vệ trước hội đồng chấm luận án cấp nhà nước
họp tại Viện Cơ Học - Viện Khoa học và Công nghệ Việt Nam

Vào hồi … giờ ngày tháng năm 2010









Có thể tìm hiểu luận án tại :
- Thư viện Quốc gia
- Thư viện Viện Cơ học Việt Nam




CÁC CÔNG TRÌNH KHOA HỌC ĐÃ CÔNG BỐ:
1. Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam: “Về một phương
pháp số giải bài toán động học ngược robot dạng chuỗi”. Tuyển tập
Hội nghị Cơ học toàn quốc lần thứ 8, Tập 1, Tr. 250-259. NXB Bách
khoa, Hà Nội 2007.
2. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng
Nam : “Về một thuật toán điều khiển trượt robot dư
dẫn động”. Tạp

chí Tin học và Điều khiển học, Tập 24 (2008), No.3, Tr.269-280.
3. Nguyễn Quang Hoàng, Nguyễn Văn Khang, Trần Hoàng Nam: “Bài
toán động học ngược rôbốt dư dẫn động có chú ý đến sự cố kẹt khớp”.
Tuyển tập Hội nghị Cơ học toàn quốc, Tập 2, Tr. 282-290, NXB Khoa
học Tự nhiên và Công nghệ, Hà Nội 2009.
4. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Trần Hoàng Nam: “Về
bài toán động l
ực học ngược rôbốt dư dẫn động”. Tuyển tập Hội nghị
Khoa học Công nghệ Cơ khí chế tạo toàn quốc lần thứ hai, Phân ban
“Tự động hóa và Cơ điện tử”, Tr 41-48, Hà Nội 2009.
5. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang
Nam : “In fluence of trajectories on the joint torques of kinematically
redundant manipulators”. Vietnam Journal of Mechanics, vol. 29
(2007), No.2, pp. 65-72.
6. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran
Hoang Nam : “Inverse kinematic and dynamic analysis of redundant
measuring manipulator BKHN-MCX-04”. Vietnam Journal of
Mechanics, vol. 32 (2010), No.1, pp. 15-26.

1
MỞ ĐẦU
1. Tính cấp thiết của đề tài nghiên cứu
Theo các tài liệu về rôbốt, một rôbốt được gọi là dư dẫn động khi số tọa độ
suy rộng nhiều hơn số tọa độ tối thiểu xác lập nên vị trí và hướng của khâu
thao tác. Nhờ tính dư dẫn động mà rôbốt dư dẫn động có khả năng tránh được
các điểm kỳ d
ị, các giới hạn của biến khớp, các vật cản …
Khi nghiên cứu rôbốt ta phải giải quyết các bài toán về động học, động lực
học và điều khiển. Trong các bài toán này thì các bài toán ngược là các bài
toán khó, nhất là đối với các bài toán ngược của rôbốt dư dẫn động. Bài toán
ngược của rôbốt dư dẫn động ở nước ta hãy còn ít được nghiên cứu. Do đó việc
nghiên cứu, tìm ra phương pháp mới để gi
ải bài toán ngược là việc làm cấp
thiết và vì vậy tác giả đã chọn đề tài nghiên cứu là: ”Giải bài toán ngược động
học, động lực học và điều khiển trượt rôbốt dư dẫn động dựa trên thuật toán
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”.
2. Mục đích nghiên cứu
Xây dựng một thuật toán đưa lại độ chính xác cao khi giải các bài toán
ngượ
c động học, động lực học và điều khiển dạng trượt rôbốt dư dẫn động.
3. Đối tượng và nội dung nghiên cứu
Đối tượng nghiên cứu của luận án là các rôbốt dư dẫn động.
Nội dung nghiên cứu là khảo sát bài toán động học ngược, bài toán động
lực học ngược và bài toán điều khiển trượt rôbốt dư dẫn động.
4. Các phương pháp nghiên c
ứu
• Phương pháp tự động hóa thiết lập các phương trình động học và động

lực học của hệ nhiều vật.
• Phương pháp mô phỏng số dựa trên phần mềm đa năng MATLAB và
MAPLE.
• Phương pháp thực nghiệm.
5. Những đóng góp mới của luận án
Đã đề xuất “thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” và áp
dụng nó để giải các bài toán ngược động học, động lực học và điều khiển
chuyển động của rôbốt dư dẫn động bằng phương pháp trượt. Đã tiến hành giải
một số ví dụ minh họa chứng tỏ tính ưu việt của phương pháp giải bài toán
ngược khi sử dụng thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” so
với khi gi
ải bài toán mà không sử dụng thuật toán hiệu chỉnh gia lượng véc tơ
tọa độ suy rộng.



2
6. Bố cục của luận án
Luận án có 142 trang. Ngoài các phần mở đầu, kết luận chung, tài liệu
tham khảo, các công trình liên quan đến luận án, nội dung chính của luận án
được trình bày trong 4 chương :
Chương 1: “Tính toán động học ngược rôbốt dư dẫn động bằng thuật toán
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”.
Chương 2: “Tính toán động lực học ngược rôbốt dư dẫn
động trong không
gian thao tác dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”.
Chương 3: “Điều khiển trượt rôbốt dư dẫn động dựa trên thuật toán số
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”.
Chương 4: “Động lực học và điều khiển trượt rôbốt đo BKHN-MCX-04”.
Là chương áp dụng các kết quả nghiên cứu lý thuyết cho một mô hình rôbố

t đo
BKHN-MCX-04 mới được chế tạo.
CHƯƠNG 1
TÍNH TOÁN ĐỘNG HỌC NGƯỢC RÔBỐT DƯ DẪN ĐỘNG
BẰNG THUẬT TOÁN HIỆU CHỈNH GIA LƯỢNG
VÉCTƠ TỌA ĐỘ SUY RỘNG
Từ việc giải bài toán động học thuận ta xác định được quan hệ
x = f(q) (1.1)
còn khi giải bài toán ngược ta phải xác định quan hệ hình thức được suy ra từ
biểu thức (1.1) dưới dạng
q = f
-1
(x) (1.2)
1.1 Phương pháp khai triển Taylor
Trong các cuốn sách [48, 51] đã trình bày một thuật toán số như sau:

t)t()t()t(
kk1k
Δ
+
=
+
qqq
&
(1.3)
Trong đó
)t(
k
q
&

được xác định từ công thức
)t())t(()t(
kk
1
k
xqJq
&
&&
−
= (1.4)
Thế (1.4) vào (1.3) ta được
t)t())t(()t()t(
kk
1
k1k
Δ+=
−
+
xqJqq
&
&
(1.5)
Kết quả tính toán véc tơ tọa độ suy rộng theo (1.5) là khá thô. Do đó ta
phải tìm cách cải tiến công thức (1.5) để có độ chính xác cao hơn.
1.2 Các công thức xác định véc tơ vận tốc và véc tơ gia tốc suy rộng
Từ bài toán động học thuận, ta có hệ thức
)(qfx = (1.6)

3
Đạo hàm 2 vế của (1.6) theo thời gian, ta được :

qqJq
q
f
x
&&
&
)(=
∂
∂
=
(1.7)
Trong đó :
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎣
⎡
∂
∂
∂
∂

∂
∂
∂
∂
∂
∂
∂
∂
=
∂
∂
=
n
m
2
m
1
m
n
1
2
1
1
1
q
f

q
f
q

f

q
f

q
f
q
f
)(
q
f
qJ
(1.8)
Giả sử J(q) có hạng đầy đủ. Theo [41, 56], ta chọn tựa nghịch đảo của
J(q) dưới dạng
[
]
1
)()()()(
−
+
= qJqJqJqJ
TT
(1.9)
Khi đó từ biểu thức (1.7) ta suy ra công thức tính véc tơ vận tốc suy rộng:
)t())t(()t(
kkk
xqJq
&

&
+
= (1.10)
Và suy ra:
)t())t(()t())t(()t( xqJxqJq
&
&
&&
&&
++
+= (1.11)
Để áp dụng được công thức (1.11) cần phải tính được
))t((qJ
+
&
.
. Từ biểu thức (1.9) ta suy ra :
()()
(
)
(
)
)t()t()t()t(
TT
qJqJqJqJ =
+
(1.12)
Đạo hàm 2 vế của (1.12) theo thời gian, ta được
()()
(

)
(
)
(
)
(
)
(
)
(
)
[
]
(
)
qJqJqJqJqJqJqJqJqJ
TTTT
&&&&
=++
++
(1.13)
Từ (1.13) ta suy ra
() () ()() () () ()
[
]
{}
() ()
[
]
1

TTTT
−
++
+−= qJqJqJqJqJqJqJqJqJ
&&&&
(1.14)
Ma trận
()
qJ
&
được tính bằng cách đạo hàm trực tiếp các phần tử của ma
trận
()
qJ theo thời gian. Thế (1.14) vào (1.11) ta tìm được gia tốc )t(q
&&
.
1.3 Các công thức xác định véc tơ tọa độ suy rộng
Áp dung khai triển Taylor đối với q
k+1
quanh giá trị q
k
ta có
)t(
2
1
t)tt(
2
kkkk1k
+Δ+Δ+=Δ+=
+

qqqqq
&&&
(1.15)
Thế biểu thức (1.10) vào (1.14) và bỏ qua các vô cùng bé bậc ≥ 2 ta được :

4
(
)
t
kk1k
Δ+=
+
+
xqJqq
&
với k = 0, 1, …, N-1 (1.16)
Từ đó, ta có các bước tính toán như sau:
1. Tìm
0
q .
2. Tính
)(),(),(
000
qJqJqJ
&
+
.
3. Tính
0
)0t( qq

&&
=
= theo (1.10) và tính
(
)
0
0t qq
&&&&
=
=
theo (1.11).
4. Tính
1k+
q theo (1.14), rồi tính
1k+
q
&
,
1k+
q
&&
theo(1.10) và (1.11).
Ta thấy việc tính
1k+
q theo (1.16) là khá thô. Vì vậy ta cần có một thuật toán
xác định
1k+
q chính xác hơn. Trong luận án đã đưa ra thuật toán hiệu chỉnh
gia lượng véc tơ tọa độ suy rộng
1k+

q khi biết
k
q . Sơ đồ khối của thuật toán
này được trình bầy trên hình 1.1.

1.4 Đánh giá sai số
Để đánh giá sai số của phương pháp ta đưa vào các công thức xác định sai
số của dịch chuyển, của vận tốc và của gia tốc như sau

1kkkkkkkk
kkkkkk
kkkkk
)()()t()t()t(
)()t()t()t(
)()t()t()t(
+
−−=−=
−=−=
−
=
−=
qqJqqJxxxe
qqJxxxe
qfxxxe
&&&
&
&&&&&&
&&
&
&&&

&
(1.17)
Trong đó
[]
T
kmk2k1k
)t(e)t(e)t(e)t( L=e .
Độ lớn của các chuẩn của các véc tơ )t(),t(),t(
kkk
eee
&&&
cho biết độ chính
xác của thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng.
Trong chương trình tính toán ta sẽ dùng chuẩn Euclid
)t(e)t(e)t(e)t(
k
2
mk
2
2
k
2
1
k
+++= Le

5

Hình 1.1. Sơ đồ khối giải bài toán động học ngược
Sai
Sai
Cho
)(qfx
=
,
)t(xx
=
,
()
)(q
q
f
qJ
∂
∂
=
, t
0
,

0
q
, N, T

k: = 0
0k
t:t
=
,
0k
:
~
qq
=
Tính
)
~
(
k
qJ
+
,
)t(
kk
xx
=
,
)
~
(

kk
qff
=

kkk
kkkk
~
:
~
))(
~
(
qqq
fxqJq
Δ+=
−
=
Δ
+
ε
<
Δ
k
q
N
k
≥
1
k
:

k
+=
t)(
~
:
~
1k1k1kk
Δ
+=
−−
+
−
xqJqq
&
Đúng
Đúng
h = ∆t = T/N
KẾT THÚC
Xuất kết qủa
q
k




6
1.5 Ví dụ minh họa
Giải bài toán động học ngược của rôbốt dư dẫn động 5 khâu động như hình vẽ

q
1

O
E
x
y
q
2

q
3
q
4
ϕ

q
5
x
1
x
2
x
3
x
4

x
5

Cho biết chiều dài của các khâu là:
a
1
= 0.55(m); a
2
= 0.50(m); a
3
= 0.45(m); a
4
= 0.40(m); a
5
= 0.20(m).
Phương trình chuyển động của điểm thao tác E là:
x
E
= 0.8+0.1cos(2t) (m); y
E
= - 0.8+0.1sin(2t) (m)
Bàn kẹp của rôbốt phải luôn tạo với phương thẳng đứng 1 góc ϕ=1(rad).
Qua việc áp dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng ta tìm
được quy luật chuyển động của các khớp động là :
0 1 2 3 4 5 6 7 8 9 10
-2
-1
0
1
2

3
4
time [s]
q
1
[rad]
qd
1
[1/s]
qdd
1
[1/s
2
]

Hình 1.2
Các đặc tính chuyển động của khâu 1
0 1 2 3 4 5 6 7 8 9 10
-2
-1.5
-1
-0.5
0
0.5
1
1.5
time [s]
q
2
[rad]

qd
2
[1/s]
qdd
2
[1/s
2
]

Hình 1.3.
Các đặc tính chuyển động của khâu 2
0 1 2 3 4 5 6 7 8 9 10
-3
-2
-1
0
1
2
time [s]
q
3
[rad]
qd
3
[1/s]
qdd
3
[1/s
2
]

Hình 1.4.
Các đặc tính chuyển động của khâu 3

7
0 1 2 3 4 5 6 7 8 9 10
-4
-2
0
2
4
6
time [s]
q
4
[rad]
qd
4
[1/s]
qdd
4
[1/s
2
]

Hình 1.5.
Các đặc tính chuyển động của khâu 4
0 1 2 3 4 5 6 7 8 9 10
-10
-8

-6
-4
-2
0
2
4
time [s]
q
5
[rad]
qd
5
[1/s]
qdd
5
[1/s
2
]

Hình 1.6.
Các đặc tính chuyển động của khâu 5
0 1 2 3 4 5 6 7 8 9 10
-6
-4
-2
0
2
4
6
x 10

-16
time [s]
e
x
[m]

Hình 1.7.
Sai số theo trục x của điểm thao tác E
0 1 2 3 4 5 6 7 8 9 10
-1
0
1
2
3
x 10
-15
time [s]
e
y
[m]

Hình 1.8.
Sai số theo trục y của điểm thao tác E

0 1 2 3 4 5 6 7 8 9 10
-1
-0.5
0
0.5
1

1.5
2
2.5
x 10
-15
time [s]
e
φ
[rad]

Hình 1.9. Sai số góc định hướng của bàn kẹp


8
-0.4 -0.2 0 0.2 0.4 0.6 0.8 1
0
0.2
0.4
0.6
0.8
1
x [m]
y [m]

Hình 1.10.
Dạng chuyển động của rôbốt theo kết quả tính toán
Các ví dụ trong luận án đã chứng tỏ tính ưu việt của phương pháp giải bài
toán ngược động học rôbốt dư dẫn động khi sử dụng thuật toán hiệu chỉnh gia
lượng véc tơ tọa độ suy rộng (đạt độ chính xác cỡ 10
-15

) so với phương pháp
giải không sử dụng thuật toán (chỉ đạt độ chính xác 10
-4
).
CHƯƠNG 2
TÍNH TOÁN ĐỘNG LỰC HỌC NGƯỢC
RÔBỐT DƯ DẪN ĐỘNG TRONG KHÔNG GIAN THAO TÁC DỰA
TRÊN THUẬT TOÁN HIỆU CHỈNH GIA LƯỢNG VÉC TƠ TỌA ĐỘ
SUY RỘNG
2.1 Phương trình động lực học của rôbốt
Trong các tài liệu về rôbốt ta đã có biểu thức:
τqgqqqCqqM
=
+
+ )(),()(
&&&&
(2.1)
trong đó :
•
n
R∈q
là véctơ tọa độ suy rộng,
•
nn
R)(
×
∈qM
là ma trận khối lượng,
•
nn

R),(
×
∈qqC
&
là ma trận liên quan lực coriolis, lực ly tâm,
•
n
R)( ∈qg
là véctơ lực do trọng lực,
•
n
R∈τ
là véctơ lực/mômen dẫn động từ các động cơ.
2.1 Giải bài toán ngược động lực học rôbốt dư dẫn động trong không
gian thao tác
Khi tính toán, thiết kế rôbốt ta thường phải xác định các lực/mômen dẫn
động cần thiết tác động trên các khâu của rôbốt để khâu thao tác của rôbốt có

9
thể làm việc theo một chương trình đã định trước. Bài toán này được gọi là bài
toán động lực học ngược.
Mối liên hệ giữa vị trí của bàn kẹp với các biến khớp có dạng
x = f(q) (2.2)
trong đó
n
R∈q
là véctơ chứa các biến khớp,
m
R
∈

x là véctơ chứa vị trí
tâm và hướng của bàn kẹp trong một hệ tọa độ cố định.
Đạo hàm 2 vế biểu thức (2.2) theo thời gian, ta được

qqJq
q
f
x
&&
&
)(=
∂
∂
=
(2.3)
Từ (2.3) ta có

xqJq
&
&
)(
+
=
(2.4)
Đạo hàm 2 vế của (2.4) theo thời gian, ta được

xqJxqJq
&
&
&&

&&
)()(
++
+= (2.5)
với
).(
dt
d
)( qJqJ
++
=
&

Sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng chúng ta sẽ
xác định được q,
q
&
, q
&&
tại các thời điểm khác nhau.
Do phương trình động lực học của rôbốt có dạng (2.1), nên sử dụng file số
liệu các véc tơ
qqq
&&&
,, của bài toán động học ngược thì từ (2.1) ta có thể xác
định được mômen/lực cần thiết tương ứng với chuyển động mong muốn x(t)
của bàn kẹp.
Vì vậy ta có các bước tiến hành tính mômen/lực của động cơ để bàn kẹp
chuyển động theo một quy luật x(t) định trước như sau:
1. Giải bài toán động học ngược để xác định các tọa độ, vận tốc và gia

tốc suy rộng qqq
&&&
,, của các khớp động từ phương trình chuyển động
của bàn kẹp
)t(),t(),t( xxx
&&&
.
2. Sử dụng phương trình (2.1) để tính các mômen/lực
τ
của các động cơ
dẫn động.
Từ đó ta xây dựng được sơ đồ khối giải bài toán động lực học ngược dựa trên
thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng như sau (hình 2.1):


10

KẾT THÚC
ε
<

Δ
k
q

N
k
≥
N/Tth);()( =Δ=
∂
∂
= q
q
f
qJ
Xác định
)(),,(),(),( qgqqCqMqf
&
Cho x=x(t), t
0
, q
0
, N, T
k:=0; t
k
:=t
0
;
0k
~
qq

=
Tính
)
~
(),t(),
~
(
kkkk
qffxxqJ
=
=
+

))(
~
(
kkkk
fxqJq
−
=
Δ
+


kkk
~
~
qqq
Δ
+

=

t)
~
(
~
:
~
1k1k1kk
Δ
+=
−−
+
−
xqJqq
&

kkkkkkk
)
~
()
~
(;)
~
( xqJxqJqxqJq
&
&
&&
&&
&

&
+++
+==

Tính
)(),,(),(
kkkk
qgqqCqM
&

)(),()(
kkkkkk
qgqqqCqqMτ
+
+
=
&&&&
Xuất kết quả
τ
k

1
k
:
k
+=

Đúng
Đúng
Sai

Sai


Hình 2.1 Sơ đồ khối giải bài toán động lực học ngược rôbốt dư dẫn động


11
CHƯƠNG 3
ĐIỀU KHIỂN TRƯỢT RÔBỐT DƯ DẪN ĐỘNG
DỰA TRÊN THUẬT TOÁN HIỆU CHỈNH GIA LƯỢNG
VÉC TƠ TỌA ĐỘ SUY RỘNG
Trọng tâm của chương là trình bày điều khiển trượt rôbốt dư dẫn động dựa
trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng [19, 49, 50].
3.1 Bài toán điều khiển chuyển động của rôbốt












Nhiệm vụ của bài toán điều khiển chuyển động rôbốt là đảm bảo cho khâu
thao tác luôn bám theo quỹ đạo cho trước trong không gian thao tác.
Từ đó ta có các dạng hệ thống điều khiển như sau:
• Hệ thống điều khiển trong không gian khớp.

• Hệ thống điều khiển trong không gian thao tác.
a. Bài toán điều khiển trong không gian khớp
Bài toán này được phân thành 2 bài toán nhỏ:
•
Bài toán động học ngược: Cho x
d
, tìm )(
d
1
xfq
−
= .
• Hệ thống điều khiển trong không gian khớp được thiết kế đảm bảo vị
trí khớp q luôn bám theo vị trí mong muốn q
d
, sao cho lượng sai lệch
min
d
→−qq .
Ưu điểm của phương pháp này là bộ điều khiển tác động trực tiếp đến hệ
thống truyền động của các khớp. Nhược điểm của nó là khó đảm bảo độ chính
xác cho vị trí của khâu thao tác do sự tồn tại các sai lệch trong cơ cấu dẫn động
(khe hở của các khớp, ma sát v.v) và thiếu thông tin về sai lệch
d
xx − trong
quá trình điều khiển.
x
P
q
1

q
3
q
2
P
Hình 3.1

12
b. Bài toán điều khiển trong không gian thao tác
Hệ thống điều khiển trong không gian thao tác có chức năng làm cho sai
số giữa x và x
d
bằng không.

0
d
→− xx
Trong đó
d
x là véc tơ vị trí mong muốn của khâu thao tác, x là véc tơ
phản hồi vị trí thực tế của khâu thao tác.
Ưu điểm của hệ thống điều khiển này là nó tác động trực tiếp tới các biến
của không gian thao tác x. Nhược điểm là khối lượng tính toán sẽ lớn do đó
thời gian điều khiển sẽ lâu.
3.2 Điều khiển trượt rôbốt dư dẫ
n động
Phương trình động lực học của rôbốt có dạng

τdqgqqqCqqM

=
+
+
+
)(),()(
&&&&
(3.1)
Trong đó:
M(q) - ma trận khối lượng,
qqqC
&&
),( - véctơ chứa các lực côriôlis và lực ly tâm,
g(q) - véctơ chứa các lực do trọng trường,
d - véctơ chứa các lực/mômen do kích động nhiễu,
τ - lực/mômen của động cơ dẫn động.
Để sử dụng điều khiển dạng trượt, ta đưa vào ký hiệu véc tơ sai số bám được
xác định bởi
)t()t()t(
d
qqe −= (3.2)
và véc tơ sai số suy rộng như sau
)t()t( Λees +=
&
(3.3)
trong đó
), ,,(diag
n21
λ
λ
λ=Λ , 0

i
>
λ
. (3.4)
Nhiệm vụ của bài toán điều khiển là chuyển hàm mục tiêu
0)t( →e sang
hàm mục tiêu
0)t( →s khi
∞
→t


∞
→→⇒→ tkhi0)t(0)t( se . (3.5)
Bây giờ ta tìm luật điều khiển sao cho
0)t(s
i
→ khi
∞
→t
.
Ta đặt

)t()t()t(
dr
Λeqq
−
=
&&

từ đó suy ra
)t()t()t()t()t()t()t()t(
dr
sΛeeΛeqqqq
=
+
=
+
−
=−
&&&&&
(3.6)
do đó
)t()t()t(
r
qqs
&&&&&
−= (3.7)
Để tìm luật điều khiển, ta chọn hàm Lyapunov như sau


13
sqMs )(
2
1
V
T
= (3.8)
Đạo hàm V theo thời gian t ta được
sqMssqMs )(

2
1
)(V
TT
&
&
&
+=
(3.9)
Từ (3.1) và chú ý đến (3.7) ta có

()
τdqgqqqCqqMqqqM
=
+
+
+
+
− )(),()t()())t()t((
rr
&&&&&&&&

()
(
)
dqqMqgqqqCτsqM
−
−
−
−

=⇒
r
)(),(
&&&&&
(3.10)
Từ (3.6) ta suy ra
()
(
)
tt
r
qsq
&&
+= (3.11)
Do đó ta có
r
),(),(),( qqqCsqqCqqqC
&&&&&
+
= (3.12)
Thế (3.12 ) vào (3.10), ta được
dqgqqqCqqMsqqCτsqM
−
−
−
−
−= )(),()(),()(
rr
&&&&&&
(3.13)

Thay (3.13) vào (3.9) ta được
[]
sqqCqMsdqgqqqCqqMτs
⎥
⎦
⎤
⎢
⎣
⎡
−+−−−−= ),()(
2
1
)(),()(V
T
rr
T
&
&
&&&&
&
(3.14)
Do tính chất
0),()(
2
1
TT
=− sqqCssqMs
&
&

Ta suy ra
[]
dqgqqqCqqMτs −−−−= )(),()(V
rr
T
&&&&
&
(3.15)
Căn cứ vào (3.15) ta chọn luật điều khiển (mômen cần thiết để đảm bảo
chuyển động theo chương trình) như sau
)sgn()(
ˆ
),(
ˆ
)(
ˆ
spdrr
sKsKqgqqqCqqMτ −−++=
&&&&
(3.16)
Trong đó:
[
]
T
n21
)ssgn(), ,ssgn(),ssgn()sgn( =s , và K
pd
, K
s
là các ma

trận thực đối xứng xác định dương,
0KK >=
T
pdpd
, 0KK >=
T
ss
.
Để đơn giản, ta chọn hai ma trận này có dạng là các ma trận đường chéo
{
}
nn
pd
22
pd
11
pdpd
k, ,k,kdiag=K và
{
}
nn
s
22
s
11
ss
k, ,k,kdiag=K

Với cách chọn (3.16), hệ thức (3.15) trở thành
[

]
=++++−−= )(
~
),(
~
)(
~
)sgn(V
rr
TT
s
T
pd
T
qgqqqCqqMsdssKssKs
&&&&
&


()
()
∑∑∑
===
ρ++−−=
n
1i
iii
n
1i
i

ii
s
n
1i
2
i
ii
pd
)d(s|s|ksk

14
()
()
∑∑
==
ρ+−−−≤
n
1i
iii
ii
s
n
1i
2
i
ii
pd
|s||)d|k(skV
&

với các sai lệch giữa mô hình thực sử dụng trong (3.1) và các thông số mô hình
sử dụng trong bộ điều khiển (3.16) như sau
⎪
⎪
⎭
⎪
⎪
⎬
⎫
−=
−=
−=
)(
ˆ
)()(
~
)(
ˆ
)()(
~
)(
ˆ
)()(
~
qgqgqg
qq,Cqq,Cqq,C
qMqMqM
&&&
(3.17)
và

[
]
i
rri
)(
~
),(
~
)(
~
qgqqqCqqM ++=ρ
&&&&

Như vậy, để đảm bảo
0V ≤
&
thì ta phải chọn các phần tử của K
s
sao cho
()
ii
ii
s
dk ρ+>
. Thành phần K
pd
chỉ là thành phần điều khiển PD thêm vào để
rút ngắn thời gian chuyển tiếp.
Từ các vấn đề nêu trên, đã xây dựng được sơ đồ khối để giải bài toán điều
khiển chuyển động rôbốt dư dẫn động trong không gian khớp theo phương

pháp trượt như trên hình 3.3.
Do đặc điểm của hàm sgn(s
i
) là không liên tục tại giá trị s
i
= 0, do đó ở bộ
điều khiển sẽ xảy ra hiện tượng chattering. Để khử chattering ta thay hàm dấu
sgn(s) bằng hàm bão hoà
)/(sat
ξ
s .
T
n21
)]/s(sat), ,/s(sat),/s(sat[)/(sat ξξξ=ξs .
Sai số điều khiển trong trường hợp này phải chấp nhận tăng lên. Lúc này,
thành phần
()
ii
pd
k và
()
ii
s
k trong phạm vi sai số suy rộng s
i
nằm trong khoảng ξ
đều đóng vai trò là bộ điều khiển PD.
Với
⎪
⎩

⎪
⎨
⎧
≥
<
=
1x)xsgn(
1xx
)x(sat



Hình 3.2 Hàm sat(x)

15

Hình 3.3. Sơ đồ tính toán và mô phỏng điều khiển rôbốt
Cho biết
τd
q
g
q
q
q
C
q
q
M
=
+
+
+

)(),()(
&&&&
)t(),t(),t(),t(),t(
00ddd
qqqqq
&&&&

N
T
h,T,,,
spd
=KKΛ

t
k
: =
t
0
)t()t()t(
kdkk
qqe
−
=

)t()t()t(
kdkk
qqe
&&&
−
=

)t()t()t(
kkk
Λees
+
=
&

In, vẽ đồ thị
)t(
k
q
,
)t(
k
q
&


)t(),t(
kk
se
; k=0,1, N)
)t()t()t(
kkdkr
Λeqq
−
=
&&

)t()t()t(
kkdkr
eΛqq
&&&&&
−
=

(
)
(
)
() ()
)t(sgn)t()t(
ˆ
)t()t(),t(
ˆ
)t()t(
ˆ
)t(
kskpdk
krkkkrkk
sKsKqg
qqqCqqMτ
−−+
++=
&&&&
t
k+1
:= t
k

+ h
Giải PTVP chuyển động từ
httttt
k1kk
+
=
=
→
=
+

với bước tích phân
N
T
t =Δ

)t()t()(),()(
k
τdqgqqqCqqM
=
+
+
+
&&&&

Thu được
)t(
1k+
q
,

)t(
1k+
q
&

k ≥ N
KẾT THÚC
Đúng
Sai
k:=k+1


16
CHƯƠNG 4
BÀI TOÁN NGƯỢC ĐỘNG HỌC, ĐỘNG LỰC HỌC
VÀ ĐIỀU KHIỂN TRƯỢT RÔBỐT ĐO BKHN-MCX-04
Rôbốt đo BKHN-MCX-04 đã được thiết kế và chế tạo để tiến hành các bài
toán động học ngược, động lực học ngược và điều khiển chuyển động.
4.1 Kết cấu của rô bốt đo BKHN-MCX-04
Rôbốt đo BKHN-MCX-04 có hình dạng như hình 4.1 và có các thông số
hình học nh
ư trong bảng 4.1.




Hình 4.1. Mô hình rôbốt
đo BKHN-MCX-04

Bảng 4.1. Thông số hình học của rôbốt
Khâu
i O
i-1
O
i
(m)
1 0.14
2 0.15
3 0.20
4 0.0
5 0.163
6 0.080
4.2 Tính toán động học ngược
Từ hình 4.1, ta xác định được các tham số DH như trong bảng 4.2.
Bảng 4.2. Bảng tham số động học DH
Khâu
i
θ
i
d
i
a
i
α
i
1 q
1
d

1
0 π/2
2 q
2
0 a
2
0

3 q
3
0

a
3
0
4 q
4
0 0 -π/2
5 q
5
d
5
0 π/2
Qu
ỹ đạo
đ
ối
1
3
5

4
6
2
Đ
ốit
ư
ợng
c
ầ
n
đo
Quỹ đạo định trước

17
Từ bảng tham số DH ta xác định được các ma trận H
1
, H
2
, H
3
, H
4
, H
5
và sau
đó là ma trận D
5
= H
1
H

2
H
3
H
4
H
5


⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
+++
++−+−−+
++−+−−
=
1000
SaSaCddSSCCS
CSaCSaSSdCSSCCSSSCCCS
CCaCCaSCdCSCCSSCSSCCC
233222345123452345234

231321223415234515123415152341
231321223415234515123415152341
5
D

Từ các phần tử của ma trận D
5
, ta xác định được các tọa độ của điểm E
trên hệ tọa độ cố định

1234523322E
234152313212E
234152313212E
dCdSaSaz
SSdCSaCSay
SCdCCaCCax
+++=
−+=
−
+
=
(4.1)
Phương trình (4.1) biểu diễn quan hệ
x = f(q) (4.2)
Từ (4.1) ta xác định được ma trận Jacobi J(q)
⎥
⎥
⎥
⎦
⎤

⎢
⎢
⎢
⎣
⎡
=
3534333231
2524232221
1514131211
JJJJJ
JJJJJ
JJJJJ
)(qJ
(4.3)
Trong đó các phần tử của ma trận J(q) có dạng
J
11
= -d
5
S
1
S
234
- a
3
S
1
C
23
- a

2
S
1
C
2

J
12
= -d
5
C
1
C
234
- a
3
C
1
S
23
- a
2
C
1
S
2

J
13
= -d

5
C
1
C
234
- a
3
C
1
S
23

J
14
= -d
5
C
1
C
234

J
15
= 0
J
21
= -d
5
C
1

S
234
+ a
3
C
1
C
23
+ a
2
C
1
C
2

J
22
= -d
5
S
1
C
234
+ a
3
S
1
S
23
+ a

2
S
1
S
2

J
23
= -d
5
S
1
C
234
+ a
3
S
1
S
23

J
24
= -d
5
S
1
C
234

J
25
= 0
J
31
= 0
J
32
= -d
5
S
234
+ a
3
C
23
+ a
2
C
2

J
33
= -d
5
S
234
+ a
3
C

23

J
34
= -d
5
S
234

J
35
= 0
Với ma trận J(q) được xác định theo hệ thức (4.3) và một quĩ đạo định
trước của điểm E, ta có thể xây dựng chương trình tính toán các giá trị biến
khớp q
i
và các đạo hàm )5, ,2,1i(,
ii
=
qq
&&&
dựa trên thuật giải đã được trình

18
bày trong chương 1.
Ví dụ điểm E có quỹ đạo chuyển động là một đường xoắn ốc bám trên một
mặt cầu như hình 4.2.

Hình 4.2. Quỹ đạo định trước của điểm E (đường xoắn ốc)
Quy luật chuyển động của điểm E có dạng

)3/tsin(1.012.0z
)3/tsin()t2cos(1.0y
);3/tsin()t2sin(1.03x
E
E
E
+=
π=
π
+=
(4.4)
trong đó các tọa độ được tính theo đơn vị đo là mét.
Kết quả tính toán bài toán động học ngược được biểu diễn trên các hình
4.3, a, b và c. Tham số góc quay q
5
là hằng số và không có ảnh hưởng đến quỹ
đạo chuyển động của điểm E.


Hình 4.3. a. Đồ thị biến khớp q
b.
Đồ thị vận tốc góc

c.
Đồ thị gia tốc góc
4.3 Tính toán động lực học ngược
Bỏ qua ma sát tại các khớp và các lực cản khác và xem lực tác động của
vật cần đo lên khâu 6 tại vị trí tiếp xúc là không đáng kể.
Phương trình động lực được biểu diễn dưới dạng tổng quát như sau:

τqgqqqCqqM
=
+
+ )(),()(
&&&&
(4.5)
Quĩ đạo điểm E

19
Giả sử quy luật chuyển động của điểm E (điểm O
5
) trên mặt phẳng
{}
00
zOx được mô tả bởi các phương trình tọa độ (4.6) và được minh họa trên
hình 4.4.
)m(t
4
sin12.014.0z
0y);m(t
4
cos112.02.0x
E

EE
π
+=
=
⎟
⎠
⎞
⎜
⎝
⎛
π
−+=
(4.6)
Với các thông số động lực học xác định theo bảng 4.4 và vị trí khối tâm
của các khâu theo bảng 4.3
Bảng 4.3. Vị trí khối tâm khâu i của rôbốt trên hệ động
Vị trí trọng tâm
Khâu i
)i(
Ci
x

)i(
Ci
y

)i(
Ci
z

1 0 -(d
1
-l
1
) 0
2 -(a
2
-l
2
) 0 0
3 -(a
3
-l
3
) 0 0
4 0 0 l
4
5 0 -(d
5
-l
5
) 0
6 -(a
6
-l
6
) 0 0
Bảng 4.4. Các thông số động lực học của rôbốt đo
Khâu
i

m
i

(kg)

I
xi
(kgm
2
)
I
yi
(kgm
2
)
I
zi
(kgm
2
)

l
i

(m)
1 2.0
4.0×10
-3
3.0×10
-3

1.0×10
-3

0.10
2 0.9
0.2×10
-3
3.0×10
-3
3.0×10
-3

0.06
3 1.2
0.5×10
-3
3.5×10
-3
4.0×10
-3

0.10
4 1.1
0.6×10
-3
2.5×10
-3
3.5×10
-3

0.04
5 0.5
0.7×10
-3
0.2×10
-3
0.3×10
-3

0.03
6 0.05
0.3×10
-4
0.2×10
-4
0.1×10
-4

0.02


Hình 4.4 Hình 4.5 (a)

20
Qua tính toán, ta có được kết quả giải bài toán động lực học ngược như
trên các hính 4.5 a, b, c và d.



b. c. d.

Hình 4.5 a. Trị số các biến khớp 2, 3 và 4
b. Vận tốc góc của động cơ dẫn động khâu 2, 3, 4
c. Gia tốc góc của động cơ dẫn động khâu 2, 3, 4
d. Mômen dẫn động cần thiết các khâu của rôbốt.
4.4 Điều khiển trượt rôbốt BKHN-MCX-04
Giả sử chi tiết cần đo có dạng hình cầu như trên hình 4.2
Để đo vật thể dạng hình cầu này ta có thể tiến hành điề
u khiển điểm O
5
chuyển động theo 1 đường xoắn ốc trên bề mặt của hình cầu. Giả sử phương
trình của đường xoắn ốc có dạng
)3/tsin(1.012.0z
)3/tsin()t2cos(1.0y
)3/tsin()t2sin(1.03x
E
E
E
+=
π=
π
+
=

Các bài toán động học ngược và động lực học ngược cho trường hợp này
đã được giải quyết trong phần 4.2 và 4.3. Ta sẽ sử dụng các kết quả này cho
phần điều khiển.
Để ổn định chuyển động của điểm O
5
ta chọn hàm Lyapunov dạng

sqMs )(
2
1
V
T
=
Trong đó M(q) là ma trận khối lượng của rôbốt, s là sai số suy rộng trong
điều khiển dạng trượt, với

)t()t( Λees +=
&

trong đó e(t),
)t(e
&
là sai lệch vị trí và sai lệch vận tốc của các khớp động
e(t) = q(t) – q
d
(t); )t()t()t(
d
qqe
&&&
−
=

), ,,(diag
621
λ
λλ=Λ
Sử dụng chương trình giải bài toán điều khiển trượt cho rôbốt ở chương 3,

với các thông số của bộ điều khiển đã được chọn :
K
s
= diag([20,20,20,20,1,0.1]);
K
p
= diag([0.2,0.2,0.2,0.2,0.02,0.01]);
λ= diag([50,50,50,50,30,20]);
Sai số ước lượng của mô hình 5%, Nhiễu d(t)=random(6,6)*0.01;

21
Ta thu được kết quả điều khiển chuyển động rôbốt BKHN-MCX-04 như sau:

a. b. c.
Hình 4.6 a. Đồ thị tọa độ x(t) trong không gian thao tác
b. Đồ thị tọa độ y(t) trong không gian thao tác.
c. Đồ thị tọa độ z(t) trong không gian thao tác.
Qua kết quả trên ta thấy rằng điểm cần điều khiển O
5
của rôbốt BKHN-
MCX-04 bám khá tốt theo quỹ đạo mong muốn, thời gian để điểm O
5
đạt được
quỹ đạo mong muốn là 0.25 giây.
4.5 Thí nghiệm
Thí nghiệm được tiến hành để kiểm tra độ chính xác của việc giải bài toán
động học ngược. Mô hình thí nghiệm có dạng như hình ảnh dưới đây:

Hình 4.7 Mô hình thí nghiệm rôbốt đo BKHN-MCX-04
4.5.1 Cấu tạo của hệ thống thí nghiệm

Bộ thí nghiệm gồm có các phần như sau:
Rôbốt BKHN-MCX-04
Camera thu nhận hình ảnh hoạt động của rôbốt
Kính lọc màu
Máy tính

22
4.5.2 Nguyên lý hoạt động của hệ thống thí nghiệm
Trên rôbốt, tại các khớp động ta gắn các đèn led. Khi rôbốt chuyển động,
camera chụp lại ảnh và lưu vào bộ nhớ của máy tính. Phần mềm xử lý ảnh sẽ
xác định vị trí tâm của các khớp và lưu vào tệp “toadotam.txt” của các khớp.
Chương trình điều khiển rôbốt được xây dựng bằng ngôn ngữ Visual
Basic. Thông qua dao diện của chương trình, ta nhập vào các giá trị góc quay
của các khớp động mà ta tính toán được qua bài toán độ
ng học ngược. Chương
trình điều khiển sẽ điều khiển các khớp rôbốt quay với các góc tương ứng. Còn
phần mềm xử lý ảnh sẽ ghi và lưu lại các giá trị góc quay thực và tọa độ của
các tâm khớp cũng như vẽ được quỹ đạo chuyển động của khâu thao tác.
Dưới đây là kết quả thí nghiệm đã tiến hành trên rôbốt BKHN-MCX-04
4.5.3 Kết quả thí nghiệm
Giả sử quỹ đạo mong muốn của bàn kẹp của rôbốt BKHN-MCX-04 có
dạng là nửa vòng tròn với phương trình chuyển động là
)m()4/tsin(12.014.0z
0y
);m()4/tcos(1(12.02.0x
E
E
E
π+=
=

π
−
+
=

Qua bài toán động học ngược ta nhận được giá trị tọa độ điểm cuối E và
các tọa độ suy rộng của các khớp động theo bảng dưới đây

TT t (s) x
E
(m) y
E
(m) q
2
(rad) q
3
(rad) q
4
(rad)
1 0 0.2000 0.1405 1.0472 3.5511 2.1206
2 0.2 0.2015 0.1578 1.0957 3.5814 2.1564
3 0.4 0.2059 0.1776 1.1396 3.6379 2.1711
4 0.6 0.2131 0.1935 1.1767 3.7157 2.1622
5 0.8 0.2229 0.2110 1.2045 3.8077 2.1294
6 1.0 0.2351 0.2239 1.2212 3.9070 2.0750
7 1.2 0.2495 0.2376 1.2261 4.0085 2.0030
8 1.4 0.2655 0.2459 1.2195 4.1089 1.9177
9 1.6 0.2829 0.2546 1.2025 4.2068 1.8234
10 1.8 0.3012 0.2575 1.1763 4.3013 1.7236
11 2.0 0.3200 0.2605 1.1425 4.3922 1.6213

12 2.2 0.3388 0.2575 1.1023 4.4791 1.5192
13 2.4 0.3571 0.2546 1.0569 4.5616 1.4194
14 2.6 0.3745 0.2459 1.0076 4.6389 1.3242
15 2.8 0.3905 0.2376 0.9557 4.7099 1.2355
16 3.0 0.4049 0.2239 0.9027 4.7733 1.1549
17 3.2 0.4171 0.2110 0.8501 4.8272 1.0843
18 3.4 0.4269 0.1935 0.7997 4.8696 1.0248
19 3.6 0.4341 0.1776 0.7532 4.8987 0.9776
20 3.8 0.4385 0.1578 0.7122 4.9125 0.9432
21 4.0 0.4400 0.1400 0.6783 4.9095 0.9218

23


Hình 4.8 Quỹ đạo thực nghiệm của điểm cuối E
Qua kết quả thí nghiệm ta thấy sự khác biệt giữa quỹ đạo chuyển động
mong muốn của khâu thao tác và kết quả chuyển động thực thu được qua thí
nghiệm là không đáng kể. Điều đó chứng tỏ thuật toán mà chúng tôi đã sử
dụng để giải bài toán động học ngược là hoàn toàn phù hợp.
4.6 Kết luận ch
ương 4
Trên cơ sở rôbốt đo tự chế tạo, đã tiến hành xác định các tham số động
học, động lực học của rôbốt. Sau đó đã tiến hành tính toán động học ngược,
động lực học ngược và điều khiển rôbốt bám theo quỹ đạo cần đo. Đây là một
loại rôbốt có cấu trúc động học không gian nên việc thiết lập các phương trình
vi phân chuyể
n động khá phức tạp. Do đó việc tính toán động lực học ngược và
điều khiển chuyển động không đơn giản nếu ta sử dụng các công cụ tính toán
cũ. Nhờ thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”, bài toán này
được giải một cách khá nhanh chóng.

Các kết quả của chương này một mặt chứng minh tính chất đúng đắn của
các thuật toán trong các chương trước, mặt khác nêu ra mộ
t khả năng chế tạo
và sử dụng rôbốt trong đo lường chính xác các chi tiết và cụm các chi tiết máy.