Đồ án Thiết kế Robot
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (644.17 KB, 47 trang )
1
Danh mục hình vẽ.
2
LỜI NÓI ĐẦU
Khi xét về vấn đề Robot chúng ta đặt ra các bài toán: động học Robot, động lực học
Robot và điều khiển Robot. Đây là những bước cơ sở ban đầu hết sức quan trọng trước khi
thiết kế Robot.
Với sự phát triển như vũ bão của công nghệ thông tin như ngày này, rất nhiều các lĩnh
vực trong cơ khí đã tận dụng được sự phát triển này để tạo ra những bước nhảy vọt, rong đó
có công nghiệp Robot.
Trên cơ sở đó, môn học Robotics đã mang lại cho sinh viên những kiến thức vô cùng
quan trọng cho sinh viên chúng em. Bên cạnh đó, nó cũng tạo ra một cơ hội để sinh viên được
tiếp cận với những phần mềm tính toán, mô phỏng phổ biến trên thế giới hiện nay như Maple
và Matlab.
Để thực hiện được bài tập lớn này, em xin chân thành cảm ơn thầy giáo, PGS.TS Phan
Bùi Khôi đã tận tình, chu đáo dạy học trên lớp. Em xin chân thành cảm ơn thầy.
3
CHƯƠNG I . GIẢI BÀI TOÁN ĐỘNG HỌC
1.1. Giải bài toán động học thuận
1.1.1. Cơ sở lý thuyết
Vị trí mỗi khâu trong không gian được xác định bởi vị trí một điểm định vị và hướng
của khâu đó đối với một hệ quy chiếu đã chọn. Điểm định vị là một điểm xác định nào đó của
khâu, thông thường trong động lực học ta hay lấy khối tâm của khâu đó làm điểm định vị.
Hướng của khâu được xác định bằng ma trận cosin chỉ hướng hoặc bằng các tọa độ suy rộng
xác địnhvị trí của vật rắn quay quanh một điểm.
Động học robot nghiên cứu chuyển động của các khâu của robot về phương diện hình
học, không quan tâm đến các lực và momen gây ra chuyển động.Động học robot là bài toán
qua trọng phục vụ tính toán và thiết kế robot.Nhiệm vụ chủ yếu của bài toán động học thuận
là xác định vị trí và hương của bàn kẹp dưới dạng hàm của biến khớp.
Các phương pháp ma trận 4x4 và các phương pháp ma trận 3x3 hay được sử dụng trong
phân tích động học robot. Hai phương pháp ma trận 4x4 phổ biến là phương pháp ma trận
Denavit-Hartenberg và phương pháp ma trận Craig. Trong báo cáo này chúng em trình bày và
áp dụng phương pháp ma trận Denavit-Hartenberg để tính toán động học robot.
Giải bài toán động học thuận robot công nghiệp bằng phương pháp ma trận Denavit-
Hartenberg
Cách xác định các trục cuả hệ tọa độ khớp.
Đối với robot công nghiệp ,Denavit-Hartenberg đã đưa ra cách chọn các hệ trục tọa độ
có gốc tại khớp thứ i như sau:
Trục
1i
z
−
được chọn dọc theo hướng của trục khớp động thứ i.
Trục
1i
x
−
được chọn dọc theo đường vuông góc chung của 2 trục
2i
z
−
và
1i
z
−
hương đi
từ trục
2i
z
−
sang trục
1i
z
−
. Nếu trục
1i
z
−
cắt trục
2i
z
−
thì hướng của trục
1i
x
−
được chọn tùy ý
miễn là vuông góc với trục
1i
z
−
.Khi 2 trục
2i
z
−
và
1i
z
−
song song với nhau, giữa 2 trục này
có nhiều đường vuông góc chung , ta có thể chọn trục
1i
x
−
hướng theo pháp tuyến chung nào
cũng được.
Gốc tọa độ
1i
O
−
được chọn tại giao điểm cuả trục
1i
x
−
và trục
1i
z
−
.
4
Trục
1i
y
−
được chọn sao cho hệ
1
( x )
i
O yz
−
là hệ quy chiếu thuận.
Hệ tọa độ
1
( x )
i
O yz
−
được xác định như trên trong một số tài liệu được quy ước là hệ
tọa độ khớp.
Chú ý: Với cách chọn hệ tọa độ như trên , đôi khi hệ tọa độ khớp
1
( x )
i
O yz
−
không
được một cách duy nhất. vì vậy, ta có một số bổ sung thích hợp như sau.
Đối với hệ tọa độ
0
( x )O yz
theo quy ước trên ta mới chỉ chọn được trục
0
z
, còn trục
0
x
chưa có trong quy ước trên.Ta có thể chọn trục
0
x
một cách tùy ý, miễn là
0
x
vuông góc
với
0
z
.
Đối với hệ tọa độ
( x )
n
O yz
, do không có khớp (n+1) nên theo quy ước trên ta không
xác định
n
z
. Trục
n
z
không được xác định duy nhất, trong khi trục
n
x
lại được chọn theo
đường pháp tuyến của trục
1n
z
−
. Trong trường hợp này, nếu khớp n là khớp quay, ta có thể
chọn trục
n
z
song song trục
1n
z
−
. Ngòai ra ta có thể chọn tùy ý sao cho hợp lý.
Khi khớp thứ i là tịnh tiến, về nguyên tắc ta có thể chọn trục
1i
z
−
một cách tùy ý. Tuy
nhiên trong nhiều trường hợp người ta thường chọn trục
1i
z
−
dọc theo trục cuả khớp tịnh tiến
này.
5
Hình 1. 1. Diễn các thông số Denavit-Hartenberg giữa các trục hệ tọa độ
Các tham số động học Denavit-Hartenberg
Vị trí của hệ toạ độ khớp (Oxyz)
i
đối với hệ tọa độ khớp (Oxyz)
i-1
được 4 tham số
Denavit-haartenderg d
i
,a
i
, như sau:
i
d
dịch chuyển tịnh tiến dọc theo trục
1i
z
−
để gốc tọa độ
1i
O
−
chuyển đến
'
i
O
giao điểm của trục
i
x
và trục
1i
z
−
.
i
θ
: góc quay quanh trục
1i
z
−
để trục
1i
x
−
chuyển đến trục
'
i
x
(
'
i
x
//
i
x
).
i
a
: dịch chuyển tịnh tiến theo dọc trục
i
x
để điểm
'
i
O
chuyển đến điểm
i
O
.
i
α
: góc quay quanh trục
i
x
sao cho trục
'
1i
z
−
(
'
1i
z
−
//
1i
z
−
). Chuyển đến trục
i
z
.
Do hệ trục tọa độ
1
( xyz)
i
O
−
gắn liền vào khâu thứ i-1 , còn hệ trục tọa độ
( xyz)
i
O
gắn liền vào khâu thứ i , cho nên vị trí cuả khâu thứ i đối với khâu thứ i-1, được xác định
bới 4 tham số Denavit-hartenberg.
Trong 4 tham số trên, các tham số
i
a
và
i
α
luôn luôn là các hằng số , độ lớn của chúng
phụ thuộc vào hình dáng và sự ghép nối các khâu thứ i-1 và thứ i. Hai tham số còn lại
i
θ
và
i
d
một là hằng số, một là biến số phụ thuộc vào khớp i là khớp quay hay khớp tịnh tiến.Khi
khớp i là khớp quay thì
i
θ
là biến, còn
i
d
là hằng số. Khi khớp i là khớp tịnh tiến thì
i
d
là
biến, còn
i
θ
là hằng số.
Chú ý về việc xác định hệ tọa độ khớp tại khớp tịnh tiến.Trong trường hợp khớp i là
khớp tịnh tiến, về nguyên tắc ta có thể chọn trục
1i
z
−
một cách tùy ý, do đó việc xác định các
tham số Denavit-Hartenberg phụ thuộc vào việc chọn hệ trục tọa độ.
Ma trận của phép biến đổi, kí hiệu là
i
H
, là tích của 4 na trận biến đổi cơ bản,và có
dạng như sau:
6
cos( ) sin( )cos( ) sin( )sin( ) cos( )
sin( ) cos( )cos( ) cos( )sin( ) sin( )
0 sin( ) cos( )
0 0 0 1
i i i i i i i
i i i i i i i
i i i
θ θ α θ α a θ
θ θ α θ α a θ
α α d
−
−
=
H
11Equation Section (Next)212\* MERGEFORMAT (.)
Ma trận
i
H
được xác định bởi công thức (1.1) được gọi là ma trận Denavit-Hartenberg
địa phương.
Phương trình xác định vị trí khâu thao tác (bàn kẹp) cuả robot.
Xét mô hình cơ học của một robot n khâu động như hình vẽ:
Hình 1. 2. Robot n khâu
Theo nguyên tắc nêu trên, ta thiết lập được hệ trục tọa độ gắn liền với giá cố định và hệ
tọa độ gắn liền với các vật.Gọi
0
R
là hệ quy chiếu
0
( xyz)O
gắn liền với giá cố định, hệ quy
chiếu
( xyz)
i i
R O
=
gắn liền với khâu thứ i.Ma trận
1i
i
H
−
cho ta biết vị trí và hướng của khâu
i đối với hệ quy chiếu
1i
R
−
gắn vào khâu thứ i-1.
Từ đó suy ra ma trận Denavit-Hartenberg
i
H
cho biết vị trí của hệ quy chiếu
( xyz)
i i
R O=
đối với hệ quy chiếu
1 1
( xyz)
i i
R O
− −
=
.Áp dụng liên tiếp các phép biến đổi
đối với robot n khâu, ta có:
7
0
1 2 3
n
= =D H H H H
313\* MERGEFORMAT (.)
( )
0
0 1
n E
T
n
=
A r
D
`414\* MERGEFORMAT (.)
Ma trận
n
D
cho biết vị trí của điểm tác động cuối E và hướng cuả khâu thao tác (bàn
kẹp) của robot đối với hệ quy chiếu cố định
0
R
Như vậy khi biết được các đặc tính hình học cuả các khâu và quy luật chuyển động của
các khớp là ta có thể xác định được vị trí và hướng của bàn kẹp.
Xác định vận tốc, gia tốc điểm tác động cuối và vận tốc góc, gia tốc góc các khâu cuả
robot bằng phương pháp trực tiếp.
Vận tốc và gia tốc dài của bàn kẹp có thể dễ dàng suy ra từ đạo hàm vector tọa độ
(0)
E
r
Vận tốc điểm thao tác:
( )
( )
( )
( )
( )
( )
( )
( )
0
0
0
0
0 0 0
0
0
, :
E
R
E E
Ex
Ey E
Ez
E
d
x
dt
v
d d
hay v y
dt dt
v
d
z
dt
= =v r
515\* MERGEFORMAT (.)
Gia tốc điểm thao tác:
( )
( )
( )
( )
( )
( )
( )
( )
0
0
0 0
0
0
0 0
0
, :
Ex
R
E Ey
Ez
Ex
E Ey
Ez
d
v
dt
a
d d
hay a v
dt dt
a
d
v
dt
= =a v
616\* MERGEFORMAT (.)
Ta có thể tính trực tiếp vận tốc góc khâu thứ i của robot dựa trên công thức tính vận tốc góc
vật rắn thông qua ma trận cosin chỉ hướng như sau:
°
( )
°
( )
0
0
T
i i
i
T
i
i i
=
=
ω A A
ω A A
&
&
717\* MERGEFORMAT (.)
Từ (1.6) suy ra biểu thức vận tốc góc khâu thứ i của robot.
8
Áp dụng định nghĩa gia tốc góc của vật rắn, khi biết vận tốc góc của các khâi của robot
ta có thể tính gia tốc góc các khâu của robot theo công thức sau:
0
0 0
R
R B R B
d
dt
=α ω
ur uur
818\* MERGEFORMAT (.)
1.1.2. Thiết lập phương trình động học thuận cho robot RRR.
Hình 1. 3. Đặt các trục tọa độ cho robot RRR
Thiết lập bảng động học DH.
Từ hình vẽ ta tìm được bảng động học:
i
d
,
i
θ
,
i
a
,
i
α
Khâu
i
θ
i
d
i
a
i
α
1
1
q
0
1
a
2
π
2
2
q
0
2
a
0
3
3
q
0
3
a
0
a. Tìm các ma trận biến đổi
Dựa vào công thức (1.1) ở trên ta thiết lập các ma trận Denavit-Hartenberg H
i
như sau:
9
1 1 1 1
1 1 1 1
1
0
0
0 1 0 0
0 0 0 1
C S a C
S C a S
−
=
H
2 2 2 2
2 2 2 2
2
0
0
0 1 0 0
0 0 0 1
C S a C
S C a S
−
=
H
3 3 3 3
3 3 3 3
3
0
0
0 1 0 0
0 0 0 1
C S a C
S C a S
−
=
H
Dựa vào (1.2) ta có ma trận Denavit-Hartenberg của khâu thao tác đối với khâu 0 như
sau:
1 23 1 23 1 1 3 23 2 2 1
1 23 1 23 1 1 3 23 2 2 1
3
23 23 3 23 2 2
( )
( )
0
0 0 0 1
C C C S S C a C a C a
S C S S C S a C a C a
S C a S a S
− + +
− − + +
=
+
D
(1.8)
b. Xác định vị trí và hướng của bàn kẹp
Vị trí của khâu thao tác (bàn kẹp) được xác định trong hệ tọa độ cố định R
0
được xác
định bởi điểm E (điểm tác động cuối) và hướng của khâu thao tác. Theo (1.8) ta có:
1 3 23 2 2 1
1 3 23 2 2 1
3 23 2 2
( )
( )
E
C a C a C a
S a C a C a
a S a S
+ +
= + +
+
x
(1.9)
Ma trận cosin chỉ hướng (xác định hướng của bàn kẹp) xác định từ ma trận
3
T
như sau:
1 23 1 23 1
3 1 23 1 23 1
23 23
0
C C C S S
S C S S C
S C
−
= − −
A
(1.10)
Xác định hướng của bàn kẹp:
Sử dụng phép quay Roll-Pitch-Yaw trong phép quay hệ quy chiếu cố định
0
R
sang hệ
quy chiếu
n
R
thì ta có ma trận cosin chỉ hướng RPY, như sau:
10
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( )
cos cos cos sin sin sin cos cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
sin cos sin cos cos
φ θ φ θ ψ φ ψ φ θ ψ φ ψ
φ θ φ θ ψ φ ψ φ θ ψ φ ψ
θ θ ψ θ ψ
− +
+ −
−
=
R
Hình 1. 4. Phép quay Roll – Pitch – Yaw
Đồng nhất các phần tử của 2 ma trận A
3
và A, ta tìm được góc
φ
, hướng của bàn kẹp
như sau:
1 2 3
; ( );
2
π
φ q θ q q ψ= = − + =
c. Xác định vận tốc của điểm tác động cuối so với hệ cố định
( )
( )
1 1 3 23 2 2 1 1 3 23 2 3 2 2 2 1
0
0
1 1 3 23 2 2 1 1 3 23 2 3 2 2 2 1
3 23 2 3 2 2 2
0
. ( ) ( ( ) . )
. ( ) ( ( ) . )
( )
R
E E
S q a C a C a C a S q q a C q a
C q a C a C a S a S q q a C q a
a C q q a C q
d
dt
− + + + − + − +
= + + + − + − +
+ +
=v r
& & & &
& & & &
& & &
(1.11)
d. Vận tốc góc của mỗi khâu so với hệ cố định
1 2 1 2 1 3
1 2 1 2 3 1 2 1 3
1 1 1
0
0 ; ;
S q S q S q
ω ω C q ω C q C q
q q q
+
= = − = − −
& & &
& & &
& & &
(1.12)
Với
, 1 3
i
ω i =
lần lượt là vận tốc góc của khâu 1, 2, 3 so với hệ tọa độ cố định.
e. Các đồ thị thể hiện vị trí, vận tốc điểm tác động cuối E
Ta cho các số liệu đầu vào như sau:
11
1 2 3
0.4( ), 0.3( ), 0.3( )a m a m a m
= = =
Và giả sử các góc quay tại mỗi khớp là các hàm:
1 1 1
sin( ), sin( ), os( )
30 2 30
π π π
q t q t q c t= = =
Với kết quả (1.9) và (1.11), sử dụng phần mềm Matlab ta có các đồ thị như sau:
Hình 1. 5. Vị trí điểm tác động cuối
Hình 1. 6. Vận tốc điểm tác động cuối so với hệ cố định
12
1.2. Giải bài toán động học ngược
Bài
toán động học ngược có ý nghĩa đặc biệt quan trọng trong lập trình và điều khiển
chuyển động của robot. Bởi lẽ, trong thực tế thường phải điều khiển robot sao cho bàn kẹp
di chuyển tới các vị trí nhất định trong không gian
t
hao tác theo một quy luật nào đó. Ta
cần xác định các giá trị biến khớp tương ứng với vị trí và hướng của robot theo yêu cầu
đó. Đây cũng chính là nội dung của
bài toán động học ngược.
Từ bài toán thuận ta biết phương trình xác định vị trí bàn kẹp
( )
=x f q
. Bây giờ
giả sử x đã biết, cần tìm q một cách hình thức như sau
:
( )
-1
=q f x
92Equation Section (Next)1011Equation Chapter (Next) Section 1
Trong đó:112Equation Section (Next)
122Equation Section 2
1
[ ]
T
n
q q=q
là vectơ toạ độ suy rộng biến khớp133Equation Section
(Next)1412Equation Chapter (Next) Section 1152Equation Section 2
1
[ ]
T
n
q q=q
là vectơ toạ độ suy rộng của khâu thao tác (bàn kẹp).
V
ới n là số toạ độ suy rộng khớp (số bậc tự do của robot), m là số toạ độ suy rộng
của bàn kẹp ( m =
6
)
Có 3 trường hợp xảy ra
:
- Khi
m = n
, robot có cấu trúc động học cân bằng hay cấu trúc
chuẩn.
Phương trình (2.1) có thể có nghiệm duy nhất phụ thuộc vào cấu trúc của robot.
- Khi m < n , robot có cấu trúc dư dẫn động. Số toạ độ suy rộng khớp q
i
lớn hơn số
tọa độ suy rộng khâu thao tác x
j
cần xác định. Bài toán có nhiều nghiệm, để giải bài toán có
thể đưa thêm vào các điều kiện phụ như là: điều kiện về công nghệ, điều kiện về cơ học, các
điều kiện về toán học…để đưa bài toán về bài toán cấu trúc động học cân bằng, giải bài toán
bằng phương pháp ma trận tựa nghịch đảo, trong đó số phương trình nhỏ hơn số ẩn.
- Khi m > n , robot có số toạ độ suy rộng khớp ít hơn số toạ độ suy rộng khâu thao
tác, phương trình (2.1) không giải được. Để bài toán có nghiệm, ta cần đưa thêm vào các
điều kiện ràng buộc, tuy nhiên đây là bài toán không có nhiều ý nghĩa trong thực
t
ế.
Các phương pháp giải bài toán động học
n
g
ư
ợc
13
Việc đi tìm nghiệm của bài toán động học ngược có ý nghĩa rất quan trọng trong lập
trình và điều khiển robot. Tuy nhiên, việc này khá khó khăn và h
i
ện chưa có phương pháp
tổng quát nào để giải quyết vấn đề này một cách thật h
i
ệu quả. Có hai nhóm phương pháp
hay được sử dụng là
:
- Nhóm phương pháp giải tích
- Nhóm phương pháp số
1.2.1. Giải bài toán động học ngược bằng phương pháp giải tích
a. Cơ sở lý thuyết
Khi giải bài toán động học thuận bằng phương pháp ma trận Denavit-Hartenberg ta có
ma trận biến đổi xác định vị trí thao tác là:
(0)
1 2 3 1
( ) ( )
( ) ( )
1
n E
n n n n
T
−
= = =
A q r q
T q D q H H H H H
0
(1.13)
Từ đó ta xác định được ma trận cosin chỉ hướng khâu thao tác và vector điểm thao tác E
là các hàm của tọa độ suy rộng.
Mặt khác, từ nhiệm vụ công nghệ của robot ta có ma trận cấu hình khâu thao tác (ma
trận cosin chỉ hướng của khâu thao tác và vector xác định vị trí điểm thao tác) dưới dạng hàm
của các tọa độ thao tác:
( )
0 0 0 1
x x x x
y y y y
n
z z z z
n s a p
n s a p
n s a p
=
T x
(1.14)
Từ đó ta có phương trình ma trận:
( ) ( )
n n
=T q T x
(1.15)
Từ phương trình (1.15) sử dụng các phương pháp đại số và hình học ta có thể tìm được
các hàm xác định tọa độ khớp
( )
-1
=q f x
.
b. Áp dụng giải bài toán ngược cho robot RRR
Sử dụng kết quả (1.8) ta có ma trận Denavit-Hartenberg của khâu thao tác đối với khâu
0 như sau:
14
1 23 1 23 1 1 3 23 2 2 1
1 23 1 23 1 1 3 23 2 2 1
3 1 2 3
23 23 3 23 2 2
( )
( )
0
0 0 0 1
C C C S S C a C a C a
S C S S C S a C a C a
S C a S a S
− + +
− − + +
= =
+
D H H H
(1.16)
Cho các phần tử (1,4) và (2,4) của hai ma trận (1.14) và (1.16) bằng nhau ta được:
1
1
1
1
tan( )
atan2( , )
y
x
y x
P
S
q
C P
q P P
= =
⇒ =
(1.17)
Từ phương trình ma trận (1.16) ta suy ra:
( )
1
1 2 3 3
−
=
H H D H
Ta có:
( )
3 3 3 3 3
1
3 3 3 3 3
3 3
3 3 3 3 3
0 0 0 1
x x x x x x x x
y y y y y y y y
z z z z z z z z
C n S s S n C s a a n a p
C n S s S n C s a a n a p
C n S s S n C s a a n a p
−
− + − +
− + − +
=
− + − +
D H
(1.18)
1 2 1 2 1 1 2 2 1
1 2 1 2 1 1 2 2 1
1 2
2 2 2 2
( )
( )
0
0 0 0 1
C C C S S C a C a
S C S S C S a C a
S C a S
− +
− − +
=
H H
(1.19)
So sánh các phần tử (3,4) và (2,4) của hai ma trân (1.18) và (1.19) ta có:
3
2
2
z z z
a n a p
S
a
− +
=
(1.20)
3 1 1
2
2
y y y
a n a p a S
C
a
− + −
=
(1.21)
2 2 2
atan2( , )q S C⇒ =
(1.22)
So sánh các phần tử (3,1) và (3,2) của hai ma trận (1.14) và (1.16) ta có:
23 23
;
z z
S n C s
= =
(1.23)
Suy ra:
2 3 23 23
atan2(S ,C )q q+ =
(1.24)
Vậy
3 23 23 2
atan2(S ,C )q q= −
(1.25)
15
Sử dụng các góc Roll-Pitch-Yall, với các thông số hướng của khâu thao tác cuối như
sau:
; ;
6 3 3
π π π
φ θ ψ= = =
và khâu thao tác cuối di chuyển theo các phương trình
1+0.15(1+3sin3t)cost; 0.3(1+3sin3t)sin(t); =0.3+0.2cost
x y z
p p p= =
. Từ các phương trình
(1.17), (1.22) và (1.25) ta vẽ đồ thị gồm góc quay, tốc đọ góc và gia tốc góc tại mỗi khớp như
sau:
0
0.5
1
1.5
2
-2
-1
0
1
0.1
0.2
0.3
0.4
0.5
Phuong trinh diem thao tac cuoi trong khong gian
0.4 0.6 0.8 1 1.2 1.4 1.6 1.8
-1.2
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
Phuong trinh diem thao tac cuoi trong khong gian
Hình 1. 7. Chuyển động khâu thác cuối trong không gian (hình hoa ba cánh)
0 1 2 3 4 5 6 7 8 9 10
-4
-3
-2
-1
0
1
2
3
Thoi gian (s)
Goc quay (rad)
Do thi bien khop
q1
q2
q3
Hình 1. 8. Đồ thị góc quay các biến khớp
16
0 100 200 300 400 500 600 700 800 900 1000
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
Thoi gian (s)
Toc do goc (rad/s)
Van toc goc bien khop
dq1
dq2
dq3
Hình 1. 9. Đồ thị vận tốc góc quay các biến khớp
0 100 200 300 400 500 600 700 800 900 1000
-1.5
-1
-0.5
0
0.5
1
1.5
x 10
-3
Thoi gian (s)
Gia toc goc (rad/s
2
)
Gia toc goc bien khop
ddq1
ddq2
ddq3
Hình 1. 10. Đồ thị gia tốc góc của biến khớp
1.2.2. Giải bài toán ngược bằng phương pháp số
a. Cơ sở lý thuyết
Nhìn chung, các phương pháp số có thể giải được bài toán động học ngược một
cách tổng quát, tính tự động hoá cao. Tuy nhiên, trong thực tế, v
i
ệc tìm lời giải bằng
17
phương pháp này lại gặp khó khăn như thời gian tính toán lâu do gặp phải hệ phương
trình siêu việt, hoặc vì tính đa trị của lời giải,… Dướ
i
đây, ta sẽ trình bày các nội dung
chính để giải bài toán động học ngược của robot bằng phương pháp số.
Phương trình xác định vị trí bàn kẹp
( )=x f q
(1.26)
Đạo hàm hai vế phương trình (1.26) theo t, ta được
( )
∂
= =
∂
f
x q J q q
q
& &
&
(1.27)
trong đó
:’
1 1 1
1 2
1 2
( )
n
m m m
n
f f f
q q q
f f f
q q q
∂ ∂ ∂
∂ ∂ ∂
∂
= =
∂
∂ ∂ ∂
∂ ∂ ∂
f
J q
q
(1.28)
-Khi m=n
ta có bài toán có cấu trúc động học cân bằng.
x,
q
,
f R
n
.
Từ (1.27) ta suy ra:
( ) ( )
( )
( )
1
t t t
−
=
q J q x
&
&
(1.29)
Đạo hàm (1.27) theo t, ta được:
( ) ( ) ( ) ( ) ( )
t t t t t
= +
x J q q J q q
&
& &&
&&
(1.30)
Từ (1.30) suy ra
( ) ( ) ( ) ( ) ( )
t t t t t
= −
J q q x J q q
&
&& &
&&
(1.31)
Từ
(1.31) suy ra
ta được
:
( ) ( ) ( ) ( ) ( )
1
t t t t t
−
= −
q J q x J q q
&
&& &
&&
(1.32)
Công thức (1.29) và (1.32) cho phép ta xác định được vectơ vận tốc suy rộng gia tốc
suy rộng khi b
i
ế
t
( )
tq
tại thời điểm khảo sát và các quy luật
( ) ( ) ( )
, ,t t tx x x
& &&
.
Giả sử robot làm việc trong khoảng thời gian [0,T]. Ta chia khoảng thời gian đó thành
n phần bằng nhau.
18
h=
→
=T/=T/h
Ta có:
( )
*
k+1 k
t =t +Δt k=0,1, ,n
Áp
dụng khai triển Taylor hàm vectơ
q
t
ở lân cận giá tr
ị
t =t
k
, ta có
( ) ( ) ( ) ( ) ( ) ( )
2
k+1 k k k k
1
q t =q t +Δt =q t +q t Δt+ q t Δt
2
+
Bỏ qua các vô cùng bé bậc hai ta có:
( ) ( ) ( )
( )
( )
1k k
t t t t t
+
≈ + ∆
-1
q q J q x
&
với (k=0,1,…,n
*
) (1.33)
Nhận xét:
Tính q(t
k+1
) theo (1.33) ta nhận được giá trị thô của q(t) tại thởi điểm t=t
k
, vì vậy cần
nâng cao độ chính xác của
( )
1k
t
+
q
bằng việc tính q0.
Thuật toán xác
đị
nh
( )
0 0
t=q q
Ký hiệu t
0
=0 là thời điểm ban đầu.
Ta có thể xác định vectơ gần đúng ban đầu
0
q
%
của
vectơ
0
q
bằng phương pháp vẽ hoặc thực nghiệm. Áp dụng khai triển Taylor tìm nghiệm
0
q
gần đúng như sau:
( )
0 0 0 0
t
+
= = ∆q q q q
%
(1.34)
Theo phương trình (1.34) ta có:
( ) ( ) ( ) ( ) ( )
0 0 0 0 0 0 0
t
+
∂
= = ∆ = + ∆
∂
f
x f q f q q f q q q
q
% %
Từ đó, suy ra các công thức gần đúng:
( ) ( )
0 0 0 0
( )t∆ ≈ −J q q x f q
% %
(1.35)
Giải phương trình gần đúng (1.35) ta có:
( ) ( )
1
0 0 0 0
( )t
−
∆ = −
q J q x f q
% %
(1.36)
Lấy
0 0 0
+
= ∆
q q q
% %
(1.37)
19
Nếu
0 1
ε∆ ≥q
thì ta lại thế phương trình (1.37) vào (1.34) rồi giải nó. Quy trình này
dừng lại khi
0 1
ε∆ ≤q
. Trong đó là sai số dương bé chọn trước. Kết quả của quá trình này
ta được
0 0
=
q q
%
(1.38)
Biết được
0
q
thay vào (1.29) và (1.32) để tính
0
q
&
,
0
q
&&
.
b. Áp dụng giải bài toán cho robot RRR
Sử dụng phương pháp lặp Newton-Raphson ta tìm giá trị của vectơ
0
q
. Xem chương
trình code viết trên MATLAB ở phần phụ lục.
Lấy phương trình điểm tác động cuối như bài toán giải bằng phương pháp giải tích, ta
có kết quả góc quay của các tọa độ suy rộng như sau:
0 1 2 3 4 5 6 7 8 9 10
-10
-5
0
5
10
15
t (s)
q (rad)
Do thi goc quay cac bien khop
q1
q2
q3
Hình 1. 11. Đồ thị mối quan hệ giữa các tọa độ suy rộng với thời gian
20
CHƯƠNG II. GIẢI BÀI TOÁN TĨNH HỌC ROBOT
2.1. Cơ sở lý thuyết
Cho trước các thông số:
Coi các khâu là thanh đồng chất , tiết diện ngang không đáng kể, khối lượng các khâu là
m
1
, m
2
, m
3
. Cho lực tác dụng vào khâu thao tác tại điểm E,
0
F = [ F
x
, F
y
, F
z
]
T
. Chọn một vị trí
của robot theo bài toán động học:
1
2
3
70 0 15
90 ( ); 0 ( ); 14.5 ( ).
60 0 13
x
E y E
z
F m
F F N M Nm m m kg
F m
= = = = =
(2.1)
Cho lực tác động vào khâu thao tác tại điểm E gồm véc tơ lực , và momen , tính lực (mô
men) tác động vào các khớp đảm bảo Robot cân bằng tĩnh.
Xét khâu thứ i:
Ta có hệ phương trình cân bằng lực:
, 1 1,
, 1 1, , 1
i i i i i
i i i i i i i Ci i
F F P
M M r F r P
− +
− + −
= −
= − × − ×
ur ur ur
uur uur r ur r ur
(2.2)
Dạng ma trận trong hệ tọa độ cơ sở:
0 0 0
, 1 1,
0 0 0 0 0 0
, 1 1, , 1
i i i i i
i i i i i i i Ci i
F F P
M M r F r P
− +
− + −
= −
= − −
% %
(2.3)
Dạng ma trận trong hệ tọa độ khâu i:
, 1 1,
, 1 1, , 1
i i i
i i i i i
i i i i i i
i i i i i i i Ci i
F F P
M M r F r P
− +
− + −
= −
= − −
% %
(2.4)
Trong đó:
,i j
F
ur
: là lực do khâu j tác dụng lên khâu i.
,i j
M
uur
: là momen do khâu j tác dụng lên khâu i.
i
P
ur
: là trọng lực tác dụng lên khâu i.
i
r
r
: vector
1i i
O O
−
uuuuuur
.
Ci
r
r
: vector
i i
O C
uuuur
21
2.2. Áp dụng cho Robot RRR
• Xét khâu thứ 3:
[ ]
0 0
3,4
0 0
3,4
3
3 3
3
3
3
, ,
, ,
,0,0
,0,0
2
T
E x y z
T
E x y z
T
T
C
F F F F F
M M M M M
r a
a
r
= =
= =
= −
= −
[ ]
0
3 3
0 0
T
P m g
= −
Theo (2.3) ta có:
0 0 0
3,2 3,4 3
0 0 0 0 0 0
3,2 3,4 3 3,2 3 3C
F F P
M M r F r P
= −
= − −
% %
0
3,2
3 3
0 0 0
3,2 3 3
3 3
0
0
0
0
x x
y y
z z
x x
y y c
z z
F F
F F F
F m g F m g
M F
M M r F r
M F m g m g
= − =
− +
⇒
= − −
+ −
% %
(2.5)
Trong đó :
1 23 1 23 1 3
0 0 3
3 3 3 1 23 1 23 1
23 23
3 1 23
3 1 23
3 23
0
0 0
C C C S S a
r R r S C S S C
S C
a C C
a S C
a S
− −
= = − −
−
= −
−
3 23 3 1 23
0
3 3 23 3 1 23
3 1 23 3 1 23
0
0
0
a S a S C
r a S a C C
a S C a C C
−
⇒ = −
−
%
(2.6)
22
3
1 23 1 23 1
0 0 3
3 3 3 1 23 1 23 1
23 23
3
1 23
3
1 23
3
23
2
0
0 0
2
2
2
C C
a
C C C S S
r R r S C S S C
S C
a
C C
a
S C
a
S
−
−
= = − −
−
−
=
−
3 3
23 1 23
0
3 3
3 23 1 23
3 3
1 23 1 23
0
2 2
0
2 2
0
2 2
c
a a
S S C
a a
r S C C
a a
S C C C
−
⇒ = −
−
%
(2.7)
Thay (2.6) và (2.7) vào (2.5) thay các thông số cho trước ta được:
0
3,2 3
0
3,2
( )
( )
T
x y z
F F F F m g N
M Nm
= +
• Xét khâu thứ 2
Xét khâu thứ 2:
[ ]
0
2 2
0 0
T
P m g= −
[ ]
2
2 2
2
2
2
,0,0
,0,0
2
T
T
C
r a
a
r
= −
= −
Theo (2.3) ta có:
0 0 0
2,1 3,2 2
0 0 0 0 0 0
2,1 3,2 2 2,1 2 2c
F F P
M M r F r P
= −
= − −
% %
(2.8)
Trong đó :
23
1 2 1 2 1 2 2 1 2
0 0 2
2 2 2 1 2 1 2 1 2 1 2
2 2 2 2
0
0 0
C C C S S a a C C
r R r C S S S C a C S
S C a S
− − − −
= = − − = −
−
2 2 2 1 2
0
2 2 2 2 1 2
2 1 2 2 1 2
0
0
0
a S a C S
r a S a C C
a C S a C C
−
⇒ = −
−
%
(2.9)
2
2 1 2
1 2 1 2 1
0 0 2
2
2 2 2 1 2 1 2 1 1 2
2 2
2
2
2
2
0
2
0 0
2
C C
a
a C C
C C C S S
a
r R r C S S S C C S
S C
a
S
−
−
− −
−
= = − − =
−
2 2
2 1 2
0
2 2
2 2 1 2
2 2
1 2 1 2
0
2 2
0
2 2
0
2 2
C
a a
S C S
a a
r S C C
a a
C S C C
−
−
⇒ =
− −
%
(2.10)
Ta thay kết quả (2.9) và (2.10) vào (2.8) ta được:
0
2,1 3 2
( )
T
x y z
F F F F m g m g N
= + +
0
2,1
( )M Nm
• Xét khâu thứ 1:
[ ]
1
1 1
1
1
1
,0,0
,0,0
2
T
T
C
r a
a
r
= −
= −
[ ]
0
1 3
0,0, g
T
P m
= −
Theo (2.3) ta có:
0 0 0
1,0 2,1 1
0 0 0 0 0 0
1,0 2,1 1 1,0 1 1c
F F P
M M r F r P
= −
= − −
% %
(2.11)
Trong đó :
24
1 1 1 1 1
0 0 1
1 1 1 1 1 1 1
0
0 0
0 1 0 0 0
C S a a C
r R r S C a S
− −
= = − = −
Suy ra:
1 1
0
1 1 1
1 1 1 1
0 0
0 0
0
a S
r a C
a S a C
−
=
−
%
(2.12)
1
1 1
1 1
0 0 1
1
1 1 1 1 1 1
2
0
2
0 0
2
0 1 0 0
0
c c
a
a C
C S
a
r R r S C S
− −
= = − = −
Suy ra:
1
1
0
1
1 1
1 1
1 1
0 0
2
0 0
2
0
2 2
c
a
S
a
r C
a a
S C
−
=
−
%
(2.13)
0
2,1 3 2 1
( )
T
x y z
F F F F m g m g m g N
= + + +
0
10
( )M Nm
Giả sử với các phương trình biến khớp như đã biết như ở bài toán thuận, và các số liệu
đầu vào của bài toán tĩnh học như (2.1), ta tính độ lớn lực và vẽ đồ thị moment tác động lên
các khớp như sau:
0
3,2
0
2,1
0
1,0
[70 90 187.53]
[70 90 329.775]
[70 90 476.925]
T
T
T
F
F
F
=
=
=
25
