Hội nghị kỹ thuật cơ khí lần 2 tại Đại học Bách Khoa Tp.

HCM
Ngày …-…, tháng 5 - 2011, Đại học Bách Khoa thành phố Hồ Chí Minh, Việt Nam
TÍNH TOÁN VÀ MÔ PHỎNG ĐỘNG HỌC TRONG MATLAB

Nguyễn Minh Dương1

Nguyễn Duy Đạt2
Trần Thiên Phúc3
1
Khoa Cơ khí, Đại học Bách Khoa, thành phố Hồ Chí Minh, Việt Nam
(Tel : 0984044842; E-mail: 20800375@stu.hcmut.edu.vn)
2
Khoa Cơ khí, Đại học Bách Khoa, thành phố Hồ Chí Minh, Việt Nam
(Tel : 01678892615; E-mail:20800402@stu.hcmut.edu.vn)
3
Khoa Cơ khí, Đại học Bách Khoa, thành phố Hồ Chí Minh, Việt Nam
(Tel : 0903951022; E-mail:ttphuc.rectie@ hcmut.edu.vn)

Tóm tắt:
Tìm hiểu về công cụ Simmechanics và công cụ
Simulink trong Matlab.Tính động học thuận và động
học nghịch của tay robot .Thực hiện một số ví dụ minh
họa mô phỏng vị trí động học của tay robot.
Từ khóa: Simmechanics,simulation.....
I.GIỚI THIỆU:
MATLAB là một phần mềm lớn với nhiều công cụ
mạnh trong việc tính toán và mô phỏng dựa trên các
module được tích hợp sẵng .Trong đó Sim mechanics là
một môi trường thuận lợi cho kĩ sư trong quá trình thiết
kế và mô phỏng các cơ cấu máy, cánh tay, những
chuyển động của cơ cấu dựa vào những định luật tác
dụng lực và moment.
Matlab còn hổ trợ chuyển các khối chi tiết từ những
phần mềm vẽ khác sang môi trường làm việc của
Matlab như Solicwork,Iventer….
Matlab xây dựng được mô hình tính toán hợp lí và
hiệu quả nhất sao cho việc mô phỏng đạt hiểu quả
cao.Matlab cho phép liên kết Simmechanics với
Simulink một cách dể dàng trong việc xuất nhập kết quả
tính toán một cách rõ ràng,và nhanh do Matlab được
xây dựng trên mô hình tính toán ma trận..
MATLAB còn được trang bị thêm các ToolBox – các
gói chương trình (thư viện) cho các lĩnh vực ứng dụng
rất đa dạng như xử lý tín hiệu, nhận dạng hệ thống, xử
lý ảnh, mạng nơ ron, logic mờ, tài chính, tối ưu hóa,
phương trình đạo hàm riêng, sinh tin học,….
II.GIỚI THIỆU VỀ THƯ VIÊN CÔNG CỤ
SIMMECHANICS:
II.1.Khối thư viện bodies:
II.1.1.Khối body:
Mục đích: Biểu diễn một vật thể cứng tùy ý:
Mô tả:
Mục đích: Biểu diễn một điểm trên một giá xác định .
Mô tả:
Một khối Ground biểu diễn một điểm cố định ,định
trong hệ trục tọa độ tuyệt đối World.
II.2.Thư viện Constraints & Drivers:
II.2.1.Khối Angle driver:
Mục đích: Định rõ góc giữa hai vector trục của Body
như một hàm theo thời gian
Mô tả:
Khối Angle Driver truyền dẫn động các vector trục
đã được định trên hai Body. Bạn định rõ các vector trục
của “follower body” và “base body” cố định aB, Af.
II.2.2.Khối Distance driver:
Mục đích: Định rõ khoảng cách giữa hai gốc hệ tọa
độ Body như một hàm theo thời gian
Mô tả:
Khối Distance Driver truyền động khoảng cách giữa
hai gốc của hai hệ tọa độ Body như một hàm thời
gian mà bạn định rõ. Hàm này phải luôn luôn được
giữ không âm trong khi mô phỏng.
II.2.3.Khối Linear driver:
Mục đích: Định rõ một thành phần vector nối hai gốc
hệ tọa độ Body như hàm theo thời gian.
Mô tả:

• Khối lượng của vật thể và tensor mômen quán

Khối Linear Driver định rõ một thành phần của
tính.
vector nối hai gốc hệ tọa độ như một hàm của thời gian.
• Toạ độ trọng tâm của vật thể (CG) II..2.4.Khối Velocity driver:
• Một số hệ toạ độ Body tuỳ ý (CSs) Mục đích: Định rõ một sự kết hợp tuyến tính giữa
II.1.2.Khối Ground: vận tốc dài và vận tốc góc của hai Body như một hàm
của thời gian.
 Mô tả:
Khối Revolute miêu tả một bậc tự do quay đơn xung
quanh một trục xác định giữa hai Body.
II.3.3.Khối Spherical:
Mục đích: Biểu diễn một khớp cầu lắp ghép với ba
Khối Velocity Driver truyền dẫn sự kết hợp tuyến tính bậc tự do quay.
của vận tốc góc và vận tốc dài đã được chiếu của hai Body. Mô tả:
II.2.2.5.Khối Point- curve driver:
Mục đích: Cưỡng bức chuyển động của một điểm
trên một Body di chuyển dọc theo một đường cong trên một
Body khác.
Mô tả: Khối Spherical biểu diễn ba bậc tự do quay tại một
điểm trung tâm, khớp quả cầu-hốc.
II.3.4. Khối Planar:
Mục đích:
Biểu diễn một khớp kết hợp với hai bậc tự do tịnh
Hai Body được nối bởi Point-Curve Constraint chỉ có tiến và một bậc tự do quay, với trục quay trực giao với
thể di chuyển tương đối với Body kia nếu một điểm trên một mặt phẳng các trục tịnh tiến.
Body di chuyển dọc theo một đường cong trên một Body Mô tả:
khác.
II..2.6.Khối Parallel constraint:
Mục đích: Cưỡng bức vector trục Body của hai Body
luôn song song.
Mô tả: Khối Planar biểu diễn một khớp kết hợp với hai bậc
tự do tịnh tiến như hai khớp nguyên thuỷ Prismatic và
một bậc tự do quay như một khớp nguyển thuỷ
Revolute.
II.3.5.Khối Univeral:
Hai Body được nối bởi Parallel Constraint là được Mục đích: Biểu diễn một khớp kết hợp hai bậc tự do
khống chế chuyển động quay tương đối giữa chúng. quay( cơ cấu các_đăng) .
II..2.7.Khối Gear constraint: Mô tả:
Mục đích: Cưỡng bức chuyển động quay của hai
Body di chuyển dọc theo vòng tròn bước răng tiếp tuyến.
Mô tả:

Khối Universal biểu diễn một khớp kết hợp hai bậc
tự do quay như hai khớp nguyên thuỷ Revolute. Không
có sự cưỡng bức giữa các khớp.
Hai Body được nối bởi một khối Gear Constraint là II.3.6. Khối Cylindrical:
được khống chế quay tương đối dọc theo vòng tròn bước ở Mục đích: Biểu diễn một khớp kết hợp một bậc tự do
mỗi Body. tịnh tiến và một bậc tự do quay, với trục quay và trục
II..3.Thư viện Joints: tịnh tiến song song với nhau( khớp trụ).
II..3.1.Khối Primastic: Mô tả:
Mục đích: Biểu diễn một khớp lăn trụ với một bậc tự
do tịnh tiến.
Mô tả:

Khối Cylindrical biểu diễn một khớp kết hợp với

một bậc tự do tịnh tiến như một khớp lăng trụ và một
Khối Prismatic biểu diẽn một bậc tự do tịnh tiến bậc tự do quay như một khớp nguyên thuỷ quay.
đơn dọc một trục được xác định. II.3.7.Khối Gimbal:
II..3.2. Khối Revolute: Mục đích: Biểu diễn một khớp kết hợp ba bậc tự do
Mục đích: Biểu diễn một khớp quay kết hợp với một quay.
bậc tự do tịnh tiến. Mô tả:
Mô tả:
 Khối Gimbal biểu diễn một khớp kết hợp với ba II.4. Thư viện Sensor & Actuators:
bậc tự do quay như ba khớp nguyên thuỷ quay. Không II.4.1. Khối Body actuator:
có sự cưỡng bức giữa các khớp. Mục đích: Tác dụng lực/ mômen vào một Body.
II.3.8. Khối Custom joint: Mô tả:
Mục đích: Biểu diễn một khớp kết hợp tuỳ ý với
dưới ba bậc tự do tịnh tiến và dưới ba bậc tự do quay.
Mô tả:

II.4.2. Khối Joint actuator:

Mục đích:Tác động lực/mômen hoặc tạo chuyển
Khối Custom là một khối kết hợp mà bạn có thể động cho một khớp nguyên thuỷ.
tuỳ biến với sự kết hợp đặc biệt các khớp nguyên Mô tả:
thuỷ miêu tả chung nhất và không cưỡng bức bậc tự do
theo các hướng.
II.3.9. Khối Weld:
Mục đích: Biểu diễn một khớp không bậc tự
do( Mối nối cứng).
II..4.3. Khối Driver Actuator:
Mô tả:
Mục đích:Gây ra chuyển động tương đối giữa một
cặp Body cưỡng bức thông qua một khối.
Mô tả:

Khối Weld biểu diễn một khóp không bậc tự do.

Hai Body đã nối với các phía khác nhau của khối Weld
được khoá cứng với nhau, mà không thể chuyển động
tương đối.
II.4.4. Khối Body sensor:
II.3.10.Khối Telescoping:
Mục đích: Đo sự chuyển động của Body.
Mục đích: Biểu diễn một khớp kết hợp một bậc tự
Mô tả:
do tịnh tiến và ba bậc tự do quay.
Mô tả:

Khối Body Sensor dò tìm vị trí, vận tốc, gia

tốc của Body đã được biểu diễn bởi khối Body
Khối Telescoping biểu diễn một khớp kết hợp với II..4.5.Joint sensor:
một bậc tự do tịnh tiến như khớp nguyên thuỷ lăng trụ Mục đích:đo chuyển động lực và moment của một khớp
và ba bậc tự do quay như khớp nguyên thuỷ cầu. nguyên thủy.
II.3.11.Khối In-Plane: Mô tả:
Mục đích: Biểu diễn một khớp kết hợp với hai bậc
tự do tịnh tiến.
Mô tả:

Khối Joint Sensor đo vị trí, vận tốc, hoặc gia

tốc của một khớp nguyên thuỷ trong một khối Joint.
II.4.6. Khối Constraint & driver sensor:
Khối biểu diễn một khớp kết hợp với hai bậc tự do Mục đích:Đo lực/ mômen cưỡng bức giữa một
tịnh tiến như hai khớp nguyên thuỷ lăng trụ. Không có cặp Body cưỡng bức.
sự cưỡng bức giữa các khớp. Mô tả:
II.3.12. Khối Bushing:
Mục đích:thông số bước điều khiển Screw dịch
chuyển đoạn bao nhiêu cho mỗi vòng quay.
Khối Constraint & Driver Sensor đo lực hoặc
Mô tả:
môment (phản lực moment) cưỡng bức giữa một cặp
Body.
2.4.7. Khối Joint Initial Condition Actuator:  cosθ3 − sinθ3 0 l 2
Mục đích: Đặt vị trí và vận tốc ban đầu cho một khớp  
sinθ3 cosθ 3 0 0
trước khi bắt đầu mô phỏng. T2,3 = 
 0 0 1 0
Mô tả:  
 0 0 0 1

Ma trận chuyển cuối cùng : T0,3=T0,1T1,2T2,3

Khối Joint Initial Condition Actuator cung cấp giá  C12 C3 − S12 S3 −C12 S3 −S12 C3 0 C12 l2 C+1 l1 
trị dữ kiện ban đầu cho một khớp nguyên thuỷ trong  
S C+ C S −S12 S3 +C12 C3 0 S12 l2 S1+l1
khối Joint. Giá trị ban đầu là vị trí và vận tốc của T0,3=  12 3 12 3 
khớp nguyên thuỷ đó và hoàn toàn xác định  0 0 1 0 
 
hướng tình trạng chuyển động ban đầu của khớp  0 0 0 1 
nguyên thuỷ đó.
II.4.8. Khối Joint Stiction Actuator:  C123 − S123 0 C12 l2 +C1 l1 
Mục đích:Gây ra ma sát cổ điển cho một khớp nguy  
S C123 0 S12 l+2 S1 l1
ên thuỷ. T0,3=  123 
Mô tả:  0 0 1 0 
 
 0 0 0 1 

Với C123=cos(θ1+θ2+θ3),S123= sin(θ1+θ2 + θ3)

Khối Joint Stiction Actuator gây ra ma sát III.1.2.Mô hình tính động học nghịch:
tỉnh(static friction = stiction) cho một khớp tịnh tiến hay
khớp quay nguyên thuỷ. Joint Stiction Actuator gây ra Cφ −Sφ 0 x
 
ma sát tĩnh cho khớp nguyên thuỷ dưới dạng lực/ môm Sφ Cφ 0 y
T=
en 0 0 1 0
giữa những Body được nối của khớp. Những Body có t  
0 0 0 1
hể cảm thấy những lực thêm vào độc lập của khớp.
III.MỘT SỐ VÍ DỤ VỀ MÔ PHỎNG TÍNH TOÁN
TRÊN PHẦN MỀM SIMMECHANICS: Với φ = θ1+ θ2+ θ3 là góc mà khâu 3 hợp với trục
III.1.Ví dụ tay robot hai bậc tự do chuyển động theo x của gốc tọa đọ chính.
quỹ đạo trong một mặt phẳng:
III.1.1.Mô hình tính động học thuận: T=T0,3
Bảng thông số D-H:
x2 + y2 − l12 − l 22
i αi-1 ai-1 di θi C= ; s = 1− c2
2l1l 2
1 0 0 0 θ1
2 0 l1 0 θ2 θ2 = atan2(s2 ,c2 ) ;
3 0 l2 0 θ3 k1 = l1 + l 2c2 ;

Bảng 3.1.bảng thông số D-H

k2 = l 2s2 ;
Các ma trận chuyển θ1 = atan2(y,x) − atan2(k2 ,k1 );
θ3 = φ − θ1 − θ 2
 cosθ1 − sinθ1 0 0
 
sinθ1 cosθ1 0 0
T0,1 = 
 0 0 1 0
 
 0 0 0 1 II.1.3.Mô phỏng trên công cụ Simmechanics:
Xây dựng mô hình hình học của tay robot :
 cosθ2 − sinθ 2 0 l 1
 
sinθ2 cosθ2 0 0
T1,2 = 
 0 0 1 0
 
 0 0 0 1
 Hình 3.1:Chân robot
Khai báo tọa độ từng khâu trong hộp thoại
Body.Simmechanis yêu cầu khai báo chính xác tọa độ
từng khâu.Gắn các khâu lại với nhau bởi các khớp
xoay,lưu ý chọn góc xoay cho các khâu.
Xây dựng mô hình điều khiển mô phỏng bằng các
khối trong Simmechanics.

Hình 3.5:Đồ thị vị trí ra mà sensor cảm biến điểm A

tay robot nhận được .

Hình 3.2:sơ đồ khối

Viết chương trình chuyển động theo quỹ đạo của
điểm A trên tay robot.Ta có thể cho điểm cuối chuyển
động theo đường thẳng bất kì….
Dựa trên mô hình toán động học thuận và động
học nghịch ta viết chương trình giải thuật cho tay robot Hình 3.6:Đồ thị tọa độ x,y,z của điểm A theo thời
trong hộp thoại Embedded Matlab Function. gian t.
Vàng:tọa độ x
Tím:tọa độ y
Xanh dương:tọa độ z

III.2.Ví dụ cánh tay robot 2 bậc tự do chuyển động

theo quỹ đạo trong không gian:
III.2.1.Mô hình tính toán động học thuận:
Bảng thông số D-H:

i αi-1 ai-1 di θi
1 0 0 0 θ1
2 -90 0 L1 θ2
3 0 L2 0 0

Hình 3.3:giải thuật cho Simulink Bảng 3.2.bảng thông số D-H

Cho điểm A của tay robot chuyển động theo đường Các ma trận chuyển
thẳng x=10.
cosθ1 − sinθ 1 0 
0
 
sin θ1 cosθ 1 0 0
T0,1 =  
 0 0 1 
0
 
 0 0 0 1


 cosθ 2 −sin 2θ 0 0 
 
0 1 0 1L
T1,2 =  
 − sin( 2θ ) −
cos 2
θ1 0 
 
 0 0 0 1 
Hình 3.4:Đồ thị tập hợp điểm đầu vào
 1 0 0 
L 2
 
0 1 0 0
T2,3 =
0 0 1 0
 
0 0 0 1

Ma trận chuyển từ cuối cùng

 C1C2 − C1S2 0 C 1C 2L 2 −S1L 1 

 
S C − S1S23 0 S1C 2L 2 +C 1L 1
T0,3=  1 2 
 Hình 3.8-Tay robot
−S −C 2 1 S−2L 2
 2  Thực hiện viết chương trình giải động học nghịch
 0 0 0 1 
cho tay robot chuyển động theo một đường bất kì.
Viết một đường công quỹ đạo cho tay robot chuyển
Với C1=cosθ1,S1= sinθ1)
động.
Matlab sẽ xuất ra hình ảnh mô phỏng và các thông số
II.2.2.Mô hình tính toán động học nghịch: của điểm A:

 nx ox ax px 
 
n oy ay py 
T0,3 =  y
n ( 1)
oz az pz 
 z 
 0 0 0 1 

(T0,1)-1.T0,3=T1,2.T2,

Hình 3.9-Tay robot đang chuyển động

−S1px + C1py = L1

Từ (1) ta có  −S2 .L 2 = pz .
C .L = C p + S p
 2 2 1 x 1 y

θ1 = atan2(py ,px )− atan2(L 1, px2+ py2− L 21)

θ2 = ata n 2(− pz , C1p x + S1p y )
Với px,py,pz là tọa độ điểm cuối .

III.2.3.Mô phỏng trên Simmechanics:

Xây dựng sơ đồ khối cho cơ cấu tay 2bậc tự do trong
không gian với các khối body,khớp xoay,tác động Hình3.10-Tọa độ y, z theo thời gian của A
moment lực,cảm biến đưa về.

Hình 3.7-Sơ đồ khối tay robot

Ta được mô hình tay robot: Hình3.11-Tọa độ điểm x, y theo thời gian của A
Để mô hình giống thực tế hơn, ta có thể chuyển mô hình
3D từ những phần mềm CAD khác sang môi trường làm
việc của Matlab.
 Almeida University of Coimbra, Portugal, M o
Tokhi The University of Sheffield, UK G
SVirk, Advances in mobile Robotics. nhà xuất
bản World Scientific, 10 – 2008.

Hình 3.12-Tay máy chuyển động theo quỹ đạo

thẳng trong mặt phẳng

Hình3.13-mô hình tay máy chuyển động theo quỹ

đạo trong không gian

IV.KẾT LUẬN:
Matlab với thư viện mô phỏng hầu như đầy đủ cho
người sử dụng,do đó ta chỉ tập trung vào nghiên cứu mô
hình toán,cách giải thuật logic, tối ưu hóa việc tính toán
mô phỏng .Đây là công việc rất cần thiết cho sinh viên ,
nghiên cứu sinh ,kĩ sư các ngành kĩ thuật trong việc tính
toán các bài toán mô phỏng phức tạp.
Công cụ Simmechanics trong Matlab có ý nghĩa thực
tế cao đối với việc mô phỏng các cơ cấu máy móc chi
tiết trước khi tiến hành gia công chế tạo,hạn chế được
tối đa các sai sót trong thiết kế của người kĩ sư .
Hướng phát triển của đề tài là tiếp tục sử dụng công
cụ Simechanics trong việc xây dựng mô hình điều khiển
cho các chân của robot hoạt động đồng bộ theo hướng
có điều khiển…

THAM KHẢO
[1] Prof.RezaN.Jazar, Theory of Applied Robotics,
nhà xuất bản Springer, 2010.
[2] Gang Wang, Lixun Zhang, Dongliang Chen,
Defeng Liu and Xi Chen,”Modeling and
Simulation of multi-legged walking machine
prototype”.2009 International Conference on
Measuring Technology and Mechatronics
Automation.
[3] TS. Phạm Đăng Phước, Giáo trìnhRobot công
nghiệp, NXB ĐH Xây dựng, 2007.
[4] John J.Craig, Introduction to Robotics.
Mechanics and Control, 2nd Edition,NXB
Addison Wesley ,1989.
[5] L Marques -University of Coimbra, A de