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

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ỡ On, 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ín

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

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

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à

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 địnhq0 =q( )t0 29 

2.1.2.3 Thuật toán xác định qk+1=q( )t k+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 

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 

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ầ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ị

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

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

z x y

CSJ

( )

k

k nJ

rP

( )

k

k nP

Q

τ

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 Chỉ số vật rắn thứ (k) trong hệ

k

G

C 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

I 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ó

C 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

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:

( )k [ ]3 1

i EUL = ×

r 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ó

( ) , 1

k

J i= 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

( ) [4 1 ,] 1

k

τ 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ự

k

Q

n 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:

( )k i [1 2], 1 k

J = × i= n 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

#p, thì dữ liệu nhập vào sẽ có dạng: ( )k i [ ]

Con

J = p s Và tất nhiên, chúng ta cũng sẽ cần nhập dữ liệu

( )p s [ ]

Con

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:

z z 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ừ x2i−1 sang x1i Nếu chiều quay để đưa được x2i−1 về trùng với 1

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 độ

C và

2

i J

C gắn với các khớp vào và khớp ra của nó Tức là hệ tọa độ

1

i J

C

sẽ trùng trục z và gốc tọa độ với hệ tọa độ

2 1

i J

−

C của khâu trước đó

• Ma trận cosin chỉ hướng chuyển từ hệ tọa độ

H R R R (Ở đây được thiết lập theo 3 góc Euler XYZ)

• Vector nối gốc C iđến gốc của

k

i J

C là

k

i J

r với k = 1,2 xét trong hệ tọa độ gắn với khối tâm

i J

−

C sang hệ tọa độ

1

i J

C được thực

hiện quanh trục quay z1i ≡z2i−1 góc quay làθi Ma trận thực hiện việc đó là:

00

i J

−

C sang hệ tọa độ

1

i J

C

được thực hiện bởi phép tịnh tiến dọc trục

1

1i ≡ 2i−

z z đoạn d Ma trận thực hiện việc i

chuyển hướng hệ tọa độ đó là TR i =E Vector chuyển vị trí để thực hiện việc đó là 3 z1iθi

• Ma trận cosin chỉ hướng chuyển từ hệ tọa độ CG i−1 sang hệ tọa độ

• 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 độ

2

n J

C

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ỏ )

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

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:

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

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

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ì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 đó

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:

function visit(k)

i=1;

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

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

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

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:

n = length(JCon); % so luong vat ran trong he

SBid = 1; % chi so mang cua SB

if Checked(CObjID) == 1 % neu vat ran dc ket noi da duyet -> vong kin

% -> pha vo lien ket tai day.

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:

Ở đâ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ì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:

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)

Sau khi thực hiện việc duyệt theo chiều rộng và tách thành các nhánh đơn, ta có được

kết quả như sau:

Hình 2.14: Kết quả chuyển từ hệ kín thành hệ dạng cây sau BFS

Đến lúc này chúng ta có thể sử dụng lại giải thuật DFS để thực hiện việc chia nhỏ hệ

dạng cây này thành các hệ chuỗi đơn như ở phần trước đã nêu

2 Bài toán động học

2.1 Bài toán động học cho hệ dạng chuỗi nối tiếp

Bài toán động học cho hệ dạng chuỗi nối tiếp chính là cơ sở cơ bản để phát triển cho hệ

dạng cây sau này Bời như trong phần lý thuyết đồ thị trong mục 2, chúng ta đã thấy được

rằng có thể biến đổi một hệ có dạng cây thành một tập các nhánh dạng chuỗi

2.1.1 Bài toán động học thuận

Theo quy ước thiết lập các hệ tọa độ ở trên, Ma trận cosin chỉ hướng chuyển từ WCS

sang hệ tọa độ CSi của khâu thứ i sẽ đc xác định bởi công thức:

i i i

T R T

Như vậy, cách thức trên cho phép chúng ta xác định được một giải thuật truy hồi để xác định

được hướng của hệ tọa độ gắn với khối tâm của khâu thứ n là A Từ đó có thể xác định n

được hệ tọa độ gắn với bàn kẹp là: 2

n

n

A H

Vấn đề thứ hai là phải xác định hướng của điểm cuối khâu thao tác đó Ở đây, ta cũng

sẽ đi tìm một công thức truy hồi cho phép xác định được vị trí này:

Từ hình (2.2) ta thấy vị trí trọng tâm của vật rắn thứ i được xác định bởi:

=

=

Công thức (2.5) cho ta một biểu thức truy hồi xác định được vị trí của khối tâm vật

rắn thứ #n trong hệ tọa độ tuyệt đối Và từ đó xác định được vị trí của điểm cuối khâu thao

Như vậy có thể tóm lại giải thuật để thực hiện tính toán bài toán thuận này theo công

thức truy hồi như sau:

Bảng 2 3:giải thuật truy hồi tính động học thuận robot dạng chuỗi

2.1.2 Bài toán động học ngược

Bài toán động học ngược rôbốt là bài toán cơ bản, đặt nền tảng cho bài toán điều

khiển rôbốt Bài toán động học ngược là bài toán xác định các toạ độ, vận tốc, gia tốc của các

biến khớp khi cho biết quỹ đạo chuyển động trong không gian thao tác với một cấu hình

rôbốt đã cho Khó khăn của bài toán động học ngược là phương trình động học cần giải là

một phương trình phi tuyến với nhiều hàm lượng giác Bài toán động học ngược có thể có 0,

1, 2, nhiều hoặc vô số lời giải tuỳ theo vị trí trong không gian thao tác và cấu hình của rôbốt

Có hai cách tiếp cận cho bài toán động học ngược của rôbốt công nghiệp dạng chuỗi

là phương pháp giải tích và phương pháp số Phương pháp giải tích là phương pháp dựa vào

các phương trình động học thuận để đưa được các phương trình dưới dạng tường minh mối

quan hệ của toạ độ, vận tốc, gia tốc các khớp theo quỹ đạo cho trong không gian thao tác

Đây là phương pháp được sử dụng cho hầu hết các rôbốt công nghiệp hiện nay, vì nó đảm

bảo tốc độ tính toán trong điều khiển thời gian thực Tuy nhiên, để áp dụng giải thuật với một

cách tổng quát, ứng dụng cho một lớp các cấu hình robot dạng hở, nên giải thuật số ở đây

được xem là hiệu quả hơn

2.1.2.1 Đặt bài toán và các công thức cơ bản

Cho biết

( ) , R m,

Khi m < n, rôbốt có số bậc tự do f = n lớn hơn số toạ độ xác định vị trí của bàn kẹp (m), rôbốt

được gọi là rôbốt dư dẫn động

Đạo hàm 2 vế của (2.8) theo thời gian, ta suy ra

Đạo hàm 2 vế của (2.14) theo thời gian thu được:

Ma trận J&( )q(t) thu được bằng cách đạo hàm trực tiếp Jacobian J( )q(t)

Thay vào (2.13) ta tìm được gia tốc q&& (t)

Các công thức (2.12),(2.13) và (2.15) cho phép ta xác định được vector vận tốc suy rộng và

vector gia tốc suy rộng, nếu như biết q(t) tại thời điểm khảo sát và các quy luật

1( ) ( ) ( ) ( ) ( )( )

Từ đó, ta có sơ đồ tính toán như sau:

1) Tìm q (Dùng phương pháp lặp), sau đó tính được 0 J(q0),J+(q0),J&(q0)

Theo (2.12) tìm được q& =(0) q&0, theo (2.13) tìm được q& =&( )0 q&0

2) Cho k = 0, 1, 2, , n*-1, tính q(tk+ 1)theo (2.17), theo (2.12) và (2.13) tìm được

q& & ,q&(tk+1)= q&k+1

Nhận xét: Tính q(tk+ 1) theo (2.17) là khá thô Vì vậy để nâng cao độ chính xác cần phải nêu

ra một thuật toán xác định q(tk+ 1) chính xác hơn

2.1.2.2 Thuật toán xác địnhq0 =q( )t0

Giả sử t0 =0 là thời điểm đầu Ta có thể xác định giá trị gần đúng ~q của 0 q bằng cách vẽ 0

(hoặc bằng thực nghiệm) Sau đó áp dụng khai triển Taylor tìm gần đúng tốt hơn của q như 0

sau:

Giả sử lấy q(t0)=~q0 +∆q0

Theo phương trình (2.8) ta có

)

~()

~()

~()()t

∂

∂+

=

∆+

=

q

f q f q q f q f x

Nếu ∆q0 ≥ε1 thì ta lại tiếp tục thế (2.19) vào phương trình (2.18) và lại giải phương trình

này Quá trình lặp này được dừng tại ∆q0 <ε1

Sau đó lấy:

0:= 0

Biết được nghiệm q của phương trình (2.8) tại thời điểm 0 t0 =0 Thay vào (2.12),(2.13) ta

tìm được q& =(0) q&0và q& =&(0) q&0

Giá trị sai số cho phép ε xác định theo yêu cầu về độ chính xác của bài toán 1

( k 1 k 1 k 1 k 1 k 1 k1 ∆ k 1+

∂

∂+

=

∆+

f q q

f q

f x

Biết được q(tk+ 1), ta tính q&(tk+1) theo công thức (2.12) và q&&(tk+1) theo công thức (2.13)

Giá trị sai số cho phép ε xác định theo yêu cầu về độ chính xác của bài toán 2

Hình 2 14: Sơ đồ phương pháp số giải bài toán động học ngược

Trong phần trên chúng ta đã cùng xem xét cách giải quyết bài toán động học ngược, tuy nhiên cũng có những vấn đề nhất định cần bàn luận Trước hết chúng ta cần xem xét hàm f(q) được nêu ra trong giải thuật là hàm gì, như thế nào và làm sao xác định được nó

Hàm f(q) trong thực tế được chọn như sau: ( ) T

f q& = ⎣⎡X& Y& Z& ω ω ω ⎤⎦

2.1.3 Các giải thuật bổ sung

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

Biểu thức tính vận tốc góc của vật rắn i so với WCS:

p u

P r C r

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

Ta chọn điểm P tùy ý thuộc vật khâu i, từ hình vẽ ta có:

ur uur uur

(2.27) Đạo hàm biểu thức trên theo thời gian trong hệ quy chiếu cố định WCS ta có:

WCS WCS

C P

ur uur uur uur ur uur

(2.28) Nếu sử dụng kí hiệu ma trận trong hệ tọa độ tuyệt đối ta có:

T T& H T H H T H& H T H H T H& H T T H& (2.38)

Với trường hợp khớp quay, ta có:

²

( )

σσσ

2.1.3.2 Xác định vận tốc khối tâm của vật rắn:

Vị trí trọng tâm của vật rắn thứ i được xác định bởi:

( ) ( )

Vậy tọa độ của điểm cuối khâu thao tác là:

( ) 0 ( ) 0

r& r& A r& r& ω A r (2.50)

2.1.3.3 Giải thuật tính Jacobian J(q)

Như đã thấy ở phần trên, nếu sử dụng bằng phương pháp symbolic thì việc tính toán

ma trận Jacobian được thực hiện bằng cách đạo hàm symbolic biểu thức hàm f(q) Cách thức

đạo hàm này có hạn chế là tốc độ tính toán khá chậm, đặc biệt với trường hợp hệ có số lượng

vật rắn lớn Vì thế ở đây đề xuất một phương án mới là giải thuật tính số theo phương pháp

truy hồi với chi phí tính cỡ O(N) để tính ma trận Jacobian này

Như phân tích ở phần trên, vector X gồm có hai thành phần ( )0

P

r vàφ, vì thế, ở đây ta cũng phân chia Jacobian ra làm hai thành phần, đó là Jacobian do thành phần chỉ vị trí

q q& Tiếp theo sau đây ta

sẽ tìm giải thuật để tìm ra được các ma trận Jacobian này

∂

=

∂

r J

& là cột thứ j của ma trận Jacobian quay:

( ) 0

n R

∂

=

∂

ω J

Như vậy công thức (2.54) cho ta một biểu thức tính được ma trận Jacobian quay

Tiếp theo ta tìm công thức tính ma trận Jacobian tịnh tiến

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

j n

Ta thấy nếu cho j = n thì biểu thức (2.62) sẽ trở thành (2.56) Như vậy biểu thức (2.62) có thể

coi là biểu thức chung để tính được cột thứ j của ma trận Jacobian tịnh tiến

Từ những phân tích trên, ta đưa ra giải thuật số để tính ma trận Jacobian J(q) như sau:

Vậy giải thuật đệ quy như trên nêu ra cho phép chúng ta tính được ma trận Jacobian J(q) với

chi phí tính toán là 9n phép tính nhân và 5n phép tính cộng ma trận

2.1.3.4 Giải thuật tính ma trận J q&( )

Trong giải thuật được nêu ra ở mục 2.1.2.2 , để tính giá trị của q&&thì chúng ta cần thiết

phải xác định được ma trận J q&( ) Với trường hợp tính ma trận này bằng phương pháp

symbolic thì chúng ta chỉ cần thực hiện đạo hàm trực tiếp theo thời gian biểu thức của

jacobian J(q) Tuy nhiên ở đây, chúng ta đang muốn xây dựng được một phương pháp số nên

việc tìm ra một giải thuật số để thực hiện được việc tính toán ra J q&( )là cần thiết Vì thế ở

đây đề xuất thêm một giải thuật mới để xác định giá trị này

Cũng như trong 2.1.3.3 Ở đây ta cũng phân chia ra làm hai thành phần, đó là vi phân

của Jacobian chỉ vị tríJ q&T( ) và vi phân Jacobian chỉ hướngJ q&R( ) Ta sẽ đi tìm cách thức

Vậy ta có giải thuật để thực hiện được việc tính vi phân của ma trận Jacobian là:

Bảng 2 5: Giải thuật tính J q&( )Chiều thuận:

quay ( )0 ( )0

1

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

Nghiệm thu được theo phương pháp trên là nghiệm có chuẩn nhỏ nhất trong tập vô số nghiệm

vận tốc của bài toán động học ngược Trong trường hợp nghiệm tổng quát có dạng:

= + −

với z là vectơ được chọn phù hợp để rôbốt thoả mãn thêm các yêu cầu cho phép

Để thực hiện ta đưa vào hàm mục tiêu φ(q), khi đó z được chọn (xem [2], [14])

α: hằng số dương nếu hàm φ(q)cần cực đại hoá, hằng số âm nếu )φ(q cần cực tiểu

(q = J q JT q

Hàm này triệt tiêu tại cấu hình suy biến Do đó, cực đại hàm này sẽ giúp rôbốt tránh được các

điểm suy biến trong quá trình hoạt động

2) Hàm khoảng cách tới giới hạn khớp:

2 n

1

i i

qq

qqn

1)

Với qiM (qim) là ký hiệu của giới hạn lớn nhất (nhỏ nhất) và q là giá trị giữa của khoảng làm i

việc của khớp Do đó, cực đại khoảng cách này, tính dư dẫn động sẽ được khai thác để giữ

cho các biến khớp gần giá trị giữa của khoảng làm việc của rôbốt, tránh được các giới hạn

khớp

3) Hàm khoảng cách tới vật cản

o q p

q)=min ( )−

(

Với o là vectơ vị trí của một điểm thích hợp trên chướng ngại vật (ví dụ tâm trong trường hợp

mô hình vật cản là hình cầu) và p là vectơ vị trí suy rộng cấu trúc của rôbốt Do đó, cực đại

khoảng cách này sẽ giúp rôbốt tránh được vật cản trong quá trình hoạt động Trên thực tế

rôbốt không gian, việc mô hình các vật cản cũng như xác định giá trị hàm này là khá phức tạp

2.1.3.6 Vấn đề giới hạn khớp trong bài toán động học ngược

Bài toán giới hạn khớp là một trong những bài toán thực tế rất quan trọng cần giải

quyết, bởi lẽ, nó cho chúng ta biết được hệ thực có đáp ứng được nghiệm giải ra hay không

Trong trường hợp nếu như nghiệm giải ra trong bài toán động học ngược nằm ngoài giới hạn

khớp cho phép, thì có nghĩa là kết quả giải đó hệ thực không thể đáp ứng được, hay nói cách

khác, kết quả giải như vậy là vô nghĩa Vì thế, trong bài toán động học ngược, chúng ta cần