Xây dựng Forward kinematics cho hệ Stewart trong một số trường hợp riêng

Nghiên cứu khoa học công nghệ

XÂY DỰNG FORWARD KINEMATICS CHO HỆ STEWART
TRONG MỘT SỐ TRƯỜNG HỢP RIÊNG
Trần Trung Kiên1*, Nguyễn Vũ1, Nguyễn Trung Kiên2,
Vũ Đức Tuấn1, Phạm Minh Tân1
Tóm tắt:

,

s
s

.
Từ khóa: FFT; FDT; Forward kinematics; Nền tảng động học nghịch Stewart.

1. ĐẶT VẤN ĐỀ
Những năm gần đây, tay máy song song kiểu Stewart đã được nghiên cứu và ứng dụng
trong nhiều lĩnh vực như: hệ cân bằng các khí tài quân sự, hệ mô phỏng chuyển động, gia
công cơ khí, giải phẫu trong y học, thiên văn học,… Tay máy song song kiểu Stewart có
những ưu điểm vượt trội so với tay máy nối tiếp như: độ cứng vững cao, khả năng chịu tải
trọng lớn, khả năng thay đổi vị trí và hướng linh hoạt.
Tuy nhiên, việc điều khiển hệ động c a hệ stewart theo các dạng dao động định trước
vẫn c n là vấn đề phức tạp, các thu t toán này c n chưa được công bố. V v y, việc ây
dựng orwart kinematics cho hệ stawart trong một số trư ng hợp riêng là hết sức cần
thiết. Các trư ng hợp riêng cần quan tâm trực tiếp đến các chế độ mô phỏng chuyển
động như:
- Khi gia tốc c a phương tiện thay đổi, lực tác động lên ngư i ngồi trên phương tiện sẽ
thay đổi theo. Đó là các lực theo hướng ngồi c a ngư i điều khiển, các lực này được tạo ra
bởi góc quay theo trục y, hay góc chúc, ngóc, khi đó, lực trọng trư ng sẽ có tác dụng như
lực tạo ra bởi gia tốc chuyển động thẳng.

- Khi góc hướng c a phương tiện thay đổi, góc quay theo trục z cũng thay đổi, khi đó,
cần tạo góc quay theo trục z để có thể thay đổi cảnh quan chung.
- Khi góc hướng c a phương tiện thay đổi, lực li tâm sẽ tác động lên ngư i điều khiển
theo hướng vuông góc với hướng ngồi, để tạo ra các lực này cần tạo góc quay theo trục ,
khi đó, lực trọng trư ng sẽ có tác dụng như lực li tâm trong chuyển động quay.
ài báo tr nh bày việc ây dựng orward kinematics cho hệ stewart trong trư ng hợp
riêng nêu trên, phương pháp ây dựng dựa trên tính toán các góc oay, nghiêng, chúc,
ngóc trong t ng th i điểm. Kết quả kiểm tra bằng mô phỏng.
2. BÀI TOÁN XÂY DỰNG FORWARD KINEMATICS CHO HỆ STEWART
TRONG MỘT SỐ TRƯỜNG HỢP RIÊNG
2.1. Lực tác động lên các chân ở trang thái ban đầu
211
Giả sử, tọa độ đầu cuối (End E ector) c a hệ Stewart-Gough Plaform (SGP), trong
không gian được biểu diễn ở dạng vector như sau: q   x

y z 

  .
T

Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020

91


Kỹ thuật Điều khiển – Tự động hóa

Trong đó, các chuyển động dài gồm chuyển động dọc (tịnh tiến theo trục ), nằm ngang
(tịnh tiến theo trục y), thẳng đứng (lên uống theo trục z) đối với hệ quy chiếu quán tính;
Các chuyển động góc được thể hiện bằng các góc Euler đó là các góc quay quanh trục

(roll: α), trục y (pitch: β) và trục z (yaw: γ).
Hệ tọa độ chân dẫn động c a robot song song là t p hợp các biến về chiều dài. Đối với
robot song song dạng SGP 6DOF - SPS được cấu tạo gồm 6 khớp trượt. Viết dưới dạng
véc tơ trong không gian khớp, ta có: L  l1 l2

l3

l4

l5

l6  .
T

ài toán động học ngược có nhiệm vụ t m véc tơ chiều dài Li t tọa độ đầu cuối q cho
trước. Đối với robot song song dạng SGP, hệ tọa độ c a mặt phẳng nền { } và hệ tọa độ
c a tấm chuyển động {P} sẽ được đặt tại tâm c a mặt phẳng nền và tấm chuyển động
(được thể hiện qua h nh 1). Khi đó, véc tơ tọa độ c a các khớp trên mặt phẳng nền { }
được biểu diễn:

Hình 1.

ắ

 rB cos i    Bix 


Bi   rB sin i     Biy 

  Biz 

0



i,

Pi.

(1)

i  B

khi i  1,3,5 ; i  i 1   B khi i  2,4,6.
3
2
Và trên mặt phẳng {P} được biểu diễn:
Với: i 

 rP cos i    Pix 


Pi   rP sin i     Piy 

  Piz 
0



(2)

i  P

khi i  1,3,5 ; i  i 1   P khi i  2,4,6.
3
2
Giả sử vị trí mong đợi c a End E ector trên tấm chuyển động so với gốc tọa độ gắn trên
tấm nền được biểu diễn bằng vector:
Với: i 

92

T. T. Kiên, …, P. M. Tân, “Xây dựng forward kinematics … một số trường hợp riêng.”


Nghiên cứu khoa học công nghệ
B

P   xd

yd

zd 

T

Ma tr n chuyển đổi Euler:

Trong đó, ta sử dụng c(.) và s(.) để thay cho cos(.) và sin(.) .
Khi đó, vector thể hiện độ dài chân dẫn động được ác định như sau:

Li  Pi  B P  Bi
Xét trong hệ tọa độ gắn trên tấm nền:
B

Li  B RT Pi  B P  B Bi

(3)

Do đó, chiều dài c a các chân dẫn động c a SGP được ác định là chuẩn Euclide c a
Li như sau:
li  Li  Lix  Liy  Liz

(4)

Như v y, nếu biết vị trí và hướng c a tấm chuyển động ta có thể tính toán được độ dài
c a các chân Li và chuẩn Euclide c a Li .
212 L
ở
Trạng thái ban đầu c a robot là trạng thái khi robot không chuyển động và vị trí c a các
chân robot ở vị trí trung gian. V trọng lượng c a các chân robot so với trọng lượng c a tấm
chuyển động là rất nhỏ, nên ở đây, ta chỉ ét đến trọng lượng c a tấm chuyển động.
Giả sử tấm chuyển động có khối lượng là m, khi đó, lực tác động lên tâm c a tấm
chuyển động sẽ là P=m.g.
Giả sử lực tác dụng lên các chân là (với
). Véc tơ lực sẽ có độ lớn là và
có hướng dọc theo các chân thứ i. Để ác định véc tơ lực ta thiết l p véc tơ đơn vị
dọc theo các trục i c a bệ. Véc tơ được ác định là:
(5)
Trong đó,
được tính theo công thức (3) và (4).

Khi đó, theo phương tr nh cân bằng về lực ta có:
(6)
Trong đó, là véc tơ trọng lực
.
Mô men quay tâm
c a tấm chuyển động gây ra bởi các lực

được tính là:

Trong đó, ký hiệu [] là tích có hướng c a 2 véc tơ;
là tâm c a hệ trục tọa độ c a tấm
chuyển động; là tọa độ c a các khớp nối trên tấm chuyển động. Do đó, véc tơ
chính là tọa độ c a các điểm được tính theo công thức 2.

Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020

93


Kỹ thuật Điều khiển – Tự động hóa

Như v y, theo phương tr nh cân bằng về mô men ta có:
(7)
T (6) và (7), ta được hệ 6 phương trình tuyến tính với 6 ẩn là các
hệ phương tr nh trên ta thiết l p ma tr n A.

. Để giải

(8)

Với: i=1,6 ta có:

T (6), (7), (8), ta có:
(9)
Với: Ma tr n A được cho bởi công thức (8), F là vec tơ lực tác dụng lên các trục
, PM là ma tr n tổng hợp lực và mô men
g,0,0,0) T. Kết quả được tính toán cho trư ng hợp robot song song (h nh 2) có các giá trị:
- rB: Bán kính v ng tr n tạo bởi các khớp nối trên mặt phẳng nền (= 1 m);
- rP: Bán kính v ng tr n tạo bởi các khớp nối trên tấm chuyển động (= 0.55 m);
- αB: Góc tạo bởi cặp khớp nối đối ứng trên mặt phẳng nền (= 90 độ);
- αP: Góc tạo bởi cặp khớp nối đối ứng trên tấm chuyển động (= 90 độ).

Hình 2. M
G
-Stewart.
Khối lượng m c a tấm chuyển động (=500kg) và vị trí cần ban đầu c a robot khi độ dài
các trục là 1,5 m, khi đó, khoảng giữa tâm c a tấm cơ sở và tâm c a tấm chuyển động
(theo trục Z) là 1.37845 m.

94

T. T. Kiên, …, P. M. Tân, “Xây dựng forward kinematics … một số trường hợp riêng.”


Nghiên cứu khoa học công nghệ

Theo công thức (9) ta tính được kết quả:
Nhận xét: Ở trạng thái ban đầu, lực tác động đều lên các chân và giá trị lực:

Giá trị 1.500 và 1.37845 theo thứ tự là độ dài chân và độ cao c a chân theo trục Z.

2.2. Tính toán lực tác động lên các trục
221

X

Như trư ng hợp robot ở trạng thái cân bằng, khi robot quay quanh trục X ta cũng sử
dụng 3 phương tr nh cân bằng về lực và 3 phương tr nh cân bằng về mô men để tính các
đại lượng về lực tác động lên các chân.
Giả sử, robot quay quanh trục X với một góc ω. Khi đó, ma tr n chuyển đổi Euler được
ác định là:

T các phương tr nh về động học, ta ác định được véc tơ các chân và độ dài c a chúng:
B

Li  B RT Pi  B P  B Bi

li  Li  Lix  Liy  Liz

Xác định các véc tơ

c a các trục theo công thức (5):

T đó, ta thiết l p được 3 phương tr nh cần bằng về lực theo công thức (6).
(10)
Trong đó,

là véc tơ trọng lực

.

Để ác định 3 phương tr nh cân bằng về mô men. Ta cần chuyển các véc tơ lực về hệ
trục tọa độ c a bệ chuyển động.
Ma tr n chuyển đổi Euler t hệ tọa độ cơ sở về hệ trục tọa độ c a tấm chuyển động là:

Do đó, các lực

Với véc tơ

trong hệ trục tọa độ c a bệ chuyển động được tính là:

được tính theo công thức:

Mô men quay tâm

c a tấm chuyển động gây ra bởi các lực

được tính là:

Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020

95


Kỹ thuật Điều khiển – Tự động hóa

Như v y, theo phương tr nh cân bằng về mô men ta có:
(11)
Trong đó, là giá trị mô men quán tính c a tấm chuyển động quay quanh trục X; là
gia tốc quay quanh trục X. Tấm chuyển động được ác định là tấm tr n, mỏng và mô men
quán tính c a nó được tính là:

T (10) và (11), ta được hệ 6 phương trính tuyến tính với 6 ẩn là các
giải hệ phương tr nh trên ta thiết l p ma tr n A.

. Để

(12)

Với i = 1.6, ta có:

T (10), (11), (12), ta có:
(13)
Với:
- Ma tr n A được cho bởi công thức (12);
- F là vec tơ lực tác dụng lên các trục:
- PM là ma tr n tổng hợp lực và mô men:
222

Y

Tương tự như quay quanh trục X, khi quay quanh trục Y ta cần giải hệ phương tr nh:
Trong đó, ma tr n A tính theo công thức (12),
,
,
và véc tơ lực tác dụng lên
các trục F đã cho ở trên th PM là ma tr n tổng hợp lực và mô men được ác định bởi công
thức:
.
Ở đây, khi tính các véc tơ

96

ta phải sử dụng ma tr n chuyển đổi Euler t hệ trục tọa độ

T. T. Kiên, …, P. M. Tân, “Xây dựng forward kinematics … một số trường hợp riêng.”


Nghiên cứu khoa học công nghệ

c a bệ chuyển động về hệ trục tọa độ cơ sở và ngược lại, t hệ trục tọa độ c a hệ cơ sở về
hệ trục tọa độ c a bệ chuyển động.
Ma tr n chuyển đổi Euler t hệ chuyển động (P) về hệ cơ sở ( ) theo trục Y là:

Ma tr n chuyển đổi Euler theo chiều ngược lại:

Mô men quán tính quay quanh trục X và trục Y là giống nhau:

223

Z

Cũng tương tự như quay quanh trục X và trục Y, khi quay quanh trục Z ta cần giải hệ
phương tr nh:
Trong đó, ma tr n A tính theo công thức (12),
,
,
và véc tơ lực tác dụng lên
các trục F như tính cho trục X và Y th PM là ma tr n tổng hợp lực và mô men được ác
định bởi công thức:
Ở đây, khi tính các véc tơ ta phải sử dụng ma tr n chuyển đổi Euler t hệ trục tọa độ

c a bệ chuyển động về hệ trục tọa độ cơ sở và ngược lại, t hệ trục tọa độ c a hệ cơ sở về
hệ trục tọa độ c a bệ chuyển động.
Ma tr n chuyển đổi Euler t bệ chuyển động về bệ cơ sở theo trục Z là:

Ma tr n chuyển đổi Euler theo chiều ngược lại sẽ là:

Mô men quán tính quay quanh trục Z là:

3. KẾT QUẢ TÍNH TOÁN
3.1. Kết quả tính toán mặt chuyển động quay quanh trục X
Thực hiện tính toán với robot song song có các thông số như đã cho với góc quay
quanh trục X theo hàm sin với góc lớn nhất là 30 độ: ω=30.sin(3t), khi đó, gia tốc quay là
Ta được kết quả thể hiện như h nh 3.

Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020

97


Kỹ thuật Điều khiển – Tự động hóa

Hình 3. L
X
Nhận xét: Khi quay quanh trục X, lực tác dụng lên cặp trục 1-2; 3-6; 4-5 có giá trị đối
nhau v chúng đối ứng với nhau qua trục X, tuy nhiên, giá trị tuyệt đối c a chúng không
bằng nhau do khi quay vị trí c a chúng so với trục Z là khác nhau.
3.2. Kết quả tính toán mặt chuyển động quay quanh trục Y
Thực hiện tính toán với robot song song có các thông số như đã cho với góc quay
quanh trục Y theo hàm sin với góc lớn nhất là 30 độ: ω = 30.sin(3t), khi đó, gia tốc quay là
Ta được kết quả thể hiện như h nh 4.

Hình 4. L
Y
Nhận xét: Khi quay quanh trục Y lực tác dụng lên các cặp chân F1-F2; F3-F6; F4_F5
là bằng nhau. Lực tác dụng lên cặp chân F1-F2 có chiều ngược với lực tác dụng lên các
cặp chân F3-F6 và F4-F5 do chúng nằm ở 2 phía đối nhau so với trục Y.

98

T. T. Kiên, …, P. M. Tân, “Xây dựng forward kinematics … một số trường hợp riêng.”


Nghiên cứu khoa học công nghệ

3.3. Kết quả tính toán mặt chuyển động quay quanh trục Z
Thực hiện tính toán với robot song song có các thông số như đã cho với góc quay
quanh trục Z theo hàm sin với góc lớn nhất là 30 độ: ω = 30.sin(3t), khi đó, gia tốc quay là
Ta được kết quả thể hiện như h nh 5.

Hình 5. L
Z.
Nhận xét: Khi quay quanh trục Z lực tác dụng lên các chân F1-F3-F5 và lực tác dụng
lên các chân F2-F4-F6 là bằng nhau. Lực tác dụng lên chân F1-F3-F5 có chiều ngược với
lực tác dụng lên các chân F2-F4-F6 và F4-F5 và có giá trị tuyệt đối bằng nhau (nếu chỉ
tính phần lực gây ra bởi mô men quay).

4. KẾT LUẬN
ài báo đã ác định được FORWARD KINEMATICS c a robot song song kiểu
Stewart trong 3 trư ng hợp riêng, đã tính toán và đưa ra được kết quả lực tác động lên
t ng trục c a robot. Kết quả này có thể ứng dụng ây dựng các thu t toán điều khiển cho

robot song song kiểu Stewart trong các ứng dụng tạo chuyển động cho các bài toán cân
bằng khí tài quân sự, hệ tạo dạo động trong bài toán mô phỏng thiết bị bay,...
TÀI LIỆU THAM KHẢO
[1]. Phan Bùi Khôi, “G
”,
Tạp chí Khoa học và Công nghệ, T p 48, số 1,2010 Tr.33-34.
[2]. Phan Bui Khoi – “I
”. Proceedings of The 5th Asian symposium on Applied Electromagnatics
and Mechanics, Hanoi. Vietnam, 2005, pp. 224-231.
[3]. Nguyen Xuan Vinh, Nguyen Ngoc Lam, Le Quoc Ha, Le Hoai Quoc, Nguyen Minh
Thanh, (2015), “Optimal Design and Control of a Stewart-Gough Platform”, 7th
IEEE International Conferences on Cybernetics and Intelligent Systems (CIS)
Robotics, Automation and Mechatronics (RAM), Cambodia.
[4]. Sébastien Briot, Wisama Khalil, “Dynamics of Parallel Robots”, ISBN 978-3-31919787-6 ISBN 978-3-319-19788-3.
[5]. Merlet J.P. (2006), “Parallel Robots”, Kluwer Academic Publishers.
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Hội thảo Quốc gia FEE, 10 - 2020

99


Kỹ thuật Điều khiển – Tự động hóa

[6].

. Dasgupta and T. S. Mruthyunjaya (1998), “A Newton– Euler formulation for the
inverse dynamics of the Stewart–Gough Platform manipulator”, Mech. Mach.Theory,
vol. 33, no. 8, pp. 1135–1152.
ABSTRACT
DESIGN OF FORWARD KINEMATICS IN STEWART SYSTEM
WITH SOME UNIQUE CASES

Nowadays, using a stewart as a system to generate oscillations for testing
stability and simulation is a popular method. The creating of a stewart and
independent controlling of each leg does not face up with many difficulties in
current technological conditions. However, controlling the dynamic system of the
stewart in predetermined forms of oscillation is still a complex issue, these
algorithms have not been published yet. Therefore, it is necessary to build forward
kinematics for stewart in some unique cases. The article shows the forward
kinematics construction for the stewart in the unique cases is legs oscillating in the
same cycle, even legs in the same phase oscillator, odd legs in an inverted phase
oscillator. The given method is based on calculating the angles of rotation, tilt
angle, wedge angle and rising angle in each period of time and restored by DFT.
Test results by reverse kinetic simulation and actual tests.
Keywords: FFT; FDT; Forward kinematics; Inverse kinematics steward platform.

N
C
Đ

100

09
08
05 tháng 10
05 tháng 10

2020
2020
2020

ỉ: 1Viện Tự động hóa KTQS - Viện Khoa học và Công nghệ quân sự;
2
Viện Khoa học và Công nghệ quân sự.
*Email:

T. T. Kiên, …, P. M. Tân, “Xây dựng forward kinematics … một số trường hợp riêng.”