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ệ 
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 
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 
2 1 1 
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:  
T
q x y z   . 
Kỹ thuật Điều khiển – Tự động hóa 
 T. T. Kiên, , P. M. Tân, “Xây dựng forward kinematics  một số trường hợp riêng.” 92 
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ó:  1 2 3 4 5 6 .
T
L l l l l l l 
 ài toán động học ngược có nhiệm vụ t m véc tơ chiều dài iL 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. ắ i, Pi. 
cos
sin
0
B i ix
i B i iy
iz
r B
B r B
B
 (1) 
Với:
3 2
B
i
i 
 khi 1,3,5i ; 1i i B khi 2,4,6.i 
Và trên mặt phẳng {P} được biểu diễn: 
cos
sin
0
P i ix
i P i iy
iz
r P
P r P
P


 (2) 
Với: 
3 2
P
i
i 
 khi 1,3,5i ; 1i i P  khi 2,4,6.i 
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: 
Nghiên cứu khoa học công nghệ 
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 
 
TB
d d dP x y z 
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: 
B
i i iL P P B 
Xét trong hệ tọa độ gắn trên tấm nền: 
B B B B
i T i iL R P P B (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 
iL như sau: 
i i ix iy izl L L L L (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 iL và chuẩn Euclide c a iL . 
2 1 2 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. 
Kỹ thuật Điều khiển – Tự động hóa 
 T. T. Kiên, , P. M. Tân, “Xây dựng forward kinematics  một số trường hợp riêng.” 94 
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 . Để giải 
hệ phương tr nh trên ta thiết l p ma tr n A. 
 (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. 
Nghiên cứu khoa học công nghệ 
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 
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 
2 2 1 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 B B B
i T i iL R P P B 
i i ix iy izl L L L L 
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 trong hệ trục tọa độ c a bệ chuyển động được tính là: 
Với véc tơ 
 đượ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à: 
Kỹ thuật Điều khiển – Tự động hóa 
 T. T. Kiên, , P. M. Tân, “Xây dựng forward kinematics  một số trường hợp riêng.” 96 
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: 
2 2 2 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ơ 
 ta phải sử dụng ma tr n chuyển đổi Euler t hệ trục tọa độ 
Nghiên cứu khoa học công nghệ 
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 
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: 
2 2 3 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. 
Kỹ thuật Điều khiển – Tự động hóa 
 T. T. Kiên, , P. M. Tân, “Xây dựng forward kinematics  một số trường hợp riêng.” 98 
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. 
Nghiên cứu khoa học công nghệ 
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 
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-319-
19787-6 ISBN 978-3-319-19788-3. 
[5]. Merlet J.P. (2006), “Parallel Robots”, Kluwer Academic Publishers. 
Kỹ thuật Điều khiển – Tự động hóa 
 T. T. Kiên, , P. M. Tân, “Xây dựng forward kinematics  một số trường hợp riêng.” 100 
[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 09 08 2020 
 05 tháng 10 2020 
C 05 tháng 10 2020 
Đ ỉ: 1Viện Tự động hóa KTQS - Viện Khoa học và Công nghệ quân sự; 
2Viện Khoa học và Công nghệ quân sự. 
*Email: kienttcapit@gmail.com.