Tài liệu Báo cáo Tính toán động học và mô phỏng 3D rôbốt Gryphon: Tính toán động học và mô phỏng 3D rôbốt Gryphon
Đinh Văn Phong *, Đỗ Sanh *, Nguyễn Trọng Thuần *, Đỗ Đăng Khoa **
*Tr−ờng Đại học Bách Khoa Hà nội, **Đề tài 3.312.01
Tóm tắt: Bài báo đề cập đến bài toán điều khiển động học của rôbốt và áp dụng vào
rôbốt Gryphon. Nội dung của bài báo nhằm giải quyết bài toán thuận, bài toán ng−ợc và mô
phỏng hoạt động của rô bốt trong không gian đồ hoạ ba chiều. Đồng thời trong bài báo
cũng giới thiệu h−ớng nghiên cứu việc điều khiển rô bốt Gryphon bằng máy tính.
1. Mở đầu
Trong bài báo này khảo sát bài toán điều khiển động học rô bốt, chủ yếu tập trung
vào hai bài toán :
• Bài toán thuận : xác định vị trí điểm cuối và h−ớng tay kẹp mà tay máy đạt đ−ợc khi qui
luật thay đổi theo thời gian của các thông số định vị của tay máy là hàm đã biết. Bài
toán này nhằm phục vụ bài toán xác định phạm vi hoạt động của tay máy, bài toán
thuận trong động lực học tay máy….
• Bài toán ng−ợc : xác định qui luật thay đổi theo thời gian của ta...
10 trang |
Chia sẻ: haohao | Lượt xem: 1332 | Lượt tải: 0
Bạn đang xem nội dung tài liệu Báo cáo Tính toán động học và mô phỏng 3D rôbốt Gryphon, để tải tài liệu về máy bạn click vào nút DOWNLOAD ở trên
Tính toán động học và mô phỏng 3D rôbốt Gryphon
Đinh Văn Phong *, Đỗ Sanh *, Nguyễn Trọng Thuần *, Đỗ Đăng Khoa **
*Tr−ờng Đại học Bách Khoa Hà nội, **Đề tài 3.312.01
Tóm tắt: Bài báo đề cập đến bài toán điều khiển động học của rôbốt và áp dụng vào
rôbốt Gryphon. Nội dung của bài báo nhằm giải quyết bài toán thuận, bài toán ng−ợc và mô
phỏng hoạt động của rô bốt trong không gian đồ hoạ ba chiều. Đồng thời trong bài báo
cũng giới thiệu h−ớng nghiên cứu việc điều khiển rô bốt Gryphon bằng máy tính.
1. Mở đầu
Trong bài báo này khảo sát bài toán điều khiển động học rô bốt, chủ yếu tập trung
vào hai bài toán :
• Bài toán thuận : xác định vị trí điểm cuối và h−ớng tay kẹp mà tay máy đạt đ−ợc khi qui
luật thay đổi theo thời gian của các thông số định vị của tay máy là hàm đã biết. Bài
toán này nhằm phục vụ bài toán xác định phạm vi hoạt động của tay máy, bài toán
thuận trong động lực học tay máy….
• Bài toán ng−ợc : xác định qui luật thay đổi theo thời gian của tay máy để nó nắm bắt
đ−ợc vị trí đã cho của đối t−ợng theo một h−ớng định tr−ớc của tay kẹp. Bài toán này
nhằm phục vụ các bài toán điều khiển quĩ đạo, các bài toán điều khiển tối −u...
Bài báo d−ới đây sẽ trình bày cơ sở lý thuyết của cả hai bài toán và minh hoạ qua việc áp
dụng để mô phỏng một rô bốt cụ thể.
2. Cơ sở lý thuyết.
2.1 Công thức xác định vị trí
Trong bài báo này chúng ta sẽ sử dụng ph−ơng pháp ma trận Denavit-Hartenberg.
gắn các hệ trục tọa độ vào từng khâu của tay máy (rôbốt). Nhờ đó mối quan hệ về toạ độ
của cùng một điểm trên hai hệ trục kế tiếp nhau sẽ đ−ợc xác định bằng một ma trận chuyển
4x4.,xem tr 53 [3].
Gọi jj-1H là ma trận chuyển toạ độ một điểm từ hệ j ( Oxjyjzj ) về hệ j-1 ( Oxj-1yj-1zj-1 ), ta
có:
cos( ) cos( ).sin( ) sin( ).sin( ) .cos( )
sin( ) cos( ).cos( ) -sin( ).cos( ) .sin( )
0 sin( ) cos( )
0 0 0 1
j j j j j k j
j j j j j k j
j j j
a
a
d
θ α θ α θ θ
θ α θ α θ θ
α α
=
j
j-1H (2.1)
trong đó các tham số có ý nghĩa nh− sau:
• θj là góc quay trục xj-1 đến trục xj quanh trục zj-1,
• dj là đoạn dịch trục xj-1 đến trục xj dọc trục zj-1,
• aj là đoạn dịch trục zj-1 đến trục zj dọc trục xj-1,
• αj là góc quay trục zj-1 đến trục zj quanh trục xj-1.
Thông qua việc sử dụng liên tiếp các hệ toạ độ, ma trận chuyển hệ toạ độ gắn vào
tay kẹp về hệ toạ độ gắn vào đế của rô bốt có dạng:
( ) ( ) ( ) ( )1 2 nq q qtay kep 1 2 n nde 0 1 n-1 0H = H .H ....H = H q
( )n0H q có dạng: ( ) ( )1
R q p q
0
(2.2)
trong đó hệ toạ độ đế rô bốt kí hiệu là 0, hệ toạ độ tay kẹp rô bốt kí hiệu là n, q là ma trận
nx1 của các toạ độ suy rộng,chúng là các thông số định vị của rôbốt. R(q) là ma trận 3x3
xác định h−ớng của tay kẹp, p(q) là vectơ 3x1 xác định vị trí đầu bàn kẹp so với hệ đế. Ba
cột của ma trận R t−ơng ứng với h−ớng của ba vectơ đơn vị trên hệ gắn với tay kẹp so với
hệ đế rôbốt (hệ toạ độ nền).
Vị trí của một điểm P thuộc tay kẹp đ−ợc xác định theo công thức :
( )0n ′p pr = H q r (2.3)
trong đó pr là vectơ định vị điểm P thuộc tay kẹp so với hệ toạ độ nền, ′pr là vectơ định vị
điểm P trong hệ toạ độ gắn vào tay kẹp rôbốt.
2.2 Công thức xác định vận tốc.
Từ công thức (2.3) ta dễ dàng xác định đ−ợc vận tốc điểm P gắn vào tay kẹp rôbốt
so với hệ toạ độ nền :
′p p n p0v = r = H .r&& (2.4)
Để tiến hành điều khiển hoạt động của từng khâu rôbốt, ta tiến hành biểu diễn ma
trận n0H& d−ới dạng sau :
∑nn (i)0 i
i=1
H = H q& & (2.5)
trong đó H(i) là ma trận ứng với khớp i và chỉ phụ thuộc vào biến khớp, pr& là véc tơ vận tốc
của điểm cuối tay kẹp so với hệ toạ độ nền, n0H& là ma trận đạo hàm của ma trận Denavit-
Khâu j-2
Khâu j-1 Khâu j Khâu j+1
zj-2
dj zj-1 yj-1
xj-1
θj
xj
yj
zj
aj
aj-1
xj
Hình 1
zj
Khớp j-1
Khớp j Khớp j+1
αj
Hartenberg giữa hệ toạ độ nền với hệ toạ độ gắn vào tay kẹp, iq& là vận tốc của chuyển
động tại khớp thứ i.
3. Khảo sát động học của rô bốt Gryphon.
3.1 Giới thiệu mô hình và nguyên tắc hoạt động.
Rô bốt Gryphon do hãng FeedBack của Anh sản xuất phục vụ cho mục đích nghiên
cứu. Đây là một rôbốt năm trục kèm theo bàn kẹp nh− hình 2
• Trục hông (waist axis).
• Trục vai (shoulder axis).
• Trục khuỷu tay (elbow axis).
• Trục cổ tay (tool pitch axis).
• Trục cổ tay (tool roll axis).
• Bàn kẹp (gripper).
Hình 2
Những −u điểm nổi bật của rôbốt là chuyển động nhanh, chính xác và mềm mại.
Rôbốt đ−ợc điều khiển bởi bốn vi xử lý cho phép điều khiển đặt vật chính xác. Mỗi trục của
rôbốt đ−ợc điều khiển bởi một động cơ b−ớc với bộ mã hoá phản hồi. Trong bộ điều khiển,
một vi xử lý sẽ giám sát vị trí của các trục. Hai cái khác sẽ quản lý các động cơ và cái còn
lại sẽ giám sát cả ba cái trên đồng thời làm nhiệm vụ giao tiếp với máy chủ.
3.2 Công thức xác định vị trí
Khi áp dụng ph−ơng pháp Denavit-Hartenberg gắn các hệ trục toạ độ vào các khâu
x0
y0 z0
a1
θ1 z1
x1
y1
a2 a4 a5 a6
z5
y5
x5
a3
x4
y4
z4
z3
x3
y3
z2
θ4
θ5
θ2
x2
y2
θ3
Hình 3
ta thu đ−ợc sơ đồ động học của rôbốt Gryphon nh− hình 3.
• Bảng tham số Denavit-Hartenberg:
Theo thuật toán Denavit-Hartenberg ta có bảng tham số Denavit-Hartenberg ứng
với sơ đồ động học trên :
Trục θ D α a
1 θ1 a1 π/2 a2
2 θ2 0 0 a4
3 θ3 0 0 a5
4 θ4 -a3 π/2 0
5 θ5 a6 0 0
• Xác định các ma trận chuyền.
Đây là các ma trận chuyển thành chuyển các hệ toạ độ trên sơ đồ động học (hình 3).
1 1 2 1
1 1 2 1
1
0
0
0 1 0
0 0 0 1
C S a C
S C a S
a
− =
1
0H ,
2 2 4 2
2 2 4 2
0
0
0 0 1 0
0 0 0 1
C S a C
S C a S
− =
2
1H ,
3 3 5 3
3 3 5 3
0
0
0 0 1 0
0 0 0 1
C S a C
S C a S
− =
3
2H
,
4 4
4 4
3
0 0
0 0
0 1 0
0 0 0 1
C S
S C
a
− = −
4
3H ,
5 5
5 5
6
0 0
0 0
0 0 1
0 0 0 1
C S
S C
a
− =
5
4H (3.1)
• Ma trận chuyển toàn hệ
taykep
deH là ma trận truyền thuần nhất giữa hệ toạ độ nền và hệ toạ độ gắn vào tay kẹp.
taykep 1 2 3 4 5
de 0 1 2 3 4H = H .H .H .H .H
( )
( )
1 234 5 1 5 1 234 5 1 5 1 234 1 234 6 23 5 2 4 2 1 3
1 234 5 1 5 1 234 5 1 5 1 234 1 234 6 23 5 2 4 2 1 3
234 5 234 5 234 234 6 23 5 2 4 1
0 0 0 1
C C C S S C C S S C C S C S a C a C a a S a
S C C C S S C S C C S S S S a C a C a a C a
S C S S C C a S a S a a
+ − + + + + − − − − + + + + = − − − + + +
taykep
deH
(3.2).
Để viết đơn giản ta sử dụng các kí hiệu sau: C1=cos(θ1), C234=cos(θ2+θ3+θ4), S1=sin(θ1),
S234=sin(θ2+θ3+θ4)….
• Ma trận R
1 234 5 1 5 1 234 5 1 5 1 234
1 234 5 1 5 1 234 5 1 5 1 234
234 5 234 5 234
C C C S S C C S S C C S
S C C C S S C S C C S S
S C S S C
+ − + = − − − − −
R (3.3)
• Vectơ p
( )
( )
1 234 6 23 5 2 4 2 1 3
1 234 6 23 5 2 4 2 1 3
234 6 23 5 2 4 1
C S a C a C a a S a
S S a C a C a a C a
C a S a S a a
+ + + − = + + + + − + + +
p (3.4)
Xem [1].
3.3 Bài toán thuận
Ta có thể xác định vị trí và vận tốc nh− sau:
Trong bài toán thuận ( )n0H q từ (2.3) là hàm đã biết của thời gian, do đó ta tính
đ−ợc ( )t=p pr r
Trong công thức (2.5) vì ( ) ( ),t t= =q q q q& & do đó (i)H là hàm đã biết của t, từ (2.4)
ta tính đ−ợc ( )tp pv = v .
3.4 Bài toán ng−ợc
3.4.1 Bài toán vị trí
Mục đích bài toán nhằm xác định các góc ( 1,5)i iθ = để tay máy nắm bắt đ−ợc đối
t−ợng tại một vị trí đã cho và theo một h−ớng xác định. Do đó dữ liệu vào là vị trí đầu bàn
kẹp
T
x y zp p p = Tp và h−ớng tay kẹp R.
Từ các công thức (3.3) và (3.4) ta nhận thấy rằng cột thứ 3 của ma trận R và p
không phụ thuộc vào θ5 mà chỉ phụ thuộc vào các góc θ1, θ2, θ3, θ4 do đó các góc này sẽ
đ−ợc xác định theo vectơ cấu hình tay kẹp :
w=[px, py, pz, R13, R23, R33]. Nói cách khác ta thu đ−ợc hệ ph−ơng trình sau:
( )
( )
11 234 6 23 5 2 4 2 1 3
21 234 6 23 5 2 4 2 1 3
3234 6 23 5 2 4 1
4 131 234
5 231 234
6 33234
w
w
w
w
w
w
w
x
y
z
pC S a C a C a a S a
pS S a C a C a a C a
pC a S a S a a
RC S
RS S
RC
+ + + − + + + + − + + += = = −
(3.5)
thực hiện các phép biến đổi giải tích, ta thu đ−ợc kết quả sau:
• Góc quay ở khớp hông θ1 đ−ợc tính nh− sau
Đặt 2 2 21 2 3w wb a= ± + −
1 2 1 3 1 2 3atan2(w b-w a ,w b+w a )q⇒ = (3.6)
• Góc quay ở khớp khuỷu θ3 đựoc tính nh− sau:
( ) ( )1 1 1 3 1 2 1 3 1 234 6 2
1 1 2 1 234 6 2
2 3 1 234 6
w w
w w
w
b S a C C a S S a a
C S S a a
b a C a
= + + − − −
= + − −
= − +
với 234 1 4 1 5 6atan2( ,-w )q C w S w= +
2 2 2 2
1 2 4 5
3
4 52
b b a aq
a a
+ − −⇒ = ± (3.7)
• Góc quay ở khớp vai θ2 đ−ợc tính nh− sau:
( ) ( ) ( ) ( )( )2 2 4 5 3 1 5 3 1 4 5 3 2 5 3atan2 ,q b a a C b a S b a a C b a S= + − + + (3.8)
• Góc quay ở khớp pitch θ4 đ−ợc tính nh− sau:
4 234 2 3q q q q= − − (3.9)
• Góc quay ở khớp roll θ5 đ−ợc tính nh− sau:
( )5 1 11 1 21 1 12 1 22atan2 ,q S R C R S R C R= − − (3.10)
Xem [1].
3.4.2 Bài toán vận tốc.
Khi cần di chuyển vật kẹp với một vận tốc cho tr−ớc, ta phải tính vận tốc quay tại
các khớp nhờ các công thức (2.4) và (2.5). Muốn vậy ta phải tính đ−ợc các ma trận (i)H
trong (2.5). Đối với rôbốt Gryphon ta nhận đ−ợc:
5 234 1 1 5 1 5 234 1 5 1 234 1 6 234 5 23 4 2 2 1 3
1 5 234 1 5 1 5 234 1 5 1 234 1 6 234 5 23 4 2 2 1 3
( )
( )
0 0 0 0
0 0 0 0
C C S C S S S C C C S S S a S a C a C a C a
C C C S S C S C S C C C C a S a C a C a S a
− + + − − + + + − + − + + + + − =
(1)H
5 234 1 1 5 234 1 234 1 6 234 5 23 4 2
1 5 234 1 5 234 1 234 1 6 234 5 23 4 2
5 234 5 234 234 234 6 23 5 2 4
( )
( )
0 0 0 0
C S C C S S C C C a C a S a S
S C S S S S S C S a C a S a S
C C S C S S a C a C a
− − − − − − = − + +
(2)H
5 234 1 1 5 234 1 234 1 6 234 5 23
1 5 234 1 5 234 1 234 1 6 234 5 23
5 234 5 234 234 234 6 23 5
( )
( )
0 0 0 0
C S C C S S C C C a C a S
S C S S S S S C S a C a S
C C S C S S a C a
− − − − = − +
(3)H
5 234 1 1 5 234 1 234 1 6 234
1 5 234 1 5 234 1 234 1 6 234
5 234 5 234 234 234 6
0 0 0 0
C S C C S S C C C a C
S C S S S S S C S a C
C C S C S S a
− − = −
(4)H
5 234 1 1 5 1 5 234 1 5
1 5 234 1 5 1 5 234 1 5
5 234 5 234
0 0
0 0
0 0
0 0 0 0
S C C S C C C C S S
S S C C C S C C C S
S S C S
− + − − − − + = − −
(2)H (3.11)
Sử dụng (3.11) có thể giải quyết bài toán điều khiển vận tốc. Tuy nhiên vấn đề này
sẽ đề cập trong một công trình khác.
4. Ch−ơng trình mô phỏng.
4.1 Giới thiệu ch−ơng trình.
Đây là ch−ơng trình máy tính viết bằng ngôn ngữ C++ sử dụng công cụ lập trình
Visual C++ 6.0 có sự hỗ trợ của th− viện đồ hoạ OpenGL. Ch−ơng trình này làm nhiệm vụ
giải bài toán động học thuận và bài toán động học ng−ợc của rôbốt Gryphon và thể hiện
chuyển động của rôbốt theo các bộ số liệu góc khớp.
4.2 Th− viện đồ hoạ OpenGL.
The Open Graphics Library (OpenGL) là một th− viện đồ hoạ tốc độ cao, độc lập
với các hệ thống giao diện của hệ điều hành. Th− viện này đ−ợc hãng Silicon Graphics Inc
phát triển cho các workstation đồ hoạ tốc độ cao từ năm 1982 d−ới cái tên IRIS GL.
Đây là một th− viện đồ hoạ có thể triển khai trên nhiều hệ thống khác nhau nh− :
Microsoft Windows 95/98/NT/2000. Đồng thời các ứng dụng đồ hoạ OpenGL cũng có thể
viết trên nhiều ngôn ngữ lập trình khác nhau nh− : C/C++, Fortran, Ada, Java…
Về cơ bản OpenGL là một th− viện gồm tập hợp của khoảng 150 hàm hỗ trợ các
thao tác nh− sau, xem [2], [8] :
• Thể hiện các đối t−ợng cơ bản nh− điểm, đ−ờng, đa giác, và từ đó có thể tạo ra những
đối t−ợng đồ hoạ phức tạp hơn nh− mặt cầu, mặt nón, mặt hình trụ...hoặc các đối t−ợng
do lập trình viên tạo ra;
• Quan sát đối t−ợng : các đối t−ợng sau khi đ−ợc vẽ ra có thể đ−ợc quan sát từ những
góc nhìn khác nhau thông qua các phép biến hình (transformation);
• Định màu sắc : các đối t−ợng có thể đ−ợc thể hiện với màu sắc bằng nhiều cách chỉ
định màu khác nhau: RGBA hay color-index...;
• Sử dụng ánh sáng : ánh sáng có thể đ−ợc sử dụng để tạo ra những cảnh "thật" từ những
đối t−ợng đã có. OpenGL cho phép sử dụng nhiều loại nguồn sáng khác nhau nh− phát
xạ (emitted), bao quanh(ambient), phân tán (diffuse) và phản chiếu (specular);
• Các kỹ thuật tăng chất l−ợng hiện thị ảnh nh− chống răng c−a (anti-aliasing), trộn màu
(blending), s−ơng khói trong ảnh (fog)...;
• Thao tác trên các ảnh bitmap : lập trình viên có thể "dán" các ảnh của cảnh thật lên trên
bề mặt các đối t−ợng tạo ra bằng OpenGL...;
4.3 Giao diện chính của ch−ơng trình mô phỏng.
Phần bên trái của giao diện là các nút điều khiển từng khớp của rô bốt, phần bên
phải là thể hiện hình ảnh ba chiều của rô bốt t−ơng ứng với sự điều khiển
4.4 Các menu điều khiển của ch−ơng trình.
4.5 Các thanh công cụ của ch−ơng trình.
• Thanh công cụ thay đổi vị trí vật
• Thanh công cụ thay đổi h−ớng nhìn vật
Bật tắt thanh công cụ chuẩn
Bật tắt thanh trạng thái
Bật tắt thanh công cụ thay đổi vị trí vật
Bật tắt thanh công cụ thay đổi h−ớng nhin vật
Bật tắt thanh công cụ đặt chế độ
Quay vật theo chiều d−ơng trục x
Quay vật theo chiều âm trục x
Quay vật theo chiều d−ơng trục y
Quay vật theo chiều âm trục y
Quay vật theo chiều d−ơng trục z
Quay vật theo chiều âm trục z
Tịnh tiến vật sang bên trái
Tịnh tiến vật sang bên phải
Tịnh tiến vật xuống d−ới
Tịnh tiến vật lên trên
Thu nhỏ vật
Phóng to vật
Đ−a vật về vị trí ban đầu
Thông tin về tác giả
Đặt giá trị b−ớc góc cho từng khớp
Đặt hiệu ứng ánh sáng
• Thanh công cụ thay chế độ điều khiển
5. Kết luận.
Bài báo đã trình bày ph−ơng pháp khảo sát bài toán điều khiển động học rôbốt và áp
dụng vào một ví dụ cụ thể ,rôbốt Gryphon cùng với một ch−ơng trình máy tính mô phỏng
sinh động dễ dùng. Bài báo cũng đặt cơ sở cho việc tính toán động lực học rôbốt. Công
trình này đã đ−ợc tài trợ của Hội đồng khoa học tự nhiên của Việt nam.
Summary:
Kinematic calculations and 3D simulation of the Gryphon robot.
In the paper, a method of investigating the problem of kinematic controlling a robot is
presented. The results obtained are applied to the Gryphon robot and used to construct a 3D
simulation software. The results are also basic to the dynamic simulation.
Tài liệu tham khảo
1. Đồ án tôt nghiệp :Tính toán động học và mô phỏng 3D rô bốt Gryphon, Đỗ
Đăng Khoa.
2. Báo cáo nội bộ: OpenGL và ứng dụng, Phan Mạnh Dần và nhóm nghiên cứu.
3. Fundamentals of Robotíc Analysis and Control, Robert. J. Schilling.
4. Lập trình Window dùng Visual C++ 5.0 và MFC, D−ơng Quang Thiện.
5. Computer Aided Kinematics and Dynamics of Mechanical Systems, Hang. EJ,
Volume 1:Basic method, Alyn and Bacon, New York (1989).
H−ớng nhìn từ trên xuống
H−ớng nhìn từ d−ới lên
H−ớng nhìn từ trái sang
H−ớng nhìn từ phải sang
H−ớng nhìn từ phía tr−ớc
H−ớng nhìn từ phía sau
H−ớng nhìn tây nam
H−ớng nhìn đông nam
H−ớng nhìn đông bắc
H−ớng nhìn tây bắc
Dừng chuyển động theo quá trình
Bắt đầu chuyển động theo quá trinh
Chế độ động học thuận rôbốt
Chế độ động học ng−ợc rôbốt
Thiết kế quĩ đạo rôbốt
6. Computer Aided Analysis of Mechanical Systems, Nikravesh P.E, Printice-Hall, New
Jersey.
7. Dynamics of Multibody Systems, Shabana, Wiley, New York.
8. OpenGL Programming Guide, Mason Woo, Jackie Neider, Tom Davis.
Các file đính kèm theo tài liệu này:
- Báo cáo Tính toán động học và mô phỏng 3D rôbốt Gryphon.pdf