Động lực học và điều khiển rôbot có cấu trúc dạng mạch hở

Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

LỜI MỞ ĐẦU
Robot được thiết kế để thực hiện những chức năng thay thế con người, thao tác tương tự
con người và thực hiện các tác vụ hỗ trợ con người. Chúng ta sẽ điều khiển chúng. Mục đích
của luận văn này là để phát triển một framework cho phép tính toán và điều khiển một lớp các
robot có cấu trúc mạch hở bất kỳ.
Ngày nay, với sự phát triển nhanh chóng trong nghiên cứu robot, hàng loạt các mẫu robot
được phát triển, phục vụ con người trên nhiều lĩnh vực với kết cấu đa dạng và phong phú. Thế
nhưng, xét trên khía cạnh cơ học thì có thể phân chia các robot này thành ba dạng chính. Thứ
nhất là dạng hệ có cấu trúc mạch hở, mà đại diện ra robot dạng tay máy, và dạng thứ hai là
robot có cấu trúc mạch kín, tiêu biểu là robot song song. Vấn đề tính toán động lực học và
điều khiển cho hai loại robot này có nhiều điểm khác biệt. Còn dạng thứ ba, đó là những loại
robot có thể chuyển đổi qua lại giữa hai dạng trên, từ dạng hở sang dạng kín và ngược lại, đó
là các robot di động có chân, chẳng hạn robot dáng người. Trong quá trình đi lại của robot,
tùy vào từng thời điểm, ví dụ khi hai chân cùng tiếp xúc, thì robot có dạng hệ kín, còn khi chỉ
có một chân tiếp đất, thì robot lại có dạng hệ hở. Trong phạm vi của luận văn này, chúng tôi
phát triển những phương pháp cho phép thực hiện việc điều khiển cho các robot loại thứ nhất
– dạng mạch hở. Để thực hiện việc điều khiển được cho robot của dạng thứ hai, cách thức
chúng tôi đang phát triển đó là thực hiện cắt liên kết để chuyển robot từ dạng mạch kín thành
dạng mạch hở, và thay thế các vết cắt đó bởi cặp phản lực liên kết. Việc lựa chọn điểm cắt
được thực hiện dựa trên lý thuyết đồ thị để đảm bảo chọn ra được điểm cắt tốt nhất, thuận lợi
nhất cho việc tính toán. Và để thực hiện việc điều khiển cho dạng thứ ba, chúng tôi thực hiện
ý tưởng dùng một ngắt chuyển đổi chương trình điều khiển theo trạng thái của robot. Tức là
khi robot ở dạng hệ kín, nó sẽ sử dụng chương trình điều khiển cho hệ kín, và khi chuyển
sang hệ hở, sẽ sử dụng chương trình điều khiển của hệ hở.
Frameworks hiện tại được phát triển nhằm mục đích cho phép điều khiển thời gian thực,
vì thế chúng tôi tập trung chủ yếu vào các giải thuật có chi phí tính toán thấp, và viết trên nền
tảng Matlab để thuận lợi cho việc thử nghiệm và kiểm tra. Và tới đây, chúng tôi hi vọng sẽ có
thể thử nghiệm được những kết quả tính toán này cho một mô hình robot cụ thể có kết cấu
phức tạp – robot dáng người - để kiểm nghiệm tính đúng đắn của các giải thuật được đưa ra.

Do những hạn chế về thời gian và trình độ hiểu biết nên chắc chắn sẽ không tránh khỏi
những thiếu xót trong quá trình thực hiện luận văn tốt nghiệp. Vì thế rất mong nhận được sự
nhận xét, đánh giá của quý thầy cô và các bạn để luận văn được hoàn thiện hơn.

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 1 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

LỜI CẢM ƠN
Trong suốt năm năm học đại học và hai năm học cao học tại trường Đại học Bách
Khoa Hà Nội, tôi đã có cơ hội theo đuổi những nghiên cứu về một chủ đề rất hấp dẫn trong
cuộc đời mình, đó là Robotics. Thời gian học đại học cho tôi cơ hội được tìm hiểu về các
robot di động và robot tay máy, và giờ đây, trong quá trình nghiên cứu ở cao học, tôi có điều
kiện để tìm hiểu sâu hơn về một mô hình robot rất lý thú nhưng cũng rất phức tạp, đó là robot
dáng người. Việc nghiên cứu về robot dáng người có liên quan đến nhiều lĩnh vực khoa học,
như robotics, nghiên cứu về vận động có chân (cơ - sinh), lập kế hoạch và thực thi điều khiển
theo mức ưu tiên, điều khiển phi tuyến… Tôi đã học được nhiều điều trong mỗi lĩnh vực và
kết hợp lại để hoàn thành mục tiêu nghiên cứu của mình.
Việc viết và hoàn thành luận văn là một nỗ lực cá nhân mạnh mẽ, bởi lẽ những dự định
nghiên cứu ban đầu không hoàn toàn giống với những kết quả thu được. Nó đòi hỏi sự kiên
nhẫn và phải chấp nhận rằng không phải mọi thực nghiệm đều đúng như mong đợi. Trong
quá trình thực hiện việc nghiên cứu, không ít lần tôi đã gặp thất bại hoặc đi sai hướng, nhưng
những thất bại như những dòng suối nhỏ, hợp lại để giúp tôi đạt được những thành công,
hoàn thành mục đích nghiên cứu đặt ra ban đầu của mình. Và tôi nhận ra rằng, để thành công,
không chỉ dựa trên những nỗ lực của cá nhân, mà còn cần có sự hợp tác với các giáo sư,
nghiên cứu sinh, các sinh viên và những người có cùng niềm ham thích đôi với vấn đề mình

quan tâm. Tôi muốn được gửi lời cảm ơn chân thành đến những người đã giúp tôi hoàn thành
luận văn này.
Trước tiên tôi muốn cảm ơn thầy Nguyễn Văn Khang, giáo viên hướng dẫn của tôi,
người đã truyền thụ cho tôi rất nhiều những kiến thức về động lực học và điều khiển, và cho
tôi nhiều lời khuyên vô cùng hữu ích những lúc tôi dường như bế tắc. Thầy cũng là người đã
chỉ cho tôi phương pháp nghiên cứu, cách tìm hiểu và giải quyết vấn đề. Đồng thời, tôi cũng
muốn gửi lời cảm ơn tới thầy Phan Bùi Khôi, giáo viên hướng dẫn của tôi thời đại học, người
đã cho tôi cơ hội để được thiết kế, chế tạo thực robot, từ đó học được nhiều những kinh
nghiệm thực tế vô cùng có giá trị đối với tôi. Thầy cũng là người cho tôi nhiều lời khuyển
chân tình về cuộc sống và công việc và nghiên cứu.
Bên cạnh đó, tôi cũng muốn gửi lời cảm ơn đền những bạn bè của tôi, những người có
cùng niềm ham mê nghiên cứu robot với tôi. Cám ơn Lê Đức Đạt, người bạn đã đọc những
bản thảo của tôi, cùng tôi trao đổi, cho tôi nhiều lời khuyên hữu ích và cũng là người giúp tôi
tìm được nhiều bài báo giá trị. Cám ơn Nguyễn Quốc Nam, người thường xuyên trao đổi với
tôi về thiết kế robot và dạy tôi nhiều về vấn đề xử lý ảnh, một vấn đề tôi rất quan tâm. Cám
ơn Nguyễn Hải Nguyên, người bạn cùng phòng và cùng lớp cao học với tôi, người đã động
viên, giúp đỡ tôi cố gắng đề hoàn thành luận văn này.
Ngoài ra, tôi cũng muốn gửi lời cám ơn tới những người đã luôn ở bên tôi trong suốt
thời gian tôi học cao học. Tôi muốn cảm ơn mẹ tôi, người đã động viên tôi đi học cao học,
luôn bên tôi, giúp đỡ tôi suốt thời gian qua và luôn hiểu tôi, chia sẻ với tôi những khó khăn.

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 2 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
Tôi muốn cám ơn bố tôi, người cố vấn và giúp tôi đưa ra những quyết định quan trọng của
mình, mặc dù bố tôi thường xuyên phải đi làm việc xa nhà.

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 3 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

MỤC LỤC
LỜI MỞ ĐẦU............................................................................................................................1 
LỜI CẢM ƠN ............................................................................................................................2 
CHƯƠNG 1 ...............................................................................................................................7 
GIỚI THIỆU ..............................................................................................................................7 
1. Các vấn đề động học và động lực học của hệ nhiều vật ....................................................7 
2. Tóm tắt các chương............................................................................................................7 
CHƯƠNG 2 ...............................................................................................................................9 
NHỮNG VẤN ĐỀ CƠ BẢN CỦA ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC HỆ NHIỀU VẬT ..9 
1. Xây dựng mô hình .............................................................................................................9 
1.1. Cấu trúc dữ liệu biểu diễn cho một vật rắn .................................................................9 
1.2. Quy tắc thiết lập quan hệ tọa độ trong hệ .................................................................12 
1.3. Lý thuyết đồ thị và ứng dụng trong bài toán hệ nhiều vật ........................................13 
1.3.1. Cách thiết lập dữ liệu cho bài toán đồ thị ..........................................................13 
1.3.2. Tìm kiếm ưu tiên độ sâu....................................................................................16 
1.3.3. Ví dụ áp dụng với mô hình robot dáng người....................................................21 
1.3.4. Tìm kiếm ưu tiên theo chiều rộng ....................................................................24 
2. Bài toán động học ............................................................................................................26 
2.1. Bài toán động học cho hệ dạng chuỗi nối tiếp ..........................................................26 
2.1.1. Bài toán động học thuận ....................................................................................26 
2.1.2. Bài toán động học ngược ...................................................................................28 

2.1.2.1. Đặt bài toán và các công thức cơ bản .........................................................28 
2.1.2.2. Thuật toán xác định q 0 = q ( t0 ) ...................................................................29 
2.1.2.3. Thuật toán xác định q k +1 = q ( tk +1 ) (k=0, 1, ..., n* - 1) ..............................30 
2.1.3. Các giải thuật bổ sung........................................................................................31 
2.1.3.1. Xác định vận tốc góc và gia tốc góc của vật rắn.........................................31 
2.1.3.2. Xác định vận tốc khối tâm của vật rắn:.......................................................33 
2.1.3.3. Giải thuật tính Jacobian J(q) .......................................................................34 
2.1.3.4. Giải thuật tính ma trận J&( q ) ......................................................................37 

2.1.3.5. Một số bài toán mở rộng ứng dụng trong trường hợp robot dư dẫn động ..39 
2.1.3.6. Vấn đề giới hạn khớp trong bài toán động học ngược................................40 
2.2. Bài toán động học cho hệ dạng cây ..........................................................................41 
2.2.1. Bài toán động học thuận ....................................................................................42 
2.2.2. Bài toán động học ngược ...................................................................................44 
3. Bài toán động lực học ......................................................................................................46 
3.1. Bài toán động lực học ngược ....................................................................................46 
3.1.1. Bài toán động lực học ngược cho hệ dạng chuỗi nối tiếp.................................46 
3.1.1.1. Công thức xác định vận tốc góc và gia tốc góc .........................................46 
Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 4 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
3.1.1.2. Công thức xác định gia tốc khối tâm vật rắn ..............................................46 
3.1.1.3. Công thức xác định lực. ..............................................................................47 
3.1.1.4. Công thức xác định mô men: ......................................................................48 
3.1.1.5. Giải thuật Newton – Euler Recusive...........................................................49 
3.1.1.6. Các ví dụ áp dụng .......................................................................................51 

3.1.2. Bài toán động lực học ngược cho hệ dạng chuỗi nối tiếp với trường hợp có nhân
tử lagrange liên kết.......................................................................................................54 
3.1.2.1. Công thức xác định lực ...............................................................................54 
3.1.2.2. Công thức xác định mô men: ......................................................................55 
3.1.3. Bài toán động lực học ngược cho hệ dạng cây ..................................................60 
3.2. Bài toán động lực học thuận cho hệ hở.....................................................................62 
3.2.1. Giải thuật tính N ( q,q&) ......................................................................................63 
3.2.2. Giải thuật tính M ( q ) ........................................................................................63 
3.2.3. Giải thuật mới để tính ma trận M(q) ..................................................................63 
CHƯƠNG 3: ............................................................................................................................68 
KIẾN TRÚC ĐIỀU KHIỂN VÀ ỨNG DỤNG .......................................................................68 
1. Kiến trúc điều khiển cho hệ .............................................................................................68 
KẾT LUẬN..............................................................................................................................72 
1. Tóm lược các kết quả và đóng góp ..................................................................................72 
2. Các nghiên cứu tương lai .................................................................................................72 
Tài liệu tham khảo ...................................................................................................................74 
PHỤ LỤC.................................................................................................................................79 
Phụ lục 1: .............................................................................................................................79 
Phụ lục 2: .............................................................................................................................82 

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 5 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
MỤC LỤC HÌNH ẢNH
Hình 2. 1: Mô hình vật rắn và các cấu trúc nhập liệu ..............................................................10 
Hình 2. 2: Sơ đồ cách thức cấu hình hệ dạng chuỗi nỗi tiếp ...................................................12 

Hình 2. 3: Đồ thị của cấu trúc liên kết của một hệ nhiều vật..................................................14 
Hình 2. 4: Lưu trữ dữ liệu đồ thị bằng ma trận kề dạng tam giác trên ....................................14 
Hình 2. 5: Lữu trữ dữ liệu bằng cấu trúc kề.............................................................................15 
Hình 2. 6: Phân tích nhập liệu cho hệ nhiều vật ......................................................................16 
Hình 2. 7: Lữu trữ dữ liệu bằng cấu trúc kề mở rộng ..............................................................16 
Hình 2. 8: Trình tự duyện qua các đỉnh và cạnh đồng thời xác định điểm cắt của một đồ thị
bằng giải thuật DFS .................................................................................................................18 
Hình 2. 9: Tóm lược ý nghĩa của DFS.....................................................................................19 
Hình 2. 10: Mô hình robot 35 DOFs được sử dụng trong luận văn.........................................21 
Hình 2. 11: Mô hình hóa robot sang dạng đồ thị .....................................................................22 
Hình 2. 12: Giải thuật duyệt theo chiều rộng...........................................................................25 
Hình 2. 13: Sơ đồ phân tầng ....................................................................................................25 
Hình 2. 14: Sơ đồ phương pháp số giải bài toán động học ngược...........................................31 
Hình 2. 15: Phép biến đổi từ hệ tọa độ tuyệt đối sang hệ tọa độ tương đối gắn với vật rắn....32 
Hình 2. 16: Dùng DFS để phân tích cấu trúc robot dáng người thành các chuối nối tiếp .......43 
Hình 2. 17: DFS cho mô hình robot dáng người .....................................................................45 
Hình 2. 18: Tính phân cấp(2 mức) trong viêc tính toán động học/ động lực học cho hệ nhiều
vật bằng phương pháp đệ quy ..................................................................................................60 

MỤC LỤC BẢNG BIỂU
Bảng 2. 1: Ký hiệu và ý nghĩa các tham số biểu diễn vật rắn trong hệ nhiều vật....................10 
Bảng 2. 2: Kết quả sau hàm DFS đối với cấu trúc liên kết......................................................23 
Bảng 2. 3:giải thuật truy hồi tính động học thuận robot dạng chuỗi .......................................27 
Bảng 2. 4: Giải thuật tính J(q)..................................................................................................37 
Bảng 2. 5: Giải thuật tính J&( q ) ...............................................................................................38 
Bảng 2. 6: Quy tắc ký hiệu cho hệ có cấu trục dạng cây .........................................................42 
Bảng 2. 7: kết phân tách một hệ dạng cây thành các chuỗi nối tiếp .......................................45 
Bảng 2. 8: Giải thuật RNE tính động lực học cho hệ dạng chuỗi nối tiếp...............................49 
Bảng 2. 9: giải thuật tính động lực học hệ dạng chuỗi nối tiếp khi có ngoại lực/moment tác
dụng..........................................................................................................................................58 

Bảng 2. 10: Giải thuật tính ma trận M ( q ) ..............................................................................66 

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 6 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

CHƯƠNG 1
GIỚI THIỆU
1. Các vấn đề động học và động lực học của hệ nhiều vật
Nền tảng cơ học của robotics đến từ Động lực học hệ nhiều vật. Các nghiên cứu về
động học, động lực học và điều khiển áp dụng cho robot dạng tay máy được phát triển mạnh
những năm 80 của thế kỷ trước, và sau đó, với sự phát triển nhanh chóng của khoa học máy
tính, hàng loạt các phương pháp số đã được phát triển, cho phép giải quyết những bài toán
động lực học cho hệ nhiều vật có cấu trúc phức tạp.
Hầu hết các phương pháp tính nhanh hiện nay đều dựa trên một phương thức tính là
đệ quy. Stepanenko và cộng sự (Stepanenko Y, 1976) lần đầu tiên đã thiết lập phương thức
đệ quy Newton Euler cho chuỗi không gian dạng hở. Trong công thức của họ, động học của
các khâu được biểu thị trong các khung cố định. Công thức Newton Euler Recursive được đề
xuất bởi Luh và cộng sự (Luh JYS, 1980) sử dụng các khung toạ độ động dường như đã trở
thành giải thuật hiệu quả nhất cho bài toán động lực học ngược, ít nhất cho cấu hình thông
thường của tay máy dạng nối tiếp với 6 bậc tự do (DOFs). Chi phí tính toán của phương pháp
này tỷ lệ tuyến tính với số lượng vật rắn có trong hệ, tức là độ phức tạp tính toán cỡ O(n), với
n là số lượng vật rắn có trong hệ. Hollerbach (Hollerbach, 1980) đưa ra dạng đệ quy với cách
tiếp cận theo dạng Lagrange, và từ đó đưa ra một giải thuật khác cũng có độ phức tạp cỡ O(n)
(WM, 1982) được coi là tương đương về hiệu năng tính toán so với phương pháp NE.
Sự phát triển giải thuật hiệu quả trong tính toán động lực học ngược được nhắc lại bởi

Walker và Orin [50] được sử dụng như là cơ sở cho việc đưa ra giải thuật tính toán động lực
học thuận. Đó là giải thuật cỡ O(n3) và được gọi là composite inertia method. Jain (Jain,
1991) đã đề xuất một giải thuật động lực học thuận được cho là hoàn hảo cho hệ nhiều vật
có cấu trúc nối tiếp cũng có chi phí tính toán cỡ O(n). Featherstone (FeatherStone, 1987)
cũng phát triển một giải thuật đệ quy đầy đủ cỡ O(n) cho bài toán động lực học thuận của hệ
dạng chuỗi hở sử dụng kí hiệu vector không gian. Trong đó, sử dụng các kí hiệu twist và
wrench là các vector 6x1 để đơn giản hóa việc phân tích động lực vật rắn. Bằng cách đưa ra
moment quán tính của vật rắn có khớp, ông ta cũng đưa ra công thức động lực học thuận cỡ
O(n), trong đó, gia tốc khớp có thể tính toán mà không cần phải thực hiện phép tính
nghịch đảo của ma trận khối lượng. Công thức cỡ O(n) của Featherstone đã có tác dụng rất
lớn trong việc phát triển giải thuật động lực học đối với hệ dạng cây , hệ dạng kín và tay máy
(Rodriguez G, 1991). Gần đây, Saha (Saha, 1999) đã phát triển giải thuật cỡ O(n) cho cả
động lực học thuận và ngược của hệ nhiều vật dạng nối tiếp sử dụng ma trận DeNOC. Trong
luận văn này, tôi đã dựa trên ý tưởng giải thuật đệ quy Newton – Euler để thực hiện việc tính
toán động lực học cho hệ nhiều vật dạng chuỗi nối tiếp và dạng cây. Và áp dụng những kết
quả đó cho mô hình robot dáng người.

2. Tóm tắt các chương
Luận văn này được tổ chức như sau. Trong chương 2, chúng tôi sẽ đưa ra các giải thuật
cơ bản dùng trong việc nhận dạng hệ nhiều vật, sử dụng các giải thuật của lý thuyết đồ thị.
Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 7 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
Tiếp đó, xây dựng các giải thuật đệ quy để thực hiện việc tính toán động học, động lực học
của hệ nhiều vật dạng chuỗi nối tiếp và dạng cây. Đây chính là cơ sở nền tảng để áp dụng cho
các chương tiếp theo.

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 8 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

CHƯƠNG 2
NHỮNG VẤN ĐỀ CƠ BẢN CỦA ĐỘNG HỌC VÀ
ĐỘNG LỰC HỌC HỆ NHIỀU VẬT
1. Xây dựng mô hình
Xây dựng mô hình là cách biểu diễn các liên kết giữa các vật rắn trong hệ nhiều vật để
nhằm đạt được những thuận lợi nhất định trong việc tính toán. Trong động lực học hệ nhiều
vật, được sử dụng nhiều nhất đó là các nguyên tắc thiết lập hệ tọa độ của Denavit –
Hartenberg và thiết lập hệ tọa độ Cray. Ưu điểm của các phương pháp này là dữ liệu đầu vào
là tối thiểu đảm bảo đủ thông tin. Về bản chất, các phương pháp thiết lập hệ tọa độ này đều
có chung một mục đích là đề tìm ra ma trận chuyển từ hệ tọa độ gắn với vật rắn này sang hệ
tọa độ gắn với vật rắn khác trong hệ. Trong luận văn của mình, tôi đưa ra một phương pháp
khác để thiết lập phép chuyển đổi giữa các hệ tọa độ của các vật rắn, phương pháp thiết lập
này có ưu điểm là thuận lợi cho việc chuyển đổi dữ liệu từ các phần mềm thiết kế như
Solidworks, Inventor sang mô hình tính toán trên matlab.

1.1. Cấu trúc dữ liệu biểu diễn cho một vật rắn

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 9 

Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

τ Q2( k )
FP1( k )
FP2( k )

τ Q1( k )

P2( k )

(k )

JCon2

Jtype1( k )
z

P1(

#k

rP2

JCon1( k )
Jtype2( k )
z

z

(k )

k)

rP1(

CSJ1( ) x
k

k)

y

rJ1( k )

rJ 2(

x
k

rPnP( k)
k

CSGk

rJ nJ( kk)

( )
JConnJ

k
k

CSJ nJ( kk)

z

y

k)

y

PnP( k)

CSJ 2( )
x
k

y

( )
JtypenJ
k
k

x

FPnP( kk )

(k )
τ QnQ

k

Hình 2. 1: Mô hình vật rắn và các cấu trúc nhập liệu

Cách thức cấu hình cho hệ dựa trên dạng dữ liệu là các vật rắn, với các thông tin cần thiết
như được mô tả trên hình 1 theo bảng sau:
Bảng 2. 1: Ký hiệu và ý nghĩa các tham số biểu diễn vật rắn trong hệ nhiều vật

Dạng dữ liệu chung
#k

CGk

Chỉ số vật rắn thứ (k) trong hệ.
Hệ tọa độ gắn với khối tâm của vật rắn #k. Do Matlab
không hỗ trợ chỉ số 0, nên ở đây sử dụng nền làm vật
rắn được đánh chỉ số là 1.

mk

Khối lượng của vật rắn #k

I Ck = [3 × 3]

Tensor quán tính của vật rắn #k lấy so với hệ tọa độ
gắn với khối tâm của nó CGk

Dạng dữ liệu khớp

C(Jki ) = [3 × 3], i = 1..nJ k

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Hệ tọa độ gắn với khớp thứ (i) của vật rắn #k được
tính so với hệ tọa độ gắn với khối tâm của vật rắn #k.
Ở đây có một số quy tắc thiết lập hệ tọa độ gắn với
Trang 10 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
khớp này như sau:
1. Nếu khớp (i) là khớp trượt thì hướng của trục z của
hệ tọa độ này sẽ phải trùng với trục quay của khớp
2. Nếu khớp (i) là khớp tịnh tiến thì trục z của hệ tọa
độ này phải trùng với hướng tịnh tiến của khớp.
Thực tế là hệ tọa độ này thực hiện chức năng chỉ
hướng, vì thế có thể sử dụng 3 góc RPY, 3 góc Euler
hoặc sử dụng quaternian để xác định. Ở đây hiện tại
hỗ trợ việc sử dụng 3 góc Euler XYZ:
EUL(i ) = [3 × 1]
k

nJ k

Tổng số khớp có trên vật rắn #k

rJ(ik ) = [3 × 1], i = 1..nJ k

Vector nối khối tâm của vật rắn #k với gốc của hệ tọa
độ gắn với khớp (i) của nó.
Kiểu khớp: nếu khớp (i) là khớp quay thì có giá trị 1,
nếu khớp (i) là khớp tịnh tiến thì có giá trị 0.

(k )
J type
, i = 1..nJ k
i
( )
qlim
= [ min
i
k

max ] , i = 1..nJ k

Giới hạn khớp (i).

Các thành phần ngoại lực tác dụng
rPi = [3 × 1] , i = 1..nPk
(k )

Vector vị trí điểm chịu lực Pi ( k ) xét trong hệ tọa độ
gắn với khối tâm của vật rắn #k

nPk
(k )

Pi

F

Tổng số ngoại lực tác dụng lên vật rắn #k.
= [ 4 × 1] , i = 1..nPk

τ Q( i ) = [ 4 × 1] , i = 1..nQk
k

nQk

Giá trị ngoại lực tác dụng lên điểm Pi ( k ) của vật rắn
#k. vector ngoại lực này được mô tả bởi 4 thành
phần, trong đó 3 thành phần đầu để mô tả hướng và
độ lớn. Thành phần thứ 4 để xác định hệ tọa độ mà
ngoại lực đó xét theo. Ở đây, nếu ngoại lực được xét
trong hệ tọa độ gắn với khối tâm vật rắn #k thì giá trị
của thành phần thứ tư này sẽ là 1. Còn nếu ngoại lực
được xét trong hệ tọa độ tuyệt đối, giá trị của thành
phần thứ 4 này sẽ là 0.
Mô ment ngoại tác dụng lên vật rắn thứ #k. Cũng như
ngoại lực ở trên, môment ngoại này cũng có 4 thành
phần có ý nghĩa tương tự.
Tổng số moment ngoại tác dụng lên vật rắn #k.

Sau khi nhập xong các dữ liệu này, chúng ta sẽ định nghĩa cách thức các vật rắn được kết nối
với nhau để tạo thành hệ. Việc đó được thực hiện bởi việc nhập dữ liệu cho mảng cell
JCon như sau:
( )

J Con
= [1× 2], i = 1..nJ k
i
k

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Biểu thị kết nối của khớp (i) của vật rắn #k đến vật rắn
khác. Ví dụ, giả sử nó sẽ kết nối với khớp s của vật rắn

Trang 11 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
(k )
#p, thì dữ liệu nhập vào sẽ có dạng: J Con
= [ p s ] . Và
i

tất nhiên, chúng ta cũng sẽ cần nhập dữ liệu
( p)
J Con
= [k
s

i] .

1.2. Quy tắc thiết lập quan hệ tọa độ trong hệ
Trong đoạn này ta chỉ xét các hệ vật rắn nối ghép với nhau bằng các khớp quay và các

khớp tịnh tiến. Khi đó quan hệ vị trí giữa hai khâu kế tiếp nhau có thể được xác định bởi hai
tham số khớp. Trên hình 3.1 khâu #i-1 nối với khâu #i bằng khớp i. Trong đó khâu đầu tiên,
thường là khâu cố định hoặc khâu có gia tốc góc và gia tốc dài đã biết, và được đánh chỉ số là
khâu #0. Khớp vào của một khâu sẽ được đánh chỉ số khớp là 1, khớp ra của nó được đánh
chỉ số khớp là 2 (như hình vẽ). Như vậy, trục z của hệ tọa độ khớp ra của vật rắn #i-1 sẽ trùng
với trục z của hệ tọa độ khớp vào của vật rắn #i: z1i ≡ z 2i−1 . Trường hợp nếu khớp (i) này là
khớp quay, thì trục z1i ≡ z 2i−1 sẽ chính là trục quay của khớp, và biến khớp là góc quay θi
quanh trục quay đó theo chiều từ x 2i−1 sang x1i . Nếu chiều quay để đưa được x 2i−1 về trùng
với x1i qua phép quay quanh trục z1i ≡ z 2i−1 như trên thuận chiều kim đồng hồ thì góc quay

θi của nó sẽ có giá trị dương, và ngược lại, sẽ có giá trị âm. Chú ý, các kí hiệu z1 , x1 là để chỉ
i

i

các vector đơn vị của hệ tọa độ gắn với khớp thứ (1) của vật rắn #i được xét trong hệ tọa độ
tuyệt đối, tương ứng với khớp thứ (2) của nó.
z1i ≡ z2i −1

z2i

TRi

CG i −1

CG i

z1i −1
CSJ1i −1

rJ

rJ 2i −1

TRi −1
rJ1i −1

CSJ1i

CSJ 2i −1

i
1

rJ 2i

CSJ 2i

#i

# ( i − 1)

TR1

CSJ 20
rJ 20

CG 0 ≡ WCS

Hình 2. 2: Sơ đồ cách thức cấu hình hệ dạng chuỗi nỗi tiếp

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 12 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
Sau đây là một số quy ước :
• Với mỗi khấu sẽ gồm có ba hệ tọa độ gắn với nó. Hệ tọa độ CGi gắn với khối tâm của vật
rắn. Hệ tọa độ CiJ1 và CiJ 2 gắn với các khớp vào và khớp ra của nó. Tức là hệ tọa độ CiJ1
sẽ trùng trục z và gốc tọa độ với hệ tọa độ CiJ−21 của khâu trước đó.
•

Ma trận cosin chỉ hướng chuyển từ hệ tọa độ CGi sang hệ tọa độ địa phương gắn với khớp
CiJ1 là: H i = Rϕxi Rθyi Rψz i (Ở đây được thiết lập theo 3 góc Euler XYZ)

•

Vector nối gốc Ci đến gốc của CiJ k là rJi k với k = 1,2 xét trong hệ tọa độ gắn với khối tâm
CGi .

•

Nếu khớp i là khớp quay, việc chuyển từ hệ tọa độ CiJ−21 sang hệ tọa độ CiJ1 được thực
hiện quanh trục quay z1i ≡ z 2i−1 góc quay là θi . Ma trận thực hiện việc đó là:
⎡Cθi
⎢
TRi = ⎢ Sθi
⎢0

⎣

•

− Sθi
Cθi
0

0⎤
⎥
0⎥
1 ⎥⎦

Nếu khớp (i) là khớp tịnh tiến thì việc chuyển đổi từ hệ tọa độ CiJ−21 sang hệ tọa độ CiJ1
được thực hiện bởi phép tịnh tiến dọc trục z1i ≡ z 2i−1 đoạn di . Ma trận thực hiện việc
chuyển hướng hệ tọa độ đó là TRi = E3 . Vector chuyển vị trí để thực hiện việc đó là z1i θi .

•

Ma trận cosin chỉ hướng chuyển từ hệ tọa độ CGi−1 sang hệ tọa độ CGi là Ti .

•

Khung tham chiếu của vật rắn i được chọn là trùng với CGi , tức là: Fi ≡ CGi .

•

Khâu đầu tiên được gọi là khâu 0, có CG0 ≡ WCS ( world cordinate system ) .

•

Tọa độ của cơ cấu chấp hành khâu cuối mặc dù không kết nối với vật rắn nào phía sau,
nhưng để thuận lợi cho việc tính toán, ta đặt nó bởi hệ tọa độ CnJ2

1.3. Lý thuyết đồ thị và ứng dụng trong bài toán hệ nhiều vật
Trong các hệ nhiều vật thực tế, có rất nhiều các vật rắn được liên kết với nhau với cấu
trúc rất phức tạp. Để có thể tính toán được cho các hệ phức tạp đó, công việc đầu tiên bao giờ
cũng là phân tích nó thành các hệ đơn giản hơn đã có cách tính toán, trong hệ nhiều vật, dạng
đơn giản đó là dạng chuỗi đơn nối tiếp. Công việc đơn giản hóa hệ nhiều vật có cấu trúc phức
tạp này thành các chuỗi đơn thường được thực hiện nhờ phương pháp đồ thị đã có từ lâu
trong toán học.

1.3.1. Cách thiết lập dữ liệu cho bài toán đồ thị
Để xử lý các đồ thị bằng chương trình máy tính, trước hết chúng ta phải tìm cách biểu
diễn được chúng trên máy tính. Thông thường, với đồ thị người ta hay sử dụng hai phương
pháp biểu diễn, đó là biểu diễn dạng ma trận kề (thích hợp với các bài toán có số lượng cạnh
lớn) và biểu diễn dạng xâu kề (thích hợp với những bài toán có số lượng cạnh nhỏ ).

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 13 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
Biểu diễn ma trận kề là cách thức biểu diễn trực quan nhất. chúng ta sử dụng một mảng hai
chiều kích thước VxV , mỗi phần tử của nó nhận một giá trị boolean, trong đó a[x,y] nhận giá
trị true - 1 nếu có một cạnh từ đỉnh x nối tới đỉnh y và nhận giá trị false - 0 nếu ngược lại.
#0

#14

#13

#7

#1

#6

#9

#2

#8

#10

#3

#15

#11

#4

#17

#5

#16

#12

#19

#18

# 20

Hình 2. 3: Đồ thị của cấu trúc liên kết của một hệ nhiều vật
Ví dụ xét một cây như hình vẽ trên, thì bảng ma trận kề của nó sẽ có dạng như sau:
#0 #1 #2 #3 #4 #5 #6 #7 #8 #9 #10 #11 #12 #13 #14 #15 #16 #17 #18 #19 #20
#0 1 1 0 0 0 0
#1
1 1 0 0 0
#2
1 1 0 0
#3
1 1 0
#4
1 1
#5
1

0
1
0
0
0

0

0
0
1
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0

0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
1

0
0
0
0
0
0

#6
#7
#8
#9

#10
#11

1 0 1 0
1 0 0
1 0
1

0
0
0
1
1

0
0
0
0
1
1

0
0
1
0
0
0

0
0

0
0
1
1

0
1
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0

0
0

0
0
0
0
0
0

0
0
0
0
0
0

0
0
0
0
0
0

1

0
1

0

0
1

1
0
0
1

0
1
0
0
1

0
0
0
1
0
1

0
0
0
0
0
1
1

0

0
0
0
0
0
1

0
0
0
0
0
0
0

1

1
1

#12
#13
#14
#15
#16
#17
#18
#19
#20

0
1
0
0
0
0

0
0
1
0
0
0

Hình 2. 4: Lưu trữ dữ liệu đồ thị bằng ma trận kề dạng tam giác trên
Chú ý rằng mỗi cạnh được biểu diễn thực sự bằng hai bit, một cạnh nối x và y sẽ được
biểu diễn bằng giá trị true trong cả hai phần tử của mảng a[x,y] và a[y,x]. thông thường, để
tiết kiệm bộ nhớ ta có thể sử dụng một nửa của ma trận này như cách ở trên. Nhưng trong
Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 14 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
một số trường hợp khác, để giải thuật đơn giản, người ta có thể sử dụng ma trận đầy. Tương
tự, người ta thường quy ước một cạnh nối từ một đỉnh tới chính nó luôn nhận giá trị true, tức
a[x,x] = 1.
Theo cách thức biểu diễn ma trận kề ở trên, số lượng bit cần thiết để lưu trữ là V2, và
thường cần V2 bước để khởi tạo nó. Trong trường hợp nếu số cạnh trong ma trận xấp xỉ bằng

V2 thì cách biểu diễn theo ma trận kề như thế này là phù hợp. Tuy nhiên, nếu trường hợp ma
trận là thưa thì việc sử dụng ma trận kề như trên sẽ tốn khá nhiều thời gian cho việc khởi tạo
so với thời gian chạy thuật toán. Từ đó, nảy sinh ra một dạng cấu trúc lưu trữ nhẹ hơn, phù
hợp với trường hợp hệ dạng thưa, đó là biểu diễn đồ thị dạng cấu trúc kề. Trong biểu diễn
cấu trúc kề, tất cả các đỉnh trong đồ thị được nối tới một đỉnh trong đồ thị sẽ được đặt vào
trong danh sách kề của đỉnh đó. Cấu trúc này có thể cài đặt được một cách dễ dàng bằng cách
dùng một mảng hoặc xâu liên kết. Để thêm một cạnh nối x tới đỉnh y trong biểu diễn này,
chúng ta thêm x vào xâu kề của y và thêm y vào xâu kề của x như trong hình vẽ thứ 2:
#0 #1 #2 #3 #4 #5 #6 #7 #8 #9 #10 #11 #12 #13 #14 #15 #16 #17 #18 #19 #20
1 0 1 2 3 4 1 1 2 2 9 10 8 10 7 12 13 15 17 5 19
2
6
7

3
8
9

4

5 19 8 14 6 10 11
12
13

13

15

11
16

17

18

19

18
20

Hình 2. 5: Lữu trữ dữ liệu bằng cấu trúc kề
Một lần nữa chúng ta hãy chú ý rằng, mỗi cạnh được biểu diễn hai lần: một cạnh nối x
với y được biểu diễn bởi một nút chứa x nằm trong danh sách kề của y và một nút chứa y
nằm trong danh sách kề của x. Việc lưu trữ hai lần như trên rất quan trọng, vì nếu ngược lại
thì các câu hỏi chẳng hạn như các nút nào được nối trực tiếp tới nút x sẽ không được trả lời
nhanh chóng.
Trong cách biểu diễn này, thứ tự xuất hiện các cạnh trong dữ liệu được nhập vào là vô cùng
quan trọng, và tùy theo thứ tự các đỉnh được nhập vào mỗi xâu kề, ta lại có một dạng ma trận
kề khác nhau. Nói cách khác, cùng một đồ thị, nhưng với cách nhập thứ tự các đỉnh trong các
xâu kề khác nhau sẽ tạo ra các cấu trúc kề khác nhau. Thứ tự các cạnh trong xâu kề cũng ảnh
hưởng tới thứ tự xử lý nó trong thuật toán.
Qua phân tích trên, có thể nhận thấy ý tưởng về cách thức lưu trữ của cấu trúc kề này
rất thuận lợi cho việc áp dụng đối với bài toán nhận dạng hệ nhiều vật. Và nó cũng là cơ sở
cách thiết lập dữ liệu nhập được sử dụng trong chương trình tính toán robot dáng người mà e
sử dụng trong luận văn này. Tuy nhiên, do có những đặc tính riêng cần thiết cho chương trình
tính toán nên ở đây em phát triển dạng cấu trúc kề mở rộng: thay vì mỗi xâu kề chỉ là một
danh sách các vật rắn được kết nối với vật rắn hiện tại, nó còn cần phải lưu trữ thêm chỉ số
khớp của vật rắn hiện tại, chỉ số khớp của vật rắn được nó kết nối với và chỉ số vật rắn mà nó
được kết nối với như được mô tả trong hình vẽ sau:

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 15 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

#0
#14

#7

#1

#6

#9

#2

#8

#10

#3

#15

#11

#4

#17

#13
#16

#5

#19

#12

#18

# 20
Hình 2. 6: Phân tích nhập liệu cho hệ nhiều vật
Trong đó kí hiệu # để chỉ vật rắn, còn chỉ số nhỏ để chỉ số thứ tự của các khớp của vật
rắn đó. Chú ý là các khớp phải được đánh số từ 1 cho đến tổng số khớp của vật rắn đó.
#0
#1
#1 1 # 0 1
#6 1
#2 1
#7 1

#2
#3
#4

#5
#6
#7
#8
#9
#10
#11
#12
#13
#14
#15
#16
#17
#18
#19
# 20
#1 3 # 2 3 #3 2 # 4 2 #1 2 #1 4 # 6 2 # 2 4 #9 2 #10 2 #8 2 #10 3 # 7 2 #12 2 #13 3 #15 2 #17 2 #5 2 #19 2
#8 3 # 4 1 #5 1 #19 1 #8 1 #14 1 #12 1 #10 1 #11 1 #13 2 #15 1 #11 2
#17 1
#18 1 #19 3 # 20 1
#3 1
#2 2
#13 1
#16 1
#18 2
#9 1

Hình 2. 7: Lữu trữ dữ liệu bằng cấu trúc kề mở rộng
Chú ý, đáng lẽ mỗi thành phần phải được lưu bởi 3 chỉ số, nhưng để tiết kiệm lượng
biến cần lưu, nên mỗi thành phần ta chỉ cần phải lưu hai chỉ số như ở trên hình vẽ. ví dụ, với

vật rắn #2, Nó được kết nối khớp 1 với vật rắn #0 bởi chỉ số khớp 1 của vật rắn #0 đó. Tiếp
đó, khớp 2 của nó sẽ được nối với khớp 3 của vật rắn #8. Khớp 3 của nó được nối với khớp 1
của vậ rắn #3. Và khớp 4 của nó được nối với khớp 1 của vật rắn #9.
Đây cũng chính là nguyên tắc nhập dữ liệu cho ma trận Jcon trong chương trình. Tác dụng
của cách biểu thị ma trận như thế này sẽ được thấy ở phần sau, đó là giải thuật để duyệt đồ thị.

1.3.2. Tìm kiếm ưu tiên độ sâu
Trong bài toán đồ thị, có các câu hỏi thường đặt ra như sau: Đồ thị có liên thông hay
không, nếu không, hãy cho biết các thành phần liên thông của đồ thị? Đồ thị có một chu trình
nào hay không? Các xác định ra các chu trình tối thiểu?...Những câu hỏi như vậy, và nhiều
các câu hỏi khác có thể được giải quyết bằng một giải thuật, được gọi là tìm kiếm ưu tiên
theo độ sâu (Depth First Search). Đây là một phương pháp tự nhiên để duyệt qua tất cả các
nút và các cạnh của đồ thị một cách có hệ thống.
Sau đây sẽ là dạng giải thuật DFS đệ quy để áp dụng với cấu trúc kề mở rộng đã nói ở trên:

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 16 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
function visit(k)
i=1;
while i<=length(JCon{k}) %duyet tat ca cac VR duoc ket noi voi VR k
if visited(JCon{k}(i,1)) = 0 % neu VR chua duoc duyet thi duyet no
visited(JCon{k}(i,1)) = 1; % bao vat ran do da duoc duyet
% ham duyet vat ran
visit (Jcon{k}(i,1)); % chuyen sang VR do de duyet cac VR ket noi voi no
else

%ham xu ly truong hop VR do da duyet roi
end
i = i+1;
end
end
function DFS()
visited = zeros(length(JCon),1); % luc dau, tat ca cac VR trong he deu chu duoc duyet
for i = 1:length(JCon) % duyet tat ca cac vat ran trong he
if visited(i) = 0 % neu VR i chu duoc duyet
visit(i); % thi duyet VR do
end
end
end

Để minh họa về giải thuật này, ta xem hình vẽ minh họa sau:

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 17 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
#0
#14

#13

#1

#6

#9

#2

#8

#10
#11

#1

#6

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#1

#6

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#19

#14

#5

# 20

#13

#1

#6

#9

#2

#8

#10

#11

#1

#6

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#5

#19

#13

#6

#9

#2

#8

#10
#11

#11

#18

#16

#17

#13

#11

#18

#16

#14

#16

#13

#11

#18

#16

#13
#16

#6

#9

#2

#8

#10
#11

#16

#11

#18

#16

# 20

#8

#3

#15

#10

#3

#4

#17

#13

#11

#4

#18

#16

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#19

#17
#19

#6

#12

#9

#2

#8

#3

#15

#10

#3

#4

#17

#13

#11

#4

#18

#16

#5

#18

# 20

#1

#19

#14

#5

# 20

#12
#15
#17

#19

#18

# 20

#0

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#7

#1

#6

#12

#9

#2

#8

#3

#15

#10

#3

#4

#17

#13

#11

#4

#18

#16

#5

#5

#12

#0

#12

#5

#18

#15

#7

# 20

#14

#19

#6

#6

# 20

#2

#1

#1

#19

#9

#7

#7

#5

#6

#12

#5

#0

#13

#1

#14

# 20

#14

#17
#19

# 20

#0

#18

#14

#5

#12
#15

#7

# 20

#11

#17
#19

#16

#13

#4

#5

#18

#19

#18

#0

#11

#17

#10

#16

#13

#4

#15

#18

#19

#17

#10

#3

#4

#4

#15

#8

#11

#10

#3

#2

#13

#15

#8

#9

#17

#3

#2

#12

#4

#8

#9

#6

#3

#2

#12

#1

#10

#9

#6

#7

#14

#15

#12

#5

#0

#1

#8

#3

#6

# 20

#0
#7

#2

#1

#1

#19

#9

#7

#7

#5

#6

#12

#5

#0

#17

#1

#14

# 20

#14

#17
#19

# 20

#0

#18

#14

#15

#7

# 20

#0

#19

#16

#19

#14

#12

#0

#18

#5

#5

# 20

#11

#11

#4

#16

#13

#13

#10

#18

#19

#17

#17

#15

#4

#4

#4

#3

#11

#10

#10

#8

#13

#15

#15

#2

#17

#3

#3

#9

#4

#8

#8

#12

#3

#2

#2

#6

#10

#9

#9

#1

#15

#12

#12

#7

#8

#3

#6

# 20

#14

#2

#1

#6

#19

#9

#7

#1

# 20

#14

#14

#7

#5

#6

#12

#5

#0

#4

#5

#16

#19

#1

#14

#0

#13

#10

#11

#16

# 20

#0

#13

#18

#19

#17

#15

#10

#11

#4

#3

#8

#13

#10

#8

#2

#17

#15

#2

#9

#4

#3

#9

#6

#10

#8

#12

#1

#15

#2

#6

#7

#3

#9

# 20

#14

#8

#12

#1

#19

#2

#6

#7

#5

#16

#9

#7

# 20

#0

#1

#12

#1

#5

#0
#7

#6

#7

# 20

#14

#1

#14

#0

#7

#14

#0

#7

#5

#0

#7

#16

#19

#14

# 20

#0
#14

#0

#7

#7

#5

#16

#0

#0

#7

#19

# 20

#14

#5

#12
#15
#17

#19

#18

# 20

Hình 2. 8: Trình tự duyện qua các đỉnh và cạnh đồng thời xác định điểm cắt của một đồ
thị bằng giải thuật DFS
Từ hình ảnh trên, ta có thể rút ngắn lại việc phân tích hệ trên lại thành hình dưới đây:

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 18 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở

#0
#14

#13
#16

#0
#7

#1

#6

#12

#9

#2

#8

#3

#15

#10

#3

#15

#4

#17

#13

#4

#17

#18

#16

#7

#1

#6

#9

#2

#8

#10

#11

#5

#19

#14

#11

# 20

#5

#19
# 20

#12

#18

#0

#19

#2

#1

#1

#18

#9

#7

#2

#17

#10

#14

#3

#15

#11

#4

#12

#13

#5

#8

#16

#19

#6

# 20

Hình 2. 9: Tóm lược ý nghĩa của DFS
Với giải thuật DFS như ở trên, có thể nhận biết được ngay số lượng các thành phần
liên thông của đồ thị, nó chính bằng số lượng các lần được gọi đến hàm visit trong hàm DFS.
Đồng thời, nó cũng cho phép ta xác định được hệ có chu trình hay không, và cạnh nào sẽ là
cạnh tạo vòng kín. Ở đây, cạnh tạo vòng kín là cạnh mà tại đó nút trong hàm visit ở trên, nó
gặp nút đã được duyệt rồi.
Cách xây dựng giải thuật DFS như ở trên, được gọi là DFS đệ quy. Sự đệ quy đó có
thể được khử bằng cách sử dụng một ngăn xếp. Việc khử đệ quy sẽ cho ta một cài đặt thuật
toán khác đơn giản hơn, và hiển nhiên, tiết kiệm bộ nhớ hơn so với phương pháp đệ quy. Và
đây cũng là phương pháp được sử dụng trong luận văn trong việc duyệt cây.
Để thực hiện được giải thuật này, trước hết ta cần xây dựng một ngăn xếp. Các đỉnh đã được
“gặp” nhưng chưa được “thăm” sẽ lưu vào ngăn xếp đó. Để thăm một đỉnh, chúng ta sẽ duyệt
qua tất cả các cạnh của nó và đặt vào ngăn xếp bất kì đỉnh nào chưa được thăm và chưa có
trong ngăn xếp. Với hệ nhiều vật, trong chương trình cài đặt một ngăn xếp được hỗ trợ bởi
các hàm push và pop như sau:
% a: chi so vat ran

% b: chi so khop lien ket cua VR do
% c: chi so vat ran ke tiep phia sau cua VR do
% d: chi so khop lk cua VR sau do voi VR hien tai
% e: chi so khop vao cua VR hien tai
function id = Push(a,b,c,d,e)
global stack;
global stackID;
stackID = stackID +1;
stack(stackID,:)=[a,b,c,d,e];
end
function [a,b,c,d,e] = Pop()
global stack;
global stackID;
[a,b,c,d,e] = stack(stackID,:);
stackID = stackID -1;
end

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 19 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
Và ứng dụng giải thuật DFS vào việc phân tích một hệ nhiều vật dạng cây ra thành
một hệ các nhánh cây có dạng nối tiếp, được lưu vào cell SB như sau:
function SB = DFS(JCon)
global stack;
global stackID;
stack = [];

stackID = 0;
n = length(JCon); % so luong vat ran trong he
SBid = 1; % chi so mang cua SB
Checked = zeros(n,1);
[n1,n2] = size(JCon{1});
for i = 1:n1
Push(1,i,JCon{1}(i,1),JCon{1}(i,2),0); % nap tat ca cac lien ket tu VR 1 vao stack
end
Checked(1) =1; % xac nhan phan tu 1 da duoc duyet.
while stackID ~= 0
SBI = [];
SBIid = 1;
while 1 % lay ra lien ket den VR tiep theo va kiem tra lien ket do co phai lk tao vong kin ko?
[ObjID,JID,CObjID,CJID,iJID] = Pop();
if Checked (CObjID) == 0
break;
end
end;
SBI = [SBI;[ObjID,iJID,JID]];
% sprintf('q%d%d',ObjID,CObjID)
SBIid = SBIid + 1;
while 1
if Checked(CObjID) == 0 % vat ran chua duoc duyet
Checked(CObjID) = 1;
[n1,n2] = size(JCon{CObjID});
if n1 == 1 % neu la vat ran nam o cuoi chuoi
SBI = [SBI;[CObjID,CJID,0]];
break;
end;
for i = 1:n1

if i ~= CJID
Push(CObjID,i,JCon{CObjID}(i,1),JCon{CObjID}(i,2),CJID);
end;
end;
SBI(SBIid,1)= CObjID;
SBI(SBIid,2) = CJID;

%

[ObjID,JID,CObjID,CJID,iJID] = Pop();
sprintf('q%d%d',ObjID,CObjID)

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 20 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
if Checked(CObjID) == 1 % neu vat ran dc ket noi da duyet -> vong kin
% -> pha vo lien ket tai day.
SBI(SBIid,3) = 0;
break;
else
SBI(SBIid,3) = JID;
SBIid = SBIid + 1;
end;
else % neu vat ran da duoc duyet -> pha vo lien ket o day
break;
end;

end;
SB{SBid} = SBI;
SBid = SBid + 1;
end;

1.3.3. Ví dụ áp dụng với mô hình robot dáng người
Mô hình robot dáng người 35 DOFs được sử dụng ở đây như sau:

Hình 2. 10: Mô hình robot 35 DOFs được sử dụng trong luận văn
Dạng mô hình hóa:
Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 21 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
#16

#15

#14

#4

#5

#6

#7

#17

#3

#8

#18

#2

#9

#19

# 29

#10

# 20

# 28

#30

# 21

# 27

#31

#12

# 22

# 26

#32

#13

# 25

#33

# 24

#34

# 23

#35

#1

#36

#11

Hình 2. 11: Mô hình hóa robot sang dạng đồ thị

Và cấu trúc kề mở rộng của nó có dạng:
JCon{1} = [23,1];
JCon{23} = [1,1;24 1];
JCon{24} = [23 2;25 1];
JCon{25} = [24 2;26 1];
JCon{26} = [25 2;27 1];
JCon{27} = [26 2;28 1];
JCon{28} = [27 2;29 1];
JCon{29} = [28 2;2 1;30 1];
JCon{2} = [29 2;3,1];
JCon{3} = [2,2;4,1];
JCon{4} = [3,2;14,1;5,1];
JCon{30} = [29 3;31 1];
JCon{31} = [30 2;32 1];
JCon{32} = [31 2;33 1];
JCon{33} = [32 2;34 1];
JCon{34} = [33 2;35 1];
JCon{35} = [34 2;36 1];
JCon{36} = [35 2];

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 22 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
JCon{5} = [4,3;6,1];
JCon{6} = [5,2;7,1];
JCon{7} = [6,2;8,1];

JCon{8} = [7,2;9,1];
JCon{9} = [8,2;10,1];
JCon{10} = [9,2;11,1];
JCon{11} = [10,2;12,1];
JCon{12} = [11,2;13,1];
JCon{13} = [12,1];
JCon{14} = [4,2;15,1];
JCon{15} = [14,2;16,1];
JCon{16} = [15,2;17,1];
JCon{17} = [16,2;18,1];
JCon{18} = [17,2;19,1];
JCon{19} = [18,2;20,1];
JCon{20} = [19,2;21,1];
JCon{21} = [20,2;22,1];
JCon{22} = [21,2];

Bảng 2. 2: Kết quả sau hàm DFS đối với cấu trúc liên kết
SB{1}=

SB{2}=

SB{3}=

1

0

1

1

29

1

2 15

4

1

2 27

23

1

2

2

2

1

2 16

14

1

2 28

24

1

2

3

3

1

2 17

15

1

2 29

25

1

2

4

4

1

3 18

16

1

2 30

26

1

2

5

5

1

2 19

17

1

2 31

27

1

2

6

6

1

2 20

18

1

2 32

28

1

2

7

7

1

2 21

19

1

2 33

29

1

3

8

8

1

2 22

20

1

2 34

30

1

2

9

9

1

2 23

21

1

2 35

31

1

2 10

10

1

2 24

22

1

0

32

1

2 11

11

1

2 25

33

1

2 12

12

1

2 26

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

0

Trang 23 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
34

1

2 13

35

1

2 14

36

1

0

13

1

0

0

0

Kết quả ở đây là một cell SB, trong đó mỗi thành phần SB{i} là một ma trận tương
ứng với một nhánh được tách ra từ hệ, như mô tả trong hình 2.7.
Ở đây ta giải thích các thành phần của ma trận nhánh SB{i} như sau:
Cột đầu tiên của ma trận là chỉ số của vật rắn
Cột thứ 2 cho ta biết chỉ số “khớp vào” của vật rắn đó.
Cột thứ 3 cho ta biết chỉ số “khớp ra” của vật rắn
Cột thứ 4 cho ta chỉ số biến khớp của khớp kết nối giữa vật rắn hiện thời với vật rắn
nằm ở hàng dưới nó
Khớp vào và khớp ra ở đây có ý nghĩa để xác định được thứ tự vật rắn trong chuỗi tính
từ gốc lên.
Để hiểu rõ hơn, ta lấy một ví dụ: Xét SB{1}, như vậy vật rắn #1 (nằm ở hàng 1, cột 1)
sẽ không được kết nối với vật rắn nào trước đó (do chỉ số vào của nó = 0), hay nó chính là vật
rắn gốc (base). Vật rắn #1 đó kết nối với vật rắn #23 (nằm ở hàng 2, cột 1) bởi khớp số 1 của
nó(chính là chỉ số ra của #1). Và ngược lại, vật rắn #23 được kết nối với vật rắn #1 bởi khớp
1 của nó (chính là chỉ số vào của #23).
Ngoài ra, ở vật rắn #36 chẳng hạn, chỉ số ra của nó bằng 0 có nghĩa là nó không kết nối
thêm với vật rắn nào phía sau cả.

1.3.4. Tìm kiếm ưu tiên theo chiều rộng
Cũng như tìm kiếm ưu tiên theo chiều sâu, tìm kiếm ưu tiên theo chiều rộng là một giải
thuật phổ biến trong duyệt đồ thị. Việc duyệt theo chiều rộng được thực hiện theo ý tưởng
phân tầng các đối tượng được duyệt, sau đó duyệt trong từng tầng đó. Ý tưởng này chính là
nền tảng cơ bản để em đưa ra đề xuất giải thuật duyệt hệ kín và cắt liên kết “tốt” để chuyển từ
hệ kín thành hệ hở tương ứng. Trước hết, chúng ta hãy cùng xem một ví dụ về trình tự duyệt
một hệ theo chiều rộng như thế nào:

Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 24 


Luận văn cao học: Động lực học và điều khiển robot dạng mạch hở
#0
#14

#13
#16

#0

#0

#7

#1

#6

#9

#2

#8

#10

#11

#1

#6

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#1

#6

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#5

#19

#5

# 20

#14

#19

#1

#6

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#5

#13

#6

#9

#2

#8

#10

#11

#1

#6

#12

#9

#2

#8

#3

#15

#10

#3

#4

#17

#13

#11

#4

#18

#16

#19

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

#12

#9

#2

#8

#3

#15

#10

#4

#17

#13

#11

#18

#16

# 20

#5

#19

#14

#15
#17
#19

#1

#6

#12

#9

#2

#8

#3

#15

#10

#3

#4

#17

#13

#11

#4

#18

#16

#19

#14

#5

#12

#15
#17

#19

#18

# 20

# 20

# 20

#18

# 20

#7

#5

#12

#0
#6

#6

#19

#5

#0

#1

#1

#14

#14

# 20

#7

#7

#5

#7

#5

#0

#1

#16

#19

#14

# 20

#0
#7

#0

#7

# 20

#14

#0

#7

#7

#14

Hình 2. 12: Giải thuật duyệt theo chiều rộng
Một đặc điểm rất đáng quan tâm của phương pháp duyệt cây theo chiều rộng này đó là
nó thể hiện tính chất phân tầng của từng vật rắn chứa trong hệ. Như vậy, vị trí tầng mà một
vật rắn đang đứng cũng chính bằng với số vật rắn trung gian để đi được từ gốc lên đến nó.
Hay nói cách khác, duyệt cây theo chiều rộng sẽ cho chúng ta tìm được con đường ngắn nhất

từ gốc đến một vật rắn bất kì trong hệ, đây là nền tảng cơ bản để giúp tối ưu hóa việc chọn
điểm cắt trong bài toán hệ kín. Để thấy rõ điều đó, chúng ta hãy xem lại thí dụ trên, nhưng
được nhìn theo ý nghĩa phân tầng:

1

#0

2

#1

3

4

5

6

#6

#8

#12

#15

#17

#18

#2

#3

#4

#5

#19

# 20

#7

#9

#10

#11

#16

#14

7

8

#13

Hình 2. 13: Sơ đồ phân tầng
Như vậy, trong hình vẽ trên, ví dụ vật rắn #15 sẽ nằm ở tầng thứ 4, và con đường ngắn
nhất để đi từ gốc đến nó sẽ duyệt qua 4 phần tử (tính cả chính nó và phần tử gốc). Chúng ta
cũng xét thêm một ví dụ về việc chọn điểm cắt chia một hệ kín thành các hệ đơn có độ dài tối
thiểu. Trong hệ trên, nếu ta luôn ưu tiên duyệt tới vật rắn kề có chỉ số nhỏ hơn, khi đó, vòng
kín (1, 2, 8, 6) sẽ có thể chọn được 2 điểm cắt là (2, 8) và (6, 8). Do chọn ưu tiên cho phần tử
có chỉ số thấp hơn, nên ta sẽ chọn được điểm cắt tại liên kết (6, 8); Tương tự, với vòng kín
(2,3, 4, 5, 19, 18, 17, 15, 12, 8), có thể chọn được hai điểm cắt là (17, 18) và (19, 18). Nhưng
do ưu tiên cho liên kết với chỉ số thấp hơn, nên ta sẽ chọn được điểm cắt là liên kết (19, 18).
Học viên: Nguyễn Mạnh Tiến 
Lớp: Cơ học kỹ thuật 2008 - 2010

Trang 25