GIẢI bài TOÁN ĐỘNG học NGƯỢC CHO TAY máy TRÁNH vật cản BẰNG MẠNG NƠRON

HỘI NGHỊ KHCN TOÀN QUỐC VỀ CƠ KHÍ - ĐỘNG LỰC NĂM 2017
Ngày 14 tháng 10 năm 2017 tại Trường ĐH Bách Khoa – ĐHQG TP HCM

GIẢI BÀI TOÁN ĐỘNG HỌC NGƢỢC CHO TAY MÁY
TRÁNH VẬT CẢN BẰNG MẠNG NƠRON
Lƣơng Hồng Sâm
Trường Đại học Trần Đại Nghĩa

TÓM TẮT :
Việc điều khiển bàn kẹp tay máy đến được
một vị trí nào đó và tay máy tránh được vật cản
trong không gian hoạt động của nó là bài toán
phức tạp vì phải giải đồng thời bài toán động học
ngược và bài toán tránh vật cản cho tay máy.
Việc giải bài toán tránh vật cản ngoài việc giúp
cho tay máy không bị va chạm trong quá trình
hoạt động mà còn phải đảm bảo tay máy chuyển
động êm dịu trong quá trình hoạt động. Bài báo

giới thiệu một phương pháp giải bài toán động học
ngược cho tay máy tránh vật cản bằng mạng
nơron với một vật cản và vị trí của vật cản đã
được xác định. Việc tránh vật cản sẽ được thực
hiện bằng việc xác định và loại trừ các biến khớp
tương ứng với không gian của vật cản trong bộ dữ
liệu huấn luyện mạng nơron để giải bài toán động
học ngược.

Từ khóa : Động học ngược, Mạng nơron, Tránh vật cản

1. GIỚI THIỆU

Nhiệm vụ của bài toán động học ngược là xác
định các giá trị của biến khớp qi, (i=1,…n) khi biết
trước vị trí và hướng của bàn kẹp tay máy. Có ba
phương pháp truyền thống thường được sử dụng
để giải bài toán động học ngược: Phương pháp
giải tích, phương pháp hình học và phương pháp
lặp. Phương pháp giải tích là xác định các biến
khớp q1, q2,…,qn theo các biến vị trí. Phương
pháp hình học là chiếu hệ tọa độ của khâu lên
trục tọa độ Xi-1 và Yi-1. Phương pháp lặp là lặp lại
giá trị các biến khớp nhằm xác định các biến
khớp phù hợp. Ứng dụng mạng nơron để giải bài
toán động học ngược đã có một số công trình
nghiên cứu [1]. Phương pháp này có thể giải bài
toán động học ngược tay máy có n bậc tự do, độ
chính xác cao và bảo đảm được thời gian thực.
Bài báo này giới thiệu một phương pháp giải
bài toán động học ngược cho tay máy tránh vật
cản bằng mạng nơron với giả thiết là tay máy có
ba khớp quay, vật cản là duy nhất và có vị trí xác
định với hình dáng hình học đã biết. Từ đó đi xây
dựng thuật toán tránh vật cản cho tay máy này
sao cho các khâu của tay máy không chạm vào
vật cản mà vẫn đảm bảo bàn kẹp đến được vị trí

Trang 344

mong muốn trong không gian với một quỹ đạo
xác định. Từ các điều kiện đó, bài báo xác định và
loại trừ các biến khớp tương ứng với không gian

của vật cản trong bộ dữ liệu huấn luyện của mạng
nơron để giải bài toán động học ngược.
2. GIẢI THUẬT TRÁNH VẬT CẢN
Tay máy gồm 3 khớp quay với các tham số
động học được cho trong bảng 1. Giả sử bàn kẹp
tay máy cần chuyển động đến các điểm Pi (i =
1..n) (Hình 1). Thiết lập hệ tọa độ của hệ thống
bao gồm tay máy và vật cản sao cho gốc O của
hệ tọa độ trùng với gốc hệ tọa độ cơ sở của tay
máy. Trục z của hệ tọa độ này trùng với trục z
của hệ tọa độ cơ sở tay máy (Hình 1).
Vật cản được đặt trong không gian hoạt động
tay máy với giả thiết vật cản được bao kín bởi
một mặt cầu. Như vậy hình dáng và kích thước
của vật cản được đặc trưng bởi hình dáng và kích
thước của hình cầu. Vị trí và kích thước của vật
cản được thể hiện khi chiếu hệ thống lên hai mặt
phẳng tọa độ (Hình 2, Hình 3). [2].


HỘI NGHỊ KHCN TOÀN QUỐC VỀ CƠ KHÍ - ĐỘNG LỰC NĂM 2017
Ngày 14 tháng 10 năm 2017 tại Trường ĐH Bách Khoa – ĐHQG TP HCM

D'  d12  d 2 ;   a tan(d1 , d ) (3)
a2

Z

a3

L3  P cos(3   )  R

•
P

X

Y

Hình 1. Hình dạng tay máy và vật cản
Y

r

O

X

D

Hình 2. Tay máy và vật cản trong mặt phẳng XY

K3

a2

Z

K2
θ2

d1
O

θ3

a3

β

α

D’

•

d

R
X

Hình 3. Tay máy và vật cản trong mặt phẳng XZ

Từ hình 1, để đảm bảo cho các khâu không
chạm vào vật cản và kích thước của vật cản
không quá lớn thì cần thỏa mãn phương trình (1):



D  R ; 1  a tan R, D 2  R 2



(1)

Trong đó : D - khoảng cách từ tâm vật cản đến
gốc tọa độ hệ thống, R - bán kính hình cầu bao
vật cản.
Điều kiện để khâu 2 và 3 không chạm vào vật
cản là khoảng cách từ hai khâu này tới tâm hình
cầu phải lớn hơn bán kính hình cầu. Từ hình 3:

L2  D sin(2   )  R
'

Trong đó :

(6)

Nếu khâu 2 và khâu 3 tiếp xúc với bề mặt
hình cầu có nghĩa là khoảng cách L2 = L3 = R thì
ta có các góc giới hạn :


 a tan  


P  R , R  

(2)

3 min

2

2

(8)

Các bước để xác định bộ thông số các biến di
chuyển cho tay máy tránh được vật cản và vẫn
đảm bảo khâu tác động cuối đến được vị trí cuối
cùng của quỹ đạo theo giải thuật như sau :
1- Giải bài toán động học ngược cho tay máy tại
vị trí điểm đầu và điểm cuối quỹ đạo và một số
điểm trung gian trên quỹ đạo.

L3

L2

P  a22  2a2 D' 2  L22  D' 2

2 min  a tan R,  D' 2  R 2   (7)

R

•

(5)

  a tan a2  D' 2  L22 , L2

d

d





i

d1
O

(4)

2- Với mỗi θ1 tính được từ bước 1, kiểm tra theo
điều kiện (1). Nếu thỏa mãn, thì θ1 được nhận và
chuyển sang bước 2, nếu không sẽ chọn lại θ 1 từ
bước 1.
3- Với mỗi θ2 tính được từ bước 1, tính L2 theo
công thức (2). Kiểm tra theo điều kiện (2), nếu
thỏa mãn thì θ2 được nhận và chuyển sang bước
4, nếu không sẽ chọn lại θ2 từ bước 1.
4- Với mỗi θ3 tính được từ bước 1 và L2 tính được
từ bước 3, tính L3 theo phương trình (4). Kiểm tra
theo điều kiện (4), nếu thỏa mãn thì θ3 được
nhận, nếu không sẽ chọn lại θ3 từ bước 1.

Bộ thông số các biến di chuyển {θ i} tìm được
từ các bước trên sẽ được dùng làm bộ dữ liệu
huấn luyện mạng Nơron để giải bài toán động học
ngược cho tay máy tránh vật cản.
3. ĐỘNG HỌC TAY MÁY THÍ NGHIỆM
Giải các bài toán động học tay máy nhằm xác
định bộ dữ liệu để huấn luyện mạng nơron cho
bài toán động học ngược cho tay máy tránh vật
cản. Sơ đồ động học và các tham số động học
của tay máy được trình bày trong Hình 4 và
Bảng 1.

Trang 345


HỘI NGHỊ KHCN TOÀN QUỐC VỀ CƠ KHÍ - ĐỘNG LỰC NĂM 2017
Ngày 14 tháng 10 năm 2017 tại Trường ĐH Bách Khoa – ĐHQG TP HCM
Y2

Z0
θ2

θ3

a2
X1

X2
Z2

Z1

a3

c1 0
s 0
A0,1   1
0 1

0 0

s1
0
c1 0 
0
d1 

0
1 

(11)

A1,2

c2
s
 2
0

0

 s2 0 a2c2 
c2
0 a2 s2 
0
1 0 

0
0 1 

(12)

A2,3

c3
s
 3
0

0

 s3 0 a3c3 
c3
0 a3 s3 
0
1 0 

0
0 1 

(13)

A0,3

c1c23
s c
  1 23
 s23

0

d1
Y3

θ1
Z3
X0

X3

Hình 4. Sơ đồ động học tay máy
Bảng 1. Thông số động học
Khớp i

αi

1

π/2

0

2

0

a2 = 500

3

0

a3 = 150

ai(mm)

di(mm)
d1 = 200

θi (phạm vi
làm việc)
0

θ1 ( 45 )
θ2 ( 450)

0

θ3 ( 300)

3.1. Bài toán động học thuận
Bài toán động học thuận là xác định vị trí và
hướng của bàn kẹp tay máy khi biết trước các
thông số vị trí của tay máy. Giải bài toán động học
thuận nhằm xác định bộ dữ liệu huấn luyện cho
mạng nơron tổng quát.
Vị trí và hướng của bàn kẹp tay máy so với hệ
tọa độ cơ sở được biểu diễn bằng ma trận sau:

B0, n

 ix
i
y
 iz

0

jx
jy
jz
0

k x px 
k y p y 
k z pz 

0 1 

(9)

Trong đó : px, py, pz là tọa độ tâm bàn kẹp, i, j, k là
véctơ chỉ phương của hệ tọa độ gắn với bàn kẹp
so với hệ tọa độ cơ sở. Vị trí tâm bàn kẹp và
hướng bàn kẹp cũng được xác định bằng ma
trận:

A0, n

l x
l
n
  Aij   y
l z
i 1

0

mx
my
mz
0

nx q x 
n y q y 
(10)
nz q z 

0 1 

Cân bằng các phần tử của hai ma trận trong
phương trình (9) và (10), có phương trình xác
định vị trí bàn kẹp và là lời giải bài toán động học
tay máy. Các ma trận động học của tay máy :

Trang 346

c1s23
 s1s23
c23
0

s1
c1
0
0

c1  a3c23  a2 c2  

s1  a3c23  a2 c2  
(14)
d1  a3 s23  a2 s2 


1

Trong đó : s1, c1, sij, cij biểu diễn cho sin(θi),
cos(θi), sin(θi+θj), cos(θi+θj).
Từ (14) ta có lời giải bài toán động học thuận:

px  c1 (a3c23  a2c2 )
p y  s1 (a3c23  a2c2 )

(15)

pz  d1  a3 s23  a2 s2
3.2. Bài toán động học ngƣợc
Giải bài toán động học ngược là đi xác định
bộ thông số của các biến khớp khi biết trước vị
trí và hướng của bàn kẹp tay máy. Kết quả lời
giải bài toán động học ngược bằng giải tích
được sử dụng để kiểm chứng lại độ chính xác
lời giải bài toán động học ngược bằng mạng
nơron. Cân bằng các phần tử hai ma trận (9) và
(10), ta có lời giải bài toán động học ngược:

1  a tan 2( p y , px )
 2  a tan 2( pz  d1 , px2  p y2
 a tan 2(a3 s3 , a2  a3 s3 )

(16)

3  a tan 2( s3 , c3 ); s3   1  c32
c3 

px2  p y2  ( pz  d1 )2  a22  a32
2a2 a3


HỘI NGHỊ KHCN TOÀN QUỐC VỀ CƠ KHÍ - ĐỘNG LỰC NĂM 2017

Ngày 14 tháng 10 năm 2017 tại Trường ĐH Bách Khoa – ĐHQG TP HCM

4. ỨNG DỤNG MẠNG NƠ RON CHO BÀI
TOÁN ĐỘNG HỌC NGƢỢC TAY MÁY TRÁNH
VẬT CẢN
Phần này trình bày việc xác định cấu trúc
mạng, bộ dữ liệu huấn luyện mạng nơron cho tay
máy tránh vật cản mà bộ dữ liệu này tìm được
theo thuật toán trong phần 2. Mạng nơron được
dùng là mạng truyền thẳng nhiều lớp gồm có một
lớp vào, một lớp ra, có nhiều lớp ẩn. Mỗi nơron
thứ q ở lớp ẩn liên kết với lớp vào bằng trọng số
Vqj và liên kết với lớp ra bằng trọng số W iq. Việc
huấn luyện mạng được sử dụng luật học lan
truyền ngược Back propagation và hàm truyền
sigmoid [1].

máy va chạm với vật cản. Một phần kết quả này
được thể hiện trong bảng 2 và hình 6.
Bảng 2. Một số kết quả khi chưa tránh vật cản
Vị trí 1

Vị trí 2

Vị trí 3

Vị trí 4

Vị trí 5

Vị trí 6

px 588,603 591,281 610,301 601,045 593,447 559,407
py 93,045 49,557 37,691 -16,990 -81,989 -212,827
pz -37,853 -29,131 -15,654 -19,645 -24,437 -36,090
θ1 8,984

4,799

3,532

-1,735

-7,933

-20,826

θ2 -16,764 -27,614 -17,201 -25,516 -26,186 -17,082
θ3 -21,886 28,141
Va chạm

-9,621

23,763

24,230 -19,426

Va chạm Va chạm

Tay máy

Quỹ đạo

4.1. Giải bài toán động học ngƣợc tay máy thí
nghiệm khi chƣa tránh vật cản
Theo bước 1 của thuật toán, cần giải bài toán
động ngược cho tay máy trong trường hợp tổng
quát, chưa áp dụng giải thuật tránh vật cản với
mục đích xác định bộ thông số tổng quát {θi}. Việc
giải bài toán động học ngược được áp dụng
phương pháp mạng nơron theo nghiên cứu [1].
Bộ dữ liệu huấn luyện được lấy từ phương
trình (15) với giới hạn các góc : -450<θ1<450; 450<θ2<00; -300<θ3<300. Bộ dữ liệu gồm 6000
mẫu trong đó 4000 mẫu dùng để huấn luyện,
2000 mẫu dùng để kiểm tra khả năng tổng quát
của mạng. Miền dữ liệu này được biểu diễn qua
hình 5. Hàm trung bình bình phương MSE được
dùng để đánh giá chất lượng huấn luyện mạng.
Việc huấn luyện được sử dụng với phần mềm
Matlab.

Tay máy

Các vị trí
bàn kẹp

Vật cản

Vật cản

Hình 6. Một số cấu hình tay máy va chạm với

vật cản khi sử mạng chưa tránh vật cản

4.1. Giải bài toán động học ngƣợc tay máy thí
nghiệm tránh vật cản
Sau khi giải bài toán động học ngược cho tay
máy trong trường hợp chưa tránh vật cản nhằm
xác định bộ thông số các biến di chuyển tổng quát
của tay máy {θi}. Bước tiếp theo là xác định bộ dữ
liệu huấn luyện mạng nơron cho bài toán tránh
vật cản. Bộ dữ liệu này là tập con của bộ dữ liệu
tổng quát {θi} và được lấy theo thuật toán đã trình
bày ở phần 2. Bộ dữ liệu này gồm 6000 mẫu,
trong đó 4000 mẫu dùng để huấn luyện, 2000
mẫu dùng để kiểm tra khả năng tổng quát của
mạng. Hàm MSE được dùng để đánh giá chất
lượng huấn luyện mạng. Giới hạn các góc: 45<θ1<45; -45<θ2<0; -30<θ3<0. Miền dữ liệu huấn
luyện này được thể hiện trên Hình 7. Quá trình
huấn luyện mạng được thể hiện trên Hình 8.

Hình 5. Miền dữ liệu của mạng tổng quát

Sử dụng mạng nơron vừa tìm được để giải bài
toán động học ngược cho tay máy khi chưa tránh
vật cản thì sẽ có một số cấu hình mà ở đó tay

Trang 347


HỘI NGHỊ KHCN TOÀN QUỐC VỀ CƠ KHÍ - ĐỘNG LỰC NĂM 2017
Ngày 14 tháng 10 năm 2017 tại Trường ĐH Bách Khoa – ĐHQG TP HCM

Bảng 3. Một số kết quả khi tránh vật cản
Vị trí 1

Tay máy

Vị trí 2

Vị trí 3

Vị trí 4

Vị trí 5

Vị trí 6

px 588,603 591,281 610,301 601,045 593,447 559,407
py 93,045 49,557 37,691 -16,990 -81,989 -212,827
pz -37,853 -29,131 -15,654 -19,645 -24,437 -36,090
Vật cản

θ1 8,978
Các vị trí
bàn kẹp

4,803

3,518

-1,619

-7,874

-20,851

θ2 -16,804 -14,627 -17,194 -14,657 -15,016 -17,132
θ3 -21,902 -28,354 -9,612 -23,767 -24,260 -19,399
Các cấu hình trên đều không va chạm với vật cản

Hình 7. Miền dữ liệu của mạng tránh vật cản
Tay máy

Quỹ đạo

Vật cản

Hình 10. Quỹ đạo và cấu hình tay máy sau khi sử dụng
mạng tránh vật cản
Hình 8. Quá trình huấn luyện mạng tránh vật cản

5. KẾT LUẬN
Sau khi huấn luyện, bộ trọng số và cấu trúc
của mạng nơron dùng để giải bài toán động học
ngược cho tay máy tránh vật cản được xác định
bao gồm lớp nhập với 3 nút tương ứng với vị trí
bàn kẹp tay máy (px, py, pz). Lớp ẩn gồm 3 lớp,
mỗi lớp có 30 nút. Lớp xuất gồm 3 nút tương ứng
với 3 biến khớp (θ1, θ2, θ3). Cấu trúc mạng tìm
được thể hiện trên hình 9. Một số kết quả giải bài
toán động học ngược cho tay máy tránh vật cản
với mạng nơ ron vừa tìm được thể hiện trên bảng

3. Quỹ đạo và cấu hình của tay máy sau khi tránh
vật cản thể hiện trên hình 10.

Hình 9. Cấu trúc mạng nơ ron

Trang 348

Bài báo giới thiệu một nghiên cứu ứng dụng
mạng nơron để giải bài toán động học ngược cho
tay máy tránh vật cản. Kết quả là đã tìm được
một mạng nơron với cấu trúc và bộ trọng số thích
hợp cho bài toán này. Với mạng nơron vừa tìm
được thì các khâu của tay máy đã không va chạm
với vật cản khi làm việc. Điều đó chứng tỏ thuật
toán tránh vật cản là chính xác, mạng nơron tìm
được đã nhận biết được chính xác lời giải của bài
toán động học ngược tay máy tránh vật cản.
Nghiên cứu này đã mở ra một hướng mới, hiệu
quả cho vấn đề tránh vật cản của tay máy.


HỘI NGHỊ KHCN TOÀN QUỐC VỀ CƠ KHÍ - ĐỘNG LỰC NĂM 2017
Ngày 14 tháng 10 năm 2017 tại Trường ĐH Bách Khoa – ĐHQG TP HCM

REFERENCES
[1]. Lương Hồng Sâm, Nguyễn Việt Hùng, Ảnh
hưởng của vùng dữ liệu đến độ chính xác lời
giải bài toán động học ngược tay máy bằng
mạng nơron, Hội nghị Cơ khí - Động lực lần
toàn quốc (2016).

[2]. Wei

zhang, Tarek M. Sobh, Obstacle
avoidance for manipulator, System analysisd
ed. Norwell, MA: Kluwer, 159–160 (2001).
Model simul, Vol 43, No 1, pp 67-74 (2003).

SOLVE INVERSE KINEMATICS FOR ROBOT MANIPULATOR WITH
OBSTACLE AVOIDANCE USING NEURAL NETWORK

ABTRACT :
Controlling the grip of manipulator to a certain
position and the manipulator avoids obstacles in
its operation space is a complicated problem
because it requires solving the inverse kinematics
and solving the avoidance obstacle problem.
Solutions of the obstacle avoidance problem not
only help the manipulators to eliminate collisions
during operation, but also to ensure them move
smoothly. This paper introduces a method based

on neural network for solving inverse kinematics
of robotic arm avoiding obstacle with one obstacte
and the location of the obstacle is determined.
The obstacle avoidance will be accomplished by
identifying
and
excluding
joint

variables
corresponding with space of obstacle in the neural
network training data set to solve inverse
kinematics.

Keywords : Inverse kinematics, Neural netwoks, Obstacle avoidance

Trang 349